diff --git a/.github/workflows/colcon.yml b/.github/workflows/colcon.yml index 8bccf2acd156b..45c068d05cf61 100644 --- a/.github/workflows/colcon.yml +++ b/.github/workflows/colcon.yml @@ -156,7 +156,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v4 + uses: actions/cache@v5 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/cygwin_build.yml b/.github/workflows/cygwin_build.yml index 79e30dd5d6ea2..10e055ef4c8b3 100644 --- a/.github/workflows/cygwin_build.yml +++ b/.github/workflows/cygwin_build.yml @@ -180,7 +180,7 @@ jobs: source ~/ccache.conf && ccache -s - name: ccache cache files - uses: actions/cache@v4 + uses: actions/cache@v5 with: path: D:/a/ardupilot/ardupilot/ccache key: ${{ steps.ccache_cache_timestamp.outputs.cache-key }}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -220,7 +220,7 @@ jobs: fail: true - name: Archive build - uses: actions/upload-artifact@v5 + uses: actions/upload-artifact@v6 with: name: binaries path: artifacts diff --git a/.github/workflows/esp32_build.yml b/.github/workflows/esp32_build.yml index a1883e6eff663..02499f5982a94 100644 --- a/.github/workflows/esp32_build.yml +++ b/.github/workflows/esp32_build.yml @@ -226,7 +226,7 @@ jobs: ls bootloader* partition* Ardu*.elf Ardu*.bin >> $GITHUB_STEP_SUMMARY - name: Archive artifacts - uses: actions/upload-artifact@v5 + uses: actions/upload-artifact@v6 with: name: esp32-binaries -${{matrix.config}} path: | diff --git a/.github/workflows/macos_build.yml b/.github/workflows/macos_build.yml index 20ed07c5572f1..0185920d7ab02 100644 --- a/.github/workflows/macos_build.yml +++ b/.github/workflows/macos_build.yml @@ -169,7 +169,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v4 + uses: actions/cache@v5 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{matrix.config}}-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/qurt_build.yml b/.github/workflows/qurt_build.yml index 27f4953ed47eb..f716ed0d0d6a2 100644 --- a/.github/workflows/qurt_build.yml +++ b/.github/workflows/qurt_build.yml @@ -164,7 +164,7 @@ jobs: libraries/AP_HAL_QURT/packaging/make_package.sh Rover ardurover - name: Archive build - uses: actions/upload-artifact@v5 + uses: actions/upload-artifact@v6 with: name: qurt-binaries path: | diff --git a/.github/workflows/test_ccache.yml b/.github/workflows/test_ccache.yml index d7a03bfac1f50..df636d7da3fb1 100644 --- a/.github/workflows/test_ccache.yml +++ b/.github/workflows/test_ccache.yml @@ -143,5 +143,4 @@ jobs: 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 Durandal-bdshot,Pixhawk6X --min-cache-pct=60 diff --git a/.github/workflows/test_chibios.yml b/.github/workflows/test_chibios.yml index e12c2da6d6dbe..1e69c746c3e13 100644 --- a/.github/workflows/test_chibios.yml +++ b/.github/workflows/test_chibios.yml @@ -182,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@v5 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{matrix.config}}-${{ matrix.toolchain }}-${{ matrix.gcc }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_coverage.yml b/.github/workflows/test_coverage.yml index 4e8b26cb6172e..e8813be12e976 100644 --- a/.github/workflows/test_coverage.yml +++ b/.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@v5 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_dds.yml b/.github/workflows/test_dds.yml index 1553801a05288..48a6e28502cd9 100644 --- a/.github/workflows/test_dds.yml +++ b/.github/workflows/test_dds.yml @@ -161,7 +161,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v4 + uses: actions/cache@v5 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.config }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -188,7 +188,7 @@ jobs: Tools/scripts/build_ci.sh - name: Archive buildlog artifacts - uses: actions/upload-artifact@v5 + uses: actions/upload-artifact@v6 if: failure() with: name: fail-${{matrix.config}} diff --git a/.github/workflows/test_linux_sbc.yml b/.github/workflows/test_linux_sbc.yml index 6823119ff6521..9a102a1d8a367 100644 --- a/.github/workflows/test_linux_sbc.yml +++ b/.github/workflows/test_linux_sbc.yml @@ -183,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@v5 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{matrix.config}}-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_replay.yml b/.github/workflows/test_replay.yml index 566fe4733d240..6cb22f5711a70 100644 --- a/.github/workflows/test_replay.yml +++ b/.github/workflows/test_replay.yml @@ -172,7 +172,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v4 + uses: actions/cache@v5 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -194,7 +194,7 @@ jobs: Tools/scripts/build_ci.sh - name: Archive buildlog artifacts - uses: actions/upload-artifact@v5 + uses: actions/upload-artifact@v6 if: failure() with: name: fail-${{ matrix.toolchain }}-${{matrix.config}} diff --git a/.github/workflows/test_scripting.yml b/.github/workflows/test_scripting.yml index 0329c06eb2ab8..c466753783b5d 100644 --- a/.github/workflows/test_scripting.yml +++ b/.github/workflows/test_scripting.yml @@ -85,7 +85,7 @@ jobs: 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@v5 + uses: actions/upload-artifact@v6 with: name: Docs path: ScriptingDocs.md diff --git a/.github/workflows/test_sitl_blimp.yml b/.github/workflows/test_sitl_blimp.yml index 1a90ae44365ac..2646cd0017c12 100644 --- a/.github/workflows/test_sitl_blimp.yml +++ b/.github/workflows/test_sitl_blimp.yml @@ -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@v5 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -231,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@v5 with: path: ~/.ccache key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -249,7 +249,7 @@ jobs: Tools/scripts/build_ci.sh - name: Archive buildlog artifacts - uses: actions/upload-artifact@v5 + uses: actions/upload-artifact@v6 if: failure() with: name: fail-${{matrix.config}} @@ -263,7 +263,7 @@ jobs: retention-days: 14 - name: Archive .bin artifacts - uses: actions/upload-artifact@v5 + uses: actions/upload-artifact@v6 with: name: BIN-${{matrix.config}} path: /__w/ardupilot/ardupilot/logs diff --git a/.github/workflows/test_sitl_copter.yml b/.github/workflows/test_sitl_copter.yml index f0003dc4f3d4e..d37c72f0bc9a0 100644 --- a/.github/workflows/test_sitl_copter.yml +++ b/.github/workflows/test_sitl_copter.yml @@ -183,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@v5 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -249,7 +249,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@v5 with: path: ~/.ccache key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -267,7 +267,7 @@ jobs: Tools/scripts/build_ci.sh - name: Archive buildlog artifacts - uses: actions/upload-artifact@v5 + uses: actions/upload-artifact@v6 if: failure() with: name: fail-${{matrix.config}} @@ -281,7 +281,7 @@ jobs: retention-days: 14 - name: Archive .bin artifacts - uses: actions/upload-artifact@v5 + uses: actions/upload-artifact@v6 with: name: BIN-${{matrix.config}} path: /__w/ardupilot/ardupilot/logs @@ -304,7 +304,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v4 + uses: actions/cache@v5 with: path: ~/.ccache key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -345,7 +345,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@v5 with: path: ~/.ccache key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -363,7 +363,7 @@ jobs: Tools/scripts/build_ci.sh - name: Archive buildlog artifacts - uses: actions/upload-artifact@v5 + uses: actions/upload-artifact@v6 if: failure() with: name: fail-${{matrix.config}} @@ -377,7 +377,7 @@ jobs: retention-days: 14 - name: Archive .bin artifacts - uses: actions/upload-artifact@v5 + uses: actions/upload-artifact@v6 with: name: BIN-${{matrix.config}} path: /__w/ardupilot/ardupilot/logs diff --git a/.github/workflows/test_sitl_periph.yml b/.github/workflows/test_sitl_periph.yml index 47a1865b3b0c4..64f71997a30b4 100644 --- a/.github/workflows/test_sitl_periph.yml +++ b/.github/workflows/test_sitl_periph.yml @@ -174,7 +174,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v4 + uses: actions/cache@v5 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -224,7 +224,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v4 + uses: actions/cache@v5 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_sitl_plane.yml b/.github/workflows/test_sitl_plane.yml index 3c2ec9d20f985..f4676bd8634c9 100644 --- a/.github/workflows/test_sitl_plane.yml +++ b/.github/workflows/test_sitl_plane.yml @@ -183,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@v5 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -233,7 +233,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@v5 with: path: ~/.ccache key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -251,7 +251,7 @@ jobs: Tools/scripts/build_ci.sh - name: Archive buildlog artifacts - uses: actions/upload-artifact@v5 + uses: actions/upload-artifact@v6 if: failure() with: name: fail-${{matrix.config}} @@ -265,7 +265,7 @@ jobs: retention-days: 14 - name: Archive .bin artifacts - uses: actions/upload-artifact@v5 + uses: actions/upload-artifact@v6 with: name: BIN-${{matrix.config}} path: /__w/ardupilot/ardupilot/logs diff --git a/.github/workflows/test_sitl_rover.yml b/.github/workflows/test_sitl_rover.yml index 40fffa06a8be1..7cf3580d55801 100644 --- a/.github/workflows/test_sitl_rover.yml +++ b/.github/workflows/test_sitl_rover.yml @@ -182,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@v5 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -247,7 +247,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@v5 with: path: ~/.ccache key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -265,7 +265,7 @@ jobs: Tools/scripts/build_ci.sh - name: Archive buildlog artifacts - uses: actions/upload-artifact@v5 + uses: actions/upload-artifact@v6 if: failure() with: name: fail-${{matrix.config}} @@ -279,7 +279,7 @@ jobs: retention-days: 14 - name: Archive .bin artifacts - uses: actions/upload-artifact@v5 + uses: actions/upload-artifact@v6 with: name: BIN-${{matrix.config}} path: /__w/ardupilot/ardupilot/logs diff --git a/.github/workflows/test_sitl_sub.yml b/.github/workflows/test_sitl_sub.yml index 77dbd2181eb88..3dfc7d2100ff4 100644 --- a/.github/workflows/test_sitl_sub.yml +++ b/.github/workflows/test_sitl_sub.yml @@ -186,7 +186,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v4 + uses: actions/cache@v5 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -246,7 +246,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@v5 with: path: ~/.ccache key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -264,7 +264,7 @@ jobs: Tools/scripts/build_ci.sh - name: Archive buildlog artifacts - uses: actions/upload-artifact@v5 + uses: actions/upload-artifact@v6 if: failure() with: name: fail-${{matrix.config}} @@ -278,7 +278,7 @@ jobs: retention-days: 14 - name: Archive .bin artifacts - uses: actions/upload-artifact@v5 + uses: actions/upload-artifact@v6 with: name: BIN-${{matrix.config}} path: /__w/ardupilot/ardupilot/logs diff --git a/.github/workflows/test_sitl_tracker.yml b/.github/workflows/test_sitl_tracker.yml index c798b008f140f..9bbef2fe4ef9d 100644 --- a/.github/workflows/test_sitl_tracker.yml +++ b/.github/workflows/test_sitl_tracker.yml @@ -185,7 +185,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v4 + uses: actions/cache@v5 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -232,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@v5 with: path: ~/.ccache key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -250,7 +250,7 @@ jobs: Tools/scripts/build_ci.sh - name: Archive buildlog artifacts - uses: actions/upload-artifact@v5 + uses: actions/upload-artifact@v6 if: failure() with: name: fail-${{matrix.config}} @@ -264,7 +264,7 @@ jobs: retention-days: 14 - name: Archive .bin artifacts - uses: actions/upload-artifact@v5 + uses: actions/upload-artifact@v6 with: name: BIN-${{matrix.config}} path: /__w/ardupilot/ardupilot/logs diff --git a/.github/workflows/test_size.yml b/.github/workflows/test_size.yml index 3e2e30b7124b5..3fcd015c1287e 100644 --- a/.github/workflows/test_size.yml +++ b/.github/workflows/test_size.yml @@ -60,270 +60,169 @@ concurrency: jobs: build: runs-on: ubuntu-22.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.1.3 - strategy: - fail-fast: false # don't cancel if a job from the matrix fails - matrix: - toolchain: [ - chibios, - ] - config: [ - Durandal, - MatekF405, - KakuteF7, - MatekH743-bdshot, - MambaH743v4, # for littlefs support - Pixhawk1-1M, - MatekF405-CAN, # see special "build bootloader" code below - DrotekP3Pro, # see special "build bootloader" code below - Hitec-Airspeed, # see special code for Periph below (3 places!) - f103-GPS # see special code for Periph below (3 places!) - ] - include: - - config: disco - toolchain: armhf - exclude: - - config: disco - toolchain: chibios + container: ardupilot/ardupilot-dev-chibios:v0.1.3 + permissions: + contents: read + pull-requests: write + steps: - uses: actions/checkout@v6 with: - ref: ${{ github.event.pull_request.base.ref }} - path: base_branch + fetch-depth: 0 submodules: 'recursive' + # Put ccache into github cache for faster build - name: Prepare ccache timestamp id: ccache_cache_timestamp run: | NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT + - name: ccache cache files - uses: actions/cache@v4 + uses: actions/cache@v5 with: path: ~/.ccache - key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} - restore-keys: ${{github.workflow}}-ccache-${{ matrix.toolchain }}- # restore ccache from either previous build on this branch or on base branch + key: ${{github.workflow}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} + restore-keys: ${{github.workflow}}-ccache- + - name: setup ccache run: | - . base_branch/.github/workflows/ccache.env - - name: Build ${{ github.event.pull_request.base.ref }} ${{matrix.config}} ${{ matrix.toolchain }} - env: - CI_BUILD_TARGET: ${{matrix.config}} - shell: bash - run: | - set -ex - # set up some variables based on what sort of build we're doing: - BOOTLOADER=0 - AP_PERIPH=0 - if [ "${{matrix.config}}" = "Hitec-Airspeed" ] || - [ "${{matrix.config}}" = "f103-GPS" ]; then - AP_PERIPH=1 - elif [ "${{matrix.config}}" = "MatekF405-CAN" ] || - [ "${{matrix.config}}" = "DrotekP3Pro" ]; then - BOOTLOADER=1 - fi - if [ $BOOTLOADER -eq 1 ]; then - BIN_SRC="build/${{matrix.config}}/bootloader" - else - BIN_SRC="build/${{matrix.config}}/bin" - fi - - git config --global --add safe.directory ${GITHUB_WORKSPACE} - PATH="/github/home/.local/bin:$PATH" - - # build the base branch - cd base_branch - # configure: - if [ $BOOTLOADER -eq 1 ]; then - ./waf configure --board ${{matrix.config}} --bootloader - else - ./waf configure --board ${{matrix.config}} - fi - # build: - if [ $AP_PERIPH -eq 1 ]; then - ./waf AP_Periph - elif [ $BOOTLOADER -eq 1 ]; then - ./waf bootloader - else - ./waf - fi - mkdir -p $GITHUB_WORKSPACE/base_branch_bin - - cp -r $BIN_SRC/* $GITHUB_WORKSPACE/base_branch_bin/ - - # build a set of binaries without symbols so we can check if - # the binaries have changed. - echo [`date`] Building ${{ github.event.pull_request.base.ref }} with no versions - - NO_VERSIONS_DIR="$GITHUB_WORKSPACE/base_branch_bin_no_versions" - mkdir "$NO_VERSIONS_DIR" - - # export some environment variables designed to get - # repeatable builds from the same source: - export CHIBIOS_GIT_VERSION="12345678" - export GIT_VERSION_EXTENDED="0123456789abcdef" - export GIT_VERSION="abcdef" - export GIT_VERSION_INT="15" - - if [ $AP_PERIPH -eq 1 ]; then - ./waf AP_Periph - elif [ $BOOTLOADER -eq 1 ]; then - ./waf bootloader - else - ./waf - fi - cp -r $BIN_SRC/* "$NO_VERSIONS_DIR" - - echo [`date`] Built ${{ github.event.pull_request.base.ref }} with no versions - - - uses: actions/checkout@v6 - with: - fetch-depth: 0 - path: 'pr' + . .github/workflows/ccache.env - - name: Build PR rebased ${{matrix.config}} ${{ matrix.toolchain }} - env: - CI_BUILD_TARGET: ${{matrix.config}} - shell: bash + - name: Install rsync run: | - set -ex - # set up some variables based on what sort of build we're doing: - BOOTLOADER=0 - AP_PERIPH=0 - if [ "${{matrix.config}}" = "Hitec-Airspeed" ] || - [ "${{matrix.config}}" = "f103-GPS" ]; then - AP_PERIPH=1 - elif [ "${{matrix.config}}" = "MatekF405-CAN" ] || - [ "${{matrix.config}}" = "DrotekP3Pro" ]; then - BOOTLOADER=1 - fi - if [ $BOOTLOADER -eq 1 ]; then - BIN_SRC="build/${{matrix.config}}/bootloader" - else - BIN_SRC="build/${{matrix.config}}/bin" - fi - git config --global --add safe.directory ${GITHUB_WORKSPACE} - PATH="/github/home/.local/bin:$PATH" - cd pr/ - git config user.email "ardupilot-ci@ardupilot.org" - git config user.name "ArduPilot CI" - git remote add target_repo https://github.com/${{github.event.pull_request.base.repo.full_name}} - git fetch --no-tags --prune --progress target_repo ${{ github.event.pull_request.base.ref }} - git rebase target_repo/${{ github.event.pull_request.base.ref }} - git submodule update --init --recursive --depth=1 - # configure - if [ $BOOTLOADER -eq 1 ]; then - ./waf configure --board ${{matrix.config}} --bootloader - else - ./waf configure --board ${{matrix.config}} - fi - # build - if [ $AP_PERIPH -eq 1 ]; then - ./waf AP_Periph - elif [ $BOOTLOADER -eq 1 ]; then - ./waf bootloader - else - ./waf - fi - mkdir $GITHUB_WORKSPACE/pr_bin - cp -r $BIN_SRC/* $GITHUB_WORKSPACE/pr_bin/ - - # build a set of binaries without symbols so we can check if - # the binaries have changed. - echo [`date`] Building PR with no versions - - NO_VERSIONS_DIR="$GITHUB_WORKSPACE/pr_bin_no_versions" - mkdir "$NO_VERSIONS_DIR" - - # export some environment variables designed to get - # repeatable builds from the same source: - export CHIBIOS_GIT_VERSION="12345678" - export GIT_VERSION_EXTENDED="0123456789abcdef" - export GIT_VERSION="abcdef" - export GIT_VERSION_INT="15" + apt-get update + apt-get install -y rsync - if [ $AP_PERIPH -eq 1 ]; then - ./waf AP_Periph - elif [ $BOOTLOADER -eq 1 ]; then - ./waf bootloader - else - ./waf - fi - cp -r $BIN_SRC/* "$NO_VERSIONS_DIR" - - echo [`date`] Built PR with no versions - - # build MatekF405 Plane without quadplane - if [ "${{matrix.config}}" = "MatekF405" ]; then - PLANE_BINARY="build/MatekF405/bin/arduplane.bin" - echo "normal size" - ls -l "$PLANE_BINARY" - EXTRA_HWDEF="/tmp/extra-options.def" - echo "define HAL_QUADPLANE_ENABLED 0" >"$EXTRA_HWDEF" - ./waf configure --board ${{matrix.config}} --extra-hwdef="$EXTRA_HWDEF" - ./waf plane - rm "$EXTRA_HWDEF" - echo "non-quadplane size:" - ls -l "$PLANE_BINARY" - fi - - - name: Full size compare with base branch + - name: Setup git references for size comparison shell: bash run: | - cd pr/ - Tools/scripts/build_tests/pretty_diff_size.py -m $GITHUB_WORKSPACE/base_branch_bin_no_versions -s $GITHUB_WORKSPACE/pr_bin_no_versions + git config --global --add safe.directory ${GITHUB_WORKSPACE} + # Add the base repository as a remote and fetch the base branch + git remote add upstream ${{ github.event.pull_request.base.repo.clone_url }} + git fetch --no-tags --prune --progress upstream ${{ github.event.pull_request.base.ref }} - - name: Feature compare with ${{ github.event.pull_request.base.ref }} + - name: Build and compare sizes using size_compare_branches.py shell: bash run: | - set -ex - cd pr/ - BIN_PREFIX="arm-none-eabi-" - if [ "${{matrix.toolchain}}" = "armhf" ]; then - BIN_PREFIX="arm-linux-gnueabihf-" - fi - BOOTLOADER=0 - AP_PERIPH=0 - if [ "${{matrix.config}}" = "Hitec-Airspeed" ] || - [ "${{matrix.config}}" = "f103-GPS" ]; then - AP_PERIPH=1 - elif [ "${{matrix.config}}" = "MatekF405-CAN" ] || - [ "${{matrix.config}}" = "DrotekP3Pro" ]; then - BOOTLOADER=1 - fi - if [ $AP_PERIPH -eq 1 ]; then - EF_BINARY_NAME="AP_Periph" - elif [ $BOOTLOADER -eq 1 ]; then - EF_BINARY_NAME="AP_Bootloader" - else - EF_BINARY_NAME="arduplane" - fi - EF_BASE_BRANCH_BINARY="$GITHUB_WORKSPACE/base_branch_bin/$EF_BINARY_NAME" - EF_PR_BRANCH_BINARY="$GITHUB_WORKSPACE/pr_bin/$EF_BINARY_NAME" - - 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 + PATH="/github/home/.local/bin:$PATH" - - name: Binary 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=*.abin --exclude=*.apj || true - diff_output=$(diff -r $GITHUB_WORKSPACE/base_branch_bin_no_versions $GITHUB_WORKSPACE/pr_bin_no_versions --exclude=*.abin --exclude=*.apj || true) - echo "### Binary 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 + # Run size_compare_branches.py with parallelization + # Use upstream/ref instead of just ref since that's where we fetched it + python3 Tools/scripts/size_compare_branches.py \ + --board Durandal \ + --board MatekF405 \ + --board KakuteF7 \ + --board MatekH743-bdshot \ + --board MambaH743v4 \ + --board Pixhawk1-1M \ + --board MatekF405-CAN \ + --board DrotekP3Pro \ + --board Hitec-Airspeed \ + --board f103-GPS \ + --all-vehicles \ + --features \ + -j 8 \ + --master-branch upstream/${{ github.event.pull_request.base.ref }} \ + | tee /tmp/size_compare_output.txt + + - name: Format and post size comparison results + if: always() && github.event_name == 'pull_request' + uses: actions/github-script@v7 + with: + script: | + const fs = require('fs'); + const pr_number = context.issue.number; + + // Read the output from size_compare_branches.py + let output = ''; + try { + output = fs.readFileSync('/tmp/size_compare_output.txt', 'utf8'); + } catch (error) { + output = 'Error: Could not read size comparison output'; + } + + // Parse and format the output + const lines = output.split('\n'); + let sizeSection = ''; + let featureSection = ''; + let inFeatures = false; + + for (const line of lines) { + // Detect feature changes section + if (line.includes('Feature changes') || line.includes('features added') || line.includes('features removed')) { + inFeatures = true; + } + + // Format size changes - match CSV header and data rows + // Header: "Board,AP_Periph,..." + // Data rows: "MatekF405,1234,*,-500" etc + if (line.match(/^Board,/) || line.match(/^[A-Za-z0-9_-]+,/)) { + sizeSection += line + '\n'; + } + + // Format feature changes + if (inFeatures && line.trim()) { + // Make feature lists more readable + if (line.includes('added:') || line.includes('removed:')) { + featureSection += '\n**' + line.trim() + '**\n'; + } else if (line.trim().startsWith('-') || line.trim().startsWith('+')) { + // Format added/removed features with better symbols + const formatted = line.replace(/^\s*\+/, ' ✅ ').replace(/^\s*-/, ' ❌ '); + featureSection += formatted + '\n'; + } else { + featureSection += line + '\n'; + } + } + } + + // Build the comment body + let commentBody = `## 📊 Size Comparison Report\n\n`; + commentBody += `Build completed for commit ${context.sha.substring(0, 7)}\n\n`; + + if (sizeSection.trim()) { + commentBody += `### 📦 Size Changes\n\n`; + commentBody += '```\n' + sizeSection.trim() + '\n```\n\n'; + } + + if (featureSection.trim()) { + commentBody += `### 🔧 Feature Changes\n\n`; + commentBody += featureSection.trim() + '\n\n'; + } + + if (!sizeSection.trim() && !featureSection.trim()) { + commentBody += `No size or feature changes detected.\n\n`; + } + + commentBody += `---\n`; + commentBody += `*Updated automatically by the size test workflow*`; + + // Find and update or create comment + const comments = await github.rest.issues.listComments({ + owner: context.repo.owner, + repo: context.repo.repo, + issue_number: pr_number, + }); + + const botComment = comments.data.find(comment => + comment.user.type === 'Bot' && + comment.body.includes('Size Comparison Report') + ); + + if (botComment) { + await github.rest.issues.updateComment({ + owner: context.repo.owner, + repo: context.repo.repo, + comment_id: botComment.id, + body: commentBody + }); + console.log('Updated existing comment'); + } else { + await github.rest.issues.createComment({ + owner: context.repo.owner, + repo: context.repo.repo, + issue_number: pr_number, + body: commentBody + }); + console.log('Created new comment'); + } diff --git a/.github/workflows/test_unit_tests.yml b/.github/workflows/test_unit_tests.yml index 5d8152c531674..f12ce023ec4c9 100644 --- a/.github/workflows/test_unit_tests.yml +++ b/.github/workflows/test_unit_tests.yml @@ -128,7 +128,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v4 + uses: actions/cache@v5 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -166,7 +166,7 @@ jobs: Tools/autotest/unittest/annotate_params_unittest.py - name: Archive buildlog artifacts - uses: actions/upload-artifact@v5 + uses: actions/upload-artifact@v6 if: failure() with: name: fail-${{ matrix.toolchain }}-${{matrix.config}} diff --git a/ArduCopter/AP_Arming_Copter.cpp b/ArduCopter/AP_Arming_Copter.cpp index 930c433ceea81..390967a20d739 100644 --- a/ArduCopter/AP_Arming_Copter.cpp +++ b/ArduCopter/AP_Arming_Copter.cpp @@ -283,9 +283,9 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure) check_failed(Check::PARAMETERS, display_failure, failure_template, "no rangefinder"); return false; } - // check if RTL_ALT is higher than rangefinder's max range - if (copter.g.rtl_altitude_cm > copter.rangefinder.max_distance_orient(ROTATION_PITCH_270) * 100) { - check_failed(Check::PARAMETERS, display_failure, failure_template, "RTL_ALT (in cm) above RNGFND_MAX (in metres)"); + // check if RTL_ALT_M is higher than rangefinder's max range + if (copter.mode_rtl.get_altitude_m() > copter.rangefinder.max_distance_orient(ROTATION_PITCH_270)) { + check_failed(Check::PARAMETERS, display_failure, failure_template, "RTL_ALT_M above RNGFND_MAX"); return false; } #else diff --git a/ArduCopter/AP_ExternalControl_Copter.cpp b/ArduCopter/AP_ExternalControl_Copter.cpp index 9421436a3ec4b..4e8bc368d1b82 100644 --- a/ArduCopter/AP_ExternalControl_Copter.cpp +++ b/ArduCopter/AP_ExternalControl_Copter.cpp @@ -20,11 +20,7 @@ bool AP_ExternalControl_Copter::set_linear_velocity_and_yaw_rate(const Vector3f const float checked_yaw_rate_rad = isnan(yaw_rate_rads)? 0: yaw_rate_rads; // Copter velocity is positive if aircraft is moving up which is opposite the incoming NED frame. - const Vector3f velocity_neu_ms { - linear_velocity_ned_ms.x, - linear_velocity_ned_ms.y, - -linear_velocity_ned_ms.z }; - copter.mode_guided.set_vel_NEU_ms(velocity_neu_ms, false, 0, !isnan(yaw_rate_rads), checked_yaw_rate_rad); + copter.mode_guided.set_vel_NED_ms(linear_velocity_ned_ms, false, 0, !isnan(yaw_rate_rads), checked_yaw_rate_rad); return true; } diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index da06d45b3ca67..ef7f3b1a1395f 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -123,7 +123,7 @@ void Copter::set_accel_throttle_I_from_pilot_throttle() // get last throttle input sent to attitude controller float pilot_throttle = constrain_float(attitude_control->get_throttle_in(), 0.0, 1.0); // shift difference between pilot's throttle and hover throttle into accelerometer I - pos_control->get_accel_U_pid().set_integrator((pilot_throttle-motors->get_throttle_hover()) * 1000.0); + pos_control->D_get_accel_pid().set_integrator(-(pilot_throttle - motors->get_throttle_hover())); } // It will return the PILOT_SPEED_DN value if non zero, otherwise if zero it returns the PILOT_SPEED_UP value. diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 5b250a8cc3f96..96796f2309614 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -316,9 +316,7 @@ bool Copter::set_target_pos_NED(const Vector3f& target_pos_ned_m, bool use_yaw, return false; } - const Vector3p pos_neu_m{target_pos_ned_m.x, target_pos_ned_m.y, -target_pos_ned_m.z}; - - return mode_guided.set_pos_NEU_m(pos_neu_m, use_yaw, radians(yaw_deg), use_yaw_rate, radians(yaw_rate_degs), yaw_relative, is_terrain_alt); + return mode_guided.set_pos_NED_m(target_pos_ned_m.topostype(), use_yaw, radians(yaw_deg), use_yaw_rate, radians(yaw_rate_degs), yaw_relative, is_terrain_alt); } // set target position and velocity (for use by scripting) @@ -329,10 +327,7 @@ bool Copter::set_target_posvel_NED(const Vector3f& target_pos_ned_m, const Vecto return false; } - const Vector3p pos_neu_m{target_pos_ned_m.x, target_pos_ned_m.y, -target_pos_ned_m.z}; - const Vector3f vel_neu_ms{target_vel_ned_ms.x, target_vel_ned_ms.y, -target_vel_ned_ms.z}; - - return mode_guided.set_pos_vel_accel_NEU_m(pos_neu_m, vel_neu_ms, Vector3f()); + return mode_guided.set_pos_vel_accel_NED_m(target_pos_ned_m.topostype(), target_vel_ned_ms, Vector3f()); } // set target position, velocity and acceleration (for use by scripting) @@ -343,22 +338,26 @@ bool Copter::set_target_posvelaccel_NED(const Vector3f& target_pos_ned_m, const return false; } - const Vector3p pos_neu_m{target_pos_ned_m.x, target_pos_ned_m.y, -target_pos_ned_m.z}; - const Vector3f vel_neu_ms{target_vel_ned_ms.x, target_vel_ned_ms.y, -target_vel_ned_ms.z}; - const Vector3f accel_neu_mss{target_accel_ned_mss.x, target_accel_ned_mss.y, -target_accel_ned_mss.z}; - - return mode_guided.set_pos_vel_accel_NEU_m(pos_neu_m, vel_neu_ms, accel_neu_mss, use_yaw, radians(yaw_deg), use_yaw_rate, radians(yaw_rate_degs), yaw_relative); + return mode_guided.set_pos_vel_accel_NED_m(target_pos_ned_m.topostype(), target_vel_ned_ms, target_accel_ned_mss, use_yaw, radians(yaw_deg), use_yaw_rate, radians(yaw_rate_degs), yaw_relative); } -bool Copter::set_target_velocity_NED(const Vector3f& target_vel_ned_ms) +bool Copter::set_target_velocity_NED(const Vector3f& target_vel_ned_ms, bool align_yaw_to_target) { // exit if vehicle is not in Guided mode or Auto-Guided mode if (!flightmode->in_guided_mode()) { return false; } - const Vector3f vel_neu_ms{target_vel_ned_ms.x, target_vel_ned_ms.y, -target_vel_ned_ms.z}; - mode_guided.set_vel_NEU_ms(vel_neu_ms); + // optionally line up the copter with the velocity vector + float yaw_rads = 0.0f; + if (align_yaw_to_target) { + const float speed_sq = target_vel_ned_ms.xy().length_squared(); + if (copter.position_ok() && (speed_sq > (YAW_LOOK_AHEAD_MIN_SPEED_MS * YAW_LOOK_AHEAD_MIN_SPEED_MS))) { + yaw_rads = atan2f(target_vel_ned_ms.y, target_vel_ned_ms.x); + } + } + + mode_guided.set_vel_accel_NED_m(target_vel_ned_ms, Vector3f(), align_yaw_to_target, yaw_rads); return true; } @@ -370,10 +369,7 @@ bool Copter::set_target_velaccel_NED(const Vector3f& target_vel_ned_ms, const Ve return false; } - const Vector3f vel_neu_ms{target_vel_ned_ms.x, target_vel_ned_ms.y, -target_vel_ned_ms.z}; - const Vector3f accel_neu_mss{target_accel_ned_mss.x, target_accel_ned_mss.y, -target_accel_ned_mss.z}; - - mode_guided.set_vel_accel_NEU_m(vel_neu_ms, accel_neu_mss, use_yaw, radians(yaw_deg), use_yaw_rate, radians(yaw_rate_degs), relative_yaw); + mode_guided.set_vel_accel_NED_m(target_vel_ned_ms, target_accel_ned_mss, use_yaw, radians(yaw_deg), use_yaw_rate, radians(yaw_rate_degs), relative_yaw); return true; } @@ -413,6 +409,25 @@ bool Copter::set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_ return true; } +// set target roll pitch and yaw angles and roll pitch and yaw rates with throttle (for use by scripting) +bool Copter::set_target_angle_and_rate_and_throttle(float roll_deg, float pitch_deg, float yaw_deg, float roll_rate_degs, float pitch_rate_degs, float yaw_rate_degs, float throttle) +{ + // exit if vehicle is not in Guided mode or Auto-Guided mode + if (!flightmode->in_guided_mode()) { + return false; + } + + Quaternion q; + q.from_euler(radians(roll_deg),radians(pitch_deg),radians(yaw_deg)); + + // Convert from degrees per second to radians per second + Vector3f ang_vel_body_degs { roll_rate_degs, pitch_rate_degs, yaw_rate_degs }; + ang_vel_body_degs *= DEG_TO_RAD; + + mode_guided.set_angle(q, ang_vel_body_degs, 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) { @@ -798,7 +813,7 @@ void Copter::one_hz_loop() if (!using_rate_thread) { attitude_control->set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); } - pos_control->get_accel_U_pid().set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); + pos_control->D_get_accel_pid().set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); #if AC_CUSTOMCONTROL_MULTI_ENABLED custom_control.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); #endif diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 4b11ff59b3ff5..ee32e7549f43c 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -684,10 +684,11 @@ class Copter : public AP_Vehicle { 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 is_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_velocity_NED(const Vector3f& vel_ned, bool align_yaw_to_target) 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; + bool set_target_angle_and_rate_and_throttle(float roll_deg, float pitch_deg, float yaw_deg, float roll_rate_degs, float pitch_rate_degs, float yaw_rate_degs, 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; @@ -911,7 +912,7 @@ class Copter : public AP_Vehicle { void Log_Write_Data(LogDataID id, float value); void Log_Write_PTUN(uint8_t param, float tuning_val, float tune_min, float tune_max, float norm_in); void Log_Video_Stabilisation(); - void Log_Write_Guided_Position_Target(ModeGuided::SubMode submode, const Vector3p& pos_target_m, bool is_terrain_alt, const Vector3f& vel_target_ms, const Vector3f& accel_target_mss); + void Log_Write_Guided_Position_Target(ModeGuided::SubMode submode, const Vector3p& pos_target_ned_m, bool is_terrain_alt, const Vector3f& vel_target_ms, const Vector3f& accel_target_mss); void Log_Write_Guided_Attitude_Target(ModeGuided::SubMode submode, 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); diff --git a/ArduCopter/GCS_Copter.cpp b/ArduCopter/GCS_Copter.cpp index 5ac046104dee3..bec42681e4336 100644 --- a/ArduCopter/GCS_Copter.cpp +++ b/ArduCopter/GCS_Copter.cpp @@ -43,13 +43,13 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void) control_sensors_present |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // XY position controller - if (copter.pos_control->is_active_NE()) { + if (copter.pos_control->NE_is_active()) { 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_U()) { + if (copter.pos_control->D_is_active()) { control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; } diff --git a/ArduCopter/GCS_MAVLink_Copter.cpp b/ArduCopter/GCS_MAVLink_Copter.cpp index d9689b0c91b85..96a67d638a87c 100644 --- a/ArduCopter/GCS_MAVLink_Copter.cpp +++ b/ArduCopter/GCS_MAVLink_Copter.cpp @@ -37,7 +37,7 @@ uint8_t 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_NE()) { + if ((copter.pos_control != nullptr) && copter.pos_control->NE_is_active()) { _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 @@ -147,9 +147,9 @@ void GCS_MAVLINK_Copter::send_position_target_local_ned() } const ModeGuided::SubMode guided_mode = copter.mode_guided.submode(); - Vector3f target_pos_neu_m; - Vector3f target_vel_neu_ms; - Vector3f target_accel_neu_mss; + Vector3f target_pos_ned_m; + Vector3f target_vel_ned_ms; + Vector3f target_accel_ned_mss; uint16_t type_mask = 0; switch (guided_mode) { @@ -162,25 +162,25 @@ void GCS_MAVLINK_Copter::send_position_target_local_ned() type_mask = POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE | POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE | POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except position - target_pos_neu_m = copter.mode_guided.get_target_pos_NEU_m().tofloat(); + target_pos_ned_m = copter.mode_guided.get_target_pos_NED_m().tofloat(); break; case ModeGuided::SubMode::PosVelAccel: type_mask = POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except position, velocity & acceleration - target_pos_neu_m = copter.mode_guided.get_target_pos_NEU_m().tofloat(); - target_vel_neu_ms = copter.mode_guided.get_target_vel_NEU_ms(); - target_accel_neu_mss = copter.mode_guided.get_target_accel_NEU_mss(); + target_pos_ned_m = copter.mode_guided.get_target_pos_NED_m().tofloat(); + target_vel_ned_ms = copter.mode_guided.get_target_vel_NED_ms(); + target_accel_ned_mss = copter.mode_guided.get_target_accel_NED_mss(); break; case ModeGuided::SubMode::VelAccel: type_mask = POSITION_TARGET_TYPEMASK_X_IGNORE | POSITION_TARGET_TYPEMASK_Y_IGNORE | POSITION_TARGET_TYPEMASK_Z_IGNORE | POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except velocity & acceleration - target_vel_neu_ms = copter.mode_guided.get_target_vel_NEU_ms(); - target_accel_neu_mss = copter.mode_guided.get_target_accel_NEU_mss(); + target_vel_ned_ms = copter.mode_guided.get_target_vel_NED_ms(); + target_accel_ned_mss = copter.mode_guided.get_target_accel_NED_mss(); break; case ModeGuided::SubMode::Accel: type_mask = POSITION_TARGET_TYPEMASK_X_IGNORE | POSITION_TARGET_TYPEMASK_Y_IGNORE | POSITION_TARGET_TYPEMASK_Z_IGNORE | POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE | POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except velocity & acceleration - target_accel_neu_mss = copter.mode_guided.get_target_accel_NEU_mss(); + target_accel_ned_mss = copter.mode_guided.get_target_accel_NED_mss(); break; } @@ -189,17 +189,17 @@ void GCS_MAVLINK_Copter::send_position_target_local_ned() AP_HAL::millis(), // time boot ms MAV_FRAME_LOCAL_NED, type_mask, - target_pos_neu_m.x, // x in metres - target_pos_neu_m.y, // y in metres - -target_pos_neu_m.z, // z in metres NED frame - target_vel_neu_ms.x, // vx in m/s - target_vel_neu_ms.y, // vy in m/s - -target_vel_neu_ms.z, // vz in m/s NED frame - target_accel_neu_mss.x, // afx in m/s/s - target_accel_neu_mss.y, // afy in m/s/s - -target_accel_neu_mss.z,// afz in m/s/s NED frame - 0.0f, // yaw - 0.0f); // yaw_rate + target_pos_ned_m.x, // x in metres + target_pos_ned_m.y, // y in metres + target_pos_ned_m.z, // z in metres NED frame + target_vel_ned_ms.x, // vx in m/s + target_vel_ned_ms.y, // vy in m/s + target_vel_ned_ms.z, // vz in m/s NED frame + target_accel_ned_mss.x, // afx in m/s/s + target_accel_ned_mss.y, // afy in m/s/s + target_accel_ned_mss.z, // afz in m/s/s NED frame + 0.0f, // yaw + 0.0f); // yaw_rate #endif } @@ -217,7 +217,7 @@ void GCS_MAVLINK_Copter::send_nav_controller_output() const degrees(targets_rad.z), flightmode->wp_bearing_deg(), MIN(flightmode->wp_distance_m(), UINT16_MAX), - copter.pos_control->get_pos_error_U_m(), + copter.pos_control->get_pos_error_D_m(), 0, flightmode->crosstrack_error_m()); } @@ -281,7 +281,7 @@ void GCS_MAVLINK_Copter::send_pid_tuning() pid_info = &copter.attitude_control->get_rate_yaw_pid().get_pid_info(); break; case PID_TUNING_ACCZ: - pid_info = &copter.pos_control->get_accel_U_pid().get_pid_info(); + pid_info = &copter.pos_control->D_get_accel_pid().get_pid_info(); break; default: continue; @@ -875,26 +875,6 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_pause_continue(const mavlink_comma return MAV_RESULT_DENIED; } -#if HAL_MOUNT_ENABLED -void GCS_MAVLINK_Copter::handle_mount_message(const mavlink_message_t &msg) -{ - switch (msg.msgid) { - 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_deg = mavlink_msg_mount_control_get_input_c(&msg) * 0.01f; - copter.flightmode->auto_yaw.set_yaw_angle_offset_deg(yaw_offset_deg); - break; - } - } - GCS_MAVLINK::handle_mount_message(msg); -} -#endif - // this is called on receipt of a MANUAL_CONTROL packet and is // expected to call manual_override to override RC input on desired // axes. @@ -1063,52 +1043,51 @@ void GCS_MAVLINK_Copter::handle_message_set_position_target_local_ned(const mavl } // prepare position - Vector3p pos_neu_m; + Vector3p pos_ned_m; if (!pos_ignore) { // convert to m - pos_neu_m = Vector3p{packet.x, packet.y, -packet.z}; + pos_ned_m = Vector3p{packet.x, packet.y, packet.z}; // rotate to body-frame if necessary if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { - pos_neu_m.xy() = copter.ahrs.body_to_earth2D_p(pos_neu_m.xy()); + pos_ned_m.xy() = copter.ahrs.body_to_earth2D_p(pos_ned_m.xy()); } // add body offset if necessary if (packet.coordinate_frame == MAV_FRAME_LOCAL_OFFSET_NED || packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { - Vector3p pos_ned_m; - if (!AP::ahrs().get_relative_position_NED_origin(pos_ned_m)) { + Vector3p rel_pos_ned_m; + if (!AP::ahrs().get_relative_position_NED_origin(rel_pos_ned_m)) { // need position estimate to calculate target position copter.mode_guided.init(true); return; } - pos_neu_m.xy() += pos_ned_m.xy(); - pos_neu_m.z -= pos_ned_m.z; + pos_ned_m += rel_pos_ned_m; } } // prepare velocity - Vector3f vel_neu_ms; + Vector3f vel_ned_ms; if (!vel_ignore) { - vel_neu_ms = Vector3f{packet.vx, packet.vy, -packet.vz}; - if (!sane_vel_or_acc_vector(vel_neu_ms)) { + vel_ned_ms = Vector3f{packet.vx, packet.vy, packet.vz}; + if (!sane_vel_or_acc_vector(vel_ned_ms)) { // input is not valid so stop copter.mode_guided.init(true); return; } // rotate to body-frame if necessary if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { - vel_neu_ms.xy() = copter.ahrs.body_to_earth2D(vel_neu_ms.xy()); + vel_ned_ms.xy() = copter.ahrs.body_to_earth2D(vel_ned_ms.xy()); } } // prepare acceleration - Vector3f accel_neu_mss; + Vector3f accel_ned_mss; if (!acc_ignore) { - accel_neu_mss = Vector3f{packet.afx, packet.afy, -packet.afz}; + accel_ned_mss = Vector3f{packet.afx, packet.afy, packet.afz}; // rotate to body-frame if necessary if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { - accel_neu_mss.xy() = copter.ahrs.body_to_earth2D(accel_neu_mss.xy()); + accel_ned_mss.xy() = copter.ahrs.body_to_earth2D(accel_ned_mss.xy()); } } @@ -1126,13 +1105,13 @@ void GCS_MAVLINK_Copter::handle_message_set_position_target_local_ned(const mavl // send request if (!pos_ignore && !vel_ignore) { - copter.mode_guided.set_pos_vel_accel_NEU_m(pos_neu_m, vel_neu_ms, accel_neu_mss, !yaw_ignore, yaw_rad, !yaw_rate_ignore, yaw_rate_rads, yaw_relative); + copter.mode_guided.set_pos_vel_accel_NED_m(pos_ned_m, vel_ned_ms, accel_ned_mss, !yaw_ignore, yaw_rad, !yaw_rate_ignore, yaw_rate_rads, yaw_relative); } else if (pos_ignore && !vel_ignore) { - copter.mode_guided.set_vel_accel_NEU_m(vel_neu_ms, accel_neu_mss, !yaw_ignore, yaw_rad, !yaw_rate_ignore, yaw_rate_rads, yaw_relative); + copter.mode_guided.set_vel_accel_NED_m(vel_ned_ms, accel_ned_mss, !yaw_ignore, yaw_rad, !yaw_rate_ignore, yaw_rate_rads, yaw_relative); } else if (pos_ignore && vel_ignore && !acc_ignore) { - copter.mode_guided.set_accel_NEU_mss(accel_neu_mss, !yaw_ignore, yaw_rad, !yaw_rate_ignore, yaw_rate_rads, yaw_relative); + copter.mode_guided.set_accel_NED_mss(accel_ned_mss, !yaw_ignore, yaw_rad, !yaw_rate_ignore, yaw_rate_rads, yaw_relative); } else if (!pos_ignore && vel_ignore && acc_ignore) { - copter.mode_guided.set_pos_NEU_m(pos_neu_m, !yaw_ignore, yaw_rad, !yaw_rate_ignore, yaw_rate_rads, yaw_relative, false); + copter.mode_guided.set_pos_NED_m(pos_ned_m, !yaw_ignore, yaw_rad, !yaw_rate_ignore, yaw_rate_rads, yaw_relative, false); } else { // input is not valid so stop copter.mode_guided.init(true); @@ -1186,10 +1165,10 @@ void GCS_MAVLINK_Copter::handle_message_set_position_target_global_int(const mav } // prepare velocity - Vector3f vel_neu_ms; + Vector3f vel_ned_ms; if (!vel_ignore) { - vel_neu_ms = Vector3f{packet.vx, packet.vy, -packet.vz}; - if (!sane_vel_or_acc_vector(vel_neu_ms)) { + vel_ned_ms = Vector3f{packet.vx, packet.vy, packet.vz}; + if (!sane_vel_or_acc_vector(vel_ned_ms)) { // input is not valid so stop copter.mode_guided.init(true); return; @@ -1197,9 +1176,9 @@ void GCS_MAVLINK_Copter::handle_message_set_position_target_global_int(const mav } // prepare acceleration - Vector3f accel_neu_mss; + Vector3f accel_ned_mss; if (!acc_ignore) { - accel_neu_mss = Vector3f{packet.afx, packet.afy, -packet.afz}; + accel_ned_mss = Vector3f{packet.afx, packet.afy, packet.afz}; } // prepare yaw @@ -1221,17 +1200,17 @@ void GCS_MAVLINK_Copter::handle_message_set_position_target_global_int(const mav copter.mode_guided.init(true); return; } - Vector3p pos_neu_m; - if (!loc.get_vector_from_origin_NEU_m(pos_neu_m)) { + Vector3p pos_ned_m; + if (!loc.get_vector_from_origin_NED_m(pos_ned_m)) { // input is not valid so stop copter.mode_guided.init(true); return; } - copter.mode_guided.set_pos_vel_NEU_m(pos_neu_m, vel_neu_ms, !yaw_ignore, yaw_rad, !yaw_rate_ignore, yaw_rate_rads); + copter.mode_guided.set_pos_vel_NED_m(pos_ned_m, vel_ned_ms, !yaw_ignore, yaw_rad, !yaw_rate_ignore, yaw_rate_rads); } else if (pos_ignore && !vel_ignore) { - copter.mode_guided.set_vel_accel_NEU_m(vel_neu_ms, accel_neu_mss, !yaw_ignore, yaw_rad, !yaw_rate_ignore, yaw_rate_rads); + copter.mode_guided.set_vel_accel_NED_m(vel_ned_ms, accel_ned_mss, !yaw_ignore, yaw_rad, !yaw_rate_ignore, yaw_rate_rads); } else if (pos_ignore && vel_ignore && !acc_ignore) { - copter.mode_guided.set_accel_NEU_mss(accel_neu_mss, !yaw_ignore, yaw_rad, !yaw_rate_ignore, yaw_rate_rads); + copter.mode_guided.set_accel_NED_mss(accel_ned_mss, !yaw_ignore, yaw_rad, !yaw_rate_ignore, yaw_rate_rads); } else if (!pos_ignore && vel_ignore && acc_ignore) { copter.mode_guided.set_destination(loc, !yaw_ignore, yaw_rad, !yaw_rate_ignore, yaw_rate_rads); } else { @@ -1351,7 +1330,7 @@ int16_t GCS_MAVLINK_Copter::high_latency_target_altitude() const //return units are m if (copter.ap.initialised) { - return global_position_current.alt * 0.01 + copter.pos_control->get_pos_error_U_m(); + return global_position_current.alt * 0.01 - copter.pos_control->get_pos_error_D_m(); } return 0; @@ -1382,7 +1361,7 @@ uint8_t GCS_MAVLINK_Copter::high_latency_tgt_airspeed() const { if (copter.ap.initialised) { // return units are m/s*5 - return MIN(copter.pos_control->get_vel_target_NEU_ms().length() * 5.0, UINT8_MAX); + return MIN(copter.pos_control->get_vel_target_NED_ms().length() * 5.0, UINT8_MAX); } return 0; } @@ -1536,11 +1515,11 @@ uint8_t GCS_MAVLINK_Copter::send_available_mode(uint8_t index) const // Have to deal with is separately because 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"; + name = "Auto RTL"; } else if (index_zero == 1) { mode_number = (uint8_t)Mode::Number::AUTO; - name = "AUTO"; + name = "Auto"; } #endif diff --git a/ArduCopter/GCS_MAVLink_Copter.h b/ArduCopter/GCS_MAVLink_Copter.h index df6f7e5e7d2bd..53ac3b56c619f 100644 --- a/ArduCopter/GCS_MAVLink_Copter.h +++ b/ArduCopter/GCS_MAVLink_Copter.h @@ -37,10 +37,6 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet); MAV_RESULT handle_command_pause_continue(const mavlink_command_int_t &packet); -#if HAL_MOUNT_ENABLED - 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); diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index e4f05fc3e3142..1d0627508f808 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -91,10 +91,10 @@ void Copter::Log_Write_PIDS() logger.Write_PID(LOG_PIDR_MSG, attitude_control->get_rate_roll_pid().get_pid_info()); logger.Write_PID(LOG_PIDP_MSG, attitude_control->get_rate_pitch_pid().get_pid_info()); logger.Write_PID(LOG_PIDY_MSG, attitude_control->get_rate_yaw_pid().get_pid_info()); - logger.Write_PID(LOG_PIDA_MSG, pos_control->get_accel_U_pid().get_pid_info() ); + logger.Write_PID(LOG_PIDA_MSG, pos_control->D_get_accel_pid().get_pid_info() ); if (should_log(MASK_LOG_NTUN) && (flightmode->requires_GPS() || landing_with_GPS())) { - logger.Write_PID(LOG_PIDN_MSG, pos_control->get_vel_NE_pid().get_pid_info_x()); - logger.Write_PID(LOG_PIDE_MSG, pos_control->get_vel_NE_pid().get_pid_info_y()); + logger.Write_PID(LOG_PIDN_MSG, pos_control->NE_get_vel_pid().get_pid_info_x()); + logger.Write_PID(LOG_PIDE_MSG, pos_control->NE_get_vel_pid().get_pid_info_y()); } } } @@ -357,18 +357,18 @@ struct PACKED log_Rate_Thread_Dt { }; // Write a Guided mode position target -// pos_target_m is lat, lon, alt OR offset from ekf origin in m -// terrain should be 0 if pos_target_m.z is alt-above-ekf-origin, 1 if alt-above-terrain +// pos_target_ned_m is lat, lon, alt OR offset from ekf origin in m +// terrain should be 0 if pos_target_ned_m.z is alt-above-ekf-origin, 1 if alt-above-terrain // vel_target_ms is m/s -void Copter::Log_Write_Guided_Position_Target(ModeGuided::SubMode submode, const Vector3p& pos_target_m, bool is_terrain_alt, const Vector3f& vel_target_ms, const Vector3f& accel_target_mss) +void Copter::Log_Write_Guided_Position_Target(ModeGuided::SubMode submode, const Vector3p& pos_target_ned_m, bool is_terrain_alt, const Vector3f& vel_target_ms, const Vector3f& accel_target_mss) { const log_Guided_Position_Target pkt { LOG_PACKET_HEADER_INIT(LOG_GUIDED_POSITION_TARGET_MSG), time_us : AP_HAL::micros64(), type : (uint8_t)submode, - pos_target_x : (float)pos_target_m.x, - pos_target_y : (float)pos_target_m.y, - pos_target_z : (float)pos_target_m.z, + pos_target_x : (float)pos_target_ned_m.x, + pos_target_y : (float)pos_target_ned_m.y, + pos_target_z : (float)pos_target_ned_m.z, terrain : is_terrain_alt, vel_target_x : vel_target_ms.x, vel_target_y : vel_target_ms.y, diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index d283fc415ff90..d71252af221ee 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -76,15 +76,6 @@ const AP_Param::Info Copter::var_info[] = { GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 0), #if MODE_RTL_ENABLED - // @Param: RTL_ALT - // @DisplayName: RTL Altitude - // @Description: The minimum alt above home the vehicle will climb to before returning. If the vehicle is flying higher than this value it will return at its current altitude. - // @Units: cm - // @Range: 30 300000 - // @Increment: 1 - // @User: Standard - GSCALAR(rtl_altitude_cm, "RTL_ALT", RTL_ALT), - // @Param: RTL_CONE_SLOPE // @DisplayName: RTL cone slope // @Description: Defines a cone above home which determines maximum climb @@ -94,33 +85,6 @@ const AP_Param::Info Copter::var_info[] = { // @User: Standard GSCALAR(rtl_cone_slope, "RTL_CONE_SLOPE", RTL_CONE_SLOPE_DEFAULT), - // @Param: RTL_SPEED - // @DisplayName: RTL speed - // @Description: Defines the speed in cm/s which the aircraft will attempt to maintain horizontally while flying home. If this is set to zero, WPNAV_SPEED will be used instead. - // @Units: cm/s - // @Range: 0 2000 - // @Increment: 50 - // @User: Standard - GSCALAR(rtl_speed_cms, "RTL_SPEED", 0), - - // @Param: RTL_ALT_FINAL - // @DisplayName: RTL Final Altitude - // @Description: This is the altitude the vehicle will move to as the final stage of Returning to Launch or after completing a mission. Set to zero to land. - // @Units: cm - // @Range: 0 1000 - // @Increment: 1 - // @User: Standard - GSCALAR(rtl_alt_final_cm, "RTL_ALT_FINAL", RTL_ALT_FINAL), - - // @Param: RTL_CLIMB_MIN - // @DisplayName: RTL minimum climb - // @Description: The vehicle will climb this many cm during the initial climb portion of the RTL - // @Units: cm - // @Range: 0 3000 - // @Increment: 10 - // @User: Standard - GSCALAR(rtl_climb_min_cm, "RTL_CLIMB_MIN", RTL_CLIMB_MIN_DEFAULT), - // @Param: RTL_LOIT_TIME // @DisplayName: RTL loiter time // @Description: Time (in milliseconds) to loiter above home before beginning final descent @@ -165,24 +129,6 @@ const AP_Param::Info Copter::var_info[] = { // @Values: 0:Never change yaw, 1:Face next waypoint, 2:Face next waypoint except RTL, 3:Face along GPS course // @User: Standard GSCALAR(wp_yaw_behavior, "WP_YAW_BEHAVIOR", WP_YAW_BEHAVIOR_DEFAULT), - - // @Param: LAND_SPEED - // @DisplayName: Land speed - // @Description: The descent speed for the final stage of landing in cm/s - // @Units: cm/s - // @Range: 30 200 - // @Increment: 10 - // @User: Standard - GSCALAR(land_speed_cms, "LAND_SPEED", LAND_SPEED), - - // @Param: LAND_SPEED_HIGH - // @DisplayName: Land speed high - // @Description: The descent speed for the first stage of landing in cm/s. If this is zero then WPNAV_SPEED_DN is used - // @Units: cm/s - // @Range: 0 500 - // @Increment: 10 - // @User: Standard - GSCALAR(land_speed_high_cms, "LAND_SPEED_HIGH", 0), // @Param: PILOT_SPEED_UP // @DisplayName: Pilot maximum vertical speed ascending @@ -191,7 +137,7 @@ const AP_Param::Info Copter::var_info[] = { // @Range: 50 500 // @Increment: 10 // @User: Standard - GSCALAR(pilot_speed_up_cms, "PILOT_SPEED_UP", PILOT_VELZ_MAX), + GSCALAR(pilot_speed_up_cms, "PILOT_SPEED_UP", PILOT_SPEED_UP_DEFAULT), // @Param: PILOT_ACCEL_Z // @DisplayName: Pilot vertical acceleration @@ -200,7 +146,7 @@ const AP_Param::Info Copter::var_info[] = { // @Range: 50 500 // @Increment: 10 // @User: Standard - GSCALAR(pilot_accel_u_cmss, "PILOT_ACCEL_Z", PILOT_ACCEL_Z_DEFAULT), + GSCALAR(pilot_accel_d_cmss, "PILOT_ACCEL_Z", PILOT_ACCEL_Z_DEFAULT), // @Param: FS_THR_ENABLE // @DisplayName: Throttle Failsafe Enable @@ -366,7 +312,7 @@ const AP_Param::Info Copter::var_info[] = { // @Param: FS_EKF_ACTION // @DisplayName: EKF Failsafe Action // @Description: Controls the action that will be taken when an EKF failsafe is invoked - // @Values: 0:Report only, 1:Switch to Land mode if current mode requires postion, 2:Switch to AltHold mode if current mode requires postion, 3:Switch to Land mode from all modes + // @Values: 0:Report only, 1:Switch to Land mode if current mode requires position, 2:Switch to AltHold mode if current mode requires position, 3:Switch to Land mode from all modes // @User: Advanced GSCALAR(fs_ekf_action, "FS_EKF_ACTION", FS_EKF_ACTION_DEFAULT), @@ -829,14 +775,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Standard AP_GROUPINFO("PILOT_SPEED_DN", 24, ParametersG2, pilot_speed_dn_cms, 0), - // @Param: LAND_ALT_LOW - // @DisplayName: Land alt low - // @Description: Altitude during Landing at which vehicle slows to LAND_SPEED - // @Units: cm - // @Range: 100 10000 - // @Increment: 10 - // @User: Advanced - AP_GROUPINFO("LAND_ALT_LOW", 25, ParametersG2, land_alt_low_cm, 1000), + // 25 was LAND_ALT_LOW #if MODE_FLOWHOLD_ENABLED // @Group: FHLD @@ -1154,7 +1093,7 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = { // @Param: PLDP_SPEED_DN // @DisplayName: Payload Place decent speed - // @Description: The maximum vertical decent velocity in m/s. If 0 LAND_SPEED value is used. + // @Description: The maximum vertical decent velocity in m/s. If 0 LAND_SPD_MS value is used. // @Units: m/s // @Range: 0 5 // @User: Standard @@ -1228,6 +1167,16 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = { AP_GROUPINFO("TUNE2", 13, ParametersG2, rc_tuning2_param, 0), #endif // AP_RC_TRANSMITTER_TUNING_ENABLED +#if MODE_RTL_ENABLED + // @Group: RTL_ + // @Path: mode_rtl.cpp + AP_SUBGROUPPTR(mode_rtl_ptr, "RTL_", 14, ParametersG2, ModeRTL), +#endif + + // @Group: LAND_ + // @Path: mode_land.cpp + AP_SUBGROUPPTR(mode_land_ptr, "LAND_", 15, ParametersG2, ModeLand), + // ID 62 is reserved for the AP_SUBGROUPEXTENSION AP_GROUPEND @@ -1290,6 +1239,10 @@ ParametersG2::ParametersG2(void) : #if WEATHERVANE_ENABLED ,weathervane() #endif +#if MODE_RTL_ENABLED + ,mode_rtl_ptr(&copter.mode_rtl) +#endif + ,mode_land_ptr(&copter.mode_land) { AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info2); @@ -1299,11 +1252,6 @@ void Copter::load_parameters(void) { AP_Vehicle::load_parameters(g.format_version, Parameters::k_format_version); -#if MODE_RTL_ENABLED - // PARAMETER_CONVERSION - Added: Sep-2021 - g.rtl_altitude_cm.convert_parameter_width(AP_PARAM_INT16); -#endif - // PARAMETER_CONVERSION - Added: Mar-2022 #if AP_FENCE_ENABLED AP_Param::convert_class(g.k_param_fence_old, &fence, fence.var_info, 0, true); @@ -1359,6 +1307,14 @@ void Copter::load_parameters(void) } #endif // HAL_GCS_ENABLED +#if MODE_RTL_ENABLED + // convert RTL parameters + copter.mode_rtl.convert_params(); +#endif + + // convert LAND parameters + copter.mode_land.convert_params(); + // setup AP_Param frame type flags AP_Param::set_frame_type_flags(AP_PARAM_FRAME_COPTER); } @@ -1368,7 +1324,7 @@ void Copter::convert_pid_parameters(void) { const AP_Param::ConversionInfo angle_and_filt_conversion_info[] = { // PARAMETER_CONVERSION - Added: Aug-2021 - { Parameters::k_param_pi_vel_xy, 3, AP_PARAM_FLOAT, "PSC_VELXY_FLTE" }, + { Parameters::k_param_pi_vel_xy, 3, AP_PARAM_FLOAT, "PSC_NE_VEL_FLTE" }, }; // convert angle controller gain and filter without scaling @@ -1386,10 +1342,10 @@ void Copter::convert_pid_parameters(void) { "LOIT_SPEED", 3000.0f }, { "PHLD_BRAKE_ANGLE", 800.0f }, { "PHLD_BRAKE_RATE", 4.0f }, - { "PSC_ACCZ_P", 0.28f }, - { "PSC_VELXY_D", 0.0f }, - { "PSC_VELXY_I", 0.5f }, - { "PSC_VELXY_P", 1.0f }, + { "PSC_D_ACC_P", 0.028f }, + { "PSC_NE_VEL_D", 0.0f }, + { "PSC_NE_VEL_I", 0.5f }, + { "PSC_NE_VEL_P", 1.0f }, { "RC8_OPTION", 32 }, { "RC_OPTIONS", 0 }, { "ATC_RAT_RLL_ILMI", 0.05}, diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 6cb3f22ed7b65..2661fe4a406ac 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -6,6 +6,8 @@ #include "RC_Channel_Copter.h" #include +class ModeRTL; + #if MODE_FOLLOW_ENABLED # include #endif @@ -122,7 +124,7 @@ class Parameters { k_param_rally, k_param_poshold_brake_rate_degs, k_param_poshold_brake_angle_max, - k_param_pilot_accel_u_cmss, + k_param_pilot_accel_d_cmss, k_param_serial0_baud, // deprecated - remove k_param_serial1_baud, // deprecated - remove k_param_serial2_baud, // deprecated - remove @@ -229,7 +231,7 @@ class Parameters { // // 135 : reserved for Solo until features merged with master // - k_param_rtl_speed_cms = 135, + k_param_rtl_speed_cms = 135, // remove k_param_fs_batt_curr_rtl, k_param_rtl_cone_slope, // 137 @@ -259,10 +261,10 @@ class Parameters { // // 160: Navigation parameters // - k_param_rtl_altitude_cm = 160, + k_param_rtl_altitude_cm = 160, // remove k_param_crosstrack_gain, // deprecated - remove with next eeprom number change k_param_rtl_loiter_time, - k_param_rtl_alt_final_cm, + k_param_rtl_alt_final_cm, // remove k_param_tilt_comp, // 164 deprecated - remove with next eeprom number change @@ -334,10 +336,10 @@ class Parameters { k_param_waypoint_radius, // remove k_param_circle_radius, // remove k_param_waypoint_speed_max, // remove - k_param_land_speed_cms, + k_param_land_speed_cms, // remove k_param_auto_velocity_z_min, // remove k_param_auto_velocity_z_max, // remove - 219 - k_param_land_speed_high_cms, + k_param_land_speed_high_cms, // remove // // 220: PI/D Controllers @@ -370,7 +372,7 @@ class Parameters { k_param_autotune_aggressiveness, // remove k_param_pi_vel_xy, // remove k_param_fs_ekf_action, - k_param_rtl_climb_min_cm, + k_param_rtl_climb_min_cm, // remove k_param_rpm_sensor_old, // remove k_param_autotune_min_d, // remove k_param_arming, // 252 - AP_Arming @@ -396,11 +398,7 @@ class Parameters { AP_Float pilot_takeoff_alt_cm; #if MODE_RTL_ENABLED - AP_Int32 rtl_altitude_cm; - AP_Int16 rtl_speed_cms; AP_Float rtl_cone_slope; - AP_Int16 rtl_alt_final_cm; - AP_Int16 rtl_climb_min_cm; // rtl minimum climb in cm AP_Int32 rtl_loiter_time; AP_Enum rtl_alt_type; #endif @@ -419,10 +417,8 @@ class Parameters { // Waypoints // - AP_Int16 land_speed_cms; - AP_Int16 land_speed_high_cms; AP_Int16 pilot_speed_up_cms; // maximum vertical ascending velocity the pilot may request - AP_Int16 pilot_accel_u_cmss; // vertical acceleration the pilot may request + AP_Int16 pilot_accel_d_cmss; // vertical acceleration the pilot may request // Throttle // @@ -569,9 +565,6 @@ class ParametersG2 { // Additional pilot velocity items AP_Int16 pilot_speed_dn_cms; - // Land alt final stage - AP_Int16 land_alt_low_cm; - #if TOY_MODE_ENABLED ToyMode toy_mode; #endif @@ -701,6 +694,12 @@ class ParametersG2 { AP_Float rc_tuning2_min; AP_Float rc_tuning2_max; #endif // AP_RC_TRANSMITTER_TUNING_ENABLED + +#if MODE_RTL_ENABLED + void *mode_rtl_ptr; +#endif + + void *mode_land_ptr; }; extern const AP_Param::Info var_info[]; diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp index d91481ca5575c..0196fbebf92f2 100644 --- a/ArduCopter/autoyaw.cpp +++ b/ArduCopter/autoyaw.cpp @@ -2,12 +2,12 @@ Mode::AutoYaw Mode::auto_yaw; -// roi_yaw_rad - returns heading towards location held in roi_neu_m +// roi_yaw_rad - returns heading towards location held in roi_ned_m float Mode::AutoYaw::roi_yaw_rad() const { Vector2f pos_ne_m; if (AP::ahrs().get_relative_position_NE_origin_float(pos_ne_m)){ - return get_bearing_rad(pos_ne_m, roi_neu_m.xy()); + return get_bearing_rad(pos_ne_m, roi_ned_m.xy()); } return copter.attitude_control->get_att_target_euler_rad().z; } @@ -168,7 +168,7 @@ void Mode::AutoYaw::set_yaw_angle_offset_deg(const float yaw_angle_offset_deg) set_mode(Mode::ANGLE_RATE); } -// set_roi - sets the yaw to look at roi_neu_m for auto mode +// set_roi - sets the yaw to look at roi_ned_m for auto mode void Mode::AutoYaw::set_roi(const Location &roi_location) { // if location is zero lat, lon and altitude turn off ROI @@ -183,7 +183,7 @@ void Mode::AutoYaw::set_roi(const Location &roi_location) #if HAL_MOUNT_ENABLED // check if mount type requires us to rotate the quad if (!copter.camera_mount.has_pan_control()) { - if (roi_location.get_vector_from_origin_NEU_m(roi_neu_m)) { + if (roi_location.get_vector_from_origin_NED_m(roi_ned_m)) { auto_yaw.set_mode(Mode::ROI); } } @@ -198,7 +198,7 @@ void Mode::AutoYaw::set_roi(const Location &roi_location) // 4: point at a target given a target id (can't be implemented) #else // if we have no camera mount aim the quad at the location - if (roi_location.get_vector_from_origin_NEU_m(roi_neu_m)) { + if (roi_location.get_vector_from_origin_NED_m(roi_ned_m)) { auto_yaw.set_mode(Mode::ROI); } #endif // HAL_MOUNT_ENABLED @@ -235,7 +235,7 @@ float Mode::AutoYaw::yaw_rad() switch (_mode) { case Mode::ROI: - // point towards a location held in roi_neu_m + // point towards a location held in roi_ned_m _yaw_angle_rad = roi_yaw_rad(); break; diff --git a/ArduCopter/avoidance_adsb.cpp b/ArduCopter/avoidance_adsb.cpp index fd4a879a123dc..77cb44ffe4048 100644 --- a/ArduCopter/avoidance_adsb.cpp +++ b/ArduCopter/avoidance_adsb.cpp @@ -156,8 +156,8 @@ void AP_Avoidance_Copter::set_mode_else_try_RTL_else_LAND(Mode::Number mode) float AP_Avoidance_Copter::get_altitude_minimum_m() const { #if MODE_RTL_ENABLED - // do not descend if below RTL alt - return copter.g.rtl_altitude_cm * 0.01; + // do not descend if below RTL_ALT_M + return copter.mode_rtl.get_altitude_m(); #else return 0; #endif diff --git a/ArduCopter/baro_ground_effect.cpp b/ArduCopter/baro_ground_effect.cpp index ad139e66f4f6f..07c3f8d2706a9 100644 --- a/ArduCopter/baro_ground_effect.cpp +++ b/ArduCopter/baro_ground_effect.cpp @@ -16,9 +16,8 @@ void Copter::update_ground_effect_detector(void) float des_speed_ne_ms = 0.0f; float des_climb_rate_ms = pos_control->get_vel_desired_U_ms(); - if (pos_control->is_active_NE()) { - Vector3f vel_target_neu_ms = pos_control->get_vel_target_NEU_ms(); - des_speed_ne_ms = vel_target_neu_ms.xy().length(); + if (pos_control->NE_is_active()) { + des_speed_ne_ms = pos_control->get_vel_target_NED_ms().xy().length(); } // takeoff logic @@ -53,10 +52,10 @@ void Copter::update_ground_effect_detector(void) bool small_angle_request = cosf(angle_target_rad.x) * cosf(angle_target_rad.y) > cosf(radians(7.5f)); Vector3f vel_ned_ms; bool xy_speed_low = AP::ahrs().get_velocity_NED(vel_ned_ms) && (vel_ned_ms.xy().length() < 1.25); - bool xy_speed_demand_low = pos_control->is_active_NE() && des_speed_ne_ms <= 1.25; - bool slow_horizontal = xy_speed_demand_low || (xy_speed_low && !pos_control->is_active_NE()) || (flightmode->mode_number() == Mode::Number::ALT_HOLD && small_angle_request); + bool xy_speed_demand_low = pos_control->NE_is_active() && des_speed_ne_ms <= 1.25; + bool slow_horizontal = xy_speed_demand_low || (xy_speed_low && !pos_control->NE_is_active()) || (flightmode->mode_number() == Mode::Number::ALT_HOLD && small_angle_request); - bool descent_demanded = pos_control->is_active_U() && des_climb_rate_ms < 0.0f; + bool descent_demanded = pos_control->D_is_active() && des_climb_rate_ms < 0.0f; bool slow_descent_demanded = descent_demanded && des_climb_rate_ms >= -1.00; bool speed_low_d_ms = AP::ahrs().get_velocity_D(vel_ned_ms.z, vibration_check.high_vibes) && fabsf(vel_ned_ms.z) <= 0.6f; bool slow_descent = (slow_descent_demanded || (speed_low_d_ms && descent_demanded)); diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 2a2d298edddb0..c3aa363a58e21 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -211,7 +211,7 @@ ////////////////////////////////////////////////////////////////////////////// // Sport - fly vehicle in rate-controlled (earth-frame) mode #ifndef MODE_SPORT_ENABLED -# define MODE_SPORT_ENABLED 0 +# define MODE_SPORT_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif ////////////////////////////////////////////////////////////////////////////// @@ -318,8 +318,8 @@ ////////////////////////////////////////////////////////////////////////////// // Landing // -#ifndef LAND_SPEED - # define LAND_SPEED 50 // the descent speed for the final stage of landing in cm/s +#ifndef LAND_SPD_MS_DEFAULT + # define LAND_SPD_MS_DEFAULT 0.5f // the descent speed for the final stage of landing in m/s #endif #ifndef LAND_REPOSITION_DEFAULT # define LAND_REPOSITION_DEFAULT 1 // by default the pilot can override roll/pitch during landing @@ -334,6 +334,11 @@ #define LAND_RANGEFINDER_MIN_ALT_M 2.0 #endif +// error if old LAND parameter default definitions are used +#ifdef LAND_SPEED + #error "LAND_SPEED definition replaced with LAND_SPD_MS_DEFAULT" +#endif + ////////////////////////////////////////////////////////////////////////////// // Landing Detector // @@ -406,20 +411,20 @@ #endif // RTL Mode -#ifndef RTL_ALT_FINAL - # define RTL_ALT_FINAL 0 // the altitude, in cm, the vehicle will move to as the final stage of Returning to Launch. Set to zero to land. +#ifndef RTL_ALT_FINAL_M_DEFAULT + # define RTL_ALT_FINAL_M_DEFAULT 0 // the altitude, in meters, the vehicle will move to as the final stage of Returning to Launch. Set to zero to land. #endif -#ifndef RTL_ALT - # define RTL_ALT 1500 // default alt to return to home in cm, 0 = Maintain current altitude +#ifndef RTL_ALT_M_DEFAULT + # define RTL_ALT_M_DEFAULT 15 // default alt to return to home in meters, 0 = Maintain current altitude #endif #ifndef RTL_ALT_MIN_M - # define RTL_ALT_MIN_M 0.30 // min height above ground for RTL (i.e 0.3 m) + # define RTL_ALT_MIN_M 0.30 // min height above ground for RTL (i.e 0.3 m) #endif -#ifndef RTL_CLIMB_MIN_DEFAULT - # define RTL_CLIMB_MIN_DEFAULT 0 // vehicle will always climb this many cm as first stage of RTL +#ifndef RTL_CLIMB_MIN_M_DEFAULT + # define RTL_CLIMB_MIN_M_DEFAULT 0 // vehicle will always climb this many meters during the first stage of RTL #endif #ifndef RTL_CONE_SLOPE_DEFAULT @@ -434,6 +439,17 @@ # define RTL_LOITER_TIME 5000 // Time (in milliseconds) to loiter above home before beginning final descent #endif +// error if old RTL parameter default definitions are used +#ifdef RTL_ALT_FINAL + #error "RTL_ALT_FINAL definition replaced with RTL_ALT_FINAL_M_DEFAULT" +#endif +#ifdef RTL_ALT + #error "RTL_ALT definition replaced with RTL_ALT_M_DEFAULT" +#endif +#ifdef RTL_CLIMB_MIN_DEFAULT + #error "RTL_CLIMB_MIN_DEFAULT definition replaced with RTL_CLIMB_MIN_M_DEFAULT" +#endif + // AUTO Mode #ifndef WP_YAW_BEHAVIOR_DEFAULT # define WP_YAW_BEHAVIOR_DEFAULT WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL @@ -488,8 +504,8 @@ #endif // default maximum vertical velocity and acceleration the pilot may request -#ifndef PILOT_VELZ_MAX - # define PILOT_VELZ_MAX 250 // maximum vertical velocity in cm/s +#ifndef PILOT_SPEED_UP_DEFAULT + # define PILOT_SPEED_UP_DEFAULT 250 // maximum vertical velocity in cm/s #endif #ifndef PILOT_ACCEL_Z_DEFAULT # define PILOT_ACCEL_Z_DEFAULT 250 // vertical acceleration in cm/s/s while altitude is under pilot control diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index 40bda04ecb7f8..b50c7d104d16a 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -16,7 +16,7 @@ void Copter::heli_init() { // pre-load stab col values as mode is initialized as Stabilize, but stabilize_init() function is not run on start-up. input_manager.set_use_stab_col(true); - input_manager.set_stab_col_ramp(1.0); + input_manager.set_collective_ramp(0); } // heli_check_dynamic_flight - updates the dynamic_flight flag based on our horizontal velocity diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index a5a4d37231781..3f2043fcfab1d 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -315,7 +315,7 @@ void Copter::update_throttle_mix() const bool landing = flightmode->is_landing(); if (((large_angle_request || get_force_flying()) && !landing) || large_angle_error || accel_moving || descent_not_demanded) { - attitude_control->set_throttle_mix_max(pos_control->get_vel_U_control_ratio()); + attitude_control->set_throttle_mix_max(pos_control->get_vel_D_control_ratio()); } else { attitude_control->set_throttle_mix_min(); } diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 6fca5f9cc2569..5104b7f368af8 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -443,15 +443,13 @@ void Copter::exit_mode(Mode *&old_flightmode, motors->set_acro_tail(false); } + //last collective output + input_manager.set_last_coll_output(motors->get_throttle()); + // if we are changing from a mode that did not use manual throttle, - // stab col ramp value should be pre-loaded to the correct value to avoid a twitch - // heli_stab_col_ramp should really only be active switching between Stabilize and Acro modes - if (!old_flightmode->has_manual_throttle()){ - if (new_flightmode == &mode_stabilize){ - input_manager.set_stab_col_ramp(1.0); - } else if (new_flightmode == &mode_acro){ - input_manager.set_stab_col_ramp(0.0); - } + // collective ramp functions should be called to blend the transition + if (new_flightmode->has_manual_throttle()) { + input_manager.set_collective_ramp(1.0); } // Make sure inverted flight is disabled if not supported in the new mode @@ -587,10 +585,10 @@ void Mode::make_safe_ground_handling(bool force_throttle_unlimited) break; } - pos_control->relax_velocity_controller_NE(); - pos_control->update_NE_controller(); - pos_control->relax_U_controller(0.0f); // forces throttle output to decay to zero - pos_control->update_U_controller(); + pos_control->NE_relax_velocity_controller(); + pos_control->NE_update_controller(); + pos_control->D_relax_controller(0.0f); // forces throttle output to decay to zero + pos_control->D_update_controller(); // we may need to move this out attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(0.0f, 0.0f, 0.0f); } @@ -623,26 +621,29 @@ void Mode::land_run_vertical_control(bool pause_descent) if (!pause_descent) { // do not ignore limits until we have slowed down for landing - ignore_descent_limit = (MAX(g2.land_alt_low_cm, 100) * 0.01 > get_alt_above_ground_m()) || copter.ap.land_complete_maybe; + const float land_alt_low_m = copter.mode_land.get_land_alt_low_m(); + ignore_descent_limit = (MAX(land_alt_low_m, 1) > get_alt_above_ground_m()) || copter.ap.land_complete_maybe; float max_land_descent_speed_ms; - if (g.land_speed_high_cms > 0) { - max_land_descent_speed_ms = g.land_speed_high_cms * 0.01; + const float land_speed_high_ms = copter.mode_land.get_land_speed_high_ms(); + if (land_speed_high_ms > 0) { + max_land_descent_speed_ms = land_speed_high_ms; } else { max_land_descent_speed_ms = pos_control->get_max_speed_down_ms(); } // Don't speed up for landing. - max_land_descent_speed_ms = MAX(max_land_descent_speed_ms, abs(g.land_speed_cms) * 0.01); + const float land_speed_ms = copter.mode_land.get_land_speed_ms(); + max_land_descent_speed_ms = MAX(max_land_descent_speed_ms, fabsf(land_speed_ms)); - // Compute a vertical velocity demand such that the vehicle approaches g2.land_alt_low_cm. Without the below constraint, this would cause the vehicle to hover at g2.land_alt_low_cm. - climb_rate_ms = sqrt_controller(MAX(g2.land_alt_low_cm, 100) * 0.01 - get_alt_above_ground_m(), pos_control->get_pos_U_p().kP(), pos_control->get_max_accel_U_mss(), G_Dt); + // Compute a vertical velocity demand such that the vehicle approaches land_alt_low. Without the below constraint, this would cause the vehicle to hover at land_alt_low. + climb_rate_ms = sqrt_controller(MAX(land_alt_low_m, 1) - get_alt_above_ground_m(), pos_control->D_get_pos_p().kP(), pos_control->D_get_max_accel_mss(), G_Dt); // Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed. - climb_rate_ms = constrain_float(climb_rate_ms, -max_land_descent_speed_ms, -abs(g.land_speed_cms) * 0.01); + climb_rate_ms = constrain_float(climb_rate_ms, -max_land_descent_speed_ms, -fabsf(land_speed_ms)); #if AC_PRECLAND_ENABLED - const bool navigating = pos_control->is_active_NE(); + const bool navigating = pos_control->NE_is_active(); bool doing_precision_landing = !copter.ap.land_repo_active && copter.precland.target_acquired() && navigating; if (doing_precision_landing) { @@ -650,7 +651,7 @@ void Mode::land_run_vertical_control(bool pause_descent) Vector2p target_pos_ne_m; float target_error_m = 0.0f; if (copter.precland.get_target_position_m(target_pos_ne_m)) { - const Vector2p current_pos_ne_m = pos_control->get_pos_estimate_NEU_m().xy(); + const Vector2p current_pos_ne_m = pos_control->get_pos_estimate_NED_m().xy(); // target is this many m away from the vehicle target_error_m = (target_pos_ne_m - current_pos_ne_m).tofloat().length(); } @@ -667,7 +668,7 @@ void Mode::land_run_vertical_control(bool pause_descent) // compute desired descent velocity const float precland_acceptable_error_m = 0.15; const float precland_min_descent_speed_ms = 0.1; - const float max_descent_speed_ms = abs(g.land_speed_cms) * 0.005; + const float max_descent_speed_ms = fabsf(land_speed_ms) * 0.5; const float land_slowdown_ms = MAX(0.0f, target_error_m * (max_descent_speed_ms / precland_acceptable_error_m)); climb_rate_ms = MIN(-precland_min_descent_speed_ms, -max_descent_speed_ms + land_slowdown_ms); } @@ -676,8 +677,8 @@ void Mode::land_run_vertical_control(bool pause_descent) } // update altitude target and call position controller - pos_control->land_at_climb_rate_ms(climb_rate_ms, ignore_descent_limit); - pos_control->update_U_controller(); + pos_control->D_set_pos_target_from_climb_rate_ms(climb_rate_ms, ignore_descent_limit); + pos_control->D_update_controller(); } void Mode::land_run_horizontal_control() @@ -686,7 +687,7 @@ void Mode::land_run_horizontal_control() // relax loiter target if we might be landed if (copter.ap.land_complete_maybe) { - pos_control->soften_for_landing_NE(); + pos_control->NE_soften_for_landing(); } // process pilot inputs @@ -735,10 +736,10 @@ void Mode::land_run_horizontal_control() Vector2p target_pos_ne_m; Vector2f target_vel_ne_ms; if (!copter.precland.get_target_position_m(target_pos_ne_m)) { - target_pos_ne_m = pos_control->get_pos_estimate_NEU_m().xy(); + target_pos_ne_m = pos_control->get_pos_estimate_NED_m().xy(); } // get the velocity of the target - copter.precland.get_target_velocity_ms(pos_control->get_vel_estimate_NEU_ms().xy(), target_vel_ne_ms); + copter.precland.get_target_velocity_ms(pos_control->get_vel_estimate_NED_ms().xy(), target_vel_ne_ms); Vector2f accel_zero; // target vel will remain zero if landing target is stationary @@ -752,7 +753,7 @@ void Mode::land_run_horizontal_control() } // run pos controller - pos_control->update_NE_controller(); + pos_control->NE_update_controller(); Vector3f thrust_vector = pos_control->get_thrust_vector(); // call attitude controller @@ -812,12 +813,11 @@ void Mode::precland_retry_position(const Vector3p &retry_pos_ned_m) } } - Vector3p retry_pos_neu_m{retry_pos_ned_m.x, retry_pos_ned_m.y, -retry_pos_ned_m.z}; - pos_control->input_pos_NEU_m(retry_pos_neu_m, 0.0f, 10.0); + pos_control->input_pos_NED_m(retry_pos_ned_m, 0.0f, 10.0); // run position controllers - pos_control->update_NE_controller(); - pos_control->update_U_controller(); + pos_control->NE_update_controller(); + pos_control->D_update_controller(); // call attitude controller attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading()); @@ -909,7 +909,7 @@ float Mode::get_avoidance_adjusted_climbrate_ms(float target_rate_ms) { #if AP_AVOIDANCE_ENABLED float target_rate_cms = target_rate_ms * 100.0; - AP::ac_avoid()->adjust_velocity_z(pos_control->get_pos_U_p().kP(), pos_control->get_max_accel_U_mss() * 100.0, target_rate_cms, G_Dt); + AP::ac_avoid()->adjust_velocity_z(pos_control->D_get_pos_p().kP(), pos_control->D_get_max_accel_mss() * 100.0, target_rate_cms, G_Dt); return target_rate_cms * 0.01; #else return target_rate_ms; @@ -922,7 +922,7 @@ void Mode::output_to_motors() motors->output(); } -Mode::AltHoldModeState Mode::get_alt_hold_state_U_ms(float target_climb_rate_ms) +Mode::AltHoldModeState Mode::get_alt_hold_state_D_ms(float target_climb_rate_ms) { // Alt Hold State Machine Determination if (!motors->armed()) { @@ -992,55 +992,65 @@ float Mode::get_pilot_desired_yaw_rate_rads() const // pass-through functions to reduce code churn on conversion; // these are candidates for moving into the Mode base // class. + +// Returns the pilot’s commanded climb rate in m/s. float Mode::get_pilot_desired_climb_rate_ms() const { return copter.get_pilot_desired_climb_rate_ms(); } +// Returns half the hover throttle. float Mode::get_non_takeoff_throttle() const { return copter.get_non_takeoff_throttle(); } +// Updates simple/super-simple heading reference based on current yaw and mode. void Mode::update_simple_mode(void) { copter.update_simple_mode(); } +// Requests a mode change with the specified reason; returns true if accepted. bool Mode::set_mode(Mode::Number mode, ModeReason reason) { return copter.set_mode(mode, reason); } +// Sets the “land complete” state flag. void Mode::set_land_complete(bool b) { return copter.set_land_complete(b); } +// Returns a reference to the GCS interface for Copter. GCS_Copter &Mode::gcs() const { return copter.gcs(); } +// Returns the pilot’s maximum upward speed in m/s. float Mode::get_pilot_speed_up_ms() const { return g.pilot_speed_up_cms * 0.01; } +// Returns the pilot’s maximum downward speed in m/s. float Mode::get_pilot_speed_dn_ms() const { return copter.get_pilot_speed_dn() * 0.01; } -float Mode::get_pilot_accel_U_mss() const +// Returns the pilot’s vertical acceleration limit in m/s². +float Mode::get_pilot_accel_D_mss() const { - return g.pilot_accel_u_cmss * 0.01; + return g.pilot_accel_d_cmss * 0.01; } // Return stopping point as a location with above origin alt frame Location Mode::get_stopping_point() const { - Vector3p stopping_point_neu_m; - copter.pos_control->get_stopping_point_NE_m(stopping_point_neu_m.xy()); - copter.pos_control->get_stopping_point_U_m(stopping_point_neu_m.z); - return Location { stopping_point_neu_m.tofloat() * 100.0, Location::AltFrame::ABOVE_ORIGIN }; + Vector3p stopping_point_ned_m; + copter.pos_control->get_stopping_point_NE_m(stopping_point_ned_m.xy()); + copter.pos_control->get_stopping_point_D_m(stopping_point_ned_m.z); + return Location::from_ekf_offset_NED_m(stopping_point_ned_m, Location::AltFrame::ABOVE_ORIGIN); } diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index d1d4c9962deed..6f57f3ae2ebc0 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -20,7 +20,7 @@ class _AutoTakeoff { public: void run(); void start_m(float complete_alt_m, bool is_terrain_alt); - bool get_completion_pos_neu_m(Vector3p& pos_neu_m); + bool get_completion_pos_ned_m(Vector3p& pos_ned_m); bool complete; // true when takeoff is complete @@ -32,7 +32,7 @@ class _AutoTakeoff { // auto takeoff variables float complete_alt_m; // completion altitude expressed in m above ekf origin or above terrain (depending upon auto_takeoff_terrain_alt) bool is_terrain_alt; // true if altitudes are above terrain - Vector3p complete_pos_neu_m; // target takeoff position as offset from ekf origin in m + Vector3p complete_pos_ned_m; // target takeoff position as offset from ekf origin in m }; #if AC_PAYLOAD_PLACE_ENABLED @@ -256,7 +256,7 @@ class Mode { Landed_Pre_Takeoff, Flying }; - AltHoldModeState get_alt_hold_state_U_ms(float target_climb_rate_ms); + AltHoldModeState get_alt_hold_state_D_ms(float target_climb_rate_ms); // convenience references to avoid code churn in conversion: Parameters &g; @@ -287,7 +287,7 @@ class Mode { public: void start_m(float alt_m); void stop(); - void do_pilot_takeoff_ms(float& pilot_climb_rate_ms); + void do_pilot_takeoff_ms(float pilot_climb_rate_ms); bool triggered_ms(float target_climb_rate_ms) const; bool running() const { return _running; } @@ -313,7 +313,7 @@ class Mode { enum class Mode { HOLD = 0, // hold zero yaw rate LOOK_AT_NEXT_WP = 1, // point towards next waypoint (no pilot input accepted) - ROI = 2, // point towards a location held in roi_neu_m (no pilot input accepted) + ROI = 2, // point towards a location held in roi_ned_m (no pilot input accepted) FIXED = 3, // point towards a particular angle (no pilot input accepted) LOOK_AHEAD = 4, // point in the direction the copter is moving RESET_TO_ARMED_YAW = 5, // point towards heading at time motors were armed @@ -370,7 +370,7 @@ class Mode { Mode _last_mode; // Yaw will point at this location if mode is set to Mode::ROI - Vector3f roi_neu_m; + Vector3f roi_ned_m; // yaw used for YAW_FIXED yaw_mode float _fixed_yaw_offset_rad; @@ -394,15 +394,33 @@ class Mode { // pass-through functions to reduce code churn on conversion; // these are candidates for moving into the Mode base // class. + + // Returns the pilot’s commanded climb rate in m/s. float get_pilot_desired_climb_rate_ms() const; + + // Returns the throttle level to maintain altitude (excluding takeoff boost). float get_non_takeoff_throttle() const; + + // Updates simple/super-simple heading reference based on current yaw and mode. void update_simple_mode(); + + // Requests a mode change with the specified reason; returns true if accepted. bool set_mode(Mode::Number mode, ModeReason reason); + + // Sets the “land complete” state flag. void set_land_complete(bool b); + + // Returns a reference to the GCS interface for Copter. GCS_Copter &gcs() const; + + // Returns the pilot’s maximum upward speed in m/s. float get_pilot_speed_up_ms() const; + + // Returns the pilot’s maximum downward speed in m/s. float get_pilot_speed_dn_ms() const; - float get_pilot_accel_U_mss() const; + + // Returns the pilot’s vertical acceleration limit in m/s². + float get_pilot_accel_D_mss() const; // end pass-through functions }; @@ -443,7 +461,7 @@ class ModeAcro : public Mode { protected: - const char *name() const override { return "ACRO"; } + const char *name() const override { return "Acro"; } const char *name4() const override { return "ACRO"; } // get_pilot_desired_rates_rads - transform pilot's normalised roll pitch and yaw input into a desired lean angle rates @@ -500,7 +518,7 @@ class ModeAltHold : public Mode { #endif protected: - const char *name() const override { return "ALT_HOLD"; } + const char *name() const override { return "Altitude Hold"; } const char *name4() const override { return "ALTH"; } private: @@ -620,7 +638,7 @@ class ModeAuto : public Mode { protected: - const char *name() const override { return auto_RTL? "AUTO RTL" : "AUTO"; } + const char *name() const override { return auto_RTL? "Auto RTL" : "Auto"; } const char *name4() const override { return auto_RTL? "ARTL" : "AUTO"; } float wp_distance_m() const override; @@ -839,7 +857,7 @@ class ModeAutoTune : public Mode { protected: - const char *name() const override { return "AUTOTUNE"; } + const char *name() const override { return "Autotune"; } const char *name4() const override { return "ATUN"; } }; #endif @@ -864,7 +882,7 @@ class ModeBrake : public Mode { protected: - const char *name() const override { return "BRAKE"; } + const char *name() const override { return "Brake"; } const char *name4() const override { return "BRAK"; } private: @@ -892,7 +910,7 @@ class ModeCircle : public Mode { protected: - const char *name() const override { return "CIRCLE"; } + const char *name() const override { return "Circle"; } const char *name4() const override { return "CIRC"; } float wp_distance_m() const override; @@ -923,12 +941,12 @@ class ModeDrift : public Mode { protected: - const char *name() const override { return "DRIFT"; } + const char *name() const override { return "Drift"; } const char *name4() const override { return "DRIF"; } private: - float get_throttle_assist(float vel_u_ms, float pilot_throttle_scaled); + float get_throttle_assist(float vel_d_ms, float pilot_throttle_scaled); }; @@ -951,7 +969,7 @@ class ModeFlip : public Mode { protected: - const char *name() const override { return "FLIP"; } + const char *name() const override { return "Flip"; } const char *name4() const override { return "FLIP"; } private: @@ -1002,7 +1020,7 @@ class ModeFlowHold : public Mode { static const struct AP_Param::GroupInfo var_info[]; protected: - const char *name() const override { return "FLOWHOLD"; } + const char *name() const override { return "Flow Hold"; } const char *name4() const override { return "FHLD"; } private: @@ -1105,19 +1123,19 @@ class ModeGuided : public Mode { // IF false: climb_rate_ms_or_thrust represents climb_rate (m/s) void set_angle(const Quaternion &attitude_quat, const Vector3f &ang_vel, float climb_rate_ms_or_thrust, bool use_thrust); - bool set_pos_NEU_m(const Vector3p& pos_neu_m, bool use_yaw = false, float yaw_rad = 0.0, bool use_yaw_rate = false, float yaw_rate_rads = 0.0, bool yaw_relative = false, bool is_terrain_alt = false); + bool set_pos_NED_m(const Vector3p& pos_ned_m, bool use_yaw = false, float yaw_rad = 0.0, bool use_yaw_rate = false, float yaw_rate_rads = 0.0, bool yaw_relative = false, bool is_terrain_alt = false); bool set_destination(const Location& dest_loc, bool use_yaw = false, float yaw_rad = 0.0, bool use_yaw_rate = false, float yaw_rate_rads = 0.0, bool yaw_relative = false); bool get_wp(Location &loc) const override; - void set_accel_NEU_mss(const Vector3f& accel_neu_mss, bool use_yaw = false, float yaw_rad = 0.0, bool use_yaw_rate = false, float yaw_rate_rads = 0.0, bool yaw_relative = false, bool log_request = true); - void set_vel_NEU_ms(const Vector3f& vel_neu_ms, bool use_yaw = false, float yaw_rad = 0.0, bool use_yaw_rate = false, float yaw_rate_rads = 0.0, bool yaw_relative = false, bool log_request = true); - void set_vel_accel_NEU_m(const Vector3f& vel_neu_ms, const Vector3f& accel_neu_mss, bool use_yaw = false, float yaw_rad = 0.0, bool use_yaw_rate = false, float yaw_rate_rads = 0.0, bool yaw_relative = false, bool log_request = true); - bool set_pos_vel_NEU_m(const Vector3p& pos_neu_m, const Vector3f& vel_neu_ms, bool use_yaw = false, float yaw_rad = 0.0, bool use_yaw_rate = false, float yaw_rate_rads = 0.0, bool yaw_relative = false); - bool set_pos_vel_accel_NEU_m(const Vector3p& pos_neu_m, const Vector3f& vel_neu_ms, const Vector3f& accel_neu_mss, bool use_yaw = false, float yaw_rad = 0.0, bool use_yaw_rate = false, float yaw_rate_rads = 0.0, bool yaw_relative = false); + void set_accel_NED_mss(const Vector3f& accel_ned_mss, bool use_yaw = false, float yaw_rad = 0.0, bool use_yaw_rate = false, float yaw_rate_rads = 0.0, bool yaw_relative = false, bool log_request = true); + void set_vel_NED_ms(const Vector3f& vel_ned_ms, bool use_yaw = false, float yaw_rad = 0.0, bool use_yaw_rate = false, float yaw_rate_rads = 0.0, bool yaw_relative = false, bool log_request = true); + void set_vel_accel_NED_m(const Vector3f& vel_ned_ms, const Vector3f& accel_ned_mss, bool use_yaw = false, float yaw_rad = 0.0, bool use_yaw_rate = false, float yaw_rate_rads = 0.0, bool yaw_relative = false, bool log_request = true); + bool set_pos_vel_NED_m(const Vector3p& pos_ned_m, const Vector3f& vel_ned_ms, bool use_yaw = false, float yaw_rad = 0.0, bool use_yaw_rate = false, float yaw_rate_rads = 0.0, bool yaw_relative = false); + bool set_pos_vel_accel_NED_m(const Vector3p& pos_ned_m, const Vector3f& vel_ned_ms, const Vector3f& accel_ned_mss, bool use_yaw = false, float yaw_rad = 0.0, bool use_yaw_rate = false, float yaw_rate_rads = 0.0, bool yaw_relative = false); // get position, velocity and acceleration targets - const Vector3p& get_target_pos_NEU_m() const; - const Vector3f& get_target_vel_NEU_ms() const; - const Vector3f& get_target_accel_NEU_mss() const; + const Vector3p& get_target_pos_NED_m() const; + const Vector3f& get_target_vel_NED_ms() const; + const Vector3f& get_target_accel_NED_mss() const; // returns true if GUIDED_OPTIONS param suggests SET_ATTITUDE_TARGET's "thrust" field should be interpreted as thrust instead of climb rate bool set_attitude_target_provides_thrust() const; @@ -1171,7 +1189,7 @@ class ModeGuided : public Mode { protected: - const char *name() const override { return "GUIDED"; } + const char *name() const override { return "Guided"; } const char *name4() const override { return "GUID"; } float wp_distance_m() const override; @@ -1261,7 +1279,7 @@ class ModeGuidedNoGPS : public ModeGuided { protected: - const char *name() const override { return "GUIDED_NOGPS"; } + const char *name() const override { return "Guided No GPS"; } const char *name4() const override { return "GNGP"; } private: @@ -1272,8 +1290,8 @@ class ModeGuidedNoGPS : public ModeGuided { class ModeLand : public Mode { public: - // inherit constructor - using Mode::Mode; + // need a constructor for parameters + ModeLand(void); Number mode_number() const override { return Number::LAND; } bool init(bool ignore_checks) override; @@ -1297,9 +1315,20 @@ class ModeLand : public Mode { void set_land_pause(bool new_value) { land_pause = new_value; } + // parameter accessors + float get_land_speed_ms() const { return land_speed_ms.get(); } + float get_land_speed_high_ms() const { return land_speed_high_ms.get(); } + float get_land_alt_low_m() const { return land_alt_low_m.get(); } + + // convert parameters + void convert_params(); + + // mode specific parameter variable table + static const struct AP_Param::GroupInfo var_info[]; + protected: - const char *name() const override { return "LAND"; } + const char *name() const override { return "Land"; } const char *name4() const override { return "LAND"; } private: @@ -1309,6 +1338,11 @@ class ModeLand : public Mode { bool control_position; // true if we are using an external reference to control position + // parameters + AP_Float land_speed_ms; + AP_Float land_speed_high_ms; + AP_Float land_alt_low_m; + uint32_t land_start_time; bool land_pause; }; @@ -1342,7 +1376,7 @@ class ModeLoiter : public Mode { protected: - const char *name() const override { return "LOITER"; } + const char *name() const override { return "Loiter"; } const char *name4() const override { return "LOIT"; } float wp_distance_m() const override; @@ -1384,7 +1418,7 @@ class ModePosHold : public Mode { protected: - const char *name() const override { return "POSHOLD"; } + const char *name() const override { return "Position Hold"; } const char *name4() const override { return "PHLD"; } private: @@ -1452,8 +1486,8 @@ class ModePosHold : public Mode { class ModeRTL : public Mode { public: - // inherit constructor - using Mode::Mode; + // need a constructor for parameters + ModeRTL(void); Number mode_number() const override { return Number::RTL; } bool init(bool ignore_checks) override; @@ -1508,6 +1542,18 @@ class ModeRTL : public Mode { }; ModeRTL::RTLAltType get_alt_type() const; + // parameter accessors + float get_altitude_m() const { return altitude_m.get(); } + float get_speed_ms() const { return speed_ms.get(); } + float get_alt_final_m() const { return alt_final_m.get(); } + float get_climb_min_m() const { return climb_min_m.get(); } + + // convert parameters + void convert_params(); + + // mode specific parameter variable table + static const struct AP_Param::GroupInfo var_info[]; + protected: const char *name() const override { return "RTL"; } @@ -1535,11 +1581,16 @@ class ModeRTL : public Mode { void build_path(); void compute_return_target(); + // RTL parameters + AP_Float altitude_m; + AP_Float speed_ms; + AP_Float alt_final_m; + AP_Float climb_min_m; + SubMode _state = SubMode::INITIAL_CLIMB; // records state of rtl (initial climb, returning home, etc) bool _state_complete = false; // set to true if the current state is completed struct { - // NEU w/ Z element alt-above-ekf-origin unless use_terrain is true in which case Z element is alt-above-terrain Location origin_point; Location climb_target; Location return_target; @@ -1601,7 +1652,7 @@ class ModeSmartRTL : public ModeRTL { protected: - const char *name() const override { return "SMARTRTL"; } + const char *name() const override { return "Smart RTL"; } const char *name4() const override { return "SRTL"; } // for reporting to GCS @@ -1649,7 +1700,7 @@ class ModeSport : public Mode { protected: - const char *name() const override { return "SPORT"; } + const char *name() const override { return "Sport"; } const char *name4() const override { return "SPRT"; } private: @@ -1678,7 +1729,7 @@ class ModeStabilize : public Mode { protected: - const char *name() const override { return "STABILIZE"; } + const char *name() const override { return "Stabilize"; } const char *name4() const override { return "STAB"; } private: @@ -1728,7 +1779,7 @@ class ModeSystemId : public Mode { protected: - const char *name() const override { return "SYSTEMID"; } + const char *name() const override { return "SystemID"; } const char *name4() const override { return "SYSI"; } private: @@ -1811,7 +1862,7 @@ class ModeThrow : public Mode { protected: - const char *name() const override { return "THROW"; } + const char *name() const override { return "Throw"; } const char *name4() const override { return "THRW"; } private: @@ -1860,7 +1911,7 @@ class ModeTurtle : public Mode { bool allows_entry_in_rc_failsafe() const override { return false; } protected: - const char *name() const override { return "TURTLE"; } + const char *name() const override { return "Turtle"; } const char *name4() const override { return "TRTL"; } private: @@ -1899,7 +1950,7 @@ class ModeAvoidADSB : public ModeGuided { protected: - const char *name() const override { return "AVOID_ADSB"; } + const char *name() const override { return "Avoid ADSB"; } const char *name4() const override { return "AVOI"; } private: @@ -1927,7 +1978,7 @@ class ModeFollow : public ModeGuided { protected: - const char *name() const override { return "FOLLOW"; } + const char *name() const override { return "Follow"; } const char *name4() const override { return "FOLL"; } // for reporting to GCS @@ -1985,7 +2036,7 @@ class ModeZigZag : public Mode { protected: - const char *name() const override { return "ZIGZAG"; } + const char *name() const override { return "ZigZag"; } const char *name4() const override { return "ZIGZ"; } float wp_distance_m() const override; float wp_bearing_deg() const override; @@ -1998,12 +2049,12 @@ class ModeZigZag : public Mode { bool reached_destination(); bool calculate_next_dest_m(Destination ab_dest, bool use_wpnav_alt, Vector3p& next_dest, bool& is_terrain_alt) const; void spray(bool b); - bool calculate_side_dest_m(Vector3p& next_dest_neu_m, bool& is_terrain_alt) const; + bool calculate_side_dest_m(Vector3p& next_dest_ned_m, bool& is_terrain_alt) const; void move_to_side(); - Vector2p dest_A_ne_m; // in NEU frame in m relative to ekf origin - Vector2p dest_B_ne_m; // in NEU frame in m relative to ekf origin - Vector3p current_dest_neu_m; // current target destination (use for resume after suspending) + Vector2p dest_A_ne_m; // in NE frame in m relative to ekf origin + Vector2p dest_B_ne_m; // in NE frame in m relative to ekf origin + Vector3p current_dest_ned_m; // current target destination (use for resume after suspending) bool current_is_terr_alt; // parameters @@ -2057,7 +2108,7 @@ class ModeAutorotate : public Mode { protected: - const char *name() const override { return "AUTOROTATE"; } + const char *name() const override { return "Autorotate"; } const char *name4() const override { return "AROT"; } private: diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp index a504d6e97a87a..46dbd4a5bbfbe 100644 --- a/ArduCopter/mode_althold.cpp +++ b/ArduCopter/mode_althold.cpp @@ -10,13 +10,13 @@ bool ModeAltHold::init(bool ignore_checks) { // initialise the vertical position controller - if (!pos_control->is_active_U()) { - pos_control->init_U_controller(); + if (!pos_control->D_is_active()) { + pos_control->D_init_controller(); } // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_U_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_U_mss()); - pos_control->set_correction_speed_accel_U_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_U_mss()); + pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss()); + pos_control->D_set_correction_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss()); return true; } @@ -26,7 +26,7 @@ bool ModeAltHold::init(bool ignore_checks) void ModeAltHold::run() { // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_U_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_U_mss()); + pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss()); // apply SIMPLE mode transform to pilot inputs update_simple_mode(); @@ -43,7 +43,7 @@ void ModeAltHold::run() target_climb_rate_ms = constrain_float(target_climb_rate_ms, -get_pilot_speed_dn_ms(), get_pilot_speed_up_ms()); // Alt Hold State Machine Determination - AltHoldModeState althold_state = get_alt_hold_state_U_ms(target_climb_rate_ms); + AltHoldModeState althold_state = get_alt_hold_state_D_ms(target_climb_rate_ms); // Alt Hold State Machine switch (althold_state) { @@ -51,7 +51,7 @@ void ModeAltHold::run() case AltHoldModeState::MotorStopped: attitude_control->reset_rate_controller_I_terms(); attitude_control->reset_yaw_target_and_rate(false); - pos_control->relax_U_controller(0.0f); // forces throttle output to decay to zero + pos_control->D_relax_controller(0.0f); // forces throttle output to decay to zero break; case AltHoldModeState::Landed_Ground_Idle: @@ -60,7 +60,7 @@ void ModeAltHold::run() case AltHoldModeState::Landed_Pre_Takeoff: attitude_control->reset_rate_controller_I_terms_smoothly(); - pos_control->relax_U_controller(0.0f); // forces throttle output to decay to zero + pos_control->D_relax_controller(0.0f); // forces throttle output to decay to zero break; case AltHoldModeState::Takeoff: @@ -93,7 +93,7 @@ void ModeAltHold::run() #endif // Send the commanded climb rate to the position controller - pos_control->set_pos_target_U_from_climb_rate_ms(target_climb_rate_ms); + pos_control->D_set_pos_target_from_climb_rate_ms(target_climb_rate_ms); break; } @@ -101,5 +101,5 @@ void ModeAltHold::run() attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(target_roll_rad, target_pitch_rad, target_yaw_rate_rads); // run the vertical position controller and set output throttle - pos_control->update_U_controller(); + pos_control->D_update_controller(); } diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index b9bee6fd63218..3fd767cafbc22 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -369,11 +369,11 @@ bool ModeAuto::loiter_start() _mode = SubMode::LOITER; // calculate stopping point - Vector3p stopping_point_neu_m; - wp_nav->get_wp_stopping_point_NEU_m(stopping_point_neu_m); + Vector3p stopping_point_ned_m; + wp_nav->get_wp_stopping_point_NED_m(stopping_point_ned_m); // initialise waypoint controller target to stopping point - wp_nav->set_wp_destination_NEU_m(stopping_point_neu_m); + wp_nav->set_wp_destination_NED_m(stopping_point_ned_m); // hold yaw at current heading auto_yaw.set_mode(AutoYaw::Mode::HOLD); @@ -439,7 +439,7 @@ void ModeAuto::takeoff_start(const Location& dest_loc) auto_yaw.set_mode(AutoYaw::Mode::HOLD); // clear i term when we're taking off - pos_control->init_U_controller(); + pos_control->D_init_controller(); // initialise alt for WP_NAVALT_MIN and set completion alt auto_takeoff.start_m(alt_target_m, alt_target_terrain); @@ -453,15 +453,15 @@ bool ModeAuto::wp_start(const Location& dest_loc) { // init wpnav and set origin if transitioning from takeoff if (!wp_nav->is_active()) { - Vector3p stopping_point_neu_m; + Vector3p stopping_point_ned_m; if (_mode == SubMode::TAKEOFF) { - Vector3p takeoff_complete_pos_neu_m; - if (auto_takeoff.get_completion_pos_neu_m(takeoff_complete_pos_neu_m)) { - stopping_point_neu_m = takeoff_complete_pos_neu_m; + Vector3p takeoff_complete_pos_ned_m; + if (auto_takeoff.get_completion_pos_ned_m(takeoff_complete_pos_ned_m)) { + stopping_point_ned_m = takeoff_complete_pos_ned_m; } } float des_speed_xy_ms = is_positive(desired_speed_override_ms.xy) ? desired_speed_override_ms.xy : 0; - wp_nav->wp_and_spline_init_m(des_speed_xy_ms, stopping_point_neu_m); + wp_nav->wp_and_spline_init_m(des_speed_xy_ms, stopping_point_ned_m); // override speeds up and down if necessary if (is_positive(desired_speed_override_ms.up)) { @@ -492,21 +492,21 @@ bool ModeAuto::wp_start(const Location& dest_loc) void ModeAuto::land_start() { // set horizontal speed and acceleration limits - pos_control->set_max_speed_accel_NE_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); - pos_control->set_correction_speed_accel_NE_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); + pos_control->NE_set_max_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); + pos_control->NE_set_correction_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); // initialise the vertical position controller - if (!pos_control->is_active_NE()) { - pos_control->init_NE_controller(); + if (!pos_control->NE_is_active()) { + pos_control->NE_init_controller(); } // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_U_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_U_mss()); - pos_control->set_correction_speed_accel_U_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_U_mss()); + pos_control->D_set_max_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss()); + pos_control->D_set_correction_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss()); // initialise the vertical position controller - if (!pos_control->is_active_U()) { - pos_control->init_U_controller(); + if (!pos_control->D_is_active()) { + pos_control->D_init_controller(); } // initialise yaw @@ -543,14 +543,14 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi copter.circle_nav->set_rate_degs(current_rate); // check our distance from edge of circle - Vector3p circle_edge_neu_m; + Vector3p circle_edge_ned_m; float dist_to_edge_m; - copter.circle_nav->get_closest_point_on_circle_NEU_m(circle_edge_neu_m, dist_to_edge_m); + copter.circle_nav->get_closest_point_on_circle_NED_m(circle_edge_ned_m, dist_to_edge_m); // if more than 3m then fly to edge if (dist_to_edge_m > 3.0) { - // convert circle_edge_neu_m to Location - Location circle_edge(circle_edge_neu_m * 100.0, Location::AltFrame::ABOVE_ORIGIN); + // convert circle_edge_ned_m to Location + Location circle_edge = Location::from_ekf_offset_NED_m(circle_edge_ned_m, Location::AltFrame::ABOVE_ORIGIN); // convert altitude to same as command circle_edge.copy_alt_from(circle_center); @@ -562,7 +562,7 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi } // if we are outside the circle, point at the edge, otherwise hold yaw - const float dist_to_center_m = get_horizontal_distance(pos_control->get_pos_estimate_NEU_m().xy().tofloat(), copter.circle_nav->get_center_NEU_m().xy().tofloat()); + const float dist_to_center_m = get_horizontal_distance(pos_control->get_pos_estimate_NED_m().xy().tofloat(), copter.circle_nav->get_center_NED_m().xy().tofloat()); // 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) { @@ -586,7 +586,7 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi void ModeAuto::circle_start() { // initialise circle controller - copter.circle_nav->init_NEU_m(copter.circle_nav->get_center_NEU_m(), copter.circle_nav->center_is_terrain_alt(), copter.circle_nav->get_rate_degs()); + copter.circle_nav->init_NED_m(copter.circle_nav->get_center_NED_m(), copter.circle_nav->center_is_terrain_alt(), copter.circle_nav->get_rate_degs()); if (auto_yaw.mode() != AutoYaw::Mode::ROI) { auto_yaw.set_mode(AutoYaw::Mode::CIRCLE); @@ -641,21 +641,21 @@ void PayloadPlace::start_descent() auto *wp_nav = copter.wp_nav; // set horizontal speed and acceleration limits - pos_control->set_max_speed_accel_NE_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); - pos_control->set_correction_speed_accel_NE_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); + pos_control->NE_set_max_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); + pos_control->NE_set_correction_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); // initialise the vertical position controller - if (!pos_control->is_active_NE()) { - pos_control->init_NE_controller(); + if (!pos_control->NE_is_active()) { + pos_control->NE_init_controller(); } // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_U_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_U_mss()); - pos_control->set_correction_speed_accel_U_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_U_mss()); + pos_control->D_set_max_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss()); + pos_control->D_set_correction_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss()); // initialise the vertical position controller - if (!pos_control->is_active_U()) { - pos_control->init_U_controller(); + if (!pos_control->D_is_active()) { + pos_control->D_init_controller(); } // initialise yaw @@ -1091,7 +1091,7 @@ void ModeAuto::wp_run() // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle - pos_control->update_U_controller(); + pos_control->D_update_controller(); // call attitude controller with auto yaw attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading()); @@ -1132,7 +1132,7 @@ void ModeAuto::circle_run() // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle - pos_control->update_U_controller(); + pos_control->D_update_controller(); // call attitude controller with auto yaw attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading()); @@ -1164,7 +1164,7 @@ void ModeAuto::loiter_run() // run waypoint and z-axis position controller copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); - pos_control->update_U_controller(); + pos_control->D_update_controller(); // call attitude controller with auto yaw attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading()); @@ -1191,11 +1191,11 @@ void ModeAuto::loiter_to_alt_run() if (!loiter_to_alt.loiter_start_done) { // set horizontal speed and acceleration limits - pos_control->set_max_speed_accel_NE_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); - pos_control->set_correction_speed_accel_NE_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); + pos_control->NE_set_max_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); + pos_control->NE_set_correction_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); - if (!pos_control->is_active_NE()) { - pos_control->init_NE_controller(); + if (!pos_control->NE_is_active()) { + pos_control->NE_init_controller(); } loiter_to_alt.loiter_start_done = true; @@ -1217,8 +1217,8 @@ void ModeAuto::loiter_to_alt_run() // approaches the desired altitude. float target_climb_rate_ms = sqrt_controller( -alt_error_m, - pos_control->get_pos_U_p().kP(), - pos_control->get_max_accel_U_mss(), + pos_control->D_get_pos_p().kP(), + pos_control->D_get_max_accel_mss(), G_Dt); target_climb_rate_ms = constrain_float(target_climb_rate_ms, -pos_control->get_max_speed_down_ms(), pos_control->get_max_speed_up_ms()); @@ -1231,9 +1231,9 @@ void ModeAuto::loiter_to_alt_run() #endif // Send the commanded climb rate to the position controller - pos_control->set_pos_target_U_from_climb_rate_ms(target_climb_rate_ms); + pos_control->D_set_pos_target_from_climb_rate_ms(target_climb_rate_ms); - pos_control->update_U_controller(); + pos_control->D_update_controller(); } // maintain an attitude for a specified time @@ -1261,9 +1261,9 @@ void ModeAuto::nav_attitude_time_run() attitude_control->input_euler_angle_roll_pitch_yaw_rad(target_rp_rad.x, target_rp_rad.y, radians(nav_attitude_time.yaw_deg), true); // Send the commanded climb rate to the position controller - pos_control->set_pos_target_U_from_climb_rate_ms(target_climb_rate_ms); + pos_control->D_set_pos_target_from_climb_rate_ms(target_climb_rate_ms); - pos_control->update_U_controller(); + pos_control->D_update_controller(); } #if AC_PAYLOAD_PLACE_ENABLED @@ -1285,7 +1285,6 @@ void PayloadPlace::run() 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; @@ -1297,7 +1296,7 @@ void PayloadPlace::run() // 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_NE(); + pos_control->NE_soften_for_landing(); switch (state) { case State::FlyToLocation: // this is handled in wp_run() @@ -1356,7 +1355,7 @@ void PayloadPlace::run() descent_established_time_ms = now_ms; descent_start_altitude_m = pos_control->get_pos_desired_U_m(); // limiting the decent rate to the limit set in wp_nav is not necessary but done for safety - descent_speed_ms = MIN((is_positive(g2.pldp_descent_speed_ms)) ? g2.pldp_descent_speed_ms : abs(g.land_speed_cms) * 0.01, wp_nav->get_default_speed_down_ms()); + descent_speed_ms = MIN((is_positive(g2.pldp_descent_speed_ms)) ? g2.pldp_descent_speed_ms : copter.mode_land.get_land_speed_ms(), wp_nav->get_default_speed_down_ms()); descent_thrust_level = 1.0; state = State::Descent; FALLTHROUGH; @@ -1413,7 +1412,7 @@ void PayloadPlace::run() case State::Release: // Reinitialise vertical position controller to remove discontinuity due to touch down of payload - pos_control->init_U_controller_no_descent(); + pos_control->D_init_controller_no_descent(); #if AP_GRIPPER_ENABLED if (AP::gripper().valid()) { gcs().send_text(MAV_SEVERITY_INFO, "%s Releasing the gripper", prefix_str); @@ -1452,7 +1451,7 @@ void PayloadPlace::run() // distance from the target altitude stopping distance from // vel_threshold_fraction * max velocity const float vel_threshold_fraction = 0.1; - const float stop_distance_m = 0.5 * sq(vel_threshold_fraction * copter.pos_control->get_max_speed_up_ms()) / copter.pos_control->get_max_accel_U_mss(); + const float stop_distance_m = 0.5 * sq(vel_threshold_fraction * copter.pos_control->get_max_speed_up_ms()) / copter.pos_control->D_get_max_accel_mss(); bool reached_altitude = pos_control->get_pos_desired_U_m() >= descent_start_altitude_m - stop_distance_m; if (reached_altitude) { state = State::Done; @@ -1475,7 +1474,7 @@ void PayloadPlace::run() case State::Descent: copter.flightmode->land_run_horizontal_control(); // update altitude target and call position controller - pos_control->land_at_climb_rate_ms(-descent_speed_ms, true); + pos_control->D_set_pos_target_from_climb_rate_ms(-descent_speed_ms, true); break; case State::Release: case State::Releasing: @@ -1483,16 +1482,17 @@ void PayloadPlace::run() case State::Ascent_Start: copter.flightmode->land_run_horizontal_control(); // update altitude target and call position controller - pos_control->land_at_climb_rate_ms(0.0, false); + pos_control->D_set_pos_target_from_climb_rate_ms(0.0); break; case State::Ascent: case State::Done: float vel = 0.0; copter.flightmode->land_run_horizontal_control(); - pos_control->input_pos_vel_accel_U_m(descent_start_altitude_m, vel, 0.0); + float pos_d_m = -descent_start_altitude_m; + pos_control->input_pos_vel_accel_D_m(pos_d_m, vel, 0.0); break; } - pos_control->update_U_controller(); + pos_control->D_update_controller(); } #endif @@ -1532,8 +1532,7 @@ bool ModeAuto::shift_alt_to_current_alt(Location& target_loc) const void ModeAuto::subtract_pos_offsets(Location& target_loc) const { // subtract position controller offsets from target location - const Vector3p& pos_ofs_neu_m = pos_control->get_pos_offset_NEU_m(); - Vector3p pos_ofs_ned_m = Vector3p{pos_ofs_neu_m.x, pos_ofs_neu_m.y, -pos_ofs_neu_m.z}; + const Vector3p& pos_ofs_ned_m = pos_control->get_pos_offset_NED_m(); target_loc.offset(-pos_ofs_ned_m); } @@ -1805,8 +1804,8 @@ void ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) loiter_to_alt.alt_error_m = 0; // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_U_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_U_mss()); - pos_control->set_correction_speed_accel_U_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_U_mss()); + pos_control->D_set_max_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss()); + pos_control->D_set_correction_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss()); // set submode set_submode(SubMode::LOITER_TO_ALT); diff --git a/ArduCopter/mode_autotune.cpp b/ArduCopter/mode_autotune.cpp index e8cf85bbb9156..af8611d9246c6 100644 --- a/ArduCopter/mode_autotune.cpp +++ b/ArduCopter/mode_autotune.cpp @@ -82,8 +82,8 @@ void AutoTune::get_pilot_desired_rp_yrate_rad(float &des_roll_rad, float &des_pi void AutoTune::init_z_limits() { // set vertical speed and acceleration limits - copter.pos_control->set_max_speed_accel_U_m(copter.flightmode->get_pilot_speed_dn_ms(), copter.flightmode->get_pilot_speed_up_ms(), copter.flightmode->get_pilot_accel_U_mss()); - copter.pos_control->set_correction_speed_accel_U_m(copter.flightmode->get_pilot_speed_dn_ms(), copter.flightmode->get_pilot_speed_up_ms(), copter.flightmode->get_pilot_accel_U_mss()); + copter.pos_control->D_set_max_speed_accel_m(copter.flightmode->get_pilot_speed_dn_ms(), copter.flightmode->get_pilot_speed_up_ms(), copter.flightmode->get_pilot_accel_D_mss()); + copter.pos_control->D_set_correction_speed_accel_m(copter.flightmode->get_pilot_speed_dn_ms(), copter.flightmode->get_pilot_speed_up_ms(), copter.flightmode->get_pilot_accel_D_mss()); } #if HAL_LOGGING_ENABLED diff --git a/ArduCopter/mode_avoid_adsb.cpp b/ArduCopter/mode_avoid_adsb.cpp index a32b5e6f12fab..03a8a6117fd0e 100644 --- a/ArduCopter/mode_avoid_adsb.cpp +++ b/ArduCopter/mode_avoid_adsb.cpp @@ -26,7 +26,7 @@ bool ModeAvoidADSB::set_velocity_NEU_ms(const Vector3f& velocity_neu_ms) } // re-use guided mode's velocity controller - ModeGuided::set_vel_NEU_ms(velocity_neu_ms); + ModeGuided::set_vel_NED_ms(velocity_neu_ms); return true; } diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index ce7dbdf57ecd3..90f999be19c5f 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -10,19 +10,19 @@ bool ModeBrake::init(bool ignore_checks) { // initialise pos controller speed and acceleration - pos_control->set_max_speed_accel_NE_m(pos_control->get_vel_estimate_NEU_ms().xy().length(), BRAKE_MODE_DECEL_RATE_MSS); - pos_control->set_correction_speed_accel_NE_m(pos_control->get_vel_estimate_NEU_ms().xy().length(), BRAKE_MODE_DECEL_RATE_MSS); + pos_control->NE_set_max_speed_accel_m(pos_control->get_vel_estimate_NED_ms().xy().length(), BRAKE_MODE_DECEL_RATE_MSS); + pos_control->NE_set_correction_speed_accel_m(pos_control->get_vel_estimate_NED_ms().xy().length(), BRAKE_MODE_DECEL_RATE_MSS); // initialise position controller - pos_control->init_NE_controller(); + pos_control->NE_init_controller(); // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_U_m(BRAKE_MODE_SPEED_Z_MS, BRAKE_MODE_SPEED_Z_MS, BRAKE_MODE_DECEL_RATE_MSS); - pos_control->set_correction_speed_accel_U_m(BRAKE_MODE_SPEED_Z_MS, BRAKE_MODE_SPEED_Z_MS, BRAKE_MODE_DECEL_RATE_MSS); + pos_control->D_set_max_speed_accel_m(BRAKE_MODE_SPEED_Z_MS, BRAKE_MODE_SPEED_Z_MS, BRAKE_MODE_DECEL_RATE_MSS); + pos_control->D_set_correction_speed_accel_m(BRAKE_MODE_SPEED_Z_MS, BRAKE_MODE_SPEED_Z_MS, BRAKE_MODE_DECEL_RATE_MSS); // initialise the vertical position controller - if (!pos_control->is_active_U()) { - pos_control->init_U_controller(); + if (!pos_control->D_is_active()) { + pos_control->D_init_controller(); } _timeout_ms = 0; @@ -37,7 +37,7 @@ void ModeBrake::run() // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { make_safe_ground_handling(); - pos_control->relax_U_controller(0.0f); + pos_control->D_relax_controller(0.0f); return; } @@ -46,20 +46,20 @@ void ModeBrake::run() // relax stop target if we might be landed if (copter.ap.land_complete_maybe) { - pos_control->soften_for_landing_NE(); + pos_control->NE_soften_for_landing(); } // use position controller to stop Vector2f vel; Vector2f accel; pos_control->input_vel_accel_NE_m(vel, accel); - pos_control->update_NE_controller(); + pos_control->NE_update_controller(); // call attitude controller attitude_control->input_thrust_vector_rate_heading_rads(pos_control->get_thrust_vector(), 0.0f); - pos_control->set_pos_target_U_from_climb_rate_ms(0.0f); - pos_control->update_U_controller(); + pos_control->D_set_pos_target_from_climb_rate_ms(0.0f); + pos_control->D_update_controller(); // MAV_CMD_SOLO_BTN_PAUSE_CLICK (Solo only) is used to set the timeout. if (_timeout_ms != 0 && millis()-_timeout_start >= _timeout_ms) { diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index c35ae9c230204..5063bd938cb43 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -13,10 +13,10 @@ bool ModeCircle::init(bool ignore_checks) speed_changing = false; // set speed and acceleration limits - pos_control->set_max_speed_accel_NE_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); - pos_control->set_correction_speed_accel_NE_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); - pos_control->set_max_speed_accel_U_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_U_mss()); - pos_control->set_correction_speed_accel_U_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_U_mss()); + pos_control->NE_set_max_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); + pos_control->NE_set_correction_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); + pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss()); + pos_control->D_set_correction_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss()); // initialise circle controller including setting the circle center based on vehicle speed copter.circle_nav->init(); @@ -24,9 +24,9 @@ bool ModeCircle::init(bool ignore_checks) #if HAL_MOUNT_ENABLED // Check if the CIRCLE_OPTIONS parameter have roi_at_center if (copter.circle_nav->roi_at_center()) { - const Vector3p &pos_neu_m { copter.circle_nav->get_center_NEU_m() }; + const Vector3p &pos_ned_m { copter.circle_nav->get_center_NED_m() }; Location circle_center; - if (!AP::ahrs().get_location_from_origin_offset_NED(circle_center, pos_neu_m)) { + if (!AP::ahrs().get_location_from_origin_offset_NED(circle_center, pos_ned_m)) { return false; } // point at the ground: @@ -47,8 +47,8 @@ bool ModeCircle::init(bool ignore_checks) void ModeCircle::run() { // set speed and acceleration limits - pos_control->set_max_speed_accel_NE_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); - pos_control->set_max_speed_accel_U_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_U_mss()); + pos_control->NE_set_max_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); + pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss()); // Check for any change in params and update in real time copter.circle_nav->check_param_change(); @@ -124,7 +124,7 @@ void ModeCircle::run() #endif copter.failsafe_terrain_set_status(copter.circle_nav->update_ms(target_climb_rate_ms)); - pos_control->update_U_controller(); + pos_control->D_update_controller(); // call attitude controller with auto yaw attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading()); diff --git a/ArduCopter/mode_drift.cpp b/ArduCopter/mode_drift.cpp index 8d83ade39aed6..72f8b5d55b2f2 100644 --- a/ArduCopter/mode_drift.cpp +++ b/ArduCopter/mode_drift.cpp @@ -63,12 +63,12 @@ void ModeDrift::run() get_pilot_desired_lean_angles_rad(target_roll_rad, target_pitch_rad, attitude_control->lean_angle_max_rad(), attitude_control->get_althold_lean_angle_max_rad()); // Grab inertial velocity (already m/s) - const Vector3f& vel_neu_ms = pos_control->get_vel_estimate_NEU_ms(); + const Vector3f& vel_ned_ms = pos_control->get_vel_estimate_NED_ms(); // rotate roll, pitch input from north facing to vehicle's perspective // body-frame components in m/s - float vel_right_ms = vel_neu_ms.y * ahrs.cos_yaw() - vel_neu_ms.x * ahrs.sin_yaw(); // body roll axis - float vel_forward_ms = vel_neu_ms.y * ahrs.sin_yaw() + vel_neu_ms.x * ahrs.cos_yaw(); // body pitch axis + float vel_right_ms = vel_ned_ms.y * ahrs.cos_yaw() - vel_ned_ms.x * ahrs.sin_yaw(); // body roll axis + float vel_forward_ms = vel_ned_ms.y * ahrs.sin_yaw() + vel_ned_ms.x * ahrs.cos_yaw(); // body pitch axis // gain scheduling for yaw const float vel_forward_2_ms = MIN(fabsf(vel_forward_ms), DRIFT_VEL_FORWARD_MIN_MS); @@ -140,12 +140,12 @@ void ModeDrift::run() target_roll_rad, target_pitch_rad, target_yaw_rate_rads); // output pilot's throttle with angle boost (velz now m/s) - const float assisted_throttle = get_throttle_assist(vel_neu_ms.z, get_pilot_desired_throttle()); + const float assisted_throttle = get_throttle_assist(vel_ned_ms.z, get_pilot_desired_throttle()); attitude_control->set_throttle_out(assisted_throttle, true, g.throttle_filt); } -// get_throttle_assist - return throttle output (range 0 ~ 1) based on pilot input and z-axis velocity -float ModeDrift::get_throttle_assist(float vel_u_ms, float pilot_throttle_scaled) +// get_throttle_assist - return throttle output (range 0 ~ 1) based on pilot input and D-axis velocity (positive down) +float ModeDrift::get_throttle_assist(float vel_d_ms, float pilot_throttle_scaled) { // throttle assist - adjusts throttle to slow the vehicle's vertical velocity // Only active when pilot's throttle is between 0.213 ~ 0.787 @@ -154,7 +154,7 @@ float ModeDrift::get_throttle_assist(float vel_u_ms, float pilot_throttle_scaled if (pilot_throttle_scaled > DRIFT_THR_MIN && pilot_throttle_scaled < DRIFT_THR_MAX) { // calculate throttle assist gain thr_assist = 1.2f - ((float)fabsf(pilot_throttle_scaled - 0.5f) / 0.24f); - thr_assist = constrain_float(thr_assist, 0.0f, 1.0f) * -DRIFT_THR_ASSIST_GAIN_MS * vel_u_ms; + thr_assist = constrain_float(thr_assist, 0.0f, 1.0f) * DRIFT_THR_ASSIST_GAIN_MS * vel_d_ms; // ensure throttle assist never adjusts the throttle by more than 0.3 (≈300 pwm) thr_assist = constrain_float(thr_assist, -DRIFT_THR_ASSIST_MAX, DRIFT_THR_ASSIST_MAX); diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 51327d5012a05..2b10634a1c45c 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -89,12 +89,12 @@ bool ModeFlowHold::init(bool ignore_checks) } // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_U_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_U_mss()); - pos_control->set_correction_speed_accel_U_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_U_mss()); + pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss()); + pos_control->D_set_correction_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss()); // initialise the vertical position controller - if (!copter.pos_control->is_active_U()) { - pos_control->init_U_controller(); + if (!copter.pos_control->D_is_active()) { + pos_control->D_init_controller(); } flow_filter.set_cutoff_frequency(copter.scheduler.get_loop_rate_hz(), flow_filter_hz.get()); @@ -234,7 +234,7 @@ void ModeFlowHold::run() update_height_estimate(); // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_U_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_U_mss()); + pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss()); // apply SIMPLE mode transform to pilot inputs update_simple_mode(); @@ -252,7 +252,7 @@ void ModeFlowHold::run() float target_yaw_rate_rads = get_pilot_desired_yaw_rate_rads(); // Flow Hold State Machine Determination - AltHoldModeState flowhold_state = get_alt_hold_state_U_ms(target_climb_rate_ms); + AltHoldModeState flowhold_state = get_alt_hold_state_D_ms(target_climb_rate_ms); if (copter.optflow.healthy()) { const float filter_constant = 0.95; @@ -268,7 +268,7 @@ void ModeFlowHold::run() 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(); - copter.pos_control->relax_U_controller(0.0f); // forces throttle output to decay to zero + copter.pos_control->D_relax_controller(0.0f); // forces throttle output to decay to zero flow_pi_xy.reset_I(); break; @@ -294,7 +294,7 @@ void ModeFlowHold::run() case AltHoldModeState::Landed_Pre_Takeoff: attitude_control->reset_rate_controller_I_terms_smoothly(); - pos_control->relax_U_controller(0.0f); // forces throttle output to decay to zero + pos_control->D_relax_controller(0.0f); // forces throttle output to decay to zero break; case AltHoldModeState::Flying: @@ -309,7 +309,7 @@ void ModeFlowHold::run() #endif // Send the commanded climb rate to the position controller - pos_control->set_pos_target_U_from_climb_rate_ms(target_climb_rate_ms); + pos_control->D_set_pos_target_from_climb_rate_ms(target_climb_rate_ms); break; } @@ -348,7 +348,7 @@ void ModeFlowHold::run() copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(bf_angles_rad.x, bf_angles_rad.y, target_yaw_rate_rads); // run the vertical position controller and set output throttle - pos_control->update_U_controller(); + pos_control->D_update_controller(); } /* diff --git a/ArduCopter/mode_follow.cpp b/ArduCopter/mode_follow.cpp index 37e33fdd90b34..1c0b124384c51 100644 --- a/ArduCopter/mode_follow.cpp +++ b/ArduCopter/mode_follow.cpp @@ -30,16 +30,16 @@ bool ModeFollow::init(const bool ignore_checks) #endif // initialise horizontal speed, acceleration - pos_control->set_max_speed_accel_NE_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); - pos_control->set_correction_speed_accel_NE_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); + pos_control->NE_set_max_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); + pos_control->NE_set_correction_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); // initialize vertical speeds and acceleration - pos_control->set_max_speed_accel_U_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_U_mss()); - pos_control->set_correction_speed_accel_U_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_U_mss()); + pos_control->D_set_max_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss()); + pos_control->D_set_correction_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss()); // initialise velocity controller - pos_control->init_U_controller(); - pos_control->init_NE_controller(); + pos_control->D_init_controller(); + pos_control->NE_init_controller(); // initialise yaw auto_yaw.set_mode_to_default(false); @@ -75,21 +75,16 @@ void ModeFollow::run() Vector3f vel_ofs_ned_ms; // velocity of lead vehicle + offset Vector3f accel_ofs_ned_mss; // accel of lead vehicle + offset if (g2.follow.get_ofs_pos_vel_accel_NED_m(pos_ofs_ned_m, vel_ofs_ned_ms, accel_ofs_ned_mss)) { - Vector2p pos_ofs_ne_m = pos_ofs_ned_m.xy(); - Vector2f vel_ofs_ne_ms = vel_ofs_ned_ms.xy(); - Vector2f accel_ofs_ne_mss = accel_ofs_ned_mss.xy(); float target_heading_deg = 0.0f; float target_heading_rate_degs = 0.0f; g2.follow.get_target_heading_deg(target_heading_deg); g2.follow.get_target_heading_rate_degs(target_heading_rate_degs); - pos_control->input_pos_vel_accel_NE_m(pos_ofs_ne_m, vel_ofs_ne_ms, accel_ofs_ne_mss, false); + pos_control->input_pos_vel_accel_NE_m(pos_ofs_ned_m.xy(), vel_ofs_ned_ms.xy(), accel_ofs_ned_mss.xy(), false); - float pos_ofs_u_m = -pos_ofs_ned_m.z; - float vel_ofs_u_ms = -vel_ofs_ned_ms.z; - float accel_ofs_u_mss = -accel_ofs_ned_mss.z; - pos_control->input_pos_vel_accel_U_m(pos_ofs_u_m, vel_ofs_u_ms, accel_ofs_u_mss, false); + float pos_ofs_d_m = pos_ofs_ned_m.z; + pos_control->input_pos_vel_accel_D_m(pos_ofs_d_m, vel_ofs_ned_ms.z, accel_ofs_ned_mss.z, false); // Determine desired yaw behavior based on configured follow mode switch (g2.follow.get_yaw_behave()) { @@ -100,7 +95,7 @@ void ModeFollow::run() Vector3f accel_ned_mss; // accel of lead vehicle if (g2.follow.get_target_pos_vel_accel_NED_m(pos_ned_m, vel_ned_ms, accel_ned_mss)) if (pos_ned_m.xy().length_squared() > 1.0) { - yaw_rad = (pos_ned_m.xy() - pos_control->get_pos_target_NEU_m().xy()).tofloat().angle(); + yaw_rad = (pos_ned_m.xy() - pos_control->get_pos_target_NED_m().xy()).tofloat().angle(); } break; } @@ -114,8 +109,8 @@ void ModeFollow::run() case AP_Follow::YAW_BEHAVE_DIR_OF_FLIGHT: { // Face the direction of travel - if (vel_ofs_ne_ms.length_squared() > 1.0) { - yaw_rad = vel_ofs_ne_ms.angle(); + if (vel_ofs_ned_ms.xy().length_squared() > 1.0) { + yaw_rad = vel_ofs_ned_ms.xy().angle(); } break; } @@ -131,14 +126,14 @@ void ModeFollow::run() Vector2f vel_ne_zero; Vector2f accel_ne_zero; pos_control->input_vel_accel_NE_m(vel_ne_zero, accel_ne_zero, false); - float vel_u_zero = 0.0; - pos_control->input_vel_accel_U_m(vel_u_zero, 0.0, false); + float vel_d_zero = 0.0; + pos_control->input_vel_accel_D_m(vel_d_zero, 0.0, false); yaw_rate_rads = 0.0f; } // update the position controller - pos_control->update_NE_controller(); - pos_control->update_U_controller(); + pos_control->NE_update_controller(); + pos_control->D_update_controller(); // call attitude controller attitude_control->input_thrust_vector_heading_rad(pos_control->get_thrust_vector(), yaw_rad, yaw_rate_rads); diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index ba39acf586c0c..0130ccfc1a9bc 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -6,10 +6,10 @@ * Init and run calls for guided flight mode */ -static Vector3p guided_pos_target_neu_m; // position target (used by posvel controller only) -static bool guided_is_terrain_alt; // true if guided_pos_target_neu_m.z is an alt above terrain -static Vector3f guided_vel_target_neu_ms; // velocity target (used by pos_vel_accel controller and vel_accel controller) -static Vector3f guided_accel_target_neu_mss; // acceleration target (used by pos_vel_accel controller vel_accel controller and accel controller) +static Vector3p guided_pos_target_ned_m; // position target (used by posvel controller only) +static bool guided_is_terrain_alt; // true if guided_pos_target_ned_m.z should be offset by the terrain altitude +static Vector3f guided_vel_target_ned_ms; // velocity target (used by pos_vel_accel controller and vel_accel controller) +static Vector3f guided_accel_target_ned_mss; // 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 struct { @@ -27,7 +27,7 @@ struct Guided_Limit { float alt_max_m; // upper altitude limit in m above home (0 = no limit) float horiz_max_m; // horizontal position limit in m from where guided mode was initiated (0 = no limit) uint32_t start_time_ms; // system time in milliseconds that control was handed to the external computer - Vector3p start_pos_neu_m; // start position as a distance from home in m. used for checking horiz_max limit + Vector3p start_pos_ned_m; // start position as an offset from home in m. used for checking horiz_max limit } static guided_limit; // controls which controller is run (pos or vel): @@ -43,8 +43,8 @@ bool ModeGuided::init(bool ignore_checks) { // start in velaccel control mode velaccel_control_start(); - guided_vel_target_neu_ms.zero(); - guided_accel_target_neu_mss.zero(); + guided_vel_target_ned_ms.zero(); + guided_accel_target_ned_mss.zero(); send_notification = false; // clear pause state when entering guided mode @@ -187,7 +187,7 @@ bool ModeGuided::do_user_takeoff_start_m(float takeoff_alt_m) auto_yaw.set_mode(AutoYaw::Mode::HOLD); // clear i term when we're taking off - pos_control->init_U_controller(); + pos_control->D_init_controller(); // initialise alt for WP_NAVALT_MIN and set completion alt auto_takeoff.start_m(alt_target_m, alt_target_terrain); @@ -208,9 +208,9 @@ void ModeGuided::wp_control_start() wp_nav->wp_and_spline_init_m(); // initialise wpnav to stopping point - Vector3p stopping_point_neu_m; - wp_nav->get_wp_stopping_point_NEU_m(stopping_point_neu_m); - if (!wp_nav->set_wp_destination_NEU_m(stopping_point_neu_m, false)) { + Vector3p stopping_point_ned_m; + wp_nav->get_wp_stopping_point_NED_m(stopping_point_ned_m); + if (!wp_nav->set_wp_destination_NED_m(stopping_point_ned_m, false)) { // this should never happen because terrain data is not used INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } @@ -236,7 +236,7 @@ void ModeGuided::wp_control_run() copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); // call z-axis position controller (wpnav should have already updated it's alt target) - pos_control->update_U_controller(); + pos_control->D_update_controller(); // call attitude controller with auto yaw attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading()); @@ -246,16 +246,16 @@ void ModeGuided::wp_control_run() void ModeGuided::pva_control_start() { // initialise horizontal speed, acceleration - pos_control->set_max_speed_accel_NE_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); - pos_control->set_correction_speed_accel_NE_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); + pos_control->NE_set_max_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); + pos_control->NE_set_correction_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); // initialize vertical speeds and acceleration - pos_control->set_max_speed_accel_U_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_U_mss()); - pos_control->set_correction_speed_accel_U_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_U_mss()); + pos_control->D_set_max_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss()); + pos_control->D_set_correction_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss()); // initialise velocity controller - pos_control->init_U_controller(); - pos_control->init_NE_controller(); + pos_control->D_init_controller(); + pos_control->NE_init_controller(); // initialise yaw auto_yaw.set_mode_to_default(false); @@ -312,24 +312,24 @@ bool ModeGuided::is_taking_off() const bool ModeGuided::set_speed_NE_ms(float speed_ne_ms) { // initialise horizontal speed, acceleration - pos_control->set_max_speed_accel_NE_m(speed_ne_ms, wp_nav->get_wp_acceleration_mss()); - pos_control->set_correction_speed_accel_NE_m(speed_ne_ms, wp_nav->get_wp_acceleration_mss()); + pos_control->NE_set_max_speed_accel_m(speed_ne_ms, wp_nav->get_wp_acceleration_mss()); + pos_control->NE_set_correction_speed_accel_m(speed_ne_ms, wp_nav->get_wp_acceleration_mss()); return true; } bool ModeGuided::set_speed_up_ms(float speed_up_ms) { // initialize vertical speeds and acceleration - pos_control->set_max_speed_accel_U_m(wp_nav->get_default_speed_down_ms(), speed_up_ms, wp_nav->get_accel_U_mss()); - pos_control->set_correction_speed_accel_U_m(wp_nav->get_default_speed_down_ms(), speed_up_ms, wp_nav->get_accel_U_mss()); + pos_control->D_set_max_speed_accel_m(wp_nav->get_default_speed_down_ms(), speed_up_ms, wp_nav->get_accel_D_mss()); + pos_control->D_set_correction_speed_accel_m(wp_nav->get_default_speed_down_ms(), speed_up_ms, wp_nav->get_accel_D_mss()); return true; } bool ModeGuided::set_speed_down_ms(float speed_down_ms) { // initialize vertical speeds and acceleration - pos_control->set_max_speed_accel_U_m(speed_down_ms, wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_U_mss()); - pos_control->set_correction_speed_accel_U_m(speed_down_ms, wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_U_mss()); + pos_control->D_set_max_speed_accel_m(speed_down_ms, wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss()); + pos_control->D_set_correction_speed_accel_m(speed_down_ms, wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss()); return true; } @@ -340,12 +340,12 @@ void ModeGuided::angle_control_start() guided_mode = SubMode::Angle; // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_U_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_U_mss()); - pos_control->set_correction_speed_accel_U_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_U_mss()); + pos_control->D_set_max_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss()); + pos_control->D_set_correction_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss()); // initialise the vertical position controller - if (!pos_control->is_active_U()) { - pos_control->init_U_controller(); + if (!pos_control->D_is_active()) { + pos_control->D_init_controller(); } // initialise targets @@ -355,14 +355,14 @@ void ModeGuided::angle_control_start() guided_angle_state.climb_rate_ms = 0.0f; } -// set_pos_neu_m - sets guided mode's target pos_neu_m +// set_pos_ned_m - sets guided mode's target pos_ned_m // Returns true if the fence is enabled and guided waypoint is within the fence // else return false if the waypoint is outside the fence -bool ModeGuided::set_pos_NEU_m(const Vector3p& pos_neu_m, bool use_yaw, float yaw_rad, bool use_yaw_rate, float yaw_rate_rads, bool relative_yaw, bool is_terrain_alt) +bool ModeGuided::set_pos_NED_m(const Vector3p& pos_ned_m, bool use_yaw, float yaw_rad, bool use_yaw_rate, float yaw_rate_rads, bool relative_yaw, bool is_terrain_alt) { #if AP_FENCE_ENABLED // reject destination if outside the fence - const Location dest_loc(pos_neu_m * 100.0, is_terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); + const Location dest_loc = Location::from_ekf_offset_NED_m(pos_ned_m, is_terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); if (!copter.fence.check_destination_within_fence(dest_loc)) { LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK @@ -381,11 +381,11 @@ bool ModeGuided::set_pos_NEU_m(const Vector3p& pos_neu_m, bool use_yaw, float ya set_yaw_state_rad(use_yaw, yaw_rad, use_yaw_rate, yaw_rate_rads, relative_yaw); // no need to check return status because terrain data is not used - wp_nav->set_wp_destination_NEU_m(pos_neu_m, is_terrain_alt); + wp_nav->set_wp_destination_NED_m(pos_ned_m, is_terrain_alt); #if HAL_LOGGING_ENABLED // log target - copter.Log_Write_Guided_Position_Target(guided_mode, pos_neu_m, is_terrain_alt, Vector3f(), Vector3f()); + copter.Log_Write_Guided_Position_Target(guided_mode, pos_ned_m, is_terrain_alt, Vector3f(), Vector3f()); #endif send_notification = true; return true; @@ -400,8 +400,8 @@ bool ModeGuided::set_pos_NEU_m(const Vector3p& pos_neu_m, bool use_yaw, float ya // initialise terrain following if needed if (is_terrain_alt) { // get current alt above terrain - float terrain_u_m; - if (!wp_nav->get_terrain_U_m(terrain_u_m)) { + float terrain_d_m; + if (!wp_nav->get_terrain_D_m(terrain_d_m)) { // if we don't have terrain altitude then stop init(true); return false; @@ -409,25 +409,25 @@ bool ModeGuided::set_pos_NEU_m(const Vector3p& pos_neu_m, bool use_yaw, float ya // convert origin to alt-above-terrain if necessary if (!guided_is_terrain_alt) { // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin - pos_control->init_pos_terrain_U_m(terrain_u_m); + pos_control->init_pos_terrain_D_m(terrain_d_m); } } else { - pos_control->init_pos_terrain_U_m(0.0); + pos_control->init_pos_terrain_D_m(0.0); } // set yaw state set_yaw_state_rad(use_yaw, yaw_rad, use_yaw_rate, yaw_rate_rads, relative_yaw); // set position target and zero velocity and acceleration - guided_pos_target_neu_m = pos_neu_m; + guided_pos_target_ned_m = pos_ned_m; guided_is_terrain_alt = is_terrain_alt; - guided_vel_target_neu_ms.zero(); - guided_accel_target_neu_mss.zero(); + guided_vel_target_ned_ms.zero(); + guided_accel_target_ned_mss.zero(); update_time_ms = millis(); #if HAL_LOGGING_ENABLED // log target - copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_neu_m, guided_is_terrain_alt, guided_vel_target_neu_ms, guided_accel_target_neu_mss); + copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_ned_m, guided_is_terrain_alt, guided_vel_target_ned_ms, guided_accel_target_ned_mss); #endif send_notification = true; @@ -441,7 +441,7 @@ bool ModeGuided::get_wp(Location& destination) const case SubMode::WP: return wp_nav->get_oa_wp_destination(destination); case SubMode::Pos: - destination = Location(guided_pos_target_neu_m.tofloat(), guided_is_terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); + destination = Location::from_ekf_offset_NED_m(guided_pos_target_ned_m, guided_is_terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); return true; case SubMode::Angle: case SubMode::TakeOff: @@ -495,9 +495,9 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y } // set position target and zero velocity and acceleration - Vector3p pos_target_neu_m; + Vector3p pos_target_ned_m; bool is_terrain_alt; - if (!wp_nav->get_vector_NEU_m(dest_loc, pos_target_neu_m, is_terrain_alt)) { + if (!wp_nav->get_vector_NED_m(dest_loc, pos_target_ned_m, is_terrain_alt)) { return false; } @@ -513,8 +513,8 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y // initialise terrain following if needed if (is_terrain_alt) { // get current alt above terrain - float terrain_u_m; - if (!wp_nav->get_terrain_U_m(terrain_u_m)) { + float terrain_d_m; + if (!wp_nav->get_terrain_D_m(terrain_d_m)) { // if we don't have terrain altitude then stop init(true); return false; @@ -522,21 +522,21 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y // convert origin to alt-above-terrain if necessary if (!guided_is_terrain_alt) { // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin - pos_control->init_pos_terrain_U_m(terrain_u_m); + pos_control->init_pos_terrain_D_m(-terrain_d_m); } } else { - pos_control->init_pos_terrain_U_m(0.0); + pos_control->init_pos_terrain_D_m(0.0); } - guided_pos_target_neu_m = pos_target_neu_m; + guided_pos_target_ned_m = pos_target_ned_m; guided_is_terrain_alt = is_terrain_alt; - guided_vel_target_neu_ms.zero(); - guided_accel_target_neu_mss.zero(); + guided_vel_target_ned_ms.zero(); + guided_accel_target_ned_mss.zero(); update_time_ms = millis(); // log target #if HAL_LOGGING_ENABLED - copter.Log_Write_Guided_Position_Target(guided_mode, Vector3p(dest_loc.lat, dest_loc.lng, dest_loc.alt), guided_is_terrain_alt, guided_vel_target_neu_ms, guided_accel_target_neu_mss); + copter.Log_Write_Guided_Position_Target(guided_mode, Vector3p(dest_loc.lat, dest_loc.lng, dest_loc.alt), guided_is_terrain_alt, guided_vel_target_ned_ms, guided_accel_target_ned_mss); #endif send_notification = true; @@ -544,8 +544,8 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y return true; } -// set_vel_accel_NEU_m - sets guided mode's target velocity and acceleration -void ModeGuided::set_accel_NEU_mss(const Vector3f& accel_neu_mss, bool use_yaw, float yaw_rad, bool use_yaw_rate, float yaw_rate_rads, bool relative_yaw, bool log_request) +// set_vel_accel_NED_m - sets guided mode's target velocity and acceleration +void ModeGuided::set_accel_NED_mss(const Vector3f& accel_ned_mss, bool use_yaw, float yaw_rad, bool use_yaw_rate, float yaw_rate_rads, bool relative_yaw, bool log_request) { // check we are in acceleration control mode if (guided_mode != SubMode::Accel) { @@ -556,28 +556,28 @@ void ModeGuided::set_accel_NEU_mss(const Vector3f& accel_neu_mss, bool use_yaw, set_yaw_state_rad(use_yaw, yaw_rad, use_yaw_rate, yaw_rate_rads, relative_yaw); // set velocity and acceleration targets and zero position - guided_pos_target_neu_m.zero(); + guided_pos_target_ned_m.zero(); guided_is_terrain_alt = false; - guided_vel_target_neu_ms.zero(); - guided_accel_target_neu_mss = accel_neu_mss; + guided_vel_target_ned_ms.zero(); + guided_accel_target_ned_mss = accel_ned_mss; update_time_ms = millis(); #if HAL_LOGGING_ENABLED // log target if (log_request) { - copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_neu_m, guided_is_terrain_alt, guided_vel_target_neu_ms, guided_accel_target_neu_mss); + copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_ned_m, guided_is_terrain_alt, guided_vel_target_ned_ms, guided_accel_target_ned_mss); } #endif } -// set_vel_NEU_ms - sets guided mode's target velocity -void ModeGuided::set_vel_NEU_ms(const Vector3f& vel_neu_ms, bool use_yaw, float yaw_rad, bool use_yaw_rate, float yaw_rate_rads, bool relative_yaw, bool log_request) +// set_vel_NED_ms - sets guided mode's target velocity +void ModeGuided::set_vel_NED_ms(const Vector3f& vel_ned_ms, bool use_yaw, float yaw_rad, bool use_yaw_rate, float yaw_rate_rads, bool relative_yaw, bool log_request) { - set_vel_accel_NEU_m(vel_neu_ms, Vector3f(), use_yaw, yaw_rad, use_yaw_rate, yaw_rate_rads, relative_yaw, log_request); + set_vel_accel_NED_m(vel_ned_ms, Vector3f(), use_yaw, yaw_rad, use_yaw_rate, yaw_rate_rads, relative_yaw, log_request); } -// set_vel_accel_NEU_m - sets guided mode's target velocity and acceleration -void ModeGuided::set_vel_accel_NEU_m(const Vector3f& vel_neu_ms, const Vector3f& accel_neu_mss, bool use_yaw, float yaw_rad, bool use_yaw_rate, float yaw_rate_rads, bool relative_yaw, bool log_request) +// set_vel_accel_NED_m - sets guided mode's target velocity and acceleration +void ModeGuided::set_vel_accel_NED_m(const Vector3f& vel_ned_ms, const Vector3f& accel_ned_mss, bool use_yaw, float yaw_rad, bool use_yaw_rate, float yaw_rate_rads, bool relative_yaw, bool log_request) { // check we are in velocity and acceleration control mode if (guided_mode != SubMode::VelAccel) { @@ -588,32 +588,32 @@ void ModeGuided::set_vel_accel_NEU_m(const Vector3f& vel_neu_ms, const Vector3f& set_yaw_state_rad(use_yaw, yaw_rad, use_yaw_rate, yaw_rate_rads, relative_yaw); // set velocity and acceleration targets and zero position - guided_pos_target_neu_m.zero(); + guided_pos_target_ned_m.zero(); guided_is_terrain_alt = false; - guided_vel_target_neu_ms = vel_neu_ms; - guided_accel_target_neu_mss = accel_neu_mss; + guided_vel_target_ned_ms = vel_ned_ms; + guided_accel_target_ned_mss = accel_ned_mss; update_time_ms = millis(); #if HAL_LOGGING_ENABLED // log target if (log_request) { - copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_neu_m, guided_is_terrain_alt, guided_vel_target_neu_ms, guided_accel_target_neu_mss); + copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_ned_m, guided_is_terrain_alt, guided_vel_target_ned_ms, guided_accel_target_ned_mss); } #endif } -// set_pos_vel_NEU_m - set guided mode position and velocity target -bool ModeGuided::set_pos_vel_NEU_m(const Vector3p& pos_neu_m, const Vector3f& vel_neu_ms, bool use_yaw, float yaw_rad, bool use_yaw_rate, float yaw_rate_rads, bool relative_yaw) +// set guided mode position and velocity target +bool ModeGuided::set_pos_vel_NED_m(const Vector3p& pos_ned_m, const Vector3f& vel_ned_ms, bool use_yaw, float yaw_rad, bool use_yaw_rate, float yaw_rate_rads, bool relative_yaw) { - return set_pos_vel_accel_NEU_m(pos_neu_m, vel_neu_ms, Vector3f(), use_yaw, yaw_rad, use_yaw_rate, yaw_rate_rads, relative_yaw); + return set_pos_vel_accel_NED_m(pos_ned_m, vel_ned_ms, Vector3f(), use_yaw, yaw_rad, use_yaw_rate, yaw_rate_rads, relative_yaw); } -// set_pos_vel_accel_NEU_m - set guided mode position, velocity and acceleration target -bool ModeGuided::set_pos_vel_accel_NEU_m(const Vector3p& pos_neu_m, const Vector3f& vel_neu_ms, const Vector3f& accel_neu_mss, bool use_yaw, float yaw_rad, bool use_yaw_rate, float yaw_rate_rads, bool relative_yaw) +// set_pos_vel_accel_NED_m - set guided mode position, velocity and acceleration target +bool ModeGuided::set_pos_vel_accel_NED_m(const Vector3p& pos_ned_m, const Vector3f& vel_ned_ms, const Vector3f& accel_ned_mss, bool use_yaw, float yaw_rad, bool use_yaw_rate, float yaw_rate_rads, bool relative_yaw) { #if AP_FENCE_ENABLED // reject destination if outside the fence - const Location dest_loc(pos_neu_m * 100.0, Location::AltFrame::ABOVE_ORIGIN); + const Location dest_loc = Location::from_ekf_offset_NED_m(pos_ned_m, Location::AltFrame::ABOVE_ORIGIN); if (!copter.fence.check_destination_within_fence(dest_loc)) { LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK @@ -630,14 +630,14 @@ bool ModeGuided::set_pos_vel_accel_NEU_m(const Vector3p& pos_neu_m, const Vector set_yaw_state_rad(use_yaw, yaw_rad, use_yaw_rate, yaw_rate_rads, relative_yaw); update_time_ms = millis(); - guided_pos_target_neu_m = pos_neu_m; + guided_pos_target_ned_m = pos_ned_m; guided_is_terrain_alt = false; - guided_vel_target_neu_ms = vel_neu_ms; - guided_accel_target_neu_mss = accel_neu_mss; + guided_vel_target_ned_ms = vel_ned_ms; + guided_accel_target_ned_mss = accel_ned_mss; #if HAL_LOGGING_ENABLED // log target - copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_neu_m, guided_is_terrain_alt, guided_vel_target_neu_ms, guided_accel_target_neu_mss); + copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_ned_m, guided_is_terrain_alt, guided_vel_target_ned_ms, guided_accel_target_ned_mss); #endif return true; } @@ -680,7 +680,7 @@ void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_ 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_U_controller(); + pos_control->D_init_controller(); } guided_angle_state.attitude_quat = attitude_quat; @@ -736,8 +736,8 @@ void ModeGuided::pos_control_run() } // calculate terrain adjustments - float terrain_u_m = 0.0f; - if (guided_is_terrain_alt && !wp_nav->get_terrain_U_m(terrain_u_m)) { + float terrain_d_m = 0.0f; + if (guided_is_terrain_alt && !wp_nav->get_terrain_D_m(terrain_d_m)) { // failure to set destination can only be because of missing terrain data copter.failsafe_terrain_on_event(); return; @@ -747,8 +747,8 @@ void ModeGuided::pos_control_run() motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // send position and velocity targets to position controller - guided_accel_target_neu_mss.zero(); - guided_vel_target_neu_ms.zero(); + guided_accel_target_ned_mss.zero(); + guided_vel_target_ned_ms.zero(); // stop rotating if no updates received within timeout_ms if (millis() - update_time_ms > get_timeout_ms()) { @@ -759,13 +759,13 @@ void ModeGuided::pos_control_run() float terrain_margin_m = 0.0; // Vertical buffer size in m if (guided_is_terrain_alt) { - terrain_margin_m = MIN(copter.wp_nav->get_terrain_margin_m(), 0.5 * fabsF(guided_pos_target_neu_m.z)); + terrain_margin_m = MIN(copter.wp_nav->get_terrain_margin_m(), 0.5 * fabsF(guided_pos_target_ned_m.z)); } - pos_control->input_pos_NEU_m(guided_pos_target_neu_m, terrain_u_m, terrain_margin_m); + pos_control->input_pos_NED_m(guided_pos_target_ned_m, terrain_d_m, terrain_margin_m); // run position controllers - pos_control->update_NE_controller(); - pos_control->update_U_controller(); + pos_control->NE_update_controller(); + pos_control->D_update_controller(); // call attitude controller with auto yaw attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading()); @@ -788,29 +788,29 @@ void ModeGuided::accel_control_run() // set velocity to zero and stop rotating if no updates received for 3 seconds uint32_t tnow = millis(); if (tnow - update_time_ms > get_timeout_ms()) { - guided_vel_target_neu_ms.zero(); - guided_accel_target_neu_mss.zero(); + guided_vel_target_ned_ms.zero(); + guided_accel_target_ned_mss.zero(); if ((auto_yaw.mode() == AutoYaw::Mode::RATE) || (auto_yaw.mode() == AutoYaw::Mode::ANGLE_RATE)) { auto_yaw.set_mode(AutoYaw::Mode::HOLD); } - pos_control->input_vel_accel_NE_m(guided_vel_target_neu_ms.xy(), guided_accel_target_neu_mss.xy(), false); - pos_control->input_vel_accel_U_m(guided_vel_target_neu_ms.z, guided_accel_target_neu_mss.z, false); + pos_control->input_vel_accel_NE_m(guided_vel_target_ned_ms.xy(), guided_accel_target_ned_mss.xy(), false); + pos_control->input_vel_accel_D_m(guided_vel_target_ned_ms.z, guided_accel_target_ned_mss.z, false); } else { // update position controller with new target - pos_control->input_accel_NE_m(guided_accel_target_neu_mss); + pos_control->input_accel_NE_m(guided_accel_target_ned_mss.xy()); if (!stabilizing_vel_NE()) { // set position and velocity errors to zero - pos_control->stop_vel_NE_stabilisation(); + pos_control->NE_stop_vel_stabilisation(); } else if (!stabilizing_pos_NE()) { // set position errors to zero - pos_control->stop_pos_NE_stabilisation(); + pos_control->NE_stop_pos_stabilisation(); } - pos_control->input_accel_U_m(guided_accel_target_neu_mss.z); + pos_control->input_accel_D_m(guided_accel_target_ned_mss.z); } // call velocity controller which includes z axis controller - pos_control->update_NE_controller(); - pos_control->update_U_controller(); + pos_control->NE_update_controller(); + pos_control->D_update_controller(); // call attitude controller with auto yaw attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading()); @@ -833,8 +833,8 @@ void ModeGuided::velaccel_control_run() // set velocity to zero and stop rotating if no updates received for 3 seconds uint32_t tnow = millis(); if (tnow - update_time_ms > get_timeout_ms()) { - guided_vel_target_neu_ms.zero(); - guided_accel_target_neu_mss.zero(); + guided_vel_target_ned_ms.zero(); + guided_accel_target_ned_mss.zero(); if ((auto_yaw.mode() == AutoYaw::Mode::RATE) || (auto_yaw.mode() == AutoYaw::Mode::ANGLE_RATE)) { auto_yaw.set_mode(AutoYaw::Mode::HOLD); } @@ -843,7 +843,7 @@ void ModeGuided::velaccel_control_run() bool do_avoid = false; #if AP_AVOIDANCE_ENABLED // limit the velocity for obstacle/fence avoidance - copter.avoid.adjust_velocity_m(guided_vel_target_neu_ms, pos_control->get_pos_NE_p().kP(), pos_control->get_max_accel_NE_mss(), pos_control->get_pos_U_p().kP(), pos_control->get_max_accel_U_mss(), G_Dt); + copter.avoid.adjust_velocity_NED_m(guided_vel_target_ned_ms, pos_control->NE_get_pos_p().kP(), pos_control->NE_get_max_accel_mss(), pos_control->D_get_pos_p().kP(), pos_control->D_get_max_accel_mss(), G_Dt); do_avoid = copter.avoid.limits_active(); #endif @@ -851,21 +851,21 @@ void ModeGuided::velaccel_control_run() if (!stabilizing_vel_NE() && !do_avoid) { // set the current commanded xy vel to the desired vel - guided_vel_target_neu_ms.xy() = pos_control->get_vel_desired_NEU_ms().xy(); + guided_vel_target_ned_ms.xy() = pos_control->get_vel_desired_NED_ms().xy(); } - pos_control->input_vel_accel_NE_m(guided_vel_target_neu_ms.xy(), guided_accel_target_neu_mss.xy(), false); + pos_control->input_vel_accel_NE_m(guided_vel_target_ned_ms.xy(), guided_accel_target_ned_mss.xy(), false); if (!stabilizing_vel_NE() && !do_avoid) { // set position and velocity errors to zero - pos_control->stop_vel_NE_stabilisation(); + pos_control->NE_stop_vel_stabilisation(); } else if (!stabilizing_pos_NE() && !do_avoid) { // set position errors to zero - pos_control->stop_pos_NE_stabilisation(); + pos_control->NE_stop_pos_stabilisation(); } - pos_control->input_vel_accel_U_m(guided_vel_target_neu_ms.z, guided_accel_target_neu_mss.z, false); + pos_control->input_vel_accel_D_m(guided_vel_target_ned_ms.z, guided_accel_target_ned_mss.z, false); // call velocity controller which includes z axis controller - pos_control->update_NE_controller(); - pos_control->update_U_controller(); + pos_control->NE_update_controller(); + pos_control->D_update_controller(); // call attitude controller with auto yaw attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading()); @@ -891,11 +891,11 @@ void ModeGuided::pause_control_run() // set the vertical velocity and acceleration targets to zero float vel_z = 0.0; - pos_control->input_vel_accel_U_m(vel_z, 0.0, false); + pos_control->input_vel_accel_D_m(vel_z, 0.0, false); // call velocity controller which includes z axis controller - pos_control->update_NE_controller(); - pos_control->update_U_controller(); + pos_control->NE_update_controller(); + pos_control->D_update_controller(); // call attitude controller attitude_control->input_thrust_vector_rate_heading_rads(pos_control->get_thrust_vector(), 0.0); @@ -918,8 +918,8 @@ void ModeGuided::posvelaccel_control_run() // set velocity to zero and stop rotating if no updates received for 3 seconds uint32_t tnow = millis(); if (tnow - update_time_ms > get_timeout_ms()) { - guided_vel_target_neu_ms.zero(); - guided_accel_target_neu_mss.zero(); + guided_vel_target_ned_ms.zero(); + guided_accel_target_ned_mss.zero(); if ((auto_yaw.mode() == AutoYaw::Mode::RATE) || (auto_yaw.mode() == AutoYaw::Mode::ANGLE_RATE)) { auto_yaw.set_mode(AutoYaw::Mode::HOLD); } @@ -928,19 +928,19 @@ void ModeGuided::posvelaccel_control_run() // send position and velocity targets to position controller if (!stabilizing_vel_NE()) { // set the current commanded xy pos to the target pos and xy vel to the desired vel - guided_pos_target_neu_m.xy() = pos_control->get_pos_desired_NEU_m().xy(); - guided_vel_target_neu_ms.xy() = pos_control->get_vel_desired_NEU_ms().xy(); + guided_pos_target_ned_m.xy() = pos_control->get_pos_desired_NED_m().xy(); + guided_vel_target_ned_ms.xy() = pos_control->get_vel_desired_NED_ms().xy(); } else if (!stabilizing_pos_NE()) { // set the current commanded xy pos to the target pos - guided_pos_target_neu_m.xy() = pos_control->get_pos_desired_NEU_m().xy(); + guided_pos_target_ned_m.xy() = pos_control->get_pos_desired_NED_m().xy(); } - pos_control->input_pos_vel_accel_NE_m(guided_pos_target_neu_m.xy(), guided_vel_target_neu_ms.xy(), guided_accel_target_neu_mss.xy(), false); + pos_control->input_pos_vel_accel_NE_m(guided_pos_target_ned_m.xy(), guided_vel_target_ned_ms.xy(), guided_accel_target_ned_mss.xy(), false); if (!stabilizing_vel_NE()) { // set position and velocity errors to zero - pos_control->stop_vel_NE_stabilisation(); + pos_control->NE_stop_vel_stabilisation(); } else if (!stabilizing_pos_NE()) { // set position errors to zero - pos_control->stop_pos_NE_stabilisation(); + pos_control->NE_stop_pos_stabilisation(); } // guided_pos_target z-axis should never be a terrain altitude @@ -948,13 +948,13 @@ void ModeGuided::posvelaccel_control_run() INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } - float pz = guided_pos_target_neu_m.z; - pos_control->input_pos_vel_accel_U_m(pz, guided_vel_target_neu_ms.z, guided_accel_target_neu_mss.z, false); - guided_pos_target_neu_m.z = pz; + float pz = guided_pos_target_ned_m.z; + pos_control->input_pos_vel_accel_D_m(pz, guided_vel_target_ned_ms.z, guided_accel_target_ned_mss.z, false); + guided_pos_target_ned_m.z = pz; // run position controllers - pos_control->update_NE_controller(); - pos_control->update_U_controller(); + pos_control->NE_update_controller(); + pos_control->D_update_controller(); // call attitude controller with auto yaw attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading()); @@ -981,7 +981,7 @@ void ModeGuided::angle_control_run() climb_rate_ms = 0.0f; if (guided_angle_state.use_thrust) { // initialise vertical velocity controller - pos_control->init_U_controller(); + pos_control->D_init_controller(); guided_angle_state.use_thrust = false; } } @@ -999,14 +999,14 @@ void ModeGuided::angle_control_run() return; } - // TODO: use get_alt_hold_state_U_ms + // TODO: use get_alt_hold_state_D_ms // landed with positive desired climb rate or thrust, takeoff if (copter.ap.land_complete && positive_thrust_or_climbrate) { 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) { set_land_complete(false); - pos_control->init_U_controller(); + pos_control->D_init_controller(); } return; } @@ -1025,8 +1025,8 @@ void ModeGuided::angle_control_run() if (guided_angle_state.use_thrust) { attitude_control->set_throttle_out(guided_angle_state.thrust_norm, true, copter.g.throttle_filt); } else { - pos_control->set_pos_target_U_from_climb_rate_ms(climb_rate_ms); - pos_control->update_U_controller(); + pos_control->D_set_pos_target_from_climb_rate_ms(climb_rate_ms); + pos_control->D_update_controller(); } } @@ -1080,7 +1080,7 @@ void ModeGuided::limit_init_time_and_pos() guided_limit.start_time_ms = AP_HAL::millis(); // initialise start position from current position - guided_limit.start_pos_neu_m = pos_control->get_pos_estimate_NEU_m(); + guided_limit.start_pos_ned_m = pos_control->get_pos_estimate_NED_m(); } // limit_check - returns true if guided mode has breached a limit @@ -1093,21 +1093,21 @@ bool ModeGuided::limit_check() } // get current location - const Vector3p& curr_pos_neu_m = pos_control->get_pos_estimate_NEU_m(); + const Vector3p& curr_pos_ned_m = pos_control->get_pos_estimate_NED_m(); // check if we have gone below min alt - if (!is_zero(guided_limit.alt_min_m) && (curr_pos_neu_m.z < guided_limit.alt_min_m)) { + if (!is_zero(guided_limit.alt_min_m) && (-curr_pos_ned_m.z < guided_limit.alt_min_m)) { return true; } // check if we have gone above max alt - if (!is_zero(guided_limit.alt_max_m) && (curr_pos_neu_m.z > guided_limit.alt_max_m)) { + if (!is_zero(guided_limit.alt_max_m) && (-curr_pos_ned_m.z > guided_limit.alt_max_m)) { return true; } // check if we have gone beyond horizontal limit if (guided_limit.horiz_max_m > 0.0f) { - const float horiz_offset_m = get_horizontal_distance(guided_limit.start_pos_neu_m.xy(), curr_pos_neu_m.xy()); + const float horiz_offset_m = get_horizontal_distance(guided_limit.start_pos_ned_m.xy(), curr_pos_ned_m.xy()); if (horiz_offset_m > guided_limit.horiz_max_m) { return true; } @@ -1117,19 +1117,19 @@ bool ModeGuided::limit_check() return false; } -const Vector3p &ModeGuided::get_target_pos_NEU_m() const +const Vector3p &ModeGuided::get_target_pos_NED_m() const { - return guided_pos_target_neu_m; + return guided_pos_target_ned_m; } -const Vector3f& ModeGuided::get_target_vel_NEU_ms() const +const Vector3f& ModeGuided::get_target_vel_NED_ms() const { - return guided_vel_target_neu_ms; + return guided_vel_target_ned_ms; } -const Vector3f& ModeGuided::get_target_accel_NEU_mss() const +const Vector3f& ModeGuided::get_target_accel_NED_mss() const { - return guided_accel_target_neu_mss; + return guided_accel_target_ned_mss; } float ModeGuided::wp_distance_m() const @@ -1138,7 +1138,7 @@ float ModeGuided::wp_distance_m() const case SubMode::WP: return wp_nav->get_wp_distance_to_destination_m(); case SubMode::Pos: - return get_horizontal_distance(pos_control->get_pos_estimate_NEU_m().xy().tofloat(), guided_pos_target_neu_m.xy().tofloat()); + return get_horizontal_distance(pos_control->get_pos_estimate_NED_m().xy().tofloat(), guided_pos_target_ned_m.xy().tofloat()); case SubMode::PosVelAccel: return pos_control->get_pos_error_NE_m(); default: @@ -1152,7 +1152,7 @@ float ModeGuided::wp_bearing_deg() const case SubMode::WP: return degrees(wp_nav->get_wp_bearing_to_destination_rad()); case SubMode::Pos: - return degrees(get_bearing_rad(pos_control->get_pos_estimate_NEU_m().xy().tofloat(), guided_pos_target_neu_m.xy().tofloat())); + return degrees(get_bearing_rad(pos_control->get_pos_estimate_NED_m().xy().tofloat(), guided_pos_target_ned_m.xy().tofloat())); case SubMode::PosVelAccel: return degrees(pos_control->get_bearing_to_target_rad()); case SubMode::TakeOff: diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 8a2868d7631f4..1d59ff58fe6ca 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -1,5 +1,63 @@ #include "Copter.h" +// table of user settable parameters +const AP_Param::GroupInfo ModeLand::var_info[] = { + + // @Param: SPD_MS + // @DisplayName: Land speed + // @Description: The descent speed for the final stage of landing in m/s + // @Units: m/s + // @Range: 0.3 2 + // @Increment: 0.1 + // @User: Standard + AP_GROUPINFO("SPD_MS", 1, ModeLand, land_speed_ms, LAND_SPD_MS_DEFAULT), + + // @Param: SPD_HIGH_MS + // @DisplayName: Land speed high + // @Description: The descent speed for the first stage of landing in m/s. If this is zero then WPNAV_SPEED_DN is used + // @Units: m/s + // @Range: 0 5 + // @Increment: 0.1 + // @User: Standard + AP_GROUPINFO("SPD_HIGH_MS", 2, ModeLand, land_speed_high_ms, 0), + + // @Param: ALT_LOW_M + // @DisplayName: Land alt low + // @Description: Altitude during Landing at which vehicle slows to LAND_SPD_MS + // @Units: m + // @Range: 1 100 + // @Increment: 0.1 + // @User: Advanced + AP_GROUPINFO("ALT_LOW_M", 3, ModeLand, land_alt_low_m, 10), + + AP_GROUPEND +}; + +// constructor +ModeLand::ModeLand() : Mode() +{ + // load parameter defaults + AP_Param::setup_object_defaults(this, var_info); +} + +// convert parameters +void ModeLand::convert_params() +{ + // PARAMETER_CONVERSION - Added: Jan 2026 + + // return immediately if parameter conversion has already been performed + if (land_speed_ms.configured() || land_speed_high_ms.configured() || land_alt_low_m.configured()) { + return; + } + + static const AP_Param::ConversionInfo conversion_info[] = { + { Parameters::k_param_land_speed_cms, 0, AP_PARAM_INT16, "LAND_SPD_MS" }, // LAND_SPEED moved to LAND_SPD_MS + { Parameters::k_param_land_speed_high_cms, 0, AP_PARAM_INT16, "LAND_SPD_HIGH_MS" }, // LAND_SPEED_HIGH moved to LAND_SPD_HIGH_MS + { Parameters::k_param_g2, 25, AP_PARAM_INT16, "LAND_ALT_LOW_M" }, // LAND_ALT_LOW moved to LAND_ALT_LOW_M + }; + AP_Param::convert_old_parameters_scaled(conversion_info, ARRAY_SIZE(conversion_info), 0.01, 0); +} + // land_init - initialise land controller bool ModeLand::init(bool ignore_checks) { @@ -7,21 +65,21 @@ bool ModeLand::init(bool ignore_checks) control_position = copter.position_ok(); // set horizontal speed and acceleration limits - pos_control->set_max_speed_accel_NE_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); - pos_control->set_correction_speed_accel_NE_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); + pos_control->NE_set_max_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); + pos_control->NE_set_correction_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); // initialise the horizontal position controller - if (control_position && !pos_control->is_active_NE()) { - pos_control->init_NE_controller(); + if (control_position && !pos_control->NE_is_active()) { + pos_control->NE_init_controller(); } // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_U_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_U_mss()); - pos_control->set_correction_speed_accel_U_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_U_mss()); + pos_control->D_set_max_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss()); + pos_control->D_set_correction_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss()); // initialise the vertical position controller - if (!pos_control->is_active_U()) { - pos_control->init_U_controller(); + if (!pos_control->D_is_active()) { + pos_control->D_init_controller(); } land_start_time = millis(); diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 91e8cbbc5e746..bc2cceb64d790 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -22,13 +22,13 @@ bool ModeLoiter::init(bool ignore_checks) loiter_nav->init_target(); // initialise the vertical position controller - if (!pos_control->is_active_U()) { - pos_control->init_U_controller(); + if (!pos_control->D_is_active()) { + pos_control->D_init_controller(); } // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_U_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_U_mss()); - pos_control->set_correction_speed_accel_U_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_U_mss()); + pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss()); + pos_control->D_set_correction_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss()); #if AC_PRECLAND_ENABLED _precision_loiter_active = false; @@ -62,16 +62,16 @@ void ModeLoiter::precision_loiter_xy() Vector2p target_pos_ne_m; Vector2f target_vel_ne_ms; if (!copter.precland.get_target_position_m(target_pos_ne_m)) { - target_pos_ne_m = pos_control->get_pos_estimate_NEU_m().xy(); + target_pos_ne_m = pos_control->get_pos_estimate_NED_m().xy(); } // get the velocity of the target - copter.precland.get_target_velocity_ms(pos_control->get_vel_estimate_NEU_ms().xy(), target_vel_ne_ms); + copter.precland.get_target_velocity_ms(pos_control->get_vel_estimate_NED_ms().xy(), target_vel_ne_ms); Vector2f zero; // target vel will remain zero if landing target is stationary pos_control->input_pos_vel_accel_NE_m(target_pos_ne_m, target_vel_ne_ms, zero); // run pos controller - pos_control->update_NE_controller(); + pos_control->NE_update_controller(); } #endif @@ -84,7 +84,7 @@ void ModeLoiter::run() float target_climb_rate_ms = 0.0f; // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_U_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_U_mss()); + pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss()); // apply SIMPLE mode transform to pilot inputs update_simple_mode(); @@ -108,7 +108,7 @@ void ModeLoiter::run() } // Loiter State Machine Determination - AltHoldModeState loiter_state = get_alt_hold_state_U_ms(target_climb_rate_ms); + AltHoldModeState loiter_state = get_alt_hold_state_D_ms(target_climb_rate_ms); // Loiter State Machine switch (loiter_state) { @@ -116,7 +116,7 @@ void ModeLoiter::run() case AltHoldModeState::MotorStopped: attitude_control->reset_rate_controller_I_terms(); attitude_control->reset_yaw_target_and_rate(); - pos_control->relax_U_controller(0.0f); // forces throttle output to decay to zero + pos_control->D_relax_controller(0.0f); // forces throttle output to decay to zero loiter_nav->init_target(); break; @@ -127,7 +127,7 @@ void ModeLoiter::run() case AltHoldModeState::Landed_Pre_Takeoff: attitude_control->reset_rate_controller_I_terms_smoothly(); loiter_nav->init_target(); - pos_control->relax_U_controller(0.0f); // forces throttle output to decay to zero + pos_control->D_relax_controller(0.0f); // forces throttle output to decay to zero break; case AltHoldModeState::Takeoff: @@ -180,14 +180,14 @@ void ModeLoiter::run() #endif // Send the commanded climb rate to the position controller - pos_control->set_pos_target_U_from_climb_rate_ms(target_climb_rate_ms); + pos_control->D_set_pos_target_from_climb_rate_ms(target_climb_rate_ms); break; } // call attitude controller attitude_control->input_thrust_vector_rate_heading_rads(loiter_nav->get_thrust_vector(), target_yaw_rate_rads, false); // run the vertical position controller and set output throttle - pos_control->update_U_controller(); + pos_control->D_update_controller(); } float ModeLoiter::wp_distance_m() const diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index 60cacf0c5a0e9..5fdc1bc48a1dc 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -25,12 +25,12 @@ bool ModePosHold::init(bool ignore_checks) { // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_U_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_U_mss()); - pos_control->set_correction_speed_accel_U_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_U_mss()); + pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss()); + pos_control->D_set_correction_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss()); // initialise the vertical position controller - if (!pos_control->is_active_U()) { - pos_control->init_U_controller(); + if (!pos_control->D_is_active()) { + pos_control->D_init_controller(); } // initialise lean angles to current attitude @@ -67,7 +67,7 @@ void ModePosHold::run() const uint32_t now_ms = AP_HAL::millis(); float controller_to_pilot_roll_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls float controller_to_pilot_pitch_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls - const Vector3f& vel_neu_ms = pos_control->get_vel_estimate_NEU_ms(); + const Vector3f& vel_ned_ms = pos_control->get_vel_estimate_NED_ms(); // enforce minimum allowed value for poshold_brake_rate_degs if (g.poshold_brake_rate_degs < POSHOLD_BRAKE_RATE_MIN) { @@ -75,7 +75,7 @@ void ModePosHold::run() } // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_U_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_U_mss()); + pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss()); loiter_nav->clear_pilot_desired_acceleration(); // apply SIMPLE mode transform to pilot inputs @@ -98,14 +98,14 @@ void ModePosHold::run() } // state machine - AltHoldModeState poshold_state = get_alt_hold_state_U_ms(target_climb_rate_ms); + AltHoldModeState poshold_state = get_alt_hold_state_D_ms(target_climb_rate_ms); switch (poshold_state) { case AltHoldModeState::MotorStopped: attitude_control->reset_rate_controller_I_terms(); attitude_control->reset_yaw_target_and_rate(false); - pos_control->relax_U_controller(0.0f); // forces throttle output to decay to zero + pos_control->D_relax_controller(0.0f); // forces throttle output to decay to zero loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); @@ -126,7 +126,7 @@ void ModePosHold::run() case AltHoldModeState::Landed_Pre_Takeoff: attitude_control->reset_rate_controller_I_terms_smoothly(); - pos_control->relax_U_controller(0.0f); // forces throttle output to decay to zero + pos_control->D_relax_controller(0.0f); // forces throttle output to decay to zero // set poshold state to pilot override roll_mode = RPMode::PILOT_OVERRIDE; @@ -166,13 +166,13 @@ void ModePosHold::run() #endif // Send the commanded climb rate to the position controller - pos_control->set_pos_target_U_from_climb_rate_ms(target_climb_rate_ms); + pos_control->D_set_pos_target_from_climb_rate_ms(target_climb_rate_ms); break; } // convert earth-frame velocities to body-frame (m/s) - float vel_fw_ms = vel_neu_ms.x * ahrs.cos_yaw() + vel_neu_ms.y * ahrs.sin_yaw(); - float vel_right_ms = -vel_neu_ms.x * ahrs.sin_yaw() + vel_neu_ms.y * ahrs.cos_yaw(); + float vel_fw_ms = vel_ned_ms.x * ahrs.cos_yaw() + vel_ned_ms.y * ahrs.sin_yaw(); + float vel_right_ms = -vel_ned_ms.x * ahrs.sin_yaw() + vel_ned_ms.y * ahrs.cos_yaw(); // If not in LOITER, retrieve latest wind compensation lean angles (radians) related to current yaw if (roll_mode != RPMode::LOITER || pitch_mode != RPMode::LOITER) { @@ -378,7 +378,7 @@ void ModePosHold::run() pitch_mode = RPMode::BRAKE_TO_LOITER; brake.loiter_transition_start_time_ms = now_ms; // init loiter controller - loiter_nav->init_target_m((pos_control->get_pos_estimate_NEU_m().xy() - pos_control->get_pos_offset_NEU_m().xy())); + loiter_nav->init_target_m((pos_control->get_pos_estimate_NED_m().xy() - pos_control->get_pos_offset_NED_m().xy())); // set delay to start of wind compensation estimate updates wind_comp_start_time_ms = now_ms; } @@ -487,7 +487,7 @@ void ModePosHold::run() attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(roll_rad, pitch_rad, target_yaw_rate_rads); // run the vertical position controller and set output throttle - pos_control->update_U_controller(); + pos_control->D_update_controller(); } // poshold_update_pilot_lean_angle - update the pilot's filtered lean angle with the latest raw input received @@ -558,27 +558,27 @@ void ModePosHold::update_wind_comp_estimate() } // check horizontal velocity is low - if (pos_control->get_vel_estimate_NEU_ms().xy().length() > POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX_MS) { + if (pos_control->get_vel_estimate_NED_ms().xy().length() > POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX_MS) { return; } // get position controller accel target - const Vector3f& accel_target_neu_mss = pos_control->get_accel_target_NEU_mss(); + const Vector3f& accel_target_ned_mss = pos_control->get_accel_target_NED_mss(); // update wind compensation in earth-frame lean angles if (is_zero(wind_comp_ne_mss.x)) { // if wind compensation has not been initialised set it immediately to the pos controller's desired accel in north direction - wind_comp_ne_mss.x = accel_target_neu_mss.x; + wind_comp_ne_mss.x = accel_target_ned_mss.x; } else { // low pass filter the position controller's lean angle output - wind_comp_ne_mss.x = (1.0f - TC_WIND_COMP) * wind_comp_ne_mss.x + TC_WIND_COMP * accel_target_neu_mss.x; + wind_comp_ne_mss.x = (1.0f - TC_WIND_COMP) * wind_comp_ne_mss.x + TC_WIND_COMP * accel_target_ned_mss.x; } if (is_zero(wind_comp_ne_mss.y)) { // if wind compensation has not been initialised set it immediately to the pos controller's desired accel in north direction - wind_comp_ne_mss.y = accel_target_neu_mss.y; + wind_comp_ne_mss.y = accel_target_ned_mss.y; } else { // low pass filter the position controller's lean angle output - wind_comp_ne_mss.y = (1.0f - TC_WIND_COMP) * wind_comp_ne_mss.y + TC_WIND_COMP * accel_target_neu_mss.y; + wind_comp_ne_mss.y = (1.0f - TC_WIND_COMP) * wind_comp_ne_mss.y + TC_WIND_COMP * accel_target_ned_mss.y; } // limit acceleration diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 15a4a29a7564a..3b1f4eee0a8b5 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -2,6 +2,74 @@ #if MODE_RTL_ENABLED +// table of user settable parameters +const AP_Param::GroupInfo ModeRTL::var_info[] = { + + // @Param: ALT_M + // @DisplayName: RTL Altitude + // @Description: The minimum alt above home the vehicle will climb to before returning. If the vehicle is flying higher than this value it will return at its current altitude. + // @Units: m + // @Range: 0.30 3000 + // @Increment: 0.1 + // @User: Standard + AP_GROUPINFO("ALT_M", 1, ModeRTL, altitude_m, RTL_ALT_M_DEFAULT), + + // @Param: ALT_FINAL_M + // @DisplayName: RTL Final Altitude + // @Description: Altitude the vehicle will move to as the final stage of Returning to Launch or after completing a mission. Set to zero to land. + // @Units: m + // @Range: 0 10 + // @Increment: 0.1 + // @User: Standard + AP_GROUPINFO("ALT_FINAL_M", 2, ModeRTL, alt_final_m, RTL_ALT_FINAL_M_DEFAULT), + + // @Param: CLIMB_MIN_M + // @DisplayName: RTL minimum climb + // @Description: The vehicle will climb this many meters during the initial climb portion of the RTL + // @Units: m + // @Range: 0 30 + // @Increment: 0.1 + // @User: Standard + AP_GROUPINFO("CLIMB_MIN_M", 3, ModeRTL, climb_min_m, RTL_CLIMB_MIN_M_DEFAULT), + + // @Param: SPEED_MS + // @DisplayName: RTL speed + // @Description: The speed in m/s which the aircraft will attempt to maintain horizontally while flying home. If this is set to zero, WPNAV_SPEED will be used instead. + // @Units: m/s + // @Range: 0 20 + // @Increment: 0.5 + // @User: Standard + AP_GROUPINFO("SPEED_MS", 4, ModeRTL, speed_ms, 0), + + AP_GROUPEND +}; + +// constructor +ModeRTL::ModeRTL() : Mode() +{ + // load parameter defaults + AP_Param::setup_object_defaults(this, var_info); +} + +// convert parameters +void ModeRTL::convert_params() +{ + // PARAMETER_CONVERSION - Added: Jan 2026 + + // return immediately if parameter conversion has already been performed + if (altitude_m.configured() || speed_ms.configured() || alt_final_m.configured() || climb_min_m.configured()) { + return; + } + + static const AP_Param::ConversionInfo conversion_info[] = { + { Parameters::k_param_rtl_altitude_cm, 0, AP_PARAM_INT32, "RTL_ALT_M" }, // RTL_ALT moved to RTL_ALT_M + { Parameters::k_param_rtl_speed_cms, 0, AP_PARAM_INT16, "RTL_SPEED_MS" }, // RTL_SPEED moved to RTL_SPEED_MS + { Parameters::k_param_rtl_alt_final_cm, 0, AP_PARAM_INT16, "RTL_ALT_FINAL_M" }, // RTL_ALT_FINAL moved to RTL_ALT_FINAL_M + { Parameters::k_param_rtl_climb_min_cm, 0, AP_PARAM_INT16, "RTL_CLIMB_MIN_M" }, // RTL_CLIMB_MIN moved to RTL_CLIMB_MIN_M + }; + AP_Param::convert_old_parameters_scaled(conversion_info, ARRAY_SIZE(conversion_info), 0.01, 0); +} + /* * Init and run calls for RTL flight mode * @@ -18,7 +86,7 @@ bool ModeRTL::init(bool ignore_checks) } } // initialise waypoint and spline controller - wp_nav->wp_and_spline_init_m(g.rtl_speed_cms * 0.01); + wp_nav->wp_and_spline_init_m(speed_ms.get()); _state = SubMode::STARTING; _state_complete = true; // see run() method below terrain_following_allowed = !copter.failsafe.terrain; @@ -175,7 +243,7 @@ void ModeRTL::climb_return_run() // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle - pos_control->update_U_controller(); + pos_control->D_update_controller(); // call attitude controller with auto yaw attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading()); @@ -217,7 +285,7 @@ void ModeRTL::loiterathome_run() // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle - pos_control->update_U_controller(); + pos_control->D_update_controller(); // call attitude controller with auto yaw attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading()); @@ -244,7 +312,7 @@ void ModeRTL::descent_start() _state_complete = false; // initialise altitude target to stopping point - pos_control->init_U_controller_stopping_point(); + pos_control->D_init_controller_stopping_point(); // initialise yaw auto_yaw.set_mode(AutoYaw::Mode::HOLD); @@ -255,7 +323,7 @@ void ModeRTL::descent_start() #endif } -// rtl_descent_run - implements the final descent to the RTL_ALT +// rtl_descent_run - implements the final descent to the RTL_ALT_M // called by rtl_run at 100hz or more void ModeRTL::descent_run() { @@ -299,12 +367,12 @@ void ModeRTL::descent_run() Vector2f accel; pos_control->input_vel_accel_NE_m(vel_correction_ms, accel); - pos_control->update_NE_controller(); + pos_control->NE_update_controller(); // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle - pos_control->set_alt_target_with_slew_m(rtl_path.descent_target.alt * 0.01); - pos_control->update_U_controller(); + pos_control->D_set_alt_target_with_slew_m(rtl_path.descent_target.alt * 0.01); + pos_control->D_update_controller(); // roll & pitch from waypoint controller, yaw rate from pilot attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading()); @@ -320,17 +388,17 @@ void ModeRTL::land_start() _state_complete = false; // set horizontal speed and acceleration limits - pos_control->set_max_speed_accel_NE_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); - pos_control->set_correction_speed_accel_NE_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); + pos_control->NE_set_max_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); + pos_control->NE_set_correction_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); // initialise loiter target destination - if (!pos_control->is_active_NE()) { - pos_control->init_NE_controller(); + if (!pos_control->NE_is_active()) { + pos_control->NE_init_controller(); } // initialise the vertical position controller - if (!pos_control->is_active_U()) { - pos_control->init_U_controller(); + if (!pos_control->D_is_active()) { + pos_control->D_init_controller(); } // initialise yaw @@ -384,14 +452,14 @@ void ModeRTL::build_path() // climb target is above our origin point at the return altitude rtl_path.climb_target = Location(rtl_path.origin_point.lat, rtl_path.origin_point.lng, rtl_path.return_target.alt, rtl_path.return_target.get_alt_frame()); - // descent target is below return target at rtl_alt_final_cm - rtl_path.descent_target = Location(rtl_path.return_target.lat, rtl_path.return_target.lng, g.rtl_alt_final_cm, Location::AltFrame::ABOVE_HOME); + // descent target is below return target at rtl_alt_final_m + rtl_path.descent_target = Location(rtl_path.return_target.lat, rtl_path.return_target.lng, alt_final_m.get() * 100, Location::AltFrame::ABOVE_HOME); // Target altitude is passed directly to the position controller so must be relative to origin rtl_path.descent_target.change_alt_frame(Location::AltFrame::ABOVE_ORIGIN); // set land flag - rtl_path.land = g.rtl_alt_final_cm <= 0; + rtl_path.land = alt_final_m.get() <= 0; } // compute the return target - home or rally point @@ -409,7 +477,7 @@ void ModeRTL::compute_return_target() // get position controller Z-axis offset in cm above EKF origin float pos_offset_u_m = pos_control->get_pos_offset_U_m(); - // curr_alt_m is current altitude above home or above terrain depending upon use_terrain + // curr_alt_m is current altitude, with any offset removed, above home or above terrain depending upon use_terrain float curr_alt_m = copter.current_loc.alt * 0.01 - pos_offset_u_m; // determine altitude type of return journey (alt-above-home, alt-above-terrain using range finder or alt-above-terrain using terrain database) @@ -434,10 +502,10 @@ void ModeRTL::compute_return_target() // set curr_alt_m and return_target.alt from range finder if (alt_type == ReturnTargetAltType::RANGEFINDER) { if (copter.get_rangefinder_height_interpolated_m(curr_alt_m)) { - // subtract position controller offset + // subtract vertical offset from altitude. curr_alt_m -= pos_offset_u_m; // set return_target.alt - rtl_path.return_target.set_alt_m(MAX(curr_alt_m + MAX(0.0, g.rtl_climb_min_cm * 0.01), MAX(g.rtl_altitude_cm * 0.01, RTL_ALT_MIN_M)), Location::AltFrame::ABOVE_TERRAIN); + rtl_path.return_target.set_alt_m(MAX(curr_alt_m + MAX(0.0f, climb_min_m.get()), MAX(altitude_m.get(), RTL_ALT_MIN_M)), Location::AltFrame::ABOVE_TERRAIN); } else { // fallback to relative alt and warn user alt_type = ReturnTargetAltType::RELATIVE; @@ -454,6 +522,7 @@ void ModeRTL::compute_return_target() float curr_terr_alt_m; if (copter.current_loc.get_alt_m(Location::AltFrame::ABOVE_TERRAIN, curr_terr_alt_m) && rtl_path.return_target.change_alt_frame(Location::AltFrame::ABOVE_TERRAIN)) { + // subtract vertical offset from altitude. curr_alt_m = curr_terr_alt_m - pos_offset_u_m; } else { // fallback to relative alt and warn user @@ -478,8 +547,8 @@ void ModeRTL::compute_return_target() float target_alt_m = MAX(rtl_path.return_target.alt, 0) * 0.01; // increase target to maximum of current altitude + climb_min and rtl altitude - const float min_rtl_alt_m = MAX(RTL_ALT_MIN_M, curr_alt_m + MAX(0.0, g.rtl_climb_min_cm * 0.01)); - target_alt_m = MAX(target_alt_m, MAX(g.rtl_altitude_cm * 0.01, min_rtl_alt_m)); + const float min_rtl_alt_m = MAX(RTL_ALT_MIN_M, curr_alt_m + MAX(0.0f, climb_min_m.get())); + target_alt_m = MAX(target_alt_m, MAX(altitude_m.get(), min_rtl_alt_m)); // reduce climb if close to return target float rtl_return_dist_m = rtl_path.return_target.get_distance(rtl_path.origin_point); diff --git a/ArduCopter/mode_smart_rtl.cpp b/ArduCopter/mode_smart_rtl.cpp index 54a5064f228f5..0f715b09f76cb 100644 --- a/ArduCopter/mode_smart_rtl.cpp +++ b/ArduCopter/mode_smart_rtl.cpp @@ -16,10 +16,10 @@ bool ModeSmartRTL::init(bool ignore_checks) wp_nav->wp_and_spline_init_m(); // set current target to a reasonable stopping point - Vector3p stopping_point_neu_m; - pos_control->get_stopping_point_NE_m(stopping_point_neu_m.xy()); - pos_control->get_stopping_point_U_m(stopping_point_neu_m.z); - wp_nav->set_wp_destination_NEU_m(stopping_point_neu_m); + Vector3p stopping_point_ned_m; + pos_control->get_stopping_point_NE_m(stopping_point_ned_m.xy()); + pos_control->get_stopping_point_D_m(stopping_point_ned_m.z); + wp_nav->set_wp_destination_NED_m(stopping_point_ned_m); // initialise yaw to obey user parameter auto_yaw.set_mode_to_default(true); @@ -77,7 +77,7 @@ void ModeSmartRTL::wait_cleanup_run() // hover at current target position motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); wp_nav->update_wpnav(); - pos_control->update_U_controller(); + pos_control->D_update_controller(); attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading()); // check if return path is computed and if yes, begin journey home @@ -143,7 +143,7 @@ void ModeSmartRTL::path_follow_run() // update controllers motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); wp_nav->update_wpnav(); - pos_control->update_U_controller(); + pos_control->D_update_controller(); // call attitude controller with auto yaw attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading()); @@ -154,11 +154,11 @@ void ModeSmartRTL::pre_land_position_run() // if we are close to 2m above start point, we are ready to land. if (wp_nav->reached_wp_destination()) { // choose descend and hold, or land based on user parameter rtl_alt_final_cm - if (g.rtl_alt_final_cm <= 0 || copter.failsafe.radio) { + if (get_alt_final_m() <= 0 || copter.failsafe.radio) { land_start(); smart_rtl_state = SubMode::LAND; } else { - set_descent_target_alt(copter.g.rtl_alt_final_cm); + set_descent_target_alt(get_alt_final_m() * 100); descent_start(); smart_rtl_state = SubMode::DESCEND; } @@ -167,7 +167,7 @@ void ModeSmartRTL::pre_land_position_run() // update controllers motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); wp_nav->update_wpnav(); - pos_control->update_U_controller(); + pos_control->D_update_controller(); attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading()); } diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index 4a58794ca7d4f..c1c0cb3b08a60 100644 --- a/ArduCopter/mode_sport.cpp +++ b/ArduCopter/mode_sport.cpp @@ -10,12 +10,12 @@ bool ModeSport::init(bool ignore_checks) { // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_U_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_U_mss()); - pos_control->set_correction_speed_accel_U_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_U_mss()); + pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss()); + pos_control->D_set_correction_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss()); // initialise the vertical position controller - if (!pos_control->is_active_U()) { - pos_control->init_U_controller(); + if (!pos_control->D_is_active()) { + pos_control->D_init_controller(); } return true; @@ -26,7 +26,7 @@ bool ModeSport::init(bool ignore_checks) void ModeSport::run() { // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_U_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_U_mss()); + pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss()); // apply SIMPLE mode transform update_simple_mode(); @@ -68,7 +68,7 @@ void ModeSport::run() target_climb_rate_ms = constrain_float(target_climb_rate_ms, -get_pilot_speed_dn_ms(), get_pilot_speed_up_ms()); // Sport State Machine Determination - AltHoldModeState sport_state = get_alt_hold_state_U_ms(target_climb_rate_ms); + AltHoldModeState sport_state = get_alt_hold_state_D_ms(target_climb_rate_ms); // State Machine switch (sport_state) { @@ -76,7 +76,7 @@ void ModeSport::run() case AltHoldModeState::MotorStopped: attitude_control->reset_rate_controller_I_terms(); attitude_control->reset_yaw_target_and_rate(false); - pos_control->relax_U_controller(0.0f); // forces throttle output to decay to zero + pos_control->D_relax_controller(0.0f); // forces throttle output to decay to zero break; case AltHoldModeState::Landed_Ground_Idle: @@ -85,7 +85,7 @@ void ModeSport::run() case AltHoldModeState::Landed_Pre_Takeoff: attitude_control->reset_rate_controller_I_terms_smoothly(); - pos_control->relax_U_controller(0.0f); // forces throttle output to decay to zero + pos_control->D_relax_controller(0.0f); // forces throttle output to decay to zero break; case AltHoldModeState::Takeoff: @@ -113,7 +113,7 @@ void ModeSport::run() #endif // Send the commanded climb rate to the position controller - pos_control->set_pos_target_U_from_climb_rate_ms(target_climb_rate_ms); + pos_control->D_set_pos_target_from_climb_rate_ms(target_climb_rate_ms); break; } @@ -121,7 +121,7 @@ void ModeSport::run() attitude_control->input_euler_rate_roll_pitch_yaw_rads(target_roll_rads, target_pitch_rads, target_yaw_rads); // run the vertical position controller and set output throttle - pos_control->update_U_controller(); + pos_control->D_update_controller(); } #endif diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp index ef1eb9794621e..a570e40bf753b 100644 --- a/ArduCopter/mode_systemid.cpp +++ b/ArduCopter/mode_systemid.cpp @@ -112,23 +112,23 @@ bool ModeSystemId::init(bool ignore_checks) } // set horizontal speed and acceleration limits - pos_control->set_max_speed_accel_NE_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); - pos_control->set_correction_speed_accel_NE_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); + pos_control->NE_set_max_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); + pos_control->NE_set_correction_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss()); // initialise the horizontal position controller - if (!pos_control->is_active_NE()) { - pos_control->init_NE_controller(); + if (!pos_control->NE_is_active()) { + pos_control->NE_init_controller(); } // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_U_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_U_mss()); - pos_control->set_correction_speed_accel_U_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_U_mss()); + pos_control->D_set_max_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss()); + pos_control->D_set_correction_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss()); // initialise the vertical position controller - if (!pos_control->is_active_U()) { - pos_control->init_U_controller(); + if (!pos_control->D_is_active()) { + pos_control->D_init_controller(); } - target_pos_ne_m = pos_control->get_pos_estimate_NEU_m().xy(); + target_pos_ne_m = pos_control->get_pos_estimate_NED_m().xy(); } att_bf_feedforward = attitude_control->get_bf_feedforward(); @@ -357,7 +357,7 @@ void ModeSystemId::run() // relax loiter target if we might be landed if (copter.ap.land_complete_maybe) { - pos_control->soften_for_landing_NE(); + pos_control->NE_soften_for_landing(); } Vector2f accel_ne_mss; @@ -369,16 +369,16 @@ void ModeSystemId::run() pos_control->set_pos_vel_accel_NE_m(target_pos_ne_m, input_vel_ne_ms, accel_ne_mss); // run pos controller - pos_control->update_NE_controller(); + pos_control->NE_update_controller(); // call attitude controller attitude_control->input_thrust_vector_rate_heading_rads(pos_control->get_thrust_vector(), target_yaw_rate_rads, false); // Send the commanded climb rate to the position controller - pos_control->set_pos_target_U_from_climb_rate_ms(target_climb_rate_ms); + pos_control->D_set_pos_target_from_climb_rate_ms(target_climb_rate_ms); // run the vertical position controller and set output throttle - pos_control->update_U_controller(); + pos_control->D_update_controller(); } if (log_subsample <= 0) { @@ -418,8 +418,8 @@ void ModeSystemId::log_data() const if (is_poscontrol_axis_type()) { pos_control->write_log(); - copter.logger.Write_PID(LOG_PIDN_MSG, pos_control->get_vel_NE_pid().get_pid_info_x()); - copter.logger.Write_PID(LOG_PIDE_MSG, pos_control->get_vel_NE_pid().get_pid_info_y()); + copter.logger.Write_PID(LOG_PIDN_MSG, pos_control->NE_get_vel_pid().get_pid_info_x()); + copter.logger.Write_PID(LOG_PIDE_MSG, pos_control->NE_get_vel_pid().get_pid_info_y()); } } diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp index 82f4397cda7fd..911207fa7e35a 100644 --- a/ArduCopter/mode_throw.cpp +++ b/ArduCopter/mode_throw.cpp @@ -20,12 +20,12 @@ bool ModeThrow::init(bool ignore_checks) nextmode_attempted = false; // initialise pos controller speed and acceleration - pos_control->set_max_speed_accel_NE_m(wp_nav->get_default_speed_NE_ms(), BRAKE_MODE_DECEL_RATE_MSS); - pos_control->set_correction_speed_accel_NE_m(wp_nav->get_default_speed_NE_ms(), BRAKE_MODE_DECEL_RATE_MSS); + pos_control->NE_set_max_speed_accel_m(wp_nav->get_default_speed_NE_ms(), BRAKE_MODE_DECEL_RATE_MSS); + pos_control->NE_set_correction_speed_accel_m(wp_nav->get_default_speed_NE_ms(), BRAKE_MODE_DECEL_RATE_MSS); // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_U_m(BRAKE_MODE_SPEED_Z_MS, BRAKE_MODE_SPEED_Z_MS, BRAKE_MODE_DECEL_RATE_MSS); - pos_control->set_correction_speed_accel_U_m(BRAKE_MODE_SPEED_Z_MS, BRAKE_MODE_SPEED_Z_MS, BRAKE_MODE_DECEL_RATE_MSS); + pos_control->D_set_max_speed_accel_m(BRAKE_MODE_SPEED_Z_MS, BRAKE_MODE_SPEED_Z_MS, BRAKE_MODE_DECEL_RATE_MSS); + pos_control->D_set_correction_speed_accel_m(BRAKE_MODE_SPEED_Z_MS, BRAKE_MODE_SPEED_Z_MS, BRAKE_MODE_DECEL_RATE_MSS); return true; } @@ -67,7 +67,7 @@ void ModeThrow::run() stage = Throw_HgtStabilise; // initialise the z controller - pos_control->init_U_controller_no_descent(); + pos_control->D_init_controller_no_descent(); // initialise the demanded height below/above the throw height from user parameters // this allows for rapidly clearing surrounding obstacles @@ -85,7 +85,7 @@ void ModeThrow::run() stage = Throw_PosHold; // initialise position controller - pos_control->init_NE_controller(); + pos_control->NE_init_controller(); // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum copter.set_auto_armed(true); @@ -174,8 +174,8 @@ void ModeThrow::run() attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(0.0f, 0.0f, 0.0f); // call height controller - pos_control->set_pos_target_U_from_climb_rate_ms(0.0f); - pos_control->update_U_controller(); + pos_control->D_set_pos_target_from_climb_rate_ms(0.0f); + pos_control->D_update_controller(); break; @@ -188,14 +188,14 @@ void ModeThrow::run() Vector2f vel_zero; Vector2f accel_zero; pos_control->input_vel_accel_NE_m(vel_zero, accel_zero); - pos_control->update_NE_controller(); + pos_control->NE_update_controller(); // call attitude controller attitude_control->input_thrust_vector_rate_heading_rads(pos_control->get_thrust_vector(), 0.0f); // call height controller - pos_control->set_pos_target_U_from_climb_rate_ms(0.0f); - pos_control->update_U_controller(); + pos_control->D_set_pos_target_from_climb_rate_ms(0.0f); + pos_control->D_update_controller(); break; } @@ -206,7 +206,7 @@ void ModeThrow::run() if ((stage != prev_stage) || (now - last_log_ms) > 100) { prev_stage = stage; last_log_ms = now; - const float velocity_ms = pos_control->get_vel_estimate_NEU_ms().length(); + const float velocity_ms = pos_control->get_vel_estimate_NED_ms().length(); const float velocity_z_ms = pos_control->get_vel_estimate_U_ms(); const float accel = copter.ins.get_accel().length(); const float ef_accel_z = ahrs.get_accel_ef().z; @@ -263,7 +263,7 @@ bool ModeThrow::throw_detected() } // Check for high speed ( >5 m/s) - bool high_speed = pos_control->get_vel_estimate_NEU_ms().length_squared() > (THROW_HIGH_SPEED_MS * THROW_HIGH_SPEED_MS); + bool high_speed = pos_control->get_vel_estimate_NED_ms().length_squared() > (THROW_HIGH_SPEED_MS * THROW_HIGH_SPEED_MS); // check for upwards or downwards trajectory (airdrop) of 0.50 m/s bool changing_height; @@ -318,7 +318,7 @@ bool ModeThrow::throw_attitude_good() const bool ModeThrow::throw_height_good() const { // Check that we are within 0.5m of the demanded height - return (pos_control->get_pos_error_U_m() < 0.5); + return (fabsf(pos_control->get_pos_error_D_m()) < 0.5); } bool ModeThrow::throw_position_good() const diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index f98089267dbfe..3077d7921bfb2 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -80,12 +80,12 @@ bool ModeZigZag::init(bool ignore_checks) loiter_nav->init_target(); // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_U_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_U_mss()); - pos_control->set_correction_speed_accel_U_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_U_mss()); + pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss()); + pos_control->D_set_correction_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss()); // initialise the vertical position controller - if (!pos_control->is_active_U()) { - pos_control->init_U_controller(); + if (!pos_control->D_is_active()) { + pos_control->D_init_controller(); } // initialise waypoint state @@ -111,7 +111,7 @@ void ModeZigZag::exit() void ModeZigZag::run() { // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_U_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_U_mss()); + pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss()); // set the direction and the total number of lines zigzag_direction = (Direction)constrain_int16(_direction, 0, 3); @@ -157,7 +157,7 @@ void ModeZigZag::run() void ModeZigZag::save_or_move_to_destination(Destination ab_dest) { // get current position as an offset from EKF origin - const Vector2p curr_pos_neu_m = pos_control->get_pos_desired_NEU_m().xy(); + const Vector2p curr_pos_ned_m = pos_control->get_pos_desired_NED_m().xy(); // handle state machine changes switch (stage) { @@ -165,12 +165,12 @@ void ModeZigZag::save_or_move_to_destination(Destination ab_dest) case STORING_POINTS: if (ab_dest == Destination::A) { // store point A - dest_A_ne_m = curr_pos_neu_m; + dest_A_ne_m = curr_pos_ned_m; gcs().send_text(MAV_SEVERITY_INFO, "%s: point A stored", name()); LOGGER_WRITE_EVENT(LogEvent::ZIGZAG_STORE_A); } else { // store point B - dest_B_ne_m = curr_pos_neu_m; + dest_B_ne_m = curr_pos_ned_m; gcs().send_text(MAV_SEVERITY_INFO, "%s: point B stored", name()); LOGGER_WRITE_EVENT(LogEvent::ZIGZAG_STORE_B); } @@ -187,11 +187,11 @@ void ModeZigZag::save_or_move_to_destination(Destination ab_dest) case AUTO: case MANUAL_REGAIN: // A and B have been defined, move vehicle to destination A or B - Vector3p next_dest_neu_m; + Vector3p next_dest_ned_m; bool is_terrain_alt; - if (calculate_next_dest_m(ab_dest, stage == AUTO, next_dest_neu_m, is_terrain_alt)) { + if (calculate_next_dest_m(ab_dest, stage == AUTO, next_dest_ned_m, is_terrain_alt)) { wp_nav->wp_and_spline_init_m(); - if (wp_nav->set_wp_destination_NEU_m(next_dest_neu_m, is_terrain_alt)) { + if (wp_nav->set_wp_destination_NED_m(next_dest_ned_m, is_terrain_alt)) { stage = AUTO; auto_stage = AutoState::AB_MOVING; ab_dest_stored = ab_dest; @@ -213,14 +213,14 @@ void ModeZigZag::save_or_move_to_destination(Destination ab_dest) void ModeZigZag::move_to_side() { if (!dest_A_ne_m.is_zero() && !dest_B_ne_m.is_zero() && !is_zero((dest_B_ne_m - dest_A_ne_m).length_squared())) { - Vector3p next_dest_neu_m; + Vector3p next_dest_ned_m; bool is_terrain_alt; - if (calculate_side_dest_m(next_dest_neu_m, is_terrain_alt)) { + if (calculate_side_dest_m(next_dest_ned_m, is_terrain_alt)) { wp_nav->wp_and_spline_init_m(); - if (wp_nav->set_wp_destination_NEU_m(next_dest_neu_m, is_terrain_alt)) { + if (wp_nav->set_wp_destination_NED_m(next_dest_ned_m, is_terrain_alt)) { stage = AUTO; auto_stage = AutoState::SIDEWAYS; - current_dest_neu_m = next_dest_neu_m; + current_dest_ned_m = next_dest_ned_m; current_is_terr_alt = is_terrain_alt; reach_wp_time_ms = 0; char const *dir[] = {"forward", "right", "backward", "left"}; @@ -238,8 +238,8 @@ void ModeZigZag::return_to_manual_control(bool maintain_target) spray(false); loiter_nav->clear_pilot_desired_acceleration(); if (maintain_target) { - const Vector3p& wp_dest_neu_m = wp_nav->get_wp_destination_NEU_m(); - loiter_nav->init_target_m(wp_dest_neu_m.xy()); + const Vector3p& wp_dest_ned_m = wp_nav->get_wp_destination_NED_m(); + loiter_nav->init_target_m(wp_dest_ned_m.xy()); #if AP_RANGEFINDER_ENABLED if (copter.rangefinder_alt_ok() && wp_nav->rangefinder_used_and_healthy()) { copter.surface_tracking.external_init(); @@ -267,7 +267,7 @@ void ModeZigZag::auto_control() // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle - pos_control->update_U_controller(); + pos_control->D_update_controller(); // call attitude controller // roll & pitch from waypoint controller, yaw rate from pilot @@ -311,7 +311,7 @@ void ModeZigZag::manual_control() } // Loiter State Machine Determination - AltHoldModeState althold_state = get_alt_hold_state_U_ms(target_climb_rate_ms); + AltHoldModeState althold_state = get_alt_hold_state_D_ms(target_climb_rate_ms); // althold state machine switch (althold_state) { @@ -319,7 +319,7 @@ void ModeZigZag::manual_control() case AltHoldModeState::MotorStopped: attitude_control->reset_rate_controller_I_terms(); attitude_control->reset_yaw_target_and_rate(); - pos_control->relax_U_controller(0.0f); // forces throttle output to decay to zero + pos_control->D_relax_controller(0.0f); // forces throttle output to decay to zero loiter_nav->init_target(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(loiter_nav->get_roll_rad(), loiter_nav->get_pitch_rad(), target_yaw_rate_rads); break; @@ -351,7 +351,7 @@ void ModeZigZag::manual_control() attitude_control->reset_rate_controller_I_terms_smoothly(); loiter_nav->init_target(); attitude_control->input_thrust_vector_rate_heading_rads(loiter_nav->get_thrust_vector(), target_yaw_rate_rads); - pos_control->relax_U_controller(0.0f); // forces throttle output to decay to zero + pos_control->D_relax_controller(0.0f); // forces throttle output to decay to zero break; case AltHoldModeState::Flying: @@ -373,12 +373,12 @@ void ModeZigZag::manual_control() #endif // Send the commanded climb rate to the position controller - pos_control->set_pos_target_U_from_climb_rate_ms(target_climb_rate_ms); + pos_control->D_set_pos_target_from_climb_rate_ms(target_climb_rate_ms); break; } // run the vertical position controller and set output throttle - pos_control->update_U_controller(); + pos_control->D_update_controller(); } // return true if vehicle is within a small area around the destination @@ -404,8 +404,8 @@ bool ModeZigZag::reached_destination() // calculate next destination according to vector A-B and current position // use_wpnav_alt should be true if waypoint controller's altitude target should be used, false for position control or current altitude target -// is_terrain_alt is returned as true if the next_dest_neu_m should be considered a terrain alt -bool ModeZigZag::calculate_next_dest_m(Destination ab_dest, bool use_wpnav_alt, Vector3p& next_dest_neu_m, bool& is_terrain_alt) const +// is_terrain_alt is returned as true if the next_dest_ned_m.z is relative to the terrain surface +bool ModeZigZag::calculate_next_dest_m(Destination ab_dest, bool use_wpnav_alt, Vector3p& next_dest_ned_m, bool& is_terrain_alt) const { // define start_pos_ne_m as either destination A or B Vector2p start_pos_ne_m = (ab_dest == Destination::A) ? dest_A_ne_m : dest_B_ne_m; @@ -419,7 +419,7 @@ bool ModeZigZag::calculate_next_dest_m(Destination ab_dest, bool use_wpnav_alt, } // get distance from vehicle to start_pos_ne_m - const Vector2p curr_pos_ne_m = pos_control->get_pos_desired_NEU_m().xy(); + const Vector2p curr_pos_ne_m = pos_control->get_pos_desired_NED_m().xy(); Vector2p veh_to_start_pos_ne_m = (curr_pos_ne_m - start_pos_ne_m); // lengthen AB_diff_ne_m so that it is at least as long as vehicle is from start point @@ -435,18 +435,18 @@ bool ModeZigZag::calculate_next_dest_m(Destination ab_dest, bool use_wpnav_alt, // find the closest point on the perpendicular line const Vector2p closest2d_ne_m = Vector2p::closest_point(curr_pos_ne_m, perp1, perp2); - next_dest_neu_m.x = closest2d_ne_m.x; - next_dest_neu_m.y = closest2d_ne_m.y; + next_dest_ned_m.x = closest2d_ne_m.x; + next_dest_ned_m.y = closest2d_ne_m.y; if (use_wpnav_alt) { // get altitude target from waypoint controller is_terrain_alt = wp_nav->origin_and_destination_are_terrain_alt(); - next_dest_neu_m.z = wp_nav->get_wp_destination_NEU_m().z; + next_dest_ned_m.z = wp_nav->get_wp_destination_NED_m().z; } else { is_terrain_alt = copter.rangefinder_alt_ok() && wp_nav->rangefinder_used_and_healthy(); - next_dest_neu_m.z = pos_control->get_pos_desired_NEU_m().z; + next_dest_ned_m.z = pos_control->get_pos_desired_NED_m().z; if (!is_terrain_alt) { - next_dest_neu_m.z += pos_control->get_pos_terrain_U_m(); + next_dest_ned_m.z += pos_control->get_pos_terrain_D_m(); } } @@ -454,8 +454,8 @@ bool ModeZigZag::calculate_next_dest_m(Destination ab_dest, bool use_wpnav_alt, } // calculate side destination according to vertical vector A-B and current position -// is_terrain_alt is returned as true if the next_dest_neu_m should be considered a terrain alt -bool ModeZigZag::calculate_side_dest_m(Vector3p& next_dest_neu_m, bool& is_terrain_alt) const +// is_terrain_alt is returned as true if the next_dest_ned_m.z is relative to the terrain surfaces +bool ModeZigZag::calculate_side_dest_m(Vector3p& next_dest_ned_m, bool& is_terrain_alt) const { // calculate vector from A to B Vector2f AB_diff_ne_m = (dest_B_ne_m - dest_A_ne_m).tofloat(); @@ -488,12 +488,12 @@ bool ModeZigZag::calculate_side_dest_m(Vector3p& next_dest_neu_m, bool& is_terra float scalar = constrain_float(_side_dist_m, 0.1, 100.0) / AB_side_ne_m_length; // get distance from vehicle to start_pos_ne_m - const Vector3p curr_pos_neu_m = pos_control->get_pos_desired_NEU_m(); - next_dest_neu_m.xy() = curr_pos_neu_m.xy() + (AB_side_ne_m.topostype() * scalar); + const Vector3p curr_pos_ned_m = pos_control->get_pos_desired_NED_m(); + next_dest_ned_m.xy() = curr_pos_ned_m.xy() + (AB_side_ne_m.topostype() * scalar); // if we have a downward facing range finder then use terrain altitude targets is_terrain_alt = copter.rangefinder_alt_ok() && wp_nav->rangefinder_used_and_healthy(); - next_dest_neu_m.z = curr_pos_neu_m.z; + next_dest_ned_m.z = curr_pos_ned_m.z; return true; } @@ -520,7 +520,7 @@ void ModeZigZag::run_auto() save_or_move_to_destination(ab_dest_stored); } else if (auto_stage == AutoState::SIDEWAYS) { wp_nav->wp_and_spline_init_m(); - if (wp_nav->set_wp_destination_NEU_m(current_dest_neu_m, current_is_terr_alt)) { + if (wp_nav->set_wp_destination_NED_m(current_dest_ned_m, current_is_terr_alt)) { stage = AUTO; reach_wp_time_ms = 0; char const *dir[] = {"forward", "right", "backward", "left"}; diff --git a/ArduCopter/standby.cpp b/ArduCopter/standby.cpp index 59d40b1d8894a..61df25d4d7b79 100644 --- a/ArduCopter/standby.cpp +++ b/ArduCopter/standby.cpp @@ -19,5 +19,5 @@ void Copter::standby_update() attitude_control->reset_rate_controller_I_terms(); attitude_control->reset_yaw_target_and_rate(); - pos_control->standby_NEU_reset(); + pos_control->NED_standby_reset(); } diff --git a/ArduCopter/surface_tracking.cpp b/ArduCopter/surface_tracking.cpp index 25488b811dc15..471eb61cb8ea4 100644 --- a/ArduCopter/surface_tracking.cpp +++ b/ArduCopter/surface_tracking.cpp @@ -18,7 +18,7 @@ void Copter::SurfaceTracking::update_surface_offset() AP_SurfaceDistance &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state; // update position controller target offset to the surface's alt above the EKF origin - copter.pos_control->set_pos_terrain_target_U_m(rf_state.terrain_u_m); + copter.pos_control->set_pos_terrain_target_D_m(-rf_state.terrain_u_m); last_update_ms = now_ms; valid_for_logging = true; @@ -28,7 +28,7 @@ void Copter::SurfaceTracking::update_surface_offset() if (timeout || reset_target || (last_glitch_cleared_ms != rf_state.glitch_cleared_ms)) { - copter.pos_control->init_pos_terrain_U_m(rf_state.terrain_u_m); + copter.pos_control->init_pos_terrain_D_m(-rf_state.terrain_u_m); reset_target = false; last_glitch_cleared_ms = rf_state.glitch_cleared_ms; } @@ -37,7 +37,7 @@ void Copter::SurfaceTracking::update_surface_offset() // reset position controller offsets if surface tracking is inactive // flag target should be reset when/if it next becomes active if (timeout && !reset_target) { - copter.pos_control->init_pos_terrain_U_m(0); + copter.pos_control->init_pos_terrain_D_m(0); valid_for_logging = false; reset_target = true; } diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 9f7f1e192a4ae..e4887d5b090c9 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -523,6 +523,9 @@ void Copter::allocate_motors(void) convert_prx_parameters(); #endif + // upgrade position controller parameters + copter.pos_control->convert_parameters(); + // param count could have changed AP_Param::invalidate_count(); } diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index fffb2691b3a02..79bf59d42ced5 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -71,7 +71,7 @@ void Mode::_TakeOff::stop() // take off is complete when the vertical target reaches the take off altitude. // climb is cancelled if pilot_climb_rate_ms becomes negative // sets take off to complete when target altitude is within 1% of the take off altitude -void Mode::_TakeOff::do_pilot_takeoff_ms(float& pilot_climb_rate_ms) +void Mode::_TakeOff::do_pilot_takeoff_ms(float pilot_climb_rate_ms) { // return pilot_climb_rate if take-off inactive if (!_running) { @@ -83,9 +83,9 @@ void Mode::_TakeOff::do_pilot_takeoff_ms(float& pilot_climb_rate_ms) float throttle_norm = constrain_float(copter.attitude_control->get_throttle_in() + copter.G_Dt / copter.g2.takeoff_throttle_slew_time, 0.0, 1.0); copter.attitude_control->set_throttle_out(throttle_norm, true, 0.0); // tell position controller to reset alt target and reset I terms - copter.pos_control->init_U_controller(); + copter.pos_control->D_init_controller(); if (throttle_norm >= MIN(copter.g2.takeoff_throttle_max, 0.9) || - (copter.pos_control->get_estimate_accel_U_mss() >= 0.5 * copter.pos_control->get_max_accel_U_mss()) || + (copter.pos_control->get_estimated_accel_U_mss() >= 0.5 * copter.pos_control->D_get_max_accel_mss()) || (copter.pos_control->get_vel_estimate_U_ms() >= constrain_float(pilot_climb_rate_ms, copter.pos_control->get_max_speed_up_ms() * 0.1, copter.pos_control->get_max_speed_up_ms() * 0.5)) || (is_positive(take_off_complete_alt_m - take_off_start_alt_m) && copter.pos_control->get_pos_estimate_U_m() - take_off_start_alt_m > 0.5 * (take_off_complete_alt_m - take_off_start_alt_m))) { // throttle > 90% @@ -96,11 +96,11 @@ void Mode::_TakeOff::do_pilot_takeoff_ms(float& pilot_climb_rate_ms) copter.set_land_complete(false); } } else { - float pos_u_m = take_off_complete_alt_m; - float vel_u_ms = pilot_climb_rate_ms; + float pos_d_m = -take_off_complete_alt_m; + float vel_d_ms = -pilot_climb_rate_ms; // command the aircraft to the take off altitude and current pilot climb rate - copter.pos_control->input_pos_vel_accel_U_m(pos_u_m, vel_u_ms, 0); + copter.pos_control->input_pos_vel_accel_D_m(pos_d_m, vel_d_ms, 0.0); // stop take off early and return if negative climb rate is commanded or we are within 0.1% of our take off altitude if (is_negative(pilot_climb_rate_ms) || @@ -143,10 +143,10 @@ void _AutoTakeoff::run() // aircraft stays in landed state until rotor speed run up has finished if (motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED) { // motors have not completed spool up yet so relax navigation and position controllers - pos_control->relax_velocity_controller_NE(); - pos_control->update_NE_controller(); - pos_control->relax_U_controller(0.0f); // forces throttle output to decay to zero - pos_control->update_U_controller(); + pos_control->NE_relax_velocity_controller(); + pos_control->NE_update_controller(); + pos_control->D_relax_controller(0.0f); // forces throttle output to decay to zero + pos_control->D_update_controller(); attitude_control->reset_yaw_target_and_rate(); attitude_control->reset_rate_controller_I_terms(); attitude_control->input_thrust_vector_rate_heading_rads(pos_control->get_thrust_vector(), 0.0); @@ -161,13 +161,13 @@ void _AutoTakeoff::run() float throttle = constrain_float(copter.attitude_control->get_throttle_in() + copter.G_Dt / copter.g2.takeoff_throttle_slew_time, 0.0, 1.0); copter.attitude_control->set_throttle_out(throttle, true, 0.0); // tell position controller to reset alt target and reset I terms - copter.pos_control->init_U_controller(); - pos_control->relax_velocity_controller_NE(); - pos_control->update_NE_controller(); + copter.pos_control->D_init_controller(); + pos_control->NE_relax_velocity_controller(); + pos_control->NE_update_controller(); attitude_control->reset_rate_controller_I_terms(); attitude_control->input_thrust_vector_rate_heading_rads(pos_control->get_thrust_vector(), 0.0); if (throttle >= MIN(copter.g2.takeoff_throttle_max, 0.9) || - (copter.pos_control->get_estimate_accel_U_mss() >= 0.5 * copter.pos_control->get_max_accel_U_mss()) || + (copter.pos_control->get_estimated_accel_U_mss() >= 0.5 * copter.pos_control->D_get_max_accel_mss()) || (copter.pos_control->get_vel_estimate_U_ms() >= 0.1 * copter.pos_control->get_max_speed_up_ms()) || ( no_nav_active && (pos_control->get_pos_estimate_U_m() >= no_nav_alt_m))) { // throttle > 90% @@ -185,21 +185,21 @@ void _AutoTakeoff::run() if (pos_control->get_pos_estimate_U_m() >= no_nav_alt_m) { no_nav_active = false; } - pos_control->relax_velocity_controller_NE(); + pos_control->NE_relax_velocity_controller(); } else { Vector2f vel_zero; Vector2f accel_zero; pos_control->input_vel_accel_NE_m(vel_zero, accel_zero); } - pos_control->update_NE_controller(); + pos_control->NE_update_controller(); // command the aircraft to the take off altitude - float pos_u_m = complete_alt_m + terrain_u_m; + float pos_d_m = -(complete_alt_m + terrain_u_m); float vel_zero = 0.0; - copter.pos_control->input_pos_vel_accel_U_m(pos_u_m, vel_zero, 0.0); + copter.pos_control->input_pos_vel_accel_D_m(pos_d_m, vel_zero, 0.0); // run the vertical position controller and set output throttle - pos_control->update_U_controller(); + pos_control->D_update_controller(); // call attitude controller with auto yaw attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), copter.flightmode->auto_yaw.get_heading()); @@ -208,15 +208,16 @@ void _AutoTakeoff::run() // and 10% our maximum climb rate const float vel_threshold_fraction = 0.1; // stopping distance from vel_threshold_fraction * max velocity - const float stop_distance_m = 0.5 * sq(vel_threshold_fraction * copter.pos_control->get_max_speed_up_ms()) / copter.pos_control->get_max_accel_U_mss(); - bool reached_altitude = copter.pos_control->get_pos_desired_U_m() >= pos_u_m - stop_distance_m; + const float stop_distance_m = 0.5 * sq(vel_threshold_fraction * copter.pos_control->get_max_speed_up_ms()) / copter.pos_control->D_get_max_accel_mss(); + bool reached_altitude = copter.pos_control->get_pos_desired_U_m() >= -pos_d_m - stop_distance_m; bool reached_climb_rate = copter.pos_control->get_vel_desired_U_ms() < copter.pos_control->get_max_speed_up_ms() * vel_threshold_fraction; complete = reached_altitude && reached_climb_rate; // calculate completion for location in case it is needed for a smooth transition to wp_nav if (complete) { - const Vector3p& _complete_pos_neu_m = copter.pos_control->get_pos_desired_NEU_m(); - complete_pos_neu_m = Vector3p{_complete_pos_neu_m.x, _complete_pos_neu_m.y, pos_u_m}; + no_nav_active = false; + const Vector3p& _complete_pos_ned_m = copter.pos_control->get_pos_desired_NED_m(); + complete_pos_ned_m = Vector3p{_complete_pos_ned_m.x, _complete_pos_ned_m.y, pos_d_m}; } } @@ -238,14 +239,14 @@ void _AutoTakeoff::start_m(float _complete_alt_m, bool _is_terrain_alt) } // return takeoff final target position in m from the EKF origin if takeoff has completed successfully -bool _AutoTakeoff::get_completion_pos_neu_m(Vector3p& pos_neu_m) +bool _AutoTakeoff::get_completion_pos_ned_m(Vector3p& pos_ned_m) { // only provide location if takeoff has completed if (!complete) { return false; } - pos_neu_m = complete_pos_neu_m; + pos_ned_m = complete_pos_ned_m; return true; } diff --git a/ArduCopter/tuning.cpp b/ArduCopter/tuning.cpp index 17c0a82209845..a60ef33738e16 100644 --- a/ArduCopter/tuning.cpp +++ b/ArduCopter/tuning.cpp @@ -92,36 +92,36 @@ void Copter::tuning(const RC_Channel *tuning_ch, int8_t tuning_param, float tuni // Altitude and throttle tuning case TUNING_ALTITUDE_HOLD_KP: - pos_control->get_pos_U_p().set_kP(tuning_value); + pos_control->D_get_pos_p().set_kP(tuning_value); break; case TUNING_THROTTLE_RATE_KP: - pos_control->get_vel_U_pid().set_kP(tuning_value); + pos_control->D_get_vel_pid().set_kP(tuning_value); break; case TUNING_ACCEL_Z_KP: - pos_control->get_accel_U_pid().set_kP(tuning_value); + pos_control->D_get_accel_pid().set_kP(tuning_value); break; case TUNING_ACCEL_Z_KI: - pos_control->get_accel_U_pid().set_kI(tuning_value); + pos_control->D_get_accel_pid().set_kI(tuning_value); break; case TUNING_ACCEL_Z_KD: - pos_control->get_accel_U_pid().set_kD(tuning_value); + pos_control->D_get_accel_pid().set_kD(tuning_value); break; // Loiter and navigation tuning case TUNING_LOITER_POSITION_KP: - pos_control->get_pos_NE_p().set_kP(tuning_value); + pos_control->NE_get_pos_p().set_kP(tuning_value); break; case TUNING_VEL_XY_KP: - pos_control->get_vel_NE_pid().set_kP(tuning_value); + pos_control->NE_get_vel_pid().set_kP(tuning_value); break; case TUNING_VEL_XY_KI: - pos_control->get_vel_NE_pid().set_kI(tuning_value); + pos_control->NE_get_vel_pid().set_kI(tuning_value); break; case TUNING_WP_SPEED: diff --git a/ArduPlane/GCS_MAVLink_Plane.cpp b/ArduPlane/GCS_MAVLink_Plane.cpp index 0aef55685979b..99ae3ea420a1d 100644 --- a/ArduPlane/GCS_MAVLink_Plane.cpp +++ b/ArduPlane/GCS_MAVLink_Plane.cpp @@ -212,7 +212,7 @@ void GCS_MAVLINK_Plane::send_nav_controller_output() const const Vector3f &targets = quadplane.attitude_control->get_att_target_euler_cd(); const Vector2f& curr_pos_m = quadplane.inertial_nav.get_position_xy_cm() * 0.01; - const Vector2f& target_pos_m = quadplane.pos_control->get_pos_target_NEU_m().xy().tofloat(); + const Vector2f& target_pos_m = quadplane.pos_control->get_pos_target_NED_m().xy().tofloat(); const Vector2f error_m = (target_pos_m - curr_pos_m); mavlink_msg_nav_controller_output_send( @@ -222,7 +222,7 @@ void GCS_MAVLINK_Plane::send_nav_controller_output() const targets.z * 0.01, degrees(error_m.angle()), MIN(error_m.length(), UINT16_MAX), - (plane.control_mode != &plane.mode_qstabilize) ? quadplane.pos_control->get_pos_error_U_m() : 0, + (plane.control_mode != &plane.mode_qstabilize) ? -quadplane.pos_control->get_pos_error_D_m() : 0, plane.airspeed_error * 100, // incorrect units; see PR#7933 quadplane.wp_nav->crosstrack_error_m()); return; @@ -397,7 +397,7 @@ void GCS_MAVLINK_Plane::send_pid_tuning() } #if HAL_QUADPLANE_ENABLED if (g.gcs_pid_mask & TUNING_BITS_ACCZ && plane.quadplane.in_vtol_mode()) { - pid_info = &plane.quadplane.pos_control->get_accel_U_pid().get_pid_info(); + pid_info = &plane.quadplane.pos_control->D_get_accel_pid().get_pid_info(); send_pid_info(pid_info, PID_TUNING_ACCZ, pid_info->actual); } #endif @@ -1232,7 +1232,7 @@ int16_t GCS_MAVLINK_Plane::high_latency_target_altitude() const const QuadPlane &quadplane = plane.quadplane; //return units are m if (quadplane.show_vtol_view()) { - return (plane.control_mode != &plane.mode_qstabilize) ? (global_position_current.alt * 0.01 + quadplane.pos_control->get_pos_error_U_m()) : 0; + return (plane.control_mode != &plane.mode_qstabilize) ? (global_position_current.alt * 0.01 - quadplane.pos_control->get_pos_error_D_m()) : 0; } #endif return 0.01 * (global_position_current.alt + plane.calc_altitude_error_cm()); diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index d4e3c2c5562c3..827960bcde15a 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -32,14 +32,14 @@ void Plane::Log_Write_Attitude(void) logger.Write_PID(LOG_PIQR_MSG, quadplane.attitude_control->get_rate_roll_pid().get_pid_info()); logger.Write_PID(LOG_PIQP_MSG, quadplane.attitude_control->get_rate_pitch_pid().get_pid_info()); logger.Write_PID(LOG_PIQY_MSG, quadplane.attitude_control->get_rate_yaw_pid().get_pid_info()); - logger.Write_PID(LOG_PIQA_MSG, quadplane.pos_control->get_accel_U_pid().get_pid_info() ); + logger.Write_PID(LOG_PIQA_MSG, quadplane.pos_control->D_get_accel_pid().get_pid_info() ); // Write tailsitter specific log at same rate as PIDs quadplane.tailsitter.write_log(); } - if (quadplane.in_vtol_mode() && quadplane.pos_control->is_active_NE()) { - logger.Write_PID(LOG_PIDN_MSG, quadplane.pos_control->get_vel_NE_pid().get_pid_info_x()); - logger.Write_PID(LOG_PIDE_MSG, quadplane.pos_control->get_vel_NE_pid().get_pid_info_y()); + if (quadplane.in_vtol_mode() && quadplane.pos_control->NE_is_active()) { + logger.Write_PID(LOG_PIDN_MSG, quadplane.pos_control->NE_get_vel_pid().get_pid_info_x()); + logger.Write_PID(LOG_PIDE_MSG, quadplane.pos_control->NE_get_vel_pid().get_pid_info_y()); } #endif diff --git a/ArduPlane/VTOL_Assist.cpp b/ArduPlane/VTOL_Assist.cpp index a42dd9ee4e561..b4430d2f1ac3a 100644 --- a/ArduPlane/VTOL_Assist.cpp +++ b/ArduPlane/VTOL_Assist.cpp @@ -182,8 +182,8 @@ bool VTOL_Assist::check_VTOL_recovery(void) controller may limit pitch after a strong acceleration event */ - quadplane.pos_control->init_U_controller(); - quadplane.pos_control->init_NE_controller(); + quadplane.pos_control->D_init_controller(); + quadplane.pos_control->NE_init_controller(); } } } diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 34406b150580e..9bae00e73a9f0 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -218,7 +218,7 @@ friend class ModeQAcro; public: Mode::Number mode_number() const override { return Mode::Number::ACRO; } - const char *name() const override { return "ACRO"; } + const char *name() const override { return "Acro"; } const char *name4() const override { return "ACRO"; } // methods that affect movement of the vehicle in this mode @@ -258,7 +258,7 @@ class ModeAuto : public Mode friend class Plane; Number mode_number() const override { return Number::AUTO; } - const char *name() const override { return "AUTO"; } + const char *name() const override { return "Auto"; } const char *name4() const override { return "AUTO"; } bool does_automatic_thermal_switch() const override { return true; } @@ -331,7 +331,7 @@ class ModeAutoTune : public Mode public: Number mode_number() const override { return Number::AUTOTUNE; } - const char *name() const override { return "AUTOTUNE"; } + const char *name() const override { return "Autotune"; } const char *name4() const override { return "ATUN"; } // methods that affect movement of the vehicle in this mode @@ -356,7 +356,7 @@ class ModeGuided : public Mode public: Number mode_number() const override { return Number::GUIDED; } - const char *name() const override { return "GUIDED"; } + const char *name() const override { return "Guided"; } const char *name4() const override { return "GUID"; } // methods that affect movement of the vehicle in this mode @@ -406,7 +406,7 @@ class ModeCircle: public Mode public: Number mode_number() const override { return Number::CIRCLE; } - const char *name() const override { return "CIRCLE"; } + const char *name() const override { return "Circle"; } const char *name4() const override { return "CIRC"; } // methods that affect movement of the vehicle in this mode @@ -431,7 +431,7 @@ class ModeLoiter : public Mode public: Number mode_number() const override { return Number::LOITER; } - const char *name() const override { return "LOITER"; } + const char *name() const override { return "Loiter"; } const char *name4() const override { return "LOIT"; } // methods that affect movement of the vehicle in this mode @@ -471,7 +471,7 @@ class ModeLoiterAltQLand : public ModeLoiter public: Number mode_number() const override { return Number::LOITER_ALT_QLAND; } - const char *name() const override { return "Loiter to QLAND"; } + const char *name() const override { return "Loiter to QLand"; } const char *name4() const override { return "L2QL"; } // handle a guided target request from GCS @@ -493,7 +493,7 @@ class ModeManual : public Mode public: Number mode_number() const override { return Number::MANUAL; } - const char *name() const override { return "MANUAL"; } + const char *name() const override { return "Manual"; } const char *name4() const override { return "MANU"; } // methods that affect movement of the vehicle in this mode @@ -550,7 +550,7 @@ class ModeStabilize : public Mode public: Number mode_number() const override { return Number::STABILIZE; } - const char *name() const override { return "STABILIZE"; } + const char *name() const override { return "Stabilize"; } const char *name4() const override { return "STAB"; } // methods that affect movement of the vehicle in this mode @@ -573,7 +573,7 @@ class ModeTraining : public Mode public: Number mode_number() const override { return Number::TRAINING; } - const char *name() const override { return "TRAINING"; } + const char *name() const override { return "Training"; } const char *name4() const override { return "TRAN"; } // methods that affect movement of the vehicle in this mode @@ -592,7 +592,7 @@ class ModeInitializing : public Mode public: Number mode_number() const override { return Number::INITIALISING; } - const char *name() const override { return "INITIALISING"; } + const char *name() const override { return "Initialising"; } const char *name4() const override { return "INIT"; } bool _enter() override { return false; } @@ -614,7 +614,7 @@ class ModeFBWA : public Mode public: Number mode_number() const override { return Number::FLY_BY_WIRE_A; } - const char *name() const override { return "FLY_BY_WIRE_A"; } + const char *name() const override { return "FBWA"; } const char *name4() const override { return "FBWA"; } // methods that affect movement of the vehicle in this mode @@ -641,7 +641,7 @@ class ModeFBWB : public Mode public: Number mode_number() const override { return Number::FLY_BY_WIRE_B; } - const char *name() const override { return "FLY_BY_WIRE_B"; } + const char *name() const override { return "FBWB"; } const char *name4() const override { return "FBWB"; } bool allows_terrain_disable() const override { return true; } @@ -667,7 +667,7 @@ class ModeCruise : public Mode public: Number mode_number() const override { return Number::CRUISE; } - const char *name() const override { return "CRUISE"; } + const char *name() const override { return "Cruise"; } const char *name4() const override { return "CRUS"; } bool allows_terrain_disable() const override { return true; } @@ -705,7 +705,7 @@ class ModeAvoidADSB : public Mode public: Number mode_number() const override { return Number::AVOID_ADSB; } - const char *name() const override { return "AVOID_ADSB"; } + const char *name() const override { return "Avoid ADSB"; } const char *name4() const override { return "AVOI"; } // methods that affect movement of the vehicle in this mode @@ -729,7 +729,7 @@ class ModeQStabilize : public Mode public: Number mode_number() const override { return Number::QSTABILIZE; } - const char *name() const override { return "QSTABILIZE"; } + const char *name() const override { return "QStabilize"; } const char *name4() const override { return "QSTB"; } bool is_vtol_mode() const override { return true; } @@ -763,7 +763,7 @@ class ModeQHover : public Mode public: Number mode_number() const override { return Number::QHOVER; } - const char *name() const override { return "QHOVER"; } + const char *name() const override { return "QHover"; } const char *name4() const override { return "QHOV"; } bool is_vtol_mode() const override { return true; } @@ -796,7 +796,7 @@ friend class Plane; public: Number mode_number() const override { return Number::QLOITER; } - const char *name() const override { return "QLOITER"; } + const char *name() const override { return "QLoiter"; } const char *name4() const override { return "QLOT"; } bool is_vtol_mode() const override { return true; } @@ -826,7 +826,7 @@ class ModeQLand : public Mode { public: Number mode_number() const override { return Number::QLAND; } - const char *name() const override { return "QLAND"; } + const char *name() const override { return "QLand"; } const char *name4() const override { return "QLND"; } bool is_vtol_mode() const override { return true; } @@ -883,7 +883,7 @@ class ModeQAcro : public Mode public: Number mode_number() const override { return Number::QACRO; } - const char *name() const override { return "QACRO"; } + const char *name() const override { return "QAcro"; } const char *name4() const override { return "QACO"; } bool is_vtol_mode() const override { return true; } @@ -906,7 +906,7 @@ class ModeQAutotune : public Mode public: Number mode_number() const override { return Number::QAUTOTUNE; } - const char *name() const override { return "QAUTOTUNE"; } + const char *name() const override { return "QAutotune"; } const char *name4() const override { return "QATN"; } bool is_vtol_mode() const override { return true; } @@ -932,7 +932,7 @@ class ModeTakeoff: public Mode ModeTakeoff(); Number mode_number() const override { return Number::TAKEOFF; } - const char *name() const override { return "TAKEOFF"; } + const char *name() const override { return "Takeoff"; } const char *name4() const override { return "TKOF"; } // methods that affect movement of the vehicle in this mode @@ -980,7 +980,7 @@ class ModeAutoLand: public Mode ModeAutoLand(); Number mode_number() const override { return Number::AUTOLAND; } - const char *name() const override { return "AUTOLAND"; } + const char *name() const override { return "Autoland"; } const char *name4() const override { return "ALND"; } // methods that affect movement of the vehicle in this mode @@ -1045,7 +1045,7 @@ class ModeThermal: public Mode public: Number mode_number() const override { return Number::THERMAL; } - const char *name() const override { return "THERMAL"; } + const char *name() const override { return "Thermal"; } const char *name4() const override { return "THML"; } // methods that affect movement of the vehicle in this mode diff --git a/ArduPlane/mode_qhover.cpp b/ArduPlane/mode_qhover.cpp index 808d6391e0ce9..edf008125b08f 100644 --- a/ArduPlane/mode_qhover.cpp +++ b/ArduPlane/mode_qhover.cpp @@ -7,8 +7,8 @@ bool ModeQHover::_enter() { // set vertical speed and acceleration limits // All limits must be positive - pos_control->set_max_speed_accel_U_m(quadplane.get_pilot_velocity_z_max_dn_m(), quadplane.pilot_speed_z_max_up_ms, quadplane.pilot_accel_z_mss); - pos_control->set_correction_speed_accel_U_m(quadplane.get_pilot_velocity_z_max_dn_m(), quadplane.pilot_speed_z_max_up_ms, quadplane.pilot_accel_z_mss); + pos_control->D_set_max_speed_accel_m(quadplane.get_pilot_velocity_z_max_dn_m(), quadplane.pilot_speed_z_max_up_ms, quadplane.pilot_accel_z_mss); + pos_control->D_set_correction_speed_accel_m(quadplane.get_pilot_velocity_z_max_dn_m(), quadplane.pilot_speed_z_max_up_ms, quadplane.pilot_accel_z_mss); quadplane.set_climb_rate_ms(0); quadplane.init_throttle_wait(); @@ -38,7 +38,7 @@ void ModeQHover::run() quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); attitude_control->set_throttle_out(0, true, 0); quadplane.relax_attitude_control(); - pos_control->relax_U_controller(0); + pos_control->D_relax_controller(0); } else { plane.quadplane.assign_tilt_to_fwd_thr(); quadplane.hold_hover(quadplane.get_pilot_desired_climb_rate_cms()); diff --git a/ArduPlane/mode_qloiter.cpp b/ArduPlane/mode_qloiter.cpp index 7b246d89bdc1e..ae7594ca29a19 100644 --- a/ArduPlane/mode_qloiter.cpp +++ b/ArduPlane/mode_qloiter.cpp @@ -11,8 +11,8 @@ bool ModeQLoiter::_enter() // set vertical speed and acceleration limits // All limits must be positive - pos_control->set_max_speed_accel_U_m(quadplane.get_pilot_velocity_z_max_dn_m(), quadplane.pilot_speed_z_max_up_ms, quadplane.pilot_accel_z_mss); - pos_control->set_correction_speed_accel_U_m(quadplane.get_pilot_velocity_z_max_dn_m(), quadplane.pilot_speed_z_max_up_ms, quadplane.pilot_accel_z_mss); + pos_control->D_set_max_speed_accel_m(quadplane.get_pilot_velocity_z_max_dn_m(), quadplane.pilot_speed_z_max_up_ms, quadplane.pilot_accel_z_mss); + pos_control->D_set_correction_speed_accel_m(quadplane.get_pilot_velocity_z_max_dn_m(), quadplane.pilot_speed_z_max_up_ms, quadplane.pilot_accel_z_mss); quadplane.init_throttle_wait(); @@ -81,7 +81,7 @@ void ModeQLoiter::run() quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); attitude_control->set_throttle_out(0, true, 0); quadplane.relax_attitude_control(); - pos_control->relax_U_controller(0); + pos_control->D_relax_controller(0); loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); @@ -109,7 +109,7 @@ void ModeQLoiter::run() // set vertical speed and acceleration limits // All limits must be positive - pos_control->set_max_speed_accel_U_m(quadplane.get_pilot_velocity_z_max_dn_m(), quadplane.pilot_speed_z_max_up_ms, quadplane.pilot_accel_z_mss); + pos_control->D_set_max_speed_accel_m(quadplane.get_pilot_velocity_z_max_dn_m(), quadplane.pilot_speed_z_max_up_ms, quadplane.pilot_accel_z_mss); // process pilot's roll and pitch input float target_roll_cd, target_pitch_cd; @@ -117,8 +117,8 @@ void ModeQLoiter::run() loiter_nav->set_pilot_desired_acceleration_cd(target_roll_cd, target_pitch_cd); // run loiter controller - if (!pos_control->is_active_NE()) { - pos_control->init_NE_controller(); + if (!pos_control->NE_is_active()) { + pos_control->NE_init_controller(); } loiter_nav->update(); @@ -129,7 +129,7 @@ void ModeQLoiter::run() plane.quadplane.assign_tilt_to_fwd_thr(); if (quadplane.transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) { - pos_control->set_externally_limited_NE(); + pos_control->NE_set_externally_limited(); } // Pilot input, use yaw rate time constant @@ -166,7 +166,7 @@ void ModeQLoiter::run() ahrs.set_touchdown_expected(true); } - pos_control->land_at_climb_rate_ms(-descent_rate_ms, descent_rate_ms>0); + pos_control->D_set_pos_target_from_climb_rate_ms(-descent_rate_ms, descent_rate_ms>0); quadplane.check_land_complete(); } else if (plane.control_mode == &plane.mode_guided && quadplane.guided_takeoff) { quadplane.set_climb_rate_ms(0); diff --git a/ArduPlane/mode_qrtl.cpp b/ArduPlane/mode_qrtl.cpp index dcdfb0cac296a..b2c308585da56 100644 --- a/ArduPlane/mode_qrtl.cpp +++ b/ArduPlane/mode_qrtl.cpp @@ -107,7 +107,7 @@ void ModeQRTL::run() plane.quadplane.assign_tilt_to_fwd_thr(); if (quadplane.transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) { - pos_control->set_externally_limited_NE(); + pos_control->NE_set_externally_limited(); } // weathervane with no pilot input quadplane.disable_yaw_rate_time_constant(); @@ -120,9 +120,9 @@ void ModeQRTL::run() quadplane.run_z_controller(); // Climb done when stopping point reaches target altitude - Vector3p stopping_point_neu_m; - pos_control->get_stopping_point_U_m(stopping_point_neu_m.z); - Location stopping_loc = Location(stopping_point_neu_m.tofloat() * 100.0, Location::AltFrame::ABOVE_ORIGIN); + Vector3p stopping_point_ned_m; + pos_control->get_stopping_point_D_m(stopping_point_ned_m.z); + Location stopping_loc = Location::from_ekf_offset_NED_m(stopping_point_ned_m, Location::AltFrame::ABOVE_ORIGIN); ftype alt_diff; if (!stopping_loc.get_height_above(plane.next_WP_loc, alt_diff) || is_positive(alt_diff)) { diff --git a/ArduPlane/qautotune.cpp b/ArduPlane/qautotune.cpp index c0875c9e4b49b..554067237445d 100644 --- a/ArduPlane/qautotune.cpp +++ b/ArduPlane/qautotune.cpp @@ -42,10 +42,10 @@ void QAutoTune::init_z_limits() { // set vertical speed and acceleration limits // All limits must be positive - plane.quadplane.pos_control->set_max_speed_accel_U_m(plane.quadplane.get_pilot_velocity_z_max_dn_m(), + plane.quadplane.pos_control->D_set_max_speed_accel_m(plane.quadplane.get_pilot_velocity_z_max_dn_m(), plane.quadplane.pilot_speed_z_max_up_ms, plane.quadplane.pilot_accel_z_mss); - plane.quadplane.pos_control->set_correction_speed_accel_U_m(plane.quadplane.get_pilot_velocity_z_max_dn_m(), + plane.quadplane.pos_control->D_set_correction_speed_accel_m(plane.quadplane.get_pilot_velocity_z_max_dn_m(), plane.quadplane.pilot_speed_z_max_up_ms, plane.quadplane.pilot_accel_z_mss); } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 59c23de417f68..da0c02583d2d1 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -599,7 +599,7 @@ static const struct AP_Param::defaults_table_struct defaults_table[] = { { "Q_LOIT_SPEED", 500 }, { "Q_WP_SPEED", 500 }, { "Q_WP_ACCEL", 100 }, - { "Q_P_JERK_XY", 2 }, + { "Q_P_JERK_NE", 2 }, // lower rotational accel limits { "Q_A_ACCEL_R_MAX", 40000 }, { "Q_A_ACCEL_P_MAX", 40000 }, @@ -825,6 +825,9 @@ bool QuadPlane::setup(void) pilot_speed_z_max_dn_ms.convert_centi_parameter(AP_PARAM_INT16); pilot_accel_z_mss.convert_centi_parameter(AP_PARAM_INT16); + // upgrade position controller parameters added Dec 2025 + pos_control->convert_parameters(); + // Provisionally assign the SLT thrust type. // It will be overwritten by tailsitter or tiltorotor setups. thrust_type = ThrustType::SLT; @@ -1035,22 +1038,22 @@ void QuadPlane::run_z_controller(void) // never run Z controller in tailsitter transition return; } - if ((now - last_pidz_active_ms) > 20 || !pos_control->is_active_U()) { + if ((now - last_pidz_active_ms) > 20 || !pos_control->D_is_active()) { // set vertical speed and acceleration limits // All limits must be positive - pos_control->set_max_speed_accel_U_m(get_pilot_velocity_z_max_dn_m(), pilot_speed_z_max_up_ms, pilot_accel_z_mss); + pos_control->D_set_max_speed_accel_m(get_pilot_velocity_z_max_dn_m(), pilot_speed_z_max_up_ms, pilot_accel_z_mss); // initialise the vertical position controller if (!tailsitter.enabled()) { - pos_control->init_U_controller(); + pos_control->D_init_controller(); } else { // initialise the vertical position controller with no descent - pos_control->init_U_controller_no_descent(); + pos_control->D_init_controller_no_descent(); } last_pidz_init_ms = now; } last_pidz_active_ms = now; - pos_control->update_U_controller(); + pos_control->D_update_controller(); } void QuadPlane::relax_attitude_control() @@ -1079,7 +1082,8 @@ void QuadPlane::check_yaw_reset(void) void QuadPlane::set_climb_rate_ms(float target_climb_rate_ms) { - pos_control->input_vel_accel_U_m(target_climb_rate_ms, 0, false); + float vel_d_m = -target_climb_rate_ms; + pos_control->input_vel_accel_D_m(vel_d_m, 0, false); } /* @@ -1092,7 +1096,7 @@ void QuadPlane::hold_hover(float target_climb_rate_cms) // set vertical speed and acceleration limits // All limits must be positive - pos_control->set_max_speed_accel_U_m(get_pilot_velocity_z_max_dn_m(), pilot_speed_z_max_up_ms, pilot_accel_z_mss); + pos_control->D_set_max_speed_accel_m(get_pilot_velocity_z_max_dn_m(), pilot_speed_z_max_up_ms, pilot_accel_z_mss); // call attitude controller multicopter_attitude_rate_update(get_desired_yaw_rate_cds(false)); @@ -1744,7 +1748,7 @@ void QuadPlane::update(void) attitude_control->relax_attitude_controllers(); } // todo: do you want to set the throttle at this point? - pos_control->relax_U_controller(0); + pos_control->D_relax_controller(0); } const uint32_t now = AP_HAL::millis(); @@ -2192,17 +2196,17 @@ void QuadPlane::run_xy_controller(float accel_limit_mss) accel_mss = MAX(accel_mss, accel_limit_mss); } const float speed_ms = wp_nav->get_default_speed_NE_ms(); - pos_control->set_max_speed_accel_NE_m(speed_ms, accel_mss); - pos_control->set_correction_speed_accel_NE_m(speed_ms, accel_mss); - if (!pos_control->is_active_NE()) { - pos_control->init_NE_controller(); + pos_control->NE_set_max_speed_accel_m(speed_ms, accel_mss); + pos_control->NE_set_correction_speed_accel_m(speed_ms, accel_mss); + if (!pos_control->NE_is_active()) { + pos_control->NE_init_controller(); } pos_control->set_lean_angle_max_cd(MIN(4500, MAX(accel_mss_to_angle_deg(accel_limit_mss) * 100, aparm.angle_max))); if (q_fwd_throttle > 0.95f) { // prevent wind up of the velocity controller I term due to a saturated forward throttle - pos_control->set_externally_limited_NE(); + pos_control->NE_set_externally_limited(); } - pos_control->update_NE_controller(); + pos_control->NE_update_controller(); } /* @@ -2288,7 +2292,7 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s) done_accel_init = false; } else if (s == QPOS_AIRBRAKE) { // start with zero integrator on vertical throttle - qp.pos_control->get_accel_U_pid().set_integrator(0); + qp.pos_control->D_get_accel_pid().set_integrator(0); } else if (s == QPOS_LAND_DESCEND) { // reset throttle descent control qp.thr_ctrl_land = false; @@ -2657,7 +2661,7 @@ void QuadPlane::vtol_position_controller(void) // use input shaping and abide by accel and jerk limits pos_control->input_vel_accel_NE_m(target_speed_ne_ms, target_accel_ne_mss); // During POS1, we only want to control velocity and acceleration - pos_control->stop_pos_NE_stabilisation(); + pos_control->NE_stop_pos_stabilisation(); // run horizontal velocity controller run_xy_controller(MAX(approach_accel_mss, transition_decel_mss)*1.5); @@ -2679,7 +2683,7 @@ void QuadPlane::vtol_position_controller(void) assign_tilt_to_fwd_thr(); if (transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) { - pos_control->set_externally_limited_NE(); + pos_control->NE_set_externally_limited(); } // call attitude controller @@ -2718,7 +2722,7 @@ void QuadPlane::vtol_position_controller(void) */ Vector2f zero; Vector2f vel_ne_ms = poscontrol.target_vel_ms.xy() + landing_velocity_ne_ms; - pos_control->input_pos_vel_accel_NE_m(poscontrol.target_neu_m.xy(), vel_ne_ms, zero); + pos_control->input_pos_vel_accel_NE_m(poscontrol.target_ned_m.xy(), vel_ne_ms, zero); // also run fixed wing navigation plane.nav_controller->update_waypoint(plane.current_loc, loc); @@ -2734,7 +2738,7 @@ void QuadPlane::vtol_position_controller(void) assign_tilt_to_fwd_thr(); if (transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) { - pos_control->set_externally_limited_NE(); + pos_control->NE_set_externally_limited(); } // call attitude controller @@ -2746,11 +2750,12 @@ void QuadPlane::vtol_position_controller(void) } case QPOS_LAND_FINAL: + setup_target_position(); update_land_positioning(); // relax when close to the ground if (should_relax()) { - pos_control->relax_velocity_controller_NE(); + pos_control->NE_relax_velocity_controller(); } else { Vector2f zero; Vector2f vel_ne_ms = poscontrol.target_vel_ms.xy() + landing_velocity_ne_ms; @@ -2767,7 +2772,7 @@ void QuadPlane::vtol_position_controller(void) pos_control->input_vel_accel_NE_m(vel_ne_ms, zero); } else { // otherwise use full pos control - pos_control->input_pos_vel_accel_NE_m(poscontrol.target_neu_m.xy(), vel_ne_ms, zero); + pos_control->input_pos_vel_accel_NE_m(poscontrol.target_ned_m.xy(), vel_ne_ms, zero); } } @@ -2804,12 +2809,12 @@ void QuadPlane::vtol_position_controller(void) // phases of landing, so relax the Z controller, unless we are // providing assistance if (transition->complete()) { - pos_control->relax_U_controller(0); + pos_control->D_relax_controller(0); } break; case QPOS_POSITION1: if (tailsitter.in_vtol_transition(now_ms)) { - pos_control->relax_U_controller(0); + pos_control->D_relax_controller(0); break; } FALLTHROUGH; @@ -2844,14 +2849,14 @@ void QuadPlane::vtol_position_controller(void) } } float zero = 0; - float target_u_m = target_altitude_cm * 0.01; - pos_control->input_pos_vel_accel_U_m(target_u_m, zero, 0); + float target_d_m = -target_altitude_cm * 0.01; + pos_control->input_pos_vel_accel_D_m(target_d_m, zero, 0); } else if (plane.control_mode == &plane.mode_qrtl) { Location loc2 = loc; loc2.change_alt_frame(Location::AltFrame::ABOVE_ORIGIN); - float target_u_m = loc2.alt * 0.01; + float target_d_m = -(loc2.alt * 0.01); float zero = 0; - pos_control->input_pos_vel_accel_U_m(target_u_m, zero, 0); + pos_control->input_pos_vel_accel_D_m(target_d_m, zero, 0); } else { set_climb_rate_ms(0); } @@ -2872,7 +2877,7 @@ void QuadPlane::vtol_position_controller(void) break; } const float descent_rate_ms = landing_descent_rate_ms(height_above_ground_m); - pos_control->land_at_climb_rate_ms(-descent_rate_ms, descent_rate_ms > 0); + pos_control->D_set_pos_target_from_climb_rate_ms(-descent_rate_ms, descent_rate_ms > 0); break; } @@ -2927,11 +2932,11 @@ QuadPlane::ActiveFwdThr QuadPlane::get_vfwd_method(void) const if (q_fwd_thr_use == FwdThrUse::ALL) { return ActiveFwdThr::NEW; } - if (q_fwd_thr_use == FwdThrUse::POSCTRL && pos_control->is_active_NE()) { + if (q_fwd_thr_use == FwdThrUse::POSCTRL && pos_control->NE_is_active()) { return ActiveFwdThr::NEW; } } - if (have_vfwd_gain && pos_control->is_active_NE()) { + if (have_vfwd_gain && pos_control->NE_is_active()) { return ActiveFwdThr::OLD; } return ActiveFwdThr::NONE; @@ -2962,7 +2967,7 @@ void QuadPlane::assign_tilt_to_fwd_thr(void) if (is_positive(fwd_tilt_range_cd)) { // rate limit the forward tilt change to slew between the motor good and motor failed // value over 10 seconds - const bool fwd_limited = plane.quadplane.pos_control->is_active_NE() and plane.quadplane.pos_control->get_fwd_pitch_is_limited(); + const bool fwd_limited = plane.quadplane.pos_control->NE_is_active() and plane.quadplane.pos_control->get_fwd_pitch_is_limited(); const float fwd_pitch_lim_cd_tgt = fwd_limited ? (float)aparm.angle_max : 100.0f * q_fwd_pitch_lim; const float delta_max = 0.1f * fwd_tilt_range_cd * plane.G_Dt; q_fwd_pitch_lim_cd += constrain_float((fwd_pitch_lim_cd_tgt - q_fwd_pitch_lim_cd), -delta_max, delta_max); @@ -3087,20 +3092,21 @@ void QuadPlane::setup_target_position(void) if (!ahrs.get_origin(origin)) { origin.zero(); } - if (!in_vtol_land_approach() || poscontrol.get_state() > QPOS_APPROACH) { + if ((!in_vtol_land_approach() || poscontrol.get_state() > QPOS_APPROACH) && + poscontrol.get_state() != QPOS_LAND_FINAL) { set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); } Vector2f diff2d = origin.get_distance_NE(loc); diff2d += poscontrol.correction_ne_m; - poscontrol.target_neu_m.x = diff2d.x; - poscontrol.target_neu_m.y = diff2d.y; - poscontrol.target_neu_m.z = (plane.next_WP_loc.alt - origin.alt) * 0.01; + poscontrol.target_ned_m.x = diff2d.x; + poscontrol.target_ned_m.y = diff2d.y; + poscontrol.target_ned_m.z = -(plane.next_WP_loc.alt - origin.alt) * 0.01; // set vertical speed and acceleration limits // All limits must be positive - pos_control->set_max_speed_accel_U_m(get_pilot_velocity_z_max_dn_m(), pilot_speed_z_max_up_ms, pilot_accel_z_mss); - pos_control->set_correction_speed_accel_U_m(get_pilot_velocity_z_max_dn_m(), pilot_speed_z_max_up_ms, pilot_accel_z_mss); + pos_control->D_set_max_speed_accel_m(get_pilot_velocity_z_max_dn_m(), pilot_speed_z_max_up_ms, pilot_accel_z_mss); + pos_control->D_set_correction_speed_accel_m(get_pilot_velocity_z_max_dn_m(), pilot_speed_z_max_up_ms, pilot_accel_z_mss); } /* @@ -3187,7 +3193,7 @@ void QuadPlane::takeoff_controller(void) takeoff_last_run_ms = now; if (no_navigation) { - pos_control->relax_velocity_controller_NE(); + pos_control->NE_relax_velocity_controller(); } else { pos_control->input_vel_accel_NE_m(vel_ne_ms, zero); @@ -3214,9 +3220,9 @@ void QuadPlane::takeoff_controller(void) // a small margin to ensure we do move to the next takeoff // stage const int32_t margin_cm = 5; - float pos_u_m = (margin_cm + plane.next_WP_loc.alt - origin.alt) * 0.01; - vel_u_ms = 0; - pos_control->input_pos_vel_accel_U_m(pos_u_m, vel_u_ms, 0); + float pos_d_m = -(margin_cm + plane.next_WP_loc.alt - origin.alt) * 0.01; + float vel_d_ms = 0.0; + pos_control->input_pos_vel_accel_D_m(pos_d_m, vel_d_ms, 0); } else { set_climb_rate_ms(vel_u_ms); } @@ -3238,7 +3244,7 @@ void QuadPlane::waypoint_controller(void) const uint32_t now = AP_HAL::millis(); if (!loc.same_loc_as(last_auto_target) || now - last_loiter_ms > 500) { - wp_nav->set_wp_destination_NEU_m(poscontrol.target_neu_m); + wp_nav->set_wp_destination_NED_m(poscontrol.target_ned_m); last_auto_target = loc; } last_loiter_ms = now; @@ -3256,7 +3262,7 @@ void QuadPlane::waypoint_controller(void) assign_tilt_to_fwd_thr(); if (transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) { - pos_control->set_externally_limited_NE(); + pos_control->NE_set_externally_limited(); } // call attitude controller @@ -3365,11 +3371,11 @@ bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd) // set vertical speed and acceleration limits // All limits must be positive - pos_control->set_max_speed_accel_U_m(get_pilot_velocity_z_max_dn_m(), pilot_speed_z_max_up_ms, pilot_accel_z_mss); - pos_control->set_correction_speed_accel_U_m(get_pilot_velocity_z_max_dn_m(), pilot_speed_z_max_up_ms, pilot_accel_z_mss); + pos_control->D_set_max_speed_accel_m(get_pilot_velocity_z_max_dn_m(), pilot_speed_z_max_up_ms, pilot_accel_z_mss); + pos_control->D_set_correction_speed_accel_m(get_pilot_velocity_z_max_dn_m(), pilot_speed_z_max_up_ms, pilot_accel_z_mss); // initialise the vertical position controller - pos_control->init_U_controller(); + pos_control->D_init_controller(); // also update nav_controller for status output plane.nav_controller->update_waypoint(plane.current_loc, plane.next_WP_loc); @@ -3414,8 +3420,8 @@ bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd) plane.next_WP_loc.copy_alt_from(plane.current_loc); // initialise the position controller - pos_control->init_NE_controller(); - pos_control->init_U_controller(); + pos_control->NE_init_controller(); + pos_control->D_init_controller(); throttle_wait = false; landing_detect.lower_limit_start_ms = 0; @@ -3603,7 +3609,7 @@ bool QuadPlane::verify_vtol_land(void) if (poscontrol.pilot_correction_done) { reached_position = !poscontrol.pilot_correction_active; } else { - const float dist_m = (inertial_nav.get_position_neu_cm().topostype() * 0.01 - poscontrol.target_neu_m).xy().length(); + const float dist_m = (inertial_nav.get_position_neu_cm().topostype() * 0.01 - poscontrol.target_ned_m).xy().length(); reached_position = dist_m < descend_dist_threshold_m; } Vector2f approach_vel_ne_ms; @@ -3802,8 +3808,7 @@ float QuadPlane::forward_throttle_pct() vel_forward.last_ms = AP_HAL::millis(); // work out the desired speed in forward direction - Vector3f desired_velocity_ned_ms = pos_control->get_vel_desired_NEU_ms(); - desired_velocity_ned_ms.z *= -1; // convert to NED m/s + Vector3f desired_velocity_ned_ms = pos_control->get_vel_desired_NED_ms(); Vector3f vel_ned_ms; if (!plane.ahrs.get_velocity_NED(vel_ned_ms)) { @@ -4272,7 +4277,7 @@ bool SLT_Transition::show_vtol_view() const } /* - return the PILOT_VELZ_MAX_DN value if non zero, otherwise returns the PILOT_VELZ_MAX value. + return the PILOT_SPD_DN value if non zero, otherwise returns the PILOT_SPD_UP value. return is in cm/s */ uint16_t QuadPlane::get_pilot_velocity_z_max_dn_m() const diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index ba7990584c8be..d4fb86d4ff21a 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -515,7 +515,7 @@ class QuadPlane uint32_t time_since_state_start_ms() const { return AP_HAL::millis() - last_state_change_ms; } - Vector3p target_neu_m; + Vector3p target_ned_m; Vector2f correction_ne_m; Vector3f target_vel_ms; bool slow_descent; diff --git a/ArduPlane/systemid.cpp b/ArduPlane/systemid.cpp index 79299efe690c7..f5a3c3336fad1 100644 --- a/ArduPlane/systemid.cpp +++ b/ArduPlane/systemid.cpp @@ -197,10 +197,10 @@ void AP_SystemID::stop() attitude_control->rate_bf_roll_sysid_rads(0); attitude_control->rate_bf_pitch_sysid_rads(0); attitude_control->rate_bf_yaw_sysid_rads(0); - plane.quadplane.pos_control->set_NE_control_scale_factor(1); + plane.quadplane.pos_control->NE_set_control_scale_factor(1); // re-initialise the XY controller so we take current position as target - plane.quadplane.pos_control->init_NE_controller(); + plane.quadplane.pos_control->NE_init_controller(); } #endif gcs().send_text(MAV_SEVERITY_INFO, "SystemID stopped"); @@ -280,7 +280,7 @@ void AP_SystemID::vtol_update() } // reduce control in NE axis when in position controlled modes - plane.quadplane.pos_control->set_NE_control_scale_factor(xy_control_mul); + plane.quadplane.pos_control->NE_set_control_scale_factor(xy_control_mul); if (log_subsample <= 0) { log_data(); diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 360a07b12b30c..de4e45a120850 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -185,10 +185,10 @@ static const struct AP_Param::defaults_table_struct defaults_table_tailsitter[] { "Q_TRANS_DECEL", 6 }, { "Q_A_ACCEL_P_MAX", 30000}, { "Q_A_ACCEL_R_MAX", 30000}, - { "Q_P_POSXY_P", 0.5}, - { "Q_P_VELXY_P", 1.0}, - { "Q_P_VELXY_I", 0.5}, - { "Q_P_VELXY_D", 0.25}, + { "Q_P_NE_POS_P", 0.5}, + { "Q_P_NE_VEL_P", 1.0}, + { "Q_P_NE_VEL_I", 0.5}, + { "Q_P_NE_VEL_D", 0.25}, }; diff --git a/ArduPlane/tuning.cpp b/ArduPlane/tuning.cpp index 7a98b1cbe0606..b3d8405f7107a 100644 --- a/ArduPlane/tuning.cpp +++ b/ArduPlane/tuning.cpp @@ -166,28 +166,28 @@ AP_Float *AP_Tuning_Plane::get_param_pointer(uint8_t parm) return &plane.quadplane.attitude_control->get_angle_yaw_p().kP(); case TUNING_PXY_P: - return &plane.quadplane.pos_control->get_pos_NE_p().kP(); + return &plane.quadplane.pos_control->NE_get_pos_p().kP(); case TUNING_PZ_P: - return &plane.quadplane.pos_control->get_pos_U_p().kP(); + return &plane.quadplane.pos_control->D_get_pos_p().kP(); case TUNING_VXY_P: - return &plane.quadplane.pos_control->get_vel_NE_pid().kP(); + return &plane.quadplane.pos_control->NE_get_vel_pid().kP(); case TUNING_VXY_I: - return &plane.quadplane.pos_control->get_vel_NE_pid().kI(); + return &plane.quadplane.pos_control->NE_get_vel_pid().kI(); case TUNING_VZ_P: - return &plane.quadplane.pos_control->get_vel_U_pid().kP(); + return &plane.quadplane.pos_control->D_get_vel_pid().kP(); case TUNING_AZ_P: - return &plane.quadplane.pos_control->get_accel_U_pid().kP(); + return &plane.quadplane.pos_control->D_get_accel_pid().kP(); case TUNING_AZ_I: - return &plane.quadplane.pos_control->get_accel_U_pid().kI(); + return &plane.quadplane.pos_control->D_get_accel_pid().kI(); case TUNING_AZ_D: - return &plane.quadplane.pos_control->get_accel_U_pid().kD(); + return &plane.quadplane.pos_control->D_get_accel_pid().kD(); case TUNING_RATE_PITCH_FF: return &plane.quadplane.attitude_control->get_rate_pitch_pid().ff(); diff --git a/ArduSub/GCS_MAVLink_Sub.cpp b/ArduSub/GCS_MAVLink_Sub.cpp index cf6dc0b3a896f..403b1fbbefe54 100644 --- a/ArduSub/GCS_MAVLink_Sub.cpp +++ b/ArduSub/GCS_MAVLink_Sub.cpp @@ -219,7 +219,7 @@ void GCS_MAVLINK_Sub::send_pid_tuning() } } if (g.gcs_pid_mask & 8) { - const AP_PIDInfo &pid_info = sub.pos_control.get_accel_U_pid().get_pid_info(); + const AP_PIDInfo &pid_info = sub.pos_control.D_get_accel_pid().get_pid_info(); mavlink_msg_pid_tuning_send(chan, PID_TUNING_ACCZ, pid_info.target*0.01f, -(ahrs.get_accel_ef().z + GRAVITY_MSS), @@ -457,21 +457,11 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command_i return MAV_RESULT_ACCEPTED; } -void GCS_MAVLINK_Sub::handle_message(const mavlink_message_t &msg) +// this is called on receipt of a MANUAL_CONTROL packet and is +// expected to call manual_override to override RC input on desired +// axes. +void GCS_MAVLINK_Sub::handle_manual_control_axes(const mavlink_manual_control_t &packet, const uint32_t tnow) { - switch (msg.msgid) { - - case MAVLINK_MSG_ID_MANUAL_CONTROL: { // MAV ID: 69 - if (!gcs().sysid_is_gcs(msg.sysid)) { - break; // Only accept control from our gcs - } - mavlink_manual_control_t packet; - mavlink_msg_manual_control_decode(&msg, &packet); - - if (packet.target != gcs().sysid_this_mav()) { - break; // only accept control aimed at us - } - sub.transform_manual_control_to_rc_override( packet.x, packet.y, @@ -491,11 +481,11 @@ void GCS_MAVLINK_Sub::handle_message(const mavlink_message_t &msg) ); sub.failsafe.last_pilot_input_ms = AP_HAL::millis(); - // a RC override message is considered to be a 'heartbeat' - // from the ground station for failsafe purposes - sysid_mygcs_seen(AP_HAL::millis()); - break; - } +} + +void GCS_MAVLINK_Sub::handle_message(const mavlink_message_t &msg) +{ + switch (msg.msgid) { case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: { // MAV ID: 70 if (!gcs().sysid_is_gcs(msg.sysid)) { diff --git a/ArduSub/GCS_MAVLink_Sub.h b/ArduSub/GCS_MAVLink_Sub.h index 6c05ed74dcdcc..b415ff4feb4d7 100644 --- a/ArduSub/GCS_MAVLink_Sub.h +++ b/ArduSub/GCS_MAVLink_Sub.h @@ -18,6 +18,7 @@ class GCS_MAVLINK_Sub : public GCS_MAVLINK { MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet); + void handle_manual_control_axes(const mavlink_manual_control_t &packet, const uint32_t tnow) override; // override sending of scaled_pressure3 to send on-board temperature: void send_scaled_pressure3() override; diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 6e917899abc68..00bed63d08399 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -897,3 +897,66 @@ void Sub::convert_old_parameters() SRV_Channels::upgrade_parameters(); } + +#if LEAKDETECTOR_MAX_INSTANCES > 0 +// PARAMETER_CONVERSION - Added: Dec-2025 +// Deals with leak detector getting misconfigured when updating from Sub 4.1 +void Sub::update_leak_pins() +{ + for (uint8_t instance = 0; instance < LEAKDETECTOR_MAX_INSTANCES; instance++) { + if (leak_detector.get_pin(instance) <= 0) { + // leak detector does not use pin + continue; + } + uint8_t servo_channel; + if (!hal.gpio->pin_to_servo_channel(leak_detector.get_pin(instance), servo_channel)) { + // leak detector pin does not map to a servo channel + continue; + } + if (SRV_Channels::is_GPIO(servo_channel)) { + // servo channel is already set to GPIO + continue; + } + if (SRV_Channels::channel_function(servo_channel) != SRV_Channel::Function::k_none) { + // servo channel is already set to a function + gcs().send_text(MAV_SEVERITY_WARNING, "Leak detector %u error. Please set SERVO%u_FUNCTION to GPIO", instance + 1, servo_channel + 1); + continue; + } + // servo channel is disabled, let's set it to GPIO for the user + gcs().send_text(MAV_SEVERITY_INFO, "Leak detector %u pin (servo %u) auto-set to GPIO", instance + 1, servo_channel + 1); + char param_name[20]; + snprintf(param_name, sizeof(param_name), "SERVO%u_FUNCTION", servo_channel + 1); + AP_Param::set_and_save_by_name(param_name, static_cast(SRV_Channel::Function::k_GPIO)); + } +} +#endif + +#if AP_RELAY_ENABLED +// PARAMETER_CONVERSION - Added: Dec-2025 +// Deals with relay getting misconfigured when updating from Sub 4.1 +void Sub::update_relay_pins() +{ + for (uint8_t instance = 0; instance < AP_RELAY_NUM_RELAYS; instance++) { + uint8_t servo_channel; + uint8_t pin; + if (!relay.get_pin_by_instance(instance, pin) || !hal.gpio->pin_to_servo_channel(pin, servo_channel)) { + // instance does not use pin or pin does not map to a servo channel + continue; + } + if (!relay.enabled(instance) || SRV_Channels::is_GPIO(servo_channel)) { + // instance is not enabled or servo channel is already set to GPIO + continue; + } + if (SRV_Channels::channel_function(servo_channel) != SRV_Channel::Function::k_none) { + // servo channel is already set to a function + gcs().send_text(MAV_SEVERITY_WARNING, "Relay %u error. Please set SERVO%u_FUNCTION to GPIO", instance + 1, servo_channel + 1); + continue; + } + // servo channel is disabled, let's set it to GPIO for the user + gcs().send_text(MAV_SEVERITY_INFO, "Relay %u pin (servo %u) auto-set to GPIO", instance + 1, servo_channel + 1); + char param_name[20]; + snprintf(param_name, sizeof(param_name), "SERVO%u_FUNCTION", servo_channel + 1); + AP_Param::set_and_save_by_name(param_name, static_cast(SRV_Channel::Function::k_GPIO)); + } +} +#endif diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index 445ca02d70bf0..1bc8649e438b1 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -441,10 +441,10 @@ static const struct AP_Param::defaults_table_struct defaults_table[] = { { "RC8_OPTION", 213}, // MOUNT1_PITCH { "MOT_PWM_MIN", 1100}, { "MOT_PWM_MAX", 1900}, - { "PSC_JERK_Z", 50.0f}, + { "PSC_JERK_D", 50.0f}, { "WPNAV_SPEED", 100.0f}, { "PILOT_SPEED_UP", 100.0f}, - { "PSC_VELXY_P", 6.0f}, + { "PSC_NE_VEL_P", 6.0f}, { "EK3_SRC1_VELZ", 0}, #if AP_SUB_RC_ENABLED { "RC_PROTOCOLS", 0}, diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp index da37603ba3233..bc3b9218de2dd 100644 --- a/ArduSub/Sub.cpp +++ b/ArduSub/Sub.cpp @@ -232,7 +232,7 @@ void Sub::ten_hz_logging_loop() logger.Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info()); logger.Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info()); logger.Write_PID(LOG_PIDY_MSG, attitude_control.get_rate_yaw_pid().get_pid_info()); - logger.Write_PID(LOG_PIDA_MSG, pos_control.get_accel_U_pid().get_pid_info()); + logger.Write_PID(LOG_PIDA_MSG, pos_control.D_get_accel_pid().get_pid_info()); } } if (should_log(MASK_LOG_MOTBATT)) { @@ -269,7 +269,7 @@ void Sub::twentyfive_hz_logging() logger.Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info()); logger.Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info()); logger.Write_PID(LOG_PIDY_MSG, attitude_control.get_rate_yaw_pid().get_pid_info()); - logger.Write_PID(LOG_PIDA_MSG, pos_control.get_accel_U_pid().get_pid_info()); + logger.Write_PID(LOG_PIDA_MSG, pos_control.D_get_accel_pid().get_pid_info()); } } @@ -342,7 +342,7 @@ void Sub::one_hz_loop() set_likely_flying(hal.util->get_soft_armed()); attitude_control.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); - pos_control.get_accel_U_pid().set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); + pos_control.D_get_accel_pid().set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); } void Sub::read_AHRS() diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index c571e3067e1de..cdc8df618cc83 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -391,7 +391,7 @@ class Sub : public AP_Vehicle { // setup the var_info table AP_Param param_loader; - uint32_t last_pilot_heading; + float last_pilot_heading_rad; uint32_t last_pilot_yaw_input_ms; uint32_t fs_terrain_recover_start_ms; @@ -577,6 +577,13 @@ class Sub : public AP_Vehicle { uint16_t get_pilot_speed_dn() const; void convert_old_parameters(void); + +#if LEAKDETECTOR_MAX_INSTANCES > 0 + void update_leak_pins(); +#endif +#if AP_RELAY_ENABLED + void update_relay_pins(); +#endif bool handle_do_motor_test(mavlink_command_int_t command); bool init_motor_test(); bool verify_motor_test(); diff --git a/ArduSub/actuators.cpp b/ArduSub/actuators.cpp index 7050ecdc36812..650282db841a3 100644 --- a/ArduSub/actuators.cpp +++ b/ArduSub/actuators.cpp @@ -45,7 +45,7 @@ const AP_Param::GroupInfo Actuators::var_info[] = { // @Description: Initial increment step for changing the actuator's PWM // @Units: us // @User: Standard - AP_GROUPINFO("6_INC", 6, Actuators, Actuators::actuator_increment_step[5], ACTUATOR_DEFAULT_INCREMENT), + AP_GROUPINFO("6_INC", 6, Actuators, actuator_increment_step[5], ACTUATOR_DEFAULT_INCREMENT), AP_GROUPEND }; diff --git a/ArduSub/mode.h b/ArduSub/mode.h index dfd5d246b319d..0ba5fff37c5b7 100644 --- a/ArduSub/mode.h +++ b/ArduSub/mode.h @@ -136,7 +136,7 @@ class ModeManual : public Mode protected: - const char *name() const override { return "MANUAL"; } + const char *name() const override { return "Manual"; } const char *name4() const override { return "MANU"; } Mode::Number number() const override { return Mode::Number::MANUAL; } }; @@ -159,7 +159,7 @@ class ModeAcro : public Mode protected: - const char *name() const override { return "ACRO"; } + const char *name() const override { return "Acro"; } const char *name4() const override { return "ACRO"; } Mode::Number number() const override { return Mode::Number::ACRO; } }; @@ -182,7 +182,7 @@ class ModeStabilize : public Mode protected: - const char *name() const override { return "STABILIZE"; } + const char *name() const override { return "Stabilize"; } const char *name4() const override { return "STAB"; } Mode::Number number() const override { return Mode::Number::STABILIZE; } }; @@ -209,7 +209,7 @@ class ModeAlthold : public Mode void run_pre(); void run_post(); - const char *name() const override { return "ALT_HOLD"; } + const char *name() const override { return "Depth Hold"; } const char *name4() const override { return "ALTH"; } Mode::Number number() const override { return Mode::Number::ALT_HOLD; } }; @@ -231,7 +231,7 @@ class ModeSurftrak : public ModeAlthold protected: - const char *name() const override { return "SURFTRAK"; } + const char *name() const override { return "Surftrak"; } const char *name4() const override { return "STRK"; } Mode::Number number() const override { return Mode::Number::SURFTRAK; } @@ -281,7 +281,7 @@ class ModeGuided : public Mode protected: - const char *name() const override { return "GUIDED"; } + const char *name() const override { return "Guided"; } const char *name4() const override { return "GUID"; } Mode::Number number() const override { return Mode::Number::GUIDED; } @@ -328,7 +328,7 @@ class ModeAuto : public ModeGuided protected: - const char *name() const override { return "AUTO"; } + const char *name() const override { return "Auto"; } const char *name4() const override { return "AUTO"; } Mode::Number number() const override { return Mode::Number::AUTO; } @@ -359,7 +359,7 @@ class ModePoshold : public ModeAlthold protected: - const char *name() const override { return "POSHOLD"; } + const char *name() const override { return "Position Hold"; } const char *name4() const override { return "POSH"; } Mode::Number number() const override { return Mode::Number::POSHOLD; } @@ -386,7 +386,7 @@ class ModeCircle : public Mode protected: - const char *name() const override { return "CIRCLE"; } + const char *name() const override { return "Circle"; } const char *name4() const override { return "CIRC"; } Mode::Number number() const override { return Mode::Number::CIRCLE; } }; @@ -407,7 +407,7 @@ class ModeSurface : public Mode bool is_autopilot() const override { return true; } protected: - const char *name() const override { return "SURFACE"; } + const char *name() const override { return "Surface"; } const char *name4() const override { return "SURF"; } Mode::Number number() const override { return Mode::Number::SURFACE; } bool nobaro_mode; @@ -431,7 +431,7 @@ class ModeMotordetect : public Mode protected: - const char *name() const override { return "MOTORDETECT"; } + const char *name() const override { return "Motor Detection"; } const char *name4() const override { return "DETE"; } Mode::Number number() const override { return Mode::Number::MOTOR_DETECT; } }; diff --git a/ArduSub/mode_althold.cpp b/ArduSub/mode_althold.cpp index 152b920234c81..5484c54a1db30 100644 --- a/ArduSub/mode_althold.cpp +++ b/ArduSub/mode_althold.cpp @@ -9,13 +9,13 @@ bool ModeAlthold::init(bool ignore_checks) { // initialize vertical maximum speeds and acceleration // sets the maximum speed up and down returned by position controller // All limits must be positive - position_control->set_max_speed_accel_U_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - position_control->set_correction_speed_accel_U_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->D_set_max_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->D_set_correction_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise position and desired velocity - position_control->init_U_controller(); + position_control->D_init_controller(); - sub.last_pilot_heading = ahrs.yaw_sensor; + sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); return true; } @@ -35,15 +35,15 @@ void ModeAlthold::run_pre() // initialize vertical speeds and acceleration // All limits must be positive - position_control->set_max_speed_accel_U_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->D_set_max_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle) attitude_control->set_throttle_out(0.5,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); - position_control->relax_U_controller(motors.get_throttle_hover()); - sub.last_pilot_heading = ahrs.yaw_sensor; + position_control->D_relax_controller(motors.get_throttle_hover()); + sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); return; } @@ -79,7 +79,7 @@ void ModeAlthold::run_pre() // call attitude controller if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate); - sub.last_pilot_heading = ahrs.yaw_sensor; + sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); sub.last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading } else { // hold current heading @@ -91,10 +91,10 @@ void ModeAlthold::run_pre() // call attitude controller with target yaw rate = 0 to decelerate on yaw axis attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate); - sub.last_pilot_heading = ahrs.yaw_sensor; // update heading to hold + sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); // update heading to hold } else { // call attitude controller holding absolute bearing - attitude_control->input_euler_angle_roll_pitch_yaw_cd(target_roll, target_pitch, sub.last_pilot_heading, true); + attitude_control->input_euler_angle_roll_pitch_yaw_cd(target_roll, target_pitch, rad_to_cd(sub.last_pilot_heading_rad), true); } } } @@ -125,6 +125,6 @@ void ModeAlthold::control_depth() { } } - position_control->set_pos_target_U_from_climb_rate_cms(target_climb_rate_cms); - position_control->update_U_controller(); + position_control->D_set_pos_target_from_climb_rate_cms(target_climb_rate_cms); + position_control->D_update_controller(); } diff --git a/ArduSub/mode_auto.cpp b/ArduSub/mode_auto.cpp index dd2ff876bc8f7..2c1eef6db2955 100644 --- a/ArduSub/mode_auto.cpp +++ b/ArduSub/mode_auto.cpp @@ -148,7 +148,7 @@ void ModeAuto::auto_wp_run() // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle - position_control->update_U_controller(); + position_control->D_update_controller(); //////////////////////////// // update attitude output // @@ -186,17 +186,17 @@ void ModeAuto::auto_circle_movetoedge_start(const Location &circle_center, float sub.circle_nav.set_rate_degs(current_rate); // check our distance from edge of circle - Vector3f circle_edge_neu; + Vector3f circle_edge_neu_cm; float dist_to_edge; - sub.circle_nav.get_closest_point_on_circle_NEU_cm(circle_edge_neu, dist_to_edge); + sub.circle_nav.get_closest_point_on_circle_NEU_cm(circle_edge_neu_cm, dist_to_edge); // if more than 3m then fly to edge if (dist_to_edge > 300.0f) { // set the state to move to the edge of the circle sub.auto_mode = Auto_CircleMoveToEdge; - // convert circle_edge_neu to Location - Location circle_edge(circle_edge_neu, Location::AltFrame::ABOVE_ORIGIN); + // convert circle_edge_neu_cm to Location + Location circle_edge(circle_edge_neu_cm, Location::AltFrame::ABOVE_ORIGIN); // convert altitude to same as command circle_edge.copy_alt_from(circle_center); @@ -246,7 +246,7 @@ void ModeAuto::auto_circle_run() // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle - position_control->update_U_controller(); + position_control->D_update_controller(); // roll & pitch from waypoint controller, yaw rate from pilot attitude_control->input_euler_angle_roll_pitch_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), sub.circle_nav.get_yaw_cd(), true); @@ -332,7 +332,7 @@ void ModeAuto::auto_loiter_run() // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle - position_control->update_U_controller(); + position_control->D_update_controller(); // get pilot desired lean angles float target_roll, target_pitch; @@ -455,12 +455,12 @@ bool ModeAuto::auto_terrain_recover_start() sub.loiter_nav.init_target(); // Reset z axis controller - position_control->relax_U_controller(motors.get_throttle_hover()); + position_control->D_relax_controller(motors.get_throttle_hover()); // initialize vertical maximum speeds and acceleration // All limits must be positive - position_control->set_max_speed_accel_U_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_U_cmss()); - position_control->set_correction_speed_accel_U_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_U_cmss()); + position_control->D_set_max_speed_accel_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_D_cmss()); + position_control->D_set_correction_speed_accel_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_D_cmss()); gcs().send_text(MAV_SEVERITY_WARNING, "Attempting auto failsafe recovery"); return true; @@ -482,8 +482,8 @@ void ModeAuto::auto_terrain_recover_run() attitude_control->set_throttle_out(0,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); - sub.loiter_nav.init_target(); // Reset xy target - position_control->relax_U_controller(motors.get_throttle_hover()); // Reset z axis controller + sub.loiter_nav.init_target(); // Reset xy target + position_control->D_relax_controller(motors.get_throttle_hover()); // Reset z axis controller return; } @@ -510,7 +510,7 @@ void ModeAuto::auto_terrain_recover_run() // Start timer as soon as rangefinder is healthy if (rangefinder_recovery_ms == 0) { rangefinder_recovery_ms = AP_HAL::millis(); - position_control->relax_U_controller(motors.get_throttle_hover()); // Reset alt hold targets + position_control->D_relax_controller(motors.get_throttle_hover()); // Reset alt hold targets } // 1.5 seconds of healthy rangefinder means we can resume mission with terrain enabled @@ -561,8 +561,8 @@ void ModeAuto::auto_terrain_recover_run() ///////////////////// // update z target // - position_control->set_pos_target_U_from_climb_rate_cms(target_climb_rate); - position_control->update_U_controller(); + position_control->D_set_pos_target_from_climb_rate_cms(target_climb_rate); + position_control->D_update_controller(); //////////////////////////// // update angular targets // diff --git a/ArduSub/mode_circle.cpp b/ArduSub/mode_circle.cpp index 13ac49e50da6c..27ad85d6788a6 100644 --- a/ArduSub/mode_circle.cpp +++ b/ArduSub/mode_circle.cpp @@ -15,10 +15,10 @@ bool ModeCircle::init(bool ignore_checks) // initialize speeds and accelerations // All limits must be positive - position_control->set_max_speed_accel_NE_cm(sub.wp_nav.get_default_speed_NE_cms(), sub.wp_nav.get_wp_acceleration_cmss()); - position_control->set_correction_speed_accel_NE_cm(sub.wp_nav.get_default_speed_NE_cms(), sub.wp_nav.get_wp_acceleration_cmss()); - position_control->set_max_speed_accel_U_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - position_control->set_correction_speed_accel_U_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->NE_set_max_speed_accel_cm(sub.wp_nav.get_default_speed_NE_cms(), sub.wp_nav.get_wp_acceleration_cmss()); + position_control->NE_set_correction_speed_accel_cm(sub.wp_nav.get_default_speed_NE_cms(), sub.wp_nav.get_wp_acceleration_cmss()); + position_control->D_set_max_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->D_set_correction_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise circle controller including setting the circle center based on vehicle speed sub.circle_nav.init(); @@ -35,8 +35,8 @@ void ModeCircle::run() // update parameters, to allow changing at runtime // All limits must be positive - position_control->set_max_speed_accel_NE_cm(sub.wp_nav.get_default_speed_NE_cms(), sub.wp_nav.get_wp_acceleration_cmss()); - position_control->set_max_speed_accel_U_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->NE_set_max_speed_accel_cm(sub.wp_nav.get_default_speed_NE_cms(), sub.wp_nav.get_wp_acceleration_cmss()); + position_control->D_set_max_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // if not armed set throttle to zero and exit immediately if (!motors.armed()) { @@ -83,6 +83,6 @@ void ModeCircle::run() } // update altitude target and call position controller - position_control->set_pos_target_U_from_climb_rate_cms(target_climb_rate); - position_control->update_U_controller(); + position_control->D_set_pos_target_from_climb_rate_cms(target_climb_rate); + position_control->D_update_controller(); } diff --git a/ArduSub/mode_guided.cpp b/ArduSub/mode_guided.cpp index 122bfbc064ab7..36c03366ce327 100644 --- a/ArduSub/mode_guided.cpp +++ b/ArduSub/mode_guided.cpp @@ -105,12 +105,12 @@ void ModeGuided::guided_vel_control_start() // initialize vertical maximum speeds and acceleration // All limits must be positive - position_control->set_max_speed_accel_U_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - position_control->set_correction_speed_accel_U_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->D_set_max_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->D_set_correction_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise velocity controller - position_control->init_U_controller(); - position_control->init_NE_controller(); + position_control->D_init_controller(); + position_control->NE_init_controller(); // pilot always controls yaw sub.yaw_rate_only = false; @@ -125,12 +125,12 @@ void ModeGuided::guided_posvel_control_start() // set vertical speed and acceleration // All limits must be positive - position_control->set_max_speed_accel_U_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_U_cmss()); - position_control->set_correction_speed_accel_U_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_U_cmss()); + position_control->D_set_max_speed_accel_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_D_cmss()); + position_control->D_set_correction_speed_accel_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_D_cmss()); // initialise velocity controller - position_control->init_U_controller(); - position_control->init_NE_controller(); + position_control->D_init_controller(); + position_control->NE_init_controller(); // pilot always controls yaw sub.yaw_rate_only = false; @@ -145,11 +145,11 @@ void ModeGuided::guided_angle_control_start() // set vertical speed and acceleration // All limits must be positive - position_control->set_max_speed_accel_U_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_U_cmss()); - position_control->set_correction_speed_accel_U_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_U_cmss()); + position_control->D_set_max_speed_accel_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_D_cmss()); + position_control->D_set_correction_speed_accel_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_D_cmss()); // initialise velocity controller - position_control->init_U_controller(); + position_control->D_init_controller(); // initialise targets guided_angle_state.update_time_ms = AP_HAL::millis(); @@ -501,7 +501,7 @@ void ModeGuided::guided_pos_control_run() // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle - position_control->update_U_controller(); + position_control->D_update_controller(); // call attitude controller if (sub.auto_yaw_mode == AUTO_YAW_HOLD) { @@ -532,8 +532,8 @@ void ModeGuided::guided_vel_control_run() attitude_control->set_throttle_out(0,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); // initialise velocity controller - position_control->init_U_controller(); - position_control->init_NE_controller(); + position_control->D_init_controller(); + position_control->NE_init_controller(); return; } @@ -562,12 +562,12 @@ void ModeGuided::guided_vel_control_run() position_control->set_vel_desired_NEU_cms(Vector3f(0,0,0)); } - position_control->stop_pos_NE_stabilisation(); + position_control->NE_stop_pos_stabilisation(); // call velocity controller which includes z axis controller - position_control->update_NE_controller(); + position_control->NE_update_controller(); - position_control->set_pos_target_U_from_climb_rate_cms(position_control->get_vel_desired_NEU_cms().z); - position_control->update_U_controller(); + position_control->D_set_pos_target_from_climb_rate_cms(position_control->get_vel_desired_NEU_cms().z); + position_control->D_update_controller(); float lateral_out, forward_out; sub.translate_pos_control_rp(lateral_out, forward_out); @@ -605,8 +605,8 @@ void ModeGuided::guided_posvel_control_run() attitude_control->set_throttle_out(0,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); // initialise velocity controller - position_control->init_U_controller(); - position_control->init_NE_controller(); + position_control->D_init_controller(); + position_control->NE_init_controller(); return; } @@ -646,8 +646,8 @@ void ModeGuided::guided_posvel_control_run() posvel_pos_target_cm.z = pz; // run position controller - position_control->update_NE_controller(); - position_control->update_U_controller(); + position_control->NE_update_controller(); + position_control->D_update_controller(); float lateral_out, forward_out; sub.translate_pos_control_rp(lateral_out, forward_out); @@ -685,7 +685,7 @@ void ModeGuided::guided_angle_control_run() attitude_control->set_throttle_out(0.0f,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); // initialise velocity controller - position_control->init_U_controller(); + position_control->D_init_controller(); return; } @@ -721,8 +721,8 @@ void ModeGuided::guided_angle_control_run() attitude_control->input_euler_angle_roll_pitch_yaw_cd(roll_in, pitch_in, yaw_in, true); // call position controller - position_control->set_pos_target_U_from_climb_rate_cms(climb_rate_cms); - position_control->update_U_controller(); + position_control->D_set_pos_target_from_climb_rate_cms(climb_rate_cms); + position_control->D_update_controller(); } // Guided Limit code @@ -816,7 +816,7 @@ float ModeGuided::get_auto_heading() // Bearing from current position towards intermediate position target (centidegrees) const Vector2f target_vel_ne_cms = position_control->get_vel_target_NEU_cms().xy(); float angle_error = 0.0f; - if (target_vel_ne_cms.length() >= position_control->get_max_speed_NE_cms() * 0.1f) { + if (target_vel_ne_cms.length() >= position_control->NE_get_max_speed_cms() * 0.1f) { const float desired_angle_cd = degrees(target_vel_ne_cms.angle()) * 100.0f; angle_error = wrap_180_cd(desired_angle_cd - track_bearing); } diff --git a/ArduSub/mode_poshold.cpp b/ArduSub/mode_poshold.cpp index 557eca7b96f01..794c953304b41 100644 --- a/ArduSub/mode_poshold.cpp +++ b/ArduSub/mode_poshold.cpp @@ -16,21 +16,21 @@ bool ModePoshold::init(bool ignore_checks) // initialize vertical speeds and acceleration // All limits must be positive - position_control->set_max_speed_accel_NE_cm(g.pilot_speed, g.pilot_accel_z); - position_control->set_correction_speed_accel_NE_cm(g.pilot_speed, g.pilot_accel_z); - position_control->set_max_speed_accel_U_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - position_control->set_correction_speed_accel_U_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->NE_set_max_speed_accel_cm(g.pilot_speed, g.pilot_accel_z); + position_control->NE_set_correction_speed_accel_cm(g.pilot_speed, g.pilot_accel_z); + position_control->D_set_max_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->D_set_correction_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise position and desired velocity - position_control->init_NE_controller_stopping_point(); - position_control->init_U_controller(); + position_control->NE_init_controller_stopping_point(); + position_control->D_init_controller(); // Stop all thrusters attitude_control->set_throttle_out(0.5f ,true, g.throttle_filt); attitude_control->relax_attitude_controllers(); - position_control->relax_U_controller(0.5f); + position_control->D_relax_controller(0.5f); - sub.last_pilot_heading = ahrs.yaw_sensor; + sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); return true; } @@ -46,9 +46,9 @@ void ModePoshold::run() // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle) attitude_control->set_throttle_out(0.5f ,true, g.throttle_filt); attitude_control->relax_attitude_controllers(); - position_control->init_NE_controller_stopping_point(); - position_control->relax_U_controller(0.5f); - sub.last_pilot_heading = ahrs.yaw_sensor; + position_control->NE_init_controller_stopping_point(); + position_control->D_relax_controller(0.5f); + sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); return; } @@ -70,7 +70,7 @@ void ModePoshold::run() // update attitude controller targets if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate); - sub.last_pilot_heading = ahrs.yaw_sensor; + sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); sub.last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading } else { // hold current heading @@ -82,10 +82,10 @@ void ModePoshold::run() // call attitude controller with target yaw rate = 0 to decelerate on yaw axis attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate); - sub.last_pilot_heading = ahrs.yaw_sensor; // update heading to hold + sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); // update heading to hold } else { // call attitude controller holding absolute bearing - attitude_control->input_euler_angle_roll_pitch_yaw_cd(target_roll, target_pitch, sub.last_pilot_heading, true); + attitude_control->input_euler_angle_roll_pitch_yaw_cd(target_roll, target_pitch, rad_to_cd(sub.last_pilot_heading_rad), true); } } @@ -108,9 +108,9 @@ void ModePoshold::control_horizontal() { }; if (sub.position_ok()) { - if (!position_control->is_active_NE()) { + if (!position_control->NE_is_active()) { // the xy controller timed out, re-initialize - position_control->init_NE_controller_stopping_point(); + position_control->NE_init_controller_stopping_point(); } // convert to the earth frame and set target rates @@ -121,7 +121,7 @@ void ModePoshold::control_horizontal() { sub.translate_pos_control_rp(lateral_out, forward_out); // update the xy controller - position_control->update_NE_controller(); + position_control->NE_update_controller(); } else if (g.pilot_speed > 0) { // allow the pilot to reposition manually forward_out = body_rates_cms.x / (float)g.pilot_speed; diff --git a/ArduSub/mode_stabilize.cpp b/ArduSub/mode_stabilize.cpp index c5474f66d7bf6..5b95377baa4e5 100644 --- a/ArduSub/mode_stabilize.cpp +++ b/ArduSub/mode_stabilize.cpp @@ -4,7 +4,7 @@ bool ModeStabilize::init(bool ignore_checks) { // set target altitude to zero for reporting position_control->set_pos_desired_U_cm(0); - sub.last_pilot_heading = ahrs.yaw_sensor; + sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); return true; return true; @@ -20,7 +20,7 @@ void ModeStabilize::run() motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); attitude_control->set_throttle_out(0,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); - sub.last_pilot_heading = ahrs.yaw_sensor; + sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); return; } @@ -40,7 +40,7 @@ void ModeStabilize::run() if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate); - sub.last_pilot_heading = ahrs.yaw_sensor; + sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); sub.last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading } else { // hold current heading @@ -52,10 +52,10 @@ void ModeStabilize::run() // call attitude controller with target yaw rate = 0 to decelerate on yaw axis attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate); - sub.last_pilot_heading = ahrs.yaw_sensor; // update heading to hold + sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); // update heading to hold } else { // call attitude controller holding absolute absolute bearing - attitude_control->input_euler_angle_roll_pitch_yaw_cd(target_roll, target_pitch, sub.last_pilot_heading, true); + attitude_control->input_euler_angle_roll_pitch_yaw_cd(target_roll, target_pitch, rad_to_cd(sub.last_pilot_heading_rad), true); } } diff --git a/ArduSub/mode_surface.cpp b/ArduSub/mode_surface.cpp index 1e312c6123da2..9561aea4f40cf 100644 --- a/ArduSub/mode_surface.cpp +++ b/ArduSub/mode_surface.cpp @@ -7,11 +7,11 @@ bool ModeSurface::init(bool ignore_checks) // initialize vertical speeds and acceleration // All limits must be positive - position_control->set_max_speed_accel_U_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - position_control->set_correction_speed_accel_U_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->D_set_max_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->D_set_correction_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise position and desired velocity - position_control->init_U_controller(); + position_control->D_init_controller(); return true; @@ -27,7 +27,7 @@ void ModeSurface::run() motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); attitude_control->set_throttle_out(0,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); - position_control->init_U_controller(); + position_control->D_init_controller(); return; } @@ -55,8 +55,8 @@ void ModeSurface::run() float cmb_rate_cms = constrain_float(fabsf(sub.wp_nav.get_default_speed_up_cms()), 1, position_control->get_max_speed_up_cms()); // update altitude target and call position controller - position_control->set_pos_target_U_from_climb_rate_cms(cmb_rate_cms); - position_control->update_U_controller(); + position_control->D_set_pos_target_from_climb_rate_cms(cmb_rate_cms); + position_control->D_update_controller(); } // pilot has control for repositioning motors.set_forward(channel_forward->norm_input()); diff --git a/ArduSub/mode_surftrak.cpp b/ArduSub/mode_surftrak.cpp index 6f0404271eddd..856eb54f3f464 100644 --- a/ArduSub/mode_surftrak.cpp +++ b/ArduSub/mode_surftrak.cpp @@ -139,10 +139,10 @@ void ModeSurftrak::control_range() { } // Set the target altitude from the climb rate and the terrain offset - position_control->set_pos_target_U_from_climb_rate_cms(target_climb_rate_cms); + position_control->D_set_pos_target_from_climb_rate_cms(target_climb_rate_cms); // Run the PID controllers - position_control->update_U_controller(); + position_control->D_update_controller(); } /* diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index 52e23414fdf5d..7e439b71b9150 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -130,7 +130,7 @@ void Sub::init_ardupilot() leak_detector.init(); - last_pilot_heading = ahrs.yaw_sensor; + last_pilot_heading_rad = ahrs.get_yaw_rad(); // initialise rangefinder #if AP_RANGEFINDER_ENABLED @@ -156,6 +156,12 @@ void Sub::init_ardupilot() ins.set_log_raw_bit(MASK_LOG_IMU_RAW); g2.actuators.initialize_actuators(); +#if LEAKDETECTOR_MAX_INSTANCES > 0 + update_leak_pins(); +#endif +#if AP_RELAY_ENABLED + update_relay_pins(); +#endif // flag that initialisation has completed ap.initialised = true; } diff --git a/Blimp/mode.h b/Blimp/mode.h index 4234c12d3df74..fd123cbc9e4de 100644 --- a/Blimp/mode.h +++ b/Blimp/mode.h @@ -156,7 +156,7 @@ class ModeManual : public Mode const char *name() const override { - return "MANUAL"; + return "Manual"; } const char *name4() const override { @@ -200,7 +200,7 @@ class ModeVelocity : public Mode const char *name() const override { - return "VELOCITY"; + return "Velocity"; } const char *name4() const override { @@ -245,7 +245,7 @@ class ModeLoiter : public Mode const char *name() const override { - return "LOITER"; + return "Loiter"; } const char *name4() const override { @@ -289,7 +289,7 @@ class ModeLand : public Mode const char *name() const override { - return "LAND"; + return "Land"; } const char *name4() const override { diff --git a/Rover/GCS_MAVLink_Rover.cpp b/Rover/GCS_MAVLink_Rover.cpp index 46b7794afdeb0..f91076bf7af96 100644 --- a/Rover/GCS_MAVLink_Rover.cpp +++ b/Rover/GCS_MAVLink_Rover.cpp @@ -1056,7 +1056,7 @@ uint8_t GCS_MAVLINK_Rover::send_available_mode(uint8_t index) const } // Ask the mode for its name and number - const char* name = modes[index_zero]->name4(); + const char* name = modes[index_zero]->name(); const uint8_t mode_number = (uint8_t)modes[index_zero]->mode_number(); mavlink_msg_available_modes_send( diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index 57a9c3a6f847c..38aba1d6cd523 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -819,14 +819,6 @@ void Rover::load_parameters(void) AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_ARMED); #endif -#if AP_AIRSPEED_ENABLED | AP_AIS_ENABLED | AP_FENCE_ENABLED - // Find G2's Top Level Key - AP_Param::ConversionInfo info; - if (!AP_Param::find_top_level_key_by_pointer(&g2, info.old_key)) { - return; - } -#endif - static const AP_Param::G2ObjectConversion g2_conversions[] { #if AP_AIRSPEED_ENABLED // PARAMETER_CONVERSION - Added: JAN-2022 diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp index b95de7bb49c18..d676be98983da 100644 --- a/Rover/Rover.cpp +++ b/Rover/Rover.cpp @@ -172,7 +172,7 @@ bool Rover::set_target_location(const Location& target_loc) #if AP_SCRIPTING_ENABLED // set target velocity (for use by scripting) -bool Rover::set_target_velocity_NED(const Vector3f& vel_ned) +bool Rover::set_target_velocity_NED(const Vector3f& vel_ned, bool align_yaw_to_target) { // exit if vehicle is not in Guided mode or Auto-Guided mode if (!control_mode->in_guided_mode()) { diff --git a/Rover/Rover.h b/Rover/Rover.h index ca9ecd6079d27..e43a1cfeed0b5 100644 --- a/Rover/Rover.h +++ b/Rover/Rover.h @@ -268,7 +268,7 @@ class Rover : public AP_Vehicle { #endif #if AP_SCRIPTING_ENABLED - bool set_target_velocity_NED(const Vector3f& vel_ned) override; + bool set_target_velocity_NED(const Vector3f& vel_ned, bool align_yaw_to_target) override; bool set_steering_and_throttle(float steering, float throttle) override; bool get_steering_and_throttle(float& steering, float& throttle) override; // set desired turn rate (degrees/sec) and speed (m/s). Used for scripting diff --git a/Rover/mode.h b/Rover/mode.h index bf88165c12c8b..c8176c7e3061f 100644 --- a/Rover/mode.h +++ b/Rover/mode.h @@ -46,6 +46,9 @@ class Mode // returns a unique number specific to this mode virtual Number mode_number() const = 0; + // returns full text name + virtual const char *name() const = 0; + // returns short text name (up to 4 bytes) virtual const char *name4() const = 0; @@ -223,6 +226,7 @@ class ModeAcro : public Mode public: Number mode_number() const override { return Number::ACRO; } + const char *name() const override { return "Acro"; } const char *name4() const override { return "ACRO"; } // methods that affect movement of the vehicle in this mode @@ -245,6 +249,7 @@ class ModeAuto : public Mode public: Number mode_number() const override { return Number::AUTO; } + const char *name() const override { return "Auto"; } const char *name4() const override { return "AUTO"; } // methods that affect movement of the vehicle in this mode @@ -417,6 +422,7 @@ class ModeCircle : public Mode CLASS_NO_COPY(ModeCircle); Number mode_number() const override { return Number::CIRCLE; } + const char *name() const override { return "Circle"; } const char *name4() const override { return "CIRC"; } // return the distance at which the vehicle is considered to be on track along the circle @@ -514,6 +520,7 @@ class ModeGuided : public Mode #endif Number mode_number() const override { return Number::GUIDED; } + const char *name() const override { return "Guided"; } const char *name4() const override { return "GUID"; } // methods that affect movement of the vehicle in this mode @@ -618,6 +625,7 @@ class ModeHold : public Mode public: Number mode_number() const override { return Number::HOLD; } + const char *name() const override { return "Hold"; } const char *name4() const override { return "HOLD"; } // methods that affect movement of the vehicle in this mode @@ -636,6 +644,7 @@ class ModeLoiter : public Mode public: Number mode_number() const override { return Number::LOITER; } + const char *name() const override { return "Loiter"; } const char *name4() const override { return "LOIT"; } // methods that affect movement of the vehicle in this mode @@ -668,6 +677,7 @@ class ModeManual : public Mode public: Number mode_number() const override { return Number::MANUAL; } + const char *name() const override { return "Manual"; } const char *name4() const override { return "MANU"; } // methods that affect movement of the vehicle in this mode @@ -692,6 +702,7 @@ class ModeRTL : public Mode public: Number mode_number() const override { return Number::RTL; } + const char *name() const override { return "RTL"; } const char *name4() const override { return "RTL"; } // methods that affect movement of the vehicle in this mode @@ -727,6 +738,7 @@ class ModeSmartRTL : public Mode public: Number mode_number() const override { return Number::SMART_RTL; } + const char *name() const override { return "Smart RTL"; } const char *name4() const override { return "SRTL"; } // methods that affect movement of the vehicle in this mode @@ -773,6 +785,7 @@ class ModeSteering : public Mode public: Number mode_number() const override { return Number::STEERING; } + const char *name() const override { return "Steering"; } const char *name4() const override { return "STER"; } // methods that affect movement of the vehicle in this mode @@ -798,6 +811,7 @@ class ModeInitializing : public Mode public: Number mode_number() const override { return Number::INITIALISING; } + const char *name() const override { return "Initialising"; } const char *name4() const override { return "INIT"; } // methods that affect movement of the vehicle in this mode @@ -819,6 +833,7 @@ class ModeFollow : public Mode public: Number mode_number() const override { return Number::FOLLOW; } + const char *name() const override { return "Follow"; } const char *name4() const override { return "FOLL"; } // methods that affect movement of the vehicle in this mode @@ -855,6 +870,7 @@ class ModeSimple : public Mode public: Number mode_number() const override { return Number::SIMPLE; } + const char *name() const override { return "Simple"; } const char *name4() const override { return "SMPL"; } // methods that affect movement of the vehicle in this mode @@ -885,6 +901,7 @@ class ModeDock : public Mode CLASS_NO_COPY(ModeDock); Number mode_number() const override { return Number::DOCK; } + const char *name() const override { return "Dock"; } const char *name4() const override { return "DOCK"; } // methods that affect movement of the vehicle in this mode diff --git a/Rover/system.cpp b/Rover/system.cpp index e752b29343e01..bbd529e930d63 100644 --- a/Rover/system.cpp +++ b/Rover/system.cpp @@ -216,7 +216,7 @@ bool Rover::set_mode(Mode &new_mode, ModeReason reason) // Check if GCS mode change is disabled via parameter if ((reason == ModeReason::GCS_COMMAND) && !gcs_mode_enabled((Mode::Number)new_mode.mode_number())) { - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"Mode change to %s denied, GCS entry disabled (FLTMODE_GCSBLOCK)", new_mode.name4()); + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"Mode change to %s denied, GCS entry disabled (FLTMODE_GCSBLOCK)", new_mode.name()); return false; } diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index b91591646a21c..fbd4bbfe1a6a6 100644 --- a/Tools/AP_Bootloader/board_types.txt +++ b/Tools/AP_Bootloader/board_types.txt @@ -330,12 +330,16 @@ AP_HW_AIRBRAIN_H743 1209 AP_HW_Morakot 1210 AP_HW_CORVON_V5 1211 -AP_HW_CSKY-PMU 1212 +AP_HW_CSKY_PMU 1212 AP_HW_MUPilot 1222 - AP_HW_HWH7 1223 +AP_HW_ATLAS_CONTROL 1227 +AP_HW_MatekH7A3_Wing 1228 +AP_HW_CORVON743V2 1229 +AP_HW_CORVON_V6 1230 + # IDs 1231-1240 reserved for YARI Robotics AP_HW_YARIV6X 1234 AP_HW_YARI_GNSS 1235 @@ -456,6 +460,9 @@ AP_HW_UAV-DEV-FC-M10S-H7 5231 AP_HW_UAV-DEV-UM982-H7 5232 AP_HW_UAV-DEV-PM-G4 5233 AP_HW_UAV-DEV-AUAV-G4 5234 +AP_HW_UAV-DEV-ETHERNET-H7 5235 +AP_HW_UAV-DEV-FC-PIZERO-H7 5236 +AP_HW_UAV-DEV-M10S-G4 5237 # IDs 5240-5249 reserved for TM IT-Systemhaus AP_HW_TM-SYS-BeastFC 5240 @@ -525,6 +532,9 @@ AP_HW_DroneerF405 5800 AP_HW_BrotherHobbyH743 5810 AP_HW_BrotherHobbyF405v3 5811 +#IDs 5820 ~ 5829 reserved for VimDrones +AP_HW_VIMDRONES_BMS 5820 + # IDs 6000-6099 reserved for SpektreWorks AP_HW_sw-spar-f407 6000 AP_HW_sw-boom-f407 6001 diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index 9ca65b16ef9df..cc112d3c15dc6 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -568,6 +568,9 @@ void AP_Periph_FW::update() #if AP_PERIPH_BATTERY_TAG_ENABLED battery_tag.update(); #endif +#if AP_PERIPH_BATTERY_BMS_ENABLED + battery_bms.update(); +#endif } #ifdef HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index bc3342a7edd29..d255980d61638 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -41,6 +41,7 @@ #include "rc_in.h" #include "batt_balance.h" #include "battery_tag.h" +#include "battery_bms.h" #include "actuator_telem.h" #include "networking.h" #include "serial_options.h" @@ -397,6 +398,10 @@ class AP_Periph_FW { BatteryTag battery_tag; #endif +#if AP_PERIPH_BATTERY_BMS_ENABLED + BatteryBMS battery_bms; +#endif + #if AP_PERIPH_ACTUATOR_TELEM_ENABLED ActuatorTelem actuator_telem; #endif diff --git a/Tools/AP_Periph/battery.cpp b/Tools/AP_Periph/battery.cpp index 31cf65b3d24e4..f4ccd0f939221 100644 --- a/Tools/AP_Periph/battery.cpp +++ b/Tools/AP_Periph/battery.cpp @@ -38,6 +38,7 @@ void AP_Periph_FW::can_battery_update(void) uavcan_equipment_power_BatteryInfo pkt {}; // if a battery serial number is assigned, use that as the ID. Else, use the index. + // batteries should start their serial numbers at numbers above 255 to avoid conflicts with the battery index offset method of populating the serial number const int32_t serial_number = battery_lib.get_serial_number(i); pkt.battery_id = (serial_number >= 0) ? serial_number : i+1; @@ -103,8 +104,17 @@ void AP_Periph_FW::can_battery_send_cells(uint8_t instance) delete [] buffer; return; } + + // fill in timestamp + pkt->timestamp.usec = AP_HAL::micros(); + + // if a battery serial number is assigned, use that as the ID. Else, use the index. + // batteries should start their serial numbers at numbers above 255 to avoid conflicts with the battery index offset method of populating the serial number + const int32_t serial_number = battery_lib.get_serial_number(instance); + pkt->battery_id = (serial_number >= 0) ? serial_number : instance+1; + + // fill in cell voltages const auto &cell_voltages = battery_lib.get_cell_voltages(instance); - for (uint8_t i = 0; i < ARRAY_SIZE(cell_voltages.cells); i++) { if (cell_voltages.cells[i] == 0xFFFFU) { break; @@ -112,8 +122,14 @@ void AP_Periph_FW::can_battery_send_cells(uint8_t instance) pkt->voltage_cell.data[i] = cell_voltages.cells[i]*0.001; pkt->voltage_cell.len = i+1; } - - pkt->max_current = nanf(""); + + // fill in max current + float current_amps; + if (battery_lib.current_amps(current_amps, instance)) { + pkt->max_current = current_amps; + } else { + pkt->max_current = nanf(""); + } pkt->nominal_voltage = nanf(""); // encode and send message: diff --git a/Tools/AP_Periph/battery_bms.cpp b/Tools/AP_Periph/battery_bms.cpp new file mode 100644 index 0000000000000..5d86168b1d4ef --- /dev/null +++ b/Tools/AP_Periph/battery_bms.cpp @@ -0,0 +1,411 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + battery BMS includes a button which, when pressed, shows the state of charge percentage using LEDs + */ +#include "AP_Periph.h" + +#if AP_PERIPH_BATTERY_BMS_ENABLED +#include "stdio.h" +#include "battery_bms.h" + +extern const AP_HAL::HAL &hal; +extern AP_Periph_FW periph; + +// GPIO pins used for BMS LEDs +const uint8_t BatteryBMS::led_gpios[] = {AP_PERIPH_BMS_LED_PINS}; + +// configure gpio pins. returns true once configured +bool BatteryBMS::configured() +{ + if (config_complete) { + return true; + } + + // when HAL_GPIO_LED_ON is 0 then we must not use pinMode() + // as it could remove the OPENDRAIN attribute on the pin + // configure LED pins as outputs +#if HAL_GPIO_LED_ON != 0 + for (uint8_t i = 0; i < ARRAY_SIZE(led_gpios); i++) { + hal.gpio->pinMode(led_gpios[i], HAL_GPIO_OUTPUT); + } +#endif + + // configure button as input with pullup +#ifdef HAL_GPIO_PIN_BMS_BTN1 + hal.gpio->pinMode(HAL_GPIO_PIN_BMS_BTN1, HAL_GPIO_INPUT); + hal.gpio->write(HAL_GPIO_PIN_BMS_BTN1, 1); +#endif + + // mark configuration as complete + config_complete = true; + return true; +} + +// main update function +void BatteryBMS::update(void) +{ + // exit immediately if no batteries are enabled + if (periph.battery_lib.num_instances() == 0) { + return; + } + + // configure + if (!configured()) { + return; + } + +#ifdef HAL_GPIO_PIN_BMS_BTN1 + // check and handle button press events + handle_button_press(); +#endif + + // update BMS state machine + update_bms_state(); + + // update LED state machine + update_led_state(); +} + +// check and handle button press events +void BatteryBMS::handle_button_press(void) +{ + // we take action in these cases + // 1. button is released after being pressed between 10ms an 1 sec, we display the battery percentage (short press) + // 2. button is pressed for at least 1 second (long press), we start the power-up or power-down counter and animation + // 3. button is released after case 2 but before the counter and animation has completed, we cancel the power-up or power-down action + // 4. button is released after the power-up or power-down counter and animation have completed, no further action is taken + + // ignore button presses during startup to avoid false detections from GPIO initialization + uint32_t now_ms = AP_HAL::millis(); + if (!button.startup_complete && (now_ms < BUTTON_STARTUP_DELAY_MS)) { + return; + } + button.startup_complete = true; + + // read current button state (assuming active low - pressed = 0) + bool button_pressed = (hal.gpio->read(HAL_GPIO_PIN_BMS_BTN1) == 0); + + // check if button has just been pressed (transition from released to pressed) + if (button_pressed && !button.pressed_prev) { + // record time button was pressed + button.pressed_start_ms = now_ms; + + // record button pressed state before returning + button.pressed_prev = button_pressed; + button.long_press_handled = false; + return; + } + + // calculate how long the button was pressed including when button has just been released + const uint32_t press_duration_ms = (button_pressed || button.pressed_prev) ? (now_ms - button.pressed_start_ms) : 0; + const bool short_press = (press_duration_ms > BUTTON_SHORT_PRESS_THRESHOLD_MS) && (press_duration_ms < BUTTON_LONG_PRESS_THRESHOLD_MS); + const bool long_press = (press_duration_ms >= BUTTON_LONG_PRESS_THRESHOLD_MS); + + // check if button has just been released (transition from pressed to released) + bool button_released = false; + if (!button_pressed && button.pressed_prev) { + button.pressed_start_ms = 0; + button_released = true; + } + + // update button state for next iteration + button.pressed_prev = button_pressed; + + // handle release after short press + // displays battery SOC percentage on LEDs and prints voltages to the console + if (short_press && button_released) { + request_display_percentage(); + return; + } + + // handle long press event + // starts power on/off sequence + if (long_press && !button.long_press_handled) { + + // ensure we only handle long press once + button.long_press_handled = true; + + switch (bms_state) { + case BmsState::IDLE: + case BmsState::POWERING_OFF: + case BmsState::POWERED_OFF: + // if battery is off, request power on + request_bms_state(BmsState::POWERED_ON); + break; + case BmsState::POWERING_ON: + case BmsState::POWERED_ON: + // if battery if on, request power off + request_bms_state(BmsState::POWERED_OFF); + break; + } + return; + } + + // handle release after long press + if (long_press && button_released) { + // if the BMS is transitioning to powered on or off, abort the transition + switch (bms_state) { + case BmsState::POWERING_ON: + // request power off + request_bms_state(BmsState::POWERED_OFF); + break; + case BmsState::POWERING_OFF: + request_bms_state(BmsState::POWERED_ON); + break; + case BmsState::IDLE: + case BmsState::POWERED_ON: + case BmsState::POWERED_OFF: + // BMS/battery have already completed the power on/off action -- do nothing + break; + } + return; + } +} + +// request display of battery percentage using LEDs +void BatteryBMS::request_display_percentage() +{ + led_display_soc_start_ms = AP_HAL::millis(); +} + +// display battery SOC percentage using LEDs +void BatteryBMS::display_percentage() +{ + // get battery percentage + uint8_t batt_soc_pct; + if (!get_percentage(batt_soc_pct)) { + return; + } + + // calculate how many LEDs to light up based on battery percentage + // uses ceiling division to round up: 0% = 0 LEDs, 1-12% = 1 LED, etc + const uint8_t num_leds = ARRAY_SIZE(led_gpios); + const uint8_t num_leds_on = MIN(num_leds, (batt_soc_pct * num_leds + 99) / 100); + + // build bitmask for LEDs (e.g., num_leds=3 gives 0b00000111) + const uint8_t pattern = (1U << num_leds_on) - 1; + + // set the LED pattern and start display timer + set_led_pattern(pattern); +} + +// get battery percentage (0-100). returns true on success +bool BatteryBMS::get_percentage(uint8_t &percentage) +{ + percentage = 0; + + // try to get capacity remaining percentage first + if (periph.battery_lib.capacity_remaining_pct(percentage, 0)) { + return true; + } + + // fallback: calculate percentage from average cell voltage + // Li-ion/LiPo typical range: 3.0V (0%) to 4.2V (100%) + if (!periph.battery_lib.has_cell_voltages()) { + return false; + } + const AP_BattMonitor::cells &cell_voltages = periph.battery_lib.get_cell_voltages(); + uint32_t total_voltage_mv = 0; + uint8_t cell_count = 0; + + for (uint8_t i = 0; i < ARRAY_SIZE(cell_voltages.cells); i++) { + if (cell_voltages.cells[i] == UINT16_MAX) { + break; + } + total_voltage_mv += cell_voltages.cells[i]; + cell_count++; + } + + if (cell_count > 0) { + uint16_t avg_cell_voltage_mv = total_voltage_mv / cell_count; + // map 3000mV-4200mV to 0-100% + if (avg_cell_voltage_mv <= 3000) { + percentage = 0; + } else if (avg_cell_voltage_mv >= 4200) { + percentage = 100; + } else { + percentage = constrain_uint16((avg_cell_voltage_mv - 3000) * 100 / 1200, 0, 100); + } + return true; + } + + return false; +} + +// set LED pattern based on 8-bit bitmask +void BatteryBMS::set_led_pattern(uint8_t pattern) +{ + // configure LED pins as outputs + for (uint8_t i = 0; i < ARRAY_SIZE(led_gpios); i++) { + hal.gpio->write(led_gpios[i], BIT_IS_SET(pattern, i) ? HAL_GPIO_LED_ON : HAL_GPIO_LED_OFF); + } +} + +// LED state machine - manages LED display based on battery state +void BatteryBMS::update_led_state(void) +{ + // slow down LED updates + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - led_last_update_ms < LED_UPDATE_INTERVAL_MS) { + return; + } + led_last_update_ms = now_ms; + + // display state-of-charge (SOC) percentage + if (led_display_soc_start_ms > 0) { + // display SOC percentage + display_percentage(); + + // turn off SOC display after 1 second + if (now_ms - led_display_soc_start_ms >= LED_DISPLAY_SOC_DURATION_MS) { + led_display_soc_start_ms = 0; + } + return; + } + + // handle POWERING_ON / OFF transition + if (bms_state == BmsState::POWERING_ON || bms_state == BmsState::POWERING_OFF) { + // turn on LEDs sequentially + set_led_pattern((1 << bms_transition_counter) - 1); + return; + } + + // run animation while charging + auto battery_charging_state = periph.battery_lib.get_charging_state(); + if (battery_charging_state == AP_BattMonitor::ChargingState::CHARGING) { + led_charging_animation_step = (led_charging_animation_step + 1) % 8; + + // charging: chase forward (bit 0 -> 7) + uint8_t pattern = 1 << led_charging_animation_step; + set_led_pattern(pattern); + return; + } + + // handle POWERED_ON state - display SOC percentage + if (bms_state == BmsState::POWERED_ON) { + display_percentage(); + return; + } + + // if we get this far, turn all LEDs off + set_led_pattern(0x00); +} + +// set bms state. the only valid inputs are POWERED_ON and POWERING_OFF +// return true on success +bool BatteryBMS::request_bms_state(BmsState new_state) +{ + // sanity check inputs + if (new_state != BmsState::POWERED_ON && new_state != BmsState::POWERED_OFF) { + return false; + } + + // if request is already in progress then return true + if (req_bms_state == new_state) { + return true; + } + + // handle request to power on the battery + if (new_state == BmsState::POWERED_ON) { + // allow cancelling a power-off transition + if (bms_state == BmsState::POWERING_OFF) { + req_bms_state = new_state; + return true; + } + // allow power on when battery is in IDLE state + if (periph.battery_lib.get_charging_state() == AP_BattMonitor::ChargingState::IDLE) { + req_bms_state = new_state; + return true; + } + return false; + } + + // always accept request to power off the battery + req_bms_state = new_state; + return true; +} + +// update bms state. transitions bms_state to req_bms_state +void BatteryBMS::update_bms_state() +{ + // return immediately if current state matches requested state or requested state is IDLE + if (bms_state == req_bms_state || req_bms_state == BmsState::IDLE) { + return; + } + + // rate limit state transitions to match LED update interval (~1.2 seconds for full animation) + uint32_t now_ms = AP_HAL::millis(); + if (now_ms - bms_last_update_ms < LED_UPDATE_INTERVAL_MS) { + return; + } + bms_last_update_ms = now_ms; + + // handle request to power on + if (req_bms_state == BmsState::POWERED_ON) { + switch (bms_state) { + case BmsState::IDLE: + case BmsState::POWERING_OFF: + case BmsState::POWERED_OFF: + // move to powering on + bms_state = BmsState::POWERING_ON; + bms_transition_counter = 0; + break; + case BmsState::POWERING_ON: + bms_transition_counter++; + if (bms_transition_counter >= 8) { + bms_state = BmsState::POWERED_ON; + bms_transition_counter = 0; + periph.battery_lib.set_powered_state(0, true); + } + break; + case BmsState::POWERED_ON: + // already powered on -- do nothing + break; + } + return; + } + + // handle request to power off + if (req_bms_state == BmsState::POWERED_OFF) { + switch (bms_state) { + case BmsState::IDLE: + case BmsState::POWERED_OFF: + // already powered off -- do nothing + break; + case BmsState::POWERED_ON: + // move to powering off state + bms_transition_counter = 8; + bms_state = BmsState::POWERING_OFF; + break; + case BmsState::POWERING_ON: + // move to powering off state. transition counter should already be between 0 and 8 + bms_state = BmsState::POWERING_OFF; + break; + case BmsState::POWERING_OFF: + if (bms_transition_counter > 0) { + bms_transition_counter--; + } + if (bms_transition_counter == 0) { + bms_state = BmsState::POWERED_OFF; + periph.battery_lib.set_powered_state(0, false); + } + break; + } + return; + } +} + +#endif // AP_PERIPH_BATTERY_BMS_ENABLED diff --git a/Tools/AP_Periph/battery_bms.h b/Tools/AP_Periph/battery_bms.h new file mode 100644 index 0000000000000..9fb483c4e8bd6 --- /dev/null +++ b/Tools/AP_Periph/battery_bms.h @@ -0,0 +1,81 @@ +#pragma once + +#if AP_PERIPH_BATTERY_BMS_ENABLED + +class BatteryBMS { +public: + friend class AP_Periph_FW; + + // main update function + void update(void); + +private: + + // configure gpio pins. returns true once configured + bool configured(); + + // check and handle button press events + void handle_button_press(); + + // request display of battery SOC percentage using LEDs + void request_display_percentage(); + + // display battery SOC percentage using LEDs + void display_percentage(); + + // get battery SOC percentage (0-100). returns true on success + bool get_percentage(uint8_t &percentage); + + // set LED pattern based on 8-bit bitmask + void set_led_pattern(uint8_t pattern); + + // update the LEDs. called regularly from update() + void update_led_state(void); + + // BMS state machine variables + enum class BmsState : uint8_t { + IDLE = 0, + POWERING_ON, + POWERED_ON, + POWERING_OFF, + POWERED_OFF + }; + + // request change in bms state. the only valid inputs are POWERED_ON and POWERING_OFF + // return true on success + bool request_bms_state(BmsState new_state); + + // update bms state. transitions bms_state to req_bms_state + void update_bms_state(); + + // configuration variables + bool config_complete; // true once configuration has been completed + + // button handling variables + struct { + bool startup_complete; // true once startup delay has completed + bool pressed_prev; // true if button was pressed during previous iteration + uint32_t pressed_start_ms; // system time that button was first detected as pressed + bool short_press_handled; // true once a short press has been detected and handled + bool long_press_handled; // true once a long pressed has been detected and handled + } button; + static const uint32_t BUTTON_SHORT_PRESS_THRESHOLD_MS = 10; // 10 ms for short press + static const uint32_t BUTTON_LONG_PRESS_THRESHOLD_MS = 1000; // 1 second for long press + static const uint32_t BUTTON_STARTUP_DELAY_MS = 2000; // ignore button presses for first 2 seconds after startup + + // BMS state machine variables + BmsState bms_state, req_bms_state; // current and requested BMS states + uint32_t bms_last_update_ms; // system time of last state machine update + uint8_t bms_transition_counter; // transition counter also used for animation + + // LED display variables + uint32_t led_last_update_ms; // system time of last LED update. used to rate limit LED updates to 20hz + uint32_t led_display_soc_start_ms; // system time that SOC display started. 0 if not displaying SOC + uint8_t led_charging_animation_step; // LED charging animation step + static const uint32_t LED_UPDATE_INTERVAL_MS = 50; // update LEDs at 20hz + static const uint32_t LED_DISPLAY_SOC_DURATION_MS = 1000; // Display SOC percentage for 1 second + static const uint8_t led_gpios[]; // GPIO pins used for BMS LEDs +}; + +#endif // AP_PERIPH_BATTERY_BMS_ENABLED + diff --git a/Tools/Frame_params/3DR_Iris+.param b/Tools/Frame_params/3DR_Iris+.param index 666f3db437a05..c710f4da161e4 100644 --- a/Tools/Frame_params/3DR_Iris+.param +++ b/Tools/Frame_params/3DR_Iris+.param @@ -35,5 +35,5 @@ SERVO9_MAX,1520 SERVO9_MIN,1000 SERIAL2_PROTOCOL,3 THR_DZ,50 -PSC_VELXY_I,0.5 -PSC_VELXY_P,1 +PSC_NE_VEL_I,0.5 +PSC_NE_VEL_P,1 diff --git a/Tools/Frame_params/DJI_AGRAS_MG-1.param b/Tools/Frame_params/DJI_AGRAS_MG-1.param index c0dbcf802307c..76477a9fca783 100644 --- a/Tools/Frame_params/DJI_AGRAS_MG-1.param +++ b/Tools/Frame_params/DJI_AGRAS_MG-1.param @@ -21,5 +21,5 @@ MOT_SPIN_ARM,0.15 MOT_SPIN_MIN,0.2 MOT_THST_EXPO,0.75 MOT_THST_HOVER,0.1869573 -PSC_ACCZ_I,0.4 -PSC_ACCZ_P,0.2 +PSC_D_ACC_I,0.04 +PSC_D_ACC_P,0.02 diff --git a/Tools/Frame_params/EAMS_445.param b/Tools/Frame_params/EAMS_445.param index bc6812658f6da..e6a4d9a28a4f4 100644 --- a/Tools/Frame_params/EAMS_445.param +++ b/Tools/Frame_params/EAMS_445.param @@ -25,4 +25,4 @@ BATT_FS_LOW_ACT,2 MOT_THST_HOVER,0.2557016 SERIAL1_BAUD,38 NTF_BUZZ_VOLUME,0 -PSC_VELXY_P,1.03 +PSC_NE_VEL_P,1.03 diff --git a/Tools/Frame_params/Hexsoon-edu650.param b/Tools/Frame_params/Hexsoon-edu650.param index 9c4dab09910b1..f0f070e87b7bf 100644 --- a/Tools/Frame_params/Hexsoon-edu650.param +++ b/Tools/Frame_params/Hexsoon-edu650.param @@ -35,5 +35,5 @@ MOT_SPIN_MIN,0.18 MOT_THST_EXPO,0.70 MOT_THST_HOVER,0.20 PILOT_Y_RATE,60 -PSC_ACCZ_I,0.5 -PSC_ACCZ_P,0.3 +PSC_D_ACC_I,0.05 +PSC_D_ACC_P,0.03 diff --git a/Tools/Frame_params/Hexsoon-td860.param b/Tools/Frame_params/Hexsoon-td860.param index 0b8989122366c..e7e102973ce9f 100644 --- a/Tools/Frame_params/Hexsoon-td860.param +++ b/Tools/Frame_params/Hexsoon-td860.param @@ -32,5 +32,5 @@ MOT_SPIN_ARM,0.09 MOT_SPIN_MIN,0.15 MOT_THST_EXPO,0.7 MOT_THST_HOVER,0.185 -PSC_ACCZ_I,0.370 -PSC_ACCZ_P,0.185 +PSC_D_ACC_I,0.037 +PSC_D_ACC_P,0.0185 diff --git a/Tools/Frame_params/Holybro-QAV250-bdshot.param b/Tools/Frame_params/Holybro-QAV250-bdshot.param index 5ee424b27b2c9..0eb2098b5e28f 100644 --- a/Tools/Frame_params/Holybro-QAV250-bdshot.param +++ b/Tools/Frame_params/Holybro-QAV250-bdshot.param @@ -48,8 +48,8 @@ MOT_SPIN_ARM 0.06 MOT_SPIN_MIN 0.08 MOT_THST_EXPO 0.55 MOT_THST_HOVER 0.125 -PSC_ACCZ_I 0.168 -PSC_ACCZ_P 0.084 +PSC_D_ACC_I 0.0168 +PSC_D_ACC_P 0.0084 SERVO_BLH_AUTO 1 SERVO_DSHOT_ESC 2 SERVO_DSHOT_RATE 3 diff --git a/Tools/Frame_params/Holybro-QAV250.param b/Tools/Frame_params/Holybro-QAV250.param index af5072de6d903..c6fefc07c2919 100644 --- a/Tools/Frame_params/Holybro-QAV250.param +++ b/Tools/Frame_params/Holybro-QAV250.param @@ -48,7 +48,7 @@ MOT_SPIN_ARM 0.06 MOT_SPIN_MIN 0.08 MOT_THST_EXPO 0.55 MOT_THST_HOVER 0.125 -PSC_ACCZ_I 0.168 -PSC_ACCZ_P 0.084 +PSC_D_ACC_I 0.0168 +PSC_D_ACC_P 0.0084 SERVO_DSHOT_ESC 2 SERVO_DSHOT_RATE 3 diff --git a/Tools/Frame_params/Holybro-X500v2-bdshot.param b/Tools/Frame_params/Holybro-X500v2-bdshot.param index 51f8733d21b43..e8382fe9893a0 100644 --- a/Tools/Frame_params/Holybro-X500v2-bdshot.param +++ b/Tools/Frame_params/Holybro-X500v2-bdshot.param @@ -35,8 +35,8 @@ MOT_BAT_VOLT_MIN 13.2 MOT_SPIN_ARM 0.07 MOT_SPIN_MIN 0.09 MOT_THST_HOVER 0.2905434 -PSC_ACCZ_I 0.6 -PSC_ACCZ_P 0.3 +PSC_D_ACC_I 0.06 +PSC_D_ACC_P 0.03 SERVO_BLH_AUTO 1 SERVO_BLH_MASK 3840 SERVO_BLH_OTYPE 5 diff --git a/Tools/Frame_params/Holybro-kospi1.param b/Tools/Frame_params/Holybro-kospi1.param index 89b2bfa9a361b..f0ab12a8afa26 100644 --- a/Tools/Frame_params/Holybro-kospi1.param +++ b/Tools/Frame_params/Holybro-kospi1.param @@ -38,9 +38,9 @@ MOT_THST_EXPO,0.5 MOT_THST_HOVER,0.125 PILOT_ACCEL_Z,800 PILOT_SPEED_UP,500 -PSC_ACCZ_P,0.3 -PSC_POSZ_P,2 -PSC_VELZ_P,7 +PSC_D_ACC_P,0.03 +PSC_D_POS_P,2 +PSC_D_VEL_P,7 SCHED_LOOP_RATE,500 SERVO_BLH_AUTO,1 SERVO_BLH_DEBUG,1 diff --git a/Tools/Frame_params/QuadPlanes/Aerofox_AYK320.param b/Tools/Frame_params/QuadPlanes/Aerofox_AYK320.param index ec82b5cf04de7..4879ae8365462 100644 --- a/Tools/Frame_params/QuadPlanes/Aerofox_AYK320.param +++ b/Tools/Frame_params/QuadPlanes/Aerofox_AYK320.param @@ -86,10 +86,10 @@ Q_M_BAT_VOLT_MIN 40 Q_M_THST_HOVER 0.338 # soften VTOL position controller -Q_P_POSXY_P 0.5 -Q_P_VELXY_D 0.17 -Q_P_VELXY_I 0.35 -Q_P_VELXY_P 0.7 +Q_P_NE_POS_P 0.5 +Q_P_NE_VEL_D 0.17 +Q_P_NE_VEL_I 0.35 +Q_P_NE_VEL_P 0.7 Q_RTL_ALT 40 Q_TRANS_DECEL 1.5 diff --git a/Tools/Frame_params/QuadPlanes/Foxtech_GreatShark.param b/Tools/Frame_params/QuadPlanes/Foxtech_GreatShark.param index a75dc7b7b3921..0544434afd1a5 100644 --- a/Tools/Frame_params/QuadPlanes/Foxtech_GreatShark.param +++ b/Tools/Frame_params/QuadPlanes/Foxtech_GreatShark.param @@ -88,10 +88,10 @@ Q_M_BAT_VOLT_MIN 40 Q_M_THST_HOVER 0.338 # soften VTOL position controller -Q_P_POSXY_P 0.5 -Q_P_VELXY_D 0.17 -Q_P_VELXY_I 0.35 -Q_P_VELXY_P 0.7 +Q_P_NE_POS_P 0.5 +Q_P_NE_VEL_D 0.17 +Q_P_NE_VEL_I 0.35 +Q_P_NE_VEL_P 0.7 Q_RTL_ALT 40 Q_TRANS_DECEL 1.5 diff --git a/Tools/Frame_params/QuadPlanes/MFD_Crosswind_VTOL.param b/Tools/Frame_params/QuadPlanes/MFD_Crosswind_VTOL.param index 18bbf11b86776..7354dd73aed66 100644 --- a/Tools/Frame_params/QuadPlanes/MFD_Crosswind_VTOL.param +++ b/Tools/Frame_params/QuadPlanes/MFD_Crosswind_VTOL.param @@ -80,9 +80,9 @@ Q_M_BAT_VOLT_MIN 18.6 Q_M_THST_EXPO 0.7 Q_M_THST_HOVER 0.445192 -Q_P_JERK_Z 2 -Q_P_POSXY_P 0.7 -Q_P_VELXY_P 1 +Q_P_JERK_D 2 +Q_P_NE_POS_P 0.7 +Q_P_NE_VEL_P 1 Q_RTL_ALT 60 diff --git a/Tools/Frame_params/QuadPlanes/Mugin_EV350.param b/Tools/Frame_params/QuadPlanes/Mugin_EV350.param index a75aabdbe015a..b2ae96f617176 100644 --- a/Tools/Frame_params/QuadPlanes/Mugin_EV350.param +++ b/Tools/Frame_params/QuadPlanes/Mugin_EV350.param @@ -104,11 +104,11 @@ Q_M_BAT_VOLT_MIN 39.0 Q_M_THST_EXPO 0.7 Q_M_THST_HOVER 0.264 Q_M_YAW_HEADROOM 200 -Q_P_JERK_XY 2 -Q_P_POSXY_P 0.5 -Q_P_VELXY_D 0.17 -Q_P_VELXY_I 0.37 -Q_P_VELXY_P 0.7 +Q_P_JERK_NE 2 +Q_P_NE_POS_P 0.5 +Q_P_NE_VEL_D 0.17 +Q_P_NE_VEL_I 0.37 +Q_P_NE_VEL_P 0.7 Q_RTL_ALT 40 Q_THROTTLE_EXPO 0.2 Q_TRANS_DECEL 1.5 diff --git a/Tools/Frame_params/QuadPlanes/SparkleTech-EagleHero.param b/Tools/Frame_params/QuadPlanes/SparkleTech-EagleHero.param index 901b3ac166fd3..62bde3cdf7107 100644 --- a/Tools/Frame_params/QuadPlanes/SparkleTech-EagleHero.param +++ b/Tools/Frame_params/QuadPlanes/SparkleTech-EagleHero.param @@ -84,10 +84,10 @@ Q_M_SLEW_UP_TIME 0.2 Q_M_THST_EXPO 0.7 Q_M_THST_HOVER 0.19841 -Q_P_ACCZ_I 0.5 -Q_P_ACCZ_P 0.15 -Q_P_POSZ_P 0.5 -Q_P_VELZ_P 2.5 +Q_P_D_ACC_I 0.05 +Q_P_D_ACC_P 0.015 +Q_P_D_POS_P 0.5 +Q_P_D_VEL_P 2.5 Q_TRANS_DECEL 1.5 diff --git a/Tools/Frame_params/QuadPlanes/XPlane-Alia.parm b/Tools/Frame_params/QuadPlanes/XPlane-Alia.parm index 7c038475871b9..7ddeb3f424e7c 100644 --- a/Tools/Frame_params/QuadPlanes/XPlane-Alia.parm +++ b/Tools/Frame_params/QuadPlanes/XPlane-Alia.parm @@ -27,12 +27,12 @@ Q_M_SPIN_MIN,0.25 Q_M_THST_HOVER,0.66 Q_ANGLE_MAX,1500 Q_TRANSITION_MS,10000 -Q_P_POSZ_P,0.5 -Q_P_VELZ_P,2.5 -Q_P_POSXY_P,0.1 -Q_P_VELXY_P,0.2 -Q_P_VELXY_I,0.1 -Q_P_VELXY_D,0.059 +Q_P_D_POS_P,0.5 +Q_P_D_VEL_P,2.5 +Q_P_NE_POS_P,0.1 +Q_P_NE_VEL_P,0.2 +Q_P_NE_VEL_I,0.1 +Q_P_NE_VEL_D,0.059 Q_PILOT_SPD_UP,5.00 Q_WP_SPEED,1500 Q_WP_SPEED_DN,500 diff --git a/Tools/Frame_params/Solo-Copter-GreenCube.param b/Tools/Frame_params/Solo-Copter-GreenCube.param index d99d5228729df..cda8a68bcfd6b 100644 --- a/Tools/Frame_params/Solo-Copter-GreenCube.param +++ b/Tools/Frame_params/Solo-Copter-GreenCube.param @@ -64,9 +64,6 @@ GPS_NAVFILTER,6 INS_ACC_BODYFIX,3 INS_FAST_SAMPLE,3 INS_TRIM_OPTION,2 -LAND_ALT_LOW,600 -LAND_SPEED,45 -LAND_SPEED_HIGH,220 LOG_BACKEND_TYPE,3 LOG_DISARMED,1 LOG_FILE_BUFSIZE,8 @@ -99,8 +96,8 @@ PILOT_SPEED_DN,250 PILOT_THR_BHV,7 PILOT_THR_FILT,2 PILOT_TKOFF_ALT,214 -PSC_ACCZ_I,1.5 -PSC_ACCZ_P,0.75 +PSC_D_ACC_I,0.15 +PSC_D_ACC_P,0.075 RC1_MAX,2000 RC1_MIN,1000 RC2_MAX,2000 @@ -116,9 +113,6 @@ RC6_OPTION,213 RC6_TRIM,1000 RC7_MAX,2000 RC7_MIN,1000 -RTL_ALT,4500 -RTL_CLIMB_MIN,1000 -RTL_SPEED,800 SERIAL1_BAUD,921 SERIAL4_BAUD,230 SERIAL4_PROTOCOL,1 diff --git a/Tools/Frame_params/Sub/bluerov2-4_0_0.params b/Tools/Frame_params/Sub/bluerov2-4_0_0.params index d2794c79fa37d..73656cfdb03d2 100644 --- a/Tools/Frame_params/Sub/bluerov2-4_0_0.params +++ b/Tools/Frame_params/Sub/bluerov2-4_0_0.params @@ -396,25 +396,25 @@ PILOT_ACCEL_Z,100 PILOT_SPEED_DN,0 PILOT_SPEED_UP,500 PILOT_THR_FILT,0.0 -PSC_ACCZ_D,0.0 -PSC_ACCZ_FF,0.0 -PSC_ACCZ_FLTD,0.0 -PSC_ACCZ_FLTE,20.0 -PSC_ACCZ_FLTT,0.0 -PSC_ACCZ_I,0.10000000149011612 -PSC_ACCZ_IMAX,100.0 -PSC_ACCZ_P,0.5 +PSC_D_ACC_D,0 +PSC_D_ACC_FF,0.0 +PSC_D_ACC_FLTD,0.0 +PSC_D_ACC_FLTE,20.0 +PSC_D_ACC_FLTT,0.0 +PSC_D_ACC_I,0.01000000015 +PSC_D_ACC_IMAX,0.1 +PSC_D_ACC_P,0.05 PSC_ACC_XY_FILT,2.0 PSC_ANGLE_MAX,0.0 -PSC_POSXY_P,1.0 -PSC_POSZ_P,3.0 -PSC_VELXY_D,0.0 -PSC_VELXY_D_FILT,5.0 -PSC_VELXY_FILT,5.0 -PSC_VELXY_I,0.5 -PSC_VELXY_IMAX,1000.0 -PSC_VELXY_P,1.0 -PSC_VELZ_P,8.0 +PSC_NE_POS_P,1.0 +PSC_D_POS_P,3.0 +PSC_NE_VEL_D,0.0 +PSC_NE_VEL_D_FILT,5.0 +PSC_NE_VEL_FILT,5.0 +PSC_NE_VEL_I,0.5 +PSC_NE_VEL_IMAX,10 +PSC_NE_VEL_P,1.0 +PSC_D_VEL_P,8.0 RC10_DZ,0 RC10_MAX,1900 RC10_MIN,1100 diff --git a/Tools/Frame_params/Sub/bluerov2-heavy-4_0_0.params b/Tools/Frame_params/Sub/bluerov2-heavy-4_0_0.params index 56ec37693d5b6..524cb39f6c397 100644 --- a/Tools/Frame_params/Sub/bluerov2-heavy-4_0_0.params +++ b/Tools/Frame_params/Sub/bluerov2-heavy-4_0_0.params @@ -396,25 +396,25 @@ PILOT_ACCEL_Z,100 PILOT_SPEED_DN,0 PILOT_SPEED_UP,500 PILOT_THR_FILT,0.0 -PSC_ACCZ_D,0.0 -PSC_ACCZ_FF,0.0 -PSC_ACCZ_FLTD,0.0 -PSC_ACCZ_FLTE,20.0 -PSC_ACCZ_FLTT,0.0 -PSC_ACCZ_I,0.10000000149011612 -PSC_ACCZ_IMAX,100.0 -PSC_ACCZ_P,0.5 +PSC_D_ACC_D,0 +PSC_D_ACC_FF,0.0 +PSC_D_ACC_FLTD,0.0 +PSC_D_ACC_FLTE,20.0 +PSC_D_ACC_FLTT,0.0 +PSC_D_ACC_I,0.01000000015 +PSC_D_ACC_IMAX,0.1 +PSC_D_ACC_P,0.05 PSC_ACC_XY_FILT,2.0 PSC_ANGLE_MAX,0.0 -PSC_POSXY_P,1.0 -PSC_POSZ_P,3.0 -PSC_VELXY_D,0.0 -PSC_VELXY_D_FILT,5.0 -PSC_VELXY_FILT,5.0 -PSC_VELXY_I,0.5 -PSC_VELXY_IMAX,1000.0 -PSC_VELXY_P,1.0 -PSC_VELZ_P,8.0 +PSC_NE_POS_P,1.0 +PSC_D_POS_P,3.0 +PSC_NE_VEL_D,0.0 +PSC_NE_VEL_D_FILT,5.0 +PSC_NE_VEL_FILT,5.0 +PSC_NE_VEL_I,0.5 +PSC_NE_VEL_IMAX,10 +PSC_NE_VEL_P,1.0 +PSC_D_VEL_P,8.0 RC10_DZ,0 RC10_MAX,1900 RC10_MIN,1100 diff --git a/Tools/Frame_params/iflight-chimera7-4.2.param b/Tools/Frame_params/iflight-chimera7-4.2.param index aa4d0857cbfd8..d09149f362f85 100644 --- a/Tools/Frame_params/iflight-chimera7-4.2.param +++ b/Tools/Frame_params/iflight-chimera7-4.2.param @@ -62,8 +62,8 @@ MOT_THST_EXPO,0.62 MOT_THST_HOVER,0.12 PILOT_Y_EXPO,0.4 PILOT_Y_RATE,600 -PSC_ACCZ_I,0.24 -PSC_ACCZ_P,0.12 +PSC_D_ACC_I,0.024 +PSC_D_ACC_P,0.012 SCHED_LOOP_RATE,800 SERVO_BLH_AUTO,1 SERVO_BLH_BDMASK,15 diff --git a/Tools/GIT_Test/GIT_Success.txt b/Tools/GIT_Test/GIT_Success.txt index f6a8b9fe6bfd0..f8249d154caf7 100644 --- a/Tools/GIT_Test/GIT_Success.txt +++ b/Tools/GIT_Test/GIT_Success.txt @@ -213,4 +213,6 @@ Cameron Cho Zhou Su Nithin P Shaik Imran -Yoonwhoi Kim \ No newline at end of file +Yoonwhoi Kim +Takemichi Matsushita +Kosei Noda \ No newline at end of file diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp index 0c9e57c99acab..d3de022678bba 100644 --- a/Tools/Replay/Replay.cpp +++ b/Tools/Replay/Replay.cpp @@ -160,7 +160,7 @@ void Replay::_parse_command_line(uint8_t argc, char * const argv[]) } struct user_parameter *u = NEW_NOTHROW user_parameter; strncpy(u->name, gopt.optarg, eq-gopt.optarg); - u->value = atof(eq+1); + u->value = strtof(eq+1, nullptr); u->next = user_parameters; user_parameters = u; break; @@ -368,7 +368,7 @@ bool Replay::parse_param_line(char *line, char **vname, float &value) if (value_s == NULL) { return false; } - value = atof(value_s); + value = strtof(value_s, nullptr); *vname = pname; return true; } diff --git a/Tools/Replay/check_replay_branch.py b/Tools/Replay/check_replay_branch.py index 0047a8b8c97b3..7f4d992c941fc 100755 --- a/Tools/Replay/check_replay_branch.py +++ b/Tools/Replay/check_replay_branch.py @@ -8,7 +8,7 @@ Run check_replay.py over the produced log ''' -import git +import git # https://pypi.org/project/GitPython/ import glob import os import subprocess @@ -20,8 +20,10 @@ import check_replay class CheckReplayBranch(object): - def __init__(self, master='remotes/origin/master'): + def __init__(self, master='master', no_clean=False, no_debug=False): self.master = master + self.no_clean = no_clean + self.no_debug = no_debug def find_topdir(self): here = os.getcwd() @@ -111,7 +113,19 @@ def run_autotest_replay_on_master(self): self.repo.head.reset(index=True, working_tree=True) # generate logs: - subprocess.check_call(["Tools/autotest/autotest.py", "--debug", "build.Copter", "test.Copter.Replay"]) + args = ["Tools/autotest/autotest.py"] + + if self.no_debug: + args.append("--no-debug") + else: + args.append("--debug") + + if self.no_clean: + args.append("--no-clean") + + args.extend(["build.Copter", "test.Copter.Replay"]) + + subprocess.check_call(args) # actually run the test # check out the original branch: self.repo.head.reference = old_branch @@ -127,7 +141,7 @@ def find_replayed_logs(self): m = dfreader.recv_match(type='MSG') if m is None: break - match = re.match(".*Running replay on \(([^)]+)\).*", m.Message) + match = re.match(r".*Running replay on \(([^)]+)\).*", m.Message) if match is None: continue replayed_logs.add(match.group(1)) @@ -182,11 +196,13 @@ def run(self): import sys from argparse import ArgumentParser parser = ArgumentParser(description=__doc__) - parser.add_argument("--master", default='remotes/origin/master', help="branch to consider master branch") + parser.add_argument("--master", default='master', help="branch to consider master branch") + parser.add_argument("--no-clean", action="store_true", help="do not clean SITL before building") + parser.add_argument("--no-debug", action="store_true", help="do not make built SITL binaries debug binaries") args = parser.parse_args() - s = CheckReplayBranch(master=args.master) + s = CheckReplayBranch(master=args.master, no_clean=args.no_clean, no_debug=args.no_debug) if not s.run(): sys.exit(1) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index a8c8439b89ead..66e2a1789980f 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -394,6 +394,7 @@ def configure_env(self, cfg, env): '-Wno-redundant-decls', '-Wno-unknown-pragmas', '-Wno-expansion-to-defined', + '-Wno-invalid-offsetof', '-Werror=reorder', '-Werror=cast-align', '-Werror=attributes', @@ -664,6 +665,10 @@ def add_dynamic_boards_linux(): '''add boards based on existence of hwdef.dat in subdirectories for ''' add_dynamic_boards_from_hwdef_dir(LinuxBoard, 'libraries/AP_HAL_Linux/hwdef') +def add_dynamic_boards_sitl(): + '''add boards based on existence of hwdef.dat in subdirectories for ''' + add_dynamic_boards_from_hwdef_dir(SITLBoard, 'libraries/AP_HAL_SITL/hwdef') + def add_dynamic_boards_from_hwdef_dir(base_type, hwdef_dir): '''add boards based on existence of hwdef.dat in subdirectory''' dirname, dirlist, filenames = next(os.walk(hwdef_dir)) @@ -693,6 +698,7 @@ def get_boards_names(): add_dynamic_boards_chibios() add_dynamic_boards_esp32() add_dynamic_boards_linux() + add_dynamic_boards_sitl() return sorted(list(_board_classes.keys()), key=str.lower) @@ -751,7 +757,8 @@ def get_board(ctx): # identify opportunities to simplify common flags. In the future might # be worthy to keep board definitions in files of their own. -class sitl(Board): +class SITLBoard(Board): + abstract = True def __init__(self): super().__init__() @@ -759,8 +766,22 @@ def __init__(self): self.with_can = True self.with_littlefs = True + def configure(self, cfg): + if hasattr(self, 'hwdef'): + cfg.env.HWDEF = self.hwdef + + # load hwdef, extract toolchain from cfg.env: + cfg.load('sitl') + if cfg.env.TOOLCHAIN: + self.toolchain = cfg.env.TOOLCHAIN + else: + # default tool-chain for SITL-based boards: + self.toolchain = 'native' + + super().configure(cfg) + def configure_env(self, cfg, env): - super(sitl, self).configure_env(cfg, env) + super().configure_env(cfg, env) env.DEFINES.update( CONFIG_HAL_BOARD = 'HAL_BOARD_SITL', CONFIG_HAL_BOARD_SUBTYPE = 'HAL_BOARD_SUBTYPE_NONE', @@ -785,11 +806,12 @@ def configure_env(self, cfg, env): '-DAC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT=HAL_GCS_ENABLED&&AP_FENCE_ENABLED' ]) - try: - env.CXXFLAGS.remove('-DHAL_NAVEKF2_AVAILABLE=0') - except ValueError: - pass - env.CXXFLAGS += ['-DHAL_NAVEKF2_AVAILABLE=1'] + if not cfg.env.AP_PERIPH: + try: + env.CXXFLAGS.remove('-DHAL_NAVEKF2_AVAILABLE=0') + except ValueError: + pass + env.CXXFLAGS += ['-DHAL_NAVEKF2_AVAILABLE=1'] if self.with_can: cfg.define('HAL_NUM_CAN_IFACES', 2) @@ -960,179 +982,14 @@ def configure_env(self, cfg, env): def get_name(self): return self.__class__.__name__ - -class sitl_periph(sitl): - def configure_env(self, cfg, env): - cfg.env.AP_PERIPH = 1 - super(sitl_periph, self).configure_env(cfg, env) - env.DEFINES.update( - HAL_BUILD_AP_PERIPH = 1, - PERIPH_FW = 1, - HAL_RAM_RESERVE_START = 0, - - CANARD_ENABLE_CANFD = 1, - CANARD_ENABLE_TAO_OPTION = 1, - CANARD_MULTI_IFACE = 1, - - # FIXME: SITL library should not be using AP_AHRS: - AP_AHRS_ENABLED = 1, - AP_AHRS_BACKEND_DEFAULT_ENABLED = 0, - AP_AHRS_DCM_ENABLED = 1, # need a default backend - AP_EXTERNAL_AHRS_ENABLED = 0, - - HAL_MAVLINK_BINDINGS_ENABLED = 1, - - AP_AIRSPEED_AUTOCAL_ENABLE = 0, - AP_CAN_SLCAN_ENABLED = 0, - AP_ICENGINE_ENABLED = 0, - AP_MISSION_ENABLED = 0, - AP_RCPROTOCOL_ENABLED = 0, - AP_RTC_ENABLED = 0, - AP_SCHEDULER_ENABLED = 0, - AP_SCRIPTING_ENABLED = 0, - AP_STATS_ENABLED = 0, - AP_UART_MONITOR_ENABLED = 1, - COMPASS_CAL_ENABLED = 0, - COMPASS_LEARN_ENABLED = 0, - COMPASS_MOT_ENABLED = 0, - HAL_CAN_DEFAULT_NODE_ID = 0, - HAL_CANMANAGER_ENABLED = 0, - HAL_GCS_ENABLED = 0, - HAL_GENERATOR_ENABLED = 0, - HAL_LOGGING_ENABLED = 0, - HAL_LOGGING_MAVLINK_ENABLED = 0, - HAL_PROXIMITY_ENABLED = 0, - HAL_RALLY_ENABLED = 0, - HAL_SUPPORT_RCOUT_SERIAL = 0, - AP_TERRAIN_AVAILABLE = 0, - AP_CUSTOMROTATIONS_ENABLED = 0, - AP_PERIPH_BATTERY_ENABLED = 0, - AP_PERIPH_DEVICE_TEMPERATURE_ENABLED = 0, - AP_PERIPH_SERIAL_OPTIONS_ENABLED = 0, - AP_PERIPH_ADSB_ENABLED = 0, - AP_PERIPH_PROXIMITY_ENABLED = 0, - AP_PERIPH_GPS_ENABLED = 0, - AP_PERIPH_RELAY_ENABLED = 0, - AP_PERIPH_IMU_ENABLED = 0, - AP_PERIPH_MAG_ENABLED = 0, - AP_PERIPH_BATTERY_BALANCE_ENABLED = 0, - AP_PERIPH_BATTERY_TAG_ENABLED = 0, - AP_PERIPH_MSP_ENABLED = 0, - AP_PERIPH_BARO_ENABLED = 0, - AP_PERIPH_EFI_ENABLED = 0, - AP_PERIPH_RANGEFINDER_ENABLED = 0, - AP_PERIPH_RC_OUT_ENABLED = 0, - AP_PERIPH_RTC_ENABLED = 0, - AP_PERIPH_RCIN_ENABLED = 0, - AP_PERIPH_RPM_ENABLED = 0, - AP_PERIPH_RPM_STREAM_ENABLED = 0, - AP_PERIPH_AIRSPEED_ENABLED = 0, - AP_PERIPH_HOBBYWING_ESC_ENABLED = 0, - AP_PERIPH_NETWORKING_ENABLED = 0, - AP_PERIPH_NOTIFY_ENABLED = 0, - AP_PERIPH_PWM_HARDPOINT_ENABLED = 0, - AP_PERIPH_ESC_APD_ENABLED = 0, - AP_PERIPH_NCP5623_LED_WITHOUT_NOTIFY_ENABLED = 0, - AP_PERIPH_NCP5623_BGR_LED_WITHOUT_NOTIFY_ENABLED = 0, - AP_PERIPH_TOSHIBA_LED_WITHOUT_NOTIFY_ENABLED = 0, - AP_PERIPH_BUZZER_ENABLED = 0, - AP_PERIPH_BUZZER_WITHOUT_NOTIFY_ENABLED = 0, - AP_PERIPH_RTC_GLOBALTIME_ENABLED = 0, - AP_PERIPH_ACTUATOR_TELEM_ENABLED = 0, - ) - - try: - env.CXXFLAGS.remove('-DHAL_NAVEKF2_AVAILABLE=1') - except ValueError: - pass - env.CXXFLAGS += ['-DHAL_NAVEKF2_AVAILABLE=0'] - -class sitl_periph_universal(sitl_periph): - def configure_env(self, cfg, env): - super(sitl_periph_universal, self).configure_env(cfg, env) - env.DEFINES.update( - CAN_APP_NODE_NAME = '"org.ardupilot.ap_periph_universal"', - APJ_BOARD_ID = 100, - - AP_PERIPH_GPS_ENABLED = 1, - AP_PERIPH_AIRSPEED_ENABLED = 1, - AP_PERIPH_MAG_ENABLED = 1, - AP_PERIPH_BARO_ENABLED = 1, - AP_PERIPH_IMU_ENABLED = 1, - AP_PERIPH_RANGEFINDER_ENABLED = 1, - AP_PERIPH_BATTERY_ENABLED = 1, - AP_PERIPH_EFI_ENABLED = 1, - AP_PERIPH_RPM_ENABLED = 1, - AP_PERIPH_RPM_STREAM_ENABLED = 1, - AP_RPM_STREAM_ENABLED = 1, - AP_PERIPH_RC_OUT_ENABLED = 1, - AP_PERIPH_ADSB_ENABLED = 1, - AP_PERIPH_SERIAL_OPTIONS_ENABLED = 1, - AP_AIRSPEED_ENABLED = 1, - AP_BATTERY_ESC_ENABLED = 1, - HAL_PWM_COUNT = 32, - HAL_WITH_ESC_TELEM = 1, - AP_EXTENDED_ESC_TELEM_ENABLED = 1, - AP_TERRAIN_AVAILABLE = 1, - HAL_GYROFFT_ENABLED = 0, - ) - -class sitl_periph_gps(sitl_periph): - def configure_env(self, cfg, env): - cfg.env.AP_PERIPH = 1 - super(sitl_periph_gps, self).configure_env(cfg, env) - env.DEFINES.update( - HAL_BUILD_AP_PERIPH = 1, - PERIPH_FW = 1, - CAN_APP_NODE_NAME = '"org.ardupilot.ap_periph_gps"', - APJ_BOARD_ID = 101, - - AP_PERIPH_GPS_ENABLED = 1, - ) - -class sitl_periph_battmon(sitl_periph): - def configure_env(self, cfg, env): - cfg.env.AP_PERIPH = 1 - super(sitl_periph_battmon, self).configure_env(cfg, env) - env.DEFINES.update( - HAL_BUILD_AP_PERIPH = 1, - PERIPH_FW = 1, - CAN_APP_NODE_NAME = '"org.ardupilot.ap_periph_battmon"', - APJ_BOARD_ID = 101, - - AP_PERIPH_BATTERY_ENABLED = 1, - ) - -class sitl_periph_battery_tag(sitl_periph): - def configure_env(self, cfg, env): - cfg.env.AP_PERIPH = 1 - super(sitl_periph_battery_tag, self).configure_env(cfg, env) - env.DEFINES.update( - HAL_BUILD_AP_PERIPH = 1, - PERIPH_FW = 1, - CAN_APP_NODE_NAME = '"org.ardupilot.battery_tag"', - APJ_BOARD_ID = 101, - - AP_SIM_PARAM_ENABLED = 0, - AP_KDECAN_ENABLED = 0, - AP_TEMPERATURE_SENSOR_ENABLED = 0, - AP_PERIPH_BATTERY_TAG_ENABLED = 1, - AP_RTC_ENABLED = 1, - AP_PERIPH_RTC_ENABLED = 1, - AP_PERIPH_RTC_GLOBALTIME_ENABLED = 1, - ) - -class sitl_periph_can_to_serial(sitl_periph): - def configure_env(self, cfg, env): - cfg.env.AP_PERIPH = 1 - super().configure_env(cfg, env) - env.DEFINES.update( - HAL_BUILD_AP_PERIPH = 1, - PERIPH_FW = 1, - CAN_APP_NODE_NAME = '"org.ardupilot.serial_passthrough"', - APJ_BOARD_ID = 101, - - ) + def pre_build(self, bld): + '''pre-build hook that gets called before dynamic sources''' + from waflib.Context import load_tool + module = load_tool('sitl', [], with_sys_path=True) + fun = getattr(module, 'pre_build', None) + if fun: + fun(bld) + super().pre_build(bld) class esp32(Board): abstract = True @@ -1622,17 +1479,6 @@ def get_name(self): # get name of class return self.__class__.__name__ -class SITL_static(sitl): - def configure_env(self, cfg, env): - super(SITL_static, self).configure_env(cfg, env) - cfg.env.STATIC_LINKING = True - -class SITL_x86_64_linux_gnu(SITL_static): - toolchain = 'x86_64-linux-gnu' - -class SITL_arm_linux_gnueabihf(SITL_static): - toolchain = 'arm-linux-gnueabihf' - class QURT(Board): '''support for QURT based boards''' toolchain = 'custom' diff --git a/Tools/ardupilotwaf/build_summary.py b/Tools/ardupilotwaf/build_summary.py index 43bcb57cca2f8..8d892ba1cbb9a 100644 --- a/Tools/ardupilotwaf/build_summary.py +++ b/Tools/ardupilotwaf/build_summary.py @@ -173,9 +173,12 @@ def _build_summary(bld): # totals=True means relying on -t flag to give us a "(TOTALS)" output def _parse_size_output(s, s_all, totals=False): - # Get the size of .crash_log to remove it from .bss reporting - # also get external flash size if applicable + # Get the size of .crash_log and .heap to remove them from .bss reporting as + # binutils size includes in bss the size of any section that has the ALLOC + # flag but neither CODE nor HAS_CONTENTS (as reported by objdump -h). also + # get external flash size if applicable. crash_log_size = None + heap_size = 0 ext_flash_used = 0 if s_all is not None: lines = s_all.splitlines()[1:] @@ -183,6 +186,9 @@ def _parse_size_output(s, s_all, totals=False): if ".crash_log" in line: row = line.strip().split() crash_log_size = int(row[1]) + if ".heap" in line: + row = line.strip().split() + heap_size = int(row[1]) if ".extflash" in line: row = line.strip().split() if int(row[1]) > 0: @@ -206,6 +212,7 @@ def _parse_size_output(s, s_all, totals=False): # reports BSS with crash_log included size_bss = int(row[2]) - crash_log_size size_free_flash = crash_log_size + size_bss -= heap_size # remove also-included default heap section size from bss l.append(dict( size_text=int(row[0]), diff --git a/Tools/ardupilotwaf/chibios.py b/Tools/ardupilotwaf/chibios.py index bb3d4ce95201c..dd971a33912c6 100644 --- a/Tools/ardupilotwaf/chibios.py +++ b/Tools/ardupilotwaf/chibios.py @@ -378,6 +378,9 @@ def run(self): # we omit build_time when we don't have build_dates so that apj # file is identical for same git hash and compiler d["build_time"] = int(time.time()) + if self.env.AP_SIGNED_FIRMWARE and self.env.PRIVATE_KEY: + # The firmware file was signed during the build process, so set the flag + d['signed_firmware'] = True apj_file = self.outputs[0].abspath() f = open(apj_file, "w") f.write(json.dumps(d, indent=4)) @@ -723,7 +726,8 @@ def build(bld): 'fopen', 'fflush', 'fwrite', 'fread', 'fputs', 'fgets', 'clearerr', 'fseek', 'ferror', 'fclose', 'tmpfile', 'getc', 'ungetc', 'feof', 'ftell', 'freopen', 'remove', 'vfprintf', 'vfprintf_r', 'fscanf', - '_gettimeofday', '_times', '_times_r', '_gettimeofday_r', 'time', 'clock'] + '_gettimeofday', '_times', '_times_r', '_gettimeofday_r', 'time', 'clock', + 'setjmp'] # these functions use global state that is not thread safe blacklist += ['gmtime'] diff --git a/Tools/ardupilotwaf/sitl.py b/Tools/ardupilotwaf/sitl.py new file mode 100644 index 0000000000000..6b1f9a921a8ff --- /dev/null +++ b/Tools/ardupilotwaf/sitl.py @@ -0,0 +1,80 @@ +# encoding: utf-8 + +""" +Waf tool for SITL build + +AP_FLAKE8_CLEAN +""" + +from waflib.TaskGen import after_method, feature + +import os +import sys +import traceback + +import hal_common + +sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), '../../libraries/AP_HAL_SITL/hwdef/scripts')) +import sitl_hwdef # noqa:501 + + +@feature('sitl_ap_program') +@after_method('process_source') +def sitl_firmware(self): + pass + + +def configure(cfg): + + def srcpath(path): + return cfg.srcnode.make_node(path).abspath() + + def bldpath(path): + return bldnode.make_node(path).abspath() + + env = cfg.env + bldnode = cfg.bldnode.make_node(cfg.variant) + env.SRCROOT = srcpath('') + env.BUILDROOT = bldpath('') + + def srcpath(path): + return cfg.srcnode.make_node(path).abspath() + + def bldpath(path): + return bldnode.make_node(path).abspath() + env.AP_PROGRAM_FEATURES += ['sitl_ap_program'] + + try: + hwdef_obj = generate_hwdef_h(env) + except Exception: + traceback.print_exc() + cfg.fatal("Failed to process hwdef.dat") + hal_common.process_hwdef_results(cfg, hwdef_obj) + + +def generate_hwdef_h(env): + '''run sitl_hwdef.py''' + hwdef_dir = os.path.join(env.SRCROOT, 'libraries/AP_HAL_SITL/hwdef') + + if len(env.HWDEF) == 0: + env.HWDEF = os.path.join(hwdef_dir, env.BOARD, 'hwdef.dat') + hwdef_out = env.BUILDROOT + if not os.path.exists(hwdef_out): + os.mkdir(hwdef_out) + hwdef = [env.HWDEF] + if env.HWDEF_EXTRA: + hwdef.append(env.HWDEF_EXTRA) + + hwdef_obj = sitl_hwdef.SITLHWDef( + outdir=hwdef_out, + hwdef=hwdef, + quiet=False, + ) + hwdef_obj.run() + + return hwdef_obj + + +def pre_build(bld): + '''pre-build hook to change dynamic sources''' + pass diff --git a/Tools/autotest/antennatracker.py b/Tools/autotest/antennatracker.py index cf890272e4a06..0312943001f40 100644 --- a/Tools/autotest/antennatracker.py +++ b/Tools/autotest/antennatracker.py @@ -191,6 +191,49 @@ def GPSForYaw(self): raise NotAchievedException("GPS_RAW not tracking simstate yaw") self.progress(f"yaw match ({gps_raw_hdg} vs {sim_hdg}") + def LoggerMsgChunks(self): + '''create MSG dataflash entries for very long messages''' + self.assert_parameter_value('LOG_DISARMED', 1) + short_message_text = "This is a short message" + long_message_text = "This is undubitably a ridiculously verbose and needlessly wordy missive, unavoidably designed to exceed disappointingly minuscule log buffers" # noqa:E501 + self.send_statustext(short_message_text) + self.send_statustext(long_message_text) + + self.delay_sim_time(10) + dfreader = self.dfreader_for_current_onboard_log() + self.reboot_sitl() + + phase = "short" + seq = 0 + received_long_message_text = "" + while True: + m = dfreader.recv_match(type='MSG') + if m is None: + break + self.progress(f"{m.Message=}") + msg = m.Message.lstrip("SRC=250/250:") + if phase == "short": + if msg != short_message_text: + continue + self.progress("Received short message") + phase = "long" + elif phase == "long": + if m.Seq != seq: + received_long_message_text = "" + seq = 0 + if m.Seq != 0: + raise NotAchievedException("Weird") + else: + seq += 1 + received_long_message_text += msg + self.progress(f"Text: {received_long_message_text}") + if received_long_message_text == long_message_text: + phase = "done" + break + + if phase != "done": + raise NotAchievedException(f"Did not get message text; {phase=}") # noqa:E501 + def tests(self): '''return list of all tests''' ret = super(AutoTestTracker, self).tests() @@ -203,5 +246,6 @@ def tests(self): self.SCAN, self.BaseMessageSet, self.GPSForYaw, + self.LoggerMsgChunks, ]) return ret diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 6c9980028cde7..6c77728aa0b9b 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -4629,7 +4629,7 @@ def RTLSpeed(self): self.load_mission("copter_rtl_speed.txt") self.set_parameters({ 'WPNAV_ACCEL': wpnav_accel_mss * 100, - 'RTL_SPEED': rtl_speed_ms * 100, + 'RTL_SPEED_MS': rtl_speed_ms, 'WPNAV_SPEED': wpnav_speed_ms * 100, }) self.change_mode('LOITER') @@ -5190,19 +5190,19 @@ def check_landing_speeds(land_speed_high, land_speed_low, land_alt_low, land_spe self.wait_descent_rate(land_speed_low, minimum_duration=land_low_maintain) self.wait_disarmed() - # test the defaults. By default LAND_SPEED_HIGH is 0 so + # test the defaults. By default LAND_SPD_HIGH_MS is 0 so # WPNAV_SPEED_DN is used check_landing_speeds( self.get_parameter("WPNAV_SPEED_DN") / 100, # cm/s -> m/s - self.get_parameter("LAND_SPEED") / 100, # cm/s -> m/s - self.get_parameter("LAND_ALT_LOW") / 100 # cm -> m + self.get_parameter("LAND_SPD_MS"), + self.get_parameter("LAND_ALT_LOW_M") ) def test_landing_speeds(land_speed_high, land_speed_low, land_alt_low, **kwargs): self.set_parameters({ - "LAND_SPEED_HIGH": land_speed_high * 100, # m/s -> cm/s - "LAND_SPEED": land_speed_low * 100, # m/s -> cm/s - "LAND_ALT_LOW": land_alt_low * 100, # m -> cm + "LAND_SPD_HIGH_MS": land_speed_high, + "LAND_SPD_MS": land_speed_low, + "LAND_ALT_LOW_M": land_alt_low }) check_landing_speeds(land_speed_high, land_speed_low, land_alt_low, **kwargs) @@ -5900,21 +5900,11 @@ def set_servo_gripper_parameters(self): def PayloadPlaceMission(self): """Test payload placing in auto.""" - self.context_push() - self.set_analog_rangefinder_parameters() self.set_servo_gripper_parameters() self.reboot_sitl() - self.load_mission("copter_payload_place.txt") - if self.mavproxy is not None: - self.mavproxy.send('wp list\n') - - self.set_parameter("AUTO_OPTIONS", 3) - self.change_mode('AUTO') - self.wait_ready_to_arm() - - self.arm_vehicle() + num_wp = self.load_and_start_mission("copter_payload_place.txt") self.wait_text("Gripper load releas(ed|ing)", timeout=90, regex=True) dist_limit = 1 @@ -5929,11 +5919,8 @@ def PayloadPlaceMission(self): raise NotAchievedException("Did not honour target lat/lng (dist=%f want <%f" % (dist, dist_limit)) - self.wait_disarmed() - - self.context_pop() - self.reboot_sitl() - self.progress("All done") + self.wait_waypoint(num_wp-1, num_wp-1) + self.do_RTL() def PayloadPlaceMissionOpenGripper(self): '''test running the mission when the gripper is open''' @@ -6210,6 +6197,7 @@ def constrained_mount_pitch(self, pitch_angle_deg, mount_instance=1): return min(max(pitch_angle_deg, PITCH_MIN), PITCH_MAX) def test_mount_pitch(self, despitch, despitch_tolerance, mount_mode, timeout=10, hold=0, constrained=True): + self.set_mount_mode(mount_mode) tstart = self.get_sim_time() success_start = 0 @@ -6640,6 +6628,39 @@ def Mount(self): self.reboot_sitl() # to handle MNT_TYPE changing self.mount_test_body() + def MountPOIFromAuxFunction(self): + '''test we can lock onto a lat/lng/alt with the flick of a switch''' + self.install_terrain_handlers_context() + self.setup_servo_mount() + self.set_parameters({ + "RC10_OPTION": 186, + "MNT1_RC_RATE": 0, + "TERRAIN_ENABLE": 1, + "SIM_TERRAIN": 1, + }) + self.reboot_sitl() # to handle MNT1_TYPE changing # to handle MNT1_TYPE changing + self.wait_ready_to_arm() + + self.takeoff(10, mode='GUIDED') + self.set_rc(6, 1000) + self.set_rc(10, 1900) # engage poi lock + + self.progress("checking mount angles") + mount_roll, mount_pitch, mount_yaw, mount_yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg() + assert -46 <= mount_pitch <= -44, f"Pitch is out of range: {mount_pitch}" + self.fly_guided_move_local(100, 100, 70) # move and check that pitch and yaw track + mount_roll, mount_pitch, mount_yaw, mount_yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg() + assert -17 <= mount_pitch <= -15, f"Pitch2 is out of range: {mount_pitch}" + assert -179 <= mount_yaw <= -176, f"Yaw2 is out of range: {mount_yaw}" + self.set_rc(10, 1500) # switch to middle to return to RC target mode and check that mount reverts to initial angles + mount_roll, mount_pitch, mount_yaw, mount_yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg() + assert -46 <= mount_pitch <= -44, f"Pitch3 is out of range: {mount_pitch}" + assert -1 <= mount_yaw <= 1, f"Yaw3 is out of range: {mount_yaw}" + + self.change_mode('RTL') + self.wait_disarmed() + self.assert_at_home() + def MountSolo(self): '''test type=2, a "Solo" mount''' self.set_parameters({ @@ -9321,15 +9342,15 @@ def LoweheiserAuto(self): self.customise_SITL_commandline(["--serial5=sim:loweheiser"]) self.set_parameters({ - "GEN_IDLE_TH": 25, - "GEN_IDLE_TH_H": 40, - "GEN_RUN_TEMP": 60, - "GEN_IDLE_TEMP": 80, + "GEN_L_IDLE_TH": 25, + "GEN_L_IDLE_TH_H": 40, + "GEN_L_RUN_TEMP": 60, + "GEN_L_IDLE_TEMP": 80, }) self.reboot_sitl() - self.assert_parameter_value("GEN_IDLE_TH", 25) + self.assert_parameter_value("GEN_L_IDLE_TH", 25) self.delay_sim_time(10) # so we can actually receive messages... @@ -9624,15 +9645,15 @@ def LoweheiserManual(self): self.customise_SITL_commandline(["--serial5=sim:loweheiser"]) self.set_parameters({ - "GEN_IDLE_TH": 25, - "GEN_IDLE_TH_H": 40, - "GEN_RUN_TEMP": 60, - "GEN_IDLE_TEMP": 80, + "GEN_L_IDLE_TH": 25, + "GEN_L_IDLE_TH_H": 40, + "GEN_L_RUN_TEMP": 60, + "GEN_L_IDLE_TEMP": 80, }) self.reboot_sitl() - self.assert_parameter_value("GEN_IDLE_TH", 25) + self.assert_parameter_value("GEN_L_IDLE_TH", 25) self.delay_sim_time(10) # so we can actually receive messages... @@ -10417,7 +10438,7 @@ def FlyRangeFinderMAVlink(self): self.set_parameters({ "SERIAL5_PROTOCOL": 1, "RNGFND1_TYPE": 10, - "RTL_ALT": 500, + "RTL_ALT_M": 5, "RTL_ALT_TYPE": 1, "SIM_TERRAIN": 0, }) @@ -10524,7 +10545,7 @@ def FlyRangeFinderSITL(self): '''fly the type=100 perfect rangefinder''' self.set_parameters({ "RNGFND1_TYPE": 100, - "RTL_ALT": 500, + "RTL_ALT_M": 5, "RTL_ALT_TYPE": 1, "SIM_TERRAIN": 0, }) @@ -10535,7 +10556,7 @@ def FlyRangeFinderSITL(self): def RangeFinderDrivers(self): '''Test rangefinder drivers''' self.set_parameters({ - "RTL_ALT": 500, + "RTL_ALT_M": 5, "RTL_ALT_TYPE": 1, "SIM_TERRAIN": 0, }) @@ -11133,6 +11154,31 @@ def test_replay_gps_bit(self): return current_log_filepath + def test_replay_gps_yaw_bit(self): + self.load_default_params_file("copter-gps-for-yaw.parm") + self.set_parameters({ + "LOG_REPLAY": 1, + "LOG_DISARMED": 1, + }) + self.zero_throttle() + self.reboot_sitl() + + self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_LOGGING, True, True, True) + self.wait_gps_fix_type_gte(6, message_type="GPS2_RAW", verbose=True) + + current_log_filepath = self.current_onboard_log_filepath() + self.progress("Current log path: %s" % str(current_log_filepath)) + + self.change_mode("LOITER") + self.wait_ready_to_arm(require_absolute=True) + self.arm_vehicle() + self.takeoffAndMoveAway() + self.do_RTL() + + self.reboot_sitl() + + return current_log_filepath + def test_replay_beacon_bit(self): self.set_parameters({ "LOG_REPLAY": 1, @@ -11143,6 +11189,8 @@ def test_replay_beacon_bit(self): self.BeaconPosition() new_onboard_logs = sorted(self.log_list()) + self.reboot_sitl() + log_difference = [x for x in new_onboard_logs if x not in old_onboard_logs] return log_difference[1] # index depends on the reboots and ordering thereof in BeaconPosition! @@ -11520,6 +11568,7 @@ def Replay(self): bits = [ ('GPS', self.test_replay_gps_bit), + ('GPSForYaw', self.test_replay_gps_yaw_bit), ('Beacon', self.test_replay_beacon_bit), ('OpticalFlow', self.test_replay_optical_flow_bit), ] @@ -11667,8 +11716,8 @@ def PositionWhenGPSIsZero(self): self.wait_disarmed() self.reboot_sitl() - def RTL_ALT_FINAL(self): - '''Test RTL with RTL_ALT_FINAL''' + def RTL_ALT_FINAL_M(self): + '''Test RTL with RTL_ALT_FINAL_M''' self.progress("arm the vehicle and takeoff in Guided") self.takeoff(20, mode='GUIDED') @@ -11678,7 +11727,7 @@ def RTL_ALT_FINAL(self): self.progress("fly 50m North (or whatever)") self.fly_guided_move_local(50, 0, 50) target_alt = 10 - self.set_parameter('RTL_ALT_FINAL', target_alt * 100) + self.set_parameter('RTL_ALT_FINAL_M', target_alt) self.progress("Waiting RTL to reach Home and hold") self.change_mode('RTL') @@ -12877,6 +12926,7 @@ def tests1e(self): self.MAV_CMD_DO_MOUNT_CONTROL, self.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, self.AutoYawDO_MOUNT_CONTROL, + self.MountPOIFromAuxFunction, self.Button, self.ShipTakeoff, self.RangeFinder, @@ -13394,7 +13444,7 @@ def RPLidarA1(self): def BrakeZ(self): '''check jerk limit correct in Brake mode''' - self.set_parameter('PSC_JERK_Z', 3) + self.set_parameter('PSC_JERK_D', 3) self.takeoff(50, mode='GUIDED') vx, vy, vz_up = (0, 0, -1) self.test_guided_local_velocity_target(vx=vx, vy=vy, vz_up=vz_up, timeout=10) @@ -13770,7 +13820,7 @@ def DualTuningChannels(self): "RC7_OPTION": 220, # RC7 used for second param tuning "RC7_MIN": RC7_MIN, "RC7_MAX": RC7_MAX, - "TUNE2": 28, # PSC_VELXY_I + "TUNE2": 28, # PSC_NE_VEL_I "TUNE2_MIN": TUNE2_MIN, "TUNE2_MAX": TUNE2_MAX, }) @@ -13796,18 +13846,18 @@ def DualTuningChannels(self): self.set_rc(7, RC7_MIN) self.delay_sim_time(1) - self.assert_parameter_value("PSC_VELXY_I", TUNE2_MIN) + self.assert_parameter_value("PSC_NE_VEL_I", TUNE2_MIN) self.set_rc(7, RC7_MAX) self.delay_sim_time(1) - self.assert_parameter_value("PSC_VELXY_I", TUNE2_MAX) + self.assert_parameter_value("PSC_NE_VEL_I", TUNE2_MAX) # make sure RC6 is unaffected: self.assert_parameter_value("LOIT_SPEED", int((TUNE_MIN+TUNE_MAX)/2), epsilon=1) self.set_rc(7, int((RC7_MIN+RC7_MAX)/2)) self.delay_sim_time(1) - self.assert_parameter_value("PSC_VELXY_I", int((TUNE2_MIN+TUNE2_MAX)/2), epsilon=1) + self.assert_parameter_value("PSC_NE_VEL_I", int((TUNE2_MIN+TUNE2_MAX)/2), epsilon=1) def PILOT_THR_BHV(self): '''test the PILOT_THR_BHV parameter''' @@ -14541,8 +14591,8 @@ def ReadOnlyDefaults(self): defaults_filepath = tempfile.NamedTemporaryFile(mode='w', delete=False) defaults_filepath.write(""" DISARM_DELAY 77 @READONLY -RTL_ALT 123 -RTL_ALT_FINAL 129 +RTL_ALT_M 123 +RTL_ALT_FINAL_M 129 """) defaults_filepath.close() self.customise_SITL_commandline([ @@ -14553,15 +14603,15 @@ def ReadOnlyDefaults(self): self.wait_statustext("Param write denied (DISARM_DELAY)") self.assert_parameter_value("DISARM_DELAY", 77) - self.assert_parameter_value("RTL_ALT", 123) + self.assert_parameter_value("RTL_ALT_M", 123) self.start_subtest('Ensure something is writable....') - self.set_parameter('RTL_ALT_FINAL', 101) + self.set_parameter('RTL_ALT_FINAL_M', 101) new_values_filepath = tempfile.NamedTemporaryFile(mode='w', delete=False) new_values_filepath.write(""" DISARM_DELAY 99 -RTL_ALT 111 +RTL_ALT_M 111 """) new_values_filepath.close() @@ -14576,8 +14626,8 @@ def ReadOnlyDefaults(self): self.stop_mavproxy(mavproxy) self.assert_parameter_value("DISARM_DELAY", 77) - self.assert_parameter_value("RTL_ALT", 111) - self.assert_parameter_value('RTL_ALT_FINAL', 101) + self.assert_parameter_value("RTL_ALT_M", 111) + self.assert_parameter_value('RTL_ALT_FINAL_M', 101) def ScriptingFlipMode(self): '''test adding custom mode from scripting''' @@ -14875,7 +14925,7 @@ def RTLStoppingDistanceSpeed(self): }) self.context_push() self.set_parameters({ - 'RTL_SPEED': 100, # cm/s + 'RTL_SPEED_MS': 1, # m/s }) self.change_mode('AUTO') @@ -15089,15 +15139,15 @@ def LuaParamSet(self): # PARAM_SET_ENABLE back to its original value (enabled!), # which can cause problems resetting orig_parameter_values = self.get_parameters([ - 'RTL_ALT', + 'RTL_ALT_M', 'DISARM_DELAY', ]) self.wait_ready_to_arm() # scripts will be ready by now! - self.start_subtest("set RTL_ALT freely") + self.start_subtest("set RTL_ALT_M freely") self.context_collect("STATUSTEXT") - self.set_parameter("RTL_ALT", 23) - self.set_parameter("RTL_ALT", 28) + self.set_parameter("RTL_ALT_M", 0.23) + self.set_parameter("RTL_ALT_M", 0.28) # Ensure there are no scripting errors. error = self.statustext_in_collections("Internal Error") if error is not None: @@ -15600,7 +15650,7 @@ def tests2b(self): # this block currently around 9.5mins here self.GSF, self.GSF_reset, self.AP_Avoidance, - self.RTL_ALT_FINAL, + self.RTL_ALT_FINAL_M, self.SMART_RTL, self.SMART_RTL_EnterLeave, self.SMART_RTL_Repeat, diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 7372b3be2dee6..ca7577a3d00c3 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -7156,46 +7156,100 @@ def SET_POSITION_TARGET_GLOBAL_INT_for_altitude(self): def mavlink_AIRSPEED(self): '''check receiving of two airspeed sensors''' self.set_parameters({ - "ARSPD_PIN": 2, - "ARSPD_RATIO": 0, - "ARSPD2_RATIO": 4, - "ARSPD2_TYPE": 3, # MS5525 - "ARSPD2_BUS": 1, + "ARSPD_PIN": 1, + "ARSPD_RATIO": 2, + "ARSPD2_RATIO": 2, + "ARSPD2_TYPE": 100, "ARSPD2_AUTOCAL": 1, }) self.reboot_sitl() - self.start_subtest('Ensure we get both instances') - self.wait_message_field_values('AIRSPEED', { - "id": 0, - "flags": mavutil.mavlink.AIRSPEED_SENSOR_USING, - }) - self.wait_message_field_values('AIRSPEED', { - "id": 1, - "flags": 0, - }) + # Add listener to airspeed message + airspeed_active = [None, None] + + def update_airspeed_active(mav, m): + if m.get_type() != 'AIRSPEED': + return + # 2 here is the correct value of mavutil.mavlink.AIRSPEED_SENSOR_USING + # However it was wrong in the MAVLink spec for a while + airspeed_active[m.id] = (m.flags & 2) != 0 + + self.install_message_hook_context(update_airspeed_active) self.wait_ready_to_arm() - self.takeoff() + + self.start_subtest('Ensure we get both instances') + if any(v is None for v in airspeed_active): + raise NotAchievedException("Did not get both airspeed messages %s" % str(airspeed_active)) + + if airspeed_active != [True, False]: + raise NotAchievedException("Not using expected airspeed sensors %s" % str(airspeed_active)) + + self.takeoff(mode="TAKEOFF", alt=100) + + self.start_subtest("Now testing sensor 1 is used in flight") + if airspeed_active != [True, False]: + raise NotAchievedException("Not using expected airspeed sensors %s" % str(airspeed_active)) self.start_subtest("Now testing failure of sensor 1 - fail to many m/s") self.set_parameter("SIM_ARSPD_FAIL", 60) # airspeed sensor never becomes unhealthy - we just stop using - # it as EKF3 starts to reject: - self.wait_message_field_values('AIRSPEED', { - "id": 0, - "flags": 0, - }) - # ArduPilot's airspeed redundancy is only available through - # EKF3 affinity: - self.progress("Checking we're not using second airspeed sensor") - self.wait_message_field_values('AIRSPEED', { - "id": 1, - "flags": 0, - }) + # it as EKF3 starts to reject, allow 10 seconds + self.delay_sim_time(10) + if airspeed_active != [False, False]: + raise NotAchievedException("Not using expected airspeed sensors %s" % str(airspeed_active)) + + # Re-enable sensors, after some time the sensor should be re-enabled self.set_parameter("SIM_ARSPD_FAIL", 0) + self.delay_sim_time(60) + if airspeed_active != [True, False]: + raise NotAchievedException("Not using expected airspeed sensors %s" % str(airspeed_active)) + + self.start_subtest("Now testing combinations of use and primary params") + + # Changing the primary should have no effect as the second sensor is not marked to use + self.set_parameter("ARSPD_PRIMARY", 1) + self.delay_sim_time(10) + if airspeed_active != [True, False]: + raise NotAchievedException("Not using expected airspeed sensors %s" % str(airspeed_active)) + + # Allowing the second sensor to be used should not result in a switch when armed despite the primary parameter + self.set_parameter("ARSPD2_USE", 1) + self.delay_sim_time(10) + if airspeed_active != [True, False]: + raise NotAchievedException("Not using expected airspeed sensors %s" % str(airspeed_active)) + + # Changing the primary back to the first sensor, and then again to the second + # Now the second can be used it should switch + self.set_parameter("ARSPD_PRIMARY", 0) + self.set_parameter("ARSPD_PRIMARY", 1) + self.delay_sim_time(10) + if airspeed_active != [False, True]: + raise NotAchievedException("Not using expected airspeed sensors %s" % str(airspeed_active)) + + # Now stop using the second + self.set_parameter("ARSPD2_USE", 0) + self.delay_sim_time(10) + if airspeed_active != [True, False]: + raise NotAchievedException("Not using expected airspeed sensors %s" % str(airspeed_active)) + self.fly_home_land_and_disarm() + self.change_mode("MANUAL") + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_ALL) + + self.start_subtest("Now testing primary arming check") + + # Should not be able to arm with primary sensor not set to use + self.assert_prearm_failure("Airspeed: not using Primary (2)") + + self.start_subtest("Testing primary changes when disarmed") + + # Now the vehicle is disarmed, enabling use for the primary sensor should result in it being used + self.set_parameter("ARSPD2_USE", 1) + self.delay_sim_time(10) + if airspeed_active != [False, True]: + raise NotAchievedException("Not using expected airspeed sensors %s" % str(airspeed_active)) def RudderArmingWithArmingChecksSkipped(self): '''check we can't arm with rudder even if all checks are skipped''' diff --git a/Tools/autotest/default_params/airsim-quadX.parm b/Tools/autotest/default_params/airsim-quadX.parm index 59765cdf524a5..a1c0b11e969d7 100644 --- a/Tools/autotest/default_params/airsim-quadX.parm +++ b/Tools/autotest/default_params/airsim-quadX.parm @@ -16,7 +16,7 @@ WPNAV_ACCEL_Z 200 WPNAV_SPEED_UP 400 WPNAV_SPEED_DN 300 WPNAV_SPEED 2000 -RTL_ALT 2500 +RTL_ALT_M 25 ANGLE_MAX 3000 SIM_GPS1_LAG_MS 0 GPS1_DELAY_MS 20 diff --git a/Tools/autotest/default_params/copter-heli.parm b/Tools/autotest/default_params/copter-heli.parm index 994bf370801ec..57c410b1fdc75 100644 --- a/Tools/autotest/default_params/copter-heli.parm +++ b/Tools/autotest/default_params/copter-heli.parm @@ -100,6 +100,6 @@ RC8_MIN 1000.000000 RC8_TRIM 1500.000000 RPM1_TYPE 10 IM_ACRO_COL_EXP 0.2 -PSC_VELXY_D 0.5 +PSC_NE_VEL_D 0.5 SERVO8_MIN 1000.000000 SERVO8_MAX 2000.000000 diff --git a/Tools/autotest/default_params/sub.parm b/Tools/autotest/default_params/sub.parm index f243a9e0d6594..e5a340c3a4eb8 100644 --- a/Tools/autotest/default_params/sub.parm +++ b/Tools/autotest/default_params/sub.parm @@ -66,15 +66,15 @@ MNT1_PITCH_MIN -45 MNT1_DEFLT_MODE 3 MNT1_RC_RATE 30 MNT1_TYPE 7 -PSC_ACCZ_P 2 -PSC_ACCZ_I 4 -PSC_ACCZ_FF 0.75 -PSC_VELZ_P 8 -PSC_POSZ_P 3 -PSC_POSXY_P 2.5 -PSC_VELXY_D 0.8 -PSC_VELXY_I 0.5 -PSC_VELXY_P 5.0 +PSC_D_ACC_P 0.2 +PSC_D_ACC_I 0.4 +PSC_D_ACC_FF 0.075 +PSC_D_VEL_P 8 +PSC_D_POS_P 3 +PSC_NE_POS_P 2.5 +PSC_NE_VEL_D 0.8 +PSC_NE_VEL_I 0.5 +PSC_NE_VEL_P 5.0 RNGFND1_MAX 30.00 RNGFND1_PIN 0 RNGFND1_SCALING 12.12 @@ -85,7 +85,7 @@ SERVO8_MIN 1100 BARO_EXT_BUS 1 PILOT_ACCEL_Z 200 PILOT_SPEED_UP 200 -PSC_JERK_Z 8 +PSC_JERK_D 8 LOG_BITMASK 180222 SIM_BARO_RND 0.02 RC_OPTIONS 0 diff --git a/Tools/autotest/logger_metadata/parse.py b/Tools/autotest/logger_metadata/parse.py index a6b4d9077e512..2b5697eeac577 100755 --- a/Tools/autotest/logger_metadata/parse.py +++ b/Tools/autotest/logger_metadata/parse.py @@ -153,6 +153,11 @@ def set_field_description(self, field, description): self.ensure_field(field) self.fields[field]["description"] = description + def get_field_description(self, field): + if field not in self.fields: + return None + return self.fields[field].get('description', None) + def set_field_bits(self, field, bits): bits = bits.split(",") count = 0 @@ -513,6 +518,14 @@ def emit_output(self): elif units is not None or mults is not None: print(f"Cannot find matching units/mults for message {docco.name}") + # every field must have a description. Things like + # FieldBitmaskEnum can create the field object but not fill + # description in. + for docco in new_doccos: + for field in docco.fields: + if docco.get_field_description(field) is None: + raise ValueError(f"{docco.name}.{field} missing description") + enums_by_name = {} for enum in self.enumerations: enums_by_name[enum.name] = enum diff --git a/Tools/autotest/models/Callisto.param b/Tools/autotest/models/Callisto.param index b3789db7da7b0..c06db1e5352a3 100644 --- a/Tools/autotest/models/Callisto.param +++ b/Tools/autotest/models/Callisto.param @@ -48,13 +48,13 @@ MOT_YAW_HEADROOM,50 PILOT_THR_BHV,3 PILOT_Y_RATE,60 PILOT_Y_RATE_TC,0.25 -PSC_ACCZ_FLTD,10 -PSC_ACCZ_FLTE,0 -PSC_ACCZ_FLTT,10 +PSC_D_ACC_FLTD,10 +PSC_D_ACC_FLTE,0 +PSC_D_ACC_FLTT,10 PSC_ANGLE_MAX,45 -PSC_JERK_Z,10 -PSC_POSZ_P,0.5 -PSC_VELZ_P,2.5 +PSC_JERK_D,10 +PSC_D_POS_P,0.5 +PSC_D_VEL_P,2.5 RNGFND1_MAX,100.00 RNGFND1_PIN,0 RNGFND1_SCALING,12.1212 diff --git a/Tools/autotest/models/freestyle.param b/Tools/autotest/models/freestyle.param index 6ab0ddad76446..5553505d8d8de 100644 --- a/Tools/autotest/models/freestyle.param +++ b/Tools/autotest/models/freestyle.param @@ -56,7 +56,7 @@ INS_HNTC2_HMNCS 15.000000 INS_HNTC2_MODE 3.000000 INS_HNTC2_OPTS 22.000000 INS_HNTC2_REF 1.000000 -LAND_ALT_LOW 700.000000 +LAND_ALT_LOW_M 7.000000 LOG_BITMASK 196607.000000 LOG_DARM_RATEMAX 1.000000 LOIT_SPEED 4000.000000 @@ -74,15 +74,15 @@ MOT_THST_HOVER 0.125000 PILOT_SPEED_DN 500.000000 PILOT_SPEED_UP 500.000000 PILOT_Y_RATE 250.000000 -PSC_ACCZ_FLTD 40.000000 -PSC_ACCZ_FLTT 40.000000 -PSC_ACCZ_P 0.200000 -PSC_JERK_XY 10.000000 -PSC_VELZ_P 3.000000 +PSC_D_ACC_FLTD 40.000000 +PSC_D_ACC_FLTT 40.000000 +PSC_D_ACC_P 0.020000 +PSC_JERK_NE 10.000000 +PSC_D_VEL_P 3.000000 RC7_OPTION 0.000000 -RTL_ALT 700.000000 +RTL_ALT_M 7.000000 RTL_LOIT_TIME 2000.000000 -RTL_SPEED 900.000000 +RTL_SPEED_MS 9.000000 WPNAV_ACCEL 1100.000000 WPNAV_ACCEL_Z 300.000000 WPNAV_JERK 6.000000 diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 08fb66ed972cf..261ebad19e928 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -1269,6 +1269,51 @@ def ICEngineMission(self): self.change_mode('AUTO') self.wait_disarmed(timeout=300) + def ICEngineRPMGovernor(self): + '''Test ICE idle and redline governor''' + self.setup_ICEngine_vehicle() + + # allow running while disarmed + options = int(self.get_parameter("ICE_OPTIONS")) + options |= 1 << 2 + self.set_parameter("ICE_OPTIONS", options) + + self.start_subtest("ICEngine idle governor") + # idle governor should work even in non-manual mode + self.change_mode('QHOVER') + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=1) + + deadband = 50 + self.set_parameter("ICE_IDLE_DB", deadband) + # Test two RPM settings to make sure we don't pass as a fluke + for idle_rpm in (1000, 1500): + self.set_parameter("ICE_IDLE_RPM", idle_rpm) + self.wait_rpm( + 1, + idle_rpm - 1.1 * deadband, + idle_rpm + 1.1 * deadband, + timeout=60, + minimum_duration=15, + ) + + self.start_subtest("ICEngine redline governor") + self.change_mode('MANUAL') + # The redline governor only works properly while armed + self.wait_ready_to_arm() + self.arm_vehicle() + self.set_rc(3, 2000) + for redline_rpm in (6500, 6000): + self.set_parameter("ICE_REDLINE_RPM", redline_rpm) + self.wait_rpm( + 1, + redline_rpm - 2 * deadband, + redline_rpm, + timeout=60, + minimum_duration=15, + ) + self.set_rc(3, 1000) + self.disarm_vehicle() + def MAV_CMD_DO_ENGINE_CONTROL(self): '''test MAV_CMD_DO_ENGINE_CONTROL mavlink command''' @@ -3044,6 +3089,7 @@ def tests(self): self.CopterTailsitter, self.ICEngine, self.ICEngineMission, + self.ICEngineRPMGovernor, self.MAV_CMD_DO_ENGINE_CONTROL, self.MidAirDisarmDisallowed, self.GUIDEDToAUTO, diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 70b884d240c33..efb17142e67a1 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -6010,6 +6010,121 @@ def RangeFinder(self): if abs(m.distance - want_range) > 0.5: raise NotAchievedException("Expected %fm got %fm" % (want_range, m.distance)) + def AIS(self): + '''Test AIS receiver''' + self.customise_SITL_commandline([ + "--serial5=sim:AIS", + ]) + self.set_parameters({ + "SERIAL5_PROTOCOL": 40, + "SERIAL5_BAUD": 38, + "AIS_TYPE": 1, + }) + self.reboot_sitl() + + # Set SIM parameter after reboot when AIS sim is loaded + self.set_parameter("SIM_AIS_COUNT", 5) + + self.delay_sim_time(10) + + m = self.assert_receive_message('AIS_VESSEL', timeout=60) + + if m.MMSI == 0: + raise NotAchievedException("Invalid MMSI") + + self.progress("Received AIS vessel MMSI=%u" % m.MMSI) + + def AISMultipleVessels(self): + '''Test tracking multiple AIS vessels''' + self.customise_SITL_commandline([ + "--serial5=sim:AIS", + ]) + self.set_parameters({ + "SERIAL5_PROTOCOL": 40, + "SERIAL5_BAUD": 38, + "AIS_TYPE": 1, + }) + self.reboot_sitl() + + # Test with 1 vessel + self.set_parameter("SIM_AIS_COUNT", 1) + m = self.assert_receive_message('AIS_VESSEL', timeout=30) + if m.MMSI == 0: + raise NotAchievedException("No vessel with count=1") + self.progress("Received vessel with count=1") + + # Test with 3 vessels + self.set_parameter("SIM_AIS_COUNT", 3) + vessels_seen = set() + start_time = self.get_sim_time() + while len(vessels_seen) < 3: + if self.get_sim_time() - start_time > 60: + raise NotAchievedException( + "Only saw %d vessels, expected 3" % len(vessels_seen) + ) + m = self.assert_receive_message('AIS_VESSEL', timeout=10) + vessels_seen.add(m.MMSI) + self.progress("Tracked 3 unique vessels") + + # Test with 5 vessels + self.set_parameter("SIM_AIS_COUNT", 5) + vessels_seen = set() + start_time = self.get_sim_time() + while len(vessels_seen) < 5: + if self.get_sim_time() - start_time > 90: + raise NotAchievedException( + "Only saw %d vessels, expected 5" % len(vessels_seen) + ) + m = self.assert_receive_message('AIS_VESSEL', timeout=10) + vessels_seen.add(m.MMSI) + + self.progress("Successfully tracked vessels with counts 1, 3, and 5") + + def AISDataValidation(self): + '''Test AIS vessel data validity''' + self.customise_SITL_commandline([ + "--serial5=sim:AIS", + ]) + self.set_parameters({ + "SERIAL5_PROTOCOL": 40, + "SERIAL5_BAUD": 38, + "AIS_TYPE": 1, + }) + self.reboot_sitl() + + self.set_parameter("SIM_AIS_COUNT", 3) + + m = self.assert_receive_message('AIS_VESSEL', timeout=60) + + # Validate MMSI + if m.MMSI == 0: + raise NotAchievedException("Invalid MMSI: must be non-zero") + + # Validate latitude (-90 to +90 degrees) + lat_deg = m.lat / 1e7 + if abs(lat_deg) > 90: + raise NotAchievedException("Invalid latitude: %f" % lat_deg) + + # Validate longitude (-180 to +180 degrees) + lon_deg = m.lon / 1e7 + if abs(lon_deg) > 180: + raise NotAchievedException("Invalid longitude: %f" % lon_deg) + + # Validate heading is reasonable (allow up to 65535 for unavailable) + if m.heading > 65535: + raise NotAchievedException("Invalid heading: %u" % m.heading) + + # Validate COG is reasonable (allow up to 65535 for unavailable) + if m.COG > 65535: + raise NotAchievedException("Invalid COG: %u" % m.COG) + + # Validate velocity (max ~100 knots = 10000 in 0.01 knot units) + if m.velocity > 10000 and m.velocity != 65535: + raise NotAchievedException("Invalid velocity: %u" % m.velocity) + + self.progress("AIS data validation passed: MMSI=%u lat=%.6f lon=%.6f" % + (m.MMSI, lat_deg, lon_deg)) + def DepthFinder(self): '''Test multiple depthfinders for boats''' # Setup rangefinders @@ -7214,6 +7329,9 @@ def tests(self): self.SetpointGlobalVel, self.AccelCal, self.RangeFinder, + self.AIS, + self.AISMultipleVessels, + self.AISDataValidation, self.AP_Proximity_MAV, self.EndMissionBehavior, self.FlashStorage, diff --git a/Tools/autotest/sim_vehicle.py b/Tools/autotest/sim_vehicle.py index c35a8313770e0..d45bc48fcbd9e 100755 --- a/Tools/autotest/sim_vehicle.py +++ b/Tools/autotest/sim_vehicle.py @@ -514,12 +514,29 @@ def find_geocoder_location(locname): except ImportError: print("geocoder not installed") return None - j = geocoder.osm(locname) + # Step 1: Attempt the lookup + try: + j = geocoder.osm(locname) + except Exception as e: + # Handle network/blocked errors (like 403) + err_msg = str(e) + if '403' in err_msg: + print(f"geocoder access denied (HTTP 403) for '{locname}'") + else: + print(f"geocoder error: {err_msg}") + return None + # Step 2: Validate the response object (Handles status_code 403 if no exception was raised) + status_code = getattr(j, 'status_code', None) + if status_code == 403: + print(f"geocoder access denied (HTTP 403) for '{locname}'") + return None + if j is None or not hasattr(j, 'lat') or j.lat is None: - print("geocoder failed to find '%s'" % locname) + print(f"geocoder failed to find '{locname}'") return None - lat = j.lat - lon = j.lng + # Step 3: Success logic + lat, lon = j.lat, j.lng + from MAVProxy.modules.mavproxy_map import srtm downloader = srtm.SRTMDownloader() downloader.loadFileList() @@ -531,7 +548,7 @@ def find_geocoder_location(locname): alt = tile.getAltitudeFromLatLon(lat, lon) break if alt is None: - print("timed out getting altitude for '%s'" % locname) + print(f"timed out getting altitude for '{locname}'") return None return [lat, lon, alt, 0.0] diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 7564dcdf0840e..619bce298ba5e 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -5613,7 +5613,7 @@ def set_rc_from_map(self, _map, timeout=20, quiet=False): tstart = self.get_sim_time() while True: - if tstart - self.get_sim_time_cached() > timeout: + if self.get_sim_time_cached() - tstart > timeout: raise NotAchievedException("Failed to set RC values") m = self.mav.recv_match(type='RC_CHANNELS', blocking=True, timeout=1) if m is None: @@ -5967,8 +5967,8 @@ def wait_attitude(self, desroll=None, despitch=None, timeout=2, tolerance=10, me m = self.assert_receive_message(message_type, timeout=60) roll_deg = math.degrees(m.roll) pitch_deg = math.degrees(m.pitch) - self.progress("wait_att: roll=%f desroll=%s pitch=%f despitch=%s" % - (roll_deg, desroll, pitch_deg, despitch)) + self.progress("wait_att[%s]: roll=%f desroll=%s pitch=%f despitch=%s" % + (message_type, roll_deg, desroll, pitch_deg, despitch)) if desroll is not None and abs(roll_deg - desroll) > tolerance: continue if despitch is not None and abs(pitch_deg - despitch) > tolerance: @@ -5987,16 +5987,16 @@ def wait_attitude_quaternion(self, tstart = self.get_sim_time() while True: if self.get_sim_time_cached() - tstart > timeout: - raise AutoTestTimeoutException("Failed to achieve attitude") - m = self.poll_message(message_type) + raise AutoTestTimeoutException("Failed to achieve (quaternion) attitude") + m = self.poll_message(message_type, quiet=True) q = quaternion.Quaternion([m.q1, m.q2, m.q3, m.q4]) euler = q.euler roll = euler[0] pitch = euler[1] roll_deg = math.degrees(roll) pitch_deg = math.degrees(pitch) - self.progress("wait_att_quat: roll=%f desroll=%s pitch=%f despitch=%s" % - (roll_deg, desroll, pitch_deg, despitch)) + self.progress("wait_att_quat[%s]: roll=%f desroll=%s pitch=%f despitch=%s" % + (message_type, roll_deg, desroll, pitch_deg, despitch)) if desroll is not None and abs(roll_deg - desroll) > tolerance: continue if despitch is not None and abs(pitch_deg - despitch) > tolerance: @@ -12753,7 +12753,7 @@ def disabled_tests(self): def test_parameter_checks_poscontrol(self, param_prefix): self.wait_ready_to_arm() self.context_push() - self.set_parameter("%s_POSXY_P" % param_prefix, -1) + self.set_parameter("%s_NE_POS_P" % param_prefix, -1) self.run_cmd( mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, p1=1, # ARM @@ -13090,12 +13090,100 @@ def ahrstrim_attitude_correctness(self): self.customise_SITL_commandline([ "--home", "%s,%s,%s,%s" % (HOME.lat, HOME.lng, HOME.alt, heading) ]) - for ahrs_type in [0, 2, 3]: + for ahrs_type in [0, 2, 3, 11]: self.start_subsubtest("Testing AHRS_TYPE=%u" % ahrs_type) self.context_push() - self.set_parameter("AHRS_EKF_TYPE", ahrs_type) - self.reboot_sitl() - self.wait_prearm_sys_status_healthy() + + # Special setup for ExternalAHRS (type 11) + if ahrs_type == 11: + # Test all simulated ExternalAHRS backends + external_ahrs_configs = [ + { + "name": "VectorNav", + "device": "VectorNav", + "eahrs_type": 1, + }, + { + "name": "MicroStrain5", + "device": "MicroStrain5", + "eahrs_type": 2, + }, + { + "name": "InertialLabs", + "device": "ILabs", + "eahrs_type": 5, + }, + { + "name": "MicroStrain7", + "device": "MicroStrain7", + "eahrs_type": 7, + }, + ] + + for config in external_ahrs_configs: + self.start_subsubtest("Testing ExternalAHRS backend: %s" % config["name"]) + self.context_push() + + self.customise_SITL_commandline([ + "--serial4=sim:%s" % config["device"], + ]) + self.set_parameters({ + "EAHRS_TYPE": config["eahrs_type"], + "SERIAL4_PROTOCOL": 36, # ExternalAHRS protocol + "SERIAL4_BAUD": 230400, + "GPS1_TYPE": 21, # External AHRS + "AHRS_EKF_TYPE": ahrs_type, + "INS_GYR_CAL": 1, + "EAHRS_SENSORS": 0xD, # GPS|BARO|COMPASS (exclude IMU) + }) + self.reboot_sitl() + self.delay_sim_time(5) + self.progress("Running accelcal") + self.run_cmd( + mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, + p5=4, + timeout=5, + ) + self.wait_prearm_sys_status_healthy(timeout=120) + + for (r, p) in [(0, 0), (9, 0), (2, -6), (10, 10)]: + self.set_parameters({ + 'AHRS_TRIM_X': math.radians(r), + 'AHRS_TRIM_Y': math.radians(p), + "SIM_ACC_TRIM_X": math.radians(r), + "SIM_ACC_TRIM_Y": math.radians(p), + }) + self.reboot_sitl() + self.context_set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_SIM_STATE, 10) + att_desroll = 0 + att_despitch = 0 + if ahrs_type == 11: + # this is very nasty compatibility + # code for the fact our rotations are + # incorrect for ExternalAHRS eulers + # and rotation matrix! It is here to + # ensure behaviour is preserved until + # we can fix the bug! Search for + # "note that this is suspect" to find + # the problem code. + att_desroll = -r + att_despitch = -p + self.wait_attitude(desroll=att_desroll, despitch=att_despitch, timeout=120, tolerance=1.5) + if ahrs_type != 0: + self.wait_attitude(desroll=0, despitch=0, message_type='AHRS2', tolerance=1, timeout=120) + self.wait_attitude_quaternion(desroll=0, despitch=0, tolerance=1, timeout=120) + self.wait_attitude(desroll=0, despitch=0, message_type='SIM_STATE', tolerance=1, timeout=120) + + self.context_pop() + self.reboot_sitl() + + # Skip the normal test loop for ahrs_type 11 since we already tested it above + self.context_pop() + continue + else: + self.set_parameter("AHRS_EKF_TYPE", ahrs_type) + self.reboot_sitl() + self.wait_prearm_sys_status_healthy(timeout=120) for (r, p) in [(0, 0), (9, 0), (2, -6), (10, 10)]: self.set_parameters({ 'AHRS_TRIM_X': math.radians(r), @@ -13103,10 +13191,14 @@ def ahrstrim_attitude_correctness(self): "SIM_ACC_TRIM_X": math.radians(r), "SIM_ACC_TRIM_Y": math.radians(p), }) + self.reboot_sitl() + self.context_set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_SIM_STATE, 10) self.wait_attitude(desroll=0, despitch=0, timeout=120, tolerance=1.5) + self.wait_attitude(desroll=0, despitch=0, timeout=120, tolerance=1.5, message_type='SIM_STATE') if ahrs_type != 0: # we don't get secondary msgs while DCM is primary self.wait_attitude(desroll=0, despitch=0, message_type='AHRS2', tolerance=1, timeout=120) self.wait_attitude_quaternion(desroll=0, despitch=0, tolerance=1, timeout=120) + self.wait_attitude_quaternion(desroll=0, despitch=0, tolerance=1, timeout=120, message_type='SIM_STATE') self.context_pop() self.reboot_sitl() diff --git a/Tools/bootloaders/3DR-L431-ASAUAV_bl.bin b/Tools/bootloaders/3DR-L431-ASAUAV_bl.bin new file mode 100755 index 0000000000000..1fd71033b6cd4 Binary files /dev/null and b/Tools/bootloaders/3DR-L431-ASAUAV_bl.bin differ diff --git a/Tools/bootloaders/3DR-L431-ASAUAV_bl.hex b/Tools/bootloaders/3DR-L431-ASAUAV_bl.hex new file mode 100644 index 0000000000000..2f0cf040062fd --- /dev/null +++ b/Tools/bootloaders/3DR-L431-ASAUAV_bl.hex @@ -0,0 +1,1395 @@ +:020000040800F2 +:1000000000090020B10100083D310008F53000086A +:100010001D310008F530000815310008B301000853 +:10002000B3010008B3010008B3010008B54100089E +:10003000B3010008B3010008B3010008B3010008D0 +:10004000B3010008B3010008B3010008B3010008C0 +:10005000B3010008B3010008B3010008B3010008B0 +:10006000B3010008B3010008B3010008B3010008A0 +:10007000B3010008B3010008B3010008B301000890 +:10008000B3010008B3010008B3010008A93000085B +:10009000D5300008E5300008B3010008B3010008BE +:1000A000B3010008B3010008B3010008B301000860 +:1000B00039550008B3010008B3010008B301000876 +:1000C000B3010008B3010008B3010008B301000840 +:1000D000B301000825550008B3010008B30100086A +:1000E000B3010008B3010008B3010008B301000820 +:1000F000B3010008B3010008B3010008B301000810 +:10010000B3010008B3010008B3010008B3010008FF +:10011000B3010008B3010008B3010008B3010008EF +:10012000B3010008B3010008B3010008B3010008DF +:10013000B3010008B3010008B3010008B3010008CF +:10014000B3010008B3010008B3010008B3010008BF +:10015000B3010008B3010008B3010008B3010008AF +:10016000B3010008B3010008B3010008B30100089F +:10017000B3010008B3010008B3010008B30100088F +:10018000B3010008B3010008B3010008B30100087F +:10019000B3010008B3010008B3010008B30100086F +:1001A000B91B0008FFFFFFFFFFFFFFFFFFFFFFFF7F +:1001B00002E000F000F8FEE772B6374880F30888E6 +:1001C000364880F3098836483649086040F2000016 +:1001D000CCF200004EF63471CEF200010860BFF39D +:1001E0004F8FBFF36F8F40F20000C0F2F0004EF669 +:1001F0008851CEF200010860BFF34F8FBFF36F8FBD +:100200004FF00000E1EE100A4EF63C71CEF2000114 +:100210000860062080F31488BFF36F8F04F096FF08 +:1002200005F018F84FF055301F491B4A91423CBF6A +:1002300041F8040BFAE71D49184A91423CBF41F8C6 +:10024000040BFAE71A491B4A1B4B9A423EBF51F86E +:10025000040B42F8040BF8E700201849184A9142B1 +:100260003CBF41F8040BFAE704F074FF05F04AF8CC +:10027000144C154DAC4203DA54F8041B8847F9E7D7 +:1002800000F0E8FB114C124DAC4203DA54F8041BA9 +:100290008847F9E704F05CBF000900200011002046 +:1002A0000000000808ED00E0000100200009002027 +:1002B00098560008001100207811002078110020C5 +:1002C000583E0020A0010008A4010008A401000875 +:1002D000A40100082DE9F04F2DED108AC1F80CD0D3 +:1002E000C3689D46BDEC108ABDE8F08F002383F300 +:1002F00011882846A047002004F0B4FBFEE704F074 +:1003000037FB00DFFEE7000080EA0102844612F0BE +:10031000030F4FD111F0030F32D14DF8044D11F0FE +:10032000040F51F8043B0BD0A3F101329A4312F0B1 +:10033000803F04BF4CF8043B51F8043B16D100BF8A +:1003400051F8044BA3F101329A4312F0803FA4F11B +:1003500001320BD14CF8043BA24312F0803F04BFA2 +:1003600051F8043B4CF8044BEAD023460CF8013B0F +:1003700013F0FF0F4FEA3323F8D15DF8044B7047B9 +:1003800011F0010F06D011F8012B0CF8012B002AF7 +:1003900008BF704711F0020FBFD031F8022B12F0E6 +:1003A000FF0F16BF2CF8022B8CF8002012F47F4FA1 +:1003B000B3D1704711F8012B0CF8012B002AF9D1A9 +:1003C000704700BF10B4044C03460A4620685DF82D +:1003D000044B194604F0CCBD0011002010B5054CAB +:1003E00034B113460A4601460220AFF3008010BD27 +:1003F000204610BD00000000024B0146186804F0C2 +:10040000BBBD00BF00110020024B0146186804F07C +:10041000B7BD00BF0011002001292DE9F0415BD9D3 +:100420004FF00108C64600244FF0FF35434608E070 +:10043000A3EB05081C464FF0010E04EB0E03994296 +:1004400010D900EB050CC75C1CF80E60B742EFD367 +:1004500032D04FF001082546C646444404EB0E0353 +:100460009942EED80127C2F80080BE4600244FF022 +:10047000FF363B4607E09F1B1C464FF0010E04EB86 +:100480000E03994210D900EB060C10F803801CF8FB +:100490000EC0E045EFD817D001272646BE463C44A3 +:1004A00004EB0E039942EED80136681C864201D354 +:1004B00017603046BDE8F081C6450ABF74440EF1AE +:1004C000010E4FF0010EB8E7BE450ABF74440EF1AD +:1004D000010E4FF0010ED2E701270026E8E700BF2A +:1004E0002DE9F04F164685B003AA0D46074619467A +:1004F00030461C46FFF790FFDDF80CA00246804610 +:1005000006EB0A01304600F027FA2A1B002852D1D8 +:1005100001468446A4EB0A0BC8F10109CDF804A0FA +:100520008845434638BF0B469C4219D95D1E03EBF4 +:100530000C003544384402E001339C4210D010F8DE +:1005400001EB15F801AFF245F6D09C4208D9CC4436 +:100550009C4400219445E3D9002005B0BDE8F08F0C +:10056000884508F1FF3367D908EB0C00384406EBE7 +:100570000805CDF8008002E08B4212D0534615F8F2 +:10058000018D10F801EDF04503F1FF3AF4D0DDF8EC +:10059000008001330131994208D8019B59469C449F +:1005A000D8E701319942DDF80080F6D907EB0C005D +:1005B00005B0BDE8F08FA4EB0801414538BF4146C6 +:1005C000444501F101014FF0000C31D908F1FF3E23 +:1005D000C8F101090CEB080006EB0E053844434650 +:1005E00002E001339C4223D010F801AB15F801BFA3 +:1005F000D345F6D09C421BD9CC449C449445ABD8FF +:100600004445E7D806EB08090CEB08053D4448468D +:1006100001E0B342CAD010F801AD15F801EDF24582 +:100620000346F6D08C44944596D84445CED8EBE7A3 +:10063000B8F1000FE6D1B9E74346ABE72DE9F04F3B +:100640001546ADF21C4D06468A4605AA19462846AF +:100650000093FFF7E1FE009B80460DF2144142F843 +:10066000043F8A42FBD15C1E002B00F0CE80691E45 +:1006700004EB050C06AF11F8010F5A1A013A2A448F +:10068000614547F82020F6D1DDF814B0009305EB62 +:100690000B014246284600F05FF9009B0146AAEB99 +:1006A000030A00285CD1A3EB0B03C8F101020093FD +:1006B000CDE901B20346A9460AE021B10199914270 +:1006C000009988BF0A46134453454FF0000142D8B1 +:1006D00006EB030C1CF8042057F82220002AECD16A +:1006E0008845424638BF0A46A24212D2551ED0184B +:1006F0004D44304402E00132A2420AD010F801EB2E +:1007000015F801BFF345F6D0A24202D202990B447C +:10071000D9E7884508F1FF327BD908EB0300304464 +:1007200009EB0805CDF80C8002E08A426AD05A46EF +:1007300015F8018D10F801EDF04502F1FF3BF4D002 +:10074000DDF80C8001320131914260D8019A0099A4 +:1007500013445345BCD900200DF21C4DBDE8F08F69 +:10076000A3EB080343454FF0000C38BF434606EBAC +:100770000C0001330193035D57F8233008F1FF3972 +:10078000C8F1010B4BB19C44D445E4D806EB0C00F6 +:10079000035D57F82330002BF5D1A04517D208EBA5 +:1007A0000C02324405EB09014346CDF800C002E0DB +:1007B0000133A34223D012F801CB11F801EFE64533 +:1007C000F6D0DDF800C0A34238BFDC44DBD3B8F17B +:1007D000000FC1D008EB0C01314405EB0802CDF845 +:1007E00000C001E0AB42B7D012F801ED11F801CD25 +:1007F000E6451346F6D0DDE900C39C44C4E7DDF8C6 +:1008000000C0E4E7DDF80C809DE706AF3CE76046FA +:10081000A2E7424697E700BF2DE9F0470446C0B083 +:10082000002B6AD0012B0D4690461F4600F08680B3 +:100830008B425ED8CD1A022B00EB050661D0B3F5D2 +:10084000807F00F282804FF48072002168465D1E36 +:1008500001F0D4FA98F80010012240460B4610F837 +:10086000011FA1EBC303DBB20DF803200132954257 +:10087000F4D808EB07034FF0000A13F8022C18F81D +:100880000530A3EBC203DBB21DF803900DF8035053 +:10089000A5EB0909B44201D92BE063B92C44A64267 +:1008A00014F8013C94F800C0ACEBC30C5FFA8CFC6C +:1008B0001DF80C30F1D29D42A4EB0304EAD80F2DB1 +:1008C00007D9042208EB0A0104EB0A0000F044F8FF +:1008D00060B92A464146204600F03EF868B1BAF1B8 +:1008E000030F8CBFAAF1040AA7F1050A4C44B442D5 +:1008F000D4D9002040B0BDE8F087204640B0BDE824 +:10090000F08713785078217840EA0340234613F8A3 +:10091000012F9E4242EA01421BD39042EDD00136A4 +:1009200001E0904208D013F8011F9E4241EA0242C2 +:10093000F7D18242DDD13346581EDBE711782A46D3 +:1009400040B0BDE8F04700F035B840B0BDE8F04732 +:1009500074E61E46EDE700BF032A30B415D941EA1C +:100960000004A40784460B4604D0541E0FE0043A4A +:10097000032A08D91946604653F8044B5CF8045B17 +:10098000A542F4D0F1E760461946541E7AB1431EE1 +:100990000139021901E09A4209D013F8010F11F848 +:1009A00001CF6045F7D0A0EB0C0030BC70470020B1 +:1009B00030BC704701F0FF01102A2BDB10F0070F4D +:1009C00008D010F8013B013A8B422DD010F0070FF0 +:1009D00042B3F6D1F0B441EA012141EA014122F0EB +:1009E00007047FF000070023F0E80256083C85EA80 +:1009F000010586EA010685FA47F5A3FA87F586FA26 +:100A000047F6A5FA87F68EB9EED1F0BC01F0FF01EA +:100A100002F0070232B110F8013B013A83EA010308 +:100A200013B1F8D10020704701387047002D06BF80 +:100A300035460338073815F0010F07D1013015F49A +:100A4000807F02BF013015F4C03F0130F0BC013897 +:100A5000704700BFF8B501F01BF904F015FB07461D +:100A600004F066FB0546A8BB1F4B9F4232D0013302 +:100A70009F4232D01D4B27F0FF029A4230D1F8B28C +:100A800000F0F2FE2E4642F2107400F0F3FE08B1C0 +:100A90000024264601F05EFC20B1032000F078F827 +:100AA0000024264635B1124B9F4203D004F038FB98 +:100AB00000242646002004F0F1FA0EB100F07EF882 +:100AC00001F0F2F900F00AFF204600F0D3F800F040 +:100AD00075F8F9E72E460024D7E704460126D4E747 +:100AE00006464FF47A74D0E7010007B0000008B062 +:100AF000263A09B008B501F099F8A0F12003584250 +:100B0000584108BD07B5042101900DEB010001F02B +:100B1000A9F803B05DF804FB38B5302383F31188DE +:100B2000174803680BB104F05BF8164A1448002319 +:100B30004FF47A7104F04AF8002383F31188124CC1 +:100B4000236813B12368013B2360636813B16368B2 +:100B5000013B63600D4D2B7833B963687BB902208C +:100B600001F036F9322363602B78032B07D16368D9 +:100B70002BB9022001F02CF94FF47A73636038BD71 +:100B800088120020190B00087C120020841200201B +:100B9000084B187003280CD8DFE800F0080502089D +:100BA000022001F00BB9022001F000B9024B002233 +:100BB0005A607047841200207C12002010B501F0AA +:100BC000C9FB30B1244B03221A70244B00225A6017 +:100BD00010BD234B234A1C4619680131F8D0043359 +:100BE0009342F9D16268204B9A42F1D91F4B9B681E +:100BF00003F1006303F580339A42E9D204F062FA0C +:100C000004F074FA002001F05DF80220FFF7C0FF45 +:100C1000174B9A6D00229A65996F9A67996FD96DF3 +:100C2000DA65D96FDA67D96F196E1A66D3F8801052 +:100C3000C3F88020D3F8803072B64FF0E023302123 +:100C4000C3F8084DD4E9003281F311889D4683F33F +:100C500008881047BCE700BF841200207C120020E7 +:100C60000000010820000108FFFF000850110020CB +:100C7000001002402DE9F04F93B0DFF8108301908F +:100C80002022FF210AA8D8F8086001F0B7F8B64A78 +:100C90001378A3B9B548012103601170302383F3A1 +:100CA000118803680BB103F09BFFB14AAF480023E2 +:100CB0004FF47A7103F08AFF002383F31188019BBC +:100CC00013B1AC4B019A1A60AB4A019D1378032B08 +:100CD0001EBF00231370A74A4FF0000A18BF5360CD +:100CE000D3465446D146012001F068F825B1A14B06 +:100CF0001B68002B00F0F581002000F097FF071E15 +:100D000038DB012001F052F8A7F121001628EAD8BB +:100D100001A353F820F000BF7B0D0008970D0008D9 +:100D2000210E0008E70C0008E70C0008E70C00089B +:100D30009F0E0008551000084B0F0008D30F000845 +:100D4000F90F00081F100008E70C00083110000818 +:100D5000E70C00089D100008010E0008E70C0008D1 +:100D6000E9100008870D0008010E0008E70C0008D4 +:100D7000D30F000800F0F8FDB8E70220FFF7BAFE35 +:100D8000002840F0E080019B7C48BAF1000F08BFCA +:100D90001D4602213EE04FF47A7000F047FF071E27 +:100DA000F1DB0220FFF7A6FE0028ECD0013F052F63 +:100DB00000F2C481DFE807F003070A0D10310520B7 +:100DC000FFF7A0FE14E0D8F80000F9E7D8F8040017 +:100DD000F6E7D8F80800F3E74FF01C09484600F0A2 +:100DE00073FF09F10409FFF78DFEB9F12C0FF5D15E +:100DF0000123BB404BEA03035FFA83FB4FF000097A +:100E000001F048F824B10BF00B030B2B08BF0025B1 +:100E10005B48022100F026FF65E7D8F80C00CFE719 +:100E2000002CB0D00BF00B030B2B5AD10220FFF794 +:100E300061FE0446002854D0012000F043FF022048 +:100E4000FFF7A6FE00271FFA87FA504600F04AFF78 +:100E5000054688B9484B012202201A7000F0A6FF0F +:100E60002E46D8F80830B34212D8434B00261E70E5 +:100E7000404BA2465E60C3E70021504600F04AFFA7 +:100E8000013705460028DED1A2463E480221C1E7CF +:100E9000304600F019FF0130F6D10436E1E7002CAE +:100EA0003FF471AF0BF00B030B2B1AD10220FFF7AD +:100EB0006FFE322000F0BAFE071E12DB17F00303AC +:100EC0000FD1D8F80810BA198A420AD8B7F5807F2E +:100ED00007D82D4A9F4206DCC820FFF70BFE0446C8 +:100EE00080B901244FE74FF47A70CDE9023200F067 +:100EF0009DFE0028FFF647AF039A029B02F8010B04 +:100F00000133E7E71F2E11D8C6F12002BA420AAB1F +:100F100026F0030028BF3A461B490292184400F00D +:100F200061FF029A1848FF2100F068FFBB101649C4 +:100F30000293C7F38702304600F068FF0028A4D070 +:100F4000029B06EB830604465AE70220FFF7D2FD18 +:100F500000283FF418AF00F0EFFE00283FF413AF75 +:100F600000273B4627E000BF7812002088120020AF +:100F7000190B00087C1200208412002090550008F4 +:100F8000945500088C55000878110020501100205D +:100F90001F2F18D80A9A013215D027F003024832C1 +:100FA0006A4452F8200C05900422184605A901F065 +:100FB000BBF904370346D8F80820BA42E8D81846E7 +:100FC000FFF7A0FD1CE73846029300F07DFE029B70 +:100FD000E9E70023642105A8059300F03BFE002803 +:100FE0007FF4D1AE0220FFF785FD00283FF4CBAEA1 +:100FF000059800F097FEE3E70023642105A8059318 +:1010000000F028FE00287FF4BEAE0220FFF772FD3C +:1010100000283FF4B8AE059800F086FED0E7022025 +:10102000FFF768FD00283FF4AEAE00F095FEC7E77D +:101030000220FFF75FFD00283FF4A5AE05A91420AC +:1010400000F090FE0746FFF75DFD394605A800F069 +:1010500009FED5E6322000F0E9FD071EFFF693AE4B +:10106000BB077FF490AED8F8082007EB0903934242 +:101070003FF689AE0220FFF73DFD00283FF483AE26 +:1010800027F003074F44B9453FF4BAAE484600F095 +:101090001BFEFFF737FD09F10409F4E74FF47A70FE +:1010A000FFF728FD00283FF46EAE00F045FE002853 +:1010B0003FF4EBAE0A9B01330CD008220AA90020B2 +:1010C00000F0A4FE00283FF4E0AE2022FF210AA891 +:1010D00000F094FE1B48022100F0C4FD1A4803F002 +:1010E000C7FC13B0BDE8F08F002C3FF44CAE0BF002 +:1010F0000B030B2B7FF447AE0023642105A8059357 +:1011000000F0A8FD074600287FF43DAE0220FFF75F +:10111000F1FC029000283FF436AE0221094800F0AD +:10112000A1FD41F2883003F0A3FC059800F0C8FE51 +:1011300000F0B0FE029B3D461C46D4E54FF000098E +:1011400021E600BF94550008A0860100094A1368F3 +:1011500049F2690099B21B0C00FB01331360064B86 +:10116000186844F2506182B2000C01FB0200186062 +:1011700080B27047641100206011002070B50C46E9 +:1011800015461E4602F01EFB50B9022E10D1012D4D +:101190000ED112A3D3E90023C4E90023012007E004 +:1011A000282D10D005D8012D09D0052D0FD00020F5 +:1011B00070BD302DFBD10BA3D3E90023ECE70BA3CB +:1011C000D3E90023E8E70BA3D3E90023E4E70BA36B +:1011D000D3E90023E0E700BFAFF30080401DA1206A +:1011E00026812A0B78F6339F93CACD8D9E6AC4213F +:1011F000818A46EE26417272DF25D7B7F017304A52 +:1012000039059E5610B500211022044600F0F6FD67 +:10121000034B03CB206061601868A06010BD00BF65 +:101220009075FF1FF0B5ADF5517D6EAC4FF4C472F3 +:1012300007460D462046002100F0E0FD01F086FF44 +:10124000264B4FF47A72B0FBF2F0186093E8070077 +:10125000022384E807000DF5ED702382FFF7D2FF2B +:1012600048F604531E49238407A8FFF74DF81D23B1 +:1012700084F832310DF2EB2207AB0DF1340C1E462F +:1012800003CE664510605160334602F10802F6D184 +:1012900030681060337913710FAE31460122204659 +:1012A00000F028FE80B2CDE9046000230393AB7EFA +:1012B000029305F11903019301230093E97E05A32D +:1012C000D3E90023384602F01FFB0DF5517DF0BD38 +:1012D000AFF300809E6AC421818A46EEE0170020A9 +:1012E000985500082DE9F0412C4C237ADAB080465D +:1012F0000D465BBB27A9284600F022FF07460028C1 +:1013000042D19DF89D60C82E3ED801464FF4A6629A +:10131000204600F073FD4FF48073C4F8F8314FF4A9 +:101320000073C4F80C3332464FF440730DF19E0144 +:1013300004F10900C4F82034264400F053FD9DF860 +:101340009C30777223720BB9EB7E237281220021CD +:1013500006AC27A800F052FD0122214627A800F084 +:1013600035FF80B2CDE9044000230393AB7E0293A6 +:1013700005F11903019328230093E97E05A3D3E91E +:101380000023404602F0C0FA5AB0BDE8F08100BF29 +:10139000AFF3008026417272DF25D7B7A812002074 +:1013A000F0B5254E4FF48A73F1B003FB00650024BD +:1013B00096F8D83085F8DC30D02221463CA885F854 +:1013C000E84000F01BFD06F1090000F015FDD5F81E +:1013D000E4308DF8F000C2B206AF06F109010DF15C +:1013E000F100CDE93A3400F0FDFC394601223AA87B +:1013F00000F020FF80B2CDE9047008230127CDE979 +:10140000023706F1D803019330230093317A0B4859 +:1014100007A3D3E9002302F077FAA04206DD01F02A +:1014200095FEC5F8E000384671B0F0BD2046FBE7F8 +:1014300078F6339F93CACD8DA8120020DC380020A7 +:101440002DE9F0411D4D1E4E1E4F86B0284602F07C +:1014500087FA034658B30024CDE90344ADF814409D +:101460001A7B8DF8142058689968029403AA03C265 +:101470001B68DFF8548043F00043029301F068FEDC +:10148000821941F10003009402A9384601F03EF9A7 +:10149000A04205DD284602F067FA88F80040D5E74B +:1014A00098F80030072B05D8013388F8003006B0D3 +:1014B000BDE8F081014802F057FAF8E7DC38002077 +:1014C00040420F00F0170020A512002010B50021A7 +:1014D000044600F01FFC601C0B4C80B2A4F82C05E5 +:1014E00000F000FC70B1B4F82C0500F00BFC0146D4 +:1014F00040B9B4F82C0500F00DFCB4F82C0501300F +:1015000080B2EBE710BD00BFA812002010B50A4B57 +:101510000A4A1A6003F5805393F870203AB9DC6EDA +:101520002CB1204600F090FF204601F0CDF8BDE838 +:101530001040034800F088BFF0170020FC55000859 +:10154000482800202DE9F04F8FB000AF05460C462B +:1015500002F038F900284AD1237E022B1AD1E38AFF +:10156000012B17D101F0F2FD0646FFF7EFFD4FF416 +:10157000C873DFF8EC82B0FBF3F206F5167602FBD7 +:10158000130316FA83F3C8F80030E37E33B9AE4B89 +:1015900000221A703C37BD46BDE8F08F07F12401E8 +:1015A000204600F081FD0028F4D107F11400FFF778 +:1015B00029FE97F8264007F11401224607F127007B +:1015C000FFF7CAF90028E2D10F2C08D89E4B1C70F7 +:1015D000D8F80030A3F51673C8F80030DAE797F8AA +:1015E00024100029D6D0284602F0E4F8D2E7E38A96 +:1015F000282B38D010D8012B30D0052BCAD1BFF3FF +:101600004F8F9249924BCA6802F4E0621343CB6059 +:10161000BFF34F8F00BFFDE7302BBBD1DFF844B2E3 +:10162000E17E9BF808209142B4D1607E59460022A9 +:1016300091F8DC50854200F0B4800132042A01F5B3 +:101640008A71F5D1DBF8002040F6B831FA328A42CF +:1016500094BFCBF80020CBF800109BE7214628462A +:10166000FFF7E0FD96E721462846FFF73BFE91E7AE +:10167000B2F8EC507B60EA1C930822F003020732B8 +:10168000D208BB60A8EBC2039D46BB68EA469A003D +:101690000021504600F0B2FB7B6803F1EE012A46C0 +:1016A0005944504600F09EFB14B9012000F00AFB9B +:1016B0009BF8D20000F016FB044638B98BF80800FE +:1016C00000F03AFB204600F0FDFAB4E0DBF8D4204D +:1016D0003AB99BF8D200BBF82C25824201D8FFF71B +:1016E000F5FEDBF8D4202A44944208D29BF8D200BD +:1016F000BBF82C250130824201D8FFF7E7FE3A7A89 +:10170000DBF80400514600F081FB08B9C5467BE0D8 +:10171000DBF804209BF8D2002A44CBF80420DBF845 +:10172000D42005EB020ACBF8D4A000F0DBFA824506 +:1017300009D39BF8D22001328BF8D220DBF8D420D9 +:10174000121BCBF8D420FF2D10D800248BF80840B2 +:1017500000F0F2FA204600F0B5FA00F0FBFD3D4B38 +:10176000188108B9FFF72AFAC54613E7FB689BF80A +:10177000D90009FB03B2D2F8E41082F8E86001F561 +:101780008061C2F8E060C2F8E410FFF709FEFFF7DD +:1017900057FE9BF8D920013202F003028BF8D920C2 +:1017A000B4E74FF48A7909FB02F505F1EA015944DF +:1017B000204600F073FD064600287FF4EBAE5D4442 +:1017C000012285F8E82001F0C1FCD5F8E020DBED2E +:1017D000007ADFED216A801A192838BF192040F6F7 +:1017E000B832904228BF1046B8EE677A07EE900AEA +:1017F000F8EEE77A67EEA67ADFED186AE7EE267A6A +:10180000FCEEE77ACBED007A9BF8D930FB6009FB60 +:1018100003F30BEB0302DBF8040092F8E81059B174 +:10182000D2F8E4408442E8463FF422AF002182F837 +:10183000E810C2F8E010C546DBF80430054A9B0A00 +:1018400001331381A6E600BFD817002000ED00E0A9 +:101850000400FA05E0170020CDCCCC3D6666663F5B +:10186000DC170020A8120020014B1870704700BF41 +:10187000D828002038B54FF00054144B22689A4203 +:1018800021D1607DF0B1124B124918701248237DAE +:1018900003724FF48073C0F8F8314FF40073C0F84E +:1018A0000C3300254FF44073C0F82034C0F8E450E6 +:1018B000C922093000F096FAE0222946204600F0BD +:1018C0009DFA012038BD0020FCE700BF9AAD44C559 +:1018D000D828002016000020A812002037B500F0FC +:1018E00039FD184D18492881022318486B7101F001 +:1018F000D7FA00230193164B164900931648174B4D +:101900004FF4805201F02EFF154B197811B1124897 +:1019100001F050FF01F01AFC0446FFF717FC4FF4EA +:10192000C87304F51674B0FBF3F202FB13000D4B01 +:1019300014FA80F0186003F0FBFB08B10F232B8131 +:1019400003B030BDE017002040420F00F017002028 +:101950007D110008DC280020DC3800204515000837 +:10196000D8280020DC1700202DE9F04F2DED028B48 +:101970000FF23829D9E90089834C93B00BAD9FED64 +:101980007E8BFFF75DFD814FDFF828A200230A93CD +:10199000ADF834300B936B604FF0000B5B468DED70 +:1019A000008B01260DF11D0207A938468DF81C6039 +:1019B0008DF81DB001F024FA9DF81C30002B40F08A +:1019C000A380204601F0FEFE0546002843D1704F5B +:1019D00001F0BCFB3B6898423DD301F0B7FB824667 +:1019E000FFF7B4FB4FF4C8730AF5167AB0FBF3F2B5 +:1019F00002FB13001AFA80F03860664F97F800B0C7 +:101A0000BBFA8BF3CBF1100A5FFA8AFA5B090EA8D6 +:101A10008DF82830FFF7F6FBBAF1060F28BF4FF01C +:101A2000060A0EAB03EB0B0152460DF1290000F044 +:101A3000D9F90AAB0393182302930AF10102564B1A +:101A4000D2B2CDE90063049220464DA3D3E900232E +:101A500001F0FCFE3D7001F079FB504A504D1368D7 +:101A6000C31AB3F57A7F2ED3106001F071FB0246E2 +:101A70000B46204601F082FF204601F0A3FE10B382 +:101A80002B7A484E002B0CBF02230323737101F005 +:101A90005DFB0EAF4FF47A730122B0FBF3F03946D1 +:101AA0003060304600F0D8FA182302933E4B019381 +:101AB00080B240F25513CDE90370009342464B4685 +:101AC000204601F0C3FE2B7A93B101F03FFB0026C4 +:101AD00007464FF48A7A95F8D900304400F00300A5 +:101AE0000AFB005393F8E82092B30136042EF2D19A +:101AF000C82002F0BDFF2B7A002B7FF43FAF13B05C +:101B0000BDEC028BBDE8F08FDAF8143083F00203ED +:101B1000CAF81430594610220EA800F06FF90DF1E2 +:101B20001E0308AA0AA9384600F0F6FE95E803004D +:101B30000FAB83E803009DF834308DF844300A9BE6 +:101B40000E930EA9DDE90823204602F0E7F823E70B +:101B5000D3F8E02042B12B68FA2B38BFFA23BA1A27 +:101B60000533B2EB430FC0D3FFF71AFC0028BCD1FA +:101B7000BEE700BFAFF300800000000000000000DF +:101B8000401DA12026812A0BDC380020F017002000 +:101B9000DC170020D8170020EC170020A01200202E +:101BA000A8120020E0170020A4120020F1C6A7C14F +:101BB000D068080F0004004808B5054800F046FF4B +:101BC000BDE80840034A0449002003F0D9BC00BF27 +:101BD000F0170020603900200D15000870B5104B7B +:101BE0001B780133DBB2012B05460C4611D80D4B97 +:101BF000186801684FF47A730E6AA2FB0332294613 +:101C00002246B047844204D1074B00221A700120BB +:101C100070BD4FF4FA7002F02BFF0020F8E700BF10 +:101C2000681100206C1100203C39002007B500230A +:101C3000024601210DF107008DF80730FFF7CEFFB6 +:101C400020B19DF8070003B05DF804FB4FF0FF30B2 +:101C5000F9E7000008B50A460421FFF7BFFF013885 +:101C600008BD000030B4074B0A461978064B0546FC +:101C700053F821000368DC69044B2946A44630BCB4 +:101C8000604700BF3C3900206C110020A086010095 +:101C900070B503F0A3F8094E094C30800025206888 +:101CA0003388834208D903F095F82368054401334B +:101CB000B5F5803F2360F2D370BD00BF34390020FA +:101CC0003839002003F038B900F1006000F58030A9 +:101CD0000068704700F10060920000F5803003F06A +:101CE000BBB80000054B1A68054B1B889B1A834242 +:101CF00002D9104403F06EB8002070473839002034 +:101D000034390020024B1B68184403F069B800BF47 +:101D100038390020024B1B68184403F075B800BF27 +:101D2000383900200020704700F10050A0F5104025 +:101D3000D0F8900570470000064991F8243033B17F +:101D40000023086A81F824300822FFF7C3BF01206E +:101D5000704700BF0C390020014B1868704700BF66 +:101D6000002004E010B501380E4A0B180E48126826 +:101D70000488C2F30B0294420ED142680846013A2D +:101D800098420CD212F9014F1CB92C2300F8013BE8 +:101D900005E000F8014BF3E799420846F5D3401AF5 +:101DA00010BD00BF002004E070110020022804BF15 +:101DB000014B9861704700BF00040048022802BF31 +:101DC000024B4FF400329A61704700BF0004004894 +:101DD000022801BF024A536983F00203536170472E +:101DE0000004004810B50023934200D110BDCC5C24 +:101DF000C4540133F8E70000FFF7FEBF0244034676 +:101E0000934200D1704703F8011BF9E72DE9F8432D +:101E10001F4D144695F824200746884622B9DFF85E +:101E200070906CB9012009E02B6A03EB82038342B6 +:101E3000F5D0FFF781FF0028F1D10020BDE8F8833D +:101E400095F824302BB92022FF2148462F62FFF756 +:101E5000D5FF95F82400C0F10802A24228BF22460F +:101E6000D6B24146920005EB8000FFF7BBFF95F824 +:101E70002430A41B1E44F6B2082E17449044E4B24A +:101E800085F82460CDD1FFF757FF0028C9D1D4E7EA +:101E90000C390020024B1A78024B1A70704700BFB1 +:101EA0003C3900206811002003494FF461430B6066 +:101EB000024B186802F0B8BC403900206C110020B9 +:101EC00038B5094C0546182200212046FFF796FF39 +:101ED000064B25601A78064B214653F82200BDE8D0 +:101EE000384002F0A1BC00BF403900203C3900203E +:101EF0006C1100202DE9F3470D4604460021904661 +:101F0000284640F27912FFF779FF2346202200216C +:101F1000284601F0A1FE231D02222021284601F0BF +:101F20009BFE631D03222221284601F095FEA31D7E +:101F300003222521284601F08FFE04F10803102218 +:101F40002821284601F088FE04F1100308223821D8 +:101F5000284601F081FE04F11103082240212846A1 +:101F600001F07AFE04F1120308224821284601F00C +:101F700073FE04F1140320225021284601F06CFE68 +:101F800004F1180340227021284601F065FE04F197 +:101F900020030822B021284601F05EFE04F121034F +:101FA0000822B821284601F057FE04F12207C02676 +:101FB0003B46314608222846083601F04DFEB6F56C +:101FC000A07F07F10107F3D194F832308DF8073084 +:101FD00031460DF107030822284601F03DFE002698 +:101FE00004F1330A9DF807304FEAC6099E4209F50D +:101FF000A47720D394F83231502B28BF50238DF88A +:102000000730B8F1000F08D139460DF10703072258 +:10201000284601F021FE09F24F17002604F233147E +:102020009DF807309E4207EBC6010DD30731C80863 +:1020300002B0BDE8F0870AEB0603082239462846BD +:1020400001F00AFE0136CDE7A3190822284601F067 +:1020500003FE0136E4E7000038B50C4600210546D2 +:1020600021600346C4F803102046202201F0F4FD4D +:102070002B1D20460222202101F0EEFD6B1D204683 +:102080000322222101F0E8FDAB1D20460322252179 +:1020900001F0E2FD204605F108031022282101F09D +:1020A000DBFD072038BD0000F7B5057F04460E466E +:1020B0002DB1838A122B02D9012003B0F0BD002379 +:1020C000194607220096204601F092FC731C0093EB +:1020D000012200230721204601F08AFCD5B9B31C58 +:1020E000009305222B460821204601F081FC0D2596 +:1020F000B378102BE0D83746B278BB1B934210D38D +:10210000237FA08A0735ED08B3B9A8422CBF002071 +:102110000120D2E7A38ADB00083BDB08B370082567 +:10212000E6E7FB1C0093294600230822204601F025 +:102130005FFC08350137DFE7401B18BF0120BCE713 +:10214000F7B5047F05460E462CB1838ACA2B02D907 +:10215000012003B0F0BD002300960822194628464E +:1021600001F046FCDCB9731C08220093114623469B +:10217000284601F03DFC10247378C82BE8D80123D1 +:1021800072785F1C013B934210D32B7FA88A0734DF +:10219000E408B3B9A0422CBF00200120D9E7AB8AE4 +:1021A000DB00083BDB0873700824E5E7F3190093B4 +:1021B000214600230822284601F01AFC08343B4639 +:1021C000DEE7001B18BF0120C3E70000F7B50E468D +:1021D00004460021154630468122FFF70FFE2346B4 +:1021E00008220021304601F037FD63787F2B28BF9D +:1021F0007F238DF807309DB90DF1070307220821D1 +:10220000304601F029FD0F27002502349DF80730E4 +:102210009D4207EBC50105D30731C80803B0F0BDE7 +:102220000827F1E763190822304601F015FD013552 +:10223000ECE70000F7B50E4605460021144630468F +:10224000CE22FFF7DBFD2B4628220021304601F08D +:1022500003FD2B7AC82B28BFC8238DF807308CB913 +:102260000DF1070308222821304601F0F5FC302447 +:102270002F469DF807207B1B934205D3E01DC00825 +:1022800003B0F0BD2824F3E707F109032146082233 +:10229000304601F0E1FC08340137EAE7F7B5047F86 +:1022A00005460E4634B1838AB3F5827F02D90120F8 +:1022B00003B0F0BD0096012310220021284601F052 +:1022C00097FBDCB9B31C0093092223461021284652 +:1022D00001F08EFB19247388B3F5807FE7D8374669 +:1022E0007288BB1B934210D32A7FAB8A0734E40861 +:1022F000B2B99C4294BF00200120D9E7AB8ADB0031 +:10230000103BDB0873801024E5E73B1D009321465A +:1023100000230822284601F06BFB08340137DFE771 +:10232000E01A18BF0120C3E730B5094D0A449142B5 +:1023300000D130BD11F8013B5840082340F30004A0 +:10234000013B2C4013F0FF0384EA5000F6D1EEE786 +:102350002083B8EDF7B5364A106851686B4603C361 +:102360006A46344934480823FEF756FA044618B33F +:102370000369B3F5403F54D8418B40F28D429142FE +:1023800051D12E4A024402F110018B424DD3103B31 +:10239000294900209D1AFFF7C7FF2A46064604F187 +:1023A00018010020FFF7C0FFA3689E4202D1E36836 +:1023B000984232D00D2500E00A25214A1068516864 +:1023C0006B4603C36A461F491B480823FEF724FADD +:1023D000044618B30369B3F5403F1FD8B0F8661040 +:1023E00040F28D42914219D1144A024402F15C013B +:1023F0008B4213D35C3B104900209E1AFFF794FFD9 +:102400003246074604F164010020FFF78DFFA36800 +:102410009F4203D1E368984200D10025284603B0CB +:10242000F0BD0B25C9E70C25C7E71025C5E700BFA0 +:10243000C0550008DCFF0200000001080800FFF79B +:10244000C955000890FF020010B5037C044613B97B +:102450000068FDF7D9FF204610BD00000023BFF340 +:102460005B8FC360BFF35B8FBFF35B8F8360BFF392 +:102470005B8F7047BFF35B8F0068BFF35B8F704764 +:1024800070B505460C30FFF7F5FF05F10806044668 +:102490003046FFF7EFFFA04206D930466D68FFF7E0 +:1024A000E9FF2544281A70BD3046FFF7E3FF201AE4 +:1024B000F9E7000070B50546406898B105F10800DD +:1024C000FFF7D8FF05F10C0604463046FFF7D2FFB0 +:1024D0008442304694BF6D680025FFF7CBFF013C76 +:1024E0002C44201A70BD000038B50C460546FFF795 +:1024F000C7FFA04210D305F10800FFF7BBFF6B68D0 +:10250000BFF35B8F0444B4FBF3F203FB1244AC60F3 +:102510000120BFF35B8F38BD0020FCE72DE9F041BF +:10252000144607460D46FFF7C5FF844228BF044600 +:10253000D4B1B84658F80C6B4046FFF79BFF0644F1 +:102540002E6040467E68FFF795FF331A9C4203D801 +:102550006C600120BDE8F0816B60A61B3B68AB603E +:102560003044E8600220F5E72046F3E738B50C4632 +:102570000546FFF79FFFA04210D305F10C00FFF7BF +:1025800079FF6B68BFF35B8F0444B4FBF3F203FB8A +:102590001244EC600120BFF35B8F38BD0020FCE7E4 +:1025A0002DE9F04385B0884669460746FFF7B6FF38 +:1025B0006C4606B204EBC6060025B44207D1294694 +:1025C0003846FFF7D3FF284605B0BDE8F083D4F8BE +:1025D000049054F8080B08EB05014A46FFF702FC8B +:1025E0004D44EAE7F8B505460C300F46FFF742FFC9 +:1025F00005F1080604463046FFF73CFFA04230468E +:1026000088BF6C68FFF736FF201A386020B130466B +:102610002C68FFF72FFF2044F8BD000073B5144667 +:1026200006460D46FFF72CFF844228BF0446DCB166 +:1026300001A93046FFF7D6FF019B3BB93268C5E9D7 +:102640000233C5E90024012002B070BD9C4238BFAE +:102650000194019B28609C426B60F4D93268AA60A7 +:10266000E41AEC600220EFE72046EDE72DE9FF4198 +:102670000F466946FFF7D2FF6C4605B204EBC5056D +:102680000026AC4203D1304604B0BDE8F081D4F856 +:10269000048054F8081BB8194246FFF7A3FB4644D0 +:1026A000EFE7000038B50546FFF7E0FF04460146B6 +:1026B0002846FFF719FF204638BD00000121884257 +:1026C00038BF0846FDF77EBE08B1FDF79DBE7047D6 +:1026D00003685B68184700000020704770470000DF +:1026E0000020704700F5805090F8D800C0F34000FB +:1026F0007047000000F5805090F9E000704700003E +:1027000000F58050C0F8DC1001207047F7B50C6868 +:10271000BDF8207014F0005473D10D7B082D70D8D3 +:10272000302585F31188056AAE6876010CD4AC6853 +:10273000240108D4AC6814F0805460D184F311886B +:10274000204603B0F0BD01240E6804F1180C002EE1 +:10275000B8BFF6004FEA0C1CB4BF46F0040676057D +:1027600045F80C600E680FFA84FC16F0804F18BF15 +:1027700005EB0C1E05EB0C1C1EBFDEF8806146F05D +:102780000206CEF880610E7BCCF8846105EB04155F +:102790008E68C5F88C614E68C5F88861DCF8805198 +:1027A00045F00105CCF8805141F2780500EB441C5E +:1027B000660105EB4414AC440444CCE900230D4607 +:1027C000083401F10C0C55F804EB44F804EB6545B2 +:1027D000F9D12D882580841904F5805407F003056C +:1027E00094F8946026F00B06354384F89450002446 +:1027F00084F31188009700F0FDFC0120A1E702247A +:10280000A2E74FF0FF309CE738B500F580540D4645 +:10281000E06EFFF74FFE1F2809D9E06E20222946FF +:10282000FFF7BEFEA0F120035842584138BD0020FA +:10283000FCE7000008B5302383F3118800F58050D1 +:10284000C06EFFF70BFE002383F3118808BD000064 +:1028500000220260828142608260704710B50022CF +:102860000023C0E900230023044603810C30FFF756 +:10287000EFFF204610BD0000F0B5054600F5805082 +:102880000C4690F8D83013F0040FC3F3800108BF52 +:10289000114661F3820380F8D83005EB441303F549 +:1028A0008453103389B01B79D8074FEA44162FD5CB +:1028B00082B319072ED46846FFF7D0FF05EB441307 +:1028C00003F5845303AA03F10807186859681446EE +:1028D00003C40833BB422246F7D1186820609B88A6 +:1028E000A380DDE90E23CDE900230123ADF80830F4 +:1028F0002B6869469B6B28469847354405F58455F7 +:1029000010352B791A075CBF43F008032B7101E0E7 +:10291000002AF2D109B0F0BD2DE9F0479A4688B0FF +:10292000074688469146302383F3118807F5805483 +:102930006846FFF793FFE06EFFF7A2FD1F282AD934 +:10294000E06E20226946FFF7ADFE202823D103ADBB +:10295000444605AB2E4603CE9E422060616035465C +:1029600004F10804F6D130682060B388A380DDE963 +:102970000023C9E90023BDF80830AAF8003000237D +:1029800083F3118853464A464146384608B0BDE8AD +:10299000F04700F0E9BB002080F3118808B0BDE8E3 +:1029A000F08700002DE9F84F182204460F46043046 +:1029B0000021FFF723FA2046214B40F8243BFFF784 +:1029C00047FF04F13800FFF749FF04F1580804F508 +:1029D00082554646183530462036FFF73FFFAE4257 +:1029E000F9D104F580554FF48053C5F858804FF065 +:1029F0000008C5E917380123EE66C5F8648085F83C +:102A0000683085F8703004F5845604F587594FF026 +:102A1000000A4FF0000B46E902AB3046FFF718FF03 +:102A2000203646F8108C4E45F5D185F8E07017B188 +:102A3000044800F085FB044B23622046BDE8F88F74 +:102A4000FC550008D25500080064004010B5044B46 +:102A5000197804464A1C1A70FFF7A4FF204610BDDF +:102A6000583900202DE9F04711B90020BDE8F08762 +:102A7000274B284EB6FBF1F4994294BF11230A2349 +:102A8000013BDBB2581CB4FBF0FC00FB1C405FFABE +:102A900083FEC0BB0CF1FF37B7F5806FE5D2C3EB07 +:102AA000C303D81C4FEAE009C0F3C700AEEB00082F +:102AB00009F1010A4FF47A755FFA88F409FB0555AC +:102AC0005AFA88F8B5FBF8F5B5F5617FC1BF03F197 +:102AD000FF33C3F3C700AEEB0003DCB2431C54FA70 +:102AE00083F30CFB03F3B6FBF3F68E42BDD1013C3E +:102AF000E4B2072CB9D801380023D0701780937046 +:102B000014710120B2E7BEF1020FAED0013BB9E76C +:102B10003F420F0000B4C40470B505460E464FF4A2 +:102B20007A742B6A5B6803F00103B34207D04FF459 +:102B30007A7001F09DFF013CF3D1204670BD012069 +:102B4000FCE7000030B5026A936913F0700F17D0EC +:102B50000023936141F2900400EB43122244117967 +:102B60004D070AD5890708D5064D016A55F8235047 +:102B70008D60117941F0040111710133032BEBD108 +:102B800030BD00BF3856000873B51D46036A1646AF +:102B90009A68D207044609D59A6801219960C2F360 +:102BA0004002CDE900650021FFF766FE236A9A68BE +:102BB000D1050BD59A684FF480719960C2F3402219 +:102BC000CDE9006501212046FFF756FE236A9A6889 +:102BD000D2030BD59A684FF480319960C2F340421A +:102BE000CDE9006502212046FFF746FE04F580533B +:102BF000D3F8DC0010B103681B699847204602B087 +:102C0000BDE87040FFF79EBFF8B50446066A00298C +:102C100077D106F10C073868800775D006EB0115EF +:102C20003868D5F8B00110F0040FD5F8B0011ABF1C +:102C3000C00840F00040400D6062D5F8B0C11CF003 +:102C4000020F1CBF40F080406062D5F8B40106EB73 +:102C5000011100F00F0084F83000D1F8B80184F8B9 +:102C60002800D1F8B801000A84F82900D1F8B80189 +:102C7000000C84F82A00D1F8B801000E84F82B006B +:102C8000D1F8BC0184F82C00D1F8BC01000A84F80A +:102C90002D00D1F8BC01000C84F82E00D1F8BC1135 +:102CA000090E84F82F103821396004F1440004F132 +:102CB000240104F1300551F8046B40F8046BA9427B +:102CC000F9D109880180C4E90E2321460023A4F824 +:102CD000403051F8383B20469B6B984704F58053B1 +:102CE00093F8D820D3F8DC0042F0040283F8D8200F +:102CF00010B103681B6998472046BDE8F840FFF70C +:102D000021BF06F1100786E7F8BD000010B50446A4 +:102D100000F01EFA02460B4652EA030102D0013AC5 +:102D200063F100030449086820B12146BDE8104062 +:102D3000FFF76ABF10BD00BF5C390020F0B530213D +:102D400081F3118800F583511831002441F2900C71 +:102D500000EB441565442E7977070FD4F6060DD5A0 +:102D6000D1E9007697429E4108D2094F066A57F88A +:102D70002470B7602E7946F004062E710134032CBE +:102D800001F12001E4D1002383F31188F0BD00BFDD +:102D90003856000808B5302383F31188FFF7D2FEB8 +:102DA000002383F3118808BD036A9B6813F0E05F7A +:102DB00014BF01200020704708B5302383F3118829 +:102DC00000F58050C06EFFF75BFB002383F3118892 +:102DD00040090CBF0120184608BD0000F8B51E468A +:102DE000002313700F4605461446FFF7E5FF80F0F9 +:102DF000010038701EB12846FFF7D6FF2070F8BDDD +:102E0000F8B50C4615461E46074600F0A1F90B46DC +:102E10002178024609B9287850B13846FFF78EFF6D +:102E2000FFF7B8FF33462A462146FFF7D7FF0120B8 +:102E3000F8BD000070B5302686F31188174B9A6DE7 +:102E400042F000729A659A6B42F000729A639A6B34 +:102E500022F000729A63002383F3118800F58054F6 +:102E600094F8D83013F0010516D1A9B186F3118872 +:102E70000321132001F020FB0321142001F01CFB8F +:102E80000321152001F018FB94F8D83043F001031A +:102E900084F8D83085F3118870BD00BF001002405F +:102EA0002DE9F04300F5805589B095F8E030012B0D +:102EB00004460F4676D8DFF8EC8158F823200AB98B +:102EC00048F82300D8F80090E761B9F1000F0BD162 +:102ED00041F2E800FFF7F2FB0646002860D04946C1 +:102EE000FFF760FDC8F8006095F8E030012B5DD079 +:102EF00001212046FFF79EFF302383F31188226AC9 +:102F0000136823F002031360226A136843F001037D +:102F10001360236A00265E6186F311880121204632 +:102F2000FFF7FAFD00287DD0E86EFFF797FA04F569 +:102F3000835808F11808202200216846FEF75EFF3A +:102F400002A8FFF785FC6A4608EB06030DF1180E90 +:102F50009446BCE80300F44518605960624603F1EA +:102F60000803F5D1DCF80000186020369CF8042036 +:102F70001A71602EDFD195F8D83023F0040385F85C +:102F8000D83000266A463946204600968DF80460FF +:102F9000FFF768FD236AF0B94FF400421A6001E0C0 +:102FA000C8F80000002009B0BDE8F083D8F80000A0 +:102FB00003689B6A98470146002899D1D8F8000019 +:102FC000FFF738FFD8F80000036839465B68984778 +:102FD00000288DD1E6E765221A609DF803109DF860 +:102FE0000230226A1B06090401F4702103F04073C9 +:102FF0000B43BDF80010C1F309010B439DF8041009 +:10300000090501F4E0010B43D361236A13225A61DD +:10301000226A136823F00103136031462046FFF74C +:103020007BFD08B9236AB7E795F8E02092BB216AD7 +:10303000D1F8003243F00103C1F80032216AD1F81F +:10304000003223F47C5323F00E03C1F80032216ACE +:10305000D1F8003243F46063C1F80032236AC3F848 +:103060001422236AC3F80422236A41F6FF71C3F8CD +:103070000C12236AC3F84022236AC3F84422236A4D +:103080000122C3F81C22226AD2F8003223F0010385 +:10309000C2F8003295F8D83043F0020385F8D830F2 +:1030A00081E700BF5C39002008B500F051F8024606 +:1030B0000B4652EA030102D0013A63F100030449CE +:1030C000086808B1FFF760FDBDE8084001F0A6B848 +:1030D0005C39002008B50020FFF718FEBDE8084065 +:1030E00001F09CB808B50120FFF710FEBDE80840CC +:1030F00001F094B800B59BB0EFF3098168226846EF +:10310000FEF770FEEFF30583014B9B6BFEE700BFFC +:1031100000ED00E008B5FFF7EDFF000000B59BB043 +:10312000EFF3098168226846FEF75CFEEFF3058342 +:10313000014B5B6BFEE700BF00ED00E0FEE7000027 +:103140000FB408B5029801F043FCFEE701F038BF68 +:1031500001F01ABF0139C9B202299EBF00EBC100BC +:103160000023C0E9013370472DE9F8431E461B8850 +:103170005B070546884605D400F10C0400F12409DC +:10318000A14502D10120BDE8F88354F8083C13B9E9 +:1031900054F8043C6BB12B6828465B6B9847338826 +:1031A00054F8047CC1B243F0040354F8080C4246BE +:1031B000B8470834E4E7000073B56C4684E80600BD +:1031C000014600224E68551C66B98E6856B900EB60 +:1031D000C200021D94E8030082E803001D70012074 +:1031E00002B070BD032D2A4601F10801EAD100208A +:1031F000F6E700002DE9F04F89B00446BDF848809D +:103200008B4600F10C0500F12409002708F0040AA0 +:10321000A94503D1012009B0BDE8F08F55F8083C5D +:1032200013B955F8043C63B1BAF1000F0BD1236810 +:1032300020465B6B98474346C1B25A4655E90206A1 +:10324000B0470835E4E70FBB00220023CDE9002397 +:10325000ADF808705B4603AA0BF1080C18685968B2 +:10326000174603C7083363453A46F7D118683860F4 +:103270009B88BB80FFF76CFF0423ADF80830236800 +:10328000CDE900019B6B6946204698470127D8E7A6 +:103290000E2809D8DFE800F01515151515151515B8 +:1032A000150A0C0E10121400402070470C207047B5 +:1032B0001020704714207047182070472020704756 +:1032C000302070472DE9F043456A35B94162BDE8C9 +:1032D000F083CEF800100B60F9E74E6826F0604CE2 +:1032E000C6F38A47C6F3807817EA260738BF6746D1 +:1032F000AE462B465C6894EA060F24F060420CD57B +:10330000002C1EDA920CBA4201D0BA420FD99D426B +:10331000DFD10D60DAE71346ECE7944505D104F000 +:103320008049C4F38074444507D19445EFD31A68AB +:103330009E46002AEFD11960C9E7B9F1000FF6D017 +:10334000E5E7BA42F3D0E0E72DE9F047089E01F047 +:1033500007054FEAD6082A4406F0070600EBD1011C +:103360004FF47F49954201D1BDE8F08705F0070C85 +:1033700006F0070AD445674638BF5746C7F1080725 +:10338000501BEC08874228BF07460C5D08EBD600AF +:1033900004FA0CF413F800E029FA07FC24FA0AF402 +:1033A0005FFA8CFC8EEA04044CFA0AFC04EA0C0472 +:1033B0008EEA040E03F800E03D443E44D2E70000EC +:1033C00080EA0120082341F2210201B240000029D5 +:1033D00080B203F1FF33B8BF504013F0FF03F4D1C4 +:1033E0007047000038B50C468D18A54200D138BD95 +:1033F00014F8011BFFF7E4FFF7E7000003464068FD +:1034000048B102685A605A899989013292B2914250 +:103410005A8138BF9A81704770B588B0202204461F +:103420000D4668460021FEF7E9FC20460495FFF7AB +:10343000E5FF024658B16B46054608AE1C4603CC74 +:10344000B44228606960234605F10805F6D11046AC +:1034500008B070BD2DE9F843078C072F04461ED92C +:10346000D0E9029800254FF6FF73C5F12006A5F1BB +:10347000200029FA05F108FA06F628FA00F031438F +:103480000143C9B21846FFF79BFF0835402D03469C +:10349000EBD1E1693A46BDE8F843FFF7A3BF4FF629 +:1034A000FF70BDE8F883000010B54B6823B9CA8AE5 +:1034B00063F30902CA8210BD04691A681C600361C3 +:1034C000C38A013BC3824A60EFE700002DE9F84F51 +:1034D0001C46CB8A8046C3F3090005280F46914657 +:1034E00003462ED000221A319DB2042D96B211D877 +:1034F000B44203F1010308D80444FB8AC4F309046D +:1035000064F30903FB82012019E019F80250CD543D +:103510000132E9E7B442EFD9A0F1050A1C237D6826 +:10352000BAFBF3F203FB12AA1FFA8AF295BB4046DC +:10353000FFF764FF054630B978606FF00200BDE820 +:10354000F88F0026E8E70022026078604FF0000A5A +:10355000B44242D9D31C2B44901B25E0013189B2DF +:103560001D462B68002BF9D1631F03441C20B3FBBD +:10357000F0F301339BB29942E8D2BAF1000FE5D1E2 +:103580004046FFF73BFF38B1C0F800A05246286024 +:103590000546DBE70121E4E7C5F800A0CDE719F80F +:1035A000061003F8011F013610FA86F189B21B29B3 +:1035B0001FFA86FB03D85C45F1D85E46C8E75C4538 +:1035C000FBD94046FFF71AFF28B1C0F800A000223F +:1035D00028600546F1E72860AFE7FB8A1C448CE7CA +:1035E0002DE9F74F1C465B69009207468846002B81 +:1035F00000F0B580238C2BB1E269002A00F0AF8087 +:10360000072B4AD807F10C00FFF7F8FE054620B952 +:103610006FF0020003B0BDE8F08F14220021FEF726 +:10362000EDFB228CE16905F10800FEF7DBFB208C45 +:10363000013080B208280DD90C2824D9102824D9AB +:10364000142824D9182824D9202824D9312834BF73 +:103650000E200F20FFF71CFE013880B220840130BD +:1036600028746369228C1B782A4403F01F0363F0DB +:103670003F03137248F0004338466B602946FFF75A +:1036800021FE0120C6E70920E4E70A20E2E70B203B +:10369000E0E70C20DEE70D20DCE702339BB20722D7 +:1036A000C18A0633B3FBF2F3828A521A9BB292B2FA +:1036B0009342ADD8002500F10C034FF08009AA46D3 +:1036C0002E460193238CB34201D128B2A2E7019880 +:1036D000FFF794FE834600289AD014220021FEF7BB +:1036E0008DFB56B9009BABF8083002220BF1080C99 +:1036F000218CB14203D890B20BE00022F6E7E169D9 +:10370000895D0CF80210013290B201360728B6B27A +:10371000EED1228C013080B2FFF7BAFDB24208BF71 +:103720004FF0400962691278013802F01F021BFA5B +:1037300080F142EA4A1249EA02020A72013048F074 +:1037400000428BF81000CBF8042059463846FFF7AA +:10375000B9FD8AF0010A01354FF00009B2E76FF0B8 +:10376000010057E7F8B515460E4630220021044601 +:103770001F46FEF743FBB5F5001F069B636034BF91 +:103780006A094FF6FF72079BA76095B2A362E661D4 +:1037900004F1100006EB4212964206D100230360AA +:1037A000A582E3822383E360F8BD066030462036BD +:1037B000F2E7000003781BB94BB2002BC8BF0170C1 +:1037C0007047000000787047F8B50C46C969074695 +:1037D00009B9238CB3BB257E1F2D33D83B7823BB7F +:1037E000228C072A2BD8268A032E2BD84FF6FF705F +:1037F000FFF7F8FD20F001003602400446EA0566B6 +:10380000400C46EA40254FF6FF72234629463846CB +:10381000FFF7E6FE002807DD626913780133DBB2AB +:103820001F2B88BF00231370F8BD228A2D0645EA9E +:10383000022520461D43FFF70DFE0246E5E76FF027 +:103840000300F1E76FF00100EEE7000070B58AB009 +:10385000044616460021282268461D46FEF7CEFA89 +:10386000BDF83830ADF810300F9B05939DF840300F +:103870008DF81830119B07936946BDF84830ADF8B4 +:1038800020302046CDE90265FFF79EFF0AB070BDEB +:103890002DE9F041D36905460C4616460BB9138C49 +:1038A0005BBB377E1F2F28D895F80080B8F1000F3A +:1038B00026D03046FFF7CEFD3378210241EAC331EE +:1038C00041EA0801338A41EA076141EA03410246BD +:1038D000334641F080012846FFF782FE00280ADDCA +:1038E0003378012B07D1726913780133DBB21F2BB8 +:1038F00088BF00231370BDE8F0816FF00100FAE784 +:103900006FF00300F7E70000F0B58BB004460D46FA +:1039100017460021282268461E46FEF76FFA9DF8DA +:103920004C305A1E534253418DF800309DF84030C0 +:10393000ADF81030119B05939DF848308DF8183084 +:10394000149B07936A46BDF85430ADF820302946E1 +:103950002046CDE90276FFF79BFF0BB0F0BD0000DB +:10396000406A00B104307047436A1A6842620269D3 +:103970001A600361C38A013BC38270472DE9F84393 +:10398000D0F82090194F14461E4649464FF00008C3 +:1039900009B9BDE8F883D1E90252651B66EB020262 +:1039A000AF4278EB02021ED2036A8B420DD1FFF7C1 +:1039B0007BFD036A1B68036203690B60C38A0161B4 +:1039C000016A013BC3828946E2E7FFF76DFD0B68A0 +:1039D000C9F8003003690B60C38A0161013BC382EF +:1039E000D9F80010D4E789460968D1E780841E0021 +:1039F0002DE9F04F8BB00D46DDF85080049314464E +:103A00008246002800F01881B8F1000F00F0148100 +:103A1000531E3F2B00F21081012A03D1049B002B7F +:103A200040F00A81BAF814B00023B5EBCB0FCDE912 +:103A300008334FEACB0703D300200BB0BDE8F08F6B +:103A40002B199F42DAF80C3003933ABF7F1BFFB269 +:103A500027461BB9DAF81030002B75D0272D4AD82D +:103A6000C5F12806B7424FF000032CBFF6B23E4620 +:103A700000932946DAF8080008AB3246FFF764FCE9 +:103A8000A7EB060935445FFA89F90BF1005BABF14E +:103A9000050B4FEACB0B2821039B13B1B9F1000FA3 +:103AA0002CD1DAF8100040B1B9F1000F05D0009622 +:103AB00008AB4A46691AFFF747FC38B2002FBBD063 +:103AC00067070AD00AAB03EBD401624211F8083C45 +:103AD00002F00702134101F8083C082C3BD9102CD6 +:103AE00040F2B580202C40F2AD80049B002B00F00A +:103AF0009780082332E0B9460026C6E75B46E02BF4 +:103B000028BFE02306930B44AB42059314D95A1BFC +:103B100003980096914534BF4A46D2B2691A08AB61 +:103B200004300792FFF710FC079A16441544A9EBDE +:103B30000202F6B25FFA82F9069B0599ABEB030B22 +:103B4000039B1B680393A7E70093DAF8080008AB10 +:103B50003A462946AFE7049B93B10123B4EBC30F68 +:103B600063D0082C11D89DF82030621E23FA02F28F +:103B7000D60705D54FF0FF32A24013438DF8203011 +:103B80009DF8203088F8003057E7102C11D8BDF888 +:103B90002030621E23FA02F2D50705D54FF0FF321E +:103BA000A2401343ADF82030BDF82030A8F8003013 +:103BB00043E7202C0ED8089A631E22FA03F3D90794 +:103BC00004D54FF0FF33A34013430893089BC8F874 +:103BD000003032E7402C32D0DDE90851631EC4F1D9 +:103BE0002106A4F1210225FA03F301FA06F6334374 +:103BF00021FA02F21343DB0712D50123A4F12002BC +:103C0000C4F1200603FA02F223FA06F6A3405B424F +:103C100042EA060262EB42022B430A43CDE9083234 +:103C2000DDE90823C8E9002307E7082CA8D0102CF9 +:103C3000BAD0202CF4D1C9E76FF00100FDE66FF097 +:103C40000800FAE6049B002BC0D0042386E7049BFF +:103C5000002BA9D0022381E730B5012A144638BFD2 +:103C60000122402A85B028BF40220025012CCDE941 +:103C700002550ED81B788DF8083004AB03EBD20543 +:103C8000544215F8083C04F00704A34005F8083C2A +:103C900006E0082C0CD81B788DF808305307ECD1BF +:103CA00000910346002102A8FFF74EFB05B030BD8E +:103CB000102C03D81B88ADF80830EFE7202C8DBFFF +:103CC000D3E900451B680293CDE90245E6E7000011 +:103CD00010B5CB681BB98B600B618B8210BD04697A +:103CE0001A681C600361C38A013BC382CA60F0E7A3 +:103CF00003064CBFC0F3C0300220704708B502462F +:103D0000FFF7F6FF022806D15306C2F30F2001D1B8 +:103D100000F0030008BDC2F30740FBE72DE9F04FB8 +:103D2000D1F8008097B004464046CDE90623FFF75E +:103D3000DFFF022814BFC8F306260026B8F1000FE3 +:103D40000D46039080F2F88118F0C04A40F0F481EB +:103D5000097B002900F0F081039B022B03D023781C +:103D6000B34240F0EC8108F07F0340460493FFF734 +:103D7000C5FF049B059029444FEA8349039B91F8B2 +:103D800003B049EA034949EA4669002200235FEA91 +:103D9000DB16CDE90C2349EA000978D0049B009397 +:103DA00002466768039B0CA92046B847002800F02C +:103DB000C981276A002F58D1494604F10C00FFF74A +:103DC0002BFB0746002800F0EF8027626B7B0A93ED +:103DD000BB7E0B93FB7D0BF01F09C3F38403A3EBA6 +:103DE00009001FFA80FC0028B8BF0CF1200C0890D5 +:103DF000A9EB0303B8BF0FFA8CF0D7E902121FFA40 +:103E000083FCB8BF0890002BB8BF0CF1200C0993BD +:103E1000BCBF0FFA8CF3099351EA020ED7F804C025 +:103E200000F0A981069B591A079B63EB0202B14B74 +:103E30004FF0000E8B427EEB0203C0F09C81DDE967 +:103E40000A30834240F08A81002E36D0089B012B35 +:103E500000F39181BCF1000F40F08D81A6488842AB +:103E60007CEB0202C0F0838130E03B699945ADD024 +:103E70003F68002FF9D1494604F10C00FFF7CCFA56 +:103E80000746002800F09080236A3B609DE7276A80 +:103E900077B9049B00936568039B059A0CA920469B +:103EA000A847C0F57F40F43000B217B0BDE8F08FEE +:103EB0003B6999458AD03F68EAE78F4B8B4276EBA6 +:103EC000020203D26A7BBB7E9A4239D1C8F30468EE +:103ED000002E37D01BF0400F34D0069BBB60079BF1 +:103EE000FB601422002111A8FDF788FF069B0E93AA +:103EF000079B0F932B1D10932B7B8DF85190013B4B +:103F0000DBB2ADF84C30059BADF84E30039B8DF81D +:103F10005030049B8DF8533094F82C308DF852803B +:103F200083F001038DF854300EA9A368204698470A +:103F3000FB7DC3F38403013303F01F039B02FB8269 +:103F40000020B2E7FB7DCBF34012B2EBD31F40F071 +:103F5000FC80C3F384034B4540F0FA802B7B002E9A +:103F600051D0032B40F2F780069BBB60079BFB60A0 +:103F7000FB8A6FF30903FB822B7BAE1D033BDBB295 +:103F80003246394604F10C00FFF7A0FA00280EDA99 +:103F900039462046FFF788FAFB7DC3F384030133DB +:103FA00003F01F039B02FB826FF002007DE7DDE957 +:103FB0000C84AB883B834FF6FF73CAF12000AAF153 +:103FC000200228FA0AF104FA00F0014324FA02F26E +:103FD00011431846C9B2FFF7F3F90AF1080ABAF11A +:103FE000400F0346E9D1B8822A7B033AD2B2314668 +:103FF000FFF7F8F9FB7DB882DA43C2F3C01262F32F +:10400000C713FB759CE7013B5FEA9B1BDBB20ED13C +:104010002E1D3246394604F10C00FFF757FA0028EE +:10402000B6DB2A7BB88A013AD2B23146E0E7FA8A97 +:10403000C2F30902042A61D86BB1E91C07F11B0025 +:1040400011F801CF00F802C001320136052AF6B29C +:1040500001D09E42F4D3069A0E92079A0F929E4286 +:1040600007F11B02109238BF04367A68119234BFF0 +:1040700055FA86F600261296FA8A8DF85190C2F308 +:1040800009021344ADF84C30059BADF84E30039B4C +:104090008DF85030049B8DF8533094F82C308DF807 +:1040A000528083F001038DF8543000237B602A7B1B +:1040B000B88A013A291DFFF795F93B8BB8828342F4 +:1040C00003D1A3680EA92046984720460EA9FFF702 +:1040D000FFFDFB7DBA8AC3F38403013303F01F03A2 +:1040E0009B02FB823B8B9A4214BF6FF010000020B2 +:1040F000DBE600BF80841E0040420F007868002885 +:10410000A9D0052101E01C3130460668002EFAD105 +:10411000521AD11C05F1030C014402EB0A001B28C2 +:104120005FFA8AF697D89E4295D21CF8010F01F8E3 +:10413000010F0AF1010AF0E76FF00900B5E66FF030 +:104140000A00B2E66FF00B00AFE66FF00D00ACE6D0 +:104150006FF00E00A9E66FF00F00A6E61448884243 +:104160007EEB0202BFF4ECAE002E3FF4ABAE099B37 +:104170000F2B3FF7A7AEFA7D4FEA8B0302F0030245 +:1041800003F07C031343FB7539462046FFF78CF997 +:104190006B7BBB76002E7FF495AEFB7DC3F3840270 +:1041A000013262F38603FB756FF00C007DE600BF01 +:1041B00040420F00EFF3098305494A6B22F00102E8 +:1041C0004A63683383F30988002383F311887047B7 +:1041D00000EF00E0302080F3118862B60C4B0D4AEE +:1041E000D96821F4E0610904090C0A43DA60D3F8C4 +:1041F000FC20094942F08072C3F8FC200A6842F0B2 +:1042000001020A602022DA7783F82200704700BF9B +:1042100000ED00E00003FA05001000E010B53023C7 +:1042200083F311880E4B5B6813F4006314D0F1EE36 +:10423000103AEFF30984683C4FF08073E361094B57 +:10424000DB6B236684F3098800F086FB10B1064B14 +:10425000A36110BD054BFBE783F31188F9E700BFAD +:1042600000ED00E000EF00E0FF020008020300089C +:10427000026843680A43026003B1184770470000B0 +:10428000024A136843F0C0031360704700380140CE +:1042900013B50E4C204600F0BBFA04F114000C4993 +:1042A000009400234FF4807200F078F9094B0A491A +:1042B00000944FF4807204F1380000F0F1F9074ADD +:1042C000074BC4E9172302B010BD00BF643B0020B8 +:1042D000643A00208142000864390020003801401F +:1042E00000B4C404254B30B5002914BF0C461C464D +:1042F000037C012B0FD1224B98420CD1214B1A6E1B +:1043000042F480421A66D3F8802042F48042C3F817 +:104310008020D3F880302268036EC16D846603EB81 +:104320005203B3FBF2F36268150442BF23F00705A2 +:1043300003F0070343EA4503CB60A36843F040035F +:104340004B60E36843F001038B6042F4967343F0E3 +:1043500001030B604FF0FF330B6251050AD512F0D9 +:10436000102209D0B2F1805F14BFFF233F2380F8F1 +:10437000643030BDFF23FAE77F23F8E74456000896 +:10438000643B0020001002402DE9F047C66D3768FD +:10439000F56935622A07044618D045F30001C5F3D4 +:1043A000C00301F0200141EA0321AB0748BF41F0FF +:1043B0004001680748BF41F08001302383F3118832 +:1043C000201DFFF755FF002383F31188EA050AD566 +:1043D000302383F311884FF48061201DFFF748FFDD +:1043E000002383F311884FF030084FF0000A0BE0F0 +:1043F00088F31188B38C94F864102046194000F0BB +:1044000021FA8AF31188F56915F02009F0D13B06ED +:1044100016D54FF0300A04F13808280610D58AF373 +:104420001188404600F070F9002829DA0821201D83 +:10443000FFF71EFF27F080033360002383F311880A +:10444000790614D56A0612D5302383F31188D4E98E +:1044500013239A4208D1236C33B11021201D27F079 +:104460004007FFF705FF3760002383F31188EB0651 +:104470000CD5A26E13694BB1BDE8F04750691847DF +:1044800080B2308589F31188F569C6E7BDE8F08709 +:104490004FF0E023002258684FF0FF31930003F102 +:1044A000604303F5614301329042C3F88010C3F8C2 +:1044B0008011F3D27047000000F1604303F56143BF +:1044C0000901C9B283F80013012200F01F039A40CA +:1044D00043099B0003F1604303F56143C3F8802166 +:1044E0001A607047F8B5154682680669AA420B46FD +:1044F000816938BF8568761AB54204460BD22A46D0 +:104500001846FDF76FFCA2692A44A368A2615B1BF1 +:10451000A3602846F8BD0CD918463246FDF762FC68 +:10452000AF1B3A46E1683044FDF75CFCE368DA19FA +:10453000EBE72A461846FDF755FCE268E5E7000080 +:10454000836893422DE9F0411546044638BF8568DB +:10455000D0E904703F1ABD420E460BD22A46FDF741 +:1045600041FC62692A44A36862615B1BA360284620 +:10457000BDE8F0810DD93A46A5EB0708FDF732FCFE +:104580004246E068F119FDF72DFCE36803EB0802F1 +:10459000E9E72A46FDF726FCE268E4E710B5C0E942 +:1045A00005110024C160C3611144029B8460C0E90D +:1045B00000000161036210BD08B5D0E905329342E5 +:1045C00001D1826882B98268013282605A1C4261DC +:1045D0001970D0E904329A4224BFC36843610021B4 +:1045E00000F062FA002008BD4FF0FF30FBE700004A +:1045F00070B5302304460D4683F31188A668A6B132 +:10460000A368A269013BA360531CA36115782269CA +:10461000934224BFE368A361E3690BB12046984746 +:10462000002383F31188284607E02946204600F03E +:104630002BFA0028E2DA86F3118870BD2DE9F74FD6 +:1046400005460F4614469946D0F81CA04FF0300896 +:1046500088F311884FF0000B164666B13246394692 +:104660002846FFF73FFF034660B94946284600F059 +:104670000BFA0028F2D0002383F31188A01B03B0AB +:10468000BDE8F08FBAF1000F03D001902846D04763 +:10469000019B8BF31188F61A1F4488F31188DCE71D +:1046A000C0E90511C160C3611144009B8260C0E98B +:1046B0000000016103627047F8B504460D461646D6 +:1046C000302383F31188A768A7B1A368013BA360D7 +:1046D00063695A1C62611D70D4E904329A4224BF96 +:1046E000E3686361E3690BB120469847002080F3DB +:1046F000118807E03146204600F0C6F90028E2DACA +:1047000087F31188F8BD0000D0E905239A4210B55F +:1047100001D182687AB98268013282605A1C826152 +:104720001C7803699A4224BFC3688361002100F0AA +:10473000BBF9204610BD4FF0FF30FBE72DE9F74FE6 +:1047400005460F4614469946D0F81CA04FF0300895 +:1047500088F311884FF0000B164666B13246394691 +:104760002846FFF7EDFE034660B94946284600F0AB +:104770008BF90028F2D0002383F31188A01B03B02B +:10478000BDE8F08FBAF1000F03D001902846D04762 +:10479000019B8BF31188F61A1F4488F31188DCE71C +:1047A000026843680A43026003B11847704700007B +:1047B0001430FFF743BF00004FF0FF331430FFF712 +:1047C0003DBF00003830FFF7B9BF00004FF0FF33A6 +:1047D0003830FFF7B3BF00001430FFF709BF000007 +:1047E0004FF0FF311430FFF703BF00003830FFF700 +:1047F00063BF00004FF0FF323830FFF75DBF0000AD +:10480000012914BF6FF0130000207047FFF740BD6F +:10481000044B03600023C0E90233436001230374A7 +:10482000704700BF5C56000810B53023044683F380 +:104830001188FFF757FD02232374002080F31188AD +:1048400010BD000038B5C36904460D461BB90421EC +:104850000844FFF7A5FF294604F11400FFF7ACFE5A +:10486000002806DA201D4FF40061BDE83840FFF74C +:1048700097BF38BD00230375826803691B68996878 +:104880009142FBD25A6803604260106058607047E2 +:1048900000230375826803691B6899689142FBD8FD +:1048A0005A680360426010605860704708B5084657 +:1048B000302383F311880B7D032B05D0042B0DD0FF +:1048C0002BB983F3118808BD8B6900221A604FF061 +:1048D000FF338361FFF7CEFF0023F2E7D1E9003217 +:1048E00013605A60F3E70000FFF7C4BF38B50A4B06 +:1048F000DD681C68287522681A605360DC60A36854 +:1049000001229342227501D100F01CFC2946204669 +:10491000BDE83840FBF7DEBCE03D002030B50C4B75 +:10492000DD684B1C87B004460FD02B46094A684609 +:1049300000F04CF92046FFF7D9FF009B13B1684601 +:1049400000F04EF9A86907B030BDFFF7CFFFF9E7D7 +:10495000E03D0020AD480008044B1A68DB68906811 +:104960009B68984294BF002001207047E03D0020E2 +:1049700038B50B4B1C68DD6822681A605360012251 +:104980002275DC60AB68934201D100F0DDFB284664 +:10499000FFF77EFF01462046BDE83840FBF79ABC92 +:1049A000E03D002038B5074C0749084801230025A1 +:1049B0002370656000F006FC0223237085F31188E4 +:1049C00038BD00BF383E002084560008E03D00207E +:1049D00008B572B6044B186500F0C2FA00F070FB1F +:1049E000024B03221A70FEE7E03D0020383E002013 +:1049F00000F02CB98B60022308618B820846704757 +:104A00008368A3F1840243F8142C026943F8442C10 +:104A1000426943F8402C094A43F8242CC26843F801 +:104A2000182C022203F80C2C002203F80B2C044A49 +:104A300043F8102CA3F12000704700BFED020008DE +:104A4000E03D002008B5FFF7DBFFBDE80840FFF7B9 +:104A50004BBF0000024BDB6898610F20FFF746BF99 +:104A6000E03D0020302383F31188FFF7F3BF0000FF +:104A700008B50146302383F311880820FFF74EFF65 +:104A8000002383F3118808BD064BDB6839B1426807 +:104A900018605A60136043600420FFF73FBF4FF077 +:104AA000FF307047E03D002003460068834205D098 +:104AB00002681A6053608161FFF716BF70470000FB +:104AC00010B503689C68A2420CD85C688A600B60D1 +:104AD0004C60216059609968891A99604FF0FF33E2 +:104AE000836010BD1B68121BECE700000A2938BF69 +:104AF0000A2170B504460D460A26601900F05CFBD9 +:104B000000F048FB041BA54203D8751C2E46044642 +:104B1000F3E70A2E04D9BDE87040012000F092BBF3 +:104B200070BD0000F8B5144B1446D961826003F1E2 +:104B3000100242600A2C1A69026038BF0A245060D1 +:104B4000186108190D4600F029FB0A2700F022FB26 +:104B5000431BA342064606D37C1C281900F02CFBFD +:104B600027463546F2E70A2F04D9BDE8F840012070 +:104B700000F068BBF8BD00BFE03D002070B50D46F9 +:104B8000064600F007FB0F490B4653F8102F9A42D8 +:104B900006D12A4601463046BDE87040FFF7C2BF45 +:104BA000CB69C41A2C19936828BF2C46A34202D99A +:104BB0002946FFF79BFF224631460348BDE8704077 +:104BC000FFF77EBFE03D0020F03D002010B4C0E9BB +:104BD000032300235DF8044B4361FFF7CFBF0000C0 +:104BE00010B5194C236998420DD0D0E90032816884 +:104BF00013605A609A680A449A60002303604FF079 +:104C0000FF33A36110BD2346026843F8102F5360A1 +:104C10000022026022699A4203D1BDE8104000F0F0 +:104C2000C5BA936881680B44936000F0B3FA2269B7 +:104C3000E1699268441AA242E4D91144BDE81040E7 +:104C4000091AFFF753BF00BFE03D00202DE9F047F0 +:104C5000DFF8B88008F110072B4ED8F8105000F09C +:104C600099FAD8F81C40AA68011B8A423DD814441E +:104C7000D5E90032C8F81C4013605A6000232B604D +:104C8000D8F81020B242994601D100F08FFA89F38A +:104C90001188D5E9033128469847302383F31188DA +:104CA0006B69002BD9D000F075FA6A69A0EB040992 +:104CB0004A4582460DD2022000F0C4FA0022D8F8FC +:104CC0001030B34208D151462846BDE8F047FFF7FF +:104CD00029BF2244121AF2E712EB090938BF4A46EB +:104CE00029463846FFF7ECFEB6E7D8F81010B14277 +:104CF00008D01444211AC8F81C00A960BDE8F04788 +:104D0000FFF7F4BEBDE8F087F03D0020E03D002055 +:104D100038B500F03FFA054AD2E90845031B1819D7 +:104D200045F10001C2E9080138BD00BFE03D0020A7 +:104D300000207047FEE70000704700004FF0FF3092 +:104D400070470000BFF34F8F024A1369DB03FCD4A6 +:104D5000704700BF0020024008B5094B1B7873B9AB +:104D6000FFF7F0FF074B5A69002ABFBF064A9A6057 +:104D700002F188329A601A6822F480621A6008BDD3 +:104D8000503E0020002002402301674508B50B4B30 +:104D90001B7893B9FFF7D6FF094B5A6942F00042DE +:104DA0005A611A6842F480521A601A6822F48052DA +:104DB0001A601A6842F480621A6008BD503E0020F2 +:104DC000002002407F289ABF00F58030C0020020FA +:104DD000704700004FF4006070470000802070476B +:104DE0007F2808B50ED8FFF7EDFF0368013309D11E +:104DF00000F500630430834201D1012008BD026840 +:104E00000132F7D00020F9E77F2810B504461FD8FB +:104E1000FFF798FFFFF7A0FF0E4BF3221A61022263 +:104E20005A615A6942EAC0025A615A6942F48032B0 +:104E30005A61FFF787FF4FF40061FFF7C3FF00F0EF +:104E40003BF9FFF7A3FF2046BDE81040FFF7C8BFBE +:104E5000002010BD0020024040EA02035B072DE95C +:104E6000F041144606D02E4B4FF461721A600020B8 +:104E7000BDE8F0812B4B82189A420FD9284B40F2A3 +:104E80008932F3E702680B689A420AD1031D1B6856 +:104E90004A68934205D1083C08300831072CF1D804 +:104EA00064B1FFF759FFFFF74DFF1F4E083101278F +:104EB0004FF00008012C03D8FFF768FF0120D7E767 +:104EC0007761054651F8083C45F8043B51F8043C2D +:104ED0004360FFF737FF336943F001033361C6F8DE +:104EE0001480026851F8083C9A420DD00C4B40F2F5 +:104EF000B1321A600D4B18600D4B1C600D4B083918 +:104F00001960FFF743FFB2E72D6851F8042C954272 +:104F100001F10803EAD1083C08301946CAE700BF8E +:104F20004C3E00200000040800200240483E0020C3 +:104F3000443E0020403E0020084908B50B7828B1C7 +:104F40001BB9FFF709FF01230B7008BD002BFCD034 +:104F5000BDE808400870FFF719BF00BF503E0020B1 +:104F600002484FF47F4100F0A9B800BF00010020C3 +:104F700008461146FBF726BA0120FBF723BA0000CA +:104F80000846FBF741BA000038B5EFF311859DB92B +:104F9000EFF30584C4F30804302334B183F311889C +:104FA000FFF7B6FE85F3118838BD83F31188FFF74C +:104FB000AFFE84F3118838BDBDE83840FFF7A8BEC6 +:104FC00038B5FFF7E1FF104C104AC00840EA4170C5 +:104FD000A0FB025EC908A0FB040C1CEB05004FF00F +:104FE0000003A1FB04455B410019A1FB021043EB48 +:104FF000050311EB0E0140F100005B1840F10000C9 +:105000001B0943EA007038BDCFF753E3A59BC420CA +:1050100010B50244064BD2B2904200D110BD441CE0 +:1050200000B253F8200041F8040BE0B2F4E700BFEF +:1050300050280040114B30B5D3F89040240409D4D7 +:10504000D3F89040C3F89040D3F8904044F4004423 +:10505000C3F890400A4C236843F4807323600244F1 +:10506000084BD2B2904200D130BD441C00B251F87E +:10507000045B43F82050E0B2F4E700BF00100240A8 +:10508000007000405028004007B5012201A900200F +:10509000FFF7BEFF019803B05DF804FB13B50446AB +:1050A000FFF7F2FFA04205D0012201A900200194E0 +:1050B000FFF7C0FF02B010BD70470000704700004E +:1050C00070470000074B45F255521A6003225A60A0 +:1050D00040F2FF729A604CF6CC421A60024B0122F9 +:1050E0001A70704700300040513E0020034B1B787F +:1050F0001BB1034B4AF6AA221A607047513E0020AA +:1051000000300040054B1A6832B902F1804202F5C6 +:105110000432D2F894201A60704700BF543E002039 +:10512000024B4FF40002C3F8942070470010024075 +:1051300008B5FFF7E7FF024B1868C0F3407008BDE1 +:10514000543E002070470000704700007047000088 +:10515000FEE700000A4B0B480B4A90420BD30B4B67 +:10516000DA1C121AC11E22F00302994288BF0022E3 +:105170000021FCF743BE53F8041B40F8041BECE786 +:1051800010570008583E0020583E0020583E00208E +:1051900000F0C4B84FF08043586A70474FF0804326 +:1051A000002258631A610222DA6070474FF0804390 +:1051B0000022DA60704700004FF080435863704768 +:1051C000FEE7000070B51B4B01630025044686B066 +:1051D00085620E465860FEF7FDFF04F11003C4E936 +:1051E00004334FF0FF33C4E90635C4E90044A56039 +:1051F000E562FFF7CFFF2B460246C4E9082304F11E +:1052000034010D4A256580232046FFF7F3FB012377 +:10521000E0600A4A0375009272680192B268CDE9B3 +:105220000223074B6846CDE90435FFF70BFC06B0B7 +:1052300070BD00BF383E0020E9550008EE5500085B +:10524000C1510008024AD36A0343D362704700BFCA +:10525000E03D0020274B2849DA6A42F0070210B4EB +:10526000DA62DA6A22F00702DA62DA6ADA6C42F0AB +:105270000702DA64DA6E42F00702DA664FF0904213 +:10528000DB6E4FF0AA30002353604FF41924906076 +:10529000D1604FF6FF71516113625462174C146074 +:1052A000174CC2F80434C2F808444FF4A044C2F8C2 +:1052B0000C444FF6FD74C2F814444FF0EE44C2F8AB +:1052C00020444AF20404C2F82434C2F80044C2F86C +:1052D0000438C2F80808C2F80C38C2F81418C2F82A +:1052E0002038C2F82438C2F800385DF8044B00F0CA +:1052F00055B800BF00100240000100240001802AC0 +:10530000AAFAAAAA08B500F02BF9FFF74BFBBDE8F3 +:105310000840FFF7F7BE0000704700000F4B9A6D82 +:1053200042F001029A659A6F42F001029A670C4AB4 +:105330009B6F936843F0010393604FF080434F22CB +:105340009A624FF0FF32DA6200229A615A63DA60A1 +:105350005A6001225A611A60704700BF0010024073 +:10536000002004E04FF0804208B51169D3680B407B +:10537000D9B2C9439B07116107D5302383F3118844 +:10538000FFF736FB002383F3118808BD08B5FFF74C +:105390007FF8BDE8084000F0D7B800005B4B4FF045 +:1053A000FF319A6A99629A6A00229A62986AD86A68 +:1053B00060F00700D862D86A00F00700D862D86AA7 +:1053C000186B1963186B1A63186B986B9963986B59 +:1053D0009A63986BD86BD963D86BDA63D86B186C07 +:1053E0001964196C1A641A6C494A4FF4E06111602F +:1053F0001A6E484942F001021A66D3F8802022F062 +:105400000102C3F88020D3F880209A6D42F08052C8 +:105410009A659A6F22F080529A679A6F4FF44072A1 +:105420000A604A6912F48062FBD14A601A6822F06D +:10543000F00242F060021A601A6842F001021A603B +:105440001A689107FCD500229A609A6812F00C0243 +:10545000FBD1D3F8901011F4407F1EBF4FF4803180 +:10546000C3F89010C3F89020D3F8902042F01902AE +:10547000C3F89020D3F890209207FBD565221A60DC +:105480001A689007FCD500229A609A6812F00C0FF7 +:10549000FBD16D221A60D3F8942022F4706242F49A +:1054A000C062C3F89420D3F8902022F00322C3F8FE +:1054B0009020194ADA601A6842F080721A601A68FD +:1054C0009101FCD5154A1A611A6842F080621A608F +:1054D0001A681201FCD500229A600D49104AC3F8DF +:1054E00088200A6822F0070242F004020A600A6873 +:1054F00002F00702042AFAD19A6842F003029A6085 +:105500009A6802F00C020C2AFAD17047001002408F +:105510000020024000700040012800010018100027 +:105520005555013408B50348FEF72EFFBDE8084085 +:10553000FEF774BE643B002008B5FFF713FFBDE81B +:105540000840FEF76BBE000008B507211C20FEF7DF +:10555000B3FFBDE808400C212520FEF7ADBF0000D9 +:1055600008B5FFF713FF00F009F8FFF74FF9FFF751 +:10557000D3FEBDE80840FFF70BBE000070470000F7 +:105580000B4601461846FAF729BF00001211000029 +:1055900012130000121000006F72672E61726475A2 +:1055A00070696C6F742E3344522D4C3433312D415D +:1055B00053415541560053544D33324C343F3F0014 +:1055C00040A2E4F1646891060041A3E5F2656992A6 +:1055D00007004261642043414E496661636520696A +:1055E0006E6465782E006330006D61696E006964D9 +:1055F0006C6500000000000000000000D1260008DB +:10560000A12E0008012E0008012700080D27000820 +:1056100019290008D9260008E1260008DD2600081F +:1056200035280008E5260008B93100085531000882 +:10563000F52600080928000880000000008000000E +:105640000000800000960000000000000000000044 +:10565000000000000000000000000000000000004A +:10566000CD470008B9470008F5470008E1470008A2 +:10567000ED470008D9470008C5470008B1470008B2 +:1056800001480008E6550008D03B0020E03D00201E +:1056900024B3FF7F0100000004110020000000007F +:1056A0000000000068000000D000000000000000C2 +:1056B00000000000000000000000000000000000EA +:1056C00000000000000000000000000000000000DA +:1056D00000000000000000000000000000000000CA +:1056E00000000000000000008D0400000000000029 +:1056F0000000030000000000FE2A0100D2040000A8 +:10570000FF000000643B002000000000B6550008C8 +:00000001FF diff --git a/Tools/bootloaders/Atlas-Control_bl.bin b/Tools/bootloaders/Atlas-Control_bl.bin new file mode 100755 index 0000000000000..1cd4eefffa430 Binary files /dev/null and b/Tools/bootloaders/Atlas-Control_bl.bin differ diff --git a/Tools/bootloaders/Atlas-Control_bl.hex b/Tools/bootloaders/Atlas-Control_bl.hex new file mode 100644 index 0000000000000..82bee35b15424 --- /dev/null +++ b/Tools/bootloaders/Atlas-Control_bl.hex @@ -0,0 +1,1462 @@ +:020000040800F2 +:1000000000060020E1020008E3020008E302000805 +:10001000E3020008E3020008E3020008E30200082C +:10002000E3020008E3020008E302000819320008B6 +:10003000E3020008E3020008E3020008E30200080C +:10004000E3020008E3020008E3020008E3020008FC +:10005000E3020008E3020008E3020008E3020008EC +:10006000E3020008E3020008E3020008C1110008EF +:10007000E911000815120008411200086D1200086D +:1000800095120008C1120008E3020008E30200080C +:10009000E3020008E3020008E3020008E3020008AC +:1000A000E3020008E3020008E3020008E30200089C +:1000B0001D4A0008E3020008E3020008E30200080A +:1000C000E3020008E3020008E3020008E30200087C +:1000D000E3020008E3020008E3020008E30200086C +:1000E000E3020008E3020008E3020008E30200085C +:1000F000E3020008E3020008E3020008ED12000832 +:10010000E3020008F5490008E3020008E3020008E2 +:10011000E3020008E3020008E3020008E30200082B +:1001200019130008411300086D1300089913000803 +:10013000C5130008E3020008E3020008E302000818 +:10014000E3020008E3020008E3020008E3020008FB +:10015000ED1300081914000845140008E302000814 +:10016000E3020008E3020008E3020008E3020008DB +:10017000E3020008053E0008E3020008E30200086D +:10018000E3020008E3020008094A0008E30200084D +:10019000E3020008E3020008E3020008E3020008AB +:1001A000E3020008E3020008E3020008E30200089B +:1001B000E3020008E3020008E3020008E30200088B +:1001C000E3020008E3020008E3020008E30200087B +:1001D000E3020008F13D0008E3020008E302000822 +:1001E000E3020008E3020008E3020008E30200085B +:1001F000E3020008E3020008E3020008E30200084B +:10020000E3020008E3020008E3020008E30200083A +:10021000E3020008E3020008E3020008E30200082A +:10022000E3020008E3020008E3020008E30200081A +:10023000E3020008E3020008E3020008E30200080A +:10024000E3020008E3020008E3020008E3020008FA +:10025000E3020008E3020008E3020008E3020008EA +:10026000E3020008E3020008E3020008E3020008DA +:10027000E3020008E3020008E3020008E3020008CA +:10028000E3020008E3020008E3020008E3020008BA +:10029000E3020008E3020008E3020008E3020008AA +:1002A000E3020008E3020008E3020008E30200089A +:1002B000E3020008E3020008E3020008E30200088A +:1002C000E3020008E3020008E3020008E30200087A +:1002D000E3020008E3020008E3020008E30200086A +:1002E00002E000F000F8FEE772B6374880F30888B5 +:1002F000364880F3098836483649086040F20000E5 +:10030000CCF200004EF63471CEF200010860BFF36B +:100310004F8FBFF36F8F40F20000C0F2F0004EF637 +:100320008851CEF200010860BFF34F8FBFF36F8F8B +:100330004FF00000E1EE100A4EF63C71CEF20001E3 +:100340000860062080F31488BFF36F8F02F0C6FFA9 +:1003500004F008F94FF055301F491B4A91423CBF49 +:1003600041F8040BFAE71D49184A91423CBF41F895 +:10037000040BFAE71A491B4A1B4B9A423EBF51F83D +:10038000040B42F8040BF8E700201849184A914280 +:100390003CBF41F8040BFAE702F0DEFF04F066F917 +:1003A000144C154DAC4203DA54F8041B8847F9E7A6 +:1003B00000F042F8114C124DAC4203DA54F8041B21 +:1003C0008847F9E702F0C6BF00060020002200209F +:1003D0000000000808ED00E00000002000060020FA +:1003E000D85A000800220020642200206822002041 +:1003F000E44A0020E0020008E0020008E0020008F1 +:10040000E00200082DE9F04F2DED108AC1F80CD064 +:10041000D0F80CD0BDEC108ABDE8F08F002383F338 +:1004200011882846A047002002F05EF9FEE702F09E +:10043000E3F800DFFEE7000038B500F04DFC00F007 +:1004400011FE02F093FE054602F0D8FE0446C0B944 +:100450000E4B9D4217D001339D4241F2883512BFA9 +:10046000044600250124002002F08AFE0CB100F0B1 +:1004700073F800F095FD284600F01CF900F06CF8C8 +:10048000F9E70025EFE70546EDE700BF010007B0FB +:1004900008B500F0EDFBA0F120035842584108BD1B +:1004A00007B5042101900DEB010000F0FFFB03B044 +:1004B0005DF804FB38B5302383F3118817480368CF +:1004C0000BB102F0F5F90023154A4FF47A71134885 +:1004D00002F0E4F9002383F31188124C236813B16E +:1004E0002368013B2360636813B16368013B636069 +:1004F0000D4D2B7833B963687BB9022000F0B2FC54 +:10050000322363602B78032B07D163682BB9022059 +:1005100000F0A8FC4FF47A73636038BD68220020B5 +:10052000B50400088823002080220020084B1870A2 +:1005300003280CD8DFE800F008050208022000F0CC +:1005400083BC022000F070BC024B00225A6070474E +:100550008022002088230020F8B5504B504A1C46CA +:100560001968013100F0998004339342F8D1626830 +:100570004C4B9A4240F291804B4B9B6803F10063D5 +:1005800003F500339A4280F08880002000F0BEFB23 +:100590000220FFF7CBFF454B0021D3F8E820C3F83A +:1005A000E810D3F81021C3F81011D3F81021D3F8B4 +:1005B000EC20C3F8EC10D3F81421C3F81411D3F8CD +:1005C0001421D3F8F020C3F8F010D3F81821C3F8A1 +:1005D0001811D3F81821D3F8802042F00062C3F834 +:1005E0008020D3F8802022F00062C3F88020D3F866 +:1005F0008020D3F8802042F00072C3F88020D3F826 +:10060000802022F00072C3F88020D3F8803072B6C8 +:100610004FF0E023C3F8084DD4E90004BFF34F8F37 +:10062000BFF36F8F224AC2F88410BFF34F8F536914 +:1006300023F480335361BFF34F8FD2F8803043F6F9 +:10064000E076C3F3C905C3F34E335B0103EA060C3E +:1006500029464CEA81770139C2F87472F9D2203BFD +:1006600013F1200FF2D1BFF34F8FBFF36F8FBFF3A2 +:100670004F8FBFF36F8F536923F40033536100230F +:10068000C2F85032BFF34F8FBFF36F8F302383F325 +:100690001188854680F308882047F8BD00000208CD +:1006A00020000208FFFF0108002200200044025839 +:1006B00000ED00E02DE9F04F95B0DFF8D892202250 +:1006C000FF21029004A8D9F8085000F013FCA84AB2 +:1006D0001378A3B90121A74811700360302383F375 +:1006E000118803680BB102F0E3F80023A24A4FF42B +:1006F0007A71A04802F0D2F8002383F31188029B9C +:1007000013B19E4B029A1A609D4A1378032B03D0B3 +:1007100000231370994A53604FF0000B029C5E4611 +:10072000DA46CDF804B0012000F08EFB24B1934BE3 +:100730001B68002B00F02882002000F099FAB0F12D +:100740000008F3DB012000F06FFBA8F121031F2B51 +:10075000E9D801A252F823F0D9070008F5070008EC +:100760008308000827070008270700082707000854 +:100770000D090008FF0A0008F10900084D0A0008E9 +:10078000730A0008990A000827070008AB0A000846 +:10079000CF0A0008470B0008610800082707000877 +:1007A0008F0B0008E507000861080008270700080C +:1007B0004D0A000827070008270700082707000838 +:1007C0002707000827070008270700082707000851 +:1007D00027070008830800080220FFF759FE0028B9 +:1007E00040F0FF81029B02216648BBF1000F08BF69 +:1007F0001C4640E04FF47A7000F03AFA071EF1DB35 +:100800000220FFF745FE0028ECD0013F052F00F243 +:10081000EA81DFE807F003070A0D10330520FFF730 +:100820003FFE14E0D9F80000F9E7D9F80400F6E734 +:10083000D9F80800F3E74FF01C08404608F1040817 +:1008400000F066FAFFF72CFEB8F12C0FF5D101206D +:10085000019B4FF0000A00FA07F71F43FBB2019318 +:1008600000F094FB2EB1019B03F00B030B2B08BF90 +:1008700000240221444800F019FA54E7D9F80C008A +:10088000CDE7002EAED0019B03F00B030B2BA9D1BB +:100890000220FFF7FDFD07460028A3D00120002617 +:1008A00000F034FA0220FFF741FE1FFA86FB58469B +:1008B00000F03CFA044688B1A8F1400258460136DF +:1008C0005142514100F042FA0028EED1BB460446A5 +:1008D00002212E483E4600F0E9F924E72546012092 +:1008E000FFF724FED9F80830AB4207D9284600F0BC +:1008F0000FFA013040F07A810435F3E70025204BF0 +:10090000BB463E461D701D4B5D60A9E7002E3FF4BF +:1009100069AF019B03F00B030B2B7FF463AF022045 +:10092000FFF704FE322000F0A3F9B0F10008FFF653 +:1009300059AF18F003077FF455AF08EB0503D9F85A +:10094000082093423FF64EAFB8F5807F3FF74AAF9D +:100950000F4BB84503931FDD4FF47A7000F088F910 +:100960000028FFF63FAF039B013703F8010BF0E7C8 +:100970008423002068220020B5040008882300207A +:1009800080220020145600080C560008105600085B +:100990008422002000220020C820FFF779FD0746AE +:1009A00000283FF41FAF1F2D11D8C5F1200204AB62 +:1009B00025F003008F494245184428BF4246039260 +:1009C00000F072FA039AFF218A4800F093FA4FEA86 +:1009D000A803C8F3870287492846039300F092FAD8 +:1009E000064600283FF474AF039B05EB830537E709 +:1009F0000220FFF74DFD00283FF4F4AE00F0C4F9EB +:100A000000283FF4EFAE0027B846D9F80830BB42C3 +:100A100018D91F2F11D8049B01330ED027F00303E0 +:100A200014AA134453F8403C0C93404604220CA9EA +:100A3000043700F0AFFB8046E7E7384600F068F97E +:100A40000C90F2E74046FFF72BFD09E700236421F5 +:100A50000CA80C9300F020F900287FF4C3AE02200C +:100A6000FFF716FD00283FF4BDAE0C9800F082F9A8 +:100A7000E9E7002364210CA80C9300F00DF900288D +:100A80007FF4B0AE0220FFF703FD00283FF4AAAECA +:100A90000C9800F071F9D6E70220FFF7F9FC002866 +:100AA0003FF4A0AE00F080F9CDE70220FFF7F0FCA4 +:100AB00000283FF497AE0CA9142000F07BF90390B6 +:100AC000FFF7EEFC03990CA800F0F0F8C8E602204E +:100AD000FFF7DEFC00283FF485AE474B0CAF03F177 +:100AE000100C1868083353F8041C3A46634503C2D7 +:100AF0001746F6D11020FFF7D3FC1021E3E7322090 +:100B000000F0B6F8071EFFF66DAEBB077FF46AAEC5 +:100B100007EB0A03D9F8082093423FF663AE0220A0 +:100B2000FFF7B6FC00283FF45DAE27F003075744FB +:100B3000BA453FF495AE50460AF1040A00F0E8F8D1 +:100B4000FFF7AEFCF4E74FF47A70FFF7A1FC002842 +:100B50003FF448AE00F018F9002849D0049B013357 +:100B60000BD0082204A9002000F0CCF900283FD0C7 +:100B70002022FF2104A800F0BDF92048022100F046 +:100B800095F81F4801F0B6FD15B0BDE8F08F002EB6 +:100B90003FF428AE019B03F00B030B2B7FF422AE36 +:100BA000002364210CA80C9300F076F80746002877 +:100BB0007FF418AE0220FFF76BFC804600283FF45C +:100BC00011AE02210D4800F071F841F2883001F0B9 +:100BD00091FD0C9800F024FA46463C4600F0D6F908 +:100BE000A1E506463CE64FF0000AFBE5BB466FE692 +:100BF00037466DE684220020F85500080C560008A0 +:100C0000A08601002DE9F84F4FF47A7306460D4691 +:100C1000002402FB03F7DFF85080DFF8509098F9CA +:100C200000305FFA84FA5A1C01D0A34212D159F85D +:100C300024002A4631460368D3F820B03B46D84703 +:100C4000854207D1074B012083F800A0BDE8F88F4B +:100C50000124E4E7002CFBD04FF4FA7001F04AFDC8 +:100C60000020F3E7D42300201022002014220020CB +:100C7000002307B5024601210DF107008DF807306A +:100C8000FFF7C0FF20B19DF8070003B05DF804FB3B +:100C90004FF0FF30F9E700000A46042108B5FFF7DE +:100CA000B1FF80F00100C0B2404208BD074B0A46C8 +:100CB00030B41978064B53F821400146236820468A +:100CC000DD69044BAC4630BC604700BFD423002034 +:100CD00014220020A086010070B50A4E00240A4D9F +:100CE00002F03CF8308028683388834208D902F04B +:100CF00031F82B6804440133B4F5003F2B60F2D384 +:100D000070BD00BFD62300209023002002F000B960 +:100D100000F1006000F500300068704700F10060ED +:100D2000920000F5003002F075B80000054B1A681B +:100D3000054B1B889B1A834202D9104402F00AB863 +:100D40000020704790230020D623002038B50446A9 +:100D5000074D29B128682044BDE8384002F012B898 +:100D60002868204401F0FCFF0028F3D038BD00BF04 +:100D7000902300200020704700F1FF5000F58F10F5 +:100D8000D0F8000870470000064991F8243033B1CC +:100D900000230822086A81F82430FFF7BFBF012032 +:100DA000704700BF94230020014B1868704700BFB4 +:100DB0000010005C194B01380322084470B51D680F +:100DC000174BC5F30B042D0C1E88A6420BD15C6893 +:100DD0000A46013C824213460FD214F9016F4EB10C +:100DE00002F8016BF6E7013A03F10803ECD1814206 +:100DF0000B4602D22C2203F8012B0424094A168840 +:100E0000AE4204D1984284BF967803F8016B013C4E +:100E100002F10402F3D1581A70BD00BF0010005C4B +:100E20001C22002044560008022804D1054B4FF430 +:100E300080029A6170470128FCD1024B4FF40002F6 +:100E4000F7E700BF00200258022803D1044B4022DC +:100E50009A6170470128FCD1014B8022F8E700BF5E +:100E600000200258022805D1064A536983F0400346 +:100E7000536170470128FCD1024A536983F0800313 +:100E8000F6E700BF0020025870B504464FF47A76AA +:100E90004CB1412C254628BF412506FB05F0641BBB +:100EA00001F028FCF4E770BD002310B5934203D095 +:100EB000CC5CC4540133F9E710BD0000013810B513 +:100EC00010F9013F3BB191F900409C4203D11AB1A6 +:100ED0000131013AF4E71AB191F90020981A10BDD6 +:100EE0001046FCE703460246D01A12F9011B0029FE +:100EF000FAD1704702440346934202D003F8011B23 +:100F0000FAE770472DE9F8431F4D14460746884617 +:100F100095F8242052BBDFF870909CB395F82430EC +:100F20002BB92022FF2148462F62FFF7E3FF95F8F7 +:100F300024004146C0F1080205EB8000A24228BF10 +:100F40002246D6B29200FFF7AFFF95F82430A41BDB +:100F500017441E449044E4B2F6B2082E85F824608B +:100F6000DBD1FFF711FF0028D7D108E02B6A03EB94 +:100F700082038342CFD0FFF707FF0028CBD10020A8 +:100F8000BDE8F8830120FBE794230020024B1A7888 +:100F9000024B1A70704700BFD4230020102200209B +:100FA00038B51A4C1A4D204600F0BEFE29462046A0 +:100FB00000F0E6FE2D684FF47A70D5F89020D2F854 +:100FC000043843F00203C2F80438FFF75DFF11490B +:100FD000284600F0E3FFD5F890200F4DD2F80438F2 +:100FE000286823F002030D49A042C2F804384FF4E8 +:100FF000E1330B6001D000F0F5FD6868A04204D039 +:101000000649BDE8384000F0EDBD38BD382B002062 +:10101000405800084858000814220020BC23002033 +:101020000C4B70B50C4D04461E780C4B55F8262021 +:101030009A420DD00A4B002118221846FFF75AFF9A +:101040000460014655F82600BDE8704000F0CABDB6 +:1010500070BD00BFD423002014220020382B0020B4 +:10106000BC2300202DE9F04771B6464B01201D68D6 +:10107000454BB3F8008000231A46194600F08AFA5F +:101080000446802001F0E2FF0746002862D0002CD1 +:1010900060D03E4EADB24FF0800A7369284643F0EF +:1010A000C0637361D6F8143143F0C063C6F81431DD +:1010B00001F050FE05EB080200FB05F34FF4D06C85 +:1010C00050434FF03D0E984233D93269520144D417 +:1010D000D6F81021510140D4216803F100628A60E2 +:1010E0002268D7602268C2F804A02268C2F800C053 +:1010F000D4F80090D9F8002042F00102C9F800208D +:10110000D4F80090D9F8002022F01E02C9F800207F +:10111000D4F80090D9F80020D207FBD494F80C90B2 +:10112000803361680EFA09F20A60CCE7002001F012 +:10113000EFFE204600F06EFA384601F0C1FF134B77 +:101140005A6942F0C0625A61D3F8142142F0C06279 +:10115000C3F8142161B6BDE8F0870D4B9A6A42F4DA +:1011600000729A629A6A42F400429A6261B600265C +:10117000012001F0CDFEB045D8D9A819013601F003 +:1011800001FEF8E790230020D62300200020005223 +:1011900000ED00E030B50A44084D91420DD011F841 +:1011A000013B5840082340F30004013B2C4013F05E +:1011B000FF0384EA5000F6D1EFE730BD2083B8ED9D +:1011C00008B5074B074A196801F03D01996053685B +:1011D0000BB190689847BDE8084002F055B800BFD1 +:1011E00000000240D823002008B5084B196889097F +:1011F00001F03D018A019A60054AD3680BB110697C +:101200009847BDE8084002F03FB800BF0000024028 +:10121000D823002008B5084B1968090C01F03D01DE +:101220000A049A60054A53690BB190699847BDE872 +:10123000084002F029B800BF00000240D823002077 +:1012400008B5084B1968890D01F03D018A059A60BF +:10125000054AD3690BB1106A9847BDE8084002F00F +:1012600013B800BF00000240D823002008B5074B88 +:10127000074A596801F03D01D960536A0BB1906A81 +:101280009847BDE8084001F0FFBF00BF00000240E2 +:10129000D823002008B5084B5968890901F03D01A1 +:1012A0008A01DA60054AD36A0BB1106B9847BDE832 +:1012B000084001F0E9BF00BF00000240D823002031 +:1012C00008B5084B5968090C01F03D010A04DA60C1 +:1012D000054A536B0BB1906B9847BDE8084001F08D +:1012E000D3BF00BF00000240D823002008B5084B40 +:1012F0005968890D01F03D018A05DA60054AD36B12 +:101300000BB1106C9847BDE8084001F0BDBF00BFAD +:1013100000000240D823002008B5074B074A19688F +:1013200001F03D019960536C0BB1906C9847BDE89A +:10133000084001F0A9BF00BF00040240D8230020EC +:1013400008B5084B1968890901F03D018A019A60C6 +:10135000054AD36C0BB1106D9847BDE8084001F009 +:1013600093BF00BF00040240D823002008B5084BFB +:101370001968090C01F03D010A049A60054A536D91 +:101380000BB1906D9847BDE8084001F07DBF00BFEC +:1013900000040240D823002008B5084B1968890DC5 +:1013A00001F03D018A059A60054AD36D0BB1106EBC +:1013B0009847BDE8084001F067BF00BF0004024045 +:1013C000D823002008B5074B074A596801F03D01B2 +:1013D000D960536E0BB1906E9847BDE8084001F09C +:1013E00053BF00BF00040240D823002008B5084BBB +:1013F0005968890901F03D018A01DA60054AD36E16 +:101400000BB1106F9847BDE8084001F03DBF00BF29 +:1014100000040240D823002008B5084B5968090C85 +:1014200001F03D010A04DA60054A536F0BB1906F79 +:101430009847BDE8084001F027BF00BF0004024004 +:10144000D823002008B5084B5968890D01F03D01EB +:101450008A05DA60054AD36F13B1D2F88000984745 +:10146000BDE8084001F010BF00040240D82300206E +:1014700000230C4910B51A460B4C0B6054F823009E +:10148000026001EB430004334260402BF6D1074A6F +:101490004FF0FF339360D360C2F80834C2F80C34C5 +:1014A00010BD00BFD82300205456000800000240A1 +:1014B0000F28F8B510D9102810D0112811D01228F3 +:1014C00008D10F240720DFF8C8E00126DEF800501D +:1014D000A04208D9002653E00446F4E70F24002078 +:1014E000F1E70724FBE706FA00F73D424AD1264C14 +:1014F0004FEA001C3D4304EB00160EEBC000CEF893 +:101500000050C0E90123FBB273B12048D0F8D830B5 +:1015100043F00103C0F8D830D0F8003143F00103A4 +:10152000C0F80031D0F8003117F47F4F0ED01748C3 +:10153000D0F8D83043F00203C0F8D830D0F80031EA +:1015400043F00203C0F80031D0F8003154F80C0029 +:10155000036823F01F030360056815F00105FBD144 +:1015600004EB0C033D2493F80CC05F6804FA0CF400 +:101570003C6021240560446112B1987B00F066F95B +:101580003046F8BD0130A3E7545600080044025825 +:10159000D823002010B5302484F31188FFF788FF8A +:1015A000002383F3118810BD10B50446807B00F042 +:1015B00063F901231549627B03FA02F20B6823EAFF +:1015C0000203DAB20B6072B9114AD2F8D81021F0D6 +:1015D0000101C2F8D810D2F8001121F00101C2F8BF +:1015E0000011D2F8002113F47F4F0ED1084BD3F82D +:1015F000D82022F00202C3F8D820D3F8002122F02C +:101600000202C3F80021D3F8003110BDD823002016 +:101610000044025808B5302383F31188FFF7C4FF54 +:10162000002383F3118808BD0268436811430160F9 +:1016300003B1184770470000024A136843F0C00323 +:10164000136070470078004013B50E4C204600F040 +:10165000BDFA04F1140000234FF400720A4900940B +:1016600000F07AF9094B4FF40072094904F138008F +:10167000009400F0F3F9074A074BC4E9172302B0BE +:1016800010BD00BF5C240020C824002039160008CB +:10169000C82600200078004000E1F505037C30B545 +:1016A000244C002918BF0C46012B11D1224B984223 +:1016B0000ED1224BD3F8E82042F08042C3F8E82054 +:1016C000D3F8102142F08042C3F81021D3F8103132 +:1016D0002268036EC16D03EB52038466B3FBF2F321 +:1016E0006268150442BF23F0070503F0070343EACD +:1016F0004503CB60A36843F040034B60E36843F0CD +:1017000001038B6042F4967343F001030B604FF0CA +:10171000FF330B62510505D512F0102205D0B2F14E +:10172000805F04D080F8643030BD7F23FAE73F2328 +:10173000F8E700BF545700085C240020004402581A +:101740002DE9F047C66D05463768F4692107346214 +:101750001AD014F0080118BF4FF48071E20748BF97 +:1017600041F02001A3074FF0300348BF41F0400192 +:10177000600748BF41F0800183F31188281DFFF7FF +:1017800053FF002383F31188E2050AD5302383F346 +:1017900011884FF48061281DFFF746FF002383F373 +:1017A00011884FF030094FF0000A14F0200838D1AA +:1017B0003B0616D54FF0300905F1380A200610D542 +:1017C00089F31188504600F07DF9002836DA0821A7 +:1017D000281DFFF729FF27F080033360002383F3E0 +:1017E0001188790614D5620612D5302383F3118847 +:1017F000D5E913239A4208D12B6C33B127F0400767 +:101800001021281DFFF710FF3760002383F3118894 +:10181000E30618D5AA6E1369ABB15069BDE8F0476D +:10182000184789F31188736A284695F8641019409F +:1018300000F0E6F98AF31188F469B6E7B06288F33C +:101840001188F469BAE7BDE8F0870000090100F1EA +:101850006043012203F56143C9B283F8001300F02D +:101860001F039A4043099B0003F1604303F5614362 +:10187000C3F880211A60704700F01F0301229A40CC +:10188000430900F160409B0000F5614003F16043B3 +:1018900003F56143C3F88020C3F88021002380F85A +:1018A00000337047F8B51546826804460B46AA42D5 +:1018B00000D28568A1692669761AB5420BD218460E +:1018C0002A46FFF7F1FAA3692B44A3612846A368CF +:1018D0005B1BA360F8BD0CD9AF1B18463246FFF75F +:1018E000E3FA3A46E1683044FFF7DEFAE3683B4446 +:1018F000EBE718462A46FFF7D7FAE368E5E700006A +:1019000083689342F7B50446154600D28568D4E94A +:101910000460361AB5420BD22A46FFF7C5FA63694E +:101920002B4463612846A3685B1BA36003B0F0BD32 +:101930000DD93246AF1B0191FFF7B6FA01993A462D +:10194000E0683144FFF7B0FAE3683B44E9E72A4630 +:10195000FFF7AAFAE368E4E710B50A440024C3617C +:10196000029B8460C16002610362C0E90000C0E9BB +:10197000051110BD08B5D0E90532934201D1826846 +:1019800082B98268013282605A1C4261197000215A +:10199000D0E904329A4224BFC368436100F0DEFEFE +:1019A000002008BD4FF0FF30FBE7000070B530238A +:1019B00004460E4683F31188A568A5B1A368A26901 +:1019C000013BA360531CA36115782269934224BF95 +:1019D000E368A361E3690BB120469847002383F3D2 +:1019E0001188284607E03146204600F0A7FE00286F +:1019F000E2DA85F3118870BD2DE9F74F04460E46F3 +:101A000017469846D0F81C904FF0300A8AF3118898 +:101A10004FF0000B154665B12A4631462046FFF7C8 +:101A200041FF034660B94146204600F087FE00288A +:101A3000F1D0002383F31188781B03B0BDE8F08F49 +:101A4000B9F1000F03D001902046C847019B8BF3EA +:101A50001188ED1A1E448AF31188DCE7C160C36166 +:101A6000009B82600362C0E905111144C0E90000D7 +:101A700001617047F8B504460D461646302383F3DE +:101A80001188A768A7B1A368013BA36063695A1CCA +:101A900062611D70D4E904329A4224BFE368636135 +:101AA000E3690BB120469847002080F3118807E0D6 +:101AB0003146204600F042FE0028E2DA87F3118822 +:101AC000F8BD0000D0E9052310B59A4201D1826823 +:101AD0007AB982680021013282605A1C82611C78C6 +:101AE00003699A4224BFC368836100F037FE204631 +:101AF00010BD4FF0FF30FBE72DE9F74F04460E46CF +:101B000017469846D0F81C904FF0300A8AF3118897 +:101B10004FF0000B154665B12A4631462046FFF7C7 +:101B2000EFFE034660B94146204600F007FE00285C +:101B3000F1D0002383F31188781B03B0BDE8F08F48 +:101B4000B9F1000F03D001902046C847019B8BF3E9 +:101B50001188ED1A1E448AF31188DCE70268436895 +:101B60001143016003B11847704700001430FFF7BC +:101B700043BF00004FF0FF331430FFF73DBF0000BC +:101B80003830FFF7B9BF00004FF0FF333830FFF7B0 +:101B9000B3BF00001430FFF709BF00004FF0FF3162 +:101BA0001430FFF703BF00003830FFF763BF0000B9 +:101BB0004FF0FF323830FFF75DBF0000012914BF3E +:101BC0006FF0130000207047FFF73EBD044B036029 +:101BD00000234360C0E9023301230374704700BF50 +:101BE0006C57000810B53023044683F31188FFF7C3 +:101BF00055FD02230020237480F3118810BD0000DE +:101C000038B5C36904460D461BB904210844FFF7E3 +:101C1000A5FF294604F11400FFF7ACFE002806DA00 +:101C2000201D4FF40061BDE83840FFF797BF38BD75 +:101C3000026843681143016003B118477047000010 +:101C400013B5406B00F58054D4F8A4381A681178A5 +:101C5000042914D1017C022911D119790123128997 +:101C60008B4013420BD101A94C3002F0FDF9D4F89E +:101C7000A4480246019B2179206800F0DFF902B0F8 +:101C800010BD0000143002F07FB900004FF0FF33A8 +:101C9000143002F079B900004C3002F051BA000063 +:101CA0004FF0FF334C3002F04BBA0000143002F01A +:101CB0004DB900004FF0FF31143002F047B9000079 +:101CC0004C3002F01DBA00004FF0FF324C3002F0F1 +:101CD00017BA00000020704710B500F58054D4F802 +:101CE000A4381A681178042917D1017C022914D16B +:101CF0005979012352898B4013420ED1143002F0DE +:101D0000DFF8024648B1D4F8A4484FF44073617933 +:101D10002068BDE8104000F07FB910BD406BFFF7B0 +:101D2000DBBF0000704700007FB5124B0125042681 +:101D3000044603600023057400F1840243602946D1 +:101D4000C0E902330C4B0290143001934FF44073FE +:101D5000009602F091F8094B04F69442294604F1EA +:101D60004C000294CDE900634FF4407302F058F93F +:101D700004B070BD945700081D1D0008411C0008E8 +:101D80000A68302383F311880B790B3342F8230060 +:101D90004B79133342F823008B7913B10B3342F89C +:101DA000230000F58053C3F8A41802230374002015 +:101DB00080F311887047000038B5037F044613B1E3 +:101DC00090F85430ABB90125201D0221FFF730FFF8 +:101DD00004F114006FF00101257700F0CBFC04F151 +:101DE0004C0084F854506FF00101BDE8384000F019 +:101DF000C1BC38BD10B5012104460430FFF718FFFF +:101E00000023237784F8543010BD000038B5044611 +:101E10000025143002F048F804F14C00257702F058 +:101E200017F9201D84F854500121FFF701FF2046C7 +:101E3000BDE83840FFF750BF90F8803003F06003F2 +:101E4000202B06D190F881200023212A03D81F2AB5 +:101E500006D800207047222AFBD1C0E91D3303E0D9 +:101E6000034A426707228267C3670120704700BFA9 +:101E70003422002037B500F58055D5F8A4381A680B +:101E8000117804291AD1017C022917D1197901236B +:101E900012898B40134211D100F14C04204602F00C +:101EA00097F958B101A9204602F0DEF8D5F8A44808 +:101EB0000246019B2179206800F0C0F803B030BDD4 +:101EC00001F10B03F0B550F8236085B004460D46D0 +:101ED000FEB1302383F3118804EB8507301D082100 +:101EE000FFF7A6FEFB6806F14C005B691B681BB19F +:101EF000019002F0C7F8019803A902F0B5F8024674 +:101F000048B1039B2946204600F098F8002383F34C +:101F1000118805B0F0BDFB685A691268002AF5D037 +:101F20001B8A013B1340F1D104F18002EAE7000073 +:101F3000133138B550F82140ECB1302383F31188C8 +:101F400004F58053D3F8A4281368527903EB820375 +:101F5000DB689B695D6845B104216018FFF768FE86 +:101F6000294604F1140001F0B5FF2046FFF7B4FE46 +:101F7000002383F3118838BD7047000001F082BA56 +:101F800001234022002110B5044600F8303BFEF743 +:101F9000B1FF0023C4E9013310BD000010B53023A8 +:101FA000044683F311882422416000210C30FEF79F +:101FB000A1FF204601F088FA02230020237080F35D +:101FC000118810BD70B500EB8103054650690E46BF +:101FD0001446DA6018B110220021FEF78BFFA069C9 +:101FE00018B110220021FEF785FF31462846BDE8D2 +:101FF000704001F06FBB000083682022002103F0D5 +:10200000011310B5044683601030FEF773FF2046BD +:10201000BDE8104001F0EABBF0B4012500EB8104FB +:1020200047898D40E4683D43A469458123600023CE +:10203000A2606360F0BC01F007BC0000F0B40125B1 +:1020400000EB810407898D40E4683D4364690581A4 +:1020500023600023A2606360F0BC01F07DBC00003F +:1020600070B5022300250446242203702946C0F8D7 +:1020700088500C3040F8045CFEF73CFF204684F8A2 +:10208000705001F0BBFA63681B6823B129462046F3 +:10209000BDE87040184770BD0378052B10B50446A5 +:1020A0000AD080F88C300523037043681B680BB19D +:1020B000042198470023A36010BD00000178052982 +:1020C00006D190F88C20436802701B6803B1184752 +:1020D0007047000070B590F87030044613B10023CB +:1020E00080F8703004F18002204601F0A3FB6368A1 +:1020F0009B68B3B994F8803013F0600535D00021A7 +:10210000204601F095FE0021204601F085FE63681F +:102110001B6813B1062120469847062384F87030C7 +:1021200070BD204698470028E4D0B4F88630A26FEE +:102130009A4288BFA36794F98030A56F002B4FF0B7 +:10214000300380F20381002D00F0F280092284F830 +:10215000702083F3118800212046D4E91D23FFF766 +:102160006DFF002383F31188DAE794F8812003F0F0 +:102170007F0343EA022340F20232934200F0C5801B +:1021800021D8B3F5807F48D00DD8012B3FD0022B4A +:1021900000F09380002BB2D104F188026267022222 +:1021A000A267E367C1E7B3F5817F00F09B80B3F5D9 +:1021B000407FA4D194F88230012BA0D1B4F88830AC +:1021C00043F0020332E0B3F5006F4DD017D8B3F5FA +:1021D000A06F31D0A3F5C063012B90D8636820466F +:1021E00094F882205E6894F88310B4F88430B04785 +:1021F000002884D0436863670368A3671AE0B3F5D7 +:10220000106F36D040F6024293427FF478AF5C4BB9 +:1022100063670223A3670023C3E794F88230012B8E +:102220007FF46DAFB4F8883023F00203A4F888304F +:10223000C4E91D55E56778E7B4F88030B3F5A06FC1 +:102240000ED194F88230204684F88A3001F034FAB6 +:1022500063681B6813B1012120469847032323704C +:102260000023C4E91D339CE704F18B03636701235A +:10227000C3E72378042B10D1302383F31188204641 +:10228000FFF7BAFE85F311880321636884F88B5049 +:1022900021701B680BB12046984794F88230002BC0 +:1022A000DED084F88B300423237063681B68002B16 +:1022B000D6D0022120469847D2E794F884302046B1 +:1022C0001D0603F00F010AD501F0A6FA012804D07B +:1022D00002287FF414AF2B4B9AE72B4B98E701F0C1 +:1022E0008DFAF3E794F88230002B7FF408AF94F86E +:1022F000843013F00F01B3D01A06204602D501F046 +:10230000AFFDADE701F0A0FDAAE794F88230002B05 +:102310007FF4F5AE94F8843013F00F01A0D01B06C3 +:10232000204602D501F084FD9AE701F075FD97E79C +:10233000142284F8702083F311882B462A462946FC +:102340002046FFF769FE85F31188E9E65DB11522A5 +:1023500084F8702083F3118800212046D4E91D23DE +:10236000FFF75AFEFDE60B2284F8702083F31188F4 +:102370002B462A4629462046FFF760FEE3E700BFCA +:10238000C4570008BC570008C057000838B590F87B +:1023900070300446002B3ED0063BDAB20F2A34D808 +:1023A0000F2B32D8DFE803F03731310822323131D8 +:1023B0003131313131313737856FB0F886309D4258 +:1023C00014D2C3681B8AB5FBF3F203FB12556DB937 +:1023D000302383F311882B462A462946FFF72EFE29 +:1023E00085F311880A2384F870300EE0142384F8F2 +:1023F0007030302383F31188002320461A46194693 +:10240000FFF70AFE002383F3118838BDC36F03B1C1 +:1024100098470023E7E70021204601F009FD00214D +:10242000204601F0F9FC63681B6813B106212046C1 +:1024300098470623D7E7000010B590F8703004469F +:10244000142B29D017D8062B05D001D81BB110BDED +:10245000093B022BFBD80021204601F0E9FC0021BA +:10246000204601F0D9FC63681B6813B106212046A1 +:102470009847062319E0152BE9D10B2380F870301B +:10248000302383F3118800231A461946FFF7D6FD3F +:10249000002383F31188DAE7C3689B695B68002B2C +:1024A000D5D1C36F03B19847002384F87030CEE7CD +:1024B00000238268037503691B6899689142FBD207 +:1024C0005A68036042601060586070470023826859 +:1024D000037503691B6899689142FBD85A680360C9 +:1024E000426010605860704708B50846302383F397 +:1024F00011880A7D0023052A06D8DFE802F00B05C3 +:102500000503120E826913604FF0FF338361FFF7FA +:10251000CFFF002383F3118808BD826993680133DC +:102520009360D0E9003213605A60EDE7FFF7C0BF57 +:1025300038B50A4B1C68DD682875226853601A603C +:102540000122DC60A3682275934201D100F060FE95 +:1025500029462046BDE83840FDF754BFC828002072 +:102560000C4B30B5DD684B1C87B004460FD02B46B2 +:10257000094A684600F092F92046FFF7D9FF009B10 +:1025800013B1684600F094F9A86907B030BDFFF7B1 +:10259000CFFFF9E7C8280020E924000870B50F4EE6 +:1025A00004468161F36881689A68914203D8BDE866 +:1025B0007040FFF77DBF1846FFF788FF8368054628 +:1025C000012B01D100F026FE012329462046F460AC +:1025D0002375BDE87040FDF715BF00BFC828002077 +:1025E000044B1A68DB6890689B68984294BF00208F +:1025F00001207047C828002038B50B4B1C68DD68E7 +:10260000226853601A600122DC602275AB68934235 +:1026100001D100F0FFFD2846FFF758FF0146204694 +:10262000BDE83840FDF7EEBEC828002038B50123CC +:10263000084C00252370656001F000FE01F026FEC5 +:102640000549064801F0FAFE0223237085F311883C +:1026500038BD00BF302B0020CC570008C828002010 +:1026600000F05CB9034A516853685B1A9842FBD882 +:10267000704700BF001000E08B600223086108462D +:102680008B8270478368A3F1840243F8142C02699B +:1026900043F8442C426943F8402C094A43F8242C5F +:1026A000C268A3F1200043F8182C022203F80C2C76 +:1026B000002203F80B2C034A43F8102C704700BF8C +:1026C0001D040008C828002008B5FFF7DBFFBDE89F +:1026D0000840FFF72BBF0000024BDB6898610F201A +:1026E000FFF726BFC8280020302383F31188FFF7A7 +:1026F000F3BF000008B50146302383F3118808209A +:10270000FFF72EFF002383F3118808BD054BDB681C +:1027100021B1036098610320FFF722BF4FF0FF3023 +:10272000704700BFC828002003682BB10022026058 +:1027300018469961FFF7FABE70470000064BDB6848 +:1027400039B1426818605A60136043600420FFF793 +:1027500007BF4FF0FF307047C82800200368984239 +:1027600006D01A680260506018469961FFF7DEBE15 +:102770007047000038B504460D462068844200D1F9 +:1027800038BD036823605C608561FFF7CFFEF4E726 +:10279000036810B59C68A2420CD85C688A600B6024 +:1027A0004C602160596099688A1A9A604FF0FF3333 +:1027B000836010BD121B1B68ECE700000A2938BFBC +:1027C0000A2170B504460D460A26601901F032FD53 +:1027D00001F01EFD041BA54203D8751C04462E46BD +:1027E000F3E70A2E04D90120BDE8704001F066BE6F +:1027F00070BD0000F8B5144B0D460A2A4FF00A07C9 +:10280000D96103F11001826038BF0A224160196961 +:102810001446016048601861A81801F0FDFC01F041 +:10282000F7FC431B0646A34206D37C1C2819274607 +:10283000354601F0FFFCF2E70A2F04D90120BDE87C +:10284000F84001F03BBEF8BDC8280020F8B50646A8 +:102850000D4601F0DDFC0F4A134653F8107F9F42EE +:1028600006D12A4601463046BDE8F840FFF7C2BF10 +:10287000D169BB68441A2C1928BF2C46A34202D93F +:102880002946FFF79BFF224631460348BDE8F84042 +:10289000FFF77EBFC8280020D8280020C0E9032306 +:1028A000002310B45DF8044B4361FFF7CFBF000075 +:1028B00010B5194C236998420DD08168D0E90032D7 +:1028C00013605A609A680A449A60002303604FF0CC +:1028D000FF33A36110BD0268234643F8102F5360F5 +:1028E0000022026022699A4203D1BDE8104001F043 +:1028F0009BBC936881680B44936001F089FC22695A +:10290000E1699268441AA242E4D91144BDE810403A +:10291000091AFFF753BF00BFC82800202DE9F04770 +:10292000DFF8BC8008F110072C4ED8F8105001F0E9 +:102930006FFCD8F81C40AA68031B9A423ED8144486 +:102940004FF00009D5E90032C8F81C4013605A6006 +:10295000C5F80090D8F81030B34201D101F064FC02 +:1029600089F31188D5E9033128469847302383F34A +:1029700011886B69002BD8D001F04AFC6A69A0EB82 +:10298000040982464A450DD2022001F097FD00223B +:10299000D8F81030B34208D151462846BDE8F04778 +:1029A000FFF728BF121A2244F2E712EB0909294661 +:1029B000384638BF4A46FFF7EBFEB5E7D8F8103087 +:1029C000B34208D01444C8F81C00211AA960BDE81D +:1029D000F047FFF7F3BEBDE8F08700BFD82800201E +:1029E000C828002000207047FEE700007047000064 +:1029F0004FF0FF307047000002290CD0032904D0AB +:102A00000129074818BF00207047032A05D8054848 +:102A100000EBC2007047044870470020704700BFB9 +:102A2000A4580008442200205858000870B59AB0F5 +:102A300005460846144601A900F0C2F801A8FEF7B1 +:102A400051FA431C0022C6B25B001046C5E90034AF +:102A500023700323023404F8013C01ABD1B20234E9 +:102A60008E4201D81AB070BD13F8011B013204F870 +:102A7000010C04F8021CF1E708B5302383F3118838 +:102A80000348FFF7F7F9002383F3118808BD00BF5F +:102A9000382B002090F8803003F01F02012A07D164 +:102AA00090F881200B2A03D10023C0E91D3315E0E3 +:102AB00003F06003202B08D1B0F884302BB990F8D4 +:102AC0008120212A03D81F2A04D8FFF7B5B9222A6A +:102AD000EBD0FAE7034A426707228267C367012007 +:102AE000704700BF3B22002007B5052917D8DFE853 +:102AF00001F0191603191920302383F31188104AA5 +:102B000001210190FFF75EFA019802210D4AFFF7BB +:102B100059FA0D48FFF77AF9002383F3118803B0BF +:102B20005DF804FB302383F311880748FFF744F96D +:102B3000F2E7302383F311880348FFF75BF9EBE7F3 +:102B4000F85700081C580008382B002038B50C4DE9 +:102B50000C4C2A460C4904F10800FFF767FF05F109 +:102B6000CA0204F110000949FFF760FF05F5CA72B7 +:102B700004F118000649BDE83840FFF757BF00BF11 +:102B80001044002044220020D8570008E2570008D3 +:102B9000ED57000870B5044608460D46FEF7A2F949 +:102BA000C6B22046013403780BB9184670BD3246D0 +:102BB0002946FEF783F90028F3D10120F6E700004B +:102BC0002DE9F04705460C46FEF78CF92B49C6B2B5 +:102BD0002846FFF7DFFF08B10636F6B22849284637 +:102BE000FFF7D8FF08B11036F6B2632E0BD8DFF826 +:102BF0008C80DFF88C90234FDFF894A02E7846B9B4 +:102C00002670BDE8F08729462046BDE8F04702F06F +:102C1000EBBC252E2ED1072241462846FEF74EF961 +:102C200070B9194B224603F10C0153F8040B8B4287 +:102C300042F8040BF9D11B7807350D341370DDE72A +:102C4000082249462846FEF739F998B9A21C0F4BCD +:102C5000197802320909C95D02F8041C13F8011B36 +:102C600001F00F015345C95D02F8031CF0D118347F +:102C70000835C3E7013504F8016BBFE7C458000805 +:102C8000ED570008DA580008CC58000800E8F11F9A +:102C90000CE8F11FBFF34F8F044B1A695107FCD1A9 +:102CA000D3F810215207F8D1704700BF002000521E +:102CB00008B50D4B1B78ABB9FFF7ECFF0B4BDA688F +:102CC000D10704D50A4A5A6002F188325A60D3F813 +:102CD0000C21D20706D5064AC3F8042102F1883236 +:102CE000C3F8042108BD00BF6E460020002000523A +:102CF0002301674508B5114B1B78F3B9104B1A69CE +:102D0000510703D5DA6842F04002DA60D3F81021A7 +:102D1000520705D5D3F80C2142F04002C3F80C212C +:102D2000FFF7B8FF064BDA6842F00102DA60D3F829 +:102D30000C2142F00102C3F80C2108BD6E460020B0 +:102D4000002000520F289ABF00F580604004002048 +:102D5000704700004FF400307047000010207047AB +:102D60000F2808B50BD8FFF7EDFF00F50033026818 +:102D7000013204D104308342F9D1012008BD002082 +:102D8000FCE700000F2838B505463FD8FFF782FF63 +:102D90001F4CFFF78DFF4FF0FF3307286361C4F826 +:102DA00014311DD82361FFF775FF030243F024039C +:102DB000E360E36843F08003E36023695A07FCD4CF +:102DC0002846FFF767FFFFF7BDFF4FF4003100F023 +:102DD000E5F92846FFF78EFFBDE83840FFF7C0BF92 +:102DE000C4F81031FFF756FFA0F108031B0243F0AF +:102DF0002403C4F80C31D4F80C3143F08003C4F838 +:102E00000C31D4F810315B07FBD4D9E7002038BD72 +:102E1000002000522DE9F84F04460D46104644EAC2 +:102E20000203DE0602D00020BDE8F88F20F01F006C +:102E3000DFF8D4B0DFF8D4A0FFF73AFF05EB0008C5 +:102E4000454503D10120FFF755FFEDE72022294634 +:102E5000204602F0B9FB10B920342035F0E72346B4 +:102E600004F120021F68791CDDD104339342F9D1AB +:102E700004F178432048214EB3F5801F204B38BF22 +:102E8000184603F1F80332BFD946D1461E46FFF774 +:102E900001FF0760A4EB050C336805F11C0143F04A +:102EA000020333602B1FD9F8007017F00507FAD121 +:102EB00053F8042F8B424CF80320F4D1BFF34F8F0B +:102EC000FFF7E8FE4FF0FF330360336823F002039F +:102ED0003360BFF34F8F0B4BC3F85C42BFF34F8F90 +:102EE000BFF36F8F20222946204602F06DFB002899 +:102EF000B2D03846A7E700BF142100520C20005280 +:102F00001420005200ED00E0102000521021005269 +:102F100010B5084C237828B11BB9FFF7C9FE01236F +:102F2000237010BD002BFCD02070BDE81040FFF7CF +:102F3000E1BE00BF6E4600202DE9F04F0D4685B082 +:102F4000814658B111F00D0614BF2022082211F05D +:102F50000803019304D0431E03426AD0002435E0E5 +:102F6000002E37D009F11F0121F01F094FF0010891 +:102F7000324F05F00403DFF8D0A005EA080BBBF1DF +:102F8000000F32D07869C0072FD408F101080C3740 +:102F9000B8F1060FF3D19EB9294D4946A8190192FF +:102FA00001F096F9044600283AD11836019A782E95 +:102FB000F3D1494601F08CF90446002830D1019A3A +:102FC0004946204801F084F9044668BB204605B014 +:102FD000BDE8F08F0029C9D101462846029201F0D0 +:102FE00077F90446E0B9029AC0E713B1786941075E +:102FF000CBD5AC0702D578698007C6D5019911B148 +:1030000078690107C1D51820494600FB08A0CDE921 +:10301000022301F05DF90446DDE902230028B4D063 +:103020004A460021204601E04A460021FDF762FFA2 +:10303000CCE70246002E95D198E700BFEC58000877 +:10304000A046002070460020884600200121FFF79E +:1030500073BF0000F8B500231A461A600433B3F5B5 +:10306000806FFAD1114D01241827114E07FB04601F +:1030700001342A6955F80C1F01F016F9062CF5D118 +:1030800037254FF4C0542046FFF7E0FF014628B132 +:1030900022460848BDE8F84001F006B9C4EBC40474 +:1030A000013D4FEAD404EED1F8BD00BFEC58000852 +:1030B00088460020704600200421FFF73DBF000035 +:1030C00008B101F077B9704770B5AAB140EA0103C1 +:1030D00013F01F030FD1094C0144A5686D0706D5F5 +:1030E0002568A84203D366683544A94204D903334E +:1030F0000C34122BF1D10022104670BDEC580008A0 +:103100000244074BD2B210B5904200D110BD441C0E +:1031100000B253F8200041F8040BE0B2F4E700BF1E +:10312000504000580E4B30B51C6F240405D41C6F62 +:103130001C671C6F44F400441C670A4C024423685B +:10314000D2B243F480732360074B904200D130BD6C +:10315000441C51F8045B00B243F82050E0B2F4E79D +:1031600000440258004802585040005807B5012258 +:1031700001A90020FFF7C4FF019803B05DF804FB2C +:1031800013B50446FFF7F2FFA04205D0012201A9C2 +:1031900000200194FFF7C6FF02B010BD0144BFF349 +:1031A0004F8F064B884204D3BFF34F8FBFF36F8F0F +:1031B0007047C3F85C022030F4E700BF00ED00E088 +:1031C0000144BFF34F8F064B884204D3BFF34F8FA8 +:1031D000BFF36F8F7047C3F870022030F4E700BF71 +:1031E00000ED00E0034B1A681AB9034AD2F8D02464 +:1031F0001A607047184700200040025808B5FFF7D2 +:10320000F1FF024B1868C0F3806008BD184700202A +:103210007047000070470000EFF3098305496833E9 +:103220004A6B22F001024A6383F30988002383F387 +:103230001188704700EF00E0302080F3118862B6FB +:103240000D4B0E4AD96821F4E0610904090C0A43C8 +:103250000B49DA60D3F8FC2042F08072C3F8FC20FE +:10326000084AC2F8B01F116841F001011160202224 +:10327000DA7783F82200704700ED00E00003FA05DA +:1032800055CEACC5001000E0302310B583F3118893 +:103290000E4B5B6813F4006314D0F1EE103AEFF3B9 +:1032A00009844FF08073683CE361094BDB6B236654 +:1032B00084F30988FFF794F910B1064BA36110BDA0 +:1032C000054BFBE783F31188F9E700BF00ED00E051 +:1032D00000EF00E02F0400083204000870B5BFF3CF +:1032E0004F8FBFF36F8F1A4A0021C2F85012BFF3FD +:1032F0004F8FBFF36F8F536943F400335361BFF3B4 +:103300004F8FBFF36F8FC2F88410BFF34F8FD2F887 +:10331000803043F6E074C3F3C900C3F34E335B015E +:1033200003EA0406014646EA81750139C2F8605293 +:10333000F9D2203B13F1200FF2D1BFF34F8F536925 +:1033400043F480335361BFF34F8FBFF36F8F70BD72 +:1033500000ED00E0FEE70000214B2248224A70B554 +:10336000904237D3214BC11EDA1C121A22F00302FD +:103370008B4238BF00220021FDF7BCFD1C4A002310 +:10338000C2F88430BFF34F8FD2F8803043F6E07438 +:10339000C3F3C900C3F34E335B0103EA04060146DD +:1033A00046EA81750139C2F86C52F9D2203B13F11B +:1033B000200FF2D1BFF34F8FBFF36F8FBFF34F8F4B +:1033C000BFF36F8F0023C2F85032BFF34F8FBFF3AC +:1033D0006F8F70BD53F8041B40F8041BC0E700BF9B +:1033E0003C5B0008E44A0020E44A0020E44A002054 +:1033F00000ED00E0074BD3F8D81021EA0001C3F834 +:10340000D810D3F8002122EA0002C3F80021D3F833 +:10341000003170470044025870B5D0E924430022BF +:103420004FF0FF359E6804EB42135101D3F80009B9 +:10343000002805DAD3F8000940F08040C3F80009FD +:10344000D3F8000B002805DAD3F8000B40F08040D9 +:10345000C3F8000B013263189642C3F80859C3F849 +:10346000085BE0D24FF00113C4F81C3870BD0000B7 +:10347000890141F02001016103699B06FCD41220FF +:10348000FFF7F0B810B50A4C2046FEF779FD094B5E +:10349000C4F89030084BC4F89430084C2046FEF72E +:1034A0006FFD074BC4F89030064BC4F8943010BD44 +:1034B0001C4700200000084058590008B847002069 +:1034C000000004406459000870B503780546012BDC +:1034D0005CD1434BD0F89040984258D1414B0E21DB +:1034E0006520D3F8D82042F00062C3F8D820D3F882 +:1034F000002142F00062C3F80021D3F80021D3F884 +:10350000802042F00062C3F88020D3F8802022F0AF +:103510000062C3F88020D3F88030FEF797F9324B71 +:10352000E360324BC4F800380023D5F89060C4F84B +:10353000003EC02323604FF40413A3633369002BC0 +:10354000FCDA01230C203361FFF78CF83369DB07C9 +:10355000FCD41220FFF786F83369002BFCDA002632 +:103560002846A660FFF758FF6B68C4F81068DB6850 +:10357000C4F81468C4F81C6883BB1D4BA3614FF0EA +:10358000FF336361A36843F00103A36070BD194B6F +:103590009842C9D1134B4FF08060D3F8D82042F045 +:1035A0000072C3F8D820D3F8002142F00072C3F8AB +:1035B0000021D3F80021D3F8802042F00072C3F834 +:1035C0008020D3F8802022F00072C3F88020D3F846 +:1035D0008030FFF70FFF0E214D209EE7064BCDE711 +:1035E0001C47002000440258401400400300200201 +:1035F000003C30C0B8470020083C30C0F8B5D0F8D7 +:103600009040054600214FF000662046FFF730FF4E +:10361000D5F8941000234FF001128F684FF0FF305F +:10362000C4F83438C4F81C2804EB431201339F4219 +:10363000C2F80069C2F8006BC2F80809C2F8080BAA +:10364000F2D20B68D5F89020C5F898306362102349 +:103650001361166916F01006FBD11220FFF702F86D +:10366000D4F8003823F4FE63C4F80038A36943F4A7 +:10367000402343F01003A3610923C4F81038C4F8B1 +:1036800014380B4BEB604FF0C043C4F8103B094BB0 +:10369000C4F8003BC4F81069C4F80039D5F8983074 +:1036A00003F1100243F48013C5F89820A362F8BD1B +:1036B0003459000840800010D0F8902090F88A100B +:1036C000D2F8003823F4FE6343EA0113C2F800384D +:1036D000704700002DE9F84300EB8103D0F89050CB +:1036E0000C468046DA680FFA81F94801166806F040 +:1036F0000306731E022B05EB41134FF0000194BF2C +:10370000B604384EC3F8101B4FF0010104F110034A +:1037100098BF06F1805601FA03F3916998BF06F548 +:10372000004600293AD0578A04F15801374349012D +:103730006F50D5F81C180B430021C5F81C382B1806 +:103740000127C3F81019A7405369611E9BB3138A60 +:10375000928B9B08012A88BF5343D8F89820981869 +:1037600042EA034301F140022146C8F89800284686 +:1037700005EB82025360FFF77BFE08EB8900C3680C +:103780001B8A43EA845348341E4364012E51D5F802 +:103790001C381F43C5F81C78BDE8F88305EB4917B2 +:1037A000D7F8001B21F40041C7F8001BD5F81C18FE +:1037B00021EA0303C0E704F13F030B4A28462146F0 +:1037C00005EB83035A60FFF753FE05EB4910D0F871 +:1037D000003923F40043C0F80039D5F81C3823EA37 +:1037E0000707D7E70080001000040002D0F89420FB +:1037F0001268C0F89820FFF70FBE00005831D0F8CB +:10380000903049015B5813F4004004D013F4001FBA +:103810000CBF0220012070474831D0F89030490198 +:103820005B5813F4004004D013F4001F0CBF0220B7 +:103830000120704700EB8101CB68196A0B681360A7 +:103840004B6853607047000000EB810330B5DD68C2 +:10385000AA691368D36019B9402B84BF4023136051 +:103860006B8A1468D0F890201C4402EB4110013C94 +:1038700009B2B4FBF3F46343033323F0030343EAD5 +:10388000C44343F0C043C0F8103B2B6803F003036C +:10389000012B0ED1D2F8083802EB411013F4807FCF +:1038A000D0F8003B14BF43F0805343F00053C0F8FE +:1038B000003B02EB4112D2F8003B43F00443C2F854 +:1038C000003B30BD2DE9F041D0F8906005460C4634 +:1038D00006EB4113D3F8087B3A07C3F8087B08D5F9 +:1038E000D6F814381B0704D500EB8103DB685B684E +:1038F0009847FA071FD5D6F81438DB071BD505EB18 +:103900008403D968CCB98B69488A5A68B2FBF0F64F +:1039100000FB16228AB91868DA6890420DD2121A92 +:10392000C3E90024302383F3118821462846FFF79A +:103930008BFF84F31188BDE8F081012303FA04F2C0 +:103940006B8923EA02036B81CB68002BF3D02146FD +:103950002846BDE8F041184700EB81034A0170B5E5 +:10396000DD68D0F890306C692668E66056BB1A4472 +:103970004FF40020C2F810092A6802F00302012A5D +:103980000AB20ED1D3F8080803EB421410F4807F7A +:10399000D4F8000914BF40F0805040F00050C4F843 +:1039A000000903EB4212D2F8000940F00440C2F8CB +:1039B00000090122D3F8340802FA01F10143C3F8E7 +:1039C000341870BD19B9402E84BF40202060206893 +:1039D0001A442E8A8419013CB4FBF6F440EAC44030 +:1039E00040F00050C6E700002DE9F843D0F89060A1 +:1039F00005460C464F0106EB4113D3F8088918F031 +:103A0000010FC3F808891CD0D6F81038DB0718D589 +:103A100000EB8103D3F80CC0DCF81430D3F800E0DD +:103A2000DA68964530D2A2EB0E024FF000091A6018 +:103A3000C3F80490302383F31188FFF78DFF89F3D7 +:103A4000118818F0800F1DD0D6F834380126A64012 +:103A5000334217D005EB84030134D5F89050D3F8E6 +:103A60000CC0E4B22F44DCF8142005EB0434D2F887 +:103A700000E05168714514D3D5F8343823EA0606BE +:103A8000C5F83468BDE8F883012303FA01F203891D +:103A900023EA02030381DCF80830002BD1D09847D9 +:103AA000CFE7AEEB0103BCF81000834228BF03460A +:103AB000D7F8180980B2B3EB800FE3D89068A0F173 +:103AC000040959F8048FC4F80080A0EB0908984451 +:103AD000B8F1040FF5D818440B4490605360C8E760 +:103AE0002DE9F84FD0F8905004466E69AB691E403E +:103AF00016F480586E6103D0BDE8F84FFEF7B0BAF7 +:103B0000002E12DAD5F8003E9B0705D0D5F8003E0E +:103B100023F00303C5F8003ED5F80438204623F00F +:103B20000103C5F80438FEF7C9FA370505D5204664 +:103B3000FFF772FC2046FEF7AFFAB0040CD5D5F8BB +:103B4000083813F0060FEB6823F470530CBF43F4EE +:103B5000105343F4A053EB6031071BD56368DB6857 +:103B60001BB9AB6923F00803AB612378052B0CD19B +:103B7000D5F8003E9A0705D0D5F8003E23F00303A0 +:103B8000C5F8003E2046FEF799FA6368DB680BB182 +:103B900020469847F30200F1BA80B70226D5D4F840 +:103BA000909000274FF0010A09EB4712D2F8003B32 +:103BB00003F44023B3F5802F11D1D2F8003B002B42 +:103BC0000DDA62890AFA07F322EA0303638104EB40 +:103BD0008703DB68DB6813B1394620469847013715 +:103BE000D4F89430FFB29B689F42DDD9F00619D516 +:103BF000D4F89000026AC2F30A1702F00F0302F42D +:103C0000F012B2F5802F00F0CA80B2F5402F09D132 +:103C100004EB8303002200F58050DB681B6A9742A7 +:103C200040F0B0803003D5F8185835D5E90303D5F6 +:103C300000212046FFF746FEAA0303D501212046B6 +:103C4000FFF740FE6B0303D502212046FFF73AFE43 +:103C50002F0303D503212046FFF734FEE80203D5E6 +:103C600004212046FFF72EFEA90203D50521204698 +:103C7000FFF728FE6A0203D506212046FFF722FE41 +:103C80002B0203D507212046FFF71CFEEF0103D5C9 +:103C900008212046FFF716FE700340F1A780E907D0 +:103CA00003D500212046FFF79FFEAA0703D5012177 +:103CB0002046FFF799FE6B0703D502212046FFF748 +:103CC00093FE2F0703D503212046FFF78DFEEE0656 +:103CD00003D504212046FFF787FEA80603D505215A +:103CE0002046FFF781FE690603D506212046FFF72F +:103CF0007BFE2A0603D507212046FFF775FEEB055C +:103D000074D520460821BDE8F84FFFF76DBED4F802 +:103D100090904FF0000B4FF0010AD4F894305FFA06 +:103D20008BF79B689F423FF638AF09EB4713D3F8F8 +:103D3000002902F44022B2F5802F20D1D3F80029C7 +:103D4000002A1CDAD3F8002942F09042C3F8002977 +:103D5000D3F80029002AFBDB3946D4F89000FFF79E +:103D600087FB22890AFA07F322EA0303238104EB83 +:103D70008703DB689B6813B13946204698470BF1EF +:103D8000010BCAE7910701D1D0F80080072A02F1A0 +:103D900001029CBF03F8018B4FEA18283FE704EBB0 +:103DA000830300F58050DA68D2F818C0DCF80820E8 +:103DB000DCE9001CA1EB0C0C00218F4208D1DB6870 +:103DC0009B699A683A449A605A683A445A6029E76B +:103DD00011F0030F01D1D0F800808C4501F10101F1 +:103DE00084BF02F8018B4FEA1828E6E7BDE8F88F98 +:103DF00008B50348FFF774FEBDE80840FFF744BA72 +:103E00001C47002008B50348FFF76AFEBDE80840DC +:103E1000FFF73ABAB8470020D0F8903003EB4111D1 +:103E2000D1F8003B43F40013C1F8003B7047000099 +:103E3000D0F8903003EB4111D1F8003943F400136E +:103E4000C1F8003970470000D0F8903003EB411101 +:103E5000D1F8003B23F40013C1F8003B7047000089 +:103E6000D0F8903003EB4111D1F8003923F400135E +:103E7000C1F800397047000030B50433039C01726B +:103E8000002104FB0325C160C0E90653049B0363C2 +:103E9000059BC0E90000C0E90422C0E90842C0E96E +:103EA0000A11436330BD00000022416AC260C0E9CC +:103EB0000411C0E90A226FF00101FEF75BBC0000AB +:103EC000D0E90432934201D1C2680AB9181D704783 +:103ED00000207047036919600021C2680132C26086 +:103EE000C269134482699342036124BF436A036138 +:103EF000FEF734BC38B504460D46E3683BB1626951 +:103F00000020131D1268A3621344E36207E0237AC2 +:103F100033B929462046FEF711FC0028EDDA38BDFA +:103F20006FF00100FBE70000C368C269013BC3609A +:103F30004369134482699342436124BF436A4361E6 +:103F400000238362036B03B11847704770B53023B9 +:103F5000044683F31188866A3EB9FFF7CBFF054616 +:103F600018B186F31188284670BDA36AE26A13F877 +:103F7000015B9342A36202D32046FFF7D5FF0023E3 +:103F800083F31188EFE700002DE9F84F04460E4651 +:103F9000174698464FF0300989F311880025AA4644 +:103FA000D4F828B0BBF1000F09D141462046FFF7F5 +:103FB000A1FF20B18BF311882846BDE8F88FD4E922 +:103FC0000A12A7EB050B521A934528BF9346BBF183 +:103FD000400F1BD9334601F1400251F8040B9142C6 +:103FE00043F8040BF9D1A36A403640354033A3624D +:103FF000D4E90A239A4202D32046FFF795FF8AF3B9 +:104000001188BD42D8D289F31188C9E730465A4693 +:10401000FCF74AFFA36A5E445D445B44A362E7E7A2 +:1040200010B5029C0433017203FB0421C460C0E993 +:1040300006130023C0E90A33039B0363049BC0E912 +:104040000000C0E90422C0E90842436310BD00003B +:10405000026A6FF00101C260426AC0E904220022D4 +:10406000C0E90A22FEF786BBD0E904239A4201D1B7 +:10407000C26822B9184650F8043B0B607047002311 +:104080001846FAE7C3680021C2690133C360436977 +:10409000134482699342436124BF436A4361FEF73C +:1040A0005DBB000038B504460D46E3683BB12369AB +:1040B00000201A1DA262E2691344E36207E0237A3A +:1040C00033B929462046FEF739FB0028EDDA38BD22 +:1040D0006FF00100FBE7000003691960C268013A54 +:1040E000C260C269134482699342036124BF436A78 +:1040F000036100238362036B03B11847704700001C +:1041000070B530230D460446114683F31188866A44 +:104110002EB9FFF7C7FF10B186F3118870BDA36AEF +:104120001D70A36AE26A01339342A36204D3E1697A +:1041300020460439FFF7D0FF002080F31188EDE717 +:104140002DE9F84F04460D46904699464FF0300A47 +:104150008AF311880026B346A76A4FB9494620461C +:10416000FFF7A0FF20B187F311883046BDE8F88F34 +:10417000D4E90A073A1AA8EB0607974228BF174660 +:10418000402F1BD905F1400355F8042B9D4240F800 +:10419000042BF9D1A36A40364033A362D4E90A2341 +:1041A0009A4204D3E16920460439FFF795FF8BF367 +:1041B00011884645D9D28AF31188CDE729463A4677 +:1041C000FCF772FEA36A3D443E443B44A362E5E72C +:1041D000D0E904239A4217D1C3689BB1836A8BB19B +:1041E000043B9B1A0ED01360C368013BC360C369D4 +:1041F0001A4483699A42026124BF436A036100231F +:1042000083620123184670470023FBE700F05EBA83 +:104210004FF08043586A70474FF080430022586344 +:104220001A610222DA6070474FF080430022DA60A0 +:10423000704700004FF0804358637047024B034AB9 +:104240001A60034A5A6070476C480020E84A002010 +:1042500000000220074B494210B55C68201A084054 +:104260001968821A8A4203D3A24201D85A6010BD4B +:104270000020FCE76C48002008B5302383F3118848 +:10428000FFF7E8FF002383F3118808BD04480121EC +:10429000044B03600023C0E901330C3000F016B971 +:1042A0007448002079420008CB1D083A23F0070328 +:1042B000591A521A012110B4D2080024C0E900434F +:1042C00084600C301C605A605DF8044B00F0FEB84E +:1042D0002DE9F84F364ECD1D0F46002818BF064673 +:1042E000082A4FEAD50538BF082206F10C08341D0C +:1042F0009146404600F006F909F10701C9F1000EA8 +:10430000224624686CB9404600F006F93368CBB306 +:1043100008224946E8009847044698B340E90267F6 +:1043200030E004EB010CD4F804A00CEA0E0C0AF106 +:104330000100ACF1080304EBC0009842E0D9A0EB07 +:104340000C0CB5EBEC0F4FEAEC0BD9D89C421CD20D +:1043500004F10802AB45A3EB02024FEAE2026260FD +:1043600009D9691CED43206803EBC1025D44556027 +:1043700043F8310022601C465F60404644F8086BF9 +:1043800000F0CAF82046BDE8F88FAA45216802D19E +:1043900011602346EFE7013504EBC50344F83510FF +:1043A00003F10801401AC01058601360F1E700BF24 +:1043B00074480020F8B550F8043C044650F8085CF6 +:1043C000A0F1080607332F1D0C35DB0840F8043C2C +:1043D000284600F097F83B469F421A6801D0B34246 +:1043E00028D20AB1964225D244F8082C54F8042C5D +:1043F0001E60013254F8081C06EBC200814206D14F +:104400004868024444F8042C0A6844F8082C5868A8 +:10441000411C03EBC1018E4207D154F8042C013238 +:1044200002445A6054F8082C1A602846BDE8F84047 +:1044300000F072B81346CFE7FEE7000070B51B4BE3 +:104440000025044686B058600E4685620163FEF77B +:10445000F3FE04F11003A560E562C4E904334FF0F4 +:10446000FF33C4E90044C4E90635FFF7D1FE2B460B +:10447000024604F134012046C4E9082380230C4A93 +:104480002565FEF7F9F801230A4AE06000920375FA +:10449000684672680192B268CDE90223064BCDE905 +:1044A0000435FEF711F906B070BD00BF302B0020B7 +:1044B000705900087559000839440008024AD36A47 +:1044C0001843D062704700BFC8280020C0E9000030 +:1044D000816070478368013B002B10B583600CDA64 +:1044E000074BDC684368A061206063601C60446027 +:1044F0000520FEF71DF8A06910BD0020FCE700BFF5 +:10450000C828002008B5302383F31188FFF7E2FFA5 +:10451000002383F3118808BD08B5302383F3118885 +:1045200083680133002B836007DC036800211A686D +:10453000026050601846FEF731F8002383F31188BB +:1045400008BD00004B6843608B688360CB68C36024 +:104550000B6943614B6903628B6943620B680360BB +:104560007047000008B53C4B40F2FF713B48D3F860 +:1045700088200A43C3F88820D3F8882022F4FF62F9 +:1045800022F00702C3F88820D3F88820D3F8E0206F +:104590000A43C3F8E020D3F808210A43C3F80821EE +:1045A0002F4AD3F808311146FFF7CCFF00F58060A1 +:1045B00002F11C01FFF7C6FF00F5806002F138012F +:1045C000FFF7C0FF00F5806002F15401FFF7BAFF6A +:1045D00000F5806002F17001FFF7B4FF00F5806024 +:1045E00002F18C01FFF7AEFF00F5806002F1A80137 +:1045F000FFF7A8FF00F5806002F1C401FFF7A2FFFA +:1046000000F5806002F1E001FFF79CFF00F580609B +:1046100002F1FC01FFF796FF02F58C7100F5806056 +:10462000FFF790FF00F01CF90E4BD3F8902242F0F8 +:104630000102C3F89022D3F8942242F00102C3F899 +:1046400094220522C3F898204FF06052C3F89C20B2 +:10465000054AC3F8A02008BD0044025800000258D3 +:104660007C59000800ED00E01F00080308B500F0C9 +:10467000F1F9FDF7DBFF104BD3F8DC2042F04002EC +:10468000C3F8DC20D3F8042122F04002C3F804214F +:10469000D3F80431094B1A6842F008021A601A680C +:1046A00042F004021A60FEF79DFDFEF7D3FCBDE860 +:1046B0000840FEF74BBA00BF0044025800180248F9 +:1046C000012070470020704770470000114BD3F85D +:1046D000E82042F00102C3F8E820D3F8102142F0AC +:1046E0000102C3F810210C4AD3F81031D36B43F008 +:1046F0000103D3634FF08043C7229A624FF0FF3229 +:10470000DA6200229A615A63DA605A6001225A61C1 +:104710001A607047004402580010005C4FF080425D +:1047200008B51169D3680B40D9B29B076FEA010144 +:10473000116107D5302383F31188FDF791FF002322 +:1047400083F3118808BD0000064BD3F8DC20024338 +:10475000C3F8DC20D3F804211043C3F80401D3F8D4 +:10476000043170470044025808B53C4B4FF0FF310C +:10477000D3F8802062F00042C3F88020D3F8802074 +:1047800002F00042C3F88020D3F88020D3F88420C0 +:10479000C3F88410D3F884200022C3F88420D3F80F +:1047A0008400D86F40F0FF4040F4FF0040F4DF4049 +:1047B00040F07F00D867D86F20F0FF4020F4FF0062 +:1047C00020F4DF4020F07F00D867D86FD3F888004E +:1047D0006FEA40506FEA5050C3F88800D3F8880061 +:1047E000C0F30A00C3F88800D3F88800D3F890001B +:1047F000C3F89010D3F89000C3F89020D3F890003D +:10480000D3F89400C3F89410D3F89400C3F894201C +:10481000D3F89400D3F89800C3F89810D3F8980010 +:10482000C3F89820D3F89800D3F88C00C3F88C1004 +:10483000D3F88C00C3F88C20D3F88C00D3F89C00FC +:10484000C3F89C10D3F89C10C3F89C20D3F89C307C +:10485000FCF70EFEBDE8084000F0E8B8004402583E +:1048600008B50122534BC3F80821534BD3F8F42069 +:1048700042F00202C3F8F420D3F81C2142F00202F5 +:10488000C3F81C210222D3F81C314C4BDA605A6861 +:104890009104FCD54A4A1A6001229A60494ADA60BA +:1048A00000221A614FF440429A61444B9A69920483 +:1048B000FCD51A6842F480721A603F4B1A6F12F4EA +:1048C000407F04D04FF480321A6700221A671A68BA +:1048D00042F001021A60384B1A685007FCD50022DA +:1048E0001A611A6912F03802FBD1012119604FF0E8 +:1048F000804159605A67344ADA62344A1A611A6848 +:1049000042F480321A602C4B1A689103FCD51A6865 +:1049100042F480521A601A689204FCD52C4A2D4940 +:104920009A6200225A63196301F57C01DA6301F28D +:10493000E71199635A64284A1A64284ADA621A68A5 +:1049400042F0A8521A601C4B1A6802F02852B2F1C9 +:10495000285FF9D148229A614FF48862DA614022D7 +:104960001A621F4ADA641F4A1A651F4A5A651F4AAB +:104970009A6532231E4A1360136803F00F03022B5B +:10498000FAD10D4A136943F003031361136903F06D +:104990003803182BFAD14FF00050FFF7D5FE4FF037 +:1049A0008040FFF7D1FE4FF00040BDE80840FFF720 +:1049B000CBBE00BF0080005100440258004802589E +:1049C00000C000F0020000010000FF010088900814 +:1049D0002220400063020901470E0508DD0BBF01DC +:1049E00020000020000001100910E000000101106B +:1049F0000020005208B5034800F0E6FCBDE808407E +:104A0000FEF742BC8C48002008B50348FCF798FE2E +:104A1000BDE80840FEF738BC5C24002008B5FFF76D +:104A20007DFEBDE80840FEF72FBC000008B5092157 +:104A30007A20FCF70BFF09213120FCF707FF072143 +:104A40001C20FCF703FF0C215220BDE80840FCF7B6 +:104A5000FDBE000008B5FFF787FE00F015FAFDF770 +:104A6000B3F800F003FAFDF789FAFDF75BF9FFF7F9 +:104A70002BFEBDE80840FFF7C9BB00000379052BFA +:104A800005BF836A002001204B6004BF4FF4007310 +:104A90000B60704770B55D1E866A04460D44B542D2 +:104AA00005D9436B43F080034363012070BD0625A5 +:104AB000057100F03BFC05232371F7E770B55D1E1F +:104AC000866A04460D44B54205D9436B43F0800322 +:104AD0004363012070BD0725057100F04DFC0523DF +:104AE0002371F7E738B505790446052D05D108236C +:104AF000037100F067FC257138BD0120FCE7000060 +:104B00000323F0B5037185B0044600F001FA0022DA +:104B10002046114600F046FA4FF4D57203AB082147 +:104B2000204600F061FA0246B8B901232363039BD3 +:104B3000C3F30323012B09D103AB3721204600F037 +:104B400053FA18B9A44B039A1340ABB12046012580 +:104B500000F010FA0223237137E103AB0022372162 +:104B6000204600F041FA28B99B4A039B1A40002ACC +:104B700000F0A78002232363236B03F00F03022BB3 +:104B800040F0A9806425954E42F21070FDF7B2FD09 +:104B900003AB32460121204600F010FA0028D5D19F +:104BA000039B002B80F293805A0003D5236B43F0C4 +:104BB00010032363002204F108030221204600F0C1 +:104BC00071FA02460028C1D104F1380303212046BE +:104BD00000F00AFA0028B9D104F11805A26B0921E6 +:104BE00020462B4600F05EFA0028AFD102ABA26B44 +:104BF0000721204600F0F8F906460028A6D1236BCD +:104C000003F00F03022B40F08F807E227F21284685 +:104C100000F0E8FB012840F28780E76B42F2107059 +:104C2000FDF768FD08234FF40072394620460096D0 +:104C300000F054FA002889D1384600F021FC236B9B +:104C4000A06203F00F03022B72D103AB644A06216A +:104C5000204600F0C9F9002871D15F49039B194033 +:104C6000B1FA81F14909204600F064F902AB4FF432 +:104C700000721021204600F0B7F9054600287FF4A5 +:104C800065AF554E029B33427FF460AF236B13F048 +:104C90000E0F03F00F0273D0022A7FF457AFE36ABE +:104CA0001978012900F09480022900F093800029EE +:104CB00000F089804B4F204600F062F903AB3A4682 +:104CC00076E011462046226300F06CF954E7013D7E +:104CD0007FF45AAF3AE7444D6426444A3E4F012BD5 +:104CE00018BF154603AB00223721204600F07CF99F +:104CF00000287FF42BAF039B3B427FF427AF03AB2D +:104D00002A462921204600F059F900287FF41EAFD9 +:104D1000039B002BFFF648AF013E3FF417AF42F272 +:104D20001070FDF7E7FCDDE7284600F07DFB86E725 +:104D30007E227F212846E66B00F054FB08B9002153 +:104D400091E700234022314620460093062300F0DD +:104D5000C5F90028F3D1B3895BBA9B07EFD5244B83 +:104D60004022314620460093062300F0B7F9002880 +:104D7000E5D1317C01F00F010F3918BF012172E735 +:104D8000E36A1978F9B101297FF4E0AE204600F01A +:104D9000F7F803ABA26B3721204600F025F9002875 +:104DA0007FF4D4AE039B33427FF4D0AE03AB022238 +:104DB0000621204600F018F900287FF4C7AE039BB7 +:104DC00033427FF4C3AE05232371284605B0F0BDFE +:104DD000084F70E7084F6EE708E0FFFD0080FFC056 +:104DE0000001B9030000B7030080FF5000001080ED +:104DF000F1FFFF800001B7030002B70337B5044697 +:104E00000C4D01ABA26B0D21204600F0EDF878B9F6 +:104E1000019B2B420BD1C3F34323042B08D0053B4A +:104E2000022B04D84FF47A70FDF764FCE9E7012007 +:104E300003B030BD08E0FFFD70B53023054683F3B5 +:104E4000118803790024022B03D184F311882046B2 +:104E500070BD0423037184F311880226FFF7CEFF8F +:104E60000446284600F086F82E71F0E700F036B8C8 +:104E7000044B03600123037100234363C0E90A3339 +:104E8000704700BFB05A000870470000836CC26AC8 +:104E90008B42506810B506D95A1E4C0002EB4103F4 +:104EA000B3FBF4F3184410BD01F001038A0748BFB7 +:104EB00043F002034A0748BF43F008030A0748BF0C +:104EC00043F00403CA0648BF43F010038A06426B4E +:104ED00048BF43F0200313434363704710B5074CAA +:104EE0002046FFF7C5FF064B0022C4E91023054BFF +:104EF000A364054BE363054BE36410BD8C480020BD +:104F00000070005200B4C404E0480020E04A0020D1 +:104F100010B5446C0649FFF7B9FF6060236842F2A0 +:104F2000107043F003032360BDE81040FDF7E2BBBF +:104F3000801A06000129F8B5466C0B4F09D1756837 +:104F40000A493D40FFF7A2FF054345F480557560CF +:104F5000F8BD746806493C40FFF798FF044344F4E9 +:104F600080547460F4E700BF00ECFFFF80F0FA02A9 +:104F700040787D01436C00225A601A60704700003F +:104F8000426C0129536823F4404304D0022905D020 +:104F900001B95360704743F48043FAE743F4004398 +:104FA000F7E70000436C41F480519A60D9605A6B76 +:104FB0001206FCD580229A637047000010B541F4B8 +:104FC0008851446CA260E160616B11F04502FBD036 +:104FD000A26311F0040203D0FFF766FF012010BDA9 +:104FE000616910461960FAE710B541F48851446CC4 +:104FF000A260E160616B11F04502FBD0A26311F089 +:10500000050203D0FFF750FF012010BD6169104673 +:105010001960FAE773B5134604460E46302282F350 +:105020001188426CD26B32B14FF0FF3140300193A6 +:10503000FDF76CFB019B606C00220265C263C262DB +:10504000456B15F4807504D185F31188012002B0F9 +:1050500070BD4FF0FF31816382F31188012E06D9B4 +:105060000C21204602B0BDE87040FFF7BDBF1046DE +:10507000EDE7000073B5446C0E4600250192616BAC +:10508000A1632565E562FFF70FFF012E07D9019B9C +:105090002A460C2102B0BDE87040FFF7A5BF02B060 +:1050A00070BD000010B541F49851446CA260E160FD +:1050B000616B11F04502FBD0A26311F03F0203D0F7 +:1050C000FFF7F2FE012010BD216A10461960E16968 +:1050D0005960A16999606169D960F4E72DE9F743E6 +:1050E00004460191006D01A91746984600F0D8F9D1 +:1050F000064600284AD0626C2046DDF804905568C8 +:10510000C5F3090501356B00A56CB5FBF3F54FF44C +:105110007A73B5FBF3F55D435562FFF76FFE50BB45 +:10512000636C4FF0FF3201254146C3F8589020468A +:105130001D659A634FF49572DA6342F207029F622B +:10514000DA62E36C0A9AFFF74FFFA0B9E26C104BEA +:1051500011680B407BB929462046FFF75BFF0546E7 +:1051600048B92E463A460199206D00F0D1F93046F3 +:1051700003B0BDE8F0833A460199206D00F0C8F90C +:10518000E26C01212046FFF775FFF0E70126EEE70C +:1051900008E0FFFD2DE9F7431F46436C01924FF4F1 +:1051A0007A725D6804468846C5F3090501356E00CC +:1051B000856CB5FBF6F5B5FBF2F555435D62FFF77F +:1051C0001DFE20B10125284603B0BDE8F0837E0214 +:1051D00001A9206D324600F063F905460028F1D0A0 +:1051E000636C019AD4F84C909A6501221A654FF0CD +:1051F000FF329A634FF49572DA639E62236BDB068B +:105200004B4658BF4FEA4828012F42461BD912216E +:105210002046FFF7E9FEC0B9D9F80020104B134033 +:105220009BB9636C42F2930239462046DA62E26C23 +:10523000FFF7F0FE804640B932460199206D4546A1 +:1052400000F066F9BFE71121E2E732460199206DCF +:1052500000F05EF9E26C39462046FFF70BFFB2E73B +:1052600008E0FFFD2DE9F3411F46436C01924FF426 +:105270007A725D6804468846C5F3090501356E00FB +:10528000856CB5FBF6F5B5FBF2F555435D62FFF7AE +:10529000B5FD20B10125284602B0BDE8F0817E02AF +:1052A00001A9206D324600F04BF905460028F1D0E7 +:1052B000636C019A9A6501221A654FF0FF329A6376 +:1052C0004FF48D72DA639E62236BE66CDB06334625 +:1052D00058BF4FEA4828012F424619D919212046C4 +:1052E000FFF782FEB0B932680F4B134093B9636C7D +:1052F00042F2910239462046DA62E26CFFF78AFEFA +:10530000064638B901993546206D00F055F9C2E7D7 +:105310001821E4E70199206D00F04EF9E26C39465E +:105320002046FFF7A7FEB6E708E0FFFD12F0030FE7 +:105330002DE9F04107460C4615461E4617D00E448F +:10534000B44202D10020BDE8F0810123FA6B21466E +:105350003846FFF71FFF0028F5D128464FF40072AA +:10536000F96B05F500750134FBF79EFDE8E7BDE834 +:10537000F041FFF70FBF000012F0030F2DE9F041DD +:1053800007460C4615461E4617D00E44B44202D1BD +:105390000020BDE8F08129464FF40072F86B05F556 +:1053A0000075FBF781FD0123FA6B21463846FFF7B4 +:1053B00059FF0028EDD10134E8E7BDE8F041FFF7DF +:1053C00051BF000000207047302310B583F31188CF +:1053D0000024436C40302146DC63FDF7A5F984F3DB +:1053E000118810BD01F01F03F0B502F01F0456092B +:1053F0005A1C0123B6EB511F50F8265003FA02F352 +:105400004FEA511703F1FF333DBF50F82720C4F195 +:105410002000134003EA05003BBF03FA00F225FA1F +:1054200004F0E0401043F0BD70B57E227F210546B8 +:10543000FFF7D8FF18B1012819D0002070BD3E2217 +:1054400049212846FFF7CEFF2F2204463121284666 +:10545000FFF7C8FF06460134502202365321284682 +:10546000B440FFF7BFFF093804FA00F0E6E7302246 +:1054700045212846FFF7B6FF01308002DEE7000035 +:1054800090F8D63090F8D7201B0403EB026390F815 +:10549000D42090F8D500134403EB0020704700009F +:1054A000F8B50F461546044648B905F11F01012617 +:1054B000386821F01F01FDF771FE3046F8BD427BD0 +:1054C00029463868FDF700FE06460028EDD13B6806 +:1054D0006360A368AB4210D213B12068FDF7F0FD02 +:1054E000637B28462BB1FDF7E7FD206020B9A06063 +:1054F000E3E7FDF7ABFDF8E7A560206805F11F01C4 +:10550000012621F01F013860FDF748FE2673D4E71D +:1055100070B50E4602F11F010446154621F01F0129 +:105520003046FDF74DFE4CB12168B14206D16068AE +:1055300010B12A46FBF7B8FC0023237370BD0000AE +:10554000F8B50F461446054648B904F11F01012677 +:10555000386821F01F01FDF733FE3046F8BD427B6D +:1055600021463868FDF7B0FD06460028EDD1AB684E +:10557000A34210D213B12868FDF7A2FD6B7B204631 +:105580002BB1FDF799FD286020B9A860E5E7FDF78C +:105590005DFDF8E7AC60396819B122462868FBF771 +:1055A00083FC286804F11F01012621F01F013860E7 +:1055B000FDF706FE2E73D0E720B103688B4204BFCF +:1055C000002303737047000010B501390244904274 +:1055D00001D1002005E0037811F8014FA34201D06A +:1055E000181B10BD0130F2E7034611F8012B03F838 +:1055F000012B002AF9D17047313766656661346145 +:10560000653134633430343800000000121000007B +:10561000121100001213000053544D333248373F2B +:105620003F3F0053544D3332483733782F37327869 +:105630000053544D3332483734332F3735332F37F7 +:105640003530000001105A000310590001205800A5 +:1056500003205600100002400800024000080240EB +:1056600000000B002800024008000240040802402D +:1056700006010C00400002400800024008080240F9 +:1056800010020D0058000240080002400C080240C1 +:1056900016030E00700002400C0002401008024089 +:1056A00000040F00880002400C0002401408024071 +:1056B00006051000A00002400C000240180802403D +:1056C00010061100B80002400C0002401C08024005 +:1056D00016072F0010040240080402402008024070 +:1056E0000008380028040240080402402408024050 +:1056F000060939004004024008040240280802401C +:10570000100A3A0058040240080402402C080240E3 +:10571000160B3B00700402400C04024030080240AB +:10572000000C3C00880402400C0402403408024093 +:10573000060D4400A00402400C0402403808024058 +:10574000100E4500B80402400C0402403C08024020 +:10575000160F460000960000000000000000000048 +:105760000000000000000000000000000000000039 +:10577000891B0008751B0008B11B00089D1B000851 +:10578000A91B0008951B0008811B00086D1B000861 +:10579000BD1B000800000000A11C00088D1C0008B3 +:1057A000C91C0008B51C0008C11C0008AD1C00087D +:1057B000991C0008851C0008D51C0008000000008A +:1057C000010000000000000063300000C85700081E +:1057D00020290020302B00204172647550696C6FC5 +:1057E000740025424F415244252D424C002553451B +:1057F0005249414C2500000002000000000000005A +:10580000C11E0008311F000840004000E043002096 +:10581000F043002002000000000000000300000030 +:1058200000000000791F00080000000010000000C8 +:105830000044002000000000010000000000000003 +:105840001C47002001010200E92A0008F92900088C +:10585000952A0008792A00084300000060580008D3 +:1058600009024300020100C03209040000010202E3 +:105870000100052400100105240100010424020296 +:105880000524060001070582030800FF0904010042 +:10589000020A00000007050102400000070581021E +:1058A0004000000012000000AC5800081201100176 +:1058B00002000040091241570002010203010000EA +:1058C0000403090425424F415244250041746C6190 +:1058D000732D436F6E74726F6C0030313233343518 +:1058E0003637383941424344454600000000002025 +:1058F0000000020002000000000000300000040070 +:10590000080000000000002400000800040000005F +:105910000004000000FC0000020000000000043051 +:1059200000800000080000000000003800000100B6 +:105930000100000000000000D52000088D230008B1 +:1059400039240008400040005448002054480020FA +:1059500001000000644800208000000040010000B9 +:105960000800000000010000001000000800000016 +:105970006D61696E0069646C650000000000802A3A +:1059800000000000AAAAAAAA00000024FFFF00004D +:105990000000000000A00A0000000000000000005D +:1059A000AAAAAAAA00000000FFFF00000000000051 +:1059B000000000000000AA0200000000AAAAAAAA93 +:1059C00000005500FFFF000000000000CCCC0C00E0 +:1059D0002000000000000000AAAAAAAA10000000EF +:1059E000FFFF0000000C000000000000000002406B +:1059F00000000000AAAAAA2A00000140FFFF000040 +:105A0000000000000700000050251000000000000A +:105A1000AAA2AAAA50151000FFFF0000000000076C +:105A2000000000000010100000000000AA8AAAAACE +:105A300000101000FFFF0000000000000000000048 +:105A40000000000000000000AAAAAAAA00000000AE +:105A5000FFFF0000000000000000000000551105DD +:105A6000E0000000AAAAAAA800011105FFFF00009B +:105A70000000000000000000000000000000000026 +:105A8000AAAAAAAA00000000FFFF00000000000070 +:105A9000000000000000000000000000AAAAAAAA5E +:105AA00000000000FFFF00000000000000000000F8 +:105AB00000000000C1460008C5460008014B000870 +:105AC000394E0008954A0008BD4A0008E54A00081A +:105AD0007D4A000800000000CB0400000000000028 +:105AE00000001E0000000000FF000000382B002016 +:105AF0005C24002000000000185600088304000009 +:105B0000235600085004000031560008009600009B +:105B100000000800960000000008000004000000DB +:105B2000C058000800000000000000000000000055 +:0C5B300000000000000000000000000069 +:00000001FF diff --git a/Tools/bootloaders/CSKY_PMU_bl.bin b/Tools/bootloaders/CSKY_PMU_bl.bin new file mode 100755 index 0000000000000..e9bc223729898 Binary files /dev/null and b/Tools/bootloaders/CSKY_PMU_bl.bin differ diff --git a/Tools/bootloaders/CSKY_PMU_bl.hex b/Tools/bootloaders/CSKY_PMU_bl.hex new file mode 100644 index 0000000000000..7424572fb2c10 --- /dev/null +++ b/Tools/bootloaders/CSKY_PMU_bl.hex @@ -0,0 +1,1180 @@ +:020000040800F2 +:1000000000070020E1010008A91E0008611E000889 +:10001000891E0008611E0008811E0008E301000817 +:10002000E3010008E3010008E301000805420008BD +:10003000E3010008E3010008E3010008E301000810 +:10004000E3010008E3010008E3010008E301000800 +:10005000E3010008E3010008E3010008E3010008F0 +:10006000E3010008E3010008E3010008E3010008E0 +:10007000E3010008E3010008E3010008E3010008D0 +:10008000E3010008E3010008E3010008DD2E000899 +:10009000492F00089D2F0008F12F0008E3010008F8 +:1000A000E3010008E3010008E3010008E3010008A0 +:1000B000C1460008E3010008E3010008E30100086D +:1000C000E3010008E3010008E3010008E301000880 +:1000D000E3010008AD460008E3010008E301000861 +:1000E000E3010008E3010008E3010008E301000860 +:1000F000E3010008E3010008E3010008E301000850 +:10010000E3010008E3010008E3010008E30100083F +:10011000E3010008E3010008E3010008E30100082F +:10012000E3010008E3010008E3010008E30100081F +:10013000E3010008E3010008E3010008E30100080F +:10014000E3010008E3010008E3010008E3010008FF +:10015000E3010008E3010008E3010008E3010008EF +:10016000E3010008E3010008E3010008E3010008DF +:10017000E3010008E3010008E3010008E3010008CF +:10018000E3010008E3010008E3010008E3010008BF +:10019000E3010008E3010008E3010008E3010008AF +:1001A000E3010008E3010008E3010008E30100089F +:1001B000E3010008E3010008E3010008E30100088F +:1001C000E3010008E3010008E3010008E30100087F +:1001D000E3010008E3010008E3010008E30100086F +:1001E00002E000F000F8FEE772B6374880F30888B6 +:1001F000364880F3098836483649086040F20000E6 +:10020000CCF200004EF63471CEF200010860BFF36C +:100210004F8FBFF36F8F40F20000C0F2F0004EF638 +:100220008851CEF200010860BFF34F8FBFF36F8F8C +:100230004FF00000E1EE100A4EF63C71CEF20001E4 +:100240000860062080F31488BFF36F8F04F038F83D +:1002500004F0CAF84FF055301F491B4A91423CBF89 +:1002600041F8040BFAE71D49184A91423CBF41F896 +:10027000040BFAE71A491B4A1B4B9A423EBF51F83E +:10028000040B42F8040BF8E700201849184A914281 +:100290003CBF41F8040BFAE704F016F804F0F2F85A +:1002A000144C154DAC4203DA54F8041B8847F9E7A7 +:1002B00000F042F8114C124DAC4203DA54F8041B22 +:1002C0008847F9E703F0FEBF000700200023002065 +:1002D0000000000808ED00E00001002000070020F9 +:1002E00070490008002300202C2300203023002028 +:1002F000183C0020E0010008E0010008E0010008CF +:10030000E00100082DE9F04F2DED108AC1F80CD066 +:10031000C3689D46BDEC108ABDE8F08F002383F3CF +:1003200011882846A047002003F0DCFBFEE703F01D +:100330006FFB00DFFEE70000F8B501F051F903F0B4 +:10034000FBFE074603F046FF0546A8BB1F4B9F4236 +:1003500032D001339F4232D01D4B27F0FF029A4228 +:1003600030D1F8B200F0B4FE2E4642F2107400F024 +:10037000B5FE08B10024264601F0F4FC20B10320AC +:1003800000F078F80024264635B1124B9F4203D086 +:1003900003F018FF00242646002003F0D7FE0EB11C +:1003A00000F07EF801F054FA00F0FAFE204600F06A +:1003B000CBF800F075F8F9E72E460024D7E704469D +:1003C0000126D4E706464FF47A74D0E7010007B05F +:1003D000000008B0263A09B008B501F0CDF8A0F148 +:1003E00020035842584108BD07B5042101900DEB88 +:1003F000010001F0DFF803B05DF804FB38B53023ED +:1004000083F31188174803680BB103F037FC164AD1 +:10041000144800234FF47A7103F026FC002383F381 +:100420001188124C236813B12368013B2360636871 +:1004300013B16368013B63600D4D2B7833B963687A +:100440007BB9022001F092F9322363602B78032BF1 +:1004500007D163682BB9022001F088F94FF47A7351 +:10046000636038BD30230020FD03000850240020C5 +:1004700048230020084B187003280CD8DFE800F050 +:1004800008050208022001F067B9022001F05AB9FC +:10049000024B00225A60704748230020502400205D +:1004A00010B501F05FFC30B1204B03221A70204BD5 +:1004B00000225A6010BD1F4B1F4A1C4619680131AB +:1004C000F8D004339342F9D162681C4B9A42F1D9B7 +:1004D0001B4B9B6803F1006303F580339A42E9D21A +:1004E00003F044FE03F056FE002001F093F80220D2 +:1004F000FFF7C0FF134B1A6C00221A64196E1A66BC +:10050000196E596C5A64596E5A665B6E72B64FF02A +:10051000E0233021C3F8084DD4E9003281F311887B +:100520009D4683F308881047C4E700BF4823002096 +:10053000502400200000010820000108FFFF0008EF +:1005400000230020003802402DE9F04F93B0DFF87F +:10055000F89202902022FF210AA8D9F8085001F051 +:1005600021F9B04A1378A3B9AF4801210360117093 +:10057000302383F3118803680BB103F07FFBAB4A90 +:10058000A94800234FF47A7103F06EFB002383F334 +:100590001188029B13B1A64B029A1A60A54A029CCD +:1005A0001378032B1EBF00231370A14A4FF0000BDA +:1005B00018BF5360CDF804B05E46DA46012001F062 +:1005C000CBF824B19A4B1B68002B00F0C5810020AA +:1005D00000F0D2FF071E80F2F58100F095FEF0E7F3 +:1005E0000220FFF7F9FE002840F0E381029B9248C9 +:1005F000BBF1000F08BF1C46022140E04FF47A70A7 +:1006000000F0BAFF071EF1DB0220FFF7E5FE00282D +:10061000ECD0013F052F00F2CE81DFE807F00307A1 +:100620000A0D10330520FFF7DFFE14E0D9F80000B3 +:10063000F9E7D9F80400F6E7D9F80800F3E74FF036 +:100640001C08404600F0E8FF08F10408FFF7CCFE64 +:10065000B8F12C0FF5D1019B012000FA07F71F43D9 +:10066000FBB201934FF0000A01F0E8F82EB1019BB4 +:1006700003F00B030B2B08BF00247048022100F08D +:1006800099FF9BE7D9F80C00CDE7002EAED0019B77 +:1006900003F00B030B2BA9D10220FFF79DFE0746A9 +:1006A0000028A3D0012000F0B5FF0220FFF7E2FEF2 +:1006B00000261FFA86F8404600F0BCFF044670B1E1 +:1006C0000021404600F0CEFF01360028F1D1BB46A4 +:1006D00004465B48022100F06DFF3E466EE70120B4 +:1006E000FFF7C8FE2546D9F80830AB4207D928469F +:1006F00000F092FF013040F061810435F3E74D4B8B +:1007000000251D704A4BBB465D603E46ACE7002E9F +:100710003FF46CAF019B03F00B030B2B7FF466AF30 +:100720000220FFF7A7FE322000F026FFB0F10008FC +:10073000FFF65CAF18F003077FF458AFD9F8082034 +:1007400008EB050393423FF651AFB8F5807F3FF7C2 +:100750004DAF3C4B0393B8450BDD4FF47A7000F07E +:100760000BFF0028FFF642AF039B013703F8010B94 +:10077000F0E7C820FFF730FE074600283FF436AF09 +:100780001F2D11D8C5F1200242450AAB25F0030008 +:1007900028BF42462B490392184400F0F1FF039A08 +:1007A0002848FF2100F0FEFF4FEAA80325490393E4 +:1007B000C8F38702284600F0FDFF0646002888D0CF +:1007C000039B05EB83054FE70220FFF705FE00289A +:1007D0003FF40CAF00F066FF00283FF407AF00279E +:1007E000B846D9F80830BB4218D91F2F11D80A9B38 +:1007F00001330ED027F0030312AA134453F8203C10 +:1008000005934046042205A901F064FA04378046A6 +:10081000E7E7384600F000FF0590F2E74046FFF7B3 +:10082000E3FD21E74C24002030230020FD030008D5 +:1008300050240020482300208847000880470008F3 +:10084000844700084C23002000230020002364215B +:1008500005A8059300F0A4FE00287FF4C7AE02208F +:10086000FFF7BAFD00283FF4C1AE059800F002FF83 +:10087000D5E70023642105A8059300F091FE002828 +:100880007FF4B4AE0220FFF7A7FD00283FF4AEAE20 +:10089000059800F0FFFEC2E70220FFF79DFD00284B +:1008A0003FF4A4AE00F00EFFB9E70220FFF794FD7D +:1008B00000283FF49BAE05A9142000F009FF039027 +:1008C000FFF792FD039905A800F074FECCE63220F4 +:1008D00000F052FE071EFFF689AEBB077FF486AE1E +:1008E000D9F8082007EB0A0393423FF67FAE0220B7 +:1008F000FFF772FD00283FF479AE27F00307574455 +:10090000BA453FF4B1AE504600F086FEFFF76CFDED +:100910000AF1040AF4E74FF47A70FFF75DFD00284E +:100920003FF464AE00F0BEFE002849D00A9B0133BC +:100930000BD008220AA9002000F03CFF00283FD07D +:100940002022FF210AA800F02DFF3B48022100F0E1 +:1009500031FE3A4803F0CCF813B0BDE8F08F002E1A +:100960003FF444AE019B03F00B030B2B7FF43EAE30 +:100970000023642105A8059300F012FE0746002815 +:100980007FF434AE0220FFF727FD804600283FF4B5 +:100990002DAE0221284800F00DFE41F2883003F010 +:1009A000A7F8059800F060FF464600F047FF3C4678 +:1009B00004E6064658E64FF0000A17E6BB4688E60E +:1009C000374686E6012000F0BDFEA7F1210016287B +:1009D0003FF6F4AD01A353F820F000BFE105000895 +:1009E000FD0500088B060008BD050008BD050008D0 +:1009F000BD0500080F070008CF080008C907000858 +:100A00004D0800087308000899080008BD05000893 +:100A1000AB080008BD0500081709000869060008B2 +:100A2000BD0500085F090008ED050008690600081B +:100A3000BD0500084D08000880470008A086010099 +:100A4000094A136849F2690099B21B0C00FB013393 +:100A50001360064B186844F2506182B2000C01FB2F +:100A60000200186080B27047202300201C23002061 +:100A700010B500211022044600F094FE034B03CB76 +:100A8000206061601868A06010BD00BF107AFF1F71 +:100A9000F0B5ADF54F7D6CAC4FF4C47207460D4612 +:100AA0002046002100F07EFE01F000FA264B4FF4B4 +:100AB0007A72B0FBF2F0186093E80700022384E832 +:100AC00007000DF5E9702382FFF7D2FF4BF60443D0 +:100AD0001E49238407A803F04BFE162384F8323105 +:100AE0000DF2E32207AB0DF12C0C1E4603CE66453A +:100AF00010605160334602F10802F6D13068106090 +:100B0000B38893800DAE31460122204600F0C6FE28 +:100B100000230393AB7E029305F11903019380B286 +:100B20000123CDE904600093E97E05A3D3E9002306 +:100B3000384601F013FD0DF54F7DF0BDAFF3008099 +:100B40009E6AC421818A46EE582400208C47000802 +:100B50002DE9F041264D2B7ADAB080460C46FBB9E0 +:100B600027A9204600F0C0FF0746002836D19DF88F +:100B70009D60C82E32D801464FF4FA72284600F024 +:100B800011FE32460DF19E0105F1090000F0F8FD5D +:100B90002E449DF89C3077722B720BB9E37E2B723A +:100BA0008122002106AD27A800F0FCFD0122294684 +:100BB00027A800F0DFFF00230393A37E029304F134 +:100BC000190380B201932823CDE904500093E17EFC +:100BD00005A3D3E90023404601F0C0FC5AB0BDE8AC +:100BE000F08100BFAFF3008026417272DF25D7B7D6 +:100BF000A0340020F0B5254E4FF48A7505FB006542 +:100C0000F1B096F8D83085F8DC300024D82221469F +:100C100085F8E8403AA800F0C5FD06F1090000F0AB +:100C2000B9FDD5F8E4308DF8F000C2B206AF06F198 +:100C300009010DF1F100CDE93A3400F0A1FD39468A +:100C400001223AA800F0CCFF80B2CDE9047008235D +:100C50000127CDE9023706F1D80301933023009331 +:100C6000317A0B4807A3D3E9002301F077FCA042B7 +:100C700006DD01F01BF9C5F8E000384671B0F0BDA3 +:100C80002046FBE778F6339F93CACD8DA034002031 +:100C900070340020F0B51E4E1E4F1F4D85B03046FB +:100CA00001F088FC044660B310220021684600F081 +:100CB00079FD227B6068A1684FF0000362F30303B3 +:100CC0008DF8003002AB03C3019B226862F31C0362 +:100CD00001939DF8003043F020038DF8003000238D +:100CE0006A461946384602F0E9FB044620B9304608 +:100CF00001F064FC2C70D2E72B78072B03D801336A +:100D00002B7005B0F0BD024801F058FCF9E700BFB8 +:100D100070340020E03600209936002070B50D4672 +:100D200014461E4601F076FB50B9022E10D1012C5C +:100D30000ED112A3D3E90023C5E90023012007E067 +:100D4000282C10D005D8012C09D0052C0FD000205C +:100D500070BD302CFBD10BA3D3E90023ECE70BA330 +:100D6000D3E90023E8E70BA3D3E90023E4E70BA3CF +:100D7000D3E90023E0E700BFAFF30080401DA120CE +:100D800026812A0B78F6339F93CACD8D9E6AC421A3 +:100D9000818A46EE26417272DF25D7B7F017304AB6 +:100DA00039059E5638B505460E4C0021013500F038 +:100DB00059FCA4F8F051B4F8F00100F03BFC78B114 +:100DC000B4F8F00100F046FC014648B9B4F8F0016F +:100DD00000F048FCB4F8F0310133A4F8F031EAE750 +:100DE00038BD00BFA03400202DE9F04F8DB000AF1A +:100DF00004460D4601F00EFB002849D12B7E022B44 +:100E00001BD1EB8A012B18D101F050F80646FFF7F1 +:100E100017FE03464FF4C870DFF8B082B3FBF0F260 +:100E200006F5167602FB103316FA83F3C8F8003085 +:100E3000EB7E33B99E4B00221A703437BD46BDE8B5 +:100E4000F08F07F11C01284600F002FE0028F4D1C3 +:100E500007F10C00FFF70CFEBD7F07F10C012A46DD +:100E600007F11F0003F052FC0028E3D10F2D08D832 +:100E70008F4B1D70D8F80030A3F51673C8F80030FA +:100E8000DBE7397F0029D8D0204601F0BBFAD4E750 +:100E9000EB8A282B6CD010D8012B64D0052BCCD139 +:100EA000BFF34F8F8349844BCA6802F4E062134357 +:100EB000CB60BFF34F8F00BFFDE7302BBDD17F4C20 +:100EC000EA7E237A9A42B8D194F8DC206B7E9A426B +:100ED00040F0E58004F1EA01284600F0B5FE064640 +:100EE0000028AAD1012384F8E83000F0DFFFD4F80D +:100EF000E030D4ED007ADFED726AC01A192838BFED +:100F0000192040F6B833984228BF1846B8EE677AE1 +:100F100007EE900AF8EEE77A67EEA67ADFED696AE7 +:100F2000E7EE267AFCEEE77AC4ED007A94F8D93041 +:100F30007B604FF48A7202FB03F56219606892F8D5 +:100F4000E83053B1D2F8E4308342E94615D00023AB +:100F500082F8E830C2F8E030CD4663685A4A9B0A0E +:100F60000133138169E729462046FFF791FD64E7C5 +:100F700029462046FFF7ECFD5FE7B2F8EC803B60C6 +:100F800008F1030A4FEA9A0A4FEA8A02D11DC908FA +:100F9000A9EBC1039D46EB460021584600F002FC38 +:100FA00005F1EE0142462144584600F0E9FB3B685A +:100FB00013B9012000F02EFB94F8D20000F03AFBA8 +:100FC000054630B9207200F06DFB284600F022FB88 +:100FD000C2E7D4F8D4303BB994F8D200B4F8F03179 +:100FE000834201D8FFF7DEFED4F8D43043449D425B +:100FF00008D294F8D200B4F8F0310130834201D81D +:10100000FFF7D0FE60685FFA8AF2594600F0D2FB23 +:1010100008B9CD468AE7636894F8D2004344636018 +:10102000D4F8D43008EB030AC4F8D4A000F002FBD3 +:10103000824509D394F8D230D4F8D4000133401B50 +:1010400084F8D230C4F8D400B8F1FF0F0FD80025CF +:10105000257200F027FB284600F0DCFA00F082FE43 +:10106000194B188108B9FFF71BFACD46E5E67B68F6 +:1010700094F8D9004FF48A7202FB0343D3F8E420BA +:1010800083F8E86002F58072C3F8E060C3F8E420FA +:10109000FFF7B0FDFFF7FEFD84F8D960B9E72368DC +:1010A00040F6B832FA33934294BF23602260C4E61C +:1010B0006934002000ED00E00400FA05A0340020AF +:1010C000CDCCCC3D6666663F582400206C340020B1 +:1010D000014B1870704700BF6424002030B54FF0FA +:1010E0000054264B22689A4285B017D1637DABB17C +:1010F000234A24481370237D037200252249C0F837 +:10110000E450C922093000F03BFB2046E02229468A +:1011100000F048FB0124204605B030BD03F064F820 +:10112000044608B90024F6E7184A194D136C43F039 +:1011300000731364AA6D174B9A42F3D12B6E013BD7 +:101140007E2BEFD8144A07CA01AB83E80700184684 +:10115000032100F0D5FD6B6D83424FF00003E1D118 +:101160002A6D8A4201BFAB65054B2A6E1A7003BF18 +:101170000A4BEA6D1A601C46CDE700BF9AAD44C524 +:1011800064240020A0340020160000200038024013 +:10119000006600405041A0B058660040182300206F +:1011A0002DE9FF474C4C00F0DDFD02236371002365 +:1011B00002934A4B2081D3F800C0BCF57A7F12D34A +:1011C000474B484FB7FBFCF69C458CBF0A231123C5 +:1011D000581EB6FBF3F503FB1562C1B2002A3ED0E0 +:1011E00002280346F4D89DF80B303F493F485A1E69 +:1011F0009DF80A30013B1B0443EA0253BDF8082066 +:10120000013A13434B6002F049F900230193384B34 +:10121000384900933848394B4FF4805201F0CAF8EE +:10122000374B197811B1344801F0ECF800F03EFE6C +:101230000546FFF705FC4FF4C873B0FBF3F202FB61 +:10124000130305F5167010FA83F02E4B186002F0A8 +:10125000C1FF08B10F23238104B0BDE8F0876B1EE6 +:10126000B3F5806FBFD2C1EBC10E0EF103034FEA9D +:10127000E309C3F3C703A1EB030809F1010A4FF423 +:101280007A705FFA88F609FB00005AFA88F8B0FB1A +:10129000F8F0B0F5617F08D90EF1FF33C3F3C7034F +:1012A000C91ACEB2591E0F2914D8721E072A8CBF34 +:1012B00000220122991901FB0551B7FBF1F7BC454A +:1012C00091D1002A8FD0ADF808508DF80A308DF8F2 +:1012D0000B6088E71346EDE7582400201823002010 +:1012E0003F420F0040787D0110230020E0360020AF +:1012F0001D0D00086824002070340020E90D00084E +:10130000642400206C3400202DE9F04F7EA7D7E93B +:1013100000670FF2FC19D9E90089754D93B00DF102 +:10132000300AFFF7B7FC18220021504600F03AFAC5 +:10133000DFF8E4B16F4C002319465246584602F0DC +:10134000E5F8014650BB102208A800F02BFA6369AB +:1013500083F48043636100F0ABFD0B4612A90246A3 +:1013600011E903000DF1240C8CE803009DF8341002 +:10137000C1F3030089064CBF0E99BDF838C08DF843 +:101380002C0046BFC1F31C0C4CF00041CCF30A0109 +:101390000891284608A901F0EFFACCE7284601F0A9 +:1013A00039F80446002848D1DFF870B100F07EFD1E +:1013B000DBF80030984240D300F078FD0790FFF74B +:1013C0003FFB079A8DF8204003464FF4C87002F5A2 +:1013D0001672B3FBF0F101FB103312FA83F3CBF872 +:1013E0000030DFF83CB19BF8001011B901238DF8F3 +:1013F000203050460791FFF73BFB0799C1F11004DD +:10140000E4B2062C28BF0624224651440DF12100E7 +:1014100000F0B6F908AB0393182302930134364B5E +:101420000193E4B20123009304943B4632462846DC +:1014300001F036F800238BF8003000F037FD2F4A1A +:101440002F4C1368C31AB3F57A7F30D3106000F0C5 +:101450002FFD02460B46284601F0BAF8284600F058 +:10146000D9FF20B3237ADFF8BCB0002B14BF0323CD +:1014700002238BF8053000F019FD4FF47A73012236 +:10148000B0FBF3F05146CBF80000584600F0B8FA34 +:10149000182302931B4B019380B240F25513CDE900 +:1014A00003A0009342464B46284600F0F9FF237AFA +:1014B0009BB100F0FBFC94F8E83073B9D4F8E0304D +:1014C00043B1C01A2368FA2B38BFFA230533B0EBB7 +:1014D000430F02D30020FFF78DFBC82002F008FB6A +:1014E000237A002B7FF41BAF13B0BDE8F08F00BF51 +:1014F000703400200004024068340020943600203C +:10150000A034002098360020401DA12026812A0BFF +:10151000F1C6A7C1D068080FE03600206C34002067 +:10152000693400205824002070B5104B1B7801331B +:10153000DBB2012B0C4612D80D4B1D6829684FF405 +:101540007A730E6AA2FB0332014622462846B04750 +:10155000844204D1074B00221A70012070BD4FF461 +:10156000FA7002F0C5FA0020F8E700BF242300203B +:1015700028230020DC36002007B500230246012185 +:101580000DF107008DF80730FFF7CEFF20B19DF871 +:10159000070003B05DF804FB4FF0FF30F9E70000EF +:1015A0000A4608B50421FFF7BFFF80F00100C0B272 +:1015B000404208BD30B4074B0A461978064B53F831 +:1015C00021402368DD69054B0146AC46204630BC0E +:1015D000604700BFDC36002028230020A0860100E1 +:1015E00070B502F067FC094E094D30800024286870 +:1015F0003388834208D902F057FC2B680444013336 +:10160000B4F5803F2B60F2D370BD00BFDE36002002 +:101610009C36002002F0FEBC00F1006000F5803036 +:101620000068704700F10060920000F5803002F021 +:1016300087BC0000054B1A68054B1B889B1A834228 +:1016400002D9104402F030BC002070479C360020C4 +:10165000DE360020024B1B68184402F02DBC00BF90 +:101660009C360020024B1B68184402F03DBC00BFB2 +:101670009C36002010F003030AD1B0F5047F05D298 +:1016800000F10050A0F51040D0F80038184670471F +:101690000023FBE700F10050A0F51040D0F8100A3D +:1016A00070470000064991F8243033B10023086ADE +:1016B00081F824300822FFF7B5BF0120704700BF32 +:1016C000A0360020014B1868704700BF002004E0DE +:1016D00070B5194B1D68194B0138C5F30B0408444C +:1016E0002D0C04221E88A6420BD15C680A46013CE0 +:1016F000824213460FD214F9016F4EB102F8016B0A +:10170000F6E7013A03F10803ECD181420B4602D21D +:101710002C2203F8012B0A4A05241688AE4204D174 +:10172000984284BF967803F8016B013C02F10402F1 +:10173000F3D1581A70BD00BF002004E0E447000850 +:10174000D0470008022802BF024B4FF080429A6146 +:10175000704700BF00040240022802BF024B4FF452 +:1017600080429A61704700BF00040240022801BF16 +:10177000024A536983F48043536170470004024076 +:1017800010B50023934203D0CC5CC4540133F9E775 +:1017900010BD000003460246D01A12F9011B0029B1 +:1017A000FAD1704702440346934202D003F8011B6A +:1017B000FAE770472DE9F8431F4D144695F82420A9 +:1017C0000746884652BBDFF870909CB395F82430EA +:1017D0002BB92022FF2148462F62FFF7E3FF95F83F +:1017E0002400C0F10802A24228BF2246D6B24146D8 +:1017F000920005EB8000FFF7C3FF95F82430A41B8F +:101800001E44F6B2082E17449044E4B285F82460D2 +:10181000DBD1FFF747FF0028D7D108E02B6A03EBA5 +:1018200082038342CFD0FFF73DFF0028CBD10020B9 +:10183000BDE8F8830120FBE7A0360020024B1A78B0 +:10184000024B1A70704700BFDC36002024230020B2 +:1018500003494FF461430B60024B186802F034B83F +:10186000C836002028230020094B10B51422044656 +:1018700000211846FFF796FF064A074B12780460CE +:101880000146BDE8104053F8220002F01DB800BF29 +:10189000C8360020DC360020282300202DE9F3473D +:1018A0000D46044600219046284640F27912FFF783 +:1018B00079FF234620220021284600F0FFFF231D48 +:1018C00002222021284600F0F9FF631D0322222175 +:1018D000284600F0F3FFA31D03222521284600F02F +:1018E000EDFF04F1080310222821284600F0E6FF4E +:1018F00004F1100308223821284600F0DFFF04F12C +:10190000110308224021284600F0D8FF04F11203F9 +:1019100008224821284600F0D1FF04F114032022B8 +:101920005021284600F0CAFF04F11803402270211C +:10193000284600F0C3FF04F120030822B021284606 +:1019400000F0BCFF04F121030822B821284600F072 +:10195000B5FF04F12207C0263B463146082228463F +:10196000083600F0ABFFB6F5A07F07F10107F3D111 +:1019700094F832308DF8073031460DF10703082214 +:10198000284600F09BFF002604F1330A9DF807303B +:101990004FEAC6099E4209F5A47720D394F8323164 +:1019A000502B28BF50238DF80730B8F1000F08D115 +:1019B00039460DF107030722284600F07FFF09F2A0 +:1019C0004F17002604F233149DF807309E4207EBB0 +:1019D000C6010DD30731C80802B0BDE8F0870AEB95 +:1019E000060308223946284600F068FF0136CDE795 +:1019F000A3190822284600F061FF0136E4E7000041 +:101A000013B504460846002101602346C0F80310C0 +:101A10002022019000F052FF0198231D0222202174 +:101A200000F04CFF0198631D0322222100F046FFC5 +:101A30000198A31D0322252100F040FF019804F125 +:101A400008031022282100F039FF072002B010BD42 +:101A5000F7B5047F05460E462CB1838A122B02D9B6 +:101A6000012003B0F0BD0023194607220096284646 +:101A700000F0E8FD731C0093012200230721284693 +:101A800000F0E0FDD4B9B31C0093052223460821E1 +:101A9000284600F0D7FD0D24B378102BE0D8374648 +:101AA000B278BB1B934210D32B7FA88A0734E4087B +:101AB000B3B9844294BF00200120D2E7AB8ADB0097 +:101AC000083BDB08B3700824E6E7FB1C00932146C3 +:101AD00000230822284600F0B5FD08340137DFE76F +:101AE000201A18BF0120BCE7F7B5047F05460E4653 +:101AF0002CB1838ACA2B02D9012003B0F0BD002388 +:101B0000194600960822284600F09CFD731CD4B9A3 +:101B10000822009311462346284600F093FD102426 +:101B20007378C82BE8D8012372785F1C013B93427D +:101B300010D32B7FA88A0734E408B3B9844294BF3A +:101B400000200120D9E7AB8ADB00083BDB0873707B +:101B50000824E5E7F31900932146002308222846CC +:101B600000F070FD08343B46DEE7201A18BF012064 +:101B7000C3E70000F7B50D46044600211646284687 +:101B80008122FFF70FFE234608220021284600F09D +:101B900095FE94F901206378002AB8BF7F238DF861 +:101BA00007309EB90DF1070307220821284600F0EF +:101BB00085FE0F27002602349DF807309E4207EB72 +:101BC000C60105D30731C80803B0F0BD0827F1E707 +:101BD000A3190822284600F071FE0136ECE7000048 +:101BE000F7B50E460546002114463046CE22FFF7D3 +:101BF000D9FD2B4628220021304600F05FFE2B7ACB +:101C0000C82B28BFC8238DF807308CB90DF1070306 +:101C100008222821304600F051FE30242F469DF83E +:101C200007207B1B934205D3E01DC00803B0F0BD25 +:101C30002824F3E707F1090321460822304600F083 +:101C40003DFE08340137EAE7F7B5047F05460E4646 +:101C500034B1838AB3F5827F02D9012003B0F0BD8D +:101C60000096012310220021284600F0EBFCDCB98D +:101C7000B31C0093092223461021284600F0E2FC01 +:101C800019247388B3F5807FE7D837467288BB1B69 +:101C9000934210D32B7FA88A0734E408B3B9844257 +:101CA00094BF00200120D9E7AB8ADB00103BDB08A2 +:101CB00073801024E5E73B1D009321460023082292 +:101CC000284600F0BFFC08340137DFE7201A18BFB0 +:101CD0000120C3E730B5094D0A4491420DD011F8F7 +:101CE000013B5840082340F30004013B2C4013F013 +:101CF000FF0384EA5000F6D1EFE730BD2083B8ED52 +:101D0000F7B54FF0FF33DFF854C0DFF854E000EBD5 +:101D100081011A4688421CD050F8044B019401AF4F +:101D2000042417F8015B82EA05620825DB181646D1 +:101D300005F1FF355241002EBCBF83EA0C0382EA55 +:101D40000E0215F0FF05F1D1013C14F0FF04E8D1BB +:101D5000E0E7D843D14303B0F0BD00BF9336EAA912 +:101D6000EBE1F042F7B5384A106851686B4603C39F +:101D70006A4636493648082302F0D8FC0446002853 +:101D800033D10A25334A106851686B4603C36A464B +:101D900031492F48082302F0C9FC0446002849D0E5 +:101DA0000369B3F5E02F45D8B0F8661040F2BC42A5 +:101DB00091423FD1294A024402F15C018B4239D35E +:101DC0005C3B234900209E1AFFF784FF32460746FA +:101DD00004F164010020FFF77DFFA3689F4229D131 +:101DE000E368984208BF002524E00369B3F5E02FBB +:101DF00027D8418B40F2BC42914220D1174A02447D +:101E000002F110018B4218D3103B114900209D1A9A +:101E1000FFF760FF2A46064604F118010020FFF78D +:101E200059FFA3689E4202D1E368984201D00D2574 +:101E3000A8E70025284603B0F0BD1025A2E70C2531 +:101E4000A0E70B259EE700BF04480008DCFF060062 +:101E5000000001080D48000890FF06000800FFF789 +:101E600000B59BB0EFF3098168226846FFF788FC54 +:101E7000EFF30583014B9B6BFEE700BF00ED00E035 +:101E800008B5FFF7EDFF000000B59BB0EFF3098147 +:101E900068226846FFF774FCEFF30583014B5B6B28 +:101EA000FEE700BF00ED00E0FEE7000002F0E2B850 +:101EB00002F0C4B82DE9F041456A15B94162BDE8A8 +:101EC000F0814B6823F06047C3F38A464FEAD37E24 +:101ED000C3F3807816EA230638BF3E46AC462B464D +:101EE0005A68BEEBD27F22F060440AD0002A18DA8A +:101EF000A40CB44217D19D420FD10D60DEE713460A +:101F0000EEE7A74207D102F08044C2F38072424557 +:101F10000BD054B1EFE708D2EDE7CCF800100B601E +:101F2000CDE7B44201D0B442E5D81A689C46002AF5 +:101F3000E5D11960C3E700002DE9F047089D01F0E5 +:101F400007044FEAD508224405F0070500EBD1004D +:101F50004FF47F49944201D1BDE8F08704F00707B0 +:101F600005F0070A57453E4638BF5646C6F10806F3 +:101F7000111B8E4228BF0E46E10808EBD50E415CCE +:101F800013F80EC0B94029FA06F721FA0AF1FFB298 +:101F90008CEA010147FA0AF739408CEA010C03F890 +:101FA0000EC034443544D5E780EA0120082341F2CD +:101FB000210201B24000002980B203F1FF33B8BF13 +:101FC000504013F0FF03F4D17047000038B50C46C1 +:101FD0008D18A54200D138BD14F8011BFFF7E4FFAE +:101FE000F7E7000042684AB113684360438981897A +:101FF00001339BB29942438138BF838110467047B9 +:1020000070B588B0202204460D4668460021FFF7CF +:10201000C9FB20460495FFF7E5FF024658B16B4621 +:10202000054608AE1C4603CCB442286069602346CE +:1020300005F10805F6D1104608B070BD082817D97B +:1020400009280CD00A280CD00B280CD00C280CD056 +:102050000D280CD00E2814BF4020302070470C20D3 +:1020600070471020704714207047182070472020B8 +:1020700070470000082817D90C280CD910280CD953 +:1020800014280CD918280CD920280CD930288CBF3A +:102090000F200E207047092070470A2070470B2040 +:1020A00070470C2070470D20704700002DE9F84361 +:1020B000078C072F04461ED9D0E9029800254FF659 +:1020C000FF73C5F12006A5F1200029FA05F108FAF1 +:1020D00006F628FA00F031430143C9B21846FFF76B +:1020E00063FF0835402D0346EBD1E1693A46BDE870 +:1020F000F843FFF76BBF4FF6FF70BDE8F8830000B1 +:1021000010B54B6823B9CA8A63F30902CA8210BDAD +:1021100004691A681C600361C38A013BC3824A6078 +:10212000EFE700002DE9F84F1D46CB8A0F46C3F3B9 +:1021300009010529814692460B4630D00020AAB2FB +:1021400007F11A049EB2042E1FFA80F80FD89045AA +:1021500003F1010306D3FB8A0A4462F30903FB82FD +:1021600001201AE01AF80060E6540130EAE79045D1 +:10217000F1D2A1F1050B1C237C68BBFBF3F203FB3E +:1021800012BB1FFA8BF6002C45D14846FFF72AFFF9 +:10219000044638B978606FF00200BDE8F88F4FF060 +:1021A0000008E6E7002606607860ADB24FF0000B4D +:1021B000454510D90AEB0803221D13F8011B915560 +:1021C000B1B208F101081B291FFA88F82BD0454548 +:1021D00006F10106F1D8FB8AC3F30902154465F341 +:1021E0000903BCE7013292B21C462368002BF9D1E7 +:1021F0006B1F0B441C21B3FBF1F301339BB29A42DA +:10220000D3D2BBF1000FD0D14846FFF7EBFE20B987 +:10221000C4F800B0BFE70122E7E7C0F800B05E46AF +:1022200020600446C1E74545D5D94846FFF7DAFEA8 +:1022300008B92060AFE7C0F800B00026206004466F +:10224000B6E700002DE9F04F2DED028B1C4683B060 +:102250005B69019207468846002B00F0A780238C1B +:102260002BB1E269002A00F0A180072B35D807F1D5 +:102270000C00FFF7B7FE054638B96FF00205284697 +:1022800003B0BDEC028BBDE8F08F14220021FFF7F4 +:1022900089FA228CE16905F10800FFF771FA208CB8 +:1022A000013080B2FFF7E6FEFFF7C8FE013880B2CA +:1022B0002084013028746369228C1B782A4403F03F +:1022C0001F0363F03F0348F0004113723846696012 +:1022D0002946FFF7EFFD0125D1E702339BB2072224 +:1022E000C18A0633B3FBF2F3828A521A9BB292B2CE +:1022F0009342C2D800F10C034FF0000908EE103AE7 +:102300004FF0800A4E464D4618EE100AFFF76AFE5F +:1023100083460028B1D014220021FFF743FA002E93 +:102320003AD1019BABF8083002220BF1080E1FFADC +:1023300082FC0CF10100BCF1060F218C80B201D8A7 +:102340008E422BD3FFF796FEFFF778FE6269127874 +:10235000013802F01F028E4208BF4FF0400A42EAE5 +:1023600049121BFA80F14AEA020A013048F00042A1 +:1023700081F808A08BF81000CBF8042059463846A5 +:10238000FFF798FD238C0135B3422DB289F0010986 +:102390004FF0000AB8D172E70022C6E7E169895D13 +:1023A0000EF802100136B6B20132C0E76FF0010537 +:1023B00065E70000F8B515460E46302200210446B8 +:1023C0001F46FFF7EFF9069B6360B5F5001F079BFB +:1023D000A76034BF6A094FF6FF72A36297B2E66145 +:1023E00004F1100000239A4206D800230360A7825C +:1023F000E3822383E360F8BD066001333046203674 +:10240000F1E7000003781BB94BB2002BC8BF017085 +:102410007047000000787047F8B50C46C969074658 +:1024200011B9238C002B37D1257E1F2D34D8387855 +:1024300028BB228C072A2CD8268A36F003032BD1FE +:102440004FF6FF70FFF7C2FD20F00100310240049B +:1024500041EA0561400C41EA40254FF6FF722346F0 +:1024600029463846FFF7EEFE002807DD626913783B +:102470000133DBB21F2B88BF00231370F8BD218A04 +:102480002D0645EA012505432046FFF70FFE0246CB +:10249000E5E76FF00300F1E76FF00100EEE7000001 +:1024A00070B58AB0044616460021282268461D46AB +:1024B000FFF778F9BDF83830ADF810300F9B059371 +:1024C0009DF840308DF81830119B07936946BDF890 +:1024D0004830ADF820302046CDE90265FFF79CFF7B +:1024E0000AB070BD2DE9F041D36905460C46164689 +:1024F0000BB9138C5BBB377E1F2F28D895F8008053 +:10250000B8F1000F26D03046FFF7D0FD3378210216 +:1025100041EAC33141EA0801338A41EA076141EAED +:1025200003410246334641F080012846FFF78AFE08 +:1025300000280ADD3378012B07D172691378013343 +:10254000DBB21F2B88BF00231370BDE8F0816FF052 +:102550000100FAE76FF00300F7E70000F0B58BB079 +:1025600004460D4617460021282268461E46FFF7FE +:1025700019F99DF84C305A1E534253418DF80030E2 +:102580009DF84030ADF81030119B05939DF8483010 +:102590008DF81830149B07936A46BDF85430ADF897 +:1025A000203029462046CDE90276FFF79BFF0BB08D +:1025B000F0BD0000406A00B104307047436A1A68F9 +:1025C000426202691A600361C38A013BC382704799 +:1025D0002DE9F041D0F82080194E14461D464146A1 +:1025E000002709B9BDE8F081D1E90223A21A65EB01 +:1025F0000303964277EB03031ED2036A8B420DD18D +:10260000FFF77EFD036A1B68036203690B60C38AE0 +:102610000161016A013BC3828846E2E7FFF770FD72 +:102620000B68C8F8003003690B60C38A0161013B85 +:10263000C382D8F80010D4E788460968D1E700BF04 +:1026400080841E002DE9F04F8BB00D46DDF85090D0 +:1026500014469B468046002800F01981B9F1000F0E +:1026600000F01581531E3F2B00F21181012A03D186 +:10267000BBF1000F40F00B810023CDE90833B8F81F +:102680001430B5EBC30F4FEAC30703D300200BB0E0 +:10269000BDE8F08F2B199F42D8F80C303ABF7F1B52 +:1026A000FFB227461BB9D8F81030002B7AD0272D5F +:1026B0004ED8C5F12806B7424FF000032CBFF6B242 +:1026C0003E4600932946D8F8080008AB3246FFF78B +:1026D00033FCA7EB060A35445FFA8AFAB8F81430DF +:1026E00003F10053053BDB000493D8F80C3003934F +:1026F0002821039B13B1BAF1000F2CD1D8F8100098 +:1027000040B1BAF1000F05D0009608AB5246691AE5 +:10271000FFF712FC38B2002FB8D066070AD00AAB18 +:1027200003EBD401624211F8083C02F007021341A6 +:1027300001F8083C082C3CD9102C40F2B580202C24 +:1027400040F2B780BBF1000F00F09C80082334E01A +:10275000BA460026C2E7049BE02B28BFE02306937D +:102760000B44AB42059314D95A1B0398009692452B +:1027700034BF5246D2B2691A08AB04300792FFF751 +:10278000DBFB079A1644AAEB020A1544F6B25FFA7D +:102790008AFA049B069A05999B1A0493039B1B686B +:1027A0000393A6E70093D8F8080008AB3A462946F9 +:1027B000AEE7BBF1000F13D00123B4EBC30F6CD015 +:1027C000082C12D89DF82030621E23FA02F2D50799 +:1027D00006D54FF0FF3202FA04F423438DF820307F +:1027E0009DF8203089F8003051E7102C12D8BDF840 +:1027F0002030621E23FA02F2D10706D54FF0FF32D5 +:1028000002FA04F42343ADF82030BDF82030A9F8D3 +:1028100000303CE7202C0FD80899631E21FA03F3FF +:10282000DA0705D54FF0FF3202FA04F40C4308949E +:10283000089BC9F800302AE7402C2BD0DDE9086559 +:10284000611EC4F12102A4F1210326FA01F105FA67 +:1028500002F225FA03F311431943CB0712D50122E3 +:10286000A4F12003C4F1200102FA03F322FA01F1DA +:10287000A240524243EA010363EB430332432B433A +:10288000CDE90823DDE90823C9E90023FFE66FF05D +:102890000100FCE66FF00800F9E6082CA0D9102C26 +:1028A000B3D9202CEED8C3E7BBF1000FADD0022383 +:1028B00083E7BBF1000FBBD004237EE730B5012ACC +:1028C000144638BF0124402C85B028BF4024002581 +:1028D000012ACDE9025518D81B788DF80830630716 +:1028E0000AD004AB03EBD405624215F8083C02F0B1 +:1028F0000702934005F8083C009103462246002158 +:1029000002A8FFF719FB05B030BD082AE4D9102A48 +:1029100003D81B88ADF80830E1E7202A8DBFD3E942 +:1029200000231B680293CDE90223D8E710B5CB68DA +:102930001BB98B600B618B8210BD04691A681C6027 +:102940000361C38A013BC382CA60F0E703064CBF40 +:10295000C0F3C0300220704708B50246FFF7F6FF0B +:10296000022806D15306C2F30F2001D100F0030064 +:1029700008BDC2F30740FBE72DE9F04F93B0CDE966 +:1029800004230A6804461046FFF7E0FF022814BF3C +:10299000C2F306260026002A0D46824680F20282F5 +:1029A00012F0C04940F0FE81097B002900F0FA8155 +:1029B000022803D02378B34240F0F781C2F30463C6 +:1029C0000693104602F07F030393FFF7C5FF039BB6 +:1029D00029444FEA834848EA0A4848EA4668CE78DC +:1029E00000230022CDE90823F309834648EA0008C2 +:1029F000029367D0039B009302466768534608A979 +:102A00002046B847002868D0276A4FB9414604F1EC +:102A10000C00FFF7F5FA074690B96FF002006AE084 +:102A20003B6998450DD03F68002FF9D1414604F12C +:102A30000C00FFF7E5FA07460028EED0236A3B605A +:102A4000276297F817C006F01F08CCF3840CACEB94 +:102A500008001FFA80FE0028B8BF0EF12000A8EB86 +:102A60000C031FFA83FED7E90221B8BF00B2002B86 +:102A70000793BEBF0EF120031BB2079352EA010376 +:102A800042D0049BDFF834E39A1A059B4FF0000C08 +:102A900063EB010196457CEB010335D36B7B97F823 +:102AA0001AE0734521D1029B002B00F0848001289D +:102AB0002ADC786840BBDFF800C3944570EB010363 +:102AC00043D21EE0276A5FB9039B009365685346B3 +:102AD0005A4608A92046A84768BB6FF00B000AE0D9 +:102AE0003B699845ADD03F68EDE7B34890427CEB39 +:102AF000010303D3002013B0BDE8F08F029B002B2D +:102B0000F8D0079B0F2B19DCFA7DB30002F003020B +:102B100003F07C031343FB7539462046FFF7F0FAB8 +:102B20006B7BBB76029B4BB9FB7DC3F38402013206 +:102B300062F38603FB756FF00C00DCE76A7BBB7EFB +:102B40009A42D7D1029B002B35D0B309022B32D049 +:102B5000049BBB60059BFB60142200210DA8FEF7BF +:102B600021FE049B0A93059B0B932B1D0C932B7B3F +:102B7000ADF83EB0013BDBB2ADF83C30069B8DF8C2 +:102B80004230039B8DF8433094F82C308DF840A0F0 +:102B900083F001038DF844308DF84180A3680AA9C1 +:102BA00020469847FB7DC3F38403013303F01F03E2 +:102BB0009B02FB829EE7FB7DC6F34012B2EBD31F64 +:102BC00040F0F680C3F38403434540F0F980029A55 +:102BD0002B7BB609002A52D016F0010661D1032BD7 +:102BE00040F2F180049BBB60059BFB60FB8A66F3AF +:102BF0000903FB822B7BAE1D033BDBB23246394619 +:102C000004F10C00FFF78EFA00280CDA3946204652 +:102C1000FFF776FAFB7DC3F38403013303F01F0350 +:102C20009B02FB82F9E6DDE90884AB883B834FF623 +:102C3000FF73C9F12000A9F1200228FA09F104FA72 +:102C400000F0014324FA02F211431846C9B2FFF71B +:102C5000ABF909F10809B9F1400F0346E9D1B8828F +:102C60002A7B033AD2B23146FFF7B0F9FB7DB88236 +:102C7000DA43C2F3C01262F3C713FB753AE786B9B1 +:102C80002E1D013BDBB23246394604F10C00FFF742 +:102C900049FA0028BADB2A7BB88A013AD2B2314617 +:102CA000E2E7F98AC1F30901013B0429DAB25BD8F2 +:102CB000281D002307F11B069A4208D910F801CB02 +:102CC00006F801C0013101330529DBB2F4D10499C2 +:102CD0000A9105990B91934207F11B010C9138BFA2 +:102CE000043379680D9134BF55FA83F300230E93B2 +:102CF000FB8AADF83EB0C3F309031A44069B8DF876 +:102D00004230039B8DF8433094F82C30ADF83C20D2 +:102D100083F001038DF8443000238DF840A08DF836 +:102D200041807B602A7BB88A013A291DFFF74EF962 +:102D30003B8BB882834203D1A3680AA920469847F7 +:102D400020460AA9FFF7F2FDFB7DBA8AC3F384038C +:102D5000013303F01F039B02FB823B8B9A420CBFA3 +:102D600000206FF01000C6E67B68002BAFD0052076 +:102D700001E01C3033461E68002EFAD1091A081DE6 +:102D80002E1D184401EB090CBCF11B0F5FFA89F3EF +:102D90009DD89A429BD916F8013B00F8013B09F1F6 +:102DA0000109EFE76FF00900A5E66FF00A00A2E65F +:102DB0006FF00D009FE600BF40420F0080841E00B0 +:102DC0006FF00E0097E66FF00F0094E6404BF0B501 +:102DD0001C6C404E44F000741C641D6E45F0007580 +:102DE0001D661B6E3C4B9B6AD3F80052354045F084 +:102DF0000105C3F80052D3F80042344044EA0020F1 +:102E000040F00100C3F80002002952D00020C3F8AE +:102E10001C020546C3F80402C3F80C02C3F81402EE +:102E200003EBC00401301C28C4F84052C4F84452DB +:102E3000F6D100254FF0010C96781488F70748BFAB +:102E4000D3F804720CFA04F044BF0743C3F80472C9 +:102E5000B70742BFD3F80C720743C3F80C7276076A +:102E600042BFD3F814620643C3F8146203EBC404F0 +:102E70005668C4F840629668C4F84462D3F81C42AD +:102E800001352043A942C3F81C0202F10C02D3D140 +:102E9000D3F8002222F00102C3F800220C4B1A6C76 +:102EA00022F000721A641A6E22F000721A661B6E0B +:102EB000F0BD0122C3F84012C3F84412C3F8041253 +:102EC000C3F81412C3F80C22C3F81C22E0E700BFB9 +:102ED000003802400000FFFFE0360020184A916AE7 +:102EE00008B58B688B6013F0010104D013F00C0F50 +:102EF00018BF4FF48031D80506D513F4406F14BFC6 +:102F000041F4003141F00201D80306D513F4402FFB +:102F100014BF41F4802141F00401D3690BB108488A +:102F20009847302383F311880648002100F008FEFB +:102F3000002383F31188BDE8084001F097B900BF72 +:102F4000E0360020E836002038B5124CA36ADD6870 +:102F5000AA0712D05A6922F002025A61A36913B17A +:102F6000012120469847302383F311880A48002125 +:102F700000F0E6FD002383F31188EB0606D5A36A73 +:102F80001021D960236A0BB102489847BDE8384048 +:102F900001F06CB9E0360020F036002038B5124C54 +:102FA000A36A1D69AA0712D05A6922F010025A6159 +:102FB000A36913B1022120469847302383F3118877 +:102FC0000A48002100F0BCFD002383F31188EB06C2 +:102FD00006D5A36A10211961236A0BB102489847EC +:102FE000BDE8384001F042B9E0360020F03600205C +:102FF00038B50F4CA36A5D685D602A070AD50422C4 +:1030000022701A6822F002021A60636A13B100216A +:10301000204698476B0706D5A36A9969236A13B1BE +:10302000034809049847BDE8384001F01FB900BFC4 +:10303000E036002010B50E4C204600F01DFA0D4B76 +:10304000A3620B21132000F0FFF90B21142000F0E4 +:10305000FBF90B21152000F0F7F90B21162000F0E9 +:10306000F3F90022BDE8104011460E20FFF7AEBE76 +:10307000E036002000640040114B984210B5044631 +:1030800009D1104B1A6C42F000721A641A6E42F0A9 +:1030900000721A661B6EA36A01221A60A36A5A683C +:1030A000D20707D5626851681268D9611A60064A6A +:1030B0005A6110BD0121082000F080FCEEE700BF3E +:1030C000E0360020003802405B87010003291AD84F +:1030D000DFE801F0020A0F14836A9B6813F0E05FD7 +:1030E00014BF012000207047836A9868C0F3806095 +:1030F0007047836A9868C0F3C0607047836A9868B5 +:10310000C0F30070704700207047000010B503291D +:1031100025D8DFE801F00225292D836A9968C1F3DB +:103120000161183103EB0113107884064CBF546819 +:103130009488C0F300114FEA410148BF41EAC4013D +:1031400000F00F004CBF41F0040141EA44515860C7 +:1031500041F001019068D2689860DA60196010BD92 +:10316000836A03F5C073DFE7836A03F5C873DBE79F +:10317000836A03F5D073D7E701290AD002290FD05B +:1031800081B9836ADA68920701D1186903E00120E6 +:103190007047836AD86810F0030018BF0120704799 +:1031A000836AF2E70020704710B539B9836AD9689D +:1031B00089071BD11B699C0704D110BD012915D0BB +:1031C0000229FAD1816AD1F8C031D1F8C441D1F8CD +:1031D000C8011061D1F8CC01506120200861086954 +:1031E000800717D1486940F0100012E0816AD1F8D9 +:1031F000B031D1F8B441D1F8B8011061D1F8BC01B7 +:1032000050612020C860C868800703D1486940F039 +:1032100002004861C3F34000C3F38001000140EAAB +:103220004111107920F030000143117189064BBF24 +:1032300091681189DB085B0D4CBF63F31C0163F3DC +:103240000A01137948BF916064F3030313714FEAD5 +:1032500014234FEA144458BF118113705480ACE713 +:10326000026843681143016003B1184770470000CA +:10327000024AD36843F0C003D36070470010014096 +:1032800010B5054C054A0021204600F0FBFA044A1F +:10329000044BC4E9972310BD0C370020713200089D +:1032A0000010014080F0FA02234A037C002918BF75 +:1032B0000A46012B30B5C0F868220CD11F4B98424A +:1032C00009D11F4B596C41F010015964596E41F0FE +:1032D000100159665B6EB2F904501468D0F8603280 +:1032E000D0F85C12002D03EB5403B3FBF4F3BEBF24 +:1032F00023F0070503F0070343EA450394888B6036 +:10330000D38843F040030B61138943F001034B6101 +:1033100044F4045343F02C03CB6004F4A054002382 +:103320000B60B4F5806F0B684B680CBF7F23FF23E5 +:1033300080F8643230BD00BF184800080C37002008 +:10334000003802402DE9F041D0F85C62F76833683C +:10335000DA0504469DB20DD5302383F311884FF46E +:1033600080610430FFF77CFF6FF4807333600023CB +:1033700083F31188302383F3118804F1040815F0D6 +:103380002F033AD183F31188380615D5290613D5B2 +:10339000302383F3118804F1380000F0EFF900289E +:1033A0004EDA0821201DFFF75BFF4FF67F733B408D +:1033B000F360002383F311887A0616D56B0614D5C3 +:1033C000302383F31188D4E913239A420AD1236C62 +:1033D00043B127F040073F041021201D3F0CFFF7A9 +:1033E0003FFFF760002383F31188D4F86822D36885 +:1033F00043B3BDE8F041106918472B0714D015F00E +:10340000080F0CBF00214FF48071E80748BF41F05E +:103410002001AA0748BF41F040016B0748BF41F0B7 +:1034200080014046FFF71CFFAD06736805D594F890 +:1034300064122046194000F055FA3568ADB29EE797 +:103440007060B6E7BDE8F08100F1604303F56143C9 +:103450000901C9B283F80013012200F01F039A404A +:1034600043099B0003F1604303F56143C3F88021E6 +:103470001A607047FFF7DEBD012300F10802C0E9C2 +:103480000222037000F110020023C0E90422C0E907 +:103490000633C0E9083343607047000010B530239D +:1034A000044683F31188022303704160FFF7E4FDB3 +:1034B00004232370002080F3118810BD2DE9F04112 +:1034C0001F4604460D461646302383F3118800F14B +:1034D00008082378052B0DD029462046FFF7F6FD76 +:1034E00040B1204632462946FFF710FE002080F307 +:1034F000118808E03946404600F006FB0028E8D075 +:10350000002383F31188BDE8F08100002DE9F0412C +:103510001F4604460D461646302383F3118800F1FA +:1035200010082378052B0DD029462046FFF724FEEE +:1035300040B1204632462946FFF736FE002080F390 +:10354000118808E03946404600F0DEFA0028E8D04D +:10355000002383F31188BDE8F0810000F8B515461B +:1035600082680669AA420B46816938BF8568761A67 +:10357000B54204460BD218462A46FEF701F9A36964 +:103580002B44A361A3685B1BA3602846F8BD0CD93C +:1035900018463246FEF7F4F8AF1BE1683A4630446D +:1035A000FEF7EEF8E3683B44EBE718462A46FEF7E1 +:1035B000E7F8E368E5E7000083689342F7B515464E +:1035C000044638BF8568D0E90460361AB5420BD28C +:1035D0002A46FEF7D5F863692B446361A368284641 +:1035E0005B1BA36003B0F0BD0DD932460191FEF71D +:1035F000C7F80199E068AF1B3A463144FEF7C0F8BE +:10360000E3683B44E9E72A46FEF7BAF8E368E4E7F3 +:1036100010B50A440024C361029B8460C0E9000025 +:10362000C0E90511C1600261036210BD08B5D0E9AF +:103630000532934201D1826882B982680132826088 +:103640005A1C42611970D0E904329A4224BFC368FF +:103650004361002100F068FA002008BD4FF0FF3000 +:10366000FBE7000070B5302304460E4683F3118853 +:10367000A568A5B1A368A269013BA360531CA3611F +:1036800015782269934224BFE368A361E3690BB113 +:1036900020469847002383F31188284607E03146E7 +:1036A000204600F031FA0028E2DA85F3118870BD77 +:1036B0002DE9F74F04460E4617469846D0F81C9061 +:1036C0004FF0300A8AF311884FF0000B154665B1B0 +:1036D0002A4631462046FFF741FF034660B941467E +:1036E000204600F011FA0028F1D0002383F311885E +:1036F000781B03B0BDE8F08FB9F1000F03D0019043 +:103700002046C847019B8BF31188ED1A1E448AF3AB +:103710001188DCE7C0E90511C160C3611144009B59 +:103720008260C0E90000016103627047F8B5044699 +:103730000D461646302383F31188A768A7B1A36806 +:10374000013BA36063695A1C62611D70D4E90432B5 +:103750009A4224BFE3686361E3690BB1204698474E +:10376000002080F3118807E03146204600F0CCF9B4 +:103770000028E2DA87F31188F8BD0000D0E90523BC +:103780009A4210B501D182687AB9826801328260AA +:103790005A1C82611C7803699A4224BFC368836102 +:1037A000002100F0C1F9204610BD4FF0FF30FBE7CB +:1037B0002DE9F74F04460E4617469846D0F81C9060 +:1037C0004FF0300A8AF311884FF0000B154665B1AF +:1037D0002A4631462046FFF7EFFE034660B94146D0 +:1037E000204600F091F90028F1D0002383F31188DE +:1037F000781B03B0BDE8F08FB9F1000F03D0019042 +:103800002046C847019B8BF31188ED1A1E448AF3AA +:103810001188DCE7026843681143016003B118476F +:10382000704700001430FFF743BF00004FF0FF3334 +:103830001430FFF73DBF00003830FFF7B9BF00007C +:103840004FF0FF333830FFF7B3BF00001430FFF7FD +:1038500009BF00004FF0FF311430FFF703BF000035 +:103860003830FFF763BF00004FF0FF323830FFF70A +:103870005DBF0000012914BF6FF0130000207047E6 +:10388000FFF7FEBC37B515460E4A026000224260C3 +:10389000C0E902220122044602740B46009000F1A6 +:1038A0005C014FF480721430FFF7B2FE00942B4697 +:1038B0004FF4807204F5AE7104F13800FFF72AFF6F +:1038C00003B030BD2C48000810B53023044683F304 +:1038D0001188FFF7E9FC02232374002080F311888C +:1038E00010BD000038B5C36904460D461BB904215C +:1038F0000844FFF78FFF294604F11400FFF796FEF6 +:10390000002806DA201D4FF40061BDE83840FFF7BB +:1039100081BF38BD00230375826803691B689968FD +:103920009142FBD25A680360426010605860704751 +:1039300000230375826803691B6899689142FBD86C +:103940005A680360426010605860704708B50846C6 +:10395000302383F311880B7D032B05D0042B0DD06E +:103960002BB983F3118808BD8B6900221A604FF0D0 +:10397000FF338361FFF7CEFF0023F2E7D1E9003286 +:1039800013605A60F3E70000FFF7C4BF38B50A4B75 +:10399000DD681C68287522681A605360DC60A368C3 +:1039A00001229342227501D100F028FC29462046CD +:1039B000BDE83840FCF7A6BC7839002030B50C4B88 +:1039C000DD684B1C87B004460FD02B46094A684679 +:1039D00000F04AF92046FFF7D9FF009B13B1684673 +:1039E00000F04CF9A86907B030BDFFF7CFFFF9E749 +:1039F000783900204D390008044B1A68DB6890685C +:103A00009B68984294BF00200120704778390020BD +:103A100038B50B4B1C68DD6822681A6053600122C0 +:103A20002275DC60AB68934201D100F0E9FB2846C7 +:103A3000FFF77EFF01462046BDE83840FCF762BC38 +:103A40007839002038B5074C07490848012300257C +:103A50002370656000F070FC0223237085F31188E9 +:103A600038BD00BFE03B00205848000878390020EE +:103A700000F03AB98B60022308618B8208467047D8 +:103A80008368A3F1840243F8142C026943F8442CA0 +:103A9000426943F8402C094A43F8242CC26843F891 +:103AA000182C022203F80C2C002203F80B2C044AD9 +:103AB00043F8102CA3F12000704700BF1D0300083D +:103AC0007839002008B5FFF7DBFFBDE80840FFF7B5 +:103AD0005BBF0000024BDB6898610F20FFF756BF09 +:103AE00078390020302383F31188FFF7F3BF0000FB +:103AF00008B50146302383F311880820FFF75EFFE5 +:103B0000002383F3118808BD064BDB6839B1426896 +:103B100018605A60136043600420FFF74FBF4FF0F6 +:103B2000FF307047783900200368984206D01A6841 +:103B30000260506099611846FFF726BF7047000089 +:103B400038B504460D462068844200D138BD03686C +:103B500023605C608561FFF717FFF4E710B5036829 +:103B60009C68A2420CD85C688A600B604C60216043 +:103B7000596099688A1A9A604FF0FF33836010BDCC +:103B80001B68121BECE700000A2938BF0A2170B538 +:103B900004460D460A26601900F0C8FB00F0B4FB8D +:103BA000041BA54203D8751C2E460446F3E70A2ED3 +:103BB00004D9BDE87040012000F0FEBB70BD0000DC +:103BC000F8B5144B0D46D96103F1100141600A2A82 +:103BD0001969826038BF0A22016048601861A8181C +:103BE000144600F095FB0A2700F08EFB431BA3420E +:103BF000064606D37C1C281900F098FB274635465C +:103C0000F2E70A2F04D9BDE8F840012000F0D4BB48 +:103C1000F8BD00BF78390020F8B506460D4600F023 +:103C200073FB0F4A134653F8107F9F4206D12A4672 +:103C300001463046BDE8F840FFF7C2BFD169BB6816 +:103C4000441A2C1928BF2C46A34202D92946FFF753 +:103C50009BFF224631460348BDE8F840FFF77EBF90 +:103C6000783900208839002010B4C0E903230023EC +:103C70005DF8044B4361FFF7CFBF000010B5194C4E +:103C8000236998420DD0D0E90032816813605A60F0 +:103C90009A680A449A60002303604FF0FF33A361DF +:103CA00010BD2346026843F8102F536000220260C3 +:103CB00022699A4203D1BDE8104000F031BB9368FD +:103CC00081680B44936000F01FFB2269E1699268F0 +:103CD000441AA242E4D91144BDE81040091AFFF782 +:103CE00053BF00BF783900202DE9F047DFF8BC80D2 +:103CF00008F110072C4ED8F8105000F005FBD8F84A +:103D00001C40AA68031B9A423ED81444D5E90032ED +:103D10004FF00009C8F81C4013605A60C5F80090C5 +:103D2000D8F81030B34201D100F0FAFA89F31188C3 +:103D3000D5E9033128469847302383F311886B690E +:103D4000002BD8D000F0E0FA6A69A0EB04094A45DC +:103D500082460DD2022000F02FFB0022D8F810304E +:103D6000B34208D151462846BDE8F047FFF728BFC7 +:103D7000121A2244F2E712EB090938BF4A462946D3 +:103D80003846FFF7EBFEB5E7D8F81030B34208D05D +:103D90001444211AC8F81C00A960BDE8F047FFF7D9 +:103DA000F3BEBDE8F08700BF8839002078390020D5 +:103DB00038B500F0A9FA054AD2E90845031B1819DD +:103DC00045F10001C2E9080138BD00BF7839002083 +:103DD00000207047FEE70000704700004FF0FF3002 +:103DE00070470000BFF34F8F024AD368DB03FCD457 +:103DF000704700BF003C024008B5094B1B7873B9FF +:103E0000FFF7F0FF074B1A69002ABFBF064A5A6046 +:103E100002F188325A601A6822F480621A6008BD82 +:103E2000E83B0020003C02402301674508B50B4BEE +:103E30001B7893B9FFF7D6FF094B1A6942F000428D +:103E40001A611A6842F480521A601A6822F4805289 +:103E50001A601A6842F480621A6008BDE83B0020CC +:103E6000003C02400728F0B516D80C4C0C492378CA +:103E70007BB90C4D0E4608234FF0006255F8047BC9 +:103E800046F8042B013B13F0FF033A44F6D101231B +:103E9000237051F82000F0BD0020FCE70C3C00200E +:103EA000EC3B002064480008014B53F820007047A9 +:103EB0006448000808207047072810B5044601D957 +:103EC000002010BDFFF7CEFF064B53F824301844F6 +:103ED000C21A0BB90120F4E712680132F0D1043B99 +:103EE000F6E700BF64480008072810B5044621D84B +:103EF000FFF778FFFFF780FF0F4AF323D360C3007B +:103F0000DBB243F4007343F002031361136943F41B +:103F100080331361FFF766FFFFF7A4FF074B53F8E9 +:103F2000241000F021F9FFF781FF2046BDE8104082 +:103F3000FFF7C2BF002010BD003C024064480008EB +:103F4000F8B512F00103144642D185182E4A954265 +:103F500057D82E4B1B6813F0010352D02C4DFFF79E +:103F60004BFFF323EB60FFF73DFF40F20127032CEB +:103F700015D824F001046618254C401A40F20117A8 +:103F8000B142236900EB010524D123F00103236131 +:103F9000FFF74CFF0120F8BD043C0430E7E783073E +:103FA000E7D12B6923F440732B612B693B432B61D1 +:103FB00051F8046B0660BFF34F8FFFF713FF0368E0 +:103FC0009E42E9D02B6923F001032B61FFF72EFFFE +:103FD0000020E0E723F44073236123693B4323611E +:103FE0000B882B80BFF34F8FFFF7FCFE2D8831F835 +:103FF000023BADB2AB42C3D0236923F0010323617E +:10400000E4E71846C7E700BF000008080038024090 +:10401000003C0240084908B50B7828B11BB9FFF7EE +:10402000EBFE01230B7008BD002BFCD0BDE808405F +:104030000870FFF7FBBE00BFE83B002038B5EFF388 +:1040400011859DB9EFF30584C4F30804302334B11E +:1040500083F31188FFF7ACFE85F3118838BD83F335 +:104060001188FFF7A5FE84F3118838BDBDE83840FC +:10407000FFF79EBE38B5FFF7E1FF114C114AC008AB +:1040800040EA4170A0FB025EC908A0FB040C1CEBD7 +:10409000050CA1FB04404FF000035B41A1FB0212A1 +:1040A0001CEB040C43EB000011EB0E0142F100028B +:1040B000411842F10000090941EA007038BD00BF13 +:1040C000CFF753E3A59BC42010B50244064BD2B2F0 +:1040D000904200D110BD441C00B253F8200041F8BA +:1040E000040BE0B2F4E700BF502800400F4B30B59E +:1040F0001C6F240407D41C6F44F400741C671C6FED +:1041000044F400441C670A4C236843F48073236022 +:104110000244084BD2B2904200D130BD441C00B2E0 +:1041200051F8045B43F82050E0B2F4E70038024055 +:10413000007000405028004007B5012201A900206E +:10414000FFF7C2FF019803B05DF804FB13B5044606 +:10415000FFF7F2FFA04205D0012201A9002001943F +:10416000FFF7C4FF02B010BD70470000074B45F2D7 +:1041700055521A6003225A6040F2FF729A604CF660 +:10418000CC421A60024B01221A7070470030004086 +:10419000143C0020034B1B781BB1034B4AF6AA22A8 +:1041A0001A607047143C002000300040034B1A682E +:1041B0001AB9034AD2F874281A607047103C0020DC +:1041C00000300240024B4FF08072C3F874287047F1 +:1041D0000030024008B5FFF7E9FF024B1868C0F352 +:1041E000407008BD103C002008B5FFF7DFFF024B10 +:1041F0001868C0F3007008BD103C00207047000034 +:1042000070470000EFF3098305494A6B22F0010271 +:104210004A63683383F30988002383F31188704766 +:1042200000EF00E0302080F3118862B60C4B0D4A9D +:10423000D96821F4E0610904090C0A43DA60D3F873 +:10424000FC20094942F08072C3F8FC200A6842F061 +:1042500001020A602022DA7783F82200704700BF4B +:1042600000ED00E00003FA05001000E010B5302377 +:1042700083F311880E4B5B6813F4006314D0F1EEE6 +:10428000103AEFF30984683C4FF08073E361094B07 +:10429000DB6B236684F30988FFF7AEFB10B1064B96 +:1042A000A36110BD054BFBE783F31188F9E700BF5D +:1042B00000ED00E000EF00E02F03000832030008EB +:1042C00070470000FEE700000A4B0B480B4A904283 +:1042D0000BD30B4BDA1C121AC11E22F003028B42C5 +:1042E00038BF00220021FDF75DBA53F8041B40F8E7 +:1042F000041BECE79C490008183C0020183C0020F7 +:10430000183C002000F0CAB84FF08043586A70474C +:104310004FF08043002258631A610222DA6070472E +:104320004FF080430022DA60704700004FF0804376 +:1043300058637047FEE7000070B51B4B0163002512 +:10434000044686B0586085620E46FFF76BFF04F1A5 +:104350001003C4E904334FF0FF33C4E90635C4E960 +:104360000044A560E562FFF7CFFF2B460246C4E993 +:10437000082304F134010D4A256580232046FFF708 +:1043800079FB0123E0600A4A03750092726801928A +:10439000B268CDE90223074B6846CDE90435FFF743 +:1043A00091FB06B070BD00BFE03B002084480008D0 +:1043B0008948000835430008024AD36A1843D0628E +:1043C000704700BF783900204B6843608B6883607A +:1043D000CB68C3600B6943614B6903628B694362BD +:1043E0000B6803607047000008B5234B23481A6927 +:1043F00042F0FF021A611A6922F0FF021A611A697B +:104400001A6B42F0FF021A631A6D42F0FF021A653E +:104410001B4A1B6D1146FFF7D7FF02F11C0100F587 +:104420008060FFF7D1FF02F1380100F58060FFF7EF +:10443000CBFF02F1540100F58060FFF7C5FF02F1E8 +:10444000700100F58060FFF7BFFF02F18C0100F5FD +:104450008060FFF7B9FF02F1A80100F58060FFF767 +:10446000B3FF02F1C40100F58060FFF7ADFFBDE8C6 +:10447000084000F08DB800BF003802400000024044 +:104480009048000808B500F02FF9FFF7DBFABDE807 +:104490000840FFF78BBE0000704700000F4B1A6CFE +:1044A00042F001021A641A6E42F001021A660C4AC6 +:1044B0001B6E936843F0010393604FF080433122F9 +:1044C0009A624FF0FF32DA6200229A615A63DA6030 +:1044D0005A6001225A611A60704700BF00380240DA +:1044E000002004E04FF0804208B51169D3680B400A +:1044F000D9B2C9439B07116107D5302383F31188D3 +:10450000FFF7B6FA002383F3118808BD1E4B1A6922 +:1045100062F0FF021A611A69D2B21A614FF0FF30DD +:104520001A695A69586100215A6959615A691A6AA7 +:1045300062F080521A621A6A02F080521A621A6A93 +:104540005A6A58625A6A59625A6A1A6C42F0805220 +:104550001A641A6E42F080521A661A6E0B4A10687C +:1045600040F480701060186F00F44070B0F5007F68 +:104570001EBF4FF4803018671967536823F4007327 +:10458000536000F0A5B800BF003802400070004042 +:104590003B4B3C4A1A643C4A4FF4404111601A6854 +:1045A00042F001021A601A689007FCD59A6822F05E +:1045B00003029A60324B9A6812F00C02FBD1196820 +:1045C00001F0F90119609A601A6842F480321A60A9 +:1045D0001A689103FCD55A6F42F001025A67284BC2 +:1045E0005A6F9207FCD5294A5A601A6842F08072C5 +:1045F0001A60254A53685804FCD5214B1A6891016A +:10460000FCD5234AC3F884201A6842F080621A60FD +:104610001A681201FCD51F4A9A600322C3F88C2045 +:104620004FF00062C3F894201B4B1A681B4B9A4250 +:104630001B4B21D11B4A11681B4A91421CD140F2ED +:1046400003121A60164A136803F00F03032BFAD102 +:104650000B4B9A6842F002029A609A6802F00C02D0 +:10466000082AFAD15A6C42F480425A645A6E42F4D3 +:1046700080425A665B6E704740F20372E1E700BF0A +:10468000003802400004001000700040041940028D +:104690000830002400948838002004E011640020D1 +:1046A000003C024000ED00E041C20F4108B5034864 +:1046B000FEF748FEBDE80840FFF7D8BD0C370020E4 +:1046C00008B5FFF70FFFBDE80840FFF7CFBD0000BA +:1046D00008B507211C20FEF7B7FEBDE808400C21F5 +:1046E0002520FEF7B1BE000008B5FFF70FFF00F070 +:1046F0000BF8FEF7BFFEFFF7C3F8FFF7CDFEBDE8EE +:104700000840FFF7FFBD00007047000010B50139F9 +:104710000244904201D1002005E0037811F8014FD6 +:10472000A34201D0181B10BD0130F2E72DE9F04182 +:10473000A3B1C91A17780144044603F1FF3C8C4227 +:10474000204601D9002009E00578BD4204F10104AA +:10475000F5D10CEB0405D618A54201D1BDE8F081D6 +:1047600015F8018D16F801EDF045F5D0E7E70000EA +:10477000034611F8012B03F8012B002AF9D17047E9 +:104780001210000012110000121300006F72672E49 +:104790006172647570696C6F742E43534B595F502E +:1047A0004D55000053544D3332463F3F3F00535464 +:1047B0004D3332463430780053544D333246343220 +:1047C000780053544D333246343436585800000084 +:1047D000012033000010410001105A00031059005D +:1047E0000710310000000000A44700081304000077 +:1047F000AE47000819040000B84700082104000073 +:10480000C247000840A2E4F1646891060041A3E5B4 +:10481000F2656992070000000096000000000000A9 +:104820000000000000000000000000000000000088 +:10483000413800082D38000869380008553800084C +:10484000613800084D38000839380008253800085C +:10485000753800086330000054480008D039002043 +:10486000E03B00200040000000400000004000004D +:1048700000400000000001000000020000000200F3 +:10488000000002006D61696E0069646C65000000E3 +:104890000001002800000000AAAAAAAA0001002422 +:1048A000FFFF0000000000000000000000A40A104C +:1048B00000000000AAA6AAAA00500000DFFF000026 +:1048C00000000077880000000000000000000000E9 +:1048D000AAAAAAAA00000000FFFF00000000000032 +:1048E000000000000000000000000000AAAAAAAA20 +:1048F00000000000FFFF00000000000000000000BA +:104900000000000000000000AAAAAAAA00000000FF +:10491000FFFF000000000000000000000000000099 +:1049200000000000AAAAAAAA00000000FFFF0000E1 +:104930000000000000000000000000000000000077 +:10494000AAAAAAAA00000000FFFF000000000000C1 +:104950000000000000000000000000000A0000004D +:104960000000000003000000000000000000000044 +:10497000BC04000000000000000007000000000070 +:10498000640000000000000040420F00FE2A010009 +:0C499000D2040000FF0000000C370020E3 +:00000001FF diff --git a/Tools/bootloaders/MatekH7A3-Wing_bl.bin b/Tools/bootloaders/MatekH7A3-Wing_bl.bin new file mode 100755 index 0000000000000..a5cd2112dedd1 Binary files /dev/null and b/Tools/bootloaders/MatekH7A3-Wing_bl.bin differ diff --git a/Tools/bootloaders/MatekH7A3-Wing_bl.elf b/Tools/bootloaders/MatekH7A3-Wing_bl.elf new file mode 100755 index 0000000000000..473be4053c51b Binary files /dev/null and b/Tools/bootloaders/MatekH7A3-Wing_bl.elf differ diff --git a/Tools/bootloaders/MatekH7A3-Wing_bl.hex b/Tools/bootloaders/MatekH7A3-Wing_bl.hex new file mode 100644 index 0000000000000..81187efa78f14 --- /dev/null +++ b/Tools/bootloaders/MatekH7A3-Wing_bl.hex @@ -0,0 +1,1124 @@ +:020000040800F2 +:1000000000060024E1020008E3020008E302000801 +:10001000E3020008E3020008E3020008E30200082C +:10002000E3020008E3020008E3020008D5390008F3 +:10003000E3020008E3020008E3020008E30200080C +:10004000E3020008E3020008E3020008E3020008FC +:10005000E3020008E3020008E3020008E3020008EC +:10006000E3020008E3020008E3020008253E00085E +:100070004D3E0008793E0008A53E0008D13E00082C +:10008000F93E0008253F0008E3020008E3020008EB +:10009000E3020008E3020008E3020008E3020008AC +:1000A000E3020008E3020008E3020008E30200089C +:1000B000E3020008E3020008E3020008E30200088C +:1000C000E3020008E3020008E3020008E30200087C +:1000D000E3020008BD3D0008E3020008E302000857 +:1000E000E3020008E3020008E3020008E30200085C +:1000F000E3020008E3020008E3020008513F0008A1 +:10010000E3020008E3020008D13D0008E302000812 +:10011000E3020008E3020008E3020008E30200082B +:100120007D3F0008A53F0008D13F0008FD3F0008C3 +:1001300029400008E3020008E3020008E302000887 +:10014000E3020008E3020008E3020008E3020008FB +:10015000514000087D400008A9400008E302000863 +:10016000E3020008E3020008E3020008E3020008DB +:10017000E302000839300008E3020008E302000847 +:10018000E3020008E3020008E3020008E3020008BB +:10019000E3020008E3020008E3020008E3020008AB +:1001A000E3020008E3020008E3020008E30200089B +:1001B000E3020008E3020008E3020008E30200088B +:1001C000E3020008E3020008E3020008E30200087B +:1001D000E3020008E3020008E3020008E30200086B +:1001E000E3020008E3020008E3020008E30200085B +:1001F000E3020008E3020008E3020008E30200084B +:10020000E3020008E3020008E3020008E30200083A +:10021000E3020008E3020008E3020008E30200082A +:10022000E3020008E3020008E3020008E30200081A +:10023000E3020008E3020008E3020008E30200080A +:10024000E3020008E3020008E3020008E3020008FA +:10025000E3020008E3020008E3020008E3020008EA +:10026000E3020008E3020008E3020008E3020008DA +:10027000E3020008E3020008E3020008E3020008CA +:10028000E3020008E3020008E3020008E3020008BA +:10029000E3020008E3020008E3020008E3020008AA +:1002A000E3020008E3020008E3020008E30200089A +:1002B000E3020008E3020008E3020008E30200088A +:1002C000E3020008E3020008E3020008E30200087A +:1002D000E3020008E3020008E3020008E30200086A +:1002E00002E000F000F8FEE772B6374880F30888B5 +:1002F000364880F3098836483649086040F20000E5 +:10030000CCF200004EF63471CEF200010860BFF36B +:100310004F8FBFF36F8F40F20000C0F2F0004EF637 +:100320008851CEF200010860BFF34F8FBFF36F8F8B +:100330004FF00000E1EE100A4EF63C71CEF20001E3 +:100340000860062080F31488BFF36F8F02F0A8FACC +:1003500003F07CFA4FF055301F491B4A91423CBFD5 +:1003600041F8040BFAE71D49184A91423CBF41F895 +:10037000040BFAE71A491B4A1B4B9A423EBF51F83D +:10038000040B42F8040BF8E700201849184A914280 +:100390003CBF41F8040BFAE702F0C0FA03F0E4FABC +:1003A000144C154DAC4203DA54F8041B8847F9E7A6 +:1003B00000F042F8114C124DAC4203DA54F8041B21 +:1003C0008847F9E702F0A8BA0006002400220024BA +:1003D0000000000808ED00E00000002400060024F2 +:1003E000B045000800220024642200246822002472 +:1003F0002C470024E0020008E0020008E0020008A8 +:10040000E00200082DE9F04F2DED108AC1F80CD064 +:10041000D0F80CD0BDEC108ABDE8F08F002383F338 +:1004200011882846A047002001F012FEFEE701F0E7 +:100430009BFD00DFFEE7000008B500F01BFC00F0AC +:1004400085FD41F2883000F003F900F065F8F8E727 +:1004500008B500F0DBFBA0F120035842584108BD6D +:1004600007B5042101900DEB010000F0EDFB03B096 +:100470005DF804FB38B5302383F31188174803680F +:100480000BB101F0B1FE0023154A4FF47A71134805 +:1004900001F0A0FE002383F31188124C236813B1EE +:1004A0002368013B2360636813B16368013B6360A9 +:1004B0000D4D2B7833B963687BB9022000F0B0FC96 +:1004C000322363602B78032B07D163682BB902209A +:1004D00000F0A6FC4FF47A73636038BD68220024F4 +:1004E000750400088823002480220024084B18701B +:1004F00003280CD8DFE800F008050208022000F00D +:1005000085BC022000F078BC024B00225A60704784 +:100510008022002488230024F8B5484B484A1C4612 +:100520001968013100F0898004339342F8D1626880 +:10053000A242C0F08280434B9B6803F1006303F545 +:1005400000439A4279D2002000F0BAFB0220FFF764 +:10055000CDFF3D4B0021D3F84821C3F84811D3F813 +:100560007021C3F87011D3F87021D3F84C21C3F86F +:100570004C11D3F87421C3F87411D3F87421D3F853 +:100580005021C3F85011D3F87821C3F87811D3F86B +:100590007821D3F8802042F00072C3F88020D3F88D +:1005A000802022F00072C3F88020D3F8803072B629 +:1005B0004FF0E023C3F8084DD4E90004BFF34F8F98 +:1005C000BFF36F8F214AC2F88410BFF34F8F536976 +:1005D00023F480335361BFF34F8FD2F8803043F65A +:1005E000E076C3F3C905C3F34E335B0103EA060C9F +:1005F00029464CEA81770139C2F87472F9D2203B5E +:1006000013F1200FF2D1BFF34F8FBFF36F8FBFF302 +:100610004F8FBFF36F8F536923F40033536100236F +:10062000C2F85032BFF34F8FBFF36F8F302383F385 +:100630001188854680F308882047F8BD00800008AF +:1006400020800008002200240044025800ED00E051 +:100650002DE9F04F95B0DFF8D8922022FF210290CB +:1006600004A8D9F8085000F01BFCA84A1378A3B9D5 +:100670000121A74811700360302383F311880368B8 +:100680000BB101F0B1FD0023A24A4FF47A71A048EA +:1006900001F0A0FD002383F31188029B13B19E4B50 +:1006A000029A1A609D4A1378032B03D0002313701B +:1006B000994A53604FF0000B029C5E46DA46CDF833 +:1006C00004B0012000F0A2FB24B1934B1B68002B67 +:1006D00000F02882002000F099FAB0F10008F3DB66 +:1006E000012000F089FBA8F121031F2BE9D801A20A +:1006F00052F823F075070008910700081F0800084A +:10070000C3060008C3060008C3060008A9080008BD +:100710009B0A00088D090008E90900080F0A000873 +:10072000350A0008C3060008470A00086B0A0008DB +:10073000E30A0008FD070008C30600082B0B0008A9 +:1007400081070008FD070008C3060008E909000842 +:10075000C3060008C3060008C3060008C306000855 +:10076000C3060008C3060008C3060008C306000845 +:100770001F0800080220FFF76BFE002840F0FF81F1 +:10078000029B02216648BBF1000F08BF1C4640E0F7 +:100790004FF47A7000F03AFA071EF1DB0220FFF7FF +:1007A00057FE0028ECD0013F052F00F2EA81DFE878 +:1007B00007F003070A0D10330520FFF751FE14E080 +:1007C000D9F80000F9E7D9F80400F6E7D9F80800ED +:1007D000F3E74FF01C08404608F1040800F072FAF5 +:1007E000FFF73EFEB8F12C0FF5D10120019B4FF031 +:1007F000000A00FA07F71F43FBB2019300F09CFBCD +:100800002EB1019B03F00B030B2B08BF0024022128 +:10081000444800F019FA54E7D9F80C00CDE7002E4F +:10082000AED0019B03F00B030B2BA9D10220FFF7E5 +:100830000FFE07460028A3D00120002600F040FA52 +:100840000220FFF753FE1FFA86FB584600F048FAD5 +:10085000044688B1A8F14002584601365142514140 +:1008600000F04EFA0028EED1BB46044602212E4885 +:100870003E4600F0E9F924E725460120FFF736FE61 +:10088000D9F80830AB4207D9284600F01BFA0130EE +:1008900040F07A810435F3E70025204BBB463E4605 +:1008A0001D701D4B5D60A9E7002E3FF469AF019BF1 +:1008B00003F00B030B2B7FF463AF0220FFF716FE50 +:1008C000322000F0A3F9B0F10008FFF659AF18F09C +:1008D00003077FF455AF08EB0503D9F808209342CE +:1008E0003FF64EAFB8F5807F3FF74AAF0F4BB845A4 +:1008F00003931FDD4FF47A7000F088F90028FFF6AB +:100900003FAF039B013703F8010BF0E7842300247A +:100910006822002475040008882300248022002413 +:10092000644100085C410008604100088422002402 +:1009300000220024C820FFF78BFD074600283FF463 +:100940001FAF1F2D11D8C5F1200204AB25F0030005 +:100950008F494245184428BF4246039200F07AFA74 +:10096000039AFF218A4800F09BFA4FEAA803C8F3D4 +:10097000870287492846039300F09AFA0646002822 +:100980003FF474AF039B05EB830537E70220FFF7C5 +:100990005FFD00283FF4F4AE00F0DEF900283FF4DC +:1009A000EFAE0027B846D9F80830BB4218D91F2F40 +:1009B00011D8049B01330ED027F0030314AA13446B +:1009C00053F8403C0C93404604220CA9043700F035 +:1009D0001FFB8046E7E7384600F074F90C90F2E719 +:1009E0004046FFF73DFD09E7002364210CA80C9366 +:1009F00000F020F900287FF4C3AE0220FFF728FDA5 +:100A000000283FF4BDAE0C9800F08EF9E9E7002312 +:100A100064210CA80C9300F00DF900287FF4B0AE0F +:100A20000220FFF715FD00283FF4AAAE0C9800F055 +:100A30008BF9D6E70220FFF70BFD00283FF4A0AEAC +:100A400000F09AF9CDE70220FFF702FD00283FF4FD +:100A500097AE0CA9142000F095F90390FFF700FD64 +:100A600003990CA800F0F0F8C8E60220FFF7F0FCAC +:100A700000283FF485AE474B0CAF03F1100C18680B +:100A8000083353F8041C3A46634503C21746F6D1AF +:100A90001020FFF7E5FC1021E3E7322000F0B6F864 +:100AA000071EFFF66DAEBB077FF46AAE07EB0A03C5 +:100AB000D9F8082093423FF663AE0220FFF7C8FC46 +:100AC00000283FF45DAE27F003075744BA453FF4D2 +:100AD00095AE50460AF1040A00F0F4F8FFF7C0FCA6 +:100AE000F4E74FF47A70FFF7B3FC00283FF448AE08 +:100AF00000F032F9002849D0049B01330BD00822C2 +:100B000004A9002000F0D4F900283FD02022FF21C2 +:100B100004A800F0C5F92048022100F095F81F480C +:100B200001F09CFA15B0BDE8F08F002E3FF428AE1E +:100B3000019B03F00B030B2B7FF422AE00236421F7 +:100B40000CA80C9300F076F8074600287FF418AE46 +:100B50000220FFF77DFC804600283FF411AE022101 +:100B60000D4800F071F841F2883001F077FA0C98E6 +:100B700000F02CFA46463C4600F0DEF9A1E50646B8 +:100B80003CE64FF0000AFBE5BB466FE637466DE6F4 +:100B900084220024484100085C410008A08601002E +:100BA0002DE9F84F4FF47A7306460D46002402FBF8 +:100BB00003F7DFF85080DFF8509098F900305FFAC3 +:100BC00084FA5A1C01D0A34212D159F824002A46B3 +:100BD00031460368D3F820B03B46D847854207D159 +:100BE000074B012083F800A0BDE8F88F0124E4E75B +:100BF000002CFBD04FF4FA7001F030FA0020F3E73C +:100C0000D42300241022002414220024002307B53A +:100C1000024601210DF107008DF80730FFF7C0FFF4 +:100C200020B19DF8070003B05DF804FB4FF0FF30E2 +:100C3000F9E700000A46042108B5FFF7B1FF80F08C +:100C40000100C0B2404208BD074B0A4630B41978D3 +:100C5000064B53F82140014623682046DD69044BCA +:100C6000AC4630BC604700BFD423002414220024CB +:100C7000A086010070B5104C0025104E01F00AFD51 +:100C800020803068238883420CD8002520880138D2 +:100C900001F0FCFC23880544013BB5F5004F23809F +:100CA000F4D370BD01F0F2FC336805440133B5F5AF +:100CB000004F3360E5D3E8E7D623002490230024D7 +:100CC00001F0C4BD00F1006000F50040006870470D +:100CD00000F10060920000F5004001F039BD000015 +:100CE000054B1A68054B1B889B1A834202D9104496 +:100CF00001F0CCBC0020704790230024D6230024B0 +:100D000038B50446074D29B128682044BDE838406D +:100D100001F0D6BC2868204401F0C0FC0028F3D0C4 +:100D200038BD00BF9023002410F0030309D1B0F5B3 +:100D3000806F04D200F10F6000F57F200368184631 +:100D400070470023FBE7000000F10F6000F57F20F3 +:100D5000D0F8000870470000064991F8243033B1FC +:100D600000230822086A81F82430FFF7B1BF012070 +:100D7000704700BF94230024014B1868704700BFE0 +:100D80000010005C194B01380322084470B51D683F +:100D9000174BC5F30B042D0C1E88A6420BD15C68C3 +:100DA0000A46013C824213460FD214F9016F4EB13C +:100DB00002F8016BF6E7013A03F10803ECD1814236 +:100DC0000B4602D22C2203F8012B0424094A168870 +:100DD000AE4204D1984284BF967803F8016B013C7F +:100DE00002F10402F3D1581A70BD00BF0010005C7C +:100DF0001C22002494410008022803D1024B4FF02A +:100E000080429A61704700BF00000258022803D157 +:100E1000024B4FF480429A61704700BF00000258B5 +:100E2000022804D1024A536983F480435361704716 +:100E30000000025870B504464FF47A764CB1412C4C +:100E4000254628BF412506FB05F0641B01F006F985 +:100E5000F4E770BD002310B5934203D0CC5CC454BA +:100E60000133F9E710BD0000013810B510F9013F5A +:100E70003BB191F900409C4203D11AB10131013AD2 +:100E8000F4E71AB191F90020981A10BD1046FCE75A +:100E900003460246D01A12F9011B0029FAD1704705 +:100EA00002440346934202D003F8011BFAE770475D +:100EB0002DE9F8431F4D14460746884695F824202F +:100EC00052BBDFF870909CB395F824302BB92022E8 +:100ED000FF2148462F62FFF7E3FF95F824004146C3 +:100EE000C0F1080205EB8000A24228BF2246D6B21C +:100EF0009200FFF7AFFF95F82430A41B17441E445F +:100F00009044E4B2F6B2082E85F82460DBD1FFF7F6 +:100F100023FF0028D7D108E02B6A03EB820383422A +:100F2000CFD0FFF719FF0028CBD10020BDE8F88310 +:100F30000120FBE794230024024B1A78024B1A701D +:100F4000704700BFD42300241022002438B51A4C67 +:100F50001A4D204600F0C6FB2946204600F0EEFB65 +:100F60002D684FF47A70D5F89020D2F8043843F009 +:100F70000203C2F80438FFF75DFF1149284600F06C +:100F8000EBFCD5F890200F4DD2F80438286823F0F8 +:100F900002030D49A042C2F804384FF4E1330B605C +:100FA00001D000F0FDFA6868A04204D00649BDE80F +:100FB000384000F0F5BA38BDB82A00249042000845 +:100FC0009842000814220024BC2300240C4B70B566 +:100FD0000C4D04461E780C4B55F826209A420DD035 +:100FE0000A4B002118221846FFF75AFF04600146F9 +:100FF00055F82600BDE8704000F0D2BA70BD00BFC1 +:10100000D423002414220024B82A0024BC23002462 +:1010100030B50A44084D91420DD011F8013B5840BB +:10102000082340F30004013B2C4013F0FF0384EA43 +:101030005000F6D1EFE730BD2083B8ED0268436879 +:101040001143016003B1184770470000024A13685A +:1010500043F0C003136070470010014013B50E4CFD +:10106000204600F091FA04F1140000234FF40072BE +:101070000A49009400F04EF9094B4FF400720949F7 +:1010800004F13800009400F0C7F9074A074BC4E99F +:10109000172302B010BD00BFD8230024442400242D +:1010A0004D10000844260024001001400076B010C6 +:1010B000037C30B5244C002918BF0C46012B11D1FC +:1010C000224B98420ED1224BD3F8502142F010020D +:1010D000C3F85021D3F8782142F01002C3F87821E8 +:1010E000D3F878312268036EC16D03EB5203846636 +:1010F000B3FBF2F36268150442BF23F0070503F067 +:10110000070343EA4503CB60A36843F040034B6009 +:10111000E36843F001038B6042F4967343F00103EC +:101120000B604FF0FF330B62510505D512F0102212 +:1011300005D0B2F1805F04D080F8643030BD7F23E9 +:10114000FAE73F23F8E700BFA4410008D8230024B2 +:10115000004402582DE9F047C66D05463768F4692A +:10116000210734621AD014F0080118BF4FF48071BF +:10117000E20748BF41F02001A3074FF0300348BF0A +:1011800041F04001600748BF41F0800183F31188BE +:10119000281DFFF753FF002383F31188E2050AD5CA +:1011A000302383F311884FF48061281DFFF746FF39 +:1011B000002383F311884FF030094FF0000A14F038 +:1011C000200838D13B0616D54FF0300905F1380A12 +:1011D000200610D589F31188504600F051F90028F7 +:1011E00036DA0821281DFFF729FF27F08003336036 +:1011F000002383F31188790614D5620612D53023B3 +:1012000083F31188D5E913239A4208D12B6C33B1AB +:1012100027F040071021281DFFF710FF376000233B +:1012200083F31188E30618D5AA6E1369ABB1506930 +:10123000BDE8F047184789F31188736A284695F886 +:101240006410194000F0BAF98AF31188F469B6E71E +:10125000B06288F31188F469BAE7BDE8F08700004E +:10126000F8B51546826804460B46AA4200D2856846 +:10127000A1692669761AB5420BD218462A46FFF7AD +:10128000E9FDA3692B44A3612846A3685B1BA36007 +:10129000F8BD0CD9AF1B18463246FFF7DBFD3A46C6 +:1012A000E1683044FFF7D6FDE3683B44EBE71846BE +:1012B0002A46FFF7CFFDE368E5E700008368934225 +:1012C000F7B50446154600D28568D4E90460361A9D +:1012D000B5420BD22A46FFF7BDFD63692B4463611B +:1012E0002846A3685B1BA36003B0F0BD0DD932464E +:1012F000AF1B0191FFF7AEFD01993A46E06831441A +:10130000FFF7A8FDE3683B44E9E72A46FFF7A2FDA3 +:10131000E368E4E710B50A440024C361029B8460DB +:10132000C16002610362C0E90000C0E9051110BD9F +:1013300008B5D0E90532934201D1826882B982684A +:10134000013282605A1C426119700021D0E90432D6 +:101350009A4224BFC368436100F09CFE002008BD90 +:101360004FF0FF30FBE7000070B5302304460E4617 +:1013700083F31188A568A5B1A368A269013BA360A6 +:10138000531CA36115782269934224BFE368A361CB +:10139000E3690BB120469847002383F31188284660 +:1013A00007E03146204600F065FE0028E2DA85F3CA +:1013B000118870BD2DE9F74F04460E461746984632 +:1013C000D0F81C904FF0300A8AF311884FF0000BD0 +:1013D000154665B12A4631462046FFF741FF0346D0 +:1013E00060B94146204600F045FE0028F1D00023B8 +:1013F00083F31188781B03B0BDE8F08FB9F1000FBB +:1014000003D001902046C847019B8BF31188ED1A49 +:101410001E448AF31188DCE7C160C361009B8260CF +:101420000362C0E905111144C0E900000161704781 +:10143000F8B504460D461646302383F31188A76895 +:10144000A7B1A368013BA36063695A1C62611D7068 +:10145000D4E904329A4224BFE3686361E3690BB1C3 +:1014600020469847002080F3118807E03146204647 +:1014700000F000FE0028E2DA87F31188F8BD0000D2 +:10148000D0E9052310B59A4201D182687AB9826801 +:101490000021013282605A1C82611C7803699A42E1 +:1014A00024BFC368836100F0F5FD204610BD4FF0F6 +:1014B000FF30FBE72DE9F74F04460E4617469846E6 +:1014C000D0F81C904FF0300A8AF311884FF0000BCF +:1014D000154665B12A4631462046FFF7EFFE034622 +:1014E00060B94146204600F0C5FD0028F1D0002338 +:1014F00083F31188781B03B0BDE8F08FB9F1000FBA +:1015000003D001902046C847019B8BF31188ED1A48 +:101510001E448AF31188DCE70268436811430160C6 +:1015200003B11847704700001430FFF743BF0000B5 +:101530004FF0FF331430FFF73DBF00003830FFF7A6 +:10154000B9BF00004FF0FF333830FFF7B3BF0000E2 +:101550001430FFF709BF00004FF0FF311430FFF7E0 +:1015600003BF00003830FFF763BF00004FF0FF32C9 +:101570003830FFF75DBF0000012914BF6FF0130082 +:1015800000207047FFF76ABD044B036000234360EF +:10159000C0E9023301230374704700BFBC41000857 +:1015A00010B53023044683F31188FFF781FD022331 +:1015B0000020237480F3118810BD000038B5C36982 +:1015C00004460D461BB904210844FFF7A5FF294630 +:1015D00004F11400FFF7ACFE002806DA201D4FF4DA +:1015E0000061BDE83840FFF797BF38BD0268436827 +:1015F0001143016003B118477047000013B5406BF9 +:1016000000F58054D4F8A4381A681178042914D14C +:10161000017C022911D11979012312898B401342CF +:101620000BD101A94C3001F0C7FFD4F8A448024601 +:10163000019B2179206800F0DFF902B010BD0000A5 +:10164000143001F049BF00004FF0FF33143001F0B7 +:1016500043BF00004C3002F01BB800004FF0FF33D6 +:101660004C3002F015B80000143001F017BF000034 +:101670004FF0FF31143001F011BF00004C3001F089 +:10168000E7BF00004FF0FF324C3001F0E1BF000037 +:101690000020704710B500F58054D4F8A4381A68BB +:1016A0001178042917D1017C022914D15979012319 +:1016B00052898B4013420ED1143001F0A9FE02462C +:1016C00048B1D4F8A4484FF4407361792068BDE86C +:1016D000104000F07FB910BD406BFFF7DBBF00008A +:1016E000704700007FB5124B0125042604460360B5 +:1016F0000023057400F1840243602946C0E90233E7 +:101700000C4B0290143001934FF44073009601F09B +:101710005BFE094B04F69442294604F14C00029406 +:10172000CDE900634FF4407301F022FF04B070BDB7 +:10173000E4410008D9160008FD1500080A683023A6 +:1017400083F311880B790B3342F823004B79133361 +:1017500042F823008B7913B10B3342F8230000F5D4 +:101760008053C3F8A41802230374002080F3118867 +:101770007047000038B5037F044613B190F8543029 +:10178000ABB90125201D0221FFF730FF04F1140041 +:101790006FF00101257700F089FC04F14C0084F81A +:1017A00054506FF00101BDE8384000F07FBC38BDF7 +:1017B00010B5012104460430FFF718FF00232377FA +:1017C00084F8543010BD000038B5044600251430AC +:1017D00001F012FE04F14C00257701F0E1FE201D1E +:1017E00084F854500121FFF701FF2046BDE838403E +:1017F000FFF750BF90F8803003F06003202B06D134 +:1018000090F881200023212A03D81F2A06D800201F +:101810007047222AFBD1C0E91D3303E0034A426727 +:1018200007228267C3670120704700BF342200246B +:1018300037B500F58055D5F8A4381A681178042911 +:101840001AD1017C022917D11979012312898B4001 +:10185000134211D100F14C04204601F061FF58B150 +:1018600001A9204601F0A8FED5F8A4480246019B34 +:101870002179206800F0C0F803B030BD01F10B03FE +:10188000F0B550F8236085B004460D46FEB1302314 +:1018900083F3118804EB8507301D0821FFF7A6FEAE +:1018A000FB6806F14C005B691B681BB1019001F0FD +:1018B00091FE019803A901F07FFE024648B1039B07 +:1018C0002946204600F098F8002383F3118805B0DC +:1018D000F0BDFB685A691268002AF5D01B8A013BEB +:1018E0001340F1D104F18002EAE70000133138B56A +:1018F00050F82140ECB1302383F3118804F5805374 +:10190000D3F8A4281368527903EB8203DB689B6940 +:101910005D6845B104216018FFF768FE294604F1AF +:10192000140001F07FFD2046FFF7B4FE002383F38F +:10193000118838BD7047000001F074B801234022BF +:10194000002110B5044600F8303BFFF7A9FA002348 +:10195000C4E9013310BD000010B53023044683F301 +:1019600011882422416000210C30FFF799FA2046AB +:1019700001F06AF802230020237080F3118810BD63 +:1019800070B500EB8103054650690E461446DA60D7 +:1019900018B110220021FFF783FAA06918B11022B4 +:1019A0000021FFF77DFA31462846BDE8704001F07E +:1019B00037B9000083682022002103F0011310B51D +:1019C000044683601030FFF76BFA2046BDE81040F4 +:1019D00001F0B2B9F0B4012500EB810447898D40D4 +:1019E000E4683D43A469458123600023A2606360ED +:1019F000F0BC01F0CFB90000F0B4012500EB810488 +:101A000007898D40E4683D436469058123600023B4 +:101A1000A2606360F0BC01F049BA000070B5022317 +:101A200000250446242203702946C0F888500C3053 +:101A300040F8045CFFF734FA204684F8705001F057 +:101A400083F863681B6823B129462046BDE87040CF +:101A5000184770BD0378052B10B504460AD080F8EE +:101A60008C300523037043681B680BB10421984731 +:101A70000023A36010BD00000178052906D190F86D +:101A80008C20436802701B6803B118477047000040 +:101A900070B590F87030044613B1002380F87030B0 +:101AA00004F18002204601F06BF963689B68B3B9CA +:101AB00094F8803013F0600535D00021204601F005 +:101AC00049FC0021204601F039FC63681B6813B112 +:101AD000062120469847062384F8703070BD2046C2 +:101AE00098470028E4D0B4F88630A26F9A4288BFA5 +:101AF000A36794F98030A56F002B4FF0300380F27C +:101B00000381002D00F0F280092284F8702083F315 +:101B1000118800212046D4E91D23FFF76DFF002323 +:101B200083F31188DAE794F8812003F07F0343EA16 +:101B3000022340F20232934200F0C58021D8B3F56F +:101B4000807F48D00DD8012B3FD0022B00F093802E +:101B5000002BB2D104F1880262670222A267E36718 +:101B6000C1E7B3F5817F00F09B80B3F5407FA4D13E +:101B700094F88230012BA0D1B4F8883043F00203EE +:101B800032E0B3F5006F4DD017D8B3F5A06F31D068 +:101B9000A3F5C063012B90D86368204694F8822097 +:101BA0005E6894F88310B4F88430B047002884D07D +:101BB000436863670368A3671AE0B3F5106F36D014 +:101BC00040F6024293427FF478AF5C4B6367022396 +:101BD000A3670023C3E794F88230012B7FF46DAF35 +:101BE000B4F8883023F00203A4F88830C4E91D5506 +:101BF000E56778E7B4F88030B3F5A06F0ED194F8BC +:101C00008230204684F88A3000F0FCFF63681B684D +:101C100013B1012120469847032323700023C4E910 +:101C20001D339CE704F18B0363670123C3E723782B +:101C3000042B10D1302383F311882046FFF7BAFE1E +:101C400085F311880321636884F88B5021701B6829 +:101C50000BB12046984794F88230002BDED084F8F0 +:101C60008B300423237063681B68002BD6D00221BD +:101C700020469847D2E794F8843020461D0603F0AA +:101C80000F010AD501F06EF8012804D002287FF474 +:101C900014AF2B4B9AE72B4B98E701F055F8F3E77D +:101CA00094F88230002B7FF408AF94F8843013F05E +:101CB0000F01B3D01A06204602D501F063FBADE751 +:101CC00001F054FBAAE794F88230002B7FF4F5AEC4 +:101CD00094F8843013F00F01A0D01B06204602D5E3 +:101CE00001F038FB9AE701F029FB97E7142284F80A +:101CF000702083F311882B462A4629462046FFF799 +:101D000069FE85F31188E9E65DB1152284F870203B +:101D100083F3118800212046D4E91D23FFF75AFEE2 +:101D2000FDE60B2284F8702083F311882B462A46A7 +:101D300029462046FFF760FEE3E700BF1442000893 +:101D40000C4200081042000838B590F87030044684 +:101D5000002B3ED0063BDAB20F2A34D80F2B32D8F4 +:101D6000DFE803F03731310822323131313131319E +:101D700031313737856FB0F886309D4214D2C36851 +:101D80001B8AB5FBF3F203FB12556DB9302383F3C5 +:101D900011882B462A462946FFF72EFE85F3118827 +:101DA0000A2384F870300EE0142384F87030302356 +:101DB00083F31188002320461A461946FFF70AFECE +:101DC000002383F3118838BDC36F03B19847002304 +:101DD000E7E70021204601F0BDFA0021204601F08E +:101DE000ADFA63681B6813B10621204698470623A5 +:101DF000D7E7000010B590F870300446142B29D0B6 +:101E000017D8062B05D001D81BB110BD093B022BFA +:101E1000FBD80021204601F09DFA0021204601F068 +:101E20008DFA63681B6813B1062120469847062384 +:101E300019E0152BE9D10B2380F87030302383F3A0 +:101E4000118800231A461946FFF7D6FD002383F3B5 +:101E50001188DAE7C3689B695B68002BD5D1C36F33 +:101E600003B19847002384F87030CEE700238268DE +:101E7000037503691B6899689142FBD25A68036035 +:101E800042601060586070470023826803750369E0 +:101E90001B6899689142FBD85A68036042601060E1 +:101EA0005860704708B50846302383F311880B7DCE +:101EB000032B05D0042B0DD02BB983F3118808BD5B +:101EC0008B6900221A604FF0FF338361FFF7CEFF6A +:101ED0000023F2E7D1E9003213605A60F3E7000013 +:101EE000FFF7C4BF38B50A4B1C68DD682875226847 +:101EF00053601A600122DC60A3682275934201D10D +:101F000000F0CAFC29462046BDE83840FEF77ABA00 +:101F1000482800240C4B30B5DD684B1C87B00446C4 +:101F20000FD02B46094A684600F054F92046FFF7C7 +:101F3000D9FF009B13B1684600F056F9A86907B0B5 +:101F400030BDFFF7CFFFF9E748280024A51E0008A1 +:101F5000044B1A68DB6890689B68984294BF002025 +:101F6000012070474828002438B50B4B1C68DD68F9 +:101F7000226853601A600122DC602275AB689342CC +:101F800001D100F08BFC2846FFF77EFF014620467A +:101F9000BDE83840FEF736BA4828002438B5074C6B +:101FA00001230025064907482370656001F0F6FB10 +:101FB0000223237085F3118838BD00BFB02A0024A6 +:101FC0001C4200084828002400F044B9034A516824 +:101FD00053685B1A9842FBD8704700BF001000E0BE +:101FE0008B600223086108468B8270478368A3F1E7 +:101FF000840243F8142C026943F8442C426943F8E4 +:10200000402C094A43F8242CC268A3F1200043F86D +:10201000182C022203F80C2C002203F80B2C034A84 +:1020200043F8102C704700BF1D0400084828002406 +:1020300008B5FFF7DBFFBDE80840FFF751BF000020 +:10204000024BDB6898610F20FFF74CBF4828002443 +:10205000302383F31188FFF7F3BF000008B5014672 +:10206000302383F311880820FFF754FF002383F304 +:10207000118808BD064BDB6839B1426818605A60A8 +:10208000136043600420FFF745BF4FF0FF307047F7 +:10209000482800240368984206D01A6802605060FD +:1020A00018469961FFF71CBF7047000038B5044619 +:1020B0000D462068844200D138BD036823605C600F +:1020C0008561FFF70DFFF4E7036810B59C68A24235 +:1020D0000CD85C688A600B604C602160596099681C +:1020E0008A1A9A604FF0FF33836010BD121B1B6881 +:1020F000ECE700000A2938BF0A2170B504460D46F6 +:102100000A26601901F042FB01F02AFB041BA542DC +:1021100003D8751C04462E46F3E70A2E04D9012085 +:10212000BDE8704001F07ABB70BD0000F8B5144BFB +:102130000D460A2A4FF00A07D96103F110018260A7 +:1021400038BF0A224160196914460160486018616D +:10215000A81801F00BFB01F003FB431B0646A3424A +:1021600006D37C1C28192746354601F00FFBF2E701 +:102170000A2F04D90120BDE8F84001F04FBBF8BD9B +:1021800048280024F8B506460D4601F0E9FA0F4A42 +:10219000134653F8107F9F4206D12A460146304627 +:1021A000BDE8F840FFF7C2BFD169BB68441A2C19DB +:1021B00028BF2C46A34202D92946FFF79BFF22469F +:1021C00031460348BDE8F840FFF77EBF48280024A9 +:1021D00058280024C0E90323002310B45DF8044B01 +:1021E0004361FFF7CFBF000010B5194C2369984237 +:1021F0000DD08168D0E9003213605A609A680A44B1 +:102200009A60002303604FF0FF33A36110BD0268A2 +:10221000234643F8102F53600022026022699A423D +:1022200003D1BDE8104001F0ABBA936881680B445C +:10223000936001F095FA2269E1699268441AA2421A +:10224000E4D91144BDE81040091AFFF753BF00BF9D +:10225000482800242DE9F047DFF8BC8008F110077A +:102260002C4ED8F8105001F07BFAD8F81C40AA6820 +:10227000031B9A423ED814444FF00009D5E90032BE +:10228000C8F81C4013605A60C5F80090D8F81030A8 +:10229000B34201D101F074FA89F31188D5E9033111 +:1022A00028469847302383F311886B69002BD8D0D8 +:1022B00001F056FA6A69A0EB040982464A450DD23C +:1022C000022001F0ABFA0022D8F81030B34208D156 +:1022D00051462846BDE8F047FFF728BF121A2244AE +:1022E000F2E712EB09092946384638BF4A46FFF79C +:1022F000EBFEB5E7D8F81030B34208D01444C8F864 +:102300001C00211AA960BDE8F047FFF7F3BEBDE845 +:10231000F08700BF58280024482800240020704778 +:10232000FEE70000704700004FF0FF3070470000EC +:1023300002290CD0032904D00129074818BF002026 +:102340007047032A05D8054800EBC20070470448CF +:1023500070470020704700BFF44200084422002468 +:10236000A842000870B59AB005460846144601A96F +:1023700000F0C2F801A8FEF78BFD431C0022C6B294 +:102380005B001046C5E9003423700323023404F8CF +:10239000013C01ABD1B202348E4201D81AB070BDFB +:1023A00013F8011B013204F8010C04F8021CF1E7D8 +:1023B00008B5302383F311880348FFF739FA002367 +:1023C00083F3118808BD00BFB82A002490F880303C +:1023D00003F01F02012A07D190F881200B2A03D1B4 +:1023E0000023C0E91D3315E003F06003202B08D162 +:1023F000B0F884302BB990F88120212A03D81F2A05 +:1024000004D8FFF7F7B9222AEBD0FAE7034A42676C +:1024100007228267C3670120704700BF3B22002468 +:1024200007B5052917D8DFE801F019160319192097 +:10243000302383F31188104A01210190FFF7A0FA9D +:10244000019802210D4AFFF79BFA0D48FFF7BCF9EE +:10245000002383F3118803B05DF804FB302383F37A +:1024600011880748FFF786F9F2E7302383F31188D4 +:102470000348FFF79DF9EBE7484200086C4200086B +:10248000B82A002438B50C4D0C4C2A460C4904F1EE +:102490000800FFF767FF05F1CA0204F110000949BF +:1024A000FFF760FF05F5CA7204F118000649BDE8A0 +:1024B0003840FFF757BF00BF904300244422002458 +:1024C00028420008324200083D42000870B5044628 +:1024D00008460D46FEF7DCFCC6B220460134037800 +:1024E0000BB9184670BD32462946FEF7BDFC0028E0 +:1024F000F3D10120F6E700002DE9F04705460C4630 +:10250000FEF7C6FC2B49C6B22846FFF7DFFF08B12D +:102510000736F6B228492846FFF7D8FF08B110362B +:10252000F6B2632E0BD8DFF88C80DFF88C90234F47 +:10253000DFF894A02E7846B92670BDE8F0872946CA +:102540002046BDE8F04701F0F7BD252E2ED1072229 +:1025500041462846FEF788FC70B9194B224603F124 +:102560000C0153F8040B8B4242F8040BF9D11B8881 +:1025700007350E341380DDE7082249462846FEF76A +:1025800073FC98B9A21C0F4B197802320909C95D76 +:1025900002F8041C13F8011B01F00F015345C95D3B +:1025A00002F8031CF0D118340835C3E7013504F8EC +:1025B000016BBFE7144300083D4200082B430008AD +:1025C0001C43000800F8FF080CF8FF08BFF34F8F0A +:1025D000044B1A695107FCD1D3F810215207F8D1E6 +:1025E000704700BF0020005208B50D4B1B78ABB9F7 +:1025F000FFF7ECFF0B4BDA68D10704D50A4A5A60A3 +:1026000002F188325A60D3F80C21D20706D5064A67 +:10261000C3F8042102F18832C3F8042108BD00BFC9 +:10262000EE450024002000522301674508B5114BF8 +:102630001B78F3B9104B1A69510703D5DA6842F0D9 +:102640001002DA60D3F81021520705D5D3F80C2117 +:1026500042F01002C3F80C21FFF7B8FF064BDA680E +:1026600042F00102DA60D3F80C2142F00102C3F813 +:102670000C2108BDEE45002400200052FF289ABF1F +:1026800000F5804040030020704700004FF40050E8 +:10269000704700004FF4807070470000FF2808B5B5 +:1026A0000BD8FFF7EBFF00F500530268013204D1AD +:1026B00004308342F9D1012008BD0020FCE700006E +:1026C000FF2838B505463FD8FFF780FF1F4CFFF7BE +:1026D0008BFF4FF0FF337F286361C4F814311DD89E +:1026E0002361FFF773FF830143F00403E360E368B2 +:1026F00043F02003E36023695A07FCD42846FFF720 +:1027000065FFFFF7BBFF4FF4005100F0B3F8284618 +:10271000FFF78CFFBDE83840FFF7C0BFC4F81031A9 +:10272000FFF754FFA0F180039B0143F00403C4F8BA +:102730000C31D4F80C3143F02003C4F80C31D4F838 +:1027400010315B07FBD4D9E7002038BD00200052D0 +:102750002DE9F84F04460D46104644EA0203DE0612 +:1027600002D00020BDE8F88F20F01F00DFF8D4B0C1 +:10277000DFF8D4A0FFF738FF05EB0008454503D18B +:102780000120FFF753FFEDE720222946204601F004 +:10279000C3FC10B920342035F0E7234604F12002B1 +:1027A0001F68791CDDD104339342F9D104F17843D9 +:1027B0002048214EB3F5801F204B38BF184603F147 +:1027C000F80332BFD946D1461E46FFF7FFFE076029 +:1027D000A4EB050C336805F11C0143F002033360E0 +:1027E0002B1FD9F8007017F00507FAD153F8042F02 +:1027F0008B424CF80320F4D1BFF34F8FFFF7E6FE76 +:102800004FF0FF330360336823F002033360BFF3FC +:102810004F8F0B4BC3F85C42BFF34F8FBFF36F8FEB +:1028200020222946204601F077FC0028B2D0384605 +:10283000A7E700BF142100520C20005214200052C0 +:1028400000ED00E0102000521021005210B5084C9D +:10285000237828B11BB9FFF7C7FE0123237010BDF1 +:10286000002BFCD02070BDE81040FFF7DFBE00BF9A +:10287000EE4500240144BFF34F8F064B884204D33A +:10288000BFF34F8FBFF36F8F7047C3F85C022030E8 +:10289000F4E700BF00ED00E0704700007047000063 +:1028A00070B5BFF34F8FBFF36F8F1A4A0021C2F884 +:1028B0005012BFF34F8FBFF36F8F536943F4003350 +:1028C0005361BFF34F8FBFF36F8FC2F88410BFF314 +:1028D0004F8FD2F8803043F6E074C3F3C900C3F3DE +:1028E0004E335B0103EA0406014646EA817501396D +:1028F000C2F86052F9D2203B13F1200FF2D1BFF39E +:102900004F8F536943F480335361BFF34F8FBFF34D +:102910006F8F70BD00ED00E0FEE70000214B224804 +:10292000224A70B5904237D3214BC11EDA1C121ACD +:1029300022F003028B4238BF00220021FEF7B0FADA +:102940001C4A0023C2F88430BFF34F8FD2F8803086 +:1029500043F6E074C3F3C900C3F34E335B0103EAEB +:102960000406014646EA81750139C2F86C52F9D273 +:10297000203B13F1200FF2D1BFF34F8FBFF36F8FC6 +:10298000BFF34F8FBFF36F8F0023C2F85032BFF3F6 +:102990004F8FBFF36F8F70BD53F8041B40F8041BBB +:1029A000C0E700BF144600082C4700242C47002431 +:1029B0002C47002400ED00E070B5D0E9244300224C +:1029C0004FF0FF359E6804EB42135101D3F8000924 +:1029D000002805DAD3F8000940F08040C3F8000968 +:1029E000D3F8000B002805DAD3F8000B40F0804044 +:1029F000C3F8000B013263189642C3F80859C3F8B4 +:102A0000085BE0D24FF00113C4F81C3870BD000021 +:102A1000890141F02001016103699B06FCD4122069 +:102A2000FFF7D4BA10B5054C2046FEF787FF044BDC +:102A3000C4F89030034BC4F8943010BDF045002426 +:102A4000000004406043000870B503780546012B80 +:102A50006AD1374BD0F89040984233D1354B0E2194 +:102A60004D20D3F8382142F00072C3F83821D3F852 +:102A7000602142F00072C3F86021D3F86021D3F8DE +:102A8000802042F00072C3F88020D3F8802022F02A +:102A90000072C3F88020D3F88020D3F8382122F0C8 +:102AA0008062C3F83821D3F8602122F08062C3F835 +:102AB0006021D3F8603100F071FC1F4BE3601F4BC5 +:102AC000C4F800380023D5F89060C4F8003EC02355 +:102AD00023604FF40413A3633369002BFCDA012352 +:102AE0000C203361FFF772FA3369DB07FCD4122044 +:102AF000FFF76CFA3369002BFCDA00262846A66043 +:102B0000FFF75AFF6B68C4F81068DB68C4F81468F4 +:102B1000C4F81C684BB90A4BA3614FF0FF336361E3 +:102B2000A36843F00103A36070BD064BF4E700BF48 +:102B3000F0450024004402584014004003002002E5 +:102B4000003C30C0083C30C0F8B5D0F89040054695 +:102B500000214FF000662046FFF75AFFD5F8941089 +:102B600000234FF001128F684FF0FF30C4F8343863 +:102B7000C4F81C2804EB431201339F42C2F80069D9 +:102B8000C2F8006BC2F80809C2F8080BF2D20B6851 +:102B9000D5F89020C5F89830636210231361166948 +:102BA00016F01006FBD11220FFF710FAD4F8003807 +:102BB00023F4FE63C4F80038A36943F4402343F0D0 +:102BC0001003A3610923C4F81038C4F814380B4B60 +:102BD000EB604FF0C043C4F8103B094BC4F8003B16 +:102BE000C4F81069C4F80039D5F8983003F1100220 +:102BF00043F48013C5F89820A362F8BD3C43000855 +:102C000040800010D0F8902090F88A10D2F8003858 +:102C100023F4FE6343EA0113C2F800387047000052 +:102C20002DE9F84300EB8103D0F890500C46804624 +:102C3000DA680FFA81F94801166806F00306731E78 +:102C4000022B05EB41134FF0000194BFB604384E40 +:102C5000C3F8101B4FF0010104F1100398BF06F1F7 +:102C6000805601FA03F3916998BF06F500460029E2 +:102C70003AD0578A04F15801374349016F50D5F8CB +:102C80001C180B430021C5F81C382B180127C3F86A +:102C90001019A7405369611E9BB3138A928B9B083E +:102CA000012A88BF5343D8F89820981842EA034372 +:102CB00001F140022146C8F89800284605EB82023F +:102CC0005360FFF7A5FE08EB8900C3681B8A43EA3F +:102CD000845348341E4364012E51D5F81C381F43D9 +:102CE000C5F81C78BDE8F88305EB4917D7F8001B39 +:102CF00021F40041C7F8001BD5F81C1821EA030392 +:102D0000C0E704F13F030B4A2846214605EB830345 +:102D10005A60FFF77DFE05EB4910D0F8003923F427 +:102D20000043C0F80039D5F81C3823EA0707D7E775 +:102D30000080001000040002D0F894201268C0F84F +:102D40009820FFF739BE00005831D0F89030490183 +:102D50005B5813F4004004D013F4001F0CBF022092 +:102D6000012070474831D0F8903049015B5813F486 +:102D7000004004D013F4001F0CBF02200120704754 +:102D800000EB8101CB68196A0B6813604B685360D4 +:102D90007047000000EB810330B5DD68AA69136855 +:102DA000D36019B9402B84BF402313606B8A146829 +:102DB000D0F890201C4402EB4110013C09B2B4FB56 +:102DC000F3F46343033323F0030343EAC44343F0C0 +:102DD000C043C0F8103B2B6803F00303012B0ED156 +:102DE000D2F8083802EB411013F4807FD0F8003B92 +:102DF00014BF43F0805343F00053C0F8003B02EB94 +:102E00004112D2F8003B43F00443C2F8003B30BD0E +:102E10002DE9F041244D0446D5F8906006EB4013AF +:102E2000D3F8087B3807C3F8087B0AD5D6F81438DE +:102E3000190706D505EB840321462846DB685B6845 +:102E40009847FA071FD5D6F81438DB071BD505EBD2 +:102E50008403D968CCB98B69488A5A68B2FBF0F60A +:102E600000FB16228AB91868DA6890420DD2121A4D +:102E7000C3E90024302383F311880B482146FFF770 +:102E800089FF84F31188BDE8F081012303FA04F27D +:102E90006B8923EA02036B81CB68002BF3D02146B8 +:102EA0000148BDE8F0411847F045002400EB8103DC +:102EB0004A0170B5DD68D0F890306C692668E6602C +:102EC00056BB1A444FF40020C2F810092A6802F0D9 +:102ED0000302012A0AB20ED1D3F8080803EB421408 +:102EE00010F4807FD4F8000914BF40F0805040F007 +:102EF0000050C4F8000903EB4212D2F8000940F078 +:102F00000440C2F800090122D3F8340802FA01F1A2 +:102F10000143C3F8341870BD19B9402E84BF402056 +:102F2000206020681A442E8A8419013CB4FBF6F410 +:102F300040EAC44040F00050C6E700002DE9F843E5 +:102F40003C4D04464701D5F8906006EB4013D3F89A +:102F5000088918F0010FC3F808891AD0D6F810387C +:102F6000DB0716D505EB8003D9684B691868DA686A +:102F7000904231D2121A4FF000091A60C3F804903F +:102F8000302383F3118821462846FFF78FFF89F30A +:102F9000118818F0800F1DD0D6F834380126A640CD +:102FA000334217D005EB84030134D5F89050D3F8A1 +:102FB0000CC0E4B22F44DCF8142005EB0434D2F842 +:102FC00000E05168714515D3D5F8343823EA060678 +:102FD000C5F83468BDE8F883012303FA04F22B89AD +:102FE00023EA02032B818B68002BD2D0214628468E +:102FF0009847CEE7AEEB0103BCF81000834228BF30 +:103000000346D7F8180980B2B3EB800FE2D8906876 +:10301000A0F1040959F8048FC4F80080A0EB090856 +:103020009844B8F1040FF5D818440B4490605360ED +:10303000C7E700BFF04500242DE9F74FA94CD4F8AD +:1030400090506E69AB691E4016F480586E6107D0CF +:103050002046FEF7E3FC03B0BDE8F04F00F0F2BC01 +:10306000002E12DAD5F8003E9B0705D0D5F8003EB9 +:1030700023F00303C5F8003ED5F80438994823F03F +:103080000103C5F80438FEF7F7FC370505D5954868 +:10309000FFF792FC9348FEF7DDFCB0040CD5D5F8A1 +:1030A000083813F0060FEB6823F470530CBF43F499 +:1030B000105343F4A053EB6031071BD56368DB6802 +:1030C0001BB9AB6923F00803AB612378052B0CD146 +:1030D000D5F8003E9A0705D0D5F8003E23F003034B +:1030E000C5F8003E7F48FEF7C7FC6368DB680BB19C +:1030F0007C489847F30200F1AA80B70228D5D4F89B +:10310000909000274FF0010ADFF8D8B109EB471281 +:10311000D2F8003B03F44023B3F5802F11D1D2F84D +:10312000003B002B0DDA62890AFA07F322EA030357 +:10313000638104EB8703DB68DB6813B139465846CB +:1031400098470137D4F89430FFB29B689F42DDD98D +:10315000F00619D5D4F890703A6AC2F30A1002F05A +:103160000F0302F4F012B2F5802F00F0BD80B2F52B +:10317000402F09D104EB8303002207F58057DB6859 +:103180001B6A904240F0A0803303D5F818482CD534 +:10319000E70302D50020FFF73BFEA50302D501207F +:1031A000FFF736FE600302D50220FFF731FE210350 +:1031B00002D50320FFF72CFEE20202D50420FFF720 +:1031C00027FEA30202D50520FFF722FE670202D5E3 +:1031D0000620FFF71DFE250202D50720FFF718FE87 +:1031E000E00102D50820FFF713FE71037FF533AF2E +:1031F000E20702D50020FFF7A1FEA30702D50120B8 +:10320000FFF79CFE670702D50220FFF797FE26070F +:1032100002D50320FFF792FEE50602D50420FFF752 +:103220008DFEA00602D50520FFF788FE610602D5B7 +:103230000620FFF783FE220602D50720FFF77EFE59 +:10324000E3057FF508AF0820FFF778FE03E7D4F821 +:10325000903000274FF00109DFF888A00193D4F8DF +:1032600094305FFA87FB9B689B453FF646AF019B16 +:1032700003EB4B13D3F8001901F44021B1F5802F73 +:1032800020D1D3F8001900291CDAD3F8001941F035 +:103290009041C3F80019D3F800190029FBDB594607 +:1032A000D4F89000FFF7B4FB218909FA0BF321EA67 +:1032B0000303238104EB8B03DB689B6813B159463E +:1032C000504698470137CAE7910701D1D7F80080E7 +:1032D000072A02F101029CBF03F8018B4FEA18286C +:1032E0004FE700BFF0450024023307F5805704EB99 +:1032F00083025268D2F818C0DCF80820DCE9001C10 +:10330000A1EB0C0C002188420AD104EB8304636812 +:103310009B699A6802449A605A6802445A6033E78B +:1033200011F0030F01D1D7F800808C4501F10101A4 +:1033300084BF02F8018B4FEA1828E4E7D0F89030F8 +:1033400003EB4111D1F8003B43F40013C1F8003BFB +:1033500070470000D0F8903003EB4111D1F80039EC +:1033600043F40013C1F8003970470000D0F89030E2 +:1033700003EB4111D1F8003B23F40013C1F8003BEB +:1033800070470000D0F8903003EB4111D1F80039BC +:1033900023F40013C1F8003970470000090100F15F +:1033A0006043012203F56143C9B283F8001300F0C2 +:1033B0001F039A4043099B0003F1604303F56143F7 +:1033C000C3F880211A60704730B50433039C017242 +:1033D000002104FB0325C160C0E90653049B03637D +:1033E000059BC0E90000C0E90422C0E90842C0E929 +:1033F0000A11436330BD00000022416AC260C0E987 +:103400000411C0E90A226FF00101FEF74FBE00006F +:10341000D0E90432934201D1C2680AB9181D70473D +:1034200000207047036919600021C2680132C26040 +:10343000C269134482699342036124BF436A0361F2 +:10344000FEF728BE38B504460D46E3683BB1626915 +:103450000020131D1268A3621344E36207E0237A7D +:1034600033B929462046FEF705FE0028EDDA38BDBF +:103470006FF00100FBE70000C368C269013BC36055 +:103480004369134482699342436124BF436A4361A1 +:1034900000238362036B03B11847704770B5302374 +:1034A000044683F31188866A3EB9FFF7CBFF0546D1 +:1034B00018B186F31188284670BDA36AE26A13F832 +:1034C000015B9342A36202D32046FFF7D5FF00239E +:1034D00083F31188EFE700002DE9F84F04460E460C +:1034E000174698464FF0300989F311880025AA46FF +:1034F000D4F828B0BBF1000F09D141462046FFF7B0 +:10350000A1FF20B18BF311882846BDE8F88FD4E9DC +:103510000A12A7EB050B521A934528BF9346BBF13D +:10352000400F1BD9334601F1400251F8040B914280 +:1035300043F8040BF9D1A36A403640354033A36207 +:10354000D4E90A239A4202D32046FFF795FF8AF373 +:103550001188BD42D8D289F31188C9E730465A464E +:10356000FDF778FCA36A5E445D445B44A362E7E731 +:1035700010B5029C0433017203FB0421C460C0E94E +:1035800006130023C0E90A33039B0363049BC0E9CD +:103590000000C0E90422C0E90842436310BD0000F6 +:1035A000026A6FF00101C260426AC0E9042200228F +:1035B000C0E90A22FEF77ABDD0E904239A4201D17C +:1035C000C26822B9184650F8043B0B6070470023CC +:1035D0001846FAE7C3680021C2690133C360436932 +:1035E000134482699342436124BF436A4361FEF7F7 +:1035F00051BD000038B504460D46E3683BB1236970 +:1036000000201A1DA262E2691344E36207E0237AF4 +:1036100033B929462046FEF72DFD0028EDDA38BDE6 +:103620006FF00100FBE7000003691960C268013A0E +:10363000C260C269134482699342036124BF436A32 +:10364000036100238362036B03B1184770470000D6 +:1036500070B530230D460446114683F31188866AFF +:103660002EB9FFF7C7FF10B186F3118870BDA36AAA +:103670001D70A36AE26A01339342A36204D3E16935 +:1036800020460439FFF7D0FF002080F31188EDE7D2 +:103690002DE9F84F04460D46904699464FF0300A02 +:1036A0008AF311880026B346A76A4FB949462046D7 +:1036B000FFF7A0FF20B187F311883046BDE8F88FEF +:1036C000D4E90A073A1AA8EB0607974228BF17461B +:1036D000402F1BD905F1400355F8042B9D4240F8BB +:1036E000042BF9D1A36A40364033A362D4E90A23FC +:1036F0009A4204D3E16920460439FFF795FF8BF322 +:1037000011884645D9D28AF31188CDE729463A4631 +:10371000FDF7A0FBA36A3D443E443B44A362E5E7BA +:10372000D0E904239A4217D1C3689BB1836A8BB155 +:10373000043B9B1A0ED01360C368013BC360C3698E +:103740001A4483699A42026124BF436A03610023D9 +:1037500083620123184670470023FBE700F09CB901 +:10376000014B586A704700BF000C0040034B002219 +:1037700058631A610222DA60704700BF000C0040F3 +:10378000014B0022DA607047000C0040014B586387 +:10379000704700BF000C0040FEE7000070B51B4BF7 +:1037A0000025044686B058600E468562016300F02D +:1037B00021F904F11003A560E562C4E904334FF078 +:1037C000FF33C4E90044C4E90635FFF7C9FF2B46BF +:1037D000024604F134012046C4E9082380230C4A40 +:1037E0002565FEF7FDFB01230A4AE06000920375A0 +:1037F000684672680192B268CDE90223064BCDE9B2 +:103800000435FEF715FC06B070BD00BFB02A0024D9 +:103810006C4300087143000899370008024AD36AD4 +:103820001843D062704700BF482800244B684360AB +:103830008B688360CB68C3600B6943614B6903622B +:103840008B6943620B6803607047000008B5404B0A +:1038500040F2FF713F48D3F888200A43C3F888201C +:10386000D3F8882022F4FF6222F00702C3F88820F0 +:10387000D3F88820D3F840210A43C3F84021D3F875 +:1038800068210A43C3F86821334AD3F868311146E6 +:10389000FFF7CCFF00F5806002F11C01FFF7C6FFC7 +:1038A00000F5806002F13801FFF7C0FF00F580608D +:1038B00002F15401FFF7BAFF00F5806002F17001D8 +:1038C000FFF7B4FF00F5806002F18C01FFF7AEFF57 +:1038D00000F5806002F1A801FFF7A8FF00F5806005 +:1038E00002F1C401FFF7A2FF00F5806002F1E001E0 +:1038F000FFF79CFF00F5806002F1FC01FFF796FFE7 +:1039000002F58C7100F58060FFF790FF00F080F900 +:10391000124B0421D3F8902242F00102C3F8902206 +:10392000D3F8942242F00102C3F894220522C3F88E +:1039300098204FF06052C3F89C20094AC3F8A02099 +:10394000C3F898100749C3F89C10C3F8A02008BD1D +:1039500000440258000002587843000800ED00E0DF +:103960001B0008030040042408B500F049FAFEF7E4 +:1039700015FB0F4BD3F83C2142F04002C3F83C2129 +:10398000D3F8642122F04002C3F86421D3F86431F3 +:10399000084B1A6842F008021A601A6842F00402E2 +:1039A0001A6000F00BF8BDE80840FEF76BBD00BFE1 +:1039B000004402580018024870470000034B1A6880 +:1039C0001AB9034AD2F8D0241A607047A4460024DA +:1039D00000400258EFF30983054968334A6B22F02F +:1039E00001024A6383F30988002383F31188704737 +:1039F00000EF00E0302080F3118862B60D4B0E4AD4 +:103A0000D96821F4E0610904090C0A430B49DA6022 +:103A1000D3F8FC2042F08072C3F8FC20084AC2F8B8 +:103A2000B01F116841F0010111602022DA7783F89C +:103A30002200704700ED00E00003FA0555CEACC54A +:103A4000001000E0302310B583F311880E4B5B6843 +:103A500013F4006314D0F1EE103AEFF309844FF041 +:103A60008073683CE361094BDB6B236684F3098850 +:103A7000FEF76EFA10B1064BA36110BD054BFBE7D4 +:103A800083F31188F9E700BF00ED00E000EF00E0EC +:103A90002F04000832040008114BD3F8482142F0EB +:103AA0000802C3F84821D3F8702142F00802C3F895 +:103AB00070210C4AD3F87031D36B43F00803D36301 +:103AC00040F21712084B9A624FF0FF32DA6200227E +:103AD0009A615A63DA605A6001225A611A6070472B +:103AE000004402580010005C000C0040094A08B570 +:103AF0001169D3680B40D9B29B076FEA01011161CC +:103B000007D5302383F31188FEF75EFA002383F391 +:103B1000118808BD000C004008B53C4B4FF0FF3148 +:103B2000D3F8802062F00042C3F88020D3F88020D0 +:103B300002F00042C3F88020D3F88020D3F884201C +:103B4000C3F88410D3F884200022C3F88420D3F86B +:103B50008400D86F40F0FF4040F4FF0040F4DF40A5 +:103B600040F07F00D867D86F20F0FF4020F4FF00BE +:103B700020F4DF4020F07F00D867D86FD3F88800AA +:103B80006FEA40506FEA5050C3F88800D3F88800BD +:103B9000C0F30A00C3F88800D3F88800D3F8900077 +:103BA000C3F89010D3F89000C3F89020D3F8900099 +:103BB000D3F89400C3F89410D3F89400C3F8942079 +:103BC000D3F89400D3F89800C3F89810D3F898006D +:103BD000C3F89820D3F89800D3F88C00C3F88C1061 +:103BE000D3F88C00C3F88C20D3F88C00D3F89C0059 +:103BF000C3F89C10D3F89C10C3F89C20D3F89C30D9 +:103C000000F068FABDE8084000F0EAB80044025845 +:103C10005B4B0122C3F808215A4BD3F8542142F0E0 +:103C20000202C3F85421D3F87C2142F00202C3F807 +:103C30007C210222D3F87C31534BDA605A6891041C +:103C4000FCD5524A1A6001229A60514ADA60002279 +:103C50001A614FF440429A614B4B9A699204FCD529 +:103C60001A6842F480721A60464B1A6F02F440726E +:103C7000B2F5007F04D04FF480321A6700221A6731 +:103C80001A6842F001021A603E4B1A685007FCD5D0 +:103C900000221A611A6912F03802FBD10121196061 +:103CA0004FF0804159605A673A4ADA623A4A1A61DB +:103CB0001A6842F480321A60324B1A689103FCD5BC +:103CC0001A6842F480521A601A689204FCD5334A8A +:103CD00033499A6200225A63196301F5FF31DA63AE +:103CE00001F5C47199635A642E4A1A642E4ADA6245 +:103CF0001A6842F0A8521A60224B1A6802F0285241 +:103D0000B2F1285FF9D100229A61DA611A62274A7A +:103D1000DA644FF020221A65254A5A65254A9A65C9 +:103D20003623254A1360136803F00F03062BFAD1DC +:103D3000144B1A6942F003021A611A6902F0380240 +:103D4000182AFAD1D3F83C2142F00052C3F83C21A2 +:103D5000D3F8642142F00052C3F86421D3F86421FF +:103D6000D3F83C2142F08042C3F83C21D3F86421CF +:103D700042F08042C3F86421D3F86431704700BF39 +:103D800000800051004402580048025800C000F072 +:103D9000020000010000FF01008284028280800096 +:103DA00017030101630201035505FF010000012013 +:103DB0000210F000004101200020005208B5034825 +:103DC000FDF7C8F9BDE80840FFF73CBED823002442 +:103DD00008B5FFF78BFEBDE80840FFF733BE0000D3 +:103DE00008B509217A20FFF7D9FA07213220FFF719 +:103DF000D5FA0C212520BDE80840FFF7CFBA000016 +:103E000008B5FFF789FE00F085F9FDF7BBFBFDF76C +:103E100093FDFDF765FCFFF7CFFDBDE80840FFF718 +:103E20009DBC000008B5074B074A196801F03D0129 +:103E3000996053680BB190689847BDE80840FFF758 +:103E400001BE00BF00000240A846002408B5084B90 +:103E50001968890901F03D018A019A60054AD36811 +:103E60000BB110699847BDE80840FFF7EBBD00BFF4 +:103E700000000240A846002408B5084B1968090C48 +:103E800001F03D010A049A60054A53690BB190693B +:103E90009847BDE80840FFF7D5BD00BF00000240CD +:103EA000A846002408B5084B1968890D01F03D01AA +:103EB0008A059A60054AD3690BB1106A9847BDE834 +:103EC0000840FFF7BFBD00BF00000240A846002425 +:103ED00008B5074B074A596801F03D01D960536A9C +:103EE0000BB1906A9847BDE80840FFF7ABBD00BF33 +:103EF00000000240A846002408B5084B596889090B +:103F000001F03D018A01DA60054AD36A0BB1106BFA +:103F10009847BDE80840FFF795BD00BF000002408C +:103F2000A846002408B5084B5968090C01F03D016A +:103F30000A04DA60054A536B0BB1906B9847BDE8F1 +:103F40000840FFF77FBD00BF00000240A8460024E4 +:103F500008B5084B5968890D01F03D018A05DA6002 +:103F6000054AD36B0BB1106C9847BDE80840FFF7CA +:103F700069BD00BF00000240A846002408B5074BF9 +:103F8000074A196801F03D019960536C0BB1906CC0 +:103F90009847BDE80840FFF755BD00BF0004024048 +:103FA000A846002408B5084B1968890901F03D01AD +:103FB0008A019A60054AD36C0BB1106D9847BDE831 +:103FC0000840FFF73FBD00BF00040240A8460024A0 +:103FD00008B5084B1968090C01F03D010A049A6004 +:103FE000054A536D0BB1906D9847BDE80840FFF747 +:103FF00029BD00BF00040240A846002408B5084BB4 +:104000001968890D01F03D018A059A60054AD36D52 +:104010000BB1106E9847BDE80840FFF713BD00BF15 +:1040200000040240A846002408B5074B074A596817 +:1040300001F03D01D960536E0BB1906E9847BDE819 +:104040000840FFF7FFBC00BF00040240A846002460 +:1040500008B5084B5968890901F03D018A01DA6009 +:10406000054AD36E0BB1106F9847BDE80840FFF7C3 +:10407000E9BC00BF00040240A846002408B5084B74 +:104080005968090C01F03D010A04DA60054A536FD2 +:104090000BB1906F9847BDE80840FFF7D3BC00BF55 +:1040A00000040240A846002408B5084B5968890D51 +:1040B00001F03D018A05DA60054AD36F13B1D2F8E9 +:1040C00080009847BDE80840FFF7BCBC00040240F0 +:1040D000A846002400230C4910B51A460B4C0B606F +:1040E00054F82300026001EB430004334260402B8C +:1040F000F6D1074A4FF0FF339360D360C2F808341B +:10410000C2F80C3410BD00BFA8460024AC4400081F +:10411000000002407047000010B50139024490428F +:1041200001D1002005E0037811F8014FA34201D02E +:10413000181B10BD0130F2E7034611F8012B03F8FC +:10414000012B002AF9D17047303738316635623695 +:10415000383036386263663100000000121000000B +:10416000121100001213000053544D333248373FF0 +:104170003F3F0053544D3332483733782F3732782E +:104180000053544D3332483734332F3735332F37BC +:104190003530000001105A0003105900012058006A +:1041A0000320560000960000000000000000000000 +:1041B00000000000000000000000000000000000FF +:1041C00045150008311500086D150008591500083F +:1041D00065150008511500083D150008291500084F +:1041E00079150008000000005D1600084916000857 +:1041F00085160008711600087D160008691600086B +:10420000551600084116000891160008000000002D +:1042100001000000000000006330000018420008A8 +:10422000A0280024B02A00244172647550696C6F84 +:10423000740025424F415244252D424C00255345E0 +:104240005249414C2500000002000000000000001F +:104250007D180008ED18000840004000604300246D +:104260007043002402000000000000000300000072 +:1042700000000000351900080000000010000000D8 +:104280008043002400000000010000000000000046 +:10429000F045002401010200212400083123000818 +:1042A000CD230008B123000843000000B0420008FD +:1042B00009024300020100C03209040000010202A9 +:1042C000010005240010010524010001042402025C +:1042D0000524060001070582030800FF0904010008 +:1042E000020A0000000705010240000007058102E4 +:1042F0004000000012000000FC4200081201100102 +:1043000002000040091241570002010203010000AF +:104310000403090425424F41524425004D61746550 +:104320006B483741332D57696E6700303132333473 +:1043300035363738394142434445460000000000D5 +:10434000911A0008491D0008F51D000840004000B2 +:104350008C4600248C460024010000009C4600246A +:104360000001000000100000080000006D61696E8F +:104370000069646C6500000000008012000000000D +:10438000AAAAAAAA00000000FFBF000000000000C7 +:1043900000000000100000A000000000AAAAAAAAC5 +:1043A00010000050FFFF000000000000000000446B +:1043B0000001005400000000AAAAAAAA00010050AF +:1043C000FFDF00000000000000000000000000000F +:1043D00000000000AAAAAAAA00000000FFFF000037 +:1043E00000000000000000000000000000000000CD +:1043F000AAAAAAAA00000000FFFF00000000000017 +:10440000000000000000000000000000AAAAAAAA04 +:1044100000000000FFFF000000000000000000009E +:104420000000000000000000AAAAAAAA00000000E4 +:10443000FFFF00000000000000000000000000007E +:1044400000000000AAAAAAAA00000000FFFF0000C6 +:10445000000000000000000000000000000000005C +:10446000AAAAAAAA00000000FFFF000000000000A6 +:10447000000000000000000000000000AAAAAAAA94 +:1044800000000000FFFF000000000000000000002E +:104490000000000000000000AAAAAAAA0000000074 +:1044A000FFFF0000000000000000000010000240BC +:1044B000080002400008024000000B0028000240F3 +:1044C000080002400408024006010C0040000240BF +:1044D000080002400808024010020D005800024087 +:1044E000080002400C08024016030E007000024053 +:1044F0000C0002401008024000040F008800024037 +:104500000C0002401408024006051000A000024002 +:104510000C0002401808024010061100B8000240CA +:104520000C0002401C08024016072F001004024035 +:104530000804024020080240000838002804024015 +:1045400008040240240802400609390040040240E1 +:104550000804024028080240100A3A0058040240A9 +:10456000080402402C080240160B3B007004024075 +:104570000C04024030080240000C3C008804024059 +:104580000C04024034080240060D4400A00402401E +:104590000C04024038080240100E4500B8040240E6 +:1045A0000C0402403C080240160F4600FFFFFFFFCC +:1045B000CC0400000000000000001F00000000000C +:1045C000FF000000B82A0024D823002400000000C7 +:1045D0006841000883040000734100085004000093 +:1045E00081410008009600000000080096000000CD +:1045F0000008000004000000104300080000000054 +:1046000000000000000000000000000000000000AA +:0446100000000000A6 +:00000001FF diff --git a/Tools/bootloaders/Morakot_bl.bin b/Tools/bootloaders/Morakot_bl.bin new file mode 100755 index 0000000000000..ad2e47ad4f914 Binary files /dev/null and b/Tools/bootloaders/Morakot_bl.bin differ diff --git a/Tools/bootloaders/Morakot_bl.hex b/Tools/bootloaders/Morakot_bl.hex new file mode 100644 index 0000000000000..bf55b9e8a56de --- /dev/null +++ b/Tools/bootloaders/Morakot_bl.hex @@ -0,0 +1,1319 @@ +:020000040800F2 +:1000000000060020E1020008E3020008E302000805 +:10001000E3020008E3020008E3020008E30200082C +:10002000E3020008E3020008E3020008FD330008D1 +:10003000E3020008E3020008E3020008E30200080C +:10004000E3020008E3020008E3020008E3020008FC +:10005000E3020008E3020008E3020008E3020008EC +:10006000E3020008E3020008E3020008911100081F +:10007000B9110008E5110008111200083D1200082E +:100080006512000891120008E3020008E30200086C +:10009000E3020008E3020008E3020008E3020008AC +:1000A000E3020008E3020008E3020008E30200089C +:1000B000E3020008E3020008E3020008E30200088C +:1000C000E3020008E3020008E3020008E30200087C +:1000D000E3020008E14B0008F54B0008094C00085A +:1000E000E3020008E3020008E3020008E30200085C +:1000F000E3020008E3020008E3020008BD12000862 +:10010000E3020008E30200086D4C0008E302000867 +:10011000E30200081D4C0008E3020008E3020008A7 +:10012000E9120008111300083D13000869130008C4 +:1001300095130008E3020008E3020008E302000848 +:10014000E3020008E3020008E3020008E3020008FB +:10015000BD130008E913000815140008314C00080D +:10016000E3020008E3020008E3020008E3020008DB +:10017000E3020008E93F0008E3020008E302000888 +:10018000E3020008E3020008454C0008594C00084F +:10019000E3020008E3020008E3020008E3020008AB +:1001A000E3020008E3020008E3020008E30200089B +:1001B000E3020008E3020008E3020008E30200088B +:1001C000E3020008E3020008E3020008E30200087B +:1001D000E3020008D53F0008E3020008E30200083C +:1001E000E3020008E3020008E3020008E30200085B +:1001F000E3020008E3020008E3020008E30200084B +:10020000E3020008E3020008E3020008E30200083A +:10021000E3020008E3020008E3020008E30200082A +:10022000E3020008E3020008E3020008E30200081A +:10023000E3020008E3020008E3020008E30200080A +:10024000E3020008E3020008E3020008E3020008FA +:10025000E3020008E3020008E3020008E3020008EA +:10026000E3020008E3020008E3020008E3020008DA +:10027000E3020008E3020008E3020008E3020008CA +:10028000E3020008E3020008E3020008E3020008BA +:10029000E3020008E3020008E3020008E3020008AA +:1002A000E3020008E3020008E3020008E30200089A +:1002B000E3020008E3020008E3020008E30200088A +:1002C000E3020008E3020008E3020008E30200087A +:1002D000E3020008E3020008E3020008E30200086A +:1002E00002E000F000F8FEE772B6374880F30888B5 +:1002F000364880F3098836483649086040F20000E5 +:10030000CCF200004EF63471CEF200010860BFF36B +:100310004F8FBFF36F8F40F20000C0F2F0004EF637 +:100320008851CEF200010860BFF34F8FBFF36F8F8B +:100330004FF00000E1EE100A4EF63C71CEF20001E3 +:100340000860062080F31488BFF36F8F03F0B8F8BD +:1003500004F000FA4FF055301F491B4A91423CBF50 +:1003600041F8040BFAE71D49184A91423CBF41F895 +:10037000040BFAE71A491B4A1B4B9A423EBF51F83D +:10038000040B42F8040BF8E700201849184A914280 +:100390003CBF41F8040BFAE703F0D0F804F05EFA32 +:1003A000144C154DAC4203DA54F8041B8847F9E7A6 +:1003B00000F042F8114C124DAC4203DA54F8041B21 +:1003C0008847F9E703F0B8B80006002000220020B3 +:1003D0000000000808ED00E00000002000060020FA +:1003E000D85100080022002074220020782200202A +:1003F00024630020E0020008E0020008E002000898 +:10040000E00200082DE9F04F2DED108AC1F80CD064 +:10041000D0F80CD0BDEC108ABDE8F08F002383F338 +:1004200011882846A047002002F09AFAFEE702F061 +:100430001FFA00DFFEE7000038B500F0DBFC00F03B +:100440007FFE02F097FF054602F0CAFF0446C0B9DE +:100450000E4B9D4217D001339D4241F2883512BFA9 +:10046000044600250124002002F08EFF0CB100F0AC +:1004700073F800F005FE284600F01CF900F06CF857 +:10048000F9E70025EFE70546EDE700BF010007B0FB +:1004900008B500F07BFCA0F120035842584108BD8C +:1004A00007B5042101900DEB010000F08DFC03B0B5 +:1004B0005DF804FB38B5302383F3118817480368CF +:1004C0000BB102F019FB0023154A4FF47A7113485F +:1004D00002F008FB002383F31188124C236813B148 +:1004E0002368013B2360636813B16368013B636069 +:1004F0000D4D2B7833B963687BB9022000F032FDD3 +:10050000322363602B78032B07D163682BB9022059 +:1005100000F028FD4FF47A73636038BD7822002024 +:10052000B50400089823002090220020084B187082 +:1005300003280CD8DFE800F008050208022000F0CC +:100540000FBD022000F00ABD024B00225A60704726 +:100550009022002098230020F8B5504B504A1C46AA +:100560001968013100F0998004339342F8D1626830 +:100570004C4B9A4240F291804B4B9B6803F10063D5 +:1005800003F500339A4280F08880002000F058FC88 +:100590000220FFF7CBFF454B0021D3F8E820C3F83A +:1005A000E810D3F81021C3F81011D3F81021D3F8B4 +:1005B000EC20C3F8EC10D3F81421C3F81411D3F8CD +:1005C0001421D3F8F020C3F8F010D3F81821C3F8A1 +:1005D0001811D3F81821D3F8802042F00062C3F834 +:1005E0008020D3F8802022F00062C3F88020D3F866 +:1005F0008020D3F8802042F00072C3F88020D3F826 +:10060000802022F00072C3F88020D3F8803072B6C8 +:100610004FF0E023C3F8084DD4E90004BFF34F8F37 +:10062000BFF36F8F224AC2F88410BFF34F8F536914 +:1006300023F480335361BFF34F8FD2F8803043F6F9 +:10064000E076C3F3C905C3F34E335B0103EA060C3E +:1006500029464CEA81770139C2F87472F9D2203BFD +:1006600013F1200FF2D1BFF34F8FBFF36F8FBFF3A2 +:100670004F8FBFF36F8F536923F40033536100230F +:10068000C2F85032BFF34F8FBFF36F8F302383F325 +:100690001188854680F308882047F8BD00000208CD +:1006A00020000208FFFF0108002200200044025839 +:1006B00000ED00E02DE9F04F95B0DFF8D892202250 +:1006C000FF21029004A8D9F8085000F083FCA84A42 +:1006D0001378A3B90121A74811700360302383F375 +:1006E000118803680BB102F007FA0023A24A4FF405 +:1006F0007A71A04802F0F6F9002383F31188029B77 +:1007000013B19E4B029A1A609D4A1378032B03D0B3 +:1007100000231370994A53604FF0000B029C5E4611 +:10072000DA46CDF804B0012000F01AFC24B1934B56 +:100730001B68002B00F02882002000F027FBB0F19E +:100740000008F3DB012000F009FCA8F121031F2BB6 +:10075000E9D801A252F823F0D9070008F5070008EC +:100760008308000827070008270700082707000854 +:100770000D090008FF0A0008F10900084D0A0008E9 +:10078000730A0008990A000827070008AB0A000846 +:10079000CF0A0008470B0008610800082707000877 +:1007A0008F0B0008E507000861080008270700080C +:1007B0004D0A000827070008270700082707000838 +:1007C0002707000827070008270700082707000851 +:1007D00027070008830800080220FFF759FE0028B9 +:1007E00040F0FF81029B02216648BBF1000F08BF69 +:1007F0001C4640E04FF47A7000F0C8FA071EF1DBA7 +:100800000220FFF745FE0028ECD0013F052F00F243 +:10081000EA81DFE807F003070A0D10330520FFF730 +:100820003FFE14E0D9F80000F9E7D9F80400F6E734 +:10083000D9F80800F3E74FF01C08404608F1040817 +:1008400000F000FBFFF72CFEB8F12C0FF5D10120D2 +:10085000019B4FF0000A00FA07F71F43FBB2019318 +:1008600000F004FC2EB1019B03F00B030B2B08BF1F +:1008700000240221444800F0A7FA54E7D9F80C00FC +:10088000CDE7002EAED0019B03F00B030B2BA9D1BB +:100890000220FFF7FDFD07460028A3D00120002617 +:1008A00000F0CEFA0220FFF741FE1FFA86FB584601 +:1008B00000F0D6FA044688B1A8F140025846013645 +:1008C0005142514100F0DCFA0028EED1BB4604460B +:1008D00002212E483E4600F077FA24E72546012003 +:1008E000FFF724FED9F80830AB4207D9284600F0BC +:1008F000A9FA013040F07A810435F3E70025204B56 +:10090000BB463E461D701D4B5D60A9E7002E3FF4BF +:1009100069AF019B03F00B030B2B7FF463AF022045 +:10092000FFF704FE322000F031FAB0F10008FFF6C4 +:1009300059AF18F003077FF455AF08EB0503D9F85A +:10094000082093423FF64EAFB8F5807F3FF74AAF9D +:100950000F4BB84503931FDD4FF47A7000F016FA81 +:100960000028FFF63FAF039B013703F8010BF0E7C8 +:100970009423002078220020B5040008982300204A +:1009800090220020404D0008384D00083C4D0008E2 +:100990009422002000220020C820FFF779FD07469E +:1009A00000283FF41FAF1F2D11D8C5F1200204AB62 +:1009B00025F003008F494245184428BF4246039260 +:1009C00000F0E2FA039AFF218A4800F003FB4FEAA5 +:1009D000A803C8F3870287492846039300F002FB67 +:1009E000064600283FF474AF039B05EB830537E709 +:1009F0000220FFF74DFD00283FF4F4AE00F05EFA50 +:100A000000283FF4EFAE0027B846D9F80830BB42C3 +:100A100018D91F2F11D8049B01330ED027F00303E0 +:100A200014AA134453F8403C0C93404604220CA9EA +:100A3000043700F097FB8046E7E7384600F002FAFB +:100A40000C90F2E74046FFF72BFD09E700236421F5 +:100A50000CA80C9300F0AEF900287FF4C3AE02207E +:100A6000FFF716FD00283FF4BDAE0C9800F01CFA0D +:100A7000E9E7002364210CA80C9300F09BF90028FF +:100A80007FF4B0AE0220FFF703FD00283FF4AAAECA +:100A90000C9800F00BFAD6E70220FFF7F9FC0028CB +:100AA0003FF4A0AE00F01AFACDE70220FFF7F0FC09 +:100AB00000283FF497AE0CA9142000F015FA03901B +:100AC000FFF7EEFC03990CA800F07EF9C8E60220BF +:100AD000FFF7DEFC00283FF485AE474B0CAF03F177 +:100AE000100C1868083353F8041C3A46634503C2D7 +:100AF0001746F6D11020FFF7D3FC1021E3E7322090 +:100B000000F044F9071EFFF66DAEBB077FF46AAE36 +:100B100007EB0A03D9F8082093423FF663AE0220A0 +:100B2000FFF7B6FC00283FF45DAE27F003075744FB +:100B3000BA453FF495AE50460AF1040A00F082F936 +:100B4000FFF7AEFCF4E74FF47A70FFF7A1FC002842 +:100B50003FF448AE00F0B2F9002849D0049B0133BD +:100B60000BD0082204A9002000F03CFA00283FD056 +:100B70002022FF2104A800F02DFA2048022100F0D5 +:100B800023F91F4801F0F2FE15B0BDE8F08F002EEA +:100B90003FF428AE019B03F00B030B2B7FF422AE36 +:100BA000002364210CA80C9300F004F907460028E8 +:100BB0007FF418AE0220FFF76BFC804600283FF45C +:100BC00011AE02210D4800F0FFF841F2883001F02B +:100BD000CDFE0C9800F092FA46463C4600F046FAEC +:100BE000A1E506463CE64FF0000AFBE5BB466FE692 +:100BF00037466DE694220020244D0008384D000849 +:100C0000A08601002DE9F84F4FF47A7306460D4691 +:100C1000002402FB03F7DFF85080DFF8509098F9CA +:100C200000305FFA84FA5A1C01D0A34210D159F85F +:100C300024002A4631460368D3F820B03B46D84703 +:100C4000854205D1074B012083F800A0BDE8F88F4D +:100C50000134062CE3D14FF4FA7001F087FE002036 +:100C6000F4E700BFE423002010220020142200201B +:100C700000232DE9F0471A4605468846012019460B +:100C800000F070FC0446802002F004FB07460028B8 +:100C900061D0002C5FD03B4E28464FF0800A73692C +:100CA00043F0C0637361D6F8143143F0C063C6F8F3 +:100CB000143102F073F905EB080200FB05F34FF461 +:100CC000D06C50434FF03D0E834233D23269520113 +:100CD00043D4D6F8102151013FD4216803F10062BA +:100CE0008A602268D7602268C2F804A02268C2F82D +:100CF00000C0D4F80090D9F8002042F00102C9F8F1 +:100D00000020D4F80090D9F8002022F01E02C9F883 +:100D10000020D4F80090D9F80020D207FBD494F832 +:100D20000C90803361680EFA09F20A60CCE700206B +:100D300002F012FA204600F055FC384602F0E0FAC4 +:100D4000104B5A6942F0C0625A61D3F8142142F044 +:100D5000C062C3F81421BDE8F0870B4B9A6A42F4D5 +:100D600000729A629A6A42F400429A6261B6002660 +:100D7000012002F0F1F9B045D9D9A819013602F0E5 +:100D800025F9F8E70020005200ED00E0002307B548 +:100D9000024601210DF107008DF80730FFF732FF01 +:100DA00020B19DF8070003B05DF804FB4FF0FF3061 +:100DB000F9E700000A46042108B5FFF723FF80F099 +:100DC0000100C0B2404208BD074B0A4630B4197852 +:100DD000064B53F82140014623682046DD69044B49 +:100DE000AC4630BC604700BFE42300201422002042 +:100DF000A086010070B5104C0025104E02F0D2F80C +:100E000020803068238883420CD800252088013850 +:100E100002F0C4F823880544013BB5F5802F2380F8 +:100E2000F4D370BD02F0BAF8336805440133B5F568 +:100E3000003F3360E5D3E8E7E6230020A02300204D +:100E400002F08AB900F1006000F5003000687047D8 +:100E500000F10060920000F5003002F0FFB80000E1 +:100E6000054B1A68054B1B889B1A834202D9104414 +:100E700002F094B800207047A0230020E623002051 +:100E800038B50446074D29B128682044BDE83840EC +:100E900002F09CB82868204402F086F80028F3D0BD +:100EA00038BD00BFA02300200020704700F1FF5094 +:100EB00000F58F10D0F8000870470000064991F83F +:100EC000243033B100230822086A81F82430FFF768 +:100ED000BFBF0120704700BFA4230020014B18684A +:100EE000704700BF0010005C194B01380322084412 +:100EF00070B51D68174BC5F30B042D0C1E88A64258 +:100F00000BD15C680A46013C824213460FD214F9A9 +:100F1000016F4EB102F8016BF6E7013A03F10803E5 +:100F2000ECD181420B4602D22C2203F8012B04247F +:100F3000094A1688AE4204D1984284BF967803F8D5 +:100F4000016B013C02F10402F3D1581A70BD00BFDD +:100F50000010005C2C220020704D0008704700003B +:100F6000704700007047000070B504464FF47A7671 +:100F70004CB1412C254628BF412506FB05F0641BDA +:100F800001F0F4FCF4E770BD002310B5934203D0E8 +:100F9000CC5CC4540133F9E710BD0000013810B532 +:100FA00010F9013F3BB191F900409C4203D11AB1C5 +:100FB0000131013AF4E71AB191F90020981A10BDF5 +:100FC0001046FCE703460246D01A12F9011B00291D +:100FD000FAD1704702440346934202D003F8011B42 +:100FE000FAE770472DE9F8431F4D14460746884637 +:100FF00095F8242052BBDFF870909CB395F824300C +:101000002BB92022FF2148462F62FFF7E3FF95F816 +:1010100024004146C0F1080205EB8000A24228BF2F +:101020002246D6B29200FFF7AFFF95F82430A41BFA +:1010300017441E449044E4B2F6B2082E85F82460AA +:10104000DBD1FFF73BFF0028D7D108E02B6A03EB89 +:1010500082038342CFD0FFF731FF0028CBD100209D +:10106000BDE8F8830120FBE7A4230020024B1A7897 +:10107000024B1A70704700BFE423002010220020AA +:10108000F8B5194C194800F08BFF2146174800F0BD +:10109000B3FF24684FF47A70154ED4F89020154DA4 +:1010A000D2F80438114F43F00203C2F80438FFF7B6 +:1010B0005BFF2046104901F0ADF8D4F890200624DB +:1010C000D2F8043823F00203C2F804384FF4E133B5 +:1010D000336055F8040BB84202D0314600F0BEFE32 +:1010E000013CF6D1F8BD00BF6C4F0008D045002090 +:1010F000CC23002014220020744F00080C4B70B544 +:101100000C4D04461E780C4B55F826209A420DD003 +:101110000A4B002118221846FFF75CFF04600146C5 +:1011200055F82600BDE8704000F098BE70BD00BFC5 +:10113000E423002014220020D0450020CC230020EE +:1011400008B571B6054B1988054B1888FFF790FD57 +:1011500002210E20FFF78CFD61B608BDE6230020BA +:10116000A023002030B50A44084D91420DD011F85B +:10117000013B5840082340F30004013B2C4013F08E +:10118000FF0384EA5000F6D1EFE730BD2083B8EDCD +:1011900008B5074B074A196801F03D01996053688B +:1011A0000BB190689847BDE8084002F05FB900BFF6 +:1011B00000000240E823002008B5084B196889099F +:1011C00001F03D018A019A60054AD3680BB11069AC +:1011D0009847BDE8084002F049B900BF000002404E +:1011E000E823002008B5084B1968090C01F03D01FF +:1011F0000A049A60054A53690BB190699847BDE8A3 +:10120000084002F033B900BF00000240E82300208C +:1012100008B5084B1968890D01F03D018A059A60EF +:10122000054AD3690BB1106A9847BDE8084002F03F +:101230001DB900BF00000240E823002008B5074B9D +:10124000074A596801F03D01D960536A0BB1906AB1 +:101250009847BDE8084002F009B900BF000002400D +:10126000E823002008B5084B5968890901F03D01C1 +:101270008A01DA60054AD36A0BB1106B9847BDE862 +:10128000084002F0F3B800BF00000240E82300204D +:1012900008B5084B5968090C01F03D010A04DA60F1 +:1012A000054A536B0BB1906B9847BDE8084002F0BC +:1012B000DDB800BF00000240E823002008B5084B5D +:1012C0005968890D01F03D018A05DA60054AD36B42 +:1012D0000BB1106C9847BDE8084002F0C7B800BFDA +:1012E00000000240E823002008B5074B074A1968B0 +:1012F00001F03D019960536C0BB1906C9847BDE8CB +:10130000084002F0B3B800BF00040240E823002008 +:1013100008B5084B1968890901F03D018A019A60F6 +:10132000054AD36C0BB1106D9847BDE8084002F038 +:101330009DB800BF00040240E823002008B5084B18 +:101340001968090C01F03D010A049A60054A536DC1 +:101350000BB1906D9847BDE8084002F087B800BF18 +:1013600000040240E823002008B5084B1968890DE5 +:1013700001F03D018A059A60054AD36D0BB1106EEC +:101380009847BDE8084002F071B800BF0004024071 +:10139000E823002008B5074B074A596801F03D01D2 +:1013A000D960536E0BB1906E9847BDE8084002F0CB +:1013B0005DB800BF00040240E823002008B5084BD8 +:1013C0005968890901F03D018A01DA60054AD36E46 +:1013D0000BB1106F9847BDE8084002F047B800BF56 +:1013E00000040240E823002008B5084B5968090CA6 +:1013F00001F03D010A04DA60054A536F0BB1906FAA +:101400009847BDE8084002F031B800BF0004024030 +:10141000E823002008B5084B5968890D01F03D010B +:101420008A05DA60054AD36F13B1D2F88000984775 +:10143000BDE8084002F01AB800040240E82300208A +:1014400000230C4910B51A460B4C0B6054F82300CE +:10145000026001EB430004334260402BF6D1074A9F +:101460004FF0FF339360D360C2F80834C2F80C34F5 +:1014700010BD00BFE8230020804D0008000002409E +:101480000F28F8B510D9102810D0112811D0122823 +:1014900008D10F240720DFF8C8E00126DEF800504D +:1014A000A04208D9002653E00446F4E70F240020A8 +:1014B000F1E70724FBE706FA00F73D424AD1264C44 +:1014C0004FEA001C3D4304EB00160EEBC000CEF8C3 +:1014D0000050C0E90123FBB273B12048D0F8D830E6 +:1014E00043F00103C0F8D830D0F8003143F00103D5 +:1014F000C0F80031D0F8003117F47F4F0ED01748F4 +:10150000D0F8D83043F00203C0F8D830D0F800311A +:1015100043F00203C0F80031D0F8003154F80C0059 +:10152000036823F01F030360056815F00105FBD174 +:1015300004EB0C033D2493F80CC05F6804FA0CF430 +:101540003C6021240560446112B1987B00F0BAFA36 +:101550003046F8BD0130A3E7804D00080044025832 +:10156000E823002010B5302484F31188FFF788FFAA +:10157000002383F3118810BD10B50446807B00F072 +:10158000B7FA01231549627B03FA02F20B6823EADA +:101590000203DAB20B6072B9114AD2F8D81021F006 +:1015A0000101C2F8D810D2F8001121F00101C2F8EF +:1015B0000011D2F8002113F47F4F0ED1084BD3F85D +:1015C000D82022F00202C3F8D820D3F8002122F05C +:1015D0000202C3F80021D3F8003110BDE823002037 +:1015E0000044025808B5302383F31188FFF7C4FF85 +:1015F000002383F3118808BD02684368114301602A +:1016000003B1184770470000024A136843F0C00353 +:101610001360704700100140024A136843F0C00392 +:101620001360704700440040024A136843F0C0034F +:101630001360704700480040024A136843F0C0033B +:101640001360704700500040024A136843F0C00323 +:101650001360704700140140024A136843F0C0034E +:101660001360704700780040024A136843F0C003DB +:1016700013607047007C0040064BD3F8E82002431B +:10168000C3F8E820D3F810211043C3F81001D3F8B1 +:10169000103170470044025837B5594C594D204617 +:1016A00000F0D0FB04F11400009400234FF400720A +:1016B000554900F08DFA4FF40072544904F1380096 +:1016C0000094534B00F006FB524BC4E91735524CC3 +:1016D000204600F0B7FB04F11400009400234FF4FF +:1016E00000724E4900F074FA4FF400724C4904F154 +:1016F000380000944B4B00F0EDFA4B4BC4E9173522 +:101700004A4C204600F09EFB04F114000094002394 +:101710004FF40072464900F05BFA4FF400724549FD +:1017200004F138000094444B00F0D4FA434BC4E970 +:101730001735434C204600F085FB04F1140000945B +:1017400000234FF400723F4900F042FA4FF4007258 +:101750003D4904F1380000943C4B00F0BBFA3C4B8F +:10176000C4E917353B4C204600F06CFB04F1140033 +:10177000009400234FF40072374900F029FA4FF427 +:101780000072364904F138000094354B00F0A2FA9B +:10179000344BC4E91735344C204600F053FB04F1B8 +:1017A0001400009400234FF40072304900F010FA46 +:1017B0004FF400722E4904F1380000942D4B00F0D4 +:1017C00089FA2D4BC4E917352C4C204600F03AFB22 +:1017D00004F1140000234FF400722949009400F032 +:1017E000F7F9284B4FF40072274904F138000094B0 +:1017F00000F070FA254BC4E9173503B030BD00BFC7 +:101800006C24002000E1F5056027002060350020F1 +:101810000916000800100140D8240020602900208B +:1018200060370020191600080044004044250020BD +:10183000602B002060390020291600080048004075 +:10184000B0250020602D0020603B002039160008E4 +:10185000005000401C260020602F0020603D00202A +:10186000491600080014014088260020603100203D +:10187000603F00205916000800780040F426002040 +:10188000603300206916000860410020007C0040A1 +:1018900038B5404D0446037C002918BF0D46012B86 +:1018A00046D13D4B984214D13C4BD3F8F02042F046 +:1018B0001002C3F8F020D3F8182142F01002C3F848 +:1018C0001821D3F81831364B9C420CD14FF0804090 +:1018D00019E0344B98420CD0334B98420ED14FF460 +:1018E0008020FFF7C9FE314B9C4221D14FF00040D0 +:1018F00009E04FF40030FFF7BFFEE4E72C4B9842BD +:1019000004D14FF48010FFF7B7FE11E0294B984245 +:10191000D9D1224BD3F8F02042F02002C3F8F020B6 +:10192000D3F8182142F02002C3F81821D3F8183157 +:101930002A68236EE16D03EB5203A566B3FBF2F355 +:101940006A68100442BF23F0070003F0070343EA6C +:101950004003CB60AB6843F040034B60EB6843F05F +:1019600001038B6042F4967343F001030B604FF068 +:10197000FF330B62510505D512F0102205D0B2F1EC +:10198000805F04D084F8643038BD7F23FAE73F23BA +:10199000F8E700BF804E00086C2400200044025885 +:1019A00088260020D824002044250020F42600208A +:1019B000B02500201C2600202DE9F047C66D054605 +:1019C0003768F469210734621AD014F0080118BF8F +:1019D0004FF48071E20748BF41F02001A3074FF0A8 +:1019E000300348BF41F04001600748BF41F080012B +:1019F00083F31188281DFFF7FFFD002383F311886F +:101A0000E2050AD5302383F311884FF48061281D45 +:101A1000FFF7F2FD002383F311884FF030094FF0F8 +:101A2000000A14F0200838D13B0616D54FF03009D3 +:101A300005F1380A200610D589F31188504600F0C8 +:101A40007DF9002836DA0821281DFFF7D5FD27F09B +:101A500080033360002383F31188790614D562066E +:101A600012D5302383F31188D5E913239A4208D184 +:101A70002B6C33B127F040071021281DFFF7BCFD68 +:101A80003760002383F31188E30618D5AA6E136923 +:101A9000ABB15069BDE8F047184789F31188736A04 +:101AA000284695F86410194000F0E6F98AF3118889 +:101AB000F469B6E7B06288F31188F469BAE7BDE863 +:101AC000F0870000090100F16043012203F5614342 +:101AD000C9B283F8001300F01F039A4043099B002A +:101AE00003F1604303F56143C3F880211A60704736 +:101AF00000F01F0301229A40430900F160409B005F +:101B000000F5614003F1604303F56143C3F88020B1 +:101B1000C3F88021002380F800337047F8B51546DC +:101B2000826804460B46AA4200D28568A1692669EC +:101B3000761AB5420BD218462A46FFF725FAA36952 +:101B40002B44A3612846A3685B1BA360F8BD0CD996 +:101B5000AF1B18463246FFF717FA3A46E1683044A1 +:101B6000FFF712FAE3683B44EBE718462A46FFF713 +:101B70000BFAE368E5E7000083689342F7B5044693 +:101B8000154600D28568D4E90460361AB5420BD2F6 +:101B90002A46FFF7F9F963692B4463612846A36875 +:101BA0005B1BA36003B0F0BD0DD93246AF1B0191A2 +:101BB000FFF7EAF901993A46E0683144FFF7E4F9A2 +:101BC000E3683B44E9E72A46FFF7DEF9E368E4E728 +:101BD00010B50A440024C361029B8460C1600261A5 +:101BE0000362C0E90000C0E9051110BD08B5D0E9E5 +:101BF0000532934201D1826882B9826801328260E3 +:101C00005A1C426119700021D0E904329A4224BF63 +:101C1000C368436100F0C6FE002008BD4FF0FF30EE +:101C2000FBE7000070B5302304460E4683F31188AD +:101C3000A568A5B1A368A269013BA360531CA36179 +:101C400015782269934224BFE368A361E3690BB16D +:101C500020469847002383F31188284607E0314641 +:101C6000204600F08FFE0028E2DA85F3118870BD6F +:101C70002DE9F74F04460E4617469846D0F81C90BB +:101C80004FF0300A8AF311884FF0000B154665B10A +:101C90002A4631462046FFF741FF034660B94146D8 +:101CA000204600F06FFE0028F1D0002383F3118856 +:101CB000781B03B0BDE8F08FB9F1000F03D001909D +:101CC0002046C847019B8BF31188ED1A1E448AF306 +:101CD0001188DCE7C160C361009B82600362C0E9D8 +:101CE00005111144C0E9000001617047F8B50446D0 +:101CF0000D461646302383F31188A768A7B1A36861 +:101D0000013BA36063695A1C62611D70D4E904320F +:101D10009A4224BFE3686361E3690BB120469847A8 +:101D2000002080F3118807E03146204600F02AFEAB +:101D30000028E2DA87F31188F8BD0000D0E9052316 +:101D400010B59A4201D182687AB9826800210132C5 +:101D500082605A1C82611C7803699A4224BFC3685E +:101D6000836100F01FFE204610BD4FF0FF30FBE7FF +:101D70002DE9F74F04460E4617469846D0F81C90BA +:101D80004FF0300A8AF311884FF0000B154665B109 +:101D90002A4631462046FFF7EFFE034660B941462A +:101DA000204600F0EFFD0028F1D0002383F31188D6 +:101DB000781B03B0BDE8F08FB9F1000F03D001909C +:101DC0002046C847019B8BF31188ED1A1E448AF305 +:101DD0001188DCE7026843681143016003B11847CA +:101DE000704700001430FFF743BF00004FF0FF338F +:101DF0001430FFF73DBF00003830FFF7B9BF0000D7 +:101E00004FF0FF333830FFF7B3BF00001430FFF757 +:101E100009BF00004FF0FF311430FFF703BF00008F +:101E20003830FFF763BF00004FF0FF323830FFF764 +:101E30005DBF0000012914BF6FF013000020704740 +:101E4000FFF72ABC044B036000234360C0E9023360 +:101E500001230374704700BF984E000810B530236B +:101E6000044683F31188FFF713FD02230020237437 +:101E700080F3118810BD000038B5C36904460D46D3 +:101E80001BB904210844FFF7A5FF294604F11400FB +:101E9000FFF7ACFE002806DA201D4FF40061BDE814 +:101EA0003840FFF797BF38BD0268436811430160AF +:101EB00003B118477047000013B5406B00F580541C +:101EC000D4F8A4381A681178042914D1017C0229A5 +:101ED00011D11979012312898B4013420BD101A929 +:101EE0004C3002F0B3F9D4F8A4480246019B2179A2 +:101EF000206800F0DFF902B010BD0000143002F0DD +:101F000035B900004FF0FF33143002F02FB9000054 +:101F10004C3002F007BA00004FF0FF334C3002F0B3 +:101F200001BA0000143002F003B900004FF0FF3195 +:101F3000143002F0FDB800004C3002F0D3B90000BC +:101F40004FF0FF324C3002F0CDB900000020704756 +:101F500010B500F58054D4F8A4381A681178042913 +:101F600017D1017C022914D15979012352898B4060 +:101F700013420ED1143002F095F8024648B1D4F85D +:101F8000A4484FF4407361792068BDE8104000F028 +:101F90007FB910BD406BFFF7DBBF0000704700004A +:101FA0007FB5124B01250426044603600023057407 +:101FB00000F1840243602946C0E902330C4B0290D1 +:101FC000143001934FF44073009602F047F8094B28 +:101FD00004F69442294604F14C000294CDE90063D2 +:101FE0004FF4407302F00EF904B070BDC04E00080B +:101FF000951F0008B91E00080A68302383F3118872 +:102000000B790B3342F823004B79133342F823004A +:102010008B7913B10B3342F8230000F58053C3F8DA +:10202000A41802230374002080F311887047000075 +:1020300038B5037F044613B190F85430ABB901258D +:10204000201D0221FFF730FF04F114006FF00101A1 +:10205000257700F0B3FC04F14C0084F854506FF085 +:102060000101BDE8384000F0A9BC38BD10B5012120 +:1020700004460430FFF718FF0023237784F8543018 +:1020800010BD000038B504460025143001F0FEFFF5 +:1020900004F14C00257702F0CDF8201D84F854504F +:1020A0000121FFF701FF2046BDE83840FFF750BF90 +:1020B00090F8803003F06003202B06D190F8812047 +:1020C0000023212A03D81F2A06D800207047222A7D +:1020D000FBD1C0E91D3303E0034A42670722826750 +:1020E000C3670120704700BF4422002037B500F5C8 +:1020F0008055D5F8A4381A68117804291AD1017CC2 +:10210000022917D11979012312898B40134211D169 +:1021100000F14C04204602F04DF958B101A92046C7 +:1021200002F094F8D5F8A4480246019B2179206872 +:1021300000F0C0F803B030BD01F10B03F0B550F86A +:10214000236085B004460D46FEB1302383F3118829 +:1021500004EB8507301D0821FFF7A6FEFB6806F19A +:102160004C005B691B681BB1019002F07DF801987F +:1021700003A902F06BF8024648B1039B29462046AA +:1021800000F098F8002383F3118805B0F0BDFB68D8 +:102190005A691268002AF5D01B8A013B1340F1D11D +:1021A00004F18002EAE70000133138B550F821400D +:1021B000ECB1302383F3118804F58053D3F8A428BD +:1021C0001368527903EB8203DB689B695D6845B154 +:1021D00004216018FFF768FE294604F1140001F09D +:1021E0006BFF2046FFF7B4FE002383F3118838BD50 +:1021F0007047000001F038BA01234022002110B5D9 +:10220000044600F8303BFEF7E5FE0023C4E9013345 +:1022100010BD000010B53023044683F3118824223A +:10222000416000210C30FEF7D5FE204601F03EFA59 +:1022300002230020237080F3118810BD70B500EBDD +:102240008103054650690E461446DA6018B1102223 +:102250000021FEF7BFFEA06918B110220021FEF791 +:10226000B9FE31462846BDE8704001F025BB0000AC +:1022700083682022002103F0011310B50446836017 +:102280001030FEF7A7FE2046BDE8104001F0A0BBCD +:10229000F0B4012500EB810447898D40E4683D439B +:1022A000A469458123600023A2606360F0BC01F053 +:1022B000BDBB0000F0B4012500EB810407898D400F +:1022C000E4683D436469058123600023A260636084 +:1022D000F0BC01F033BC000070B5022300250446B9 +:1022E000242203702946C0F888500C3040F8045C62 +:1022F000FEF770FE204684F8705001F071FA6368B2 +:102300001B6823B129462046BDE87040184770BDC0 +:102310000378052B10B504460AD080F88C300523CD +:10232000037043681B680BB1042198470023A36026 +:1023300010BD00000178052906D190F88C20436873 +:1023400002701B6803B118477047000070B590F821 +:102350007030044613B1002380F8703004F180021D +:10236000204601F059FB63689B68B3B994F880304C +:1023700013F0600535D00021204601F04BFE00210E +:10238000204601F03BFE63681B6813B1062120461E +:102390009847062384F8703070BD2046984700287F +:1023A000E4D0B4F88630A26F9A4288BFA36794F94C +:1023B0008030A56F002B4FF0300380F20381002D99 +:1023C00000F0F280092284F8702083F31188002144 +:1023D0002046D4E91D23FFF76DFF002383F3118806 +:1023E000DAE794F8812003F07F0343EA022340F206 +:1023F0000232934200F0C58021D8B3F5807F48D0E7 +:102400000DD8012B3FD0022B00F09380002BB2D1CE +:1024100004F1880262670222A267E367C1E7B3F5AD +:10242000817F00F09B80B3F5407FA4D194F8823087 +:10243000012BA0D1B4F8883043F0020332E0B3F5A9 +:10244000006F4DD017D8B3F5A06F31D0A3F5C0639E +:10245000012B90D86368204694F882205E6894F837 +:102460008310B4F88430B047002884D04368636791 +:102470000368A3671AE0B3F5106F36D040F6024246 +:1024800093427FF478AF5C4B63670223A36700231A +:10249000C3E794F88230012B7FF46DAFB4F8883035 +:1024A00023F00203A4F88830C4E91D55E56778E7F6 +:1024B000B4F88030B3F5A06F0ED194F88230204686 +:1024C00084F88A3001F0EAF963681B6813B10121CE +:1024D00020469847032323700023C4E91D339CE75B +:1024E00004F18B0363670123C3E72378042B10D126 +:1024F000302383F311882046FFF7BAFE85F3118855 +:102500000321636884F88B5021701B680BB120464F +:10251000984794F88230002BDED084F88B30042367 +:10252000237063681B68002BD6D002212046984791 +:10253000D2E794F8843020461D0603F00F010AD537 +:1025400001F05CFA012804D002287FF414AF2B4B71 +:102550009AE72B4B98E701F043FAF3E794F88230BF +:10256000002B7FF408AF94F8843013F00F01B3D040 +:102570001A06204602D501F065FDADE701F056FDD3 +:10258000AAE794F88230002B7FF4F5AE94F88430FB +:1025900013F00F01A0D01B06204602D501F03AFD32 +:1025A0009AE701F02BFD97E7142284F8702083F35B +:1025B00011882B462A4629462046FFF769FE85F3F7 +:1025C0001188E9E65DB1152284F8702083F3118843 +:1025D00000212046D4E91D23FFF75AFEFDE60B2219 +:1025E00084F8702083F311882B462A46294620461A +:1025F000FFF760FEE3E700BFF04E0008E84E00087A +:10260000EC4E000838B590F870300446002B3ED0F0 +:10261000063BDAB20F2A34D80F2B32D8DFE803F0AA +:1026200037313108223231313131313131313737BF +:10263000856FB0F886309D4214D2C3681B8AB5FB03 +:10264000F3F203FB12556DB9302383F311882B4647 +:102650002A462946FFF72EFE85F311880A2384F8BF +:1026600070300EE0142384F87030302383F3118827 +:10267000002320461A461946FFF70AFE002383F37B +:10268000118838BDC36F03B198470023E7E70021E5 +:10269000204601F0BFFC0021204601F0AFFC63683A +:1026A0001B6813B10621204698470623D7E7000090 +:1026B00010B590F870300446142B29D017D8062B8B +:1026C00005D001D81BB110BD093B022BFBD800215E +:1026D000204601F09FFC0021204601F08FFC63683A +:1026E0001B6813B1062120469847062319E0152BD5 +:1026F000E9D10B2380F87030302383F31188002355 +:102700001A461946FFF7D6FD002383F31188DAE74E +:10271000C3689B695B68002BD5D1C36F03B1984731 +:10272000002384F87030CEE70023826803750369C4 +:102730001B6899689142FBD25A680360426010603E +:102740005860704700238268037503691B689968A5 +:102750009142FBD85A68036042601060586070472D +:1027600008B50846302383F311880A7D0023052A23 +:1027700006D8DFE802F00B050503120E826913602C +:102780004FF0FF338361FFF7CFFF002383F31188FE +:1027900008BD8269936801339360D0E90032136009 +:1027A0005A60EDE7FFF7C0BF38B50A4B1C68DD681B +:1027B0002875226853601A600122DC60A3682275C4 +:1027C000934201D100F016FE29462046BDE838406C +:1027D000FDF718BE604300200C4B30B5DD684B1C84 +:1027E00087B004460FD02B46094A684600F07AF9B4 +:1027F0002046FFF7D9FF009B13B1684600F07CF933 +:10280000A86907B030BDFFF7CFFFF9E760430020AC +:102810006127000870B50F4E04468161F368816836 +:102820009A68914203D8BDE87040FFF77DBF184613 +:10283000FFF788FF83680546012B01D100F0DCFD1E +:10284000012329462046F4602375BDE87040FDF75A +:10285000D9BD00BF60430020044B1A68DB68906854 +:102860009B68984294BF002001207047604300207D +:1028700038B50B4B1C68DD68226853601A60012272 +:10288000DC602275AB68934201D100F0B5FD2846AB +:10289000FFF758FF01462046BDE83840FDF7B2BDBE +:1028A0006043002038B50123084C00252370656083 +:1028B00001F0BCFD01F0E2FD0549064801F0B6FE5D +:1028C0000223237085F3118838BD00BFC84500205E +:1028D000F84E00086043002000F044B9034A5168F4 +:1028E00053685B1A9842FBD8704700BF001000E0A5 +:1028F0008B600223086108468B8270478368A3F1CE +:10290000840243F8142C026943F8442C426943F8CA +:10291000402C094A43F8242CC268A3F1200043F854 +:10292000182C022203F80C2C002203F80B2C034A6B +:1029300043F8102C704700BF1D04000860430020BE +:1029400008B5FFF7DBFFBDE80840FFF72BBF00002D +:10295000024BDB6898610F20FFF726BF6043002021 +:10296000302383F31188FFF7F3BF000008B5014659 +:10297000302383F311880820FFF72EFF002383F311 +:10298000118808BD064BDB6839B1426818605A608F +:10299000136043600420FFF71FBF4FF0FF30704704 +:1029A000604300200368984206D01A6802605060B5 +:1029B00018469961FFF7F6BE7047000038B5044627 +:1029C0000D462068844200D138BD036823605C60F6 +:1029D0008561FFF7E7FEF4E7036810B59C68A24243 +:1029E0000CD85C688A600B604C6021605960996803 +:1029F0008A1A9A604FF0FF33836010BD121B1B6868 +:102A0000ECE700000A2938BF0A2170B504460D46DC +:102A10000A26601901F004FD01F0ECFC041BA5423C +:102A200003D8751C04462E46F3E70A2E04D901206C +:102A3000BDE8704001F03ABE70BD0000F8B5144B1F +:102A40000D460A2A4FF00A07D96103F1100182608E +:102A500038BF0A2241601969144601604860186154 +:102A6000A81801F0CDFC01F0C5FC431B0646A342AB +:102A700006D37C1C28192746354601F0D1FCF2E725 +:102A80000A2F04D90120BDE8F84001F00FBEF8BDBF +:102A900060430020F8B506460D4601F0ABFC0F4A36 +:102AA000134653F8107F9F4206D12A46014630460E +:102AB000BDE8F840FFF7C2BFD169BB68441A2C19C2 +:102AC00028BF2C46A34202D92946FFF79BFF224686 +:102AD00031460348BDE8F840FFF77EBF6043002061 +:102AE00070430020C0E90323002310B45DF8044BB9 +:102AF0004361FFF7CFBF000010B5194C236998421E +:102B00000DD08168D0E9003213605A609A680A4497 +:102B10009A60002303604FF0FF33A36110BD026889 +:102B2000234643F8102F53600022026022699A4224 +:102B300003D1BDE8104001F06DBC936881680B447F +:102B4000936001F057FC2269E1699268441AA2423D +:102B5000E4D91144BDE81040091AFFF753BF00BF84 +:102B6000604300202DE9F047DFF8BC8008F1100732 +:102B70002C4ED8F8105001F03DFCD8F81C40AA6843 +:102B8000031B9A423ED814444FF00009D5E90032A5 +:102B9000C8F81C4013605A60C5F80090D8F810308F +:102BA000B34201D101F036FC89F31188D5E9033134 +:102BB00028469847302383F311886B69002BD8D0BF +:102BC00001F018FC6A69A0EB040982464A450DD25F +:102BD000022001F06BFD0022D8F81030B34208D17A +:102BE00051462846BDE8F047FFF728BF121A224495 +:102BF000F2E712EB09092946384638BF4A46FFF783 +:102C0000EBFEB5E7D8F81030B34208D01444C8F84A +:102C10001C00211AA960BDE8F047FFF7F3BEBDE82C +:102C2000F08700BF70430020604300200020704701 +:102C3000FEE70000704700004FF0FF3070470000D3 +:102C400002290CD0032904D00129074818BF00200D +:102C50007047032A05D8054800EBC20070470448B6 +:102C600070470020704700BFD04F0008542200205A +:102C7000844F000870B59AB005460846144601A96D +:102C800000F0C2F801A8FEF79DF9431C0022C6B26D +:102C90005B001046C5E9003423700323023404F8B6 +:102CA000013C01ABD1B202348E4201D81AB070BDE2 +:102CB00013F8011B013204F8010C04F8021CF1E7BF +:102CC00008B5302383F311880348FFF70FFA002378 +:102CD00083F3118808BD00BFD045002090F88030F4 +:102CE00003F01F02012A07D190F881200B2A03D19B +:102CF0000023C0E91D3315E003F06003202B08D149 +:102D0000B0F884302BB990F88120212A03D81F2AEB +:102D100004D8FFF7CDB9222AEBD0FAE7034A42677D +:102D200007228267C3670120704700BF4B22002043 +:102D300007B5052917D8DFE801F01916031919207E +:102D4000302383F31188104A01210190FFF776FAAE +:102D5000019802210D4AFFF771FA0D48FFF792F929 +:102D6000002383F3118803B05DF804FB302383F361 +:102D700011880748FFF75CF9F2E7302383F31188E5 +:102D80000348FFF773F9EBE7244F0008484F0008AA +:102D9000D045002038B50C4D0C4C2A460C4904F1A6 +:102DA0000800FFF767FF05F1CA0204F110000949A6 +:102DB000FFF760FF05F5CA7204F118000649BDE887 +:102DC0003840FFF757BF00BFA85E00205422002004 +:102DD000044F00080D4F0008184F000870B5044656 +:102DE00008460D46FEF7EEF8C6B2204601340378D9 +:102DF0000BB9184670BD32462946FEF7CFF80028B9 +:102E0000F3D10120F6E700002DE9F84F05460C4606 +:102E1000FEF7D8F82B49C6B22846FFF7DFFF2A494C +:102E20002846FFF7DBFF08B11036F6B2632E0DD847 +:102E3000DFF89080DFF89090244FDFF898A0DFF85B +:102E400098B02E7846B92670BDE8F88F29462046FE +:102E5000BDE8F84F01F05EBF252E2ED10722414676 +:102E60002846FEF79BF870B9DBF8003007350734C9 +:102E700044F8073CBBF8043024F8033C9BF80630C8 +:102E800004F8013CDDE7082249462846FEF786F8AB +:102E900098B9A21C0E4B197802320909C95D02F8D3 +:102EA000041C13F8011B01F00F015345C95D02F822 +:102EB000031CF0D118340835C3E7013504F8016B61 +:102EC000BFE700BFF04F0008184F0008005000088F +:102ED00000E8F11F0CE8F11FF84F0008BFF34F8F17 +:102EE000044B1A695107FCD1D3F810215207F8D1CD +:102EF000704700BF0020005208B50D4B1B78ABB9DE +:102F0000FFF7ECFF0B4BDA68D10704D50A4A5A6089 +:102F100002F188325A60D3F80C21D20706D5064A4E +:102F2000C3F8042102F18832C3F8042108BD00BFB0 +:102F300006610020002000522301674508B5114BAF +:102F40001B78F3B9104B1A69510703D5DA6842F0C0 +:102F50004002DA60D3F81021520705D5D3F80C21CE +:102F600042F04002C3F80C21FFF7B8FF064BDA68C5 +:102F700042F00102DA60D3F80C2142F00102C3F8FA +:102F80000C2108BD06610020002000520F289ABFC6 +:102F900000F5806040040020704700004FF40030CE +:102FA00070470000102070470F2808B50BD8FFF7B6 +:102FB000EDFF00F500330268013204D10430834292 +:102FC000F9D1012008BD0020FCE700000F2838B52A +:102FD00005463FD8FFF782FF1F4CFFF78DFF4FF0EC +:102FE000FF3307286361C4F814311DD82361FFF74C +:102FF00075FF030243F02403E360E36843F08003BA +:10300000E36023695A07FCD42846FFF767FFFFF700 +:10301000BDFF4FF4003100F0C5F92846FFF78EFFE1 +:10302000BDE83840FFF7C0BFC4F81031FFF756FFC6 +:10303000A0F108031B0243F02403C4F80C31D4F8B8 +:103040000C3143F08003C4F80C31D4F810315B0725 +:10305000FBD4D9E7002038BD002000522DE9F84FFD +:1030600004460D46104644EA0203DE0602D0002064 +:10307000BDE8F88F20F01F00DFF8D4B0DFF8D4A04F +:10308000FFF73AFF05EB0008454503D10120FFF7A4 +:1030900055FFEDE720222946204601F02BFE10B90E +:1030A00020342035F0E7234604F120021F68791C04 +:1030B000DDD104339342F9D104F178432048214E05 +:1030C000B3F5801F204B38BF184603F1F80332BF19 +:1030D000D946D1461E46FFF701FF0760A4EB050C59 +:1030E000336805F11C0143F0020333602B1FD9F84C +:1030F000007017F00507FAD153F8042F8B424CF8F3 +:103100000320F4D1BFF34F8FFFF7E8FE4FF0FF33FA +:103110000360336823F002033360BFF34F8F0B4B20 +:10312000C3F85C42BFF34F8FBFF36F8F2022294655 +:10313000204601F0DFFD0028B2D03846A7E700BFE7 +:10314000142100520C2000521420005200ED00E027 +:10315000102000521021005210B5084C237828B1DD +:103160001BB9FFF7C9FE0123237010BD002BFCD053 +:103170002070BDE81040FFF7E1BE00BF06610020EF +:103180002DE9F04F0D4685B0814658B111F00D067E +:1031900014BF2022082211F00803019304D0431E1B +:1031A00003426AD0002435E0002E37D009F11F0118 +:1031B00021F01F094FF00108324F05F00403DFF83A +:1031C000D0A005EA080BBBF1000F32D07869C00728 +:1031D0002FD408F101080C37B8F1060FF3D19EB9CE +:1031E000294D4946A819019201F06AF904460028C0 +:1031F0003AD11836019A782EF3D1494601F060F998 +:103200000446002830D1019A4946204801F058F977 +:10321000044668BB204605B0BDE8F08F0029C9D13F +:1032200001462846029201F04BF90446E0B9029AA1 +:10323000C0E713B178694107CBD5AC0702D57869EF +:103240008007C6D5019911B178690107C1D5182049 +:10325000494600FB08A0CDE9022301F031F90446FC +:10326000DDE902230028B4D04A460021204601E0CF +:103270004A460021FDF7AEFECCE70246002E95D16E +:1032800098E700BF14500008386100200861002052 +:10329000206100200121FFF773BF0000F8B5002373 +:1032A0001A461A600433B3F5806FFAD1114D012428 +:1032B0001827114E07FB046001342A6955F80C1FCA +:1032C00001F0EAF8062CF5D137254FF4C05420461A +:1032D000FFF7E0FF014628B122460848BDE8F84064 +:1032E00001F0DAB8C4EBC404013D4FEAD404EED1D6 +:1032F000F8BD00BF145000082061002008610020C4 +:1033000008B101F04FB970470244074BD2B210B573 +:10331000904200D110BD441C00B253F8200041F887 +:10332000040BE0B2F4E700BF504000580E4B30B53C +:103330001C6F240405D41C6F1C671C6F44F40044EC +:103340001C670A4C02442368D2B243F480732360A2 +:10335000074B904200D130BD441C51F8045B00B2D1 +:1033600043F82050E0B2F4E7004402580048025805 +:103370005040005807B5012201A90020FFF7C4FF03 +:10338000019803B05DF804FB13B50446FFF7F2FFA4 +:10339000A04205D0012201A900200194FFF7C6FF39 +:1033A00002B010BD0144BFF34F8F064B884204D3D7 +:1033B000BFF34F8FBFF36F8F7047C3F85C022030AD +:1033C000F4E700BF00ED00E0034B1A681AB9034AA6 +:1033D000D2F8D0241A607047B06100200040025833 +:1033E00008B5FFF7F1FF024B1868C0F3806008BD15 +:1033F000B06100207047000070470000EFF30983C0 +:10340000054968334A6B22F001024A6383F3098855 +:10341000002383F31188704700EF00E0302080F331 +:10342000118862B60D4B0E4AD96821F4E061090497 +:10343000090C0A430B49DA60D3F8FC2042F0807291 +:10344000C3F8FC20084AC2F8B01F116841F001011E +:1034500011602022DA7783F82200704700ED00E047 +:103460000003FA0555CEACC5001000E0302310B5BE +:1034700083F311880E4B5B6813F4006314D0F1EEF4 +:10348000103AEFF309844FF08073683CE361094B15 +:10349000DB6B236684F30988FFF7DEF910B1064B76 +:1034A000A36110BD054BFBE783F31188F9E700BF6B +:1034B00000ED00E000EF00E02F04000832040008F7 +:1034C00070B5BFF34F8FBFF36F8F1A4A0021C2F858 +:1034D0005012BFF34F8FBFF36F8F536943F4003324 +:1034E0005361BFF34F8FBFF36F8FC2F88410BFF3E8 +:1034F0004F8FD2F8803043F6E074C3F3C900C3F3B2 +:103500004E335B0103EA0406014646EA8175013940 +:10351000C2F86052F9D2203B13F1200FF2D1BFF371 +:103520004F8F536943F480335361BFF34F8FBFF321 +:103530006F8F70BD00ED00E0FEE70000214B2248D8 +:10354000224A70B5904237D3214BC11EDA1C121AA1 +:1035500022F003028B4238BF00220021FDF73AFD22 +:103560001C4A0023C2F88430BFF34F8FD2F880305A +:1035700043F6E074C3F3C900C3F34E335B0103EABF +:103580000406014646EA81750139C2F86C52F9D247 +:10359000203B13F1200FF2D1BFF34F8FBFF36F8F9A +:1035A000BFF34F8FBFF36F8F0023C2F85032BFF3CA +:1035B0004F8FBFF36F8F70BD53F8041B40F8041B8F +:1035C000C0E700BF4C5200082463002024630020A1 +:1035D0002463002000ED00E0074BD3F8D81021EA67 +:1035E0000001C3F8D810D3F8002122EA0002C3F882 +:1035F0000021D3F8003170470044025870B5D0E97B +:10360000244300224FF0FF359E6804EB4213510122 +:10361000D3F80009002805DAD3F8000940F080400B +:10362000C3F80009D3F8000B002805DAD3F8000B23 +:1036300040F08040C3F8000B013263189642C3F893 +:103640000859C3F8085BE0D24FF00113C4F81C38E6 +:1036500070BD0000890141F02001016103699B06F2 +:10366000FCD41220FFF73AB910B50A4C2046FEF7F9 +:10367000C3FD094BC4F89030084BC4F89430084C93 +:103680002046FEF7B9FD074BC4F89030064BC4F84E +:10369000943010BDB4610020000008408050000844 +:1036A00050620020000004408C50000870B5037880 +:1036B0000546012B5CD1434BD0F89040984258D13D +:1036C000414B0E216520D3F8D82042F00062C3F8A8 +:1036D000D820D3F8002142F00062C3F80021D3F8CB +:1036E0000021D3F8802042F00062C3F88020D3F894 +:1036F000802022F00062C3F88020D3F88030FEF7EB +:10370000E1F9324BE360324BC4F800380023D5F8BE +:103710009060C4F8003EC02323604FF40413A363F9 +:103720003369002BFCDA01230C203361FFF7D6F854 +:103730003369DB07FCD41220FFF7D0F83369002B84 +:10374000FCDA00262846A660FFF758FF6B68C4F82D +:103750001068DB68C4F81468C4F81C6883BB1D4B90 +:10376000A3614FF0FF336361A36843F00103A360DB +:1037700070BD194B9842C9D1134B4FF08060D3F8FC +:10378000D82042F00072C3F8D820D3F8002142F0CC +:103790000072C3F80021D3F80021D3F8802042F052 +:1037A0000072C3F88020D3F8802022F00072C3F8A2 +:1037B0008020D3F88030FFF70FFF0E214D209EE7C9 +:1037C000064BCDE7B461002000440258401400408D +:1037D00003002002003C30C050620020083C30C092 +:1037E000F8B5D0F89040054600214FF0006620461D +:1037F000FFF730FFD5F8941000234FF001128F68C7 +:103800004FF0FF30C4F83438C4F81C2804EB4312DE +:1038100001339F42C2F80069C2F8006BC2F8080980 +:10382000C2F8080BF2D20B68D5F89020C5F8983092 +:10383000636210231361166916F01006FBD1122083 +:10384000FFF74CF8D4F8003823F4FE63C4F80038CE +:10385000A36943F4402343F01003A3610923C4F890 +:103860001038C4F814380B4BEB604FF0C043C4F869 +:10387000103B094BC4F8003BC4F81069C4F8003988 +:10388000D5F8983003F1100243F48013C5F898205E +:10389000A362F8BD5C50000840800010D0F8902072 +:1038A00090F88A10D2F8003823F4FE6343EA01133B +:1038B000C2F80038704700002DE9F84300EB81039F +:1038C000D0F890500C468046DA680FFA81F948012A +:1038D000166806F00306731E022B05EB41134FF02A +:1038E000000194BFB604384EC3F8101B4FF001011D +:1038F00004F1100398BF06F1805601FA03F39169B1 +:1039000098BF06F5004600293AD0578A04F15801BD +:10391000374349016F50D5F81C180B430021C5F8F7 +:103920001C382B180127C3F81019A7405369611ED2 +:103930009BB3138A928B9B08012A88BF5343D8F804 +:103940009820981842EA034301F140022146C8F842 +:103950009800284605EB82025360FFF77BFE08EBD8 +:103960008900C3681B8A43EA845348341E436401B8 +:103970002E51D5F81C381F43C5F81C78BDE8F883D4 +:1039800005EB4917D7F8001B21F40041C7F8001BCD +:10399000D5F81C1821EA0303C0E704F13F030B4AE2 +:1039A0002846214605EB83035A60FFF753FE05EBDB +:1039B0004910D0F8003923F40043C0F80039D5F895 +:1039C0001C3823EA0707D7E7008000100004000234 +:1039D000D0F894201268C0F89820FFF70FBE0000BE +:1039E0005831D0F8903049015B5813F4004004D0AE +:1039F00013F4001F0CBF0220012070474831D0F89B +:103A0000903049015B5813F4004004D013F4001FB8 +:103A10000CBF02200120704700EB8101CB68196ABE +:103A20000B6813604B6853607047000000EB810324 +:103A300030B5DD68AA691368D36019B9402B84BF1B +:103A4000402313606B8A1468D0F890201C4402EB6A +:103A50004110013C09B2B4FBF3F46343033323F098 +:103A6000030343EAC44343F0C043C0F8103B2B6850 +:103A700003F00303012B0ED1D2F8083802EB4110FA +:103A800013F4807FD0F8003B14BF43F0805343F021 +:103A90000053C0F8003B02EB4112D2F8003B43F068 +:103AA0000443C2F8003B30BD2DE9F041D0F89060EE +:103AB00005460C4606EB4113D3F8087B3A07C3F8DA +:103AC000087B08D5D6F814381B0704D500EB810312 +:103AD000DB685B689847FA071FD5D6F81438DB0710 +:103AE0001BD505EB8403D968CCB98B69488A5A6821 +:103AF000B2FBF0F600FB16228AB91868DA68904229 +:103B00000DD2121AC3E90024302383F31188214611 +:103B10002846FFF78BFF84F31188BDE8F08101236D +:103B200003FA04F26B8923EA02036B81CB68002B52 +:103B3000F3D021462846BDE8F041184700EB810349 +:103B40004A0170B5DD68D0F890306C692668E6608F +:103B500056BB1A444FF40020C2F810092A6802F03C +:103B60000302012A0AB20ED1D3F8080803EB42146B +:103B700010F4807FD4F8000914BF40F0805040F06A +:103B80000050C4F8000903EB4212D2F8000940F0DB +:103B90000440C2F800090122D3F8340802FA01F106 +:103BA0000143C3F8341870BD19B9402E84BF4020BA +:103BB000206020681A442E8A8419013CB4FBF6F474 +:103BC00040EAC44040F00050C6E700002DE9F84349 +:103BD000D0F8906005460C464F0106EB4113D3F830 +:103BE000088918F0010FC3F808891CD0D6F81038DE +:103BF000DB0718D500EB8103D3F80CC0DCF81430D8 +:103C0000D3F800E0DA68964530D2A2EB0E024FF00E +:103C100000091A60C3F80490302383F31188FFF77A +:103C20008DFF89F3118818F0800F1DD0D6F8343835 +:103C30000126A640334217D005EB84030134D5F8A2 +:103C40009050D3F80CC0E4B22F44DCF8142005EBFC +:103C50000434D2F800E05168714514D3D5F83438F3 +:103C600023EA0606C5F83468BDE8F883012303FAA1 +:103C700001F2038923EA02030381DCF80830002BF8 +:103C8000D1D09847CFE7AEEB0103BCF810008342D8 +:103C900028BF0346D7F8180980B2B3EB800FE3D8EA +:103CA0009068A0F1040959F8048FC4F80080A0EBD3 +:103CB00009089844B8F1040FF5D818440B449060F3 +:103CC0005360C8E72DE9F84FD0F8905004466E696C +:103CD000AB691E4016F480586E6103D0BDE8F84F02 +:103CE000FEF7FABA002E12DAD5F8003E9B0705D08F +:103CF000D5F8003E23F00303C5F8003ED5F804389C +:103D0000204623F00103C5F80438FEF713FB3705FE +:103D100005D52046FFF772FC2046FEF7F9FAB004FD +:103D20000CD5D5F8083813F0060FEB6823F4705360 +:103D30000CBF43F4105343F4A053EB6031071BD581 +:103D40006368DB681BB9AB6923F00803AB612378B8 +:103D5000052B0CD1D5F8003E9A0705D0D5F8003ECA +:103D600023F00303C5F8003E2046FEF7E3FA63683C +:103D7000DB680BB120469847F30200F1BA80B70226 +:103D800026D5D4F8909000274FF0010A09EB47128E +:103D9000D2F8003B03F44023B3F5802F11D1D2F8C1 +:103DA000003B002B0DDA62890AFA07F322EA0303CB +:103DB000638104EB8703DB68DB6813B13946204677 +:103DC00098470137D4F89430FFB29B689F42DDD901 +:103DD000F00619D5D4F89000026AC2F30A1702F06F +:103DE0000F0302F4F012B2F5802F00F0CA80B2F592 +:103DF000402F09D104EB8303002200F58050DB68DB +:103E00001B6A974240F0B0803003D5F8185835D57A +:103E1000E90303D500212046FFF746FEAA0303D598 +:103E200001212046FFF740FE6B0303D50221204607 +:103E3000FFF73AFE2F0303D503212046FFF734FE98 +:103E4000E80203D504212046FFF72EFEA90203D580 +:103E500005212046FFF728FE6A0203D506212046E9 +:103E6000FFF722FE2B0203D507212046FFF71CFE99 +:103E7000EF0103D508212046FFF716FE700340F13D +:103E8000A780E90703D500212046FFF79FFEAA0778 +:103E900003D501212046FFF799FE6B0703D50221C8 +:103EA0002046FFF793FE2F0703D503212046FFF797 +:103EB0008DFEEE0603D504212046FFF787FEA806F7 +:103EC00003D505212046FFF781FE690603D50621AB +:103ED0002046FFF77BFE2A0603D507212046FFF781 +:103EE00075FEEB0574D520460821BDE8F84FFFF7B5 +:103EF0006DBED4F890904FF0000B4FF0010AD4F84B +:103F000094305FFA8BF79B689F423FF638AF09EB1E +:103F10004713D3F8002902F44022B2F5802F20D1B4 +:103F2000D3F80029002A1CDAD3F8002942F0904285 +:103F3000C3F80029D3F80029002AFBDB3946D4F85E +:103F40009000FFF787FB22890AFA07F322EA0303AE +:103F5000238104EB8703DB689B6813B13946204655 +:103F600098470BF1010BCAE7910701D1D0F8008007 +:103F7000072A02F101029CBF03F8018B4FEA1828BF +:103F80003FE704EB830300F58050DA68D2F818C0ED +:103F9000DCF80820DCE9001CA1EB0C0C00218F42AE +:103FA00008D1DB689B699A683A449A605A683A4437 +:103FB0005A6029E711F0030F01D1D0F800808C4539 +:103FC00001F1010184BF02F8018B4FEA1828E6E7EE +:103FD000BDE8F88F08B50348FFF774FEBDE8084058 +:103FE000FFF744BAB461002008B50348FFF76AFE42 +:103FF000BDE80840FFF73ABA50620020D0F8903090 +:1040000003EB4111D1F8003B43F40013C1F8003B2E +:1040100070470000D0F8903003EB4111D1F800391F +:1040200043F40013C1F8003970470000D0F8903015 +:1040300003EB4111D1F8003B23F40013C1F8003B1E +:1040400070470000D0F8903003EB4111D1F80039EF +:1040500023F40013C1F800397047000030B5043371 +:10406000039C0172002104FB0325C160C0E90653D3 +:10407000049B0363059BC0E90000C0E90422C0E97A +:104080000842C0E90A11436330BD00000022416AC2 +:10409000C260C0E90411C0E90A226FF00101FEF715 +:1040A0008DBC0000D0E90432934201D1C2680AB944 +:1040B000181D704700207047036919600021C2680D +:1040C0000132C260C269134482699342036124BF12 +:1040D000436A0361FEF766BC38B504460D46E368E3 +:1040E0003BB162690020131D1268A3621344E362AE +:1040F00007E0237A33B929462046FEF743FC00281F +:10410000EDDA38BD6FF00100FBE70000C368C2695B +:10411000013BC3604369134482699342436124BFF6 +:10412000436A436100238362036B03B118477047FE +:1041300070B53023044683F31188866A3EB9FFF7D1 +:10414000CBFF054618B186F31188284670BDA36AD7 +:10415000E26A13F8015B9342A36202D32046FFF7A1 +:10416000D5FF002383F31188EFE700002DE9F84F16 +:1041700004460E46174698464FF0300989F31188D9 +:104180000025AA46D4F828B0BBF1000F09D141465A +:104190002046FFF7A1FF20B18BF311882846BDE828 +:1041A000F88FD4E90A12A7EB050B521A934528BFE2 +:1041B0009346BBF1400F1BD9334601F1400251F841 +:1041C000040B914243F8040BF9D1A36A4036403501 +:1041D0004033A362D4E90A239A4202D32046FFF770 +:1041E00095FF8AF31188BD42D8D289F31188C9E7B7 +:1041F00030465A46FCF7C8FEA36A5E445D445B4401 +:10420000A362E7E710B5029C0433017203FB0421AB +:10421000C460C0E906130023C0E90A33039B0363AB +:10422000049BC0E90000C0E90422C0E908424363DE +:1042300010BD0000026A6FF00101C260426AC0E96D +:1042400004220022C0E90A22FEF7B8BBD0E9042309 +:104250009A4201D1C26822B9184650F8043B0B605B +:10426000704700231846FAE7C3680021C26901338A +:10427000C3604369134482699342436124BF436A24 +:104280004361FEF78FBB000038B504460D46E36876 +:104290003BB1236900201A1DA262E2691344E36264 +:1042A00007E0237A33B929462046FEF76BFB002846 +:1042B000EDDA38BD6FF00100FBE70000036919601B +:1042C000C268013AC260C2691344826993420361C1 +:1042D00024BF436A036100238362036B03B1184761 +:1042E0007047000070B530230D460446114683F335 +:1042F0001188866A2EB9FFF7C7FF10B186F31188BF +:1043000070BDA36A1D70A36AE26A01339342A3627F +:1043100004D3E16920460439FFF7D0FF002080F381 +:104320001188EDE72DE9F84F04460D469046994671 +:104330004FF0300A8AF311880026B346A76A4FB9B6 +:1043400049462046FFF7A0FF20B187F31188304689 +:10435000BDE8F88FD4E90A073A1AA8EB0607974296 +:1043600028BF1746402F1BD905F1400355F8042BF1 +:104370009D4240F8042BF9D1A36A40364033A36232 +:10438000D4E90A239A4204D3E16920460439FFF7AD +:1043900095FF8BF311884645D9D28AF31188CDE772 +:1043A00029463A46FCF7F0FDA36A3D443E443B44AF +:1043B000A362E5E7D0E904239A4217D1C3689BB111 +:1043C000836A8BB1043B9B1A0ED01360C368013B18 +:1043D000C360C3691A4483699A42026124BF436A75 +:1043E0000361002383620123184670470023FBE723 +:1043F00000F060BA014B586A704700BF000C0040E3 +:10440000034B002258631A610222DA60704700BF32 +:10441000000C0040014B0022DA607047000C0040A5 +:10442000014B5863704700BF000C0040024B034A29 +:104430001A60034A5A607047046300202863002012 +:1044400000000220074B494210B55C68201A084062 +:104450001968821A8A4203D3A24201D85A6010BD59 +:104460000020FCE70463002008B5302383F31188A3 +:10447000FFF7E8FF002383F3118808BD04480121FA +:10448000044B03600023C0E901330C3000F016B97F +:104490000C63002069440008CB1D083A23F0070391 +:1044A000591A521A012110B4D2080024C0E900435D +:1044B00084600C301C605A605DF8044B00F0FEB85C +:1044C0002DE9F84F364ECD1D0F46002818BF064681 +:1044D000082A4FEAD50538BF082206F10C08341D1A +:1044E0009146404600F006F909F10701C9F1000EB6 +:1044F000224624686CB9404600F006F93368CBB315 +:1045000008224946E8009847044698B340E9026704 +:1045100030E004EB010CD4F804A00CEA0E0C0AF114 +:104520000100ACF1080304EBC0009842E0D9A0EB15 +:104530000C0CB5EBEC0F4FEAEC0BD9D89C421CD21B +:1045400004F10802AB45A3EB02024FEAE20262600B +:1045500009D9691CED43206803EBC1025D44556035 +:1045600043F8310022601C465F60404644F8086B07 +:1045700000F0CAF82046BDE8F88FAA45216802D1AC +:1045800011602346EFE7013504EBC50344F835100D +:1045900003F10801401AC01058601360F1E700BF32 +:1045A0000C630020F8B550F8043C044650F8085C51 +:1045B000A0F1080607332F1D0C35DB0840F8043C3A +:1045C000284600F097F83B469F421A6801D0B34254 +:1045D00028D20AB1964225D244F8082C54F8042C6B +:1045E0001E60013254F8081C06EBC200814206D15D +:1045F0004868024444F8042C0A6844F8082C5868B7 +:10460000411C03EBC1018E4207D154F8042C013246 +:1046100002445A6054F8082C1A602846BDE8F84055 +:1046200000F072B81346CFE7FEE7000070B51B4BF1 +:104630000025044686B058600E4685620163FEF789 +:10464000EDFE04F11003A560E562C4E904334FF008 +:10465000FF33C4E90044C4E90635FFF7CBFE2B461F +:10466000024604F134012046C4E9082380230C4AA1 +:104670002565FEF73DF901230A4AE06000920375C3 +:10468000684672680192B268CDE90223064BCDE913 +:104690000435FEF755F906B070BD00BFC8450020CF +:1046A000985000089D50000829460008024AD36A25 +:1046B0001843D062704700BF60430020C0E900008B +:1046C000816070478368013B002B10B583600CDA72 +:1046D000074BDC684368A061206063601C60446035 +:1046E0000520FEF761F8A06910BD0020FCE700BFBF +:1046F0006043002008B5302383F31188FFF7E2FF01 +:10470000002383F3118808BD08B5302383F3118893 +:1047100083680133002B836007DC036800211A687B +:10472000026050601846FEF775F8002383F3118885 +:1047300008BD00004B6843608B688360CB68C36032 +:104740000B6943614B6903628B6943620B680360C9 +:104750007047000008B53C4B40F2FF713B48D3F86E +:1047600088200A43C3F88820D3F8882022F4FF6207 +:1047700022F00702C3F88820D3F88820D3F8E0207D +:104780000A43C3F8E020D3F808210A43C3F80821FC +:104790002F4AD3F808311146FFF7CCFF00F58060AF +:1047A00002F11C01FFF7C6FF00F5806002F138013D +:1047B000FFF7C0FF00F5806002F15401FFF7BAFF78 +:1047C00000F5806002F17001FFF7B4FF00F5806032 +:1047D00002F18C01FFF7AEFF00F5806002F1A80145 +:1047E000FFF7A8FF00F5806002F1C401FFF7A2FF08 +:1047F00000F5806002F1E001FFF79CFF00F58060AA +:1048000002F1FC01FFF796FF02F58C7100F5806064 +:10481000FFF790FF00F01AF90E4BD3F8902242F008 +:104820000102C3F89022D3F8942242F00102C3F8A7 +:1048300094220522C3F898204FF06052C3F89C20C0 +:10484000054AC3F8A02008BD0044025800000258E1 +:10485000A450000800ED00E01F00080308B500F0B8 +:1048600035FAFEF71FF8104BD3F8DC2042F0400277 +:10487000C3F8DC20D3F8042122F04002C3F804215D +:10488000D3F80431094B1A6842F008021A601A681A +:1048900042F004021A60FEF797FDFEF7FFFCBDE848 +:1048A0000840FEF777BA00BF0044025800180248DB +:1048B00070470000114BD3F8E82042F00802C3F81B +:1048C000E820D3F8102142F00802C3F810210C4A66 +:1048D000D3F81031D36B43F00803D363C722094BDD +:1048E0009A624FF0FF32DA6200229A615A63DA600C +:1048F0005A6001225A611A60704700BF0044025892 +:104900000010005C000C0040094A08B51169D3682A +:104910000B40D9B29B076FEA0101116107D5302323 +:1049200083F31188FDF7D8FF002383F3118808BDB6 +:10493000000C0040064BD3F8DC200243C3F8DC2017 +:10494000D3F804211043C3F80401D3F804317047AD +:104950000044025808B53C4B4FF0FF31D3F880209B +:1049600062F00042C3F88020D3F8802002F00042B9 +:10497000C3F88020D3F88020D3F88420C3F88410B3 +:10498000D3F884200022C3F88420D3F88400D86FA1 +:1049900040F0FF4040F4FF0040F4DF4040F07F0073 +:1049A000D867D86F20F0FF4020F4FF0020F4DF40EC +:1049B00020F07F00D867D86FD3F888006FEA4050A6 +:1049C0006FEA5050C3F88800D3F88800C0F30A009B +:1049D000C3F88800D3F88800D3F89000C3F890108B +:1049E000D3F89000C3F89020D3F89000D3F8940047 +:1049F000C3F89410D3F89400C3F89420D3F894002B +:104A0000D3F89800C3F89810D3F89800C3F898200A +:104A1000D3F89800D3F88C00C3F88C10D3F88C002E +:104A2000C3F88C20D3F88C00D3F89C00C3F89C10FA +:104A3000D3F89C10C3F89C20D3F89C30FCF700FD01 +:104A4000BDE8084000F01AB90044025808B5012238 +:104A5000534BC3F80821534BD3F8F42042F0020221 +:104A6000C3F8F420D3F81C2142F00202C3F81C2141 +:104A70000222D3F81C314C4BDA605A689104FCD501 +:104A80004A4A1A6001229A60494ADA6000221A6191 +:104A90004FF440429A61444B9A699204FCD51A68DB +:104AA00042F480721A603F4B1A6F12F4407F04D0B8 +:104AB0004FF480321A6700221A671A6842F0010226 +:104AC0001A60384B1A685007FCD500221A611A691F +:104AD00012F03802FBD1012119604FF0804159607A +:104AE0005A67344ADA62344A1A611A6842F48032E8 +:104AF0001A602C4B1A689103FCD51A6842F4805254 +:104B00001A601A689204FCD52C4A2D499A62002238 +:104B10005A63196301F57C01DA6301F2E7119963C5 +:104B20005A64284A1A64284ADA621A6842F0A8527B +:104B30001A601C4B1A6802F02852B2F1285FF9D1B2 +:104B400048229A614FF48862DA6140221A621F4A51 +:104B5000DA641F4A1A651F4A5A651F4A9A6532234A +:104B60001E4A1360136803F00F03022BFAD10D4A9B +:104B7000136943F003031361136903F03803182B1F +:104B8000FAD14FF00050FFF7D5FE4FF08040FFF70D +:104B9000D1FE4FF00040BDE80840FFF7CBBE00BF9C +:104BA00000800051004402580048025800C000F044 +:104BB000020000010000FF01008890081210200090 +:104BC00063020901470E0508DD0BBF01200000202C +:104BD000000001100910E000000101100020005247 +:104BE00008B50348FCF7E8FEBDE80840FEF73EBC08 +:104BF0006C24002008B50348FCF7DEFEBDE8084041 +:104C0000FEF734BCD824002008B50348FCF7D4FED6 +:104C1000BDE80840FEF72ABC4425002008B503483B +:104C2000FCF7CAFEBDE80840FEF720BCB025002016 +:104C300008B50348FCF7C0FEBDE80840FEF716BC07 +:104C40001C26002008B50348FCF7B6FEBDE8084066 +:104C5000FEF70CBC8826002008B50348FCF7ACFE24 +:104C6000BDE80840FEF702BCF426002008B5FFF7B7 +:104C70004BFEBDE80840FEF7F9BB000008B509216E +:104C80007A20FCF71FFF07213220FCF71BFF0C21C5 +:104C90002520FCF717FF0C212620FCF713FF0C2121 +:104CA0002720FCF70FFF0C213520FCF70BFF0C2110 +:104CB0004720FCF707FF0C215220FCF703FF0C21D3 +:104CC0005320BDE80840FCF7FDBE000008B5FFF723 +:104CD00041FE00F00DF8FDF7B3F8FDF78BFAFDF794 +:104CE0005DF9FFF7E5FDBDE80840FFF781BB000077 +:104CF0007047000010B501390244904201D10020F4 +:104D000005E0037811F8014FA34201D0181B10BD34 +:104D10000130F2E7034611F8012B03F8012B002ABA +:104D2000F9D1704765323031343638336239663301 +:104D300065646139000000001210000012110000CB +:104D40001213000053544D333248373F3F3F005356 +:104D5000544D3332483733782F3732780053544D1F +:104D60003332483734332F3735332F37353000005F +:104D700001105A000310590001205800032056006A +:104D800010000240080002400008024000000B0032 +:104D900028000240080002400408024006010C00FE +:104DA00040000240080002400808024010020D00C6 +:104DB00058000240080002400C08024016030E0092 +:104DC000700002400C0002401008024000040F0076 +:104DD000880002400C000240140802400605100042 +:104DE000A00002400C00024018080240100611000A +:104DF000B80002400C0002401C08024016072F00B9 +:104E00001004024008040240200802400008380054 +:104E10002804024008040240240802400609390020 +:104E2000400402400804024028080240100A3A00E8 +:104E300058040240080402402C080240160B3B00B4 +:104E4000700402400C04024030080240000C3C0098 +:104E5000880402400C04024034080240060D44005D +:104E6000A00402400C04024038080240100E450025 +:104E7000B80402400C0402403C080240160F4600F1 +:104E8000009600000000000000000000000000008C +:104E9000000000000000000000000000011E0008EB +:104EA000ED1D0008291E0008151E0008211E00081F +:104EB0000D1E0008F91D0008E51D0008351E00083C +:104EC00000000000191F0008051F0008411F00080E +:104ED0002D1F0008391F0008251F0008111F00089A +:104EE000FD1E00084D1F000800000000010000002A +:104EF0000000000063300000F44E0008B8430020BA +:104F0000C845002054616970686F6F6E0025424F7C +:104F1000415244252D424C002553455249414C25D0 +:104F2000000000000200000000000000392100081D +:104F3000A921000840004000785E0020885E002023 +:104F4000020000000000000003000000000000005C +:104F5000F12100080000000010000000985E002011 +:104F6000000000000100000000000000B46100200B +:104F700001010200312D0008412C0008DD2C000841 +:104F8000C12C0008430000008C4F000809024300B8 +:104F9000020100C0320904000001020201000524E0 +:104FA000001001052401000104240202052406006A +:104FB00001070582030800FF09040100020A00003E +:104FC00000070501024000000705810240000000C3 +:104FD00012000000D84F000812011001020000402A +:104FE00009124157000201020301000004030904F1 +:104FF00025424F41524425004D6F72616B6F740022 +:1050000030313233343536373839414243444546FE +:10501000000000000000002000000200020000006C +:105020000000003000000400080000000000002420 +:1050300000000800040000000004000000FC000064 +:1050400002000000000004300080000008000000A2 +:105050000000003800000100010000000000000016 +:105060004D23000805260008B12600084000400036 +:10507000EC620020EC62002001000000FC620020D5 +:105080008000000040010000080000000001000056 +:1050900000100000080000006D61696E0069646C1A +:1050A000650000000000802A00020000AAAAAAAA47 +:1050B00000000024FFFF00000000000000A00A0024 +:1050C000002800A000000000AAAAAAAA001400500C +:1050D000FFFF00000000E00E0000004433A00000CD +:1050E00000000000AAAAAAAA00500000FFFF0000CA +:1050F000000000770000000000281A45003400007E +:10510000AAAAAAAA00140500FFCB0000000070079D +:10511000770000000A80020000000000AAAAAAAAE4 +:1051200005400100FFFF000088000070070000003C +:105130000000000000000000AAAAAAAA00000000C7 +:10514000FFFF000000000000000000000000000061 +:1051500000000000AAAAAAAA00000000FFFF0000A9 +:10516000000000000000000000000000000000003F +:10517000AAAAAAAA00000000FFFF00000000000089 +:10518000000000000000000000000000AAAAAAAA77 +:1051900000000000FFFF0000000000000000000011 +:1051A0000000000000000000AAAAAAAA0000000057 +:1051B000FFFF0000000000000000000000000000F1 +:1051C00000000000AAAAAAAA00000000FFFF000039 +:1051D0000000000000000000BA0400000000000011 +:1051E00000001A0000000000FF0000006C240020F6 +:1051F000D824002044250020B02500201C260020B3 +:105200008826002000000000444D000883040000B0 +:105210004F4D0008500400005D4D0008009600004E +:1052200000000800960000000008000004000000D4 +:10523000EC4F00080000000000000000000000002B +:0C52400000000000000000000000000062 +:00000001FF diff --git a/Tools/bootloaders/VM-L431-BMS_bl.bin b/Tools/bootloaders/VM-L431-BMS_bl.bin new file mode 100755 index 0000000000000..b454395ad3e23 Binary files /dev/null and b/Tools/bootloaders/VM-L431-BMS_bl.bin differ diff --git a/Tools/bootloaders/VM-L431-BMS_bl.hex b/Tools/bootloaders/VM-L431-BMS_bl.hex new file mode 100644 index 0000000000000..8310d38c76740 --- /dev/null +++ b/Tools/bootloaders/VM-L431-BMS_bl.hex @@ -0,0 +1,1075 @@ +:020000040800F2 +:1000000000090020B101000809230008C1220008EE +:10001000E9220008C1220008E1220008B30100081B +:10002000B3010008B3010008B301000879330008E8 +:10003000B3010008B3010008B3010008B3010008D0 +:10004000B3010008B3010008B3010008B3010008C0 +:10005000B3010008B3010008B3010008B3010008B0 +:10006000B3010008B3010008B3010008B3010008A0 +:10007000B3010008B3010008B3010008B301000890 +:10008000B3010008B3010008B3010008752200089D +:10009000A1220008B1220008B3010008B301000842 +:1000A000B3010008B3010008B3010008B301000860 +:1000B000E9400008B3010008B3010008B3010008DB +:1000C000B3010008B3010008B3010008B301000840 +:1000D000B3010008B3010008B3010008B301000830 +:1000E000B3010008B3010008B3010008B301000820 +:1000F000B3010008B3010008B3010008B301000810 +:10010000B3010008B3010008B3010008B3010008FF +:10011000B3010008B3010008B3010008B3010008EF +:10012000B3010008B3010008B3010008B3010008DF +:10013000B3010008B3010008B3010008B3010008CF +:10014000B3010008B3010008B3010008B3010008BF +:10015000B3010008B3010008B3010008B3010008AF +:10016000B3010008B3010008B3010008B30100089F +:10017000B3010008B3010008B3010008B30100088F +:10018000B3010008B3010008B3010008B30100087F +:10019000B3010008B3010008B3010008B30100086F +:1001A000090F0008FFFFFFFFFFFFFFFFFFFFFFFF3B +:1001B00002E000F000F8FEE772B6374880F30888E6 +:1001C000364880F3098836483649086040F2000016 +:1001D000CCF200004EF63471CEF200010860BFF39D +:1001E0004F8FBFF36F8F40F20000C0F2F0004EF669 +:1001F0008851CEF200010860BFF34F8FBFF36F8FBD +:100200004FF00000E1EE100A4EF63C71CEF2000114 +:100210000860062080F31488BFF36F8F03F088FD19 +:1002200003F00AFE4FF055301F491B4A91423CBF74 +:1002300041F8040BFAE71D49184A91423CBF41F8C6 +:10024000040BFAE71A491B4A1B4B9A423EBF51F86E +:10025000040B42F8040BF8E700201849184A9142B1 +:100260003CBF41F8040BFAE703F066FD03F02EFEF5 +:10027000144C154DAC4203DA54F8041B8847F9E7D7 +:1002800000F042F8114C124DAC4203DA54F8041B52 +:100290008847F9E703F04EBD000900200011002057 +:1002A0000000000808ED00E0000100200009002027 +:1002B00088420008001100207C11002080110020DD +:1002C000C43A0020A0010008A4010008A40100080D +:1002D000A40100082DE9F04F2DED108AC1F80CD0D3 +:1002E000C3689D46BDEC108ABDE8F08F002383F300 +:1002F00011882846A047002003F0BEF9FEE703F06E +:1003000041F900DFFEE70000F8B500F00FFE03F052 +:10031000ADFC074603F0FEFC0646C0BB274B9F42E0 +:1003200035D001339F4235D0254B27F0FF029A424A +:1003300033D1F8B200F03AFC354642F2107400F0C6 +:100340003BFC08B10024254601F0D8F848B303204F +:1003500000F03AF80024254636B11A4B9F4203D0EC +:1003600003F0D0FC00242546002003F089FC0DB1E9 +:1003700000F040F800F054FC01F0CEFF0546ACB9A7 +:1003800000F094FC4FF47A7003F07CF9F7E73546FF +:100390000024D4E704460125D1E705464FF47A74DA +:1003A000CDE7B4F57A7F08BF0125D5E701F0B4FFAA +:1003B000431BA342E4D900F01DF8DDE7010007B0BC +:1003C000000008B0263A09B0084B187003280CD872 +:1003D000DFE800F008050208022000F0FDBD022061 +:1003E00000F0F8BD024B00225A60704780110020D7 +:1003F0008411002010B501F081F830B1234B0322A5 +:100400001A70234B00225A6010BD224B224A1C4610 +:1004100019680131F8D004339342F9D16268A242DD +:10042000F2D31E4B9B6803F1006303F520439A420D +:10043000EAD203F039FC03F04BFC002000F090FD01 +:100440000220FFF7C1FF164B9A6D00229A65996F43 +:100450009A67996FD96DDA65D96FDA67D96F196EB6 +:100460001A66D3F88010C3F88020D3F8803072B6B3 +:100470004FF0E0233021C3F8084DD4E9003281F376 +:1004800011889D4683F308881047BDE7801100203E +:100490008411002000A0000820A000080011002006 +:1004A00000100240094A136849F2690099B21B0C16 +:1004B00000FB01331360064B186844F2506182B2AE +:1004C000000C01FB0200186080B27047141100207C +:1004D0001011002010B500211022044600F090FDFC +:1004E000034B03CB206061601868A06010BD00BFA3 +:1004F0009075FF1FF0B5ADF54F7D6CAC4FF4C47235 +:1005000007460D462046002100F07AFD01F004FF69 +:10051000264B4FF47A72B0FBF2F0186093E80700B4 +:10052000022384E807000DF5E9702382FFF7D2FF6C +:100530004BF616431E49238407A803F045FE1723F4 +:1005400084F832310DF2E32207AB0DF12C0C1E467C +:1005500003CE664510605160334602F10802F6D1C1 +:1005600030681060B188B37993710DAE91802046E8 +:100570003146012200F090FD00230393AB7E0293ED +:1005800005F11903019380B20123CDE904600093C2 +:10059000E97E04A3D3E90023384602F0B5FA0DF54D +:1005A0004F7DF0BD9E6AC421818A46EE8C110020E9 +:1005B000D84100082DE9F0412C4C237ADAB080466E +:1005C0000D465BBB27A9284600F08CFE0746002895 +:1005D00042D19DF89D60C82E3ED801464FF4A662D8 +:1005E000204600F00DFD4FF48073C4F8F8314FF44D +:1005F0000073C4F80C334FF44073C4F8203432460F +:100600000DF19E0104F1090000F0E8FC26449DF87C +:100610009C30777223720BB9EB7E2372812200210A +:1006200006AC27A800F0ECFC0122214627A800F028 +:100630009FFE00230393AB7E029305F1190380B262 +:1006400001932823CDE904400093E97E05A3D3E973 +:100650000023404602F058FA5AB0BDE8F08100BFCE +:10066000AFF3008026417272DF25D7B7D421002076 +:10067000F0B5254E4FF48A7505FB0065F1B096F88C +:10068000D83085F8DC300024D822214685F8E840AF +:100690003AA800F0B5FC06F1090000F0A9FCD5F875 +:1006A000E4308DF8F000C2B206AF06F109010DF199 +:1006B000F100CDE93A3400F091FC394601223AA824 +:1006C00000F08CFE80B2CDE9047008230127CDE94B +:1006D000023706F1D803019330230093317A0B4897 +:1006E00007A3D3E9002302F00FFAA04206DD01F0D0 +:1006F00013FEC5F8E000384671B0F0BD2046FBE7B8 +:1007000078F6339F93CACD8DD4210020A4210020F8 +:100710002DE9F0411D4D1E4E1E4F86B0284602F0B9 +:100720001FFA034658B30024CDE90344ADF8144042 +:10073000027B8DF8142099684068029403AA03C2D2 +:100740001B68DFF8548043F00043029301F0E6FD9C +:10075000821941F10003009402A9384601F0AEF875 +:10076000A04205DD284602F0FFF988F80040D5E7F1 +:1007700098F80030072B05D8013388F8003006B010 +:10078000BDE8F081014802F0EFF9F8E7A42100206C +:1007900040420F00102700200927002070B50D46A9 +:1007A00014461E4602F00CF950B9022E10D1012C4D +:1007B0000ED112A3D3E90023C5E90023012007E0ED +:1007C000282C10D005D8012C09D0052C0FD00020E2 +:1007D00070BD302CFBD10BA3D3E90023ECE70BA3B6 +:1007E000D3E90023E8E70BA3D3E90023E4E70BA355 +:1007F000D3E90023E0E700BFAFF30080401DA12054 +:1008000026812A0B78F6339F93CACD8D9E6AC42128 +:10081000818A46EE26417272DF25D7B7F017304A3B +:1008200039059E5638B505460E4C0021013500F0BD +:10083000B9FBA4F82C55B4F82C0500F09BFB78B15B +:10084000B4F82C0500F0A6FB014648B9B4F82C0515 +:1008500000F0A8FBB4F82C350133A4F82C35EAE7F6 +:1008600038BD00BFD421002010B50A4B0A4A1A60D7 +:1008700003F5805393F870203AB9DC6E2CB1204612 +:1008800000F0BAFE204600F0F7FFBDE81040034834 +:1008900000F0B2BE102700202C420008683700206C +:1008A0002DE9F04F8FB000AF05460C4602F088F8F6 +:1008B00000284BD1237E022B1BD1E38A012B18D1B8 +:1008C00001F02AFD0646FFF7EDFD03464FF4C87020 +:1008D000DFF8D482B3FBF0F206F5167602FB103394 +:1008E00016FA83F3C8F80030E37E33B9A74B002231 +:1008F0001A703C37BD46BDE8F08F07F12401204651 +:1009000000F0A4FC0028F4D107F11400FFF7E2FD89 +:1009100097F8264007F11401224607F1270003F05B +:1009200021FC0028E2D10F2C08D8984B1C70D8F875 +:100930000030A3F51673C8F80030DAE797F82410F2 +:100940000029D6D0284602F033F8D2E7E38A282BD4 +:1009500033D010D8012B2BD0052BCAD1BFF34F8F2A +:100960008B498C4BCA6802F4E0621343CB60BFF33F +:100970004F8F00BFFDE7302BBBD1874EE17E327A2F +:100980009142B6D1607E3146002291F8DC5085421A +:1009900000F0AD800132042A01F58A71F5D1326888 +:1009A00040F6B831FA328A4294BF32603160A0E733 +:1009B00021462846FFF79EFD9BE721462846FFF784 +:1009C000F9FD96E7B2F8EC507B6005F103094FEAB8 +:1009D00099094FEA8902D11DC908A8EBC1039D46B8 +:1009E000EB460021584600F00BFB04F1EE012A46CD +:1009F0003144584600F0F2FA7B6813B9012000F048 +:100A0000AFFA96F8D20000F0B5FA044630B9307269 +:100A100000F0D0FA204600F0A3FAB0E0D6F8D420D7 +:100A20003AB996F8D200B6F82C25824201D8FFF7E1 +:100A3000F9FED6F8D4202A44944208D296F8D2007F +:100A4000B6F82C250130824201D8FFF7EBFE706822 +:100A50005FFA89F2594600F0DBFA08B9C54679E039 +:100A6000726896F8D2002A447260D6F8D42005EB5A +:100A70000209C6F8D49000F07DFA814509D396F8B2 +:100A8000D220D6F8D4000132001B86F8D220C6F856 +:100A9000D400FF2D0FD80024347200F08BFA2046CA +:100AA00000F05EFA00F02AFD3C4B188108B9FFF710 +:100AB000A1FCC5461DE7BB6896F8D9000AFB036296 +:100AC000FB68D2F8E41082F8E83001F58061C2F8E2 +:100AD000E030C2F8E410FFF7CBFDFFF719FE96F8FF +:100AE000D920013202F0030286F8D920B6E74FF48C +:100AF0008A7A0AFB02F505F1EA013144204600F04A +:100B0000A1FCF86000287FF4F4AE3544012285F89A +:100B1000E82001F001FCD5F8E020D6ED007ADFED09 +:100B2000206A801A192838BF192040F6B83290423E +:100B300028BF1046B8EE677A07EE900AF8EEE77A1B +:100B400067EEA67ADFED176AE7EE267AFCEEE77A23 +:100B5000C6ED007A96F8D9307168BB600AFB03F4E1 +:100B6000321992F8E8505DB1D2F8E4308B42E84691 +:100B70003FF428AF002182F8E810C2F8E010C54623 +:100B80007368064A9B0A01331381B2E69D21002057 +:100B900000ED00E00400FA05D42100208C110020B3 +:100BA000CDCCCC3D6666663FA0210020014B18707D +:100BB000704700BF9811002038B54FF00054144B17 +:100BC00022689A4221D1607DF0B1124B124918700F +:100BD0001248237D03724FF48073C0F8F8314FF44C +:100BE0000073C0F80C3300254FF44073C0F8203474 +:100BF000C0F8E450C922093000F0F0F9E02229469B +:100C0000204600F0FDF9012038BD0020FCE700BFC0 +:100C10009AAD44C59811002016000020D421002070 +:100C200037B500F06BFC184D18492881022318488D +:100C30006B7101F015FA00230193164B16490093CE +:100C40001648174B4FF4805201F08AFE154B197865 +:100C500011B1124801F0ACFE01F05EFB0446FFF753 +:100C600021FC4FF4C873B0FBF3F202FB130304F54D +:100C7000167010FA83F00C4B186003F04BF808B1B3 +:100C80000F232B8103B030BD8C11002040420F0098 +:100C9000102700209D0700089C110020A42100209F +:100CA000A108000898110020A02100202DE9F04F94 +:100CB0002DED028B8CA7D7E900670FF23429D9E913 +:100CC0000089844C95B00DAD9FED808BFFF720FD22 +:100CD000DFF824B200230C93ADF83C300D936B6029 +:100CE00000230DF125028DED008B4FF0010A09A9BB +:100CF00058468DF825308DF824A001F05FF99DF855 +:100D000024200023002A40F0AE80204601F058FE47 +:100D10000546002847D1DFF8E4B101F0FDFADBF821 +:100D2000003098423FD301F0F7FA0790FFF7BAFB83 +:100D3000079A4FF4C873B0FBF3F101FB130302F5FC +:100D4000167010FA83F0CBF80000DFF8B4B19BF80E +:100D500000100791002914BF2B46534610A88DF8A8 +:100D60003030FFF7B7FB0799C1F11002D2B2062A63 +:100D700010AB28BF062219440DF13100079200F094 +:100D80002DF9079A0CAB0393182302930132524BAF +:100D9000D2B2CDE900A304923B463246204601F090 +:100DA00055FE8BF8005001F0B7FA4C4A4C4D1368D1 +:100DB000C31AB3F57A7F32D3106001F0AFFA02465E +:100DC0000B46204601F0DAFE204601F0F9FD30B373 +:100DD0002B7ADFF830A1002B14BF032302238AF8FB +:100DE000053001F099FA0DF1400B4FF47A730122AE +:100DF000B0FBF3F05946CAF80000504600F0FEF987 +:100E000018230293374B019380B240F25513CDE97A +:100E100003B0009342464B46204601F017FE2B7A62 +:100E2000ABB101F079FA4FF0000A83464FF48A72B1 +:100E300095F8D900504400F0030002FB005393F8EA +:100E4000E81071B30AF1010ABAF1040FF0D1C82019 +:100E500002F018FC2B7A002B7FF435AF15B0BDECF7 +:100E6000028BBDE8F08F1946102210A800F0C8F8D8 +:100E70000DF126030AAA0CA9584600F025FE95E8B4 +:100E8000030011AB83E803009DF83C308DF84C3033 +:100E90000C9B109310A9DDE90A23204602F042F8CA +:100EA0001EE7D3F8E01049B12B68FA2B38BFFA23BC +:100EB000ABEB01010533B1EB430FC3D3FFF7D8FB15 +:100EC0004FF48A720028BDD1C1E700BF00000000C6 +:100ED00000000000A42100209C2100200427002005 +:100EE000D421002008270020401DA12026812A0BA4 +:100EF000F1C6A7C1D068080F10270020A02100204C +:100F00009D2100208C11002008B5054800F07AFED4 +:100F1000BDE80840034A0449002003F0FFB800BFC1 +:100F200010270020303800206908000870B502F052 +:100F300045FD094E094D30800024286833888342DE +:100F400008D902F037FD2B6804440133B4F5204F73 +:100F50002B60F2D370BD00BF24380020F83700208A +:100F600002F0DEBD00F10060920000F5204002F0CA +:100F700061BD0000054B1A68054B1B889B1A834214 +:100F800002D9104402F016BD00207047F837002047 +:100F900024380020024B1B68184402F011BD00BF2A +:100FA000F8370020024B1B68184402F01BBD00BF3D +:100FB000F8370020064991F8243033B10023086A3D +:100FC00081F824300822FFF7CDBF0120704700BF11 +:100FD000FC370020704700007047000010B5002368 +:100FE000934203D0CC5CC4540133F9E710BD000038 +:100FF00003460246D01A12F9011B0029FAD17047A4 +:1010000002440346934202D003F8011BFAE77047FB +:101010002DE9F8431F4D144695F8242007468846CD +:1010200052BBDFF870909CB395F824302BB9202286 +:10103000FF2148462F62FFF7E3FF95F82400C0F137 +:101040000802A24228BF2246D6B24146920005EBD2 +:101050008000FFF7C3FF95F82430A41B1E44F6B2AE +:10106000082E17449044E4B285F82460DBD1FFF7E2 +:10107000A1FF0028D7D108E02B6A03EB820383424B +:10108000CFD0FFF797FF0028CBD10020BDE8F88331 +:101090000120FBE7FC3700202DE9F3470D4604460D +:1010A00000219046284640F27912FFF7A9FF234617 +:1010B00020220021284601F0D7FE231D02222021F4 +:1010C000284601F0D1FE631D03222221284601F0AB +:1010D000CBFEA31D03222521284601F0C5FE04F105 +:1010E000080310222821284601F0BEFE04F1100357 +:1010F00008223821284601F0B7FE04F11103082226 +:101100004021284601F0B0FE04F1120308224821D4 +:10111000284601F0A9FE04F114032022502128469C +:1011200001F0A2FE04F1180340227021284601F0CC +:101130009BFE04F120030822B021284601F094FE12 +:1011400004F121030822B821284601F08DFE04F1A4 +:101150002207C0263B46314608222846083601F0C1 +:1011600083FEB6F5A07F07F10107F3D194F8323082 +:101170008DF8073031460DF107030822284601F0AB +:1011800073FE002604F1330A9DF807304FEAC609C2 +:101190009E4209F5A47720D394F83231502B28BF12 +:1011A00050238DF80730B8F1000F08D139460DF102 +:1011B00007030722284601F057FE09F24F170026C1 +:1011C00004F233149DF807309E4207EBC6010DD39D +:1011D0000731C80802B0BDE8F0870AEB0603082211 +:1011E0003946284601F040FE0136CDE7A319082212 +:1011F000284601F039FE0136E4E7000013B5044645 +:101200000846002101602346C0F803102022019007 +:1012100001F02AFE0198231D0222202101F024FE64 +:101220000198631D0322222101F01EFE0198A31DD7 +:101230000322252101F018FE019804F10803102271 +:10124000282101F011FE072002B010BDF7B5047F80 +:1012500005460E462CB1838A122B02D9012003B019 +:10126000F0BD0023194607220096284601F0C0FC75 +:10127000731C0093012200230721284601F0B8FCCB +:10128000D4B9B31C0093052223460821284601F057 +:10129000AFFC0D24B378102BE0D83746B278BB1BD7 +:1012A000934210D32B7FA88A0734E408B3B9844251 +:1012B00094BF00200120D2E7AB8ADB00083BDB08AB +:1012C000B3700824E6E7FB1C0093214600230822A4 +:1012D000284601F08DFC08340137DFE7201A18BFDB +:1012E0000120BCE7F7B5047F05460E462CB1838A82 +:1012F000CA2B02D9012003B0F0BD00231946009685 +:101300000822284601F074FC731CD4B9082200930B +:1013100011462346284601F06BFC10247378C82B35 +:10132000E8D8012372785F1C013B934210D32B7FD6 +:10133000A88A0734E408B3B9844294BF002001208E +:10134000D9E7AB8ADB00083BDB0873700824E5E7CC +:10135000F3190093214600230822284601F048FC97 +:1013600008343B46DEE7201A18BF0120C3E700001F +:10137000F7B50D4604460021164628468122FFF7A0 +:101380003FFE234608220021284601F06DFD94F916 +:1013900001206378002AB8BF7F238DF807309EB9FB +:1013A0000DF1070307220821284601F05DFD0F27F4 +:1013B000002602349DF807309E4207EBC60105D394 +:1013C0000731C80803B0F0BD0827F1E7A3190822C8 +:1013D000284601F049FD0136ECE70000F7B50E465E +:1013E0000546002114463046CE22FFF709FE2B4663 +:1013F00028220021304601F037FD2B7AC82B28BF68 +:10140000C8238DF807308CB90DF107030822282175 +:10141000304601F029FD30242F469DF807207B1B24 +:10142000934205D3E01DC00803B0F0BD2824F3E7C4 +:1014300007F1090321460822304601F015FD083462 +:101440000137EAE7F7B5047F05460E4634B1838AD3 +:10145000B3F5827F02D9012003B0F0BD00960123CD +:1014600010220021284601F0C3FBDCB9B31C009315 +:10147000092223461021284601F0BAFB192473885B +:10148000B3F5807FE7D837467288BB1B934210D3F1 +:101490002B7FA88A0734E408B3B9844294BF0020A4 +:1014A0000120D9E7AB8ADB00103BDB0873801024F6 +:1014B000E5E73B1D0093214600230822284601F062 +:1014C00097FB08340137DFE7201A18BF0120C3E774 +:1014D00030B5094D0A4491420DD011F8013B5840F6 +:1014E000082340F30004013B2C4013F0FF0384EA7F +:1014F0005000F6D1EFE730BD2083B8EDF7B5384A9C +:10150000106851686B4603C36A463649364808235B +:1015100002F038FE0446002833D10A25334A106809 +:1015200051686B4603C36A4631492F48082302F0CD +:1015300029FE0446002849D00369B3F5583F45D831 +:10154000B0F8661041F2BC6291423FD1294A024490 +:1015500002F15C018B4239D35C3B234900209E1A87 +:10156000FFF7B6FF3246074604F164010020FFF79B +:10157000AFFFA3689F4229D1E368984208BF0025C6 +:1015800024E00369B3F5583F27D8418B41F2BC6290 +:10159000914220D1174A024402F110018B4218D324 +:1015A000103B114900209D1AFFF792FF2A4606467C +:1015B00004F118010020FFF78BFFA3689E4202D1BF +:1015C000E368984201D00D25A8E70025284603B01E +:1015D000F0BD1025A2E70C25A0E70B259EE700BF74 +:1015E000F0410008DC5F030000A00008F94100089A +:1015F000905F03000860FFF710B5037C044613B941 +:10160000006802F0A7FD204610BD00000023BFF3D4 +:101610005B8FC360BFF35B8FBFF35B8F8360BFF3F0 +:101620005B8F7047BFF35B8F0068BFF35B8F7047C2 +:1016300070B505460C30FFF7F5FF05F108060446C6 +:101640003046FFF7EFFFA04206D930466D68FFF73E +:10165000E9FF2544281A70BD3046FFF7E3FF201A42 +:10166000F9E7000070B50546406898B105F108003B +:10167000FFF7D8FF05F10C0604463046FFF7D2FF0E +:101680008442304694BF6D680025FFF7CBFF013CD4 +:101690002C44201A70BD000038B50C460546FFF7F3 +:1016A000C7FFA04210D305F10800FFF7BBFF0444B9 +:1016B0006868B4FBF0F100FB1144BFF35B8F0120BD +:1016C000AC60BFF35B8F38BD0020FCE72DE9F04133 +:1016D000144607460D46FFF7C5FF844228BF04465F +:1016E000D4B1B84658F80C6B4046FFF79BFF304426 +:1016F000286040467E68FFF795FF331A9C4203D866 +:101700006C600120BDE8F0816B60A41B3B68AB609E +:101710002044E8600220F5E72046F3E738B50C46A0 +:101720000546FFF79FFFA04210D305F10C00FFF71D +:1017300079FF04446868B4FBF0F100FB1144BFF387 +:101740005B8F0120EC60BFF35B8F38BD0020FCE7AE +:101750002DE9FF41884669460746FFF7B7FF6C460B +:1017600006B204EBC6060025B44209D062682068C0 +:1017700008EB0501FFF732FC636808341D44F3E70A +:1017800029463846FFF7CAFF284604B0BDE8F08175 +:10179000F8B505460C300F46FFF744FF05F1080683 +:1017A00004463046FFF73EFFA042304688BF6C68D3 +:1017B000FFF738FF201A386020B130462C68FFF759 +:1017C00031FF2044F8BD000073B5144606460D46AF +:1017D000FFF72EFF844228BF04460190DCB101A927 +:1017E0003046FFF7D5FF019B33B93268C5E90233B4 +:1017F000C5E9002401200CE09C4238BF0194286018 +:10180000019868608442F5D93368AB60241AEC60B3 +:10181000022002B070BD2046FBE700002DE9FF4129 +:101820000F466946FFF7D0FF6C4600B204EBC005D7 +:101830000026AC4209D0D4F8048054F8081BB8192B +:101840004246FFF7CBFB4644F3E7304604B0BDE821 +:10185000F081000038B50546FFF7E0FF0446014679 +:101860002846FFF719FF204638BD000001218842B5 +:1018700038BF084602F058BC08B102F06BBC704794 +:1018800003685B681847000000207047704700003D +:101890000020704700F5805090F8D800C0F3400059 +:1018A0007047000000F5805090F9E000704700009C +:1018B00000F58050C0F8DC1001207047F7B50C68C7 +:1018C000BDF8207014F0005473D10D7B082D70D832 +:1018D000302585F31188056AAE6876010CD4AC68B2 +:1018E000240108D4AC6814F0805460D184F31188CA +:1018F000204603B0F0BD01240E6804F1180C002E40 +:10190000B8BFF6004FEA0C1CB4BF46F004067605DB +:1019100045F80C600E680FFA84FC16F0804F18BF73 +:1019200005EB0C1E05EB0C1C1EBFDEF8806146F0BB +:101930000206CEF880610E7BCCF8846105EB0415BD +:101940008E68C5F88C614E68C5F88861DCF88051F6 +:1019500045F00105CCF8805141F2780500EB441CBC +:10196000660105EB4414AC440444CCE900230D4665 +:10197000083401F10C0C55F804EB44F804EB654510 +:10198000F9D12D882580841904F5805407F00305CA +:1019900094F8946026F00B06354384F894500024A4 +:1019A00084F31188009700F007FD0120A1E70224CD +:1019B000A2E74FF0FF309CE713B500F5805401918A +:1019C000E06EFFF74FFE1F280AD90199E06E202232 +:1019D000FFF7BEFEA0F120035842584102B010BDEF +:1019E0000020FBE708B5302383F3118800F5805011 +:1019F000C06EFFF70BFE002383F3118808BD0000C3 +:101A000000220260828142608260704710B500222D +:101A10000023C0E900230023044603810C30FFF7B4 +:101A2000EFFF204610BD0000F0B5054600F58050E0 +:101A30000C4690F8D83013F0040FC3F3800108BFB0 +:101A4000114661F3820380F8D83005EB441303F5A7 +:101A50008453103389B01B79D8074FEA44162FD529 +:101A600082B319072ED46846FFF7D0FF05EB441365 +:101A700003F5845303AA03F108071868596814464C +:101A800003C40833BB422246F7D1186820609B8804 +:101A9000A380DDE90E23CDE900230123ADF8083052 +:101AA0002B6869469B6B28469847354405F5845555 +:101AB00010352B791A075CBF43F008032B7101E046 +:101AC000002AF2D109B0F0BD2DE9F0479A4688B05E +:101AD000074688469146302383F3118807F58054E2 +:101AE0006846FFF793FFE06EFFF7A2FD1F282AD993 +:101AF000E06E20226946FFF7ADFE202823D103AD1A +:101B0000444605AB2E4603CE9E42206061603546BA +:101B100004F10804F6D130682060B388A380DDE9C1 +:101B20000023C9E90023BDF80830AAF800300023DB +:101B300083F3118853464A464146384608B0BDE80B +:101B4000F04700F0F7BB002080F3118808B0BDE833 +:101B5000F08700002DE9F84F182204460F460430A4 +:101B60000021FFF74DFA2046234B40F8243BFFF7B6 +:101B700047FF04F13800FFF749FF04F1580804F566 +:101B800082554646183530462036FFF73FFFAE42B5 +:101B9000F9D104F580554FF480534FF00009C5E9A1 +:101BA0001739C5F858800123EE6604F5875804F507 +:101BB0008456C5F8649085F8683085F8703018361A +:101BC00008F118084FF0000A4FF0000B46E908AB87 +:101BD000A6F11800FFF714FF203646F8289C46456A +:101BE000F4D185F8E07017B1044800F08FFB044B86 +:101BF00023622046BDE8F88F2C420008044200080A +:101C00000064004010B5044B197804464A1C1A7051 +:101C1000FFF7A0FF204610BD2C3800202DE9F0472B +:101C2000002950D0294B2A4FB7FBF1F599428CBFC0 +:101C30000A231123581EB5FBF3FC03FB1C53C4B24B +:101C40002BB102280346F5D80020BDE8F0870CF13F +:101C5000FF36B6F5806FF7D2C4EBC40E0EF1030366 +:101C60004FEAE309C3F3C703A4EB030809F1010A30 +:101C70004FF47A755FFA88F009FB05555AFA88F82F +:101C8000B5FBF8F5B5F5617FC1BF0EF1FF33C3F3C6 +:101C9000C703E01AC0B25C1C50FA84F40CFB04F4D5 +:101CA000B7FBF4F4A142CFD1013BDBB20F2BCBD871 +:101CB0000138C0B20728C7D8002110711680917072 +:101CC000D3700120C1E70846BFE700BF3F420F00C5 +:101CD00000B4C40470B505460E464FF47A742B6AFE +:101CE0005B6803F00103B34207D04FF47A7001F050 +:101CF000C9FC013CF3D1204670BD0120FCE7000087 +:101D000070B5026A936913F0700F17D000230C4D61 +:101D1000936141F2900400EB4312224411794E0783 +:101D200009D5890707D5016A55F823608E601179B6 +:101D300041F0040111710133032BECD170BD00BFE0 +:101D40001842000873B51D46036A16469A68D20702 +:101D5000044609D59A6801219960C2F34002CDE991 +:101D600000650021FFF760FE236A9A68D1050BD554 +:101D70009A684FF480719960C2F34022CDE9006502 +:101D800001212046FFF750FE236A9A68D2030BD543 +:101D90009A684FF480319960C2F34042CDE9006502 +:101DA00002212046FFF740FE04F58053D3F8DC0003 +:101DB00010B103681B699847204602B0BDE8704027 +:101DC000FFF79EBFF8B50446066A002977D106F1F1 +:101DD0000C073868800775D006EB01153868D5F810 +:101DE000B00110F0040FD5F8B0011ABFC00840F0E0 +:101DF0000040400D6062D5F8B0C11CF0020F1CBF5E +:101E000040F080406062D5F8B40106EB011100F0AB +:101E10000F0084F83000D1F8B80184F82800D1F818 +:101E2000B801000A84F82900D1F8B801000C84F840 +:101E30002A00D1F8B801000E84F82B00D1F8BC01BB +:101E400084F82C00D1F8BC01000A84F82D00D1F8E8 +:101E5000BC01000C84F82E00D1F8BC11090E84F8E6 +:101E60002F103821396004F1440004F1240104F1F9 +:101E7000300551F8046B40F8046BA942F9D1098888 +:101E80000180C4E90E2321460023A4F8403051F814 +:101E9000383B20469B6B984704F5805393F8D82035 +:101EA000D3F8DC0042F0040283F8D82010B10368B4 +:101EB0001B6998472046BDE8F840FFF721BF06F1AF +:101EC000100786E7F8BD000010B5044600F026FABA +:101ED00002460B4652EA030102D0013A63F10003C5 +:101EE0000449086820B12146BDE81040FFF76ABFE9 +:101EF00010BD00BF28380020F0B5302181F31188D3 +:101F0000DFF848E000F583511831002441F2900CCD +:101F100000EB441565442E7977070ED4F6060CD5F0 +:101F2000D1E9007697429E4107D2066A5EF8247096 +:101F3000B7602E7946F004062E710134032C01F1AE +:101F40002001E5D1002383F31188F0BD1842000879 +:101F500008B5302383F31188FFF7D2FE002383F303 +:101F6000118808BD036A9B6813F0E05F14BF01206D +:101F70000020704708B5302383F3118800F58050A6 +:101F8000C06EFFF755FB002383F3118843090CBF94 +:101F90000120002008BD0000F8B51E460023137084 +:101FA0000F4605461446FFF7E5FF80F00100387044 +:101FB0001EB12846FFF7D6FF2070F8BD2DE9B841C5 +:101FC0000C4615461F46804600F0A8F90B462178BE +:101FD000024609B9287850B14046FFF78DFFFFF758 +:101FE000B7FF3B462A462146FFF7D6FF0120BDE852 +:101FF000B881000070B5302686F31188174B9A6DB2 +:1020000042F000729A659A6B42F000729A639A6B82 +:1020100022F000729A63002383F3118800F5805444 +:1020200094F8D83013F0010516D1A9B186F31188C0 +:102030000321132001F012FA0321142001F00EFAFB +:102040000321152001F00AFA94F8D83043F0010377 +:1020500084F8D83085F3118870BD00BF00100240AD +:102060002DE9F04300F5805589B095F8E030012B5B +:1020700004460F467ED8DFF8F88158F823200AB9C5 +:1020800048F82300D8F80090E761B9F1000F64D058 +:1020900095F8E030012B6FD001212046FFF7AAFF11 +:1020A000302383F31188226A136823F0020313603C +:1020B000226A136843F001031360236A00265E61FD +:1020C00086F3118801212046FFF704FE002800F066 +:1020D0009080E86EFFF79AFA04F58358B14608F14C +:1020E0001808202200216846FEF78AFF02A8FFF7A1 +:1020F00087FCCDF818906A4608EB06030DF1180E20 +:102100009446BCE80300F44518605960624603F148 +:102110000803F5D1DCF80000186020369CF8042094 +:102120001A71602EDDD195F8D8306FF38203002646 +:1021300085F8D8306A4639462046ADF80060ADF8DB +:1021400002608DF80460FFF769FD236A50BB4FF40D +:1021500000421A6009B0BDE8F08341F2E800FFF7E1 +:1021600085FB064610B14946FFF7F4FCC8F800604D +:10217000002E8DD10020EDE7D8F8000003689B6A9F +:1021800098470146002887D1D8F80000FFF732FFB2 +:10219000D8F80000036839465B68984700287FF448 +:1021A0007BAFE7E765221A609DF802309DF80310C7 +:1021B000226A1B06090401F4702103F040730B43EB +:1021C000BDF80010C1F309010B439DF80410090587 +:1021D00001F4E0010B43D361236A13225A61226A9E +:1021E000136823F00103136031462046FFF772FDA8 +:1021F00008B9236AABE795F8E02092BB216AD1F8D1 +:10220000003243F00103C1F80032216AD1F80032F4 +:1022100023F47C5323F00E03C1F80032216AD1F875 +:10222000003243F46063C1F80032236AC3F8142219 +:10223000236AC3F80422236A41F6FF71C3F80C1223 +:10224000236AC3F84022236AC3F84422236A012286 +:10225000C3F81C22226AD2F8003223F00103C2F82C +:10226000003295F8D83043F0020385F8D83071E792 +:102270002838002008B500F051F850EA0103024662 +:1022800002D0421E61F10001044B186810B10B46E8 +:10229000FFF758FDBDE8084001F0A2B8283800203B +:1022A00008B50020FFF710FEBDE8084001F098B81F +:1022B00008B50120FFF708FEBDE8084001F090B81E +:1022C00000B59BB0EFF3098168226846FEF786FEF1 +:1022D000EFF30583014B9B6BFEE700BF00ED00E0D1 +:1022E00008B5FFF7EDFF000000B59BB0EFF30981E3 +:1022F00068226846FEF772FEEFF30583014B5B6BC5 +:10230000FEE700BF00ED00E0FEE700000FB408B5F7 +:10231000029801F067F9FEE701F042BC01F024BC2D +:102320000139C9B202299EBF00EBC1000023C0E9F8 +:10233000013370472DE9F8431E461B885B070546AD +:10234000884618D4044600F118094C4513D0636838 +:102350006BB12B6828465B6B98473388A768C1B27E +:1023600043F0040360684246B8470834EDE7A368C9 +:10237000002BEED1F9E70120BDE8F88373B56C4678 +:1023800084E80600014600224E68D5B26EB98E6818 +:102390005EB900EBC200021D94E80300013582E83B +:1023A00003001D70012002B070BD0132032A01F14B +:1023B0000801E9D10020F6E72DE9F04F2DED028B61 +:1023C00089B09FED258BBDF8508004468B460546AD +:1023D00000F11809002708F0040AA94536D06B68F7 +:1023E0000BB9AB6863B1BAF1000F0BD1236820467B +:1023F0005B6B98474346C1B25A46D5E90106B047E0 +:102400000835EAE7002FFBD18DED008BADF80870A1 +:102410005B4603AA0BF1080C18685968174603C7F6 +:10242000083363453A46F7D1186838609B88BB800B +:10243000FFF774FF0423ADF808302368CDE90001ED +:102440009B6B6946204698470127D9E7012009B0D0 +:10245000BDEC028BBDE8F08F000000000000000022 +:102460002DE9F041456A15B94162BDE8F0814B683C +:1024700023F06047C3F38A464FEAD37EC3F38078E4 +:1024800016EA230638BF3E46AC462B465A68BEEBDA +:10249000D27F22F060440AD0002A18DAA40CB44299 +:1024A00017D19D420FD10D60DEE71346EEE7A7423C +:1024B00007D102F08044C2F3807242450BD054B180 +:1024C000EFE708D2EDE7CCF800100B60CDE7B4429F +:1024D00001D0B442E5D81A689C46002AE5D11960BB +:1024E000C3E700002DE9F047089D01F007044FEA1B +:1024F000D508224405F0070500EBD1004FF47F49D1 +:10250000944201D1BDE8F08704F0070705F0070AFF +:1025100057453E4638BF5646C6F10806111B8E4247 +:1025200028BF0E46E10808EBD50E415C13F80EC03B +:10253000B94029FA06F721FA0AF1FFB28CEA010143 +:1025400047FA0AF739408CEA010C03F80EC034440C +:102550003544D5E780EA0120082341F2210201B287 +:102560004000002980B203F1FF33B8BF504013F0A0 +:10257000FF03F4D17047000038B50C468D18A54212 +:1025800000D138BD14F8011BFFF7E4FFF7E70000A6 +:1025900042684AB1136843604389818901339BB221 +:1025A0009942438138BF83811046704770B588B027 +:1025B000202204460D4668460021FEF721FD2046F4 +:1025C0000495FFF7E5FF024658B16B46054608AE95 +:1025D0001C4603CCB44228606960234605F1080517 +:1025E000F6D1104608B070BD082817D909280CD0BC +:1025F0000A280CD00B280CD00C280CD00D280CD09D +:102600000E2814BF4020302070470C207047102047 +:102610007047142070471820704720207047000032 +:10262000082817D90C280CD910280CD914280CD933 +:1026300018280CD920280CD930288CBF0F200E2048 +:102640007047092070470A2070470B2070470C2004 +:1026500070470D20704700002DE9F843078C072FC5 +:1026600004461ED9D0E9029800254FF6FF73C5F144 +:102670002006A5F1200029FA05F108FA06F628FA45 +:1026800000F031430143C9B21846FFF763FF083534 +:10269000402D0346EBD1E1693A46BDE8F843FFF728 +:1026A0006BBF4FF6FF70BDE8F883000010B54B68B4 +:1026B00023B9CA8A63F30902CA8210BD04691A6881 +:1026C0001C600361C38A013BC3824A60EFE70000DC +:1026D0002DE9F84F1D46CB8A0F46C3F309010529A2 +:1026E000814692460B4630D00020AAB207F11A0468 +:1026F0009EB2042E1FFA80F80FD8904503F1010313 +:1027000006D3FB8A0A4462F30903FB8201201AE024 +:102710001AF80060E6540130EAE79045F1D2A1F1E1 +:10272000050B1C237C68BBFBF3F203FB12BB1FFAF7 +:102730008BF6002C45D14846FFF72AFF044638B9EE +:1027400078606FF00200BDE8F88F4FF00008E6E710 +:10275000002606607860ADB24FF0000B454510D9F9 +:102760000AEB0803221D13F8011B9155B1B208F1C1 +:1027700001081B291FFA88F82BD0454506F10106F0 +:10278000F1D8FB8AC3F30902154465F30903BCE7DA +:10279000013292B21C462368002BF9D16B1F0B4407 +:1027A0001C21B3FBF1F301339BB29A42D3D2BBF1AC +:1027B000000FD0D14846FFF7EBFE20B9C4F800B0B7 +:1027C000BFE70122E7E7C0F800B05E46206004469C +:1027D000C1E74545D5D94846FFF7DAFE08B920607C +:1027E000AFE7C0F800B0002620600446B6E700005E +:1027F0002DE9F04F2DED028B1C4683B05B690192F1 +:1028000007468846002B00F0A780238C2BB1E26995 +:10281000002A00F0A180072B35D807F10C00FFF744 +:10282000B7FE054638B96FF00205284603B0BDEC87 +:10283000028BBDE8F08F14220021FEF7E1FB228C11 +:10284000E16905F10800FEF7C9FB208C013080B278 +:10285000FFF7E6FEFFF7C8FE013880B220840130A2 +:1028600028746369228C1B782A4403F01F0363F0E9 +:102870003F0348F000411372384669602946FFF76C +:10288000EFFD0125D1E702339BB20722C18A06334F +:10289000B3FBF2F3828A521A9BB292B29342C2D82D +:1028A00000F10C034FF0000908EE103A4FF0800AD7 +:1028B0004E464D4618EE100AFFF76AFE8346002882 +:1028C000B1D014220021FEF79BFB002E3AD1019BD0 +:1028D000ABF8083002220BF1080E1FFA82FC0CF153 +:1028E0000100BCF1060F218C80B201D88E422BD39F +:1028F000FFF796FEFFF778FE62691278013802F062 +:102900001F028E4208BF4FF0400A42EA49121BFAEA +:1029100080F14AEA020A013048F0004281F808A03A +:102920008BF81000CBF8042059463846FFF798FD85 +:10293000238C0135B3422DB289F001094FF0000A12 +:10294000B8D172E70022C6E7E169895D0EF802108E +:102950000136B6B20132C0E76FF0010565E700004D +:10296000F8B515460E463022002104461F46FEF7F4 +:1029700047FB069B6360B5F5001F079BA76034BF4C +:102980006A094FF6FF72A36297B2E66104F1100084 +:1029900000239A4206D800230360A782E3822383A0 +:1029A000E360F8BD0660013330462036F1E70000F1 +:1029B00003781BB94BB2002BC8BF017070470000F1 +:1029C00000787047F8B50C46C969074611B9238CE1 +:1029D000002B37D1257E1F2D34D8387828BB228C88 +:1029E000072A2CD8268A36F003032BD14FF6FF7026 +:1029F000FFF7C2FD20F001003102400441EA056109 +:102A0000400C41EA40254FF6FF72234629463846DE +:102A1000FFF7EEFE002807DD626913780133DBB2B1 +:102A20001F2B88BF00231370F8BD218A2D0645EAAD +:102A3000012505432046FFF70FFE0246E5E76FF04C +:102A40000300F1E76FF00100EEE7000070B58AB017 +:102A5000044616460021282268461D46FEF7D0FA95 +:102A6000BDF83830ADF810300F9B05939DF840301D +:102A70008DF81830119B07936946BDF84830ADF8C2 +:102A800020302046CDE90265FFF79CFF0AB070BDFB +:102A90002DE9F041D36905460C4616460BB9138C57 +:102AA0005BBB377E1F2F28D895F80080B8F1000F48 +:102AB00026D03046FFF7D0FD3378210241EAC331FA +:102AC00041EA0801338A41EA076141EA03410246CB +:102AD000334641F080012846FFF78AFE00280ADDD0 +:102AE0003378012B07D1726913780133DBB21F2BC6 +:102AF00088BF00231370BDE8F0816FF00100FAE792 +:102B00006FF00300F7E70000F0B58BB004460D4608 +:102B100017460021282268461E46FEF771FA9DF8E6 +:102B20004C305A1E534253418DF800309DF84030CE +:102B3000ADF81030119B05939DF848308DF8183092 +:102B4000149B07936A46BDF85430ADF820302946EF +:102B50002046CDE90276FFF79BFF0BB0F0BD0000E9 +:102B6000406A00B104307047436A1A6842620269E1 +:102B70001A600361C38A013BC38270472DE9F041AB +:102B8000D0F82080194E14461D464146002709B949 +:102B9000BDE8F081D1E90223A21A65EB0303964256 +:102BA00077EB03031ED2036A8B420DD1FFF77EFD44 +:102BB000036A1B68036203690B60C38A0161016ACF +:102BC000013BC3828846E2E7FFF770FD0B68C8F857 +:102BD000003003690B60C38A0161013BC382D8F8EE +:102BE0000010D4E788460968D1E700BF80841E0042 +:102BF0002DE9F04F8BB00D46DDF8509014469B4602 +:102C00008046002800F01981B9F1000F00F015810D +:102C1000531E3F2B00F21181012A03D1BBF1000F9B +:102C200040F00B810023CDE90833B8F81430B5EB40 +:102C3000C30F4FEAC30703D300200BB0BDE8F08FEA +:102C40002B199F42D8F80C303ABF7F1BFFB22746A2 +:102C50001BB9D8F81030002B7AD0272D4ED8C5F1EB +:102C60002806B7424FF000032CBFF6B23E46009351 +:102C70002946D8F8080008AB3246FFF733FCA7EB2B +:102C8000060A35445FFA8AFAB8F8143003F10053A3 +:102C9000053BDB000493D8F80C3003932821039BF9 +:102CA00013B1BAF1000F2CD1D8F8100040B1BAF12D +:102CB000000F05D0009608AB5246691AFFF712FCC8 +:102CC00038B2002FB8D066070AD00AAB03EBD401A4 +:102CD000624211F8083C02F00702134101F8083C77 +:102CE000082C3CD9102C40F2B580202C40F2B78043 +:102CF000BBF1000F00F09C80082334E0BA460026A8 +:102D0000C2E7049BE02B28BFE02306930B44AB42B1 +:102D1000059314D95A1B03980096924534BF524626 +:102D2000D2B2691A08AB04300792FFF7DBFB079AAF +:102D30001644AAEB020A1544F6B25FFA8AFA049B1B +:102D4000069A05999B1A0493039B1B680393A6E7B5 +:102D50000093D8F8080008AB3A462946AEE7BBF125 +:102D6000000F13D00123B4EBC30F6CD0082C12D882 +:102D70009DF82030621E23FA02F2D50706D54FF0E7 +:102D8000FF3202FA04F423438DF820309DF82030FE +:102D900089F8003051E7102C12D8BDF82030621E9F +:102DA00023FA02F2D10706D54FF0FF3202FA04F4FB +:102DB0002343ADF82030BDF82030A9F800303CE7BF +:102DC000202C0FD80899631E21FA03F3DA0705D5E2 +:102DD0004FF0FF3202FA04F40C430894089BC9F840 +:102DE00000302AE7402C2BD0DDE90865611EC4F1D4 +:102DF0002102A4F1210326FA01F105FA02F225FAD3 +:102E000003F311431943CB0712D50122A4F1200388 +:102E1000C4F1200102FA03F322FA01F1A240524266 +:102E200043EA010363EB430332432B43CDE9082319 +:102E3000DDE90823C9E90023FFE66FF00100FCE6A5 +:102E40006FF00800F9E6082CA0D9102CB3D9202C7B +:102E5000EED8C3E7BBF1000FADD0022383E7BBF18F +:102E6000000FBBD004237EE730B5012A144638BFDB +:102E70000124402C85B028BF40240025012ACDE93B +:102E8000025518D81B788DF8083063070AD004ABB8 +:102E900003EBD405624215F8083C02F007029340A8 +:102EA00005F8083C009103462246002102A8FFF7DE +:102EB00019FB05B030BD082AE4D9102A03D81B88B5 +:102EC000ADF80830E1E7202A8DBFD3E900231B6865 +:102ED0000293CDE90223D8E710B5CB681BB98B600C +:102EE0000B618B8210BD04691A681C600361C38A80 +:102EF000013BC382CA60F0E703064CBFC0F3C03099 +:102F00000220704708B50246FFF7F6FF022806D1F7 +:102F10005306C2F30F2001D100F0030008BDC2F335 +:102F20000740FBE72DE9F04F93B0CDE904230A6891 +:102F300004461046FFF7E0FF022814BFC2F306263E +:102F40000026002A0D46824680F2028212F0C04915 +:102F500040F0FE81097B002900F0FA81022803D0AD +:102F60002378B34240F0F781C2F30463069310461E +:102F700002F07F030393FFF7C5FF039B29444FEA49 +:102F8000834848EA0A4848EA4668CE780023002287 +:102F9000CDE90823F309834648EA0008029367D085 +:102FA000039B009302466768534608A92046B8472A +:102FB000002868D0276A4FB9414604F10C00FFF79A +:102FC000F5FA074690B96FF002006AE03B69984550 +:102FD0000DD03F68002FF9D1414604F10C00FFF7F6 +:102FE000E5FA07460028EED0236A3B60276297F88F +:102FF00017C006F01F08CCF3840CACEB08001FFAD6 +:1030000080FE0028B8BF0EF12000A8EB0C031FFAC9 +:1030100083FED7E90221B8BF00B2002B0793BEBFE1 +:103020000EF120031BB2079352EA010342D0049B26 +:10303000DFF834E39A1A059B4FF0000C63EB0101B3 +:1030400096457CEB010335D36B7B97F81AE073450B +:1030500021D1029B002B00F0848001282ADC7868B3 +:1030600040BBDFF800C3944570EB010343D21EE080 +:10307000276A5FB9039B0093656853465A4608A9BF +:103080002046A84768BB6FF00B000AE03B699845F3 +:10309000ADD03F68EDE7B34890427CEB010303D32A +:1030A000002013B0BDE8F08F029B002BF8D0079BE7 +:1030B0000F2B19DCFA7DB30002F0030203F07C034E +:1030C0001343FB7539462046FFF7F0FA6B7BBB765E +:1030D000029B4BB9FB7DC3F38402013262F386038A +:1030E000FB756FF00C00DCE76A7BBB7E9A42D7D1A0 +:1030F000029B002B35D0B309022B32D0049BBB605E +:10310000059BFB60142200210DA8FDF779FF049BAD +:103110000A93059B0B932B1D0C932B7BADF83EB0B4 +:10312000013BDBB2ADF83C30069B8DF84230039B8F +:103130008DF8433094F82C308DF840A083F00103D3 +:103140008DF844308DF84180A3680AA9204698473D +:10315000FB7DC3F38403013303F01F039B02FB8257 +:103160009EE7FB7DC6F34012B2EBD31F40F0F68022 +:10317000C3F38403434540F0F980029A2B7BB609E0 +:10318000002A52D016F0010661D1032B40F2F180E3 +:10319000049BBB60059BFB60FB8A66F30903FB8213 +:1031A0002B7BAE1D033BDBB23246394604F10C00EB +:1031B000FFF78EFA00280CDA39462046FFF776FA38 +:1031C000FB7DC3F38403013303F01F039B02FB82E7 +:1031D000F9E6DDE90884AB883B834FF6FF73C9F15C +:1031E0002000A9F1200228FA09F104FA00F00143B5 +:1031F00024FA02F211431846C9B2FFF7ABF909F1FC +:103200000809B9F1400F0346E9D1B8822A7B033A95 +:10321000D2B23146FFF7B0F9FB7DB882DA43C2F390 +:10322000C01262F3C713FB753AE786B92E1D013B46 +:10323000DBB23246394604F10C00FFF749FA0028A8 +:10324000BADB2A7BB88A013AD2B23146E2E7F98A80 +:10325000C1F30901013B0429DAB25BD8281D002320 +:1032600007F11B069A4208D910F801CB06F801C0F5 +:10327000013101330529DBB2F4D104990A91059992 +:103280000B91934207F11B010C9138BF043379680D +:103290000D9134BF55FA83F300230E93FB8AADF8EA +:1032A0003EB0C3F309031A44069B8DF84230039BDA +:1032B0008DF8433094F82C30ADF83C2083F00103B6 +:1032C0008DF8443000238DF840A08DF841807B605C +:1032D0002A7BB88A013A291DFFF74EF93B8BB88249 +:1032E000834203D1A3680AA92046984720460AA929 +:1032F000FFF7F2FDFB7DBA8AC3F38403013303F0C9 +:103300001F039B02FB823B8B9A420CBF00206FF095 +:103310001000C6E67B68002BAFD0052001E01C3012 +:1033200033461E68002EFAD1091A081D2E1D1844B6 +:1033300001EB090CBCF11B0F5FFA89F39DD89A428F +:103340009BD916F8013B00F8013B09F10109EFE7B1 +:103350006FF00900A5E66FF00A00A2E66FF00D001D +:103360009FE600BF40420F0080841E006FF00E00F9 +:1033700097E66FF00F0094E6EFF3098305494A6B77 +:1033800022F001024A63683383F30988002383F340 +:103390001188704700EF00E0302080F3118862B69A +:1033A0000C4B0D4AD96821F4E0610904090C0A4369 +:1033B000DA60D3F8FC20094942F08072C3F8FC209F +:1033C0000A6842F001020A602022DA7783F82200BC +:1033D000704700BF00ED00E00003FA05001000E0B8 +:1033E00010B5302383F311880E4B5B6813F4006330 +:1033F00014D0F1EE103AEFF30984683C4FF080737B +:10340000E361094BDB6B236684F3098800F0AEF8B7 +:1034100010B1064BA36110BD054BFBE783F3118888 +:10342000F9E700BF00ED00E000EF00E0FF02000858 +:10343000020300084FF0E023002258684FF0FF31EC +:10344000930003F1604303F5614301329042C3F8F6 +:103450008010C3F88011F3D27047000000F1604380 +:1034600003F561430901C9B283F80013012200F09A +:103470001F039A4043099B0003F1604303F5614336 +:10348000C3F880211A6070470023037582680369BE +:103490001B6899689142FBD25A68036042601060D1 +:1034A0005860704700230375826803691B68996838 +:1034B0009142FBD85A6803604260106058607047C0 +:1034C00008B50846302383F311880B7D032B05D004 +:1034D000042B0DD02BB983F3118808BD8B69002212 +:1034E0001A604FF0FF338361FFF7CEFF0023F2E74E +:1034F000D1E9003213605A60F3E70000FFF7C4BF60 +:1035000038B50A4BDD681C68287522681A6053605C +:10351000DC60A36801229342227501D100F004FC13 +:1035200029462046BDE83840FCF7D4BE3838002094 +:1035300030B50C4BDD684B1C87B004460FD02B46D2 +:10354000094A684600F030F92046FFF7D9FF009B92 +:1035500013B1684600F032F9A86907B030BDFFF733 +:10356000CFFFF9E738380020C1340008044B1A684F +:10357000DB6890689B68984294BF002001207047E8 +:103580003838002038B50B4B1C68DD6822681A609B +:10359000536001222275DC60AB68934201D100F0D8 +:1035A000C5FB2846FFF77EFF01462046BDE83840B0 +:1035B000FCF790BE3838002038B5074C074908485A +:1035C000012300252370656000F0EEFB02232370C9 +:1035D00085F3118838BD00BFA03A00206C42000876 +:1035E0003838002008B572B6044B186500F0ACFA04 +:1035F00000F058FB024B03221A70FEE73838002017 +:10360000A03A002000F010B98B60022308618B8281 +:10361000084670478368A3F1840243F8142C0269BA +:1036200043F8442C426943F8402C094A43F8242CBF +:10363000C26843F8182C022203F80C2C002203F86D +:103640000B2C044A43F8102CA3F12000704700BF54 +:10365000ED0200083838002008B5FFF7DBFFBDE8B1 +:103660000840FFF74BBF0000024BDB6898610F205A +:10367000FFF746BF38380020302383F31188FFF767 +:10368000F3BF000008B50146302383F311880820FA +:10369000FFF74EFF002383F3118808BD10B50368C0 +:1036A0009C68A2420CD85C688A600B604C60216008 +:1036B000596099688A1A9A604FF0FF33836010BD91 +:1036C0001B68121BECE700000A2938BF0A2170B5FD +:1036D00004460D460A26601900F060FB00F04CFB22 +:1036E000041BA54203D8751C2E460446F3E70A2E98 +:1036F00004D9BDE87040012000F096BB70BD000009 +:10370000F8B5144B0D46D96103F1100141600A2A46 +:103710001969826038BF0A22016048601861A818E0 +:10372000144600F02DFB0A2700F026FB431BA342A2 +:10373000064606D37C1C281900F030FB2746354688 +:10374000F2E70A2F04D9BDE8F840012000F06CBB75 +:10375000F8BD00BF38380020F8B506460D4600F029 +:103760000BFB0F4A134653F8107F9F4206D12A469F +:1037700001463046BDE8F840FFF7C2BFD169BB68DB +:10378000441A2C1928BF2C46A34202D92946FFF718 +:103790009BFF224631460348BDE8F840FFF77EBF55 +:1037A000383800204838002010B4C0E90323002333 +:1037B0005DF8044B4361FFF7CFBF000010B5194C13 +:1037C000236998420DD0D0E90032816813605A60B5 +:1037D0009A680A449A60002303604FF0FF33A361A4 +:1037E00010BD2346026843F8102F53600022026088 +:1037F00022699A4203D1BDE8104000F0C9BA93682B +:1038000081680B44936000F0B7FA2269E16992681D +:10381000441AA242E4D91144BDE81040091AFFF746 +:1038200053BF00BF383800202DE9F047DFF8BC80D7 +:1038300008F110072C4ED8F8105000F09DFAD8F877 +:103840001C40AA68031B9A423ED81444D5E90032B2 +:103850004FF00009C8F81C4013605A60C5F800908A +:10386000D8F81030B34201D100F092FA89F31188F0 +:10387000D5E9033128469847302383F311886B69D3 +:10388000002BD8D000F078FA6A69A0EB04094A4509 +:1038900082460DD2022000F0C7FA0022D8F810307C +:1038A000B34208D151462846BDE8F047FFF728BF8C +:1038B000121A2244F2E712EB090938BF4A46294698 +:1038C0003846FFF7EBFEB5E7D8F81030B34208D022 +:1038D0001444211AC8F81C00A960BDE8F047FFF79E +:1038E000F3BEBDE8F08700BF48380020383800201C +:1038F00038B500F041FA054AD2E90845031B18190A +:1039000045F10001C2E9080138BD00BF3838002088 +:1039100000207047FEE70000704700004FF0FF30C6 +:1039200070470000BFF34F8F024A1369DB03FCD4DA +:10393000704700BF0020024008B5094B1B7873B9DF +:10394000FFF7F0FF074B5A69002ABFBF064A9A608B +:1039500002F188329A601A6822F480621A6008BD07 +:10396000B83A0020002002402301674508B50B4B00 +:103970001B7893B9FFF7D6FF094B5A6942F0004212 +:103980005A611A6842F480521A601A6822F480520E +:103990001A601A6842F480621A6008BDB83A0020C2 +:1039A000002002407F289ABF00F58030C00200202E +:1039B000704700004FF4006070470000802070479F +:1039C0007F2808B50BD8FFF7EDFF00F5006302680C +:1039D000013204D104308342F9D1012008BD002016 +:1039E000FCE700007F2810B504461FD8FFF79AFFB8 +:1039F000FFF7A2FF0E4BF3221A6102225A615A69A5 +:103A000042EAC0025A615A6942F480325A61FFF7B1 +:103A100089FF4FF40061FFF7C5FF00F03FF9FFF7A2 +:103A2000A5FF2046BDE81040FFF7CABF002010BD2B +:103A3000002002402DE9F84340EA020313F0070397 +:103A4000144606D0304B4FF461721A600020BDE876 +:103A5000F88385182D4A95420CD92B4A40F28931BA +:103A60001160F3E7031D1B684A68934208D1083CC4 +:103A700008300831072C14D902680B689A42F1D03B +:103A8000FFF75AFFFFF74EFF214E08314FF00108B4 +:103A90004FF00009012CA1F1080706D8FFF766FFD7 +:103AA00001E0002CECD10120D1E7C6F814800546D6 +:103AB00051F8083C45F8043B51F8043C4360FFF7DB +:103AC00031FF336943F001033361C6F81490026893 +:103AD00051F8083C9A420CD00B4B40F2B1321A60BC +:103AE0000C4B18600C4B1C600C4B1F60FFF73EFF2B +:103AF000ACE72D6851F8043C9D4201F10801EBD17F +:103B0000083C0830C6E700BFB43A002000000408B3 +:103B100000200240A83A0020B03A0020AC3A002031 +:103B2000084908B50B7828B11BB9FFF705FF012339 +:103B30000B7008BD002BFCD0BDE808400870FFF7F3 +:103B400015BF00BFB83A002002484FF47F4100F093 +:103B5000A7B800BF000100200846114600F0E4BAF3 +:103B6000084600F0F7BA000038B5EFF311859DB9AB +:103B7000EFF30584C4F30804302334B183F31188D0 +:103B8000FFF7B6FE85F3118838BD83F31188FFF780 +:103B9000AFFE84F3118838BDBDE83840FFF7A8BEFA +:103BA00038B5FFF7E1FF114C114AC00840EA4170F7 +:103BB000A0FB025EC908A0FB040C1CEB050CA1FBDA +:103BC00004404FF000035B41A1FB02121CEB040C0C +:103BD00043EB000011EB0E0142F10002411842F1EB +:103BE0000000090941EA007038BD00BFCFF753E378 +:103BF000A59BC42010B50244064BD2B2904200D11E +:103C000010BD441C00B253F8200041F8040BE0B290 +:103C1000F4E700BF50280040114B30B5D3F8904076 +:103C2000240409D4D3F89040C3F89040D3F89040CE +:103C300044F40044C3F890400A4C236843F4807372 +:103C400023600244084BD2B2904200D130BD441CE4 +:103C500000B251F8045B43F82050E0B2F4E700BF33 +:103C600000100240007000405028004007B50122BB +:103C700001A90020FFF7BEFF019803B05DF804FB27 +:103C800013B50446FFF7F2FFA04205D0012201A9B7 +:103C900000200194FFF7C0FF02B010BD7047000084 +:103CA0007047000070470000074B45F255521A60FC +:103CB00003225A6040F2FF729A604CF6CC421A60BE +:103CC000024B01221A70704700300040C03A0020B9 +:103CD000034B1B781BB1034B4AF6AA221A607047AC +:103CE000C03A002000300040054B1A6832B902F19A +:103CF000804202F50432D2F894201A60704700BF67 +:103D0000BC3A0020024B4FF40002C3F894207047E5 +:103D10000010024008B5FFF7E7FF024B1868C0F338 +:103D2000407008BDBC3A002070470000704700009A +:103D300070470000FEE700000A4B0B480B4A904218 +:103D40000BD30B4BDA1C121AC11E22F003028B425A +:103D500038BF00220021FDF753B953F8041B40F887 +:103D6000041BECE704430008C43A0020C43A0020D6 +:103D7000C43A002000F0B6B84FF08043586A70474C +:103D80004FF08043002258631A610222DA607047C4 +:103D90004FF080430022DA60704700004FF080430C +:103DA00058637047FEE7000070B51B4B01630025A8 +:103DB000044686B0586085620E46FFF7EDFA04F1BE +:103DC0001003C4E904334FF0FF33C4E90635C4E9F6 +:103DD0000044A560E562FFF7CFFF2B460246C4E929 +:103DE000082304F134010D4A256580232046FFF79E +:103DF0000BFC0123E0600A4A03750092726801928D +:103E0000B268CDE90223074B6846CDE90435FFF7D8 +:103E100023FC06B070BD00BFA03A00207842000825 +:103E20007D420008A53D0008024AD36A1843D062CB +:103E3000704700BF38380020234BDA6A42F007028F +:103E400010B4DA62DA6A22F00702DA62DA6ADA6C4D +:103E500042F00702DA64DA6E42F00702DA664FF0E7 +:103E60009042DB6E4FF0AA3000234FF010515360A8 +:103E70004FF419249060D1604FF6FF7151611362C5 +:103E800054624FF02A541460C2F80434C2F8080493 +:103E9000C2F80C34C2F81414C2F82034C2F8243426 +:103EA000C2F80034C2F80438C2F80808C2F80C3866 +:103EB000C2F81418C2F82038C2F82438C2F8003802 +:103EC0005DF8044B00F04EB80010024008B500F059 +:103ED00017F9FFF771FBBDE80840FFF705BF0000C9 +:103EE000704700000F4B9A6D42F001029A659A6F7D +:103EF00042F001029A670C4A9B6F936843F00103FA +:103F000093604FF080434F229A624FF0FF32DA62A3 +:103F100000229A615A63DA605A6001225A611A607B +:103F2000704700BF00100240002004E04FF08042C4 +:103F300008B51169D3680B40D9B2C9439B07116119 +:103F400007D5302383F31188FFF75CFB002383F34D +:103F5000118808BD08B5FFF76DFABDE8084000F00C +:103F6000CBB800005A4B4FF0FF319A6A99629A6AB7 +:103F700000229A62986AD86A60F00700D862D86A0C +:103F800000F00700D862D86A186B1963186B1A63BF +:103F9000186B986B9963986B9A63986BD86BD9631D +:103FA000D86BDA63D86B186C1964196C1A641A6CC4 +:103FB000484A4FF4E06111601A6E474942F001022D +:103FC0001A66D3F8802022F00102C3F88020D3F8CB +:103FD00080209A6D42F080529A659A6F22F080524A +:103FE0009A679A6F4FF440720A604A6912F48062CD +:103FF000FBD14A601A6822F0F00242F060021A60B7 +:104000001A6842F001021A601A689107FCD5002272 +:104010009A609A6812F00C02FBD1D3F8901011F458 +:10402000407F1EBF4FF48031C3F89010C3F890203A +:1040300061221A601A689207FCD500229A609A6879 +:1040400012F00C0FFBD169221A60D3F8942022F4ED +:10405000706242F4C062C3F894201A6842F4A0224D +:104060001A601A689003FCD5D3F8902022F003223E +:10407000C3F89020194ADA601A6842F080721A6018 +:104080001A689101FCD5164A1A611A6842F08062DA +:104090001A601A681201FCD500229A600D49114A73 +:1040A000C3F888200A6822F0070242F004020A607E +:1040B0000A6802F00702042AFAD19A6842F0030261 +:1040C0009A609A6802F00C020C2AFAD1704700BF7D +:1040D0000010024000200240007000402314000144 +:1040E000200C10005555013408B5FFF71FFFBDE83F +:1040F0000840FFF775B9000007211C20FFF7AEB993 +:1041000008B5FFF727FF00F007F8FFF7E9FEBDE865 +:104110000840FFF72FBE0000704700000B46014625 +:10412000184600F009B80000024B0A46014618681C +:10413000FFF712BD1811002010B5054C13462CB125 +:104140000A4601460220AFF3008010BD2046FCE77E +:1041500000000000024B01461868FFF701BD00BFD8 +:104160001811002010B501390244904201D10020FD +:1041700005E0037811F8014FA34201D0181B10BDD0 +:104180000130F2E72DE9F041A3B1C91A17780144D3 +:10419000044603F1FF3C8C42204601D9002009E08F +:1041A0000578BD4204F10104F5D10CEB0405D618E5 +:1041B000A54201D1BDE8F08115F8018D16F801ED99 +:1041C000F045F5D0E7E70000034611F8012B03F8AE +:1041D000012B002AF9D17047636F6D2E76696D64EB +:1041E000726F6E65732E7065726970685F626C00C5 +:1041F00040A2E4F1646891060041A3E5F26569928A +:10420000070000004261642043414E4966616365D6 +:1042100020696E6465782E00800000000080000038 +:10422000000080000000000000000000811800086D +:1042300061200008BD1F0008B1180008BD18000863 +:10424000C91A000889180008911800088D1800087C +:10425000E5190008951800087D23000821230008AF +:10426000A5180008B919000863300000684200086A +:1042700090380020A03A00206D61696E0069646C7E +:104280006500FFFFFFFFFFFFBC16000000000000FD +:104290000060030000000000FE2A0100D2040000BC +:1042A0001C110020000000000000000000000000C1 +:1042B00000000000000000000000000000000000FE +:1042C00000000000000000000000000000000000EE +:1042D00000000000000000000000000000000000DE +:1042E00000000000000000000000000000000000CE +:1042F00000000000000000000000000000000000BE +:0443000000000000B9 +:00000001FF diff --git a/Tools/bootloaders/uav-dev-auav-g4_bl.bin b/Tools/bootloaders/uav-dev-auav-g4_bl.bin index 74b8344aeb77a..c5e6beb147a8c 100755 Binary files a/Tools/bootloaders/uav-dev-auav-g4_bl.bin and b/Tools/bootloaders/uav-dev-auav-g4_bl.bin differ diff --git a/Tools/bootloaders/uav-dev-auav-g4_bl.hex b/Tools/bootloaders/uav-dev-auav-g4_bl.hex index b858acaa5e542..5e55a8b48e107 100644 --- a/Tools/bootloaders/uav-dev-auav-g4_bl.hex +++ b/Tools/bootloaders/uav-dev-auav-g4_bl.hex @@ -1,19 +1,19 @@ :020000040800F2 -:1000000000070020F1010008BD2E0008752E000831 -:100010009D2E0008752E0008952E0008F30100089B -:10002000F3010008F3010008F3010008553F000840 +:1000000000070020F1010008952E00084D2E000881 +:10001000752E00084D2E00086D2E0008F301000813 +:10002000F3010008F3010008F30100083D3F000858 :10003000F3010008F3010008F3010008F3010008D0 :10004000F3010008F3010008F3010008F3010008C0 -:10005000F3010008F30100085D440008854400082E -:10006000AD440008D5440008FD440008F301000831 +:10005000F3010008F3010008F3010008F3010008B0 +:10006000F3010008F3010008F3010008F3010008A0 :10007000F3010008F3010008F3010008F301000890 :10008000F3010008F3010008F3010008F301000880 -:10009000F3010008252E0008392E00082545000828 +:10009000F3010008FD2D0008112E0008F3010008EF :1000A000F3010008F3010008F3010008F301000860 -:1000B0000D460008F3010008F3010008F3010008F1 +:1000B00059440008F3010008F3010008F3010008A7 :1000C000F3010008F3010008F3010008F301000840 -:1000D000F3010008F3010008F3010008F9450008E6 -:1000E00089450008F3010008F3010008F301000846 +:1000D000F3010008F3010008F3010008454400089B +:1000E000F3010008F3010008F3010008F301000820 :1000F000F3010008F3010008F3010008F301000810 :10010000F3010008F3010008F3010008F3010008FF :10011000F3010008F3010008F3010008F3010008EF @@ -24,1404 +24,1373 @@ :10016000F3010008F3010008F3010008F30100089F :10017000F3010008F3010008F3010008F30100088F :10018000F3010008F3010008F3010008F30100087F -:10019000F3010008F30100084D2E0008612E00084D +:10019000F3010008F3010008252E0008392E00089D :1001A000F3010008F3010008F3010008F30100085F :1001B000F3010008F3010008F3010008F30100084F :1001C000F3010008F3010008F3010008F30100083F :1001D000F3010008F3010008F3010008F30100082F -:1001E00031150008000000000000000000000000C1 +:1001E00009150008000000000000000000000000E9 :1001F00002E000F000F8FEE772B6374880F30888A6 :10020000364880F3098836483649086040F20000D5 :10021000CCF200004EF63471CEF200010860BFF35C :100220004F8FBFF36F8F40F20000C0F2F0004EF628 :100230008851CEF200010860BFF34F8FBFF36F8F7C :100240004FF00000E1EE100A4EF63C71CEF20001D4 -:100250000860062080F31488BFF36F8F05F05EF806 -:1002600005F03EF94FF055301F491B4A91423CBF03 +:100250000860062080F31488BFF36F8F04F068FFF6 +:1002600005F03AF84FF055301F491B4A91423CBF08 :1002700041F8040BFAE71D49184A91423CBF41F886 :10028000040BFAE71A491B4A1B4B9A423EBF51F82E :10029000040B42F8040BF8E700201849184A914271 -:1002A0003CBF41F8040BFAE705F03CF805F05EF9B5 +:1002A0003CBF41F8040BFAE704F046FF05F05AF8AA :1002B000144C154DAC4203DA54F8041B8847F9E797 :1002C00000F042F8114C124DAC4203DA54F8041B12 -:1002D0008847F9E705F024B8000700200023002034 +:1002D0008847F9E704F02EBF000700200023002024 :1002E0000000000808ED00E00001002000070020E9 -:1002F0005858000800230020B4230020B823002011 -:10030000D4690020E0010008E4010008E4010008CD +:1002F0006056000800230020B4230020B82300200B +:1003000054690020E0010008E4010008E40100084D :10031000E40100082DE9F04F2DED108AC1F80CD052 :10032000C3689D46BDEC108ABDE8F08F002383F3BF -:1003300011882846A047002004F070FCFEE704F076 -:10034000F9FB00DFFEE70000F8B501F063F904F007 -:1003500087FF074604F0D8FF0546A8BB1F4B9F4206 +:1003300011882846A047002004F07AFBFEE704F06D +:1003400003FB00DFFEE70000F8B501F04FF904F011 +:1003500091FE074604F0E2FE0546A8BB1F4B9F42F4 :1003600032D001339F4232D01D4B27F0FF029A4218 -:1003700030D1F8B200F01EFF2E4642F2107400F0A9 -:100380001FFF08B10024264601F0B0FC20B1032075 -:1003900000F07AF80024264635B1124B9F4203D074 -:1003A00004F0AAFF00242646002004F063FF0EB1EB -:1003B00000F080F801F042FA00F036FF204600F02D -:1003C000D1F800F077F8F9E72E460024D7E7044685 +:1003700030D1F8B200F00AFF2E4642F2107400F0BD +:100380000BFF08B10024264601F09CFC20B103209D +:1003900000F078F80024264635B1124B9F4203D076 +:1003A00004F0B4FE00242646002004F06DFE0EB1D9 +:1003B00000F07EF801F02EFA00F022FF204600F057 +:1003C000CFF800F075F8F9E72E460024D7E7044689 :1003D0000126D4E706464FF47A74D0E7010007B04F -:1003E000000008B0263A09B008B501F0DFF8A0F126 -:1003F00020035842584108BD07B541F212030221BB -:1004000001A8ADF8043001F0EFF803B05DF804FB8B -:1004100038B5302383F31188174803680BB104F013 -:10042000BBFC164A144800234FF47A7104F0AAFC6E -:10043000002383F31188124C236813B12368013B16 -:100440002360636813B16368013B63600D4D2B78D3 -:1004500033B963687BB9022001F07EF9322363600F -:100460002B78032B07D163682BB9022001F074F9B4 -:100470004FF47A73636038BDB8230020110400087C -:10048000D8240020D0230020084B187003280CD853 -:10049000DFE800F008050208022001F053B902204D -:1004A00001F046B9024B00225A607047D023002069 -:1004B000D824002010B501F019FC30B1234B0322E1 -:1004C0001A70234B00225A6010BD224B224A1C4650 -:1004D00019680131F8D004339342F9D16268A2421D -:1004E000F2D31E4B9B6803F1006303F510439A425D -:1004F000EAD204F0D3FE04F0E5FE002001F0A4F8F7 -:100500000220FFF7C1FF164B9A6D00229A65996F82 -:100510009A67996FD96DDA65D96FDA67D96F196EF5 -:100520001A66D3F88010C3F88020D3F8803072B6F2 -:100530004FF0E0233021C3F8084DD4E9003281F3B5 -:1005400011889D4683F308881047BDE7D02300201B -:10055000D8240020009000082090000800230020EC -:10056000001002402DE9F04F93B0AC4B00902022D8 -:10057000FF210AA89D6801F00BF9A94A1378A3B9D5 -:10058000A848012103601170302383F311880368A8 -:100590000BB104F001FCA44AA24800234FF47A7185 -:1005A00004F0F0FB002383F31188009B13B19F4BF1 -:1005B000009A1A609E4A009C1378032B1EBF0023EA -:1005C00013709A4A4FF0000A18BF5360D34656463C -:1005D000D146012001F0B6F824B1944B1B68002BE2 -:1005E00000F01582002000F0E1FF0390039B002B38 -:1005F00001DA00F06DFE039B002BEDDB012001F022 -:1006000097F8039B213B162BE3D801A252F823F065 -:100610006D0600089506000829070008D3050008A4 -:10062000D3050008D3050008B307000883090008B4 -:100630009D080008FF080008270900084D09000868 -:10064000D30500085F090008D3050008D109000898 -:100650000D070008D3050008150A000879060008F0 -:100660000D070008D3050008FF0800080220FFF767 -:10067000BBFE002840F0F581009B0221BAF1000F7B -:1006800008BF1C4605A841F21233ADF8143000F043 -:10069000ABFF9EE74FF47A7000F088FF071EEBDB9C -:1006A0000220FFF7A1FE0028E6D0013F052F00F24F -:1006B000DA81DFE807F0030A0D10133605230593EE -:1006C000042105A800F090FF17E054480421F9E741 -:1006D00058480421F6E758480421F3E74FF01C0876 -:1006E000404600F0B3FF08F104080590042105A876 -:1006F00000F07AFFB8F12C0FF2D1012000FA07F7D1 -:1007000047EA0B0B5FFA8BFB4FF0000901F08CF806 -:1007100026B10BF00B030B2B08BF0024FFF76CFE78 -:1007200057E746480421CDE7002EA5D00BF00B0378 -:100730000B2BA1D10220FFF757FE074600289BD0C4 -:10074000012000F081FF0220FFF79EFE00261FFA25 -:1007500086F8404600F088FF044690B100214046EC -:1007600000F09AFF01360028F1D1BA46044641F262 -:100770001213022105A8ADF814303E4600F034FFF4 -:1007800027E70120FFF780FE2546244B9B68AB42FC -:1007900007D9284600F05AFF013040F06781043540 -:1007A000F3E7234B00251D70204BBA465D603E46A3 -:1007B000ACE7002E3FF460AF0BF00B030B2B7FF484 -:1007C0005BAF0220FFF760FE322000F0EFFEB0F1D9 -:1007D0000008FFF651AF18F003077FF44DAF0F4A42 -:1007E000926808EB050393423FF646AFB8F5807F69 -:1007F0003FF742AF124B0193B84523DD4FF47A70B7 -:1008000000F0D4FE0390039A002AFFF635AF019B57 -:10081000039A03F8012B0137EDE700BF0023002006 -:10082000D4240020B823002011040008D82400207C -:10083000D023002004230020082300200C230020C4 -:10084000D4230020C820FFF7CFFD074600283FF43F -:1008500013AF1F2D11D8C5F1200242450AAB25F078 -:10086000030028BF424683490192184400F07EFFEE -:10087000019A8048FF2100F08BFF4FEAA8037D49D1 -:100880000193C8F38702284600F08AFF0646002835 -:100890003FF46DAF019B05EB830537E70220FFF7BF -:1008A000A3FD00283FF4E8AE00F008FF00283FF465 -:1008B000E3AE0027B846704B9B68BB4218D91F2F88 -:1008C00011D80A9B01330ED027F0030312AA134458 -:1008D00053F8203C05934046042205A901F0F0F9A5 -:1008E00004378046E7E7384600F0B0FE0590F2E7AF -:1008F000CDF81480042105A800F076FE06E7002359 -:10090000642104A8049300F065FE00287FF4B4AECF -:100910000220FFF769FD00283FF4AEAE049800F016 -:10092000C3FE0590E6E70023642104A8049300F0C9 -:1009300051FE00287FF4A0AE0220FFF755FD0028ED -:100940003FF49AAE049800F0B1FEEAE70220FFF708 -:100950004BFD00283FF490AE00F0C0FEE1E702201E -:10096000FFF742FD00283FF487AE05A9142000F0F0 -:10097000BBFE04210746049004A800F035FE39466A -:10098000B9E7322000F012FE071EFFF675AEBB0776 -:100990007FF472AE384A926807EB090393423FF640 -:1009A0006BAE0220FFF720FD00283FF465AE27F074 -:1009B00003074F44B9453FF4A9AE484600F046FE50 -:1009C0000421059005A800F00FFE09F10409F1E7E4 -:1009D0004FF47A70FFF708FD00283FF44DAE00F0A9 -:1009E0006DFE002844D00A9B01330BD008220AA9CF -:1009F000002000F0D5FE00283AD02022FF210AA8CE -:100A000000F0C6FEFFF7F8FC1C4804F00DF913B027 -:100A1000BDE8F08F002E3FF42FAE0BF00B030B2B35 -:100A20007FF42AAE0023642105A8059300F0D2FDCF -:100A3000074600287FF420AE0220FFF7D5FC804651 -:100A400000283FF419AEFFF7D7FC41F2883004F0DC -:100A5000EBF8059800F0FEFE464600F0E5FE3C4649 -:100A6000B7E5064652E64FF0000905E6BA467EE6CF -:100A700037467CE6D423002000230020A086010016 -:100A8000094A136849F2690099B21B0C00FB013353 -:100A90001360064B186844F2506182B2000C01FBEF -:100AA0000200186080B27047142300201023002039 -:100AB00010B500211022044600F06AFE034B03CB60 -:100AC000206061601868A06010BD00BF9075FF1FB6 -:100AD000F0B5ADF54D7D6AAC4FF4C47207460D46D6 -:100AE0002046002100F054FE02F0F0F9244B4FF4B0 -:100AF0007A72B0FBF2F0186093E80700022384E8F2 -:100B000007000DF5E5702382FFF7D2FF47F21423AB -:100B10001C49238406A804F091FD102384F8323187 -:100B20000DF2DB2606AB0DF1280C1A4603CA62450E -:100B300030607160134606F10806F6D10BAE3146FF -:100B40000122204600F0A0FE00230393AB7E029317 -:100B500005F11903019380B20123CDE904600093EC -:100B6000E97E05A3D3E90023384602F0C3FD0DF565 -:100B70004D7DF0BDAFF300809E6AC421818A46EEB0 -:100B8000E02400204C5600082DE9F0412C4C237A3B -:100B9000DAB080460D465BBB27A9284600F09AFFD5 -:100BA0000746002842D19DF89D60C82E3ED80146D8 -:100BB0004FF4A662204600F0EBFD4FF48073C4F8BA -:100BC000F8314FF40073C4F80C334FF44073C4F899 -:100BD000203432460DF19E0104F1090000F0C6FDFB -:100BE00026449DF89C30777223720BB9EB7E2372FA -:100BF0008122002106AC27A800F0CAFD012221466F -:100C000027A800F0ADFF00230393AB7E029305F10C -:100C1000190380B201932823CDE904400093E97EB3 -:100C200005A3D3E90023404602F064FD5AB0BDE8B5 -:100C3000F08100BFAFF3008026417272DF25D7B785 -:100C400028350020F0B5254E4FF48A7505FB006568 -:100C5000F1B096F8D83085F8DC300024D82221464F -:100C600085F8E8403AA800F093FD06F1090000F08D -:100C700087FDD5F8E4308DF8F000C2B206AF06F17A -:100C800009010DF1F100CDE93A3400F06FFD39466C -:100C900001223AA800F09AFF80B2CDE9047008233F -:100CA0000127CDE9023706F1D803019330230093E1 -:100CB000317A0B4807A3D3E9002302F01BFDA042C1 -:100CC00006DD02F003F9C5F8E000384671B0F0BD6A -:100CD0002046FBE778F6339F93CACD8D2835002058 -:100CE000F83400202DE9F04F274FDFF8A880274E79 -:100CF00087B0384602F02AFD034600283DD0002484 -:100D0000CDE90344ADF81440027B8DF814209968B6 -:100D10004068029403AA03C21B681D4D43F00043C0 -:100D20000293A146A2462B68D3F810B002F0D0F887 -:100D300010EB080241F100032846CDF800A002A9FB -:100D4000D84704F2345440F668230028C8BF49F05D -:100D5000010905F5A6559C4205F11005E3D1B9F14D -:100D6000000F05D0384602F0F5FC86F800A0C0E779 -:100D70003378072B04D80133337007B0BDE8F08F08 -:100D8000014802F0E7FCF8E7F83400205D3A002063 -:100D9000603A002040420F0070B50D4614461E46D2 -:100DA00002F004FC50B9022E10D1012C0ED112A376 -:100DB000D3E90023C5E90023012007E0282C10D047 -:100DC00005D8012C09D0052C0FD0002070BD302C87 -:100DD000FBD10BA3D3E90023ECE70BA3D3E900235A -:100DE000E8E70BA3D3E90023E4E70BA3D3E900234F -:100DF000E0E700BFAFF30080401DA12026812A0B51 -:100E000078F6339F93CACD8D9E6AC421818A46EEBF -:100E100026417272DF25D7B7F017304A39059E5642 -:100E200038B505460E4C0021013500F035FCA4F81C -:100E30002C55B4F82C0500F017FC78B1B4F82C054B -:100E400000F022FC014648B9B4F82C0500F024FC5F -:100E5000B4F82C350133A4F82C35EAE738BD00BFCF -:100E600028350020F8B50E4C0E4F0226A4F580530D -:100E700043F8407C237E3BB965692DB1284600F0DC -:100E8000B3FF284601F0EAF8204600F0ADFFA4F5D4 -:100E9000A654012EA4F1100400D1F8BD0126E5E707 -:100EA000705F0020AC5600082DE9F04F8FB000AF06 -:100EB00005460C4602F07AFB00284BD1237E022B1C -:100EC0001BD1E38A012B18D102F000F80646FFF788 -:100ED000D7FD03464FF4C870DFF8D482B3FBF0F2BD -:100EE00006F5167602FB103316FA83F3C8F80030C5 -:100EF000E37E33B9A74B00221A703C37BD46BDE8EC -:100F0000F08F07F12401204600F098FD0028F4D16D -:100F100007F11400FFF7CCFD97F8264007F1140104 -:100F2000224607F1270004F057FB0028E2D10F2CDE -:100F300008D8984B1C70D8F80030A3F51673C8F881 -:100F40000030DAE797F824100029D6D0284602F0BE -:100F500025FBD2E7E38A282B33D010D8012B2BD0E6 -:100F6000052BCAD1BFF34F8F8B498C4BCA6802F453 -:100F7000E0621343CB60BFF34F8F00BFFDE7302B20 -:100F8000BBD1874EE17E327A9142B6D1607E314646 -:100F9000002291F8DC50854200F0AD800132042A35 -:100FA00001F58A71F5D1326840F6B831FA328A42D9 -:100FB00094BF32603160A0E721462846FFF788FDE4 -:100FC0009BE721462846FFF7DFFD96E7B2F8EC5095 -:100FD0007B6005F103094FEA99094FEA8902D11DA7 -:100FE000C908A8EBC1039D46EB460021584600F016 -:100FF000CFFB04F1EE012A463144584600F0B6FB1F -:101000007B6813B9012000F01FFB96F8D20000F0B6 -:101010002BFB044630B9307200F050FB204600F044 -:1010200013FBB0E0D6F8D4203AB996F8D200B6F85F -:101030002C25824201D8FFF7F3FED6F8D4202A44AB -:10104000944208D296F8D200B6F82C25013082429C -:1010500001D8FFF7E5FE70685FFA89F2594600F0A3 -:101060009FFB08B9C54679E0726896F8D2002A4419 -:101070007260D6F8D42005EB0209C6F8D49000F0CF -:10108000F3FA814509D396F8D220D6F8D40001327C -:10109000001B86F8D220C6F8D400FF2D0FD80024FC -:1010A000347200F00BFB204600F0CEFA00F01EFE7A -:1010B0003C4B188108B9FFF7FDF9C5461DE7BB6831 -:1010C00096F8D9000AFB0362FB68D2F8E41082F8B4 -:1010D000E83001F58061C2F8E030C2F8E410FFF7B3 -:1010E000B1FDFFF7FFFD96F8D920013202F00302AF -:1010F00086F8D920B6E74FF48A7A0AFB02F505F1A3 -:10110000EA013144204600F095FDF86000287FF4A4 -:10111000F4AE3544012285F8E82001F0D7FED5F879 -:10112000E020D6ED007ADFED206A801A192838BF5A -:10113000192040F6B832904228BF1046B8EE677AC0 -:1011400007EE900AF8EEE77A67EEA67ADFED176A07 -:10115000E7EE267AFCEEE77AC6ED007A96F8D9300B -:101160007168BB600AFB03F4321992F8E8505DB174 -:10117000D2F8E4308B42E8463FF428AF002182F8F1 -:10118000E810C2F8E010C5467368064A9B0A0133AE -:101190001381B2E6F134002000ED00E00400FA050E -:1011A00028350020E0240020CDCCCC3D6666663F8B -:1011B000F4340020014B1870704700BFEC2400206D -:1011C00038B54FF00054144B22689A4221D1607D0B -:1011D000F0B1124B124918701248237D03724FF47C -:1011E0008073C0F8F8314FF40073C0F80C33002559 -:1011F0004FF44073C0F82034C0F8E450C9220930DD -:1012000000F0B4FAE0222946204600F0C1FA01209D -:1012100038BD0020FCE700BF9AAD44C5EC24002097 -:10122000160000202835002037B500F05FFD1F4C68 -:101230001F4D2049288102236B712368204604F545 -:1012400080545B6801229847D4F8D03419495B6810 -:10125000012204F59A60984700230193164B174921 -:1012600000931748174B4FF4805202F06FF9164B5A -:10127000197811B1124802F091F901F027FE0446E5 -:10128000FFF7FEFB4FF4C873B0FBF3F202FB13034E -:1012900004F5167010FA83F00C4B186004F034F863 -:1012A00008B10F232B8103B030BD00BF603A00208E -:1012B000E024002040420F00990D0008F024002097 -:1012C000F8340020A90E0008EC240020F43400209B -:1012D0002DE9F04F2DED028B002593B00DF12C0878 -:1012E0009FED838BDFF84492FFF7FCFC0A95ADF885 -:1012F00034500B95C8F804500026834CDFF80CB22C -:10130000374601238DF81C3023688DF81D508DED74 -:10131000008B0DF11D02D3F808A007A90023204679 -:10132000D0479DF81CA0BAF1000F24D0D9F8143092 -:1013300083F40053C9F81430102200210EA800F0E5 -:1013400027FA236808AA5F690AA90DF11E0320463F -:10135000B84798E803000FAB83E803009DF83430EA -:101360008DF844300A9B0E930EA9DDE908235846F8 -:1013700002F0CEFB574606F2345640F6682304F5D9 -:10138000A6549E4204F11004BBD1002FB4D15F4893 -:1013900002F00CF900283FD15D4E01F097FD336853 -:1013A000984239D301F092FD0446FFF769FB4FF4F0 -:1013B000C873B0FBF3F202FB130304F5167010FAC6 -:1013C00083F03060534E8DF82870377817B90123B9 -:1013D0008DF82830C7F11004E4B20EA8FFF768FBBF -:1013E000062C28BF06240EAB2246D9190DF1290080 -:1013F00000F0BCF90AAB0393182302930134464B67 -:101400000193E4B201230093404804943AA3D3E942 -:10141000002302F011F9357001F058FD3F4A404CAD -:101420001368C31AB3F57A7F2FD3106001F050FD13 -:1014300002460B46354802F097F9344802F0B6F8F8 -:1014400018B3237A374E002B14BF03230223737182 -:1014500001F03CFD0EAF4FF47A730122B0FBF3F0C4 -:1014600039463060304600F0C1FA182302932E4B03 -:10147000019380B240F25513CDE9037000932348E5 -:101480001FA3D3E9002302F0D7F8237A93B101F028 -:101490001DFD002607464FF48A7894F8D9003044A1 -:1014A00000F0030008FB004393F8E82072B1013616 -:1014B000042EF2D1C82003F0B7FB237A002B7FF46F -:1014C0000DAF13B0BDEC028BBDE8F08FD3F8E02078 -:1014D00042B12368FA2B38BFFA23BA1A0533B2EBAC -:1014E000430FE4D3FFF7AEFB0028E0D1E2E700BFF3 -:1014F0000000000000000000401DA12026812A0BF2 -:10150000F1C6A7C1D068080F603A0020F834002067 -:10151000F4340020F1340020F0340020583A002048 -:1015200028350020E02400205C3A00200008004814 -:1015300008B5064800F0A2FF054800F09FFFBDE88F -:101540000840044A0449002004F02AB8603A002008 -:10155000304F002054640020650E000870B5104B19 -:101560001B780133DBB2012B0C4612D80D4B1D68E2 -:1015700029684FF47A730E6AA2FB033201462246B1 -:101580002846B047844204D1074B00221A7001203C -:1015900070BD4FF4FA7003F047FB0020F8E700BF7E -:1015A000182300201C2300204464002007B50023DA -:1015B000024601210DF107008DF80730FFF7CEFF3D -:1015C00020B19DF8070003B05DF804FB4FF0FF3039 -:1015D000F9E700000A4608B50421FFF7BFFF80F0D5 -:1015E0000100C0B2404208BD30B4074B0A4619782A -:1015F000064B53F821402368DD69054B0146AC4694 -:10160000204630BC604700BF446400201C230020FB -:10161000A086010070B503F0BFFC094E094D308073 -:10162000002428683388834208D903F0B1FC2B6872 -:1016300004440133B4F5104F2B60F2D370BD00BFEA -:10164000466400200064002003F060BD00F10060EB -:1016500000F510400068704700F10060920000F54E -:10166000104003F0DDBC0000054B1A68054B1B88D9 -:101670009B1A834202D9104403F08ABC00207047B1 -:101680000064002046640020024B1B68184403F0ED -:1016900087BC00BF00640020024B1B68184403F0A5 -:1016A00091BC00BF006400200020704700F1005092 -:1016B000A0F51040D0F8900570470000064991F859 -:1016C000243033B10023086A81F824300822FFF760 -:1016D000C3BF0120704700BF04640020014B18689D -:1016E000704700BF002004E030B50F4B0F4C1B6863 -:1016F0002288C3F30B030138934208440BD164687A -:101700000A46013C824213460BD214F9015F2DB107 -:1017100002F8015BF6E781420B4602D22C2203F865 -:10172000012B581A30BD00BF002004E02023002008 -:10173000022802BF024B4FF000529A61704700BF6F -:1017400000080048022802BF024B4FF400529A6181 -:10175000704700BF00080048022801BF024A5369D1 -:1017600083F40053536170470008004810B500230C -:10177000934203D0CC5CC4540133F9E710BD0000A0 -:1017800003460246D01A12F9011B0029FAD170470C -:1017900002440346934202D003F8011BFAE7704764 -:1017A0002DE9F8431F4D144695F824200746884636 -:1017B00052BBDFF870909CB395F824302BB92022EF -:1017C000FF2148462F62FFF7E3FF95F82400C0F1A0 -:1017D0000802A24228BF2246D6B24146920005EB3B -:1017E0008000FFF7C3FF95F82430A41B1E44F6B217 -:1017F000082E17449044E4B285F82460DBD1FFF74B -:101800005DFF0028D7D108E02B6A03EB82038342F7 -:10181000CFD0FFF753FF0028CBD10020BDE8F883DD -:101820000120FBE704640020024B1A78024B1A7077 -:10183000704700BF446400201823002003494FF480 -:10184000E1330B60024B186803F0DAB82C64002017 -:101850001C230020094B10B518220446002118460D -:10186000FFF796FF064A074B127804600146BDE871 -:10187000104053F8220003F0C3B800BF2C640020CE -:10188000446400201C2300202DE9F3470D46044644 -:1018900000219046284640F27912FFF779FF23464F -:1018A00020220021284602F0D5F8231D0222202103 -:1018B000284602F0CFF8631D03222221284602F0B9 -:1018C000C9F8A31D03222521284602F0C3F804F11C -:1018D000080310222821284602F0BCF804F1100366 -:1018E00008223821284602F0B5F804F11103082235 -:1018F0004021284602F0AEF804F1120308224821E4 -:10190000284602F0A7F804F11403202250212846AB -:1019100002F0A0F804F1180340227021284602F0DA -:1019200099F804F120030822B021284602F092F829 -:1019300004F121030822B821284602F08BF804F1B3 -:101940002207C0263B46314608222846083602F0C8 -:1019500081F8B6F5A07F07F10107F3D194F8323092 -:101960008DF8073031460DF107030822284602F0B2 -:1019700071F8002604F1330A9DF807304FEAC609D2 -:101980009E4209F5A47720D394F83231502B28BF1A -:1019900050238DF80730B8F1000F08D139460DF10A -:1019A00007030722284602F055F809F24F170026D0 -:1019B00004F233149DF807309E4207EBC6010DD3A5 -:1019C0000731C80802B0BDE8F0870AEB0603082219 -:1019D0003946284602F03EF80136CDE7A319082221 -:1019E000284602F037F80136E4E7000013B5044654 -:1019F0000846002101602346C0F803102022019010 -:101A000002F028F80198231D0222202102F022F87A -:101A10000198631D0322222102F01CF80198A31DE6 -:101A20000322252102F016F8019804F10803102280 -:101A3000282102F00FF8072002B010BDF7B5047F8F -:101A400005460E462CB1838A122B02D9012003B021 -:101A5000F0BD0023194607220096284601F0BEFE7D -:101A6000731C0093012200230721284601F0B6FED3 -:101A7000D4B9B31C0093052223460821284601F05F -:101A8000ADFE0D24B378102BE0D83746B278BB1BDF -:101A9000934210D32B7FA88A0734E408B3B9844259 -:101AA00094BF00200120D2E7AB8ADB00083BDB08B3 -:101AB000B3700824E6E7FB1C0093214600230822AC -:101AC000284601F08BFE08340137DFE7201A18BFE3 -:101AD0000120BCE7F7B5047F05460E462CB1838A8A -:101AE000CA2B02D9012003B0F0BD0023194600968D -:101AF0000822284601F072FE731CD4B90822009314 -:101B000011462346284601F069FE10247378C82B3D -:101B1000E8D8012372785F1C013B934210D32B7FDE -:101B2000A88A0734E408B3B9844294BF0020012096 -:101B3000D9E7AB8ADB00083BDB0873700824E5E7D4 -:101B4000F3190093214600230822284601F046FE9F -:101B500008343B46DEE7201A18BF0120C3E7000027 -:101B6000F7B50D4604460021164628468122FFF7A8 -:101B70000FFE234608220021284601F06BFF94F94E -:101B800001206378002AB8BF7F238DF807309EB903 -:101B90000DF1070307220821284601F05BFF0F27FC -:101BA000002602349DF807309E4207EBC60105D39C -:101BB0000731C80803B0F0BD0827F1E7A3190822D0 -:101BC000284601F047FF0136ECE70000F7B50E4666 -:101BD0000546002114463046CE22FFF7D9FD2B469C -:101BE00028220021304601F035FF2B7AC82B28BF70 -:101BF000C8238DF807308CB90DF10703082228217E -:101C0000304601F027FF30242F469DF807207B1B2C -:101C1000934205D3E01DC00803B0F0BD2824F3E7CC -:101C200007F1090321460822304601F013FF08346A -:101C30000137EAE7F7B5047F05460E4634B1838ADB -:101C4000B3F5827F02D9012003B0F0BD00960123D5 -:101C500010220021284601F0C1FDDCB9B31C00931D -:101C6000092223461021284601F0B8FD1924738863 -:101C7000B3F5807FE7D837467288BB1B934210D3F9 -:101C80002B7FA88A0734E408B3B9844294BF0020AC -:101C90000120D9E7AB8ADB00103BDB0873801024FE -:101CA000E5E73B1D0093214600230822284601F06A -:101CB00095FD08340137DFE7201A18BF0120C3E77C -:101CC00030B5094D0A4491420DD011F8013B5840FE -:101CD000082340F30004013B2C4013F0FF0384EA87 -:101CE0005000F6D1EFE730BD2083B8EDF7B5384AA4 -:101CF000106851686B4603C36A4636493648082364 -:101D000003F07AFC0446002833D10A25334A1068D0 -:101D100051686B4603C36A4631492F48082303F0D4 -:101D20006BFC0446002849D00369B3F5EE2F45D873 -:101D3000B0F8661041F2724291423FD1294A024402 -:101D400002F15C018B4239D35C3B234900209E1A8F -:101D5000FFF7B6FF3246074604F164010020FFF7A3 -:101D6000AFFFA3689F4229D1E368984208BF0025CE -:101D700024E00369B3F5EE2F27D8418B41F272427C -:101D8000914220D1174A024402F110018B4218D32C -:101D9000103B114900209D1AFFF792FF2A46064684 -:101DA00004F118010020FFF78BFFA3689E4202D1C7 -:101DB000E368984201D00D25A8E70025284603B026 -:101DC000F0BD1025A2E70C25A0E70B259EE700BF7C -:101DD0006C560008DC6F070000900008755600087C -:101DE000906F07000870FFF710B5037C044613B925 -:101DF000006803F0E9FB204610BD00000023BFF39C -:101E00005B8FC360BFF35B8FBFF35B8F8360BFF3F8 -:101E10005B8F7047BFF35B8F0068BFF35B8F7047CA -:101E200070B505460C30FFF7F5FF05F108060446CE -:101E30003046FFF7EFFFA04206D930466D68FFF746 -:101E4000E9FF2544281A70BD3046FFF7E3FF201A4A -:101E5000F9E7000070B50546406898B105F1080043 -:101E6000FFF7D8FF05F10C0604463046FFF7D2FF16 -:101E70008442304694BF6D680025FFF7CBFF013CDC -:101E80002C44201A70BD000038B50C460546FFF7FB -:101E9000C7FFA04210D305F10800FFF7BBFF0444C1 -:101EA0006868B4FBF0F100FB1144BFF35B8F0120C5 -:101EB000AC60BFF35B8F38BD0020FCE72DE9F0413B -:101EC000144607460D46FFF7C5FF844228BF044667 -:101ED000D4B1B84658F80C6B4046FFF79BFF30442E -:101EE000286040467E68FFF795FF331A9C4203D86E -:101EF0006C600120BDE8F0816B60A41B3B68AB60A7 -:101F00002044E8600220F5E72046F3E738B50C46A8 -:101F10000546FFF79FFFA04210D305F10C00FFF725 -:101F200079FF04446868B4FBF0F100FB1144BFF38F -:101F30005B8F0120EC60BFF35B8F38BD0020FCE7B6 -:101F40002DE9FF41884669460746FFF7B7FF6C4613 -:101F500006B204EBC6060025B44209D062682068C8 -:101F600008EB0501FFF702FC636808341D44F3E742 -:101F700029463846FFF7CAFF284604B0BDE8F0817D -:101F8000F8B505460C300F46FFF744FF05F108068B -:101F900004463046FFF73EFFA042304688BF6C68DB -:101FA000FFF738FF201A386020B130462C68FFF761 -:101FB00031FF2044F8BD000073B5144606460D46B7 -:101FC000FFF72EFF844228BF04460190DCB101A92F -:101FD0003046FFF7D5FF019B33B93268C5E90233BC -:101FE000C5E9002401200CE09C4238BF0194286020 -:101FF000019868608442F5D93368AB60241AEC60BC -:10200000022002B070BD2046FBE700002DE9FF4131 -:102010000F466946FFF7D0FF6C4600B204EBC005DF -:102020000026AC4209D0D4F8048054F8081BB81933 -:102030004246FFF79BFB4644F3E7304604B0BDE859 -:10204000F081000038B50546FFF7E0FF0446014681 -:102050002846FFF719FF204638BD000008B103F0FD -:10206000B3BA70477047000010B413460268146892 -:102070000022A4465DF8044B6047000000F5805044 -:1020800090F869047047000000F5805090F86204F1 -:102090007047000000F5805090F968047047000018 -:1020A00050207047302383F3118800F58052D2F816 -:1020B000AC34D2F8A8041844D2F8A4341844D2F8A6 -:1020C0008C341844D2F89C341844D2F8983418440C -:1020D000002383F31188704700F58050C0F8641422 -:1020E000012070472DE9FF470C4600F58051054659 -:1020F00091F86204BDF8308098B1D1F888040130BD -:10210000C1F888042068860006D4207B08280BD9F3 -:10211000667B0EB10F2807D9D1F88C340133C1F892 -:102120008C344FF0FF30AAE0302080F31188E86B48 -:10213000D0F8C47017F4001713D0D1F89004013010 -:10214000C1F89004D1F89414002940F09B80CDF898 -:1021500000802146284600F009FF002383F3118800 -:102160008DE0D0F8C460686BC6F301464FF04809B3 -:1021700019FB060920680028B4BF40F0804C4FEAE4 -:10218000804CC9F800C02068400044BF4CF00050AB -:10219000C9F8000094F80CC0300640EA0C40C9F8B9 -:1021A000040094F80DC04FEA461ABCF1000F27D086 -:1021B00040F44010C9F80400D1F8C0040130C1F85F -:1021C000C00405EB0A0101F58351087F40F02000AF -:1021D0000877207BCDE9022300F01CFFDDE9022314 -:1021E00003308010F9B2814207F1010710DA04EBE5 -:1021F000810C09EB8101DCF804C0C1F808C0F1E7EB -:1022000005EB0A0101F58351087F6CF34510DFE708 -:10221000E96B0120B040C1F8CC0006F1830105EB69 -:102220004111C1E9002305EB461101F58351204618 -:10223000083104F10C0750F804CB41F804CBB84244 -:10224000F9D100880880AA4441F278014AF8016077 -:102250000AF5805A08F003019AF87C0041F0100159 -:1022600020F01F0001438AF87C10002181F31188BF -:10227000CDF800802146284600F078FE012004B009 -:10228000BDE8F087002068E738B5C26B936923F09A -:1022900001039361044600F019FE0546E36B9B6958 -:1022A000DB0706D500F012FE431BFA2BF6D90020FF -:1022B00004E004F58054012084F8620438BD000075 -:1022C00013B500F580540191606DFFF7C3FD1F2821 -:1022D0000AD90199606D2022FFF732FEA0F1200398 -:1022E0005842584102B010BD0020FBE708B530232A -:1022F00083F3118800F58050406DFFF77FFD0023C8 -:1023000083F3118808BD00000022026082814260D0 -:102310008260704710B500220023C0E9002300232B -:10232000044603810C30FFF7EFFF204610BD00008C -:102330002DE9F0479A4688B00746884691463023F3 -:1023400083F3118807F580546846FFF7E3FF606D5B -:10235000FFF766FD1F282DD9606D20226946FFF723 -:1023600071FE202826D194F862341BB303AD444695 -:1023700005AB2E4603CE9E4220606160354604F1D7 -:102380000804F6D130682060B388A380DDE900231B -:10239000C9E90023BDF80830AAF80030002383F310 -:1023A000118853464A464146384608B0BDE8F047D2 -:1023B00000F09ABD002080F3118808B0BDE8F087D6 -:1023C0002DE9F84F0646354688461822002104308C -:1023D000FFF7DEF9264B45F8403B06F582572C46C1 -:1023E00020462034FFF796FFA742F9D106F5805426 -:1023F0004FF4805325640025C4E911350123676536 -:10240000E56484F8503084F8583006F5845706F5B2 -:10241000A4594FF0000A4FF0000B47E908ABA7F1B1 -:102420001800FFF771FF203747F8285C4F45F4D1BB -:10243000B8F1010F84F86884A4F86A54A4F86C54C5 -:10244000A4F86E5484F87054A4F87254A4F8745428 -:10245000A4F8765484F8785402D9064800F030FD88 -:10246000054B53F82830F3633046BDE8F88F00BFC2 -:10247000AC560008805600089C56000810B5044B66 -:10248000197804464A1C1A70FFF79AFF204610BDBF -:10249000516400202DE9F84315460C4600295CD014 -:1024A000022001F0D5FF2E49B0FBF4F78C428CBF1F -:1024B0000A2111214B1EB7FBF1F601FB1671DAB2AE -:1024C00021B1022B1946F5D8002032E0731EB3F576 -:1024D000806FF9D2C2EBC20808F103014FEAE10EA6 -:1024E000C1F3C701A2EB010C0EF101094FF47A739D -:1024F0005FFA8CF70EFB033E59FA8CFCBEFBFCFC2A -:10250000BCF5617F17DC1FFA8CF34A1C57FA82F284 -:102510007243B0FBF2F08442D6D14A1E0F2AD3D8C0 -:102520007A1E072AD0D801202B806E80287169710D -:10253000AF71BDE8F88308F1FF314FEAE10CC1F358 -:10254000C701521A0CF1010ED7B20CFB03335EFA2D -:1025500082F2B3FBF2F39BB2D7E70846E9E700BF8C -:102560003F420F0030B50D4B0D4D05201C786C43DC -:102570008C420ED15978518099785171D9789171E6 -:10258000197911715B7903EB83035B0013800120E0 -:1025900030BD013803F10603E8D1F9E7F056000831 -:1025A00040420F0038B500F58053114A93F8683463 -:1025B000D55C4FF45472554305F1804303F5244331 -:1025C000044600211846FFF7E3F80A4B60622B44EB -:1025D000A362094B2B44E362084B2B442363084B53 -:1025E0002B446363E36B0022C3F8C02038BD00BFF7 -:1025F0009456000870A40040B0A4004088A5004094 -:1026000078A600402DE9F04700F580551F4695F863 -:102610006834022B88B004468946904604D90026C7 -:10262000304608B0BDE8F087A54A52F8231009B932 -:1026300042F82300A348C4F81C90017884F8207065 -:1026400099B9302383F311889F4B9A6D42F0007241 -:102650009A659A6B42F000729A639A6B22F000724C -:102660009A630123037081F3118895F86134CBB923 -:10267000302383F31188954A95F86834D35C012B95 -:102680002AD0022B2FD03BB90321152002F002F8EB -:102690000321162001F0FEFF012385F86134002399 -:1026A00083F31188302383F31188E26B936923F05D -:1026B0001003936100F00AFC8246E36B9E6916F0FA -:1026C000080617D000F002FCA0EB0A03FA2BF4D99D -:1026D000002686F31188A3E70321562001F0DAFFD4 -:1026E00003215720D6E70321582001F0D3FF03210F -:1026F0005920CFE79A6942F001029A6100F0E6FBA7 -:102700008246E36B9A69D00706D400F0DFFBA0EBAA -:102710000A03FA2BF5D9DBE79A6942F002029A61C3 -:10272000E36B4FF0000AC3F854A08AF31188686D78 -:10273000FFF764FB202200216846FFF729F802A872 -:10274000FFF7E2FD04F58353CDF818A06A46334441 -:102750000DF1180E9446BCE80300F445186059606A -:10276000624603F10803F5D1DCF80000186020365A -:102770009CF804201A71B6F5806FDBD1002304F5B4 -:10278000A35285F8603485F863340A3249462046FE -:10279000FFF780FE064690B9E26B936923F00103D0 -:1027A000936100F093FB0546E36B9B69D9077FF5C6 -:1027B00036AF00F08BFB431BFA2BF5D92FE795F8CA -:1027C0006E34C5F87C94591E95F86F34E26B013B6A -:1027D0001B0243EA416395F8701401390B43B5F8C5 -:1027E0006C14013943EA0143D361B8F1000F36D0CC -:1027F00004F5A352123241462046FFF7B3FE90B9CA -:10280000E26B936923F00103936100F05FFB0546DF -:10281000E36B9B69DA077FF502AF00F057FB431BC0 -:10282000FA2BF5D9FBE695F87724C5F88084511E7C -:1028300095F87824E36B013A120142EA012295F8F7 -:10284000761401390A43B5F87414013942EA014299 -:1028500042F40002DA60E36B4FF420629A6420468F -:10286000FFF7A0FE002385F86934E36B6FF0404268 -:102870001A65E36B164A5A65E36B44229A65E36B6B -:102880000722C3F8DC20E36B03229742DA653FF4AA -:10289000C7AEE26B936923F00103936100F016FB6E -:1028A0000746E36B9B69DB0705D500F00FFBC31BF5 -:1028B000FA2BF6D9B3E6012385F86234B0E600BFFF -:1028C0004864002050640020001002409456000824 -:1028D0009B0008002DE9F04F00F5A5578BB0054689 -:1028E000904699464FF0000B41F2780A1037EB6B97 -:1028F000D3F8D43023FA0BF3DC0753D505EB4B1197 -:1029000051444FEA4B120B7918074BD405F580560A -:10291000D6F894340133C6F89434C7E900890B79AA -:10292000990648BFD6F8C43405EB020148BF01330D -:10293000514448BFC6F8C4340B7943F008030B7107 -:10294000DB0724D596F863340BB302A8019205F592 -:102950008354FFF7DFFC019A0834144405AB04F1FB -:10296000080C206861681A4603C20834644513469F -:10297000F7D120681060A2889A800123ADF810304A -:102980002B68CDE902891B6C02A928469847D6F826 -:10299000B834D6F864040133C6F8B83410B103680B -:1029A0001B6998470BF1010BBBF1200F9FD10BB0B6 -:1029B000BDE8F08F2DE9F04F8DB004460F4600F0D2 -:1029C00087FA82468946002F56D1E36BD3F89020D0 -:1029D000920141BF04F58051D1F8A8240132C1F819 -:1029E000A824D3F89020160703D100200DB0BDE82D -:1029F000F08FD3F89050E66AC5F30125482303FB16 -:102A00000566E8464046FFF77FFC326851004ABF42 -:102A100022F06043C2F38A4343F00043920048BF70 -:102A200043F080430093736813F400131FBF04F551 -:102A3000805201238DF80D30D2F8C8340EBF8DF8C6 -:102A40000D300133C2F8C834F38803F00F038DF85A -:102A50000C304FF0000B9DF80C0000F0DBFA5FFA31 -:102A60008BF3984220D9F2180CA90B44127A03F880 -:102A70002C2C0BF1010BEEE7012FB6D1E36BD3F851 -:102A80009820950141BF04F58051D1F8A824013266 -:102A9000C1F8A824D3F898201007A6D0D3F89850EE -:102AA000266BC5F30125A9E7EFB9E36BC3F8945092 -:102AB00004A8FFF72FFC98E80F0007AD07C52B808F -:102AC0000023ADF8183023682046CDE904A91B6C1B -:102AD00004A9984704F5805458B1D4F8A0340133C0 -:102AE000C4F8A03482E7012F04BFE36BC3F89C5005 -:102AF000DEE7D4F8A4340133C4F8A434012075E728 -:102B00002DE9F04105460F4600F58054012639466F -:102B10002846FFF74FFF10B184F86364F7E7D4F855 -:102B2000B834D4F864040133C4F8B83420B103686D -:102B3000BDE8F0411B691847BDE8F081F0B5C36BF3 -:102B40001A6C12F47F0F2BD000F580541B6CC4F864 -:102B5000BC3441F27805002347194FF0010C00EB1B -:102B600043122A445E01117911F0020F15D0490772 -:102B700013D4B959C66BD6F8C8E00CFA01F111EAC2 -:102B80000E0F0AD0C6F8D010117941F0040111716E -:102B9000D4F89C240132C4F89C240133202BDED1CC -:102BA000F0BD00002B4B70B51E561B5C012B2FD8BF -:102BB000294D2A4A55F8233052F826400BB341B329 -:102BC000236D1A060FD58023236500F081F950EAA2 -:102BD00001020B4602D0013861F10003024655F8AC -:102BE0002600FFF777FE236D1B032CD555F8263002 -:102BF0004FF4002203F580532265012283F86924F3 -:102C000021E001232365082323654FF480632365B6 -:102C100070BD236DDA0702D4236D9B0706D503230D -:102C200055F8260023650021FFF76AFF236D18077A -:102C300002D4236DD90606D5182355F8260023653E -:102C40000121FFF75DFF55F82600BDE87040FFF752 -:102C500075BF00BF98560008486400209C560008C5 -:102C600008B5302383F31188FFF768FF002383F34F -:102C7000118808BDC36BD3F8C40080F40010C0F302 -:102C80004050704708B5302383F3118800F5805019 -:102C9000406DFFF7C5F8002383F3118843090CBF8B -:102CA0000120002008BD000000F5805393F869243E -:102CB00062B1C16B8A6922F001028A61D3F8AC2447 -:102CC0000132C3F8AC24002283F869247047000065 -:102CD0002DE9F743302181F31188002500F5835158 -:102CE00041F2780E4FF0010800F5805C00EB4514CE -:102CF0007444267977071CD4F6061AD5D0F83C9090 -:102D00008E69D9F8C87008FA06F63E4211D04F68AD -:102D100001970F689742019F9F410AD2C9F8D0607E -:102D2000267946F004062671DCF898440134CCF884 -:102D300098440135202D01F12001D7D1002383F3E0 -:102D4000118803B0BDE8F083F8B51E460023137068 -:102D50000F4605461446FFF795FF80F001003870D6 -:102D60001EB12846FFF786FF2070F8BD2DE9F04F11 -:102D700085B09946DDE90EA30D46029313780193C1 -:102D800091F800B08046164600F0A2F82B78044671 -:102D90000F4613B93378002B41D022463B464046BC -:102DA000FFF796FFFFF75CFFFFF77EFF4B463246CB -:102DB0002946FFF7C9FF2B7833B1BBF1000F03D0D1 -:102DC000012005B0BDE8F08F337813B1019B002BD3 -:102DD000F6D108F580530393029B544577EB030328 -:102DE0001DD2039BD3F86404C8B10368AAEB0401A5 -:102DF0001B6898474B46324629464046FFF7A4FFDA -:102E00002B7813B1BBF1000FDAD1337813B1019BEA -:102E1000002BD5D100F05CF804460F46DCE700201B -:102E2000CFE7000008B500210846FFF7BBFEBDE86C -:102E3000084001F0C3B8000008B501210020FFF7E9 -:102E4000B1FEBDE8084001F0B9B8000008B50021A6 -:102E50000120FFF7A7FEBDE8084001F0AFB8000071 -:102E600008B501210846FFF79DFEBDE8084001F0C6 -:102E7000A5B8000000B59BB0EFF309816822684651 -:102E8000FEF774FCEFF30583014B9B6BFEE700BF7D -:102E900000ED00E008B5FFF7EDFF000000B59BB0C6 -:102EA000EFF3098168226846FEF760FCEFF30583C3 -:102EB000014B5B6BFEE700BF00ED00E0FEE70000AA -:102EC0000FB408B5029801F05FFEFEE702F064B9A6 -:102ED00002F046B90139C9B202299EBF00EBC10018 -:102EE0000023C0E9013370472DE9F8431E461B88D3 -:102EF0005B070546884618D4044600F118094C457E -:102F000013D063686BB12B682846DB6B9847338816 -:102F1000A768C1B243F0040360684246B84708346A -:102F2000EDE7A368002BEED1F9E70120BDE8F883B7 -:102F300073B56C4684E80600014600224E68D5B29F -:102F40006EB98E685EB900EBC200021D94E8030002 -:102F5000013582E803001D70012002B070BD01320E -:102F6000032A01F10801E9D10020F6E72DE9F04F2D -:102F70002DED028B89B09FED258BBDF85080044666 -:102F80008B46054600F11809002708F0040AA945F8 -:102F900036D06B680BB9AB6863B1BAF1000F0BD1D7 -:102FA00023682046DB6B98474346C1B25A46D5E9B1 -:102FB0000106B0470835EAE7002FFBD18DED008B05 -:102FC000ADF808705B4603AA0BF1080C1868596845 -:102FD000174603C7083363453A46F7D11868386087 -:102FE0009B88BB80FFF774FF0423ADF8083023688B -:102FF000CDE900011B6C6946204698470127D9E7B7 -:10300000012009B0BDEC028BBDE8F08F000000008C -:1030100000000000082817D909280CD00A280CD075 -:103020000B280CD00C280CD00D280CD00E2814BF67 -:103030004020302070470C2070471020704714202B -:103040007047182070472020704700002DE9F0419C -:10305000456A15B94162BDE8F0814B6823F06047CD -:10306000C3F38A464FEAD37EC3F3807816EA230679 -:1030700038BF3E46AC462B465A68BEEBD27F22F0A4 -:1030800060440AD0002A18DAA40CB44217D19D4239 -:103090000FD10D60DEE71346EEE7A74207D102F03D -:1030A0008044C2F3807242450BD054B1EFE708D29E -:1030B000EDE7CCF800100B60CDE7B44201D0B4428C -:1030C000E5D81A689C46002AE5D11960C3E70000DC -:1030D0002DE9F047089D01F007044FEAD508224486 -:1030E00005F0070500EBD1004FF47F49944201D170 -:1030F000BDE8F08704F0070705F0070A57453E468C -:1031000038BF5646C6F10806111B8E4228BF0E4630 -:10311000E10808EBD50E415C13F80EC0B94029FA5E -:1031200006F721FA0AF1FFB28CEA010147FA0AF721 -:1031300039408CEA010C03F80EC034443544D5E71D -:1031400080EA0120082341F2210201B24000002957 -:1031500080B203F1FF33B8BF504013F0FF03F4D146 -:103160007047000038B50C468D18A54200D138BD17 -:1031700014F8011BFFF7E4FFF7E7000042684AB1CB -:10318000136843604389818901339BB2994243812B -:1031900038BF83811046704770B588B0202204463E -:1031A0000D4668460021FEF7F3FA20460495FFF726 -:1031B000E5FF024658B16B46054608AE1C4603CCF7 -:1031C000B44228606960234605F10805F6D110462F -:1031D00008B070BD082817D909280CD00A280CD0CF -:1031E0000B280CD00C280CD00D280CD00E2814BFA6 -:1031F0004020302070470C2070471020704714206A -:10320000704718207047202070470000082817D901 -:103210000C280CD910280CD914280CD918280CD932 -:1032200020280CD930288CBF0F200E207047092091 -:1032300070470A2070470B2070470C2070470D2004 -:10324000704700002DE9F843078C072F04461ED96C -:10325000D0E9029800254FF6FF73C5F12006A5F1CD -:10326000200029FA05F108FA06F628FA00F03143A1 -:103270000143C9B21846FFF763FF0835402D0346E6 -:10328000EBD1E1693A46BDE8F843FFF76BBF4FF673 -:10329000FF70BDE8F883000010B54B6823B9CA8AF7 -:1032A00063F30902CA8210BD04691A681C600361D5 -:1032B000C38A013BC3824A60EFE700002DE9F84F63 -:1032C0001D46CB8A0F46C3F3090105298146924664 -:1032D0000B4630D00020AAB207F11A049EB2042E89 -:1032E0001FFA80F80FD8904503F1010306D3FB8A3B -:1032F0000A4462F30903FB8201201AE01AF8006015 -:10330000E6540130EAE79045F1D2A1F1050B1C2308 -:103310007C68BBFBF3F203FB12BB1FFA8BF6002C9D -:1033200045D14846FFF72AFF044638B978606FF068 -:103330000200BDE8F88F4FF00008E6E700260660BF -:103340007860ADB24FF0000B454510D90AEB080389 -:10335000221D13F8011B9155B1B208F101081B2978 -:103360001FFA88F82BD0454506F10106F1D8FB8AF3 -:10337000C3F30902154465F30903BCE7013292B2B5 -:103380001C462368002BF9D16B1F0B441C21B3FB97 -:10339000F1F301339BB29A42D3D2BBF1000FD0D1EB -:1033A0004846FFF7EBFE20B9C4F800B0BFE70122A2 -:1033B000E7E7C0F800B05E4620600446C1E7454537 -:1033C000D5D94846FFF7DAFE08B92060AFE7C0F864 -:1033D00000B0002620600446B6E700002DE9F04F5B -:1033E0002DED028B1C4683B05B690192074688462F -:1033F000002B00F0A780238C2BB1E269002A00F09B -:10340000A180072B35D807F10C00FFF7B7FE054662 -:1034100038B96FF00205284603B0BDEC028BBDE859 -:10342000F08F14220021FEF7B3F9228CE16905F137 -:103430000800FEF79BF9208C013080B2FFF7E6FE12 -:10344000FFF7C8FE013880B2208401302874636918 -:10345000228C1B782A4403F01F0363F03F0348F0DB -:1034600000411372384669602946FFF7EFFD0125D8 -:10347000D1E702339BB20722C18A0633B3FBF2F3D2 -:10348000828A521A9BB292B29342C2D800F10C03C4 -:103490004FF0000908EE103A4FF0800A4E464D46B4 -:1034A00018EE100AFFF76AFE83460028B1D01422F6 -:1034B0000021FEF76DF9002E3AD1019BABF80830E0 -:1034C00002220BF1080E1FFA82FC0CF10100BCF184 -:1034D000060F218C80B201D88E422BD3FFF796FEC7 -:1034E000FFF778FE62691278013802F01F028E42FF -:1034F00008BF4FF0400A42EA49121BFA80F14AEA3B -:10350000020A013048F0004281F808A08BF8100050 -:10351000CBF8042059463846FFF798FD238C013537 -:10352000B3422DB289F001094FF0000AB8D172E719 -:103530000022C6E7E169895D0EF802100136B6B2D5 -:103540000132C0E76FF0010565E70000F8B51546E8 -:103550000E463022002104461F46FEF719F9069B4D -:103560006360B5F5001F079BA76034BF6A094FF67B -:10357000FF72A36297B2E66104F1100000239A4241 -:1035800006D800230360A782E3822383E360F8BDAB -:103590000660013330462036F1E7000003781BB99E -:1035A0004BB2002BC8BF0170704700000078704715 -:1035B000F8B50C46C969074611B9238C002B37D1E1 -:1035C000257E1F2D34D8387828BB228C072A2CD88A -:1035D000268A36F003032BD14FF6FF70FFF7C2FDAA -:1035E00020F001003102400441EA0561400C41EA4B -:1035F00040254FF6FF72234629463846FFF7EEFE78 -:10360000002807DD626913780133DBB21F2B88BF06 -:1036100000231370F8BD218A2D0645EA01250543D4 -:103620002046FFF70FFE0246E5E76FF00300F1E7E3 -:103630006FF00100EEE7000070B58AB00446164650 -:103640000021282268461D46FEF7A2F8BDF8383052 -:10365000ADF810300F9B05939DF840308DF8183071 -:10366000119B07936946BDF84830ADF820302046DD -:10367000CDE90265FFF79CFF0AB070BD2DE9F0416E -:10368000D36905460C4616460BB9138C5BBB377ED7 -:103690001F2F28D895F80080B8F1000F26D03046AB -:1036A000FFF7D0FD3378210241EAC33141EA080136 -:1036B000338A41EA076141EA03410246334641F059 -:1036C00080012846FFF78AFE00280ADD3378012BA7 -:1036D00007D1726913780133DBB21F2B88BF002337 -:1036E0001370BDE8F0816FF00100FAE76FF003009E -:1036F000F7E70000F0B58BB004460D4617460021F1 -:10370000282268461E46FEF743F89DF84C305A1EA4 -:10371000534253418DF800309DF84030ADF81030E1 -:10372000119B05939DF848308DF81830149B079332 -:103730006A46BDF85430ADF8203029462046CDE920 -:103740000276FFF79BFF0BB0F0BD0000406A00B1AE -:1037500004307047436A1A68426202691A60036162 -:10376000C38A013BC38270472DE9F041D0F8208025 -:10377000194E14461D464146002709B9BDE8F0819F -:10378000D1E90223A21A65EB0303964277EB030308 -:103790001ED2036A8B420DD1FFF77EFD036A1B68C0 -:1037A000036203690B60C38A0161016A013BC38242 -:1037B0008846E2E7FFF770FD0B68C8F80030036940 -:1037C0000B60C38A0161013BC382D8F80010D4E7C3 -:1037D00088460968D1E700BF80841E002DE9F04FBC -:1037E0008BB00D46DDF8509014469B46804600286D -:1037F00000F01981B9F1000F00F01581531E3F2B25 -:1038000000F21181012A03D1BBF1000F40F00B81BE -:103810000023CDE90833B8F81430B5EBC30F4FEAF5 -:10382000C30703D300200BB0BDE8F08F2B199F42D4 -:10383000D8F80C303ABF7F1BFFB227461BB9D8F827 -:103840001030002B7AD0272D4ED8C5F12806B7426C -:103850004FF000032CBFF6B23E4600932946D8F83D -:10386000080008AB3246FFF733FCA7EB060A3544E5 -:103870005FFA8AFAB8F8143003F10053053BDB0015 -:103880000493D8F80C3003932821039B13B1BAF1A9 -:10389000000F2CD1D8F8100040B1BAF1000F05D0BC -:1038A000009608AB5246691AFFF712FC38B2002F97 -:1038B000B8D066070AD00AAB03EBD401624211F814 -:1038C000083C02F00702134101F8083C082C3CD9DF -:1038D000102C40F2B580202C40F2B780BBF1000FD5 -:1038E00000F09C80082334E0BA460026C2E7049B1F -:1038F000E02B28BFE02306930B44AB42059314D979 -:103900005A1B03980096924534BF5246D2B2691AA8 -:1039100008AB04300792FFF7DBFB079A1644AAEBCB -:10392000020A1544F6B25FFA8AFA049B069A0599D0 -:103930009B1A0493039B1B680393A6E70093D8F894 -:10394000080008AB3A462946AEE7BBF1000F13D09A -:103950000123B4EBC30F6CD0082C12D89DF8203093 -:10396000621E23FA02F2D50706D54FF0FF3202FAA3 -:1039700004F423438DF820309DF8203089F800307E -:1039800051E7102C12D8BDF82030621E23FA02F243 -:10399000D10706D54FF0FF3202FA04F42343ADF805 -:1039A0002030BDF82030A9F800303CE7202C0FD89B -:1039B0000899631E21FA03F3DA0705D54FF0FF32A9 -:1039C00002FA04F40C430894089BC9F800302AE773 -:1039D000402C2BD0DDE90865611EC4F12102A4F161 -:1039E000210326FA01F105FA02F225FA03F3114345 -:1039F0001943CB0712D50122A4F12003C4F1200101 -:103A000002FA03F322FA01F1A240524243EA01030F -:103A100063EB430332432B43CDE90823DDE908235D -:103A2000C9E90023FFE66FF00100FCE66FF0080033 -:103A3000F9E6082CA0D9102CB3D9202CEED8C3E776 -:103A4000BBF1000FADD0022383E7BBF1000FBBD069 -:103A500004237EE730B5012A144638BF0124402CE8 -:103A600085B028BF40240025012ACDE9025518D889 -:103A70001B788DF8083063070AD004AB03EBD4053C -:103A8000624215F8083C02F00702934005F8083C32 -:103A9000009103462246002102A8FFF719FB05B05A -:103AA00030BD082AE4D9102A03D81B88ADF80830A5 -:103AB000E1E7202A8DBFD3E900231B680293CDE9FB -:103AC0000223D8E710B5CB681BB98B600B618B82E2 -:103AD00010BD04691A681C600361C38A013BC3827C -:103AE000CA60F0E703064CBFC0F3C0300220704745 -:103AF00008B50246FFF7F6FF022806D15306C2F3C7 -:103B00000F2001D100F0030008BDC2F30740FBE71E -:103B10002DE9F04F93B0CDE903230A68044610461F -:103B2000FFF7E0FF022814BFC2F306260026002A92 -:103B30000D46824680F2F78112F0C04940F0F381D1 -:103B4000097B002900F0EF81022803D02378B342DB -:103B500040F0EC81C2F304630693104602F07F0349 -:103B60000593FFF7C5FF059B29444FEA834848EAC0 -:103B70000A4848EA4668CE7800230022CDE90823A7 -:103B8000F309834648EA0008029367D0059B009337 -:103B900002466768534608A92046B847002800F047 -:103BA000C881276A4FB9414604F10C00FFF7F4FAC7 -:103BB000074690B96FF0020054E03B6998450DD07C -:103BC0003F68002FF9D1414604F10C00FFF7E4FAF9 -:103BD00007460028EED0236A3B60276297F817C09B -:103BE00006F01F08CCF3840CACEB08001FFA80FE33 -:103BF0000028B8BF0EF12000A8EB0C031FFA83FECB -:103C0000D7E90221B8BF00B2002B0793BEBF0EF167 -:103C100020031BB2079352EA010338D0039BDFF85D -:103C200024E39A1A049B4FF0000C63EB01019645C4 -:103C30007CEB01032BD36B7B97F81AE0734519D10A -:103C4000029B002B78D0012821DC7868F8B9DFF8D6 -:103C5000F0C2944570EB010316D337E0276A27B909 -:103C60006FF00C0013B0BDE8F08F3B699845B5D0FC -:103C70003F68F4E7B24890427CEB010301D3002097 -:103C8000F0E7029B002BFAD0079B0F2B17DCFA7D85 -:103C9000B30002F0030203F07C031343FB753946C3 -:103CA0002046FFF7F9FA6B7BBB76029B3BB9FB7DA5 -:103CB000C3F38402013262F38603FB75D0E76A7BAB -:103CC000BB7E9A42DBD1029B002B35D0B309022B7D -:103CD00032D0039BBB60049BFB60142200210DA823 -:103CE000FDF756FD039B0A93049B0B932B1D0C932E -:103CF0002B7BADF83EB0013BDBB2ADF83C30069B10 -:103D00008DF84230059B8DF8433094F82C308DF8B7 -:103D100040A083F001038DF844308DF84180A36802 -:103D20000AA920469847FB7DC3F38403013303F0BF -:103D30001F039B02FB82A2E7FB7DC6F34012B2EB9E -:103D4000D31F40F0F980C3F38403434540F0FC8067 -:103D5000029A2B7BB609002A52D016F0010661D1D7 -:103D6000032B40F2F480039BBB60049BFB60FB8A47 -:103D700066F30903FB822B7BAE1D033BDBB23246AD -:103D8000394604F10C00FFF799FA00280CDA39469D -:103D90002046FFF781FAFB7DC3F38403013303F070 -:103DA0001F039B02FB8205E7DDE90884AB883B83A8 -:103DB0004FF6FF73C9F12000A9F1200228FA09F19A -:103DC00004FA00F0014324FA02F211431846C9B282 -:103DD000FFF7B6F909F10809B9F1400F0346E9D137 -:103DE000B8822A7B033AD2B23146FFF7BBF9FB7D9A -:103DF000B882DA43C2F3C01262F3C713FB753EE721 -:103E000086B92E1D013BDBB23246394604F10C0067 -:103E1000FFF754FA0028BADB2A7BB88A013AD2B2FB -:103E20003146E2E7F98AC1F30901013B0429DAB21C -:103E30005BD8281D002307F11B069A4208D910F809 -:103E400001CB06F801C0013101330529DBB2F4D101 -:103E500003990A9104990B91934207F11B010C916C -:103E600038BF043379680D9134BF55FA83F30023CA -:103E70000E93FB8AADF83EB0C3F309031A44069BC8 -:103E80008DF84230059B8DF8433094F82C30ADF816 -:103E90003C2083F001038DF8443000238DF840A0CE -:103EA0008DF841807B602A7BB88A013A291DFFF793 -:103EB00059F93B8BB882834203D1A3680AA92046F3 -:103EC000984720460AA9FFF7FDFDFB7DBA8AC3F398 -:103ED0008403013303F01F039B02FB823B8B9A4256 -:103EE0000CBF00206FF01000BCE67B68002BAFD049 -:103EF000052001E01C3033461E68002EFAD1091A55 -:103F0000081D2E1D184401EB090CBCF11B0F5FFAB4 -:103F100089F39DD89A429BD916F8013B00F8013BE2 -:103F200009F10109EFE76FF009009BE66FF00A0065 -:103F300098E66FF00B0095E66FF00D0092E600BF7B -:103F400040420F0080841E006FF00E008AE66FF082 -:103F50000F0087E6EFF3098305494A6B22F001025F -:103F60004A63683383F30988002383F31188704719 -:103F700000EF00E0302080F3118862B60C4B0D4A50 -:103F8000D96821F4E0610904090C0A43DA60D3F826 -:103F9000FC20094942F08072C3F8FC200A6842F014 -:103FA00001020A602022DA7783F82200704700BFFE -:103FB00000ED00E00003FA05001000E010B530232A -:103FC00083F311880E4B5B6813F4006314D0F1EE99 -:103FD000103AEFF30984683C4FF08073E361094BBA -:103FE000DB6B236684F3098800F098FD10B1064B63 -:103FF000A36110BD054BFBE783F31188F9E700BF10 -:1040000000ED00E000EF00E03F030008420300087D -:10401000026843681143016003B11847704700000C -:10402000024A136843F0C003136070470048004021 -:1040300013B50F4C204600F0D7FC04F114000D49D5 -:10404000009400234FF4807200F094FB0A490B4B5C -:1040500000944FF4807204F1380000F00DFC084B1E -:10406000E365002000F0F4F9206602B010BD00BF47 -:1040700058640020C4640020C4650020214000086A -:104080000048004030B5037C214C002918BF0C4685 -:10409000012B0CD11F4B984209D11F4B9A6D42F452 -:1040A00080229A659A6F42F480229A679B6F2268F9 -:1040B000036EC16D846603EB5203B3FBF2F36268D7 -:1040C000150442BF23F0070503F0070343EA450345 -:1040D000CB60A36843F040034B60E36843F0010307 -:1040E0008B6042F4967343F001030B604FF0FF3393 -:1040F0000B62510505D512F0102205D0B2F1805F98 -:1041000004D080F8643030BD7F23FAE73F23F8E71E -:104110001057000858640020001002402DE9F047B5 -:10412000C66D3768F46934622107054619D014F06A -:10413000080118BF4FF48071E20748BF41F0200129 -:10414000A30748BF41F04001600748BF41F080012C -:10415000302383F31188281DFFF75AFF002383F3D0 -:104160001188E2050AD5302383F311884FF480616A -:10417000281DFFF74DFF002383F311884FF030090E -:104180004FF0000A14F0200838D13B0616D54FF046 -:10419000300905F1380A200610D589F311885046F8 -:1041A00000F09EFB002836DA0821281DFFF730FFBB -:1041B00027F080033360002383F31188790614D538 -:1041C000620612D5302383F31188D5E913239A426E -:1041D00008D12B6C33B11021281D27F04007FFF7C1 -:1041E00017FF3760002383F31188E30618D5AA6E02 -:1041F0001369ABB1BDE8F0475069184789F31188DE -:10420000736A95F864102846194000F007FC8AF399 -:104210001188F469B6E7B06288F31188F469BAE7E7 -:10422000BDE8F087434B4FF4007270B51A605A69CD -:1042300012F4C06FFBD1404B9A6802F00C02042AC2 -:1042400020D01A6842F480721A601A685205FCD5B0 -:1042500001229A609A6802F00C02042AFAD13749C6 -:10426000374A0A600A6812F00F02FBD1C3F898209F -:1042700063221A601A689603FCD42E4A4FF48071A8 -:10428000C2F88010C468E50306D51A6842F480328B -:104290001A601A689103FCD5C269D20709D5D3F810 -:1042A000982042F00102C3F89820D3F8982096078E -:1042B000FBD54269DA6044F480721A6004F08071C0 -:1042C00011B11A689501FBD5996802691B4E22F05D -:1042D000030501F003012943996085693560316860 -:1042E00069400907FBD1134905680D6046684E60B7 -:1042F0008068C1F880006E0417D448698505FCD435 -:10430000996802F0030021F00301014399609200D3 -:104310009968514011F00C0FFAD1E2055EBF1A689E -:1043200022F480721A60002070BD48698005FCD5B7 -:10433000E6E700BF0070004000100240002002408D -:104340000006040008B500F091F9BDE8084000F04F -:1043500065B9000010B5394C3948A36A4FF0FF32F7 -:10436000A262A36A0023A362A16AE16A61F07F01ED -:10437000E162E16A01F07F01E162E16A216B22639F -:10438000216B2363216BA16BA263A16BA363A16B60 -:10439000E16BE263E16BE363E16B216C2264226C0D -:1043A0002364226C226E42F001022266D4F880203F -:1043B00022F00102C4F88020D4F88020A26D42F0DF -:1043C0008052A265A26F22F08052A267A26F1D4A9E -:1043D0004FF400419160D360136253629362D362E1 -:1043E000136353639363D363136453649364D36419 -:1043F00013655365116841F480711160D4F8902001 -:1044000012F4407F1EBF4FF48032C4F89020C4F8ED -:1044100090300D4A0023A360C4F88820C4F89C3073 -:10442000FFF700FF10B1094800F0AEFBD4F8903060 -:1044300023F00323C4F8903010BD00BF00100240E9 -:1044400030570008007000405501005128570008FF -:10445000014B53F82000704728230020074A08B575 -:10446000536903F00103536123B1054A13680BB18B -:1044700050689847BDE80840FFF7A0BD0004014020 -:1044800054690020074A08B5536903F002035361D9 -:1044900023B1054A93680BB1D0689847BDE808403E -:1044A000FFF78CBD0004014054690020074A08B59D -:1044B000536903F00403536123B1054A13690BB137 -:1044C00050699847BDE80840FFF778BD00040140F7 -:1044D00054690020074A08B5536903F00803536183 -:1044E00023B1054A93690BB1D0699847BDE80840EC -:1044F000FFF764BD0004014054690020074A08B575 -:10450000536903F01003536123B1054A136A0BB1D9 -:10451000506A9847BDE80840FFF750BD00040140CD -:1045200054690020164B10B55C6904F478725A6126 -:10453000A30604D5134A936A0BB1D06A9847600664 -:1045400004D5104A136B0BB1506B9847210604D564 -:104550000C4A936B0BB1D06B9847E20504D5094A1E -:10456000136C0BB1506C9847A30504D5054A936CA6 -:104570000BB1D06C9847BDE81040FFF71FBD00BFDE -:104580000004014054690020194B10B55C6904F423 -:104590007C425A61620504D5164A136D0BB1506D09 -:1045A0009847230504D5134A936D0BB1D06D9847F6 -:1045B000E00404D50F4A136E0BB1506E9847A10466 -:1045C00004D50C4A936E0BB1D06E9847620404D5A3 -:1045D000084A136F0BB1506F9847230404D5054A5E -:1045E000936F0BB1D06F9847BDE81040FFF7E6BC62 -:1045F000000401405469002008B50348FFF78EFD10 -:10460000BDE80840FFF7DABC5864002008B500F0A8 -:10461000E1FEBDE80840FFF7D1BC0000062108B567 -:10462000084600F037F80621072000F033F806218D -:10463000082000F02FF80621092000F02BF80621B1 -:104640000A2000F027F80621172000F023F80621A1 -:10465000282000F01FF807211C2000F01BF8BDE8FF -:1046600008400C21272000F015B800004FF0E0238F -:10467000002258684FF0FF31930003F1604303F5C7 -:10468000614301329042C3F88010C3F88011F3D225 -:104690007047000000F1604303F561430901C9B2AE -:1046A00083F80013012200F01F039A4043099B0086 -:1046B00003F1604303F56143C3F880211A6070473A -:1046C000F8B5154682680669AA420B46816938BF6B -:1046D0008568761AB54204460BD218462A46FDF77D -:1046E00045F8A3692B44A361A3685B1BA36028461C -:1046F000F8BD0CD918463246FDF738F8AF1BE16813 -:104700003A463044FDF732F8E3683B44EBE718469D -:104710002A46FDF72BF8E368E5E70000836893423B -:10472000F7B51546044638BF8568D0E90460361AE7 -:10473000B5420BD22A46FDF719F863692B44636131 -:10474000A36828465B1BA36003B0F0BD0DD93246B9 -:104750000191FDF70BF80199E068AF1B3A4631442F -:10476000FDF704F8E3683B44E9E72A46FCF7FEFF5F -:10477000E368E4E710B50A440024C361029B846047 -:10478000C0E90000C0E90511C1600261036210BD0B -:1047900008B5D0E90532934201D1826882B98268B6 -:1047A000013282605A1C42611970D0E904329A4287 -:1047B00024BFC3684361002100F052FA002008BD05 -:1047C0004FF0FF30FBE7000070B5302304460E4683 -:1047D00083F31188A568A5B1A368A269013BA36012 -:1047E000531CA36115782269934224BFE368A36137 -:1047F000E3690BB120469847002383F311882846CC -:1048000007E03146204600F01BFA0028E2DA85F383 -:10481000118870BD2DE9F74F04460E46174698469D -:10482000D0F81C904FF0300A8AF311884FF0000B3B -:10483000154665B12A4631462046FFF741FF03463B -:1048400060B94146204600F0FBF90028F1D0002372 -:1048500083F31188781B03B0BDE8F08FB9F1000F26 -:1048600003D001902046C847019B8BF31188ED1AB5 -:104870001E448AF31188DCE7C0E90511C160C361F9 -:104880001144009B8260C0E900000161036270472F -:10489000F8B504460D461646302383F31188A76801 -:1048A000A7B1A368013BA36063695A1C62611D70D4 -:1048B000D4E904329A4224BFE3686361E3690BB12F -:1048C00020469847002080F3118807E031462046B3 -:1048D00000F0B6F90028E2DA87F31188F8BD00008D -:1048E000D0E905239A4210B501D182687AB982686D -:1048F000013282605A1C82611C7803699A4224BF8B -:10490000C3688361002100F0ABF9204610BD4FF071 -:10491000FF30FBE72DE9F74F04460E461746984651 -:10492000D0F81C904FF0300A8AF311884FF0000B3A -:10493000154665B12A4631462046FFF7EFFE03468D -:1049400060B94146204600F07BF90028F1D00023F1 -:1049500083F31188781B03B0BDE8F08FB9F1000F25 -:1049600003D001902046C847019B8BF31188ED1AB4 -:104970001E448AF31188DCE7026843681143016032 -:1049800003B11847704700001430FFF743BF000021 -:104990004FF0FF331430FFF73DBF00003830FFF712 -:1049A000B9BF00004FF0FF333830FFF7B3BF00004E -:1049B0001430FFF709BF00004FF0FF311430FFF74C -:1049C00003BF00003830FFF763BF00004FF0FF3235 -:1049D0003830FFF75DBF0000012914BF6FF01300EE -:1049E00000207047FFF724BB044B03600023C0E99D -:1049F0000233436001230374704700BF505700081F -:104A000010B53023044683F31188FFF73BFB0223E4 -:104A10002374002080F3118810BD000038B5C369ED -:104A200004460D461BB904210844FFF7A5FF29469B -:104A300004F11400FFF7ACFE002806DA201D4FF445 -:104A40000061BDE83840FFF797BF38BD002303750C -:104A5000826803691B6899689142FBD25A680360B7 -:104A600042601060586070470023037582680369D4 -:104A70001B6899689142FBD85A68036042601060D5 -:104A80005860704708B50846302383F311880B7DC2 -:104A9000032B05D0042B0DD02BB983F3118808BD4F -:104AA0008B6900221A604FF0FF338361FFF7CEFF5E -:104AB0000023F2E7D1E9003213605A60F3E7000007 -:104AC000FFF7C4BF054BD9680875186802681A60FB -:104AD000536001220275D860FBF71CBCC866002039 -:104AE00030B50C4BDD684B1C87B004460FD02B460D -:104AF000094A684600F046F92046FFF7E3FF009BAD -:104B000013B1684600F048F9A86907B030BDFFF757 -:104B1000D9FFF9E7C8660020854A0008044B1A68E7 -:104B2000DB6890689B68984294BF00200120704722 -:104B3000C8660020084B10B51C68D86822681A6047 -:104B4000536001222275DC60FFF78EFF014620468C -:104B5000BDE81040FBF7DEBBC866002038B5074C47 -:104B600007490848012300252370656000F060FCB8 -:104B70000223237085F3118838BD00BF30690020FF -:104B80007C570008C866002008B572B6044B18654B -:104B900000F0D0FA00F080FB024B03221A70FEE70F -:104BA000C86600203069002000F02CB98B60022319 -:104BB00008618B82084670478368A3F1840243F83A -:104BC000142C026943F8442C426943F8402C094AEA -:104BD00043F8242CC26843F8182C022203F80C2C4A -:104BE000002203F80B2C044A43F8102CA3F12000F8 -:104BF000704700BF2D030008C866002008B5FFF706 -:104C0000DBFFBDE80840FFF75BBF0000024BDB683D -:104C100098610F20FFF756BFC8660020302383F34A -:104C20001188FFF7F3BF000008B50146302383F376 -:104C300011880820FFF754FF002383F3118808BD73 -:104C4000064BDB6839B1426818605A6013604360F4 -:104C50000420FFF745BF4FF0FF307047C8660020C3 -:104C60000368984206D01A6802605060996118463D -:104C7000FFF726BF7047000010B503689C68A2428A -:104C80000CD85C688A600B604C6021605960996840 -:104C90008A1A9A604FF0FF33836010BD1B68121BA5 -:104CA000ECE700000A2938BF0A2170B504460D461A -:104CB0000A26601900F0B6FB00F0A2FB041BA54217 -:104CC00003D8751C2E460446F3E70A2E04D9BDE826 -:104CD0007040012000F0ECBB70BD0000F8B5144B33 -:104CE0000D46D96103F1100141600A2A19698260F9 -:104CF00038BF0A22016048601861A818144600F005 -:104D000083FB0A2700F07CFB431BA342064606D325 -:104D10007C1C281900F086FB27463546F2E70A2F4F -:104D200004D9BDE8F840012000F0C2BBF8BD00BFC7 -:104D3000C8660020F8B506460D4600F061FB0F4A34 -:104D4000134653F8107F9F4206D12A46014630464B -:104D5000BDE8F840FFF7C2BFD169BB68441A2C19FF -:104D600028BF2C46A34202D92946FFF79BFF2246C3 -:104D700031460348BDE8F840FFF77EBFC866002013 -:104D8000D866002010B4C0E9032300235DF8044B6B -:104D90004361FFF7CFBF000010B5194C236998425B -:104DA0000DD0D0E90032816813605A609A680A44D5 -:104DB0009A60002303604FF0FF33A36110BD2346C8 -:104DC000026843F8102F53600022026022699A4261 -:104DD00003D1BDE8104000F01FBB936881680B440D -:104DE000936000F00DFB2269E1699268441AA242C7 -:104DF000E4D91144BDE81040091AFFF753BF00BFC2 -:104E0000C86600202DE9F047DFF8BC8008F11007E4 -:104E10002C4ED8F8105000F0F3FAD8F81C40AA68CD -:104E2000031B9A423ED81444D5E900324FF00009E2 -:104E3000C8F81C4013605A60C5F80090D8F81030CC -:104E4000B34201D100F0E8FA89F31188D5E90331C2 -:104E500028469847302383F311886B69002BD8D0FC -:104E600000F0CEFA6A69A0EB04094A4582460DD2E9 -:104E7000022000F01DFB0022D8F81030B34208D108 -:104E800051462846BDE8F047FFF728BF121A2244D2 -:104E9000F2E712EB090938BF4A4629463846FFF7C0 -:104EA000EBFEB5E7D8F81030B34208D01444211A0D -:104EB000C8F81C00A960BDE8F047FFF7F3BEBDE8E5 -:104EC000F08700BFD8660020C866002038B500F023 -:104ED00097FA054AD2E90845031B181945F1000164 -:104EE000C2E9080138BD00BFC86600200020704735 -:104EF000FEE70000704700004FF0FF3070470000F1 -:104F0000BFF34F8F024A1369DB03FCD4704700BF25 -:104F10000020024008B5094B1B7873B9FFF7F0FF7A -:104F2000074B5A69002ABFBF064A9A6002F18832CD -:104F30009A601A6822F480621A6008BD48690020ED -:104F4000002002402301674508B50B4B1B7893B93D -:104F5000FFF7D6FF094B5A6942F000425A611A68BE -:104F600042F480521A601A6822F480521A601A6859 -:104F700042F480621A6008BD4869002000200240A7 -:104F8000FF289ABF00F58030C00200207047000063 -:104F90004FF40060704700004FF4807070470000CD -:104FA000FF2808B50BD8FFF7EBFF00F50063026898 -:104FB000013204D104308342F9D1012008BD002020 -:104FC000FCE70000FF2838B5044624D8FFF798FF17 -:104FD000FFF7A0FF114AF323136102235361C300BB -:104FE0005169C50903F47E7343EAC5230B4353613A -:104FF000536943F480335361FFF782FF4FF400613C -:10500000FFF7BEFF00F044F9FFF79EFF2046BDE822 -:105010003840FFF7C5BF002038BD00BF0020024068 -:105020002DE9F84340EA020313F00703144606D0C3 -:10503000304B40F282321A600020BDE8F8838518B8 -:105040002D4A95420CD92B4A40F287311160F3E783 -:10505000031D1B684A68934208D1083C0830083198 -:10506000072C14D902680B689A42F1D0FFF752FF5F -:10507000FFF746FF214E08314FF001084FF00009BD -:10508000012CA1F1080706D8FFF75EFF01E0002C14 -:10509000ECD10120D1E7C6F81480054651F8083C50 -:1050A00045F8043B51F8043C4360FFF729FF33699E -:1050B00043F001033361C6F81490026851F8083CCC -:1050C0009A420CD00B4B40F2AF321A600C4B186076 -:1050D0000C4B1C600C4B1F60FFF736FFACE72D68D4 -:1050E00051F8043C9D4201F10801EBD1083C083025 -:1050F000C6E700BF44690020000008080020024005 -:1051000038690020406900203C690020084908B542 -:105110000B7828B11BB9FFF7FDFE01230B7008BD0A -:10512000002BFCD0BDE808400870FFF70DBF00BFA2 -:105130004869002008B50649064800F0ABF8BDE80C -:1051400008404FF400414FF0805000F0A3B800BF7A -:10515000007F010000010020084600F035BA000081 -:1051600038B5EFF311859DB9EFF30584C4F3080456 -:10517000302334B183F31188FFF7A8FE85F311883B -:1051800038BD83F31188FFF7A1FE84F3118838BD81 -:10519000BDE83840FFF79ABE38B5FFF7E1FF114C84 -:1051A000114AC00840EA4170A0FB025EC908A0FB9A -:1051B000040C1CEB050CA1FB04404FF000035B4109 -:1051C000A1FB02121CEB040C43EB000011EB0E01DF -:1051D00042F10002411842F10000090941EA007061 -:1051E00038BD00BFCFF753E3A59BC4200244D2B221 -:1051F000904200D17047431C800000F1804000F5D0 -:105200001450006841F8040BD8B2F1E7124B10B506 -:10521000D3F89040240409D4D3F89040C3F89040C8 -:10522000D3F8904044F40044C3F890400B4C2368FA -:10523000024443F480732360D2B2904200D110BD87 -:10524000431C800000F1804000F5145051F8044BDD -:105250000460D8B2F1E700BF0010024000700040C7 -:1052600007B5012201A90020FFF7C0FF019803B094 -:105270005DF804FB13B50446FFF7F2FFA04205D02A -:10528000012201A900200194FFF7C0FF02B010BD68 -:10529000704700007047000070470000074B45F260 -:1052A00055521A6003225A6040F2FF729A604CF61F -:1052B000CC421A60024B01221A7070470030004045 -:1052C00050690020034B1B781BB1034B4AF6AA22FE -:1052D0001A6070475069002000300040054B1A6882 -:1052E00032B902F1804202F50432D2F894201A60F9 -:1052F000704700BF4C690020024B4FF40002C3F816 -:10530000942070470010024008B5FFF7E7FF024BFA -:105310001868C0F3407008BD4C6900207047000059 -:10532000FEE700000A4B0B480B4A90420BD30B4B95 -:10533000DA1C121AC11E22F003028B4238BF00226F -:105340000021FCF725BA53F8041B40F8041BECE7D6 -:105350000C590008D4690020D4690020D4690020C9 -:105360000023054A19460133102BC2E9001102F14E -:105370000802F8D1704700BF5469002008B5124BED -:105380009A6D42F001029A659A6F42F001029A67A3 -:105390000E4A9B6F936843F0010393600620FFF76A -:1053A00057F80B4BB0FBF3F04FF080434FF0FF3258 -:1053B00001389862DA6200229A615A63DA605A60B0 -:1053C00001225A611A6008BD00100240002004E06A -:1053D00040420F004FF0804208B51169D3680B407E -:1053E000D9B2C9439B07116107D5302383F31188D4 -:1053F000FFF7DAFB002383F3118808BDFFF7BEBF78 -:105400004FF08043586A70474FF080430022586342 -:105410001A610222DA6070474FF080430022DA609E -:10542000704700004FF0804358637047FEE700006C -:1054300070B51B4B01630025044686B05860856239 -:105440000E46FEF797FD04F11003C4E904334FF054 -:10545000FF33C4E90635C4E90044A560E562FFF7FF -:10546000CFFF2B460246C4E9082304F134010D4A5C -:10547000256580232046FFF799FB0123E0600A4A57 -:105480000375009272680192B268CDE90223074B5E -:105490006846CDE90435FFF7B1FB06B070BD00BF2B -:1054A00030690020885700088D5700082D540008E7 -:1054B000024AD36A1843D062704700BFC866002012 -:1054C0004B6843608B688360CB68C3600B69436142 -:1054D0004B6903628B6943620B680360704700008D -:1054E00008B5204BDA6A42F07F02DA62DA6A22F00B -:1054F0007F02DA62DA6ADA6C42F07F02DA64DA6E2C -:1055000042F07F02DA66184ADB6E11464FF0904097 -:10551000FFF7D6FF02F11C0100F58060FFF7D0FF16 -:1055200002F1380100F58060FFF7CAFF02F1540173 -:1055300000F58060FFF7C4FF02F1700100F58060A4 -:10554000FFF7BEFF02F18C0100F58060FFF7B8FFA6 -:1055500002F1A80100F58060FFF7B2FFBDE8084046 -:10556000FEF7F8BE001002409457000808B500F09E -:1055700009F8FFF7F3FABDE80840FFF7AFBE0000F7 -:105580007047000008B5FEF7DDFEFFF7E9FEFFF704 -:1055900029FAFFF7F5FFBDE80840FFF72FBF00002D -:1055A0000B460146184600F001B8000010B5054C46 -:1055B00013462CB10A4601460220AFF3008010BD0D -:1055C0002046FCE700000000024B01461868FFF788 -:1055D000C3BD00BF5023002010B5013902449042E2 -:1055E00001D1002005E0037811F8014FA34201D05A -:1055F000181B10BD0130F2E72DE9F041A3B1C91A23 -:1056000017780144044603F1FF3C8C42204601D93F -:10561000002009E00578BD4204F10104F5D10CEB4E -:105620000405D618A54201D1BDE8F08115F8018D19 -:1056300016F801EDF045F5D0E7E70000034611F854 -:10564000012B03F8012B002AF9D17047636F6D2EEF -:105650007561762D6465762E6175617600000000B7 -:1056600053544D333247343F3F00000040A2E4F131 -:10567000646891060041A3E5F265699207000000A5 -:105680004261642043414E496661636520696E64EE -:1056900065782E00000100000001FF00006400405A -:1056A000006800400000000000000000052600081F -:1056B000692000086D2D0008D9200008E5200008A9 -:1056C0003123000889220008A1200008A520000835 -:1056D0007D20000865200008ED22000889200008D0 -:1056E000312F0008D52E000895200008C12200089F -:1056F00001040E05054B02020E05054B04010E05C3 -:10570000054B05010B04044B08010603034600008A -:1057100000960000000000000000000000000000F3 -:105720000000000000000000636C6B7377630000F2 -:105730000003000000000000000100000000010163 -:1057400003000000232831010407040001000000C9 -:1057500000000000A549000891490008CD49000853 -:10576000B9490008C5490008B14900089D49000829 -:1057700089490008D94900086330000078570008BB -:1057800020670020306900206D61696E0069646CDB -:10579000650000000000812A00000000AAAAA9AA52 -:1057A00000000024FFFE000000000000009009003F -:1057B0000000A04A00000000AAAAAA6A0000500047 -:1057C000FF7F000000000000007799000000000447 -:1057D00000200000AAAAAAAA00000000FFFF000003 -:1057E00000000000000000000000000000000000B9 -:1057F000AAAAAAAA00000000FFFF00000000000003 -:10580000000000000000000000000000AAAAAAAAF0 -:1058100000000000FFFF000000000000000000008A -:105820000000000000000000AAAAAAAA00000000D0 -:10583000FFFF00000000000000000000000000006A -:1058400000000000AAAAAAAA00000000FFFF0000B2 -:1058500000000000000000007214000000000000C2 -:105860000070070000000000FE2A0100D2040000C2 -:10587000FF0000005864002000000000605600088F -:10588000006889096D8BB90200B4C40400688909F5 -:105890000068890900688909006889090068890920 -:1058A0000068890900000000542300200000000067 -:1058B00000000000000000000000000000000000E8 -:1058C00000000000000000000000000000000000D8 -:1058D00000000000000000000000000000000000C8 -:1058E00000000000000000000000000000000000B8 -:1058F00000000000000000000000000000000000A8 -:0C5900000000000000000000000000009B +:1003E000000008B0263A09B008B501F0CBF8A0F13A +:1003F00020035842584108BD07B5042101900DEB78 +:10040000010001F0DDF803B05DF804FB38B53023DE +:1004100083F31188174803680BB104F0C7FB164A31 +:10042000144800234FF47A7104F0B6FB002383F3E1 +:100430001188124C236813B12368013B2360636861 +:1004400013B16368013B63600D4D2B7833B963686A +:100450007BB9022001F06CF9322363602B78032B07 +:1004600007D163682BB9022001F062F94FF47A7367 +:10047000636038BDB82300200D040008D824002094 +:10048000D0230020084B187003280CD8DFE800F0B8 +:1004900008050208022001F041B9022001F034B938 +:1004A000024B00225A607047D0230020D82400203D +:1004B00010B501F007FC30B1234B03221A70234B17 +:1004C00000225A6010BD224B224A1C461968013195 +:1004D000F8D004339342F9D16268A242F2D31E4BA2 +:1004E0009B6803F1006303F510439A42EAD204F0DB +:1004F000DFFD04F0F1FD002001F092F80220FFF78B +:10050000C1FF164B9A6D00229A65996F9A67996F91 +:10051000D96DDA65D96FDA67D96F196E1A66D3F8B3 +:100520008010C3F88020D3F8803072B64FF0E023FB +:100530003021C3F8084DD4E9003281F311889D467B +:1005400083F308881047BDE7D0230020D82400207B +:1005500000900008209000080023002000100240B6 +:100560002DE9F04F93B0DFF8F89202902022FF219E +:100570000AA8D9F8085001F0F7F8B04A1378A3B9DF +:10058000AF48012103601170302383F311880368A1 +:100590000BB104F00BFBAB4AA94800234FF47A716E +:1005A00004F0FAFA002383F31188029B13B1A64BDF +:1005B000029A1A60A54A029C1378032B1EBF0023DF +:1005C0001370A14A4FF0000B18BF5360CDF804B070 +:1005D0005E46DA46012001F0A1F824B19A4B1B686F +:1005E000002B00F0C581002000F0CCFF071E80F238 +:1005F000F58100F059FEF0E70220FFF7F5FE002834 +:1006000040F0E381029B9248BBF1000F08BF1C46FB +:10061000022140E04FF47A7000F0B4FF071EF1DBD6 +:100620000220FFF7E1FE0028ECD0013F052F00F289 +:10063000CE81DFE807F003070A0D10330520FFF72E +:10064000DBFE14E0D9F80000F9E7D9F80400F6E77A +:10065000D9F80800F3E74FF01C08404600F0E2FF2D +:1006600008F10408FFF7C8FEB8F12C0FF5D1019B83 +:10067000012000FA07F71F43FBB201934FF0000A75 +:1006800001F0BEF82EB1019B03F00B030B2B08BF4A +:1006900000247048022100F093FF9BE7D9F80C007A +:1006A000CDE7002EAED0019B03F00B030B2BA9D19D +:1006B0000220FFF799FE07460028A3D0012000F092 +:1006C000AFFF0220FFF7DEFE00261FFA86F8404645 +:1006D00000F0B6FF044670B10021404600F0C8FFAC +:1006E00001360028F1D1BB4604465B48022100F0E8 +:1006F00067FF3E466EE70120FFF7C4FE2546D9F8A6 +:100700000830AB4207D9284600F08CFF013040F09A +:1007100061810435F3E74D4B00251D704A4BBB4604 +:100720005D603E46ACE7002E3FF46CAF019B03F0EA +:100730000B030B2B7FF466AF0220FFF7A3FE3220E2 +:1007400000F020FFB0F10008FFF65CAF18F00307DF +:100750007FF458AFD9F8082008EB050393423FF621 +:1007600051AFB8F5807F3FF74DAF3C4B0393B84591 +:100770000BDD4FF47A7000F005FF0028FFF642AF62 +:10078000039B013703F8010BF0E7C820FFF72CFEAD +:10079000074600283FF436AF1F2D11D8C5F12002BF +:1007A00042450AAB25F0030028BF42462B4903927D +:1007B000184400F0C7FF039A2848FF2100F0D4FF37 +:1007C0004FEAA80325490393C8F38702284600F09F +:1007D000D3FF0646002888D0039B05EB83054FE72F +:1007E0000220FFF701FE00283FF40CAF00F052FF9B +:1007F00000283FF407AF0027B846D9F80830BB42BD +:1008000018D91F2F11D80A9B01330ED027F00303EC +:1008100012AA134453F8203C05934046042205A92C +:1008200001F03AFA04378046E7E7384600F0FAFE6E +:100830000590F2E74046FFF7DFFD21E7D4240020D2 +:10084000B82300200D040008D8240020D023002065 +:1008500050540008485400084C540008D423002089 +:10086000002300200023642105A8059300F09EFECC +:1008700000287FF4C7AE0220FFF7B6FD00283FF442 +:10088000C1AE059800F0FCFED5E70023642105A861 +:10089000059300F08BFE00287FF4B4AE0220FFF732 +:1008A000A3FD00283FF4AEAE059800F0EBFEC2E7D2 +:1008B0000220FFF799FD00283FF4A4AE00F0FAFEF5 +:1008C000B9E70220FFF790FD00283FF49BAE05A991 +:1008D000142000F0F5FE0390FFF78EFD039905A8A4 +:1008E00000F06EFECCE6322000F04CFE071EFFF654 +:1008F00089AEBB077FF486AED9F8082007EB0A0360 +:1009000093423FF67FAE0220FFF76EFD00283FF4D2 +:1009100079AE27F003075744BA453FF4B1AE5046CD +:1009200000F080FEFFF768FD0AF1040AF4E74FF4D7 +:100930007A70FFF759FD00283FF464AE00F0AAFE7C +:10094000002849D00A9B01330BD008220AA90020B5 +:1009500000F012FF00283FD02022FF210AA800F05B +:1009600003FF3B48022100F02BFE3A4804F066F8F2 +:1009700013B0BDE8F08F002E3FF444AE019B03F0AE +:100980000B030B2B7FF43EAE0023642105A80593D7 +:1009900000F00CFE074600287FF434AE0220FFF77B +:1009A00023FD804600283FF42DAE0221284800F0A8 +:1009B00007FE41F2883004F041F8059800F036FF58 +:1009C000464600F01DFF3C4604E6064658E64FF05A +:1009D000000A17E6BB4688E6374686E6012000F0A7 +:1009E00093FEA7F1210016283FF6F4AD01A353F8BA +:1009F00020F000BFF905000815060008A30600084E +:100A0000D5050008D5050008D5050008270700080A +:100A1000E7080008E1070008650800088B080008DF +:100A2000B1080008D5050008C3080008D50500086E +:100A30002F09000881060008D5050008770900087D +:100A40000506000881060008D505000865080008AD +:100A500048540008A0860100094A136849F2690059 +:100A600099B21B0C00FB01331360064B186844F26B +:100A7000506182B2000C01FB0200186080B2704726 +:100A8000142300201023002010B50021102204465A +:100A900000F06AFE034B03CB206061601868A06021 +:100AA00010BD00BF9075FF1FF0B5ADF54D7D6AAC70 +:100AB0004FF4C47207460D462046002100F054FE54 +:100AC00002F0F0F9244B4FF47A72B0FBF2F01860A8 +:100AD00093E80700022384E807000DF5E570238200 +:100AE000FFF7D2FF47F214231C49238406A804F021 +:100AF000A3FC102384F832310DF2DB2606AB0DF196 +:100B0000280C1A4603CA624530607160134606F12C +:100B10000806F6D10BAE31460122204600F0A0FEB9 +:100B200000230393AB7E029305F11903019380B276 +:100B30000123CDE904600093E97E05A3D3E90023F6 +:100B4000384602F0C3FD0DF54D7DF0BDAFF30080DA +:100B50009E6AC421818A46EEE02400205454000895 +:100B60002DE9F0412C4C237ADAB080460D465BBB70 +:100B700027A9284600F09AFF0746002842D19DF891 +:100B80009D60C82E3ED801464FF4A662204600F074 +:100B9000EBFD4FF48073C4F8F8314FF40073C4F8E0 +:100BA0000C334FF44073C4F8203432460DF19E01EB +:100BB00004F1090000F0C6FD26449DF89C307772D0 +:100BC00023720BB9EB7E23728122002106AC27A889 +:100BD00000F0CAFD0122214627A800F0ADFF002346 +:100BE0000393AB7E029305F1190380B2019328238E +:100BF000CDE904400093E97E05A3D3E900234046F4 +:100C000002F064FD5AB0BDE8F08100BFAFF3008090 +:100C100026417272DF25D7B728350020F0B5254E62 +:100C20004FF48A7505FB0065F1B096F8D83085F869 +:100C3000DC300024D822214685F8E8403AA800F0AC +:100C400093FD06F1090000F087FDD5F8E4308DF83A +:100C5000F000C2B206AF06F109010DF1F100CDE9D5 +:100C60003A3400F06FFD394601223AA800F09AFFAD +:100C700080B2CDE9047008230127CDE9023706F1DF +:100C8000D803019330230093317A0B4807A3D3E9AB +:100C9000002302F01BFDA04206DD02F003F9C5F8B7 +:100CA000E000384671B0F0BD2046FBE778F6339F90 +:100CB00093CACD8D28350020F83400202DE9F04F5F +:100CC000274FDFF8A880274E87B0384602F02AFD6C +:100CD000034600283DD00024CDE90344ADF814407C +:100CE000027B8DF8142099684068029403AA03C21D +:100CF0001B681D4D43F000430293A146A2462B689A +:100D0000D3F810B002F0D0F810EB080241F1000364 +:100D10002846CDF800A002A9D84704F2345440F682 +:100D200068230028C8BF49F0010905F5A6559C4273 +:100D300005F11005E3D1B9F1000F05D0384602F0F6 +:100D4000F5FC86F800A0C0E73378072B04D8013300 +:100D5000337007B0BDE8F08F014802F0E7FCF8E718 +:100D6000F83400205D3A0020603A002040420F0035 +:100D700070B50D4614461E4602F004FC50B9022E12 +:100D800010D1012C0ED112A3D3E90023C5E9002311 +:100D9000012007E0282C10D005D8012C09D0052C03 +:100DA0000FD0002070BD302CFBD10BA3D3E9002362 +:100DB000ECE70BA3D3E90023E8E70BA3D3E9002377 +:100DC000E4E70BA3D3E90023E0E700BFAFF3008023 +:100DD000401DA12026812A0B78F6339F93CACD8D22 +:100DE0009E6AC421818A46EE26417272DF25D7B7FA +:100DF000F017304A39059E5638B505460E4C00218D +:100E0000013500F035FCA4F82C55B4F82C0500F0A1 +:100E100017FC78B1B4F82C0500F022FC014648B963 +:100E2000B4F82C0500F024FCB4F82C350133A4F8F8 +:100E30002C35EAE738BD00BF28350020F8B50E4C48 +:100E40000E4F0226A4F5805343F8407C237E3BB925 +:100E500065692DB1284600F0B3FF284601F0EAF895 +:100E6000204600F0ADFFA4F5A654012EA4F1100415 +:100E700000D1F8BD0126E5E7705F0020B4540008FA +:100E80002DE9F04F8FB000AF05460C4602F07AFB1B +:100E900000284BD1237E022B1BD1E38A012B18D1D2 +:100EA00002F000F80646FFF7D7FD03464FF4C8707E +:100EB000DFF8D482B3FBF0F206F5167602FB1033AE +:100EC00016FA83F3C8F80030E37E33B9A74B00224B +:100ED0001A703C37BD46BDE8F08F07F1240120466B +:100EE00000F098FD0028F4D107F11400FFF7CCFDC5 +:100EF00097F8264007F11401224607F1270004F075 +:100F000069FA0028E2D10F2C08D8984B1C70D8F849 +:100F10000030A3F51673C8F80030DAE797F824100C +:100F20000029D6D0284602F025FBD2E7E38A282BF9 +:100F300033D010D8012B2BD0052BCAD1BFF34F8F44 +:100F40008B498C4BCA6802F4E0621343CB60BFF359 +:100F50004F8F00BFFDE7302BBBD1874EE17E327A49 +:100F60009142B6D1607E3146002291F8DC50854234 +:100F700000F0AD800132042A01F58A71F5D13268A2 +:100F800040F6B831FA328A4294BF32603160A0E74D +:100F900021462846FFF788FD9BE721462846FFF7B4 +:100FA000DFFD96E7B2F8EC507B6005F103094FEAEC +:100FB00099094FEA8902D11DC908A8EBC1039D46D2 +:100FC000EB460021584600F0CFFB04F1EE012A4623 +:100FD0003144584600F0B6FB7B6813B9012000F09D +:100FE0001FFB96F8D20000F02BFB044630B930729C +:100FF00000F050FB204600F013FBB0E0D6F8D42000 +:101000003AB996F8D200B6F82C25824201D8FFF7FB +:10101000F3FED6F8D4202A44944208D296F8D2009F +:10102000B6F82C250130824201D8FFF7E5FE706842 +:101030005FFA89F2594600F09FFB08B9C54679E08E +:10104000726896F8D2002A447260D6F8D42005EB74 +:101050000209C6F8D49000F0F3FA814509D396F856 +:10106000D220D6F8D4000132001B86F8D220C6F870 +:10107000D400FF2D0FD80024347200F00BFB204663 +:1010800000F0CEFA00F01EFE3C4B188108B9FFF7C5 +:101090000FFAC5461DE7BB6896F8D9000AFB036244 +:1010A000FB68D2F8E41082F8E83001F58061C2F8FC +:1010B000E030C2F8E410FFF7B1FDFFF7FFFD96F84E +:1010C000D920013202F0030286F8D920B6E74FF4A6 +:1010D0008A7A0AFB02F505F1EA013144204600F064 +:1010E00095FDF86000287FF4F4AE3544012285F8C0 +:1010F000E82001F0D7FED5F8E020D6ED007ADFED4C +:10110000206A801A192838BF192040F6B832904258 +:1011100028BF1046B8EE677A07EE900AF8EEE77A35 +:1011200067EEA67ADFED176AE7EE267AFCEEE77A3D +:10113000C6ED007A96F8D9307168BB600AFB03F4FB +:10114000321992F8E8505DB1D2F8E4308B42E846AB +:101150003FF428AF002182F8E810C2F8E010C5463D +:101160007368064A9B0A01331381B2E6F13400200A +:1011700000ED00E00400FA0528350020E0240020FE +:10118000CDCCCC3D6666663FF4340020014B187030 +:10119000704700BFEC24002038B54FF00054144BCA +:1011A00022689A4221D1607DF0B1124B1249187029 +:1011B0001248237D03724FF48073C0F8F8314FF466 +:1011C0000073C0F80C3300254FF44073C0F820348E +:1011D000C0F8E450C922093000F0B4FAE0222946F0 +:1011E000204600F0C1FA012038BD0020FCE700BF16 +:1011F0009AAD44C5EC2400201600002028350020BC +:1012000037B500F05FFD1F4C1F4D20492881022398 +:101210006B712368204604F580545B68012298476F +:10122000D4F8D03419495B68012204F59A609847D4 +:1012300000230193164B174900931748174B4FF49F +:10124000805202F06FF9164B197811B1124802F072 +:1012500091F901F027FE0446FFF7FEFB4FF4C87337 +:10126000B0FBF3F202FB130304F5167010FA83F0DF +:101270000C4B186003F052FF08B10F232B8103B011 +:1012800030BD00BF603A0020E024002040420F0043 +:10129000710D0008F0240020F8340020810E0008B1 +:1012A000EC240020F43400202DE9F04F2DED028BCA +:1012B000002593B00DF12C089FED838BDFF844924D +:1012C000FFF7FCFC0A95ADF834500B95C8F80450B4 +:1012D0000026834CDFF80CB2374601238DF81C3012 +:1012E00023688DF81D508DED008B0DF11D02D3F894 +:1012F00008A007A900232046D0479DF81CA0BAF1FA +:10130000000F24D0D9F8143083F40053C9F81430F6 +:10131000102200210EA800F027FA236808AA5F69AE +:101320000AA90DF11E032046B84798E803000FAB49 +:1013300083E803009DF834308DF844300A9B0E9307 +:101340000EA9DDE90823584602F0CEFB574606F207 +:10135000345640F6682304F5A6549E4204F1100466 +:10136000BBD1002FB4D15F4802F00CF900283FD167 +:101370005D4E01F097FD3368984239D301F092FD3C +:101380000446FFF769FB4FF4C873B0FBF3F202FBAE +:10139000130304F5167010FA83F03060534E8DF885 +:1013A0002870377817B901238DF82830C7F1100459 +:1013B000E4B20EA8FFF768FB062C28BF06240EAB8C +:1013C0002246D9190DF1290000F0BCF90AAB0393AC +:1013D000182302930134464B0193E4B20123009396 +:1013E000404804943AA3D3E9002302F011F9357080 +:1013F00001F058FD3F4A404C1368C31AB3F57A7F99 +:101400002FD3106001F050FD02460B46354802F024 +:1014100097F9344802F0B6F818B3237A374E002B08 +:1014200014BF03230223737101F03CFD0EAF4FF490 +:101430007A730122B0FBF3F039463060304600F099 +:10144000C1FA182302932E4B019380B240F2551338 +:10145000CDE90370009323481FA3D3E9002302F0D2 +:10146000D7F8237A93B101F01DFD002607464FF40B +:101470008A7894F8D900304400F0030008FB004358 +:1014800093F8E82072B10136042EF2D1C82003F09F +:10149000D5FA237A002B7FF40DAF13B0BDEC028B8D +:1014A000BDE8F08FD3F8E02042B12368FA2B38BFB3 +:1014B000FA23BA1A0533B2EB430FE4D3FFF7AEFBBE +:1014C0000028E0D1E2E700BF0000000000000000BB +:1014D000401DA12026812A0BF1C6A7C1D068080FA4 +:1014E000603A0020F8340020F4340020F134002069 +:1014F000F0340020583A002028350020E024002055 +:101500005C3A00200008004808B5064800F0A2FF39 +:10151000054800F09FFFBDE80840044A0449002048 +:1015200003F03CBF603A0020304F0020546400209C +:101530003D0E000870B5104B1B780133DBB2012B58 +:101540000C4612D80D4B1D6829684FF47A730E6A49 +:10155000A2FB0332014622462846B047844204D10A +:10156000074B00221A70012070BD4FF4FA7003F08F +:1015700065FA0020F8E700BF182300201C23002094 +:101580004464002007B50023024601210DF1070045 +:101590008DF80730FFF7CEFF20B19DF8070003B0AC +:1015A0005DF804FB4FF0FF30F9E700000A4608B58C +:1015B0000421FFF7BFFF80F00100C0B2404208BD28 +:1015C00030B4074B0A461978064B53F8214023687C +:1015D000DD69054B0146AC46204630BC604700BF84 +:1015E000446400201C230020A086010070B503F095 +:1015F000DDFB094E094D3080002428683388834282 +:1016000008D903F0CFFB2B6804440133B4F5104F25 +:101610002B60F2D370BD00BF466400200064002040 +:1016200003F07EBC00F1006000F5104000687047D8 +:1016300000F10060920000F5104003F0FBBB0000D9 +:10164000054B1A68054B1B889B1A834202D910442C +:1016500003F0A8BB0020704700640020466400200F +:10166000024B1B68184403F0A5BB00BF00640020B8 +:10167000024B1B68184403F0AFBB00BF006400209E +:101680000020704700F10050A0F51040D0F8900500 +:1016900070470000064991F8243033B10023086AEE +:1016A00081F824300822FFF7C3BF0120704700BF34 +:1016B00004640020014B1868704700BF002004E05C +:1016C00030B50F4B0F4C1B682288C3F30B03013856 +:1016D000934208440BD164680A46013C8242134697 +:1016E0000BD214F9015F2DB102F8015BF6E78142DC +:1016F0000B4602D22C2203F8012B581A30BD00BF32 +:10170000002004E020230020022802BF024B4FF0FB +:1017100000529A61704700BF00080048022802BFCB +:10172000024B4FF400529A61704700BF0008004816 +:10173000022801BF024A536983F400535361704782 +:101740000008004810B50023934203D0CC5CC45479 +:101750000133F9E710BD000003460246D01A12F922 +:10176000011B0029FAD1704702440346934202D07C +:1017700003F8011BFAE770472DE9F8431F4D1446A3 +:1017800095F824200746884652BBDFF870909CB33A +:1017900095F824302BB92022FF2148462F62FFF70D +:1017A000E3FF95F82400C0F10802A24228BF2246B8 +:1017B000D6B24146920005EB8000FFF7C3FF95F8D3 +:1017C0002430A41B1E44F6B2082E17449044E4B201 +:1017D00085F82460DBD1FFF75DFF0028D7D108E052 +:1017E0002B6A03EB82038342CFD0FFF753FF00281D +:1017F000CBD10020BDE8F8830120FBE70464002082 +:10180000024B1A78024B1A70704700BF44640020E4 +:101810001823002003494FF4E1330B60024B186892 +:1018200002F0F8BF2C6400201C230020094B10B5E7 +:101830001822044600211846FFF796FF064A074B78 +:10184000127804600146BDE8104053F8220002F00F +:10185000E1BF00BF2C640020446400201C23002052 +:101860002DE9F3470D46044600219046284640F2F4 +:101870007912FFF779FF234620220021284602F043 +:10188000D5F8231D02222021284602F0CFF8631D3F +:1018900003222221284602F0C9F8A31D0322252194 +:1018A000284602F0C3F804F1080310222821284634 +:1018B00002F0BCF804F1100308223821284602F097 +:1018C000B5F804F1110308224021284602F0AEF8D1 +:1018D00004F1120308224821284602F0A7F804F177 +:1018E000140320225021284602F0A0F804F1180326 +:1018F00040227021284602F099F804F120030822C2 +:10190000B021284602F092F804F121030822B82100 +:10191000284602F08BF804F12207C0263B463146E8 +:1019200008222846083602F081F8B6F5A07F07F1B4 +:101930000107F3D194F832308DF8073031460DF1BC +:1019400007030822284602F071F8002604F1330A42 +:101950009DF807304FEAC6099E4209F5A47720D3C7 +:1019600094F83231502B28BF50238DF80730B8F14E +:10197000000F08D139460DF107030722284602F06F +:1019800055F809F24F17002604F233149DF807307A +:101990009E4207EBC6010DD30731C80802B0BDE86F +:1019A000F0870AEB060308223946284602F03EF883 +:1019B0000136CDE7A3190822284602F037F8013690 +:1019C000E4E7000013B50446084600210160234601 +:1019D000C0F803102022019002F028F80198231D7E +:1019E0000222202102F022F80198631D0322222105 +:1019F00002F01CF80198A31D0322252102F016F81D +:101A0000019804F108031022282102F00FF80720A2 +:101A100002B010BDF7B5047F05460E462CB1838A8F +:101A2000122B02D9012003B0F0BD00231946072272 +:101A30000096284601F0BEFE731C0093012200238D +:101A40000721284601F0B6FED4B9B31C0093052245 +:101A500023460821284601F0ADFE0D24B378102B53 +:101A6000E0D83746B278BB1B934210D32B7FA88AAD +:101A70000734E408B3B9844294BF00200120D2E7C0 +:101A8000AB8ADB00083BDB08B3700824E6E7FB1CED +:101A90000093214600230822284601F08BFE0834DB +:101AA0000137DFE7201A18BF0120BCE7F7B5047F34 +:101AB00005460E462CB1838ACA2B02D9012003B0F9 +:101AC000F0BD0023194600960822284601F072FE58 +:101AD000731CD4B90822009311462346284601F00E +:101AE00069FE10247378C82BE8D8012372785F1C34 +:101AF000013B934210D32B7FA88A0734E408B3B983 +:101B0000844294BF00200120D9E7AB8ADB00083B68 +:101B1000DB0873700824E5E7F319009321460023DE +:101B20000822284601F046FE08343B46DEE7201A2C +:101B300018BF0120C3E70000F7B50D460446002199 +:101B4000164628468122FFF70FFE23460822002171 +:101B5000284601F06BFF94F901206378002AB8BF92 +:101B60007F238DF807309EB90DF107030722082166 +:101B7000284601F05BFF0F27002602349DF807304E +:101B80009E4207EBC60105D30731C80803B0F0BD7C +:101B90000827F1E7A3190822284601F047FF01367C +:101BA000ECE70000F7B50E46054600211446304626 +:101BB000CE22FFF7D9FD2B4628220021304601F026 +:101BC00035FF2B7AC82B28BFC8238DF807308CB976 +:101BD0000DF1070308222821304601F027FF3024A9 +:101BE0002F469DF807207B1B934205D3E01DC008BC +:101BF00003B0F0BD2824F3E707F1090321460822CA +:101C0000304601F013FF08340137EAE7F7B5047FE7 +:101C100005460E4634B1838AB3F5827F02D901208E +:101C200003B0F0BD0096012310220021284601F0E8 +:101C3000C1FDDCB9B31C00930922234610212846BC +:101C400001F0B8FD19247388B3F5807FE7D83746D3 +:101C50007288BB1B934210D32B7FA88A0734E408F9 +:101C6000B3B9844294BF00200120D9E7AB8ADB00DE +:101C7000103BDB0873801024E5E73B1D00932146F1 +:101C800000230822284601F095FD08340137DFE7DC +:101C9000201A18BF0120C3E730B5094D0A4491420C +:101CA0000DD011F8013B5840082340F30004013BDC +:101CB0002C4013F0FF0384EA5000F6D1EFE730BD6B +:101CC0002083B8EDF7B5384A106851686B4603C3F6 +:101CD0006A4636493648082303F08CFB0446002840 +:101CE00033D10A25334A106851686B4603C36A46EC +:101CF00031492F48082303F07DFB0446002849D0D2 +:101D00000369B3F5EE2F45D8B0F8661041F2724280 +:101D100091423FD1294A024402F15C018B4239D3FE +:101D20005C3B234900209E1AFFF7B6FF3246074668 +:101D300004F164010020FFF7AFFFA3689F4229D19F +:101D4000E368984208BF002524E00369B3F5EE2F4D +:101D500027D8418B41F27242914220D1174A024466 +:101D600002F110018B4218D3103B114900209D1A3B +:101D7000FFF792FF2A46064604F118010020FFF7FC +:101D80008BFFA3689E4202D1E368984201D00D25E3 +:101D9000A8E70025284603B0F0BD1025A2E70C25D2 +:101DA000A0E70B259EE700BF74540008DC6F070016 +:101DB000009000087D540008906F07000870FFF73E +:101DC00010B5037C044613B9006803F0FBFA204603 +:101DD00010BD00000023BFF35B8FC360BFF35B8FB8 +:101DE000BFF35B8F8360BFF35B8F7047BFF35B8F85 +:101DF0000068BFF35B8F704770B505460C30FFF786 +:101E0000F5FF05F1080604463046FFF7EFFFA04254 +:101E100006D930466D68FFF7E9FF2544281A70BDE2 +:101E20003046FFF7E3FF201AF9E7000070B50546DA +:101E3000406898B105F10800FFF7D8FF05F10C06DE +:101E400004463046FFF7D2FF8442304694BF6D68A7 +:101E50000025FFF7CBFF013C2C44201A70BD000089 +:101E600038B50C460546FFF7C7FFA04210D305F171 +:101E70000800FFF7BBFF04446868B4FBF0F100FB07 +:101E80001144BFF35B8F0120AC60BFF35B8F38BDA3 +:101E90000020FCE72DE9F041144607460D46FFF708 +:101EA000C5FF844228BF0446D4B1B84658F80C6B2D +:101EB0004046FFF79BFF3044286040467E68FFF7AE +:101EC00095FF331A9C4203D86C600120BDE8F08175 +:101ED0006B60A41B3B68AB602044E8600220F5E720 +:101EE0002046F3E738B50C460546FFF79FFFA042B2 +:101EF00010D305F10C00FFF779FF04446868B4FBC8 +:101F0000F0F100FB1144BFF35B8F0120EC60BFF3E5 +:101F10005B8F38BD0020FCE72DE9FF41884669460C +:101F20000746FFF7B7FF6C4606B204EBC60600256E +:101F3000B44209D06268206808EB0501FFF702FC93 +:101F4000636808341D44F3E729463846FFF7CAFFA3 +:101F5000284604B0BDE8F081F8B505460C300F46C0 +:101F6000FFF744FF05F1080604463046FFF73EFF41 +:101F7000A042304688BF6C68FFF738FF201A3860EF +:101F800020B130462C68FFF731FF2044F8BD000037 +:101F900073B5144606460D46FFF72EFF844228BF50 +:101FA00004460190DCB101A93046FFF7D5FF019B43 +:101FB00033B93268C5E90233C5E9002401200CE0D9 +:101FC0009C4238BF01942860019868608442F5D92A +:101FD0003368AB60241AEC60022002B070BD20466A +:101FE000FBE700002DE9FF410F466946FFF7D0FFF0 +:101FF0006C4600B204EBC0050026AC4209D0D4F810 +:10200000048054F8081BB8194246FFF79BFB46446E +:10201000F3E7304604B0BDE8F081000038B505466E +:10202000FFF7E0FF044601462846FFF719FF204668 +:1020300038BD000008B103F0C5B970477047000013 +:1020400010B41346026814680022A4465DF8044BDD +:102050006047000000F5805090F869047047000068 +:1020600000F5805090F862047047000000F5805041 +:1020700090F968047047000050207047302383F3C4 +:10208000118800F58052D2F8AC34D2F8A804184474 +:10209000D2F8A4341844D2F88C341844D2F89C34C2 +:1020A0001844D2F898341844002383F311887047F9 +:1020B00000F58050C0F86414012070472DE9FF47F7 +:1020C0000C4600F58051054691F86204BDF8308059 +:1020D00098B1D1F888040130C1F8880420688600DE +:1020E00006D4207B08280BD9667B0EB10F2807D9B0 +:1020F000D1F88C340133C1F88C344FF0FF30AAE0B2 +:10210000302080F31188E86BD0F8C47017F4001702 +:1021100013D0D1F890040130C1F89004D1F8941490 +:10212000002940F09B80CDF800802146284600F031 +:1021300009FF002383F311888DE0D0F8C460686B39 +:10214000C6F301464FF0480919FB0609206800282C +:10215000B4BF40F0804C4FEA804CC9F800C0206802 +:10216000400044BF4CF00050C9F8000094F80CC087 +:10217000300640EA0C40C9F8040094F80DC04FEA5C +:10218000461ABCF1000F27D040F44010C9F80400F3 +:10219000D1F8C0040130C1F8C00405EB0A0101F513 +:1021A0008351087F40F020000877207BCDE902238F +:1021B00000F01CFFDDE9022303308010F9B28142F8 +:1021C00007F1010710DA04EB810C09EB8101DCF85F +:1021D00004C0C1F808C0F1E705EB0A0101F583511D +:1021E000087F6CF34510DFE7E96B0120B040C1F8D0 +:1021F000CC0006F1830105EB4111C1E9002305EB99 +:10220000461101F583512046083104F10C0750F8BE +:1022100004CB41F804CBB842F9D100880880AA4425 +:1022200041F278014AF801600AF5805A08F003018A +:102230009AF87C0041F0100120F01F0001438AF859 +:102240007C10002181F31188CDF8008021462846BA +:1022500000F078FE012004B0BDE8F087002068E7B8 +:1022600038B5C26B936923F001039361044600F013 +:1022700019FE0546E36B9B69DB0706D500F012FEED +:10228000431BFA2BF6D9002004E004F5805401200A +:1022900084F8620438BD000013B500F58054019144 +:1022A000606DFFF7C3FD1F280AD90199606D2022D8 +:1022B000FFF732FEA0F120035842584102B010BD92 +:1022C0000020FBE708B5302383F3118800F5805028 +:1022D000406DFFF77FFD002383F3118808BD0000E8 +:1022E00000220260828142608260704710B5002245 +:1022F0000023C0E900230023044603810C30FFF7CC +:10230000EFFF204610BD00002DE9F0479A4688B047 +:10231000074688469146302383F3118807F5805499 +:102320006846FFF7E3FF606DFFF766FD1F282DD9B4 +:10233000606D20226946FFF771FE202826D194F8AF +:1023400062341BB303AD444605AB2E4603CE9E421A +:1023500020606160354604F10804F6D130682060E1 +:10236000B388A380DDE90023C9E90023BDF8083064 +:10237000AAF80030002383F3118853464A464146A9 +:10238000384608B0BDE8F04700F09ABD002080F361 +:10239000118808B0BDE8F0872DE9F84F06463546AC +:1023A0008846182200210430FFF7DEF9264B45F855 +:1023B000403B06F582572C4620462034FFF796FF17 +:1023C000A742F9D106F580544FF4805325640025C7 +:1023D000C4E9113501236765E56484F8503084F859 +:1023E000583006F5845706F5A4594FF0000A4FF00F +:1023F000000B47E908ABA7F11800FFF771FF203782 +:1024000047F8285C4F45F4D1B8F1010F84F868848F +:10241000A4F86A54A4F86C54A4F86E5484F8705468 +:10242000A4F87254A4F87454A4F8765484F8785438 +:1024300002D9064800F030FD054B53F82830F3630D +:102440003046BDE8F88F00BFB45400088854000837 +:10245000A454000810B5044B197804464A1C1A709D +:10246000FFF79AFF204610BD516400202DE9F84384 +:1024700015460C4600295CD0022001F0DDFF2E49F4 +:10248000B0FBF4F78C428CBF0A2111214B1EB7FB25 +:10249000F1F601FB1671DAB221B1022B1946F5D81B +:1024A000002032E0731EB3F5806FF9D2C2EBC20890 +:1024B00008F103014FEAE10EC1F3C701A2EB010CE1 +:1024C0000EF101094FF47A735FFA8CF70EFB033EAD +:1024D00059FA8CFCBEFBFCFCBCF5617F17DC1FFAD3 +:1024E0008CF34A1C57FA82F27243B0FBF2F084423A +:1024F000D6D14A1E0F2AD3D87A1E072AD0D8012057 +:102500002B806E8028716971AF71BDE8F88308F186 +:10251000FF314FEAE10CC1F3C701521A0CF1010E71 +:10252000D7B20CFB03335EFA82F2B3FBF2F39BB239 +:10253000D7E70846E9E700BF3F420F0030B50D4B33 +:102540000D4D05201C786C438C420ED1597851807A +:1025500099785171D9789171197911715B7903EB7F +:1025600083035B001380012030BD013803F10603B3 +:10257000E8D1F9E7F854000840420F0038B500F5FB +:102580008053114A93F86834D55C4FF45472554324 +:1025900005F1804303F52443044600211846FFF764 +:1025A000E3F80A4B60622B44A362094B2B44E362BD +:1025B000084B2B442363084B2B446363E36B0022DB +:1025C000C3F8C02038BD00BF9C54000870A4004070 +:1025D000B0A4004088A5004078A600402DE9F0474F +:1025E00000F580551F4695F86834022B88B00446E4 +:1025F0008946904604D90026304608B0BDE8F087E9 +:10260000A54A52F8231009B942F82300A348C4F898 +:102610001C90017884F8207099B9302383F31188D5 +:102620009F4B9A6D42F000729A659A6B42F000726D +:102630009A639A6B22F000729A630123037081F30C +:10264000118895F86134CBB9302383F31188954A0A +:1026500095F86834D35C012B2AD0022B2FD03BB9DC +:102660000321152001F020FF0321162001F01CFF9B +:10267000012385F86134002383F31188302383F329 +:102680001188E26B936923F01003936100F00AFC58 +:102690008246E36B9E6916F0080617D000F002FC34 +:1026A000A0EB0A03FA2BF4D9002686F31188A3E7DE +:1026B0000321562001F0F8FE03215720D6E703211D +:1026C000582001F0F1FE03215920CFE79A6942F02A +:1026D00001029A6100F0E6FB8246E36B9A69D0073B +:1026E00006D400F0DFFBA0EB0A03FA2BF5D9DBE7F9 +:1026F0009A6942F002029A61E36B4FF0000AC3F854 +:1027000054A08AF31188686DFFF764FB2022002132 +:102710006846FFF729F802A8FFF7E2FD04F58353A6 +:10272000CDF818A06A4633440DF1180E9446BCE863 +:102730000300F44518605960624603F10803F5D1BF +:10274000DCF80000186020369CF804201A71B6F5F9 +:10275000806FDBD1002304F5A35285F8603485F83F +:1027600063340A3249462046FFF780FE064690B998 +:10277000E26B936923F00103936100F093FB05463C +:10278000E36B9B69D9077FF536AF00F08BFB431BEA +:10279000FA2BF5D92FE795F86E34C5F87C94591EBD +:1027A00095F86F34E26B013B1B0243EA416395F8F5 +:1027B000701401390B43B5F86C14013943EA014335 +:1027C000D361B8F1000F36D004F5A352123241465E +:1027D0002046FFF7B3FE90B9E26B936923F0010343 +:1027E000936100F05FFB0546E36B9B69DA077FF5B9 +:1027F00002AF00F057FB431BFA2BF5D9FBE695F827 +:102800007724C5F88084511E95F87824E36B013A4B +:10281000120142EA012295F8761401390A43B5F80B +:102820007414013942EA014242F40002DA60E36BB7 +:102830004FF420629A642046FFF7A0FE002385F83B +:102840006934E36B6FF040421A65E36B164A5A65D0 +:10285000E36B44229A65E36B0722C3F8DC20E36B49 +:1028600003229742DA653FF4C7AEE26B936923F027 +:102870000103936100F016FB0746E36B9B69DB07DE +:1028800005D500F00FFBC31BFA2BF6D9B3E60123E5 +:1028900085F86234B0E600BF486400205064002030 +:1028A000001002409C5400089B0008002DE9F04FE6 +:1028B00000F5A5578BB00546904699464FF0000BA2 +:1028C00041F2780A1037EB6BD3F8D43023FA0BF3CC +:1028D000DC0753D505EB4B1151444FEA4B120B79F2 +:1028E00018074BD405F58056D6F894340133C6F852 +:1028F0009434C7E900890B79990648BFD6F8C434E7 +:1029000005EB020148BF0133514448BFC6F8C43447 +:102910000B7943F008030B71DB0724D596F8633479 +:102920000BB302A8019205F58354FFF7DFFC019A6F +:102930000834144405AB04F1080C206861681A4699 +:1029400003C2083464451346F7D120681060A2889A +:102950009A800123ADF810302B68CDE902891B6CF9 +:1029600002A928469847D6F8B834D6F8640401334B +:10297000C6F8B83410B103681B6998470BF1010B16 +:10298000BBF1200F9FD10BB0BDE8F08F2DE9F04FC8 +:102990008DB004460F4600F087FA82468946002F24 +:1029A00056D1E36BD3F89020920141BF04F58051DA +:1029B000D1F8A8240132C1F8A824D3F89020160732 +:1029C00003D100200DB0BDE8F08FD3F89050E66A37 +:1029D000C5F30125482303FB0566E8464046FFF79B +:1029E0007FFC326851004ABF22F06043C2F38A4341 +:1029F00043F00043920048BF43F080430093736864 +:102A000013F400131FBF04F5805201238DF80D301D +:102A1000D2F8C8340EBF8DF80D300133C2F8C83477 +:102A2000F38803F00F038DF80C304FF0000B9DF886 +:102A30000C0000F0DBFA5FFA8BF3984220D9F21811 +:102A40000CA90B44127A03F82C2C0BF1010BEEE7C6 +:102A5000012FB6D1E36BD3F89820950141BF04F55F +:102A60008051D1F8A8240132C1F8A824D3F89820C5 +:102A70001007A6D0D3F89850266BC5F30125A9E717 +:102A8000EFB9E36BC3F8945004A8FFF72FFC98E864 +:102A90000F0007AD07C52B800023ADF81830236861 +:102AA0002046CDE904A91B6C04A9984704F580547D +:102AB00058B1D4F8A0340133C4F8A03482E7012F10 +:102AC00004BFE36BC3F89C50DEE7D4F8A4340133B1 +:102AD000C4F8A434012075E72DE9F04105460F46FE +:102AE00000F58054012639462846FFF74FFF10B104 +:102AF00084F86364F7E7D4F8B834D4F86404013395 +:102B0000C4F8B83420B10368BDE8F0411B69184728 +:102B1000BDE8F081F0B5C36B1A6C12F47F0F2BD0B7 +:102B200000F580541B6CC4F8BC3441F278050023D6 +:102B300047194FF0010C00EB43122A445E01117952 +:102B400011F0020F15D0490713D4B959C66BD6F846 +:102B5000C8E00CFA01F111EA0E0F0AD0C6F8D01045 +:102B6000117941F004011171D4F89C240132C4F8A8 +:102B70009C240133202BDED1F0BD00002B4B70B51F +:102B80001E561B5C012B2FD8294D2A4A55F823309D +:102B900052F826400BB341B3236D1A060FD580239C +:102BA000236500F081F950EA01020B4602D001389A +:102BB00061F10003024655F82600FFF777FE236D0A +:102BC0001B032CD555F826304FF4002203F5805313 +:102BD0002265012283F8692421E00123236508236B +:102BE00023654FF48063236570BD236DDA0702D43B +:102BF000236D9B0706D5032355F826002365002186 +:102C0000FFF76AFF236D180702D4236DD90606D596 +:102C1000182355F8260023650121FFF75DFF55F8BD +:102C20002600BDE87040FFF775BF00BFA054000844 +:102C300048640020A454000808B5302383F31188A9 +:102C4000FFF768FF002383F3118808BDC36BD3F837 +:102C5000C40080F40010C0F34050704708B5302322 +:102C600083F3118800F58050406DFFF7C5F800230D +:102C700083F3118843090CBF0120002008BD000028 +:102C800000F5805393F8692462B1C16B8A6922F020 +:102C900001028A61D3F8AC240132C3F8AC240022CB +:102CA00083F86924704700002DE9F743302181F350 +:102CB0001188002500F5835141F2780E4FF001088C +:102CC00000F5805C00EB45147444267977071CD42A +:102CD000F6061AD5D0F83C908E69D9F8C87008FA73 +:102CE00006F63E4211D04F6801970F689742019F48 +:102CF0009F410AD2C9F8D060267946F004062671B1 +:102D0000DCF898440134CCF898440135202D01F1C9 +:102D10002001D7D1002383F3118803B0BDE8F083ED +:102D2000F8B51E46002313700F4605461446FFF7FC +:102D300095FF80F0010038701EB12846FFF786FF2E +:102D40002070F8BD2DE9F04F85B09946DDE90EA35E +:102D50000D4602931378019391F800B08046164611 +:102D600000F0A2F82B7804460F4613B93378002BF5 +:102D700041D022463B464046FFF796FFFFF75CFFF7 +:102D8000FFF77EFF4B4632462946FFF7C9FF2B78F7 +:102D900033B1BBF1000F03D0012005B0BDE8F08FC7 +:102DA000337813B1019B002BF6D108F580530393C0 +:102DB000029B544577EB03031DD2039BD3F86404B5 +:102DC000C8B10368AAEB04011B6898474B4632461A +:102DD00029464046FFF7A4FF2B7813B1BBF1000F43 +:102DE000DAD1337813B1019B002BD5D100F05CF818 +:102DF00004460F46DCE70020CFE7000008B50021BD +:102E00000846FFF7BBFEBDE8084001F0CBB8000064 +:102E100008B501210020FFF7B1FEBDE8084001F030 +:102E2000C1B8000008B500210120FFF7A7FEBDE8EA +:102E3000084001F0B7B8000008B501210846FFF7C7 +:102E40009DFEBDE8084001F0ADB8000000B59BB0A4 +:102E5000EFF3098168226846FEF774FCEFF30583FF +:102E6000014B9B6BFEE700BF00ED00E008B5FFF7EC +:102E7000EDFF000000B59BB0EFF3098168226846C2 +:102E8000FEF760FCEFF30583014B5B6BFEE700BFD1 +:102E900000ED00E0FEE700000FB408B5029801F075 +:102EA0007DFDFEE702F082B802F064B80139C9B2D4 +:102EB00002299EBF00EBC1000023C0E90133704727 +:102EC0002DE9F8431E461B885B070546884618D443 +:102ED000044600F118094C4513D063686BB12B68A8 +:102EE0002846DB6B98473388A768C1B243F00403D8 +:102EF00060684246B8470834EDE7A368002BEED17E +:102F0000F9E70120BDE8F88373B56C4684E8060054 +:102F1000014600224E68D5B26EB98E685EB900EBEC +:102F2000C200021D94E80300013582E803001D7011 +:102F3000012002B070BD0132032A01F10801E9D17C +:102F40000020F6E72DE9F04F2DED028B89B09FEDC3 +:102F5000258BBDF8508004468B46054600F11809C4 +:102F6000002708F0040AA94536D06B680BB9AB6896 +:102F700063B1BAF1000F0BD123682046DB6B984791 +:102F80004346C1B25A46D5E90106B0470835EAE7DB +:102F9000002FFBD18DED008BADF808705B4603AAC6 +:102FA0000BF1080C18685968174603C708336345C6 +:102FB0003A46F7D1186838609B88BB80FFF774FFEA +:102FC0000423ADF808302368CDE900011B6C694685 +:102FD000204698470127D9E7012009B0BDEC028BB4 +:102FE000BDE8F08F0000000000000000082817D99D +:102FF00009280CD00A280CD00B280CD00C280CD097 +:103000000D280CD00E2814BF4020302070470C2013 +:1030100070471020704714207047182070472020F8 +:10302000704700002DE9F041456A15B94162BDE8DD +:10303000F0814B6823F06047C3F38A464FEAD37EA2 +:10304000C3F3807816EA230638BF3E46AC462B46CB +:103050005A68BEEBD27F22F060440AD0002A18DA08 +:10306000A40CB44217D19D420FD10D60DEE7134688 +:10307000EEE7A74207D102F08044C2F380724245D6 +:103080000BD054B1EFE708D2EDE7CCF800100B609D +:10309000CDE7B44201D0B442E5D81A689C46002A74 +:1030A000E5D11960C3E700002DE9F047089D01F064 +:1030B00007044FEAD508224405F0070500EBD100CC +:1030C0004FF47F49944201D1BDE8F08704F007072F +:1030D00005F0070A57453E4638BF5646C6F1080672 +:1030E000111B8E4228BF0E46E10808EBD50E415C4D +:1030F00013F80EC0B94029FA06F721FA0AF1FFB217 +:103100008CEA010147FA0AF739408CEA010C03F80E +:103110000EC034443544D5E780EA0120082341F24B +:10312000210201B24000002980B203F1FF33B8BF91 +:10313000504013F0FF03F4D17047000038B50C463F +:103140008D18A54200D138BD14F8011BFFF7E4FF2C +:10315000F7E7000042684AB11368436043898189F8 +:1031600001339BB29942438138BF83811046704737 +:1031700070B588B0202204460D4668460021FEF74F +:10318000F3FA20460495FFF7E5FF024658B16B4677 +:10319000054608AE1C4603CCB4422860696023464D +:1031A00005F10805F6D1104608B070BD082817D9FA +:1031B00009280CD00A280CD00B280CD00C280CD0D5 +:1031C0000D280CD00E2814BF4020302070470C2052 +:1031D0007047102070471420704718207047202037 +:1031E00070470000082817D90C280CD910280CD9D2 +:1031F00014280CD918280CD920280CD930288CBFB9 +:103200000F200E207047092070470A2070470B20BE +:1032100070470C2070470D20704700002DE9F843DF +:10322000078C072F04461ED9D0E9029800254FF6D7 +:10323000FF73C5F12006A5F1200029FA05F108FA6F +:1032400006F628FA00F031430143C9B21846FFF7E9 +:1032500063FF0835402D0346EBD1E1693A46BDE8EE +:10326000F843FFF76BBF4FF6FF70BDE8F88300002F +:1032700010B54B6823B9CA8A63F30902CA8210BD2C +:1032800004691A681C600361C38A013BC3824A60F7 +:10329000EFE700002DE9F84F1D46CB8A0F46C3F338 +:1032A00009010529814692460B4630D00020AAB27A +:1032B00007F11A049EB2042E1FFA80F80FD8904529 +:1032C00003F1010306D3FB8A0A4462F30903FB827C +:1032D00001201AE01AF80060E6540130EAE7904550 +:1032E000F1D2A1F1050B1C237C68BBFBF3F203FBBD +:1032F00012BB1FFA8BF6002C45D14846FFF72AFF78 +:10330000044638B978606FF00200BDE8F88F4FF0DE +:103310000008E6E7002606607860ADB24FF0000BCB +:10332000454510D90AEB0803221D13F8011B9155DE +:10333000B1B208F101081B291FFA88F82BD04545C6 +:1033400006F10106F1D8FB8AC3F30902154465F3BF +:103350000903BCE7013292B21C462368002BF9D165 +:103360006B1F0B441C21B3FBF1F301339BB29A4258 +:10337000D3D2BBF1000FD0D14846FFF7EBFE20B906 +:10338000C4F800B0BFE70122E7E7C0F800B05E462E +:1033900020600446C1E74545D5D94846FFF7DAFE27 +:1033A00008B92060AFE7C0F800B0002620600446EE +:1033B000B6E700002DE9F04F2DED028B1C4683B0DF +:1033C0005B69019207468846002B00F0A780238C9A +:1033D0002BB1E269002A00F0A180072B35D807F154 +:1033E0000C00FFF7B7FE054638B96FF00205284616 +:1033F00003B0BDEC028BBDE8F08F14220021FEF774 +:10340000B3F9228CE16905F10800FEF79BF9208CE5 +:10341000013080B2FFF7E6FEFFF7C8FE013880B248 +:103420002084013028746369228C1B782A4403F0BD +:103430001F0363F03F0348F0004113723846696090 +:103440002946FFF7EFFD0125D1E702339BB20722A2 +:10345000C18A0633B3FBF2F3828A521A9BB292B24C +:103460009342C2D800F10C034FF0000908EE103A65 +:103470004FF0800A4E464D4618EE100AFFF76AFEDE +:1034800083460028B1D014220021FEF76DF9002EEA +:103490003AD1019BABF8083002220BF1080E1FFA5B +:1034A00082FC0CF10100BCF1060F218C80B201D826 +:1034B0008E422BD3FFF796FEFFF778FE62691278F3 +:1034C000013802F01F028E4208BF4FF0400A42EA64 +:1034D00049121BFA80F14AEA020A013048F0004220 +:1034E00081F808A08BF81000CBF804205946384624 +:1034F000FFF798FD238C0135B3422DB289F0010905 +:103500004FF0000AB8D172E70022C6E7E169895D91 +:103510000EF802100136B6B20132C0E76FF00105B5 +:1035200065E70000F8B515460E4630220021044636 +:103530001F46FEF719F9069B6360B5F5001F079B50 +:10354000A76034BF6A094FF6FF72A36297B2E661C3 +:1035500004F1100000239A4206D800230360A782DA +:10356000E3822383E360F8BD0660013330462036F2 +:10357000F1E7000003781BB94BB2002BC8BF017004 +:103580007047000000787047F8B50C46C9690746D7 +:1035900011B9238C002B37D1257E1F2D34D83878D4 +:1035A00028BB228C072A2CD8268A36F003032BD17D +:1035B0004FF6FF70FFF7C2FD20F00100310240041A +:1035C00041EA0561400C41EA40254FF6FF7223466F +:1035D00029463846FFF7EEFE002807DD62691378BA +:1035E0000133DBB21F2B88BF00231370F8BD218A83 +:1035F0002D0645EA012505432046FFF70FFE02464A +:10360000E5E76FF00300F1E76FF00100EEE700007F +:1036100070B58AB0044616460021282268461D4629 +:10362000FEF7A2F8BDF83830ADF810300F9B0593C7 +:103630009DF840308DF81830119B07936946BDF80E +:103640004830ADF820302046CDE90265FFF79CFFF9 +:103650000AB070BD2DE9F041D36905460C46164607 +:103660000BB9138C5BBB377E1F2F28D895F80080D1 +:10367000B8F1000F26D03046FFF7D0FD3378210295 +:1036800041EAC33141EA0801338A41EA076141EA6C +:1036900003410246334641F080012846FFF78AFE87 +:1036A00000280ADD3378012B07D1726913780133C2 +:1036B000DBB21F2B88BF00231370BDE8F0816FF0D1 +:1036C0000100FAE76FF00300F7E70000F0B58BB0F8 +:1036D00004460D4617460021282268461E46FEF77E +:1036E00043F89DF84C305A1E534253418DF8003038 +:1036F0009DF84030ADF81030119B05939DF848308F +:103700008DF81830149B07936A46BDF85430ADF815 +:10371000203029462046CDE90276FFF79BFF0BB00B +:10372000F0BD0000406A00B104307047436A1A6877 +:10373000426202691A600361C38A013BC382704717 +:103740002DE9F041D0F82080194E14461D4641461F +:10375000002709B9BDE8F081D1E90223A21A65EB7F +:103760000303964277EB03031ED2036A8B420DD10B +:10377000FFF77EFD036A1B68036203690B60C38A5F +:103780000161016A013BC3828846E2E7FFF770FDF1 +:103790000B68C8F8003003690B60C38A0161013B04 +:1037A000C382D8F80010D4E788460968D1E700BF83 +:1037B00080841E002DE9F04F8BB00D46DDF850904F +:1037C00014469B468046002800F01981B9F1000F8D +:1037D00000F01581531E3F2B00F21181012A03D105 +:1037E000BBF1000F40F00B810023CDE90833B8F89E +:1037F0001430B5EBC30F4FEAC30703D300200BB05F +:10380000BDE8F08F2B199F42D8F80C303ABF7F1BD0 +:10381000FFB227461BB9D8F81030002B7AD0272DDD +:103820004ED8C5F12806B7424FF000032CBFF6B2C0 +:103830003E4600932946D8F8080008AB3246FFF709 +:1038400033FCA7EB060A35445FFA8AFAB8F814305D +:1038500003F10053053BDB000493D8F80C300393CD +:103860002821039B13B1BAF1000F2CD1D8F8100016 +:1038700040B1BAF1000F05D0009608AB5246691A64 +:10388000FFF712FC38B2002FB8D066070AD00AAB97 +:1038900003EBD401624211F8083C02F00702134125 +:1038A00001F8083C082C3CD9102C40F2B580202CA3 +:1038B00040F2B780BBF1000F00F09C80082334E099 +:1038C000BA460026C2E7049BE02B28BFE0230693FC +:1038D0000B44AB42059314D95A1B039800969245AA +:1038E00034BF5246D2B2691A08AB04300792FFF7D0 +:1038F000DBFB079A1644AAEB020A1544F6B25FFAFC +:103900008AFA049B069A05999B1A0493039B1B68E9 +:103910000393A6E70093D8F8080008AB3A46294677 +:10392000AEE7BBF1000F13D00123B4EBC30F6CD093 +:10393000082C12D89DF82030621E23FA02F2D50717 +:1039400006D54FF0FF3202FA04F423438DF82030FD +:103950009DF8203089F8003051E7102C12D8BDF8BE +:103960002030621E23FA02F2D10706D54FF0FF3253 +:1039700002FA04F42343ADF82030BDF82030A9F852 +:1039800000303CE7202C0FD80899631E21FA03F37E +:10399000DA0705D54FF0FF3202FA04F40C4308941D +:1039A000089BC9F800302AE7402C2BD0DDE90865D8 +:1039B000611EC4F12102A4F1210326FA01F105FAE6 +:1039C00002F225FA03F311431943CB0712D5012262 +:1039D000A4F12003C4F1200102FA03F322FA01F159 +:1039E000A240524243EA010363EB430332432B43B9 +:1039F000CDE90823DDE90823C9E90023FFE66FF0DC +:103A00000100FCE66FF00800F9E6082CA0D9102CA4 +:103A1000B3D9202CEED8C3E7BBF1000FADD0022301 +:103A200083E7BBF1000FBBD004237EE730B5012A4A +:103A3000144638BF0124402C85B028BF40240025FF +:103A4000012ACDE9025518D81B788DF80830630794 +:103A50000AD004AB03EBD405624215F8083C02F02F +:103A60000702934005F8083C0091034622460021D6 +:103A700002A8FFF719FB05B030BD082AE4D9102AC7 +:103A800003D81B88ADF80830E1E7202A8DBFD3E9C1 +:103A900000231B680293CDE90223D8E710B5CB6859 +:103AA0001BB98B600B618B8210BD04691A681C60A6 +:103AB0000361C38A013BC382CA60F0E703064CBFBF +:103AC000C0F3C0300220704708B50246FFF7F6FF8A +:103AD000022806D15306C2F30F2001D100F00300E3 +:103AE00008BDC2F30740FBE72DE9F04F93B0CDE9E5 +:103AF00004230A6804461046FFF7E0FF022814BFBB +:103B0000C2F306260026002A0D46824680F2028273 +:103B100012F0C04940F0FE81097B002900F0FA81D3 +:103B2000022803D02378B34240F0F781C2F3046344 +:103B30000693104602F07F030393FFF7C5FF039B34 +:103B400029444FEA834848EA0A4848EA4668CE785A +:103B500000230022CDE90823F309834648EA000840 +:103B6000029367D0039B009302466768534608A9F7 +:103B70002046B847002868D0276A4FB9414604F16B +:103B80000C00FFF7F5FA074690B96FF002006AE003 +:103B90003B6998450DD03F68002FF9D1414604F1AB +:103BA0000C00FFF7E5FA07460028EED0236A3B60D9 +:103BB000276297F817C006F01F08CCF3840CACEB13 +:103BC00008001FFA80FE0028B8BF0EF12000A8EB05 +:103BD0000C031FFA83FED7E90221B8BF00B2002B05 +:103BE0000793BEBF0EF120031BB2079352EA0103F5 +:103BF00042D0049BDFF834E39A1A059B4FF0000C87 +:103C000063EB010196457CEB010335D36B7B97F8A1 +:103C10001AE0734521D1029B002B00F0848001281B +:103C20002ADC786840BBDFF800C3944570EB0103E1 +:103C300043D21EE0276A5FB9039B00936568534631 +:103C40005A4608A92046A84768BB6FF00B000AE057 +:103C50003B699845ADD03F68EDE7B34890427CEBB7 +:103C6000010303D3002013B0BDE8F08F029B002BAB +:103C7000F8D0079B0F2B19DCFA7DB30002F003028A +:103C800003F07C031343FB7539462046FFF7F0FA37 +:103C90006B7BBB76029B4BB9FB7DC3F38402013285 +:103CA00062F38603FB756FF00C00DCE76A7BBB7E7A +:103CB0009A42D7D1029B002B35D0B309022B32D0C8 +:103CC000049BBB60059BFB60142200210DA8FDF73F +:103CD0004BFD049B0A93059B0B932B1D0C932B7B95 +:103CE000ADF83EB0013BDBB2ADF83C30069B8DF841 +:103CF0004230039B8DF8433094F82C308DF840A06F +:103D000083F001038DF844308DF84180A3680AA93F +:103D100020469847FB7DC3F38403013303F01F0360 +:103D20009B02FB829EE7FB7DC6F34012B2EBD31FE2 +:103D300040F0F680C3F38403434540F0F980029AD3 +:103D40002B7BB609002A52D016F0010661D1032B55 +:103D500040F2F180049BBB60059BFB60FB8A66F32D +:103D60000903FB822B7BAE1D033BDBB23246394697 +:103D700004F10C00FFF78EFA00280CDA39462046D1 +:103D8000FFF776FAFB7DC3F38403013303F01F03CF +:103D90009B02FB82F9E6DDE90884AB883B834FF6A2 +:103DA000FF73C9F12000A9F1200228FA09F104FAF1 +:103DB00000F0014324FA02F211431846C9B2FFF79A +:103DC000ABF909F10809B9F1400F0346E9D1B8820E +:103DD0002A7B033AD2B23146FFF7B0F9FB7DB882B5 +:103DE000DA43C2F3C01262F3C713FB753AE786B930 +:103DF0002E1D013BDBB23246394604F10C00FFF7C1 +:103E000049FA0028BADB2A7BB88A013AD2B2314695 +:103E1000E2E7F98AC1F30901013B0429DAB25BD870 +:103E2000281D002307F11B069A4208D910F801CB80 +:103E300006F801C0013101330529DBB2F4D1049940 +:103E40000A9105990B91934207F11B010C9138BF20 +:103E5000043379680D9134BF55FA83F300230E9330 +:103E6000FB8AADF83EB0C3F309031A44069B8DF8F4 +:103E70004230039B8DF8433094F82C30ADF83C2051 +:103E800083F001038DF8443000238DF840A08DF8B5 +:103E900041807B602A7BB88A013A291DFFF74EF9E1 +:103EA0003B8BB882834203D1A3680AA92046984776 +:103EB00020460AA9FFF7F2FDFB7DBA8AC3F384030B +:103EC000013303F01F039B02FB823B8B9A420CBF22 +:103ED00000206FF01000C6E67B68002BAFD00520F5 +:103EE00001E01C3033461E68002EFAD1091A081D65 +:103EF0002E1D184401EB090CBCF11B0F5FFA89F36E +:103F00009DD89A429BD916F8013B00F8013B09F174 +:103F10000109EFE76FF00900A5E66FF00A00A2E6DD +:103F20006FF00D009FE600BF40420F0080841E002E +:103F30006FF00E0097E66FF00F0094E6EFF3098341 +:103F400005494A6B22F001024A63683383F309880A +:103F5000002383F31188704700EF00E0302080F3E6 +:103F6000118862B60C4B0D4AD96821F4E06109044E +:103F7000090C0A43DA60D3F8FC20094942F0807248 +:103F8000C3F8FC200A6842F001020A602022DA77B6 +:103F900083F82200704700BF00ED00E00003FA053F +:103FA000001000E010B5302383F311880E4B5B68DE +:103FB00013F4006314D0F1EE103AEFF30984683C77 +:103FC0004FF08073E361094BDB6B236684F3098850 +:103FD00000F0AEFC10B1064BA36110BD054BFBE732 +:103FE00083F31188F9E700BF00ED00E000EF00E087 +:103FF0003F03000842030008026843681143016060 +:1040000003B1184770470000024A136843F0C00329 +:10401000136070470048004013B50F4C204600F075 +:10402000EDFB04F114000D49009400234FF480725D +:1040300000F0AAFA0A490B4B00944FF4807204F185 +:10404000380000F023FB084BE365002000F0F4F992 +:10405000206602B010BD00BF58640020C464002078 +:10406000C4650020094000080048004030B5037CCA +:10407000214C002918BF0C46012B0CD11F4B984234 +:1040800009D11F4B9A6D42F480229A659A6F42F4CF +:1040900080229A679B6F2268036EC16D846603EB72 +:1040A0005203B3FBF2F36268150442BF23F0070525 +:1040B00003F0070343EA4503CB60A36843F04003E2 +:1040C0004B60E36843F001038B6042F4967343F066 +:1040D00001030B604FF0FF330B62510505D512F061 +:1040E000102205D0B2F1805F04D080F8643030BD7A +:1040F0007F23FAE73F23F8E71855000858640020AB +:10410000001002402DE9F047C66D3768F46934624B +:104110002107054619D014F0080118BF4FF480712B +:10412000E20748BF41F02001A30748BF41F040012A +:10413000600748BF41F08001302383F31188281DB8 +:10414000FFF75AFF002383F31188E2050AD53023D5 +:1041500083F311884FF48061281DFFF74DFF002382 +:1041600083F311884FF030094FF0000A14F0200853 +:1041700038D13B0616D54FF0300905F1380A200634 +:1041800010D589F31188504600F0B4FA002836DAC9 +:104190000821281DFFF730FF27F08003336000233C +:1041A00083F31188790614D5620612D5302383F380 +:1041B0001188D5E913239A4208D12B6C33B1102111 +:1041C000281D27F04007FFF717FF3760002383F310 +:1041D0001188E30618D5AA6E1369ABB1BDE8F047A4 +:1041E0005069184789F31188736A95F86410284656 +:1041F000194000F01DFB8AF31188F469B6E7B0623C +:1042000088F31188F469BAE7BDE8F087434B4FF4AF +:10421000007270B51A605A6912F4C06FFBD1404B3E +:104220009A6802F00C02042A20D01A6842F48072C4 +:104230001A601A685205FCD501229A609A6802F049 +:104240000C02042AFAD13749374A0A600A6812F088 +:104250000F02FBD1C3F8982063221A601A689603F4 +:10426000FCD42E4A4FF48071C2F88010C468E50374 +:1042700006D51A6842F480321A601A689103FCD598 +:10428000C269D20709D5D3F8982042F00102C3F8D9 +:104290009820D3F898209607FBD54269DA6044F459 +:1042A00080721A6004F0807111B11A689501FBD513 +:1042B000996802691B4E22F0030501F003012943AE +:1042C000996085693560316869400907FBD11349F8 +:1042D00005680D6046684E608068C1F880006E0415 +:1042E00017D448698505FCD4996802F0030021F0D1 +:1042F00003010143996092009968514011F00C0F3D +:10430000FAD1E2055EBF1A6822F480721A600020BA +:1043100070BD48698005FCD5E6E700BF007000402D +:1043200000100240002002400006040008B500F022 +:10433000A7F8BDE8084000F097B8000010B5394C68 +:104340003948A36A4FF0FF32A262A36A0023A36236 +:10435000A16AE16A61F07F01E162E16A01F07F0137 +:10436000E162E16A216B2263216B2363216BA16B04 +:10437000A263A16BA363A16BE16BE263E16BE363F7 +:10438000E16B216C2264226C2364226C226E42F069 +:1043900001022266D4F8802022F00102C4F88020B5 +:1043A000D4F88020A26D42F08052A265A26F22F064 +:1043B0008052A267A26F1D4A4FF400419160D36002 +:1043C000136253629362D362136353639363D36341 +:1043D000136453649364D36413655365116841F4A3 +:1043E00080711160D4F8902012F4407F1EBF4FF40A +:1043F0008032C4F89020C4F890300D4A0023A360A6 +:10440000C4F88820C4F89C30FFF700FF10B10948B9 +:1044100000F0C4FAD4F8903023F00323C4F89030AD +:1044200010BD00BF00100240385500080070004069 +:104430005501005130550008014B53F820007047DA +:104440002823002008B50348FFF75CFEBDE80840BC +:10445000FFF7A8BD5864002008B500F0B7FEBDE81E +:104460000840FFF79FBD000008B507211C2000F0A1 +:104470001BF8BDE808400C21272000F015B800000B +:104480004FF0E023002258684FF0FF31930003F112 +:10449000604303F5614301329042C3F88010C3F8D2 +:1044A0008011F3D27047000000F1604303F56143CF +:1044B0000901C9B283F80013012200F01F039A40DA +:1044C00043099B0003F1604303F56143C3F8802176 +:1044D0001A607047F8B5154682680669AA420B460D +:1044E000816938BF8568761AB54204460BD21846F2 +:1044F0002A46FDF727F9A3692B44A361A3685B1B38 +:10450000A3602846F8BD0CD918463246FDF71AF9C3 +:10451000AF1BE1683A463044FDF714F9E3683B44C9 +:10452000EBE718462A46FDF70DF9E368E5E70000DA +:1045300083689342F7B51546044638BF8568D0E9CD +:104540000460361AB5420BD22A46FDF7FBF86369C0 +:104550002B446361A36828465B1BA36003B0F0BDD6 +:104560000DD932460191FDF7EDF80199E068AF1BD6 +:104570003A463144FDF7E6F8E3683B44E9E72A466A +:10458000FDF7E0F8E368E4E710B50A440024C361EE +:10459000029B8460C0E90000C0E90511C1600261AE +:1045A000036210BD08B5D0E90532934201D182689B +:1045B00082B98268013282605A1C42611970D0E966 +:1045C00004329A4224BFC3684361002100F052FACA +:1045D000002008BD4FF0FF30FBE7000070B530232E +:1045E00004460E4683F31188A568A5B1A368A269A5 +:1045F000013BA360531CA36115782269934224BF39 +:10460000E368A361E3690BB120469847002383F375 +:104610001188284607E03146204600F01BFA0028A2 +:10462000E2DA85F3118870BD2DE9F74F04460E4696 +:1046300017469846D0F81C904FF0300A8AF311883C +:104640004FF0000B154665B12A4631462046FFF76C +:1046500041FF034660B94146204600F0FBF90028BF +:10466000F1D0002383F31188781B03B0BDE8F08FED +:10467000B9F1000F03D001902046C847019B8BF38E +:104680001188ED1A1E448AF31188DCE7C0E9051190 +:10469000C160C3611144009B8260C0E900000161F8 +:1046A00003627047F8B504460D461646302383F37F +:1046B0001188A768A7B1A368013BA36063695A1C6E +:1046C00062611D70D4E904329A4224BFE3686361D9 +:1046D000E3690BB120469847002080F3118807E07A +:1046E0003146204600F0B6F90028E2DA87F3118857 +:1046F000F8BD0000D0E905239A4210B501D18268C7 +:104700007AB98268013282605A1C82611C7803691E +:104710009A4224BFC3688361002100F0ABF92046B0 +:1047200010BD4FF0FF30FBE72DE9F74F04460E4672 +:1047300017469846D0F81C904FF0300A8AF311883B +:104740004FF0000B154665B12A4631462046FFF76B +:10475000EFFE034660B94146204600F07BF9002891 +:10476000F1D0002383F31188781B03B0BDE8F08FEC +:10477000B9F1000F03D001902046C847019B8BF38D +:104780001188ED1A1E448AF31188DCE70268436839 +:104790001143016003B11847704700001430FFF760 +:1047A00043BF00004FF0FF331430FFF73DBF000060 +:1047B0003830FFF7B9BF00004FF0FF333830FFF754 +:1047C000B3BF00001430FFF709BF00004FF0FF3106 +:1047D0001430FFF703BF00003830FFF763BF00005D +:1047E0004FF0FF323830FFF75DBF0000012914BFE2 +:1047F0006FF0130000207047FFF70EBC044B0360FE +:104800000023C0E90233436001230374704700BFF3 +:104810005855000810B53023044683F31188FFF77C +:1048200025FC02232374002080F3118810BD0000B2 +:1048300038B5C36904460D461BB904210844FFF787 +:10484000A5FF294604F11400FFF7ACFE002806DAA4 +:10485000201D4FF40061BDE83840FFF797BF38BD19 +:1048600000230375826803691B6899689142FBD233 +:104870005A680360426010605860704700230375F7 +:10488000826803691B6899689142FBD85A68036083 +:10489000426010605860704708B50846302383F3C3 +:1048A00011880B7D032B05D0042B0DD02BB983F37E +:1048B000118808BD8B6900221A604FF0FF338361B5 +:1048C000FFF7CEFF0023F2E7D1E9003213605A6010 +:1048D000F3E70000FFF7C4BF054BD96808751868F7 +:1048E00002681A60536001220275D860FBF712BD9E +:1048F000C866002030B50C4BDD684B1C87B0044601 +:104900000FD02B46094A684600F046F92046FFF7CB +:10491000E3FF009B13B1684600F048F9A86907B0AF +:1049200030BDFFF7D9FFF9E7C866002099480008B5 +:10493000044B1A68DB6890689B68984294BF00201B +:1049400001207047C8660020084B10B51C68D86865 +:1049500022681A60536001222275DC60FFF78EFF27 +:1049600001462046BDE81040FBF7D4BCC8660020D5 +:1049700038B5074C074908480123002523706560B6 +:1049800000F052FC0223237085F3118838BD00BF6C +:104990003069002084550008C866002008B572B64A +:1049A000044B186500F0D0FA00F080FB024B0322A4 +:1049B0001A70FEE7C86600203069002000F02CB9AC +:1049C0008B60022308618B82084670478368A3F1DD +:1049D000840243F8142C026943F8442C426943F8DA +:1049E000402C094A43F8242CC26843F8182C0222B0 +:1049F00003F80C2C002203F80B2C044A43F8102C6B +:104A0000A3F12000704700BF2D030008C8660020F6 +:104A100008B5FFF7DBFFBDE80840FFF75BBF00000C +:104A2000024BDB6898610F20FFF756BFC866002075 +:104A3000302383F31188FFF7F3BF000008B5014668 +:104A4000302383F311880820FFF754FF002383F3FA +:104A5000118808BD064BDB6839B1426818605A609E +:104A6000136043600420FFF745BF4FF0FF307047ED +:104A7000C86600200368984206D01A680260506039 +:104A800099611846FFF726BF7047000010B503680C +:104A90009C68A2420CD85C688A600B604C60216004 +:104AA000596099688A1A9A604FF0FF33836010BD8D +:104AB0001B68121BECE700000A2938BF0A2170B5F9 +:104AC00004460D460A26601900F0A8FB00F094FB8E +:104AD000041BA54203D8751C2E460446F3E70A2E94 +:104AE00004D9BDE87040012000F0DEBB70BD0000BD +:104AF000F8B5144B0D46D96103F1100141600A2A43 +:104B00001969826038BF0A22016048601861A818DC +:104B1000144600F075FB0A2700F06EFB431BA3420E +:104B2000064606D37C1C281900F078FB274635463C +:104B3000F2E70A2F04D9BDE8F840012000F0B4BB29 +:104B4000F8BD00BFC8660020F8B506460D4600F067 +:104B500053FB0F4A134653F8107F9F4206D12A4653 +:104B600001463046BDE8F840FFF7C2BFD169BB68D7 +:104B7000441A2C1928BF2C46A34202D92946FFF714 +:104B80009BFF224631460348BDE8F840FFF77EBF51 +:104B9000C8660020D866002010B4C0E903230023B3 +:104BA0005DF8044B4361FFF7CFBF000010B5194C0F +:104BB000236998420DD0D0E90032816813605A60B1 +:104BC0009A680A449A60002303604FF0FF33A361A0 +:104BD00010BD2346026843F8102F53600022026084 +:104BE00022699A4203D1BDE8104000F011BB9368DE +:104BF00081680B44936000F0FFFA2269E1699268D2 +:104C0000441AA242E4D91144BDE81040091AFFF742 +:104C100053BF00BFC86600202DE9F047DFF8BC8015 +:104C200008F110072C4ED8F8105000F0E5FAD8F82B +:104C30001C40AA68031B9A423ED81444D5E90032AE +:104C40004FF00009C8F81C4013605A60C5F8009086 +:104C5000D8F81030B34201D100F0DAFA89F31188A4 +:104C6000D5E9033128469847302383F311886B69CF +:104C7000002BD8D000F0C0FA6A69A0EB04094A45BD +:104C800082460DD2022000F00FFB0022D8F810302F +:104C9000B34208D151462846BDE8F047FFF728BF88 +:104CA000121A2244F2E712EB090938BF4A46294694 +:104CB0003846FFF7EBFEB5E7D8F81030B34208D01E +:104CC0001444211AC8F81C00A960BDE8F047FFF79A +:104CD000F3BEBDE8F08700BFD8660020C86600209C +:104CE00038B500F089FA054AD2E90845031B1819BE +:104CF00045F10001C2E9080138BD00BFC8660020C7 +:104D000000207047FEE70000704700004FF0FF30C2 +:104D100070470000BFF34F8F024A1369DB03FCD4D6 +:104D2000704700BF0020024008B5094B1B7873B9DB +:104D3000FFF7F0FF074B5A69002ABFBF064A9A6087 +:104D400002F188329A601A6822F480621A6008BD03 +:104D500048690020002002402301674508B50B4B3D +:104D60001B7893B9FFF7D6FF094B5A6942F000420E +:104D70005A611A6842F480521A601A6822F480520A +:104D80001A601A6842F480621A6008BD48690020FF +:104D900000200240FF289ABF00F58030C0020020AA +:104DA000704700004FF40060704700004FF48070BF +:104DB00070470000FF2808B50BD8FFF7EBFF00F5A0 +:104DC00000630268013204D104308342F9D101202A +:104DD00008BD0020FCE70000FF2838B5044624D8B1 +:104DE000FFF798FFFFF7A0FF114AF3231361022397 +:104DF0005361C3005169C50903F47E7343EAC523B7 +:104E00000B435361536943F480335361FFF782FFCF +:104E10004FF40061FFF7BEFF00F044F9FFF79EFF7B +:104E20002046BDE83840FFF7C5BF002038BD00BFB1 +:104E3000002002402DE9F84340EA020313F0070383 +:104E4000144606D0304B4FF461721A600020BDE862 +:104E5000F88385182D4A95420CD92B4A40F28931A6 +:104E60001160F3E7031D1B684A68934208D1083CB0 +:104E700008300831072C14D902680B689A42F1D027 +:104E8000FFF752FFFFF746FF214E08314FF00108B0 +:104E90004FF00009012CA1F1080706D8FFF75EFFCB +:104EA00001E0002CECD10120D1E7C6F814800546C2 +:104EB00051F8083C45F8043B51F8043C4360FFF7C7 +:104EC00029FF336943F001033361C6F81490026887 +:104ED00051F8083C9A420CD00B4B40F2B1321A60A8 +:104EE0000C4B18600C4B1C600C4B1F60FFF736FF1F +:104EF000ACE72D6851F8043C9D4201F10801EBD16B +:104F0000083C0830C6E700BF4469002000000808DC +:104F10000020024038690020406900203C690020E0 +:104F2000084908B50B7828B11BB9FFF7FDFE01232E +:104F30000B7008BD002BFCD0BDE808400870FFF7DF +:104F40000DBF00BF4869002008B50649064800F0BB +:104F5000ABF8BDE808404FF400414FF0805000F03E +:104F6000A3B800BF007F010000010020084600F048 +:104F700029BA000038B5EFF311859DB9EFF3058428 +:104F8000C4F30804302334B183F31188FFF7A8FE7B +:104F900085F3118838BD83F31188FFF7A1FE84F3F0 +:104FA000118838BDBDE83840FFF79ABE38B5FFF725 +:104FB000E1FF114C114AC00840EA4170A0FB025EBB +:104FC000C908A0FB040C1CEB050CA1FB04404FF02E +:104FD00000035B41A1FB02121CEB040C43EB00003D +:104FE00011EB0E0142F10002411842F100000909E3 +:104FF00041EA007038BD00BFCFF753E3A59BC42042 +:105000000244D2B2904200D17047431C800000F1AC +:10501000804000F51450006841F8040BD8B2F1E765 +:10502000124B10B5D3F89040240409D4D3F8904023 +:10503000C3F89040D3F8904044F40044C3F8904043 +:105040000B4C2368024443F480732360D2B2904235 +:1050500000D110BD431C800000F1804000F51450C9 +:1050600051F8044B0460D8B2F1E700BF00100240D1 +:105070000070004007B5012201A90020FFF7C0FF22 +:10508000019803B05DF804FB13B50446FFF7F2FF87 +:10509000A04205D0012201A900200194FFF7C0FF22 +:1050A00002B010BD7047000070470000704700005C +:1050B000074B45F255521A6003225A6040F2FF72C4 +:1050C0009A604CF6CC421A60024B01221A7070476B +:1050D0000030004050690020034B1B781BB1034B8C +:1050E0004AF6AA221A60704750690020003000403A +:1050F000054B1A6832B902F1804202F50432D2F847 +:1051000094201A60704700BF4C690020024B4FF496 +:105110000002C3F8942070470010024008B5FFF762 +:10512000E7FF024B1868C0F3407008BD4C690020CF +:1051300070470000FEE700000A4B0B480B4A904204 +:105140000BD30B4BDA1C121AC11E22F003028B4246 +:1051500038BF00220021FCF707BB53F8041B40F8BE +:10516000041BECE714570008546900205469002020 +:105170005469002008B5124B9A6D42F001029A65FD +:105180009A6F42F001029A670E4A9B6F936843F050 +:10519000010393600620FFF74FF90B4BB0FBF3F0D0 +:1051A0004FF080434FF0FF3201389862DA620022FC +:1051B0009A615A63DA605A6001225A611A6008BD26 +:1051C00000100240002004E040420F004FF08042F7 +:1051D00008B51169D3680B40D9B2C9439B07116167 +:1051E00007D5302383F31188FFF7E8FB002383F30F +:1051F000118808BDFFF7BEBF4FF08043586A704763 +:105200004FF08043002258631A610222DA6070472F +:105210004FF080430022DA60704700004FF0804377 +:1052200058637047FEE7000070B51B4B0163002513 +:10523000044686B0586085620E46FEF78FFE04F184 +:105240001003C4E904334FF0FF33C4E90635C4E961 +:105250000044A560E562FFF7CFFF2B460246C4E994 +:10526000082304F134010D4A256580232046FFF709 +:10527000A7FB0123E0600A4A03750092726801925D +:10528000B268CDE90223074B6846CDE90435FFF744 +:10529000BFFB06B070BD00BF30690020905500080C +:1052A0009555000825520008024AD36A1843D06277 +:1052B000704700BFC86600204B6843608B688360FE +:1052C000CB68C3600B6943614B6903628B694362BE +:1052D0000B6803607047000008B5204BDA6A42F0A3 +:1052E0007F02DA62DA6A22F07F02DA62DA6ADA6C64 +:1052F00042F07F02DA64DA6E42F07F02DA66184A20 +:10530000DB6E11464FF09040FFF7D6FF02F11C0113 +:1053100000F58060FFF7D0FF02F1380100F58060F2 +:10532000FFF7CAFF02F1540100F58060FFF7C4FFE8 +:1053300002F1700100F58060FFF7BEFF02F18C0101 +:1053400000F58060FFF7B8FF02F1A80100F580606A +:10535000FFF7B2FFBDE80840FEF7F0BF00100240C3 +:105360009C55000808B500F009F8FFF701FBBDE8FF +:105370000840FFF7BDBE00007047000008B5FEF70B +:10538000D5FF00F009F8FFF737FAFFF7F5FFBDE8A2 +:105390000840FFF72FBF0000704700000B46014692 +:1053A000184600F001B8000010B5054C13462CB1AA +:1053B0000A4601460220AFF3008010BD2046FCE7FC +:1053C00000000000024B01461868FFF7CFBD00BF88 +:1053D0005023002010B501390244904201D1002031 +:1053E00005E0037811F8014FA34201D0181B10BD4E +:1053F0000130F2E72DE9F041A3B1C91A1778014451 +:10540000044603F1FF3C8C42204601D9002009E00C +:105410000578BD4204F10104F5D10CEB0405D61862 +:10542000A54201D1BDE8F08115F8018D16F801ED16 +:10543000F045F5D0E7E70000034611F8012B03F82B +:10544000012B002AF9D17047121000001211000040 +:1054500012130000636F6D2E7561762D6465762ED4 +:10546000617561760000000053544D333247343F7C +:105470003F00000040A2E4F1646891060041A3E50A +:10548000F2656992070000004261642043414E4981 +:105490006661636520696E6465782E000001000016 +:1054A0000001FF00006400400068004000000000B0 +:1054B00000000000DD25000841200008452D0008FF +:1054C000B1200008BD20000809230008612200085F +:1054D000792000087D200008552000083D200008A4 +:1054E000C522000861200008092F0008AD2E000821 +:1054F0006D2000089922000801040E05054B0202E8 +:105500000E05054B04010E05054B05010B04044B6C +:10551000080106030346000000960000000000009A +:10552000000000000000000000000000000000007B +:10553000636C6B73776300000003000000000000E1 +:1055400000010000000001010300000023283101D8 +:10555000040704000100000000000000B947000833 +:10556000A5470008E1470008CD470008D9470008D3 +:10557000C5470008B14700089D470008ED470008EF +:10558000633000008055000820670020306900204B +:105590006D61696E0069646C650000000001912A0C +:1055A00010000000AAAAA9AA00000024EFFE000033 +:1055B00000000000009009000000A04A0000000068 +:1055C000AAAAAA6A00005000FF7F000000000000A5 +:1055D000007799000000000400200000AAAAAAAAEF +:1055E00000000000FFFF00000000000000000000BD +:1055F0000000000000000000AAAAAAAA0000000003 +:10560000FFFF00000000000000000000000000009C +:1056100000000000AAAAAAAA00000000FFFF0000E4 +:10562000000000000000000000000000000000007A +:10563000AAAAAAAA00000000FFFF000000000000C4 +:10564000000000000000000000000000AAAAAAAAB2 +:1056500000000000FFFF000000000000000000004C +:10566000721400000000000000700700000000003D +:10567000FE2A0100D2040000FF0000005864002050 +:105680000000000068540008006889096D8BB902A9 +:1056900000B4C404006889090068890900688909A0 +:1056A000006889090068890900688909000000000C +:1056B0005423002000000000000000000000000053 +:1056C00000000000000000000000000000000000DA +:1056D00000000000000000000000000000000000CA +:1056E00000000000000000000000000000000000BA +:1056F00000000000000000000000000000000000AA +:105700000000000000000000000000000000000099 +:045710000000000095 :00000001FF diff --git a/Tools/environment_install/install-prereqs-mac.sh b/Tools/environment_install/install-prereqs-mac.sh index bab4d6f2892d0..616de58e9d9fc 100755 --- a/Tools/environment_install/install-prereqs-mac.sh +++ b/Tools/environment_install/install-prereqs-mac.sh @@ -46,6 +46,9 @@ $(which -s brew) || } echo "Homebrew installed" +# Detect Homebrew prefix (different on Apple Silicon vs Intel) +HOMEBREW_PREFIX="$(brew --prefix)" + #install command line tools echo "Checking CLI Tools installed..." { @@ -78,9 +81,9 @@ function install_arm_none_eabi_toolchain() { ) fi echo "Registering STM32 Toolchain for ccache" - sudo mkdir -p /usr/local/opt/ccache/libexec - sudo ln -s -f $CCACHE_PATH /usr/local/opt/ccache/libexec/arm-none-eabi-g++ - sudo ln -s -f $CCACHE_PATH /usr/local/opt/ccache/libexec/arm-none-eabi-gcc + sudo mkdir -p $HOMEBREW_PREFIX/opt/ccache/libexec + sudo ln -s -f $CCACHE_PATH $HOMEBREW_PREFIX/opt/ccache/libexec/arm-none-eabi-g++ + sudo ln -s -f $CCACHE_PATH $HOMEBREW_PREFIX/opt/ccache/libexec/arm-none-eabi-gcc echo "Done!" } @@ -102,7 +105,10 @@ function maybe_prompt_user() { # auto-updates things when you install other packages which depend on # more recent versions. # see https://github.com/orgs/Homebrew/discussions/3895 -find /usr/local/bin -lname '*/Library/Frameworks/Python.framework/*' -delete +# Note: /usr/local/bin may not exist on Apple Silicon Macs +if [ -d /usr/local/bin ]; then + find /usr/local/bin -lname '*/Library/Frameworks/Python.framework/*' -delete +fi # brew update randomly failing on CI, so ignore errors: brew update @@ -152,7 +158,7 @@ echo "Checking ccache..." } || { brew install ccache - exportline="export PATH=/usr/local/opt/ccache/libexec:\$PATH"; + exportline="export PATH=$HOMEBREW_PREFIX/opt/ccache/libexec:\$PATH"; eval $exportline } CCACHE_PATH=$(which ccache) @@ -214,7 +220,7 @@ grep -Fxq "$exportline3" ~/$SHELL_LOGIN 2>/dev/null || { } fi -exportline4="export PATH=/usr/local/opt/ccache/libexec:\$PATH"; +exportline4="export PATH=$HOMEBREW_PREFIX/opt/ccache/libexec:\$PATH"; grep -Fxq "$exportline4" ~/$SHELL_LOGIN 2>/dev/null || { if maybe_prompt_user "Append CCache to your PATH [N/y]?" ; then echo $exportline4 >> ~/$SHELL_LOGIN diff --git a/Tools/scripts/board_list.py b/Tools/scripts/board_list.py index 0ce93e9d7e12a..7a02ea0f112b7 100755 --- a/Tools/scripts/board_list.py +++ b/Tools/scripts/board_list.py @@ -27,12 +27,6 @@ def __init__(self, name): 'Rover', 'Sub', ] - SITL_toolchain = { - "SITL_x86_64_linux_gnu": "x86_64-linux-gnu", - "SITL_arm_linux_gnueabihf": "arm-linux-gnueabihf", - } - if name in SITL_toolchain: - self.toolchain = SITL_toolchain[name] def in_boardlist(boards : Collection[str], board : str) -> bool: @@ -72,19 +66,13 @@ def set_hwdef_dir(self): ) self.hwdef_dir = [] - for haldir in 'AP_HAL_ChibiOS', 'AP_HAL_Linux', 'AP_HAL_ESP32': + for haldir in 'AP_HAL_ChibiOS', 'AP_HAL_Linux', 'AP_HAL_ESP32', 'AP_HAL_SITL': self.hwdef_dir.append(os.path.join(realpath, haldir, "hwdef")) def __init__(self): self.set_hwdef_dir() - # no hwdefs for Linux boards - yet? - self.boards = [ - Board("SITL_x86_64_linux_gnu"), - Board("SITL_arm_linux_gnueabihf"), - ] - for b in self.boards: - b.hal = "AP_HAL_SITL" + self.boards = [] for hwdef_dir in self.hwdef_dir: self.add_hwdefs_from_hwdef_dir(hwdef_dir) @@ -142,6 +130,8 @@ def add_hwdefs_from_hwdef_dir(self, hwdef_dir): board.toolchain = 'arm-none-eabi' elif "ESP32" in hwdef_dir: board.toolchain = 'xtensa-esp32-elf' + elif "SITL" in hwdef_dir: + board.toolchain = 'native' else: raise ValueError(f"Unable to determine toolchain for {hwdef_dir}") @@ -151,6 +141,8 @@ def add_hwdefs_from_hwdef_dir(self, hwdef_dir): board.hal = "ChibiOS" elif "ESP32" in hwdef_dir: board.hal = "ESP32" + elif "SITL" in hwdef_dir: + board.hal = "SITL" else: raise ValueError(f"Unable to determine HAL for {hwdef_dir}") diff --git a/Tools/scripts/build_binaries.py b/Tools/scripts/build_binaries.py index a1bf9648cb782..e783da17c4103 100755 --- a/Tools/scripts/build_binaries.py +++ b/Tools/scripts/build_binaries.py @@ -29,11 +29,6 @@ import board_list from board_list import AP_PERIPH_BOARDS -if sys.version_info[0] < 3: - running_python3 = False -else: - running_python3 = True - def topdir(): '''return path to ardupilot checkout directory. This is to cope with @@ -137,10 +132,9 @@ def run_program(self, prefix, cmd_list, show_output=True, env=None, force_succes # select not available on Windows... probably... time.sleep(0.1) continue - if running_python3: - x = bytearray(x) - x = filter(lambda x : chr(x) in string.printable, x) - x = "".join([chr(c) for c in x]) + x = bytearray(x) + x = filter(lambda x : chr(x) in string.printable, x) + x = "".join([chr(c) for c in x]) output += x x = x.rstrip() if show_output: @@ -364,10 +358,7 @@ def read_string_from_filepath(self, filepath): with open(filepath, 'rb') as fh: content = fh.read() - if running_python3: - return content.decode('ascii') - - return content + return content.decode('ascii') def string_in_filepath(self, string, filepath): '''returns true if string exists in the contents of filepath''' diff --git a/Tools/scripts/build_bootloaders.py b/Tools/scripts/build_bootloaders.py index c7bce30d5843d..6249ca92e9eba 100755 --- a/Tools/scripts/build_bootloaders.py +++ b/Tools/scripts/build_bootloaders.py @@ -15,18 +15,14 @@ # get command line arguments from argparse import ArgumentParser -parser = ArgumentParser(description='make_secure_bl') -parser.add_argument("--signing-key", type=str, default=None, help="signing key for secure bootloader") +parser = ArgumentParser(description='This Program is used to build ArduPilot bootloaders for boards.') +parser.add_argument("--signing-key", type=str, action='append', help="signing key for secure bootloader (can be used multiple times)") +parser.add_argument("--omit-ardupilot-keys", action='store_true', default=False, help="omit ArduPilot signing keys") parser.add_argument("--debug", action='store_true', default=False, help="build with debug symbols") parser.add_argument("--periph-only", action='store_true', default=False, help="only build AP_Periph boards") parser.add_argument("pattern", type=str, default='*', help="board wildcard pattern", nargs='?') args = parser.parse_args() -if args.signing_key is not None and os.path.basename(args.signing_key).lower().find("private") != -1: - # prevent the easy mistake of using private key - print("You must use the public key in the bootloader") - sys.exit(1) - os.environ['PYTHONUNBUFFERED'] = '1' failed_boards = set() @@ -70,6 +66,32 @@ def get_board_list(): board_list.append(d) return board_list +def validate_signing_keys(keys): + """Validate that all signing key files exist and are not private keys""" + missing_keys = [] + private_keys = [] + + for key in keys: + if not os.path.isfile(key): + missing_keys.append(key) + elif os.path.basename(key).lower().find("private") != -1: + private_keys.append(key) + + if missing_keys: + print("Error: The following files were not found:") + for key in missing_keys: + print(f" {key}") + sys.exit(1) + + if private_keys: + print("Error: You must use the public key in the bootloader. Check the following files:") + for key in private_keys: + print(f" {key}") + sys.exit(1) + +if args.signing_key is not None: + validate_signing_keys(args.signing_key) + def run_program(cmd_list): print("Running (%s)" % " ".join(cmd_list)) retcode = subprocess.call(cmd_list) @@ -122,6 +144,10 @@ def get_all_board_dirs(): if b not in board_list: print(f"Skipping {b}: no hwdef-bl.dat (no bootloader for this board)") +additional_args = [] +if args.omit_ardupilot_keys: + # If the user has requested to omit ardupilot keys, ensure it is forwarded to the make_secure_bl program + additional_args.append("--omit-ardupilot-keys") # check that the user-supplied board pattern matches something; if not, warn and exit for board in board_list: @@ -142,11 +168,11 @@ def get_all_board_dirs(): shutil.copy('build/%s/bootloader/AP_Bootloader' % board, elf_file) print("Created %s" % elf_file) if args.signing_key is not None: - print("Signing bootloader with %s" % args.signing_key) - if not run_program(["./Tools/scripts/signing/make_secure_bl.py", bl_file, args.signing_key]): + print("Signing bootloader with %s" % ", ".join(args.signing_key)) + if not run_program(["./Tools/scripts/signing/make_secure_bl.py", *additional_args, bl_file] + args.signing_key): print("Failed to sign bootloader for %s" % board) sys.exit(1) - if not run_program(["./Tools/scripts/signing/make_secure_bl.py", elf_file, args.signing_key]): + if not run_program(["./Tools/scripts/signing/make_secure_bl.py", *additional_args, elf_file] + args.signing_key): print("Failed to sign ELF bootloader for %s" % board) sys.exit(1) if not run_program([sys.executable, "Tools/scripts/bin2hex.py", "--offset", "0x08000000", bl_file, hex_file]): diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 2eaf89baa00ce..e19ff8682359b 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -28,9 +28,18 @@ def config_option(self): return "enable-" + self.label.replace(" ", "-") -# list of build options to offer NOTE: the dependencies must be -# written as a single string with commas and no spaces, -# eg. 'dependency1,dependency2' +# list of build options to offer + +# Feature(category, label, macro, description, default, dependencies) +# +# category : The category/group this feature belongs to +# label : Globally unique identifier for the feature (max 30 chars) +# macro : The macro which enables/disables the feature at compile-time. +# e.g., "AP_AIRSPEED_ENABLED". +# description : User visible description shown on Custom build server. +# default : Default state (1 = enabled, 0 = disabled) +# dependencies : Comma-separated list of feature labels that this feature depends on +# (a single string with commas and no spaces). e.g., 'dependency1,dependency2'. BUILD_OPTIONS = [ Feature('AHRS', 'EKF3', 'HAL_NAVEKF3_AVAILABLE', 'Enable EKF3', 1, None), Feature('AHRS', 'EKF2', 'HAL_NAVEKF2_AVAILABLE', 'Enable EKF2', 0, None), @@ -134,7 +143,7 @@ def config_option(self): Feature('VTX', 'SMARTAUDIO', 'AP_SMARTAUDIO_ENABLED', 'Enable SmartAudio VTX contol', 0, "VIDEO_TX"), Feature('VTX', 'TRAMP', 'AP_TRAMP_ENABLED', 'Enable IRC Tramp VTX control', 0, "VIDEO_TX"), - Feature('ESC', 'PICCOLOCAN', 'HAL_PICCOLO_CAN_ENABLE', 'Enable PiccoloCAN', 0, 'DroneCAN'), + Feature('ESC', 'PICCOLOCAN', 'AP_PICCOLOCAN_ENABLED', 'Enable PiccoloCAN', 0, 'DroneCAN'), Feature('ESC', 'TORQEEDO', 'HAL_TORQEEDO_ENABLED', 'Enable Torqeedo motors', 0, None), Feature('ESC', 'ESC_EXTENDED_TELM', 'AP_EXTENDED_ESC_TELEM_ENABLED', 'Enable Extended ESC telemetry', 0, 'DroneCAN'), @@ -148,6 +157,7 @@ def config_option(self): Feature('AP_Periph', 'PERIPH_RELAY', 'AP_PERIPH_RELAY_ENABLED', 'Handle DroneCAN hardpoint command', 0, 'RELAY'), Feature('AP_Periph', 'PERIPH_BATTERY_BALANCE', 'AP_PERIPH_BATTERY_BALANCE_ENABLED', 'Emit DroneCAN BatteryInfoAux messages for monitoring Battery Balance', 0, None), # noqa Feature('AP_Periph', 'PERIPH_BATTERY_TAG', 'AP_PERIPH_BATTERY_TAG_ENABLED', 'Emit DroneCAN BatteryTag messages', 0, None), # noqa + Feature('AP_Periph', 'PERIPH_BATTERY_BMS', 'AP_PERIPH_BATTERY_BMS_ENABLED', 'Enable Battery BMS button and LED support', 0, None), # noqa Feature('AP_Periph', 'PERIPH_PROXIMITY', 'AP_PERIPH_PROXIMITY_ENABLED', 'Emit DroneCAN Proximity Messages for AP_Proximity sensors', 0, 'PROXIMITY'), # noqa Feature('AP_Periph', 'PERIPH_GPS', 'AP_PERIPH_GPS_ENABLED', 'Emit DroneCAN GNSS Messages for AP_GPS sensors', 0, None), # noqa Feature('AP_Periph', 'PERIPH_ADSB', 'AP_PERIPH_ADSB_ENABLED', 'Emit DroneCAN TrafficReport Messages for ADSB_VEHICLE MAVLink messages', 0, None), # noqa @@ -230,6 +240,7 @@ def config_option(self): Feature('Gimbal', 'XACTI', 'HAL_MOUNT_XACTI_ENABLED', 'Enable Xacti gimbal', 0, "MOUNT,DroneCAN"), Feature('Gimbal', 'XFROBOT', 'HAL_MOUNT_XFROBOT_ENABLED', 'Enable XFRobot gimbal', 0, "MOUNT"), Feature('Gimbal', 'VIEWPRO', 'HAL_MOUNT_VIEWPRO_ENABLED', 'Enable Viewpro gimbal', 0, "MOUNT"), + Feature('Gimbal', 'POILOCK', 'AP_MOUNT_POI_LOCK_ENABLED', 'Enable Aux Switch POI lock', 0, "MOUNT"), Feature('VTOL Frame', 'TRI', 'AP_MOTORS_TRI_ENABLED', 'TriCopters', 0, None), Feature('VTOL Frame', 'QUAD', 'AP_MOTORS_FRAME_QUAD_ENABLED', 'QUAD', 1, None), @@ -451,6 +462,7 @@ def config_option(self): Feature('GPS Drivers', 'SIRF', 'AP_GPS_SIRF_ENABLED', 'Enable SiRF GPS', 0, None), Feature('GPS Drivers', 'DroneCAN_GPS_Out', 'AP_DRONECAN_SEND_GPS', 'Enable Sending GPS data from Autopilot', 0, "DroneCAN"), # noqa:401 Feature('GPS Drivers', 'GPS_Blending', 'AP_GPS_BLENDED_ENABLED', 'Enable GPS Blending', 0, None), + Feature('GPS Drivers', 'GPS Debug Logging', 'AP_GPS_DEBUG_LOGGING_ENABLED', 'Enable GPS Debug logging', 0, None), Feature('Airspeed Drivers', 'Analog', 'AP_AIRSPEED_ANALOG_ENABLED', 'Enable Analog Airspeed', 0, 'AIRSPEED'), diff --git a/Tools/scripts/build_script_base.py b/Tools/scripts/build_script_base.py index 2ef2c9c4c34c0..ea321db66e048 100644 --- a/Tools/scripts/build_script_base.py +++ b/Tools/scripts/build_script_base.py @@ -10,6 +10,7 @@ import pathlib import string import subprocess +import sys import time @@ -134,4 +135,4 @@ def run_waf(self, args, compiler=None, show_output=True, source_dir=None): def progress(self, string): '''pretty-print progress''' - print(f"{self.progress_prefix()}: {string}") + print(f"{self.progress_prefix()}: {string}", file=sys.stderr) diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index e4637e7da7003..e03ced0cb59f8 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -6,29 +6,19 @@ AP_FLAKE8_CLEAN """ import argparse -import os import re -import string -import subprocess -import sys -import time import build_options -import select +from build_script_base import BuildScriptBase -if sys.version_info[0] < 3: - running_python3 = False -else: - running_python3 = True - - -class ExtractFeatures(object): +class ExtractFeatures(BuildScriptBase): class FindString(object): def __init__(self, string): self.string = string def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): + super().__init__() self.filename = filename self.nm = nm self.strings = strings @@ -117,6 +107,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('HAL_SOLO_GIMBAL_ENABLED', 'AP_Mount_SoloGimbal::init',), ('HAL_MOUNT_STORM32SERIAL_ENABLED', 'AP_Mount_SToRM32_serial::update',), ('HAL_MOUNT_STORM32MAVLINK_ENABLED', 'AP_Mount_SToRM32::update',), + ('AP_MOUNT_POI_LOCK_ENABLED', 'AP_Mount::set_poi_lock'), ('HAL_SPEKTRUM_TELEM_ENABLED', r'AP::spektrum_telem',), ('HAL_{type}_TELEM_ENABLED', r'AP_(?P.*)_Telem::init',), @@ -213,6 +204,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('GPS_MOVING_BASELINE', r'MovingBase::var_info',), ('AP_DRONECAN_SEND_GPS', r'AP_GPS_DroneCAN::instance_exists\b',), ('AP_GPS_BLENDED_ENABLED', r'AP_GPS::calc_blend_weights\b',), + ('AP_GPS_DEBUG_LOGGING_ENABLED', 'AP_GPS_Backend::log_data',), ('HAL_WITH_DSP', r'AP_HAL::DSP::find_peaks\b',), ('AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED', r'AP_InertialSensor::HarmonicNotch::update_params\b',), @@ -225,7 +217,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('AP_BARO_THST_COMP_ENABLED', r'AP_Baro::thrust_pressure_correction\b',), ('AP_TEMPCALIBRATION_ENABLED', r'AP_TempCalibration::apply_calibration',), - ('HAL_PICCOLO_CAN_ENABLE', r'AP_PiccoloCAN::update',), + ('AP_PICCOLOCAN_ENABLED', r'AP_PiccoloCAN::update',), ('EK3_FEATURE_EXTERNAL_NAV', r'NavEKF3_core::CorrectExtNavVelForSensorOffset'), ('EK3_FEATURE_DRAG_FUSION', r'NavEKF3_core::FuseDragForces'), ('EK3_FEATURE_OPTFLOW_FUSION', r'NavEKF3_core::FuseOptFlow'), @@ -315,6 +307,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('AP_PERIPH_RELAY_ENABLED', r'AP_Periph_FW::handle_hardpoint_command'), ('AP_PERIPH_BATTERY_BALANCE_ENABLED', r'AP_Periph_FW::batt_balance_update'), ('AP_PERIPH_BATTERY_TAG_ENABLED', r'BatteryTag::update'), + ('AP_PERIPH_BATTERY_BMS_ENABLED', r'BatteryBMS::update'), ('AP_PERIPH_PROXIMITY_ENABLED', r'AP_Periph_FW::can_proximity_update'), ('AP_PERIPH_GPS_ENABLED', r'AP_Periph_FW::can_gps_update'), ('AP_PERIPH_ADSB_ENABLED', r'AP_Periph_FW::adsb_update'), @@ -332,9 +325,9 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('AP_SCRIPTING_BINDING_MOTORS_ENABLED', 'AP__motors___index'), ] - def progress(self, msg): - """Pretty-print progress.""" - print("EF: %s" % msg) + def progress_prefix(self): + """Return prefix for progress messages.""" + return 'EF' def validate_features_list(self): '''ensures that every define present in build_options.py could be @@ -360,59 +353,6 @@ def validate_features_list(self): raise ValueError("feature (%s) is not matched in extract_features" % (option.define)) - def run_program(self, prefix, cmd_list, show_output=True, env=None): - """Swiped from build_binaries.py.""" - if show_output: - self.progress("Running (%s)" % " ".join(cmd_list)) - p = subprocess.Popen( - cmd_list, - stdin=None, - stdout=subprocess.PIPE, - close_fds=True, - stderr=subprocess.PIPE, - env=env) - stderr = bytearray() - output = "" - while True: - # read all of stderr: - while True: - (rin, _, _) = select.select([p.stderr.fileno()], [], [], 0) - if p.stderr.fileno() not in rin: - break - new = p.stderr.read() - if len(new) == 0: - break - stderr += new - - x = p.stdout.readline() - if len(x) == 0: - (rin, _, _) = select.select([p.stderr.fileno()], [], [], 0) - if p.stderr.fileno() in rin: - stderr += p.stderr.read() - - returncode = os.waitpid(p.pid, 0) - if returncode: - break - # select not available on Windows... probably... - time.sleep(0.1) - continue - if running_python3: - x = bytearray(x) - x = filter(lambda x: chr(x) in string.printable, x) - x = "".join([chr(c) for c in x]) - output += x - x = x.rstrip() - if show_output: - print("%s: %s" % (prefix, x)) - (_, status) = returncode - if status != 0: - stderr = stderr.decode('utf-8') - self.progress("Process failed (%s) (%s)" % - (str(returncode), stderr)) - raise subprocess.CalledProcessError( - status, cmd_list, output=str(output), stderr=str(stderr)) - return output - class Symbols(object): def __init__(self): self.symbols = dict() @@ -449,7 +389,7 @@ def extract_symbols_from_elf(self, filename): '--demangle', '--print-size', filename - ], show_output=False) + ], show_output=False, show_command=False) ret = ExtractFeatures.Symbols() for line in text_output.split("\n"): m = re.match("^([^ ]+) ([^ ]+) ([^ ]) (.*)", line.rstrip()) @@ -476,7 +416,7 @@ def extract_strings_from_elf(self, filename): text_output = self.run_program('EF', [ self.strings, filename - ], show_output=False) + ], show_output=False, show_command=False) return text_output.split("\n") def extract(self): diff --git a/Tools/scripts/generate_manifest.py b/Tools/scripts/generate_manifest.py index a3a0ac35db457..250ff1ced2fa8 100755 --- a/Tools/scripts/generate_manifest.py +++ b/Tools/scripts/generate_manifest.py @@ -14,16 +14,6 @@ import shutil import sys -if sys.version_info[0] < 3: - running_python3 = False - running_python310 = False -elif sys.version_info[1] < 10: - running_python3 = True - running_python310 = False -else: - running_python3 = True - running_python310 = True - FIRMWARE_TYPES = ["AntennaTracker", "Copter", "Plane", "Rover", "Sub", "AP_Periph", "Blimp"] RELEASE_TYPES = ["beta", "latest", "stable", "stable-*", "dirty"] @@ -339,8 +329,7 @@ def firmware_format_for_filepath(self, filepath): return "".join(filename.split(".")[-1:]) # no extension; ensure this is an elf: text = subprocess.check_output(["file", "-b", filepath]) - if running_python3: - text = text.decode('ascii') + text = text.decode('ascii') if re.match("^ELF", text): return "ELF" @@ -638,8 +627,7 @@ def write_json(self, content, path): # "gzip -9"s to 300k in 1 second, "xz -e"s to 80k in 26 seconds new_json_filepath_gz = path + ".gz.new" with gzip.open(new_json_filepath_gz, 'wb') as gf: - if running_python3: - content = bytes(content, 'ascii') + content = bytes(content, 'ascii') gf.write(content) gf.close() shutil.move(new_json_filepath, path) diff --git a/Tools/scripts/signing/make_secure_bl.py b/Tools/scripts/signing/make_secure_bl.py index 1226d5a6b968b..0a8bafb738ed8 100755 --- a/Tools/scripts/signing/make_secure_bl.py +++ b/Tools/scripts/signing/make_secure_bl.py @@ -12,7 +12,7 @@ # get command line arguments from argparse import ArgumentParser -parser = ArgumentParser(description='make_secure_bl') +parser = ArgumentParser(description='This program is used to apply signing keys to a signed bootloader.') parser.add_argument("--omit-ardupilot-keys", action='store_true', default=False, help="omit ArduPilot signing keys") parser.add_argument("bootloader", type=str, default=None, help="bootloader") parser.add_argument("keys", nargs='*', type=str, default=[], help="keys") @@ -70,4 +70,4 @@ def decode_key(ktype, key): desc += key desc_len += key_len img = img[:offset] + desc + img[offset+desc_len:] -open(sys.argv[1], 'wb').write(img) +open(args.bootloader, 'wb').write(img) diff --git a/Tools/scripts/signing/make_secure_fw.py b/Tools/scripts/signing/make_secure_fw.py index 49e4a2b32b60d..f72cfdf34a94d 100755 --- a/Tools/scripts/signing/make_secure_fw.py +++ b/Tools/scripts/signing/make_secure_fw.py @@ -17,29 +17,28 @@ sys.exit(1) if monocypher.__version__ != "3.1.3.2": - Logs.error("must use monocypher 3.1.3.2, please run: python3 -m pip install pymonocypher==3.1.3.2") - return None - + print("must use monocypher 3.1.3.2, please run: python3 -m pip install pymonocypher==3.1.3.2") + sys.exit(1) + +from argparse import ArgumentParser +parser = ArgumentParser(description='This program is used to sign the firmware file.') +parser.add_argument('apj_file', metavar='apj-file', type=str, help='Firmware (APJ) to sign') +parser.add_argument('key_file', metavar='key-file', type=str, help='Private key used to sign the firmware') +args = parser.parse_args() + key_len = 32 sig_len = 64 sig_version = 30437 descriptor = b'\x41\xa3\xe5\xf2\x65\x69\x92\x07' -if len(sys.argv) < 3: - print("Usage: make_secure_fw.py APJ_FILE PRIVATE_KEYFILE") - sys.exit(1) - def to_unsigned(i): '''convert a possibly signed integer to unsigned''' if i < 0: i += 2**32 return i -apj_file = sys.argv[1] -key_file = sys.argv[2] - # open apj file -apj = open(apj_file, 'r').read() +apj = open(args.apj_file, 'r').read() # decode json in apj d = json.loads(apj) @@ -55,7 +54,7 @@ def decode_key(ktype, key): sys.exit(1) return base64.b64decode(key[len(ktype):]) -key = decode_key("PRIVATE", open(key_file, 'r').read()) +key = decode_key("PRIVATE", open(args.key_file, 'r').read()) if len(key) != key_len: print("Bad key length %u" % len(key)) sys.exit(1) @@ -92,7 +91,7 @@ def decode_key(ktype, key): d["image"] = base64.b64encode(zlib.compress(img,9)).decode('utf-8') d["signed_firmware"] = True -f = open(sys.argv[1], "w") +f = open(args.apj_file, "w") f.write(json.dumps(d, indent=4)) f.close() -print("Wrote %s" % apj_file) +print("Wrote %s" % args.apj_file) diff --git a/Tools/scripts/size_compare_branches.py b/Tools/scripts/size_compare_branches.py index 76ce43bb22a06..3deaa25edeb77 100755 --- a/Tools/scripts/size_compare_branches.py +++ b/Tools/scripts/size_compare_branches.py @@ -179,8 +179,6 @@ def __init__(self, 'skyviper-journey', 'Pixhawk1-1M-bdshot', 'Pixhawk1-bdshot', - 'SITL_arm_linux_gnueabihf', - 'SITL_x86_64_linux_gnu', 'RADIX2HD', 'canzero', 't3-gem-o1', @@ -213,7 +211,7 @@ def __init__(self, for board_name in self.board: board = self.boards_by_name[board_name] - if board.hal in ["Linux", "ESP32"]: + if board.hal in ["Linux", "ESP32", "SITL"]: self.bootloader_blacklist.add(board.name) def find_bin_dir(self, toolchain_prefix="arm-none-eabi-"): @@ -290,7 +288,12 @@ def vehicles_to_build_for_board_info(self, board_info): else: if board_info.is_ap_periph: continue - if vehicle.lower() not in [x.lower() for x in board_info.autobuild_targets]: + # Map vehicle name to autobuild target name + # antennatracker (waf target) -> Tracker (autobuild target) + vehicle_for_autobuild = vehicle + if vehicle.lower() == 'antennatracker': + vehicle_for_autobuild = 'tracker' + if vehicle_for_autobuild.lower() not in [x.lower() for x in board_info.autobuild_targets]: continue vehicles_to_build.append(vehicle) diff --git a/Tools/scripts/test_new_boards.py b/Tools/scripts/test_new_boards.py index 79495cbd93781..7c6c2509fe123 100755 --- a/Tools/scripts/test_new_boards.py +++ b/Tools/scripts/test_new_boards.py @@ -108,6 +108,7 @@ class BoardToTest(): if hwdef_idx + 1 >= len(parts): raise ValueError(f"No board name after hwdef in path: {hwdef_filepath}") board_name = parts[hwdef_idx + 1] + if board_name not in boards_to_test: boards_to_test[board_name] = BoardToTest() if hwdef_filepath.endswith('hwdef-bl.dat'): @@ -117,6 +118,30 @@ class BoardToTest(): # Build each unique board (find it in board list as an additional check) bl = board_list.BoardList() + + # First pass: check for README.md for non-AP_Periph boards + for hwdef_filepath in hwdef_filepaths: + # Extract board name from path like libraries/AP_HAL_ChibiOS/hwdef/BoardName/hwdef.dat + parts = hwdef_filepath.split('/') + hwdef_idx = parts.index('hwdef') + board_name = parts[hwdef_idx + 1] + + # Find the board object to check if it's AP_Periph + board = None + for b in bl.boards: + if b.name == board_name: + board = b + break + + if board is None: + raise ValueError(f"Board {board_name} not found in board list") + + # Only require README.md for non-AP_Periph boards + if not board.is_ap_periph: + hwdef_dir = os.path.join(*parts[:hwdef_idx + 2]) + readme_path = os.path.join(hwdef_dir, 'README.md') + if not os.path.exists(readme_path): + raise ValueError(f"Missing README.md for new board: {readme_path} does not exist") for board_name in sorted(boards_to_test.keys()): # Find the board object board = None @@ -133,6 +158,11 @@ class BoardToTest(): self.progress(f"Skipping ESP32 board {board.name}") continue + # Skip arms board - CI machine can't build it + if board.toolchain == "arm-linux-gnueabihf": + self.progress(f"Skipping arm-linux board {board.name}") + continue + self.progress(f"Building board {board.name}") if boards_to_test[board_name].test_vehicles: self.build_board(board) diff --git a/Tools/scripts/uploader.py b/Tools/scripts/uploader.py index c2a3f0b3feb92..19e0fdf715982 100755 --- a/Tools/scripts/uploader.py +++ b/Tools/scripts/uploader.py @@ -276,7 +276,7 @@ def __init__(self, self.identify_only = identify_only # open the port, keep the default timeout short so we can poll quickly - self.port = serial.Serial(portname, baudrate_bootloader, timeout=2.0, write_timeout=2.0) + self.port = serial.Serial(portname, baudrate_bootloader, timeout=2.0, write_timeout=2.0, exclusive=True) self.baudrate_bootloader = baudrate_bootloader if baudrate_bootloader_flash is not None: self.baudrate_bootloader_flash = baudrate_bootloader_flash diff --git a/Tools/simulink/arducopter/sid_sim_init.m b/Tools/simulink/arducopter/sid_sim_init.m index 494ab3fc66dc9..30201f15be116 100644 --- a/Tools/simulink/arducopter/sid_sim_init.m +++ b/Tools/simulink/arducopter/sid_sim_init.m @@ -654,35 +654,35 @@ simIn.param.PSCD.VEL_MAX_DN = getParamVal(sid, 'PILOT_SPEED_DN'); % Maximum vertical descending velocity in cm/s, defined in parameters.cpp, line 906 simIn.param.PSCD.VEL_MAX_UP = getParamVal(sid, 'PILOT_SPEED_UP'); % Maximum vertical ascending velocity in cm/s, defined in parameters.cpp, line 223 simIn.param.PSCD.ACC_MAX_Z = getParamVal(sid, 'PILOT_ACCEL_Z'); % Maximum vertical acceleration used when pilot is controlling the altitude, parameters.cpp, line 232 -simIn.param.PSCD.JERK_MAX_Z = getParamVal(sid, 'PSC_JERK_Z'); % Jerk limit of vertical kinematic path generation in m/s^3. Determines how quickly aircraft changes acceleration target +simIn.param.PSCD.JERK_MAX_Z = getParamVal(sid, 'PSC_JERK_D'); % Jerk limit of vertical kinematic path generation in m/s^3. Determines how quickly aircraft changes acceleration target simIn.param.PSCD.OVERSPEED_GAIN_Z = single(2); % gain controlling rate at which z-axis speed is brought back within SPEED_UP and SPEED_DOWN range, defined in AC_PosControl.h %% Position Controller z % Parameters z position controller -simIn.param.PSCD.P_POS = getParamVal(sid, 'PSC_POSZ_P'); % P gain of the vertical position controller +simIn.param.PSCD.P_POS = getParamVal(sid, 'PSC_D_POS_P'); % P gain of the vertical position controller % Parameters z velocity controller -simIn.param.PSCD.P_VEL = getParamVal(sid, 'PSC_VELZ_P'); % P gain of the vertical velocity controller -simIn.param.PSCD.I_VEL = getParamVal(sid, 'PSC_VELZ_I'); % I gain of the vertical velocity controller -simIn.param.PSCD.D_VEL = getParamVal(sid, 'PSC_VELZ_D'); % D gain of the vertical velocity controller -simIn.param.PSCD.IMAX_VEL = getParamVal(sid, 'PSC_VELZ_IMAX'); % I gain maximu vertical velocity controller -simIn.param.PSCD.FF_VEL = getParamVal(sid, 'PSC_VELZ_FF'); % Feed Forward gain of the vertical velocity controller -simIn.param.PSCD.FLTE_VEL = getParamVal(sid, 'PSC_VELZ_FLTE'); % Cutoff frequency of the error filter (in Hz) -simIn.param.PSCD.FLTD_VEL = getParamVal(sid, 'PSC_VELZ_FLTD'); % Cutoff frequency of the D term filter (in Hz) +simIn.param.PSCD.P_VEL = getParamVal(sid, 'PSC_D_VEL_P'); % P gain of the vertical velocity controller +simIn.param.PSCD.I_VEL = getParamVal(sid, 'PSC_D_VEL_I'); % I gain of the vertical velocity controller +simIn.param.PSCD.D_VEL = getParamVal(sid, 'PSC_D_VEL_D'); % D gain of the vertical velocity controller +simIn.param.PSCD.IMAX_VEL = getParamVal(sid, 'PSC_D_VEL_IMAX'); % I gain maximu vertical velocity controller +simIn.param.PSCD.FF_VEL = getParamVal(sid, 'PSC_D_VEL_FF'); % Feed Forward gain of the vertical velocity controller +simIn.param.PSCD.FLTE_VEL = getParamVal(sid, 'PSC_D_VEL_FLTE'); % Cutoff frequency of the error filter (in Hz) +simIn.param.PSCD.FLTD_VEL = getParamVal(sid, 'PSC_D_VEL_FLTD'); % Cutoff frequency of the D term filter (in Hz) simIn.param.PSCD.CTRL_RATIO_INIT = single(2.0); % Initial value of _vel_z_control_ratio, defined in PosControl.h, line 456 % Parameters z acceleration controller -simIn.param.PIDA.P = getParamVal(sid, 'PSC_ACCZ_P'); % P gain of the vertical acceleration controller -simIn.param.PIDA.I = getParamVal(sid, 'PSC_ACCZ_I'); % I gain of the vertical acceleration controller -simIn.param.PIDA.D = getParamVal(sid, 'PSC_ACCZ_D'); % D gain of the vertical acceleration controller -simIn.param.PIDA.IMAX = getParamVal(sid, 'PSC_ACCZ_IMAX'); % I gain maximu vertical acceleration controller -simIn.param.PIDA.PDMX = getParamVal(sid, 'PSC_ACCZ_PDMX'); -simIn.param.PIDA.FF = getParamVal(sid, 'PSC_ACCZ_FF'); % Feed Forward gain of the vertical acceleration controller -simIn.param.PIDA.DFF = getParamVal(sid, 'PSC_ACCZ_D_FF'); % Derivative Feed Forward gain of the vertical acceleration controller -simIn.param.PIDA.FLTE_f = getParamVal(sid, 'PSC_ACCZ_FLTE'); % Cutoff frequency of the error filter (in Hz) -simIn.param.PIDA.FLTD_f = getParamVal(sid, 'PSC_ACCZ_FLTD'); % Cutoff frequency of the D term filter (in Hz) -simIn.param.PIDA.FLTT_f = getParamVal(sid, 'PSC_ACCZ_FLTT'); % Cutoff frequency of the target filter (in Hz) -simIn.param.PIDA.SR_MAX = getParamVal(sid, 'PSC_ACCZ_SMAX'); % Upper limit of the slew rate produced by combined P and D gains +simIn.param.PIDA.P = getParamVal(sid, 'PSC_D_ACC_P'); % P gain of the vertical acceleration controller +simIn.param.PIDA.I = getParamVal(sid, 'PSC_D_ACC_I'); % I gain of the vertical acceleration controller +simIn.param.PIDA.D = getParamVal(sid, 'PSC_D_ACC_D'); % D gain of the vertical acceleration controller +simIn.param.PIDA.IMAX = getParamVal(sid, 'PSC_D_ACC_IMAX'); % I gain maximu vertical acceleration controller +simIn.param.PIDA.PDMX = getParamVal(sid, 'PSC_D_ACC_PDMX'); +simIn.param.PIDA.FF = getParamVal(sid, 'PSC_D_ACC_FF'); % Feed Forward gain of the vertical acceleration controller +simIn.param.PIDA.DFF = getParamVal(sid, 'PSC_D_ACC_D_FF'); % Derivative Feed Forward gain of the vertical acceleration controller +simIn.param.PIDA.FLTE_f = getParamVal(sid, 'PSC_D_ACC_FLTE'); % Cutoff frequency of the error filter (in Hz) +simIn.param.PIDA.FLTD_f = getParamVal(sid, 'PSC_D_ACC_FLTD'); % Cutoff frequency of the D term filter (in Hz) +simIn.param.PIDA.FLTT_f = getParamVal(sid, 'PSC_D_ACC_FLTT'); % Cutoff frequency of the target filter (in Hz) +simIn.param.PIDA.SR_MAX = getParamVal(sid, 'PSC_D_ACC_SMAX'); % Upper limit of the slew rate produced by combined P and D gains simIn.param.PIDA.SR_TAU = single(1.0); % Slew Rate Tau - not yet available as a parameter of the copter. Set to 1.0 by default (AC_PID.h, line 24). simIn.param.PIDA.SR_FLT_f = single(25.0); % Slew Rate lowpass filter cutoff frequency. Defined in SlewLimiter.h, line 14. diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index a36cc40b8cb9a..b2d91590f90b2 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -86,6 +86,7 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = { // @Description: Roll axis angle controller P gain. Converts the error between the desired roll angle and actual angle to a desired roll rate // @Range: 3.000 12.000 // @Range{Sub}: 0.0 12.000 + // @Increment: 0.01 // @User: Standard AP_SUBGROUPINFO(_p_angle_roll, "ANG_RLL_", 13, AC_AttitudeControl, AC_P), @@ -94,6 +95,7 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = { // @Description: Pitch axis angle controller P gain. Converts the error between the desired pitch angle and actual angle to a desired pitch rate // @Range: 3.000 12.000 // @Range{Sub}: 0.0 12.000 + // @Increment: 0.01 // @User: Standard AP_SUBGROUPINFO(_p_angle_pitch, "ANG_PIT_", 14, AC_AttitudeControl, AC_P), @@ -102,6 +104,7 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = { // @Description: Yaw axis angle controller P gain. Converts the error between the desired yaw angle and actual angle to a desired yaw rate // @Range: 3.000 12.000 // @Range{Sub}: 0.0 6.000 + // @Increment: 0.01 // @User: Standard AP_SUBGROUPINFO(_p_angle_yaw, "ANG_YAW_", 15, AC_AttitudeControl, AC_P), diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp index cddc4c830079b..56c872898fbe9 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp @@ -31,7 +31,7 @@ void AC_AttitudeControl::Write_ANG() const void AC_AttitudeControl::Write_Rate(const AC_PosControl &pos_control) const { const Vector3f rate_targets_degs = rate_bf_targets() * RAD_TO_DEG; - const Vector3f &accel_target_neu_cmss = pos_control.get_accel_target_NEU_mss() * 100.0; + const Vector3f &accel_target_ned_mss = pos_control.get_accel_target_NED_mss(); const Vector3f gyro_rate = _rate_gyro_rads * RAD_TO_DEG; const struct log_Rate pkt_rate{ LOG_PACKET_HEADER_INIT(LOG_RATE_MSG), @@ -45,8 +45,8 @@ void AC_AttitudeControl::Write_Rate(const AC_PosControl &pos_control) const control_yaw : rate_targets_degs.z, yaw : gyro_rate.z, yaw_out : _motors.get_yaw()+_motors.get_yaw_ff(), - control_accel : (float)accel_target_neu_cmss.z, - accel : (float)(-(_ahrs.get_accel_ef().z + GRAVITY_MSS) * 100.0f), + control_accel : (float)(-accel_target_ned_mss.z), + accel : (float)(-(_ahrs.get_accel_ef().z + GRAVITY_MSS)), accel_out : _motors.get_throttle(), throttle_slew : _motors.get_throttle_slew_rate() }; diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 093b3234aafb5..07a9fcd45b988 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -10,64 +10,64 @@ extern const AP_HAL::HAL& hal; #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) // default gains for Plane - # define POSCONTROL_POS_Z_P 1.0f // vertical position controller P gain default - # define POSCONTROL_VEL_Z_P 5.0f // vertical velocity controller P gain default - # define POSCONTROL_VEL_Z_IMAX 1000.0f // vertical velocity controller IMAX gain default - # define POSCONTROL_VEL_Z_FILT_HZ 5.0f // vertical velocity controller input filter - # define POSCONTROL_VEL_Z_FILT_D_HZ 5.0f // vertical velocity controller input filter for D - # define POSCONTROL_ACC_Z_P 0.3f // vertical acceleration controller P gain default - # define POSCONTROL_ACC_Z_I 1.0f // vertical acceleration controller I gain default - # define POSCONTROL_ACC_Z_D 0.0f // vertical acceleration controller D gain default - # define POSCONTROL_ACC_Z_IMAX 800 // vertical acceleration controller IMAX gain default - # define POSCONTROL_ACC_Z_FILT_HZ 10.0f // vertical acceleration controller input filter default - # define POSCONTROL_ACC_Z_DT 0.02f // vertical acceleration controller dt default - # define POSCONTROL_POS_XY_P 0.5f // horizontal position controller P gain default - # define POSCONTROL_VEL_XY_P 0.7f // horizontal velocity controller P gain default - # define POSCONTROL_VEL_XY_I 0.35f // horizontal velocity controller I gain default - # define POSCONTROL_VEL_XY_D 0.17f // horizontal velocity controller D gain default - # define POSCONTROL_VEL_XY_IMAX 1000.0f // horizontal velocity controller IMAX gain default - # define POSCONTROL_VEL_XY_FILT_HZ 5.0f // horizontal velocity controller input filter - # define POSCONTROL_VEL_XY_FILT_D_HZ 5.0f // horizontal velocity controller input filter for D + # define POSCONTROL_D_POS_P 1.0f // vertical position controller P gain default + # define POSCONTROL_D_VEL_P 5.0f // vertical velocity controller P gain default + # define POSCONTROL_D_VEL_IMAX 10.0f // vertical velocity controller IMAX gain default + # define POSCONTROL_D_VEL_FILT_HZ 5.0f // vertical velocity controller input filter + # define POSCONTROL_D_VEL_FILT_D_HZ 5.0f // vertical velocity controller input filter for D + # define POSCONTROL_D_ACC_P 0.03f // vertical acceleration controller P gain default + # define POSCONTROL_D_ACC_I 0.1f // vertical acceleration controller I gain default + # define POSCONTROL_D_ACC_D 0.0f // vertical acceleration controller D gain default + # define POSCONTROL_D_ACC_IMAX 0.8f // vertical acceleration controller IMAX gain default + # define POSCONTROL_D_ACC_FILT_HZ 10.0f // vertical acceleration controller input filter default + # define POSCONTROL_D_ACC_DT 0.02f // vertical acceleration controller dt default + # define POSCONTROL_NE_POS_P 0.5f // horizontal position controller P gain default + # define POSCONTROL_NE_VEL_P 0.7f // horizontal velocity controller P gain default + # define POSCONTROL_NE_VEL_I 0.35f // horizontal velocity controller I gain default + # define POSCONTROL_NE_VEL_D 0.17f // horizontal velocity controller D gain default + # define POSCONTROL_NE_VEL_IMAX 10.0f // horizontal velocity controller IMAX gain default + # define POSCONTROL_NE_VEL_FILT_HZ 5.0f // horizontal velocity controller input filter + # define POSCONTROL_NE_VEL_FILT_D_HZ 5.0f // horizontal velocity controller input filter for D #elif APM_BUILD_TYPE(APM_BUILD_ArduSub) // default gains for Sub - # define POSCONTROL_POS_Z_P 3.0f // vertical position controller P gain default - # define POSCONTROL_VEL_Z_P 8.0f // vertical velocity controller P gain default - # define POSCONTROL_VEL_Z_IMAX 1000.0f // vertical velocity controller IMAX gain default - # define POSCONTROL_VEL_Z_FILT_HZ 5.0f // vertical velocity controller input filter - # define POSCONTROL_VEL_Z_FILT_D_HZ 5.0f // vertical velocity controller input filter for D - # define POSCONTROL_ACC_Z_P 0.5f // vertical acceleration controller P gain default - # define POSCONTROL_ACC_Z_I 0.1f // vertical acceleration controller I gain default - # define POSCONTROL_ACC_Z_D 0.0f // vertical acceleration controller D gain default - # define POSCONTROL_ACC_Z_IMAX 100 // vertical acceleration controller IMAX gain default - # define POSCONTROL_ACC_Z_FILT_HZ 20.0f // vertical acceleration controller input filter default - # define POSCONTROL_ACC_Z_DT 0.0025f // vertical acceleration controller dt default - # define POSCONTROL_POS_XY_P 1.0f // horizontal position controller P gain default - # define POSCONTROL_VEL_XY_P 1.0f // horizontal velocity controller P gain default - # define POSCONTROL_VEL_XY_I 0.5f // horizontal velocity controller I gain default - # define POSCONTROL_VEL_XY_D 0.0f // horizontal velocity controller D gain default - # define POSCONTROL_VEL_XY_IMAX 1000.0f // horizontal velocity controller IMAX gain default - # define POSCONTROL_VEL_XY_FILT_HZ 5.0f // horizontal velocity controller input filter - # define POSCONTROL_VEL_XY_FILT_D_HZ 5.0f // horizontal velocity controller input filter for D + # define POSCONTROL_D_POS_P 3.0f // vertical position controller P gain default + # define POSCONTROL_D_VEL_P 8.0f // vertical velocity controller P gain default + # define POSCONTROL_D_VEL_IMAX 10.0f // vertical velocity controller IMAX gain default + # define POSCONTROL_D_VEL_FILT_HZ 5.0f // vertical velocity controller input filter + # define POSCONTROL_D_VEL_FILT_D_HZ 5.0f // vertical velocity controller input filter for D + # define POSCONTROL_D_ACC_P 0.05f // vertical acceleration controller P gain default + # define POSCONTROL_D_ACC_I 0.01f // vertical acceleration controller I gain default + # define POSCONTROL_D_ACC_D 0.0f // vertical acceleration controller D gain default + # define POSCONTROL_D_ACC_IMAX 0.1f // vertical acceleration controller IMAX gain default + # define POSCONTROL_D_ACC_FILT_HZ 20.0f // vertical acceleration controller input filter default + # define POSCONTROL_D_ACC_DT 0.0025f // vertical acceleration controller dt default + # define POSCONTROL_NE_POS_P 1.0f // horizontal position controller P gain default + # define POSCONTROL_NE_VEL_P 1.0f // horizontal velocity controller P gain default + # define POSCONTROL_NE_VEL_I 0.5f // horizontal velocity controller I gain default + # define POSCONTROL_NE_VEL_D 0.0f // horizontal velocity controller D gain default + # define POSCONTROL_NE_VEL_IMAX 10.0f // horizontal velocity controller IMAX gain default + # define POSCONTROL_NE_VEL_FILT_HZ 5.0f // horizontal velocity controller input filter + # define POSCONTROL_NE_VEL_FILT_D_HZ 5.0f // horizontal velocity controller input filter for D #else // default gains for Copter / TradHeli - # define POSCONTROL_POS_Z_P 1.0f // vertical position controller P gain default - # define POSCONTROL_VEL_Z_P 5.0f // vertical velocity controller P gain default - # define POSCONTROL_VEL_Z_IMAX 1000.0f // vertical velocity controller IMAX gain default - # define POSCONTROL_VEL_Z_FILT_HZ 5.0f // vertical velocity controller input filter - # define POSCONTROL_VEL_Z_FILT_D_HZ 5.0f // vertical velocity controller input filter for D - # define POSCONTROL_ACC_Z_P 0.5f // vertical acceleration controller P gain default - # define POSCONTROL_ACC_Z_I 1.0f // vertical acceleration controller I gain default - # define POSCONTROL_ACC_Z_D 0.0f // vertical acceleration controller D gain default - # define POSCONTROL_ACC_Z_IMAX 800 // vertical acceleration controller IMAX gain default - # define POSCONTROL_ACC_Z_FILT_HZ 20.0f // vertical acceleration controller input filter default - # define POSCONTROL_ACC_Z_DT 0.0025f // vertical acceleration controller dt default - # define POSCONTROL_POS_XY_P 1.0f // horizontal position controller P gain default - # define POSCONTROL_VEL_XY_P 2.0f // horizontal velocity controller P gain default - # define POSCONTROL_VEL_XY_I 1.0f // horizontal velocity controller I gain default - # define POSCONTROL_VEL_XY_D 0.25f // horizontal velocity controller D gain default - # define POSCONTROL_VEL_XY_IMAX 1000.0f // horizontal velocity controller IMAX gain default - # define POSCONTROL_VEL_XY_FILT_HZ 5.0f // horizontal velocity controller input filter - # define POSCONTROL_VEL_XY_FILT_D_HZ 5.0f // horizontal velocity controller input filter for D + # define POSCONTROL_D_POS_P 1.0f // vertical position controller P gain default + # define POSCONTROL_D_VEL_P 5.0f // vertical velocity controller P gain default + # define POSCONTROL_D_VEL_IMAX 10.0f // vertical velocity controller IMAX gain default + # define POSCONTROL_D_VEL_FILT_HZ 5.0f // vertical velocity controller input filter + # define POSCONTROL_D_VEL_FILT_D_HZ 5.0f // vertical velocity controller input filter for D + # define POSCONTROL_D_ACC_P 0.05f // vertical acceleration controller P gain default + # define POSCONTROL_D_ACC_I 0.1f // vertical acceleration controller I gain default + # define POSCONTROL_D_ACC_D 0.0f // vertical acceleration controller D gain default + # define POSCONTROL_D_ACC_IMAX 0.8f // vertical acceleration controller IMAX gain default + # define POSCONTROL_D_ACC_FILT_HZ 20.0f // vertical acceleration controller input filter default + # define POSCONTROL_D_ACC_DT 0.0025f // vertical acceleration controller dt default + # define POSCONTROL_NE_POS_P 1.0f // horizontal position controller P gain default + # define POSCONTROL_NE_VEL_P 2.0f // horizontal velocity controller P gain default + # define POSCONTROL_NE_VEL_I 1.0f // horizontal velocity controller I gain default + # define POSCONTROL_NE_VEL_D 0.25f // horizontal velocity controller D gain default + # define POSCONTROL_NE_VEL_IMAX 10.0f // horizontal velocity controller IMAX gain default + # define POSCONTROL_NE_VEL_FILT_HZ 5.0f // horizontal velocity controller input filter + # define POSCONTROL_NE_VEL_FILT_D_HZ 5.0f // horizontal velocity controller input filter for D #endif // vibration compensation gains @@ -84,238 +84,255 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = { // POS_ACC_XY_FILT was here. - // @Param: _POSZ_P + // @Param: _D_POS_P // @DisplayName: Position (vertical) controller P gain - // @Description: Position (vertical) controller P gain. Converts the difference between the desired altitude and actual altitude into a climb or descent rate which is passed to the throttle rate controller - // @Range: 1.000 3.000 + // @Description: Position (vertical) controller P gain. Converts the difference between the desired altitude and actual altitude into a climb or descent rate which is passed to the throttle rate controller. Previously _POSZ_P. + // @Range: 0.50 4.00 + // @Increment: 0.01 + // @User: Standard + AP_SUBGROUPINFO(_p_pos_d_m, "_D_POS_", 2, AC_PosControl, AC_P_1D), + + // 3 was _VELZ_ which has become _D_VEL_ + + // 4 was _ACCZ_ which has become _D_ACC_ + + // @Param: _NE_POS_P + // @DisplayName: Position (horizontal) controller P gain + // @Description: Position controller P gain. Converts the distance (in the latitude direction) to the target location into a desired speed which is then passed to the loiter latitude rate controller. Previously _POSXY_P. + // @Range: 0.50 4.00 + // @Increment: 0.01 // @User: Standard - AP_SUBGROUPINFO(_p_pos_u_m, "_POSZ_", 2, AC_PosControl, AC_P_1D), + AP_SUBGROUPINFO(_p_pos_ne_m, "_NE_POS_", 5, AC_PosControl, AC_P_2D), + + // 6 was _VELXY_ which has become _NE_VEL_ + + // @Param: _ANGLE_MAX + // @DisplayName: Position Control Angle Max + // @Description: Maximum lean angle autopilot can request. Set to zero to use ANGLE_MAX parameter value + // @Units: deg + // @Range: 0 45 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("_ANGLE_MAX", 7, AC_PosControl, _lean_angle_max_deg, 0.0f), + + // IDs 8,9 used for _TC_XY and _TC_Z in beta release candidate - // @Param: _VELZ_P + // @Param: _JERK_NE + // @DisplayName: Jerk limit for the horizontal kinematic input shaping + // @Description: Jerk limit of the horizontal kinematic path generation used to determine how quickly the aircraft varies the acceleration target + // @Units: m/s/s/s + // @Range: 1 50 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("_JERK_NE", 10, AC_PosControl, _shaping_jerk_ne_msss, POSCONTROL_JERK_NE_MSSS), + + // @Param: _JERK_D + // @DisplayName: Jerk limit for the vertical kinematic input shaping + // @Description: Jerk limit of the vertical kinematic path generation used to determine how quickly the aircraft varies the acceleration target + // @Units: m/s/s/s + // @Range: 1 50 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("_JERK_D", 11, AC_PosControl, _shaping_jerk_d_msss, POSCONTROL_JERK_D_MSSS), + + // @Param: _D_VEL_P // @DisplayName: Velocity (vertical) controller P gain - // @Description: Velocity (vertical) controller P gain. Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller - // @Range: 1.000 8.000 + // @Description: Velocity (vertical) controller P gain. Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller. Previously _VELZ_P. + // @Range: 1.0 10.0 + // @Increment: 0.1 // @User: Standard - // @Param: _VELZ_I + // @Param: _D_VEL_I // @DisplayName: Velocity (vertical) controller I gain - // @Description: Velocity (vertical) controller I gain. Corrects long-term difference in desired velocity to a target acceleration - // @Range: 0.02 1.00 - // @Increment: 0.01 + // @Description: Velocity (vertical) controller I gain. Corrects long-term difference in desired velocity to a target acceleration. Previously _VELZ_I. + // @Range: 0.00 10.0 + // @Increment: 0.1 // @User: Advanced - // @Param: _VELZ_IMAX + // @Param: _D_VEL_IMAX // @DisplayName: Velocity (vertical) controller I gain maximum - // @Description: Velocity (vertical) controller I gain maximum. Constrains the target acceleration that the I gain will output - // @Range: 1.000 8.000 + // @Description: Velocity (vertical) controller I gain maximum. Constrains the target acceleration that the I gain will output. If upgrading from 4.6 this is _VELZ_IMAX * 0.01. + // @Range: 1.000 10.000 + // @Increment: 0.1 // @User: Standard - // @Param: _VELZ_D + // @Param: _D_VEL_D // @DisplayName: Velocity (vertical) controller D gain - // @Description: Velocity (vertical) controller D gain. Corrects short-term changes in velocity - // @Range: 0.00 1.00 - // @Increment: 0.001 + // @Description: Velocity (vertical) controller D gain. Corrects short-term changes in velocity. Previously _VELZ_D. + // @Range: 0.00 2.00 + // @Increment: 0.01 // @User: Advanced - // @Param: _VELZ_FF + // @Param: _D_VEL_FF // @DisplayName: Velocity (vertical) controller Feed Forward gain - // @Description: Velocity (vertical) controller Feed Forward gain. Produces an output that is proportional to the magnitude of the target - // @Range: 0 1 + // @Description: Velocity (vertical) controller Feed Forward gain. Produces an output that is proportional to the magnitude of the target. Previously _VELZ_FF. + // @Range: 0.00 2.00 // @Increment: 0.01 // @User: Advanced - // @Param: _VELZ_FLTE + // @Param: _D_VEL_FLTE // @DisplayName: Velocity (vertical) error filter - // @Description: Velocity (vertical) error filter. This filter (in Hz) is applied to the input for P and I terms + // @Description: Velocity (vertical) error filter. This filter (in Hz) is applied to the input for P and I terms. Previously _VELZ_FLTE. // @Range: 0 100 + // @Increment: 1.0 // @Units: Hz // @User: Advanced - // @Param: _VELZ_FLTD + // @Param: _D_VEL_FLTD // @DisplayName: Velocity (vertical) input filter for D term - // @Description: Velocity (vertical) input filter for D term. This filter (in Hz) is applied to the input for D terms + // @Description: Velocity (vertical) input filter for D term. This filter (in Hz) is applied to the input for D terms. Previously _VELZ_FLTD. // @Range: 0 100 + // @Increment: 1.0 // @Units: Hz // @User: Advanced - AP_SUBGROUPINFO(_pid_vel_u_cm, "_VELZ_", 3, AC_PosControl, AC_PID_Basic), + AP_SUBGROUPINFO(_pid_vel_d_m, "_D_VEL_", 12, AC_PosControl, AC_PID_Basic), - // @Param: _ACCZ_P + // @Param: _D_ACC_P // @DisplayName: Acceleration (vertical) controller P gain - // @Description: Acceleration (vertical) controller P gain. Converts the difference between desired vertical acceleration and actual acceleration into a motor output - // @Range: 0.200 1.500 - // @Increment: 0.05 + // @Description: Acceleration (vertical) controller P gain. Converts the difference between desired vertical acceleration and actual acceleration into a motor output. If upgrading from 4.6 this is _ACCZ_P * 0.1. + // @Range: 0.010 0.250 + // @Increment: 0.001 // @User: Standard - // @Param: _ACCZ_I + // @Param: _D_ACC_I // @DisplayName: Acceleration (vertical) controller I gain - // @Description: Acceleration (vertical) controller I gain. Corrects long-term difference in desired vertical acceleration and actual acceleration - // @Range: 0.000 3.000 + // @Description: Acceleration (vertical) controller I gain. Corrects long-term difference in desired vertical acceleration and actual acceleration. If upgrading from 4.6 this is _ACCZ_I * 0.1. + // @Range: 0.000 0.500 + // @Increment: 0.001 // @User: Standard - // @Param: _ACCZ_IMAX + // @Param: _D_ACC_IMAX // @DisplayName: Acceleration (vertical) controller I gain maximum - // @Description: Acceleration (vertical) controller I gain maximum. Constrains the maximum pwm that the I term will generate - // @Range: 0 1000 + // @Description: Acceleration (vertical) controller I gain maximum. Constrains the maximum pwm that the I term will generate. If upgrading from 4.6 this is _ACCZ_IMAX * 0.001. + // @Range: 0.0 1.0 + // @Increment: 0.01 // @Units: d% // @User: Standard - // @Param: _ACCZ_D + // @Param: _D_ACC_D // @DisplayName: Acceleration (vertical) controller D gain - // @Description: Acceleration (vertical) controller D gain. Compensates for short-term change in desired vertical acceleration vs actual acceleration - // @Range: 0.000 0.400 + // @Description: Acceleration (vertical) controller D gain. Compensates for short-term change in desired vertical acceleration vs actual acceleration. If upgrading from 4.6 this is _ACCZ_D * 0.1. + // @Range: 0.000 0.100 + // @Increment: 0.001 // @User: Standard - // @Param: _ACCZ_FF + // @Param: _D_ACC_FF // @DisplayName: Acceleration (vertical) controller feed forward - // @Description: Acceleration (vertical) controller feed forward - // @Range: 0 0.5 + // @Description: Acceleration (vertical) controller feed forward. If upgrading from 4.6 this is _ACCZ_FF * 0.1. + // @Range: 0.000 0.100 // @Increment: 0.001 // @User: Standard - // @Param: _ACCZ_FLTT + // @Param: _D_ACC_FLTT // @DisplayName: Acceleration (vertical) controller target frequency in Hz - // @Description: Acceleration (vertical) controller target frequency in Hz + // @Description: Acceleration (vertical) controller target frequency in Hz. Previously _ACCZ_FLTT. // @Range: 1 50 // @Increment: 1 // @Units: Hz // @User: Standard - // @Param: _ACCZ_FLTE + // @Param: _D_ACC_FLTE // @DisplayName: Acceleration (vertical) controller error frequency in Hz - // @Description: Acceleration (vertical) controller error frequency in Hz + // @Description: Acceleration (vertical) controller error frequency in Hz. Previously _ACCZ_FLTE. // @Range: 1 100 // @Increment: 1 // @Units: Hz // @User: Standard - // @Param: _ACCZ_FLTD + // @Param: _D_ACC_FLTD // @DisplayName: Acceleration (vertical) controller derivative frequency in Hz - // @Description: Acceleration (vertical) controller derivative frequency in Hz + // @Description: Acceleration (vertical) controller derivative frequency in Hz. Previously _ACCZ_FLTD. // @Range: 1 100 // @Increment: 1 // @Units: Hz // @User: Standard - // @Param: _ACCZ_SMAX + // @Param: _D_ACC_SMAX // @DisplayName: Accel (vertical) slew rate limit // @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature. - // @Range: 0 200 - // @Increment: 0.5 + // @Range: 0 100 + // @Increment: 0.1 // @User: Advanced - // @Param: _ACCZ_PDMX + // @Param: _D_ACC_PDMX // @DisplayName: Acceleration (vertical) controller PD sum maximum - // @Description: Acceleration (vertical) controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output - // @Range: 0 1000 + // @Description: Acceleration (vertical) controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output. If upgrading from 4.6 this is _ACCZ_P * 0.1. + // @Range: 0.00 1.00 + // @Increment: 0.01 // @Units: d% - // @Param: _ACCZ_D_FF + // @Param: _D_ACC_D_FF // @DisplayName: Accel (vertical) Derivative FeedForward Gain - // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target - // @Range: 0 0.02 - // @Increment: 0.0001 + // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target. If upgrading from 4.6 this is _ACCZ_P * 0.1. + // @Range: 0.000 0.050 + // @Increment: 0.001 // @User: Advanced - // @Param: _ACCZ_NTF + // @Param: _D_ACC_NTF // @DisplayName: Accel (vertical) Target notch filter index - // @Description: Accel (vertical) Target notch filter index + // @Description: Accel (vertical) Target notch filter index. If upgrading from 4.6 this is Previously _ACCZ_NTF. // @Range: 1 8 // @User: Advanced - // @Param: _ACCZ_NEF + // @Param: _D_ACC_NEF // @DisplayName: Accel (vertical) Error notch filter index - // @Description: Accel (vertical) Error notch filter index + // @Description: Accel (vertical) Error notch filter index. If upgrading from 4.6 this is Previously _ACCZ_NEF. // @Range: 1 8 // @User: Advanced + AP_SUBGROUPINFO(_pid_accel_d_m, "_D_ACC_", 13, AC_PosControl, AC_PID), - AP_SUBGROUPINFO(_pid_accel_u_cm_to_kt, "_ACCZ_", 4, AC_PosControl, AC_PID), - - // @Param: _POSXY_P - // @DisplayName: Position (horizontal) controller P gain - // @Description: Position controller P gain. Converts the distance (in the latitude direction) to the target location into a desired speed which is then passed to the loiter latitude rate controller - // @Range: 0.500 2.000 - // @User: Standard - AP_SUBGROUPINFO(_p_pos_ne_m, "_POSXY_", 5, AC_PosControl, AC_P_2D), - - // @Param: _VELXY_P + // @Param: _NE_VEL_P // @DisplayName: Velocity (horizontal) P gain - // @Description: Velocity (horizontal) P gain. Converts the difference between desired and actual velocity to a target acceleration - // @Range: 0.1 6.0 - // @Increment: 0.1 + // @Description: Velocity (horizontal) P gain. Converts the difference between desired and actual velocity to a target acceleration. Previously _VELXY_P. + // @Range: 0.10 10.00 + // @Increment: 0.01 // @User: Advanced - // @Param: _VELXY_I + // @Param: _NE_VEL_I // @DisplayName: Velocity (horizontal) I gain - // @Description: Velocity (horizontal) I gain. Corrects long-term difference between desired and actual velocity to a target acceleration - // @Range: 0.02 1.00 + // @Description: Velocity (horizontal) I gain. Corrects long-term difference between desired and actual velocity to a target acceleration. Previously _VELXY_I. + // @Range: 0.10 10.00 // @Increment: 0.01 // @User: Advanced - // @Param: _VELXY_D + // @Param: _NE_VEL_D // @DisplayName: Velocity (horizontal) D gain - // @Description: Velocity (horizontal) D gain. Corrects short-term changes in velocity + // @Description: Velocity (horizontal) D gain. Corrects short-term changes in velocity. Previously _VELXY_D. // @Range: 0.00 1.00 - // @Increment: 0.001 + // @Increment: 0.01 // @User: Advanced - // @Param: _VELXY_IMAX + // @Param: _NE_VEL_IMAX // @DisplayName: Velocity (horizontal) integrator maximum - // @Description: Velocity (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output - // @Range: 0 4500 - // @Increment: 10 - // @Units: cm/s/s + // @Description: Velocity (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output. If upgrading from 4.6 this is _VELXY_IMAX * 0.01. + // @Range: 0 10 + // @Increment: 1 + // @Units: m/s/s // @User: Advanced - // @Param: _VELXY_FLTE + // @Param: _NE_VEL_FLTE // @DisplayName: Velocity (horizontal) input filter - // @Description: Velocity (horizontal) input filter. This filter (in Hz) is applied to the input for P and I terms + // @Description: Velocity (horizontal) input filter. This filter (in Hz) is applied to the input for P and I terms. Previously _VELXY_FLTE. // @Range: 0 100 + // @Increment: 1 // @Units: Hz // @User: Advanced - // @Param: _VELXY_FLTD + // @Param: _NE_VEL_FLTD // @DisplayName: Velocity (horizontal) input filter - // @Description: Velocity (horizontal) input filter. This filter (in Hz) is applied to the input for D term + // @Description: Velocity (horizontal) input filter. This filter (in Hz) is applied to the input for D term. Previously _VELXY_FLTD. // @Range: 0 100 + // @Increment: 1 // @Units: Hz // @User: Advanced - // @Param: _VELXY_FF + // @Param: _NE_VEL_FF // @DisplayName: Velocity (horizontal) feed forward gain - // @Description: Velocity (horizontal) feed forward gain. Converts the difference between desired velocity to a target acceleration - // @Range: 0 6 + // @Description: Velocity (horizontal) feed forward gain. Converts the difference between desired velocity to a target acceleration. Previously _VELXY_FF. + // @Range: 0.10 10.00 // @Increment: 0.01 // @User: Advanced - AP_SUBGROUPINFO(_pid_vel_ne_cm, "_VELXY_", 6, AC_PosControl, AC_PID_2D), - - // @Param: _ANGLE_MAX - // @DisplayName: Position Control Angle Max - // @Description: Maximum lean angle autopilot can request. Set to zero to use ANGLE_MAX parameter value - // @Units: deg - // @Range: 0 45 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("_ANGLE_MAX", 7, AC_PosControl, _lean_angle_max_deg, 0.0f), - - // IDs 8,9 used for _TC_XY and _TC_Z in beta release candidate - - // @Param: _JERK_XY - // @DisplayName: Jerk limit for the horizontal kinematic input shaping - // @Description: Jerk limit of the horizontal kinematic path generation used to determine how quickly the aircraft varies the acceleration target - // @Units: m/s/s/s - // @Range: 1 20 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("_JERK_XY", 10, AC_PosControl, _shaping_jerk_ne_msss, POSCONTROL_JERK_NE_MSSS), - - // @Param: _JERK_Z - // @DisplayName: Jerk limit for the vertical kinematic input shaping - // @Description: Jerk limit of the vertical kinematic path generation used to determine how quickly the aircraft varies the acceleration target - // @Units: m/s/s/s - // @Range: 5 50 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("_JERK_Z", 11, AC_PosControl, _shaping_jerk_u_msss, POSCONTROL_JERK_U_MSSS), + AP_SUBGROUPINFO(_pid_vel_ne_m, "_NE_VEL_", 14, AC_PosControl, AC_PID_2D), AP_GROUPEND }; @@ -328,18 +345,18 @@ AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_Motors& motors, AC_Att _ahrs(ahrs), _motors(motors), _attitude_control(attitude_control), - _p_pos_ne_m(POSCONTROL_POS_XY_P), - _p_pos_u_m(POSCONTROL_POS_Z_P), - _pid_vel_ne_cm(POSCONTROL_VEL_XY_P, POSCONTROL_VEL_XY_I, POSCONTROL_VEL_XY_D, 0.0f, POSCONTROL_VEL_XY_IMAX, POSCONTROL_VEL_XY_FILT_HZ, POSCONTROL_VEL_XY_FILT_D_HZ), - _pid_vel_u_cm(POSCONTROL_VEL_Z_P, 0.0f, 0.0f, 0.0f, POSCONTROL_VEL_Z_IMAX, POSCONTROL_VEL_Z_FILT_HZ, POSCONTROL_VEL_Z_FILT_D_HZ), - _pid_accel_u_cm_to_kt(POSCONTROL_ACC_Z_P, POSCONTROL_ACC_Z_I, POSCONTROL_ACC_Z_D, 0.0f, POSCONTROL_ACC_Z_IMAX, 0.0f, POSCONTROL_ACC_Z_FILT_HZ, 0.0f), + _p_pos_ne_m(POSCONTROL_NE_POS_P), + _p_pos_d_m(POSCONTROL_D_POS_P), + _pid_vel_ne_m(POSCONTROL_NE_VEL_P, POSCONTROL_NE_VEL_I, POSCONTROL_NE_VEL_D, 0.0f, POSCONTROL_NE_VEL_IMAX, POSCONTROL_NE_VEL_FILT_HZ, POSCONTROL_NE_VEL_FILT_D_HZ), + _pid_vel_d_m(POSCONTROL_D_VEL_P, 0.0f, 0.0f, 0.0f, POSCONTROL_D_VEL_IMAX, POSCONTROL_D_VEL_FILT_HZ, POSCONTROL_D_VEL_FILT_D_HZ), + _pid_accel_d_m(POSCONTROL_D_ACC_P, POSCONTROL_D_ACC_I, POSCONTROL_D_ACC_D, 0.0f, POSCONTROL_D_ACC_IMAX, 0.0f, POSCONTROL_D_ACC_FILT_HZ, 0.0f), _vel_max_ne_ms(POSCONTROL_SPEED_MS), _vel_max_up_ms(POSCONTROL_SPEED_UP_MS), _vel_max_down_ms(POSCONTROL_SPEED_DOWN_MS), _accel_max_ne_mss(POSCONTROL_ACCEL_NE_MSS), - _accel_max_u_mss(POSCONTROL_ACCEL_U_MSS), + _accel_max_d_mss(POSCONTROL_ACCEL_D_MSS), _jerk_max_ne_msss(POSCONTROL_JERK_NE_MSSS), - _jerk_max_u_msss(POSCONTROL_JERK_U_MSSS) + _jerk_max_d_msss(POSCONTROL_JERK_D_MSSS) { AP_Param::setup_object_defaults(this, var_info); @@ -351,64 +368,66 @@ AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_Motors& motors, AC_Att /// 3D position shaper /// -// Sets a new NEU position target in meters and computes a jerk-limited trajectory. +// Sets a new NED position target in meters and computes a jerk-limited trajectory. // Updates internal acceleration commands using a smooth kinematic path constrained -// by configured acceleration and jerk limits. Terrain margin is used to constrain -// horizontal velocity to avoid vertical buffer violation. -void AC_PosControl::input_pos_NEU_m(const Vector3p& pos_neu_m, float pos_terrain_target_u_m, float terrain_buffer_m) +// by configured acceleration and jerk limits. +// The path can be offset vertically to follow the terrain by providing the current +// terrain level in the NED frame and the terrain margin. Terrain margin is used to +// constrain horizontal velocity to avoid vertical buffer violation. +void AC_PosControl::input_pos_NED_m(const Vector3p& pos_ned_m, float pos_terrain_target_d_m, float terrain_margin_m) { // Terrain following velocity scalar must be calculated before we remove the position offset - const float offset_u_scalar = pos_terrain_U_scaler_m(pos_terrain_target_u_m, terrain_buffer_m); - set_pos_terrain_target_U_m(pos_terrain_target_u_m); + const float offset_d_scalar = terrain_scaler_D_m(pos_terrain_target_d_m, terrain_margin_m); + set_pos_terrain_target_D_m(pos_terrain_target_d_m); // calculated increased maximum acceleration and jerk if over speed const float overspeed_gain = calculate_overspeed_gain(); - const float accel_max_u_mss = _accel_max_u_mss * overspeed_gain; - const float jerk_max_u_msss = _jerk_max_u_msss * overspeed_gain; + const float accel_max_d_mss = _accel_max_d_mss * overspeed_gain; + const float jerk_max_d_msss = _jerk_max_d_msss * overspeed_gain; - update_pos_vel_accel_xy(_pos_desired_neu_m.xy(), _vel_desired_neu_ms.xy(), _accel_desired_neu_mss.xy(), _dt_s, _limit_vector_neu.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_cm.get_error()); + update_pos_vel_accel_xy(_pos_desired_ned_m.xy(), _vel_desired_ned_ms.xy(), _accel_desired_ned_mss.xy(), _dt_s, _limit_vector_ned.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_m.get_error()); // adjust desired altitude if motors have not hit their limits - update_pos_vel_accel(_pos_desired_neu_m.z, _vel_desired_neu_ms.z, _accel_desired_neu_mss.z, _dt_s, _limit_vector_neu.z, _p_pos_u_m.get_error(), _pid_vel_u_cm.get_error()); + update_pos_vel_accel(_pos_desired_ned_m.z, _vel_desired_ned_ms.z, _accel_desired_ned_mss.z, _dt_s, _limit_vector_ned.z, _p_pos_d_m.get_error(), _pid_vel_d_m.get_error()); // calculate the horizontal and vertical velocity limits to travel directly to the destination defined by pos_ne_m float vel_max_ne_ms = 0.0f; - float vel_max_u_ms = 0.0f; - Vector3f travel_dir_unit = (pos_neu_m - _pos_desired_neu_m).tofloat(); + float vel_max_d_ms = 0.0f; + Vector3f travel_dir_unit = (pos_ned_m - _pos_desired_ned_m).tofloat(); if (is_positive(travel_dir_unit.length_squared()) ) { travel_dir_unit.normalize(); float travel_dir_unit_ne_length = travel_dir_unit.xy().length(); float vel_max_ms = kinematic_limit(travel_dir_unit, _vel_max_ne_ms, _vel_max_up_ms, _vel_max_down_ms); vel_max_ne_ms = vel_max_ms * travel_dir_unit_ne_length; - vel_max_u_ms = fabsf(vel_max_ms * travel_dir_unit.z); + vel_max_d_ms = fabsf(vel_max_ms * travel_dir_unit.z); } // reduce speed if we are reaching the edge of our vertical buffer - vel_max_ne_ms *= offset_u_scalar; + vel_max_ne_ms *= offset_d_scalar; Vector2f vel_ne_ms; Vector2f accel_ne_mss; - shape_pos_vel_accel_xy(pos_neu_m.xy(), vel_ne_ms, accel_ne_mss, _pos_desired_neu_m.xy(), _vel_desired_neu_ms.xy(), _accel_desired_neu_mss.xy(), + shape_pos_vel_accel_xy(pos_ned_m.xy(), vel_ne_ms, accel_ne_mss, _pos_desired_ned_m.xy(), _vel_desired_ned_ms.xy(), _accel_desired_ned_mss.xy(), vel_max_ne_ms, _accel_max_ne_mss, _jerk_max_ne_msss, _dt_s, false); - float pos_u_m = pos_neu_m.z; - shape_pos_vel_accel(pos_u_m, 0, 0, - _pos_desired_neu_m.z, _vel_desired_neu_ms.z, _accel_desired_neu_mss.z, - -vel_max_u_ms, vel_max_u_ms, - -constrain_float(accel_max_u_mss, 0.0, 7.5), accel_max_u_mss, - jerk_max_u_msss, _dt_s, false); + float pos_d_m = pos_ned_m.z; + shape_pos_vel_accel(pos_d_m, 0, 0, + _pos_desired_ned_m.z, _vel_desired_ned_ms.z, _accel_desired_ned_mss.z, + -vel_max_d_ms, vel_max_d_ms, + -accel_max_d_mss, constrain_float(accel_max_d_mss, 0.0, 7.5), + jerk_max_d_msss, _dt_s, false); } // Returns a scaling factor for horizontal velocity in m/s to ensure // the vertical controller maintains a safe distance above terrain. -float AC_PosControl::pos_terrain_U_scaler_m(float pos_terrain_u_m, float pos_terrain_u_buffer_m) const +float AC_PosControl::terrain_scaler_D_m(float pos_terrain_d_m, float terrain_margin_m) const { - if (is_zero(pos_terrain_u_buffer_m)) { + if (is_zero(terrain_margin_m)) { return 1.0; } - float pos_offset_error_u_m = _pos_estimate_neu_m.z - (_pos_target_neu_m.z + (pos_terrain_u_m - _pos_terrain_u_m)); - return constrain_float((1.0 - (fabsf(pos_offset_error_u_m) - 0.5 * pos_terrain_u_buffer_m) / (0.5 * pos_terrain_u_buffer_m)), 0.01, 1.0); + float pos_offset_error_d_m = _pos_estimate_ned_m.z - (_pos_target_ned_m.z + (pos_terrain_d_m - _pos_terrain_d_m)); + return constrain_float((1.0 - (fabsf(pos_offset_error_d_m) - 0.5 * terrain_margin_m) / (0.5 * terrain_margin_m)), 0.01, 1.0); } /// @@ -418,16 +437,16 @@ float AC_PosControl::pos_terrain_U_scaler_m(float pos_terrain_u_m, float pos_ter // Sets maximum horizontal speed (cm/s) and acceleration (cm/s²) for NE-axis shaping. // Can be called anytime; transitions are handled smoothly. // All arguments should be positive. -// See set_max_speed_accel_NE_m() for full details. -void AC_PosControl::set_max_speed_accel_NE_cm(float speed_ne_cms, float accel_ne_cmss) +// See NE_set_max_speed_accel_m() for full details. +void AC_PosControl::NE_set_max_speed_accel_cm(float speed_ne_cms, float accel_ne_cmss) { - set_max_speed_accel_NE_m(speed_ne_cms * 0.01, accel_ne_cmss * 0.01); + NE_set_max_speed_accel_m(speed_ne_cms * 0.01, accel_ne_cmss * 0.01); } // Sets maximum horizontal speed (m/s) and acceleration (m/s²) for NE-axis shaping. // These values constrain the kinematic trajectory used by the lateral controller. // All arguments should be positive. -void AC_PosControl::set_max_speed_accel_NE_m(float speed_ne_ms, float accel_ne_mss) +void AC_PosControl::NE_set_max_speed_accel_m(float speed_ne_ms, float accel_ne_mss) { _vel_max_ne_ms = fabsf(speed_ne_ms); _accel_max_ne_mss = fabsf(accel_ne_mss); @@ -453,16 +472,16 @@ void AC_PosControl::set_max_speed_accel_NE_m(float speed_ne_ms, float accel_ne_m // Sets horizontal correction limits for velocity (cm/s) and acceleration (cm/s²). // Should be called only during initialization to avoid control discontinuities. // All arguments should be positive. -// See set_correction_speed_accel_NE_m() for full details. -void AC_PosControl::set_correction_speed_accel_NE_cm(float speed_ne_cms, float accel_ne_cmss) +// See NE_set_correction_speed_accel_m() for full details. +void AC_PosControl::NE_set_correction_speed_accel_cm(float speed_ne_cms, float accel_ne_cmss) { - set_correction_speed_accel_NE_m(speed_ne_cms * 0.01, accel_ne_cmss * 0.01); + NE_set_correction_speed_accel_m(speed_ne_cms * 0.01, accel_ne_cmss * 0.01); } // Sets horizontal correction limits for velocity (m/s) and acceleration (m/s²). // These values constrain the PID correction path, not the desired trajectory. // All arguments should be positive. -void AC_PosControl::set_correction_speed_accel_NE_m(float speed_ne_ms, float accel_ne_mss) +void AC_PosControl::NE_set_correction_speed_accel_m(float speed_ne_ms, float accel_ne_mss) { // limits that are not positive are ignored _p_pos_ne_m.set_limits(speed_ne_ms, accel_ne_mss, 0.0f); @@ -470,53 +489,53 @@ void AC_PosControl::set_correction_speed_accel_NE_m(float speed_ne_ms, float acc // Initializes NE controller to a stationary stopping point with zero velocity and acceleration. // Use when the expected trajectory begins at rest but the starting position is unspecified. -// The starting position can be retrieved with get_pos_target_NEU_m(). -void AC_PosControl::init_NE_controller_stopping_point() +// The starting position can be retrieved with get_pos_target_NED_m(). +void AC_PosControl::NE_init_controller_stopping_point() { - init_NE_controller(); + NE_init_controller(); - get_stopping_point_NE_m(_pos_desired_neu_m.xy()); - _pos_target_neu_m.xy() = _pos_desired_neu_m.xy() + _pos_offset_neu_m.xy(); - _vel_desired_neu_ms.xy().zero(); - _accel_desired_neu_mss.xy().zero(); + get_stopping_point_NE_m(_pos_desired_ned_m.xy()); + _pos_target_ned_m.xy() = _pos_desired_ned_m.xy() + _pos_offset_ned_m.xy(); + _vel_desired_ned_ms.xy().zero(); + _accel_desired_ned_mss.xy().zero(); } // Smoothly decays NE acceleration over time to zero while maintaining current velocity and position. // Reduces output acceleration by ~95% over 0.5 seconds to avoid abrupt transitions. -void AC_PosControl::relax_velocity_controller_NE() +void AC_PosControl::NE_relax_velocity_controller() { // decay acceleration and therefore current attitude target to zero - // this will be reset by init_NE_controller() if !is_active_NE() + // this will be reset by NE_init_controller() if !NE_is_active() if (is_positive(_dt_s)) { float decay = 1.0 - _dt_s / (_dt_s + POSCONTROL_RELAX_TC); - _accel_target_neu_mss.xy() *= decay; + _accel_target_ned_mss.xy() *= decay; } - init_NE_controller(); + NE_init_controller(); } // Softens NE controller for landing by reducing position error and suppressing I-term windup. // Used to make descent behavior more stable near ground contact. -void AC_PosControl::soften_for_landing_NE() +void AC_PosControl::NE_soften_for_landing() { // decay position error to zero if (is_positive(_dt_s)) { - _pos_target_neu_m.xy() += (_pos_estimate_neu_m.xy() - _pos_target_neu_m.xy()) * (_dt_s / (_dt_s + POSCONTROL_RELAX_TC)); - _pos_desired_neu_m.xy() = _pos_target_neu_m.xy() - _pos_offset_neu_m.xy(); + _pos_target_ned_m.xy() += (_pos_estimate_ned_m.xy() - _pos_target_ned_m.xy()) * (_dt_s / (_dt_s + POSCONTROL_RELAX_TC)); + _pos_desired_ned_m.xy() = _pos_target_ned_m.xy() - _pos_offset_ned_m.xy(); } // Prevent I term build up in xy velocity controller. - // Note that this flag is reset on each loop in update_NE_controller() - set_externally_limited_NE(); + // Note that this flag is reset on each loop in NE_update_controller() + NE_set_externally_limited(); } // Fully initializes the NE controller with current position, velocity, acceleration, and attitude. // Intended for normal startup when the full state is known. // Private function shared by other NE initializers. -void AC_PosControl::init_NE_controller() +void AC_PosControl::NE_init_controller() { // initialise offsets to target offsets and ensure offset targets are zero if they have not been updated. - init_offsets_NE(); + NE_init_offsets(); // set roll, pitch lean angle targets to current attitude const Vector3f &att_target_euler_rad = _attitude_control.get_att_target_euler_rad(); @@ -526,32 +545,32 @@ void AC_PosControl::init_NE_controller() _yaw_rate_target_rads = 0.0f; _angle_max_override_rad = 0.0; - _pos_target_neu_m.xy() = _pos_estimate_neu_m.xy(); - _pos_desired_neu_m.xy() = _pos_target_neu_m.xy() - _pos_offset_neu_m.xy(); + _pos_target_ned_m.xy() = _pos_estimate_ned_m.xy(); + _pos_desired_ned_m.xy() = _pos_target_ned_m.xy() - _pos_offset_ned_m.xy(); - _vel_target_neu_ms.xy() = _vel_estimate_neu_ms.xy(); - _vel_desired_neu_ms.xy() = _vel_target_neu_ms.xy() - _vel_offset_neu_ms.xy(); + _vel_target_ned_ms.xy() = _vel_estimate_ned_ms.xy(); + _vel_desired_ned_ms.xy() = _vel_target_ned_ms.xy() - _vel_offset_ned_ms.xy(); // Set desired acceleration to zero because raw acceleration is prone to noise - _accel_desired_neu_mss.xy().zero(); + _accel_desired_ned_mss.xy().zero(); - if (!is_active_NE()) { - lean_angles_to_accel_NE_mss(_accel_target_neu_mss.x, _accel_target_neu_mss.y); + if (!NE_is_active()) { + lean_angles_to_accel_NE_mss(_accel_target_ned_mss.x, _accel_target_ned_mss.y); } // limit acceleration using maximum lean angles const float angle_max_rad = MIN(_attitude_control.get_althold_lean_angle_max_rad(), get_lean_angle_max_rad()); const float accel_max_mss = angle_rad_to_accel_mss(angle_max_rad); - _accel_target_neu_mss.xy().limit_length(accel_max_mss); + _accel_target_ned_mss.xy().limit_length(accel_max_mss); // initialise I terms from lean angles - _pid_vel_ne_cm.reset_filter(); - // initialise the I term to (_accel_target_neu_mss - _accel_desired_neu_mss) - // _accel_desired_neu_mss is zero and can be removed from the equation - _pid_vel_ne_cm.set_integrator((_accel_target_neu_mss.xy() - _vel_target_neu_ms.xy() * _pid_vel_ne_cm.ff()) * 100.0); + _pid_vel_ne_m.reset_filter(); + // initialise the I term to (_accel_target_ned_mss - _accel_desired_ned_mss) + // _accel_desired_ned_mss is zero and can be removed from the equation + _pid_vel_ne_m.set_integrator((_accel_target_ned_mss.xy() - _vel_target_ned_ms.xy() * _pid_vel_ne_m.ff())); // initialise ekf xy reset handler - init_ekf_NE_reset(); + NE_init_ekf_reset(); // initialise z_controller time out _last_update_ne_ticks = AP::scheduler().ticks32(); @@ -559,11 +578,11 @@ void AC_PosControl::init_NE_controller() // Sets the desired NE-plane acceleration in m/s² using jerk-limited shaping. // Smoothly transitions to the specified acceleration from current kinematic state. -// Constraints: max acceleration and jerk set via set_max_speed_accel_NE_m(). -void AC_PosControl::input_accel_NE_m(const Vector3f& accel_neu_mss) +// Constraints: max acceleration and jerk set via NE_set_max_speed_accel_m(). +void AC_PosControl::input_accel_NE_m(const Vector2f& accel_ne_mss) { - update_pos_vel_accel_xy(_pos_desired_neu_m.xy(), _vel_desired_neu_ms.xy(), _accel_desired_neu_mss.xy(), _dt_s, _limit_vector_neu.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_cm.get_error()); - shape_accel_xy(accel_neu_mss.xy(), _accel_desired_neu_mss.xy(), _jerk_max_ne_msss, _dt_s); + update_pos_vel_accel_xy(_pos_desired_ned_m.xy(), _vel_desired_ned_ms.xy(), _accel_desired_ned_mss.xy(), _dt_s, _limit_vector_ned.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_m.get_error()); + shape_accel_xy(accel_ne_mss, _accel_desired_ned_mss.xy(), _jerk_max_ne_msss, _dt_s); } // Sets desired NE-plane velocity and acceleration (cm/s, cm/s²) using jerk-limited shaping. @@ -580,9 +599,9 @@ void AC_PosControl::input_vel_accel_NE_cm(Vector2f& vel_ne_cms, const Vector2f& // If `limit_output` is true, applies limits to total command (desired + correction). void AC_PosControl::input_vel_accel_NE_m(Vector2f& vel_ne_ms, const Vector2f& accel_ne_mss, bool limit_output) { - update_pos_vel_accel_xy(_pos_desired_neu_m.xy(), _vel_desired_neu_ms.xy(), _accel_desired_neu_mss.xy(), _dt_s, _limit_vector_neu.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_cm.get_error()); + update_pos_vel_accel_xy(_pos_desired_ned_m.xy(), _vel_desired_ned_ms.xy(), _accel_desired_ned_mss.xy(), _dt_s, _limit_vector_ned.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_m.get_error()); - shape_vel_accel_xy(vel_ne_ms, accel_ne_mss, _vel_desired_neu_ms.xy(), _accel_desired_neu_mss.xy(), + shape_vel_accel_xy(vel_ne_ms, accel_ne_mss, _vel_desired_ned_ms.xy(), _accel_desired_ned_mss.xy(), _accel_max_ne_mss, _jerk_max_ne_msss, _dt_s, limit_output); update_vel_accel_xy(vel_ne_ms, accel_ne_mss, _dt_s, Vector2f(), Vector2f()); @@ -604,61 +623,61 @@ void AC_PosControl::input_pos_vel_accel_NE_cm(Vector2p& pos_ne_cm, Vector2f& vel // If `limit_output` is true, limits apply to full command (desired + correction). void AC_PosControl::input_pos_vel_accel_NE_m(Vector2p& pos_ne_m, Vector2f& vel_ne_ms, const Vector2f& accel_ne_mss, bool limit_output) { - update_pos_vel_accel_xy(_pos_desired_neu_m.xy(), _vel_desired_neu_ms.xy(), _accel_desired_neu_mss.xy(), _dt_s, _limit_vector_neu.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_cm.get_error()); + update_pos_vel_accel_xy(_pos_desired_ned_m.xy(), _vel_desired_ned_ms.xy(), _accel_desired_ned_mss.xy(), _dt_s, _limit_vector_ned.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_m.get_error()); - shape_pos_vel_accel_xy(pos_ne_m, vel_ne_ms, accel_ne_mss, _pos_desired_neu_m.xy(), _vel_desired_neu_ms.xy(), _accel_desired_neu_mss.xy(), + shape_pos_vel_accel_xy(pos_ne_m, vel_ne_ms, accel_ne_mss, _pos_desired_ned_m.xy(), _vel_desired_ned_ms.xy(), _accel_desired_ned_mss.xy(), _vel_max_ne_ms, _accel_max_ne_mss, _jerk_max_ne_msss, _dt_s, limit_output); update_pos_vel_accel_xy(pos_ne_m, vel_ne_ms, accel_ne_mss, _dt_s, Vector2f(), Vector2f(), Vector2f()); } // Updates NE offsets by gradually moving them toward their targets. -void AC_PosControl::update_offsets_NE() +void AC_PosControl::NE_update_offsets() { // Check if NE offset targets have timed out uint32_t now_ms = AP_HAL::millis(); if (now_ms - _posvelaccel_offset_target_ne_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) { // Timeout: reset all NE offset targets to zero - _pos_offset_target_neu_m.xy().zero(); - _vel_offset_target_neu_ms.xy().zero(); - _accel_offset_target_neu_mss.xy().zero(); + _pos_offset_target_ned_m.xy().zero(); + _vel_offset_target_ned_ms.xy().zero(); + _accel_offset_target_ned_mss.xy().zero(); } // Advance offset target kinematic state (position, velocity, accel) - update_pos_vel_accel_xy(_pos_offset_target_neu_m.xy(), _vel_offset_target_neu_ms.xy(), _accel_offset_target_neu_mss.xy(), _dt_s, Vector2f(), Vector2f(), Vector2f()); - update_pos_vel_accel_xy(_pos_offset_neu_m.xy(), _vel_offset_neu_ms.xy(), _accel_offset_neu_mss.xy(), _dt_s, _limit_vector_neu.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_cm.get_error()); + update_pos_vel_accel_xy(_pos_offset_target_ned_m.xy(), _vel_offset_target_ned_ms.xy(), _accel_offset_target_ned_mss.xy(), _dt_s, Vector2f(), Vector2f(), Vector2f()); + update_pos_vel_accel_xy(_pos_offset_ned_m.xy(), _vel_offset_ned_ms.xy(), _accel_offset_ned_mss.xy(), _dt_s, _limit_vector_ned.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_m.get_error()); // Shape the offset path from current to target using jerk-limited smoothing - shape_pos_vel_accel_xy(_pos_offset_target_neu_m.xy(), _vel_offset_target_neu_ms.xy(), _accel_offset_target_neu_mss.xy(), - _pos_offset_neu_m.xy(), _vel_offset_neu_ms.xy(), _accel_offset_neu_mss.xy(), + shape_pos_vel_accel_xy(_pos_offset_target_ned_m.xy(), _vel_offset_target_ned_ms.xy(), _accel_offset_target_ned_mss.xy(), + _pos_offset_ned_m.xy(), _vel_offset_ned_ms.xy(), _accel_offset_ned_mss.xy(), _vel_max_ne_ms, _accel_max_ne_mss, _jerk_max_ne_msss, _dt_s, false); } // Disables NE position correction by setting the target position to the current position. // Useful to freeze positional control without disrupting velocity control. -void AC_PosControl::stop_pos_NE_stabilisation() +void AC_PosControl::NE_stop_pos_stabilisation() { - _pos_target_neu_m.xy() = _pos_estimate_neu_m.xy(); - _pos_desired_neu_m.xy() = _pos_target_neu_m.xy() - _pos_offset_neu_m.xy(); + _pos_target_ned_m.xy() = _pos_estimate_ned_m.xy(); + _pos_desired_ned_m.xy() = _pos_target_ned_m.xy() - _pos_offset_ned_m.xy(); } // Disables NE position and velocity correction by setting target values to current state. // Useful to prevent further corrections and freeze motion stabilization in NE axes. -void AC_PosControl::stop_vel_NE_stabilisation() +void AC_PosControl::NE_stop_vel_stabilisation() { - _pos_target_neu_m.xy() = _pos_estimate_neu_m.xy(); - _pos_desired_neu_m.xy() = _pos_target_neu_m.xy() - _pos_offset_neu_m.xy(); + _pos_target_ned_m.xy() = _pos_estimate_ned_m.xy(); + _pos_desired_ned_m.xy() = _pos_target_ned_m.xy() - _pos_offset_ned_m.xy(); - _vel_target_neu_ms.xy() = _vel_estimate_neu_ms.xy(); - _vel_desired_neu_ms.xy() = _vel_target_neu_ms.xy() - _vel_offset_neu_ms.xy(); + _vel_target_ned_ms.xy() = _vel_estimate_ned_ms.xy(); + _vel_desired_ned_ms.xy() = _vel_target_ned_ms.xy() - _vel_offset_ned_ms.xy(); - // initialise I terms from lean angles - _pid_vel_ne_cm.reset_filter(); - _pid_vel_ne_cm.reset_I(); + // reset I terms + _pid_vel_ne_m.reset_filter(); + _pid_vel_ne_m.reset_I(); } // Returns true if the NE position controller has run in the last 5 control loop cycles. -bool AC_PosControl::is_active_NE() const +bool AC_PosControl::NE_is_active() const { const uint32_t dt_ticks = AP::scheduler().ticks32() - _last_update_ne_ticks; return dt_ticks <= 1; @@ -666,14 +685,14 @@ bool AC_PosControl::is_active_NE() const // Uses P and PID controllers to generate corrections which are added to feedforward velocity/acceleration. // Requires all desired targets to be pre-set using the input_* or set_* methods. -void AC_PosControl::update_NE_controller() +void AC_PosControl::NE_update_controller() { // check for ekf xy position reset - handle_ekf_NE_reset(); + NE_handle_ekf_reset(); // Check for position control time out - if (!is_active_NE()) { - init_NE_controller(); + if (!NE_is_active()) { + NE_init_controller(); if (has_good_timing()) { // call internal error because initialisation has not been done INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); @@ -685,21 +704,21 @@ void AC_PosControl::update_NE_controller() AP::ahrs().getControlLimits(ahrsGndSpdLimit, ahrsControlScaleXY); // Update lateral position, velocity, and acceleration offsets using path shaping - update_offsets_NE(); + NE_update_offsets(); // Position Controller // Combine position target with active NE offset to get absolute target - _pos_target_neu_m.xy() = _pos_desired_neu_m.xy() + _pos_offset_neu_m.xy(); + _pos_target_ned_m.xy() = _pos_desired_ned_m.xy() + _pos_offset_ned_m.xy(); // determine the combined position of the actual position and the disturbance from system ID mode // calculate the target velocity correction - Vector2p comb_pos_ne_m = _pos_estimate_neu_m.xy(); + Vector2p comb_pos_ne_m = _pos_estimate_ned_m.xy(); comb_pos_ne_m += _disturb_pos_ne_m.topostype(); // Run P controller to compute velocity setpoint from position error - Vector2f vel_target_ne_ms = _p_pos_ne_m.update_all(_pos_target_neu_m.xy(), comb_pos_ne_m); - _pos_desired_neu_m.xy() = _pos_target_neu_m.xy() - _pos_offset_neu_m.xy(); + Vector2f vel_target_ne_ms = _p_pos_ne_m.update_all(_pos_target_ned_m.xy(), comb_pos_ne_m); + _pos_desired_ned_m.xy() = _pos_target_ned_m.xy() - _pos_offset_ned_m.xy(); // Velocity Controller @@ -707,17 +726,17 @@ void AC_PosControl::update_NE_controller() vel_target_ne_ms *= ahrsControlScaleXY; vel_target_ne_ms *= _ne_control_scale_factor; - _vel_target_neu_ms.xy() = vel_target_ne_ms; - _vel_target_neu_ms.xy() += _vel_desired_neu_ms.xy() + _vel_offset_neu_ms.xy(); + _vel_target_ned_ms.xy() = vel_target_ne_ms; + _vel_target_ned_ms.xy() += _vel_desired_ned_ms.xy() + _vel_offset_ned_ms.xy(); // Velocity Controller // determine the combined velocity of the actual velocity and the disturbance from system ID mode - Vector2f comb_vel_ne_ms = _vel_estimate_neu_ms.xy(); + Vector2f comb_vel_ne_ms = _vel_estimate_ned_ms.xy(); comb_vel_ne_ms += _disturb_vel_ne_ms; // Run velocity PID controller and scale result for control authority - Vector2f accel_target_ne_mss = _pid_vel_ne_cm.update_all(_vel_target_neu_ms.xy() * 100.0, comb_vel_ne_ms * 100.0, _dt_s, _limit_vector_neu.xy()) * 0.01; + Vector2f accel_target_ne_mss = _pid_vel_ne_m.update_all(_vel_target_ned_ms.xy(), comb_vel_ne_ms, _dt_s, _limit_vector_ned.xy()); // Acceleration Controller @@ -728,21 +747,21 @@ void AC_PosControl::update_NE_controller() _ne_control_scale_factor = 1.0; // pass the correction acceleration to the target acceleration output - _accel_target_neu_mss.xy() = accel_target_ne_mss; - _accel_target_neu_mss.xy() += _accel_desired_neu_mss.xy() + _accel_offset_neu_mss.xy(); + _accel_target_ned_mss.xy() = accel_target_ne_mss; + _accel_target_ned_mss.xy() += _accel_desired_ned_mss.xy() + _accel_offset_ned_mss.xy(); // limit acceleration using maximum lean angles const float angle_max_rad = MIN(_attitude_control.get_althold_lean_angle_max_rad(), get_lean_angle_max_rad()); const float accel_max_mss = angle_rad_to_accel_mss(angle_max_rad); // Save unbounded target for use in "limited" check (not unit-consistent with z!) - _limit_vector_neu.xy() = _accel_target_neu_mss.xy(); - if (!limit_accel_xy(_vel_desired_neu_ms.xy(), _accel_target_neu_mss.xy(), accel_max_mss)) { - // _accel_target_neu_mss was not limited so we can zero the xy limit vector - _limit_vector_neu.xy().zero(); + _limit_vector_ned.xy() = _accel_target_ned_mss.xy(); + if (!limit_accel_xy(_vel_desired_ned_ms.xy(), _accel_target_ned_mss.xy(), accel_max_mss)) { + // _accel_target_ned_mss was not limited so we can zero the xy limit vector + _limit_vector_ned.xy().zero(); } // Convert acceleration to roll/pitch angle targets (used by attitude controller) - accel_NE_mss_to_lean_angles_rad(_accel_target_neu_mss.x, _accel_target_neu_mss.y, _roll_target_rad, _pitch_target_rad); + accel_NE_mss_to_lean_angles_rad(_accel_target_ned_mss.x, _accel_target_ned_mss.y, _roll_target_rad, _pitch_target_rad); // Update yaw and yaw rate targets to match heading of motion calculate_yaw_and_rate_yaw(); @@ -758,17 +777,17 @@ void AC_PosControl::update_NE_controller() /// // Sets maximum climb/descent rate (cm/s) and vertical acceleration (cm/s²) for the U-axis. -// See set_max_speed_accel_U_m() for full details. +// See D_set_max_speed_accel_m() for full details. // All values must be positive. -void AC_PosControl::set_max_speed_accel_U_cm(float decent_speed_max_cms, float climb_speed_max_cms, float accel_max_u_cmss) +void AC_PosControl::D_set_max_speed_accel_cm(float decent_speed_max_cms, float climb_speed_max_cms, float accel_max_u_cmss) { - set_max_speed_accel_U_m(decent_speed_max_cms * 0.01, climb_speed_max_cms * 0.01, accel_max_u_cmss * 0.01); + D_set_max_speed_accel_m(decent_speed_max_cms * 0.01, climb_speed_max_cms * 0.01, accel_max_u_cmss * 0.01); } // Sets maximum climb/descent rate (m/s) and vertical acceleration (m/s²) for the U-axis. // These values are used for jerk-limited kinematic shaping of the vertical trajectory. // All values must be positive. -void AC_PosControl::set_max_speed_accel_U_m(float decent_speed_max_ms, float climb_speed_max_ms, float accel_max_u_mss) +void AC_PosControl::D_set_max_speed_accel_m(float decent_speed_max_ms, float climb_speed_max_ms, float accel_max_d_mss) { // sanity check and update if (!is_zero(decent_speed_max_ms)) { @@ -777,17 +796,17 @@ void AC_PosControl::set_max_speed_accel_U_m(float decent_speed_max_ms, float cli if (!is_zero(climb_speed_max_ms)) { _vel_max_up_ms = fabsf(climb_speed_max_ms); } - if (!is_zero(accel_max_u_mss)) { - _accel_max_u_mss = fabsf(accel_max_u_mss); + if (!is_zero(accel_max_d_mss)) { + _accel_max_d_mss = fabsf(accel_max_d_mss); } // ensure the vertical Jerk is not limited by the filters in the Z acceleration PID object - _jerk_max_u_msss = _shaping_jerk_u_msss; - if (is_positive(_pid_accel_u_cm_to_kt.filt_T_hz())) { - _jerk_max_u_msss = MIN(_jerk_max_u_msss, MIN(GRAVITY_MSS, _accel_max_u_mss) * (M_2PI * _pid_accel_u_cm_to_kt.filt_T_hz()) / 5.0); + _jerk_max_d_msss = _shaping_jerk_d_msss; + if (is_positive(_pid_accel_d_m.filt_T_hz())) { + _jerk_max_d_msss = MIN(_jerk_max_d_msss, MIN(GRAVITY_MSS, _accel_max_d_mss) * (M_2PI * _pid_accel_d_m.filt_T_hz()) / 5.0); } - if (is_positive(_pid_accel_u_cm_to_kt.filt_E_hz())) { - _jerk_max_u_msss = MIN(_jerk_max_u_msss, MIN(GRAVITY_MSS, _accel_max_u_mss) * (M_2PI * _pid_accel_u_cm_to_kt.filt_E_hz()) / 5.0); + if (is_positive(_pid_accel_d_m.filt_E_hz())) { + _jerk_max_d_msss = MIN(_jerk_max_d_msss, MIN(GRAVITY_MSS, _accel_max_d_mss) * (M_2PI * _pid_accel_d_m.filt_E_hz()) / 5.0); } } @@ -795,249 +814,245 @@ void AC_PosControl::set_max_speed_accel_U_m(float decent_speed_max_ms, float cli // Should only be called during initialization to avoid discontinuities. // See set_correction_speed_accel_U_m() for full details. // All values must be positive. -void AC_PosControl::set_correction_speed_accel_U_cm(float decent_speed_max_cms, float climb_speed_max_cms, float accel_max_u_cmss) +void AC_PosControl::D_set_correction_speed_accel_cm(float decent_speed_max_cms, float climb_speed_max_cms, float accel_max_u_cmss) { - set_correction_speed_accel_U_m(decent_speed_max_cms * 0.01, climb_speed_max_cms * 0.01, accel_max_u_cmss * 0.01); + D_set_correction_speed_accel_m(decent_speed_max_cms * 0.01, climb_speed_max_cms * 0.01, accel_max_u_cmss * 0.01); } // Sets vertical correction velocity and acceleration limits (m/s, m/s²). // These values constrain the correction output of the PID controller. // All values must be positive. -void AC_PosControl::set_correction_speed_accel_U_m(float decent_speed_max_ms, float climb_speed_max_ms, float accel_max_u_mss) +void AC_PosControl::D_set_correction_speed_accel_m(float decent_speed_max_ms, float climb_speed_max_ms, float accel_max_d_mss) { // define maximum position error and maximum first and second differential limits - _p_pos_u_m.set_limits(-fabsf(decent_speed_max_ms), fabsf(climb_speed_max_ms), fabsf(accel_max_u_mss), 0.0f); + _p_pos_d_m.set_limits(-fabsf(decent_speed_max_ms), fabsf(climb_speed_max_ms), fabsf(accel_max_d_mss), 0.0f); } // Initializes U-axis controller to current position, velocity, and acceleration, disallowing descent. // Used for takeoff or hold scenarios where downward motion is prohibited. -void AC_PosControl::init_U_controller_no_descent() +void AC_PosControl::D_init_controller_no_descent() { // Initialise the position controller to the current throttle, position, velocity and acceleration. - init_U_controller(); + D_init_controller(); // remove all descent if present - _vel_target_neu_ms.z = MAX(0.0, _vel_target_neu_ms.z); - _vel_desired_neu_ms.z = MAX(0.0, _vel_desired_neu_ms.z); - _vel_terrain_u_ms = MAX(0.0, _vel_terrain_u_ms); - _vel_offset_neu_ms.z = MAX(0.0, _vel_offset_neu_ms.z); - _accel_target_neu_mss.z = MAX(0.0, _accel_target_neu_mss.z); - _accel_desired_neu_mss.z = MAX(0.0, _accel_desired_neu_mss.z); - _accel_terrain_u_mss = MAX(0.0, _accel_terrain_u_mss); - _accel_offset_neu_mss.z = MAX(0.0, _accel_offset_neu_mss.z); + _vel_target_ned_ms.z = MIN(0.0, _vel_target_ned_ms.z); + _vel_desired_ned_ms.z = MIN(0.0, _vel_desired_ned_ms.z); + _vel_terrain_d_ms = MIN(0.0, _vel_terrain_d_ms); + _vel_offset_ned_ms.z = MIN(0.0, _vel_offset_ned_ms.z); + _accel_target_ned_mss.z = MIN(0.0, _accel_target_ned_mss.z); + _accel_desired_ned_mss.z = MIN(0.0, _accel_desired_ned_mss.z); + _accel_terrain_d_mss = MIN(0.0, _accel_terrain_d_mss); + _accel_offset_ned_mss.z = MIN(0.0, _accel_offset_ned_mss.z); } // Initializes U-axis controller to a stationary stopping point with zero velocity and acceleration. // Used when the trajectory starts at rest but the initial altitude is unspecified. -// The resulting position target can be retrieved with get_pos_target_NEU_m(). -void AC_PosControl::init_U_controller_stopping_point() +// The resulting position target can be retrieved with get_pos_target_NED_m(). +void AC_PosControl::D_init_controller_stopping_point() { // Initialise the position controller to the current throttle, position, velocity and acceleration. - init_U_controller(); + D_init_controller(); - get_stopping_point_U_m(_pos_desired_neu_m.z); - _pos_target_neu_m.z = _pos_desired_neu_m.z + _pos_offset_neu_m.z; - _vel_desired_neu_ms.z = 0.0f; - _accel_desired_neu_mss.z = 0.0f; + get_stopping_point_D_m(_pos_desired_ned_m.z); + _pos_target_ned_m.z = _pos_desired_ned_m.z + _pos_offset_ned_m.z; + _vel_desired_ned_ms.z = 0.0f; + _accel_desired_ned_mss.z = 0.0f; } // Smoothly decays U-axis acceleration to zero over time while maintaining current vertical velocity. // Reduces requested acceleration by ~95% every 0.5 seconds to avoid abrupt transitions. // `throttle_setting` is used to determine whether to preserve positive acceleration in low-thrust cases. -void AC_PosControl::relax_U_controller(float throttle_setting) +void AC_PosControl::D_relax_controller(float throttle_setting) { // Initialise the position controller to the current position, velocity and acceleration. - init_U_controller(); + D_init_controller(); - // init_U_controller has set the acceleration PID I term to generate the current throttle set point + // D_init_controller has set the acceleration PID I term to generate the current throttle set point // Use relax_integrator to decay the throttle set point to throttle_setting - _pid_accel_u_cm_to_kt.relax_integrator((throttle_setting - _motors.get_throttle_hover()) * 10.0 * 100.0, _dt_s, POSCONTROL_RELAX_TC); + _pid_accel_d_m.relax_integrator(-(throttle_setting - _motors.get_throttle_hover()), _dt_s, POSCONTROL_RELAX_TC); } // Fully initializes the U-axis controller with current position, velocity, acceleration, and attitude. // Used during standard controller activation when full state is known. // Private function shared by other vertical initializers. -void AC_PosControl::init_U_controller() +void AC_PosControl::D_init_controller() { // initialise terrain targets and offsets to zero init_terrain(); // initialise offsets to target offsets and ensure offset targets are zero if they have not been updated. - init_offsets_U(); + D_init_offsets(); - _pos_target_neu_m.z = _pos_estimate_neu_m.z; - _pos_desired_neu_m.z = _pos_target_neu_m.z - _pos_offset_neu_m.z; + _pos_target_ned_m.z = _pos_estimate_ned_m.z; + _pos_desired_ned_m.z = _pos_target_ned_m.z - _pos_offset_ned_m.z; - _vel_target_neu_ms.z = _vel_estimate_neu_ms.z; - _vel_desired_neu_ms.z = _vel_target_neu_ms.z - _vel_offset_neu_ms.z; + _vel_target_ned_ms.z = _vel_estimate_ned_ms.z; + _vel_desired_ned_ms.z = _vel_target_ned_ms.z - _vel_offset_ned_ms.z; // Reset I term of velocity PID - _pid_vel_u_cm.reset_filter(); - _pid_vel_u_cm.set_integrator(0.0f); + _pid_vel_d_m.reset_filter(); + _pid_vel_d_m.set_integrator(0.0f); - _accel_target_neu_mss.z = constrain_float(get_estimate_accel_U_mss(), -_accel_max_u_mss, _accel_max_u_mss); - _accel_desired_neu_mss.z = _accel_target_neu_mss.z - (_accel_offset_neu_mss.z + _accel_terrain_u_mss); - _pid_accel_u_cm_to_kt.reset_filter(); + _accel_target_ned_mss.z = constrain_float(get_estimated_accel_D_mss(), -_accel_max_d_mss, _accel_max_d_mss); + _accel_desired_ned_mss.z = _accel_target_ned_mss.z - (_accel_offset_ned_mss.z + _accel_terrain_d_mss); + _pid_accel_d_m.reset_filter(); // Set acceleration PID I term based on the current throttle - // Remove the expected P term due to _accel_desired_neu_mss.z being constrained to _accel_max_u_mss - // Remove the expected FF term due to non-zero _accel_target_neu_mss.z - _pid_accel_u_cm_to_kt.set_integrator((_attitude_control.get_throttle_in() - _motors.get_throttle_hover()) * 10.0 * 100.0 - - _pid_accel_u_cm_to_kt.kP() * (_accel_target_neu_mss.z - get_estimate_accel_U_mss()) * 100.0 - - _pid_accel_u_cm_to_kt.ff() * _accel_target_neu_mss.z * 100.0); + // Remove the expected P term due to _accel_desired_ned_mss.z being constrained to _accel_max_d_mss + // Remove the expected FF term due to non-zero _accel_target_ned_mss.z + _pid_accel_d_m.set_integrator(-(_attitude_control.get_throttle_in() - _motors.get_throttle_hover()) + - _pid_accel_d_m.kP() * (_accel_target_ned_mss.z - get_estimated_accel_D_mss()) + - _pid_accel_d_m.ff() * _accel_target_ned_mss.z); // initialise ekf z reset handler - init_ekf_U_reset(); + D_init_ekf_reset(); // initialise z_controller time out - _last_update_u_ticks = AP::scheduler().ticks32(); + _last_update_d_ticks = AP::scheduler().ticks32(); } // Sets the desired vertical acceleration in m/s² using jerk-limited shaping. // Smoothly transitions to the target acceleration from current kinematic state. -// Constraints: max acceleration and jerk set via set_max_speed_accel_U_m(). -void AC_PosControl::input_accel_U_m(float accel_u_mss) +// Constraints: max acceleration and jerk set via D_set_max_speed_accel_m(). +void AC_PosControl::input_accel_D_m(float accel_d_mss) { // calculated increased maximum jerk if over speed - float jerk_max_u_msss = _jerk_max_u_msss * calculate_overspeed_gain(); + float jerk_max_d_msss = _jerk_max_d_msss * calculate_overspeed_gain(); // adjust desired alt if motors have not hit their limits - update_pos_vel_accel(_pos_desired_neu_m.z, _vel_desired_neu_ms.z, _accel_desired_neu_mss.z, _dt_s, _limit_vector_neu.z, _p_pos_u_m.get_error(), _pid_vel_u_cm.get_error()); + update_pos_vel_accel(_pos_desired_ned_m.z, _vel_desired_ned_ms.z, _accel_desired_ned_mss.z, _dt_s, _limit_vector_ned.z, _p_pos_d_m.get_error(), _pid_vel_d_m.get_error()); - shape_accel(accel_u_mss, _accel_desired_neu_mss.z, jerk_max_u_msss, _dt_s); + shape_accel(accel_d_mss, _accel_desired_ned_mss.z, jerk_max_d_msss, _dt_s); } // Sets desired vertical velocity and acceleration (m/s, m/s²) using jerk-limited shaping. // Calculates required acceleration using current vertical kinematics. // If `limit_output` is true, limits apply to the combined (desired + correction) command. -void AC_PosControl::input_vel_accel_U_m(float &vel_u_ms, float accel_u_mss, bool limit_output) +void AC_PosControl::input_vel_accel_D_m(float &vel_d_ms, float accel_d_mss, bool limit_output) { // calculated increased maximum acceleration and jerk if over speed const float overspeed_gain = calculate_overspeed_gain(); - const float accel_max_u_mss = _accel_max_u_mss * overspeed_gain; - const float jerk_max_u_msss = _jerk_max_u_msss * overspeed_gain; + const float accel_max_d_mss = _accel_max_d_mss * overspeed_gain; + const float jerk_max_d_msss = _jerk_max_d_msss * overspeed_gain; // adjust desired alt if motors have not hit their limits - update_pos_vel_accel(_pos_desired_neu_m.z, _vel_desired_neu_ms.z, _accel_desired_neu_mss.z, _dt_s, _limit_vector_neu.z, _p_pos_u_m.get_error(), _pid_vel_u_cm.get_error()); + update_pos_vel_accel(_pos_desired_ned_m.z, _vel_desired_ned_ms.z, _accel_desired_ned_mss.z, _dt_s, _limit_vector_ned.z, _p_pos_d_m.get_error(), _pid_vel_d_m.get_error()); - shape_vel_accel(vel_u_ms, accel_u_mss, - _vel_desired_neu_ms.z, _accel_desired_neu_mss.z, - -constrain_float(accel_max_u_mss, 0.0, 7.5), accel_max_u_mss, - jerk_max_u_msss, _dt_s, limit_output); + shape_vel_accel(vel_d_ms, accel_d_mss, + _vel_desired_ned_ms.z, _accel_desired_ned_mss.z, + -accel_max_d_mss, constrain_float(accel_max_d_mss, 0.0, 7.5), + jerk_max_d_msss, _dt_s, limit_output); - update_vel_accel(vel_u_ms, accel_u_mss, _dt_s, 0.0, 0.0); + update_vel_accel(vel_d_ms, accel_d_mss, _dt_s, 0.0, 0.0); } // Generates a vertical trajectory using the given climb rate in cm/s and jerk-limited shaping. // Adjusts the internal target altitude based on integrated climb rate. -// See set_pos_target_U_from_climb_rate_ms() for full details. -void AC_PosControl::set_pos_target_U_from_climb_rate_cms(float vel_u_cms) +// See set_pos_target_D_from_climb_rate_m() for full details. +void AC_PosControl::D_set_pos_target_from_climb_rate_cms(float climb_rate_cms) { - set_pos_target_U_from_climb_rate_ms(vel_u_cms * 0.01); + D_set_pos_target_from_climb_rate_ms(climb_rate_cms * 0.01); } // Generates a vertical trajectory using the given climb rate in m/s and jerk-limited shaping. // Target altitude is updated over time by integrating the climb rate. -void AC_PosControl::set_pos_target_U_from_climb_rate_ms(float vel_u_ms) -{ - input_vel_accel_U_m(vel_u_ms, 0.0); -} - -// Descends at a given rate (m/s) using jerk-limited shaping for landing. -// Used during final descent phase to ensure smooth touchdown. -void AC_PosControl::land_at_climb_rate_ms(float vel_u_ms, bool ignore_descent_limit) +void AC_PosControl::D_set_pos_target_from_climb_rate_ms(float climb_rate_ms, bool ignore_descent_limit) { if (ignore_descent_limit) { - // turn off limits in the negative z direction - _limit_vector_neu.z = MAX(_limit_vector_neu.z, 0.0f); + // turn off limits in the down (positive z) direction + _limit_vector_ned.z = MIN(_limit_vector_ned.z, 0.0f); } - input_vel_accel_U_m(vel_u_ms, 0.0); + float vel_d_ms = -climb_rate_ms; + input_vel_accel_D_m(vel_d_ms, 0.0); } // Sets vertical position, velocity, and acceleration in cm using jerk-limited shaping. -// See input_pos_vel_accel_U_m() for full details. +// See input_pos_vel_accel_D_m() for full details. void AC_PosControl::input_pos_vel_accel_U_cm(float &pos_u_cm, float &vel_u_cms, float accel_u_cmss, bool limit_output) { - float pos_u_m = pos_u_cm * 0.01; - float vel_u_ms = vel_u_cms * 0.01; - input_pos_vel_accel_U_m(pos_u_m, vel_u_ms, accel_u_cmss * 0.01, limit_output); - pos_u_cm = pos_u_m * 100.0; - vel_u_cms = vel_u_ms * 100.0; + float pos_d_m = -pos_u_cm * 0.01; + float vel_d_ms = -vel_u_cms * 0.01; + const float accel_d_mss = -accel_u_cmss * 0.01; + input_pos_vel_accel_D_m(pos_d_m, vel_d_ms, accel_d_mss, limit_output); + pos_u_cm = -pos_d_m * 100.0; + vel_u_cms = -vel_d_ms * 100.0; } // Sets vertical position, velocity, and acceleration in meters using jerk-limited shaping. // Calculates required acceleration using current state and constraints. // If `limit_output` is true, limits are applied to combined (desired + correction) command. -void AC_PosControl::input_pos_vel_accel_U_m(float &pos_u_m, float &vel_u_ms, float accel_u_mss, bool limit_output) +void AC_PosControl::input_pos_vel_accel_D_m(float &pos_d_m, float &vel_d_ms, float accel_d_mss, bool limit_output) { // calculated increased maximum acceleration and jerk if over speed const float overspeed_gain = calculate_overspeed_gain(); - const float accel_max_u_mss = _accel_max_u_mss * overspeed_gain; - const float jerk_max_u_msss = _jerk_max_u_msss * overspeed_gain; + const float accel_max_d_mss = _accel_max_d_mss * overspeed_gain; + const float jerk_max_d_msss = _jerk_max_d_msss * overspeed_gain; // adjust desired altitude if motors have not hit their limits - update_pos_vel_accel(_pos_desired_neu_m.z, _vel_desired_neu_ms.z, _accel_desired_neu_mss.z, _dt_s, _limit_vector_neu.z, _p_pos_u_m.get_error(), _pid_vel_u_cm.get_error()); + update_pos_vel_accel(_pos_desired_ned_m.z, _vel_desired_ned_ms.z, _accel_desired_ned_mss.z, _dt_s, _limit_vector_ned.z, _p_pos_d_m.get_error(), _pid_vel_d_m.get_error()); - shape_pos_vel_accel(pos_u_m, vel_u_ms, accel_u_mss, - _pos_desired_neu_m.z, _vel_desired_neu_ms.z, _accel_desired_neu_mss.z, - -_vel_max_down_ms, _vel_max_up_ms, - -constrain_float(accel_max_u_mss, 0.0, 7.5), accel_max_u_mss, - jerk_max_u_msss, _dt_s, limit_output); + shape_pos_vel_accel(pos_d_m, vel_d_ms, accel_d_mss, + _pos_desired_ned_m.z, _vel_desired_ned_ms.z, _accel_desired_ned_mss.z, + -_vel_max_up_ms, _vel_max_down_ms, + -accel_max_d_mss, constrain_float(accel_max_d_mss, 0.0, 7.5), + jerk_max_d_msss, _dt_s, limit_output); - postype_t posp = pos_u_m; - update_pos_vel_accel(posp, vel_u_ms, accel_u_mss, _dt_s, 0.0, 0.0, 0.0); - pos_u_m = posp; + postype_t posp = pos_d_m; + update_pos_vel_accel(posp, vel_d_ms, accel_d_mss, _dt_s, 0.0, 0.0, 0.0); + pos_d_m = posp; } // Sets target altitude in cm using jerk-limited shaping to gradually move to the new position. -// See set_alt_target_with_slew_m() for full details. +// See D_set_alt_target_with_slew_m() for full details. void AC_PosControl::set_alt_target_with_slew_cm(float pos_u_cm) { - set_alt_target_with_slew_m(pos_u_cm * 0.01); + D_set_alt_target_with_slew_m(pos_u_cm * 0.01); } // Sets target altitude in meters using jerk-limited shaping. -void AC_PosControl::set_alt_target_with_slew_m(float pos_u_m) +void AC_PosControl::D_set_alt_target_with_slew_m(float pos_u_m) { + float pos_d_m = -pos_u_m; float zero = 0; - input_pos_vel_accel_U_m(pos_u_m, zero, 0); + input_pos_vel_accel_D_m(pos_d_m, zero, 0); } // Updates vertical (U) offsets by gradually moving them toward their targets. -void AC_PosControl::update_offsets_U() +void AC_PosControl::D_update_offsets() { // Check if vertical offset targets have timed out uint32_t now_ms = AP_HAL::millis(); - if (now_ms - _posvelaccel_offset_target_u_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) { + if (now_ms - _posvelaccel_offset_target_d_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) { // Timeout: reset U-axis offset targets to zero - _pos_offset_target_neu_m.z = 0.0; - _vel_offset_target_neu_ms.z = 0.0; - _accel_offset_target_neu_mss.z = 0.0; + _pos_offset_target_ned_m.z = 0.0; + _vel_offset_target_ned_ms.z = 0.0; + _accel_offset_target_ned_mss.z = 0.0; } // Advance current offset state using PID-derived feedback and vertical limits - postype_t p_offset_u_m = _pos_offset_neu_m.z; - update_pos_vel_accel(p_offset_u_m, _vel_offset_neu_ms.z, _accel_offset_neu_mss.z, _dt_s, MIN(_limit_vector_neu.z, 0.0f), _p_pos_u_m.get_error(), _pid_vel_u_cm.get_error()); - _pos_offset_neu_m.z = p_offset_u_m; + postype_t p_offset_d_m = _pos_offset_ned_m.z; + update_pos_vel_accel(p_offset_d_m, _vel_offset_ned_ms.z, _accel_offset_ned_mss.z, _dt_s, MIN(_limit_vector_ned.z, 0.0f), _p_pos_d_m.get_error(), _pid_vel_d_m.get_error()); + _pos_offset_ned_m.z = p_offset_d_m; // Shape offset trajectory (position/velocity/acceleration) using jerk-limited smoothing - shape_pos_vel_accel(_pos_offset_target_neu_m.z, _vel_offset_target_neu_ms.z, _accel_offset_target_neu_mss.z, - _pos_offset_neu_m.z, _vel_offset_neu_ms.z, _accel_offset_neu_mss.z, - -get_max_speed_down_ms(), get_max_speed_up_ms(), - -get_max_accel_U_mss(), get_max_accel_U_mss(), - _jerk_max_u_msss, _dt_s, false); + shape_pos_vel_accel(_pos_offset_target_ned_m.z, _vel_offset_target_ned_ms.z, _accel_offset_target_ned_mss.z, + _pos_offset_ned_m.z, _vel_offset_ned_ms.z, _accel_offset_ned_mss.z, + -get_max_speed_up_ms(), get_max_speed_down_ms(), + -D_get_max_accel_mss(), D_get_max_accel_mss(), + _jerk_max_d_msss, _dt_s, false); // Update target state forward in time with assumed zero velocity/acceleration targets - p_offset_u_m = _pos_offset_target_neu_m.z; - update_pos_vel_accel(p_offset_u_m, _vel_offset_target_neu_ms.z, _accel_offset_target_neu_mss.z, _dt_s, 0.0, 0.0, 0.0); - _pos_offset_target_neu_m.z = p_offset_u_m; + p_offset_d_m = _pos_offset_target_ned_m.z; + update_pos_vel_accel(p_offset_d_m, _vel_offset_target_ned_ms.z, _accel_offset_target_ned_mss.z, _dt_s, 0.0, 0.0, 0.0); + _pos_offset_target_ned_m.z = p_offset_d_m; } // Returns true if the U-axis controller has run in the last 5 control loop cycles. -bool AC_PosControl::is_active_U() const +bool AC_PosControl::D_is_active() const { - const uint32_t dt_ticks = AP::scheduler().ticks32() - _last_update_u_ticks; + const uint32_t dt_ticks = AP::scheduler().ticks32() - _last_update_d_ticks; return dt_ticks <= 1; } @@ -1045,88 +1060,88 @@ bool AC_PosControl::is_active_U() const // Computes output acceleration based on position and velocity errors using PID correction. // Feedforward velocity and acceleration are combined with corrections to produce a smooth vertical command. // Desired position, velocity, and acceleration must be set before calling. -void AC_PosControl::update_U_controller() +void AC_PosControl::D_update_controller() { // check for ekf z-axis position reset - handle_ekf_U_reset(); + D_handle_ekf_reset(); // Check for z_controller time out - if (!is_active_U()) { - init_U_controller(); + if (!D_is_active()) { + D_init_controller(); if (has_good_timing()) { // call internal error because initialisation has not been done INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } } - _last_update_u_ticks = AP::scheduler().ticks32(); + _last_update_d_ticks = AP::scheduler().ticks32(); // Update vertical offset targets and terrain estimate - update_offsets_U(); + D_update_offsets(); update_terrain(); // Position Controller // Combine desired + offset + terrain for final position target - _pos_target_neu_m.z = _pos_desired_neu_m.z + _pos_offset_neu_m.z + _pos_terrain_u_m; + _pos_target_ned_m.z = _pos_desired_ned_m.z + _pos_offset_ned_m.z + _pos_terrain_d_m; // P controller: convert position error to velocity target - _vel_target_neu_ms.z = _p_pos_u_m.update_all(_pos_target_neu_m.z, _pos_estimate_neu_m.z); - _vel_target_neu_ms.z *= AP::ahrs().getControlScaleZ(); + _vel_target_ned_ms.z = _p_pos_d_m.update_all(_pos_target_ned_m.z, _pos_estimate_ned_m.z); + _vel_target_ned_ms.z *= AP::ahrs().getControlScaleZ(); - _pos_desired_neu_m.z = _pos_target_neu_m.z - (_pos_offset_neu_m.z + _pos_terrain_u_m); + _pos_desired_ned_m.z = _pos_target_ned_m.z - (_pos_offset_ned_m.z + _pos_terrain_d_m); // add feed forward component - _vel_target_neu_ms.z += _vel_desired_neu_ms.z + _vel_offset_neu_ms.z + _vel_terrain_u_ms; + _vel_target_ned_ms.z += _vel_desired_ned_ms.z + _vel_offset_ned_ms.z + _vel_terrain_d_ms; // Velocity Controller // PID controller: convert velocity error to acceleration - _accel_target_neu_mss.z = _pid_vel_u_cm.update_all(_vel_target_neu_ms.z * 100.0, _vel_estimate_neu_ms.z * 100.0, _dt_s, _motors.limit.throttle_lower, _motors.limit.throttle_upper) * 0.01; - _accel_target_neu_mss.z *= AP::ahrs().getControlScaleZ(); + _accel_target_ned_mss.z = _pid_vel_d_m.update_all(_vel_target_ned_ms.z, _vel_estimate_ned_ms.z, _dt_s, _motors.limit.throttle_lower, _motors.limit.throttle_upper); + _accel_target_ned_mss.z *= AP::ahrs().getControlScaleZ(); // add feed forward component - _accel_target_neu_mss.z += _accel_desired_neu_mss.z + _accel_offset_neu_mss.z + _accel_terrain_u_mss; + _accel_target_ned_mss.z += _accel_desired_ned_mss.z + _accel_offset_ned_mss.z + _accel_terrain_d_mss; // Acceleration Controller // Gravity-compensated vertical acceleration measurement (positive = up) - const float measured_accel_u_mss = get_estimate_accel_U_mss(); + const float estimated_accel_d_mss = get_estimated_accel_D_mss(); // Ensure integrator can produce enough thrust to overcome hover throttle - if (_motors.get_throttle_hover() * 1000.0 > _pid_accel_u_cm_to_kt.imax()) { - _pid_accel_u_cm_to_kt.set_imax(_motors.get_throttle_hover() * 1000.0); + if (_motors.get_throttle_hover() > _pid_accel_d_m.imax()) { + _pid_accel_d_m.set_imax(_motors.get_throttle_hover()); } - float thrust_u_norm; + float thrust_d_norm; if (_vibe_comp_enabled) { // Use vibration-resistant throttle estimator (feedforward + scaled integrator) - thrust_u_norm = get_throttle_with_vibration_override(); + thrust_d_norm = get_throttle_with_vibration_override(); } else { // Standard PID update using vertical acceleration error - thrust_u_norm = _pid_accel_u_cm_to_kt.update_all(_accel_target_neu_mss.z * 100.0, measured_accel_u_mss * 100.0, _dt_s, (_motors.limit.throttle_lower || _motors.limit.throttle_upper)) * 0.001; + thrust_d_norm = _pid_accel_d_m.update_all(_accel_target_ned_mss.z, estimated_accel_d_mss, _dt_s, (_motors.limit.throttle_lower || _motors.limit.throttle_upper)); // Include FF contribution to reduce delay - thrust_u_norm += _pid_accel_u_cm_to_kt.get_ff() * 0.001; + thrust_d_norm += _pid_accel_d_m.get_ff(); } - thrust_u_norm += _motors.get_throttle_hover(); + thrust_d_norm -= _motors.get_throttle_hover(); // Actuator commands // Send final throttle output to attitude controller (includes angle boost) - _attitude_control.set_throttle_out(thrust_u_norm, true, POSCONTROL_THROTTLE_CUTOFF_FREQ_HZ); + _attitude_control.set_throttle_out(-thrust_d_norm, true, POSCONTROL_THROTTLE_CUTOFF_FREQ_HZ); // Check for vertical controller health // Update health indicator based on error magnitude vs configured speed range - float error_ratio = -_pid_vel_u_cm.get_error() * 0.01 / _vel_max_down_ms; - _vel_u_control_ratio += _dt_s * 0.1f * (0.5 - error_ratio); - _vel_u_control_ratio = constrain_float(_vel_u_control_ratio, 0.0f, 2.0f); + float error_ratio = _pid_vel_d_m.get_error() / _vel_max_down_ms; + _vel_d_control_ratio += _dt_s * 0.1f * (0.5 - error_ratio); + _vel_d_control_ratio = constrain_float(_vel_d_control_ratio, 0.0f, 2.0f); // set vertical component of the limit vector if (_motors.limit.throttle_upper) { - _limit_vector_neu.z = 1.0f; + _limit_vector_ned.z = -1.0f; } else if (_motors.limit.throttle_lower) { - _limit_vector_neu.z = -1.0f; + _limit_vector_ned.z = 1.0f; } else { - _limit_vector_neu.z = 0.0f; + _limit_vector_ned.z = 0.0f; } } @@ -1147,26 +1162,26 @@ float AC_PosControl::get_lean_angle_max_rad() const return radians(_lean_angle_max_deg); } -// Sets externally computed NEU position, velocity, and acceleration in meters, m/s, and m/s². +// Sets externally computed NED position, velocity, and acceleration in meters, m/s, and m/s². // Use when path planning or shaping is done outside this controller. -void AC_PosControl::set_pos_vel_accel_NEU_m(const Vector3p& pos_neu_m, const Vector3f& vel_neu_ms, const Vector3f& accel_neu_mss) +void AC_PosControl::set_pos_vel_accel_NED_m(const Vector3p& pos_ned_m, const Vector3f& vel_ned_ms, const Vector3f& accel_ned_mss) { - _pos_desired_neu_m = pos_neu_m; - _vel_desired_neu_ms = vel_neu_ms; - _accel_desired_neu_mss = accel_neu_mss; + _pos_desired_ned_m = pos_ned_m; + _vel_desired_ned_ms = vel_ned_ms; + _accel_desired_ned_mss = accel_ned_mss; } // Sets externally computed NE position, velocity, and acceleration in meters, m/s, and m/s². // Use when path planning or shaping is done outside this controller. void AC_PosControl::set_pos_vel_accel_NE_m(const Vector2p& pos_ne_m, const Vector2f& vel_ne_ms, const Vector2f& accel_ne_mss) { - _pos_desired_neu_m.xy() = pos_ne_m; - _vel_desired_neu_ms.xy() = vel_ne_ms; - _accel_desired_neu_mss.xy() = accel_ne_mss; + _pos_desired_ned_m.xy() = pos_ne_m; + _vel_desired_ned_ms.xy() = vel_ne_ms; + _accel_desired_ned_mss.xy() = accel_ne_mss; } -// Converts lean angles (rad) to NEU acceleration in m/s². -Vector3f AC_PosControl::lean_angles_rad_to_accel_NEU_mss(const Vector3f& att_target_euler_rad) const +// Converts lean angles (rad) to NED acceleration in m/s². +Vector3f AC_PosControl::lean_angles_rad_to_accel_NED_mss(const Vector3f& att_target_euler_rad) const { // rotate our roll, pitch angles into lat/lon frame const float sin_roll = sinf(att_target_euler_rad.x); @@ -1179,7 +1194,7 @@ Vector3f AC_PosControl::lean_angles_rad_to_accel_NEU_mss(const Vector3f& att_tar return Vector3f{ GRAVITY_MSS * (-cos_yaw * sin_pitch * cos_roll - sin_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.1f), GRAVITY_MSS * (-sin_yaw * sin_pitch * cos_roll + cos_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.1f), - GRAVITY_MSS + -GRAVITY_MSS }; } @@ -1189,129 +1204,120 @@ Vector3f AC_PosControl::lean_angles_rad_to_accel_NEU_mss(const Vector3f& att_tar void AC_PosControl::init_terrain() { // set terrain position and target to zero - _pos_terrain_target_u_m = 0.0; - _pos_terrain_u_m = 0.0; + _pos_terrain_target_d_m = 0.0; + _pos_terrain_d_m = 0.0; // set velocity offset to zero - _vel_terrain_u_ms = 0.0; + _vel_terrain_d_ms = 0.0; // set acceleration offset to zero - _accel_terrain_u_mss = 0.0; + _accel_terrain_d_mss = 0.0; } -// Initializes terrain altitude and terrain target to the same value (in cm). -// See init_pos_terrain_U_m() for full details. +// Initializes both the terrain altitude and terrain target to the same value +// (altitude above EKF origin in centimeters, Up-positive). +// See init_pos_terrain_D_m() for full description. void AC_PosControl::init_pos_terrain_U_cm(float pos_terrain_u_cm) { - init_pos_terrain_U_m(pos_terrain_u_cm * 0.01); + init_pos_terrain_D_m(-pos_terrain_u_cm * 0.01); } -// Initializes terrain altitude and terrain target to the same value (in meters). -void AC_PosControl::init_pos_terrain_U_m(float pos_terrain_u_m) +// Initializes both the terrain altitude and terrain target to the same value +// (relative to EKF origin in meters, Down-positive). +void AC_PosControl::init_pos_terrain_D_m(float pos_terrain_d_m) { - _pos_desired_neu_m.z -= (pos_terrain_u_m - _pos_terrain_u_m); - _pos_terrain_target_u_m = pos_terrain_u_m; - _pos_terrain_u_m = pos_terrain_u_m; + _pos_desired_ned_m.z -= (pos_terrain_d_m - _pos_terrain_d_m); + _pos_terrain_target_d_m = pos_terrain_d_m; + _pos_terrain_d_m = pos_terrain_d_m; } /// Offsets // Initializes NE position/velocity/acceleration offsets to match their respective targets. -void AC_PosControl::init_offsets_NE() +void AC_PosControl::NE_init_offsets() { // check for offset target timeout uint32_t now_ms = AP_HAL::millis(); if (now_ms - _posvelaccel_offset_target_ne_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) { - _pos_offset_target_neu_m.xy().zero(); - _vel_offset_target_neu_ms.xy().zero(); - _accel_offset_target_neu_mss.xy().zero(); + _pos_offset_target_ned_m.xy().zero(); + _vel_offset_target_ned_ms.xy().zero(); + _accel_offset_target_ned_mss.xy().zero(); } // set position offset to target - _pos_offset_neu_m.xy() = _pos_offset_target_neu_m.xy(); + _pos_offset_ned_m.xy() = _pos_offset_target_ned_m.xy(); // set velocity offset to target - _vel_offset_neu_ms.xy() = _vel_offset_target_neu_ms.xy(); + _vel_offset_ned_ms.xy() = _vel_offset_target_ned_ms.xy(); // set acceleration offset to target - _accel_offset_neu_mss.xy() = _accel_offset_target_neu_mss.xy(); + _accel_offset_ned_mss.xy() = _accel_offset_target_ned_mss.xy(); } // Initializes vertical (U) offsets to match their respective targets. -void AC_PosControl::init_offsets_U() +void AC_PosControl::D_init_offsets() { // check for offset target timeout uint32_t now_ms = AP_HAL::millis(); - if (now_ms - _posvelaccel_offset_target_u_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) { - _pos_offset_target_neu_m.z = 0.0; - _vel_offset_target_neu_ms.z = 0.0; - _accel_offset_target_neu_mss.z = 0.0; + if (now_ms - _posvelaccel_offset_target_d_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) { + _pos_offset_target_ned_m.z = 0.0; + _vel_offset_target_ned_ms.z = 0.0; + _accel_offset_target_ned_mss.z = 0.0; } // set position offset to target - _pos_offset_neu_m.z = _pos_offset_target_neu_m.z; + _pos_offset_ned_m.z = _pos_offset_target_ned_m.z; // set velocity offset to target - _vel_offset_neu_ms.z = _vel_offset_target_neu_ms.z; + _vel_offset_ned_ms.z = _vel_offset_target_ned_ms.z; // set acceleration offset to target - _accel_offset_neu_mss.z = _accel_offset_target_neu_mss.z; + _accel_offset_ned_mss.z = _accel_offset_target_ned_mss.z; } #if AP_SCRIPTING_ENABLED // Sets additional position, velocity, and acceleration offsets in meters (NED frame) for scripting. // Offsets are added to the controller’s internal target. // Used in LUA -bool AC_PosControl::set_posvelaccel_offset(const Vector3f &pos_offset_NED_m, const Vector3f &vel_offset_NED_ms, const Vector3f &accel_offset_NED_mss) +bool AC_PosControl::set_posvelaccel_offset(const Vector3f &pos_offset_ned_m, const Vector3f &vel_offset_ned_ms, const Vector3f &accel_offset_ned_mss) { - // Convert NED inputs to NEU frame: Z is inverted - set_posvelaccel_offset_target_NE_m(pos_offset_NED_m.topostype().xy(), vel_offset_NED_ms.xy(), accel_offset_NED_mss.xy()); - set_posvelaccel_offset_target_U_m(-pos_offset_NED_m.topostype().z, -vel_offset_NED_ms.z, -accel_offset_NED_mss.z); + set_posvelaccel_offset_target_NE_m(pos_offset_ned_m.topostype().xy(), vel_offset_ned_ms.xy(), accel_offset_ned_mss.xy()); + set_posvelaccel_offset_target_D_m(pos_offset_ned_m.topostype().z, vel_offset_ned_ms.z, accel_offset_ned_mss.z); return true; } // Retrieves current scripted offsets in meters (NED frame). // Used in LUA -bool AC_PosControl::get_posvelaccel_offset(Vector3f &pos_offset_NED_m, Vector3f &vel_offset_NED_ms, Vector3f &accel_offset_NED_mss) +bool AC_PosControl::get_posvelaccel_offset(Vector3f &pos_offset_ned_m, Vector3f &vel_offset_ned_ms, Vector3f &accel_offset_ned_mss) { - // Convert from internal NEU to NED by inverting Z - pos_offset_NED_m.xy() = _pos_offset_target_neu_m.xy().tofloat(); - pos_offset_NED_m.z = -_pos_offset_target_neu_m.z; - - vel_offset_NED_ms.xy() = _vel_offset_target_neu_ms.xy(); - vel_offset_NED_ms.z = -_vel_offset_target_neu_ms.z; - - accel_offset_NED_mss.xy() = _accel_offset_target_neu_mss.xy(); - accel_offset_NED_mss.z = -_accel_offset_target_neu_mss.z; + pos_offset_ned_m = _pos_offset_target_ned_m.tofloat(); + vel_offset_ned_ms = _vel_offset_target_ned_ms; + accel_offset_ned_mss = _accel_offset_target_ned_mss; return true; } // Retrieves current target velocity (NED frame, m/s) including any scripted offset. // Used in LUA -bool AC_PosControl::get_vel_target(Vector3f &vel_target_NED_ms) +bool AC_PosControl::get_vel_target(Vector3f &vel_target_ned_ms) { - if (!is_active_NE() || !is_active_U()) { + if (!NE_is_active() || !D_is_active()) { return false; } - // Convert NEU → NED by inverting Z - vel_target_NED_ms.xy() = _vel_target_neu_ms.xy(); - vel_target_NED_ms.z = -_vel_target_neu_ms.z; + vel_target_ned_ms = _vel_target_ned_ms; return true; } // Retrieves current target acceleration (NED frame, m/s²) including any scripted offset. // Used in LUA -bool AC_PosControl::get_accel_target(Vector3f &accel_target_NED_mss) +bool AC_PosControl::get_accel_target(Vector3f &accel_target_ned_mss) { - if (!is_active_NE() || !is_active_U()) { + if (!NE_is_active() || !D_is_active()) { return false; } - // Convert NEU → NED by inverting Z - accel_target_NED_mss.xy() = _accel_target_neu_mss.xy(); - accel_target_NED_mss.z = -_accel_target_neu_mss.z; + accel_target_ned_mss = _accel_target_ned_mss; return true; } #endif @@ -1320,52 +1326,52 @@ bool AC_PosControl::get_accel_target(Vector3f &accel_target_NED_mss) void AC_PosControl::set_posvelaccel_offset_target_NE_m(const Vector2p& pos_offset_target_ne_m, const Vector2f& vel_offset_target_ne_ms, const Vector2f& accel_offset_target_ne_mss) { // set position offset target - _pos_offset_target_neu_m.xy() = pos_offset_target_ne_m; + _pos_offset_target_ned_m.xy() = pos_offset_target_ne_m; // set velocity offset target - _vel_offset_target_neu_ms.xy() = vel_offset_target_ne_ms; + _vel_offset_target_ned_ms.xy() = vel_offset_target_ne_ms; // set acceleration offset target - _accel_offset_target_neu_mss.xy() = accel_offset_target_ne_mss; + _accel_offset_target_ned_mss.xy() = accel_offset_target_ne_mss; // record time of update so we can detect timeouts _posvelaccel_offset_target_ne_ms = AP_HAL::millis(); } -// Sets vertical offset targets (m, m/s, m/s²) from EKF origin. -void AC_PosControl::set_posvelaccel_offset_target_U_m(float pos_offset_target_u_m, float vel_offset_target_u_ms, const float accel_offset_target_u_mss) +// Sets vertical offset targets (m, m/s, m/s²) relative to EKF origin in meters, Down-positive. +void AC_PosControl::set_posvelaccel_offset_target_D_m(float pos_offset_target_d_m, float vel_offset_target_d_ms, const float accel_offset_target_d_mss) { // set position offset target - _pos_offset_target_neu_m.z = pos_offset_target_u_m; + _pos_offset_target_ned_m.z = pos_offset_target_d_m; // set velocity offset target - _vel_offset_target_neu_ms.z = vel_offset_target_u_ms; + _vel_offset_target_ned_ms.z = vel_offset_target_d_ms; // set acceleration offset target - _accel_offset_target_neu_mss.z = accel_offset_target_u_mss; + _accel_offset_target_ned_mss.z = accel_offset_target_d_mss; // record time of update so we can detect timeouts - _posvelaccel_offset_target_u_ms = AP_HAL::millis(); + _posvelaccel_offset_target_d_ms = AP_HAL::millis(); } // Returns desired thrust direction as a unit vector in the body frame. Vector3f AC_PosControl::get_thrust_vector() const { - Vector3f accel_target_neu_mss = get_accel_target_NEU_mss(); - accel_target_neu_mss.z = -GRAVITY_MSS; - return accel_target_neu_mss; + Vector3f accel_target_ned_mss = get_accel_target_NED_mss(); + accel_target_ned_mss.z = -GRAVITY_MSS; + return accel_target_ned_mss; } // Computes NE stopping point in meters based on current position, velocity, and acceleration. -void AC_PosControl::get_stopping_point_NE_m(Vector2p &stopping_point_neu_m) const +void AC_PosControl::get_stopping_point_NE_m(Vector2p &stopping_point_ne_m) const { // Start from estimated NE position with offset removed // todo: we should use the current target position and velocity if we are currently running the position controller - stopping_point_neu_m = _pos_estimate_neu_m.xy(); - stopping_point_neu_m -= _pos_offset_neu_m.xy(); + stopping_point_ne_m = _pos_estimate_ned_m.xy(); + stopping_point_ne_m -= _pos_offset_ned_m.xy(); - Vector2f vel_estimate_ne_ms = _vel_estimate_neu_ms.xy(); - vel_estimate_ne_ms -= _vel_offset_neu_ms.xy(); + Vector2f vel_estimate_ne_ms = _vel_estimate_ned_ms.xy(); + vel_estimate_ne_ms -= _vel_offset_ned_ms.xy(); // Compute velocity magnitude float speed_ne_ms = vel_estimate_ne_ms.length(); @@ -1384,33 +1390,33 @@ void AC_PosControl::get_stopping_point_NE_m(Vector2p &stopping_point_neu_m) cons // Project stopping distance along current velocity direction // todo: convert velocity to a unit vector instead. const float stopping_time_s = stopping_dist_m / speed_ne_ms; - stopping_point_neu_m += (vel_estimate_ne_ms * stopping_time_s).topostype(); + stopping_point_ne_m += (vel_estimate_ne_ms * stopping_time_s).topostype(); } // Computes vertical stopping point in meters based on current velocity and acceleration. -void AC_PosControl::get_stopping_point_U_m(postype_t &stopping_point_u_m) const +void AC_PosControl::get_stopping_point_D_m(postype_t &stopping_point_d_m) const { - float curr_pos_u_m = _pos_estimate_neu_m.z; - curr_pos_u_m -= _pos_offset_neu_m.z; + float curr_pos_d_m = _pos_estimate_ned_m.z; + curr_pos_d_m -= _pos_offset_ned_m.z; - float curr_vel_u_ms = _vel_estimate_neu_ms.z; - curr_vel_u_ms -= _vel_offset_neu_ms.z; + float curr_vel_d_ms = _vel_estimate_ned_ms.z; + curr_vel_d_ms -= _vel_offset_ned_ms.z; // If controller is unconfigured or disabled, return current position - if (!is_positive(_p_pos_u_m.kP()) || !is_positive(_accel_max_u_mss)) { - stopping_point_u_m = curr_pos_u_m; + if (!is_positive(_p_pos_d_m.kP()) || !is_positive(_accel_max_d_mss)) { + stopping_point_d_m = curr_pos_d_m; return; } // Estimate stopping point using current velocity, P gain, and max vertical acceleration - stopping_point_u_m = curr_pos_u_m + constrain_float(stopping_distance(curr_vel_u_ms, _p_pos_u_m.kP(), _accel_max_u_mss), - POSCONTROL_STOPPING_DIST_DOWN_MAX_M, POSCONTROL_STOPPING_DIST_UP_MAX_M); + stopping_point_d_m = curr_pos_d_m + constrain_float(stopping_distance(curr_vel_d_ms, _p_pos_d_m.kP(), _accel_max_d_mss), - POSCONTROL_STOPPING_DIST_UP_MAX_M, POSCONTROL_STOPPING_DIST_DOWN_MAX_M); } // Returns bearing from current position to position target in radians. // 0 = North, positive = clockwise. float AC_PosControl::get_bearing_to_target_rad() const { - return (_pos_target_neu_m.xy() - _pos_estimate_neu_m.xy()).angle(); + return (_pos_target_ned_m.xy() - _pos_estimate_ned_m.xy()).angle(); } @@ -1418,7 +1424,7 @@ float AC_PosControl::get_bearing_to_target_rad() const /// System methods /// -// Updates internal NEU position and velocity estimates from AHRS. +// Updates internal NED position and velocity estimates from AHRS. // Falls back to vertical-only data if horizontal velocity or position is invalid or vibration forces it. // When high_vibes is true, forces use of vertical fallback for velocity. void AC_PosControl::update_estimates(bool high_vibes) @@ -1430,8 +1436,7 @@ void AC_PosControl::update_estimates(bool high_vibes) pos_estimate_ned_m.z = posD; } } - _pos_estimate_neu_m.xy() = pos_estimate_ned_m.xy(); - _pos_estimate_neu_m.z = -pos_estimate_ned_m.z; + _pos_estimate_ned_m = pos_estimate_ned_m; Vector3f vel_estimate_ned_ms; if (!AP::ahrs().get_velocity_NED(vel_estimate_ned_ms) || high_vibes) { @@ -1440,8 +1445,7 @@ void AC_PosControl::update_estimates(bool high_vibes) vel_estimate_ned_ms.z = rate_z; } } - _vel_estimate_neu_ms.xy() = vel_estimate_ned_ms.xy(); - _vel_estimate_neu_ms.z = -vel_estimate_ned_ms.z; + _vel_estimate_ned_ms = vel_estimate_ned_ms; } // Calculates vertical throttle using vibration-resistant feedforward estimation. @@ -1449,80 +1453,80 @@ void AC_PosControl::update_estimates(bool high_vibes) // Integrator is adjusted using velocity error when PID is being overridden. float AC_PosControl::get_throttle_with_vibration_override() { - const float thr_per_accel_u_mss = _motors.get_throttle_hover() / GRAVITY_MSS; + const float thr_per_accel_d_mss = _motors.get_throttle_hover(); // Estimate throttle based on desired acceleration (manual feedforward gain). // Used when IMU vibrations corrupt raw acceleration measurements. // Allow integrator to compensate for velocity error only if not thrust-limited, // or if integrator is actively helping counteract velocity error direction. // ToDo: clear pid_info P, I and D terms for logging - if (!(_motors.limit.throttle_lower || _motors.limit.throttle_upper) || ((is_positive(_pid_accel_u_cm_to_kt.get_i()) && is_negative(_pid_vel_u_cm.get_error())) || (is_negative(_pid_accel_u_cm_to_kt.get_i()) && is_positive(_pid_vel_u_cm.get_error())))) { + if (!(_motors.limit.throttle_lower || _motors.limit.throttle_upper) || ((is_positive(_pid_accel_d_m.get_i()) && is_negative(_pid_vel_d_m.get_error())) || (is_negative(_pid_accel_d_m.get_i()) && is_positive(_pid_vel_d_m.get_error())))) { // Adjust integrator to help reduce velocity error. // Note: scale by velocity P-gain and an override-specific I gain. - _pid_accel_u_cm_to_kt.set_integrator(_pid_accel_u_cm_to_kt.get_i() + _dt_s * (thr_per_accel_u_mss / 100.0) * 1000.0 * _pid_vel_u_cm.get_error() * _pid_vel_u_cm.kP() * POSCONTROL_VIBE_COMP_I_GAIN); + _pid_accel_d_m.set_integrator(_pid_accel_d_m.get_i() + _dt_s * thr_per_accel_d_mss * _pid_vel_d_m.get_error() * _pid_vel_d_m.kP() * POSCONTROL_VIBE_COMP_I_GAIN); } // Final throttle = P term (feedforward) + scaled I term. - return POSCONTROL_VIBE_COMP_P_GAIN * thr_per_accel_u_mss * _accel_target_neu_mss.z + _pid_accel_u_cm_to_kt.get_i() * 0.001; + return POSCONTROL_VIBE_COMP_P_GAIN * thr_per_accel_d_mss * _accel_target_ned_mss.z + _pid_accel_d_m.get_i(); } -// Resets NEU position controller state to prevent transients when exiting standby. +// Resets NED position controller state to prevent transients when exiting standby. // Zeros I-terms and aligns targets to current position. -void AC_PosControl::standby_NEU_reset() +void AC_PosControl::NED_standby_reset() { - // Reset vertical acceleration controller_cm_to_kt integrator to prevent throttle bias on reentry - _pid_accel_u_cm_to_kt.set_integrator(0.0f); + // Reset vertical acceleration controller integrator to prevent throttle bias on reentry + _pid_accel_d_m.set_integrator(0.0f); // Reset position controller targets to match current estimate — avoids position jumps - _pos_target_neu_m = _pos_estimate_neu_m; + _pos_target_ned_m = _pos_estimate_ned_m; - // Reset horizontal velocity controller_cm integrator and derivative filter - _pid_vel_ne_cm.reset_filter(); + // Reset horizontal velocity controller integrator and derivative filter + _pid_vel_ne_m.reset_filter(); // Reset EKF XY position reset tracking for NE controller - init_ekf_NE_reset(); + NE_init_ekf_reset(); } #if HAL_LOGGING_ENABLED // Writes position controller diagnostic logs (PSCN, PSCE, etc). void AC_PosControl::write_log() { - if (is_active_NE()) { + if (NE_is_active()) { float accel_n_mss, accel_e_mss; lean_angles_to_accel_NE_mss(accel_n_mss, accel_e_mss); // Log North-axis position control (PSCN): desired, target, and actual - Write_PSCN(_pos_desired_neu_m.x, _pos_target_neu_m.x, _pos_estimate_neu_m.x , - _vel_desired_neu_ms.x, _vel_target_neu_ms.x, _vel_estimate_neu_ms.x, - _accel_desired_neu_mss.x, _accel_target_neu_mss.x, accel_n_mss); + Write_PSCN(_pos_desired_ned_m.x, _pos_target_ned_m.x, _pos_estimate_ned_m.x , + _vel_desired_ned_ms.x, _vel_target_ned_ms.x, _vel_estimate_ned_ms.x, + _accel_desired_ned_mss.x, _accel_target_ned_mss.x, accel_n_mss); // Log East-axis position control (PSCE): desired, target, and actual - Write_PSCE(_pos_desired_neu_m.y, _pos_target_neu_m.y, _pos_estimate_neu_m.y, - _vel_desired_neu_ms.y, _vel_target_neu_ms.y, _vel_estimate_neu_ms.y, - _accel_desired_neu_mss.y, _accel_target_neu_mss.y, accel_e_mss); + Write_PSCE(_pos_desired_ned_m.y, _pos_target_ned_m.y, _pos_estimate_ned_m.y, + _vel_desired_ned_ms.y, _vel_target_ned_ms.y, _vel_estimate_ned_ms.y, + _accel_desired_ned_mss.y, _accel_target_ned_mss.y, accel_e_mss); // log offsets if they are being used - if (!_pos_offset_neu_m.xy().is_zero()) { + if (!_pos_offset_ned_m.xy().is_zero()) { // Log North offset tracking (PSON) - Write_PSON(_pos_offset_target_neu_m.x, _pos_offset_neu_m.x, _vel_offset_target_neu_ms.x, _vel_offset_neu_ms.x, _accel_offset_target_neu_mss.x, _accel_offset_neu_mss.x); + Write_PSON(_pos_offset_target_ned_m.x, _pos_offset_ned_m.x, _vel_offset_target_ned_ms.x, _vel_offset_ned_ms.x, _accel_offset_target_ned_mss.x, _accel_offset_ned_mss.x); // Log East offset tracking (PSOE) - Write_PSOE(_pos_offset_target_neu_m.y, _pos_offset_neu_m.y, _vel_offset_target_neu_ms.y, _vel_offset_neu_ms.y, _accel_offset_target_neu_mss.y, _accel_offset_neu_mss.y); + Write_PSOE(_pos_offset_target_ned_m.y, _pos_offset_ned_m.y, _vel_offset_target_ned_ms.y, _vel_offset_ned_ms.y, _accel_offset_target_ned_mss.y, _accel_offset_ned_mss.y); } } - if (is_active_U()) { + if (D_is_active()) { // Log Down-axis position control (PSCD) - Write_PSCD(-_pos_desired_neu_m.z, -_pos_target_neu_m.z, -_pos_estimate_neu_m.z, - -_vel_desired_neu_ms.z, -_vel_target_neu_ms.z, -_vel_estimate_neu_ms.z, - -_accel_desired_neu_mss.z, -_accel_target_neu_mss.z, -get_estimate_accel_U_mss()); + Write_PSCD(_pos_desired_ned_m.z, _pos_target_ned_m.z, _pos_estimate_ned_m.z, + _vel_desired_ned_ms.z, _vel_target_ned_ms.z, _vel_estimate_ned_ms.z, + _accel_desired_ned_mss.z, _accel_target_ned_mss.z, get_estimated_accel_D_mss()); // log down and terrain offsets if they are being used - if (!is_zero(_pos_offset_neu_m.z)) { + if (!is_zero(_pos_offset_ned_m.z)) { // Log Down offset tracking (PSOD) - Write_PSOD(-_pos_offset_target_neu_m.z, -_pos_offset_neu_m.z, -_vel_offset_target_neu_ms.z, -_vel_offset_neu_ms.z, -_accel_offset_target_neu_mss.z, -_accel_offset_neu_mss.z); + Write_PSOD(_pos_offset_target_ned_m.z, _pos_offset_ned_m.z, _vel_offset_target_ned_ms.z, _vel_offset_ned_ms.z, _accel_offset_target_ned_mss.z, _accel_offset_ned_mss.z); } - if (!is_zero(_pos_terrain_u_m)) { + if (!is_zero(_pos_terrain_d_m)) { // Log terrain-following offset (PSOT) - Write_PSOT(-_pos_terrain_target_u_m, -_pos_terrain_u_m, 0, -_vel_terrain_u_ms, 0, -_accel_terrain_u_mss); + Write_PSOT(_pos_terrain_target_d_m, _pos_terrain_d_m, 0, _vel_terrain_d_ms, 0, _accel_terrain_d_mss); } } } @@ -1532,13 +1536,13 @@ void AC_PosControl::write_log() // Used to assess horizontal deviation from path. float AC_PosControl::crosstrack_error_m() const { - const Vector2f pos_error = (_pos_target_neu_m.xy() - _pos_estimate_neu_m.xy()).tofloat(); - if (is_zero(_vel_desired_neu_ms.xy().length_squared())) { + const Vector2f pos_error = (_pos_target_ned_m.xy() - _pos_estimate_ned_m.xy()).tofloat(); + if (is_zero(_vel_desired_ned_ms.xy().length_squared())) { // No desired velocity → return direct distance to target return pos_error.length(); } else { // Project position error onto desired velocity vector - const Vector2f vel_unit = _vel_desired_neu_ms.xy().normalized(); + const Vector2f vel_unit = _vel_desired_ned_ms.xy().normalized(); const float dot_error = pos_error * vel_unit; // Use Pythagorean difference to isolate perpendicular (cross-track) component @@ -1551,15 +1555,15 @@ float AC_PosControl::crosstrack_error_m() const // Returns true if the requested forward pitch is limited by the configured tilt constraint. bool AC_PosControl::get_fwd_pitch_is_limited() const { - if (_limit_vector_neu.xy().is_zero()) { + if (_limit_vector_ned.xy().is_zero()) { return false; } const float angle_max_rad = MIN(_attitude_control.get_althold_lean_angle_max_rad(), get_lean_angle_max_rad()); const float accel_max_mss = angle_rad_to_accel_mss(angle_max_rad); // Check for pitch limiting in the forward direction - const float accel_fwd_unlimited_mss = _limit_vector_neu.x * _ahrs.cos_yaw() + _limit_vector_neu.y * _ahrs.sin_yaw(); + const float accel_fwd_unlimited_mss = _limit_vector_ned.x * _ahrs.cos_yaw() + _limit_vector_ned.y * _ahrs.sin_yaw(); const float pitch_target_unlimited_deg = accel_mss_to_angle_deg(- MIN(accel_fwd_unlimited_mss, accel_max_mss)); - const float accel_fwd_limited = _accel_target_neu_mss.x * _ahrs.cos_yaw() + _accel_target_neu_mss.y * _ahrs.sin_yaw(); + const float accel_fwd_limited = _accel_target_ned_mss.x * _ahrs.cos_yaw() + _accel_target_ned_mss.y * _ahrs.sin_yaw(); const float pitch_target_limited_deg = accel_mss_to_angle_deg(- accel_fwd_limited); return is_negative(pitch_target_unlimited_deg) && pitch_target_unlimited_deg < pitch_target_limited_deg; @@ -1572,23 +1576,23 @@ bool AC_PosControl::get_fwd_pitch_is_limited() const /// Terrain -// Updates terrain estimate (_pos_terrain_u_m) toward target using filter time constants. +// Updates terrain estimate (_pos_terrain_d_m) toward target using filter time constants. void AC_PosControl::update_terrain() { // update position, velocity, acceleration offsets for this iteration - postype_t pos_terrain_u_m = _pos_terrain_u_m; - update_pos_vel_accel(pos_terrain_u_m, _vel_terrain_u_ms, _accel_terrain_u_mss, _dt_s, MIN(_limit_vector_neu.z, 0.0f), _p_pos_u_m.get_error(), _pid_vel_u_cm.get_error()); - _pos_terrain_u_m = pos_terrain_u_m; + postype_t pos_terrain_d_m = _pos_terrain_d_m; + update_pos_vel_accel(pos_terrain_d_m, _vel_terrain_d_ms, _accel_terrain_d_mss, _dt_s, MIN(_limit_vector_ned.z, 0.0f), _p_pos_d_m.get_error(), _pid_vel_d_m.get_error()); + _pos_terrain_d_m = pos_terrain_d_m; // input shape horizontal position, velocity and acceleration offsets - shape_pos_vel_accel(_pos_terrain_target_u_m, 0.0, 0.0, - _pos_terrain_u_m, _vel_terrain_u_ms, _accel_terrain_u_mss, - -get_max_speed_down_ms(), get_max_speed_up_ms(), - -get_max_accel_U_mss(), get_max_accel_U_mss(), - _jerk_max_u_msss, _dt_s, false); + shape_pos_vel_accel(_pos_terrain_target_d_m, 0.0, 0.0, + _pos_terrain_d_m, _vel_terrain_d_ms, _accel_terrain_d_mss, + -get_max_speed_up_ms(), get_max_speed_down_ms(), + -D_get_max_accel_mss(), D_get_max_accel_mss(), + _jerk_max_d_msss, _dt_s, false); - // we do not have to update _pos_terrain_target_u_m because we assume the target velocity and acceleration are zero - // if we know how fast the terain altitude is changing we would add update_pos_vel_accel for _pos_terrain_target_u_m here + // we do not have to update _pos_terrain_target_d_m because we assume the target velocity and acceleration are zero + // if we know how fast the terrain altitude is changing we would add update_pos_vel_accel for _pos_terrain_target_d_m here } // Converts horizontal acceleration (m/s²) to roll/pitch lean angles in radians. @@ -1610,7 +1614,7 @@ void AC_PosControl::lean_angles_to_accel_NE_mss(float& accel_n_mss, float& accel // rotate our roll, pitch angles into lat/lon frame Vector3f att_target_euler_rad = _attitude_control.get_att_target_euler_rad(); att_target_euler_rad.z = _ahrs.yaw; - Vector3f accel_ne_mss = lean_angles_rad_to_accel_NEU_mss(att_target_euler_rad); + Vector3f accel_ne_mss = lean_angles_rad_to_accel_NED_mss(att_target_euler_rad); accel_n_mss = accel_ne_mss.x; accel_e_mss = accel_ne_mss.y; @@ -1622,24 +1626,24 @@ void AC_PosControl::calculate_yaw_and_rate_yaw() { // Calculate the turn rate float turn_rate_rads = 0.0f; - const float vel_desired_length_ne_ms = _vel_desired_neu_ms.xy().length(); + const float vel_desired_length_ne_ms = _vel_desired_ned_ms.xy().length(); if (is_positive(vel_desired_length_ne_ms)) { // Project acceleration vector into velocity direction to extract forward acceleration component - const float accel_forward_mss = (_accel_desired_neu_mss.x * _vel_desired_neu_ms.x + _accel_desired_neu_mss.y * _vel_desired_neu_ms.y) / vel_desired_length_ne_ms; + const float accel_forward_mss = (_accel_desired_ned_mss.x * _vel_desired_ned_ms.x + _accel_desired_ned_mss.y * _vel_desired_ned_ms.y) / vel_desired_length_ne_ms; // Subtract forward component to isolate turn acceleration perpendicular to velocity vector - const Vector2f accel_turn_ne_mss = _accel_desired_neu_mss.xy() - _vel_desired_neu_ms.xy() * accel_forward_mss / vel_desired_length_ne_ms; + const Vector2f accel_turn_ne_mss = _accel_desired_ned_mss.xy() - _vel_desired_ned_ms.xy() * accel_forward_mss / vel_desired_length_ne_ms; // Compute turn rate from lateral acceleration and velocity (centripetal formula) const float accel_turn_length_ne_mss = accel_turn_ne_mss.length(); turn_rate_rads = accel_turn_length_ne_mss / vel_desired_length_ne_ms; // Determine turn direction: positive = clockwise (right) - if ((accel_turn_ne_mss.y * _vel_desired_neu_ms.x - accel_turn_ne_mss.x * _vel_desired_neu_ms.y) < 0.0) { + if ((accel_turn_ne_mss.y * _vel_desired_ned_ms.x - accel_turn_ne_mss.x * _vel_desired_ned_ms.y) < 0.0) { turn_rate_rads = -turn_rate_rads; } } // If vehicle is moving significantly, align yaw to velocity vector and apply computed turn rate if (vel_desired_length_ne_ms > _vel_max_ne_ms * 0.05f) { - _yaw_target_rad = _vel_desired_neu_ms.xy().angle(); + _yaw_target_rad = _vel_desired_ned_ms.xy().angle(); _yaw_rate_target_rads = turn_rate_rads; return; } @@ -1653,13 +1657,13 @@ void AC_PosControl::calculate_yaw_and_rate_yaw() float AC_PosControl::calculate_overspeed_gain() { // If desired descent speed exceeds configured max, scale acceleration/jerk proportionally - if (_vel_desired_neu_ms.z < -_vel_max_down_ms && !is_zero(_vel_max_down_ms)) { - return -POSCONTROL_OVERSPEED_GAIN_U * _vel_desired_neu_ms.z / _vel_max_down_ms; + if (_vel_desired_ned_ms.z > _vel_max_down_ms && !is_zero(_vel_max_down_ms)) { + return POSCONTROL_OVERSPEED_GAIN_U * _vel_desired_ned_ms.z / _vel_max_down_ms; } // If desired climb speed exceeds configured max, scale acceleration/jerk proportionally - if (_vel_desired_neu_ms.z > _vel_max_up_ms && !is_zero(_vel_max_up_ms)) { - return POSCONTROL_OVERSPEED_GAIN_U * _vel_desired_neu_ms.z / _vel_max_up_ms; + if (_vel_desired_ned_ms.z < -_vel_max_up_ms && !is_zero(_vel_max_up_ms)) { + return -POSCONTROL_OVERSPEED_GAIN_U * _vel_desired_ned_ms.z / _vel_max_up_ms; } // Within normal speed limits — use nominal acceleration and jerk @@ -1667,14 +1671,14 @@ float AC_PosControl::calculate_overspeed_gain() } // Initializes tracking of NE EKF position resets. -void AC_PosControl::init_ekf_NE_reset() +void AC_PosControl::NE_init_ekf_reset() { Vector2f pos_shift; _ekf_ne_reset_ms = _ahrs.getLastPosNorthEastReset(pos_shift); } // Handles NE position reset detection and response (e.g., clearing accumulated errors). -void AC_PosControl::handle_ekf_NE_reset() +void AC_PosControl::NE_handle_ekf_reset() { // Check for EKF-reported NE position shift since last update Vector2f pos_shift_ne_m; @@ -1686,23 +1690,23 @@ void AC_PosControl::handle_ekf_NE_reset() // This ensures controller output remains continuous after EKF realigns the origin. // Reconstruct position target relative to the to new EKF estimation to maintain the current position error - Vector2p delta_pos_estimate_ne_m = _p_pos_ne_m.get_error().topostype() - (_pos_target_neu_m.xy() - _pos_estimate_neu_m.xy()); - _pos_target_neu_m.xy() += delta_pos_estimate_ne_m; + Vector2p delta_pos_estimate_ne_m = _p_pos_ne_m.get_error().topostype() - (_pos_target_ned_m.xy() - _pos_estimate_ned_m.xy()); + _pos_target_ned_m.xy() += delta_pos_estimate_ne_m; // Reconstruct velocity target relative to the to new EKF estimation to maintain the current velocity error - Vector2f delta_vel_estimate_ne_ms = _pid_vel_ne_cm.get_error() * 0.01 - (_vel_target_neu_ms.xy() - _vel_estimate_neu_ms.xy()); - _vel_target_neu_ms.xy() += delta_vel_estimate_ne_ms; + Vector2f delta_vel_estimate_ne_ms = _pid_vel_ne_m.get_error() - (_vel_target_ned_ms.xy() - _vel_estimate_ned_ms.xy()); + _vel_target_ned_ms.xy() += delta_vel_estimate_ne_ms; switch (_ekf_reset_method) { case EKFResetMethod::MoveTarget: // Reset NE controller desired position and velocity to preserve actual position control during Loiter, PosHold, etc. - _pos_desired_neu_m.xy() += delta_pos_estimate_ne_m; - _vel_desired_neu_ms.xy() += delta_vel_estimate_ne_ms; + _pos_desired_ned_m.xy() += delta_pos_estimate_ne_m; + _vel_desired_ned_ms.xy() += delta_vel_estimate_ne_ms; break; case EKFResetMethod::MoveVehicle: // Move the change in estimate into the offsest to move the aircraft to our new estimate smoothly during Auto, Guided, etc. - _pos_offset_neu_m.xy() += delta_pos_estimate_ne_m; - _vel_offset_neu_ms.xy() += delta_vel_estimate_ne_ms; + _pos_offset_ned_m.xy() += delta_pos_estimate_ne_m; + _vel_offset_ned_ms.xy() += delta_vel_estimate_ne_ms; break; } _ekf_ne_reset_ms = reset_ms; @@ -1710,14 +1714,14 @@ void AC_PosControl::handle_ekf_NE_reset() } // Initializes tracking of vertical (U) EKF resets. -void AC_PosControl::init_ekf_U_reset() +void AC_PosControl::D_init_ekf_reset() { float alt_shift_d_m; - _ekf_u_reset_ms = _ahrs.getLastPosDownReset(alt_shift_d_m); + _ekf_d_reset_ms = _ahrs.getLastPosDownReset(alt_shift_d_m); } // Handles U EKF reset detection and response. -void AC_PosControl::handle_ekf_U_reset() +void AC_PosControl::D_handle_ekf_reset() { // Check for EKF-reported Down-axis shift since last update float pos_shift_d_m; @@ -1725,29 +1729,29 @@ void AC_PosControl::handle_ekf_U_reset() // todo: the actual difference in position and velocity estimation. // This will prevent the need to pause error calculation for one cycle. - if (reset_ms != 0 && reset_ms != _ekf_u_reset_ms) { + if (reset_ms != 0 && reset_ms != _ekf_d_reset_ms) { // This ensures controller output remains continuous after EKF realigns the origin. // Reconstruct position target relative to the to new EKF estimation to maintain the current position error - postype_t delta_pos_estimate_u_m = _p_pos_u_m.get_error() - (_pos_target_neu_m.z - _pos_estimate_neu_m.z); - _pos_target_neu_m.z += delta_pos_estimate_u_m; + postype_t delta_pos_estimate_d_m = _p_pos_d_m.get_error() - (_pos_target_ned_m.z - _pos_estimate_ned_m.z); + _pos_target_ned_m.z += delta_pos_estimate_d_m; // Reconstruct velocity target relative to the to new EKF estimation to maintain the current velocity error - float delta_vel_estimate_u_ms = _pid_vel_u_cm.get_error() * 0.01 - (_vel_target_neu_ms.z - _vel_estimate_neu_ms.z); - _vel_target_neu_ms.z += delta_vel_estimate_u_ms; + float delta_vel_estimate_d_ms = _pid_vel_d_m.get_error() - (_vel_target_ned_ms.z - _vel_estimate_ned_ms.z); + _vel_target_ned_ms.z += delta_vel_estimate_d_ms; switch (_ekf_reset_method) { case EKFResetMethod::MoveTarget: // Reset U controller desired position and velocity to preserve actual position control during Loiter, PosHold, etc. - _pos_desired_neu_m.z += delta_pos_estimate_u_m; - _vel_desired_neu_ms.z += delta_vel_estimate_u_ms; + _pos_desired_ned_m.z += delta_pos_estimate_d_m; + _vel_desired_ned_ms.z += delta_vel_estimate_d_ms; break; case EKFResetMethod::MoveVehicle: // Move the change in estimate into the offsest to move the aircraft to our new estimate smoothly during Auto, Guided, etc. - _pos_offset_neu_m.z += delta_pos_estimate_u_m; - _vel_offset_neu_ms.z += delta_vel_estimate_u_ms; + _pos_offset_ned_m.z += delta_pos_estimate_d_m; + _vel_offset_ned_ms.z += delta_vel_estimate_d_ms; break; } - _ekf_u_reset_ms = reset_ms; + _ekf_d_reset_ms = reset_ms; } } @@ -1757,24 +1761,24 @@ bool AC_PosControl::pre_arm_checks(const char *param_prefix, char *failure_msg, const uint8_t failure_msg_len) { - if (!is_positive(get_pos_NE_p().kP())) { - hal.util->snprintf(failure_msg, failure_msg_len, "%s_POSXY_P must be > 0", param_prefix); + if (!is_positive(NE_get_pos_p().kP())) { + hal.util->snprintf(failure_msg, failure_msg_len, "%s_NE_POS_P must be > 0", param_prefix); return false; } - if (!is_positive(get_pos_U_p().kP())) { - hal.util->snprintf(failure_msg, failure_msg_len, "%s_POSZ_P must be > 0", param_prefix); + if (!is_positive(D_get_pos_p().kP())) { + hal.util->snprintf(failure_msg, failure_msg_len, "%s_D_POS_P must be > 0", param_prefix); return false; } - if (!is_positive(get_vel_U_pid().kP())) { - hal.util->snprintf(failure_msg, failure_msg_len, "%s_VELZ_P must be > 0", param_prefix); + if (!is_positive(D_get_vel_pid().kP())) { + hal.util->snprintf(failure_msg, failure_msg_len, "%s_D_VEL_P must be > 0", param_prefix); return false; } - if (!is_positive(get_accel_U_pid().kP())) { - hal.util->snprintf(failure_msg, failure_msg_len, "%s_ACCZ_P must be > 0", param_prefix); + if (!is_positive(D_get_accel_pid().kP())) { + hal.util->snprintf(failure_msg, failure_msg_len, "%s_D_ACC_P must be > 0", param_prefix); return false; } - if (!is_positive(get_accel_U_pid().kI())) { - hal.util->snprintf(failure_msg, failure_msg_len, "%s_ACCZ_I must be > 0", param_prefix); + if (!is_positive(D_get_accel_pid().kI())) { + hal.util->snprintf(failure_msg, failure_msg_len, "%s_D_ACC_I must be > 0", param_prefix); return false; } @@ -1793,3 +1797,113 @@ bool AC_PosControl::has_good_timing(void) const // real boards are assumed to have good timing return true; } + +// perform any required parameter conversions +void AC_PosControl::convert_parameters() +{ + // find param table key + uint16_t k_param_psc_key; + if (!AP_Param::find_key_by_pointer(this, k_param_psc_key)) { + return; + } + + // return immediately if parameter conversion has already been performed + if (_pid_accel_d_m.kP().configured()) { + return; + } + + // PARAMETER_CONVERSION - Added: Nov-2025 for 4.7 + // parameters that are simply moved +#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) + static const AP_Param::ConversionInfo conversion_info[] = { + { k_param_psc_key, 258257, AP_PARAM_FLOAT, "Q_P_D_VEL_P" }, // Q_P_VELZ_P moved to Q_P_D_VEL_P + { k_param_psc_key, 4305, AP_PARAM_FLOAT, "Q_P_D_VEL_I" }, // Q_P_VELZ_I moved to Q_P_D_VEL_I + { k_param_psc_key, 16593, AP_PARAM_FLOAT, "Q_P_D_VEL_D" }, // Q_P_VELZ_D moved to Q_P_D_VEL_D + { k_param_psc_key, 12497, AP_PARAM_FLOAT, "Q_P_D_VEL_FLTE" }, // Q_P_VELZ_FLTE moved to Q_P_D_VEL_FLTE + { k_param_psc_key, 20689, AP_PARAM_FLOAT, "Q_P_D_VEL_FLTD" }, // Q_P_VELZ_FLTD moved to Q_P_D_VEL_FLTD + { k_param_psc_key, 24785, AP_PARAM_FLOAT, "Q_P_D_VEL_FF" }, // Q_P_VELZ_FF moved to Q_P_D_VEL_FF + { k_param_psc_key, 16657, AP_PARAM_FLOAT, "Q_P_D_ACC_FF" }, // Q_P_ACCZ_FF moved to Q_P_D_ACC_FF + { k_param_psc_key, 37137, AP_PARAM_FLOAT, "Q_P_D_ACC_FLTT" }, // Q_P_ACCZ_FLTT moved to Q_P_D_ACC_FLTT + { k_param_psc_key, 41233, AP_PARAM_FLOAT, "Q_P_D_ACC_FLTE" }, // Q_P_ACCZ_FLTE moved to Q_P_D_ACC_FLTE + { k_param_psc_key, 45329, AP_PARAM_FLOAT, "Q_P_D_ACC_FLTD" }, // Q_P_ACCZ_FLTD moved to Q_P_D_ACC_FLTD + { k_param_psc_key, 49425, AP_PARAM_FLOAT, "Q_P_D_ACC_SMAX" }, // Q_P_ACCZ_SMAX moved to Q_P_D_ACC_SMAX + { k_param_psc_key, 53521, AP_PARAM_FLOAT, "Q_P_D_ACC_PDMX" }, // Q_P_ACCZ_PDMX moved to Q_P_D_ACC_PDMX + { k_param_psc_key, 57617, AP_PARAM_FLOAT, "Q_P_D_ACC_D_FF" }, // Q_P_ACCZ_D_FF moved to Q_P_D_ACC_D_FF + { k_param_psc_key, 65809, AP_PARAM_INT8, "Q_P_D_ACC_NEF" }, // Q_P_ACCZ_NEF moved to Q_P_D_ACC_NEF + { k_param_psc_key, 61713, AP_PARAM_INT8, "Q_P_D_ACC_NTF" }, // Q_P_ACCZ_NTF moved to Q_P_D_ACC_NTF + { k_param_psc_key, 258449, AP_PARAM_FLOAT, "Q_P_NE_VEL_P" }, // Q_P_VELXY_P moved to Q_P_NE_VEL_P + { k_param_psc_key, 4497, AP_PARAM_FLOAT, "Q_P_NE_VEL_I" }, // Q_P_VELXY_I moved to Q_P_NE_VEL_I + { k_param_psc_key, 16785, AP_PARAM_FLOAT, "Q_P_NE_VEL_D" }, // Q_P_VELXY_D moved to Q_P_NE_VEL_D + { k_param_psc_key, 12689, AP_PARAM_FLOAT, "Q_P_NE_VEL_FLTE" },// Q_P_VELXY_FLTE moved to Q_P_NE_VEL_FLTE + { k_param_psc_key, 20881, AP_PARAM_FLOAT, "Q_P_NE_VEL_FLTD" },// Q_P_VELXY_FLTD moved to Q_P_NE_VEL_FLTD + { k_param_psc_key, 24977, AP_PARAM_FLOAT, "Q_P_NE_VEL_FF" }, // Q_P_VELXY_FF moved to Q_P_NE_VEL_FF + }; +#else + static const AP_Param::ConversionInfo conversion_info[] = { + { k_param_psc_key, 4035, AP_PARAM_FLOAT, "PSC_D_VEL_P" }, // PSC_VELZ_P moved to PSC_D_VEL_P + { k_param_psc_key, 67, AP_PARAM_FLOAT, "PSC_D_VEL_I" }, // PSC_VELZ_I moved to PSC_D_VEL_I + { k_param_psc_key, 259, AP_PARAM_FLOAT, "PSC_D_VEL_D" }, // PSC_VELZ_D moved to PSC_D_VEL_D + { k_param_psc_key, 195, AP_PARAM_FLOAT, "PSC_D_VEL_FLTE" }, // PSC_VELZ_FLTE moved to PSC_D_VEL_FLTE + { k_param_psc_key, 323, AP_PARAM_FLOAT, "PSC_D_VEL_FLTD" }, // PSC_VELZ_FLTD moved to PSC_D_VEL_FLTD + { k_param_psc_key, 387, AP_PARAM_FLOAT, "PSC_D_VEL_FF" }, // PSC_VELZ_FF moved to PSC_D_VEL_FF + { k_param_psc_key, 260, AP_PARAM_FLOAT, "PSC_D_ACC_FF" }, // PSC_ACCZ_FF moved to PSC_D_ACC_FF + { k_param_psc_key, 580, AP_PARAM_FLOAT, "PSC_D_ACC_FLTT" }, // PSC_ACCZ_FLTT moved to PSC_D_ACC_FLTT + { k_param_psc_key, 644, AP_PARAM_FLOAT, "PSC_D_ACC_FLTE" }, // PSC_ACCZ_FLTE moved to PSC_D_ACC_FLTE + { k_param_psc_key, 708, AP_PARAM_FLOAT, "PSC_D_ACC_FLTD" }, // PSC_ACCZ_FLTD moved to PSC_D_ACC_FLTD + { k_param_psc_key, 772, AP_PARAM_FLOAT, "PSC_D_ACC_SMAX" }, // PSC_ACCZ_SMAX moved to PSC_D_ACC_SMAX + { k_param_psc_key, 836, AP_PARAM_FLOAT, "PSC_D_ACC_PDMX" }, // PSC_ACCZ_PDMX moved to PSC_D_ACC_PDMX + { k_param_psc_key, 900, AP_PARAM_FLOAT, "PSC_D_ACC_D_FF" }, // PSC_ACCZ_D_FF moved to PSC_D_ACC_D_FF + { k_param_psc_key, 1028, AP_PARAM_INT8, "PSC_D_ACC_NEF" }, // PSC_ACCZ_NEF moved to PSC_D_ACC_NEF + { k_param_psc_key, 964, AP_PARAM_INT8, "PSC_D_ACC_NTF" }, // PSC_ACCZ_NTF moved to PSC_D_ACC_NTF + { k_param_psc_key, 4038, AP_PARAM_FLOAT, "PSC_NE_VEL_P" }, // PSC_VELXY_P moved to PSC_NE_VEL_P + { k_param_psc_key, 70, AP_PARAM_FLOAT, "PSC_NE_VEL_I" }, // PSC_VELXY_I moved to PSC_NE_VEL_I + { k_param_psc_key, 262, AP_PARAM_FLOAT, "PSC_NE_VEL_D" }, // PSC_VELXY_D moved to PSC_NE_VEL_D + { k_param_psc_key, 198, AP_PARAM_FLOAT, "PSC_NE_VEL_FLTE" },// PSC_VELXY_FLTE moved to PSC_NE_VEL_FLTE + { k_param_psc_key, 326, AP_PARAM_FLOAT, "PSC_NE_VEL_FLTD" },// PSC_VELXY_FLTD moved to PSC_NE_VEL_FLTD + { k_param_psc_key, 390, AP_PARAM_FLOAT, "PSC_NE_VEL_FF" }, // PSC_VELXY_FF moved to PSC_NE_VEL_FF + }; +#endif + AP_Param::convert_old_parameters(conversion_info, ARRAY_SIZE(conversion_info)); + + // parameters moved and scaled by 0.1 +#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) + static const AP_Param::ConversionInfo conversion_info_01[] = { + { k_param_psc_key, 258321, AP_PARAM_FLOAT, "Q_P_D_ACC_P" }, // Q_P_ACCZ_P moved to Q_P_D_ACC_P + { k_param_psc_key, 4369, AP_PARAM_FLOAT, "Q_P_D_ACC_I" }, // Q_P_ACCZ_I moved to Q_P_D_ACC_I + { k_param_psc_key, 8465, AP_PARAM_FLOAT, "Q_P_D_ACC_D" }, // Q_P_ACCZ_D moved to Q_P_D_ACC_D + }; +#else + static const AP_Param::ConversionInfo conversion_info_01[] = { + { k_param_psc_key, 4036, AP_PARAM_FLOAT, "PSC_D_ACC_P" }, // PSC_ACCZ_P moved to PSC_D_ACC_P + { k_param_psc_key, 68, AP_PARAM_FLOAT, "PSC_D_ACC_I" }, // PSC_ACCZ_I moved to PSC_D_ACC_I + { k_param_psc_key, 132, AP_PARAM_FLOAT, "PSC_D_ACC_D" }, // PSC_ACCZ_D moved to PSC_D_ACC_D + }; +#endif + AP_Param::convert_old_parameters_scaled(conversion_info_01, ARRAY_SIZE(conversion_info_01), 0.1, 0); + + // store PSC_D_ACC_P as flag that parameter conversion was completed + _pid_accel_d_m.kP().save(true); + + // parameters moved and scaled by 0.01 +#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) + static const AP_Param::ConversionInfo conversion_info_001[] = { + { k_param_psc_key, 8401, AP_PARAM_FLOAT, "Q_P_D_VEL_IMAX" }, // Q_P_VELZ_IMAX moved to Q_P_D_VEL_IMAX + { k_param_psc_key, 8593, AP_PARAM_FLOAT, "Q_P_NE_VEL_IMAX" }, // Q_P_VELXY_IMAX moved to Q_P_NE_VEL_IMAX + }; +#else + static const AP_Param::ConversionInfo conversion_info_001[] = { + { k_param_psc_key, 131, AP_PARAM_FLOAT, "PSC_D_VEL_IMAX" }, // PSC_VELZ_IMAX moved to PSC_D_VEL_IMAX + { k_param_psc_key, 134, AP_PARAM_FLOAT, "PSC_NE_VEL_IMAX" }, // PSC_VELXY_IMAX moved to PSC_NE_VEL_IMAX + }; +#endif + AP_Param::convert_old_parameters_scaled(conversion_info_001, ARRAY_SIZE(conversion_info_001), 0.01, 0); + + // parameters moved and scaled by 0.001 + // PSC_ACCZ_IMAX replaced by PSC_D_ACC_IMAX +#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) + static const AP_Param::ConversionInfo psc_d_acc_imax_info = { k_param_psc_key, 20753, AP_PARAM_FLOAT, "Q_P_D_ACC_IMAX" }; +#else + static const AP_Param::ConversionInfo psc_d_acc_imax_info = { k_param_psc_key, 324, AP_PARAM_FLOAT, "PSC_D_ACC_IMAX" }; +#endif + AP_Param::convert_old_parameter(&psc_d_acc_imax_info, 0.001f); +} diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index d4b229a5473f9..129f7eb3d11a1 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -26,8 +26,8 @@ #define POSCONTROL_SPEED_DOWN_MS 1.5f // default descent rate in m/s #define POSCONTROL_SPEED_UP_MS 2.5f // default climb rate in m/s -#define POSCONTROL_ACCEL_U_MSS 2.5f // default vertical acceleration in m/s² -#define POSCONTROL_JERK_U_MSSS 5.0f // default vertical jerk m/s³ +#define POSCONTROL_ACCEL_D_MSS 2.5f // default vertical acceleration in m/s² +#define POSCONTROL_JERK_D_MSSS 5.0f // default vertical jerk m/s³ #define POSCONTROL_THROTTLE_CUTOFF_FREQ_HZ 2.0f // low-pass filter on acceleration error (unit: Hz) @@ -52,7 +52,7 @@ class AC_PosControl // Returns the timestep used in the controller update (seconds). float get_dt_s() const { return _dt_s; } - // Updates internal NEU position and velocity estimates from AHRS. + // Updates internal NED position and velocity estimates from AHRS. // Falls back to vertical-only data if horizontal velocity or position is invalid or vibration forces it. // When high_vibes is true, forces use of vertical fallback for velocity. void update_estimates(bool high_vibes = false); @@ -66,15 +66,17 @@ class AC_PosControl /// 3D position shaper /// - // Sets a new NEU position target in meters and computes a jerk-limited trajectory. + // Sets a new NED position target in meters and computes a jerk-limited trajectory. // Updates internal acceleration commands using a smooth kinematic path constrained - // by configured acceleration and jerk limits. Terrain margin is used to constrain - // horizontal velocity to avoid vertical buffer violation. - void input_pos_NEU_m(const Vector3p& pos_neu_m, float pos_terrain_target_alt_m, float terrain_buffer_m); + // by configured acceleration and jerk limits. + // The path can be offset vertically to follow the terrain by providing the current + // terrain level in the NED frame and the terrain margin. Terrain margin is used to + // constrain horizontal velocity to avoid vertical buffer violation. + void input_pos_NED_m(const Vector3p& pos_ned_m, float pos_terrain_target_d_m, float terrain_margin_m); // Returns a scaling factor for horizontal velocity in m/s to ensure // the vertical controller maintains a safe distance above terrain. - float pos_terrain_U_scaler_m(float pos_terrain_u_m, float pos_terrain_u_buffer_m) const; + float terrain_scaler_D_m(float pos_terrain_d_m, float terrain_margin_m) const; /// /// Lateral position controller @@ -83,64 +85,64 @@ class AC_PosControl // Sets maximum horizontal speed (cm/s) and acceleration (cm/s²) for NE-axis shaping. // Can be called anytime; transitions are handled smoothly. // All arguments should be positive. - // See set_max_speed_accel_NE_m() for full details. - void set_max_speed_accel_NE_cm(float speed_cms, float accel_cmss); + // See NE_set_max_speed_accel_m() for full details. + void NE_set_max_speed_accel_cm(float speed_cms, float accel_cmss); // Sets maximum horizontal speed (m/s) and acceleration (m/s²) for NE-axis shaping. // These values constrain the kinematic trajectory used by the lateral controller. // All arguments should be positive. - void set_max_speed_accel_NE_m(float speed_ms, float accel_mss); + void NE_set_max_speed_accel_m(float speed_ms, float accel_mss); // Sets horizontal correction limits for velocity (cm/s) and acceleration (cm/s²). // Should be called only during initialization to avoid control discontinuities. // All arguments should be positive. - // See set_correction_speed_accel_NE_m() for full details. - void set_correction_speed_accel_NE_cm(float speed_cms, float accel_cmss); + // See NE_set_correction_speed_accel_m() for full details. + void NE_set_correction_speed_accel_cm(float speed_cms, float accel_cmss); // Sets horizontal correction limits for velocity (m/s) and acceleration (m/s²). // These values constrain the PID correction path, not the desired trajectory. // All arguments should be positive. - void set_correction_speed_accel_NE_m(float speed_ms, float accel_mss); + void NE_set_correction_speed_accel_m(float speed_ms, float accel_mss); // Returns maximum horizontal speed in cm/s. - // See get_max_speed_NE_ms() for full details. - float get_max_speed_NE_cms() const { return get_max_speed_NE_ms() * 100.0; } + // See NE_get_max_speed_ms() for full details. + float NE_get_max_speed_cms() const { return NE_get_max_speed_ms() * 100.0; } // Returns maximum horizontal speed in m/s used for shaping the trajectory. - float get_max_speed_NE_ms() const { return _vel_max_ne_ms; } + float NE_get_max_speed_ms() const { return _vel_max_ne_ms; } // Returns maximum horizontal acceleration in m/s² used for trajectory shaping. - float get_max_accel_NE_mss() const { return _accel_max_ne_mss; } + float NE_get_max_accel_mss() const { return _accel_max_ne_mss; } // Sets maximum allowed horizontal position error in meters. // Used to constrain the output of the horizontal position P controller. - void set_pos_error_max_NE_m(float error_max_m) { _p_pos_ne_m.set_error_max(error_max_m); } + void NE_set_pos_error_max_m(float error_max_m) { _p_pos_ne_m.set_error_max(error_max_m); } // Returns maximum allowed horizontal position error in meters. - float get_pos_error_max_NE_m() const { return _p_pos_ne_m.get_error_max(); } + float NE_get_pos_error_max_m() const { return _p_pos_ne_m.get_error_max(); } // Initializes NE controller to a stationary stopping point with zero velocity and acceleration. // Use when the expected trajectory begins at rest but the starting position is unspecified. - // The starting position can be retrieved with get_pos_target_NEU_m(). - void init_NE_controller_stopping_point(); + // The starting position can be retrieved with get_pos_target_NED_m(). + void NE_init_controller_stopping_point(); // Smoothly decays NE acceleration over time to zero while maintaining current velocity and position. // Reduces output acceleration by ~95% over 0.5 seconds to avoid abrupt transitions. - void relax_velocity_controller_NE(); + void NE_relax_velocity_controller(); // Softens NE controller for landing by reducing position error and suppressing I-term windup. // Used to make descent behavior more stable near ground contact. - void soften_for_landing_NE(); + void NE_soften_for_landing(); // Fully initializes the NE controller with current position, velocity, acceleration, and attitude. // Intended for normal startup when the full state is known. // Private function shared by other NE initializers. - void init_NE_controller(); + void NE_init_controller(); // Sets the desired NE-plane acceleration in m/s² using jerk-limited shaping. // Smoothly transitions to the specified acceleration from current kinematic state. - // Constraints: max acceleration and jerk set via set_max_speed_accel_NE_m(). - void input_accel_NE_m(const Vector3f& accel_neu_msss); + // Constraints: max acceleration and jerk set via NE_set_max_speed_accel_m(). + void input_accel_NE_m(const Vector2f& accel_ne_msss); // Sets desired NE-plane velocity and acceleration (cm/s, cm/s²) using jerk-limited shaping. // See input_vel_accel_NE_m() for full details. @@ -161,26 +163,26 @@ class AC_PosControl void input_pos_vel_accel_NE_m(Vector2p& pos_ne_m, Vector2f& vel_ne_ms, const Vector2f& accel_ne_mss, bool limit_output = true); // Returns true if the NE position controller has run in the last 5 control loop cycles. - bool is_active_NE() const; + bool NE_is_active() const; // Disables NE position correction by setting the target position to the current position. // Useful to freeze positional control without disrupting velocity control. - void stop_pos_NE_stabilisation(); + void NE_stop_pos_stabilisation(); // Disables NE position and velocity correction by setting target values to current state. // Useful to prevent further corrections and freeze motion stabilization in NE axes. - void stop_vel_NE_stabilisation(); + void NE_stop_vel_stabilisation(); // Applies a scalar multiplier to the NE control loop. // Set to 0 to disable lateral control; 1 for full authority. - void set_NE_control_scale_factor(float ne_control_scale_factor) { + void NE_set_control_scale_factor(float ne_control_scale_factor) { _ne_control_scale_factor = ne_control_scale_factor; } // Runs the NE-axis position controller, computing output acceleration from position and velocity errors. // Uses P and PID controllers to generate corrections which are added to feedforward velocity/acceleration. // Requires all desired targets to be pre-set using the input_* or set_* methods. - void update_NE_controller(); + void NE_update_controller(); /// /// Vertical position controller @@ -188,34 +190,34 @@ class AC_PosControl // Sets maximum climb/descent rate (cm/s) and vertical acceleration (cm/s²) for the U-axis. // Descent rate may be positive or negative and is always interpreted as a descent. - // See set_max_speed_accel_U_m() for full details. + // See D_set_max_speed_accel_m() for full details. // All values must be positive. - void set_max_speed_accel_U_cm(float decent_speed_max_cms, float climb_speed_max_cms, float accel_max_u_cmss); + void D_set_max_speed_accel_cm(float decent_speed_max_cms, float climb_speed_max_cms, float accel_max_u_cmss); // Sets maximum climb/descent rate (m/s) and vertical acceleration (m/s²) for the U-axis. // These values are used for jerk-limited kinematic shaping of the vertical trajectory. // All values must be positive. - void set_max_speed_accel_U_m(float decent_speed_max_ms, float climb_speed_max_ms, float accel_max_u_mss); + void D_set_max_speed_accel_m(float decent_speed_max_ms, float climb_speed_max_ms, float accel_max_d_mss); // Sets vertical correction velocity and acceleration limits (cm/s, cm/s²). // Should only be called during initialization to avoid discontinuities. - // See set_correction_speed_accel_U_m() for full details. + // See D_set_correction_speed_accel_m() for full details. // All values must be positive. - void set_correction_speed_accel_U_cm(float decent_speed_max_cms, float climb_speed_max_cms, float accel_max_u_cmss); + void D_set_correction_speed_accel_cm(float decent_speed_max_cms, float climb_speed_max_cms, float accel_max_u_cmss); // Sets vertical correction velocity and acceleration limits (m/s, m/s²). // These values constrain the correction output of the PID controller. // All values must be positive. - void set_correction_speed_accel_U_m(float decent_speed_max_ms, float climb_speed_max_ms, float accel_max_u_mss); + void D_set_correction_speed_accel_m(float decent_speed_max_ms, float climb_speed_max_ms, float accel_max_d_mss); // Returns maximum vertical acceleration in m/s² used for shaping the climb/descent trajectory. - float get_max_accel_U_mss() const { return _accel_max_u_mss; } + float D_get_max_accel_mss() const { return _accel_max_d_mss; } // Returns maximum allowed positive (upward) position error in meters. - float get_pos_error_up_m() const { return _p_pos_u_m.get_error_max(); } + float get_pos_error_up_m() const { return _p_pos_d_m.get_error_min(); } // Returns maximum allowed negative (downward) position error in meters. - float get_pos_error_down_m() const { return _p_pos_u_m.get_error_min(); } + float get_pos_error_down_m() const { return _p_pos_d_m.get_error_max(); } // Returns maximum climb rate in cm/s. // See get_max_speed_up_ms() for full details. @@ -229,70 +231,66 @@ class AC_PosControl // Initializes U-axis controller to current position, velocity, and acceleration, disallowing descent. // Used for takeoff or hold scenarios where downward motion is prohibited. - void init_U_controller_no_descent(); + void D_init_controller_no_descent(); // Initializes U-axis controller to a stationary stopping point with zero velocity and acceleration. // Used when the trajectory starts at rest but the initial altitude is unspecified. - // The resulting position target can be retrieved with get_pos_target_NEU_m(). - void init_U_controller_stopping_point(); + // The resulting position target can be retrieved with get_pos_target_NED_m(). + void D_init_controller_stopping_point(); // Smoothly decays U-axis acceleration to zero over time while maintaining current vertical velocity. // Reduces requested acceleration by ~95% every 0.5 seconds to avoid abrupt transitions. // `throttle_setting` is used to determine whether to preserve positive acceleration in low-thrust cases. - void relax_U_controller(float throttle_setting); + void D_relax_controller(float throttle_setting); // Fully initializes the U-axis controller with current position, velocity, acceleration, and attitude. // Used during standard controller activation when full state is known. // Private function shared by other vertical initializers. - void init_U_controller(); + void D_init_controller(); // Sets the desired vertical acceleration in m/s² using jerk-limited shaping. // Smoothly transitions to the target acceleration from current kinematic state. - // Constraints: max acceleration and jerk set via set_max_speed_accel_U_m(). - void input_accel_U_m(float accel_u_mss); + // Constraints: max acceleration and jerk set via D_set_max_speed_accel_m(). + void input_accel_D_m(float accel_d_mss); // Sets desired vertical velocity and acceleration (m/s, m/s²) using jerk-limited shaping. // Calculates required acceleration using current vertical kinematics. // If `limit_output` is true, limits apply to the combined (desired + correction) command. - void input_vel_accel_U_m(float &vel_u_ms, float accel_u_mss, bool limit_output = true); + void input_vel_accel_D_m(float &vel_d_ms, float accel_d_mss, bool limit_output = true); // Generates a vertical trajectory using the given climb rate in cm/s and jerk-limited shaping. // Adjusts the internal target altitude based on integrated climb rate. - // See set_pos_target_U_from_climb_rate_ms() for full details. - void set_pos_target_U_from_climb_rate_cms(float vel_u_cms); + // See D_set_pos_target_from_climb_rate_ms() for full details. + void D_set_pos_target_from_climb_rate_cms(float climb_rate_cms); // Generates a vertical trajectory using the given climb rate in m/s and jerk-limited shaping. // Target altitude is updated over time by integrating the climb rate. - void set_pos_target_U_from_climb_rate_ms(float vel_u_ms); - - // Descends at a given rate (m/s) using jerk-limited shaping for landing. - // Used during final descent phase to ensure smooth touchdown. - void land_at_climb_rate_ms(float vel_u_ms, bool ignore_descent_limit); + void D_set_pos_target_from_climb_rate_ms(float climb_rate_ms, bool ignore_descent_limit = false); // Sets vertical position, velocity, and acceleration in cm using jerk-limited shaping. - // See input_pos_vel_accel_U_m() for full details. + // See input_pos_vel_accel_D_m() for full details. void input_pos_vel_accel_U_cm(float &pos_u_cm, float &vel_u_cms, float accel_u_cmss, bool limit_output = true); // Sets vertical position, velocity, and acceleration in meters using jerk-limited shaping. // Calculates required acceleration using current state and constraints. // If `limit_output` is true, limits are applied to combined (desired + correction) command. - void input_pos_vel_accel_U_m(float &pos_u_m, float &vel_u_ms, float accel_u_mss, bool limit_output = true); + void input_pos_vel_accel_D_m(float &pos_d_m, float &vel_d_ms, float accel_d_mss, bool limit_output = true); // Sets target altitude in cm using jerk-limited shaping to gradually move to the new position. - // See set_alt_target_with_slew_m() for full details. + // See D_set_alt_target_with_slew_m() for full details. void set_alt_target_with_slew_cm(float pos_u_cm); // Sets target altitude in meters using jerk-limited shaping. - void set_alt_target_with_slew_m(float pos_u_m); + void D_set_alt_target_with_slew_m(float pos_u_m); // Returns true if the U-axis controller has run in the last 5 control loop cycles. - bool is_active_U() const; + bool D_is_active() const; // Runs the vertical (U-axis) position controller. // Computes output acceleration based on position and velocity errors using PID correction. // Feedforward velocity and acceleration are combined with corrections to produce a smooth vertical command. // Desired position, velocity, and acceleration must be set before calling. - void update_U_controller(); + void D_update_controller(); @@ -300,9 +298,9 @@ class AC_PosControl /// Accessors /// - // Sets externally computed NEU position, velocity, and acceleration in meters, m/s, and m/s². + // Sets externally computed NED position, velocity, and acceleration in meters, m/s, and m/s². // Use when path planning or shaping is done outside this controller. - void set_pos_vel_accel_NEU_m(const Vector3p& pos_neu_m, const Vector3f& vel_neu_ms, const Vector3f& accel_neu_mss); + void set_pos_vel_accel_NED_m(const Vector3p& pos_ned_m, const Vector3f& vel_ned_ms, const Vector3f& accel_ned_mss); // Sets externally computed NE position, velocity, and acceleration in meters, m/s, and m/s². // Use when path planning or shaping is done outside this controller. @@ -311,141 +309,145 @@ class AC_PosControl /// Position - // Returns the estimated position in NEU frame, in meters relative to EKF origin. - const Vector3p& get_pos_estimate_NEU_m() const { return _pos_estimate_neu_m; } + // Returns the estimated position in NED frame, in meters relative to EKF origin. + const Vector3p& get_pos_estimate_NED_m() const { return _pos_estimate_ned_m; } // Returns estimated altitude above EKF origin in meters. - float get_pos_estimate_U_m() const { return _pos_estimate_neu_m.z; } + float get_pos_estimate_U_m() const { return -_pos_estimate_ned_m.z; } - // Returns the target position in NEU frame, in meters relative to EKF origin. - const Vector3p& get_pos_target_NEU_m() const { return _pos_target_neu_m; } + // Returns the target position in NED frame, in meters relative to EKF origin. + const Vector3p& get_pos_target_NED_m() const { return _pos_target_ned_m; } // Sets the desired NE position in meters relative to EKF origin. - void set_pos_desired_NE_m(const Vector2p& pos_desired_ne_m) { _pos_desired_neu_m.xy() = pos_desired_ne_m; } + void set_pos_desired_NE_m(const Vector2p& pos_desired_ne_m) { _pos_desired_ned_m.xy() = pos_desired_ne_m; } - // Returns the desired position in NEU frame, in meters relative to EKF origin. - const Vector3p& get_pos_desired_NEU_m() const { return _pos_desired_neu_m; } + // Returns the desired position in NED frame, in meters relative to EKF origin. + const Vector3p& get_pos_desired_NED_m() const { return _pos_desired_ned_m; } // Returns target altitude above EKF origin in centimeters. // See get_pos_target_U_m() for full details. float get_pos_target_U_cm() const { return get_pos_target_U_m() * 100.0; } // Returns target altitude above EKF origin in meters. - float get_pos_target_U_m() const { return _pos_target_neu_m.z; } + float get_pos_target_U_m() const { return -_pos_target_ned_m.z; } // Sets desired altitude above EKF origin in centimeters. // See set_pos_desired_U_m() for full details. void set_pos_desired_U_cm(float pos_desired_u_cm) { set_pos_desired_U_m(pos_desired_u_cm * 0.01); } // Sets desired altitude above EKF origin in meters. - void set_pos_desired_U_m(float pos_desired_u_m) { _pos_desired_neu_m.z = pos_desired_u_m; } + void set_pos_desired_U_m(float pos_desired_u_m) { _pos_desired_ned_m.z = -pos_desired_u_m; } // Returns desired altitude above EKF origin in centimeters. // See get_pos_desired_U_m() for full details. float get_pos_desired_U_cm() const { return get_pos_desired_U_m() * 100.0; } // Returns desired altitude above EKF origin in meters. - float get_pos_desired_U_m() const { return _pos_desired_neu_m.z; } + float get_pos_desired_U_m() const { return -_pos_desired_ned_m.z; } /// Stopping Point // Computes NE stopping point in meters based on current position, velocity, and acceleration. - void get_stopping_point_NE_m(Vector2p &stopping_point_neu_m) const; + void get_stopping_point_NE_m(Vector2p &stopping_point_ne_m) const; - // Computes vertical stopping point in meters based on current velocity and acceleration. - void get_stopping_point_U_m(postype_t &stopping_point_u_m) const; + // Computes vertical stopping point relative to EKF origin in meters, Down-positive. based on current velocity and acceleration. + void get_stopping_point_D_m(postype_t &stopping_point_d_m) const; /// Position Error - // Returns NEU position error vector in meters between current and target positions. - const Vector3f get_pos_error_NEU_m() const { return Vector3f{_p_pos_ne_m.get_error().x, _p_pos_ne_m.get_error().y, _p_pos_u_m.get_error()}; } + // Returns NED position error vector in meters between current and target positions. + const Vector3f get_pos_error_NED_m() const { return Vector3f{_p_pos_ne_m.get_error().x, _p_pos_ne_m.get_error().y, _p_pos_d_m.get_error()}; } // Returns total NE-plane position error magnitude in meters. float get_pos_error_NE_m() const { return _p_pos_ne_m.get_error().length(); } // Returns vertical position error (altitude) in centimeters. - // See get_pos_error_U_m() for full details. - float get_pos_error_U_cm() const { return get_pos_error_U_m() * 100.0; } + // See get_pos_error_D_m() for full details. + float get_pos_error_U_cm() const { return -get_pos_error_D_m() * 100.0; } // Returns vertical position error (altitude) in meters. - float get_pos_error_U_m() const { return _p_pos_u_m.get_error(); } + float get_pos_error_D_m() const { return _p_pos_d_m.get_error(); } /// Velocity - // Returns current velocity estimate in NEU frame in m/s. - const Vector3f& get_vel_estimate_NEU_ms() const { return _vel_estimate_neu_ms; } + // Returns current velocity estimate in NED frame in m/s. + const Vector3f& get_vel_estimate_NED_ms() const { return _vel_estimate_ned_ms; } // Returns current velocity estimate (Up) in m/s. - float get_vel_estimate_U_ms() const { return _vel_estimate_neu_ms.z; } + float get_vel_estimate_U_ms() const { return -_vel_estimate_ned_ms.z; } - // Sets desired velocity in NEU frame in cm/s. - // See set_vel_desired_NEU_ms() for full details. - void set_vel_desired_NEU_cms(const Vector3f &vel_desired_neu_cms) { set_vel_desired_NEU_ms(vel_desired_neu_cms * 0.01); } + // Sets desired velocity in NED frame in cm/s. + // See set_vel_desired_NED_ms() for full details. + void set_vel_desired_NEU_cms(const Vector3f &vel_desired_neu_cms) { set_vel_desired_NED_ms(Vector3f{vel_desired_neu_cms.x, vel_desired_neu_cms.y, -vel_desired_neu_cms.z} * 0.01); } - // Sets desired velocity in NEU frame in m/s. - void set_vel_desired_NEU_ms(const Vector3f &vel_desired_neu_ms) { _vel_desired_neu_ms = vel_desired_neu_ms; } + // Sets desired velocity in NED frame in m/s. + void set_vel_desired_NED_ms(const Vector3f &vel_desired_ned_ms) { _vel_desired_ned_ms = vel_desired_ned_ms; } // Sets desired horizontal (NE) velocity in m/s. - void set_vel_desired_NE_ms(const Vector2f &vel_desired_ne_ms) { _vel_desired_neu_ms.xy() = vel_desired_ne_ms; } + void set_vel_desired_NE_ms(const Vector2f &vel_desired_ne_ms) { _vel_desired_ned_ms.xy() = vel_desired_ne_ms; } // Returns desired velocity in NEU frame in cm/s. - // See get_vel_desired_NEU_ms() for full details. - const Vector3f get_vel_desired_NEU_cms() const { return get_vel_desired_NEU_ms() * 100.0; } + // See get_vel_desired_NED_ms() for full details. + const Vector3f get_vel_desired_NEU_cms() const { return Vector3f{_vel_desired_ned_ms.x, _vel_desired_ned_ms.y, -_vel_desired_ned_ms.z} * 100.0; } - // Returns desired velocity in NEU frame in m/s. - const Vector3f& get_vel_desired_NEU_ms() const { return _vel_desired_neu_ms; } + // Returns desired velocity in NED frame in m/s. + const Vector3f& get_vel_desired_NED_ms() const { return _vel_desired_ned_ms; } // Returns desired velocity (Up) in m/s. - float get_vel_desired_U_ms() const { return _vel_desired_neu_ms.z; } + float get_vel_desired_U_ms() const { return -_vel_desired_ned_ms.z; } // Returns velocity target in NEU frame in cm/s. - // See get_vel_target_NEU_ms() for full details. - const Vector3f get_vel_target_NEU_cms() const { return get_vel_target_NEU_ms() * 100.0; } + // See get_vel_target_NED_ms() for full details. + const Vector3f get_vel_target_NEU_cms() const { return Vector3f{_vel_target_ned_ms.x, _vel_target_ned_ms.y, -_vel_target_ned_ms.z} * 100.0; } - // Returns velocity target in NEU frame in m/s. - const Vector3f& get_vel_target_NEU_ms() const { return _vel_target_neu_ms; } + // Returns velocity target in NED frame in m/s. + const Vector3f& get_vel_target_NED_ms() const { return _vel_target_ned_ms; } // Sets desired vertical velocity (Up) in m/s. - void set_vel_desired_U_ms(float vel_desired_u_ms) { _vel_desired_neu_ms.z = vel_desired_u_ms; } + void set_vel_desired_D_ms(float vel_desired_d_ms) { _vel_desired_ned_ms.z = vel_desired_d_ms; } // Returns vertical velocity target (Up) in cm/s. // See get_vel_target_U_ms() for full details. float get_vel_target_U_cms() const { return get_vel_target_U_ms() * 100.0; } // Returns vertical velocity target (Up) in m/s. - float get_vel_target_U_ms() const { return _vel_target_neu_ms.z; } + float get_vel_target_U_ms() const { return -_vel_target_ned_ms.z; } /// Acceleration // Sets desired horizontal acceleration in m/s². - void set_accel_desired_NE_mss(const Vector2f &accel_desired_neu_mss) { _accel_desired_neu_mss.xy() = accel_desired_neu_mss; } + void set_accel_desired_NE_mss(const Vector2f &accel_desired_ne_mss) { _accel_desired_ned_mss.xy() = accel_desired_ne_mss; } - // Returns target NEU acceleration in m/s². - const Vector3f& get_accel_target_NEU_mss() const { return _accel_target_neu_mss; } + // Returns target NED acceleration in m/s². + const Vector3f& get_accel_target_NED_mss() const { return _accel_target_ned_mss; } /// Terrain - // Sets the terrain target altitude above EKF origin in centimeters. - // See set_pos_terrain_target_U_m() for full details. - void set_pos_terrain_target_U_cm(float pos_terrain_target_u_cm) { set_pos_terrain_target_U_m(pos_terrain_target_u_cm * 0.01); } + // Sets both the terrain altitude and terrain target to the same value + // (altitude above EKF origin in centimeters, Up-positive). + // See set_pos_terrain_target_D_m() for full description. + void set_pos_terrain_target_U_cm(float pos_terrain_target_u_cm) { set_pos_terrain_target_D_m(-pos_terrain_target_u_cm * 0.01); } - // Sets the terrain target altitude above EKF origin in meters. - void set_pos_terrain_target_U_m(float pos_terrain_target_u_m) { _pos_terrain_target_u_m = pos_terrain_target_u_m; } + // Sets both the terrain altitude and terrain target to the same value + // (relative to EKF origin in meters, Down-positive). + void set_pos_terrain_target_D_m(float pos_terrain_target_d_m) { _pos_terrain_target_d_m = pos_terrain_target_d_m; } - // Initializes terrain altitude and terrain target to the same value (in cm). - // See init_pos_terrain_U_m() for full details. + // Initializes both the terrain altitude and terrain target to the same value + // (altitude above EKF origin in centimeters, Up-positive). + // See init_pos_terrain_D_m() for full description. void init_pos_terrain_U_cm(float pos_terrain_u_cm); - // Initializes terrain altitude and terrain target to the same value (in meters). - void init_pos_terrain_U_m(float pos_terrain_u_m); + // Initializes both the terrain altitude and terrain target to the same value + // (relative to EKF origin in meters, Down-positive). + void init_pos_terrain_D_m(float pos_terrain_d_m); - // Returns current terrain altitude in meters above EKF origin. - float get_pos_terrain_U_m() const { return _pos_terrain_u_m; } + // Returns the current terrain altitude (Down-positive, relative to EKF origin, in meters). + float get_pos_terrain_D_m() const { return _pos_terrain_d_m; } /// Offset @@ -473,29 +475,29 @@ class AC_PosControl // Sets NE offset targets in meters, m/s, and m/s². void set_posvelaccel_offset_target_NE_m(const Vector2p& pos_offset_target_ne_m, const Vector2f& vel_offset_target_ne_ms, const Vector2f& accel_offset_target_ne_mss); - // Sets vertical offset targets (m, m/s, m/s²) from EKF origin. - void set_posvelaccel_offset_target_U_m(float pos_offset_target_u_m, float vel_offset_target_u_ms, float accel_offset_target_u_mss); + // Sets vertical offset targets (m, m/s, m/s²) relative to EKF origin in meters, Down-positive. + void set_posvelaccel_offset_target_D_m(float pos_offset_target_d_m, float vel_offset_target_d_ms, float accel_offset_target_d_mss); - // Returns current NEU position offset in meters. - const Vector3p& get_pos_offset_NEU_m() const { return _pos_offset_neu_m; } + // Returns current NED position offset in meters. + const Vector3p& get_pos_offset_NED_m() const { return _pos_offset_ned_m; } - // Returns current NEU velocity offset in m/s. - const Vector3f& get_vel_offset_NEU_ms() const { return _vel_offset_neu_ms; } + // Returns current NED velocity offset in m/s. + const Vector3f& get_vel_offset_NED_ms() const { return _vel_offset_ned_ms; } - // Returns current NEU acceleration offset in m/s². - const Vector3f& get_accel_offset_NEU_mss() const { return _accel_offset_neu_mss; } + // Returns current NED acceleration offset in m/s². + const Vector3f& get_accel_offset_NED_mss() const { return _accel_offset_ned_mss; } - // Sets vertical position offset in meters above EKF origin. - void set_pos_offset_U_m(float pos_offset_u_m) { _pos_offset_neu_m.z = pos_offset_u_m; } + // Sets vertical position offset in meters relative to EKF origin in meters, Down-positive. + void set_pos_offset_D_m(float pos_offset_d_m) { _pos_offset_ned_m.z = pos_offset_d_m; } - // Returns vertical position offset in meters above EKF origin. - float get_pos_offset_U_m() const { return _pos_offset_neu_m.z; } + // Returns vertical position offset in meters relative to EKF origin in meters, Down-positive. + float get_pos_offset_U_m() const { return -_pos_offset_ned_m.z; } // Returns vertical velocity offset in m/s. - float get_vel_offset_U_ms() const { return _vel_offset_neu_ms.z; } + float get_vel_offset_D_ms() const { return _vel_offset_ned_ms.z; } // Returns vertical acceleration offset in m/s². - float get_accel_offset_U_mss() const { return _accel_offset_neu_mss.z; } + float get_accel_offset_D_mss() const { return _accel_offset_ned_mss.z; } /// Outputs @@ -544,26 +546,26 @@ class AC_PosControl /// Other // Returns reference to the NE position P controller. - AC_P_2D& get_pos_NE_p() { return _p_pos_ne_m; } + AC_P_2D& NE_get_pos_p() { return _p_pos_ne_m; } - // Returns reference to the U (vertical) position P controller. - AC_P_1D& get_pos_U_p() { return _p_pos_u_m; } + // Returns reference to the D (vertical) position P controller. + AC_P_1D& D_get_pos_p() { return _p_pos_d_m; } // Returns reference to the NE velocity PID controller. - AC_PID_2D& get_vel_NE_pid() { return _pid_vel_ne_cm; } + AC_PID_2D& NE_get_vel_pid() { return _pid_vel_ne_m; } - // Returns reference to the U (vertical) velocity PID controller. - AC_PID_Basic& get_vel_U_pid() { return _pid_vel_u_cm; } + // Returns reference to the D (vertical) velocity PID controller. + AC_PID_Basic& D_get_vel_pid() { return _pid_vel_d_m; } - // Returns reference to the U acceleration PID controller. - AC_PID& get_accel_U_pid() { return _pid_accel_u_cm_to_kt; } + // Returns reference to the D acceleration PID controller. + AC_PID& D_get_accel_pid() { return _pid_accel_d_m; } // Marks that NE acceleration has been externally limited. // Prevents I-term windup by storing the current target direction. - void set_externally_limited_NE() { _limit_vector_neu.x = _accel_target_neu_mss.x; _limit_vector_neu.y = _accel_target_neu_mss.y; } + void NE_set_externally_limited() { _limit_vector_ned.x = _accel_target_ned_mss.x; _limit_vector_ned.y = _accel_target_ned_mss.y; } - // Converts lean angles (rad) to NEU acceleration in m/s². - Vector3f lean_angles_rad_to_accel_NEU_mss(const Vector3f& att_target_euler_rad) const; + // Converts lean angles (rad) to NED acceleration in m/s². + Vector3f lean_angles_rad_to_accel_NED_mss(const Vector3f& att_target_euler_rad) const; // Writes position controller diagnostic logs (PSCN, PSCE, etc). void write_log(); @@ -587,19 +589,23 @@ class AC_PosControl // Returns confidence (0–1) in vertical control authority based on output usage. // Used to assess throttle margin and PID effectiveness. - float get_vel_U_control_ratio() const { return constrain_float(_vel_u_control_ratio, 0.0f, 1.0f); } + float get_vel_D_control_ratio() const { return constrain_float(_vel_d_control_ratio, 0.0f, 1.0f); } // Returns lateral distance to closest point on active trajectory in meters. // Used to assess horizontal deviation from path. float crosstrack_error_m() const; - // Resets NEU position controller state to prevent transients when exiting standby. + // Resets NED position controller state to prevent transients when exiting standby. // Zeros I-terms and aligns targets to current position. - void standby_NEU_reset(); + void NED_standby_reset(); + + // Returns measured vertical (Down) acceleration in m/s² (Earth frame, gravity-compensated). + // Positive = downward acceleration. + float get_estimated_accel_D_mss() const { return _ahrs.get_accel_ef().z + GRAVITY_MSS; } // Returns measured vertical (Up) acceleration in m/s² (Earth frame, gravity-compensated). // Positive = upward acceleration. - float get_estimate_accel_U_mss() const { return -(_ahrs.get_accel_ef().z + GRAVITY_MSS); } + float get_estimated_accel_U_mss() const { return -get_estimated_accel_D_mss(); } // Returns true if the requested forward pitch is limited by the configured tilt constraint. bool get_fwd_pitch_is_limited() const; @@ -610,8 +616,6 @@ class AC_PosControl // Sets artificial NE velocity disturbance in m/s. void set_disturb_vel_NE_ms(const Vector2f& disturb_vel_ms) { _disturb_vel_ne_ms = disturb_vel_ms; } - static const struct AP_Param::GroupInfo var_info[]; - // Logs position controller state along the North axis to PSCN.. // Logs desired, target, and actual position [m], velocity [m/s], and acceleration [m/s²]. static void Write_PSCN(float pos_desired_m, float pos_target_m, float pos_m, float vel_desired_ms, float vel_target_ms, float vel_ms, float accel_desired_mss, float accel_target_mss, float accel_mss); @@ -640,10 +644,14 @@ class AC_PosControl // Logs target and actual offset for position [m], velocity [m/s], and acceleration [m/s²]. static void Write_PSOT(float pos_target_offset_m, float pos_offset_m, float vel_target_offset_ms, float vel_offset_ms, float accel_target_offset_mss, float accel_offset_mss); + // perform any required parameter conversions + void convert_parameters(); // Returns pointer to the global AC_PosControl singleton. static AC_PosControl *get_singleton(void) { return _singleton; } + static const struct AP_Param::GroupInfo var_info[]; + protected: // Calculates vertical throttle using vibration-resistant feedforward estimation. @@ -670,35 +678,35 @@ class AC_PosControl // Initializes terrain position, velocity, and acceleration to match the terrain target. void init_terrain(); - // Updates terrain estimate (_pos_terrain_u_m) toward target using filter time constants. + // Updates terrain estimate (_pos_terrain_d_m) toward target using filter time constants. void update_terrain(); /// Offsets // Initializes NE position/velocity/acceleration offsets to match their respective targets. - void init_offsets_NE(); + void NE_init_offsets(); - // Initializes vertical (U) offsets to match their respective targets. - void init_offsets_U(); + // Initializes vertical (D) offsets to match their respective targets. + void D_init_offsets(); // Updates NE offsets by gradually moving them toward their targets. - void update_offsets_NE(); + void NE_update_offsets(); - // Updates vertical (U) offsets by gradually moving them toward their targets. - void update_offsets_U(); + // Updates vertical (D) offsets by gradually moving them toward their targets. + void D_update_offsets(); // Initializes tracking of NE EKF position resets. - void init_ekf_NE_reset(); + void NE_init_ekf_reset(); // Handles NE position reset detection and response (e.g., clearing accumulated errors). - void handle_ekf_NE_reset(); + void NE_handle_ekf_reset(); - // Initializes tracking of vertical (U) EKF resets. - void init_ekf_U_reset(); + // Initializes tracking of vertical (D) EKF resets. + void D_init_ekf_reset(); - // Handles U EKF reset detection and response. - void handle_ekf_U_reset(); + // Handles the vertical (D) EKF reset detection and response. + void D_handle_ekf_reset(); // references to inertial nav and ahrs libraries AP_AHRS_View& _ahrs; @@ -708,66 +716,66 @@ class AC_PosControl // parameters AP_Float _lean_angle_max_deg; // Maximum autopilot commanded angle (in degrees). Set to zero for Angle Max AP_Float _shaping_jerk_ne_msss; // Jerk limit of the ne kinematic path generation in m/s³ used to determine how quickly the aircraft varies the acceleration target - AP_Float _shaping_jerk_u_msss; // Jerk limit of the u kinematic path generation in m/s³ used to determine how quickly the aircraft varies the acceleration target + AP_Float _shaping_jerk_d_msss; // Jerk limit of the u kinematic path generation in m/s³ used to determine how quickly the aircraft varies the acceleration target AC_P_2D _p_pos_ne_m; // XY axis position controller to convert target distance (m) to target velocity (m/s) - AC_P_1D _p_pos_u_m; // Z axis position controller to convert target altitude (m) to target climb rate (m/s) - AC_PID_2D _pid_vel_ne_cm; // XY axis velocity controller to convert target velocity (cm/s) to target acceleration (cm/s²) - AC_PID_Basic _pid_vel_u_cm; // Z axis velocity controller to convert target climb rate (cm/s) to target acceleration (cm/s²) - AC_PID _pid_accel_u_cm_to_kt; // Z axis acceleration controller to convert target acceleration (cm/s²) to throttle output (0 to 1000) + AC_P_1D _p_pos_d_m; // Z axis position controller to convert target altitude (m) to target climb rate (m/s) + AC_PID_2D _pid_vel_ne_m; // XY axis velocity controller to convert target velocity (m/s) to target acceleration (m/s²) + AC_PID_Basic _pid_vel_d_m; // Z axis velocity controller to convert target climb rate (m/s) to target acceleration (m/s²) + AC_PID _pid_accel_d_m; // Z axis acceleration controller to convert target acceleration (in units of gravity) to normalised throttle output // internal variables - float _dt_s; // time difference (in seconds) since the last loop time - uint32_t _last_update_ne_ticks; // ticks of last last update_NE_controller call - uint32_t _last_update_u_ticks; // ticks of last update_z_controller call - float _vel_max_ne_ms; // max horizontal speed in m/s used for kinematic shaping - float _vel_max_up_ms; // max climb rate in m/s used for kinematic shaping - float _vel_max_down_ms; // max descent rate in m/s used for kinematic shaping - float _accel_max_ne_mss; // max horizontal acceleration in m/s² used for kinematic shaping - float _accel_max_u_mss; // max vertical acceleration in m/s² used for kinematic shaping - float _jerk_max_ne_msss; // Jerk limit of the ne kinematic path generation in m/s³ used to determine how quickly the aircraft varies the acceleration target - float _jerk_max_u_msss; // Jerk limit of the z kinematic path generation in m/s³ used to determine how quickly the aircraft varies the acceleration target - float _vel_u_control_ratio = 2.0f; // confidence that we have control in the vertical axis - Vector2f _disturb_pos_ne_m; // position disturbance generated by system ID mode - Vector2f _disturb_vel_ne_ms; // velocity disturbance generated by system ID mode + float _dt_s; // time difference (in seconds) since the last loop time + uint32_t _last_update_ne_ticks; // ticks of last NE_update_controller call + uint32_t _last_update_d_ticks; // ticks of last update_z_controller call + float _vel_max_ne_ms; // max horizontal speed in m/s used for kinematic shaping + float _vel_max_up_ms; // max climb rate in m/s used for kinematic shaping + float _vel_max_down_ms; // max descent rate in m/s used for kinematic shaping + float _accel_max_ne_mss; // max horizontal acceleration in m/s² used for kinematic shaping + float _accel_max_d_mss; // max vertical acceleration in m/s² used for kinematic shaping + float _jerk_max_ne_msss; // Jerk limit of the ne kinematic path generation in m/s³ used to determine how quickly the aircraft varies the acceleration target + float _jerk_max_d_msss; // Jerk limit of the z kinematic path generation in m/s³ used to determine how quickly the aircraft varies the acceleration target + float _vel_d_control_ratio = 2.0f;// confidence that we have control in the vertical axis + Vector2f _disturb_pos_ne_m; // position disturbance generated by system ID mode + Vector2f _disturb_vel_ne_ms; // velocity disturbance generated by system ID mode float _ne_control_scale_factor = 1.0; // single loop scale factor for XY control // output from controller - float _roll_target_rad; // desired roll angle in radians calculated by position controller - float _pitch_target_rad; // desired roll pitch in radians calculated by position controller - float _yaw_target_rad; // desired yaw in radians calculated by position controller - float _yaw_rate_target_rads; // desired yaw rate in radians per second calculated by position controller + float _roll_target_rad; // desired roll angle in radians calculated by position controller + float _pitch_target_rad; // desired roll pitch in radians calculated by position controller + float _yaw_target_rad; // desired yaw in radians calculated by position controller + float _yaw_rate_target_rads; // desired yaw rate in radians per second calculated by position controller // position controller internal variables - Vector3p _pos_estimate_neu_m; - Vector3p _pos_desired_neu_m; // desired location, frame NEU in m relative to the EKF origin. This is equal to the _pos_target minus offsets - Vector3p _pos_target_neu_m; // target location, frame NEU in m relative to the EKF origin. This is equal to the _pos_desired_neu_m plus offsets - Vector3f _vel_estimate_neu_ms; - Vector3f _vel_desired_neu_ms; // desired velocity in NEU m/s - Vector3f _vel_target_neu_ms; // velocity target in NEU m/s calculated by pos_to_rate step - Vector3f _accel_desired_neu_mss; // desired acceleration in NEU m/s² (feed forward) - Vector3f _accel_target_neu_mss; // acceleration target in NEU m/s² + Vector3p _pos_estimate_ned_m; // estimated location, frame NED in m relative to the EKF origin. + Vector3p _pos_desired_ned_m; // desired location, frame NED in m relative to the EKF origin. This is equal to the _pos_target_ned_m minus offsets + Vector3p _pos_target_ned_m; // target location, frame NED in m relative to the EKF origin. This is equal to the _pos_desired_ned_m plus offsets + Vector3f _vel_estimate_ned_ms; // estimated velocity in NED m/s + Vector3f _vel_desired_ned_ms; // desired velocity in NED m/s + Vector3f _vel_target_ned_ms; // velocity target in NED m/s calculated by pos_to_rate step + Vector3f _accel_desired_ned_mss; // desired acceleration in NED m/s² (feed forward) + Vector3f _accel_target_ned_mss; // acceleration target in NED m/s² // todo: seperate the limit vector into ne and u. ne is based on acceleration while u is set +-1 based on throttle saturation. Together they don't form a direction vector because the units are different. - Vector3f _limit_vector_neu; // the direction that the position controller is limited, zero when not limited + Vector3f _limit_vector_ned; // the direction that the position controller is limited, zero when not limited // terrain handling variables - float _pos_terrain_target_u_m; // position terrain target in m relative to the EKF origin in NEU frame - float _pos_terrain_u_m; // position terrain in m from the EKF origin in NEU frame. This terrain moves towards _pos_terrain_target_u_m - float _vel_terrain_u_ms; // velocity terrain in NEU m/s calculated by pos_to_rate step. This terrain moves towards _vel_terrain_target - float _accel_terrain_u_mss; // acceleration terrain in NEU m/s² + float _pos_terrain_target_d_m; // position terrain target in m relative to the EKF origin in NED frame + float _pos_terrain_d_m; // position terrain in m from the EKF origin in NED frame. This terrain moves towards _pos_terrain_target_d_m + float _vel_terrain_d_ms; // velocity terrain in NED m/s calculated by pos_to_rate step. This terrain moves towards _vel_terrain_target + float _accel_terrain_d_mss; // acceleration terrain in NED m/s² // offset handling variables - Vector3p _pos_offset_target_neu_m; // position offset target in m relative to the EKF origin in NEU frame - Vector3p _pos_offset_neu_m; // position offset in m from the EKF origin in NEU frame. This offset moves towards _pos_offset_target_neu_m - Vector3f _vel_offset_target_neu_ms; // velocity offset target in m/s in NEU frame - Vector3f _vel_offset_neu_ms; // velocity offset in NEU m/s calculated by pos_to_rate step. This offset moves towards _vel_offset_target_neu_ms - Vector3f _accel_offset_target_neu_mss; // acceleration offset target in m/s² in NEU frame - Vector3f _accel_offset_neu_mss; // acceleration offset in NEU m/s² + Vector3p _pos_offset_target_ned_m; // position offset target in m relative to the EKF origin in NED frame + Vector3p _pos_offset_ned_m; // position offset in m from the EKF origin in NED frame. This offset moves towards _pos_offset_target_ned_m + Vector3f _vel_offset_target_ned_ms; // velocity offset target in m/s in NED frame + Vector3f _vel_offset_ned_ms; // velocity offset in NED m/s calculated by pos_to_rate step. This offset moves towards _vel_offset_target_ned_ms + Vector3f _accel_offset_target_ned_mss; // acceleration offset target in m/s² in NED frame + Vector3f _accel_offset_ned_mss; // acceleration offset in NED m/s² uint32_t _posvelaccel_offset_target_ne_ms; // system time that pos, vel, accel targets were set (used to implement timeouts) - uint32_t _posvelaccel_offset_target_u_ms; // system time that pos, vel, accel targets were set (used to implement timeouts) + uint32_t _posvelaccel_offset_target_d_ms; // system time that pos, vel, accel targets were set (used to implement timeouts) // ekf reset handling uint32_t _ekf_ne_reset_ms; // system time of last recorded ekf ne position reset - uint32_t _ekf_u_reset_ms; // system time of last recorded ekf altitude reset + uint32_t _ekf_d_reset_ms; // system time of last recorded ekf altitude reset EKFResetMethod _ekf_reset_method = EKFResetMethod::MoveTarget; // EKF reset handling method. Loiter should use MoveTarget, Auto should use MoveVehicle // high vibration handling diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 9c393fc1783a4..a1ea51835e30d 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -153,7 +153,7 @@ bool AC_AutoTune::init_position_controller(void) init_z_limits(); // initialise the vertical position controller - pos_control->init_U_controller(); + pos_control->D_init_controller(); return true; } @@ -236,7 +236,7 @@ void AC_AutoTune::run() if (!motors->armed() || !motors->get_interlock()) { motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); attitude_control->set_throttle_out(0.0f, true, 0.0f); - pos_control->relax_U_controller(0.0f); + pos_control->D_relax_controller(0.0f); return; } @@ -319,8 +319,8 @@ void AC_AutoTune::run() motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // Update vertical position controller with pilot climb rate input - pos_control->set_pos_target_U_from_climb_rate_ms(target_climb_rate_ms); - pos_control->update_U_controller(); + pos_control->D_set_pos_target_from_climb_rate_ms(target_climb_rate_ms); + pos_control->D_update_controller(); } // return true if vehicle is close to level @@ -706,7 +706,7 @@ void AC_AutoTune::get_poshold_attitude_rad(float &roll_out_rad, float &pitch_out if (!have_position) { have_position = true; - start_position_neu_m = pos_control->get_pos_estimate_NEU_m(); + start_position_ned_m = pos_control->get_pos_estimate_NED_m(); } // don't go past 10 degrees, as autotune result would deteriorate too much @@ -719,9 +719,9 @@ void AC_AutoTune::get_poshold_attitude_rad(float &roll_out_rad, float &pitch_out // target position. That corresponds to a lean angle of 2.5 degrees const float yaw_dist_limit_m = 5.0; - Vector3f pos_error_neu_m = (pos_control->get_pos_estimate_NEU_m() - start_position_neu_m).tofloat(); - pos_error_neu_m.z = 0; - float dist_m = pos_error_neu_m.length(); + Vector3f pos_error_ned_m = (pos_control->get_pos_estimate_NED_m() - start_position_ned_m).tofloat(); + pos_error_ned_m.z = 0; + float dist_m = pos_error_ned_m.length(); if (dist_m < 0.10) { // don't do anything within 10cm return; @@ -731,7 +731,7 @@ void AC_AutoTune::get_poshold_attitude_rad(float &roll_out_rad, float &pitch_out very simple linear controller */ float scaling = constrain_float(angle_max_rad * dist_m / dist_limit_m, 0, angle_max_rad); - Vector2f angle_ne(pos_error_neu_m.x, pos_error_neu_m.y); + Vector2f angle_ne(pos_error_ned_m.x, pos_error_ned_m.y); angle_ne *= scaling / dist_m; // rotate into body frame @@ -749,7 +749,7 @@ void AC_AutoTune::get_poshold_attitude_rad(float &roll_out_rad, float &pitch_out position. This ensures that autotune doesn't have to deal with more than 2.5 degrees of attitude on the axis it is tuning */ - float target_yaw_rad = atan2f(pos_error_neu_m.y, pos_error_neu_m.x); + float target_yaw_rad = atan2f(pos_error_ned_m.y, pos_error_ned_m.x); if (axis == AxisType::PITCH) { // for roll and yaw tuning we point along the wind, for pitch // we point across the wind diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h index b7dd0bf9f5130..9f5b94043b104 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -339,7 +339,7 @@ class AC_AutoTune bool pilot_override; // true = pilot is overriding controls so we suspend tuning temporarily bool use_poshold; // true = enable position hold bool have_position; // true = start_position is value - Vector3p start_position_neu_m; // target when holding position as an offset from EKF origin in cm in NEU frame + Vector3p start_position_ned_m; // target when holding position as an offset from EKF origin in meters in NED frame // variables uint32_t override_time; // the last time the pilot overrode the controls diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index 80a5c5f7c3f55..14a35b3d34498 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/libraries/AC_Avoidance/AC_Avoid.h @@ -53,11 +53,11 @@ class AC_Avoid { bool backing_up = false; adjust_velocity(desired_vel_neu_cms, backing_up, kP, accel_cmss, kP_z, accel_z_cmss, dt); } - void adjust_velocity_m(Vector3f &desired_vel_neu_ms, float kP, float accel_mss, float kP_z, float accel_z_mss, float dt) { + void adjust_velocity_NED_m(Vector3f &desired_vel_ned_ms, float kP, float accel_mss, float kP_z, float accel_z_mss, float dt) { bool backing_up = false; - Vector3f desired_vel_neu_cms = desired_vel_neu_ms * 100.0; + Vector3f desired_vel_neu_cms{desired_vel_ned_ms.x * 100.0, desired_vel_ned_ms.y * 100.0, -desired_vel_ned_ms.z * 100.0}; adjust_velocity(desired_vel_neu_cms, backing_up, kP, accel_mss * 100.0, kP_z, accel_z_mss * 100.0, dt); - desired_vel_neu_ms = desired_vel_neu_cms * 0.01; + desired_vel_ned_ms = Vector3f{desired_vel_neu_cms.x, desired_vel_neu_cms.y, -desired_vel_neu_cms.z} * 0.01; } // This method limits velocity and calculates backaway velocity from various supported fences diff --git a/libraries/AC_InputManager/AC_InputManager_Heli.cpp b/libraries/AC_InputManager/AC_InputManager_Heli.cpp index 4779ad5bf17f1..ddabd25358c9c 100644 --- a/libraries/AC_InputManager/AC_InputManager_Heli.cpp +++ b/libraries/AC_InputManager/AC_InputManager_Heli.cpp @@ -107,17 +107,25 @@ float AC_InputManager_Heli::get_pilot_desired_collective(int16_t control_in) } acro_col_out = constrain_float(acro_col_out, 0.0f, 1.0f); - // ramp to and from stab col over 1/2 second - if (_im_flags_heli.use_stab_col && (_stab_col_ramp < 1.0f)){ - _stab_col_ramp += 2.0f/(float)_loop_rate; - } else if(!_im_flags_heli.use_stab_col && (_stab_col_ramp > 0.0f)){ - _stab_col_ramp -= 2.0f/(float)_loop_rate; + // ramp function + if (is_positive(_ramp)) { + float dt = 1/(float)_loop_rate; + // factor 2 to transition over a time span of 0.5s + _ramp -= 2*dt; + _ramp = constrain_float(_ramp, 0.0f, 1.0f); } - _stab_col_ramp = constrain_float(_stab_col_ramp, 0.0f, 1.0f); - // scale collective output smoothly between acro and stab col + //set Stabilize or Acro collective output + float new_flightmode_col_output; + if (_im_flags_heli.use_stab_col) { + new_flightmode_col_output = stab_col_out; + } else { + new_flightmode_col_output = acro_col_out; + } + + // scale collective output smoothly between previous and current mode output float collective_out; - collective_out = (float)((1.0f-_stab_col_ramp)*acro_col_out + _stab_col_ramp*stab_col_out); + collective_out = new_flightmode_col_output * (1.0 - _ramp) + _ramp * _old_flightmode_col_output; collective_out = constrain_float(collective_out, 0.0f, 1.0f); return collective_out; diff --git a/libraries/AC_InputManager/AC_InputManager_Heli.h b/libraries/AC_InputManager/AC_InputManager_Heli.h index 758d542d1720d..7852eb4338a3a 100644 --- a/libraries/AC_InputManager/AC_InputManager_Heli.h +++ b/libraries/AC_InputManager/AC_InputManager_Heli.h @@ -27,14 +27,17 @@ class AC_InputManager_Heli : public AC_InputManager { /* Do not allow copies */ CLASS_NO_COPY(AC_InputManager_Heli); + //pass the last collective output from non-manual throttle mode + void set_last_coll_output(float collective) { _old_flightmode_col_output = collective; } + // get_pilot_desired_collective - rescale's pilot collective pitch input in Stabilize and Acro modes float get_pilot_desired_collective(int16_t control_in); // set_use_stab_col - setter function void set_use_stab_col(bool use) { _im_flags_heli.use_stab_col = use; } - // set_heli_stab_col_ramp - setter function - void set_stab_col_ramp(float ramp) { _stab_col_ramp = constrain_float(ramp, 0.0, 1.0); } + // set collective_ramp - setter function + void set_collective_ramp(float ramp) { _ramp = constrain_float(ramp, 0.0, 1.0); } // parameter_check - returns true if input manager specific parameters are sensible, used for pre-arm check bool parameter_check(char* fail_msg, uint8_t fail_msg_len) const; @@ -46,8 +49,11 @@ class AC_InputManager_Heli : public AC_InputManager { bool use_stab_col; // 1 if we should use Stabilise mode collective range, 0 for Acro range } _im_flags_heli; - // factor used to smoothly ramp collective from Acro value to Stab-Col value - float _stab_col_ramp = 0; + // previous flight mode collective output + float _old_flightmode_col_output; + + // ramp factor from previous mode to current mode collective output + float _ramp; AP_Int16 _heli_stab_col_min; // minimum collective pitch setting at zero throttle input in Stabilize mode AP_Int16 _heli_stab_col_low; // collective pitch setting at mid-low throttle input in Stabilize mode diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index 9585e0efa625c..3cc483504c351 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -105,7 +105,7 @@ class AC_PrecLand // return the last known landing position in Earth Frame NED meters. void get_last_detected_landing_pos_NED_m(Vector3p &pos) const { pos = _last_target_pos_rel_origin_ned_m; } - // return the last known postion of the vehicle when the target was detected in Earth Frame NED meters. + // return the last known position of the vehicle when the target was detected in Earth Frame NED meters. void get_last_vehicle_pos_when_target_detected_NED_m(Vector3p &pos) const { pos = _last_vehicle_pos_ned_m; } // Parameter getters diff --git a/libraries/AC_PrecLand/PosVelEKF.cpp b/libraries/AC_PrecLand/PosVelEKF.cpp index 7576b37c4e6eb..de0a0855f0021 100644 --- a/libraries/AC_PrecLand/PosVelEKF.cpp +++ b/libraries/AC_PrecLand/PosVelEKF.cpp @@ -73,7 +73,7 @@ void PosVelEKF::fusePos(float pos, float posVar) /* Measurement matrix H = [1 0] since we are directly measuring pos only Innovation Covariance = S = H * P * H.Transpose + R - Since this is a 1-D measurement, R = posVar, which is expected variance in postion sensor reading + Since this is a 1-D measurement, R = posVar, which is expected variance in position sensor reading Post multiplication this becomes: */ const float innovation_covariance = _cov[0] + posVar; diff --git a/libraries/AC_PrecLand/PosVelEKF.h b/libraries/AC_PrecLand/PosVelEKF.h index 3648c5a7a622b..729d9a8be7814 100644 --- a/libraries/AC_PrecLand/PosVelEKF.h +++ b/libraries/AC_PrecLand/PosVelEKF.h @@ -1,7 +1,7 @@ #pragma once /* -* This class implements a simple 1-D Extended Kalman Filter to estimate the Relative body frame postion of the lading target and its relative velocity +* This class implements a simple 1-D Extended Kalman Filter to estimate the Relative body frame position of the lading target and its relative velocity * position and velocity of the target is predicted using delta velocity * The predictions are corrected periodically using the landing target sensor(or camera) */ diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp index cbeb37ef7f93f..2a5f73368f5dd 100644 --- a/libraries/AC_WPNav/AC_Circle.cpp +++ b/libraries/AC_WPNav/AC_Circle.cpp @@ -49,31 +49,32 @@ AC_Circle::AC_Circle(const AP_AHRS_View& ahrs, AC_PosControl& pos_control) : } // Initializes circle flight using a center position in centimeters relative to the EKF origin. -// See init_NEU_m() for full details. +// See init_NED_m() for full details. void AC_Circle::init_NEU_cm(const Vector3p& center_neu_cm, bool is_terrain_alt, float rate_degs) { - // Convert input from cm to meters and delegate to meter-based initializer - init_NEU_m(center_neu_cm * 0.01, is_terrain_alt, rate_degs); + // Convert input from NEU cm to NED meters and delegate to meter-based initializer + Vector3p center_ned_m = Vector3p{center_neu_cm.x, center_neu_cm.y, -center_neu_cm.z} * 0.01; + init_NED_m(center_ned_m, is_terrain_alt, rate_degs); } // Initializes circle flight mode using a specified center position in meters. // Parameters: -// - center_neu_m: Center of the circle in NEU frame (meters, relative to EKF origin) -// - is_terrain_alt: If true, center.z is interpreted as height above terrain; otherwise, above EKF origin +// - center_ned_m: Center of the circle in NED frame (meters, relative to EKF origin) +// - is_terrain_alt: If true, center_ned_m.z is interpreted as relative to terrain; otherwise, above EKF origin // - rate_degs: Desired turn rate in degrees per second (positive = clockwise, negative = counter-clockwise) // Caller must preconfigure the position controller's speed and acceleration settings before calling. -void AC_Circle::init_NEU_m(const Vector3p& center_neu_m, bool is_terrain_alt, float rate_degs) +void AC_Circle::init_NED_m(const Vector3p& center_ned_m, bool is_terrain_alt, float rate_degs) { // Store circle center and frame reference - _center_neu_m = center_neu_m; + _center_ned_m = center_ned_m; _is_terrain_alt = is_terrain_alt; // Convert desired turn rate from degrees to radians _rotation_rate_max_rads = radians(rate_degs); // Initialise position controller using current lean angles - _pos_control.init_NE_controller_stopping_point(); - _pos_control.init_U_controller_stopping_point(); + _pos_control.NE_init_controller_stopping_point(); + _pos_control.D_init_controller_stopping_point(); // Calculate velocity and acceleration limits based on circle configuration calc_velocities(true); @@ -93,19 +94,19 @@ void AC_Circle::init() _rotation_rate_max_rads = radians(_rate_parm_degs); // Initialise position controller using current lean angles - _pos_control.init_NE_controller_stopping_point(); - _pos_control.init_U_controller_stopping_point(); + _pos_control.NE_init_controller_stopping_point(); + _pos_control.D_init_controller_stopping_point(); // Get stopping point as initial reference for center - const Vector3p& stopping_point_neu_m = _pos_control.get_pos_desired_NEU_m(); + const Vector3p& stopping_point_ned_m = _pos_control.get_pos_desired_NED_m(); // By default, set center to stopping point - _center_neu_m = stopping_point_neu_m; + _center_ned_m = stopping_point_ned_m; // If INIT_AT_CENTER is not set, project center forward by radius in heading direction if ((_options.get() & CircleOptions::INIT_AT_CENTER) == 0) { - _center_neu_m.x += _radius_m * _ahrs.cos_yaw(); - _center_neu_m.y += _radius_m * _ahrs.sin_yaw(); + _center_ned_m.x += _radius_m * _ahrs.cos_yaw(); + _center_ned_m.y += _radius_m * _ahrs.sin_yaw(); } // Circle altitude is relative to EKF origin by default @@ -131,23 +132,23 @@ void AC_Circle::set_center(const Location& center) // Attempt to convert XY and Z to NEU frame with terrain altitude if (center.get_vector_xy_from_origin_NE_m(center_ne_m) && center.get_alt_m(Location::AltFrame::ABOVE_TERRAIN, terr_alt_m)) { - set_center_NEU_m(Vector3p{center_ne_m.x, center_ne_m.y, terr_alt_m}, true); + set_center_NED_m(Vector3p{center_ne_m.x, center_ne_m.y, -terr_alt_m}, true); } else { // Conversion failed: fall back to current position and log error - set_center_NEU_m(_pos_control.get_pos_estimate_NEU_m(), false); + set_center_NED_m(_pos_control.get_pos_estimate_NED_m(), false); LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_CIRCLE_INIT); } } else { // Handle alt-above-origin, alt-above-home, or absolute altitudes - Vector3p circle_center_neu_m; - if (!center.get_vector_from_origin_NEU_m(circle_center_neu_m)) { + Vector3p circle_center_ned_m; + if (!center.get_vector_from_origin_NED_m(circle_center_ned_m)) { // Conversion failed: fall back to current position and log error - circle_center_neu_m = _pos_control.get_pos_estimate_NEU_m(); + circle_center_ned_m = _pos_control.get_pos_estimate_NED_m(); LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_CIRCLE_INIT); } // Apply converted center and mark it as origin-relative - set_center_NEU_m(circle_center_neu_m, false); + set_center_NED_m(circle_center_ned_m, false); } } @@ -225,26 +226,26 @@ bool AC_Circle::update_ms(float climb_rate_ms) } // calculate z-axis target - float target_z_m; + float target_d_m; if (_is_terrain_alt) { - target_z_m = _center_neu_m.z + terrain_u_m; + target_d_m = _center_ned_m.z - terrain_u_m; } else { - target_z_m = _pos_control.get_pos_desired_U_m(); + target_d_m = -_pos_control.get_pos_desired_U_m(); } // Construct target position centered on the circle center - Vector3p target_neu_m { - _center_neu_m.x, - _center_neu_m.y, - target_z_m + Vector3p target_ned_m { + _center_ned_m.x, + _center_ned_m.y, + target_d_m }; if (!is_zero(_radius_m)) { // Calculate position on the circle edge based on current angle - target_neu_m.x += _radius_m * cosf(-_angle_rad); - target_neu_m.y += - _radius_m * sinf(-_angle_rad); + target_ned_m.x += _radius_m * cosf(-_angle_rad); + target_ned_m.y += - _radius_m * sinf(-_angle_rad); // Compute yaw toward the circle center - _yaw_rad = get_bearing_rad(_pos_control.get_pos_desired_NEU_m().xy().tofloat(), _center_neu_m.xy().tofloat()); + _yaw_rad = get_bearing_rad(_pos_control.get_pos_desired_NED_m().xy().tofloat(), _center_ned_m.xy().tofloat()); // Optionally adjust yaw to face direction of travel if ((_options.get() & CircleOptions::FACE_DIRECTION_OF_TRAVEL) != 0) { @@ -258,17 +259,17 @@ bool AC_Circle::update_ms(float climb_rate_ms) // update position controller target Vector2f zero_ne; - _pos_control.input_pos_vel_accel_NE_m(target_neu_m.xy(), zero_ne, zero_ne); + _pos_control.input_pos_vel_accel_NE_m(target_ned_m.xy(), zero_ne, zero_ne); if (_is_terrain_alt) { - float zero_u = 0; - float target_u_m = target_neu_m.z; - _pos_control.input_pos_vel_accel_U_m(target_u_m, zero_u, 0); + float vel_zero = 0; + float target_pos_d_m = target_ned_m.z; + _pos_control.input_pos_vel_accel_D_m(target_pos_d_m, vel_zero, 0); } else { - _pos_control.set_pos_target_U_from_climb_rate_ms(climb_rate_ms); + _pos_control.D_set_pos_target_from_climb_rate_ms(climb_rate_ms); } // update position controller - _pos_control.update_NE_controller(); + _pos_control.NE_update_controller(); // set update time _last_update_ms = AP_HAL::millis(); @@ -277,18 +278,18 @@ bool AC_Circle::update_ms(float climb_rate_ms) } // Returns the closest point on the circle to the vehicle's current position in centimeters. -// See get_closest_point_on_circle_NEU_m() for full details. +// See get_closest_point_on_circle_NED_m() for full details. void AC_Circle::get_closest_point_on_circle_NEU_cm(Vector3f& result_neu_cm, float& dist_cm) const { - // Convert input arguments from cm to meters - Vector3p result_neu_m = result_neu_cm.topostype() * 0.01; + // Convert input arguments from neu cm to ned meters + Vector3p result_ned_m = Vector3p{result_neu_cm.x, result_neu_cm.y, -result_neu_cm.z} * 0.01; float dist_m = dist_cm * 0.01; // Compute closest point in meters - get_closest_point_on_circle_NEU_m(result_neu_m, dist_m); + get_closest_point_on_circle_NED_m(result_ned_m, dist_m); - // Convert results back to centimeters - result_neu_cm = result_neu_m.tofloat() * 100.0; + // Convert results back to neu centimeters + result_neu_cm = Vector3f(result_ned_m.x, result_ned_m.y, -result_ned_m.z) * 100.0; dist_cm = dist_m * 100.0; } @@ -297,38 +298,38 @@ void AC_Circle::get_closest_point_on_circle_NEU_cm(Vector3f& result_neu_cm, floa // The altitude (z) is set to match the circle center's altitude. // dist_m is updated with the 3D distance to the circle edge from the stopping point. // If the vehicle is at the center, the point directly behind the vehicle (based on yaw) is returned. -void AC_Circle::get_closest_point_on_circle_NEU_m(Vector3p& result_neu_m, float& dist_to_edge_m) const +void AC_Circle::get_closest_point_on_circle_NED_m(Vector3p& result_ned_m, float& dist_to_edge_m) const { // Get vehicle stopping point from the position controller (in NEU frame, meters) - Vector3p stopping_point_neu_m; - _pos_control.get_stopping_point_NE_m(stopping_point_neu_m.xy()); - _pos_control.get_stopping_point_U_m(stopping_point_neu_m.z); + Vector3p stopping_point_ned_m; + _pos_control.get_stopping_point_NE_m(stopping_point_ned_m.xy()); + _pos_control.get_stopping_point_D_m(stopping_point_ned_m.z); // Compute vector from stopping point to the circle center - Vector3f vec_from_center_neu_m = (stopping_point_neu_m - _center_neu_m).tofloat(); + Vector3f vec_from_center_ned_m = (stopping_point_ned_m - _center_ned_m).tofloat(); // Return circle center if radius is zero (vehicle orbits in place) if (!is_positive(_radius_m)) { - result_neu_m = _center_neu_m; + result_ned_m = _center_ned_m; dist_to_edge_m = 0; return; } - const float dist_to_center_m_sq = vec_from_center_neu_m.length_squared(); + const float dist_to_center_m_sq = vec_from_center_ned_m.length_squared(); // Handle edge case: vehicle is at the exact center of the circle if (dist_to_center_m_sq < sq(0.5)) { - result_neu_m.x = _center_neu_m.x - _radius_m * _ahrs.cos_yaw(); - result_neu_m.y = _center_neu_m.y - _radius_m * _ahrs.sin_yaw(); - result_neu_m.z = _center_neu_m.z; - dist_to_edge_m = (stopping_point_neu_m - result_neu_m).length(); + result_ned_m.x = _center_ned_m.x - _radius_m * _ahrs.cos_yaw(); + result_ned_m.y = _center_ned_m.y - _radius_m * _ahrs.sin_yaw(); + result_ned_m.z = _center_ned_m.z; + dist_to_edge_m = (stopping_point_ned_m - result_ned_m).length(); return; } // Calculate the closest point on the circle's edge by projecting out from center - const float dist_to_center_m_xy = vec_from_center_neu_m.xy().length(); - result_neu_m.x = _center_neu_m.x + vec_from_center_neu_m.x / dist_to_center_m_xy * _radius_m; - result_neu_m.y = _center_neu_m.y + vec_from_center_neu_m.y / dist_to_center_m_xy * _radius_m; - result_neu_m.z = _center_neu_m.z; - dist_to_edge_m = (stopping_point_neu_m - result_neu_m).length(); + const float dist_to_center_m_xy = vec_from_center_ned_m.xy().length(); + result_ned_m.x = _center_ned_m.x + vec_from_center_ned_m.x / dist_to_center_m_xy * _radius_m; + result_ned_m.y = _center_ned_m.y + vec_from_center_ned_m.y / dist_to_center_m_xy * _radius_m; + result_ned_m.z = _center_ned_m.z; + dist_to_edge_m = (stopping_point_ned_m - result_ned_m).length(); } // Calculates angular velocity and acceleration limits based on the configured radius and rate. @@ -342,7 +343,7 @@ void AC_Circle::calc_velocities(bool init_velocity) _angular_accel_radss = MAX(fabsf(_angular_vel_max_rads), radians(AC_CIRCLE_ANGULAR_ACCEL_MIN)); // reach maximum yaw velocity in 1 second }else{ // Limit max horizontal speed based on radius and available acceleration - float vel_max_ms = MIN(_pos_control.get_max_speed_NE_ms(), safe_sqrt(0.5f*_pos_control.get_max_accel_NE_mss()*_radius_m)); + float vel_max_ms = MIN(_pos_control.NE_get_max_speed_ms(), safe_sqrt(0.5f*_pos_control.NE_get_max_accel_mss()*_radius_m)); // Convert linear speed to angular velocity (rad/s) _angular_vel_max_rads = vel_max_ms/_radius_m; @@ -351,7 +352,7 @@ void AC_Circle::calc_velocities(bool init_velocity) _angular_vel_max_rads = constrain_float(_rotation_rate_max_rads, -_angular_vel_max_rads, _angular_vel_max_rads); // Derive maximum angular acceleration - _angular_accel_radss = MAX(_pos_control.get_max_accel_NE_mss() / _radius_m, radians(AC_CIRCLE_ANGULAR_ACCEL_MIN)); + _angular_accel_radss = MAX(_pos_control.NE_get_max_accel_mss() / _radius_m, radians(AC_CIRCLE_ANGULAR_ACCEL_MIN)); } // Reset angular velocity to zero at initialization if requested @@ -380,12 +381,12 @@ void AC_Circle::init_start_angle(bool use_heading) _angle_rad = wrap_PI(_ahrs.yaw - M_PI); } else { // If vehicle is exactly at the center, init angle behind vehicle (prevent undefined bearing) - const Vector3p &curr_pos_desired_neu_m = _pos_control.get_pos_desired_NEU_m(); - if (is_equal(curr_pos_desired_neu_m.x, _center_neu_m.x) && is_equal(curr_pos_desired_neu_m.y, _center_neu_m.y)) { + const Vector3p &curr_pos_desired_ned_m = _pos_control.get_pos_desired_NED_m(); + if (is_equal(curr_pos_desired_ned_m.x, _center_ned_m.x) && is_equal(curr_pos_desired_ned_m.y, _center_ned_m.y)) { _angle_rad = wrap_PI(_ahrs.yaw - M_PI); } else { // Calculate bearing from circle center to current position - float bearing_rad = atan2f(curr_pos_desired_neu_m.y - _center_neu_m.y, curr_pos_desired_neu_m.x - _center_neu_m.x); + float bearing_rad = atan2f(curr_pos_desired_ned_m.y - _center_ned_m.y, curr_pos_desired_ned_m.x - _center_ned_m.x); _angle_rad = wrap_PI(bearing_rad); } } diff --git a/libraries/AC_WPNav/AC_Circle.h b/libraries/AC_WPNav/AC_Circle.h index a0ba05b3b9908..2d94195374960 100644 --- a/libraries/AC_WPNav/AC_Circle.h +++ b/libraries/AC_WPNav/AC_Circle.h @@ -19,16 +19,16 @@ class AC_Circle AC_Circle(const AP_AHRS_View& ahrs, AC_PosControl& pos_control); // Initializes circle flight using a center position in centimeters relative to the EKF origin. - // See init_NEU_m() for full details. + // See init_NED_m() for full details. void init_NEU_cm(const Vector3p& center_neu_cm, bool is_terrain_alt, float rate_degs); // Initializes circle flight mode using a specified center position in meters. // Parameters: - // - center_neu_m: Center of the circle in NEU frame (meters, relative to EKF origin) - // - is_terrain_alt: If true, center.z is interpreted as height above terrain; otherwise, above EKF origin + // - center_ned_m: Center of the circle in NED frame (meters, relative to EKF origin) + // - is_terrain_alt: If true, center_ned_m.z is interpreted as relative to terrain; otherwise, above EKF origin // - rate_degs: Desired turn rate in degrees per second (positive = clockwise, negative = counter-clockwise) // Caller must preconfigure the position controller's speed and acceleration settings before calling. - void init_NEU_m(const Vector3p& center_neu_m, bool is_terrain_alt, float rate_degs); + void init_NED_m(const Vector3p& center_ned_m, bool is_terrain_alt, float rate_degs); // Initializes the circle flight mode using the current stopping point as a reference. // If the INIT_AT_CENTER option is not set, the circle center is projected one radius ahead along the vehicle's heading. @@ -40,17 +40,17 @@ class AC_Circle // If conversion fails, defaults to current position and logs a navigation error. void set_center(const Location& center); - // Sets the circle center using a NEU position vector in meters from the EKF origin. - // If `is_terrain_alt` is true, the Z component is treated as altitude above terrain; otherwise, above EKF origin. - void set_center_NEU_m(const Vector3p& center_neu_m, bool is_terrain_alt) { _center_neu_m = center_neu_m; _is_terrain_alt = is_terrain_alt; } + // Sets the circle center using a NED position vector in meters from the EKF origin. + // - is_terrain_alt: If true, center_ned_m.z is interpreted as relative to terrain; otherwise, above EKF origin + void set_center_NED_m(const Vector3p& center_ned_m, bool is_terrain_alt) { _center_ned_m = center_ned_m; _is_terrain_alt = is_terrain_alt; } // Returns the circle center in centimeters from the EKF origin. - // See get_center_NEU_m() for full details. - const Vector3p get_center_NEU_cm() const { return get_center_NEU_m() * 100.0; } + // See get_center_NED_m() for full details. + const Vector3p get_center_NEU_cm() const { return Vector3p(_center_ned_m.x, _center_ned_m.y, -_center_ned_m.z) * 100.0; } // Returns the circle center in meters from the EKF origin. // Altitude frame is determined by `center_is_terrain_alt()`. - const Vector3p& get_center_NEU_m() const { return _center_neu_m; } + const Vector3p& get_center_NED_m() const { return _center_ned_m; } // Returns true if the circle center altitude is relative to terrain height. bool center_is_terrain_alt() const { return _is_terrain_alt; } @@ -119,15 +119,15 @@ class AC_Circle bool is_active() const; // Returns the closest point on the circle to the vehicle's current position in centimeters. - // See get_closest_point_on_circle_NEU_m() for full details. - void get_closest_point_on_circle_NEU_cm(Vector3f& result_NEU_cm, float& dist_cm) const; + // See get_closest_point_on_circle_NED_m() for full details. + void get_closest_point_on_circle_NEU_cm(Vector3f& result_neu_cm, float& dist_cm) const; // Returns the closest point on the circle to the vehicle's stopping point in meters. - // The result vector is updated with the NEU position of the closest point on the circle. + // The result vector is updated with the NED position of the closest point on the circle. // The altitude (z) is set to match the circle center's altitude. // dist_m is updated with the 3D distance to the circle edge from the stopping point. // If the vehicle is at the center, the point directly behind the vehicle (based on yaw) is returned. - void get_closest_point_on_circle_NEU_m(Vector3p& result_NEU_m, float& dist_m) const; + void get_closest_point_on_circle_NED_m(Vector3p& result_ned_m, float& dist_m) const; // Returns the horizontal distance to the circle target in meters. // Calculated using the position controller’s NE position error norm. @@ -202,7 +202,7 @@ class AC_Circle AP_Int16 _options; // Bitmask of CircleOptions (e.g. manual control, ROI at center, etc.). // internal variables - Vector3p _center_neu_m; // Center of the circle in meters from EKF origin (NEU frame). + Vector3p _center_ned_m; // Center of the circle in meters from EKF origin (NED frame). float _radius_m; // Current circle radius in meters. float _rotation_rate_max_rads; // Requested circle turn rate in rad/s (+ve = CW, -ve = CCW). float _yaw_rad; // Desired yaw heading in radians (typically toward circle center or tangent). @@ -215,7 +215,7 @@ class AC_Circle float _last_radius_param_cm; // Cached copy of radius parameter (cm) to detect parameter changes. // terrain following variables - bool _is_terrain_alt; // True if _center_neu_m.z is relative to terrain height; false if relative to EKF origin. + bool _is_terrain_alt; // True if _center_ned_m.z is relative to terrain height; false if relative to EKF origin. bool _rangefinder_available; // True if rangefinder is available and enabled. bool _rangefinder_healthy; // True if rangefinder reading is within valid operating range. float _rangefinder_terrain_u_m; // Terrain height above EKF origin (meters), from rangefinder or terrain database. diff --git a/libraries/AC_WPNav/AC_Loiter.cpp b/libraries/AC_WPNav/AC_Loiter.cpp index 1ff01c631b7e1..75136834706b6 100644 --- a/libraries/AC_WPNav/AC_Loiter.cpp +++ b/libraries/AC_WPNav/AC_Loiter.cpp @@ -95,18 +95,18 @@ AC_Loiter::AC_Loiter(const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const } // Sets the initial loiter target position in meters from the EKF origin. -// - position_neu_m: horizontal position in the NE frame, in meters. +// - position_ne_m: horizontal position in the NE frame, in meters. // - Initializes internal control state including acceleration targets and feed-forward planning. void AC_Loiter::init_target_m(const Vector2p& position_ne_m) { sanity_check_params(); // Configure speed/accel limits in meters using internal parameter (_accel_max_ne_cmss) - _pos_control.set_correction_speed_accel_NE_m(LOITER_VEL_CORRECTION_MAX_MS, _accel_max_ne_cmss * 0.01); - _pos_control.set_pos_error_max_NE_m(LOITER_POS_CORRECTION_MAX_M); + _pos_control.NE_set_correction_speed_accel_m(LOITER_VEL_CORRECTION_MAX_MS, _accel_max_ne_cmss * 0.01); + _pos_control.NE_set_pos_error_max_m(LOITER_POS_CORRECTION_MAX_M); // Reset controller state for stationary loiter - _pos_control.init_NE_controller_stopping_point(); + _pos_control.NE_init_controller_stopping_point(); // Zero out desired and predicted accelerations and angles _predicted_accel_ne_mss.zero(); @@ -125,14 +125,14 @@ void AC_Loiter::init_target() sanity_check_params(); // Configure correction speed and acceleration limits (in m/s and m/s²) - _pos_control.set_correction_speed_accel_NE_m(LOITER_VEL_CORRECTION_MAX_MS, _accel_max_ne_cmss * 0.01); - _pos_control.set_pos_error_max_NE_m(LOITER_POS_CORRECTION_MAX_M); + _pos_control.NE_set_correction_speed_accel_m(LOITER_VEL_CORRECTION_MAX_MS, _accel_max_ne_cmss * 0.01); + _pos_control.NE_set_pos_error_max_m(LOITER_POS_CORRECTION_MAX_M); // Apply velocity smoothing: softly transitions target acceleration to zero - _pos_control.relax_velocity_controller_NE(); + _pos_control.NE_relax_velocity_controller(); // Initialize prediction state using current acceleration and lean angles - _predicted_accel_ne_mss = _pos_control.get_accel_target_NEU_mss().xy(); + _predicted_accel_ne_mss = _pos_control.get_accel_target_NED_mss().xy(); _predicted_euler_angle_rad.x = _pos_control.get_roll_rad(); _predicted_euler_angle_rad.y = _pos_control.get_pitch_rad(); _brake_accel_mss = 0.0f; @@ -142,7 +142,7 @@ void AC_Loiter::init_target() // Internally softens horizontal control gains. void AC_Loiter::soften_for_landing() { - _pos_control.soften_for_landing_NE(); + _pos_control.NE_soften_for_landing(); } // Sets pilot desired acceleration using Euler angles in centidegrees. @@ -162,10 +162,10 @@ void AC_Loiter::set_pilot_desired_acceleration_rad(float euler_roll_angle_rad, f // convert our desired attitude to an acceleration vector assuming we are not accelerating vertically const Vector3f desired_euler_rad {euler_roll_angle_rad, euler_pitch_angle_rad, _ahrs.yaw}; - const Vector3f desired_accel_neu_mss = _pos_control.lean_angles_rad_to_accel_NEU_mss(desired_euler_rad); + const Vector3f desired_accel_ned_mss = _pos_control.lean_angles_rad_to_accel_NED_mss(desired_euler_rad); - _desired_accel_ne_mss.x = desired_accel_neu_mss.x; - _desired_accel_ne_mss.y = desired_accel_neu_mss.y; + _desired_accel_ne_mss.x = desired_accel_ned_mss.x; + _desired_accel_ne_mss.y = desired_accel_ned_mss.y; // Compute attitude error between desired and predicted lean angles Vector2f angle_error_euler_rad(wrap_PI(euler_roll_angle_rad - _predicted_euler_angle_rad.x), wrap_PI(euler_pitch_angle_rad - _predicted_euler_angle_rad.y)); @@ -178,13 +178,13 @@ void AC_Loiter::set_pilot_desired_acceleration_rad(float euler_roll_angle_rad, f // Convert predicted angles into an acceleration vector for braking/shaping const Vector3f predicted_euler_rad {_predicted_euler_angle_rad.x, _predicted_euler_angle_rad.y, _ahrs.yaw}; - const Vector3f predicted_accel_neu_mss = _pos_control.lean_angles_rad_to_accel_NEU_mss(predicted_euler_rad); + const Vector3f predicted_accel_ned_mss = _pos_control.lean_angles_rad_to_accel_NED_mss(predicted_euler_rad); - _predicted_accel_ne_mss = predicted_accel_neu_mss.xy(); + _predicted_accel_ne_mss = predicted_accel_ned_mss.xy(); if (loiter_option_is_set(LoiterOption::COORDINATED_TURN_ENABLED)) { Vector3f target_ang_vel_rads = _attitude_control.get_attitude_target_ang_vel(); - Vector3f desired_velocity_ms = _pos_control.get_vel_desired_NEU_ms(); + Vector3f desired_velocity_ms = _pos_control.get_vel_desired_NED_ms(); Vector2f turn_accel_ne_mss = Vector2f(-desired_velocity_ms.y * target_ang_vel_rads.z, desired_velocity_ms.x * target_ang_vel_rads.z); _desired_accel_ne_mss += turn_accel_ne_mss; _predicted_accel_ne_mss += turn_accel_ne_mss; @@ -233,7 +233,7 @@ void AC_Loiter::update(bool avoidance_on) calc_desired_velocity(avoidance_on); // Run position controller to compute desired attitude and thrust - _pos_control.update_NE_controller(); + _pos_control.NE_update_controller(); } // Sets the maximum allowed horizontal loiter speed in m/s. @@ -284,7 +284,7 @@ void AC_Loiter::calc_desired_velocity(bool avoidance_on) } // Integrate predicted acceleration - Vector2f desired_vel_ne_ms = _pos_control.get_vel_desired_NEU_ms().xy(); + Vector2f desired_vel_ne_ms = _pos_control.get_vel_desired_NED_ms().xy(); // update the desired velocity using our predicted acceleration desired_vel_ne_ms += _predicted_accel_ne_mss * dt_s; @@ -301,7 +301,7 @@ void AC_Loiter::calc_desired_velocity(bool avoidance_on) float loiter_brake_accel_mss = 0.0f; if (_desired_accel_ne_mss.is_zero()) { if ((AP_HAL::millis() - _brake_timer_ms) > _brake_delay_s * 1000.0) { - float brake_gain = _pos_control.get_vel_NE_pid().kP() * 0.5f; + float brake_gain = _pos_control.NE_get_vel_pid().kP() * 0.5f; loiter_brake_accel_mss = constrain_float(sqrt_controller(desired_speed_ms, brake_gain, _brake_jerk_max_cmsss * 0.01, dt_s), 0.0f, _brake_accel_max_cmss * 0.01); } } else { @@ -333,19 +333,19 @@ void AC_Loiter::calc_desired_velocity(bool avoidance_on) // TODO: We need to also limit the _desired_accel_ne_mss AC_Avoid *_avoid = AP::ac_avoid(); if (_avoid != nullptr) { - Vector3f avoidance_vel_neu_cms{desired_vel_ne_ms.x * 100.0, desired_vel_ne_ms.y * 100.0, 0.0f}; - _avoid->adjust_velocity(avoidance_vel_neu_cms, _pos_control.get_pos_NE_p().kP(), _accel_max_ne_cmss, _pos_control.get_pos_U_p().kP(), _pos_control.get_max_accel_U_mss() * 100.0, dt_s); - desired_vel_ne_ms = avoidance_vel_neu_cms.xy() * 0.01; + Vector3f avoidance_vel_ned_cms{desired_vel_ne_ms.x * 100.0, desired_vel_ne_ms.y * 100.0, 0.0f}; + _avoid->adjust_velocity(avoidance_vel_ned_cms, _pos_control.NE_get_pos_p().kP(), _accel_max_ne_cmss, _pos_control.D_get_pos_p().kP(), _pos_control.D_get_max_accel_mss() * 100.0, dt_s); + desired_vel_ne_ms = avoidance_vel_ned_cms.xy() * 0.01; } } #endif // !APM_BUILD_ArduPlane // Retrieve current desired position - Vector2p desired_pos_neu_m = _pos_control.get_pos_desired_NEU_m().xy(); + Vector2p desired_pos_ned_m = _pos_control.get_pos_desired_NED_m().xy(); // Integrate velocity to update desired position - desired_pos_neu_m += (desired_vel_ne_ms * dt_s).topostype(); + desired_pos_ned_m += (desired_vel_ne_ms * dt_s).topostype(); // Send updated position, velocity, and acceleration to the position controller - _pos_control.set_pos_vel_accel_NE_m(desired_pos_neu_m, desired_vel_ne_ms, _desired_accel_ne_mss); + _pos_control.set_pos_vel_accel_NE_m(desired_pos_ned_m, desired_vel_ne_ms, _desired_accel_ne_mss); } diff --git a/libraries/AC_WPNav/AC_Loiter.h b/libraries/AC_WPNav/AC_Loiter.h index 8b566d24a317c..541a0e4ff8b91 100644 --- a/libraries/AC_WPNav/AC_Loiter.h +++ b/libraries/AC_WPNav/AC_Loiter.h @@ -16,9 +16,9 @@ class AC_Loiter AC_Loiter(const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control); // Sets the initial loiter target position in meters from the EKF origin. - // - position_neu_m: horizontal position in the NE frame, in meters. + // - position_ne_m: horizontal position in the NE frame, in meters. // - Initializes internal control state including acceleration targets and feed-forward planning. - void init_target_m(const Vector2p& position_neu_m); + void init_target_m(const Vector2p& position_ne_m); // Initializes the loiter controller using the current position and velocity. // Updates feed-forward velocity, predicted acceleration, and resets control state. diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index abf7622bdd97b..fbeba837afe85 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -153,7 +153,7 @@ AC_WPNav::TerrainSource AC_WPNav::get_terrain_source() const // Initializes waypoint and spline navigation using inputs in meters. // Sets speed and acceleration limits, calculates jerk constraints, // and initializes spline or S-curve leg with a defined starting point. -void AC_WPNav::wp_and_spline_init_m(float speed_ms, Vector3p stopping_point_neu_m) +void AC_WPNav::wp_and_spline_init_m(float speed_ms, Vector3p stopping_point_ned_m) { // ensure waypoint radius is not below minimum allowed value _wp_radius_cm.set_and_save_ifchanged(MAX(_wp_radius_cm, WPNAV_WP_RADIUS_MIN_CM)); @@ -162,8 +162,8 @@ void AC_WPNav::wp_and_spline_init_m(float speed_ms, Vector3p stopping_point_neu_ _wp_speed_cms.set_and_save_ifchanged(MAX(_wp_speed_cms, WPNAV_WP_SPEED_MIN_MS * 100.0)); // initialise position controller - _pos_control.init_U_controller_stopping_point(); - _pos_control.init_NE_controller_stopping_point(); + _pos_control.D_init_controller_stopping_point(); + _pos_control.NE_init_controller_stopping_point(); // determine desired waypoint speed; fallback to default if not provided _check_wp_speed_change = !is_positive(speed_ms); @@ -171,10 +171,10 @@ void AC_WPNav::wp_and_spline_init_m(float speed_ms, Vector3p stopping_point_neu_ _wp_desired_speed_ne_ms = MAX(_wp_desired_speed_ne_ms, WPNAV_WP_SPEED_MIN_MS); // initialise position controller speed and acceleration - _pos_control.set_max_speed_accel_NE_m(_wp_desired_speed_ne_ms, get_wp_acceleration_mss()); - _pos_control.set_correction_speed_accel_NE_m(_wp_desired_speed_ne_ms, get_wp_acceleration_mss()); - _pos_control.set_max_speed_accel_U_m(get_default_speed_down_ms(), get_default_speed_up_ms(), get_accel_U_mss()); - _pos_control.set_correction_speed_accel_U_m(get_default_speed_down_ms(), get_default_speed_up_ms(), get_accel_U_mss()); + _pos_control.NE_set_max_speed_accel_m(_wp_desired_speed_ne_ms, get_wp_acceleration_mss()); + _pos_control.NE_set_correction_speed_accel_m(_wp_desired_speed_ne_ms, get_wp_acceleration_mss()); + _pos_control.D_set_max_speed_accel_m(get_default_speed_down_ms(), get_default_speed_up_ms(), get_accel_D_mss()); + _pos_control.D_set_correction_speed_accel_m(get_default_speed_down_ms(), get_default_speed_up_ms(), get_accel_D_mss()); // calculate jerk limit if not explicitly set by parameter if (!is_positive(_wp_jerk_msss)) { @@ -191,10 +191,10 @@ void AC_WPNav::wp_and_spline_init_m(float speed_ms, Vector3p stopping_point_neu_ _flags.fast_waypoint = false; // determine initial origin and destination; fallback to current stopping point if not provided - if (stopping_point_neu_m.is_zero()) { - get_wp_stopping_point_NEU_m(stopping_point_neu_m); + if (stopping_point_ned_m.is_zero()) { + get_wp_stopping_point_NED_m(stopping_point_ned_m); } - _origin_neu_m = _destination_neu_m = stopping_point_neu_m; + _origin_ned_m = _destination_ned_m = stopping_point_ned_m; _is_terrain_alt = false; _this_leg_is_spline = false; @@ -227,8 +227,8 @@ void AC_WPNav::set_speed_NE_ms(float speed_ms) _wp_desired_speed_ne_ms = speed_ms; // update horizontal shaping and correction constraints - _pos_control.set_max_speed_accel_NE_m(_wp_desired_speed_ne_ms, get_wp_acceleration_mss()); - _pos_control.set_correction_speed_accel_NE_m(_wp_desired_speed_ne_ms, get_wp_acceleration_mss()); + _pos_control.NE_set_max_speed_accel_m(_wp_desired_speed_ne_ms, get_wp_acceleration_mss()); + _pos_control.NE_set_correction_speed_accel_m(_wp_desired_speed_ne_ms, get_wp_acceleration_mss()); // update internal trajectory with new limits update_track_with_speed_accel_limits(); @@ -240,7 +240,7 @@ void AC_WPNav::set_speed_NE_ms(float speed_ms) void AC_WPNav::set_speed_up_ms(float speed_up_ms) { // update vertical controller's max speed and accel limits (U axis) - _pos_control.set_max_speed_accel_U_m(_pos_control.get_max_speed_down_ms(), speed_up_ms, _pos_control.get_max_accel_U_mss()); + _pos_control.D_set_max_speed_accel_m(_pos_control.get_max_speed_down_ms(), speed_up_ms, _pos_control.D_get_max_accel_mss()); // recompute the trajectory using updated limits update_track_with_speed_accel_limits(); @@ -251,46 +251,46 @@ void AC_WPNav::set_speed_up_ms(float speed_up_ms) void AC_WPNav::set_speed_down_ms(float speed_down_ms) { // update vertical controller descent speed - _pos_control.set_max_speed_accel_U_m(speed_down_ms, _pos_control.get_max_speed_up_ms(), _pos_control.get_max_accel_U_mss()); + _pos_control.D_set_max_speed_accel_m(speed_down_ms, _pos_control.get_max_speed_up_ms(), _pos_control.D_get_max_accel_mss()); // recompute the trajectory using updated limits update_track_with_speed_accel_limits(); } // Sets the current waypoint destination using a Location object. -// Converts global coordinates to NEU position and sets destination. +// Converts global coordinates to NED position and sets destination. // arc_rad specifies the signed arc angle in radians for an ARC_WAYPOINT segment (0 for straight path) // Returns false if conversion fails (e.g. missing terrain data). bool AC_WPNav::set_wp_destination_loc(const Location& destination, float arc_rad) { bool is_terrain_alt; - Vector3p dest_neu_m; + Vector3p dest_ned_m; - // convert Location to NEU position vector in meters and determine altitude reference frame - if (!get_vector_NEU_m(destination, dest_neu_m, is_terrain_alt)) { + // convert Location to NED position vector in meters and determine altitude reference frame + if (!get_vector_NED_m(destination, dest_ned_m, is_terrain_alt)) { return false; } // apply destination as the active waypoint leg - return set_wp_destination_NEU_m(dest_neu_m, is_terrain_alt, arc_rad); + return set_wp_destination_NED_m(dest_ned_m, is_terrain_alt, arc_rad); } // Sets the next waypoint destination using a Location object. -// Converts global coordinates to NEU position and preloads the trajectory. +// Converts global coordinates to NED position and preloads the trajectory. // arc_rad specifies the signed arc angle in radians for an ARC_WAYPOINT segment (0 for straight path) // Returns false if conversion fails or terrain data is unavailable. bool AC_WPNav::set_wp_destination_next_loc(const Location& destination, float arc_rad) { bool is_terrain_alt; - Vector3p dest_neu_m; + Vector3p dest_ned_m; - // convert Location to NEU position vector in meters and determine altitude reference frame - if (!get_vector_NEU_m(destination, dest_neu_m, is_terrain_alt)) { + // convert Location to NED position vector in meters and determine altitude reference frame + if (!get_vector_NED_m(destination, dest_ned_m, is_terrain_alt)) { return false; } // apply destination as the next waypoint leg - return set_wp_destination_next_NEU_m(dest_neu_m, is_terrain_alt, arc_rad); + return set_wp_destination_next_NED_m(dest_ned_m, is_terrain_alt, arc_rad); } // Gets the current waypoint destination as a Location object. @@ -303,24 +303,25 @@ bool AC_WPNav::get_wp_destination_loc(Location& destination) const return false; } - // convert NEU waypoint to global Location format with appropriate altitude frame - destination = Location{get_wp_destination_NEU_m() * 100.0, _is_terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN}; + // convert NED waypoint to global Location format with appropriate altitude frame + destination = Location::from_ekf_offset_NED_m(get_wp_destination_NED_m(), _is_terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); return true; } // Sets waypoint destination using NEU position vector in centimeters from EKF origin. -// See set_wp_destination_NEU_m() for full details. +// See set_wp_destination_NED_m() for full details. bool AC_WPNav::set_wp_destination_NEU_cm(const Vector3f& destination_neu_cm, bool is_terrain_alt) { - return set_wp_destination_NEU_m(destination_neu_cm.topostype() * 0.01, is_terrain_alt); + Vector3p destination_ned_m = Vector3p(destination_neu_cm.x, destination_neu_cm.y, -destination_neu_cm.z) * 0.01; + return set_wp_destination_NED_m(destination_ned_m, is_terrain_alt); } -// Sets waypoint destination using NEU position vector in meters from EKF origin. +// Sets waypoint destination using NED position vector in meters from EKF origin. // If `is_terrain_alt` is true, altitude is interpreted as height above terrain. // Reinitializes the current leg if interrupted, updates origin, and computes trajectory. // arc_rad specifies the signed arc angle in radians for an ARC_WAYPOINT segment (0 for straight path) // Returns false if terrain offset cannot be determined when required. -bool AC_WPNav::set_wp_destination_NEU_m(const Vector3p& destination_neu_m, bool is_terrain_alt, float arc_rad) +bool AC_WPNav::set_wp_destination_NED_m(const Vector3p& destination_ned_m, bool is_terrain_alt, float arc_rad) { // re-initialise if previous destination has been interrupted if (!is_active() || !_flags.reached_destination) { @@ -331,39 +332,39 @@ bool AC_WPNav::set_wp_destination_NEU_m(const Vector3p& destination_neu_m, bool float origin_speed_m = 0.0f; // use previous destination as origin - _origin_neu_m = _destination_neu_m; + _origin_ned_m = _destination_ned_m; if (is_terrain_alt == _is_terrain_alt) { if (_this_leg_is_spline) { // Use velocity from end of spline leg to seed new S-curve origin speed - Vector3f curr_target_vel_neu_ms = _pos_control.get_vel_desired_NEU_ms(); - curr_target_vel_neu_ms.z -= _pos_control.get_vel_offset_U_ms(); - origin_speed_m = curr_target_vel_neu_ms.length(); + Vector3f curr_target_vel_ned_ms = _pos_control.get_vel_desired_NED_ms(); + curr_target_vel_ned_ms.z -= _pos_control.get_vel_offset_D_ms(); + origin_speed_m = curr_target_vel_ned_ms.length(); } else { // Preserve current leg profile to enable blending with new leg _scurve_prev_leg = _scurve_this_leg; } } else { // Handle transition between terrain-relative and origin-relative altitude frames - float terrain_u_m; - if (!get_terrain_U_m(terrain_u_m)) { + float terrain_d_m; + if (!get_terrain_D_m(terrain_d_m)) { return false; } // convert origin to alt-above-terrain if necessary if (is_terrain_alt) { // Convert origin.z to terrain-relative altitude - _origin_neu_m.z -= terrain_u_m; - _pos_control.init_pos_terrain_U_m(terrain_u_m); + _origin_ned_m.z -= terrain_d_m; + _pos_control.init_pos_terrain_D_m(terrain_d_m); } else { // Convert origin.z to origin-relative altitude - _origin_neu_m.z += terrain_u_m; - _pos_control.init_pos_terrain_U_m(0.0); + _origin_ned_m.z += terrain_d_m; + _pos_control.init_pos_terrain_D_m(0.0); } } // update destination - _destination_neu_m = destination_neu_m; + _destination_ned_m = destination_ned_m; _is_terrain_alt = is_terrain_alt; if (_flags.fast_waypoint && !_this_leg_is_spline && !_next_leg_is_spline && !_scurve_next_leg.finished()) { @@ -371,9 +372,9 @@ bool AC_WPNav::set_wp_destination_NEU_m(const Vector3p& destination_neu_m, bool _scurve_this_leg = _scurve_next_leg; } else { // Generate a new S-curve segment to the new destination - _scurve_this_leg.calculate_track(_origin_neu_m, _destination_neu_m, arc_rad, - _pos_control.get_max_speed_NE_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms(), - get_wp_acceleration_mss(), get_accel_U_mss(), get_corner_acceleration_mss(), + _scurve_this_leg.calculate_track(_origin_ned_m, _destination_ned_m, arc_rad, + _pos_control.NE_get_max_speed_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms(), + get_wp_acceleration_mss(), get_accel_D_mss(), get_corner_acceleration_mss(), _scurve_snap_max_mssss, _scurve_jerk_max_msss); if (!is_zero(origin_speed_m)) { // If we have a valid starting speed, seed it into the S-curve @@ -383,19 +384,19 @@ bool AC_WPNav::set_wp_destination_NEU_m(const Vector3p& destination_neu_m, bool _this_leg_is_spline = false; _scurve_next_leg.init(); - _next_destination_neu_m.zero(); // clear next destination_neu_m + _next_destination_ned_m.zero(); // clear next destination_ned_m _flags.fast_waypoint = false; // default waypoint back to slow _flags.reached_destination = false; return true; } -// Sets the next waypoint destination using a NEU position vector in meters. +// Sets the next waypoint destination using a NED position vector in meters. // Only updates if terrain frame matches current leg. // Calculates trajectory preview for smoother transition into next segment. // Updates velocity handoff if previous leg is a spline. // arc_rad specifies the signed arc angle in radians for an ARC_WAYPOINT segment (0 for straight path) -bool AC_WPNav::set_wp_destination_next_NEU_m(const Vector3p& destination_neu_m, bool is_terrain_alt, float arc_rad) +bool AC_WPNav::set_wp_destination_next_NED_m(const Vector3p& destination_ned_m, bool is_terrain_alt, float arc_rad) { // do not add next point if alt types don't match if (is_terrain_alt != _is_terrain_alt) { @@ -403,9 +404,9 @@ bool AC_WPNav::set_wp_destination_next_NEU_m(const Vector3p& destination_neu_m, } // Preload next S-curve leg with current speed and acceleration constraints - _scurve_next_leg.calculate_track(_destination_neu_m, destination_neu_m, arc_rad, - _pos_control.get_max_speed_NE_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms(), - get_wp_acceleration_mss(), get_accel_U_mss(), get_corner_acceleration_mss(), + _scurve_next_leg.calculate_track(_destination_ned_m, destination_ned_m, arc_rad, + _pos_control.NE_get_max_speed_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms(), + get_wp_acceleration_mss(), get_accel_D_mss(), get_corner_acceleration_mss(), _scurve_snap_max_mssss, _scurve_jerk_max_msss); if (_this_leg_is_spline) { const float this_leg_dest_speed_max_ms = _spline_this_leg.get_destination_speed_max(); @@ -419,29 +420,11 @@ bool AC_WPNav::set_wp_destination_next_NEU_m(const Vector3p& destination_neu_m, _flags.fast_waypoint = true; // Store the upcoming destination for reference - _next_destination_neu_m = destination_neu_m; + _next_destination_ned_m = destination_ned_m; return true; } -// Sets waypoint destination using a NED position vector in meters from EKF origin. -// Converts internally to NEU. Terrain following is not used. -bool AC_WPNav::set_wp_destination_NED_m(const Vector3p& destination_NED_m) -{ - // convert NED to NEU by inverting the Z axis - // terrain following is not used (altitude is relative to EKF origin) - return set_wp_destination_NEU_m(Vector3p{destination_NED_m.x, destination_NED_m.y, -destination_NED_m.z}, false); -} - -// Sets the next waypoint destination using a NED position vector in meters from EKF origin. -// Converts to NEU internally. Terrain following is not applied. -bool AC_WPNav::set_wp_destination_next_NED_m(const Vector3p& destination_NED_m) -{ - // convert NED to NEU by inverting the Z axis - // terrain following is not used (altitude is relative to EKF origin) - return set_wp_destination_next_NEU_m(Vector3p{destination_NED_m.x, destination_NED_m.y, -destination_NED_m.z}, false); -} - // Computes the horizontal stopping point in NE frame, returned in centimeters. // See get_wp_stopping_point_NE_m() for full details. void AC_WPNav::get_wp_stopping_point_NE_cm(Vector2f& stopping_point_ne_cm) const @@ -461,25 +444,25 @@ void AC_WPNav::get_wp_stopping_point_NE_m(Vector2p& stopping_point_ne_m) const } // Computes the full 3D NEU stopping point vector in centimeters based on current kinematics. -// See get_wp_stopping_point_NEU_m() for full details. +// See get_wp_stopping_point_NED_m() for full details. void AC_WPNav::get_wp_stopping_point_NEU_cm(Vector3f& stopping_point_neu_cm) const { // convert input from cm to m for internal calculation - Vector3p stopping_point_neu_m = stopping_point_neu_cm.topostype() * 0.01; + Vector3p stopping_point_ned_m = Vector3p(stopping_point_neu_cm.x, stopping_point_neu_cm.y, -stopping_point_neu_cm.z) * 0.01; // compute stopping point using meters - get_wp_stopping_point_NEU_m(stopping_point_neu_m); + get_wp_stopping_point_NED_m(stopping_point_ned_m); // convert result back to centimeters - stopping_point_neu_cm = stopping_point_neu_m.tofloat() * 100.0; + stopping_point_neu_cm = Vector3f(stopping_point_ned_m.x, stopping_point_ned_m.y, -stopping_point_ned_m.z) * 100.0; } -// Computes the full 3D NEU stopping point in meters based on current velocity and configured acceleration in all axes. +// Computes the full 3D NED stopping point in meters based on current velocity and configured acceleration in all axes. // Represents where the vehicle will stop if decelerated from current velocity using configured limits. -void AC_WPNav::get_wp_stopping_point_NEU_m(Vector3p& stopping_point_neu_m) const +void AC_WPNav::get_wp_stopping_point_NED_m(Vector3p& stopping_point_ned_m) const { // get horizontal stopping point (North and East) - _pos_control.get_stopping_point_NE_m(stopping_point_neu_m.xy()); - // get vertical stopping point (Up) - _pos_control.get_stopping_point_U_m(stopping_point_neu_m.z); + _pos_control.get_stopping_point_NE_m(stopping_point_ned_m.xy()); + // get vertical stopping point (Down) + _pos_control.get_stopping_point_D_m(stopping_point_ned_m.z); } // Advances the target location along the current path segment. @@ -488,43 +471,43 @@ void AC_WPNav::get_wp_stopping_point_NEU_m(Vector3p& stopping_point_neu_m) const bool AC_WPNav::advance_wp_target_along_track(float dt) { // calculate terrain offset if using alt-above-terrain frame - float terr_offset_u_m = 0.0f; - if (_is_terrain_alt && !get_terrain_U_m(terr_offset_u_m)) { + float terr_offset_d_m = 0.0f; + if (_is_terrain_alt && !get_terrain_D_m(terr_offset_d_m)) { return false; } // calculate terrain-based velocity scaling factor - const float offset_u_scalar = _pos_control.pos_terrain_U_scaler_m(terr_offset_u_m, get_terrain_margin_m()); + const float offset_d_scalar = _pos_control.terrain_scaler_D_m(terr_offset_d_m, get_terrain_margin_m()); // input shape the terrain offset - _pos_control.set_pos_terrain_target_U_m(terr_offset_u_m); + _pos_control.set_pos_terrain_target_D_m(terr_offset_d_m); // get position controller's post-shaped position offset for use in position error computation - const Vector3p& psc_pos_offset_neu_m = _pos_control.get_pos_offset_NEU_m(); + const Vector3p& psc_pos_offset_ned_m = _pos_control.get_pos_offset_NED_m(); - // compute current position in NEU frame, adjusted to destination frame (e.g., terrain-relative if needed) - Vector3p curr_pos_neu_m = _pos_control.get_pos_estimate_NEU_m() - psc_pos_offset_neu_m; - curr_pos_neu_m.z -= terr_offset_u_m; + // compute current position in NED frame, adjusted to destination frame (e.g., terrain-relative if needed) + Vector3p curr_pos_ned_m = _pos_control.get_pos_estimate_NED_m() - psc_pos_offset_ned_m; + curr_pos_ned_m.z -= terr_offset_d_m; // get desired velocity and remove offset - Vector3f curr_target_vel_neu_ms = _pos_control.get_vel_desired_NEU_ms(); - curr_target_vel_neu_ms.z -= _pos_control.get_vel_offset_U_ms(); + Vector3f curr_target_vel_ned_ms = _pos_control.get_vel_desired_NED_ms(); + curr_target_vel_ned_ms.z -= _pos_control.get_vel_offset_D_ms(); // scale progression time (track_dt_scalar) based on aircraft’s speed alignment with desired path float track_dt_scalar = 1.0f; - if (is_positive(curr_target_vel_neu_ms.length_squared())) { - Vector3f track_direction_neu = curr_target_vel_neu_ms.normalized(); - const float track_error_neu_m = _pos_control.get_pos_error_NEU_m().dot(track_direction_neu); - const float track_velocity_neu_ms = _pos_control.get_vel_estimate_NEU_ms().dot(track_direction_neu); + if (is_positive(curr_target_vel_ned_ms.length_squared())) { + Vector3f track_direction_neu = curr_target_vel_ned_ms.normalized(); + const float track_error_ned_m = _pos_control.get_pos_error_NED_m().dot(track_direction_neu); + const float track_velocity_ned_ms = _pos_control.get_vel_estimate_NED_ms().dot(track_direction_neu); // limit time step scalar to [0,1], with 5% buffer - track_dt_scalar = constrain_float(0.05f + (track_velocity_neu_ms - _pos_control.get_pos_NE_p().kP() * track_error_neu_m) / curr_target_vel_neu_ms.length(), 0.0f, 1.0f); + track_dt_scalar = constrain_float(0.05f + (track_velocity_ned_ms - _pos_control.NE_get_pos_p().kP() * track_error_ned_m) / curr_target_vel_ned_ms.length(), 0.0f, 1.0f); } // compute velocity scaling (vel_dt_scalar) and apply jerk-limited velocity shaping float vel_dt_scalar = 1.0; if (is_positive(_wp_desired_speed_ne_ms)) { update_vel_accel(_offset_vel_ms, _offset_accel_mss, dt, 0.0, 0.0); - const float vel_input_ms = !_paused ? _wp_desired_speed_ne_ms * offset_u_scalar : 0.0; + const float vel_input_ms = !_paused ? _wp_desired_speed_ne_ms * offset_d_scalar : 0.0; shape_vel_accel(vel_input_ms, 0.0, _offset_vel_ms, _offset_accel_mss, -get_wp_acceleration_mss(), get_wp_acceleration_mss(), _pos_control.get_shaping_jerk_NE_msss(), dt, true); vel_dt_scalar = _offset_vel_ms / _wp_desired_speed_ne_ms; @@ -538,33 +521,33 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) _track_dt_scalar += (track_dt_scalar - _track_dt_scalar) * (dt / track_dt_scalar_tc); // select path generator (S-curve or spline) and compute target state - Vector3p target_pos_neu_m; - Vector3f target_vel_neu_ms, target_accel_neu_mss; + Vector3p target_pos_ned_m; + Vector3f target_vel_ned_ms, target_accel_ned_mss; bool s_finished; if (!_this_leg_is_spline) { // update target position, velocity and acceleration - target_pos_neu_m = _origin_neu_m; - s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, _wp_radius_cm * 0.01, get_corner_acceleration_mss(), _flags.fast_waypoint, _track_dt_scalar * vel_dt_scalar * dt, target_pos_neu_m, target_vel_neu_ms, target_accel_neu_mss); + target_pos_ned_m = _origin_ned_m; + s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, _wp_radius_cm * 0.01, get_corner_acceleration_mss(), _flags.fast_waypoint, _track_dt_scalar * vel_dt_scalar * dt, target_pos_ned_m, target_vel_ned_ms, target_accel_ned_mss); } else { // splinetarget_vel - target_vel_neu_ms = curr_target_vel_neu_ms; - _spline_this_leg.advance_target_along_track(_track_dt_scalar * vel_dt_scalar * dt, target_pos_neu_m, target_vel_neu_ms); + target_vel_ned_ms = curr_target_vel_ned_ms; + _spline_this_leg.advance_target_along_track(_track_dt_scalar * vel_dt_scalar * dt, target_pos_ned_m, target_vel_ned_ms); s_finished = _spline_this_leg.reached_destination(); } // apply scaled acceleration offset along current direction of travel - Vector3f accel_offset_neu_mss; - if (is_positive(target_vel_neu_ms.length_squared())) { - Vector3f track_direction_neu = target_vel_neu_ms.normalized(); - accel_offset_neu_mss = track_direction_neu * _offset_accel_mss * target_vel_neu_ms.length() / _wp_desired_speed_ne_ms; + Vector3f accel_offset_ned_mss; + if (is_positive(target_vel_ned_ms.length_squared())) { + Vector3f track_direction_neu = target_vel_ned_ms.normalized(); + accel_offset_ned_mss = track_direction_neu * _offset_accel_mss * target_vel_ned_ms.length() / _wp_desired_speed_ne_ms; } - target_vel_neu_ms *= vel_dt_scalar; - target_accel_neu_mss *= sq(vel_dt_scalar); - target_accel_neu_mss += accel_offset_neu_mss; + target_vel_ned_ms *= vel_dt_scalar; + target_accel_ned_mss *= sq(vel_dt_scalar); + target_accel_ned_mss += accel_offset_ned_mss; // send updated position, velocity, and acceleration targets to position controller - _pos_control.set_pos_vel_accel_NEU_m(target_pos_neu_m, target_vel_neu_ms, target_accel_neu_mss); + _pos_control.set_pos_vel_accel_NED_m(target_pos_ned_m, target_vel_ned_ms, target_accel_ned_mss); // check if waypoint has been reached based on mode and radius if (!_flags.reached_destination) { @@ -574,7 +557,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) _flags.reached_destination = true; } else { // regular waypoints also require the copter to be within the waypoint radius - const Vector3f dist_to_dest_m = (curr_pos_neu_m - _destination_neu_m).tofloat(); + const Vector3f dist_to_dest_m = (curr_pos_ned_m - _destination_ned_m).tofloat(); if (dist_to_dest_m.length_squared() <= sq(_wp_radius_cm * 0.01)) { _flags.reached_destination = true; } @@ -592,18 +575,18 @@ void AC_WPNav::update_track_with_speed_accel_limits() { // update speed and acceleration limits for the current segment if (_this_leg_is_spline) { - _spline_this_leg.set_speed_accel(_pos_control.get_max_speed_NE_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms(), - get_wp_acceleration_mss(), get_accel_U_mss()); + _spline_this_leg.set_speed_accel(_pos_control.NE_get_max_speed_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms(), + get_wp_acceleration_mss(), get_accel_D_mss()); } else { - _scurve_this_leg.set_speed_max(_pos_control.get_max_speed_NE_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms()); + _scurve_this_leg.set_speed_max(_pos_control.NE_get_max_speed_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms()); } // update speed and acceleration limits for the next segment if (_next_leg_is_spline) { - _spline_next_leg.set_speed_accel(_pos_control.get_max_speed_NE_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms(), - get_wp_acceleration_mss(), get_accel_U_mss()); + _spline_next_leg.set_speed_accel(_pos_control.NE_get_max_speed_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms(), + get_wp_acceleration_mss(), get_accel_D_mss()); } else { - _scurve_next_leg.set_speed_max(_pos_control.get_max_speed_NE_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms()); + _scurve_next_leg.set_speed_max(_pos_control.NE_get_max_speed_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms()); } } @@ -619,7 +602,7 @@ float AC_WPNav::get_wp_distance_to_destination_cm() const float AC_WPNav::get_wp_distance_to_destination_m() const { // calculate 2D ground distance between current position and destination in NE frame - return get_horizontal_distance(_pos_control.get_pos_estimate_NEU_m().xy().tofloat(), _destination_neu_m.xy().tofloat()); + return get_horizontal_distance(_pos_control.get_pos_estimate_NED_m().xy().tofloat(), _destination_ned_m.xy().tofloat()); } // Returns the bearing to the current waypoint destination in centidegrees. @@ -627,7 +610,7 @@ float AC_WPNav::get_wp_distance_to_destination_m() const int32_t AC_WPNav::get_wp_bearing_to_destination_cd() const { // compute heading from current position to destination in centidegrees - return get_bearing_cd(_pos_control.get_pos_estimate_NEU_m().xy().tofloat(), _destination_neu_m.xy().tofloat()); + return get_bearing_cd(_pos_control.get_pos_estimate_NED_m().xy().tofloat(), _destination_ned_m.xy().tofloat()); } // Returns the bearing to the current waypoint destination in radians. @@ -635,7 +618,7 @@ int32_t AC_WPNav::get_wp_bearing_to_destination_cd() const float AC_WPNav::get_wp_bearing_to_destination_rad() const { // compute heading from current position to destination in radians - return get_bearing_rad(_pos_control.get_pos_estimate_NEU_m().xy().tofloat(), _destination_neu_m.xy().tofloat()); + return get_bearing_rad(_pos_control.get_pos_estimate_NED_m().xy().tofloat(), _destination_ned_m.xy().tofloat()); } // Runs the waypoint navigation controller. @@ -669,7 +652,7 @@ bool AC_WPNav::update_wpnav() } // run the horizontal position controller - _pos_control.update_NE_controller(); + _pos_control.NE_update_controller(); // record update time for is_active() _wp_last_update_ms = AP_HAL::millis(); @@ -744,62 +727,70 @@ bool AC_WPNav::get_terrain_U_m(float& terrain_u_m) // unreachable fallback path return false; } +bool AC_WPNav::get_terrain_D_m(float& terrain_d_m) +{ + if (!get_terrain_U_m(terrain_d_m)) { + return false; + } + terrain_d_m *= -1.0; + return true; +} /// /// spline methods /// // Sets the current spline waypoint using global coordinates. -// Converts `destination` and `next_destination` to NEU position vectors and sets up a spline between them. +// Converts `destination` and `next_destination` to NED position vectors and sets up a spline between them. // Returns false if conversion from location to vector fails. bool AC_WPNav::set_spline_destination_loc(const Location& destination, const Location& next_destination, bool next_is_spline) { - // convert destination location to NEU vector and altitude frame - Vector3p dest_neu_m; + // convert destination location to NED vector and altitude frame + Vector3p dest_ned_m; bool dest_is_terrain_alt; - if (!get_vector_NEU_m(destination, dest_neu_m, dest_is_terrain_alt)) { + if (!get_vector_NED_m(destination, dest_ned_m, dest_is_terrain_alt)) { return false; } - // convert next destination location to NEU vector and altitude frame - Vector3p next_dest_neu_m; + // convert next destination location to NED vector and altitude frame + Vector3p next_dest_ned_m; bool next_dest_is_terrain_alt; - if (!get_vector_NEU_m(next_destination, next_dest_neu_m, next_dest_is_terrain_alt)) { + if (!get_vector_NED_m(next_destination, next_dest_ned_m, next_dest_is_terrain_alt)) { return false; } // initialize spline path between converted destination and next_destination - return set_spline_destination_NEU_m(dest_neu_m, dest_is_terrain_alt, next_dest_neu_m, next_dest_is_terrain_alt, next_is_spline); + return set_spline_destination_NED_m(dest_ned_m, dest_is_terrain_alt, next_dest_ned_m, next_dest_is_terrain_alt, next_is_spline); } // Sets the next spline segment using global coordinates. -// Converts the next and next-next destinations to NEU position vectors and initializes the spline transition. +// Converts the next and next-next destinations to NED position vectors and initializes the spline transition. // Returns false if any conversion from location to vector fails. bool AC_WPNav::set_spline_destination_next_loc(const Location& next_destination, const Location& next_next_destination, bool next_next_is_spline) { - // convert next_destination location to NEU vector and determine if it uses terrain-relative altitude - Vector3p next_dest_neu_m; + // convert next_destination location to NED vector and determine if it uses terrain-relative altitude + Vector3p next_dest_ned_m; bool next_dest_is_terrain_alt; - if (!get_vector_NEU_m(next_destination, next_dest_neu_m, next_dest_is_terrain_alt)) { + if (!get_vector_NED_m(next_destination, next_dest_ned_m, next_dest_is_terrain_alt)) { return false; } - // convert next_next_destination location to NEU vector and determine if it uses terrain-relative altitude - Vector3p next_next_dest_neu_m; + // convert next_next_destination location to NED vector and determine if it uses terrain-relative altitude + Vector3p next_next_dest_ned_m; bool next_next_dest_is_terr_alt; - if (!get_vector_NEU_m(next_next_destination, next_next_dest_neu_m, next_next_dest_is_terr_alt)) { + if (!get_vector_NED_m(next_next_destination, next_next_dest_ned_m, next_next_dest_is_terr_alt)) { return false; } // initialize next spline segment using converted waypoints - return set_spline_destination_next_NEU_m(next_dest_neu_m, next_dest_is_terrain_alt, next_next_dest_neu_m, next_next_dest_is_terr_alt, next_next_is_spline); + return set_spline_destination_next_NED_m(next_dest_ned_m, next_dest_is_terrain_alt, next_next_dest_ned_m, next_next_dest_is_terr_alt, next_next_is_spline); } -// Sets the current spline waypoint using NEU position vectors in meters. -// Initializes a spline path from `destination_neu_m` to `next_destination_neu_m`, respecting terrain altitude framing. +// Sets the current spline waypoint using NED position vectors in meters. +// Initializes a spline path from `destination_ned_m` to `next_destination_ned_m`, respecting terrain altitude framing. // Both waypoints must use the same altitude frame (either above terrain or above origin). // Returns false if terrain altitude cannot be determined when required. -bool AC_WPNav::set_spline_destination_NEU_m(const Vector3p& destination_neu_m, bool is_terrain_alt, const Vector3p& next_destination_neu_m, bool next_is_terrain_alt, bool next_is_spline) +bool AC_WPNav::set_spline_destination_NED_m(const Vector3p& destination_ned_m, bool is_terrain_alt, const Vector3p& next_destination_ned_m, bool next_is_terrain_alt, bool next_is_spline) { // re-initialise path state if previous destination was not completed or controller inactive if (!is_active() || !_flags.reached_destination) { @@ -807,78 +798,78 @@ bool AC_WPNav::set_spline_destination_NEU_m(const Vector3p& destination_neu_m, b } // update spline calculators speeds and accelerations - _spline_this_leg.set_speed_accel(_pos_control.get_max_speed_NE_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms(), - _pos_control.get_max_accel_NE_mss(), _pos_control.get_max_accel_U_mss()); + _spline_this_leg.set_speed_accel(_pos_control.NE_get_max_speed_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms(), + _pos_control.NE_get_max_accel_mss(), _pos_control.D_get_max_accel_mss()); // calculate origin and origin velocity vector - Vector3f origin_vector_neu_m; + Vector3f origin_vector_ned_m; if (is_terrain_alt == _is_terrain_alt) { if (_flags.fast_waypoint) { // calculate origin vector if (_this_leg_is_spline) { // use velocity vector from end of previous spline segment - origin_vector_neu_m = _spline_this_leg.get_destination_vel(); + origin_vector_ned_m = _spline_this_leg.get_destination_vel(); } else { // use direction vector from previous straight-line segment - origin_vector_neu_m = (_destination_neu_m - _origin_neu_m).tofloat(); + origin_vector_ned_m = (_destination_ned_m - _origin_ned_m).tofloat(); } } // use previous destination as origin - _origin_neu_m = _destination_neu_m; + _origin_ned_m = _destination_ned_m; } else { // use previous destination as origin - _origin_neu_m = _destination_neu_m; + _origin_ned_m = _destination_ned_m; // get current alt above terrain - float terrain_u_m; - if (!get_terrain_U_m(terrain_u_m)) { + float terrain_d_m; + if (!get_terrain_D_m(terrain_d_m)) { return false; } // convert origin to alt-above-terrain if necessary if (is_terrain_alt) { // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin - _origin_neu_m.z -= terrain_u_m; - _pos_control.init_pos_terrain_U_m(terrain_u_m); + _origin_ned_m.z -= terrain_d_m; + _pos_control.init_pos_terrain_D_m(terrain_d_m); } else { // new destination is alt-above-ekf-origin, previous destination was alt-above-terrain - _origin_neu_m.z += terrain_u_m; - _pos_control.init_pos_terrain_U_m(0.0); + _origin_ned_m.z += terrain_d_m; + _pos_control.init_pos_terrain_D_m(0.0); } } // store destination location - _destination_neu_m = destination_neu_m; + _destination_ned_m = destination_ned_m; _is_terrain_alt = is_terrain_alt; // calculate destination velocity vector - Vector3f destination_vector_neu_m; + Vector3f destination_vector_ned_m; if (is_terrain_alt == next_is_terrain_alt) { if (next_is_spline) { // aim to leave segment in direction of full arc (origin to next) - destination_vector_neu_m = (next_destination_neu_m - _origin_neu_m).tofloat(); + destination_vector_ned_m = (next_destination_ned_m - _origin_ned_m).tofloat(); } else { // aim to leave segment in direction of following leg - destination_vector_neu_m = (next_destination_neu_m - _destination_neu_m).tofloat(); + destination_vector_ned_m = (next_destination_ned_m - _destination_ned_m).tofloat(); } } - _flags.fast_waypoint = !destination_vector_neu_m.is_zero(); + _flags.fast_waypoint = !destination_vector_ned_m.is_zero(); // setup spline leg using origin and destination vectors - _spline_this_leg.set_origin_and_destination(_origin_neu_m, _destination_neu_m, origin_vector_neu_m, destination_vector_neu_m); + _spline_this_leg.set_origin_and_destination(_origin_ned_m, _destination_ned_m, origin_vector_ned_m, destination_vector_ned_m); _this_leg_is_spline = true; _flags.reached_destination = false; return true; } -// Sets the next spline segment using NEU position vectors in meters. -// Creates a spline path from the current destination to `next_destination_neu_m`, and prepares transition toward `next_next_destination_neu_m`. +// Sets the next spline segment using NED position vectors in meters. +// Creates a spline path from the current destination to `next_destination_ned_m`, and prepares transition toward `next_next_destination_ned_m`. // All waypoints must use the same altitude frame (above terrain or origin). // Returns false if terrain data is missing and required. -bool AC_WPNav::set_spline_destination_next_NEU_m(const Vector3p& next_destination_neu_m, bool next_is_terrain_alt, const Vector3p& next_next_destination_neu_m, bool next_next_is_terrain_alt, bool next_next_is_spline) +bool AC_WPNav::set_spline_destination_next_NED_m(const Vector3p& next_destination_ned_m, bool next_is_terrain_alt, const Vector3p& next_next_destination_ned_m, bool next_next_is_terrain_alt, bool next_next_is_spline) { // do not add next point if alt types don't match if (next_is_terrain_alt != _is_terrain_alt) { @@ -886,33 +877,33 @@ bool AC_WPNav::set_spline_destination_next_NEU_m(const Vector3p& next_destinatio } // calculate origin and origin velocity vector - Vector3f origin_vector_neu_m; + Vector3f origin_vector_ned_m; if (_this_leg_is_spline) { // use final velocity vector from current spline segment - origin_vector_neu_m = _spline_this_leg.get_destination_vel(); + origin_vector_ned_m = _spline_this_leg.get_destination_vel(); } else { // use vector from previous origin to current destination - origin_vector_neu_m = (_destination_neu_m - _origin_neu_m).tofloat(); + origin_vector_ned_m = (_destination_ned_m - _origin_ned_m).tofloat(); } // calculate destination velocity vector - Vector3f destination_vector_neu_m; + Vector3f destination_vector_ned_m; if (next_is_terrain_alt == next_next_is_terrain_alt) { if (next_next_is_spline) { // aim to leave segment in direction of the arc from current to next-next - destination_vector_neu_m = (next_next_destination_neu_m - _destination_neu_m).tofloat(); + destination_vector_ned_m = (next_next_destination_ned_m - _destination_ned_m).tofloat(); } else { // aim to leave segment in direction of the upcoming straight leg - destination_vector_neu_m = (next_next_destination_neu_m - next_destination_neu_m).tofloat(); + destination_vector_ned_m = (next_next_destination_ned_m - next_destination_ned_m).tofloat(); } } // update spline calculators speeds and accelerations - _spline_next_leg.set_speed_accel(_pos_control.get_max_speed_NE_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms(), - _pos_control.get_max_accel_NE_mss(), _pos_control.get_max_accel_U_mss()); + _spline_next_leg.set_speed_accel(_pos_control.NE_get_max_speed_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms(), + _pos_control.NE_get_max_accel_mss(), _pos_control.D_get_max_accel_mss()); // setup next spline leg. Note this could be made local - _spline_next_leg.set_origin_and_destination(_destination_neu_m, next_destination_neu_m, origin_vector_neu_m, destination_vector_neu_m); + _spline_next_leg.set_origin_and_destination(_destination_ned_m, next_destination_ned_m, origin_vector_ned_m, destination_vector_ned_m); _next_leg_is_spline = true; // next destination provided so fast waypoint @@ -928,14 +919,14 @@ bool AC_WPNav::set_spline_destination_next_NEU_m(const Vector3p& next_destinatio return true; } -// Converts a Location to a NEU position vector in meters from the EKF origin. +// Converts a Location to a NED position vector in meters from the EKF origin. // Sets `is_terrain_alt` to true if the resulting Z position is relative to terrain. // Returns false if terrain data is unavailable or conversion fails. -bool AC_WPNav::get_vector_NEU_m(const Location &loc, Vector3p &pos_from_origin_neu_m, bool &is_terrain_alt) +bool AC_WPNav::get_vector_NED_m(const Location &loc, Vector3p &pos_from_origin_ned_m, bool &is_terrain_alt) { // convert horizontal position (latitude/longitude) to NE vector from EKF origin - Vector2p loc_pos_from_origin_neu_m; - if (!loc.get_vector_xy_from_origin_NE_m(loc_pos_from_origin_neu_m)) { + Vector2p loc_pos_from_origin_ned_m; + if (!loc.get_vector_xy_from_origin_NE_m(loc_pos_from_origin_ned_m)) { return false; } @@ -945,7 +936,7 @@ bool AC_WPNav::get_vector_NEU_m(const Location &loc, Vector3p &pos_from_origin_n if (!loc.get_alt_m(Location::AltFrame::ABOVE_TERRAIN, terrain_u_m)) { return false; } - pos_from_origin_neu_m.z = terrain_u_m; + pos_from_origin_ned_m.z = -terrain_u_m; is_terrain_alt = true; } else { is_terrain_alt = false; @@ -953,13 +944,12 @@ bool AC_WPNav::get_vector_NEU_m(const Location &loc, Vector3p &pos_from_origin_n if (!loc.get_alt_m(Location::AltFrame::ABOVE_ORIGIN, origin_alt_m)) { return false; } - pos_from_origin_neu_m.z = origin_alt_m; + pos_from_origin_ned_m.z = -origin_alt_m; is_terrain_alt = false; } - // set horizontal components (x/y) of NEU vector after successful conversion - pos_from_origin_neu_m.x = loc_pos_from_origin_neu_m.x; - pos_from_origin_neu_m.y = loc_pos_from_origin_neu_m.y; + // set horizontal components (x/y) of NED vector after successful conversion + pos_from_origin_ned_m.xy() = loc_pos_from_origin_ned_m; return true; } diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 02f5da72c1d82..be2ab169343d7 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -48,15 +48,16 @@ class AC_WPNav // Positive values mean terrain lies above the EKF origin altitude. // Source may be rangefinder or terrain database depending on availability. bool get_terrain_U_m(float& terrain_u_m); + bool get_terrain_D_m(float& terrain_d_m); // Returns the terrain following altitude margin in meters. // Vehicle will stop if distance from target altitude exceeds this margin. float get_terrain_margin_m() const { return MAX(_terrain_margin_m, 0.1); } - // Converts a Location to a NEU position vector in meters from the EKF origin. + // Converts a Location to a NED position vector in meters from the EKF origin. // Sets `is_terrain_alt` to true if the resulting Z position is relative to terrain. // Returns false if terrain data is unavailable or conversion fails. - bool get_vector_NEU_m(const Location &loc, Vector3p &pos_from_origin_NEU_m, bool &is_terrain_alt); + bool get_vector_NED_m(const Location &loc, Vector3p &pos_from_origin_ned_m, bool &is_terrain_alt); /// /// waypoint controller @@ -65,7 +66,7 @@ class AC_WPNav // Initializes waypoint and spline navigation using inputs in meters. // Sets speed and acceleration limits, calculates jerk constraints, // and initializes spline or S-curve leg with a defined starting point. - void wp_and_spline_init_m(float speed_ms = 0.0f, Vector3p stopping_point_neu_m = Vector3p{}); + void wp_and_spline_init_m(float speed_ms = 0.0f, Vector3p stopping_point_ned_m = Vector3p{}); // Sets the target horizontal speed in cm/s during waypoint navigation. // See set_speed_NE_ms() for full details. @@ -117,12 +118,12 @@ class AC_WPNav float get_default_speed_down_ms() const { return fabsf(_wp_speed_down_cms * 0.01); } // Returns the vertical acceleration in cm/s² used during waypoint navigation. - // Always positive. See get_accel_U_mss() for full details. - float get_accel_U_cmss() const { return get_accel_U_mss() * 100.0; } + // Always positive. See get_accel_D_mss() for full details. + float get_accel_D_cmss() const { return get_accel_D_mss() * 100.0; } // Returns the vertical acceleration in m/s² used during waypoint navigation. // Derived from the WPNAV_ACCEL_Z parameter. Always positive. - float get_accel_U_mss() const { return _wp_accel_z_cmss * 0.01; } + float get_accel_D_mss() const { return _wp_accel_z_cmss * 0.01; } // Returns the horizontal acceleration in cm/s² used during waypoint navigation. // See get_wp_acceleration_mss() for full details. @@ -137,33 +138,33 @@ class AC_WPNav float get_corner_acceleration_mss() const { return (is_positive(_wp_accel_c_cmss)) ? _wp_accel_c_cmss * 0.01 : 2.0 * get_wp_acceleration_mss(); } // Returns the destination waypoint vector in NEU frame, in centimeters from EKF origin. - // See get_wp_destination_NEU_m() for full details. - const Vector3f get_wp_destination_NEU_cm() const { return get_wp_destination_NEU_m().tofloat() * 100.0; } + // See get_wp_destination_NED_m() for full details. + const Vector3f get_wp_destination_NEU_cm() const { return Vector3f(_destination_ned_m.x, _destination_ned_m.y, -_destination_ned_m.z) * 100.0; } - // Returns the destination waypoint vector in NEU frame, in meters from EKF origin. + // Returns the destination waypoint vector in NED frame, in meters from EKF origin. // Z is relative to terrain or EKF origin, depending on _is_terrain_alt. - const Vector3p &get_wp_destination_NEU_m() const { return _destination_neu_m; } + const Vector3p &get_wp_destination_NED_m() const { return _destination_ned_m; } - // Returns the origin waypoint vector in NEU frame, in centimeters from EKF origin. - // See get_wp_origin_NEU_m() for full details. - const Vector3f get_wp_origin_NEU_cm() const { return get_wp_origin_NEU_m().tofloat() * 100.0; } + // Returns the origin waypoint vector in NED frame, in centimeters from EKF origin. + // See get_wp_origin_NED_m() for full details. + const Vector3f get_wp_origin_NEU_cm() const { return Vector3f(_origin_ned_m.x, _origin_ned_m.y, -_origin_ned_m.z) * 100.0; } - // Returns the origin waypoint vector in NEU frame, in meters from EKF origin. + // Returns the origin waypoint vector in NED frame, in meters from EKF origin. // This marks the start of the current waypoint leg. - const Vector3p &get_wp_origin_NEU_m() const { return _origin_neu_m; } + const Vector3p &get_wp_origin_NED_m() const { return _origin_ned_m; } // Returns true if origin.z and destination.z are specified as altitudes above terrain. // Returns false if altitudes are relative to the EKF origin. bool origin_and_destination_are_terrain_alt() const { return _is_terrain_alt; } // Sets the current waypoint destination using a Location object. - // Converts global coordinates to NEU position and sets destination. + // Converts global coordinates to NED position and sets destination. // arc_rad specifies the signed arc angle in radians for an ARC_WAYPOINT segment (0 for straight path) // Returns false if conversion fails (e.g. missing terrain data). bool set_wp_destination_loc(const Location& destination, float arc_rad = 0.0); // Sets the next waypoint destination using a Location object. - // Converts global coordinates to NEU position and preloads the trajectory. + // Converts global coordinates to NED position and preloads the trajectory. // arc_rad specifies the signed arc angle in radians for an ARC_WAYPOINT segment (0 for straight path) // Returns false if conversion fails or terrain data is unavailable. bool set_wp_destination_next_loc(const Location& destination, float arc_rad = 0.0); @@ -178,31 +179,23 @@ class AC_WPNav // Used to unify the AC_WPNav and AC_WPNav_OA interfaces. virtual bool get_oa_wp_destination(Location& destination) const { return get_wp_destination_loc(destination); } - // Sets waypoint destination using NEU position vector in centimeters from EKF origin. - // See set_wp_destination_NEU_m() for full details. + // Sets waypoint destination using NED position vector in centimeters from EKF origin. + // See set_wp_destination_NED_m() for full details. virtual bool set_wp_destination_NEU_cm(const Vector3f& destination_neu_cm, bool is_terrain_alt = false); - // Sets waypoint destination using NEU position vector in meters from EKF origin. + // Sets waypoint destination using NED position vector in meters from EKF origin. // If `is_terrain_alt` is true, altitude is interpreted as height above terrain. // Reinitializes the current leg if interrupted, updates origin, and computes trajectory. // arc_rad specifies the signed arc angle in radians for an ARC_WAYPOINT segment (0 for straight path) // Returns false if terrain offset cannot be determined when required. - virtual bool set_wp_destination_NEU_m(const Vector3p& destination_neu_m, bool is_terrain_alt = false, float arc_rad = 0.0); + virtual bool set_wp_destination_NED_m(const Vector3p& destination_ned_m, bool is_terrain_alt = false, float arc_rad = 0.0); - // Sets the next waypoint destination using a NEU position vector in meters. + // Sets the next waypoint destination using a NED position vector in meters. // Only updates if terrain frame matches current leg. // Calculates trajectory preview for smoother transition into next segment. // Updates velocity handoff if previous leg is a spline. // arc_rad specifies the signed arc angle in radians for an ARC_WAYPOINT segment (0 for straight path) - bool set_wp_destination_next_NEU_m(const Vector3p& destination_neu_m, bool is_terrain_alt = false, float arc_rad = 0.0); - - // Sets waypoint destination using a NED position vector in meters from EKF origin. - // Converts internally to NEU. Terrain following is not used. - bool set_wp_destination_NED_m(const Vector3p& destination_NED_m); - - // Sets the next waypoint destination using a NED position vector in meters from EKF origin. - // Converts to NEU internally. Terrain following is not applied. - bool set_wp_destination_next_NED_m(const Vector3p& destination_NED_m); + bool set_wp_destination_next_NED_m(const Vector3p& destination_ned_m, bool is_terrain_alt = false, float arc_rad = 0.0); // Computes the horizontal stopping point in NE frame, returned in centimeters. // See get_wp_stopping_point_NE_m() for full details. @@ -212,13 +205,13 @@ class AC_WPNav // This is the point where the vehicle would come to a stop if decelerated using the configured limits. void get_wp_stopping_point_NE_m(Vector2p& stopping_point_ne_m) const; - // Computes the full 3D NEU stopping point vector in centimeters based on current kinematics. - // See get_wp_stopping_point_NEU_m() for full details. + // Computes the full 3D NED stopping point vector in centimeters based on current kinematics. + // See get_wp_stopping_point_NED_m() for full details. void get_wp_stopping_point_NEU_cm(Vector3f& stopping_point_neu_cm) const; - // Computes the full 3D NEU stopping point in meters based on current velocity and configured acceleration in all axes. + // Computes the full 3D NED stopping point in meters based on current velocity and configured acceleration in all axes. // Represents where the vehicle will stop if decelerated from current velocity using configured limits. - void get_wp_stopping_point_NEU_m(Vector3p& stopping_point_neu_m) const; + void get_wp_stopping_point_NED_m(Vector3p& stopping_point_ned_m) const; // Returns the horizontal distance to the destination waypoint in centimeters. // See get_wp_distance_to_destination_m() for full details. @@ -269,26 +262,26 @@ class AC_WPNav /// // Sets the current spline waypoint using global coordinates. - // Converts `destination` and `next_destination` to NEU position vectors and sets up a spline between them. + // Converts `destination` and `next_destination` to NED position vectors and sets up a spline between them. // Returns false if conversion from location to vector fails. bool set_spline_destination_loc(const Location& destination, const Location& next_destination, bool next_is_spline); // Sets the next spline segment using global coordinates. - // Converts the next and next-next destinations to NEU position vectors and initializes the spline transition. + // Converts the next and next-next destinations to NED position vectors and initializes the spline transition. // Returns false if any conversion from location to vector fails. bool set_spline_destination_next_loc(const Location& next_destination, const Location& next_next_destination, bool next_next_is_spline); - // Sets the current spline waypoint using NEU position vectors in meters. - // Initializes a spline path from `destination_neu_m` to `next_destination_neu_m`, respecting terrain altitude framing. + // Sets the current spline waypoint using NED position vectors in meters. + // Initializes a spline path from `destination_ned_m` to `next_destination_ned_m`, respecting terrain altitude framing. // Both waypoints must use the same altitude frame (either above terrain or above origin). // Returns false if terrain altitude cannot be determined when required. - bool set_spline_destination_NEU_m(const Vector3p& destination_neu_m, bool is_terrain_alt, const Vector3p& next_destination_neu_m, bool next_terrain_alt, bool next_is_spline); + bool set_spline_destination_NED_m(const Vector3p& destination_ned_m, bool is_terrain_alt, const Vector3p& next_destination_ned_m, bool next_terrain_alt, bool next_is_spline); - // Sets the next spline segment using NEU position vectors in meters. - // Creates a spline path from the current destination to `next_destination_neu_m`, and prepares transition toward `next_next_destination_neu_m`. + // Sets the next spline segment using NED position vectors in meters. + // Creates a spline path from the current destination to `next_destination_ned_m`, and prepares transition toward `next_next_destination_ned_m`. // All waypoints must use the same altitude frame (above terrain or origin). // Returns false if terrain data is missing and required. - bool set_spline_destination_next_NEU_m(const Vector3p& next_destination_neu_m, bool next_is_terrain_alt, const Vector3p& next_next_destination_neu_m, bool next_next_is_terrain_alt, bool next_next_is_spline); + bool set_spline_destination_next_NED_m(const Vector3p& next_destination_ned_m, bool next_is_terrain_alt, const Vector3p& next_next_destination_ned_m, bool next_next_is_terrain_alt, bool next_next_is_spline); /// /// shared methods @@ -389,15 +382,15 @@ class AC_WPNav bool _next_leg_is_spline; // true if the next leg will use spline trajectory // waypoint navigation state - uint32_t _wp_last_update_ms; // timestamp of the last update_wpnav() call (milliseconds) - float _wp_desired_speed_ne_ms; // desired horizontal speed in m/s for the current segment - Vector3p _origin_neu_m; // origin of the current leg in meters (NEU frame) - Vector3p _destination_neu_m; // destination of the current leg in meters (NEU frame) - Vector3p _next_destination_neu_m; // destination of the next leg in meters (NEU frame) - float _track_dt_scalar; // scalar to reduce or increase the advancement along the track (0.0–1.0) - float _offset_vel_ms; // filtered horizontal speed target (used for terrain following or pause handling) - float _offset_accel_mss; // filtered horizontal acceleration target (used for terrain following or pause handling) - bool _paused; // true if waypoint controller is paused + uint32_t _wp_last_update_ms; // timestamp of the last update_wpnav() call (milliseconds) + float _wp_desired_speed_ne_ms; // desired horizontal speed in m/s for the current segment + Vector3p _origin_ned_m; // origin of the current leg in meters (NED frame) + Vector3p _destination_ned_m; // destination of the current leg in meters (NED frame) + Vector3p _next_destination_ned_m; // destination of the next leg in meters (NED frame) + float _track_dt_scalar; // scalar to reduce or increase the advancement along the track (0.0–1.0) + float _offset_vel_ms; // filtered horizontal speed target (used for terrain following or pause handling) + float _offset_accel_mss; // filtered horizontal acceleration target (used for terrain following or pause handling) + bool _paused; // true if waypoint controller is paused // terrain following state bool _is_terrain_alt; // true if altitude values are relative to terrain height, false if relative to EKF origin diff --git a/libraries/AC_WPNav/AC_WPNav_OA.cpp b/libraries/AC_WPNav/AC_WPNav_OA.cpp index fab29c4ffc284..0098eb55ab617 100644 --- a/libraries/AC_WPNav/AC_WPNav_OA.cpp +++ b/libraries/AC_WPNav/AC_WPNav_OA.cpp @@ -26,21 +26,22 @@ bool AC_WPNav_OA::get_oa_wp_destination(Location& destination) const } // Sets the waypoint destination using NEU coordinates in centimeters. -// See set_wp_destination_NEU_m() for full details. +// See set_wp_destination_NED_m() for full details. bool AC_WPNav_OA::set_wp_destination_NEU_cm(const Vector3f& destination_neu_cm, bool is_terrain_alt) { - // Convert input from centimeters to meters and delegate to meter version - return set_wp_destination_NEU_m(destination_neu_cm.topostype() * 0.01, is_terrain_alt); + // Convert input from NEU centimeters to NED meters and delegate to meter version + Vector3p destination_ned_m = Vector3p(destination_neu_cm.x, destination_neu_cm.y, -destination_neu_cm.z) * 0.01; + return set_wp_destination_NED_m(destination_ned_m, is_terrain_alt); } -// Sets the waypoint destination using NEU coordinates in meters. -// - destination_neu_m: NEU offset from EKF origin in meters. -// - is_terrain_alt: true if the Z component represents altitude above terrain. +// Sets the waypoint destination using NED coordinates in meters. +// - destination_ned_m: NED offset from EKF origin in meters. +// - is_terrain_alt: true if the destination_ned_m is relative to the terrain surface. // - Resets OA state on success. -bool AC_WPNav_OA::set_wp_destination_NEU_m(const Vector3p& destination_neu_m, bool is_terrain_alt, float arc_ang_rad) +bool AC_WPNav_OA::set_wp_destination_NED_m(const Vector3p& destination_ned_m, bool is_terrain_alt, float arc_ang_rad) { // Call base implementation to set destination and terrain-altitude flag - const bool ret = AC_WPNav::set_wp_destination_NEU_m(destination_neu_m, is_terrain_alt, arc_ang_rad); + const bool ret = AC_WPNav::set_wp_destination_NED_m(destination_ned_m, is_terrain_alt, arc_ang_rad); // If destination set successfully, reset OA state to inactive if (ret) { @@ -69,7 +70,7 @@ float AC_WPNav_OA::get_wp_distance_to_destination_m() const } // Compute distance to original destination using backed-up NEU position - return get_horizontal_distance(_pos_control.get_pos_estimate_NEU_m().xy(), _destination_oabak_neu_m.xy()); + return get_horizontal_distance(_pos_control.get_pos_estimate_NED_m().xy(), _destination_oabak_ned_m.xy()); } // Returns the bearing to the final destination in centidegrees. @@ -90,7 +91,7 @@ float AC_WPNav_OA::get_wp_bearing_to_destination_rad() const } // Return bearing to the original destination, not the OA-adjusted one - return get_bearing_rad(_pos_control.get_pos_estimate_NEU_m().xy().tofloat(), _destination_oabak_neu_m.xy().tofloat()); + return get_bearing_rad(_pos_control.get_pos_estimate_NED_m().xy().tofloat(), _destination_oabak_ned_m.xy().tofloat()); } // Returns true if the vehicle has reached the final destination within radius threshold. @@ -112,16 +113,16 @@ bool AC_WPNav_OA::update_wpnav() // Backup current path state before OA modifies it if (_oa_state == AP_OAPathPlanner::OA_NOT_REQUIRED) { - _origin_oabak_neu_m = _origin_neu_m; - _destination_oabak_neu_m = _destination_neu_m; + _origin_oabak_ned_m = _origin_ned_m; + _destination_oabak_ned_m = _destination_ned_m; _is_terrain_alt_oabak = _is_terrain_alt; - _next_destination_oabak_neu_m = _next_destination_neu_m; + _next_destination_oabak_ned_m = _next_destination_ned_m; } // Convert backup path state to global Location objects for planner input - const Location origin_loc(_origin_oabak_neu_m * 100.0, _is_terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); - const Location destination_loc(_destination_oabak_neu_m * 100.0, _is_terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); - const Location next_destination_loc(_next_destination_oabak_neu_m * 100.0, _is_terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); + const Location origin_loc = Location::from_ekf_offset_NED_m(_origin_oabak_ned_m, _is_terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); + const Location destination_loc = Location::from_ekf_offset_NED_m(_destination_oabak_ned_m, _is_terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); + const Location next_destination_loc = Location::from_ekf_offset_NED_m(_next_destination_oabak_ned_m, _is_terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); Location oa_origin_new, oa_destination_new, oa_next_destination_new; bool dest_to_next_dest_clear = true; AP_OAPathPlanner::OAPathPlannerUsed path_planner_used = AP_OAPathPlanner::OAPathPlannerUsed::None; @@ -143,7 +144,7 @@ bool AC_WPNav_OA::update_wpnav() // OA is no longer needed — restore original destination and optionally set next if (_oa_state != oa_retstate) { // object avoidance has become inactive so reset target to original destination - if (!set_wp_destination_NEU_m(_destination_oabak_neu_m, _is_terrain_alt_oabak)) { + if (!set_wp_destination_NED_m(_destination_oabak_ned_m, _is_terrain_alt_oabak)) { // trigger terrain failsafe return false; } @@ -151,8 +152,8 @@ bool AC_WPNav_OA::update_wpnav() // if path from destination to next_destination is clear if (dest_to_next_dest_clear && (oa_ptr->get_options() & AP_OAPathPlanner::OA_OPTION_FAST_WAYPOINTS)) { // set next destination if non-zero - if (!_next_destination_oabak_neu_m.is_zero()) { - set_wp_destination_next_NEU_m(_next_destination_oabak_neu_m); + if (!_next_destination_oabak_ned_m.is_zero()) { + set_wp_destination_next_NED_m(_next_destination_oabak_ned_m); } } _oa_state = oa_retstate; @@ -177,11 +178,11 @@ bool AC_WPNav_OA::update_wpnav() // OA temporarily failing — stop vehicle at current position if ((_oa_state != AP_OAPathPlanner::OA_PROCESSING) && (_oa_state != AP_OAPathPlanner::OA_ERROR)) { // calculate stopping point - Vector3p stopping_point_neu_m; - get_wp_stopping_point_NEU_m(stopping_point_neu_m); - _oa_destination = Location(stopping_point_neu_m * 100.0f, Location::AltFrame::ABOVE_ORIGIN); + Vector3p stopping_point_ned_m; + get_wp_stopping_point_NED_m(stopping_point_ned_m); + _oa_destination = Location::from_ekf_offset_NED_m(stopping_point_ned_m, Location::AltFrame::ABOVE_ORIGIN); _oa_next_destination.zero(); - if (set_wp_destination_NEU_m(stopping_point_neu_m, false)) { + if (set_wp_destination_NED_m(stopping_point_ned_m, false)) { _oa_state = oa_retstate; } } @@ -201,8 +202,8 @@ bool AC_WPNav_OA::update_wpnav() // Dijkstra's. Action is only needed if path planner has just became active or the target destination's lat or lon has changed // Interpolate altitude and set new target if different or first OA success if ((_oa_state != AP_OAPathPlanner::OA_SUCCESS) || !oa_destination_new.same_latlon_as(_oa_destination)) { - Location origin_oabak_loc(_origin_oabak_neu_m * 100.0, _is_terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); - Location destination_oabak_loc(_destination_oabak_neu_m * 100, _is_terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); + Location origin_oabak_loc = Location::from_ekf_offset_NED_m(_origin_oabak_ned_m, _is_terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); + Location destination_oabak_loc = Location::from_ekf_offset_NED_m(_destination_oabak_ned_m, _is_terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); oa_destination_new.linearly_interpolate_alt(origin_oabak_loc, destination_oabak_loc); // set new OA adjusted destination @@ -218,7 +219,6 @@ bool AC_WPNav_OA::update_wpnav() if ((oa_ptr->get_options() & AP_OAPathPlanner::OA_OPTION_FAST_WAYPOINTS) && !oa_next_destination_new.is_zero()) { // calculate oa_next_destination_new's altitude using linear interpolation between original origin and destination // this "next destination" is still an intermediate point between the origin and destination - Location next_destination_oabak_loc(_next_destination_oabak_neu_m * 100.0, _is_terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); oa_next_destination_new.linearly_interpolate_alt(origin_oabak_loc, destination_oabak_loc); if (set_wp_destination_next_loc(oa_next_destination_new)) { _oa_next_destination = oa_next_destination_new; @@ -236,8 +236,8 @@ bool AC_WPNav_OA::update_wpnav() target_alt_loc.linearly_interpolate_alt(origin_loc, destination_loc); // Get terrain offset if needed - float terrain_u_m = 0; - if (_is_terrain_alt_oabak && !get_terrain_U_m(terrain_u_m)) { + float terrain_d_m = 0; + if (_is_terrain_alt_oabak && !get_terrain_D_m(terrain_d_m)) { // trigger terrain failsafe return false; } @@ -251,13 +251,13 @@ bool AC_WPNav_OA::update_wpnav() } float target_alt_loc_alt_m = 0; UNUSED_RESULT(target_alt_loc.get_alt_m(target_alt_loc.get_alt_frame(), target_alt_loc_alt_m)); - Vector3p destination_neu_m{destination_ne_m.x, destination_ne_m.y, target_alt_loc_alt_m}; + Vector3p destination_ned_m{destination_ne_m.x, destination_ne_m.y, target_alt_loc_alt_m}; // pass the desired position directly to the position controller - _pos_control.input_pos_NEU_m(destination_neu_m, terrain_u_m, 10.0); + _pos_control.input_pos_NED_m(destination_ned_m, terrain_d_m, 10.0); // update horizontal position controller (vertical is updated in vehicle code) - _pos_control.update_NE_controller(); + _pos_control.NE_update_controller(); // return success without calling parent AC_WPNav return true; @@ -268,18 +268,18 @@ bool AC_WPNav_OA::update_wpnav() _oa_destination = oa_destination_new; // Convert final destination to NEU offset and push to position controller - Vector3p destination_neu_m; - if (!_oa_destination.get_vector_from_origin_NEU_m(destination_neu_m)) { + Vector3p destination_ned_m; + if (!_oa_destination.get_vector_from_origin_NED_m(destination_ned_m)) { // this should never happen because we can only get here if we have an EKF origin INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); return false; } // pass the desired position directly to the position controller as an offset from EKF origin in NEU - _pos_control.input_pos_NEU_m(destination_neu_m, 0, 10.0); + _pos_control.input_pos_NED_m(destination_ned_m, 0, 10.0); // update horizontal position controller (vertical is updated in vehicle code) - _pos_control.update_NE_controller(); + _pos_control.NE_update_controller(); // return success without calling parent AC_WPNav return true; diff --git a/libraries/AC_WPNav/AC_WPNav_OA.h b/libraries/AC_WPNav/AC_WPNav_OA.h index b15e60665cc13..0d19f5a617406 100644 --- a/libraries/AC_WPNav/AC_WPNav_OA.h +++ b/libraries/AC_WPNav/AC_WPNav_OA.h @@ -20,15 +20,15 @@ class AC_WPNav_OA : public AC_WPNav bool get_oa_wp_destination(Location& destination) const override; // Sets the waypoint destination using NEU coordinates in centimeters. - // See set_wp_destination_NEU_m() for full details. + // See set_wp_destination_NED_m() for full details. bool set_wp_destination_NEU_cm(const Vector3f& destination_neu_cm, bool is_terrain_alt = false) override; - // Sets the waypoint destination using NEU coordinates in meters. - // - destination_neu_m: NEU offset from EKF origin in meters. - // - is_terrain_alt: true if the Z component represents altitude above terrain. + // Sets the waypoint destination using NED coordinates in meters. + // - destination_ned_m: NED offset from EKF origin in meters. + // - is_terrain_alt: true if the destination_ned_m is relative to the terrain surface. // arc_rad specifies the signed arc angle in radians for an ARC_WAYPOINT segment (0 for straight path) // - Resets OA state on success. - bool set_wp_destination_NEU_m(const Vector3p& destination_neu_m, bool is_terrain_alt = false, float arc_rad = 0.0) override; + bool set_wp_destination_NED_m(const Vector3p& destination_ned_m, bool is_terrain_alt = false, float arc_rad = 0.0) override; // Returns the horizontal distance to the final destination in centimeters. // See get_wp_distance_to_destination_m() for full details. @@ -58,9 +58,9 @@ class AC_WPNav_OA : public AC_WPNav // oa path planning variables AP_OAPathPlanner::OA_RetState _oa_state; // state of object avoidance, if OA_SUCCESS we use _oa_destination to avoid obstacles - Vector3p _origin_oabak_neu_m; // backup of _origin_neu_m so it can be restored when oa completes - Vector3p _destination_oabak_neu_m; // backup of _destination_neu_m so it can be restored when oa completes - Vector3p _next_destination_oabak_neu_m; // backup of _next_destination_neu_m so it can be restored when oa completes + Vector3p _origin_oabak_ned_m; // backup of _origin_ned_m so it can be restored when oa completes + Vector3p _destination_oabak_ned_m; // backup of _destination_ned_m so it can be restored when oa completes + Vector3p _next_destination_oabak_ned_m; // backup of _next_destination_ned_m so it can be restored when oa completes bool _is_terrain_alt_oabak; // true if backup origin and destination z-axis are terrain altitudes Location _oa_destination; // intermediate destination during avoidance Location _oa_next_destination; // intermediate next destination during avoidance diff --git a/libraries/APM_Control/examples/AP_FW_Controller_test/AP_FW_Controller_test.cpp b/libraries/APM_Control/examples/AP_FW_Controller_test/AP_FW_Controller_test.cpp index e839753bc515a..2deaecfb029e2 100644 --- a/libraries/APM_Control/examples/AP_FW_Controller_test/AP_FW_Controller_test.cpp +++ b/libraries/APM_Control/examples/AP_FW_Controller_test/AP_FW_Controller_test.cpp @@ -144,7 +144,7 @@ void setup() char cmd[20] {}; strncpy(cmd, arg, eq-arg); - const float value = atof(eq+1); + const float value = strtof(eq+1, nullptr); if (strcmp(cmd, "axis") == 0) { if (strcmp(eq+1, "roll") == 0) { test_axis = Axis::Roll; diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index cdc5a25d2e410..9e511e62c43d7 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -485,8 +485,8 @@ void AP_AHRS::update(bool skip_ins_update) // update AOA and SSA update_AOA_SSA(); -#if HAL_GCS_ENABLED state.active_EKF = _active_EKF_type(); +#if HAL_GCS_ENABLED if (state.active_EKF != last_active_ekf_type) { last_active_ekf_type = state.active_EKF; const char *shortname = "???"; @@ -1136,9 +1136,9 @@ bool AP_AHRS::_airspeed_TAS(Vector3f &vec) const return false; } -// return the innovation in m/s, innovation variance in (m/s)^2 and age in msec of the last TAS measurement processed +// return the innovation in m/s, innovation variance in (m/s)^2 and age in msec of the last TAS measurement processed for a given sensor instance // returns false if the data is unavailable -bool AP_AHRS::airspeed_health_data(float &innovation, float &innovationVariance, uint32_t &age_ms) const +bool AP_AHRS::airspeed_health_data(uint8_t instance, float &innovation, float &innovationVariance, uint32_t &age_ms) const { switch (active_EKF_type()) { #if AP_AHRS_DCM_ENABLED @@ -1152,7 +1152,7 @@ bool AP_AHRS::airspeed_health_data(float &innovation, float &innovationVariance, #if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: - return EKF3.getAirSpdHealthData(innovation, innovationVariance, age_ms); + return EKF3.getAirSpdHealthData(instance, innovation, innovationVariance, age_ms); #endif #if AP_AHRS_SIM_ENABLED @@ -1202,17 +1202,23 @@ bool AP_AHRS::use_compass(void) return false; } -// return the quaternion defining the rotation from NED to XYZ (body) axes bool AP_AHRS::_get_quaternion(Quaternion &quat) const +{ + return _get_quaternion_for_ekf_type(quat, active_EKF_type()); +} + +// return the quaternion defining the rotation from NED to XYZ (body) axes +bool AP_AHRS::_get_quaternion_for_ekf_type(Quaternion &quat, EKFType type) const { // backends always return in autopilot XYZ frame; rotate result // according to trim - switch (active_EKF_type()) { + switch (type) { #if AP_AHRS_DCM_ENABLED case EKFType::DCM: - if (!dcm.get_quaternion(quat)) { + if (!dcm_estimates.attitude_valid) { return false; } + quat = dcm_estimates.quaternion; break; #endif #if HAL_NAVEKF2_AVAILABLE @@ -1233,15 +1239,20 @@ bool AP_AHRS::_get_quaternion(Quaternion &quat) const #endif #if AP_AHRS_SIM_ENABLED case EKFType::SIM: - if (!sim.get_quaternion(quat)) { + if (!sim_estimates.attitude_valid) { return false; } + quat = sim_estimates.quaternion; break; #endif #if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: // we assume the external AHRS isn't trimmed with the autopilot! - return external.get_quaternion(quat); + if (!external_estimates.attitude_valid) { + return false; + } + quat = external_estimates.quaternion; + return true; #endif } @@ -1266,7 +1277,7 @@ bool AP_AHRS::_get_secondary_attitude(Vector3f &eulers) const eulers[0] = dcm_estimates.roll_rad; eulers[1] = dcm_estimates.pitch_rad; eulers[2] = dcm_estimates.yaw_rad; - return true; + return dcm_estimates.attitude_valid; #endif #if HAL_NAVEKF2_AVAILABLE @@ -1286,7 +1297,10 @@ bool AP_AHRS::_get_secondary_attitude(Vector3f &eulers) const #if AP_AHRS_SIM_ENABLED case EKFType::SIM: // SITL is secondary (should never happen) - return false; + eulers[0] = sim_estimates.roll_rad; + eulers[1] = sim_estimates.pitch_rad; + eulers[2] = sim_estimates.yaw_rad; + return sim_estimates.attitude_valid; #endif #if AP_AHRS_EXTERNAL_ENABLED @@ -1295,7 +1309,7 @@ bool AP_AHRS::_get_secondary_attitude(Vector3f &eulers) const eulers[0] = external_estimates.roll_rad; eulers[1] = external_estimates.pitch_rad; eulers[2] = external_estimates.yaw_rad; - return true; + return external_estimates.attitude_valid; } #endif } @@ -1312,54 +1326,7 @@ bool AP_AHRS::_get_secondary_quaternion(Quaternion &quat) const if (!_get_secondary_EKF_type(secondary_ekf_type)) { return false; } - - switch (secondary_ekf_type) { - -#if AP_AHRS_DCM_ENABLED - case EKFType::DCM: - // DCM is secondary - if (!dcm.get_quaternion(quat)) { - return false; - } - break; -#endif - -#if HAL_NAVEKF2_AVAILABLE - case EKFType::TWO: - // EKF2 is secondary - if (!_ekf2_started) { - return false; - } - EKF2.getQuaternion(quat); - break; -#endif - -#if HAL_NAVEKF3_AVAILABLE - case EKFType::THREE: - // EKF3 is secondary - if (!_ekf3_started) { - return false; - } - EKF3.getQuaternion(quat); - break; -#endif - -#if AP_AHRS_SIM_ENABLED - case EKFType::SIM: - // SITL is secondary (should never happen) - return false; -#endif - -#if AP_AHRS_EXTERNAL_ENABLED - case EKFType::EXTERNAL: - // External is secondary - return external.get_quaternion(quat); -#endif - } - - quat.rotate(-_trim.get()); - - return true; + return _get_quaternion_for_ekf_type(quat, secondary_ekf_type); } // return secondary position solution if available @@ -1453,28 +1420,6 @@ Vector2f AP_AHRS::_groundspeed_vector(void) float AP_AHRS::_groundspeed(void) { - switch (active_EKF_type()) { -#if AP_AHRS_DCM_ENABLED - case EKFType::DCM: - return dcm.groundspeed(); -#endif -#if HAL_NAVEKF2_AVAILABLE - case EKFType::TWO: -#endif - -#if HAL_NAVEKF3_AVAILABLE - case EKFType::THREE: -#endif - -#if AP_AHRS_EXTERNAL_ENABLED - case EKFType::EXTERNAL: -#endif - -#if AP_AHRS_SIM_ENABLED - case EKFType::SIM: -#endif - break; - } return groundspeed_vector().length(); } @@ -1578,15 +1523,15 @@ bool AP_AHRS::_get_velocity_NED(Vector3f &vec) const #if AP_AHRS_SIM_ENABLED case EKFType::SIM: - return sim.get_velocity_NED(vec); + return sim_estimates.get_velocity_NED(vec); #endif #if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: - return external.get_velocity_NED(vec); + return external_estimates.get_velocity_NED(vec); #endif } #if AP_AHRS_DCM_ENABLED - return dcm.get_velocity_NED(vec); + return dcm_estimates.get_velocity_NED(vec); #endif return false; } @@ -1675,7 +1620,7 @@ bool AP_AHRS::get_vert_pos_rate_D(float &velocity) const switch (active_EKF_type()) { #if AP_AHRS_DCM_ENABLED case EKFType::DCM: - return dcm.get_vert_pos_rate_D(velocity); + return dcm_estimates.get_vert_pos_rate_D(velocity); #endif #if HAL_NAVEKF2_AVAILABLE @@ -1692,11 +1637,11 @@ bool AP_AHRS::get_vert_pos_rate_D(float &velocity) const #if AP_AHRS_SIM_ENABLED case EKFType::SIM: - return sim.get_vert_pos_rate_D(velocity); + return sim_estimates.get_vert_pos_rate_D(velocity); #endif #if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: - return external.get_vert_pos_rate_D(velocity); + return external_estimates.get_vert_pos_rate_D(velocity); #endif } // since there is no default case above, this is unreachable diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 81dc7c72f7642..ee4855c8baed1 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -193,9 +193,9 @@ class AP_AHRS { // returns false if estimate is unavailable bool airspeed_vector_TAS(Vector3f &vec) const; - // return the innovation in m/s, innovation variance in (m/s)^2 and age in msec of the last TAS measurement processed + // return the innovation in m/s, innovation variance in (m/s)^2 and age in msec of the last TAS measurement processed for a given sensor instance // returns false if the data is unavailable - bool airspeed_health_data(float &innovation, float &innovationVariance, uint32_t &age_ms) const; + bool airspeed_health_data(uint8_t instance, float &innovation, float &innovationVariance, uint32_t &age_ms) const; // return true if a airspeed sensor is enabled bool airspeed_sensor_enabled(void) const { @@ -257,7 +257,7 @@ class AP_AHRS { #if AP_AHRS_POSITION_RESET_ENABLED // Set the EKF's NE horizontal position states and their corresponding variances from the supplied WGS-84 location // and 1-sigma horizontal position uncertainty. This can be used when the EKF is dead reckoning to periodically - // correct the position. If the EKF is is still using data from a postion sensor such as GPS, the position set + // correct the position. If the EKF is is still using data from a position sensor such as GPS, the position set // will not be performed. // pos_accuracy is the standard deviation of the horizontal position uncertainty in metres. // The altitude element of the location is not used. @@ -931,6 +931,10 @@ class AP_AHRS { // return the quaternion defining the rotation from NED to XYZ (body) axes bool _get_quaternion(Quaternion &quat) const WARN_IF_UNUSED; + // return the quaternion defining the rotation from NED to XYZ + // (body) axes for the passed-in type + bool _get_quaternion_for_ekf_type(Quaternion &quat, EKFType type) const; + // return secondary position solution if available bool _get_secondary_position(Location &loc) const; diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h index cf7e4f4bd5ba9..1212daac0b073 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.h +++ b/libraries/AP_AHRS/AP_AHRS_Backend.h @@ -51,15 +51,54 @@ class AP_AHRS_Backend // structure to retrieve results from backends: struct Estimates { + // if attitude_valid is true then all of the + // eulers/quaternion/matrix must be valid: + bool attitude_valid; float roll_rad; float pitch_rad; float yaw_rad; Matrix3f dcm_matrix; + Quaternion quaternion; + Vector3f gyro_estimate; Vector3f gyro_drift; Vector3f accel_ef; Vector3f accel_bias; + // a ground velocity in meters/second, North/East/Down + Vector3f velocity_NED; + bool velocity_NED_valid; + // return a ground velocity in meters/second, North/East/Down + bool get_velocity_NED(Vector3f &vel) const WARN_IF_UNUSED { + if (!velocity_NED_valid) { + return false; + } + vel = velocity_NED; + return true; + }; + + // a derivative of the vertical position in m/s which is + // kinematically consistent with the vertical position is + // required by some control loops. + // This is different to the vertical velocity from the EKF + // which is not always consistent with the vertical position + // due to the various errors that are being corrected for. + float vert_pos_rate_D; + bool vert_pos_rate_D_valid; + // Get a derivative of the vertical position in m/s which is + // kinematically consistent with the vertical position is + // required by some control loops. + // This is different to the vertical velocity from the EKF + // which is not always consistent with the vertical position + // due to the various errors that are being corrected for. + bool get_vert_pos_rate_D(float &velocity) const { + if (!vert_pos_rate_D_valid) { + return false; + } + velocity = vert_pos_rate_D; + return true; + } + Location location; bool location_valid; @@ -90,9 +129,6 @@ class AP_AHRS_Backend // requires_position should be true if horizontal position configuration should be checked virtual bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const = 0; - // check all cores providing consistent attitudes for prearm checks - virtual bool attitudes_consistent(char *failure_msg, const uint8_t failure_msg_len) const { return true; } - // see if EKF lane switching is possible to avoid EKF failsafe virtual void check_lane_switch(void) {} @@ -165,18 +201,6 @@ class AP_AHRS_Backend // return a ground vector estimate in meters/second, in North/East order virtual Vector2f groundspeed_vector(void) = 0; - // return a ground velocity in meters/second, North/East/Down - // order. This will only be accurate if have_inertial_nav() is - // true - virtual bool get_velocity_NED(Vector3f &vec) const WARN_IF_UNUSED { - return false; - } - - // Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops. - // This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for. - virtual bool get_vert_pos_rate_D(float &velocity) const = 0; - - // virtual bool set_origin(const Location &loc) { return false; } @@ -201,17 +225,9 @@ class AP_AHRS_Backend return false; } - // return ground speed estimate in meters/second. Used by ground vehicles. - float groundspeed(void) { - return groundspeed_vector().length(); - } - // return true if we will use compass for yaw virtual bool use_compass(void) = 0; - // return the quaternion defining the rotation from NED to XYZ (body) axes - virtual bool get_quaternion(Quaternion &quat) const WARN_IF_UNUSED = 0; - // is the AHRS subsystem healthy? virtual bool healthy(void) const = 0; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 46301d2e6b0da..1ae693fe2e4c7 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -148,10 +148,19 @@ void AP_AHRS_DCM::get_results(AP_AHRS_Backend::Estimates &results) results.yaw_rad = yaw; results.dcm_matrix = _body_dcm_matrix; + + // quaternion is derived from transformation matrix: + results.quaternion.from_rotation_matrix(_dcm_matrix); + + results.attitude_valid = true; + results.gyro_estimate = _omega; results.gyro_drift = _omega_I; results.accel_ef = _accel_ef; + results.velocity_NED_valid = get_velocity_NED(results.velocity_NED); + results.vert_pos_rate_D_valid = get_vert_pos_rate_D(results.vert_pos_rate_D); + results.location_valid = get_location(results.location); } @@ -493,13 +502,6 @@ bool AP_AHRS_DCM::use_compass(void) return true; } -// return the quaternion defining the rotation from NED to XYZ (body) axes -bool AP_AHRS_DCM::get_quaternion(Quaternion &quat) const -{ - quat.from_rotation_matrix(_dcm_matrix); - return true; -} - // yaw drift correction using the compass or GPS // this function produces the _omega_yaw_P vector, and also // contributes to the _omega_I.z long term yaw drift estimate diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 5d08e340bcea1..6588a3bdb7575 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -93,20 +93,11 @@ class AP_AHRS_DCM : public AP_AHRS_Backend { bool use_compass() override; - // return the quaternion defining the rotation from NED to XYZ (body) axes - bool get_quaternion(Quaternion &quat) const override WARN_IF_UNUSED; - void estimate_wind(void); // is the AHRS subsystem healthy? bool healthy() const override; - bool get_velocity_NED(Vector3f &vec) const override; - - // Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops. - // This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for. - bool get_vert_pos_rate_D(float &velocity) const override; - // returns false if we fail arming checks, in which case the buffer will be populated with a failure message // requires_position should be true if horizontal position configuration should be checked (not used) bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const override; @@ -126,6 +117,12 @@ class AP_AHRS_DCM : public AP_AHRS_Backend { private: + // Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops. + // This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for. + bool get_vert_pos_rate_D(float &velocity) const; + + bool get_velocity_NED(Vector3f &vec) const; + // dead-reckoning support bool get_location(Location &loc) const; diff --git a/libraries/AP_AHRS/AP_AHRS_External.cpp b/libraries/AP_AHRS/AP_AHRS_External.cpp index c7b2cf70f39bf..8746fea421035 100644 --- a/libraries/AP_AHRS/AP_AHRS_External.cpp +++ b/libraries/AP_AHRS/AP_AHRS_External.cpp @@ -22,13 +22,16 @@ bool AP_AHRS_External::healthy() const { void AP_AHRS_External::get_results(AP_AHRS_Backend::Estimates &results) { - Quaternion quat; auto &extahrs = AP::externalAHRS(); const AP_InertialSensor &_ins = AP::ins(); - if (!extahrs.get_quaternion(quat)) { + if (!extahrs.get_quaternion(results.quaternion)) { + results.attitude_valid = false; return; } - quat.rotation_matrix(results.dcm_matrix); + results.attitude_valid = true; + results.quaternion.rotation_matrix(results.dcm_matrix); + // note that this is suspect; we are rotating the matrix and + // eulers away from alignment with the quaternion: results.dcm_matrix = results.dcm_matrix * AP::ahrs().get_rotation_vehicle_body_to_autopilot_body(); results.dcm_matrix.to_euler(&results.roll_rad, &results.pitch_rad, &results.yaw_rad); @@ -45,12 +48,13 @@ void AP_AHRS_External::get_results(AP_AHRS_Backend::Estimates &results) const Vector3f accel_ef = results.dcm_matrix * AP::ahrs().get_rotation_autopilot_body_to_vehicle_body() * accel; results.accel_ef = accel_ef; - results.location_valid = AP::externalAHRS().get_location(results.location); -} + results.velocity_NED_valid = AP::externalAHRS().get_velocity_NED(results.velocity_NED); + // a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops. + // This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for. + results.vert_pos_rate_D_valid = AP::externalAHRS().get_speed_down(results.vert_pos_rate_D); -bool AP_AHRS_External::get_quaternion(Quaternion &quat) const -{ - return AP::externalAHRS().get_quaternion(quat); + + results.location_valid = AP::externalAHRS().get_location(results.location); } Vector2f AP_AHRS_External::groundspeed_vector() @@ -99,16 +103,6 @@ bool AP_AHRS_External::get_relative_position_D_origin(postype_t &posD) const return true; } -bool AP_AHRS_External::get_velocity_NED(Vector3f &vec) const -{ - return AP::externalAHRS().get_velocity_NED(vec); -} - -bool AP_AHRS_External::get_vert_pos_rate_D(float &velocity) const -{ - return AP::externalAHRS().get_speed_down(velocity); -} - bool AP_AHRS_External::pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const { return AP::externalAHRS().pre_arm_check(failure_msg, failure_msg_len); diff --git a/libraries/AP_AHRS/AP_AHRS_External.h b/libraries/AP_AHRS/AP_AHRS_External.h index c0e421aeedb01..adbecfef45ee7 100644 --- a/libraries/AP_AHRS/AP_AHRS_External.h +++ b/libraries/AP_AHRS/AP_AHRS_External.h @@ -59,20 +59,11 @@ class AP_AHRS_External : public AP_AHRS_Backend { return true; } - // return the quaternion defining the rotation from NED to XYZ (body) axes - bool get_quaternion(Quaternion &quat) const override WARN_IF_UNUSED; - void estimate_wind(void); // is the AHRS subsystem healthy? bool healthy() const override; - bool get_velocity_NED(Vector3f &vec) const override; - - // Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops. - // This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for. - bool get_vert_pos_rate_D(float &velocity) const override; - // returns false if we fail arming checks, in which case the buffer will be populated with a failure message // requires_position should be true if horizontal position configuration should be checked (not used) bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const override; diff --git a/libraries/AP_AHRS/AP_AHRS_SIM.cpp b/libraries/AP_AHRS/AP_AHRS_SIM.cpp index 734ff7364bef7..29630fc5cd569 100644 --- a/libraries/AP_AHRS/AP_AHRS_SIM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_SIM.cpp @@ -19,18 +19,6 @@ bool AP_AHRS_SIM::get_location(Location &loc) const return true; } -bool AP_AHRS_SIM::get_velocity_NED(Vector3f &vec) const -{ - if (_sitl == nullptr) { - return false; - } - - const struct SITL::sitl_fdm &fdm = _sitl->state; - vec = Vector3f(fdm.speedN, fdm.speedE, fdm.speedD); - - return true; -} - bool AP_AHRS_SIM::wind_estimate(Vector3f &wind) const { if (_sitl == nullptr) { @@ -57,18 +45,6 @@ bool AP_AHRS_SIM::airspeed_EAS(uint8_t index, float &airspeed_ret) const return airspeed_EAS(airspeed_ret); } -bool AP_AHRS_SIM::get_quaternion(Quaternion &quat) const -{ - if (_sitl == nullptr) { - return false; - } - - const struct SITL::sitl_fdm &fdm = _sitl->state; - quat = fdm.quaternion; - - return true; -} - Vector2f AP_AHRS_SIM::groundspeed_vector(void) { if (_sitl == nullptr) { @@ -80,17 +56,6 @@ Vector2f AP_AHRS_SIM::groundspeed_vector(void) return Vector2f(fdm.speedN, fdm.speedE); } -bool AP_AHRS_SIM::get_vert_pos_rate_D(float &velocity) const -{ - if (_sitl == nullptr) { - return false; - } - - velocity = _sitl->state.speedD; - - return true; -} - bool AP_AHRS_SIM::get_hagl(float &height) const { if (_sitl == nullptr) { @@ -238,6 +203,11 @@ void AP_AHRS_SIM::get_results(AP_AHRS_Backend::Estimates &results) const struct SITL::sitl_fdm &fdm = _sitl->state; const AP_InertialSensor &_ins = AP::ins(); + results.attitude_valid = true; + + // note that this result is rotated by AP_AHRS::get_quaternion + results.quaternion = fdm.quaternion; + fdm.quaternion.rotation_matrix(results.dcm_matrix); results.dcm_matrix = results.dcm_matrix * AP::ahrs().get_rotation_vehicle_body_to_autopilot_body(); results.dcm_matrix.to_euler(&results.roll_rad, &results.pitch_rad, &results.yaw_rad); @@ -248,6 +218,14 @@ void AP_AHRS_SIM::get_results(AP_AHRS_Backend::Estimates &results) const Vector3f &accel = _ins.get_accel(); results.accel_ef = results.dcm_matrix * AP::ahrs().get_rotation_autopilot_body_to_vehicle_body() * accel; + results.velocity_NED = Vector3f(fdm.speedN, fdm.speedE, fdm.speedD); + results.velocity_NED_valid = true; + + // a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops. + // This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for. + results.vert_pos_rate_D_valid = true; + results.vert_pos_rate_D = _sitl->state.speedD; + results.location_valid = get_location(results.location); #if HAL_NAVEKF3_AVAILABLE diff --git a/libraries/AP_AHRS/AP_AHRS_SIM.h b/libraries/AP_AHRS/AP_AHRS_SIM.h index 4da08df41cab0..a69ba1970f36d 100644 --- a/libraries/AP_AHRS/AP_AHRS_SIM.h +++ b/libraries/AP_AHRS/AP_AHRS_SIM.h @@ -79,18 +79,9 @@ class AP_AHRS_SIM : public AP_AHRS_Backend { bool use_compass() override { return true; } - // return the quaternion defining the rotation from NED to XYZ (body) axes - bool get_quaternion(Quaternion &quat) const override WARN_IF_UNUSED; - // is the AHRS subsystem healthy? bool healthy() const override { return true; } - bool get_velocity_NED(Vector3f &vec) const override; - - // Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops. - // This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for. - bool get_vert_pos_rate_D(float &velocity) const override; - // returns false if we fail arming checks, in which case the buffer will be populated with a failure message // requires_position should be true if horizontal position configuration should be checked (not used) bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const override { return true; } diff --git a/libraries/AP_AIS/AP_AIS.cpp b/libraries/AP_AIS/AP_AIS.cpp index 969faac9470f3..d515ee52a2bc4 100644 --- a/libraries/AP_AIS/AP_AIS.cpp +++ b/libraries/AP_AIS/AP_AIS.cpp @@ -274,7 +274,7 @@ void AP_AIS::send(mavlink_channel_t chan) } #if AP_OADATABASE_ENABLED -// Send a AIS vessel to the object avoidance database if its postion is valid +// Send a AIS vessel to the object avoidance database if its position is valid void AP_AIS::send_to_object_avoidance_database(const struct ais_vehicle_t &vessel) { // No point if database is not enabled diff --git a/libraries/AP_AIS/AP_AIS.h b/libraries/AP_AIS/AP_AIS.h index 219249fecff11..681889beaecb8 100644 --- a/libraries/AP_AIS/AP_AIS.h +++ b/libraries/AP_AIS/AP_AIS.h @@ -94,7 +94,7 @@ class AP_AIS uint16_t _send_index; // index of the last vessel send over mavlink - // Send a AIS vessel to the object avoidance data base if its postion is valid + // Send a AIS vessel to the object avoidance data base if its position is valid void send_to_object_avoidance_database(const struct ais_vehicle_t &vessel); // Return true if location is valid diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index ca415b462b5dc..addd775fbac24 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -340,6 +340,9 @@ void AP_Airspeed::init() convert_per_instance(); + // Set primary from parameter to avoid primary changed message at boot + primary = primary_sensor.get(); + #if ENABLE_PARAMETER // if either type is set then enable if not manually set if (!_enable.configured() && ((param[0].type.get() != TYPE_NONE) || (param[1].type.get() != TYPE_NONE))) { @@ -709,6 +712,81 @@ void AP_Airspeed::read(uint8_t i) #endif // HAL_BUILD_AP_PERIPH } +// Select primary sensor based on user parameters and health +uint8_t AP_Airspeed::select_primary() +{ + // user selected primary from parameter, track changes + const uint8_t user_primary = primary_sensor.get(); + const bool user_primary_changed = user_primary != last_user_primary; + last_user_primary = user_primary; + + // If user selected instance is both healthy and set to use then it is a valid primary + const bool user_healthy = healthy(user_primary); + const bool user_healthy_and_use = user_healthy && use(user_primary); + + if ((user_primary_changed || !hal.util->get_soft_armed()) && user_healthy_and_use) { + /* + If user selected primary is healthy and set to use then: + Always change when the user changes the parameter. + Always change if disarmed, if armed its better to stick with the current sensor to avoid needless switching. + + The EKF3 innovation check only applies to the active sensor so a bad sensor will appear good after some time because + the EKF is no longer using the sensor so cannot provide feedback. + + We can't just stick with the current primary when disarmed due to variation in detection time. + */ + return user_primary; + } + + // If the currently selected primary is valid there is no need to change + const bool primary_healthy = healthy(primary); + if (primary_healthy && use(primary)) { + return primary; + } + + if (user_healthy_and_use) { + // The current primary is not valid, try the user set primary first + return user_primary; + } + + // Select the first sensor which is both healthy and set to use + for (uint8_t i=0; isnprintf(buffer, buflen, "not using Primary (%i)", primary_sensor.get() + 1); + return false; + } + } + } + return true; } #endif diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index 7313858924b70..22e3188807e44 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -333,6 +333,9 @@ class AP_Airspeed uint8_t primary; uint8_t num_sensors; + // Track primary parameter, this allows changes to be honored in flight + uint8_t last_user_primary; + uint32_t _log_bit = -1; // stores which bit in LOG_BITMASK is used to indicate we should log airspeed readings void read(uint8_t i); @@ -373,6 +376,9 @@ class AP_Airspeed void convert_per_instance(); + // Select primary sensor based on user parameters and health + uint8_t select_primary(); + }; namespace AP { diff --git a/libraries/AP_Airspeed/AP_Airspeed_Health.cpp b/libraries/AP_Airspeed/AP_Airspeed_Health.cpp index feed1a507a7db..7064e2bcd79fa 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_Health.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_Health.cpp @@ -54,7 +54,7 @@ void AP_Airspeed::check_sensor_ahrs_wind_max_failures(uint8_t i) // check for airspeed consistent with wind and vehicle velocity using the EKF uint32_t age_ms; float innovation, innovationVariance; - if (AP::ahrs().airspeed_health_data(innovation, innovationVariance, age_ms) && age_ms < 1000 && is_positive(innovationVariance)) { + if (AP::ahrs().airspeed_health_data(i, innovation, innovationVariance, age_ms) && age_ms < 1000 && is_positive(innovationVariance)) { state[i].failures.test_ratio = fabsf(innovation) / safe_sqrt(innovationVariance); } else { state[i].failures.test_ratio = 0.0f; diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 7fa9f11ff90b4..54239d349df3c 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -1306,7 +1306,7 @@ bool AP_Arming::can_checks(bool report) for (uint8_t i = 0; i < num_drivers; i++) { switch (AP::can().get_driver_type(i)) { case AP_CAN::Protocol::PiccoloCAN: { -#if HAL_PICCOLO_CAN_ENABLE +#if AP_PICCOLOCAN_ENABLED AP_PiccoloCAN *ap_pcan = AP_PiccoloCAN::get_pcan(i); if (ap_pcan != nullptr && !ap_pcan->pre_arm_check(fail_msg, ARRAY_SIZE(fail_msg))) { diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index cec41a679faea..55aafa8267ca7 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -502,9 +502,9 @@ bool AP_Baro::_add_backend(AP_Baro_Backend *backend) /* wrapper around hal.i2c_mgr->get_device() that prevents duplicate devices being opened */ -bool AP_Baro::_have_i2c_driver(uint8_t bus, uint8_t address) const +bool AP_Baro::_i2c_sensor_is_registered(uint8_t bus, uint8_t address) const { - for (int i=0; i<_num_drivers; ++i) { + for (int i=0; i<_num_sensors; ++i) { if (AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_I2C, bus, address, 0) == AP_HAL::Device::change_bus_id(uint32_t(sensors[i].bus_id.get()), 0)) { // device already has been defined. @@ -523,7 +523,7 @@ bool AP_Baro::_have_i2c_driver(uint8_t bus, uint8_t address) const } while (0) // macro for use by HAL_INS_PROBE_LIST and various helper functions -#define GET_I2C_DEVICE_PTR(bus, address) _have_i2c_driver(bus, address)?nullptr:hal.i2c_mgr->get_device_ptr(bus, address) +#define GET_I2C_DEVICE_PTR(bus, address) _i2c_sensor_is_registered(bus, address)?nullptr:hal.i2c_mgr->get_device_ptr(bus, address) // probe for an I2C barometer. void AP_Baro::probe_i2c_dev(AP_Baro_Backend* (*probefn)(AP_Baro&, AP_HAL::Device&), uint8_t bus, uint8_t addr) @@ -677,7 +677,6 @@ void AP_Baro::init(void) case AP_BoardConfig::PX4_BOARD_PHMINI: case AP_BoardConfig::PX4_BOARD_AUAV21: case AP_BoardConfig::PX4_BOARD_PH2SLIM: - case AP_BoardConfig::PX4_BOARD_FMUV5: case AP_BoardConfig::PX4_BOARD_FMUV6: #if AP_BARO_MS5611_ENABLED probe_spi_dev(AP_Baro_MS5611::probe, HAL_BARO_MS5611_NAME); diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index 704dedfc2194f..d68feac9dbf1c 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -327,8 +327,8 @@ class AP_Baro // when did we last notify the GCS of new pressure reference? uint32_t _last_notify_ms; - // see if we already have probed a i2c driver by bus number and address - bool _have_i2c_driver(uint8_t bus_num, uint8_t address) const; + // see if we already have probed a i2c sensor by bus number and address + bool _i2c_sensor_is_registered(uint8_t bus_num, uint8_t address) const; bool _add_backend(AP_Baro_Backend *backend); void _probe_i2c_barometers(void); diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index aa9734571057f..1156f8fe0043a 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -28,6 +28,7 @@ #include "AP_BattMonitor_Synthetic_Current.h" #include "AP_BattMonitor_AD7091R5.h" #include "AP_BattMonitor_Scripting.h" +#include "AP_BattMonitor_TIBQ76952.h" #include @@ -707,6 +708,11 @@ AP_BattMonitor::init() drivers[instance] = NEW_NOTHROW AP_BattMonitor_INA3221(*this, state[instance], _params[instance]); break; #endif // AP_BATTERY_INA3221_ENABLED +#if AP_BATTERY_TIBQ76952_ENABLED + case Type::TIBQ76952_I2C: + drivers[instance] = NEW_NOTHROW AP_BattMonitor_TIBQ76952(*this, state[instance], _params[instance]); + break; +#endif // AP_BATTERY_TIBQ76952_ENABLED case Type::NONE: default: break; @@ -1198,21 +1204,21 @@ bool AP_BattMonitor::get_state_of_health_pct(uint8_t instance, uint8_t &soh_pct) return drivers[instance]->get_state_of_health_pct(soh_pct); } -// Enable/Disable (Turn on/off) MPPT power to all backends who are MPPTs -void AP_BattMonitor::MPPT_set_powered_state_to_all(const bool power_on) +// Enable/Disable (Turn on/off) power to all backends who are MPPTs or BMSs +void AP_BattMonitor::set_powered_state_to_all(const bool power_on) { for (uint8_t i=0; i < _num_instances; i++) { - MPPT_set_powered_state(i, power_on); + set_powered_state(i, power_on); } } -// Enable/Disable (Turn on/off) MPPT power. When disabled, the MPPT does not +// Enable/Disable (Turn on/off) power. When disabled, the MPPT or BMS does not // supply energy to the system regardless if it's capable to or not. When enabled // it will supply energy if available. -void AP_BattMonitor::MPPT_set_powered_state(const uint8_t instance, const bool power_on) +void AP_BattMonitor::set_powered_state(const uint8_t instance, const bool power_on) { if (instance < _num_instances && drivers[instance] != nullptr) { - drivers[instance]->mppt_set_powered_state(power_on); + drivers[instance]->set_powered_state(power_on); } } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 4c410b0dce62f..1aedb543b5e3f 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -82,6 +82,14 @@ class AP_BattMonitor Critical }; + // power states + enum class ChargingState : uint8_t { + UNKNOWN = 0, + IDLE, + CHARGING, + DISCHARGING + }; + // Battery monitor driver types using Type = AP_BattMonitor_Params::Type; @@ -128,6 +136,7 @@ class AP_BattMonitor bool has_time_remaining; // time_remaining is only valid if this is true uint8_t state_of_health_pct; // state of health (SOH) in percent bool has_state_of_health_pct; // state_of_health_pct is only valid if this is true + ChargingState charging_state; // Charging state (unknown, idle, charging, discharging) uint8_t instance; // instance number of this backend Type type; // allocated instance type const struct AP_Param::GroupInfo *var_info; @@ -224,7 +233,7 @@ class AP_BattMonitor // get once cell voltage (for scripting) bool get_cell_voltage(uint8_t instance, uint8_t cell, float &voltage) const; - + // temperature bool get_temperature(float &temperature) const { return get_temperature(temperature, AP_BATT_PRIMARY_INSTANCE); } bool get_temperature(float &temperature, const uint8_t instance) const; @@ -233,9 +242,9 @@ class AP_BattMonitor bool set_temperature_by_serial_number(const float temperature, const int32_t serial_number); #endif - // MPPT Control (Solar panels) - void MPPT_set_powered_state_to_all(const bool power_on); - void MPPT_set_powered_state(const uint8_t instance, const bool power_on); + // Set powered state (Solar Panels, BMS) + void set_powered_state_to_all(const bool power_on); + void set_powered_state(const uint8_t instance, const bool power_on); bool option_is_set(uint8_t instance, AP_BattMonitor_Params::Options option) const; @@ -265,6 +274,10 @@ class AP_BattMonitor // return true if state of health (as a percentage) can be provided and fills in soh_pct argument bool get_state_of_health_pct(uint8_t instance, uint8_t &soh_pct) const; + // get charging state (idle, charging, discharging) + ChargingState get_charging_state() const { return get_charging_state(AP_BATT_PRIMARY_INSTANCE); } + ChargingState get_charging_state(uint8_t instance) const { return state[instance].charging_state; } + static const struct AP_Param::GroupInfo var_info[]; #if AP_BATTERY_SCRIPTING_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h index d6de49a9dea5e..41a581ef1f0ac 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h @@ -89,8 +89,8 @@ class AP_BattMonitor_Backend void Log_Write_BAT(const uint8_t instance, const uint64_t time_us) const; void Log_Write_BCL(const uint8_t instance, const uint64_t time_us) const; - // set desired MPPT powered state (enabled/disabled) - virtual void mppt_set_powered_state(bool power_on) {}; + // set desired powered state (enabled/disabled) + virtual void set_powered_state(bool power_on) {}; // Update an ESC telemetry channel's power information void update_esc_telem_outbound(); diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp index 807be4d84bd7d..27a4429618e7e 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp @@ -205,9 +205,9 @@ void AP_BattMonitor_DroneCAN::handle_mppt_stream(const mppt_Stream &msg) // Boot/Power-up event if (option_is_set(AP_BattMonitor_Params::Options::MPPT_Power_On_At_Boot)) { - mppt_set_powered_state(true); + set_powered_state(true); } else if (option_is_set(AP_BattMonitor_Params::Options::MPPT_Power_Off_At_Boot)) { - mppt_set_powered_state(false); + set_powered_state(false); } } @@ -370,25 +370,25 @@ void AP_BattMonitor_DroneCAN::mppt_check_powered_state() { if ((_mppt.powered_state_remote_ms != 0) && (AP_HAL::millis() - _mppt.powered_state_remote_ms >= 1000)) { // there's already a set attempt that didnt' respond. Retry at 1Hz - mppt_set_powered_state(_mppt.powered_state); + set_powered_state(_mppt.powered_state); } // check if vehicle armed state has changed const bool vehicle_armed = hal.util->get_soft_armed(); if ((!_mppt.vehicle_armed_last && vehicle_armed) && option_is_set(AP_BattMonitor_Params::Options::MPPT_Power_On_At_Arm)) { // arm event - mppt_set_powered_state(true); + set_powered_state(true); } else if ((_mppt.vehicle_armed_last && !vehicle_armed) && option_is_set(AP_BattMonitor_Params::Options::MPPT_Power_Off_At_Disarm)) { // disarm event - mppt_set_powered_state(false); + set_powered_state(false); } _mppt.vehicle_armed_last = vehicle_armed; } -// request MPPT board to power on or off +// request MPPT or BMS board to power on or off // power_on should be true to power on the MPPT, false to power off // force should be true to force sending the state change request to the MPPT -void AP_BattMonitor_DroneCAN::mppt_set_powered_state(bool power_on) +void AP_BattMonitor_DroneCAN::set_powered_state(bool power_on) { if (_ap_dronecan == nullptr || !_mppt.is_detected) { return; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h index 7d34b91acf1fa..d36b5bc8afe93 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h @@ -53,7 +53,7 @@ class AP_BattMonitor_DroneCAN : public AP_BattMonitor_Backend static void handle_battery_info_aux_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_equipment_power_BatteryInfoAux &msg); static void handle_mppt_stream_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const mppt_Stream &msg); - void mppt_set_powered_state(bool power_on) override; + void set_powered_state(bool power_on) override; // reset remaining percentage to given value bool reset_remaining(float percentage) override; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index 821f60cd8eb0b..f94cf0c5030ac 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp @@ -52,6 +52,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { // @Values: 29:Scripting // @Values: 30:INA3221 // @Values: 31:Analog Current Only + // @Values: 32:TIBQ76952-I2C (Periph only) // @User: Standard // @RebootRequired: True AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, int8_t(AP_BattMonitor::Type::NONE), AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h index ea61afe148254..e94a09a35d4b9 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h @@ -43,6 +43,7 @@ class AP_BattMonitor_Params { Scripting = 29, INA3221 = 30, ANALOG_CURRENT_ONLY = 31, + TIBQ76952_I2C = 32, }; // low voltage sources (used for BATT_LOW_TYPE parameter) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_TIBQ76952.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_TIBQ76952.cpp new file mode 100644 index 0000000000000..247e26c072222 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_TIBQ76952.cpp @@ -0,0 +1,928 @@ +#include "AP_BattMonitor_config.h" + +#if AP_BATTERY_TIBQ76952_ENABLED + +#include +#include +#include "AP_BattMonitor_TIBQ76952.h" +#include "stdio.h" + +extern const AP_HAL::HAL& hal; + +#ifndef AP_BATTMON_TIBQ76952_BUS +#define AP_BATTMON_TIBQ76952_BUS 0 +#endif +#ifndef AP_BATTMON_TIBQ76952_ADDR +#define AP_BATTMON_TIBQ76952_ADDR 0x08 +#endif + +// Data Memory registers Name in TRM +#define TIBQ769x2_Cell1Gain 0x9180 // Calibration:Voltage:Cell 1 Gain +#define TIBQ769x2_Cell2Gain 0x9182 // Calibration:Voltage:Cell 2 Gain +#define TIBQ769x2_Cell3Gain 0x9184 // Calibration:Voltage:Cell 3 Gain +#define TIBQ769x2_Cell4Gain 0x9186 // Calibration:Voltage:Cell 4 Gain +#define TIBQ769x2_Cell5Gain 0x9188 // Calibration:Voltage:Cell 5 Gain +#define TIBQ769x2_Cell6Gain 0x918A // Calibration:Voltage:Cell 6 Gain +#define TIBQ769x2_Cell7Gain 0x918C // Calibration:Voltage:Cell 7 Gain +#define TIBQ769x2_Cell8Gain 0x918E // Calibration:Voltage:Cell 8 Gain +#define TIBQ769x2_Cell9Gain 0x9190 // Calibration:Voltage:Cell 9 Gain +#define TIBQ769x2_Cell10Gain 0x9192 // Calibration:Voltage:Cell 10 Gain +#define TIBQ769x2_Cell11Gain 0x9194 // Calibration:Voltage:Cell 11 Gain +#define TIBQ769x2_Cell12Gain 0x9196 // Calibration:Voltage:Cell 12 Gain +#define TIBQ769x2_Cell13Gain 0x9198 // Calibration:Voltage:Cell 13 Gain +#define TIBQ769x2_Cell14Gain 0x919A // Calibration:Voltage:Cell 14 Gain +#define TIBQ769x2_Cell15Gain 0x919C // Calibration:Voltage:Cell 15 Gain +#define TIBQ769x2_Cell16Gain 0x919E // Calibration:Voltage:Cell 16 Gain +#define TIBQ769x2_PackGain 0x91A0 // Calibration:Voltage:Pack Gain +#define TIBQ769x2_TOSGain 0x91A2 // Calibration:Voltage:TOS Gain +#define TIBQ769x2_LDGain 0x91A4 // Calibration:Voltage:LD Gain +#define TIBQ769x2_ADCGain 0x91A6 // Calibration:Voltage:ADC Gain +#define TIBQ769x2_CCGain 0x91A8 // Calibration:Current:CC Gain +#define TIBQ769x2_CapacityGain 0x91AC // Calibration:Current:Capacity Gain +#define TIBQ769x2_VcellOffset 0x91B0 // Calibration:Vcell Offset:Vcell Offset +#define TIBQ769x2_VdivOffset 0x91B2 // Calibration:V Divider Offset:Vdiv Offset +#define TIBQ769x2_CoulombCounterOffsetSamples 0x91C6 // Calibration:Current Offset:Coulomb Counter Offset Samples +#define TIBQ769x2_BoardOffset 0x91C8 // Calibration:Current Offset:Board Offset +#define TIBQ769x2_InternalTempOffset 0x91CA // Calibration:Temperature:Internal Temp Offset +#define TIBQ769x2_CFETOFFTempOffset 0x91CB // Calibration:Temperature:CFETOFF Temp Offset +#define TIBQ769x2_DFETOFFTempOffset 0x91CC // Calibration:Temperature:DFETOFF Temp Offset +#define TIBQ769x2_ALERTTempOffset 0x91CD // Calibration:Temperature:ALERT Temp Offset +#define TIBQ769x2_TS1TempOffset 0x91CE // Calibration:Temperature:TS1 Temp Offset +#define TIBQ769x2_TS2TempOffset 0x91CF // Calibration:Temperature:TS2 Temp Offset +#define TIBQ769x2_TS3TempOffset 0x91D0 // Calibration:Temperature:TS3 Temp Offset +#define TIBQ769x2_HDQTempOffset 0x91D1 // Calibration:Temperature:HDQ Temp Offset +#define TIBQ769x2_DCHGTempOffset 0x91D2 // Calibration:Temperature:DCHG Temp Offset +#define TIBQ769x2_DDSGTempOffset 0x91D3 // Calibration:Temperature:DDSG Temp Offset +#define TIBQ769x2_IntGain 0x91E2 // Calibration:Internal Temp Model:Int Gain +#define TIBQ769x2_Intbaseoffset 0x91E4 // Calibration:Internal Temp Model:Int base offset +#define TIBQ769x2_IntMaximumAD 0x91E6 // Calibration:Internal Temp Model:Int Maximum AD +#define TIBQ769x2_IntMaximumTemp 0x91E8 // Calibration:Internal Temp Model:Int Maximum Temp +#define TIBQ769x2_T18kCoeffa1 0x91EA // Calibration:18K Temperature Model:Coeff a1 +#define TIBQ769x2_T18kCoeffa2 0x91EC // Calibration:18K Temperature Model:Coeff a2 +#define TIBQ769x2_T18kCoeffa3 0x91EE // Calibration:18K Temperature Model:Coeff a3 +#define TIBQ769x2_T18kCoeffa4 0x91F0 // Calibration:18K Temperature Model:Coeff a4 +#define TIBQ769x2_T18kCoeffa5 0x91F2 // Calibration:18K Temperature Model:Coeff a5 +#define TIBQ769x2_T18kCoeffb1 0x91F4 // Calibration:18K Temperature Model:Coeff b1 +#define TIBQ769x2_T18kCoeffb2 0x91F6 // Calibration:18K Temperature Model:Coeff b2 +#define TIBQ769x2_T18kCoeffb3 0x91F8 // Calibration:18K Temperature Model:Coeff b3 +#define TIBQ769x2_T18kCoeffb4 0x91FA // Calibration:18K Temperature Model:Coeff b4 +#define TIBQ769x2_T18kAdc0 0x91FE // Calibration:18K Temperature Model:Adc0 +#define TIBQ769x2_T180kCoeffa1 0x9200 // Calibration:180K Temperature Model:Coeff a1 +#define TIBQ769x2_T180kCoeffa2 0x9202 // Calibration:180K Temperature Model:Coeff a2 +#define TIBQ769x2_T180kCoeffa3 0x9204 // Calibration:180K Temperature Model:Coeff a3 +#define TIBQ769x2_T180kCoeffa4 0x9206 // Calibration:180K Temperature Model:Coeff a4 +#define TIBQ769x2_T180kCoeffa5 0x9208 // Calibration:180K Temperature Model:Coeff a5 +#define TIBQ769x2_T180kCoeffb1 0x920A // Calibration:180K Temperature Model:Coeff b1 +#define TIBQ769x2_T180kCoeffb2 0x920C // Calibration:180K Temperature Model:Coeff b2 +#define TIBQ769x2_T180kCoeffb3 0x920E // Calibration:180K Temperature Model:Coeff b3 +#define TIBQ769x2_T180kCoeffb4 0x9210 // Calibration:180K Temperature Model:Coeff b4 +#define TIBQ769x2_T180kAdc0 0x9214 // Calibration:180K Temperature Model:Adc0 +#define TIBQ769x2_CustomCoeffa1 0x9216 // Calibration:Custom Temperature Model:Coeff a1 +#define TIBQ769x2_CustomCoeffa2 0x9218 // Calibration:Custom Temperature Model:Coeff a2 +#define TIBQ769x2_CustomCoeffa3 0x921A // Calibration:Custom Temperature Model:Coeff a3 +#define TIBQ769x2_CustomCoeffa4 0x921C // Calibration:Custom Temperature Model:Coeff a4 +#define TIBQ769x2_CustomCoeffa5 0x921E // Calibration:Custom Temperature Model:Coeff a5 +#define TIBQ769x2_CustomCoeffb1 0x9220 // Calibration:Custom Temperature Model:Coeff b1 +#define TIBQ769x2_CustomCoeffb2 0x9222 // Calibration:Custom Temperature Model:Coeff b2 +#define TIBQ769x2_CustomCoeffb3 0x9224 // Calibration:Custom Temperature Model:Coeff b3 +#define TIBQ769x2_CustomCoeffb4 0x9226 // Calibration:Custom Temperature Model:Coeff b4 +#define TIBQ769x2_CustomRc0 0x9228 // Calibration:Custom Temperature Model:Rc0 +#define TIBQ769x2_CustomAdc0 0x922A // Calibration:Custom Temperature Model:Adc0 +#define TIBQ769x2_CoulombCounterDeadband 0x922D // Calibration:Current Deadband:Coulomb Counter Deadband +#define TIBQ769x2_CUVThresholdOverride 0x91D4 // Calibration:CUV:CUV Threshold Override +#define TIBQ769x2_COVThresholdOverride 0x91D6 // Calibration:COV:COV Threshold Override +#define TIBQ769x2_MinBlowFuseVoltage 0x9231 // Settings:Fuse:Min Blow Fuse Voltage +#define TIBQ769x2_FuseBlowTimeout 0x9233 // Settings:Fuse:Fuse Blow Timeout +#define TIBQ769x2_PowerConfig 0x9234 // Settings:Configuration:Power Config +#define TIBQ769x2_REG12Config 0x9236 // Settings:Configuration:REG12 Config +#define TIBQ769x2_REG0Config 0x9237 // Settings:Configuration:REG0 Config +#define TIBQ769x2_HWDRegulatorOptions 0x9238 // Settings:Configuration:HWD Regulator Options +#define TIBQ769x2_CommType 0x9239 // Settings:Configuration:Comm Type +#define TIBQ769x2_I2CAddress 0x923A // Settings:Configuration:I2C Address +#define TIBQ769x2_SPIConfiguration 0x923C // Settings:Configuration:SPI Configuration +#define TIBQ769x2_CommIdleTime 0x923D // Settings:Configuration:Comm Idle Time +#define TIBQ769x2_CFETOFFPinConfig 0x92FA // Settings:Configuration:CFETOFF Pin Config +#define TIBQ769x2_DFETOFFPinConfig 0x92FB // Settings:Configuration:DFETOFF Pin Config +#define TIBQ769x2_ALERTPinConfig 0x92FC // Settings:Configuration:ALERT Pin Config +#define TIBQ769x2_TS1Config 0x92FD // Settings:Configuration:TS1 Config +#define TIBQ769x2_TS2Config 0x92FE // Settings:Configuration:TS2 Config +#define TIBQ769x2_TS3Config 0x92FF // Settings:Configuration:TS3 Config +#define TIBQ769x2_HDQPinConfig 0x9300 // Settings:Configuration:HDQ Pin Config +#define TIBQ769x2_DCHGPinConfig 0x9301 // Settings:Configuration:DCHG Pin Config +#define TIBQ769x2_DDSGPinConfig 0x9302 // Settings:Configuration:DDSG Pin Config +#define TIBQ769x2_DAConfiguration 0x9303 // Settings:Configuration:DA Configuration +#define TIBQ769x2_VCellMode 0x9304 // Settings:Configuration:Vcell Mode +#define TIBQ769x2_CC3Samples 0x9307 // Settings:Configuration:CC3 Samples +#define TIBQ769x2_ProtectionConfiguration 0x925F // Settings:Protection:Protection Configuration +#define TIBQ769x2_EnabledProtectionsA 0x9261 // Settings:Protection:Enabled Protections A +#define TIBQ769x2_EnabledProtectionsB 0x9262 // Settings:Protection:Enabled Protections B +#define TIBQ769x2_EnabledProtectionsC 0x9263 // Settings:Protection:Enabled Protections C +#define TIBQ769x2_CHGFETProtectionsA 0x9265 // Settings:Protection:CHG FET Protections A +#define TIBQ769x2_CHGFETProtectionsB 0x9266 // Settings:Protection:CHG FET Protections B +#define TIBQ769x2_CHGFETProtectionsC 0x9267 // Settings:Protection:CHG FET Protections C +#define TIBQ769x2_DSGFETProtectionsA 0x9269 // Settings:Protection:DSG FET Protections A +#define TIBQ769x2_DSGFETProtectionsB 0x926A // Settings:Protection:DSG FET Protections B +#define TIBQ769x2_DSGFETProtectionsC 0x926B // Settings:Protection:DSG FET Protections C +#define TIBQ769x2_BodyDiodeThreshold 0x9273 // Settings:Protection:Body Diode Threshold +#define TIBQ769x2_DefaultAlarmMask 0x926D // Settings:Alarm:Default Alarm Mask +#define TIBQ769x2_SFAlertMaskA 0x926F // Settings:Alarm:SF Alert Mask A +#define TIBQ769x2_SFAlertMaskB 0x9270 // Settings:Alarm:SF Alert Mask B +#define TIBQ769x2_SFAlertMaskC 0x9271 // Settings:Alarm:SF Alert Mask C +#define TIBQ769x2_PFAlertMaskA 0x92C4 // Settings:Alarm:PF Alert Mask A +#define TIBQ769x2_PFAlertMaskB 0x92C5 // Settings:Alarm:PF Alert Mask B +#define TIBQ769x2_PFAlertMaskC 0x92C6 // Settings:Alarm:PF Alert Mask C +#define TIBQ769x2_PFAlertMaskD 0x92C7 // Settings:Alarm:PF Alert Mask D +#define TIBQ769x2_EnabledPFA 0x92C0 // Settings:Permanent Failure:Enabled PF A +#define TIBQ769x2_EnabledPFB 0x92C1 // Settings:Permanent Failure:Enabled PF B +#define TIBQ769x2_EnabledPFC 0x92C2 // Settings:Permanent Failure:Enabled PF C +#define TIBQ769x2_EnabledPFD 0x92C3 // Settings:Permanent Failure:Enabled PF D +#define TIBQ769x2_FETOptions 0x9308 // Settings:FET:FET Options +#define TIBQ769x2_ChgPumpControl 0x9309 // Settings:FET:Chg Pump Control +#define TIBQ769x2_PrechargeStartVoltage 0x930A // Settings:FET:Precharge Start Voltage +#define TIBQ769x2_PrechargeStopVoltage 0x930C // Settings:FET:Precharge Stop Voltage +#define TIBQ769x2_PredischargeTimeout 0x930E // Settings:FET:Predischarge Timeout +#define TIBQ769x2_PredischargeStopDelta 0x930F // Settings:FET:Predischarge Stop Delta +#define TIBQ769x2_DsgCurrentThreshold 0x9310 // Settings:Current Thresholds:Dsg Current Threshold +#define TIBQ769x2_ChgCurrentThreshold 0x9312 // Settings:Current Thresholds:Chg Current Threshold +#define TIBQ769x2_CheckTime 0x9314 // Settings:Cell Open-Wire:Check Time +#define TIBQ769x2_Cell1Interconnect 0x9315 // Settings:Interconnect Resistances:Cell 1 Interconnect +#define TIBQ769x2_Cell2Interconnect 0x9317 // Settings:Interconnect Resistances:Cell 2 Interconnect +#define TIBQ769x2_Cell3Interconnect 0x9319 // Settings:Interconnect Resistances:Cell 3 Interconnect +#define TIBQ769x2_Cell4Interconnect 0x931B // Settings:Interconnect Resistances:Cell 4 Interconnect +#define TIBQ769x2_Cell5Interconnect 0x931D // Settings:Interconnect Resistances:Cell 5 Interconnect +#define TIBQ769x2_Cell6Interconnect 0x931F // Settings:Interconnect Resistances:Cell 6 Interconnect +#define TIBQ769x2_Cell7Interconnect 0x9321 // Settings:Interconnect Resistances:Cell 7 Interconnect +#define TIBQ769x2_Cell8Interconnect 0x9323 // Settings:Interconnect Resistances:Cell 8 Interconnect +#define TIBQ769x2_Cell9Interconnect 0x9325 // Settings:Interconnect Resistances:Cell 9 Interconnect +#define TIBQ769x2_Cell10Interconnect 0x9327 // Settings:Interconnect Resistances:Cell 10 Interconnect +#define TIBQ769x2_Cell11Interconnect 0x9329 // Settings:Interconnect Resistances:Cell 11 Interconnect +#define TIBQ769x2_Cell12Interconnect 0x932B // Settings:Interconnect Resistances:Cell 12 Interconnect +#define TIBQ769x2_Cell13Interconnect 0x932D // Settings:Interconnect Resistances:Cell 13 Interconnect +#define TIBQ769x2_Cell14Interconnect 0x932F // Settings:Interconnect Resistances:Cell 14 Interconnect +#define TIBQ769x2_Cell15Interconnect 0x9331 // Settings:Interconnect Resistances:Cell 15 Interconnect +#define TIBQ769x2_Cell16Interconnect 0x9333 // Settings:Interconnect Resistances:Cell 16 Interconnect +#define TIBQ769x2_MfgStatusInit 0x9343 // Settings:Manufacturing:Mfg Status Init +#define TIBQ769x2_BalancingConfiguration 0x9335 // Settings:Cell Balancing Config:Balancing Configuration +#define TIBQ769x2_MinCellTemp 0x9336 // Settings:Cell Balancing Config:Min Cell Temp +#define TIBQ769x2_MaxCellTemp 0x9337 // Settings:Cell Balancing Config:Max Cell Temp +#define TIBQ769x2_MaxInternalTemp 0x9338 // Settings:Cell Balancing Config:Max Internal Temp +#define TIBQ769x2_CellBalanceInterval 0x9339 // Settings:Cell Balancing Config:Cell Balance Interval +#define TIBQ769x2_CellBalanceMaxCells 0x933A // Settings:Cell Balancing Config:Cell Balance Max Cells +#define TIBQ769x2_CellBalanceMinCellVCharge 0x933B // Settings:Cell Balancing Config:Cell Balance Min Cell V (Charge) +#define TIBQ769x2_CellBalanceMinDeltaCharge 0x933D // Settings:Cell Balancing Config:Cell Balance Min Delta (Charge) +#define TIBQ769x2_CellBalanceStopDeltaCharge 0x933E // Settings:Cell Balancing Config:Cell Balance Stop Delta (Charge) +#define TIBQ769x2_CellBalanceMinCellVRelax 0x933F // Settings:Cell Balancing Config:Cell Balance Min Cell V (Relax) +#define TIBQ769x2_CellBalanceMinDeltaRelax 0x9341 // Settings:Cell Balancing Config:Cell Balance Min Delta (Relax) +#define TIBQ769x2_CellBalanceStopDeltaRelax 0x9342 // Settings:Cell Balancing Config:Cell Balance Stop Delta (Relax) +#define TIBQ769x2_ShutdownCellVoltage 0x923F // Power:Shutdown:Shutdown Cell Voltage +#define TIBQ769x2_ShutdownStackVoltage 0x9241 // Power:Shutdown:Shutdown Stack Voltage +#define TIBQ769x2_LowVShutdownDelay 0x9243 // Power:Shutdown:Low V Shutdown Delay +#define TIBQ769x2_ShutdownTemperature 0x9244 // Power:Shutdown:Shutdown Temperature +#define TIBQ769x2_ShutdownTemperatureDelay 0x9245 // Power:Shutdown:Shutdown Temperature Delay +#define TIBQ769x2_FETOffDelay 0x9252 // Power:Shutdown:FET Off Delay +#define TIBQ769x2_ShutdownCommandDelay 0x9253 // Power:Shutdown:Shutdown Command Delay +#define TIBQ769x2_AutoShutdownTime 0x9254 // Power:Shutdown:Auto Shutdown Time +#define TIBQ769x2_RAMFailShutdownTime 0x9255 // Power:Shutdown:RAM Fail Shutdown Time +#define TIBQ769x2_SleepCurrent 0x9248 // Power:Sleep:Sleep Current +#define TIBQ769x2_VoltageTime 0x924A // Power:Sleep:Voltage Time +#define TIBQ769x2_WakeComparatorCurrent 0x924B // Power:Sleep:Wake Comparator Current +#define TIBQ769x2_SleepHysteresisTime 0x924D // Power:Sleep:Sleep Hysteresis Time +#define TIBQ769x2_SleepChargerVoltageThreshold 0x924E // Power:Sleep:Sleep Charger Voltage Threshold +#define TIBQ769x2_SleepChargerPACKTOSDelta 0x9250 // Power:Sleep:Sleep Charger PACK-TOS Delta +#define TIBQ769x2_ConfigRAMSignature 0x91E0 // System Data:Integrity:Config RAM Signature +#define TIBQ769x2_CUVThreshold 0x9275 // Protections:CUV:Threshold +#define TIBQ769x2_CUVDelay 0x9276 // Protections:CUV:Delay +#define TIBQ769x2_CUVRecoveryHysteresis 0x927B // Protections:CUV:Recovery Hysteresis +#define TIBQ769x2_COVThreshold 0x9278 // Protections:COV:Threshold +#define TIBQ769x2_COVDelay 0x9279 // Protections:COV:Delay +#define TIBQ769x2_COVRecoveryHysteresis 0x927C // Protections:COV:Recovery Hysteresis +#define TIBQ769x2_COVLLatchLimit 0x927D // Protections:COVL:Latch Limit +#define TIBQ769x2_COVLCounterDecDelay 0x927E // Protections:COVL:Counter Dec Delay +#define TIBQ769x2_COVLRecoveryTime 0x927F // Protections:COVL:Recovery Time +#define TIBQ769x2_OCCThreshold 0x9280 // Protections:OCC:Threshold +#define TIBQ769x2_OCCDelay 0x9281 // Protections:OCC:Delay +#define TIBQ769x2_OCCRecoveryThreshold 0x9288 // Protections:OCC:Recovery Threshold +#define TIBQ769x2_OCCPACKTOSDelta 0x92B0 // Protections:OCC:PACK-TOS Delta +#define TIBQ769x2_OCD1Threshold 0x9282 // Protections:OCD1:Threshold +#define TIBQ769x2_OCD1Delay 0x9283 // Protections:OCD1:Delay +#define TIBQ769x2_OCD2Threshold 0x9284 // Protections:OCD2:Threshold +#define TIBQ769x2_OCD2Delay 0x9285 // Protections:OCD2:Delay +#define TIBQ769x2_SCDThreshold 0x9286 // Protections:SCD:Threshold +#define TIBQ769x2_SCDDelay 0x9287 // Protections:SCD:Delay +#define TIBQ769x2_SCDRecoveryTime 0x9294 // Protections:SCD:Recovery Time +#define TIBQ769x2_OCD3Threshold 0x928A // Protections:OCD3:Threshold +#define TIBQ769x2_OCD3Delay 0x928C // Protections:OCD3:Delay +#define TIBQ769x2_OCDRecoveryThreshold 0x928D // Protections:OCD:Recovery Threshold +#define TIBQ769x2_OCDLLatchLimit 0x928F // Protections:OCDL:Latch Limit +#define TIBQ769x2_OCDLCounterDecDelay 0x9290 // Protections:OCDL:Counter Dec Delay +#define TIBQ769x2_OCDLRecoveryTime 0x9291 // Protections:OCDL:Recovery Time +#define TIBQ769x2_OCDLRecoveryThreshold 0x9292 // Protections:OCDL:Recovery Threshold +#define TIBQ769x2_SCDLLatchLimit 0x9295 // Protections:SCDL:Latch Limit +#define TIBQ769x2_SCDLCounterDecDelay 0x9296 // Protections:SCDL:Counter Dec Delay +#define TIBQ769x2_SCDLRecoveryTime 0x9297 // Protections:SCDL:Recovery Time +#define TIBQ769x2_SCDLRecoveryThreshold 0x9298 // Protections:SCDL:Recovery Threshold +#define TIBQ769x2_OTCThreshold 0x929A // Protections:OTC:Threshold +#define TIBQ769x2_OTCDelay 0x920B // Protections:OTC:Delay +#define TIBQ769x2_OTCRecovery 0x929C // Protections:OTC:Recovery +#define TIBQ769x2_OTDThreshold 0x929D // Protections:OTD:Threshold +#define TIBQ769x2_OTDDelay 0x929E // Protections:OTD:Delay +#define TIBQ769x2_OTDRecovery 0x929F // Protections:OTD:Recovery +#define TIBQ769x2_OTFThreshold 0x92A0 // Protections:OTF:Threshold +#define TIBQ769x2_OTFDelay 0x92A1 // Protections:OTF:Delay +#define TIBQ769x2_OTFRecovery 0x92A2 // Protections:OTF:Recovery +#define TIBQ769x2_OTINTThreshold 0x92A3 // Protections:OTINT:Threshold +#define TIBQ769x2_OTINTDelay 0x92A4 // Protections:OTINT:Delay +#define TIBQ769x2_OTINTRecovery 0x92A5 // Protections:OTINT:Recovery +#define TIBQ769x2_UTCThreshold 0x92A6 // Protections:UTC:Threshold +#define TIBQ769x2_UTCDelay 0x92A7 // Protections:UTC:Delay +#define TIBQ769x2_UTCRecovery 0x92A8 // Protections:UTC:Recovery +#define TIBQ769x2_UTDThreshold 0x92A9 // Protections:UTD:Threshold +#define TIBQ769x2_UTDDelay 0x92AA // Protections:UTD:Delay +#define TIBQ769x2_UTDRecovery 0x92AB // Protections:UTD:Recovery +#define TIBQ769x2_UTINTThreshold 0x92AC // Protections:UTINT:Threshold +#define TIBQ769x2_UTINTDelay 0x92AD // Protections:UTINT:Delay +#define TIBQ769x2_UTINTRecovery 0x92AE // Protections:UTINT:Recovery +#define TIBQ769x2_ProtectionsRecoveryTime 0x92AF // Protections:Recovery:Time +#define TIBQ769x2_HWDDelay 0x92B2 // Protections:HWD:Delay +#define TIBQ769x2_LoadDetectActiveTime 0x92B4 // Protections:Load Detect:Active Time +#define TIBQ769x2_LoadDetectRetryDelay 0x92B5 // Protections:Load Detect:Retry Delay +#define TIBQ769x2_LoadDetectTimeout 0x92B6 // Protections:Load Detect:Timeout +#define TIBQ769x2_PTOChargeThreshold 0x92BA // Protections:PTO:Charge Threshold +#define TIBQ769x2_PTODelay 0x92BC // Protections:PTO:Delay +#define TIBQ769x2_PTOReset 0x92BE // Protections:PTO:Reset +#define TIBQ769x2_CUDEPThreshold 0x92C8 // Permanent Fail:CUDEP:Threshold +#define TIBQ769x2_CUDEPDelay 0x92CA // Permanent Fail:CUDEP:Delay +#define TIBQ769x2_SUVThreshold 0x92CB // Permanent Fail:SUV:Threshold +#define TIBQ769x2_SUVDelay 0x92CD // Permanent Fail:SUV:Delay +#define TIBQ769x2_SOVThreshold 0x92CE // Permanent Fail:SOV:Threshold +#define TIBQ769x2_SOVDelay 0x92D0 // Permanent Fail:SOV:Delay +#define TIBQ769x2_TOSSThreshold 0x92D1 // Permanent Fail:TOS:Threshold +#define TIBQ769x2_TOSSDelay 0x92D3 // Permanent Fail:TOS:Delay +#define TIBQ769x2_SOCCThreshold 0x92D4 // Permanent Fail:SOCC:Threshold +#define TIBQ769x2_SOCCDelay 0x92D6 // Permanent Fail:SOCC:Delay +#define TIBQ769x2_SOCDThreshold 0x92D7 // Permanent Fail:SOCD:Threshold +#define TIBQ769x2_SOCDDelay 0x92D9 // Permanent Fail:SOCD:Delay +#define TIBQ769x2_SOTThreshold 0x92DA // Permanent Fail:SOT:Threshold +#define TIBQ769x2_SOTDelay 0x92DB // Permanent Fail:SOT:Delay +#define TIBQ769x2_SOTFThreshold 0x92DC // Permanent Fail:SOTF:Threshold +#define TIBQ769x2_SOTFDelay 0x92DD // Permanent Fail:SOTF:Delay +#define TIBQ769x2_VIMRCheckVoltage 0x92DE // Permanent Fail:VIMR:Check Voltage +#define TIBQ769x2_VIMRMaxRelaxCurrent 0x92E0 // Permanent Fail:VIMR:Max Relax Current +#define TIBQ769x2_VIMRThreshold 0x92E2 // Permanent Fail:VIMR:Threshold +#define TIBQ769x2_VIMRDelay 0x92E4 // Permanent Fail:VIMR:Delay +#define TIBQ769x2_VIMRRelaxMinDuration 0x92E5 // Permanent Fail:VIMR:Relax Min Duration +#define TIBQ769x2_VIMACheckVoltage 0x92E7 // Permanent Fail:VIMA:Check Voltage +#define TIBQ769x2_VIMAMinActiveCurrent 0x92E9 // Permanent Fail:VIMA:Min Active Current +#define TIBQ769x2_VIMAThreshold 0x92EB // Permanent Fail:VIMA:Threshold +#define TIBQ769x2_VIMADelay 0x92ED // Permanent Fail:VIMA:Delay +#define TIBQ769x2_CFETFOFFThreshold 0x92EE // Permanent Fail:CFETF:OFF Threshold +#define TIBQ769x2_CFETFOFFDelay 0x92F0 // Permanent Fail:CFETF:OFF Delay +#define TIBQ769x2_DFETFOFFThreshold 0x92F1 // Permanent Fail:DFETF:OFF Threshold +#define TIBQ769x2_DFETFOFFDelay 0x92F3 // Permanent Fail:DFETF:OFF Delay +#define TIBQ769x2_VSSFFailThreshold 0x92F4 // Permanent Fail:VSSF:Fail Threshold +#define TIBQ769x2_VSSFDelay 0x92F6 // Permanent Fail:VSSF:Delay +#define TIBQ769x2_PF2LVLDelay 0x92F7 // Permanent Fail:2LVL:Delay +#define TIBQ769x2_LFOFDelay 0x92F8 // Permanent Fail:LFOF:Delay +#define TIBQ769x2_HWMXDelay 0x92F9 // Permanent Fail:HWMX:Delay +#define TIBQ769x2_SecuritySettings 0x9256 // Security:Settings:Security Settings +#define TIBQ769x2_UnsealKeyStep1 0x9257 // Security:Keys:Unseal Key Step 1 +#define TIBQ769x2_UnsealKeyStep2 0x9259 // Security:Keys:Unseal Key Step 2 +#define TIBQ769x2_FullAccessKeyStep1 0x925B // Security:Keys:Full Access Key Step 1 +#define TIBQ769x2_FullAccessKeyStep2 0x925D // Security:Keys:Full Access Key Step 2 + +// Direct Commands +#define TIBQ769x2_ControlStatus 0x00 +#define TIBQ769x2_SafetyAlertA 0x02 +#define TIBQ769x2_SafetyStatusA 0x03 +#define TIBQ769x2_SafetyAlertB 0x04 +#define TIBQ769x2_SafetyStatusB 0x05 +#define TIBQ769x2_SafetyAlertC 0x06 +#define TIBQ769x2_SafetyStatusC 0x07 +#define TIBQ769x2_PFAlertA 0x0A +#define TIBQ769x2_PFStatusA 0x0B +#define TIBQ769x2_PFAlertB 0x0C +#define TIBQ769x2_PFStatusB 0x0D +#define TIBQ769x2_PFAlertC 0x0E +#define TIBQ769x2_PFStatusC 0x0F +#define TIBQ769x2_PFAlertD 0x10 +#define TIBQ769x2_PFStatusD 0x11 +#define TIBQ769x2_BatteryStatus 0x12 +#define TIBQ769x2_Cell1Voltage 0x14 +#define TIBQ769x2_Cell2Voltage 0x16 +#define TIBQ769x2_Cell3Voltage 0x18 +#define TIBQ769x2_Cell4Voltage 0x1A +#define TIBQ769x2_Cell5Voltage 0x1C +#define TIBQ769x2_Cell6Voltage 0x1E +#define TIBQ769x2_Cell7Voltage 0x20 +#define TIBQ769x2_Cell8Voltage 0x22 +#define TIBQ769x2_Cell9Voltage 0x24 +#define TIBQ769x2_Cell10Voltage 0x26 +#define TIBQ769x2_Cell11Voltage 0x28 +#define TIBQ769x2_Cell12Voltage 0x2A +#define TIBQ769x2_Cell13Voltage 0x2C +#define TIBQ769x2_Cell14Voltage 0x2E +#define TIBQ769x2_Cell15Voltage 0x30 +#define TIBQ769x2_Cell16Voltage 0x32 +#define TIBQ769x2_StackVoltage 0x34 +#define TIBQ769x2_PACKPinVoltage 0x36 +#define TIBQ769x2_LDPinVoltage 0x38 +#define TIBQ769x2_CC2Current 0x3A +#define TIBQ769x2_AlarmStatus 0x62 +#define TIBQ769x2_AlarmRawStatus 0x64 +#define TIBQ769x2_AlarmEnable 0x66 +#define TIBQ769x2_IntTemperature 0x68 +#define TIBQ769x2_CFETOFFTemperature 0x6A +#define TIBQ769x2_DFETOFFTemperature 0x6C +#define TIBQ769x2_ALERTTemperature 0x6E +#define TIBQ769x2_TS1Temperature 0x70 +#define TIBQ769x2_TS2Temperature 0x72 +#define TIBQ769x2_TS3Temperature 0x74 +#define TIBQ769x2_HDQTemperature 0x76 +#define TIBQ769x2_DCHGTemperature 0x78 +#define TIBQ769x2_DDSGTemperature 0x7A +#define TIBQ769x2_FETStatus 0x7F + +// Subcommands +#define TIBQ769x2_DEVICE_NUMBER 0x0001 +#define TIBQ769x2_FW_VERSION 0x0002 +#define TIBQ769x2_HW_VERSION 0x0003 +#define TIBQ769x2_IROM_SIG 0x0004 +#define TIBQ769x2_STATIC_CFG_SIG 0x0005 +#define TIBQ769x2_PREV_MACWRITE 0x0007 +#define TIBQ769x2_DROM_SIG 0x0009 +#define TIBQ769x2_SECURITY_KEYS 0x0035 +#define TIBQ769x2_SAVED_PF_STATUS 0x0053 +#define TIBQ769x2_MANUFACTURINGSTATUS 0x0057 +#define TIBQ769x2_MANU_DATA 0x0070 +#define TIBQ769x2_DASTATUS1 0x0071 +#define TIBQ769x2_DASTATUS2 0x0072 +#define TIBQ769x2_DASTATUS3 0x0073 +#define TIBQ769x2_DASTATUS4 0x0074 +#define TIBQ769x2_DASTATUS5 0x0075 +#define TIBQ769x2_DASTATUS6 0x0076 +#define TIBQ769x2_DASTATUS7 0x0077 +#define TIBQ769x2_CUV_SNAPSHOT 0x0080 +#define TIBQ769x2_COV_SNAPSHOT 0X0081 +#define TIBQ769x2_CB_ACTIVE_CELLS 0x0083 +#define TIBQ769x2_CB_SET_LVL 0x0084 +#define TIBQ769x2_CBSTATUS1 0x0085 +#define TIBQ769x2_CBSTATUS2 0x0086 +#define TIBQ769x2_CBSTATUS3 0x0087 +#define TIBQ769x2_FET_CONTROL 0x0097 +#define TIBQ769x2_REG12_CONTROL 0x0098 +#define TIBQ769x2_OTP_WR_CHECK 0x00A0 +#define TIBQ769x2_OTP_WRITE 0x00A1 +#define TIBQ769x2_READ_CAL1 0xF081 +#define TIBQ769x2_CAL_CUV 0xF090 +#define TIBQ769x2_CAL_COV 0xF091 + +// Command Only Subcommands +#define TIBQ769x2_EXIT_DEEPSLEEP 0x000E +#define TIBQ769x2_DEEPSLEEP 0x000F +#define TIBQ769x2_SHUTDOWN 0x0010 +#define TIBQ769x2_RESET 0x0012 //"RESET" in documentation +#define TIBQ769x2_PDSGTEST 0x001C +#define TIBQ769x2_FUSE_TOGGLE 0x001D +#define TIBQ769x2_PCHGTEST 0x001E +#define TIBQ769x2_CHGTEST 0x001F +#define TIBQ769x2_DSGTEST 0x0020 +#define TIBQ769x2_FET_ENABLE 0x0022 +#define TIBQ769x2_PF_ENABLE 0x0024 +#define TIBQ769x2_PF_RESET 0x0029 +#define TIBQ769x2_SEAL 0x0030 +#define TIBQ769x2_RESET_PASSQ 0x0082 +#define TIBQ769x2_PTO_RECOVER 0x008A +#define TIBQ769x2_SET_CFGUPDATE 0x0090 +#define TIBQ769x2_EXIT_CFGUPDATE 0x0092 +#define TIBQ769x2_DSG_PDSG_OFF 0x0093 +#define TIBQ769x2_CHG_PCHG_OFF 0x0094 +#define TIBQ769x2_ALL_FETS_OFF 0x0095 +#define TIBQ769x2_ALL_FETS_ON 0x0096 +#define TIBQ769x2_SLEEP_ENABLE 0x0099 +#define TIBQ769x2_SLEEP_DISABLE 0x009A +#define TIBQ769x2_OCDL_RECOVER 0x009B +#define TIBQ769x2_SCDL_RECOVER 0x009C +#define TIBQ769x2_LOAD_DETECT_RESTART 0x009D +#define TIBQ769x2_LOAD_DETECT_ON 0x009E +#define TIBQ769x2_LOAD_DETECT_OFF 0x009F +#define TIBQ769x2_CFETOFF_LO 0x2800 +#define TIBQ769x2_DFETOFF_LO 0x2801 +#define TIBQ769x2_ALERT_LO 0x2802 +#define TIBQ769x2_HDQ_LO 0x2806 +#define TIBQ769x2_DCHG_LO 0x2807 +#define TIBQ769x2_DDSG_LO 0x2808 +#define TIBQ769x2_CFETOFF_HI 0x2810 +#define TIBQ769x2_DFETOFF_HI 0x2811 +#define TIBQ769x2_ALERT_HI 0x2812 +#define TIBQ769x2_HDQ_HI 0x2816 +#define TIBQ769x2_DCHG_HI 0x2817 +#define TIBQ769x2_DDSG_HI 0x2818 +#define TIBQ769x2_PF_FORCE_A 0x2857 +#define TIBQ769x2_PF_FORCE_B 0x29A3 +#define TIBQ769x2_SWAP_COMM_MODE 0x29BC +#define TIBQ769x2_SWAP_TO_I2C 0x29E7 +#define TIBQ769x2_SWAP_TO_SPI 0x7C35 +#define TIBQ769x2_SWAP_TO_HDQ 0x7C40 + +// bit masks +#define MfgStatusInit_FET_EN 0x08 //bit 4 +#define FET_STATUS_DSG_FET_EN 0x04 //bit 3 +#define FET_STATUS_CHG_FET_EN 0x01 //bit 1 + +// Alarm Status bits +#define ALARM_STATUS_WAKE (1 << 0) // device is wakened from sleep mode +#define ALARM_STATUS_ADSCAN (1 << 1) // voltage ADC scan complete +#define ALARM_STATUS_CB (1 << 2) // cell balancing is active +#define ALARM_STATUS_FUSE (1 << 3) // fuse pin driven +#define ALARM_STATUS_SHUTV (1 << 4) // stack voltage is below shutdown stack voltage +#define ALARM_STATUS_XDSG (1 << 5) // discharge FET is off +#define ALARM_STATUS_XCHG (1 << 6) // charge FET is off +#define ALARM_STATUS_FULLSCAN (1 << 7) // full voltage scan complete +#define ALARM_STATUS_RSVD (1 << 8) // reserved +#define ALARM_STATUS_INITCOMP (1 << 9) // initialisation complete +#define ALARM_STATUS_INITSTART (1 << 10) // initialisation started +#define ALARM_STATUS_MSK_PFALERT (1 << 11) // masked safety alerts. set coresponding to how SF Alert Mask A~C have been set +#define ALARM_STATUS_MSK_SFALERT (1 << 12) // masked safety alerts. set coresponding to how SF Alert Mask A~C have been set +#define ALARM_STATUS_PF (1 << 13) // permanent fail status +#define ALARM_STATUS_SSA (1 << 14) // hardware safety status. set if a bit in Safety Status A is set +#define ALARM_STATUS_SSBC (1 << 15) // safety status. set if a bit in Safety Status B~C is set + +/* + TI bq76952 register definitions from datasheet SLUUBY2B + Uses direct commands (7-bit addresses) for voltage readings + Data stored in little endian byte order + */ +// Direct commands for voltage measurements (from Table 4-1) +#define REG_OTP_CHECK 0xA0 // OTP memory check register (one-time-programmable memory) +#define REG_STACK_VOLTAGE_L 0x34 // Stack (VC16 pin) voltage LSB (µV units) +#define REG_STACK_VOLTAGE_H 0x35 // Stack (VC16 pin) voltage MSB (µV units) +#define REG_PACK_VOLTAGE_L 0x36 // PACK pin voltage LSB (µV units) +#define REG_PACK_VOLTAGE_H 0x37 // PACK pin voltage MSB (µV units) +#define REG_LD_VOLTAGE_L 0x38 // LD pin voltage LSB (µV units) +#define REG_LD_VOLTAGE_H 0x39 // LD pin voltage MSB (µV units) + +// Individual cell voltage registers (mV units) +#define REG_CELL1_VOLTAGE_L 0x14 // Cell 1 voltage LSB +#define REG_CELL1_VOLTAGE_H 0x15 // Cell 1 voltage MSB + +// BQ76952 Communication Settings +#define DISABLE_TS1 1 +#define DISABLE_TS3 1 + +// battery specific definitions. These may be overwritten in hwdef.inc +#ifndef AP_BATTMON_CELL_COUNT +#define AP_BATTMON_CELL_COUNT 6 +#endif + +#define DEBUG_PRINT 1 + +#if DEBUG_PRINT + # define Debug(fmt, args ...) do {printf(fmt "\n", ## args);} while(0) +#else + # define Debug(fmt, args ...) +#endif + +// Expected device IDs +#define DEVICE_ID_TIBQ7695 0x7695 + +// Note: BQ76952 doesn't have a simple device ID register at 0x00 +// Device identification requires subcommands, not direct commands + +#ifndef HAL_BATTMON_BQ76952_MAX_VOLTAGE +#define HAL_BATTMON_BQ76952_MAX_VOLTAGE 50.0f +#endif + +// configuration settings to write during setup +const AP_BattMonitor_TIBQ76952::ConfigurationSetting AP_BattMonitor_TIBQ76952::config_settings[] { + + // 'Power Config' - 0x9234 = 0x2D80 + // Setting the DSLP_LDO bit allows the LDOs to remain active when the device goes into Deep Sleep mode + // Set wake speed bits to 00 for best performance + {TIBQ769x2_PowerConfig, 0x2D80, 2}, + + // 'REG0 Config' - set REG0_EN bit to enable pre-regulator + {TIBQ769x2_REG0Config, 0x01, 1}, + + // 'REG12 Config' - Enable REG1 with 3.3V output (0x0D for 3.3V, 0x0F for 5V) + {TIBQ769x2_REG12Config, 0x0D, 1}, + + // Set DFETOFF pin to control BOTH CHG and DSG FET - 0x92FB = 0x42 (set to 0x00 to disable) + {TIBQ769x2_DFETOFFPinConfig, 0x42, 1}, + + // Set up ALERT Pin - 0x92FC = 0x2A + // This configures the ALERT pin to drive high (REG1 voltage) when enabled. + // The ALERT pin can be used as an interrupt to the MCU when a protection has triggered or new measurements are available + {TIBQ769x2_ALERTPinConfig, 0x2A, 1}, + +#if DISABLE_TS1 + {TIBQ769x2_TS1Config, 0x00, 1}, +#else + // Set TS1 to measure Cell Temperature - 0x92FD = 0x07 + {TIBQ769x2_TS1Config, 0x07, 1}, +#endif + +#if DISABLE_TS3 + {TIBQ769x2_TS3Config, 0x00, 1}, +#else + // Set TS3 to measure FET Temperature - 0x92FF = 0x0F + {TIBQ769x2_TS3Config, 0x0F, 1}, +#endif + + // Set HDQ to measure Cell Temperature - 0x9300 = 0x00 (No thermistor installed on EVM HDQ pin) + {TIBQ769x2_HDQPinConfig, 0x00, 1}, + + // 'VCell Mode' - Enable 16 cells - 0x9304 + // Writing 0x0000 sets the default of 16 cells, but we'll calculate based on actual cell count + {TIBQ769x2_VCellMode, (1 << AP_BATTMON_CELL_COUNT) - 1, 2}, + +#if defined(DISABLE_PROTECTION_A) + {TIBQ769x2_EnabledProtectionsA, 0x00, 1}, +#else + // Enable protections in 'Enabled Protections A' 0x9261 = 0xBC + // Enables SCD (short-circuit), OCD1 (over-current in discharge), OCC (over-current in charge), + // COV (over-voltage), CUV (under-voltage) + {TIBQ769x2_EnabledProtectionsA, 0xBC, 1}, +#endif + +#if defined(DISABLE_PROTECTION_B) + {TIBQ769x2_EnabledProtectionsB, 0x00, 1}, +#else + // Enable all protections in 'Enabled Protections B' 0x9262 = 0xF7 + // Enables OTF (over-temperature FET), OTINT (internal over-temperature), OTD (over-temperature in discharge), + // OTC (over-temperature in charge), UTINT (internal under-temperature), UTD (under-temperature in discharge), UTC (under-temperature in charge) + {TIBQ769x2_EnabledProtectionsB, 0xF7, 1}, +#endif + + // 'Default Alarm Mask' - 0x926D Enables the FullScan and ADScan bits, default value = 0xF800 + {TIBQ769x2_DefaultAlarmMask, 0xF882, 2}, + + // Set up Cell Balancing Configuration - 0x9335 = 0x03 - Automated balancing while in Relax or Charge modes + {TIBQ769x2_BalancingConfiguration, 0x03, 1}, + + // Set up CUV (under-voltage) Threshold - 0x9275 = 0x31 (2479 mV) + // CUV Threshold is this value multiplied by 50.6mV + {TIBQ769x2_CUVThreshold, 0x31, 1}, + + // Set up COV (over-voltage) Threshold - 0x9278 = 0x55 (4301 mV) + // COV Threshold is this value multiplied by 50.6mV + {TIBQ769x2_COVThreshold, 0x55, 1}, + + // Set up OCC (over-current in charge) Threshold - 0x9280 = 0x05 (10 mV = 10A across 1mOhm sense resistor) Units in 2mV + {TIBQ769x2_OCCThreshold, 0x05, 1}, + + // Set up OCD1 Threshold - 0x9282 = 0x0A (20 mV = 20A across 1mOhm sense resistor) units of 2mV + {TIBQ769x2_OCD1Threshold, 0x0A, 1}, + + // Set up SCD Threshold - 0x9286 = 0x05 (100 mV = 100A across 1mOhm sense resistor) 0x05=100mV + {TIBQ769x2_SCDThreshold, 0x05, 1}, + + // Set up SCD Delay - 0x9287 = 0x03 (30 us) Enabled with a delay of (value - 1) * 15 μs; min value of 1 + {TIBQ769x2_SCDDelay, 0x03, 1}, + + // Set up SCDL Latch Limit to 1 to set SCD recovery only with load removal 0x9295 = 0x01 + // If this is not set, then SCD will recover based on time (SCD Recovery Time parameter). + {TIBQ769x2_SCDLLatchLimit, 0x01, 1}, +}; + +// initialise the TIBQ76952 battery monitor +void AP_BattMonitor_TIBQ76952::init(void) +{ + dev = hal.i2c_mgr->get_device_ptr(AP_BATTMON_TIBQ76952_BUS, AP_BATTMON_TIBQ76952_ADDR); + if (dev == nullptr) { + return; + } + + // Register periodic callback at 10hz for reading voltage + dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_TIBQ76952::timer, void)); +} + +/// read the battery_voltage, should be called at 10hz +void AP_BattMonitor_TIBQ76952::read(void) +{ + // take semaphore before accessing accumulate struct + WITH_SEMAPHORE(accumulate_sem); + + // exit immediately if no sensor updates + if (accumulate.count == 0) { + return; + } + + // copy accumulated values to state + _state.last_time_micros = AP_HAL::micros(); + _state.healthy = true; + _state.voltage = accumulate.voltage / accumulate.count; + _state.state_of_health_pct = accumulate.health_pct / accumulate.count; + _state.has_state_of_health_pct = true; + _state.current_amps = fabsf(accumulate.current / accumulate.count); + _state.temperature = accumulate.temp / accumulate.count; + const uint8_t num_cells = MIN(AP_BATTMON_CELL_COUNT, MIN(ARRAY_SIZE(_state.cell_voltages.cells), ARRAY_SIZE(accumulate.cell_voltages_mv))); + for (uint8_t i = 0; i < num_cells; i++) { + _state.cell_voltages.cells[i] = accumulate.cell_voltages_mv[i] / accumulate.count; + } + + // clear accumulate structure + accumulate = {}; +} + +// set desired powered state (enabled/disabled) by enabling/disabling discharge FET +void AP_BattMonitor_TIBQ76952::set_powered_state(bool power_on) +{ + if (!configured) { + return; + } + sub_command(power_on ? TIBQ769x2_ALL_FETS_ON : TIBQ769x2_ALL_FETS_OFF); +} + +// periodic timer callback +void AP_BattMonitor_TIBQ76952::timer(void) +{ + // configure device if required + if (!configure()) { + return; + } + + // read data from device + read_voltage_current_temperature(); + read_charging_state(); +} + +// configure device +bool AP_BattMonitor_TIBQ76952::configure() +{ + // exit immediately if already configured + if (configured) { + return true; + } + + // check device id, exit on failure + const uint32_t device_number = sub_command_read_4bytes(TIBQ769x2_DEVICE_NUMBER); + if (device_number != DEVICE_ID_TIBQ7695) { + Debug("BQ76952: Unknown device detected - ID: 0x%08lX", (unsigned long)device_number); + return false; + } + + // check device's firmware and hardware versions +#if DEBUG_PRINT + const uint32_t fw_version = sub_command_read_4bytes(TIBQ769x2_FW_VERSION); + const uint32_t hw_version = sub_command_read_4bytes(TIBQ769x2_HW_VERSION); + Debug("BQ76952 detected, fw: 0x%08lX, hw: 0x%08lX", (unsigned long)fw_version, (unsigned long)hw_version); +#endif + + // enable REG1 3.3v to provide power to the MCU + set_register(TIBQ769x2_REG12Config, 0x01, 1); + + // wake up device + sub_command(TIBQ769x2_EXIT_DEEPSLEEP); + hal.scheduler->delay(10); + sub_command(TIBQ769x2_SLEEP_DISABLE); + hal.scheduler->delay(10); + + // clear any remaining permanent failure alerts + direct_command(TIBQ769x2_PFAlertA, 0xFF, CommandType::WRITE); + direct_command(TIBQ769x2_PFAlertB, 0xFF, CommandType::WRITE); + direct_command(TIBQ769x2_PFAlertC, 0xFF, CommandType::WRITE); + direct_command(TIBQ769x2_PFAlertD, 0xFF, CommandType::WRITE); + + // enter CONFIGUPDATE mode (Subcommand 0x0090) - Required to program device RAM settings + sub_command(TIBQ769x2_SET_CFGUPDATE); + + // write configuration settings to device registers + for (uint8_t i = 0; i < ARRAY_SIZE(config_settings); i++) { + set_register(config_settings[i].reg_addr, config_settings[i].reg_data, config_settings[i].len); + } + + // exit configuration mode + sub_command(TIBQ769x2_EXIT_CFGUPDATE); + sub_command(TIBQ769x2_ALL_FETS_OFF); + sub_command(TIBQ769x2_FET_ENABLE); + + // mark configuration as complete to prevent repeated attempts + configured = true; + Debug("BQ76952: Configuration complete"); + + // report success + return true; +} + +// read battery voltage, current and temperature +void AP_BattMonitor_TIBQ76952::read_voltage_current_temperature() +{ + // exit immediately if full voltage scan has not completed (FULL_SCAN bit 7) + const uint16_t alarm_raw_status = direct_command_read_2bytes(TIBQ769x2_AlarmRawStatus); + if (alarm_raw_status & ALARM_STATUS_FULLSCAN) { + return; + } + + // get semaphore before updating accumulate structure + WITH_SEMAPHORE(accumulate_sem); + + // read stack voltage (should equal sum of cell voltages) + // we do not use the package voltage because this is floating when FETs are off + accumulate.voltage += direct_command_read_2bytes(TIBQ769x2_StackVoltage) * 0.01; + + // read individual cell voltages + for (uint8_t i = 0; i < AP_BATTMON_CELL_COUNT; i++) { + const uint16_t cell_voltage_mv = direct_command_read_2bytes(TIBQ769x2_Cell1Voltage + i*2); + accumulate.cell_voltages_mv[i] += cell_voltage_mv; + } + + // read current (positive values = discharging, negative = charging) + const int16_t cc2_current = direct_command_read_2bytes(TIBQ769x2_CC2Current); + accumulate.current += cc2_current * 0.001f; // convert to Amps + + // read temperature + const int16_t temp_internal = direct_command_read_2bytes(TIBQ769x2_IntTemperature); // 0.1K + accumulate.temp += KELVIN_TO_C(temp_internal * 0.1f); // convert to degC + + // increment number of readings + accumulate.count++; +} + +// read battery charging state (e.g. idle, charging, discharging) +void AP_BattMonitor_TIBQ76952::read_charging_state() +{ + AP_BattMonitor::ChargingState new_state = _state.charging_state; + + const uint16_t mfg_status = direct_command_read_2bytes(TIBQ769x2_MfgStatusInit); + const uint8_t fet_status = direct_command_read_1byte(TIBQ769x2_FETStatus); + + if (mfg_status & MfgStatusInit_FET_EN) { + new_state = AP_BattMonitor::ChargingState::IDLE; + if (fet_status & FET_STATUS_CHG_FET_EN && accumulate.current > 0) { + // Charging if CHG_FET bit is set + new_state = AP_BattMonitor::ChargingState::CHARGING; + } + if (fet_status & FET_STATUS_DSG_FET_EN) { + // Discharging if DSG_FET bit is set + new_state = AP_BattMonitor::ChargingState::DISCHARGING; + } + } + + _state.charging_state = new_state; +} + +// read bytes from a register. returns true on success +bool AP_BattMonitor_TIBQ76952::read_register(uint8_t reg_addr, uint8_t *reg_data, uint8_t len) const +{ + // sanity check device pointer + if (dev == nullptr) { + return false; + } + + WITH_SEMAPHORE(dev->get_semaphore()); + + // read bytes from device + return dev->read_registers(reg_addr, reg_data, len); +} + +// write a single byte to consecutive registers. returns true on success +bool AP_BattMonitor_TIBQ76952::write_register(uint8_t reg_addr, const uint8_t *reg_data, uint8_t len) const +{ + // sanity check device pointer + if (dev == nullptr) { + return false; + } + + WITH_SEMAPHORE(dev->get_semaphore()); + + // write each byte + for (uint8_t i = 0; i < len; i++) { + if (!dev->write_register(reg_addr + i, reg_data[i])) { + return false; + } + } + return true; +} + +// read or write a direct command. returns true on success +// direct commands are send using a 7-bit command address and may trigger an action or read/write a datavalue +bool AP_BattMonitor_TIBQ76952::direct_command(uint16_t command, uint16_t data, CommandType type, uint8_t *rx_data, uint8_t rx_len) const +{ + // sanity check read buffer + if (type == CommandType::READ && rx_data == nullptr) { + return false; + } + + // prepare transmit buffer + const uint8_t tx_buffer[2] {LOWBYTE(data), HIGHBYTE(data)}; + switch (type) { + case CommandType::READ: + return read_register(command, rx_data, rx_len); + case CommandType::WRITE: + return write_register(command, tx_buffer, rx_len); + } + + return false; +} + +// send a direct command to read 1 byte +uint8_t AP_BattMonitor_TIBQ76952::direct_command_read_1byte(uint16_t reg) const +{ + uint8_t rx_data; + if (direct_command(reg, 0x00, CommandType::READ, &rx_data, 1)) { + return rx_data; + } + return 0; +} + +// send a direct command to read 2 bytes +uint16_t AP_BattMonitor_TIBQ76952::direct_command_read_2bytes(uint16_t reg) const +{ + uint8_t rx_data[2]; + if (direct_command(reg, 0x00, CommandType::READ, rx_data, 2)) { + return UINT16_VALUE(rx_data[1], rx_data[0]); + } + return 0; +} + +// write 1, 2, or 4 bytes to a RAM data memory register (0x9xxx addresses) +// this includes delays so it should only be called during startup configuration +// uses subcommand protocol: write address+data to 0x3E, then checksum+length to 0x60 +void AP_BattMonitor_TIBQ76952::set_register(uint16_t reg_addr, uint32_t reg_data, uint8_t len) const +{ + // sanity check len argument + if (len != 1 && len != 2 && len != 4) { + return; + } + + // prepare transmit data + const uint8_t tx_reg_data[6] { + LOWBYTE(reg_addr), + HIGHBYTE(reg_addr), + uint8_t((reg_data >> 0) & 0xFF), + uint8_t((reg_data >> 8) & 0xFF), + uint8_t((reg_data >> 16) & 0xFF), + uint8_t((reg_data >> 24) & 0xFF) + }; + + // write sub command address + write_register(0x3E, tx_reg_data, len + 2); + hal.scheduler->delay(2); + + // write checksum and length + const uint8_t tx_buffer[2] {crc_sum_of_bytes(tx_reg_data, 3), uint8_t(len + 4)}; + write_register(0x60, tx_buffer, 2); + hal.scheduler->delay(2); +} + +// send a command-only subcommand (no data payload, no checksum) +// used for commands like ALL_FETS_ON, SLEEP_DISABLE, EXIT_CFGUPDATE, etc. +bool AP_BattMonitor_TIBQ76952::sub_command(uint16_t command) const +{ + const uint8_t tx_buffer[2] {LOWBYTE(command), HIGHBYTE(command)}; + return write_register(0x3E, tx_buffer, 2); +} + +// send a subcommand with read or write capability +// this includes delays so it should only be called during startup configuration +// READ: writes command to 0x3E, reads response from 0x40 +// WRITE: writes command+data to 0x3E, then checksum+length to 0x60 +bool AP_BattMonitor_TIBQ76952::sub_command(uint16_t command, uint16_t data, CommandType type, uint8_t *rx_data, uint8_t rx_len) const +{ + switch (type) { + case CommandType::READ: { + // read subcommand + const uint8_t tx_reg[2] {LOWBYTE(command), HIGHBYTE(command)}; + if (!write_register(0x3E, tx_reg, 2)) { + return false; + } + hal.scheduler->delay(2); + if (rx_data && rx_len > 0) { + // read into provided buffer + if (!read_register(0x40, rx_data, rx_len)) { + return false; + } + return true; + } + break; + } + case CommandType::WRITE: { + // write subcommand with 1 byte data (FET_Control, REG12_Control) + const uint8_t tx_reg[3] {LOWBYTE(command), HIGHBYTE(command), uint8_t(data & 0xFF)}; + if (!write_register(0x3E, tx_reg, 3)) { + return false; + } + hal.scheduler->delay(1); + const uint8_t tx_buffer[2] {crc_sum_of_bytes(tx_reg, 3), 0x05}; // 0x05 is combined length of registers address and data + if (!write_register(0x60, tx_buffer, 2)) { + return false; + } + hal.scheduler->delay(1); + break; + } + } + + return true; +} + +// subcommands are sent using a 16 bit command address and support block data transfers +uint32_t AP_BattMonitor_TIBQ76952::sub_command_read_4bytes(uint16_t reg) const +{ + uint8_t rx_data[4]; + if (sub_command(reg, 0x00, CommandType::READ, rx_data, 4)) { + return UINT32_VALUE(rx_data[3], rx_data[2], rx_data[1], rx_data[0]); + } + return 0; +} + +#endif // AP_BATTERY_TIBQ76952_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_TIBQ76952.h b/libraries/AP_BattMonitor/AP_BattMonitor_TIBQ76952.h new file mode 100644 index 0000000000000..48c797f94aa06 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_TIBQ76952.h @@ -0,0 +1,101 @@ +#pragma once + +#include "AP_BattMonitor_config.h" + +#if AP_BATTERY_TIBQ76952_ENABLED + +#include +#include +#include "AP_BattMonitor_Backend.h" + +class AP_BattMonitor_TIBQ76952 : public AP_BattMonitor_Backend +{ +public: + // inherit constructor + using AP_BattMonitor_Backend::AP_BattMonitor_Backend; + + // initialise + void init() override; + + // read the latest battery voltage + void read() override; + + // battery capabilities + bool has_cell_voltages() const override { return true; } + bool has_temperature() const override { return true; } + bool has_current() const override { return true; } + bool get_cycle_count(uint16_t &cycles) const override { return false; } + + // set desired powered state (enabled/disabled) by enabling/disabling discharge FET + void set_powered_state(bool power_on) override; + +protected: + + // Subcommand operation types + enum class CommandType { + READ = 0, // Read operation (R) + WRITE = 1, // Write operation with 1 byte data (W) + }; + + // periodic timer callback + void timer(); + + // configure device + // this includes delays so it should only be called during startup configuration + bool configure(); + + // read state from BMS device + void read_voltage_current_temperature(); + void read_charging_state(); + + // read bytes from a register. returns true on success + bool read_register(uint8_t reg_addr, uint8_t *reg_data, uint8_t len) const; + + // write a single byte to consecutive registers. returns true on success + bool write_register(uint8_t reg_addr, const uint8_t *reg_data, uint8_t len) const; + + // read or write a direct command. returns true on success + // direct commands are send using a 7-bit command address and may trigger an action or read/write a datavalue + bool direct_command(uint16_t command, uint16_t data, CommandType type, uint8_t *rx_data = nullptr, uint8_t rx_len = 2) const; + + // send a direct command to read 1 or 2 bytes + uint8_t direct_command_read_1byte(uint16_t reg) const; + uint16_t direct_command_read_2bytes(uint16_t reg) const; + + // write 1, 2, or 4 bytes to a RAM data memory register (0x9xxx addresses) + // this includes delays so it should only be called during startup configuration + void set_register(uint16_t reg_addr, uint32_t reg_data, uint8_t len) const; + + // send a command-only subcommand (no data payload, no checksum) + bool sub_command(uint16_t command) const; + + // send a subcommand with read or write capability + // this includes delays so it should only be called during startup configuration + bool sub_command(uint16_t command, uint16_t data, CommandType type, uint8_t *rx_data = nullptr, uint8_t rx_len = 32) const; + + // subcommands are sent using a 16 bit command address and support block data transfers + // this includes delays so it should only be called during startup configuration + uint32_t sub_command_read_4bytes(uint16_t reg) const; + + AP_HAL::I2CDevice *dev; // I2C device + bool configured; // true once device has been configured + + // configuration settings to write during setup + static const struct ConfigurationSetting { + uint16_t reg_addr; + uint32_t reg_data; + uint8_t len; + } config_settings[]; + + struct { + uint16_t count; // number of readings, values below should be divided by this number + float voltage; // battery voltage in volts + uint32_t cell_voltages_mv[AP_BATT_MONITOR_CELLS_MAX]; // individual cell voltages in mv + float current; // battery current in amps + float temp; // battery temperature in degrees Celsius + float health_pct; // battery health percentage + } accumulate; + HAL_Semaphore accumulate_sem; // semaphore for accumulate structure +}; + +#endif // AP_BATTERY_TIBQ76952_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_config.h b/libraries/AP_BattMonitor/AP_BattMonitor_config.h index 0b180f42a1887..a0d0fbfa57c66 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_config.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_config.h @@ -80,6 +80,10 @@ #define AP_BATTERY_INA3221_ENABLED (AP_BATTERY_BACKEND_DEFAULT_ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif +#ifndef AP_BATTERY_TIBQ76952_ENABLED +#define AP_BATTERY_TIBQ76952_ENABLED 0 // device must be specified in hwdef +#endif + #ifndef AP_BATTERY_LTC2946_ENABLED #define AP_BATTERY_LTC2946_ENABLED (AP_BATTERY_BACKEND_DEFAULT_ENABLED && defined(HAL_BATTMON_LTC2946_BUS) && defined(HAL_BATTMON_LTC2946_ADDR)) #endif diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.cpp b/libraries/AP_BoardConfig/AP_BoardConfig.cpp index df69481288004..763902a6e3988 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.cpp +++ b/libraries/AP_BoardConfig/AP_BoardConfig.cpp @@ -257,7 +257,7 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = { // @Param: TYPE // @DisplayName: Board type // @Description: This allows selection of a PX4 or VRBRAIN board type. If set to zero then the board type is auto-detected (PX4) - // @Values: 0:AUTO,1:PX4V1,2:Pixhawk,3:Cube/Pixhawk2,5:PixhawkMini,6:Pixhawk2Slim,13:Intel Aero FC,14:Pixhawk Pro,20:AUAV2.1,22:MINDPXV2,24:CUAVv5/FMUV5,39:PX4 FMUV6,100:PX4 OLDDRIVERS + // @Values: 0:AUTO,1:PX4V1,2:Pixhawk,3:Cube/Pixhawk2,5:PixhawkMini,6:Pixhawk2Slim,13:Intel Aero FC,14:Pixhawk Pro,20:AUAV2.1,39:PX4 FMUV6,100:PX4 OLDDRIVERS // @RebootRequired: True // @User: Advanced AP_GROUPINFO("TYPE", 9, AP_BoardConfig, state.board_type, BOARD_TYPE_DEFAULT), diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.h b/libraries/AP_BoardConfig/AP_BoardConfig.h index 6abf71663f408..5b9b73b1c7fd5 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.h +++ b/libraries/AP_BoardConfig/AP_BoardConfig.h @@ -55,9 +55,9 @@ class AP_BoardConfig { // PX4_BOARD_PIXHAWK_PRO = 14, PX4_BOARD_AUAV21 = 20, // PX4_BOARD_PCNC1 = 21, - PX4_BOARD_MINDPXV2 = 22, + // PX4_BOARD_MINDPXV2 = 22, // PX4_BOARD_SP01 = 23, - PX4_BOARD_FMUV5 = 24, + // PX4_BOARD_FMUV5 = 24, // VRX_BOARD_BRAIN51 = 30, // VRX_BOARD_BRAIN52 = 32, // VRX_BOARD_BRAIN52E = 33, diff --git a/libraries/AP_BoardConfig/AP_BoardConfig_config.h b/libraries/AP_BoardConfig/AP_BoardConfig_config.h index 003829d724c7b..06d0255bd849a 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig_config.h +++ b/libraries/AP_BoardConfig/AP_BoardConfig_config.h @@ -4,7 +4,7 @@ #include #ifndef AP_FEATURE_BOARD_DETECT -#if defined(HAL_CHIBIOS_ARCH_FMUV3) || defined(HAL_CHIBIOS_ARCH_FMUV5) || defined(HAL_CHIBIOS_ARCH_MINDPXV2) +#if defined(HAL_CHIBIOS_ARCH_FMUV3) #define AP_FEATURE_BOARD_DETECT 1 #else #define AP_FEATURE_BOARD_DETECT 0 diff --git a/libraries/AP_BoardConfig/board_drivers.cpp b/libraries/AP_BoardConfig/board_drivers.cpp index 0fc7f88667c3b..5444f40726fab 100644 --- a/libraries/AP_BoardConfig/board_drivers.cpp +++ b/libraries/AP_BoardConfig/board_drivers.cpp @@ -87,13 +87,11 @@ void AP_BoardConfig::board_setup_drivers(void) case PX4_BOARD_PX4V1: case PX4_BOARD_PIXHAWK: case PX4_BOARD_PIXHAWK2: - case PX4_BOARD_FMUV5: case PX4_BOARD_FMUV6: case PX4_BOARD_PHMINI: case PX4_BOARD_AUAV21: case PX4_BOARD_PH2SLIM: case PX4_BOARD_AEROFC: - case PX4_BOARD_MINDPXV2: case FMUV6_BOARD_HOLYBRO_6X: case FMUV6_BOARD_HOLYBRO_6X_REV6: case FMUV6_BOARD_HOLYBRO_6X_45686: @@ -335,13 +333,6 @@ void AP_BoardConfig::board_autodetect(void) } else { config_error("Unable to detect board type"); } -#elif defined(HAL_CHIBIOS_ARCH_MINDPXV2) - // only one choice - state.board_type.set_and_notify(PX4_BOARD_MINDPXV2); - DEV_PRINTF("Detected MindPX-V2\n"); -#elif defined(HAL_CHIBIOS_ARCH_FMUV5) - state.board_type.set_and_notify(PX4_BOARD_FMUV5); - DEV_PRINTF("Detected FMUv5\n"); #elif defined(HAL_CHIBIOS_ARCH_FMUV6) detect_fmuv6_variant(); #endif diff --git a/libraries/AP_CANManager/AP_CANManager.cpp b/libraries/AP_CANManager/AP_CANManager.cpp index 96db211c73a18..52379ae31376e 100644 --- a/libraries/AP_CANManager/AP_CANManager.cpp +++ b/libraries/AP_CANManager/AP_CANManager.cpp @@ -213,7 +213,7 @@ void AP_CANManager::init() AP_Param::load_object_from_eeprom((AP_DroneCAN*)_drivers[drv_num], AP_DroneCAN::var_info); break; #endif -#if HAL_PICCOLO_CAN_ENABLE +#if AP_PICCOLOCAN_ENABLED case AP_CAN::Protocol::PiccoloCAN: _drivers[drv_num] = _drv_param[drv_num]._piccolocan = NEW_NOTHROW AP_PiccoloCAN; diff --git a/libraries/AP_CANManager/AP_CANManager_CANDriver_Params.cpp b/libraries/AP_CANManager/AP_CANManager_CANDriver_Params.cpp index d1939aa804760..e2f5047f8e585 100644 --- a/libraries/AP_CANManager/AP_CANManager_CANDriver_Params.cpp +++ b/libraries/AP_CANManager/AP_CANManager_CANDriver_Params.cpp @@ -43,7 +43,7 @@ const AP_Param::GroupInfo AP_CANManager::CANDriver_Params::var_info[] = { // index 4 was CANTester -#if HAL_PICCOLO_CAN_ENABLE +#if AP_PICCOLOCAN_ENABLED // @Group: PC_ // @Path: ../AP_PiccoloCAN/AP_PiccoloCAN.cpp AP_SUBGROUPPTR(_piccolocan, "PC_", 5, AP_CANManager::CANDriver_Params, AP_PiccoloCAN), diff --git a/libraries/AP_Camera/AP_Camera_config.h b/libraries/AP_Camera/AP_Camera_config.h index 42a166bbfa5e2..1e62b69b1dff8 100644 --- a/libraries/AP_Camera/AP_Camera_config.h +++ b/libraries/AP_Camera/AP_Camera_config.h @@ -42,7 +42,7 @@ #endif #ifndef AP_CAMERA_SEND_FOV_STATUS_ENABLED -#define AP_CAMERA_SEND_FOV_STATUS_ENABLED AP_MOUNT_POI_TO_LATLONALT_ENABLED +#define AP_CAMERA_SEND_FOV_STATUS_ENABLED AP_MOUNT_POI_TO_LATLONALT_ENABLED && HAL_PROGRAM_SIZE_LIMIT_KB > 1024 #endif // set camera source is supported on cameras that may have more than one lens which is curently only cameras within gimbals/mounts diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp index f7d84cb7b36ad..318d87dd71ca6 100644 --- a/libraries/AP_Common/Location.cpp +++ b/libraries/AP_Common/Location.cpp @@ -64,6 +64,20 @@ Location::Location(const Vector3d &ekf_offset_neu_cm, AltFrame frame) offset(ekf_offset_neu_cm.x * 0.01, ekf_offset_neu_cm.y * 0.01); } } + +// named constructors +Location Location::from_ekf_offset_NED_m(const Vector3f& ekf_offset_ned_m, AltFrame frame) +{ + const Vector3f ekf_offset_neu_cm(ekf_offset_ned_m.x * 100.0, ekf_offset_ned_m.y * 100.0, -ekf_offset_ned_m.z * 100.0); + return Location(ekf_offset_neu_cm, frame); +} + +Location Location::from_ekf_offset_NED_m(const Vector3d& ekf_offset_ned_m, AltFrame frame) +{ + const Vector3d ekf_offset_neu_cm(ekf_offset_ned_m.x * 100.0, ekf_offset_ned_m.y * 100.0, -ekf_offset_ned_m.z * 100.0); + return Location(ekf_offset_neu_cm, frame); +} + #endif // AP_AHRS_ENABLED void Location::set_alt_cm(int32_t alt_cm, AltFrame frame) @@ -317,6 +331,22 @@ template bool Location::get_vector_xy_from_origin_NE_m(Vector2f &vec_n template bool Location::get_vector_xy_from_origin_NE_m(Vector2p &vec_ne) const; #endif +template +bool Location::get_vector_from_origin_NED_m(T &vec_ned) const +{ + if (!get_vector_from_origin_NEU_cm(vec_ned)) { + return false; + } + vec_ned *= 0.01; + vec_ned.z *= -1.0; + return true; +} +// define for float and position vectors +template bool Location::get_vector_from_origin_NED_m(Vector3f &vec_ned) const; +#if HAL_WITH_POSTYPE_DOUBLE +template bool Location::get_vector_from_origin_NED_m(Vector3p &vec_ned) const; +#endif + template bool Location::get_vector_from_origin_NEU_m(T &vec_neu) const { diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h index 8c2dad26ba02d..ce23c6eb49373 100644 --- a/libraries/AP_Common/Location.h +++ b/libraries/AP_Common/Location.h @@ -33,6 +33,10 @@ class Location Location(const Vector3f &ekf_offset_neu_cm, AltFrame frame); Location(const Vector3d &ekf_offset_neu_cm, AltFrame frame); + // named constructors + static Location from_ekf_offset_NED_m(const Vector3f& ekf_offset_ned_m, AltFrame frame); + static Location from_ekf_offset_NED_m(const Vector3d& ekf_offset_ned_m, AltFrame frame); + // set altitude void set_alt_cm(int32_t alt_cm, AltFrame frame); // set_alt_m - set altitude in metres @@ -88,6 +92,8 @@ class Location bool get_vector_xy_from_origin_NE_m(T &vec_ne) const; template bool get_vector_from_origin_NEU_m(T &vec_neu) const; + template + bool get_vector_from_origin_NED_m(T &vec_ned) const; // return horizontal distance in meters between two locations ftype get_distance(const Location &loc2) const; diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index c8056228e089b..0a6ecd09938e7 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -1070,7 +1070,7 @@ bool Compass::_driver_enabled(enum DriverType driver_type) /* wrapper around hal.i2c_mgr->get_device() that prevents duplicate devices being opened */ -bool Compass::_have_i2c_driver(uint8_t bus, uint8_t address) const +bool Compass::_i2c_sensor_is_registered(uint8_t bus, uint8_t address) const { for (StateIndex i(0); iget_device(bus, address) +#define GET_I2C_DEVICE(bus, address) _i2c_sensor_is_registered(bus, address)?nullptr:hal.i2c_mgr->get_device(bus, address) /* look for compasses on external i2c buses @@ -1131,16 +1131,15 @@ void Compass::_probe_external_i2c_compasses(void) RETURN_IF_NO_SPACE; } -#if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED - if (AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_MINDPXV2 && - AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_AEROFC) { +#if AP_COMPASS_HMC5843_INTERNAL_BUS_PROBING_ENABLED + if (AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_AEROFC) { // internal i2c bus FOREACH_I2C_INTERNAL(i) { probe_i2c_dev(DRIVER_HMC5843, AP_Compass_HMC5843::probe, i, HAL_COMPASS_HMC5843_I2C_ADDR, all_external, all_external?ROTATION_ROLL_180:ROTATION_YAW_270); RETURN_IF_NO_SPACE; } } -#endif // AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED +#endif // AP_COMPASS_KMC5843_INTERNAL_BUS_PROBING_ENABLED #endif // AP_COMPASS_HMC5843_ENABLED #if AP_COMPASS_QMC5883L_ENABLED @@ -1277,10 +1276,9 @@ void Compass::_probe_external_i2c_compasses(void) #endif // AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED #endif // AP_COMPASS_AK09916_ENABLED -#if AP_COMPASS_IST8310_ENABLED +#if AP_COMPASS_IST8310_EXTERNAL_BUS_PROBING_ENABLED || AP_COMPASS_IST8310_INTERNAL_BUS_PROBING_ENABLED // IST8310 on external and internal bus - if (AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_FMUV5 && - AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_FMUV6) { + if (AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_FMUV6) { enum Rotation default_rotation = AP_COMPASS_IST8310_DEFAULT_ROTATION; if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_AEROFC) { @@ -1290,10 +1288,12 @@ void Compass::_probe_external_i2c_compasses(void) const uint8_t ist8310_addr[] = { 0x0C, 0x0D, 0x0E, 0x0F }; for (uint8_t a=0; asnprintf(failure_msg, failure_msg_len, "MicroStrain5 unhealthy"); return false; } - if (gnss_data[gnss_instance].fix_type < 3) { + if (gnss_data[gnss_instance].fix_type < AP_GPS_FixType::FIX_3D) { hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain5 no GPS lock"); return false; } @@ -257,7 +257,7 @@ void AP_ExternalAHRS_MicroStrain5::get_filter_status(nav_filter_status &status) status.flags.vert_vel = true; status.flags.vert_pos = true; - if (gnss_data[gnss_instance].fix_type >= 3) { + if (gnss_data[gnss_instance].fix_type >= AP_GPS_FixType::FIX_3D) { status.flags.horiz_vel = true; status.flags.horiz_pos_rel = true; status.flags.horiz_pos_abs = true; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp index 3f4d32099aa5c..4fb22c5a188af 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp @@ -286,7 +286,7 @@ bool AP_ExternalAHRS_MicroStrain7::pre_arm_check(char *failure_msg, uint8_t fail return false; } static_assert(NUM_GNSS_INSTANCES == 2, "This check only works if there are two GPS types."); - if (gnss_data[0].fix_type < GPS_FIX_TYPE_3D_FIX && gnss_data[1].fix_type < GPS_FIX_TYPE_3D_FIX) { + if (gnss_data[0].fix_type < AP_GPS_FixType::FIX_3D && gnss_data[1].fix_type < AP_GPS_FixType::FIX_3D) { hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "missing 3D GPS fix on either GPS"); return false; } diff --git a/libraries/AP_ExternalAHRS/MicroStrain_common.cpp b/libraries/AP_ExternalAHRS/MicroStrain_common.cpp index 6e44b68683cdf..bd360fd757fb3 100644 --- a/libraries/AP_ExternalAHRS/MicroStrain_common.cpp +++ b/libraries/AP_ExternalAHRS/MicroStrain_common.cpp @@ -17,6 +17,7 @@ #define AP_MATH_ALLOW_DOUBLE_FUNCTIONS 1 #include "AP_ExternalAHRS_config.h" +#include #if AP_MICROSTRAIN_ENABLED @@ -216,29 +217,29 @@ void AP_MicroStrain::handle_gnss(const MicroStrain_Packet &packet) case GNSSPacketField::FIX_INFO: { switch ((GNSSFixType) packet.payload[i+2]) { case (GNSSFixType::FIX_RTK_FLOAT): { - gnss_data[gnss_instance].fix_type = GPS_FIX_TYPE_RTK_FLOAT; + gnss_data[gnss_instance].fix_type = AP_GPS_FixType::RTK_FLOAT; break; } case (GNSSFixType::FIX_RTK_FIXED): { - gnss_data[gnss_instance].fix_type = GPS_FIX_TYPE_RTK_FIXED; + gnss_data[gnss_instance].fix_type = AP_GPS_FixType::RTK_FIXED; break; } case (GNSSFixType::FIX_3D): { - gnss_data[gnss_instance].fix_type = GPS_FIX_TYPE_3D_FIX; + gnss_data[gnss_instance].fix_type = AP_GPS_FixType::FIX_3D; break; } case (GNSSFixType::FIX_2D): { - gnss_data[gnss_instance].fix_type = GPS_FIX_TYPE_2D_FIX; + gnss_data[gnss_instance].fix_type = AP_GPS_FixType::FIX_2D; break; } case (GNSSFixType::TIME_ONLY): case (GNSSFixType::NONE): { - gnss_data[gnss_instance].fix_type = GPS_FIX_TYPE_NO_FIX; + gnss_data[gnss_instance].fix_type = AP_GPS_FixType::NONE; break; } default: case (GNSSFixType::INVALID): { - gnss_data[gnss_instance].fix_type = GPS_FIX_TYPE_NO_GPS; + gnss_data[gnss_instance].fix_type = AP_GPS_FixType::NO_GPS; break; } } diff --git a/libraries/AP_ExternalAHRS/MicroStrain_common.h b/libraries/AP_ExternalAHRS/MicroStrain_common.h index 0e6b35db20ac6..da3aa3dbbf4ee 100644 --- a/libraries/AP_ExternalAHRS/MicroStrain_common.h +++ b/libraries/AP_ExternalAHRS/MicroStrain_common.h @@ -20,7 +20,7 @@ #if AP_MICROSTRAIN_ENABLED -#include +#include #include #include @@ -53,7 +53,7 @@ class AP_MicroStrain struct { uint16_t week; uint32_t tow_ms; - GPS_FIX_TYPE fix_type; + AP_GPS_FixType fix_type; uint8_t satellites; float horizontal_position_accuracy; float vertical_position_accuracy; diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 8d8c0db18501a..b61b6e1d1a3be 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -704,14 +704,14 @@ void AP_GPS_NMEA::parse_agrica_field(uint16_t term_number, const char *term) ag.heading_status = atol(term); break; case 25 ... 26: - ag.vel_NED[term_number-25] = atof(term); + ag.vel_NED[term_number-25] = strtof(term, nullptr); break; case 27: // AGRIC gives velocity up - ag.vel_NED.z = -atof(term); + ag.vel_NED.z = -strtof(term, nullptr); break; case 28 ... 30: - ag.vel_stddev[term_number-28] = atof(term); + ag.vel_stddev[term_number-28] = strtof(term, nullptr); break; case 31: ag.lat = atof(term); @@ -720,16 +720,16 @@ void AP_GPS_NMEA::parse_agrica_field(uint16_t term_number, const char *term) ag.lng = atof(term); break; case 33: - ag.alt = atof(term); + ag.alt = strtof(term, nullptr); break; case 49: ag.itow = atol(term); break; case 37 ... 39: - ag.pos_stddev[term_number-37] = atof(term); + ag.pos_stddev[term_number-37] = strtof(term, nullptr); break; case 52: - ag.undulation = atof(term); + ag.undulation = strtof(term, nullptr); break; } } @@ -754,16 +754,16 @@ void AP_GPS_NMEA::parse_uniheadinga_field(uint16_t term_number, const char *term auto &uh = _uniheadinga; switch (term_number) { case 4: - uh.baseline_length = atof(term); + uh.baseline_length = strtof(term, nullptr); break; case 5: - uh.heading = atof(term); + uh.heading = strtof(term, nullptr); break; case 6: - uh.pitch = atof(term); + uh.pitch = strtof(term, nullptr); break; case 8: - uh.heading_sd = atof(term); + uh.heading_sd = strtof(term, nullptr); break; } } diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index d7864dc82436b..5e7e2d3794c4d 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -1672,6 +1672,19 @@ AP_GPS_UBLOX::_parse_gps(void) // position _check_new_itow(_buffer.pvt.itow); + // Only adjust if: + // we already have a valid week, + // PVT iTOW wrapped, + // and our time_week_ms still looks like end-of-week + // (meaning we haven't already accepted the rollover via TIMEGPS/SOL) + if (state.time_week != 0 && + _last_pvt_itow != 0 && + _buffer.pvt.itow < _last_pvt_itow && + state.time_week_ms > END_OF_WEEK_THRESHOLD_MS && + _buffer.pvt.itow < START_OF_WEEK_THRESHOLD_MS) { + state.time_week++; + } + _last_pvt_itow = _buffer.pvt.itow; _last_pos_time = _buffer.pvt.itow; state.location.lng = _buffer.pvt.lon; @@ -1762,7 +1775,9 @@ AP_GPS_UBLOX::_parse_gps(void) Debug("MSG_TIMEGPS"); _check_new_itow(_buffer.timegps.itow); if (_buffer.timegps.valid & UBX_TIMEGPS_VALID_WEEK_MASK) { + state.time_week_ms = _buffer.timegps.itow; state.time_week = _buffer.timegps.week; + state.last_gps_time_ms = AP_HAL::millis(); } break; case MSG_VELNED: diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 96f38f0b5a33e..3e4a4119f2163 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -122,6 +122,9 @@ #define SAVE_CFG_ANT (1<<10) #define SAVE_CFG_ALL (SAVE_CFG_IO|SAVE_CFG_MSG|SAVE_CFG_INF|SAVE_CFG_NAV|SAVE_CFG_RXM|SAVE_CFG_RINV|SAVE_CFG_ANT) +#define START_OF_WEEK_THRESHOLD_MS (60UL * AP_MSEC_PER_SEC) +#define END_OF_WEEK_THRESHOLD_MS AP_MSEC_PER_WEEK - START_OF_WEEK_THRESHOLD_MS + class RTCM3_Parser; class AP_GPS_UBLOX : public AP_GPS_Backend diff --git a/libraries/AP_Generator/AP_Generator.cpp b/libraries/AP_Generator/AP_Generator.cpp index 9dc346ecba565..09d16a936e453 100644 --- a/libraries/AP_Generator/AP_Generator.cpp +++ b/libraries/AP_Generator/AP_Generator.cpp @@ -46,9 +46,9 @@ const AP_Param::GroupInfo AP_Generator::var_info[] = { // @User: Standard AP_GROUPINFO("OPTIONS", 2, AP_Generator, _options, 0), - // @Group: + // @Group:L_ // @Path: AP_Generator_Loweheiser.cpp - AP_SUBGROUPVARPTR(_driver_ptr, "", 3, AP_Generator, backend_var_info), + AP_SUBGROUPVARPTR(_driver_ptr, "L_", 3, AP_Generator, backend_var_info), AP_GROUPEND }; diff --git a/libraries/AP_Generator/AP_Generator_config.h b/libraries/AP_Generator/AP_Generator_config.h index 6ce396f87dc2e..ba425a55ef4c9 100644 --- a/libraries/AP_Generator/AP_Generator_config.h +++ b/libraries/AP_Generator/AP_Generator_config.h @@ -32,5 +32,5 @@ #endif // AP_GENERATOR_LOWEHEISER_ENABLED #ifndef AP_GENERATOR_CORTEX_ENABLED -#define AP_GENERATOR_CORTEX_ENABLED AP_GENERATOR_BACKEND_DEFAULT_ENABLED && HAL_PICCOLO_CAN_ENABLE && HAL_PROGRAM_SIZE_LIMIT_KB > 2048 +#define AP_GENERATOR_CORTEX_ENABLED AP_GENERATOR_BACKEND_DEFAULT_ENABLED && AP_PICCOLOCAN_ENABLED && HAL_PROGRAM_SIZE_LIMIT_KB > 2048 #endif // AP_GENERATOR_CORTEX_ENABLED diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index e33700afe87dc..684265795abc6 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -329,7 +329,7 @@ // On an F7 The difference in CPU load between 1 notch and 24 notches is about 2% // The difference in CPU load between 1Khz backend and 2Khz backend is about 10% // So at 1Khz almost all notch combinations can be supported on F7 and certainly H7 -#if defined(STM32H7) || CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if defined(STM32H7) || CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX // Enough for a double-notch per motor on an octa using three IMUs and one harmonics // plus one static notch with one double-notch harmonics #define HAL_HNF_MAX_FILTERS 54 diff --git a/libraries/AP_HAL/ap_setjmp.h b/libraries/AP_HAL/ap_setjmp.h new file mode 100644 index 0000000000000..97ed1521a20d9 --- /dev/null +++ b/libraries/AP_HAL/ap_setjmp.h @@ -0,0 +1,55 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#include "AP_HAL_Boards.h" + +// setjmp with platform-specific implementations. + +// older ARM newlib (in particular that shipped with ARM GCC 10.3.1) does not +// save the callee-saved floating point registers. if we detect a situation +// where we may be using such a newlib and such registers are available, use +// our own implementation which actually saves them. these cannot be wrappers +// as we cannot safely use the stack and have to expand the jmp_buf. +#if (CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS) && (defined(_NEWLIB_VERSION) && (defined(__ARM_FP) || defined(__ARM_FEATURE_MVE))) +#define AP_SETJMP_CORTEX_ENABLE 1 +#else +#define AP_SETJMP_CORTEX_ENABLE 0 +#endif + +#if AP_SETJMP_CORTEX_ENABLE + +#include + +// 20 8-byte words is defined by +// https://github.com/ARM-software/abi-aa/blob/c51addc3dc03e73a016a1e4edf25440bcac76431/clibabi32/clibabi32.rst#setjmp-h +// but we only need 10 regular registers and 16 FP registers meaning 13 8 byte +// words so we can save a bit of memory. switching back to the official setjmp +// implementation will re-consume it though. Lua keeps these on the C stack. + +struct _ap_jmp_buf { + int64_t data[13]; +}; + +// hack to allow ap_jmp_buf to decay to a pointer +typedef struct _ap_jmp_buf ap_jmp_buf[1]; + +int __attribute__((naked, noinline)) ap_setjmp(ap_jmp_buf env); +void __attribute__((noreturn, naked, noinline)) ap_longjmp(ap_jmp_buf env, int val); + +#else + +#define ap_setjmp(env) (setjmp(env)) +#define ap_longjmp(env, val) (longjmp(env, val)) + +typedef jmp_buf ap_jmp_buf; + +#endif + +#ifdef __cplusplus +} +#endif diff --git a/libraries/AP_HAL/board/sitl.h b/libraries/AP_HAL/board/sitl.h index e0dfda25bde48..bd45322325f4e 100644 --- a/libraries/AP_HAL/board/sitl.h +++ b/libraries/AP_HAL/board/sitl.h @@ -1,5 +1,7 @@ #pragma once +#include + #define HAL_BOARD_NAME "SITL" #define HAL_CPU_CLASS HAL_CPU_CLASS_1000 #define HAL_MEM_CLASS HAL_MEM_CLASS_1000 diff --git a/libraries/AP_HAL/hwdef/scripts/hwdef.py b/libraries/AP_HAL/hwdef/scripts/hwdef.py index 5f05a3f3021c7..66f2034613035 100644 --- a/libraries/AP_HAL/hwdef/scripts/hwdef.py +++ b/libraries/AP_HAL/hwdef/scripts/hwdef.py @@ -294,6 +294,7 @@ def get_stale_defines(self): 'HAL_SKIP_AUTO_INTERNAL_I2C_PROBE': 'HAL_SKIP_AUTO_INTERNAL_I2C_PROBE is no longer used; try "define AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED 0', # noqa:E501 'HAL_COMPASS_DISABLE_IST8310_INTERNAL_PROBE': 'HAL_COMPASS_DISABLE_IST8310_INTERNAL_PROBE is no longer used; try "define AP_COMPASS_IST8310_INTERNAL_BUS_PROBING_ENABLED 0"', # noqa:E501 'BOARD_PWM_COUNT_DEFAULT': 'BOARD_PWM_COUNT_DEFAULT is no longer used; remove it from your hwdef files', + 'HAL_PICCOLO_CAN_ENABLE': 'HAL_PICCOLO_CAN_ENABLE was renamed to AP_PICCOLOCAN_ENABLED; fix your hwdef file', } def assert_good_define(self, name): diff --git a/libraries/AP_HAL_ChibiOS/ap_setjmp.c b/libraries/AP_HAL_ChibiOS/ap_setjmp.c new file mode 100644 index 0000000000000..bbc6b8dd5f4bc --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/ap_setjmp.c @@ -0,0 +1,29 @@ +#include + +#if AP_SETJMP_CORTEX_ENABLE + +int __attribute__((naked, noinline)) ap_setjmp(ap_jmp_buf env) { + __asm__( + "mov ip, sp\n\t" // ip must be free on a function call; can't store sp + "stmia r0!, {r4-r10, fp, ip, lr}\n\t" // store 10 regular registers + "vstm r0, {d8-d15}\n\t" // store 16 FP registers. use 8 byte store for + // speed as our buffer is 8 byte aligned + "movs r0, #0\n\t" // return 0 + "bx lr\n\t" // do return + ); +} + +void __attribute__((noreturn, naked, noinline)) ap_longjmp(ap_jmp_buf env, int val) { + __asm__( + "ldmia r0!, {r4-r10, fp, ip, lr}\n\t" // load 10 regular registers + "mov sp, ip\n\t" // restore stack pointer from scratch register + "vldm r0, {d8-d15}\n\t" // load 16 FP registers. use 8 byte load for + // speed as our buffer is 8 byte aligned + "movs r0, r1\n\t" // prepare return of value + "it eq\n\t" // but if value is zero + "moveq r0, #1\n\t" // return one + "bx lr\n\t" // do return + ); +} + +#endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/3DR-L431-ASAUAV/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/3DR-L431-ASAUAV/hwdef-bl.dat new file mode 100644 index 0000000000000..29fe2b8e4784b --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/3DR-L431-ASAUAV/hwdef-bl.dat @@ -0,0 +1,5 @@ +include ../3DR-L431/hwdef-bl.inc + +APJ_BOARD_ID AP_HW_3DR-ASAUAV + +define CAN_APP_NODE_NAME "org.ardupilot.3DR-L431-ASAUAV" diff --git a/libraries/AP_HAL_ChibiOS/hwdef/3DR-L431-ASAUAV/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/3DR-L431-ASAUAV/hwdef.dat new file mode 100644 index 0000000000000..67d1c6ab5aa47 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/3DR-L431-ASAUAV/hwdef.dat @@ -0,0 +1,28 @@ +include ../3DR-L431/hwdef.inc + +define CAN_APP_NODE_NAME "org.ardupilot.3DR-L431-ASAUAV" +APJ_BOARD_ID AP_HW_3DR-ASAUAV + +define HAL_USE_ADC FALSE +define STM32_ADC_USE_ADC1 FALSE +define HAL_DISABLE_ADC_DRIVER TRUE + +# ------------------ I2C airspeed ------------------------- +define AP_PERIPH_AIRSPEED_ENABLED 1 + +# 10" AUAV sensor by default +define HAL_AIRSPEED_TYPE_DEFAULT 17 +define AIRSPEED_MAX_SENSORS 1 + +# enable Baro on AUAV +define AP_PERIPH_BARO_ENABLED 1 +define AP_BARO_BACKEND_DEFAULT_ENABLED 0 +define AP_BARO_AUAV_ENABLED 1 +define BARO_MAX_INSTANCES 1 +BARO AUAV I2C:0:0x27 + +# ----------- MSP ------------------- +define AP_PERIPH_MSP_ENABLED 1 +define HAL_MSP_ENABLED 1 +define AP_PERIPH_MSP_PORT_DEFAULT 1 + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/3DR-L431/hwdef-bl.inc b/libraries/AP_HAL_ChibiOS/hwdef/3DR-L431/hwdef-bl.inc new file mode 100644 index 0000000000000..3f10210fffa2a --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/3DR-L431/hwdef-bl.inc @@ -0,0 +1,59 @@ +# HW definition file for 3DR CAN Nodes (STM32L431) + +# MCU class and specific type +MCU STM32L431 STM32L431xx + +# use LSE clock source +OSCILLATOR_HZ 32768 + +# Board ID: Inheriting boards will set their own ID. + +# setup build for a peripheral firmware +env AP_PERIPH 1 + +FLASH_RESERVE_START_KB 0 +FLASH_BOOTLOADER_LOAD_KB 48 +FLASH_SIZE_KB 256 + +# reserve some space for params +APP_START_OFFSET_KB 16 + +STDOUT_SERIAL SD1 +STDOUT_BAUDRATE 57600 + +# order of UARTs +SERIAL_ORDER USART1 + +# USART1 +PB6 USART1_TX USART1 SPEED_HIGH NODMA +PB7 USART1_RX USART1 SPEED_HIGH NODMA +define HAL_USE_SERIAL TRUE +define STM32_SERIAL_USE_USART1 TRUE +define HAL_USE_EMPTY_IO TRUE + +define DMA_RESERVE_SIZE 0 + +# a fault LED +PB1 LED_BOOTLOADER OUTPUT LOW +define HAL_LED_ON 1 + +# CAN +PA11 CAN1_RX CAN1 +PA12 CAN1_TX CAN1 +CAN_ORDER 1 + +MAIN_STACK 0x800 +PROCESS_STACK 0x800 + +# CS Pins need to be HIGH in bootloader +PA4 MCU_CS CS + +# debugger support +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# make bl baudrate match debug baudrate for easier debugging +define BOOTLOADER_BAUDRATE 57600 + +# use a small bootloader timeout +define HAL_BOOTLOADER_TIMEOUT 1000 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/3DR-L431/hwdef.inc b/libraries/AP_HAL_ChibiOS/hwdef/3DR-L431/hwdef.inc new file mode 100644 index 0000000000000..5c230e1a85f82 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/3DR-L431/hwdef.inc @@ -0,0 +1,86 @@ +# HW definition file for 3DR CAN Nodes (STM32L431) + +# MCU class and specific type +MCU STM32L431 STM32L431xx + +# bootloader occupies 64k (48k + 16k, bootloader + STORAGE_FLASH) +FLASH_RESERVE_START_KB 64 +FLASH_SIZE_KB 256 + +# store parameters in pages 24 and 25 +STORAGE_FLASH_PAGE 24 +define HAL_STORAGE_SIZE 1800 + +# ChibiOS system timer +STM32_ST_USE_TIMER 15 +define CH_CFG_ST_RESOLUTION 16 + +# Board ID: Inheriting boards will set their own ID. + +# use LSE clock source +OSCILLATOR_HZ 32768 + +# setup build for a peripheral firmware +env AP_PERIPH 1 + +STDOUT_SERIAL SD1 +STDOUT_BAUDRATE 57600 + +define SERIAL_BUFFERS_SIZE 512 +define PORT_INT_REQUIRED_STACK 64 + +define DMA_RESERVE_SIZE 0 + +# MAIN_STACK is stack for ISR handlers +MAIN_STACK 0x300 + +# PROCESS_STACK controls stack for main thread +PROCESS_STACK 0xA00 + +# we setup a small defaults.parm +define AP_PARAM_MAX_EMBEDDED_PARAM 512 + +# LED0 marked as ACT +PB1 LED OUTPUT HIGH +define HAL_LED_ON 1 + +# debugger support +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# CAN +CAN_ORDER 1 + +PA11 CAN1_RX CAN1 +PA12 CAN1_TX CAN1 + +define HAL_CAN_POOL_SIZE 6000 + +# UARTs +SERIAL_ORDER USART1 USART2 + +# USART1 for debug +PB6 USART1_TX USART1 SPEED_HIGH +PB7 USART1_RX USART1 SPEED_HIGH + +# USART2 +PA2 USART2_TX USART2 SPEED_HIGH +PA3 USART2_RX USART2 SPEED_HIGH +PA0 USART2_CTS USART2 SPEED_HIGH +PA1 USART2_RTS USART2 SPEED_HIGH + +I2C_ORDER I2C1 +PA9 I2C1_SCL I2C1 +PA10 I2C1_SDA I2C1 + +define HAL_I2C_CLEAR_ON_TIMEOUT 0 +define HAL_I2C_INTERNAL_MASK 1 + +# allow for reboot command for faster development +define HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT 0 + +# keep ROMFS uncompressed as we don't have enough RAM +# to uncompress the bootloader at runtime +env ROMFS_UNCOMPRESSED True + +#define HAL_RCIN_THREAD_ENABLED 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Atlas-Control/README.md b/libraries/AP_HAL_ChibiOS/hwdef/Atlas-Control/README.md new file mode 100644 index 0000000000000..8eb40bfd07399 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Atlas-Control/README.md @@ -0,0 +1,401 @@ +# Atlas Control Flight Controller + +The Atlas Control flight controller +https://altasflight.github.io/atlas-docs/ + +## Features + + - STM32H743 microcontroller + - Three IMUs, ICM-42688-P, ICM-20689 and ICM-20689 + - internal heater for IMU temperature control + - two MS5611 SPI barometers + - internal RM3100 compass + - microSD card slot + - 4 UARTs plus USB + - ADSB receiver connected to SERIAL5 + - 14 PWM outputs + - I2C and CAN ports + - External Buzzer + - builtin RGB LED + - voltage monitoring for servo rail and Vcc + - two dedicated power input ports for external power bricks + - external USB connectors (micro USB and JST GH) + + + +## Pinout + +![AtlasControl Board](img.png "Atlas Control") + +- 14 PWM servo outputs（12 support DShot） + +- Analog/ PWM RSSI input + +- 2 GPS ports(GPS and UART4 ports) + +- 4 I2C buses(Two external I2C ports) + +- 2 CAN bus ports + +- 2 Power ports(Power A is an analog PMU interface, Power C is a DroneCAN PMU interface) + +- 2 ADC inputs + +- 1 USB-C port +## UART Mapping + +| Port | UART | Protocol | TX DMA | RX DMA | +| :--- | :--- | :--- | :---: | :---: | +| 0 | USB | MAVLink2 | ✘ | ✘ | +| 1 | USART2 | MAVLink2 | ✔ | ✔ | +| 2 | USART6 | MAVLink2 | ✔ | ✔ | +| 3 | USART1 | GPS | ✘ | ✘ | +| 4 | UART4 | GPS | ✘ | ✘ | +| 6 | UART7 | None | ✘ | ✘ | +| 7 | USB | SLCAN | ✘ | ✘ | + +> **Note:** SERIAL 1 and 2 have RTS/CTS flow control capability. + +## Connectors + +Unless noted otherwise all connectors are JST GH 1.25mm pitch + +### TELEM1, TELEM2 ports + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Pin Signal Volt
1 (red)VCC+5V
2 (blk)TX (OUT)+3.3V
3 (blk)RX (IN)+3.3V
4 (blk)CTS+3.3V
5 (blk)RTS+3.3V
6 (blk)GNDGND
+ + +3.3V + + + 2 + GND + GND + + + 3 + 3.3v + +3.3V + + + + + +### FMU and IO SWD + +When the case is removed there are two SWD connectors, one for FMU and +the other for IOMCU. The IO SWD connector is the one closer to the +servo rail. The GND pin of both connectors is the one furthest from +the servo rail. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
PinSignalVolt
1VCC+5V
2TX+3.3
3RX+3.3
4SWDIO+3.3
5SWCLK+3.3
6GNDGND
+ + +### CAN1&2 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
PinSignalVolt
1 (red)VCC+5V
2 (blk)CAN_H+12V
3 (blk)CAN_L+12V
4 (blk)GNDGND
+ + +### POWER_A + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
PinSignalVolt
1 (red)VCC+5V
2 (red)VCC+5V
3 (blk)CURRENTup to +3.3V
4 (blk)VOLTAGEup to +3.3V
5 (blk)GNDGND
6 (blk)GNDGND
+ +### POWER_C + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
PinSignalVolt
1 (red)VCC+5V
2 (red)VCC+5V
3 (blk)CAN_Hup to +3.3V
4 (blk)CAN_Lup to +3.3V
5 (blk)GNDGND
6 (blk)GNDGND
+ + +### USB + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Pin Signal Volt
1 (red)VCC+5V
2 (blk)D_plus+3.3V
3 (blk)D_minus+3.3V
4 (blk)GNDGND
5 (blk)BUZZERbattery voltage
6 (blk)Boot/Error LED
+ +## RC Input + +RC input is configured on the RCIN pin, at one end of the servo rail, +marked RCIN in the above diagram. All ArduPilot supported unidirectional +RC protocols can be input here including PPM. For bi-directional or half-duplex +protocols, such as CRSF/ELRS a full UART will have to be used. +See https://ardupilot.org/plane/docs/common-rc-systems.html + + +## PWM Output + +The 14 PWM outputs are in 4 groups: Each group must be the same protocol (ie PWM or DShot or Serial LED, etc.): + +* Outputs 1, 2, 3 and 4 in group1 (supports Bi-directional DShot) +* Outputs 5, 6, 7 and 8 in group2 (supports Bi-directional DShot) +* Outputs 9, 10, 11 and 12 in group3 (supports DShot) +* Outputs 13 and 14 in group4 (PWM only, no DMA) + +## Battery Monitoring + + +The board has two dedicated power monitor ports on 6 pin connectors. + +The board is supplied with an analog power module. Analog monitoring is set as the default configuration. +The module is rated for up to 60V (14S) and 60A continuous current. + +* **BATT_MONITOR**: 4 (Analog Voltage and Current) +* **BATT_VOLT_PIN**: 16 +* **BATT_CURR_PIN**: 17 +* **BATT_VOLT_MULT**: 18.000 +* **BATT_AMP_PERVLT**: 24.000 +* **BATT_VLT_OFFSET**: -0.1 + +## Compass + +The Atlas Control has an RM3100 builtin compass, and you can attach an external compass using I2C on the SDA and SCL connector. + +## GPIOs + +The 14 outputs can be used as GPIOs (relays, buttons, RPM etc). To use them you need to set the output’s `SERVOx_FUNCTION` to -1. See [GPIOs](https://ardupilot.org/copter/docs/common-gpios.html) page for more information. +The numbering of the GPIOs for use in the PIN parameters in ArduPilot is: + +| Output | GPIO Number | +| :--- | :--- | +| PWM1 (M1) | 50 | +| PWM2 (M2) | 51 | +| PWM3 (M3) | 52 | +| PWM4 (M4) | 53 | +| PWM5 (M5) | 54 | +| PWM6 (M6) | 55 | +| PWM7 (M7) | 56 | +| PWM8 (M8) | 57 | +| PWM9 (M9) | 58 | +| PWM10 (M10) | 59 | +| PWM11 (M11) | 60 | +| PWM12 (M12) | 61 | +| PWM13 (M13) | 62 | +| PWM14 (M14) | 63 | +| **Analog RSSI (PF12)** | **92** | + +## IMU Heater + +The IMU heater in the Atlas Control can be controlled with the +BRD_HEAT_TARG parameter, which is in degrees C. + +## Loading Firmware + +Firmware for these boards can be found at https://firmware.ardupilot.org in sub-folders labeled “Atlas-Control”. + +Initial firmware load can be done with DFU by plugging in USB with the +boot button pressed. Then you should load the "xxx_bl.hex" +firmware, using your favorite DFU loading tool. + +Subsequently, you can update the firmware with Mission Planner. + + + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Atlas-Control/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/Atlas-Control/defaults.parm new file mode 100644 index 0000000000000..ed6548c17dc85 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Atlas-Control/defaults.parm @@ -0,0 +1,2 @@ +# setup the parameter for the ADC power module +BATT_VLT_OFFSET -0.1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Atlas-Control/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/Atlas-Control/hwdef-bl.dat new file mode 100644 index 0000000000000..2657f7199f50f --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Atlas-Control/hwdef-bl.dat @@ -0,0 +1,69 @@ +# hw definition file for processing by chibios_hwdef.py +# for Atlas_Control + +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# crystal frequency +OSCILLATOR_HZ 16000000 + +# board ID. See Tools/AP_Bootloader/board_types.txt +APJ_BOARD_ID AP_HW_ATLAS_CONTROL + +FLASH_SIZE_KB 2048 + +# bootloader is installed at zero offset +FLASH_RESERVE_START_KB 0 + +# the location where the bootloader will put the firmware +# the H743 has 128k sectors +FLASH_BOOTLOADER_LOAD_KB 128 + +# ChibiOS system timer +STM32_ST_USE_TIMER 2 + +PI5 LED_RED OUTPUT OPENDRAIN HIGH # red +PI7 LED_ACTIVITY OUTPUT OPENDRAIN HIGH # blue +PI6 LED_BOOTLOADER OUTPUT OPENDRAIN HIGH # green + +define HAL_LED_ON 0 + +# order of UARTs (and USB) +SERIAL_ORDER OTG1 UART7 + +# UART7 is debug +PF6 UART7_RX UART7 NODMA +PE8 UART7_TX UART7 NODMA + +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +define BOOTLOADER_DEBUG SD7 + +# microSD support +PC8 SDMMC1_D0 SDMMC1 +PC9 SDMMC1_D1 SDMMC1 +PC10 SDMMC1_D2 SDMMC1 +PC11 SDMMC1_D3 SDMMC1 +PC12 SDMMC1_CK SDMMC1 +PD2 SDMMC1_CMD SDMMC1 +define AP_BOOTLOADER_FLASH_FROM_SD_ENABLED 1 + + +# Add CS pins to ensure they are high in bootloader +PF10 ADIS16470_CS CS +PF2 RM3100_CS CS +PG6 ICM20689_CS CS SPEED_VERYLOW +PI12 ICM20649_CS CS SPEED_VERYLOW +PE15 ICM20689_BOARD_CS CS SPEED_VERYLOW +PF3 BMI088_A_CS CS +PF4 BMI088_G_CS CS +PF5 FRAM_CS CS SPEED_VERYLOW +PG10 MS5611_IMU_CS CS +PI8 MS5611_BOARD_CS CS +PI4 EXT1_CS1 CS +PI10 EXT1_CS2 CS +PI13 EXT1_CS3 CS diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Atlas-Control/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Atlas-Control/hwdef.dat new file mode 100644 index 0000000000000..cc7b34d33762a --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Atlas-Control/hwdef.dat @@ -0,0 +1,304 @@ +# hw definition file for processing by chibios_hwdef.py for Atlas-Control + +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# crystal frequency +OSCILLATOR_HZ 16000000 + +# board ID. See Tools/AP_Bootloader/board_types.txt +APJ_BOARD_ID AP_HW_ATLAS_CONTROL + +FLASH_SIZE_KB 2048 + + +# ChibiOS system timer +STM32_ST_USE_TIMER 2 + +# bootloader takes first sector +FLASH_RESERVE_START_KB 128 + +# order of UARTs (and USB) +SERIAL_ORDER OTG1 USART2 USART6 USART1 UART4 EMPTY UART7 OTG2 + +# now we define the pins that USB is connected on +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +# Port switching for USB HS and FS,high = USB_FS , LOW = USB_HS +PH15 USB_HS_ENABLE OUTPUT HIGH + +# these are the pins for SWD debugging with a STlinkv2 or black-magic probe +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# SPI1 - internal sensors +PG11 SPI1_SCK SPI1 +PA6 SPI1_MISO SPI1 +PD7 SPI1_MOSI SPI1 + +# SPI2 - FRAM +PI1 SPI2_SCK SPI2 +PI2 SPI2_MISO SPI2 +PI3 SPI2_MOSI SPI2 + +# SPI4 - sensors2 +PE2 SPI4_SCK SPI4 +PE13 SPI4_MISO SPI4 +PE6 SPI4_MOSI SPI4 + +# SPI5 - external1 +PF7 SPI5_SCK SPI5 +PF8 SPI5_MISO SPI5 +PF9 SPI5_MOSI SPI5 + +# SPI6 - external2 +PG13 SPI6_SCK SPI6 +PG12 SPI6_MISO SPI6 +PA7 SPI6_MOSI SPI6 + +# sensor + +PF10 ADIS16470_CS CS +PF2 RM3100_CS CS +PG6 ICM20689_CS CS SPEED_VERYLOW +PE15 ICM20689_BOARD_CS CS SPEED_VERYLOW +PI12 ICM20649_CS CS SPEED_VERYLOW +PA15 ICM42688_CS CS SPEED_VERYLOW +PF3 BMI088_A_CS CS +PF4 BMI088_G_CS CS +PF5 FRAM_CS CS SPEED_VERYLOW +PG10 MS5611_IMU_CS CS +PI8 MS5611_BOARD_CS CS + +# external CS pins, SPI5 connector +PI4 EXT1_CS1 CS +PI10 EXT1_CS2 CS +PI13 EXT1_CS3 CS + +# I2C buses + +# I2C1 is on GPS port +PB8 I2C1_SCL I2C1 +PB9 I2C1_SDA I2C1 + +# I2C2 on GPS2 connector +PF1 I2C2_SCL I2C2 +PF0 I2C2_SDA I2C2 + +# I2C3 for onboard mag +PH7 I2C3_SCL I2C3 +PH8 I2C3_SDA I2C3 + +# I2C4 is on BDMA on DMAMUX2 +PF14 I2C4_SCL I2C4 +PF15 I2C4_SDA I2C4 + +# order of I2C buses +I2C_ORDER I2C3 I2C1 I2C2 I2C4 + +define HAL_I2C_INTERNAL_MASK 1 + +# enable pins +PE3 VDD_3V3_SENSORS_EN OUTPUT LOW + +# start peripheral power on +PD11 VDD_5V_HIPOWER_EN OUTPUT HIGH +PG4 VDD_5V_PERIPH_EN OUTPUT HIGH + +PG5 VDD_5V_RC_EN OUTPUT HIGH +PG7 VDD_3V3_SD_CARD_EN OUTPUT HIGH + +# drdy pins +PE7 DRDY1_ADIS16470 INPUT GPIO(93) +PH5 DRDY2_ICM20649 INPUT +PB14 DRDY3_BMI088_GYRO1 INPUT +PB15 DRDY4_BMI088_ACC1 INPUT +PJ0 DRDY5_ICM20689 INPUT +PC13 DRDY6_BMI088_GYRO2 INPUT +PI14 DRDY7_BMI088_ACC2 INPUT +PE4 DRDY8_RM3100 INPUT + +# use GPIO(93) for data ready on ADIS16470 +define ADIS_DRDY_PIN 93 + +# UARTs + +# USART2 is telem1 +PD6 USART2_RX USART2 +PD5 USART2_TX USART2 +PD3 USART2_CTS USART2 +PD4 USART2_RTS USART2 + +# USART1 is GPS1 +PB7 USART1_RX USART1 NODMA +PB6 USART1_TX USART1 NODMA + +# UART4 GPS2 +PD0 UART4_RX UART4 NODMA +PD1 UART4_TX UART4 NODMA + +# USART6 is telem2 +PG9 USART6_RX USART6 +PG14 USART6_TX USART6 +PG15 USART6_CTS USART6 +PG8 USART6_RTS USART6 + +# UART7 is debug +PF6 UART7_RX UART7 NODMA +PE8 UART7_TX UART7 NODMA + +# PWM AUX channels +PH10 TIM5_CH1 TIM5 PWM(1) GPIO(50) BIDIR +PH11 TIM5_CH2 TIM5 PWM(2) GPIO(51) +PH12 TIM5_CH3 TIM5 PWM(3) GPIO(52) BIDIR +PI0 TIM5_CH4 TIM5 PWM(4) GPIO(53) +PD12 TIM4_CH1 TIM4 PWM(5) GPIO(54) BIDIR +PD13 TIM4_CH2 TIM4 PWM(6) GPIO(55) +PD14 TIM4_CH3 TIM4 PWM(7) GPIO(56) BIDIR +PD15 TIM4_CH4 TIM4 PWM(8) GPIO(57) + +PE9 TIM1_CH1 TIM1 PWM(9) GPIO(58) +PE11 TIM1_CH2 TIM1 PWM(10) GPIO(59) +PA10 TIM1_CH3 TIM1 PWM(11) GPIO(60) +PE14 TIM1_CH4 TIM1 PWM(12) GPIO(61) +PH6 TIM12_CH1 TIM12 PWM(13) GPIO(62) NODMA +PH9 TIM12_CH2 TIM12 PWM(14) GPIO(63) NODMA + +# allow for 14 PWMs by default + +# PWM output for buzzer +PE5 TIM15_CH1 TIM15 GPIO(77) ALARM + +# RC input +PB4 TIM3_CH1 TIM3 RCININT PULLDOWN LOW + +# analog in +PA0 BATT_VOLTAGE_SENS ADC1 SCALE(1) +PA1 BATT_CURRENT_SENS ADC1 SCALE(1) + +# ADC3.3/ADC6.6 +PC4 SPARE1_ADC1 ADC1 SCALE(1) + +# Note that this should be SCALE(2), but we don't want to break existing setups +# See: https://github.com/ArduPilot/ardupilot/pull/22831 +PA4 SPARE2_ADC1 ADC1 SCALE(1) + +PF12 RSSI_IN ADC1 SCALE(1) + +PC5 VDD_5V_SENS ADC1 SCALE(2) +PC1 SCALED_V3V3 ADC1 SCALE(2) + +PB1 FMU_SERVORAIL_VCC_SENS ADC1 SCALE(3) + +# CAN bus +PI9 CAN1_RX CAN1 +PH13 CAN1_TX CAN1 + +# 2nd CAN bus. Cannot be used at same time as USB_HS +PB12 CAN2_RX CAN2 +PB13 CAN2_TX CAN2 + +# control for silent (no output) for CAN +PH2 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW GPIO(70) +PH3 GPIO_CAN2_SILENT OUTPUT PUSHPULL SPEED_LOW LOW GPIO(71) + +# GPIOs + +PA8 HEATER_EN OUTPUT LOW GPIO(80) +define HAL_HEATER_GPIO_PIN 80 +define HAL_HAVE_IMU_HEATER 1 + +PG1 VDD_BRICK_nVALID INPUT PULLUP # for PowerC (CAN), considered primary power +PG2 VDD_BRICK2_nVALID INPUT PULLUP # for PowerA +PG0 VBUS_nVALID INPUT PULLUP +PJ3 VDD_5V_HIPOWER_nOC INPUT PULLUP +PJ4 VDD_5V_PERIPH_nOC INPUT PULLUP +PB10 nSPI5_RESET_EXTERNAL1 OUTPUT HIGH + +# SPI devices +SPIDEV adis16470 SPI1 DEVID2 ADIS16470_CS MODE3 1*MHZ 2*MHZ +SPIDEV icm20689 SPI1 DEVID2 ICM20689_CS MODE3 2*MHZ 8*MHZ +SPIDEV bmi088_a SPI4 DEVID2 BMI088_A_CS MODE3 10*MHZ 10*MHZ +SPIDEV bmi088_g SPI4 DEVID3 BMI088_G_CS MODE3 10*MHZ 10*MHZ +SPIDEV ms5611_imu SPI4 DEVID1 MS5611_IMU_CS MODE3 20*MHZ 20*MHZ +SPIDEV ms5611_board SPI6 DEVID1 MS5611_BOARD_CS MODE3 20*MHZ 20*MHZ +SPIDEV ramtron SPI2 DEVID1 FRAM_CS MODE3 8*MHZ 8*MHZ + +#Mount icm20649 or icm20689 on SPI6 +SPIDEV icm20649 SPI6 DEVID2 ICM20649_CS MODE3 2*MHZ 8*MHZ +SPIDEV icm20689_board SPI6 DEVID2 ICM20689_BOARD_CS MODE3 2*MHZ 8*MHZ + +#Mount bmi088_a or icm42688 on SPI4 +SPIDEV icm42688 SPI4 DEVID2 ICM42688_CS MODE3 2*MHZ 8*MHZ + +# RM3100 may be on SPI1 or SPI2 (not both). Later board revisions +# have the RM3100 on SPI2, to leave SPI1 free for ADIS1647x +SPIDEV rm3100-1 SPI1 DEVID1 RM3100_CS MODE3 1*MHZ 1*MHZ +SPIDEV rm3100-2 SPI2 DEVID2 RM3100_CS MODE3 1*MHZ 1*MHZ + +# two baro +BARO MS5611 SPI:ms5611_imu +BARO MS5611 SPI:ms5611_board + +# three IMUs, only one of ICM20689 and ADIS16470 will be included +IMU ADIS1647x SPI:adis16470 ROTATION_PITCH_180_YAW_270 ADIS_DRDY_PIN +IMU Invensensev3 SPI:icm42688 ROTATION_PITCH_180_YAW_270 +IMU Invensense SPI:icm20689 ROTATION_NONE +IMU Invensense SPI:icm20689_board ROTATION_NONE + +define HAL_DEFAULT_INS_FAST_SAMPLE 5 + +# compasses +define AP_COMPASS_PROBING_ENABLED 1 +# probe for RM3100 on SPI1 or SPI2 +COMPASS RM3100 SPI:rm3100-2 false ROTATION_ROLL_180_YAW_90 + +# microSD support +PC8 SDMMC1_D0 SDMMC1 +PC9 SDMMC1_D1 SDMMC1 +PC10 SDMMC1_D2 SDMMC1 +PC11 SDMMC1_D3 SDMMC1 +PC12 SDMMC1_CK SDMMC1 +PD2 SDMMC1_CMD SDMMC1 + +# red LED marked as B/E + +PI5 LED_R1 OUTPUT OPENDRAIN HIGH GPIO(0) +PI6 LED_G1 OUTPUT OPENDRAIN LOW GPIO(1) +PI7 LED_B1 OUTPUT OPENDRAIN HIGH GPIO(2) + +define AP_NOTIFY_GPIO_LED_RGB_RED_PIN 0 +define AP_NOTIFY_GPIO_LED_RGB_GREEN_PIN 1 +define AP_NOTIFY_GPIO_LED_RGB_BLUE_PIN 2 + +# use pixracer style 3-LED indicators +define AP_NOTIFY_GPIO_LED_RGB_ENABLED 1 + +# enable RAMTROM parameter storage +define HAL_STORAGE_SIZE 32768 +define HAL_WITH_RAMTRON 1 + +# allow to have have a dedicated safety switch pin +define HAL_HAVE_SAFETY_SWITCH 1 +PE12 LED_SAFETY OUTPUT +PE10 SAFETY_IN INPUT PULLDOWN + +# enable FAT filesystem support (needs a microSD defined via SDMMC) +define HAL_OS_FATFS_IO 1 + +DMA_PRIORITY ADC* SPI1* TIM*UP* +DMA_NOSHARE SPI1* TIM*UP* + +# default to 45C target temp +define HAL_IMU_TEMP_DEFAULT 45 + +# Enable Sagetech MXS ADSB transponder +define HAL_ADSB_SAGETECH_MXS_ENABLED HAL_ADSB_ENABLED + +# Battery monitoring configuration +define HAL_BATT_MONITOR_DEFAULT 4 +define HAL_BATT_VOLT_PIN 16 +define HAL_BATT_CURR_PIN 17 +define HAL_BATT_VOLT_SCALE 18.000 +define HAL_BATT_CURR_SCALE 24.000 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Atlas-Control/img.png b/libraries/AP_HAL_ChibiOS/hwdef/Atlas-Control/img.png new file mode 100644 index 0000000000000..f2e3e47695418 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/Atlas-Control/img.png differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/BROTHERHOBBYH743/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/BROTHERHOBBYH743/hwdef.dat index 82848db9184ca..71e447d56149f 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/BROTHERHOBBYH743/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/BROTHERHOBBYH743/hwdef.dat @@ -28,11 +28,12 @@ PE2 VBUS INPUT OPENDRAIN PA13 JTMS-SWDIO SWD PA14 JTCK-SWCLK SWD -# SPI1 for IMU1 (ICM42688) +# SPI1 for IMU1 (ICM42688 / BMI088) PA5 SPI1_SCK SPI1 PA6 SPI1_MISO SPI1 PD7 SPI1_MOSI SPI1 PC15 IMU1_CS CS +PE3 BMI088_ACCEL_CS CS # SPI2 for MAX7456 OSD PB12 MAX7456_CS CS @@ -46,11 +47,12 @@ PB4 SPI3_MISO SPI3 PB5 SPI3_MOSI SPI3 PD4 EXT_CS1 CS -# SPI4 for IMU2 (ICM42688) +# SPI4 for IMU2 (ICM4268 2 / BMI088 2) PE11 IMU2_CS CS PE12 SPI4_SCK SPI4 PE13 SPI4_MISO SPI4 PE14 SPI4_MOSI SPI4 +PA4 BMI088_ACCEL2_CS CS # two I2C bus I2C_ORDER I2C2 I2C1 @@ -174,6 +176,11 @@ SPIDEV imu1 SPI1 DEVID1 IMU1_CS MODE3 2*MHZ 16*MHZ # Clock is 100Mhz so SPIDEV imu2 SPI4 DEVID1 IMU2_CS MODE3 2*MHZ 16*MHZ SPIDEV osd SPI2 DEVID4 MAX7456_CS MODE0 10*MHZ 10*MHZ +SPIDEV bmi088_g SPI1 DEVID1 IMU1_CS MODE3 10*MHZ 10*MHZ +SPIDEV bmi088_a SPI1 DEVID2 BMI088_ACCEL_CS MODE3 10*MHZ 10*MHZ +SPIDEV bmi088_g2 SPI4 DEVID1 IMU2_CS MODE3 10*MHZ 10*MHZ +SPIDEV bmi088_a2 SPI4 DEVID2 BMI088_ACCEL2_CS MODE3 10*MHZ 10*MHZ + DMA_PRIORITY SPI1* SPI4* DMA_NOSHARE SPI1* SPI4* TIM3* TIM2* TIM5* TIM4* @@ -187,6 +194,8 @@ define HAL_COMPASS_AUTO_ROT_DEFAULT 2 # two IMUs IMU Invensensev3 SPI:imu1 ROTATION_PITCH_180_YAW_270 IMU Invensensev3 SPI:imu2 ROTATION_ROLL_180 +IMU BMI088 SPI:bmi088_a SPI:bmi088_g ROTATION_PITCH_180 +IMU BMI088 SPI:bmi088_a2 SPI:bmi088_g2 ROTATION_PITCH_180 define HAL_DEFAULT_INS_FAST_SAMPLE 3 # DPS310 integrated on I2C2 bus diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CSKY_PMU/CSKYF4_PMU_Board.jpg b/libraries/AP_HAL_ChibiOS/hwdef/CSKY_PMU/CSKYF4_PMU_Board.jpg new file mode 100644 index 0000000000000..7fe3bd7823322 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/CSKY_PMU/CSKYF4_PMU_Board.jpg differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CSKY_PMU/README.md b/libraries/AP_HAL_ChibiOS/hwdef/CSKY_PMU/README.md new file mode 100644 index 0000000000000..c9761458a3f58 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CSKY_PMU/README.md @@ -0,0 +1,40 @@ +# CSKY GaN DroneCAN PMU + +## Features + +- Processor: STM32F412 100MHz 1024KB Flash 256KB RAM +- Voltage input: 4S-14S (HV) +- Continuous current: 200A +- Burst current: 300A @ 25℃ 1 sec +- Max current sensing: 200A +- Voltage accuracy: ±0.05V +- Current accuracy: ±0.7% +- Temperature accuracy: ±1℃ +- Power port 1 output (Molex): 5V/3A +- Power port 2 output (XT30): 12V/10A +- Efficiency: 97% @10A +- Protocol: DroneCAN +- Operating temperature: -25℃~105℃ +- CAN Firmware updade: Support +- Calibration: No need + +## Interface Type + +- Power 1 & CAN Port: Molex CLIK-Mate 2mm 6Pin +- Power 2 Port: XT30PW-F +- Battery IN/OUT Options: Tinned Wires + +## Status LED + +- Flashing quickly continuously: MCU is in the bootloader stage, waiting for firmware to be flashed +- Flashing quickly for 3 seconds, and then on for 1 second: Waiting for CAN connection +- Flashing slowly (one-second intervals): CAN is successfully connected + +## Mechanical Spec + +- Size: 57.5mm×57.5mm×17.1mm (not include cable) +- Weight: 140g (include cable) + +## Loading Firmware + +You can upgrade the *.bin firmware files using the DroneCan GUI tool. *.apj files can also be upgraded using Mission Planner ground station. diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CSKY_PMU/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CSKY_PMU/defaults.parm new file mode 100644 index 0000000000000..2b8820ef1066a --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CSKY_PMU/defaults.parm @@ -0,0 +1,5 @@ +TEMP1_TYPE 5 +TEMP1_PIN 3 +TEMP1_A0 -41.0 +TEMP1_A1 50.0 +TEMP_MSG_RATE 10 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CSKY_PMU/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CSKY_PMU/hwdef-bl.dat new file mode 100644 index 0000000000000..b4a74e45f4c6b --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CSKY_PMU/hwdef-bl.dat @@ -0,0 +1,44 @@ +# CSKY-PMU Bootloader + +MCU STM32F4xx STM32F412Rx +APJ_BOARD_ID AP_HW_CSKY_PMU + +OSCILLATOR_HZ 8000000 + +env AP_PERIPH 1 + +FLASH_RESERVE_START_KB 0 +FLASH_BOOTLOADER_LOAD_KB 64 +FLASH_SIZE_KB 512 + +define CH_CFG_ST_FREQUENCY 1000000 + +define HAL_USE_SERIAL TRUE + +SERIAL_ORDER USART1 + +define STM32_SERIAL_USE_USART1 TRUE +define STM32_SERIAL_USE_USART2 FALSE +define STM32_SERIAL_USE_USART3 FALSE + +PB6 USART1_TX USART1 +PB7 USART1_RX USART1 + +PB14 LED_BOOTLOADER OUTPUT HIGH +define HAL_LED_ON 0 + +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# enable CAN support +PB8 CAN1_RX CAN1 +PB9 CAN1_TX CAN1 +PB5 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW +define HAL_USE_CAN TRUE +define STM32_CAN_USE_CAN1 TRUE + +# make bl baudrate match debug baudrate for easier debugging +define BOOTLOADER_BAUDRATE 57600 +define HAL_BOOTLOADER_TIMEOUT 1000 + +PA4 INA_CS CS diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CSKY_PMU/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CSKY_PMU/hwdef.dat new file mode 100644 index 0000000000000..ca1f1a510375d --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CSKY_PMU/hwdef.dat @@ -0,0 +1,93 @@ +# hw definition file for processing by chibios_pins.py + +# MCU class and specific type +MCU STM32F4xx STM32F412Rx + +# bootloader starts firmware at 64k +FLASH_RESERVE_START_KB 64 + +# store parameters in pages 2 and 3 +STORAGE_FLASH_PAGE 2 +define HAL_STORAGE_SIZE 8192 + +env ROMFS_UNCOMPRESSED True + +# board ID for firmware load +APJ_BOARD_ID AP_HW_CSKY_PMU + +# setup build for a peripheral firmware +env AP_PERIPH 1 + +# ChibiOS system timer +STM32_ST_USE_TIMER 5 + +# crystal frequency +OSCILLATOR_HZ 8000000 + +# assume 512k flash part +FLASH_SIZE_KB 512 + +define SERIAL_BUFFERS_SIZE 512 +define PORT_INT_REQUIRED_STACK 64 + +define DMA_RESERVE_SIZE 0 + +# order of UARTs +SERIAL_ORDER USART1 + +# USART1 for debug +PB6 USART1_TX USART1 NODMA +PB7 USART1_RX USART1 NODMA +define DEFAULT_SERIAL0_BAUD 57600 + +# debugger support +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +#LEDS +define AP_PERIPH_NOTIFY_ENABLED 1 +define define AP_PERIPH_RC_OUT_ENABLED 1 + +PB14 LED_R OUTPUT HIGH GPIO(0) +PB15 LED_G OUTPUT HIGH GPIO(1) +PB13 LED OUTPUT HIGH GPIO(2) + +define AP_NOTIFY_GPIO_LED_RGB_ENABLED 1 +define AP_NOTIFY_GPIO_LED_RGB_RED_PIN 0 +define AP_NOTIFY_GPIO_LED_RGB_GREEN_PIN 1 +define AP_NOTIFY_GPIO_LED_RGB_BLUE_PIN 2 + +#CAN bus +CAN_ORDER 1 +PB8 CAN1_RX CAN1 +PB9 CAN1_TX CAN1 +PB5 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW + +define CAN_APP_NODE_NAME "org.csky.pmu" + +# SPI1 RM3100 +PA5 SPI1_SCK SPI1 +PA6 SPI1_MISO SPI1 +PA7 SPI1_MOSI SPI1 +PA4 INA_CS CS +SPIDEV INA23X SPI1 DEVID1 INA_CS MODE1 10*MHZ 10*MHZ + +# enable temperature +define AP_TEMPERATURE_SENSOR_ENABLED 1 +define AP_TEMPERATURE_SENSOR_MAX_INSTANCES 1 +define AP_PERIPH_DEVICE_TEMPERATURE_ENABLED 1 + +# ADC +define HAL_USE_ADC TRUE +define STM32_ADC_USE_ADC1 TRUE + +PA3 BATT_TEMP_SENS ADC1 SCALE(1) + +# battery monitor +define AP_PERIPH_BATTERY_ENABLED 1 +define AP_BATTERY_INA239_ENABLED 1 +define AP_BATTERY_INA239_SPI_DEVICE "INA23X" +define HAL_BATT_MONITOR_DEFAULT 26 + +# bootloader embedding / bootloader flashing not available +define AP_BOOTLOADER_FLASHING_ENABLED 0 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/defaults.parm index 9b23cbda6b5f0..55ce8ee337e30 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/defaults.parm +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/defaults.parm @@ -86,9 +86,6 @@ INS_HNTC2_ENABLE,1 INS_HNTC2_FREQ,98 INS_HNTC2_BW,49 INS_TRIM_OPTION,2 -LAND_ALT_LOW,600 -LAND_SPEED,45 -LAND_SPEED_HIGH,220 LOG_BACKEND_TYPE,3 LOG_FILE_BUFSIZE,8 LOG_FILE_DSRMROT,1 @@ -121,10 +118,10 @@ PILOT_SPEED_UP,133 PILOT_THR_BHV,7 PILOT_THR_FILT,2 PILOT_TKOFF_ALT,214 -PSC_ACCZ_I,1.0 -PSC_ACCZ_P,0.5 -PSC_VELXY_I,0.51 -PSC_VELXY_P,1.41 +PSC_D_ACC_I,0.1 +PSC_D_ACC_P,0.05 +PSC_NE_VEL_I,0.51 +PSC_NE_VEL_P,1.41 RC1_MAX,2000 RC1_MIN,1000 RC2_MAX,2000 @@ -140,9 +137,6 @@ RC6_OPTION,213 RC6_TRIM,1260 RC7_MAX,2000 RC7_MIN,1000 -RTL_ALT,4500 -RTL_CLIMB_MIN,1000 -RTL_SPEED,800 SERIAL1_BAUD,921 SERIAL4_BAUD,230 SERIAL4_PROTOCOL,1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-periph-heavy/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-periph-heavy/hwdef.dat index edb71c0c34ebc..df542ede028cd 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-periph-heavy/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-periph-heavy/hwdef.dat @@ -51,7 +51,7 @@ define HAL_CAN_DRIVER_DEFAULT 1 define HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT 0 define AP_RC_CHANNEL_ENABLED 1 -define HAL_PICCOLO_CAN_ENABLE 0 +define AP_PICCOLOCAN_ENABLED 0 define AP_RELAY_ENABLED 1 define AP_SERVORELAYEVENTS_ENABLED 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/defaults.parm index 99171e686f5af..86077c3b71895 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/defaults.parm +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/defaults.parm @@ -87,9 +87,6 @@ INS_HNTC2_ENABLE,1 INS_HNTC2_FREQ,98 INS_HNTC2_BW,49 INS_TRIM_OPTION,2 -LAND_ALT_LOW,600 -LAND_SPEED,45 -LAND_SPEED_HIGH,220 LOG_BACKEND_TYPE,3 LOG_FILE_BUFSIZE,8 LOG_FILE_DSRMROT,1 @@ -121,10 +118,10 @@ PILOT_SPEED_UP,133 PILOT_THR_BHV,7 PILOT_THR_FILT,2 PILOT_TKOFF_ALT,214 -PSC_ACCZ_I,1.5 -PSC_ACCZ_P,0.75 -PSC_VELXY_I,0.51 -PSC_VELXY_P,1.41 +PSC_D_ACC_I,0.15 +PSC_D_ACC_P,0.075 +PSC_NE_VEL_I,0.51 +PSC_NE_VEL_P,1.41 RC1_MAX,2000 RC1_MIN,1000 RC2_MAX,2000 @@ -140,9 +137,6 @@ RC6_OPTION,213 RC6_TRIM,1260 RC7_MAX,2000 RC7_MIN,1000 -RTL_ALT,4500 -RTL_CLIMB_MIN,1000 -RTL_SPEED,800 SERIAL1_BAUD,921 SERIAL1_PROTOCOL,1 SERIAL2_PROTOCOL,1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Durandal/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Durandal/hwdef.dat index e89c4d480db3b..a9a93c276615b 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Durandal/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Durandal/hwdef.dat @@ -24,9 +24,6 @@ FLASH_RESERVE_START_KB 128 # order of UARTs (and USB) SERIAL_ORDER OTG1 USART2 USART3 USART1 UART4 USART6 UART7 OTG2 -define HAL_CHIBIOS_ARCH_FMUV5 1 -define BOARD_TYPE_DEFAULT 24 - # now we define the pins that USB is connected on PA11 OTG_FS_DM OTG1 PA12 OTG_FS_DP OTG1 @@ -309,6 +306,16 @@ BARO MS5611 SPI:ms5611 define HAL_COMPASS_AUTO_ROT_DEFAULT 2 +# IS8310 compasses: +COMPASS IST8310 I2C:ALL_EXTERNAL:0x0E true ROTATION_ROLL_180_YAW_90 +COMPASS IST8310 I2C:ALL_INTERNAL:0x0E false ROTATION_ROLL_180_YAW_90 + +define AP_COMPASS_PROBING_ENABLED 1 + +# don't probe for IS8310 on external buses. For some reason. +define AP_COMPASS_IST8310_INTERNAL_BUS_PROBING_ENABLED 0 +define AP_COMPASS_IST8310_EXTERNAL_BUS_PROBING_ENABLED 0 + # enable FAT filesystem support (needs a microSD defined via SDMMC) define HAL_OS_FATFS_IO 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/F4BY_H743/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/F4BY_H743/hwdef.dat index 7afec38cc2736..5dd34bc778aed 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/F4BY_H743/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/F4BY_H743/hwdef.dat @@ -120,7 +120,7 @@ PB11 I2C2_SDA I2C2 # look for I2C compass COMPASS HMC5843 I2C:0:0x1E false ROTATION_YAW_270 -COMPASS QMC5883L I2C:0:0x0D false ROTATION_YAW_180 +COMPASS QMC5883L I2C:0:0x0D false ROTATION_YAW_270 COMPASS QMC5883P I2C:0:0x2C false ROTATION_YAW_180 define AP_COMPASS_PROBING_ENABLED 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/JFB100/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/JFB100/hwdef.dat index 1e786f8ddb5ae..67564c954b00e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/JFB100/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/JFB100/hwdef.dat @@ -8,8 +8,6 @@ MCU STM32F7xx STM32F767xx # crystal frequency OSCILLATOR_HZ 16000000 -define HAL_CHIBIOS_ARCH_FMUV5 1 - # board ID. See Tools/AP_Bootloader/board_types.txt APJ_BOARD_ID AP_HW_JFB100 @@ -257,6 +255,8 @@ define HAL_DEFAULT_INS_FAST_SAMPLE 1 # probe external I2C compasses plus some internal IST8310 # we also probe some external IST8310 with a non-standard orientation define AP_COMPASS_PROBING_ENABLED 1 +define AP_COMPASS_IST8310_EXTERNAL_BUS_PROBING_ENABLED 0 +define AP_COMPASS_IST8310_INTERNAL_BUS_PROBING_ENABLED 0 COMPASS IST8310 I2C:ALL_EXTERNAL:0x0E true ROTATION_ROLL_180_YAW_90 COMPASS IST8310 I2C:ALL_INTERNAL:0x0E false ROTATION_YAW_270 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MambaH743v4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MambaH743v4/hwdef.dat index d4d824d809ede..e7765ca110166 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MambaH743v4/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MambaH743v4/hwdef.dat @@ -22,7 +22,7 @@ define CH_CFG_ST_RESOLUTION 16 FLASH_RESERVE_START_KB 384 env OPTIMIZE -O2 -# use last 2 pages for flash storage +# use 2 pages for flash storage # H743 has 16 pages of 128k each STORAGE_FLASH_PAGE 1 define HAL_STORAGE_SIZE 32768 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3-Wing/H7A3-WING.jpg b/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3-Wing/H7A3-WING.jpg new file mode 100644 index 0000000000000..311fbb69c0180 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3-Wing/H7A3-WING.jpg differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3-Wing/README.md b/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3-Wing/README.md new file mode 100644 index 0000000000000..87a8f43725953 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3-Wing/README.md @@ -0,0 +1,117 @@ +# Matek H7A3-Wing Flight Controller + +https://www.mateksys.com/?portfolio=h7a3-wing. + + +## Features + Processor + STM32H7A3RIT6 Cortex-M7 280 MHz, 2MB flash + Sensors + ICM-42688p Acc/Gyro + SPL06-001 barometer + AT7456E OSD + W25N01GV dataflash + Power + 2S - 6S Lipo input voltage with voltage monitoring + 9V, 2A BEC for powering Video Transmitter and camera controlled by GPIO + 5V, 2A BEC for internal and peripherals + Interfaces + 11x PWM outputs DShot capable + 6x UARTs + 1x CAN + 1x I2C + 4x ADC + USB-C port + LED + Red, 3.3V power indicator + Blue and Green, FC status + Size + 36 x 36mm PCB with 30.5mm M3 mounting + + +## Overview + +![MATEKH7A3-SLIM](H7A3-WING.jpg) + +## Wiring Diagram + + +## UART Mapping + +The UARTs are marked Rx* and Tx* in the above pinouts. The Rx* pin is the +receive pin for UART*. The Tx* pin is the transmit pin for UART*. + + - SERIAL0 -> USB + - SERIAL1 -> USART1 (MAVLink2 telem) (DMA capable) + - SERIAL2 -> USART2 (Serial RC input) (DMA capable) + - SERIAL3 -> USART3 (GPS) (DMA capable) + - SERIAL4 -> UART4 (User) (NO DMA) + - SERIAL5 -> UART5 (User) (NO DMA) + - SERIAL6 -> USART6 (User) (NO DMA) + +## CAN and I2C + +H7A3-SLIM supports 1x CAN bus and 1x I2C bus +multiple CAN peripherals can be connected to one CAN bus in parallel. similarly for I2C bus. + +## RC Input + +RC input is configured on the USART2(SERIAL2). It supports all serial RC protocols. SERIAL2_PROTOCOL=23 by default. +- PPM is not supported. +- CRSF requires Tx2 & Rx2 connection, and set SERIAL2_OPTIONS to “0” (default). +- SBUS/DSM/SRXL connects to the Rx2 pin, but SBUS requires that the SERIAL2_OPTIONS be set to “3”. +- FPort requires connection to Tx2, and set SERIAL2_OPTIONS to “7”. +- SRXL2 requires a connection to Tx2, and automatically provides telemetry. Set SERIAL2_OPTIONS to “4”. + + +## OSD Support + +H7A3-SLIM supports using its internal OSD using OSD_TYPE 1 (MAX7456 driver). External OSD support such as DJI or DisplayPort is supported using any spare UART. See :ref:`common-msp-osd-overview-4.2` for more info. + +## PWM Output + +H7A3-SLIM supports up to 11 PWM outputs. All outputs support DShot. +The PWM is in 5 groups: + + - PWM 1,2 TIM1 + - PWM 3,4 TIM2 + - PWM 5,6,7,8 TIM3 + - PWM 9,10 TIM4 + - PWM 11 TIM16 + +## Battery Monitoring + +The board has 2x built-in voltage dividers and 2x current ADC. support external 3.3V based current sensor + +The correct battery setting parameters are set by default and are: + + - BATT_MONITOR 4 + + - BATT_VOLT_PIN 10 + - BATT_CURR_PIN 11 + - BATT_VOLT_MULT 21.0 ("Vbat" pad support 5.5~30V input, limited by onboard regulator chips) + - BATT_AMP_PERVLT X ("Curr" pad, set it according to external current sensor spec) + + - BATT2_VOLT_PIN 18 + - BATT2_CURR_PIN 8 + - BATT2_VOLT_MULT 21.0 ("VB2" pad support Max.69V voltage sense) + - BATT_AMP_PERVLT X ("CU2" pad, set it according to external current sensor spec) + + +## Compass + +H7A3-SLIM does not have a built-in compass, but you can attach an external compass using I2C on the SDA and SCL pads. + +## VTX power control + +GPIO 81 controls the 9V BEC output to pins marked "9V". Setting this GPIO high removes voltage supply to pins. Default GPIO 81 is low(9V output enable) + +## Loading Firmware +Firmware for these boards can be found at https://firmware.ardupilot.org in sub-folders labeled MatekH7A3-Wing. + +Initial firmware load can be done with DFU by plugging in USB with the +boot button pressed. Then you should load the "ardu*_with_bl.hex" firmware, using your favourite DFU loading tool. eg STM32CubeProgrammer + +Subsequently, you can update firmware with Mission Planner. + + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3-Wing/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3-Wing/hwdef-bl.dat new file mode 100644 index 0000000000000..1bf102f2c050a --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3-Wing/hwdef-bl.dat @@ -0,0 +1,6 @@ +# hw definition file for Matek H7A3 Wing bootloader + +include ../MatekH7A3/hwdef-bl.inc + +# board ID. See Tools/AP_Bootloader/board_types.txt +APJ_BOARD_ID AP_HW_MatekH7A3_Wing diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3-Wing/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3-Wing/hwdef.dat new file mode 100644 index 0000000000000..e7018647cb15d --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3-Wing/hwdef.dat @@ -0,0 +1,11 @@ +# hw definition file for Matek H7A3-Wing + +include ../MatekH7A3/hwdef.inc + +# board ID. See Tools/AP_Bootloader/board_types.txt +APJ_BOARD_ID AP_HW_MatekH7A3_Wing + +# --------------------- SD ---------------------- +SPIDEV sdcard SPI2 DEVID3 SD_CS MODE0 400*KHZ 25*MHZ + +define HAL_OS_FATFS_IO 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef-bl.dat index ce683c2bf1a49..a3a75ed9e807e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef-bl.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef-bl.dat @@ -1,47 +1,6 @@ -# hw definition file for processing by chibios_pins.py -# for Matek H7A3 bootloader +# hw definition file for Matek H7A3 bootloader -# MCU class and specific type -MCU STM32H7xx STM32H7A3xx +include hwdef-bl.inc # board ID. See Tools/AP_Bootloader/board_types.txt APJ_BOARD_ID AP_HW_MatekH7A3 - -# crystal frequency, setup to use external oscillator -OSCILLATOR_HZ 16000000 - -FLASH_SIZE_KB 2048 - -# bootloader starts at zero offset -FLASH_RESERVE_START_KB 0 - -# the location where the bootloader will put the firmware -# the H7A3 has 8k sectors -FLASH_BOOTLOADER_LOAD_KB 32 - - -# order of UARTs (and USB). Allow bootloading on USB and telem1 -SERIAL_ORDER OTG1 USART1 - -# USART1 (telem1) -PB15 USART1_RX USART1 NODMA -PB14 USART1_TX USART1 NODMA - -# PA10 IO-debug-console -PA11 OTG_HS_DM OTG1 -PA12 OTG_HS_DP OTG1 - -# PA13 JTMS-SWDIO SWD -# PA14 JTCK-SWCLK SWD - -# make sure Vsw is on during bootloader -PC13 PINIO1 OUTPUT LOW - -PA14 LED_BOOTLOADER OUTPUT LOW -define HAL_LED_ON 0 - -# Add CS pins to ensure they are high in bootloader -PC4 IMU_CS CS -PB2 MAX7456_CS CS -PC15 FLASH_CS CS -PC14 SD_CS CS diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef-bl.inc b/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef-bl.inc new file mode 100644 index 0000000000000..a355207acdfdd --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef-bl.inc @@ -0,0 +1,44 @@ +# hw definition file for processing by chibios_pins.py +# for Matek H7A3 bootloader + +# MCU class and specific type +MCU STM32H7xx STM32H7A3xx + +# crystal frequency, setup to use external oscillator +OSCILLATOR_HZ 16000000 + +FLASH_SIZE_KB 2048 + +# bootloader starts at zero offset +FLASH_RESERVE_START_KB 0 + +# the location where the bootloader will put the firmware +# the H7A3 has 8k sectors +FLASH_BOOTLOADER_LOAD_KB 32 + + +# order of UARTs (and USB). Allow bootloading on USB and telem1 +SERIAL_ORDER OTG1 USART1 + +# USART1 (telem1) +PB15 USART1_RX USART1 NODMA +PB14 USART1_TX USART1 NODMA + +# PA10 IO-debug-console +PA11 OTG_HS_DM OTG1 +PA12 OTG_HS_DP OTG1 + +# PA13 JTMS-SWDIO SWD +# PA14 JTCK-SWCLK SWD + +# make sure Vsw is on during bootloader +PC13 PINIO1 OUTPUT LOW + +PA14 LED_BOOTLOADER OUTPUT LOW +define HAL_LED_ON 0 + +# Add CS pins to ensure they are high in bootloader +PC4 IMU_CS CS +PB2 MAX7456_CS CS +PC15 FLASH_CS CS +PC14 SD_CS CS diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef.dat index b02244006ead3..f03270f878908 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef.dat @@ -1,177 +1,11 @@ -# hw definition file for processing by chibios_pins.py -# for Matek H7A3-slim +# hw definition file for Matek H7A3-slim -# MCU class and specific type -MCU STM32H7xx STM32H7A3xx +include hwdef.inc # board ID. See Tools/AP_Bootloader/board_types.txt APJ_BOARD_ID AP_HW_MatekH7A3 -# crystal frequency, setup to use external oscillator -OSCILLATOR_HZ 16000000 - -FLASH_SIZE_KB 2048 - -# bootloader takes first 4 sectors -FLASH_RESERVE_START_KB 32 - -# ChibiOS system timer -STM32_ST_USE_TIMER 5 -define CH_CFG_ST_RESOLUTION 32 - -define HAL_STORAGE_SIZE 15360 - -# use last 4 pages for flash storage -# H7A3 has 256 pages of 8k each -STORAGE_FLASH_PAGE 252 - -# use double page size for flash storage to effectively -# give 16k pages -define AP_FLASH_STORAGE_DOUBLE_PAGE 1 -# # # # # # # # # # # # # # # # # # # # # # - - -# USB -PA11 OTG_HS_DM OTG1 -PA12 OTG_HS_DP OTG1 - -# Debug pins, PA14/PA13 are shared with the LEDs so that needs to be disabled -#PA13 JTMS-SWDIO SWD -#PA14 JTCK-SWCLK SWD - -# SPI1 for ICM42688P & MAX7456 -PA5 SPI1_SCK SPI1 -PA6 SPI1_MISO SPI1 -PA7 SPI1_MOSI SPI1 - -# SPI2 for SD/NAND -PB10 SPI2_SCK SPI2 -PC2 SPI2_MISO SPI2 -PC3 SPI2_MOSI SPI2 - -# CS pins -PC4 IMU_CS CS -PB2 MAX7456_CS CS -PC15 FLASH_CS CS -PC14 SD_CS CS - -# ONE I2C bus -I2C_ORDER I2C3 -PA8 I2C3_SCL I2C3 -PC9 I2C3_SDA I2C3 - - -# ADC -PC0 BATT_VOLTAGE_SENS ADC1 SCALE(1) -PC1 BATT_CURRENT_SENS ADC1 SCALE(1) -PA4 BATT2_VOLTAGE_SENS ADC1 SCALE(1) -PC5 BATT2_CURRENT_SENS ADC1 SCALE(1) - -define HAL_BATT_MONITOR_DEFAULT 4 -define HAL_BATT_VOLT_PIN 10 -define HAL_BATT_CURR_PIN 11 -define HAL_BATT2_VOLT_PIN 18 -define HAL_BATT2_CURR_PIN 8 -define HAL_BATT_VOLT_SCALE 21.0 -define HAL_BATT_CURR_SCALE 40.0 -define HAL_BATT2_VOLT_SCALE 21.0 - -# LED -# green LED1 marked as B/E -# blue LED0 marked as ACT -define AP_NOTIFY_GPIO_LED_2_ENABLED 1 -PA14 LED0 OUTPUT LOW GPIO(90) # blue -PA13 LED1 OUTPUT LOW GPIO(91) # green -define HAL_GPIO_A_LED_PIN 91 -define HAL_GPIO_B_LED_PIN 90 - - -# --------------------- UARTs --------------------------- -SERIAL_ORDER OTG1 USART1 USART2 USART3 UART4 UART5 USART6 - -# USART1 -PB14 USART1_TX USART1 -PB15 USART1_RX USART1 - -# USART2 -PA2 USART2_TX USART2 -PA3 USART2_RX USART2 - -# USART3 -PC10 USART3_TX USART3 -PC11 USART3_RX USART3 - -# UART4 -PA0 UART4_TX UART4 NODMA -PA1 UART4_RX UART4 NODMA - -# UART5 -PC12 UART5_TX UART5 NODMA -PD2 UART5_RX UART5 NODMA - -# USART6 -PC6 USART6_TX USART6 NODMA -PC7 USART6_RX USART6 NODMA - - -# CAN bus -PB12 CAN2_RX CAN2 -PB13 CAN2_TX CAN2 - -# PWM -PA9 TIM1_CH2 TIM1 PWM(1) GPIO(50) BIDIR -PA10 TIM1_CH3 TIM1 PWM(2) GPIO(51) BIDIR -PA15 TIM2_CH1 TIM2 PWM(3) GPIO(52) BIDIR -PB3 TIM2_CH2 TIM2 PWM(4) GPIO(53) -PB0 TIM3_CH3 TIM3 PWM(5) GPIO(54) BIDIR -PB1 TIM3_CH4 TIM3 PWM(6) GPIO(55) -PB4 TIM3_CH1 TIM3 PWM(7) GPIO(56) -PB5 TIM3_CH2 TIM3 PWM(8) GPIO(57) BIDIR -PB6 TIM4_CH1 TIM4 PWM(9) GPIO(58) -PB7 TIM4_CH2 TIM4 PWM(10) GPIO(59) -PB8 TIM16_CH1 TIM16 PWM(11) GPIO(60) - -# Beeper -PB9 TIM17_CH1 TIM17 GPIO(32) ALARM - -# GPIOs -PC13 PINIO1 OUTPUT GPIO(81) LOW -define RELAY1_PIN_DEFAULT 81 - -DMA_PRIORITY S* -DMA_NOSHARE SPI1* SPI2* - - -# ------------------- IMU ICM42688p ------------------ -SPIDEV icm42688 SPI1 DEVID1 IMU_CS MODE3 2*MHZ 16*MHZ - -IMU Invensensev3 SPI:icm42688 ROTATION_NONE -define HAL_DEFAULT_INS_FAST_SAMPLE 1 - - -# ------------------ OSD AT7456E ---------------------- -SPIDEV osd SPI1 DEVID4 MAX7456_CS MODE0 10*MHZ 10*MHZ - -define OSD_ENABLED 1 -define HAL_OSD_TYPE_DEFAULT 1 -ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin - - -# --------------------- SD & FLASH ---------------------- -SPIDEV sdcard SPI2 DEVID3 SD_CS MODE0 400*KHZ 25*MHZ +# --------------------- FLASH ---------------------- SPIDEV dataflash SPI2 DEVID1 FLASH_CS MODE3 104*MHZ 104*MHZ DATAFLASH littlefs:w25nxx - -# ----------------- I2C compass & Baro ----------------- -# no built-in compass, but probe the i2c bus for all possible -# external compass types -define ALLOW_ARM_NO_COMPASS -define AP_COMPASS_PROBING_ENABLED 1 -define HAL_I2C_INTERNAL_MASK 0 -define HAL_COMPASS_AUTO_ROT_DEFAULT 2 - -# built-in barometer -BARO SPL06 I2C:0:0x76 -define AP_BARO_BACKEND_DEFAULT_ENABLED 0 -define AP_BARO_SPL06_ENABLED 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef.inc b/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef.inc new file mode 100644 index 0000000000000..b75cf90a01f35 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef.inc @@ -0,0 +1,168 @@ +# hw definition file for processing by chibios_pins.py +# for Matek H7A3-slim + +# MCU class and specific type +MCU STM32H7xx STM32H7A3xx + +# crystal frequency, setup to use external oscillator +OSCILLATOR_HZ 16000000 + +FLASH_SIZE_KB 2048 + +# bootloader takes first 4 sectors +FLASH_RESERVE_START_KB 32 + +# ChibiOS system timer +STM32_ST_USE_TIMER 5 +define CH_CFG_ST_RESOLUTION 32 + +define HAL_STORAGE_SIZE 15360 + +# use last 4 pages for flash storage +# H7A3 has 256 pages of 8k each +STORAGE_FLASH_PAGE 252 + +# use double page size for flash storage to effectively +# give 16k pages +define AP_FLASH_STORAGE_DOUBLE_PAGE 1 +# # # # # # # # # # # # # # # # # # # # # # + + +# USB +PA11 OTG_HS_DM OTG1 +PA12 OTG_HS_DP OTG1 + +# Debug pins, PA14/PA13 are shared with the LEDs so that needs to be disabled +#PA13 JTMS-SWDIO SWD +#PA14 JTCK-SWCLK SWD + +# SPI1 for ICM42688P & MAX7456 +PA5 SPI1_SCK SPI1 +PA6 SPI1_MISO SPI1 +PA7 SPI1_MOSI SPI1 + +# SPI2 for SD/NAND +PB10 SPI2_SCK SPI2 +PC2 SPI2_MISO SPI2 +PC3 SPI2_MOSI SPI2 + +# CS pins +PC4 IMU_CS CS +PB2 MAX7456_CS CS +PC15 FLASH_CS CS +PC14 SD_CS CS + +# ONE I2C bus +I2C_ORDER I2C3 +PA8 I2C3_SCL I2C3 +PC9 I2C3_SDA I2C3 + + +# ADC +PC0 BATT_VOLTAGE_SENS ADC1 SCALE(1) +PC1 BATT_CURRENT_SENS ADC1 SCALE(1) +PA4 BATT2_VOLTAGE_SENS ADC1 SCALE(1) +PC5 BATT2_CURRENT_SENS ADC1 SCALE(1) + +define HAL_BATT_MONITOR_DEFAULT 4 +define HAL_BATT_VOLT_PIN 10 +define HAL_BATT_CURR_PIN 11 +define HAL_BATT2_VOLT_PIN 18 +define HAL_BATT2_CURR_PIN 8 +define HAL_BATT_VOLT_SCALE 21.0 +define HAL_BATT_CURR_SCALE 40.0 +define HAL_BATT2_VOLT_SCALE 21.0 + +# LED +# green LED1 marked as B/E +# blue LED0 marked as ACT +define AP_NOTIFY_GPIO_LED_2_ENABLED 1 +PA14 LED0 OUTPUT LOW GPIO(90) # blue +PA13 LED1 OUTPUT LOW GPIO(91) # green +define HAL_GPIO_A_LED_PIN 91 +define HAL_GPIO_B_LED_PIN 90 + + +# --------------------- UARTs --------------------------- +SERIAL_ORDER OTG1 USART1 USART2 USART3 UART4 UART5 USART6 + +# USART1 +PB14 USART1_TX USART1 +PB15 USART1_RX USART1 + +# USART2 +PA2 USART2_TX USART2 +PA3 USART2_RX USART2 + +# USART3 +PC10 USART3_TX USART3 +PC11 USART3_RX USART3 + +# UART4 +PA0 UART4_TX UART4 NODMA +PA1 UART4_RX UART4 NODMA + +# UART5 +PC12 UART5_TX UART5 NODMA +PD2 UART5_RX UART5 NODMA + +# USART6 +PC6 USART6_TX USART6 NODMA +PC7 USART6_RX USART6 NODMA + + +# CAN bus +PB12 CAN2_RX CAN2 +PB13 CAN2_TX CAN2 + +# PWM +PA9 TIM1_CH2 TIM1 PWM(1) GPIO(50) BIDIR +PA10 TIM1_CH3 TIM1 PWM(2) GPIO(51) BIDIR +PA15 TIM2_CH1 TIM2 PWM(3) GPIO(52) BIDIR +PB3 TIM2_CH2 TIM2 PWM(4) GPIO(53) +PB0 TIM3_CH3 TIM3 PWM(5) GPIO(54) BIDIR +PB1 TIM3_CH4 TIM3 PWM(6) GPIO(55) +PB4 TIM3_CH1 TIM3 PWM(7) GPIO(56) +PB5 TIM3_CH2 TIM3 PWM(8) GPIO(57) BIDIR +PB6 TIM4_CH1 TIM4 PWM(9) GPIO(58) +PB7 TIM4_CH2 TIM4 PWM(10) GPIO(59) +PB8 TIM16_CH1 TIM16 PWM(11) GPIO(60) + +# Beeper +PB9 TIM17_CH1 TIM17 GPIO(32) ALARM + +# GPIOs +PC13 PINIO1 OUTPUT GPIO(81) LOW +define RELAY1_PIN_DEFAULT 81 + +DMA_PRIORITY S* +DMA_NOSHARE SPI1* SPI2* + + +# ------------------- IMU ICM42688p ------------------ +SPIDEV icm42688 SPI1 DEVID1 IMU_CS MODE3 2*MHZ 16*MHZ + +IMU Invensensev3 SPI:icm42688 ROTATION_NONE +define HAL_DEFAULT_INS_FAST_SAMPLE 1 + + +# ------------------ OSD AT7456E ---------------------- +SPIDEV osd SPI1 DEVID4 MAX7456_CS MODE0 10*MHZ 10*MHZ + +define OSD_ENABLED 1 +define HAL_OSD_TYPE_DEFAULT 1 +ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin + + +# ----------------- I2C compass & Baro ----------------- +# no built-in compass, but probe the i2c bus for all possible +# external compass types +define ALLOW_ARM_NO_COMPASS +define AP_COMPASS_PROBING_ENABLED 1 +define HAL_I2C_INTERNAL_MASK 0 +define HAL_COMPASS_AUTO_ROT_DEFAULT 2 + +# built-in barometer +BARO SPL06 I2C:0:0x76 +define AP_BARO_BACKEND_DEFAULT_ENABLED 0 +define AP_BARO_SPL06_ENABLED 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Morakot/Morakot_bottom.png b/libraries/AP_HAL_ChibiOS/hwdef/Morakot/Morakot_bottom.png new file mode 100644 index 0000000000000..4f762d46be87e Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/Morakot/Morakot_bottom.png differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Morakot/Morakot_bottom_Wired.png b/libraries/AP_HAL_ChibiOS/hwdef/Morakot/Morakot_bottom_Wired.png new file mode 100644 index 0000000000000..d17e3c1151086 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/Morakot/Morakot_bottom_Wired.png differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Morakot/Morakot_top.png b/libraries/AP_HAL_ChibiOS/hwdef/Morakot/Morakot_top.png new file mode 100644 index 0000000000000..b632d23030949 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/Morakot/Morakot_top.png differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Morakot/Morakot_top_Wired.png b/libraries/AP_HAL_ChibiOS/hwdef/Morakot/Morakot_top_Wired.png new file mode 100644 index 0000000000000..1b972bb206b6c Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/Morakot/Morakot_top_Wired.png differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Morakot/README.md b/libraries/AP_HAL_ChibiOS/hwdef/Morakot/README.md new file mode 100644 index 0000000000000..3d86022694911 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Morakot/README.md @@ -0,0 +1,197 @@ +# Morakot Flight Controller Overview + +The Morakot is a flight controller designed and produced by [Taiphoon](https://taiphoon.com.tw/) + +![top](Morakot_top.png) +![bottom](Morakot_bottom.png) + +Morakot Flight Controller — **NDAA-Compliant. Made in Taiwan.** Built for Performance. +Engineered, tested, and manufactured in Taiwan, the Morakot Flight Controller meets full NDAA compliance, ensuring trusted quality and security for professional applications. With an integrated Ethernet interface, it delivers high-speed, reliable connectivity for next-generation FPV and unmanned aerial systems. + +## Pinout +![top_wired](Morakot_top_Wired.png) +![bottom_wired](Morakot_bottom_Wired.png) + +## Features +#### Sensors +- [ICM-45686 High-Performance IMU](https://invensense.tdk.com/products/motion-tracking/6-axis/icm-45686/) +- [Bosch BMP390 Barometer](https://www.bosch-sensortec.com/products/environmental-sensors/pressure-sensors/bmp390/) +- [ST IIS2MDC Magnetometer](https://www.st.com/en/mems-and-sensors/iis2mdc.html) + +#### Microprocessor +- [STM32H743VIT MCU](https://www.st.com/en/microcontrollers-microprocessors/stm32h743vi.html) + - 480Mhz / 1MB RAM / 2MB Flash + +#### Power +- 3S–8S DC Input power +- 5V 2A BEC peripherals power +- 9V 2A BEC servos power +- 12V 2A BEC video power + +#### Other +- LED Indicators +- Battery voltage indicator LED +- MicroSD Slot +- 9x motor outputs +- 8x UART +- 1x I2C +- 1x CAN +- 1x Ethernet + +### UART Mapping +UART Mapping +The UARTs are marked Rn and Tn in the above pinouts. The Rn pin is the receive pin for UARTn. The Tn pin is the transmit pin for UARTn. +Port UART Protocol TX DMA RX DMA +--------- --------- ---------------- ------- ------- +0 USB MAVLink2 ✘ ✘ +1 USART1 MSP_DisplayPort ✔ ✔ +2 USART2 MAVLink2 ✔ ✔ +3 USART3 None ✔ ✔ +4 UART5 GPS ✔ ✔ +5 USART6 ESCTelemetry ✔ ✔ +6 UART7 MAVLink2 ✔ ✔ +7 UART8 RCIN ✔ ✔ + +RTS/CTS flow control is available on UART7. + +### VTX Support +The JST-GH 7p connector supports a DJI Air Unit / HD VTX connection. Protocol defaults to DisplayPort. Pin 1 of the connector is 12v so be careful not to connect this to a peripheral that can not tolerate this voltage. + +## Additional Information +- Dimensions: 38.5 x 30.5 mm +- Height: up to 8.5 mm +- Mounting: 30.5 x 30.5 mm (M3/M4 with dampers) +- Weight: 7 g + +## More Information +- [Morakot Flight Controller](https://taiphoon.com.tw/morakot-flight-controllor) +- [Morakot documentation](https://taiphoon-com.gitbook.io/taiphoon.com-docs/flight-stack/morakot-flight-controller) + +## Connetions +#### ESC1 - 8 Pin JST-GH +| Pin | Signal Name | Voltage | +|-----|-----------------|--------------| +| 1 | VBAT IN | 12V-33.6V | +| 2 | UART6_RX | 3.3V | +| 3 | GND | 3.3V | +| 4 | CURRENT | 3.3V | +| 5 | MOTOR/SERVO 1 | 3.3V | +| 6 | MOTOR/SERVO 2 | 3.3V | +| 7 | MOTOR/SERVO 3 | 3.3V | +| 8 | MOTOR/SERVO 4 | 3.3V | + +#### ESC2 - 8 Pin JST-GH +| Pin | Signal Name | Voltage | +|-----|-----------------|--------------| +| 1 | VBAT IN | 12V-33.6V | +| 2 | GND | 3.3V | +| 3 | CURRENT | 3.3V | +| 4 | MOTOR/SERVO 5 | 3.3V | +| 5 | MOTOR/SERVO 6 | 3.3V | +| 6 | MOTOR/SERVO 7 | 3.3V | +| 7 | MOTOR/SERVO 8 | 3.3V | +| 8 | MOTOR/SERVO 9 | 3.3V | + + +#### CAN - 4 Pin JST-GH +| Pin | Signal Name | Voltage | +|-----|-------------|---------| +| 1 | 5.0V | 5.0V | +| 2 | CAN1_H | 5.0V | +| 3 | CAN1_L | 5.0V | +| 4 | GND | GND | + +#### GPS - 6 Pin JST-GH +| Pin | Signal Name | Voltage | +|-----|---------------------|---------| +| 1 | 5.0V | 5.0V | +| 2 | UART5_TX | 3.3V | +| 3 | UART5_RX | 3.3V | +| 4 | I2C1_SCL | 3.3V | +| 5 | I2C1_SDA | 3.3V | +| 6 | GND | GND | + +#### UART(TELEM) - 6 Pin JST-GH +| Pin | Signal Name | Voltage | +|-----|-----------------------|---------| +| 1 | 5.0V | 5.0V | +| 2 | UART7_TX | 3.3V | +| 3 | UART7_RX | 3.3V | +| 4 | UART7_CTS | 3.3V | +| 5 | UART7_RTS | 3.3V | +| 6 | GND | GND | + +#### VTX - 7 Pin JST-GH +Note: connector pinout not in same order as standard HD VTX cabling +| Pin | Signal Name | Voltage | +|-----|-----------------------|---------| +| 1 | VIDEO | | +| 2 | 12.0V | 12.0V | +| 3 | GND | GND | +| 4 | USART1_RX | 3.3V | +| 5 | USART1_TX | 3.3V | +| 6 | GND | 3.3V | +| 7 | USART3_RX | GND | + +#### SPI (external OSD or IMU) - 6 Pin JST-SH +| Pin | Signal Name | Voltage | +|-----|---------------------|---------| +| 1 | 5.0V | 5.0V | +| 2 | SPI4_MOSI | 3.3V | +| 3 | SPI4_MISO | 3.3V | +| 4 | SPI4_SCK | 3.3V | +| 5 | SPI4_CS | 3.3V | +| 6 | GND | GND | + + +#### RC - 4 Pin JST-GH +| Pin | Signal Name | Voltage | +|-----|---------------------|---------| +| 1 | 5.0V | 5.0V | +| 2 | UART8_RX | 3.3V | +| 3 | UART8_TX | 3.3V | +| 4 | GND | GND | + +#### ETH - 4 Pin JST-GH +| Pin | Signal Name | Voltage | +|-----|---------------------|---------| +| 1 | RXN | 3.3V | +| 2 | RXP | 3.3V | +| 3 | TXN | 3.3V | +| 4 | TXP | 3.3V | + +## RC Input + +RC input is via SERIAL7(UART8) on the RC connector. Unidirectional protocols can be connected to R8. Bi-Directional Protocols will use the T8 pin also. +- PPM is not supported. +- SBUS/DSM/SRXL connects to the RX8 pin. +- FPort requires connection to TX8. Set :ref:`SERIAL7_OPTIONS` = 7 +- CRSF/ELRS also requires both TX8 and RX8 connections and provides telemetry automatically. +In order to use the SBUS pin on the HD VTX connector, you must change SERIAL7_PROTOCOL to something other than "23" and set :ref:`SERIAL3_PROTOCOL to "23". + +## PWM Outputs + +The Morakot supports up to 9 PWM outputs. All are DSHot and Bi-Directional DShot capable + +The PWM is in 3 groups: + + - PWM 1-4 in group1 + - PWM 4-8 in group2 + - PWM 9 in group3 + + Channels within the same group need to use the same output rate. If any channel in a group uses DShot then all channels in that group need to use DShot. + +## Compass +The Morakot has a built-in compass. Due to potential interference, the autopilot is usually used with an external I2C compass as part of a GPS/Compass combination and the internal compass disabled. + +## OSD +DisplayPort OSD is enabled by default on the HD VTX connector. Simutaneous DisplayPort operation on HD VTX connector is enabled by default also. + +## Firmware +Firmware for this board can be found here in sub-folders labeled “Morakot” + +## Loading Firmware + +Initial firmware load can be done with DFU by plugging in USB with the bootloader button pressed. Then you should load the “Morakotxx_bl.bin” firmware, using your favorite DFU loading tool. +Once the initial firmware is loaded you can update the firmware using any ArduPilot ground station software. Updates should be done with the “*.apj” firmware files. + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Morakot/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/Morakot/defaults.parm new file mode 100644 index 0000000000000..83b5fa54859e1 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Morakot/defaults.parm @@ -0,0 +1 @@ +OSD_TYPE2 5 \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Morakot/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/Morakot/hwdef-bl.dat new file mode 100644 index 0000000000000..3cbfdd4d6f2b6 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Morakot/hwdef-bl.dat @@ -0,0 +1,95 @@ +# hw definition file for processing by chibios_hwdef.py +# for the TAIPHOON flight controller + +# New board 20250818 + +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# crystal frequency +OSCILLATOR_HZ 8000000 + +# board ID. See Tools/AP_Bootloader/board_types.txt +APJ_BOARD_ID AP_HW_Morakot + +# bootloader is installed at zero offset +FLASH_RESERVE_START_KB 0 + +# flash size +FLASH_SIZE_KB 2048 + +# the location where the bootloader will put the firmware +FLASH_BOOTLOADER_LOAD_KB 128 + +# USB setup +USB_STRING_MANUFACTURER "Taiphoon" + +# baudrate to run bootloader at on uarts +define BOOTLOADER_BAUDRATE 115200 + +# pins for SWD debugging +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# order of UARTs (and USB) +SERIAL_ORDER USART1 USART2 USART3 UART5 USART6 UART7 + +# USB +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 +PA9 VBUS INPUT OPENDRAIN + +# USART1 +PB14 USART1_TX USART1 +PB15 USART1_RX USART1 + +# USART2 +PD5 USART2_TX USART2 +PD6 USART2_RX USART2 + +# USART3 +PD8 USART3_TX USART3 +PD9 USART3_RX USART3 + +# UART5 +PB6 UART5_TX UART5 +PB5 UART5_RX UART5 + +# USART6 +PC6 USART6_TX USART6 +PC7 USART6_RX USART6 + +# UART7 +PE8 UART7_TX UART7 +PE7 UART7_RX UART7 + +# UART8 +PE1 UART8_TX UART8 +PE0 UART8_RX UART8 + +# ADC +PC0 BATT_VOLTAGE ADC1 SCALE(11) +PC2 BATT_CURRENT ADC1 SCALE(20) + +# BUZZER and LED_STRIP +PD15 BUZZER OUTPUT + +# LED +PD12 LED_RED OUTPUT OPENDRAIN GPIO(89) LOW +PD13 LED_BLUE OUTPUT OPENDRAIN GPIO(90) LOW +PD10 LED_GREEN OUTPUT OPENDRAIN GPIO(91) LOW + +# WS2812B LEDs +define AP_NOTIFY_GPIO_LED_RGB_RED_PIN 89 +define AP_NOTIFY_GPIO_LED_RGB_BLUE_PIN 91 +define AP_NOTIFY_GPIO_LED_RGB_GREEN_PIN 90 +define AP_NOTIFY_GPIO_LED_RGB_ENABLED 1 + +# Battery monitor +define HAL_BATTMON_INA2XX_BUS 1 +define HAL_BATTMON_INA2XX_ADDR 0 +define HAL_BATT_MONITOR_DEFAULT 21 + +# DFU bootloader support +ENABLE_DFU_BOOT 1 + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Morakot/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Morakot/hwdef.dat new file mode 100644 index 0000000000000..5c99901b9ee2d --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Morakot/hwdef.dat @@ -0,0 +1,169 @@ +# hw definition file for processing by chibios_hwdef.py +# for the AeroCore hardware + +# New board 20250818 + +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# board ID for firmware load +APJ_BOARD_ID AP_HW_Morakot + +# crystal frequency +OSCILLATOR_HZ 8000000 + +# ChibiOS system timer +STM32_ST_USE_TIMER 4 +define CH_CFG_ST_RESOLUTION 16 + +# flash size +FLASH_SIZE_KB 2048 +FLASH_RESERVE_START_KB 128 + +# USB setup +USB_STRING_MANUFACTURER "Taiphoon" + +define HAL_STORAGE_SIZE 32768 +STORAGE_FLASH_PAGE 14 + +# default to all pins low to avoid ESD issues +DEFAULTGPIO OUTPUT LOW PULLDOWN + +# order of UARTs (and USB) +SERIAL_ORDER OTG1 USART1 USART2 USART3 UART5 USART6 UART7 UART8 + +# USB +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 +PA9 VBUS INPUT OPENDRAIN + +# USART1 (DJI/VTX) +PB14 USART1_TX USART1 +PB15 USART1_RX USART1 +define DEFAULT_SERIAL1_PROTOCOL SerialProtocol_MSP_DisplayPort + + +# USART2 +PD5 USART2_TX USART2 +PD6 USART2_RX USART2 + +# USART3 (UART/S.BUS) +PD8 USART3_TX USART3 +PD9 USART3_RX USART3 +define DEFAULT_SERIAL3_PROTOCOL SerialProtocol_None + +# UART5 (GPS) +PB6 UART5_TX UART5 +PB5 UART5_RX UART5 +define DEFAULT_SERIAL4_PROTOCOL SerialProtocol_GPS + +# USART6 (ESC1) +PC6 USART6_TX USART6 +PC7 USART6_RX USART6 +define DEFAULT_SERIAL5_PROTOCOL SerialProtocol_ESCTelemetry + +# UART7 (UART) +PE8 UART7_TX UART7 +PE7 UART7_RX UART7 +PE9 UART7_RTS UART7 +PE10 UART7_CTS UART7 +define DEFAULT_SERIAL6_PROTOCOL SerialProtocol_MAVLink2 + +# UART8 (RC) +PE1 UART8_TX UART8 +PE0 UART8_RX UART8 +define DEFAULT_SERIAL7_PROTOCOL SerialProtocol_RCIN + +# ADC for Power +PC0 BATT_VOLTAGE_SENS ADC1 SCALE(10) +PC2 BATT_CURRENT_SENS ADC1 SCALE(10) + +# SDMMC +PC12 SDMMC1_CK SDMMC1 +PD2 SDMMC1_CMD SDMMC1 +PC8 SDMMC1_D0 SDMMC1 +PC9 SDMMC1_D1 SDMMC1 +PC10 SDMMC1_D2 SDMMC1 +PC11 SDMMC1_D3 SDMMC1 +define FATFS_HAL_DEVICE SDCD1 +define HAL_OS_FATFS_IO 1 + +# PWM/MOTORS (based on schematic) +PE14 TIM1_CH4 TIM1 PWM(1) +PE13 TIM1_CH3 TIM1 PWM(2) BIDIR +PE11 TIM1_CH2 TIM1 PWM(3) +PA8 TIM1_CH1 TIM1 PWM(4) BIDIR +PA0 TIM2_CH1 TIM2 PWM(5) BIDIR +PB3 TIM2_CH2 TIM2 PWM(6) +PB10 TIM2_CH3 TIM2 PWM(7) BIDIR +PA3 TIM2_CH4 TIM2 PWM(8) +PB0 TIM3_CH3 TIM3 PWM(9) BIDIR + +# SPI1 - IMU1 +PA5 SPI1_SCK SPI1 +PA6 SPI1_MISO SPI1 +PD7 SPI1_MOSI SPI1 +PA15 ICM_45686_1_CS CS + +# SPI4 for OSD +PE2 SPI4_SCK SPI4 +PE5 SPI4_MISO SPI4 +PE6 SPI4_MOSI SPI4 +PE4 AT7456E_CS CS + +# I2C1 - external MAG/BARO +I2C_ORDER I2C1 +PB8 I2C1_SCL I2C1 PULLUP +PB9 I2C1_SDA I2C1 PULLUP + +define AP_COMPASS_PROBING_ENABLED 1 +define HAL_I2C_INTERNAL_MASK 1 + +# Barometer +BARO BMP388 I2C:0:0x76 + +# Builtin Compass +define AP_COMPASS_IIS2MDC_ENABLED 1 +COMPASS IIS2MDC I2C:0:0x1E false ROTATION_NONE +COMPASS QMC5883L I2C:0:0xd true ROTATION_NONE # GPS compass + +# LED +PD12 LED_RED OUTPUT OPENDRAIN GPIO(89) LOW +PD13 LED_BLUE OUTPUT OPENDRAIN GPIO(90) LOW +PD10 LED_GREEN OUTPUT OPENDRAIN GPIO(91) LOW +define AP_NOTIFY_GPIO_LED_RGB_RED_PIN 89 +define AP_NOTIFY_GPIO_LED_RGB_BLUE_PIN 91 +define AP_NOTIFY_GPIO_LED_RGB_GREEN_PIN 90 +define AP_NOTIFY_GPIO_LED_RGB_ENABLED 1 + +# Buzzer & Beeper +define HAL_BUZZER_PIN 80 +PD15 BUZZER OUTPUT GPIO(80) HIGH + +# CAN bus Define +PD0 CAN1_RX CAN1 +PD1 CAN1_TX CAN1 +PD4 C1_SILENT OUTPUT LOW + +# Ethernet RMII +PA1 ETH_RMII_REF_CLK ETH1 +PA2 ETH_MDIO ETH1 +PA7 ETH_RMII_CRS_DV ETH1 +PC1 ETH_MDC ETH1 +PC4 ETH_RMII_RXD0 ETH1 +PC5 ETH_RMII_RXD1 ETH1 +PB12 ETH_RMII_TXD0 ETH1 +PB13 ETH_RMII_TXD1 ETH1 +PB11 ETH_RMII_TX_EN ETH1 +define BOARD_PHY_ID MII_LAN8720_ID +define BOARD_PHY_RMII +define AP_PERIPH_NETWORKING_ENABLED 1 + +SPIDEV imu1 SPI1 DEVID1 ICM_45686_1_CS MODE3 2*MHZ 16*MHZ +SPIDEV osd SPI4 DEVID4 AT7456E_CS MODE0 10*MHZ 10*MHZ + +define HAL_OSD_TYPE_DEFAULT 1 + + +IMU Invensensev3 SPI:imu1 ROTATION_ROLL_180_YAW_90 + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/PixC4-Jetson/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/PixC4-Jetson/hwdef.dat index c633e733c0555..09d6101cf32c2 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/PixC4-Jetson/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/PixC4-Jetson/hwdef.dat @@ -9,8 +9,6 @@ MCU STM32H7xx STM32H743xx # crystal frequency OSCILLATOR_HZ 16000000 -define HAL_CHIBIOS_ARCH_FMUV5 1 - # board ID. See Tools/AP_Bootloader/board_types.txt APJ_BOARD_ID AP_HW_H31_PIXC4_JETSON @@ -249,8 +247,7 @@ IMU Invensense SPI:icm20602_2 ROTATION_YAW_90 define HAL_DEFAULT_INS_FAST_SAMPLE 7 -# probe external I2C compasses plus some internal IST8310 -# we also probe some external IST8310 with a non-standard orientation +# probe external I2C compasses plus an internal RM3100 define AP_COMPASS_PROBING_ENABLED 1 COMPASS RM3100 I2C:0:0x20 false ROTATION_NONE diff --git a/libraries/AP_HAL_ChibiOS/hwdef/StellarH7V2/README.md b/libraries/AP_HAL_ChibiOS/hwdef/StellarH7V2/README.md index 5ba29e0d51ee8..3ba548cb4a77e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/StellarH7V2/README.md +++ b/libraries/AP_HAL_ChibiOS/hwdef/StellarH7V2/README.md @@ -87,10 +87,13 @@ The default battery parameters are: * :ref:`BATT_MONITOR` = 4 * :ref:`BATT_VOLT_PIN` = 10 * :ref:`BATT_CURR_PIN` = 11 (CURR pin) -* :ref:`BATT_VOLT_MULT` = 11 -* :ref:`BATT_AMP_PERVLT` = 10 -* :ref:`BATT2_VOLT_PIN` = 18 (ADC1 pin, PA4) +* :ref:`BATT_VOLT_MULT` = 11.0 +* :ref:`BATT_AMP_PERVLT` = 66.7 +Pads for a second analog battery monitor are provided (Voltage only). To use: +* :ref:`BATT2_MONITOR` = 4 +* :ref:`BATT2_VOLT_PIN` = 18 (ADC1 pin, PA4) +* :ref:`BATT2_VOLT_MULT` = 21.0 ## Compass diff --git a/libraries/AP_HAL_ChibiOS/hwdef/StellarH7V2/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/StellarH7V2/hwdef.dat index 355a7a3ff4ebd..9680f3d4114f1 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/StellarH7V2/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/StellarH7V2/hwdef.dat @@ -57,6 +57,7 @@ PC1 BATT_CURRENT_SENS ADC1 SCALE(1) PA4 BATT2_VOLTAGE_SENS ADC1 SCALE(1) define HAL_BATT_MONITOR_DEFAULT 4 +define HAL_BATT2_MONITOR_DEFAULT 4 define HAL_BATT_VOLT_PIN 10 define HAL_BATT_CURR_PIN 11 define HAL_BATT2_VOLT_PIN 18 @@ -64,7 +65,7 @@ define HAL_BATT_VOLT_SCALE 11.0 # Onboard voltage divider 10k and 1k define HAL_BATT2_VOLT_SCALE 21.0 # PDB voltage divider 20k and 1k # External PDB INA169 current sensor: Rs = 0.15 mOhm, Rl = 100 kOhm (defines gain), 1 V = 66.7 A, 15 mV per 1 A -#define HAL_BATT_CURR_SCALE 66.7 +define HAL_BATT_CURR_SCALE 66.7 # LED # green LED1 marked as B/E diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Swan-K1/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/Swan-K1/defaults.parm index d2b144a4259e7..b2b6c7ae7e8c2 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Swan-K1/defaults.parm +++ b/libraries/AP_HAL_ChibiOS/hwdef/Swan-K1/defaults.parm @@ -97,9 +97,9 @@ Q_A_RAT_YAW_SMAX 15 Q_A_ACCEL_P_MAX 40000 Q_A_ACCEL_R_MAX 20000 Q_A_ACCEL_Y_MAX 27000 -Q_P_ACCZ_P 0.8 -Q_P_POSZ_P 1.5 -Q_P_VELZ_P 8.0 +Q_P_D_ACC_P 0.08 +Q_P_D_POS_P 1.5 +Q_P_D_VEL_P 8.0 Q_M_THST_HOVER 0.45 Q_M_BAT_VOLT_MAX 16.8 Q_M_BAT_VOLT_MIN 13.2 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/TBS-L431-BattMon/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/TBS-L431-BattMon/hwdef.dat index 4568e038bbb97..df36c07897eac 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/TBS-L431-BattMon/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/TBS-L431-BattMon/hwdef.dat @@ -46,9 +46,7 @@ define HAL_RCIN_THREAD_ENABLED 0 define AP_DAC_MCP40D1X_ENABLED 1 define AP_DAC_DEFAULT_ADDR 0x2E -define AP_DAC_DEFAULT_VREF 20 define AP_DAC_DEFAULT_BUS 0 -define AP_DAC_DEFAULT_VOLT 10 define AP_DAC_DEFAULT_TYPE AP_DAC_TYPE_MCP40D1x define AP_DAC_MCP40D1X_CONVERSION_EQ(vo,vr) ((127.0/10000.0) * (0.8 * 21000 * 866 + 0.8 * 21000 * 169000 - ((vo) - 1.0) * 866 * 169000)/(((vo) - 1.0) * 169000 - 0.8 * 21000)) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/VM-L431-BMS/README.md b/libraries/AP_HAL_ChibiOS/hwdef/VM-L431-BMS/README.md new file mode 100644 index 0000000000000..002c7abcd8e98 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/VM-L431-BMS/README.md @@ -0,0 +1,3 @@ +### VM-L431-BMS + +AP_Periph BMS Target based on Vimdrones L431 AP_Periph Hardware diff --git a/libraries/AP_HAL_ChibiOS/hwdef/VM-L431-BMS/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/VM-L431-BMS/defaults.parm new file mode 100644 index 0000000000000..7c828445f01ae --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/VM-L431-BMS/defaults.parm @@ -0,0 +1,7 @@ +# Vimdrones L431 BMS (battery management system) parameter defaults + +CAN_NODE 0 +CAN_BAUDRATE 1000000 + +# Battery Monitor Settings +BATT_MONITOR 32 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/VM-L431-BMS/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/VM-L431-BMS/hwdef-bl.dat new file mode 100644 index 0000000000000..ee306425b44c3 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/VM-L431-BMS/hwdef-bl.dat @@ -0,0 +1,7 @@ +include ../VM-L431/hwdef-bl.inc + +undef CAN_APP_NODE_NAME +define CAN_APP_NODE_NAME "com.vimdrones.bms_bl" + +# board ID for firmware load +APJ_BOARD_ID AP_HW_VIMDRONES_BMS diff --git a/libraries/AP_HAL_ChibiOS/hwdef/VM-L431-BMS/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/VM-L431-BMS/hwdef.dat new file mode 100644 index 0000000000000..680378382d320 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/VM-L431-BMS/hwdef.dat @@ -0,0 +1,53 @@ +include ../VM-L431/hwdef.inc + +define CAN_APP_NODE_NAME "com.vimdrones.bms" + +# board ID for firmware load +APJ_BOARD_ID AP_HW_VIMDRONES_BMS + +# battery specific definitions +# define AP_BATTMON_CELL_COUNT 6 + +# Serial ports +SERIAL_ORDER EMPTY EMPTY USART2 + +PA2 USART2_TX USART2 +PA3 USART2_RX USART2 + +STDOUT_BAUDRATE 57600 +STDOUT_SERIAL SD2 + +# enable battery monitor +define AP_PERIPH_BATTERY_ENABLED 1 +define AP_BATTERY_TIBQ76952_ENABLED 1 +define AP_BATTERY_TIBQ76952_I2C_DEVICE "tibq76952" +define HAL_BATT_MONITOR_DEFAULT 32 +define AP_PERIPH_BATTERY_MODEL_NAME "VM-L431-BMS" + +# I2C1 for TI BMS chip +PB6 I2C1_SCL I2C1 +PB7 I2C1_SDA I2C1 +I2C_ORDER I2C1 + +# LEDs +PA4 LED1 OUTPUT OPENDRAIN GPIO(1) HIGH +PA5 LED2 OUTPUT OPENDRAIN GPIO(2) HIGH +PA6 LED3 OUTPUT OPENDRAIN GPIO(3) HIGH +PA7 LED4 OUTPUT OPENDRAIN GPIO(4) HIGH +PB0 LED5 OUTPUT OPENDRAIN GPIO(5) HIGH +PB1 LED6 OUTPUT OPENDRAIN GPIO(6) HIGH +PA8 LED7 OUTPUT OPENDRAIN GPIO(7) HIGH +PA9 LED8 OUTPUT OPENDRAIN GPIO(8) HIGH +define AP_PERIPH_BMS_LED_PINS 1,2,3,4,5,6,7,8 + +# Buttons +PB4 BTN1 INPUT PULLUP GPIO(9) +define HAL_GPIO_PIN_BMS_BTN1 9 + +define HAL_USE_ADC FALSE +define STM32_ADC_USE_ADC1 FALSE +define HAL_DISABLE_ADC_DRIVER TRUE + +define AP_PERIPH_BATTERY_BMS_ENABLED 1 +define AP_PERIPH_RTC_ENABLED 1 +define AP_PERIPH_RTC_GLOBALTIME_ENABLED 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv5/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/fmuv5/hwdef.dat index 6b96bd8d31436..8ded83b5e43e7 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/fmuv5/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv5/hwdef.dat @@ -8,8 +8,6 @@ MCU STM32F7xx STM32F767xx # crystal frequency OSCILLATOR_HZ 16000000 -define HAL_CHIBIOS_ARCH_FMUV5 1 - # board ID. See Tools/AP_Bootloader/board_types.txt APJ_BOARD_ID TARGET_HW_PX4_FMU_V5 @@ -252,6 +250,8 @@ define HAL_DEFAULT_INS_FAST_SAMPLE 1 # probe external I2C compasses plus some internal IST8310 # we also probe some external IST8310 with a non-standard orientation define AP_COMPASS_PROBING_ENABLED 1 +define AP_COMPASS_IST8310_EXTERNAL_BUS_PROBING_ENABLED 0 +define AP_COMPASS_IST8310_INTERNAL_BUS_PROBING_ENABLED 0 COMPASS IST8310 I2C:ALL_EXTERNAL:0x0E true ROTATION_ROLL_180_YAW_90 COMPASS IST8310 I2C:ALL_INTERNAL:0x0E false ROTATION_ROLL_180_YAW_90 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc b/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc index d6fe6a1668619..b88283517b672 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc +++ b/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc @@ -54,7 +54,7 @@ define AP_CAMERA_RUNCAM_ENABLED AP_CAMERA_ENABLED define AP_CAN_SLCAN_ENABLED 0 # no PiccoloCAN: -define HAL_PICCOLO_CAN_ENABLE 0 +define AP_PICCOLOCAN_ENABLED 0 # no beacon support on minimized boards: define AP_BEACON_ENABLED 0 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/luminousbee5/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/luminousbee5/defaults.parm index 3375a49fd9073..50d05bcb2ac98 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/luminousbee5/defaults.parm +++ b/libraries/AP_HAL_ChibiOS/hwdef/luminousbee5/defaults.parm @@ -112,9 +112,6 @@ INS_HNTCH_OPTS,22 INS_HNTCH_REF,1 INS_LOG_BAT_CNT,32 INS_LOG_BAT_OPT,2 -LAND_ALT_LOW,500 -LAND_SPEED,80 -LAND_SPEED_HIGH,300 LOG_BITMASK,141310 LOG_FILE_DSRMROT,1 LOIT_SPEED,2000 @@ -131,11 +128,11 @@ MOT_THST_HOVER,0.47 NTF_BUZZ_TYPES,1 NTF_LED_BRIGHT,1 NTF_LED_TYPES,1 -PSC_ACCZ_I,0.94 -PSC_ACCZ_P,0.47 -PSC_JERK_XY,6 -PSC_JERK_Z,6 -PSC_POSZ_P,1 +PSC_D_ACC_I,0.094 +PSC_D_ACC_P,0.047 +PSC_JERK_NE,6 +PSC_JERK_D,6 +PSC_D_POS_P,1 RC1_MAX,1900 RC1_MIN,1100 RC2_MAX,1900 @@ -146,7 +143,6 @@ RC4_MAX,1900 RC4_MIN,1100 RC6_OPTION,5 RC_PROTOCOLS,0 -RTL_ALT,1500 SCHED_LOOP_RATE,800 SERIAL1_BAUD,921 SERIAL1_PROTOCOL,2 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/mindpx-v2/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/mindpx-v2/hwdef-bl.dat index 588460fee8827..f990cf6d58c15 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/mindpx-v2/hwdef-bl.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/mindpx-v2/hwdef-bl.dat @@ -4,8 +4,6 @@ # MCU class and specific type MCU STM32F4xx STM32F427xx -define HAL_CHIBIOS_ARCH_MINDPXV2 1 - # board ID. See Tools/AP_Bootloader/board_types.txt APJ_BOARD_ID TARGET_HW_MINDPX_V2 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/mindpx-v2/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/mindpx-v2/hwdef.dat index 448d3867bc88b..fbe5a24428ddc 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/mindpx-v2/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/mindpx-v2/hwdef.dat @@ -180,8 +180,7 @@ BARO MS5611 SPI:ms5611 COMPASS HMC5843 I2C:0:0x1e false ROTATION_YAW_90 COMPASS LSM303D SPI:lsm9ds0_am ROTATION_PITCH_180_YAW_270 define AP_COMPASS_PROBING_ENABLED 1 - -define HAL_CHIBIOS_ARCH_MINDPXV2 1 +define AP_COMPASS_HMC5843_INTERNAL_BUS_PROBING_ENABLED 0 define HAL_STORAGE_SIZE 16384 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h index e1bacd9ddf45c..b927f99837822 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h @@ -298,6 +298,9 @@ #ifndef AP_PERIPH_BATTERY_TAG_ENABLED #define AP_PERIPH_BATTERY_TAG_ENABLED 0 #endif +#ifndef AP_PERIPH_BATTERY_BMS_ENABLED +#define AP_PERIPH_BATTERY_BMS_ENABLED 0 +#endif #ifndef AP_PERIPH_PROXIMITY_ENABLED #define AP_PERIPH_PROXIMITY_ENABLED 0 #endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/defaults.parm index ab4d0ef36d431..ddb849a330a7b 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/defaults.parm +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/defaults.parm @@ -168,7 +168,7 @@ FS_GCS_ENABLE 0 TCAL_ENABLED 2 # landing -LAND_SPEED_HIGH 150 +LAND_SPD_HIGH_MS 1.5 # logging LOG_BACKEND_TYPE 2 @@ -198,8 +198,7 @@ RC3_MAX 2000 RC4_MIN 1000 RC4_MAX 2000 RC3_TRIM 1500 -RTL_ALT 400 -GND_EFFECT_COMP 1 +RTL_ALT_M 4 # deadzones, with auto-trim RC1_DZ 15 @@ -208,13 +207,13 @@ RC3_DZ 15 RC4_DZ 15 # pos control -PSC_POSXY_P 0.7 -PSC_VELXY_I 0.3 -PSC_VELXY_P 0.6 -PSC_VELXY_FLTE 1.0 +PSC_NE_POS_P 0.7 +PSC_NE_VEL_I 0.3 +PSC_NE_VEL_P 0.6 +PSC_NE_VEL_FLTE 1.0 LOIT_ACC_MAX 800 LOIT_SPEED 800 -RTL_SPEED 600 +RTL_SPEED_MS 6 # GPS GPS_NAVFILTER 6 @@ -225,7 +224,7 @@ GPS_HDOP_GOOD 160 # throw mode THROW_NEXTMODE 5 -PSC_ACCZ_FLTE 5.0000 -PSC_ACCZ_P 1.0000 -PSC_POSZ_P 0.5000 -PSC_VELZ_P 1.0000 +PSC_D_ACC_FLTE 5.0000 +PSC_D_ACC_P 0.1000 +PSC_D_POS_P 0.5000 +PSC_D_VEL_P 1.0000 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/defaults.parm index 29889461b1af2..591bd9c4bdfc0 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/defaults.parm +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/defaults.parm @@ -164,9 +164,6 @@ FS_GCS_ENABLE 0 # baro TCAL_ENABLED 2 -# landing -LAND_SPEED_HIGH 350 - # logging LOG_BACKEND_TYPE 2 LOG_FILE_BUFSIZE 1 @@ -196,8 +193,7 @@ RC3_MAX 2000 RC4_MIN 1000 RC4_MAX 2000 RC3_TRIM 1500 -RTL_ALT 400 -GND_EFFECT_COMP 1 +RTL_ALT_M 4 # deadzones, with auto-trim RC1_DZ 15 @@ -206,14 +202,14 @@ RC3_DZ 15 RC4_DZ 15 # pos control -PSC_POSXY_P 0.7 -PSC_VELXY_I 0.3 -PSC_VELXY_P 0.6 -PSC_VELXY_FLTE 1.0 +PSC_NE_POS_P 0.7 +PSC_NE_VEL_I 0.3 +PSC_NE_VEL_P 0.6 +PSC_NE_VEL_FLTE 1.0 LOIT_ACC_MAX 800 LOIT_SPEED 800 WPNAV_SPEED 750 -RTL_SPEED 600 +RTL_SPEED_MS 6 # GPS GPS_NAVFILTER 6 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat index a79468bce2d7a..7f8acd48381fd 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat @@ -93,7 +93,7 @@ define HAL_MOUNT_ENABLED 0 define HAL_MSP_SENSORS_ENABLED 0 define HAL_NMEA_OUTPUT_ENABLED 0 define HAL_PARACHUTE_ENABLED 0 -define HAL_PICCOLO_CAN_ENABLE 0 +define AP_PICCOLOCAN_ENABLED 0 define HAL_RUNCAM_ENABLED 0 define AP_SMARTAUDIO_ENABLED 0 define HAL_SPEKTRUM_TELEM_ENABLED 0 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/uav-dev-auav-g4/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/uav-dev-auav-g4/hwdef-bl.dat index 20a2011e7619a..8670a3d191791 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/uav-dev-auav-g4/hwdef-bl.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/uav-dev-auav-g4/hwdef-bl.dat @@ -52,5 +52,11 @@ PB12 CAN2_RX CAN2 PB13 CAN2_TX CAN2 PB15 GPIO_CAN2_SILENT OUTPUT PUSHPULL SPEED_LOW LOW +# AUAV POWER +PA10 AUAV_POWER OUTPUT HIGH + +# AUAV RESET +PA4 AUAV_RESET OUTPUT OPENDRAIN LOW + # use a smaller bootloader timeout define HAL_BOOTLOADER_TIMEOUT 1000 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/uav-dev-auav-g4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/uav-dev-auav-g4/hwdef.dat index 917815a152d8d..8df9f4ec0e3d0 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/uav-dev-auav-g4/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/uav-dev-auav-g4/hwdef.dat @@ -58,9 +58,15 @@ PA15 I2C1_SCL I2C1 PA13 JTMS-SWDIO SWD PA14 JTCK-SWCLK SWD +# AUAV POWER +PA10 AUAV_POWER OUTPUT HIGH + +# AUAV RESET +PA4 AUAV_RESET OUTPUT OPENDRAIN HIGH + define AP_PERIPH_AIRSPEED_ENABLED 1 define AIRSPEED_MAX_SENSORS 1 -define HAL_AIRSPEED_TYPE_DEFAULT 18 +define HAL_AIRSPEED_TYPE_DEFAULT 17 define AP_PERIPH_BARO_ENABLED 1 define AP_BARO_BACKEND_DEFAULT_ENABLED 0 diff --git a/libraries/AP_HAL_Linux/GPIO_Navigator.cpp b/libraries/AP_HAL_Linux/GPIO_Navigator.cpp index eea9124dba03d..e0a5d1564af99 100644 --- a/libraries/AP_HAL_Linux/GPIO_Navigator.cpp +++ b/libraries/AP_HAL_Linux/GPIO_Navigator.cpp @@ -48,4 +48,12 @@ bool GPIO_Navigator::pinAllowed(uint8_t pin) return false; } +bool GPIO_Navigator::pin_to_servo_channel(uint8_t pin, uint8_t& servo_ch) const +{ + if (1 <= pin && pin <= 16) { + servo_ch = pin - 1; + return true; + } + return false; +} #endif // HAL_LINUX_GPIO_NAVIGATOR_ENABLED diff --git a/libraries/AP_HAL_Linux/GPIO_Navigator.h b/libraries/AP_HAL_Linux/GPIO_Navigator.h index 5361616808baf..ffac781e46b93 100644 --- a/libraries/AP_HAL_Linux/GPIO_Navigator.h +++ b/libraries/AP_HAL_Linux/GPIO_Navigator.h @@ -18,6 +18,7 @@ class GPIO_Navigator : public GPIO_RPI void pinMode(uint8_t pin, uint8_t output, uint8_t alt) override; uint8_t read(uint8_t pin) override; void write(uint8_t pin, uint8_t value) override; + bool pin_to_servo_channel(uint8_t pin, uint8_t& servo_ch) const override; private: uint8_t AllowedGPIOS[3] = { RPI_GPIO_<18>(), // Aux Output for PWMs diff --git a/libraries/AP_HAL_SITL/SITL_State_common.cpp b/libraries/AP_HAL_SITL/SITL_State_common.cpp index ce23e9a9bcf60..80bc365383fb1 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.cpp +++ b/libraries/AP_HAL_SITL/SITL_State_common.cpp @@ -164,6 +164,14 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const rplidara1 = NEW_NOTHROW SITL::PS_RPLidarA1(); return rplidara1; #endif +#if AP_SIM_PS_RPLIDARS2_ENABLED + } else if (streq(name, "rplidars2")) { + if (rplidars2 != nullptr) { + AP_HAL::panic("Only one rplidars2 at a time"); + } + rplidars2 = NEW_NOTHROW SITL::PS_RPLidarS2(); + return rplidars2; +#endif #if AP_SIM_PS_TERARANGERTOWER_ENABLED } else if (streq(name, "terarangertower")) { if (terarangertower != nullptr) { @@ -360,6 +368,13 @@ void SITL_State_Common::sim_update(void) rplidara1->update(sitl_model->get_location()); } #endif + +#if AP_SIM_PS_RPLIDARS2_ENABLED + if (rplidars2 != nullptr) { + rplidars2->update(sitl_model->get_location()); + } +#endif + #if AP_SIM_PS_TERARANGERTOWER_ENABLED if (terarangertower != nullptr) { terarangertower->update(sitl_model->get_location()); diff --git a/libraries/AP_HAL_SITL/SITL_State_common.h b/libraries/AP_HAL_SITL/SITL_State_common.h index e3f781a7c78ad..82f9488f95e62 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.h +++ b/libraries/AP_HAL_SITL/SITL_State_common.h @@ -29,6 +29,7 @@ #include #include #include +#include #include #include @@ -126,11 +127,18 @@ class HALSITL::SITL_State_Common { SITL::PS_RPLidarA2 *rplidara2; #endif - // simulated FETtec OneWire ESCs: - SITL::FETtecOneWireESC *fetteconewireesc; - +#if AP_SIM_PS_RPLIDARA1_ENABLED // simulated RPLidarA1: SITL::PS_RPLidarA1 *rplidara1; +#endif + +#if AP_SIM_PS_RPLIDARS2_ENABLED + // simulated RPLidarS2: + SITL::PS_RPLidarS2 *rplidars2; +#endif + + // simulated FETtec OneWire ESCs: + SITL::FETtecOneWireESC *fetteconewireesc; #if AP_SIM_PS_LIGHTWARE_SF45B_ENABLED // simulated SF45B proximity sensor: diff --git a/libraries/AP_HAL_SITL/hwdef/SITL_arm_linux_gnueabihf/README.md b/libraries/AP_HAL_SITL/hwdef/SITL_arm_linux_gnueabihf/README.md new file mode 100644 index 0000000000000..e7cdf2d48f93e --- /dev/null +++ b/libraries/AP_HAL_SITL/hwdef/SITL_arm_linux_gnueabihf/README.md @@ -0,0 +1,5 @@ +### SITL_arm_linux_gnueabihf + +Software-In-The_Loop target to run on ARM Linux, e.g. Raspberry Pi + +Critically the output binaries are statically linked so are more-easily moved to machines of a different configuration to that which they are compiled on. diff --git a/libraries/AP_HAL_SITL/hwdef/SITL_arm_linux_gnueabihf/hwdef.dat b/libraries/AP_HAL_SITL/hwdef/SITL_arm_linux_gnueabihf/hwdef.dat new file mode 100644 index 0000000000000..af716108135c6 --- /dev/null +++ b/libraries/AP_HAL_SITL/hwdef/SITL_arm_linux_gnueabihf/hwdef.dat @@ -0,0 +1,4 @@ +include ../sitl/hwdef.dat + +env TOOLCHAIN arm-linux-gnueabihf +env STATIC_LINKING 1 diff --git a/libraries/AP_HAL_SITL/hwdef/SITL_x86_64_linux_gnu/README.md b/libraries/AP_HAL_SITL/hwdef/SITL_x86_64_linux_gnu/README.md new file mode 100644 index 0000000000000..f772d8f603bbc --- /dev/null +++ b/libraries/AP_HAL_SITL/hwdef/SITL_x86_64_linux_gnu/README.md @@ -0,0 +1,5 @@ +### SITL_x86_64_linux_gnu + +Software-In-The_Loop target to run on x86-64 Linux - i.e. most desktop computers + +Critically the output binaries are statically linked so are more-easily moved to machines of a different configuration to that which they are compiled on. diff --git a/libraries/AP_HAL_SITL/hwdef/SITL_x86_64_linux_gnu/hwdef.dat b/libraries/AP_HAL_SITL/hwdef/SITL_x86_64_linux_gnu/hwdef.dat new file mode 100644 index 0000000000000..b49a594210612 --- /dev/null +++ b/libraries/AP_HAL_SITL/hwdef/SITL_x86_64_linux_gnu/hwdef.dat @@ -0,0 +1,4 @@ +include ../sitl/hwdef.dat + +env TOOLCHAIN x86_64-linux-gnu +env STATIC_LINKING 1 diff --git a/libraries/AP_HAL_SITL/hwdef/scripts/sitl_hwdef.py b/libraries/AP_HAL_SITL/hwdef/scripts/sitl_hwdef.py new file mode 100755 index 0000000000000..6de7d4938028d --- /dev/null +++ b/libraries/AP_HAL_SITL/hwdef/scripts/sitl_hwdef.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python3 +''' +setup board.h for SITL + +AP_FLAKE8_CLEAN + +''' + +import argparse +import sys +import os + +sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), '../../../../libraries/AP_HAL/hwdef/scripts')) +import hwdef # noqa:E402 + + +class SITLHWDef(hwdef.HWDef): + + def __init__(self, **kwargs): + super(SITLHWDef, self).__init__(**kwargs) + + def write_hwdef_header_content(self, f): + for d in self.alllines: + if d.startswith('define '): + f.write('#define %s\n' % d[7:]) + + def process_line(self, line, depth): + '''process one line of pin definition file''' + # keep all config lines for later use + self.all_lines.append(line) + self.alllines.append(line) + + super(SITLHWDef, self).process_line(line, depth) + + +if __name__ == '__main__': + + parser = argparse.ArgumentParser("sitl_hwdef.py") + parser.add_argument( + '-D', '--outdir', type=str, default="/tmp", help='Output directory') + parser.add_argument( + 'hwdef', type=str, nargs='+', default=None, help='hardware definition file') + parser.add_argument( + '--quiet', action='store_true', default=False, help='quiet running') + + args = parser.parse_args() + + c = SITLHWDef( + outdir=args.outdir, + hwdef=args.hwdef, + quiet=args.quiet, + ) + c.run() diff --git a/libraries/AP_HAL_SITL/hwdef/sitl/README.md b/libraries/AP_HAL_SITL/hwdef/sitl/README.md new file mode 100644 index 0000000000000..6ccc699dc9ae9 --- /dev/null +++ b/libraries/AP_HAL_SITL/hwdef/sitl/README.md @@ -0,0 +1,5 @@ +### sitl + +This is the base hwdef.dat for ArduPilot simulations. + +You can create new boards based off this which only include features that you want in there. diff --git a/libraries/AP_HAL_SITL/hwdef/sitl/hwdef.dat b/libraries/AP_HAL_SITL/hwdef/sitl/hwdef.dat new file mode 100644 index 0000000000000..3516bc9dcad0b --- /dev/null +++ b/libraries/AP_HAL_SITL/hwdef/sitl/hwdef.dat @@ -0,0 +1,2 @@ +define AP_MOUNT_POI_LOCK_ENABLED 1 + diff --git a/libraries/AP_HAL_SITL/hwdef/sitl_periph/README.md b/libraries/AP_HAL_SITL/hwdef/sitl_periph/README.md new file mode 100644 index 0000000000000..405d856e15759 --- /dev/null +++ b/libraries/AP_HAL_SITL/hwdef/sitl_periph/README.md @@ -0,0 +1,3 @@ +### sitl_periph + +This is an include file which sets defaults for a lot of features. To be (mostly?) replaced by re-use of defaults_periph.h diff --git a/libraries/AP_HAL_SITL/hwdef/sitl_periph/hwdef.inc b/libraries/AP_HAL_SITL/hwdef/sitl_periph/hwdef.inc new file mode 100644 index 0000000000000..3ec9c67015f6c --- /dev/null +++ b/libraries/AP_HAL_SITL/hwdef/sitl_periph/hwdef.inc @@ -0,0 +1,76 @@ +env AP_PERIPH 1 + +define HAL_BUILD_AP_PERIPH 1 +define PERIPH_FW 1 +define HAL_RAM_RESERVE_START 0 + +define CANARD_ENABLE_CANFD 1 +define CANARD_ENABLE_TAO_OPTION 1 +define CANARD_MULTI_IFACE 1 + +# FIXME: SITL library should not be using AP_AHRS: +define AP_AHRS_ENABLED 1 +define AP_AHRS_BACKEND_DEFAULT_ENABLED 0 +define AP_AHRS_DCM_ENABLED 1 # need a default backend +define AP_EXTERNAL_AHRS_ENABLED 0 + +define HAL_MAVLINK_BINDINGS_ENABLED 1 + +define AP_AIRSPEED_AUTOCAL_ENABLE 0 +define AP_CAN_SLCAN_ENABLED 0 +define AP_ICENGINE_ENABLED 0 +define AP_MISSION_ENABLED 0 +define AP_RCPROTOCOL_ENABLED 0 +define AP_RTC_ENABLED 0 +define AP_SCHEDULER_ENABLED 0 +define AP_SCRIPTING_ENABLED 0 +define AP_STATS_ENABLED 0 +define AP_UART_MONITOR_ENABLED 1 +define COMPASS_CAL_ENABLED 0 +define COMPASS_LEARN_ENABLED 0 +define COMPASS_MOT_ENABLED 0 +define HAL_CAN_DEFAULT_NODE_ID 0 +define HAL_CANMANAGER_ENABLED 0 +define HAL_GCS_ENABLED 0 +define HAL_GENERATOR_ENABLED 0 +define HAL_LOGGING_ENABLED 0 +define HAL_LOGGING_MAVLINK_ENABLED 0 +define HAL_PROXIMITY_ENABLED 0 +define HAL_RALLY_ENABLED 0 +define HAL_SUPPORT_RCOUT_SERIAL 0 +define AP_TERRAIN_AVAILABLE 0 +define AP_CUSTOMROTATIONS_ENABLED 0 +define AP_PERIPH_BATTERY_ENABLED 0 +define AP_PERIPH_DEVICE_TEMPERATURE_ENABLED 0 +define AP_PERIPH_SERIAL_OPTIONS_ENABLED 0 +define AP_PERIPH_ADSB_ENABLED 0 +define AP_PERIPH_PROXIMITY_ENABLED 0 +define AP_PERIPH_GPS_ENABLED 0 +define AP_PERIPH_RELAY_ENABLED 0 +define AP_PERIPH_IMU_ENABLED 0 +define AP_PERIPH_MAG_ENABLED 0 +define AP_PERIPH_BATTERY_BALANCE_ENABLED 0 +define AP_PERIPH_BATTERY_TAG_ENABLED 0 +define AP_PERIPH_BATTERY_BMS_ENABLED 0 +define AP_PERIPH_MSP_ENABLED 0 +define AP_PERIPH_BARO_ENABLED 0 +define AP_PERIPH_EFI_ENABLED 0 +define AP_PERIPH_RANGEFINDER_ENABLED 0 +define AP_PERIPH_RC_OUT_ENABLED 0 +define AP_PERIPH_RTC_ENABLED 0 +define AP_PERIPH_RCIN_ENABLED 0 +define AP_PERIPH_RPM_ENABLED 0 +define AP_PERIPH_RPM_STREAM_ENABLED 0 +define AP_PERIPH_AIRSPEED_ENABLED 0 +define AP_PERIPH_HOBBYWING_ESC_ENABLED 0 +define AP_PERIPH_NETWORKING_ENABLED 0 +define AP_PERIPH_NOTIFY_ENABLED 0 +define AP_PERIPH_PWM_HARDPOINT_ENABLED 0 +define AP_PERIPH_ESC_APD_ENABLED 0 +define AP_PERIPH_NCP5623_LED_WITHOUT_NOTIFY_ENABLED 0 +define AP_PERIPH_NCP5623_BGR_LED_WITHOUT_NOTIFY_ENABLED 0 +define AP_PERIPH_TOSHIBA_LED_WITHOUT_NOTIFY_ENABLED 0 +define AP_PERIPH_BUZZER_ENABLED 0 +define AP_PERIPH_BUZZER_WITHOUT_NOTIFY_ENABLED 0 +define AP_PERIPH_RTC_GLOBALTIME_ENABLED 0 +define AP_PERIPH_ACTUATOR_TELEM_ENABLED 0 diff --git a/libraries/AP_HAL_SITL/hwdef/sitl_periph_battery_tag/README.md b/libraries/AP_HAL_SITL/hwdef/sitl_periph_battery_tag/README.md new file mode 100644 index 0000000000000..f2b18fb6b4fe1 --- /dev/null +++ b/libraries/AP_HAL_SITL/hwdef/sitl_periph_battery_tag/README.md @@ -0,0 +1,3 @@ +### sitl_periph_battery_tag + +A simulated peripheral which includes CAN battery tag functionality diff --git a/libraries/AP_HAL_SITL/hwdef/sitl_periph_battery_tag/hwdef.dat b/libraries/AP_HAL_SITL/hwdef/sitl_periph_battery_tag/hwdef.dat new file mode 100644 index 0000000000000..ec530b55f6813 --- /dev/null +++ b/libraries/AP_HAL_SITL/hwdef/sitl_periph_battery_tag/hwdef.dat @@ -0,0 +1,16 @@ +include ../sitl_periph/hwdef.inc + +define CAN_APP_NODE_NAME "org.ardupilot.battery_tag" +define APJ_BOARD_ID 101 + +define AP_SIM_PARAM_ENABLED 0 +define AP_KDECAN_ENABLED 0 +define AP_TEMPERATURE_SENSOR_ENABLED 0 +undef AP_PERIPH_BATTERY_TAG_ENABLED +define AP_PERIPH_BATTERY_TAG_ENABLED 1 +undef AP_RTC_ENABLED +define AP_RTC_ENABLED 1 +undef AP_PERIPH_RTC_ENABLED +define AP_PERIPH_RTC_ENABLED 1 +undef AP_PERIPH_RTC_GLOBALTIME_ENABLED +define AP_PERIPH_RTC_GLOBALTIME_ENABLED 1 diff --git a/libraries/AP_HAL_SITL/hwdef/sitl_periph_battmon/README.md b/libraries/AP_HAL_SITL/hwdef/sitl_periph_battmon/README.md new file mode 100644 index 0000000000000..1999e3f6ad2a8 --- /dev/null +++ b/libraries/AP_HAL_SITL/hwdef/sitl_periph_battmon/README.md @@ -0,0 +1,3 @@ +### sitl_periph_battmon + +A simulated peripheral for simulating a battery monitor supplying data over CAN. diff --git a/libraries/AP_HAL_SITL/hwdef/sitl_periph_battmon/hwdef.dat b/libraries/AP_HAL_SITL/hwdef/sitl_periph_battmon/hwdef.dat new file mode 100644 index 0000000000000..d1a2121f2acfc --- /dev/null +++ b/libraries/AP_HAL_SITL/hwdef/sitl_periph_battmon/hwdef.dat @@ -0,0 +1,7 @@ +include ../sitl_periph/hwdef.inc + +define CAN_APP_NODE_NAME "org.ardupilot.ap_periph_battmon" +define APJ_BOARD_ID 101 + +undef AP_PERIPH_BATTERY_ENABLED +define AP_PERIPH_BATTERY_ENABLED 1 diff --git a/libraries/AP_HAL_SITL/hwdef/sitl_periph_can_to_serial/hwdef.dat b/libraries/AP_HAL_SITL/hwdef/sitl_periph_can_to_serial/hwdef.dat new file mode 100644 index 0000000000000..01675ea63ba7e --- /dev/null +++ b/libraries/AP_HAL_SITL/hwdef/sitl_periph_can_to_serial/hwdef.dat @@ -0,0 +1,4 @@ +include ../sitl_periph/hwdef.inc + +define CAN_APP_NODE_NAME "org.ardupilot.serial_passthrough" +define APJ_BOARD_ID 101 diff --git a/libraries/AP_HAL_SITL/hwdef/sitl_periph_gps/README.md b/libraries/AP_HAL_SITL/hwdef/sitl_periph_gps/README.md new file mode 100644 index 0000000000000..2ccfadd36d780 --- /dev/null +++ b/libraries/AP_HAL_SITL/hwdef/sitl_periph_gps/README.md @@ -0,0 +1,3 @@ +### sitl_periph_gps + +A simulated peripheral which produces GPS data over CAN diff --git a/libraries/AP_HAL_SITL/hwdef/sitl_periph_gps/hwdef.dat b/libraries/AP_HAL_SITL/hwdef/sitl_periph_gps/hwdef.dat new file mode 100644 index 0000000000000..04d54cab26c12 --- /dev/null +++ b/libraries/AP_HAL_SITL/hwdef/sitl_periph_gps/hwdef.dat @@ -0,0 +1,7 @@ +include ../sitl_periph/hwdef.inc + +define CAN_APP_NODE_NAME "org.ardupilot.ap_periph_gps" +define APJ_BOARD_ID 101 + +undef AP_PERIPH_GPS_ENABLED +define AP_PERIPH_GPS_ENABLED 1 diff --git a/libraries/AP_HAL_SITL/hwdef/sitl_periph_universal/README.md b/libraries/AP_HAL_SITL/hwdef/sitl_periph_universal/README.md new file mode 100644 index 0000000000000..f8374772a9bc8 --- /dev/null +++ b/libraries/AP_HAL_SITL/hwdef/sitl_periph_universal/README.md @@ -0,0 +1,3 @@ +### sitl_periph_universal + +A simulated peripheral which contains much of the functionality that an AP_Periph offers. diff --git a/libraries/AP_HAL_SITL/hwdef/sitl_periph_universal/hwdef.dat b/libraries/AP_HAL_SITL/hwdef/sitl_periph_universal/hwdef.dat new file mode 100644 index 0000000000000..993a2085b93ad --- /dev/null +++ b/libraries/AP_HAL_SITL/hwdef/sitl_periph_universal/hwdef.dat @@ -0,0 +1,42 @@ +include ../sitl_periph/hwdef.inc + +define CAN_APP_NODE_NAME "org.ardupilot.ap_periph_universal" +define APJ_BOARD_ID 100 + +undef AP_PERIPH_GPS_ENABLED +define AP_PERIPH_GPS_ENABLED 1 +undef AP_PERIPH_AIRSPEED_ENABLED +define AP_PERIPH_AIRSPEED_ENABLED 1 +undef AP_PERIPH_MAG_ENABLED +define AP_PERIPH_MAG_ENABLED 1 +undef AP_PERIPH_BARO_ENABLED +define AP_PERIPH_BARO_ENABLED 1 +undef AP_PERIPH_IMU_ENABLED +define AP_PERIPH_IMU_ENABLED 1 +undef AP_PERIPH_RANGEFINDER_ENABLED +define AP_PERIPH_RANGEFINDER_ENABLED 1 +undef AP_PERIPH_BATTERY_ENABLED +define AP_PERIPH_BATTERY_ENABLED 1 +undef AP_PERIPH_EFI_ENABLED +define AP_PERIPH_EFI_ENABLED 1 +undef AP_PERIPH_RPM_ENABLED +define AP_PERIPH_RPM_ENABLED 1 +undef AP_PERIPH_RPM_STREAM_ENABLED +define AP_PERIPH_RPM_STREAM_ENABLED 1 +define AP_RPM_STREAM_ENABLED 1 +undef AP_PERIPH_RC_OUT_ENABLED +define AP_PERIPH_RC_OUT_ENABLED 1 +undef AP_PERIPH_ADSB_ENABLED +define AP_PERIPH_ADSB_ENABLED 1 +undef AP_PERIPH_SERIAL_OPTIONS_ENABLED +undef AP_TERRAIN_AVAILABLE +define AP_TERRAIN_AVAILABLE 1 + +define AP_PERIPH_SERIAL_OPTIONS_ENABLED 1 +define AP_AIRSPEED_ENABLED 1 +define AP_BATTERY_ESC_ENABLED 1 +define HAL_PWM_COUNT 32 +define HAL_WITH_ESC_TELEM 1 +define AP_EXTENDED_ESC_TELEM_ENABLED 1 + +define HAL_GYROFFT_ENABLED 0 diff --git a/libraries/AP_ICEngine/AP_ICEngine.cpp b/libraries/AP_ICEngine/AP_ICEngine.cpp index bb482f80c5be5..47447a4c4038d 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.cpp +++ b/libraries/AP_ICEngine/AP_ICEngine.cpp @@ -639,10 +639,10 @@ void AP_ICEngine::update_idle_governor(int8_t &min_throttle) } const int8_t min_throttle_base = min_throttle; - // Initialize idle point to min_throttle on the first run + // Initialize idle point to start_percent on the first run static bool idle_point_initialized = false; if (!idle_point_initialized) { - idle_governor_integrator = min_throttle; + idle_governor_integrator = start_percent.get(); idle_point_initialized = true; } AP_RPM *ap_rpm = AP::rpm(); @@ -652,6 +652,7 @@ void AP_ICEngine::update_idle_governor(int8_t &min_throttle) // Check to make sure we have an enabled IC Engine, EFI Instance and that the idle governor is enabled if (get_state() != AP_ICEngine::ICE_RUNNING || idle_rpm < 0) { + idle_point_initialized = false; return; } @@ -661,7 +662,7 @@ void AP_ICEngine::update_idle_governor(int8_t &min_throttle) // Double Check to make sure engine is really running if (!ap_rpm->get_rpm(rpm_instance-1, rpmv) || rpmv < 1) { // Reset idle point to the default value when the engine is stopped - idle_governor_integrator = min_throttle; + idle_point_initialized = false; return; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 545a78ef7fa03..6c962e3b145c1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -1253,7 +1253,6 @@ AP_InertialSensor::detect_backends(void) ADD_BACKEND(AP_InertialSensor_Invensensev2::probe(*this, hal.spi->get_device("icm20948"), ROTATION_YAW_270)); break; - case AP_BoardConfig::PX4_BOARD_FMUV5: case AP_BoardConfig::PX4_BOARD_FMUV6: _fast_sampling_mask.set_default(1); ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device("icm20689"), ROTATION_NONE)); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp index b7bff4541e039..86d17a7182188 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp @@ -310,6 +310,12 @@ void AP_InertialSensor_SITL::generate_gyro() void AP_InertialSensor_SITL::timer_update(void) { + // on some simulations (RealFlight) the aircraft sim decides when a new sample is available. + if (sitl->state.flightaxis_imu_frame_num > 0) { + update_from_frame(); + return; + } + uint64_t now = AP_HAL::micros64(); #if 0 // insert a 1s pause in IMU data. This triggers a pause in EK2 @@ -321,6 +327,7 @@ void AP_InertialSensor_SITL::timer_update(void) if (sitl == nullptr) { return; } + if (now >= next_accel_sample) { if (((1U << accel_instance) & sitl->accel_fail_mask) == 0) { #if AP_SIM_INS_FILE_ENABLED @@ -360,6 +367,26 @@ void AP_InertialSensor_SITL::timer_update(void) } } +void AP_InertialSensor_SITL::update_from_frame(void) +{ + if (sitl == nullptr) { + return; + } + + if (flightaxis_imu_frame_num == sitl->state.flightaxis_imu_frame_num) { + return; + } + + flightaxis_imu_frame_num = sitl->state.flightaxis_imu_frame_num; + + if (((1U << accel_instance) & sitl->accel_fail_mask) == 0) { + generate_accel(); + } + if (((1U << gyro_instance) & sitl->gyro_fail_mask) == 0) { + generate_gyro(); + } +} + float AP_InertialSensor_SITL::gyro_drift(void) const { if (is_zero(sitl->drift_speed) || diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h index 856f40ab003a7..760c7ab3d838c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h @@ -32,6 +32,7 @@ class AP_InertialSensor_SITL : public AP_InertialSensor_Backend void generate_gyro(); float get_temperature(void); void update_file(); + void update_from_frame(); #if AP_SIM_INS_FILE_ENABLED void read_gyro(const float* buf, uint8_t nsamples); void read_gyro_from_file(); @@ -47,6 +48,7 @@ class AP_InertialSensor_SITL : public AP_InertialSensor_Backend uint64_t next_gyro_sample; uint64_t next_accel_sample; + uint64_t flightaxis_imu_frame_num; float gyro_time; float accel_time; float gyro_motor_phase[32]; diff --git a/libraries/AP_JSButton/AP_JSButton.h b/libraries/AP_JSButton/AP_JSButton.h index cb5d7e79d0d49..0a49bfaed6ab9 100644 --- a/libraries/AP_JSButton/AP_JSButton.h +++ b/libraries/AP_JSButton/AP_JSButton.h @@ -65,9 +65,9 @@ class JSButton { // 60 reserved for future function k_servo_1_inc = 61, ///< increase servo output k_servo_1_dec = 62, ///< decrease servo output - k_servo_1_min = 63, ///< center servo - k_servo_1_max = 64, ///< set servo output to minimum (SERVOn_MIN) - k_servo_1_center = 65, ///< set servo output to maximum (SERVOn_MAX) + k_servo_1_min = 63, ///< set servo output to minimum (SERVOn_MIN) + k_servo_1_max = 64, ///< set servo output to maximum (SERVOn_MAX) + k_servo_1_center = 65, ///< center servo k_servo_2_inc = 66, k_servo_2_dec = 67, @@ -82,7 +82,7 @@ class JSButton { k_servo_3_center = 75, k_servo_1_min_momentary = 76, ///< set servo output to minimum (SERVOn_MIN) until released - k_servo_1_max_momentary = 77, ///< set servo output to minimum (SERVOn_MAX) until released + k_servo_1_max_momentary = 77, ///< set servo output to maximum (SERVOn_MAX) until released k_servo_1_min_toggle = 78, ///< toggle servo output btwn trim (SERVOn_TRIM) and min (SERVOn_MIN) k_servo_1_max_toggle = 79, ///< toggle servo output btwn trim (SERVOn_TRIM) and max (SERVOn_MAX) diff --git a/libraries/AP_LeakDetector/AP_LeakDetector.cpp b/libraries/AP_LeakDetector/AP_LeakDetector.cpp index cbf587172f52d..9b7a847a246fb 100644 --- a/libraries/AP_LeakDetector/AP_LeakDetector.cpp +++ b/libraries/AP_LeakDetector/AP_LeakDetector.cpp @@ -115,3 +115,11 @@ void AP_LeakDetector::set_detect() { _last_detect_ms = AP_HAL::millis(); } + +int8_t AP_LeakDetector::get_pin(uint8_t instance) const +{ + if (instance >= LEAKDETECTOR_MAX_INSTANCES) { + return 0; + } + return _pin[instance]; +} diff --git a/libraries/AP_LeakDetector/AP_LeakDetector.h b/libraries/AP_LeakDetector/AP_LeakDetector.h index 351d7cd08a01e..8e53d0d72d195 100644 --- a/libraries/AP_LeakDetector/AP_LeakDetector.h +++ b/libraries/AP_LeakDetector/AP_LeakDetector.h @@ -36,6 +36,8 @@ class AP_LeakDetector { // Update all drivers bool update(void); + int8_t get_pin(uint8_t instance) const; + static const struct AP_Param::GroupInfo var_info[]; private: diff --git a/libraries/AP_Logger/AP_Logger.cpp b/libraries/AP_Logger/AP_Logger.cpp index 06cfc081156d9..d1f0ea6b51ae9 100644 --- a/libraries/AP_Logger/AP_Logger.cpp +++ b/libraries/AP_Logger/AP_Logger.cpp @@ -908,6 +908,10 @@ void AP_Logger::Write_Message(const char *message) { FOR_EACH_BACKEND(Write_Message(message)); } +void AP_Logger::Write_MessageChunk(uint8_t id, const char *messagechunk, uint8_t chunk_seq) +{ + FOR_EACH_BACKEND(Write_MessageChunk(id, messagechunk, chunk_seq)); +} void AP_Logger::Write_Mode(uint8_t mode, const ModeReason reason) { diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 10ff63d1c2454..7451106a46a3c 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -13,7 +13,6 @@ #include #include #include -#include #include @@ -262,6 +261,16 @@ class AP_Logger void Write_Power(void); void Write_Radio(const mavlink_radio_t &packet); void Write_Message(const char *message); + // support for multi-chunk messages: + uint8_t get_MSG_id() { + uint8_t ret = ++MSG_id; + if (ret > 0) { + return ret; + } + return ++MSG_id; + } + void Write_MessageChunk(uint8_t id, const char *messagechunk, uint8_t chunk_seq); + void Write_MessageF(const char *fmt, ...); void Write_Compass(); void Write_Mode(uint8_t mode, const ModeReason reason); @@ -616,6 +625,9 @@ class AP_Logger void log_file_content(FileContent &file_content, const char *filename); void file_content_update(FileContent &file_content); #endif + + // support for multi-chunk messages: + uint8_t MSG_id; }; namespace AP { diff --git a/libraries/AP_Logger/AP_Logger_Backend.h b/libraries/AP_Logger/AP_Logger_Backend.h index 66bd2bafb3496..a25d9815b2d96 100644 --- a/libraries/AP_Logger/AP_Logger_Backend.h +++ b/libraries/AP_Logger/AP_Logger_Backend.h @@ -139,6 +139,7 @@ class AP_Logger_Backend } bool Write_Message(const char *message); bool Write_MessageF(const char *fmt, ...); + bool Write_MessageChunk(uint8_t id, const char *messagechunk, uint8_t chunk_seq); bool Write_Mission_Cmd(const AP_Mission &mission, const AP_Mission::Mission_Command &cmd, LogMessages id); @@ -275,6 +276,7 @@ class AP_Logger_Backend bool emit_format_for_type(LogMessages a_type); Bitmask<256> _formats_written; + uint8_t msg_id; // the ID of the next MSG message that will be logged }; #endif // HAL_LOGGING_ENABLED diff --git a/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp b/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp index 841bf1beea5fb..fb36066ace24f 100644 --- a/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp +++ b/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp @@ -216,6 +216,15 @@ void AP_Logger::handle_log_sending() { WITH_SEMAPHORE(_log_send_sem); + // we can spend a *long* time sending data on the main thread. We + // can spen more than 1.9 seconds doing this depending on what the + // SD card gets up to, moving us into the monitor thread's "you + // are going to watchdog" code. Until we move all these + // operations into a thread we will ask the timer thread to pat + // the watchdog and have the monitor thread chill out by being in + // an expected delay: + EXPECT_DELAY_MS(5000); + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL // assume USB speeds in SITL for the purposes of log download const uint8_t num_sends = 40; diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index d98921775ec1b..70c7a878608ac 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -18,6 +18,7 @@ #include "AP_Logger_File.h" #include "AP_Logger_MAVLink.h" #include "LoggerMessageWriter.h" +#include extern const AP_HAL::HAL& hal; @@ -339,13 +340,37 @@ bool AP_Logger_Backend::Write_EntireMission() // Write a text message to the log bool AP_Logger_Backend::Write_Message(const char *message) { - struct log_Message pkt{ - LOG_PACKET_HEADER_INIT(LOG_MESSAGE_MSG), + // i==0 here means we log an empty string if it is passed in: + const uint8_t id = AP::logger().get_MSG_id(); // there is a race condition on this ID; if a thread logs a message at the same time as the main thread then we can re-use this + + uint8_t chunk_seq = 0; + for (uint8_t i=0; i == 0 || i vel_min)) { vel_target = constrain_float(vel_target, vel_min, vel_max); } - shape_vel_accel(vel_target, accel_input, vel, accel, accel_min, accel_max, jerk_max, dt, limit_total); + shape_vel_accel(vel_target, accel_desired, vel, accel, accel_min, accel_max, jerk_max, dt, limit_total); } // Computes a jerk-limited acceleration profile to move toward a position and velocity target in 2D. @@ -330,7 +307,7 @@ void shape_pos_vel_accel(postype_t pos_input, float vel_input, float accel_input // - Limits include: maximum velocity (`vel_max`), maximum acceleration (`accel_max`), and maximum jerk (`jerk_max`). // - If `limit_total` is true, constraints are applied to the total command (not just the correction). // Provides smooth trajectory shaping for lateral motion with bounded dynamics. -void shape_pos_vel_accel_xy(const Vector2p& pos_input, const Vector2f& vel_input, const Vector2f& accel_input, +void shape_pos_vel_accel_xy(const Vector2p& pos_desired, const Vector2f& vel_desired, const Vector2f& accel_desired, const Vector2p& pos, const Vector2f& vel, Vector2f& accel, float vel_max, float accel_max, float jerk_max, float dt, bool limit_total) @@ -347,7 +324,7 @@ void shape_pos_vel_accel_xy(const Vector2p& pos_input, const Vector2f& vel_input const float accel_tc_max = 0.5 * accel_max; // Position error to be corrected — direction is preserved, magnitude used for shaping - Vector2f pos_error = (pos_input - pos).tofloat(); + Vector2f pos_error = (pos_desired - pos).tofloat(); // velocity to correct position Vector2f vel_target = sqrt_controller(pos_error, KPv, accel_tc_max, dt); @@ -358,14 +335,14 @@ void shape_pos_vel_accel_xy(const Vector2p& pos_input, const Vector2f& vel_input } // velocity correction with input velocity - vel_target = vel_target + vel_input; + vel_target = vel_target + vel_desired; // Constrain total velocity if limiting is enabled and vel_max is positive if (limit_total && is_positive(vel_max)) { vel_target.limit_length(vel_max); } - shape_vel_accel_xy(vel_target, accel_input, vel, accel, accel_max, jerk_max, dt, limit_total); + shape_vel_accel_xy(vel_target, accel_desired, vel, accel, accel_max, jerk_max, dt, limit_total); } // Computes a jerk-limited acceleration command to follow an angular position, velocity, and acceleration target. @@ -376,7 +353,7 @@ void shape_pos_vel_accel_xy(const Vector2p& pos_input, const Vector2f& vel_input // - Setting `angle_vel_max` or `angle_accel_max` to zero disables that respective limit. // - The acceleration output is shaped toward the target using `shape_vel_accel`. // Used for attitude control with limited angular velocity and angular acceleration (e.g., roll/pitch shaping). -void shape_angle_vel_accel(float angle_input, float angle_vel_input, float angle_accel_input, +void shape_angle_vel_accel(float angle_desired, float angle_vel_desired, float angle_accel_desired, float angle, float angle_vel, float& angle_accel, float angle_vel_max, float angle_accel_max, float angle_jerk_max, float dt, bool limit_total) @@ -391,7 +368,7 @@ void shape_angle_vel_accel(float angle_input, float angle_vel_input, float angle float stopping_time = fabsf(angle_vel / angle_accel_max); // Compute total angular error with prediction of future motion, then wrap to [-π, π] - float angle_error = angle_input - angle - angle_vel * stopping_time; + float angle_error = angle_desired - angle - angle_vel * stopping_time; angle_error = wrap_PI(angle_error); angle_error += angle_vel * stopping_time; @@ -409,7 +386,7 @@ void shape_angle_vel_accel(float angle_input, float angle_vel_input, float angle } // velocity correction with input velocity - angle_vel_target += angle_vel_input; + angle_vel_target += angle_vel_desired; // Constrain total velocity if limiting is enabled and angle_vel_max is positive if (limit_total && is_positive(angle_vel_max)){ @@ -417,7 +394,7 @@ void shape_angle_vel_accel(float angle_input, float angle_vel_input, float angle } // Shape the angular acceleration using jerk-limited profile - shape_vel_accel(angle_vel_target, angle_accel_input, angle_vel, angle_accel, + shape_vel_accel(angle_vel_target, angle_accel_desired, angle_vel, angle_accel, -angle_accel_max, angle_accel_max, angle_jerk_max, dt, limit_total); } @@ -440,11 +417,11 @@ bool limit_accel_xy(const Vector2f& vel, Vector2f& accel, float accel_max) accel.limit_length(accel_max); } else { // calculate acceleration in the direction of and perpendicular to the velocity input - const Vector2f vel_input_unit = vel.normalized(); + const Vector2f vel_unit = vel.normalized(); // acceleration in the direction of travel - float accel_dir = vel_input_unit * accel; + float accel_dir = vel_unit * accel; // cross track acceleration - Vector2f accel_cross = accel - (vel_input_unit * accel_dir); + Vector2f accel_cross = accel - (vel_unit * accel_dir); if (accel_cross.limit_length(accel_max)) { accel_dir = 0.0; } else { @@ -453,13 +430,53 @@ bool limit_accel_xy(const Vector2f& vel, Vector2f& accel, float accel_max) float accel_max_dir = safe_sqrt(sq(accel_max) - accel_cross.length_squared()); accel_dir = constrain_float(accel_dir, -accel_max_dir, accel_max_dir); } - accel = accel_cross + vel_input_unit * accel_dir; + accel = accel_cross + vel_unit * accel_dir; } return true; } return false; } +// Limits acceleration magnitude while preserving cross-track authority during turns. +// - Splits acceleration into components parallel and perpendicular to velocity. +// - Ensures sufficient lateral acceleration is reserved for path curvature. +// - If forward acceleration dominates, lateral acceleration is limited to +// CORNER_ACCELERATION_RATIO * accel_max. +// Returns true if limiting was applied. (including simple magnitude limiting when vel is zero). +bool limit_accel_corner_xy(const Vector2f& vel, Vector2f& accel, float accel_max) +{ + // check accel_max is defined + if (!is_positive(accel_max)) { + return false; + } + if (vel.is_zero()) { + // No along/cross decomposition possible; apply a simple magnitude limit. + return accel.limit_length(accel_max); + } + + // Unit velocity direction defines the along-track axis. + const Vector2f vel_unit = vel.normalized(); + + // Decompose accel into along-track and cross-track components. + float accel_dir = vel_unit * accel; + Vector2f accel_cross = accel - (vel_unit * accel_dir); + + // If cross-track magnitude is at least as large as along-track, a simple vector limit + // preserves at least 1/sqrt(2) of accel_max for cross-track correction. + if (sq(accel_dir) <= accel_cross.length_squared()) { + // accel can be simply limited in magnitude + return accel.limit_length(accel_max); + } + + // Along-track dominates: preserve cross-track authority by limiting cross-track first, + // then allocate remaining magnitude to along-track. + accel_cross.limit_length(CORNER_ACCELERATION_RATIO * accel_max); + float accel_max_dir = safe_sqrt(sq(accel_max) - accel_cross.length_squared()); + accel_dir = constrain_float(accel_dir, -accel_max_dir, accel_max_dir); + accel = accel_cross + vel_unit * accel_dir; + return true; +} + // Piecewise square-root + linear controller that limits second-order response (acceleration). // - Behaves like a P controller near the setpoint. // - Switches to sqrt(2·a·Δx) shaping beyond a threshold to limit acceleration. @@ -566,7 +583,7 @@ float stopping_distance(float velocity, float p, float accel_max) // - `direction` should be a non-zero vector indicating desired direction of travel. // - Limits: max_xy, max_z_pos (upward), max_z_neg (downward) // Returns the maximum achievable magnitude in that direction without violating any axis constraint. -float kinematic_limit(Vector3f direction, float max_xy, float max_z_pos, float max_z_neg) +float kinematic_limit(Vector3f direction, float max_xy, float max_z_neg, float max_z_pos) { // Reject zero-length direction vectors or undefined limits if (is_zero(direction.length_squared())) { @@ -575,13 +592,13 @@ float kinematic_limit(Vector3f direction, float max_xy, float max_z_pos, float m const float segment_length_xy = direction.xy().length(); - return kinematic_limit(segment_length_xy, direction.z, max_xy, max_z_pos, max_z_neg); + return kinematic_limit(segment_length_xy, direction.z, max_xy, max_z_neg, max_z_pos); } // compute the maximum allowed magnitude along a direction defined by segment_length_xy and segment_length_z components // constrained by independent horizontal (max_xy) and vertical (max_z_pos/max_z_neg) limits // returns the maximum achievable magnitude without exceeding any axis limit -float kinematic_limit(float segment_length_xy, float segment_length_z, float max_xy, float max_z_pos, float max_z_neg) +float kinematic_limit(float segment_length_xy, float segment_length_z, float max_xy, float max_z_neg, float max_z_pos) { // Reject zero-length direction vectors or undefined limits if (is_zero(max_xy) || is_zero(max_z_pos) || is_zero(max_z_neg)) { @@ -647,10 +664,9 @@ float input_expo(float input, float expo) } // Converts a lean angle (radians) to horizontal acceleration in m/s². -// Assumes flat Earth and small angle approximation: a = g * tan(θ) float angle_rad_to_accel_mss(float angle_rad) { - // Convert lean angle to horizontal acceleration assuming flat-Earth and small-angle + // Convert lean angle to horizontal acceleration return GRAVITY_MSS * tanf(angle_rad); } diff --git a/libraries/AP_Math/control.h b/libraries/AP_Math/control.h index ec2661048c931..f7016ec57f275 100644 --- a/libraries/AP_Math/control.h +++ b/libraries/AP_Math/control.h @@ -56,26 +56,26 @@ void update_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel // - Constrains the rate of change of acceleration to be within ±`jerk_max` over time `dt`. // - The current acceleration value is modified in-place. // Useful for ensuring smooth transitions in thrust or lean angle command profiles. -void shape_accel(float accel_input, float& accel, +void shape_accel(float accel_desired, float& accel, float jerk_max, float dt); // Applies jerk-limited shaping to a 2D acceleration vector. // - Constrains the rate of change of acceleration to a maximum of `jerk_max` over time `dt`. -// - The current acceleration vector is modified in-place to approach `accel_input`. +// - The current acceleration vector is modified in-place to approach `accel_desired`. /// Ensures smooth acceleration transitions in both axes simultaneously. -void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel, +void shape_accel_xy(const Vector2f& accel_desired, Vector2f& accel, float jerk_max, float dt); -void shape_accel_xy(const Vector3f& accel_input, Vector3f& accel, +void shape_accel_xy(const Vector3f& accel_desired, Vector3f& accel, float jerk_max, float dt); // Shapes velocity and acceleration using jerk-limited control. -// - Computes correction acceleration needed to reach `vel_input` from current `vel`. +// - Computes correction acceleration needed to reach `vel_desired` from current `vel`. // - Uses a square-root controller with max acceleration and jerk constraints. -// - Correction is combined with feedforward `accel_input`. +// - Correction is combined with feedforward `accel_desired`. // - If `limit_total_accel` is true, total acceleration is constrained to `accel_min` / `accel_max`. // The result is applied via `shape_accel`. -void shape_vel_accel(float vel_input, float accel_input, +void shape_vel_accel(float vel_desired, float accel_desired, float vel, float& accel, float accel_min, float accel_max, float jerk_max, float dt, bool limit_total_accel); @@ -83,10 +83,10 @@ void shape_vel_accel(float vel_input, float accel_input, // Computes a jerk-limited acceleration command in 2D to track a desired velocity input. // - Uses a square-root controller to calculate correction acceleration based on velocity error. // - Correction is constrained to stay within `accel_max` (total acceleration magnitude). -// - Correction is added to `accel_input` (feedforward). +// - Correction is added to `accel_desired` (feedforward). // - If `limit_total_accel` is true, total acceleration is constrained after summing. // Ensures velocity tracking with smooth, physically constrained motion. -void shape_vel_accel_xy(const Vector2f& vel_input1, const Vector2f& accel_input, +void shape_vel_accel_xy(const Vector2f& vel_desired, const Vector2f& accel_desired, const Vector2f& vel, Vector2f& accel, float accel_max, float jerk_max, float dt, bool limit_total_accel); @@ -95,7 +95,7 @@ void shape_vel_accel_xy(const Vector2f& vel_input1, const Vector2f& accel_input, // - That velocity is then shaped via `shape_vel_accel` to enforce acceleration and jerk limits. // - Limits can be applied separately to correction and total values. // Used for smooth point-to-point motion with constrained dynamics. -void shape_pos_vel_accel(const postype_t pos_input, float vel_input, float accel_input, +void shape_pos_vel_accel(const postype_t pos_desired, float vel_desired, float accel_desired, const postype_t pos, float vel, float& accel, float vel_min, float vel_max, float accel_min, float accel_max, @@ -107,7 +107,7 @@ void shape_pos_vel_accel(const postype_t pos_input, float vel_input, float accel // - Limits include: maximum velocity (`vel_max`), maximum acceleration (`accel_max`), and maximum jerk (`jerk_max`). // - If `limit_total` is true, constraints are applied to the total command (not just the correction). // Provides smooth trajectory shaping for lateral motion with bounded dynamics. -void shape_pos_vel_accel_xy(const Vector2p& pos_input, const Vector2f& vel_input, const Vector2f& accel_input, +void shape_pos_vel_accel_xy(const Vector2p& pos_desired, const Vector2f& vel_desired, const Vector2f& accel_desired, const Vector2p& pos, const Vector2f& vel, Vector2f& accel, float vel_max, float accel_max, float jerk_max, float dt, bool limit_total); @@ -120,7 +120,7 @@ void shape_pos_vel_accel_xy(const Vector2p& pos_input, const Vector2f& vel_input // - Setting `angle_vel_max` or `angle_accel_max` to zero disables that respective limit. // - The acceleration output is shaped toward the target using `shape_vel_accel`. // Used for attitude control with limited angular velocity and angular acceleration (e.g., roll/pitch shaping). -void shape_angle_vel_accel(float angle_input, float angle_vel_input, float angle_accel_input, +void shape_angle_vel_accel(float angle_desired, float angle_vel_desired, float angle_accel_desired, float angle, float angle_vel, float& angle_accel, float angle_vel_max, float angle_accel_max, float angle_jerk_max, float dt, bool limit_total); @@ -133,6 +133,14 @@ void shape_angle_vel_accel(float angle_input, float angle_vel_input, float angle // Returns true if the acceleration vector was modified. bool limit_accel_xy(const Vector2f& vel, Vector2f& accel, float accel_max); +// Limits acceleration magnitude while preserving cross-track authority during turns. +// - Splits acceleration into components parallel and perpendicular to velocity. +// - Ensures sufficient lateral acceleration is reserved for path curvature. +// - If forward acceleration dominates, lateral acceleration is limited to +// CORNER_ACCELERATION_RATIO * accel_max. +// Returns true if limiting was applied. +bool limit_accel_corner_xy(const Vector2f& vel, Vector2f& accel, float accel_max); + // Piecewise square-root + linear controller that limits second-order response (acceleration). // - Behaves like a P controller near the setpoint. // - Switches to sqrt(2·a·Δx) shaping beyond a threshold to limit acceleration. @@ -162,7 +170,7 @@ float stopping_distance(float velocity, float p, float accel_max); // - `direction` should be a non-zero vector indicating desired direction of travel. // - Limits: max_xy, max_z_pos (upward), max_z_neg (downward) // Returns the maximum achievable magnitude in that direction without violating any axis constraint. -float kinematic_limit(Vector3f direction, float max_xy, float max_z_pos, float max_z_neg); +float kinematic_limit(Vector3f direction, float max_xy, float max_z_neg, float max_z_pos); // compute the maximum allowed magnitude along a direction defined by segment_length_xy and segment_length_z components // constrained by independent horizontal (max_xy) and vertical (max_z_pos/max_z_neg) limits diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 169062dde7999..0b6512a0b80a8 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -154,6 +154,7 @@ class AP_Motors { float get_throttle_out() const { return _throttle_out; } virtual bool get_thrust(uint8_t motor_num, float& thr_out) const { return false; } virtual bool get_raw_motor_throttle(uint8_t motor_num, float& thr_out) const { return false; } + float get_throttle_in() const { return _throttle_in; } float get_throttle() const { return constrain_float(_throttle_filter.get(), 0.0f, 1.0f); } float get_throttle_bidirectional() const { return constrain_float(2 * (_throttle_filter.get() - 0.5f), -1.0f, 1.0f); } float get_throttle_slew_rate() const { return _throttle_slew_rate; } diff --git a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp index 7a0fa5306907b..45aab690d67b8 100644 --- a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp +++ b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp @@ -71,7 +71,7 @@ void setup() char cmd[20] {}; strncpy(cmd, arg, eq-arg); - const float value = atof(eq+1); + const float value = strtof(eq+1, nullptr); if (strcmp(cmd,"yaw_headroom") == 0) { if (motors_matrix != nullptr) { motors_matrix->set_yaw_headroom(value); diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index dc2e2fe4747dc..4a837b7b780ca 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -668,6 +668,42 @@ bool AP_Mount::get_poi(uint8_t instance, Quaternion &quat, Location &loc, Locati } #endif +#if AP_MOUNT_POI_LOCK_ENABLED +// lock currently viewed GPS point and switch to GPS Targeting mode +void AP_Mount::set_poi_lock(uint8_t instance) +{ + auto *backend = get_instance(instance); + if (backend == nullptr) { + return; + } + + // call backend's set_poi_lock + backend->set_poi_lock(); +} + +void AP_Mount::clear_poi_lock(uint8_t instance) +{ + auto *backend = get_instance(instance); + if (backend == nullptr) { + return; + } + + // call backend's clear_poi_lock + backend->clear_poi_lock(); +} + +void AP_Mount::suspend_poi_lock(uint8_t instance) +{ + auto *backend = get_instance(instance); + if (backend == nullptr) { + return; + } + + // call backend's suspend_poi_lock + backend->suspend_poi_lock(); +} +#endif // AP_MOUNT_POI_LOCK_ENABLED + // get attitude as a quaternion. returns true on success. // att_quat will be an earth-frame quaternion rotated such that // yaw is in body-frame. @@ -797,7 +833,7 @@ void AP_Mount::set_target_sysid(uint8_t instance, uint8_t sysid) if (backend == nullptr) { return; } - // call instance's set_roi_cmd + // call instance's set target SYSID cmd backend->set_target_sysid(sysid); } diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 8c1b3f4d3cd4a..5a12716cc5fe3 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -177,6 +177,16 @@ class AP_Mount // If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle void set_yaw_lock(bool yaw_lock) { set_yaw_lock(_primary, yaw_lock); } void set_yaw_lock(uint8_t instance, bool yaw_lock); + +#if AP_MOUNT_POI_LOCK_ENABLED + // controls POI lock which locks gimbal to GPS point currently in view and switches to GPS Targeting mode, suspends tracking poi or clears it + void set_poi_lock() { set_poi_lock(_primary); } + void set_poi_lock(uint8_t instance); + void clear_poi_lock() { clear_poi_lock(_primary); } + void clear_poi_lock(uint8_t instance); + void suspend_poi_lock() { suspend_poi_lock(_primary); } + void suspend_poi_lock(uint8_t instance); +#endif // set angle target in degrees // roll and pitch are in earth-frame diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.cpp b/libraries/AP_Mount/AP_Mount_Alexmos.cpp index b0a429b54f2be..5288635f0e925 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.cpp +++ b/libraries/AP_Mount/AP_Mount_Alexmos.cpp @@ -85,15 +85,7 @@ void AP_Mount_Alexmos::update() update_mnt_target(); // send target angles or rates depending on the target type - switch (mnt_target.target_type) { - case MountTargetType::RATE: - update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad); - FALLTHROUGH; - case MountTargetType::ANGLE: - // send latest angle targets to gimbal - control_axis(mnt_target.angle_rad); - break; - } + send_target_to_gimbal(); } // has_pan_control - returns true if this mount can control its pan (required for multicopters) @@ -155,7 +147,7 @@ void AP_Mount_Alexmos::get_boardinfo() /* control_axis : send new angle target to the gimbal at a fixed speed of 30 deg/s */ -void AP_Mount_Alexmos::control_axis(const MountTarget& angle_target_rad) +void AP_Mount_Alexmos::send_target_angles(const MountAngleTarget& angle_target_rad) { alexmos_parameters outgoing_buffer; outgoing_buffer.angle_speed.mode = AP_MOUNT_ALEXMOS_MODE_ANGLE; diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.h b/libraries/AP_Mount/AP_Mount_Alexmos.h index d3c15392189c7..00772da2de293 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.h +++ b/libraries/AP_Mount/AP_Mount_Alexmos.h @@ -33,7 +33,16 @@ class AP_Mount_Alexmos : public AP_Mount_Backend // get attitude as a quaternion. returns true on success bool get_attitude_quaternion(Quaternion& att_quat) override; + // servo only natively supports angles: + uint8_t natively_supported_mount_target_types() const override { + return NATIVE_ANGLES_ONLY; + }; + private: + // allow removing lean angles for pitch and roll locks + bool apply_bf_roll_pitch_adjustments_in_rc_targeting() const override { + return true; + } // get_angles - void get_angles(); @@ -45,7 +54,7 @@ class AP_Mount_Alexmos : public AP_Mount_Backend void get_boardinfo(); // send new angles to the gimbal at a fixed speed of 30 deg/s - void control_axis(const MountTarget& angle_target_rad); + void send_target_angles(const MountAngleTarget& angle_rad) override; // read_params - read current profile profile_id and global parameters from the gimbal settings void read_params(uint8_t profile_id); diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index cce993bb05c9a..72dc05f7a5bcf 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -45,14 +45,10 @@ void AP_Mount_Backend::update() const bool mount_open = (_mode == MAV_MOUNT_MODE_RETRACT); SRV_Channels::move_servo(_open_idx, mount_open, 0, 1); - // set target rate to zero if we have not received rate command for a while - if ((get_mode() == MAV_MOUNT_MODE_MAVLINK_TARGETING) && - (mnt_target.target_type == MountTargetType::RATE) && - (AP_HAL::millis() - mnt_target.last_rate_request_ms > 3000)) { - mnt_target.rate_rads.roll = 0; - mnt_target.rate_rads.pitch = 0; - mnt_target.rate_rads.yaw = 0; - } +#if AP_MOUNT_POI_LOCK_ENABLED + update_poi_lock_target(); + #endif // AP_MOUNT_POI_LOCK_ENABLED + // location exists for mode Location current_loc; switch (_mode) { @@ -115,22 +111,65 @@ void AP_Mount_Backend::update_mnt_target_from_rc_target() } } - MountTarget rc_target; - get_rc_target(mnt_target.target_type, rc_target); - // if the option is set, force BF lock on yaw, pitch and roll axes (FPV lock) - if (option_set(Options::FPV_LOCK)) { - _yaw_lock = false; - _roll_lock = false; - _pitch_lock = false; + // get RC input from pilot + float roll_in, pitch_in, yaw_in; + get_rc_input(roll_in, pitch_in, yaw_in); + + // if RC_RATE is zero, targets are angle + if (_params.rc_rate_max <= 0) { + mnt_target.target_type = MountTargetType::ANGLE; + + // frame locks + bool FPV_option = option_set(Options::FPV_LOCK); //FPV_LOCK forces bodyframe on all axes in RC targeting mode + mnt_target.angle_rad.yaw_is_ef = FPV_option ? false : _yaw_lock; + mnt_target.angle_rad.roll_is_ef = FPV_option ? false : _roll_lock; + mnt_target.angle_rad.pitch_is_ef = FPV_option ? false : _pitch_lock; + + // roll angle + mnt_target.angle_rad.roll = radians(((roll_in + 1.0f) * 0.5f * (_params.roll_angle_max - _params.roll_angle_min) + _params.roll_angle_min)); + + // pitch angle + mnt_target.angle_rad.pitch = radians(((pitch_in + 1.0f) * 0.5f * (_params.pitch_angle_max - _params.pitch_angle_min) + _params.pitch_angle_min)); + + // yaw angle + mnt_target.angle_rad.yaw = radians(((yaw_in + 1.0f) * 0.5f * (_params.yaw_angle_max - _params.yaw_angle_min) + _params.yaw_angle_min)); + + // if in yaw ef lock, we use the captured and adjusted yaw_lock_heading rad to + // adjust the yaw so that any RC yaw changes are reflected in locked heading + if (mnt_target.angle_rad.yaw_is_ef) { + mnt_target.angle_rad.yaw = wrap_PI(mnt_target.angle_rad.yaw + _yaw_lock_heading_rad); + } + } else { + // calculate rate targets + mnt_target.target_type = MountTargetType::RATE; + const float rc_rate_max_rads = radians(_params.rc_rate_max.get()); + mnt_target.rate_rads.roll = roll_in * rc_rate_max_rads; + mnt_target.rate_rads.pitch = pitch_in * rc_rate_max_rads; + mnt_target.rate_rads.yaw = yaw_in * rc_rate_max_rads; + } +} + +// called for stabilized mounts which use roll and pitch angle targets that are earth frame +// to remove vehicle lean angle if pitch or roll is not locked, ie convert to actual body frame +void AP_Mount_Backend::adjust_mnt_target_if_RP_locked() +{ + // retrieve lean angles from ahrs + const AP_AHRS &ahrs = AP::ahrs(); + Vector2f ahrs_angle_rad = {ahrs.get_roll_rad(), ahrs.get_pitch_rad()}; + + // rotate ahrs roll and pitch angles to gimbal yaw + if (has_pan_control()) { + const float yaw_bf_rad = constrain_float(mnt_target.angle_rad.get_bf_yaw(), radians(_params.yaw_angle_min), radians(_params.yaw_angle_max)); + ahrs_angle_rad.rotate(yaw_bf_rad); } - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - mnt_target.angle_rad = rc_target; - break; - case MountTargetType::RATE: - mnt_target.rate_rads = rc_target; - break; + + // remove roll and pitch lean angle to correct to body frame + if (!mnt_target.angle_rad.roll_is_ef){ + mnt_target.angle_rad.roll += ahrs_angle_rad.x; } + if (!mnt_target.angle_rad.pitch_is_ef){ + mnt_target.angle_rad.pitch += ahrs_angle_rad.y; + } } // set angle target in degrees @@ -202,6 +241,81 @@ void AP_Mount_Backend::set_roi_target(const Location &target_loc) } } +#if AP_MOUNT_POI_LOCK_ENABLED +// set poi_lock - switch to GPS Targeting mode using current gimbal view's GPS point or save poi location as target +void AP_Mount_Backend::set_poi_lock() +{ + saved_mount_mode = get_mode(); //save current mount mode for the suspend_poi_lock + if (!roi_is_set()) { + mnt_target.poi_start_ms = AP_HAL::millis(); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "POI: tracking r=%.1f p=%.1f y=%.1f", degrees(mnt_target.angle_rad.roll), degrees(mnt_target.angle_rad.pitch), degrees(mnt_target.angle_rad.yaw)); + } else { // there is a poi target, just turn POI tracking back on + set_mode(MAV_MOUNT_MODE_GPS_POINT); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "POI: tracking"); + mnt_target.poi_start_ms = 0; + } +} + +// clear poi_lock - clear POI location and revert to default mode +void AP_Mount_Backend::clear_poi_lock() +{ + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "POI: Cleared"); + clear_roi_target(); +} + +// suspend_poi_lock - revert to saved targeting mode, if it exists and POI target exists, otherwise do nothing +void AP_Mount_Backend::suspend_poi_lock() +{ + if (roi_is_set() && saved_mount_mode != MAV_MOUNT_MODE_ENUM_END) { + set_mode(saved_mount_mode); // set back to mode before GPS_POINT if its been set by switch going HIGH + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "POI: Revert mode,target saved");; + } +} + +// update_poi_lock_target - tries to obtain POI location and start tracking,caller only needs to set poi_start_ms to current time to execute this +void AP_Mount_Backend::update_poi_lock_target() +{ + if (mnt_target.poi_start_ms == 0) { + return; + } + // POI calculation is running and but will silently give up after 3 seconds normally if it does not succeed + // try to resolve a AuxFunc POI command to a lat/lng/alt using get_poi + // set up variables for get_poi call + Quaternion quat; + Location vehicle_location; + Location target_location; + // if poi available, use it and start tracking, otherwise give warning, stop poi retrieval attempts + if (get_poi(_instance, quat, vehicle_location, target_location)) { + set_roi_target(target_location); + mnt_target.poi_start_ms = 0; + } else if (AP_HAL::millis() - mnt_target.poi_start_ms > 5000) { + // timeout + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "POI: Failed to find poi"); + mnt_target.poi_start_ms = 0; + } +} + +#endif // AP_MOUNT_POI_LOCK_ENABLED + +// set yaw lock - sets the _yaw_lock variable and captures current earth frame heading of mount for targeting in RC Targeting mode +void AP_Mount_Backend::set_yaw_lock(bool yaw_lock) +{ + // if yaw not locked already, capture mount's earth frame heading for later possible use + if (!_yaw_lock) { + float roll_in, pitch_in, yaw_in; + get_rc_input(roll_in, pitch_in, yaw_in); + //adjust current ef mount heading by current RC yaw angle input and store for later use + Quaternion att_quat_bf_rad; + if (get_attitude_quaternion(att_quat_bf_rad)) { + const float euler_yaw_bf_rad = att_quat_bf_rad.get_euler_yaw(); + const float euler_yaw_ef_rad = wrap_PI(euler_yaw_bf_rad + AP::ahrs().get_yaw_rad()); + _yaw_lock_heading_rad = wrap_PI(euler_yaw_ef_rad - radians(wrap_180((yaw_in + 1.0f) * 0.5f * (_params.yaw_angle_max - _params.yaw_angle_min) + _params.yaw_angle_min))); + } + } + _yaw_lock = yaw_lock; + } + + // clear_roi_target - clears target location that mount should attempt to point towards void AP_Mount_Backend::clear_roi_target() { @@ -486,6 +600,7 @@ void AP_Mount_Backend::write_log(uint64_t timestamp_us) LOG_PACKET_HEADER_INIT(static_cast(LOG_MOUNT_MSG)), time_us : (timestamp_us > 0) ? timestamp_us : AP_HAL::micros64(), instance : _instance, + mode : static_cast(get_mode()), desired_roll : target_roll, actual_roll : roll, desired_pitch : target_pitch, @@ -692,51 +807,9 @@ void AP_Mount_Backend::get_rc_input(float& roll_in, float& pitch_in, float& yaw_ } } -// get angle or rate targets from pilot RC -// target_type will be either ANGLE or RATE, rpy will be the target angle in deg or rate in deg/s -void AP_Mount_Backend::get_rc_target(MountTargetType& target_type, MountTarget& target_rpy) const -{ - // get RC input from pilot - float roll_in, pitch_in, yaw_in; - get_rc_input(roll_in, pitch_in, yaw_in); - - // frame locks - target_rpy.yaw_is_ef = _yaw_lock; - target_rpy.roll_is_ef = _roll_lock; - target_rpy.pitch_is_ef = _pitch_lock; - - // if RC_RATE is zero, targets are angle - if (_params.rc_rate_max <= 0) { - target_type = MountTargetType::ANGLE; - - // roll angle - target_rpy.roll = radians(((roll_in + 1.0f) * 0.5f * (_params.roll_angle_max - _params.roll_angle_min) + _params.roll_angle_min)); - - // pitch angle - target_rpy.pitch = radians(((pitch_in + 1.0f) * 0.5f * (_params.pitch_angle_max - _params.pitch_angle_min) + _params.pitch_angle_min)); - - // yaw angle - if (target_rpy.yaw_is_ef) { - // if yaw is earth-frame pilot yaw input control angle from -180 to +180 deg - target_rpy.yaw = yaw_in * M_PI; - } else { - // yaw target in body frame so apply body frame limits - target_rpy.yaw = radians(((yaw_in + 1.0f) * 0.5f * (_params.yaw_angle_max - _params.yaw_angle_min) + _params.yaw_angle_min)); - } - return; - } - - // calculate rate targets - target_type = MountTargetType::RATE; - const float rc_rate_max_rads = radians(_params.rc_rate_max.get()); - target_rpy.roll = roll_in * rc_rate_max_rads; - target_rpy.pitch = pitch_in * rc_rate_max_rads; - target_rpy.yaw = yaw_in * rc_rate_max_rads; -} - // get angle targets (in radians) to a Location // returns true on success, false on failure -bool AP_Mount_Backend::get_angle_target_to_location(const Location &loc, MountTarget& angle_rad) const +bool AP_Mount_Backend::get_angle_target_to_location(const Location &loc, MountAngleTarget& angle_rad) const { // exit immediately if vehicle's location is unavailable Location current_loc; @@ -775,7 +848,7 @@ bool AP_Mount_Backend::get_angle_target_to_location(const Location &loc, MountTa // get angle targets (in radians) to ROI location // returns true on success, false on failure -bool AP_Mount_Backend::get_angle_target_to_roi(MountTarget& angle_rad) const +bool AP_Mount_Backend::get_angle_target_to_roi(MountAngleTarget& angle_rad) const { if (!_roi_target.initialised()) { return false; @@ -784,7 +857,7 @@ bool AP_Mount_Backend::get_angle_target_to_roi(MountTarget& angle_rad) const } // return body-frame yaw angle from a mount target -float AP_Mount_Backend::MountTarget::get_bf_yaw() const +float AP_Mount_Backend::MountAngleTarget::get_bf_yaw() const { if (yaw_is_ef) { // convert to body-frame @@ -796,7 +869,7 @@ float AP_Mount_Backend::MountTarget::get_bf_yaw() const } // return earth-frame yaw angle from a mount target -float AP_Mount_Backend::MountTarget::get_ef_yaw() const +float AP_Mount_Backend::MountAngleTarget::get_ef_yaw() const { if (yaw_is_ef) { // target is already earth-frame @@ -808,7 +881,7 @@ float AP_Mount_Backend::MountTarget::get_ef_yaw() const } // sets roll, pitch, yaw and yaw_is_ef -void AP_Mount_Backend::MountTarget::set(const Vector3f& rpy, bool yaw_is_ef_in) +void AP_Mount_Backend::MountAngleTarget::set(const Vector3f& rpy, bool yaw_is_ef_in) { roll = rpy.x; pitch = rpy.y; @@ -819,7 +892,7 @@ void AP_Mount_Backend::MountTarget::set(const Vector3f& rpy, bool yaw_is_ef_in) // update angle targets using a given rate target // the resulting angle_rad yaw frame will match the rate_rad yaw frame // assumes a 50hz update rate -void AP_Mount_Backend::update_angle_target_from_rate(const MountTarget& rate_rad, MountTarget& angle_rad) const +void AP_Mount_Backend::update_angle_target_from_rate(const MountRateTarget& rate_rad, MountAngleTarget& angle_rad) const { // update roll and pitch angles and apply limits angle_rad.roll = constrain_float(angle_rad.roll + rate_rad.roll * AP_MOUNT_UPDATE_DT, radians(_params.roll_angle_min), radians(_params.roll_angle_max)); @@ -865,6 +938,10 @@ uint16_t AP_Mount_Backend::get_gimbal_device_flags() const case MountTargetType::ANGLE: yaw_lock_state = mnt_target.angle_rad.yaw_is_ef; break; + case MountTargetType::RETRACTED: + case MountTargetType::NEUTRAL: + yaw_lock_state = false; // not locked onto the scenery + break; } break; case MAV_MOUNT_MODE_RC_TARGETING: @@ -882,8 +959,8 @@ uint16_t AP_Mount_Backend::get_gimbal_device_flags() const break; } - const uint16_t flags = (get_mode() == MAV_MOUNT_MODE_RETRACT ? GIMBAL_DEVICE_FLAGS_RETRACT : 0) | - (get_mode() == MAV_MOUNT_MODE_NEUTRAL ? GIMBAL_DEVICE_FLAGS_NEUTRAL : 0) | + const uint16_t flags = (mnt_target.target_type == MountTargetType::RETRACTED ? GIMBAL_DEVICE_FLAGS_RETRACT : 0) | + (mnt_target.target_type == MountTargetType::NEUTRAL ? GIMBAL_DEVICE_FLAGS_NEUTRAL : 0) | GIMBAL_DEVICE_FLAGS_ROLL_LOCK | // roll angle is always earth-frame GIMBAL_DEVICE_FLAGS_PITCH_LOCK| // pitch angle is always earth-frame, yaw_angle is always body-frame GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME | // yaw angle is always in vehicle-frame @@ -893,7 +970,7 @@ uint16_t AP_Mount_Backend::get_gimbal_device_flags() const // get angle targets (in radians) to home location // returns true on success, false on failure -bool AP_Mount_Backend::get_angle_target_to_home(MountTarget& angle_rad) const +bool AP_Mount_Backend::get_angle_target_to_home(MountAngleTarget& angle_rad) const { // exit immediately if home is not set if (!AP::ahrs().home_is_set()) { @@ -904,7 +981,7 @@ bool AP_Mount_Backend::get_angle_target_to_home(MountTarget& angle_rad) const // get angle targets (in radians) to a vehicle with sysid of _target_sysid // returns true on success, false on failure -bool AP_Mount_Backend::get_angle_target_to_sysid(MountTarget& angle_rad) const +bool AP_Mount_Backend::get_angle_target_to_sysid(MountAngleTarget& angle_rad) const { // exit immediately if sysid is not set or no location available if (!_target_sysid_location.initialised()) { @@ -916,35 +993,49 @@ bool AP_Mount_Backend::get_angle_target_to_sysid(MountTarget& angle_rad) const return get_angle_target_to_location(_target_sysid_location, angle_rad); } +// updates the mount target by calling the _update_mount_target and then adjusting to remove lean angles if roll and/or pitch angles locked: +// this effectively translates earth frame targets in those axes to body frame to allow using the RP lock Aux Func and FPV_LOCK mount option +// on stabilized gimbals that dont have that capability in their backends via mount commands +void AP_Mount_Backend::update_mnt_target() +{ + _update_mnt_target(); //does most of the work below + // now adjust mnt target if the gimbal requires removing vehicle lean angles from target to obtain body frame roll and pitch locks + if (apply_bf_roll_pitch_adjustments_in_rc_targeting()) { + adjust_mnt_target_if_RP_locked(); + } +} -// method for the mount backends to call to update mnt_target based on +// method for the mount backends to update mnt_target based on // the mount mode. Methods in here may be overridden by the derived -// class to customise behaviour -void AP_Mount_Backend::update_mnt_target() +// class to customise behaviour +void AP_Mount_Backend::_update_mnt_target() { // change to RC_TARGETING mode if RC input has changed set_rctargeting_on_rcinput_change(); switch (get_mode()) { - case MAV_MOUNT_MODE_RETRACT: { + case MAV_MOUNT_MODE_RETRACT: // move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode? - const Vector3f &angle_bf_target = _params.retract_angles.get(); - mnt_target.target_type = MountTargetType::ANGLE; - mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false); + mnt_target.target_type = MountTargetType::RETRACTED; return; - } - case MAV_MOUNT_MODE_NEUTRAL: { + case MAV_MOUNT_MODE_NEUTRAL: // move mount to a neutral position, typically pointing forward - const Vector3f &angle_bf_target = _params.neutral_angles.get(); - mnt_target.target_type = MountTargetType::ANGLE; - mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false); + mnt_target.target_type = MountTargetType::NEUTRAL; return; - } case MAV_MOUNT_MODE_MAVLINK_TARGETING: // point to the angles given by a mavlink message // mavlink targets are stored while handling the incoming message + + // set target rate to zero if we have not received rate + // command for a while + if ((mnt_target.target_type == MountTargetType::RATE) && + (AP_HAL::millis() - mnt_target.last_rate_request_ms > 3000)) { + mnt_target.rate_rads.roll = 0; + mnt_target.rate_rads.pitch = 0; + mnt_target.rate_rads.yaw = 0; + } return; case MAV_MOUNT_MODE_RC_TARGETING: @@ -979,6 +1070,69 @@ void AP_Mount_Backend::update_mnt_target() INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } +void AP_Mount_Backend::send_target_to_gimbal() +{ + // the easy case, where the gimbal natively supports the MntTargetType: + if (natively_supports(mnt_target.target_type)) { + switch (mnt_target.target_type) { + case MountTargetType::ANGLE: + send_target_angles(mnt_target.angle_rad); + return; + case MountTargetType::RATE: + send_target_rates(mnt_target.rate_rads); + return; + case MountTargetType::RETRACTED: + send_target_retracted(); + return; + case MountTargetType::NEUTRAL: + send_target_neutral(); + return; + } + return; // should not reach this as all cases return + } + + // the more difficult case where we need to convert to something + // the gimbal understands: + switch (mnt_target.target_type) { + case MountTargetType::ANGLE: + // we don't know how to convert ANGLE to anything else. Note + // that the Siyi backend *does* convert angles to rates, so we + // could potentially swipe code into here. + break; + case MountTargetType::RATE: + if (natively_supports(MountTargetType::ANGLE)) { + // we integrate the rates into the angle: + update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad); + send_target_angles(mnt_target.angle_rad); + return; + } + break; + case MountTargetType::RETRACTED: + if (natively_supports(MountTargetType::ANGLE)) { + // just use the parameter values + // we update mnt_target for reporting purposes + const Vector3f &angle_bf_target = _params.retract_angles.get(); + mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false); + send_target_angles(mnt_target.angle_rad); + return; + } + break; + case MountTargetType::NEUTRAL: + if (natively_supports(MountTargetType::ANGLE)) { + // just use the parameter values + // we update mnt_target for reporting purposes + const Vector3f &angle_bf_target = _params.neutral_angles.get(); + mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false); + send_target_angles(mnt_target.angle_rad); + return; + } + break; + } + + send_warning_to_GCS("Failed to convert mount target to command gimbal"); +} + + // get target rate in deg/sec. returns true on success bool AP_Mount_Backend::get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame) { diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 2aa2339e2e7a1..bfcd8d96cd9c3 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -83,7 +83,7 @@ class AP_Mount_Backend // set yaw_lock used in RC_TARGETING mode. If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North) // If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle - void set_yaw_lock(bool yaw_lock) { _yaw_lock = yaw_lock; } + void set_yaw_lock(bool yaw_lock); // set pitch_lock used in RC_TARGETING mode. If true, the gimbal's tilt target is maintained in earth-frame meaning it will lock onto an earth-frame // If false (aka "follow") the gimbal's tilt is maintained in body-frame meaning it will tilt with the vehicle @@ -93,6 +93,17 @@ class AP_Mount_Backend // If false (aka "follow") the gimbal's tilt is maintained in body-frame meaning it will roll with the vehicle void set_roll_lock(bool roll_lock) { _roll_lock = roll_lock; } +#if AP_MOUNT_POI_LOCK_ENABLED + // set poi_lock to switch to GPS Targeting mode using current GPS point in gimbal's view or current saved poi and save entry mode for suspend function + void set_poi_lock(); + // clears poi_lock and reverts to default targeting mode + void clear_poi_lock(); + // reverts to saved poi entry mode but maintains poi location if set_poi_lock is called again without clearing it + void suspend_poi_lock(); + // check that poi_target has been set + bool roi_is_set() { return !_roi_target.is_zero(); } +#endif // AP_MOUNT_POI_LOCK_ENABLED + // set angle target in degrees // roll and pitch are in earth-frame // yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame @@ -228,12 +239,14 @@ class AP_Mount_Backend protected: enum class MountTargetType { - ANGLE, - RATE, + ANGLE = 0, + RATE = 1, + RETRACTED = 2, + NEUTRAL = 3, }; // class for a single angle or rate target - class MountTarget { + class MountAngleTarget { public: float roll; float pitch; @@ -251,6 +264,48 @@ class AP_Mount_Backend // set roll, pitch, yaw and yaw_is_ef from Vector3f void set(const Vector3f& rpy, bool yaw_is_ef_in); }; + class MountRateTarget { + public: + float roll; // roll rate in radians/second + float pitch; // roll rate in radians/second + float yaw; // roll rate in radians/second + bool yaw_is_ef; // if set then `yaw` is a rate in earth frame + }; + + // returns a bitmask of MountTargetTypes which this backend supports + // FIXME: make this pure-virtual + virtual uint8_t natively_supported_mount_target_types() const { return 0; }; + + // some static const masks to try to make the backends easier to read: + static constexpr uint8_t NATIVE_ANGLES_ONLY = (1U << uint8_t(MountTargetType::ANGLE)); + static constexpr uint8_t NATIVE_RATES_ONLY = (1U << uint8_t(MountTargetType::RATE)); + static constexpr uint8_t NATIVE_ANGLES_AND_RATES_ONLY = ( + 1U << uint8_t(MountTargetType::ANGLE) | + 1U << uint8_t(MountTargetType::RATE) + ); + + // returns true if the backend natively supports type. e.g. if + // "type" here is "MountTargetType::ANGLE" and the backend has + // indicated support of angles in the + // natively_supported_mount_target_types() bitmask then we can can + // send_angle_targets on the backend and have that work (meaning + // the backend has overrriden that method). + bool natively_supports(MountTargetType type) const { + uint8_t supported = natively_supported_mount_target_types(); + return (supported & (1U<<(uint8_t(type)))) != 0; + } + + // look at what MountTargetTypes this mount supports; convert to + // supported mount target type from unsupported mount target type + // if required and then send commands to gimbal via virtual + // methods: + void send_target_to_gimbal(); + + // FIXME: make it an internal error for these to ever be called: + virtual void send_target_angles(const MountAngleTarget &angle_rad) { } + virtual void send_target_rates(const MountRateTarget &rate_rads) { } + virtual void send_target_retracted() { } + virtual void send_target_neutral() { } // options parameter bitmask handling enum class Options : uint8_t { @@ -262,11 +317,26 @@ class AP_Mount_Backend // called when mount mode is RC-targetting, updates the mnt_target object from RC inputs: void update_mnt_target_from_rc_target(); + + //called to remove lean angle in roll/pitch mount angle to convert to bodyframe + void adjust_mnt_target_if_RP_locked(); + + // if this function returns true, then when roll and pitch have + // been locked to body frame (e.g. the user is using a switch or mount option to + // lock them to body frame) we add in the vehicle's roll and + // pitch to the target angles to position the gimbal in body + // frame. This is used on mounts which do not have the option to + // move in body frame themselves (e.g. SToRM32 in its normal + // configuration) and can be overriden in their heading files + virtual bool apply_bf_roll_pitch_adjustments_in_rc_targeting() const { + return false; + } // method for the mount backends to call to update mnt_target based on // the mount mode. Methods in here may be overridden by the derived // class to customise behaviour void update_mnt_target(); + void _update_mnt_target(); // returns true if user has configured a valid roll angle range // allows user to disable roll even on 3-axis gimbal @@ -289,20 +359,20 @@ class AP_Mount_Backend // get angle targets (in radians) to ROI location // returns true on success, false on failure - bool get_angle_target_to_roi(MountTarget& angle_rad) const WARN_IF_UNUSED; + bool get_angle_target_to_roi(MountAngleTarget& angle_rad) const WARN_IF_UNUSED; // get angle targets (in radians) to home location // returns true on success, false on failure - bool get_angle_target_to_home(MountTarget& angle_rad) const WARN_IF_UNUSED; + bool get_angle_target_to_home(MountAngleTarget& angle_rad) const WARN_IF_UNUSED; // get angle targets (in radians) to a vehicle with sysid of _target_sysid // returns true on success, false on failure - bool get_angle_target_to_sysid(MountTarget& angle_rad) const WARN_IF_UNUSED; + bool get_angle_target_to_sysid(MountAngleTarget& angle_rad) const WARN_IF_UNUSED; // update angle targets using a given rate target // the resulting angle_rad yaw frame will match the rate_rad yaw frame // assumes a 50hz update rate - void update_angle_target_from_rate(const MountTarget& rate_rad, MountTarget& angle_rad) const; + void update_angle_target_from_rate(const MountRateTarget& rate_rad, MountAngleTarget& angle_rad) const; // helper function to provide GIMBAL_DEVICE_FLAGS for use in GIMBAL_DEVICE_ATTITUDE_STATUS message uint16_t get_gimbal_device_flags() const; @@ -319,9 +389,10 @@ class AP_Mount_Backend // structure for MAVLink Targeting angle and rate targets struct { MountTargetType target_type;// MAVLink targeting mode's current target type (e.g. angle or rate) - MountTarget angle_rad; // angle target in radians - MountTarget rate_rads; // rate target in rad/s + MountAngleTarget angle_rad; // angle target in radians + MountRateTarget rate_rads; // rate target in rad/s uint32_t last_rate_request_ms; + uint32_t poi_start_ms; // time we started trying to find the gimbal POI for an AuxFunc::MOUNT_POI_LOCK } mnt_target; // RP earth frame locks accessible by backend @@ -334,13 +405,9 @@ class AP_Mount_Backend // get pilot input (in the range -1 to +1) received through RC void get_rc_input(float& roll_in, float& pitch_in, float& yaw_in) const; - // get angle or rate targets from pilot RC - // target_type will be either ANGLE or RATE, rpy will be the target angle in deg or rate in deg/s - void get_rc_target(MountTargetType& target_type, MountTarget& rpy) const; - // get angle targets (in radians) to a Location // returns true on success, false on failure - bool get_angle_target_to_location(const Location &loc, MountTarget& angle_rad) const WARN_IF_UNUSED; + bool get_angle_target_to_location(const Location &loc, MountAngleTarget& angle_rad) const WARN_IF_UNUSED; #if AP_MOUNT_POI_TO_LATLONALT_ENABLED // calculate the Location that the gimbal is pointing at @@ -349,6 +416,8 @@ class AP_Mount_Backend bool _yaw_lock; // yaw_lock used in RC_TARGETING mode. True if the gimbal's yaw target is maintained in earth-frame, if false (aka "follow") it is maintained in body-frame + float _yaw_lock_heading_rad; // mount earth frame direction captured upon calling set_yaw_lock + #if AP_MOUNT_POI_TO_LATLONALT_ENABLED struct { HAL_Semaphore sem; // semaphore protecting this structure @@ -362,6 +431,14 @@ class AP_Mount_Backend Location _roi_target; // roi target location +#if AP_MOUNT_POI_LOCK_ENABLED + void update_poi_lock_target(); + + // mount mode saved here entering poi lock for + // switching poi lock back to previous mode with aux function middle position + MAV_MOUNT_MODE saved_mount_mode = MAV_MOUNT_MODE_ENUM_END; +#endif // AP_MOUNT_POI_LOCK_ENABLED + uint8_t _target_sysid; // sysid to track Location _target_sysid_location;// sysid target location diff --git a/libraries/AP_Mount/AP_Mount_CADDX.cpp b/libraries/AP_Mount/AP_Mount_CADDX.cpp index c481aac062825..ff051ef0f0629 100644 --- a/libraries/AP_Mount/AP_Mount_CADDX.cpp +++ b/libraries/AP_Mount/AP_Mount_CADDX.cpp @@ -5,7 +5,6 @@ #include "AP_Mount_CADDX.h" #include -#define AP_MOUNT_CADDX_RESEND_MS 1000 // resend angle targets to gimbal once per second #define SET_ATTITUDE_HEADER1 0xA5 #define SET_ATTITUDE_HEADER2 0x5A #define SET_ATTITUDE_BUF_SIZE 10 @@ -22,83 +21,10 @@ void AP_Mount_CADDX::update() return; } - // change to RC_TARGETING mode if RC input has changed - set_rctargeting_on_rcinput_change(); - - // flag to trigger sending target angles to gimbal - bool resend_now = false; - - // update based on mount mode - switch (get_mode()) { - // move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode? - case MAV_MOUNT_MODE_RETRACT: { - const Vector3f &target = _params.retract_angles.get(); - mnt_target.angle_rad.set(target*DEG_TO_RAD, false); - mnt_target.target_type = MountTargetType::ANGLE; - break; - } - - // move mount to a neutral position, typically pointing forward - case MAV_MOUNT_MODE_NEUTRAL: { - const Vector3f &target = _params.neutral_angles.get(); - mnt_target.angle_rad.set(target*DEG_TO_RAD, false); - mnt_target.target_type = MountTargetType::ANGLE; - break; - } - - // point to the angles given by a mavlink message - case MAV_MOUNT_MODE_MAVLINK_TARGETING: - // mnt_target should have already been filled in by set_angle_target() or set_rate_target() - resend_now = true; - break; - - // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - update_mnt_target_from_rc_target(); - resend_now = true; - break; - } - - // point mount to a GPS point given by the mission planner - case MAV_MOUNT_MODE_GPS_POINT: - if (get_angle_target_to_roi(mnt_target.angle_rad)) { - mnt_target.target_type = MountTargetType::ANGLE; - resend_now = true; - } - break; - - // point mount to Home location - case MAV_MOUNT_MODE_HOME_LOCATION: - if (get_angle_target_to_home(mnt_target.angle_rad)) { - mnt_target.target_type = MountTargetType::ANGLE; - resend_now = true; - } - break; - - // point mount to another vehicle - case MAV_MOUNT_MODE_SYSID_TARGET: - if (get_angle_target_to_sysid(mnt_target.angle_rad)) { - mnt_target.target_type = MountTargetType::ANGLE; - resend_now = true; - } - break; - - default: - // we do not know this mode so do nothing - break; - } - - // update angle targets from angle rates - if (mnt_target.target_type == MountTargetType::RATE) { - update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad); - } + AP_Mount_Backend::update_mnt_target(); - // resend target angles at least once per second - resend_now = resend_now || ((AP_HAL::millis() - _last_send_ms) > AP_MOUNT_CADDX_RESEND_MS); - if (resend_now) { - send_target_angles(mnt_target.angle_rad); - } + // send target angles (which may be derived from other target types) + AP_Mount_Backend::send_target_to_gimbal(); } // get attitude as a quaternion. returns true on success @@ -110,7 +36,7 @@ bool AP_Mount_CADDX::get_attitude_quaternion(Quaternion& att_quat) } // send_target_angles -void AP_Mount_CADDX::send_target_angles(const MountTarget& angle_target_rad) +void AP_Mount_CADDX::send_target_angles(const MountAngleTarget& angle_target_rad) { // exit immediately if not initialised if (!_initialised) { @@ -127,7 +53,7 @@ void AP_Mount_CADDX::send_target_angles(const MountTarget& angle_target_rad) const uint16_t roll_target_cmd = constrain_uint16(wrap_2PI(angle_target_rad.roll) * scalar, AXIS_MIN, AXIS_MAX); const uint16_t pitch_target_cmd = constrain_uint16(wrap_2PI(angle_target_rad.pitch) * scalar, AXIS_MIN, AXIS_MAX); const uint16_t yaw_target_cmd = constrain_uint16(wrap_2PI(angle_target_rad.get_bf_yaw()) * scalar, AXIS_MIN, AXIS_MAX); - + // prepare packet to send to gimbal uint8_t set_attitude_cmd_buf[SET_ATTITUDE_BUF_SIZE] {}; @@ -173,9 +99,6 @@ void AP_Mount_CADDX::send_target_angles(const MountTarget& angle_target_rad) // send packet to gimbal _uart->write(set_attitude_cmd_buf, sizeof(set_attitude_cmd_buf)); - - // store time of send - _last_send_ms = AP_HAL::millis(); } #endif // HAL_MOUNT_CADDX_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_CADDX.h b/libraries/AP_Mount/AP_Mount_CADDX.h index 0407736d85209..28f9e7342b739 100644 --- a/libraries/AP_Mount/AP_Mount_CADDX.h +++ b/libraries/AP_Mount/AP_Mount_CADDX.h @@ -43,10 +43,12 @@ class AP_Mount_CADDX : public AP_Mount_Backend_Serial YAW_LOCK = (1<<2), }; - // send_target_angles - void send_target_angles(const MountTarget& angle_target_rad); + // CADDX can only send angles + uint8_t natively_supported_mount_target_types() const override { + return NATIVE_ANGLES_ONLY; + }; - // internal variables - uint32_t _last_send_ms; // system time of last do_mount_control sent to gimbal + // send_target_angles + void send_target_angles(const MountAngleTarget& angle_target_rad) override; }; #endif // HAL_MOUNT_CADDX_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_MAVLink.cpp index c17c8bb968248..909979eae082b 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.cpp +++ b/libraries/AP_Mount/AP_Mount_MAVLink.cpp @@ -25,23 +25,8 @@ void AP_Mount_MAVLink::update() update_mnt_target(); - // FIXME: create and use MountTargetType::RETRACTED - if (get_mode() == MAV_MOUNT_MODE_RETRACT) { - mnt_target.target_type = MountTargetType::ANGLE; - mnt_target.angle_rad.set(Vector3f{0,0,0}, false); - send_gimbal_device_retract(); - return; - } - - // send target angles or rates depending on the target type - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - send_gimbal_device_set_attitude(mnt_target.angle_rad.roll, mnt_target.angle_rad.pitch, mnt_target.angle_rad.yaw, mnt_target.angle_rad.yaw_is_ef); - break; - case MountTargetType::RATE: - send_gimbal_device_set_rate(mnt_target.rate_rads.roll, mnt_target.rate_rads.pitch, mnt_target.rate_rads.yaw, mnt_target.rate_rads.yaw_is_ef); - break; - } + // send target angles/rates/retract depending on the target type + send_target_to_gimbal(); } // return true if healthy @@ -215,8 +200,13 @@ bool AP_Mount_MAVLink::start_sending_attitude_to_gimbal() } // send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to command gimbal to retract (aka relax) -void AP_Mount_MAVLink::send_gimbal_device_retract() const +void AP_Mount_MAVLink::send_target_retracted() { + // update the target angles. These may be absolutely bogus, of + // course, but may be useful in logs to see what the gimbal was + // doing. This is also preserving existing behaviour in a change... + mnt_target.angle_rad.set(Vector3f{0,0,0}, false); + const mavlink_gimbal_device_set_attitude_t pkt { {NAN, NAN, NAN, NAN}, // attitude 0, // angular velocity x @@ -231,9 +221,13 @@ void AP_Mount_MAVLink::send_gimbal_device_retract() const } // send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to control rate -// earth_frame should be true if yaw_rads target is an earth frame rate, false if body_frame -void AP_Mount_MAVLink::send_gimbal_device_set_rate(float roll_rads, float pitch_rads, float yaw_rads, bool earth_frame) const +void AP_Mount_MAVLink::send_target_rates(const MountRateTarget &rate_rads) { + const float roll_rads = rate_rads.roll; + const float pitch_rads = rate_rads.pitch; + const float yaw_rads = rate_rads.yaw; + const bool earth_frame = rate_rads.yaw_is_ef; + // prepare flags const uint16_t flags = earth_frame ? (GIMBAL_DEVICE_FLAGS_ROLL_LOCK | GIMBAL_DEVICE_FLAGS_PITCH_LOCK | GIMBAL_DEVICE_FLAGS_YAW_LOCK) : 0; @@ -251,9 +245,13 @@ void AP_Mount_MAVLink::send_gimbal_device_set_rate(float roll_rads, float pitch_ } // send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to control attitude -// earth_frame should be true if yaw_rad target is in earth frame angle, false if body_frame -void AP_Mount_MAVLink::send_gimbal_device_set_attitude(float roll_rad, float pitch_rad, float yaw_rad, bool earth_frame) const +void AP_Mount_MAVLink::send_target_angles(const MountAngleTarget &angle_rad) { + const float roll_rad = angle_rad.roll; + const float pitch_rad = angle_rad.pitch; + const float yaw_rad = angle_rad.yaw; + const bool earth_frame = angle_rad.yaw_is_ef; + // exit immediately if not initialised if (!_initialised) { return; diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.h b/libraries/AP_Mount/AP_Mount_MAVLink.h index 691671285358a..c9d636db41088 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.h +++ b/libraries/AP_Mount/AP_Mount_MAVLink.h @@ -37,6 +37,16 @@ class AP_Mount_MAVLink : public AP_Mount_Backend protected: + // MVAVLink can send either rates or angles, and can also be + // directly commanded to move to a retracted state + uint8_t natively_supported_mount_target_types() const override { + return ( + (1U< AP_MOUNT_STORM32_RESEND_MS)) { - send_do_mount_control(mnt_target.angle_rad); - } + // send target angles (which may be derived from other target types) + AP_Mount_Backend::send_target_to_gimbal(); } // get attitude as a quaternion. returns true on success @@ -128,8 +57,8 @@ void AP_Mount_SToRM32::find_gimbal() } } -// send_do_mount_control - send a COMMAND_LONG containing a do_mount_control message -void AP_Mount_SToRM32::send_do_mount_control(const MountTarget& angle_target_rad) +// send_target_angles - send a COMMAND_LONG containing a do_mount_control message +void AP_Mount_SToRM32::send_target_angles(const MountAngleTarget& angle_target_rad) { // exit immediately if not initialised if (!_initialised) { @@ -153,8 +82,5 @@ void AP_Mount_SToRM32::send_do_mount_control(const MountTarget& angle_target_rad -degrees(angle_target_rad.get_bf_yaw()), 0, 0, 0, // param4 ~ param6 unused MAV_MOUNT_MODE_MAVLINK_TARGETING); - - // store time of send - _last_send = AP_HAL::millis(); } #endif // HAL_MOUNT_STORM32MAVLINK_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.h b/libraries/AP_Mount/AP_Mount_SToRM32.h index 6ff1a1ad1abaf..9ea963bc6e09d 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32.h +++ b/libraries/AP_Mount/AP_Mount_SToRM32.h @@ -35,14 +35,23 @@ class AP_Mount_SToRM32 : public AP_Mount_Backend // search for gimbal in GCS_MAVLink routing table void find_gimbal(); + // SToRM32 can only send angles: + uint8_t natively_supported_mount_target_types() const override { + return NATIVE_ANGLES_ONLY; + }; + + // allow removing lean angles for pitch and roll locks + bool apply_bf_roll_pitch_adjustments_in_rc_targeting() const override { + return true; + } + // send_do_mount_control with latest angle targets - void send_do_mount_control(const MountTarget& angle_target_rad); + void send_target_angles(const MountAngleTarget& angle_target_rad) override; // internal variables bool _initialised; // true once the driver has been initialised uint8_t _sysid; // sysid of gimbal uint8_t _compid; // component id of gimbal mavlink_channel_t _chan = MAVLINK_COMM_0; // mavlink channel used to communicate with gimbal - uint32_t _last_send; // system time of last do_mount_control sent to gimbal }; #endif // HAL_MOUNT_STORM32MAVLINK_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp index 44f607fce3771..67178db8589f3 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp @@ -20,96 +20,14 @@ void AP_Mount_SToRM32_serial::update() read_incoming(); // read the incoming messages from the gimbal - // change to RC_TARGETING mode if RC input has changed - set_rctargeting_on_rcinput_change(); - - // flag to trigger sending target angles to gimbal - bool resend_now = false; - - // update based on mount mode - switch(get_mode()) { - // move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode? - case MAV_MOUNT_MODE_RETRACT: { - const Vector3f &target = _params.retract_angles.get(); - mnt_target.angle_rad.set(target*DEG_TO_RAD, false); - mnt_target.target_type = MountTargetType::ANGLE; - break; - } - - // move mount to a neutral position, typically pointing forward - case MAV_MOUNT_MODE_NEUTRAL: { - const Vector3f &target = _params.neutral_angles.get(); - mnt_target.angle_rad.set(target*DEG_TO_RAD, false); - mnt_target.target_type = MountTargetType::ANGLE; - break; - } - - // point to the angles given by a mavlink message - case MAV_MOUNT_MODE_MAVLINK_TARGETING: - // mnt_target should have already been filled in by set_angle_target() or set_rate_target() - resend_now = true; - break; - - // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: - update_mnt_target_from_rc_target(); - resend_now = true; - break; - - // point mount to a GPS point given by the mission planner - case MAV_MOUNT_MODE_GPS_POINT: - if (get_angle_target_to_roi(mnt_target.angle_rad)) { - mnt_target.target_type = MountTargetType::ANGLE; - resend_now = true; - } - break; + AP_Mount_Backend::update_mnt_target(); - // point mount to Home location - case MAV_MOUNT_MODE_HOME_LOCATION: - if (get_angle_target_to_home(mnt_target.angle_rad)) { - mnt_target.target_type = MountTargetType::ANGLE; - resend_now = true; - } - break; + // send target angles (which may be derived from other target types) + AP_Mount_Backend::send_target_to_gimbal(); - // point mount to another vehicle - case MAV_MOUNT_MODE_SYSID_TARGET: - if (get_angle_target_to_sysid(mnt_target.angle_rad)) { - mnt_target.target_type = MountTargetType::ANGLE; - resend_now = true; - } - break; - - default: - // we do not know this mode so do nothing - break; - } - - // update angle targets from angle rates - if (mnt_target.target_type == MountTargetType::RATE) { - update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad); - } - - // resend target angles at least once per second - resend_now = resend_now || ((AP_HAL::millis() - _last_send) > AP_MOUNT_STORM32_SERIAL_RESEND_MS); - - if ((AP_HAL::millis() - _last_send) > AP_MOUNT_STORM32_SERIAL_RESEND_MS*2) { + if ((AP_HAL::millis() - _last_send) > AP_MOUNT_STORM32_SERIAL_TIMEOUT_MS) { _reply_type = ReplyType_UNKNOWN; } - if (can_send(resend_now)) { - if (resend_now) { - send_target_angles(mnt_target.angle_rad); - get_angles(); - _reply_type = ReplyType_ACK; - _reply_counter = 0; - _reply_length = get_reply_size(_reply_type); - } else { - get_angles(); - _reply_type = ReplyType_DATA; - _reply_counter = 0; - _reply_length = get_reply_size(_reply_type); - } - } } // get attitude as a quaternion. returns true on success @@ -119,18 +37,19 @@ bool AP_Mount_SToRM32_serial::get_attitude_quaternion(Quaternion& att_quat) return true; } -bool AP_Mount_SToRM32_serial::can_send(bool with_control) { +bool AP_Mount_SToRM32_serial::can_send() { uint16_t required_tx = 1; - if (with_control) { - required_tx += sizeof(AP_Mount_SToRM32_serial::cmd_set_angles_struct); - } + required_tx += sizeof(AP_Mount_SToRM32_serial::cmd_set_angles_struct); return (_reply_type == ReplyType_UNKNOWN) && (_uart->txspace() >= required_tx); } // send_target_angles -void AP_Mount_SToRM32_serial::send_target_angles(const MountTarget& angle_target_rad) +void AP_Mount_SToRM32_serial::send_target_angles(const MountAngleTarget& angle_target_rad) { + if (!can_send()) { + return; + } static cmd_set_angles_struct cmd_set_angles_data = { 0xFA, @@ -168,6 +87,17 @@ void AP_Mount_SToRM32_serial::send_target_angles(const MountTarget& angle_target // store time of send _last_send = AP_HAL::millis(); + + // we pipeline commands. We have sent in a command to set angles, + // now fetch data from the device: + get_angles(); + // we expect an ACK back for the set-angles command. A state + // machine in read_incoming will move us to ReplyType_DATA once + // the ACK has been received. + _reply_type = ReplyType_ACK; + _reply_counter = 0; + _reply_length = get_reply_size(_reply_type); + } void AP_Mount_SToRM32_serial::get_angles() { diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.h b/libraries/AP_Mount/AP_Mount_SToRM32_serial.h index 07b3d3d165de8..5c19d3f2c4db7 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.h +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.h @@ -13,7 +13,9 @@ #include #include -#define AP_MOUNT_STORM32_SERIAL_RESEND_MS 1000 // resend angle targets to gimbal once per second +// timeout for no-response-received (gimbal only sends data when we +// send it data first) +#define AP_MOUNT_STORM32_SERIAL_TIMEOUT_MS 2000 class AP_Mount_SToRM32_serial : public AP_Mount_Backend_Serial { @@ -35,8 +37,18 @@ class AP_Mount_SToRM32_serial : public AP_Mount_Backend_Serial private: + // SToRM32-serial can only send angles + uint8_t natively_supported_mount_target_types() const override { + return NATIVE_ANGLES_ONLY; + }; + + // allow removing lean angles for pitch and roll locks + bool apply_bf_roll_pitch_adjustments_in_rc_targeting() const override { + return true; + } + // send_target_angles - void send_target_angles(const MountTarget& angle_target_rad); + void send_target_angles(const MountAngleTarget& angle_target_rad) override; // send read data request void get_angles(); @@ -53,7 +65,7 @@ class AP_Mount_SToRM32_serial : public AP_Mount_Backend_Serial //void add_next_reply(ReplyType reply_type); uint8_t get_reply_size(ReplyType reply_type); - bool can_send(bool with_control); + bool can_send(); struct PACKED SToRM32_reply_data_struct { uint16_t state; diff --git a/libraries/AP_Mount/AP_Mount_Scripting.h b/libraries/AP_Mount/AP_Mount_Scripting.h index 6e43da402b71c..e16bc0e411c69 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.h +++ b/libraries/AP_Mount/AP_Mount_Scripting.h @@ -38,6 +38,13 @@ class AP_Mount_Scripting : public AP_Mount_Backend protected: + // Scripting doesn't actually send anything (the script polls the + // library for the targets) + uint8_t natively_supported_mount_target_types() const override { + return NATIVE_ANGLES_ONLY; + }; + void send_target_angles(const MountAngleTarget &angle_rad) override {}; + // get attitude as a quaternion. returns true on success bool get_attitude_quaternion(Quaternion& att_quat) override; diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index ac4f5e45939d0..c6a44f9f532eb 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -30,76 +30,16 @@ void AP_Mount_Servo::update() { AP_Mount_Backend::update(); - // change to RC_TARGETING mode if RC input has changed - set_rctargeting_on_rcinput_change(); - - auto mount_mode = get_mode(); - switch (mount_mode) { - // move mount to a "retracted position" or to a position where a fourth servo can retract the entire mount into the fuselage - case MAV_MOUNT_MODE_RETRACT: { - _angle_bf_output_rad = _params.retract_angles.get() * DEG_TO_RAD; - mnt_target.angle_rad.set(_angle_bf_output_rad, false); - mnt_target.target_type = MountTargetType::ANGLE; - break; - } - - // move mount to a neutral position, typically pointing forward - case MAV_MOUNT_MODE_NEUTRAL: { - _angle_bf_output_rad = _params.neutral_angles.get() * DEG_TO_RAD; - mnt_target.angle_rad.set(_angle_bf_output_rad, false); - mnt_target.target_type = MountTargetType::ANGLE; - break; - } - - // point to the angles given by a mavlink message - case MAV_MOUNT_MODE_MAVLINK_TARGETING: { - // mavlink targets are stored while handling the incoming message and considered valid - break; - } - - // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: - update_mnt_target_from_rc_target(); - break; - - // point mount to a GPS point given by the mission planner - case MAV_MOUNT_MODE_GPS_POINT: - if (get_angle_target_to_roi(mnt_target.angle_rad)) { - mnt_target.target_type = MountTargetType::ANGLE; - } - break; - - // point mount to Home location - case MAV_MOUNT_MODE_HOME_LOCATION: - if (get_angle_target_to_home(mnt_target.angle_rad)) { - mnt_target.target_type = MountTargetType::ANGLE; - } - break; - - // point mount to another vehicle - case MAV_MOUNT_MODE_SYSID_TARGET: - if (get_angle_target_to_sysid(mnt_target.angle_rad)) { - mnt_target.target_type = MountTargetType::ANGLE; - } - break; - - default: - //do nothing - break; - } + update_mnt_target(); - // send target angles or rates depending on the target type - switch (mnt_target.target_type) { - case MountTargetType::RATE: - update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad); - FALLTHROUGH; - case MountTargetType::ANGLE: - // update _angle_bf_output_rad based on angle target - if ((mount_mode != MAV_MOUNT_MODE_RETRACT) && (mount_mode != MAV_MOUNT_MODE_NEUTRAL)) { - update_angle_outputs(mnt_target.angle_rad); - } - break; - } + // have our base class call send_target_angles to command the gimbal: + send_target_to_gimbal(); +} + +// called by the backend to set the servo angles: +void AP_Mount_Servo::send_target_angles(const MountAngleTarget& angle_rad) +{ + update_angle_outputs(mnt_target.angle_rad); // write the results to the servos move_servo(_roll_idx, degrees(_angle_bf_output_rad.x)*10, _params.roll_angle_min*10, _params.roll_angle_max*10); @@ -151,7 +91,7 @@ bool AP_Mount_Servo::get_attitude_quaternion(Quaternion& att_quat) // private methods // update body-frame angle outputs from earth-frame angle targets -void AP_Mount_Servo::update_angle_outputs(const MountTarget& angle_rad) +void AP_Mount_Servo::update_angle_outputs(const MountAngleTarget& angle_rad) { const AP_AHRS &ahrs = AP::ahrs(); @@ -163,8 +103,20 @@ void AP_Mount_Servo::update_angle_outputs(const MountTarget& angle_rad) _angle_bf_output_rad.y = angle_rad.pitch; _angle_bf_output_rad.z = yaw_bf_rad; - // this is sufficient for self-stabilising brushless gimbals + // do no stabilization in retract or neutral: + switch (mnt_target.target_type) { + case MountTargetType::NEUTRAL: + case MountTargetType::RETRACTED: + return; + case MountTargetType::ANGLE: + case MountTargetType::RATE: + break; + } + + // only have to adjust roll/pitch for body frame in self-stabilising brushless gimbals if (!requires_stabilization) { + //since this is a shared backend, must call this directly + AP_Mount_Backend::adjust_mnt_target_if_RP_locked(); return; } @@ -176,9 +128,14 @@ void AP_Mount_Servo::update_angle_outputs(const MountTarget& angle_rad) ahrs_angle_rad.rotate(-yaw_bf_rad); } - // add roll and pitch lean angle correction - _angle_bf_output_rad.x -= ahrs_angle_rad.x; - _angle_bf_output_rad.y -= ahrs_angle_rad.y; + // add roll and pitch lean angle correction for earth frame + if (angle_rad.roll_is_ef){ + _angle_bf_output_rad.x -= ahrs_angle_rad.x; + } + + if (angle_rad.pitch_is_ef){ + _angle_bf_output_rad.y -= ahrs_angle_rad.y; + } // lead filter const Vector3f &gyro = ahrs.get_gyro(); diff --git a/libraries/AP_Mount/AP_Mount_Servo.h b/libraries/AP_Mount/AP_Mount_Servo.h index b008649be7408..c53240602df24 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.h +++ b/libraries/AP_Mount/AP_Mount_Servo.h @@ -45,10 +45,18 @@ class AP_Mount_Servo : public AP_Mount_Backend // get attitude as a quaternion. returns true on success bool get_attitude_quaternion(Quaternion& att_quat) override; + // servo only natively supports angles: + uint8_t natively_supported_mount_target_types() const override { + return NATIVE_ANGLES_ONLY; + }; + private: + // called by the backend to set the servo angles: + void send_target_angles(const MountAngleTarget& angle_rad) override; + // update body-frame angle outputs from earth-frame targets - void update_angle_outputs(const MountTarget& angle_rad); + void update_angle_outputs(const MountAngleTarget& angle_rad); /// moves servo with the given function id to the specified angle. all angles are in body-frame and degrees * 10 void move_servo(uint8_t rc, int16_t angle, int16_t angle_min, int16_t angle_max); diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index de9402efcb422..74578d272173a 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -106,14 +106,7 @@ void AP_Mount_Siyi::update() update_mnt_target(); // send target angles or rates depending on the target type - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - send_target_angles(mnt_target.angle_rad.pitch, mnt_target.angle_rad.yaw, mnt_target.angle_rad.yaw_is_ef); - break; - case MountTargetType::RATE: - send_target_rates(mnt_target.rate_rads.pitch, mnt_target.rate_rads.yaw, mnt_target.rate_rads.yaw_is_ef); - break; - } + send_target_to_gimbal(); } // return true if healthy @@ -638,18 +631,24 @@ bool AP_Mount_Siyi::set_motion_mode(const GimbalMotionMode mode, const bool forc } // send target pitch and yaw rates to gimbal -// yaw_is_ef should be true if yaw_rads target is an earth frame rate, false if body_frame -void AP_Mount_Siyi::send_target_rates(float pitch_rads, float yaw_rads, bool yaw_is_ef) +void AP_Mount_Siyi::send_target_rates(const MountRateTarget &rate_rads) { + const float pitch_rads = rate_rads.pitch; + const float yaw_rads = rate_rads.yaw; + const bool yaw_is_ef = rate_rads.yaw_is_ef; + const float pitch_rate_scalar = constrain_float(100.0 * pitch_rads / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100); const float yaw_rate_scalar = constrain_float(100.0 * yaw_rads / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100); rotate_gimbal(pitch_rate_scalar, yaw_rate_scalar, yaw_is_ef); } // send target pitch and yaw angles to gimbal -// yaw_is_ef should be true if yaw_rad target is an earth frame angle, false if body_frame -void AP_Mount_Siyi::send_target_angles(float pitch_rad, float yaw_rad, bool yaw_is_ef) +void AP_Mount_Siyi::send_target_angles(const MountAngleTarget &angle_rad) { + const float pitch_rad = angle_rad.pitch; + const float yaw_rad = angle_rad.yaw; + bool yaw_is_ef = angle_rad.yaw_is_ef; + // stop gimbal if no recent actual angles uint32_t now_ms = AP_HAL::millis(); if (now_ms - _last_current_angle_rad_ms >= 200) { diff --git a/libraries/AP_Mount/AP_Mount_Siyi.h b/libraries/AP_Mount/AP_Mount_Siyi.h index 7f7f280b224aa..26fe0a9897611 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.h +++ b/libraries/AP_Mount/AP_Mount_Siyi.h @@ -282,13 +282,16 @@ class AP_Mount_Siyi : public AP_Mount_Backend_Serial // Returns true if message successfully sent to Gimbal bool set_motion_mode(const GimbalMotionMode mode, const bool force=false); + // Siyi can send either rates or angles + uint8_t natively_supported_mount_target_types() const override { + return NATIVE_ANGLES_AND_RATES_ONLY; + }; + // send target pitch and yaw rates to gimbal - // yaw_is_ef should be true if yaw_rads target is an earth frame rate, false if body_frame - void send_target_rates(float pitch_rads, float yaw_rads, bool yaw_is_ef); + void send_target_rates(const MountRateTarget &rate_rads) override; // send target pitch and yaw angles to gimbal - // yaw_is_ef should be true if yaw_rad target is an earth frame angle, false if body_frame - void send_target_angles(float pitch_rad, float yaw_rad, bool yaw_is_ef); + void send_target_angles(const MountAngleTarget &angle_rad) override; // send zoom rate command to camera. zoom out = -1, hold = 0, zoom in = 1 bool send_zoom_rate(float zoom_value); diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp index d4316714c8d03..c9a14de6b3b6e 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp @@ -61,9 +61,6 @@ void AP_Mount_SoloGimbal::update() case MAV_MOUNT_MODE_MAVLINK_TARGETING: // targets are stored while handling the incoming mavlink message _gimbal.set_lockedToBody(false); - if (mnt_target.target_type == MountTargetType::RATE) { - update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad); - } break; // RC radio manual angle control, but with stabilization from the AHRS @@ -100,6 +97,10 @@ void AP_Mount_SoloGimbal::update() // we do not know this mode so do nothing break; } + + if (mnt_target.target_type == MountTargetType::RATE) { + update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad); + } } // get attitude as a quaternion. returns true on success diff --git a/libraries/AP_Mount/AP_Mount_Topotek.cpp b/libraries/AP_Mount/AP_Mount_Topotek.cpp index 7fb09b2ec95ae..42079c24114ff 100644 --- a/libraries/AP_Mount/AP_Mount_Topotek.cpp +++ b/libraries/AP_Mount/AP_Mount_Topotek.cpp @@ -137,14 +137,7 @@ void AP_Mount_Topotek::update() update_mnt_target(); // send target angles or rates depending on the target type - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - send_angle_target(mnt_target.angle_rad); - break; - case MountTargetType::RATE: - send_rate_target(mnt_target.rate_rads); - break; - } + send_target_to_gimbal(); } // return true if healthy @@ -739,7 +732,7 @@ void AP_Mount_Topotek::request_gimbal_model_name() } // send angle target in radians to gimbal -void AP_Mount_Topotek::send_angle_target(const MountTarget& angle_rad) +void AP_Mount_Topotek::send_target_angles(const MountAngleTarget& angle_rad) { // gimbal's earth-frame angle control drifts so always use body frame // set gimbal's lock state if it has changed @@ -783,7 +776,7 @@ void AP_Mount_Topotek::send_angle_target(const MountTarget& angle_rad) } // send rate target in rad/s to gimbal -void AP_Mount_Topotek::send_rate_target(const MountTarget& rate_rads) +void AP_Mount_Topotek::send_target_rates(const MountRateTarget& rate_rads) { // set gimbal's lock state if it has changed if (!set_gimbal_lock(rate_rads.yaw_is_ef)) { @@ -967,6 +960,9 @@ void AP_Mount_Topotek::gimbal_track_analyse() GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s started", send_message_prefix, tracking_str); _is_tracking = true; break; + case TrackingStatus::LENS_UNSUPPORT_TRACK: + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s unsupported lens", send_message_prefix, tracking_str); + break; } } diff --git a/libraries/AP_Mount/AP_Mount_Topotek.h b/libraries/AP_Mount/AP_Mount_Topotek.h index 9c8882936c758..eebcee96645fe 100644 --- a/libraries/AP_Mount/AP_Mount_Topotek.h +++ b/libraries/AP_Mount/AP_Mount_Topotek.h @@ -106,6 +106,11 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial // get attitude as a quaternion. returns true on success bool get_attitude_quaternion(Quaternion& att_quat) override; + // Topotek can send either rates or angles + uint8_t natively_supported_mount_target_types() const override { + return NATIVE_ANGLES_AND_RATES_ONLY; + }; + private: // header type (fixed or variable length) @@ -154,7 +159,8 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial enum class TrackingStatus : uint8_t { STOPPED_TRACKING = 0x30, // not tracking WAITING_FOR_TRACKING = 0x31, // wait to track command status - TRACKING_IN_PROGRESS = 0x32 // the status is being tracked. + TRACKING_IN_PROGRESS = 0x32, // the status is being tracked + LENS_UNSUPPORT_TRACK = 0x34, // this lens does not support tracking }; // identifier bytes @@ -182,10 +188,10 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial void request_gimbal_model_name(); // send angle target in radians to gimbal - void send_angle_target(const MountTarget& angle_rad); + void send_target_angles(const MountAngleTarget& angle_rad) override; // send rate target in rad/s to gimbal - void send_rate_target(const MountTarget& rate_rads); + void send_target_rates(const MountRateTarget& rate_rads) override; // send time and date to gimbal bool send_time_to_gimbal(); diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.cpp b/libraries/AP_Mount/AP_Mount_Viewpro.cpp index a741ed9bce3a5..d8f752ffd6574 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.cpp +++ b/libraries/AP_Mount/AP_Mount_Viewpro.cpp @@ -77,14 +77,7 @@ void AP_Mount_Viewpro::update() update_mnt_target(); // send target angles or rates depending on the target type - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - send_target_angles(mnt_target.angle_rad.pitch, mnt_target.angle_rad.yaw, mnt_target.angle_rad.yaw_is_ef); - break; - case MountTargetType::RATE: - send_target_rates(mnt_target.rate_rads.pitch, mnt_target.rate_rads.yaw, mnt_target.rate_rads.yaw_is_ef); - break; - } + send_target_to_gimbal(); } // return true if healthy @@ -444,12 +437,15 @@ bool AP_Mount_Viewpro::send_comm_config_cmd(CommConfigCmd cmd) } // send target pitch and yaw rates to gimbal -// yaw_is_ef should be true if yaw_rads target is an earth frame rate, false if body_frame -bool AP_Mount_Viewpro::send_target_rates(float pitch_rads, float yaw_rads, bool yaw_is_ef) +void AP_Mount_Viewpro::send_target_rates(const MountRateTarget &rate_rads) { + const float pitch_rads = rate_rads.pitch; + const float yaw_rads = rate_rads.yaw; + const bool yaw_is_ef = rate_rads.yaw_is_ef; + // set lock value if (!set_lock(yaw_is_ef)) { - return false; + return; } // scale pitch and yaw to values gimbal understands @@ -467,16 +463,19 @@ bool AP_Mount_Viewpro::send_target_rates(float pitch_rads, float yaw_rads, bool }; // send targets to gimbal - return send_packet(a1_packet.bytes, sizeof(a1_packet.bytes)); + send_packet(a1_packet.bytes, sizeof(a1_packet.bytes)); } // send target pitch and yaw angles to gimbal -// yaw_is_ef should be true if yaw_rad target is an earth frame angle, false if body_frame -bool AP_Mount_Viewpro::send_target_angles(float pitch_rad, float yaw_rad, bool yaw_is_ef) +void AP_Mount_Viewpro::send_target_angles(const MountAngleTarget &angle_rad) { + const float pitch_rad = angle_rad.pitch; + const float yaw_rad = angle_rad.yaw; + bool yaw_is_ef = angle_rad.yaw_is_ef; + // gimbal does not support lock in angle control mode if (!set_lock(false)) { - return false; + return; } // convert yaw angle to body-frame @@ -505,7 +504,7 @@ bool AP_Mount_Viewpro::send_target_angles(float pitch_rad, float yaw_rad, bool y }; // send targets to gimbal - return send_packet(a1_packet.bytes, sizeof(a1_packet.bytes)); + send_packet(a1_packet.bytes, sizeof(a1_packet.bytes)); } // send camera command, affected image sensor and value (e.g. zoom speed) diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.h b/libraries/AP_Mount/AP_Mount_Viewpro.h index d9e70df593c9f..45d55a69712da 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.h +++ b/libraries/AP_Mount/AP_Mount_Viewpro.h @@ -99,6 +99,11 @@ class AP_Mount_Viewpro : public AP_Mount_Backend_Serial protected: + // Viewpro can send either rates or angles + uint8_t natively_supported_mount_target_types() const override { + return NATIVE_ANGLES_AND_RATES_ONLY; + }; + // get attitude as a quaternion. returns true on success bool get_attitude_quaternion(Quaternion& att_quat) override; @@ -356,12 +361,10 @@ class AP_Mount_Viewpro : public AP_Mount_Backend_Serial bool send_comm_config_cmd(CommConfigCmd cmd); // send target pitch and yaw rates to gimbal - // yaw_is_ef should be true if yaw_rads target is an earth frame rate, false if body_frame - bool send_target_rates(float pitch_rads, float yaw_rads, bool yaw_is_ef); + void send_target_rates(const MountRateTarget &rate_rads) override; // send target pitch and yaw angles to gimbal - // yaw_is_ef should be true if yaw_rad target is an earth frame angle, false if body_frame - bool send_target_angles(float pitch_rad, float yaw_rad, bool yaw_is_ef); + void send_target_angles(const MountAngleTarget &angle_rad) override; // send camera command, affected image sensor and value (e.g. zoom speed) bool send_camera_command(ImageSensor img_sensor, CameraCommand cmd, uint8_t value, LRFCommand lrf_cmd = LRFCommand::NO_ACTION); diff --git a/libraries/AP_Mount/AP_Mount_XFRobot.cpp b/libraries/AP_Mount/AP_Mount_XFRobot.cpp index 69c4701abb651..48c0c698e6dfa 100644 --- a/libraries/AP_Mount/AP_Mount_XFRobot.cpp +++ b/libraries/AP_Mount/AP_Mount_XFRobot.cpp @@ -129,83 +129,10 @@ void AP_Mount_XFRobot::update() // check for recording request timeout check_recording_timeout(); - // change to RC_TARGETING mode if RC input has changed - set_rctargeting_on_rcinput_change(); - - // flag to trigger sending target angles to gimbal - bool resend_now = false; - - // update based on mount mode - switch (get_mode()) { - // move mount to a "retracted" position - case MAV_MOUNT_MODE_RETRACT: { - const Vector3f &target = _params.retract_angles.get(); - mnt_target.angle_rad.set(target*DEG_TO_RAD, false); - mnt_target.target_type = MountTargetType::ANGLE; - break; - } - - // move mount to a neutral position, typically pointing forward - case MAV_MOUNT_MODE_NEUTRAL: { - const Vector3f &target = _params.neutral_angles.get(); - mnt_target.angle_rad.set(target*DEG_TO_RAD, false); - mnt_target.target_type = MountTargetType::ANGLE; - break; - } - - // point to the angles given by a mavlink message - case MAV_MOUNT_MODE_MAVLINK_TARGETING: - // mnt_target should have already been filled in by set_angle_target() or set_rate_target() - resend_now = true; - break; - - // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - update_mnt_target_from_rc_target(); - resend_now = true; - break; - } - - // point mount to a GPS point given by the mission planner - case MAV_MOUNT_MODE_GPS_POINT: - if (get_angle_target_to_roi(mnt_target.angle_rad)) { - mnt_target.target_type = MountTargetType::ANGLE; - resend_now = true; - } - break; + AP_Mount_Backend::update_mnt_target(); - // point mount to Home location - case MAV_MOUNT_MODE_HOME_LOCATION: - if (get_angle_target_to_home(mnt_target.angle_rad)) { - mnt_target.target_type = MountTargetType::ANGLE; - resend_now = true; - } - break; - - // point mount to another vehicle - case MAV_MOUNT_MODE_SYSID_TARGET: - if (get_angle_target_to_sysid(mnt_target.angle_rad)) { - mnt_target.target_type = MountTargetType::ANGLE; - resend_now = true; - } - break; - - default: - // we do not know this mode so do nothing - break; - } - - // update angle targets from angle rates - if (mnt_target.target_type == MountTargetType::RATE) { - update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad); - } - - // resend target angles at least once per second - resend_now = resend_now || ((AP_HAL::millis() - last_send_ms) > SEND_ATTITUDE_TARGET_MS); - if (resend_now) { - send_target_angles(mnt_target.angle_rad); - } + // send target angles (which may be derived from other target types) + send_target_to_gimbal(); } // return true if healthy @@ -421,7 +348,7 @@ void AP_Mount_XFRobot::process_packet() } // send_target_angles -void AP_Mount_XFRobot::send_target_angles(const MountTarget& angle_target_rad) +void AP_Mount_XFRobot::send_target_angles(const MountAngleTarget& angle_target_rad) { // exit immediately if not initialised or not enough space to send packet if (!_initialised || _uart->txspace() < sizeof(SetAttitudePacket)) { diff --git a/libraries/AP_Mount/AP_Mount_XFRobot.h b/libraries/AP_Mount/AP_Mount_XFRobot.h index e252aafd3ced1..f06de2155dfdb 100644 --- a/libraries/AP_Mount/AP_Mount_XFRobot.h +++ b/libraries/AP_Mount/AP_Mount_XFRobot.h @@ -129,8 +129,13 @@ class AP_Mount_XFRobot : public AP_Mount_Backend_Serial // process successfully decoded packets held in the _msg_buff structure void process_packet(); + // XFRobot can only send angles + uint8_t natively_supported_mount_target_types() const override { + return NATIVE_ANGLES_ONLY; + }; + // send_target_angles - void send_target_angles(const MountTarget& angle_target_rad); + void send_target_angles(const MountAngleTarget& angle_target_rad) override; // send simple (1byte) command to gimbal (e.g. take pic, start recording) // returns true on success, false on failure to send diff --git a/libraries/AP_Mount/AP_Mount_Xacti.cpp b/libraries/AP_Mount/AP_Mount_Xacti.cpp index b2561f3988383..fdd2cfcede3bf 100644 --- a/libraries/AP_Mount/AP_Mount_Xacti.cpp +++ b/libraries/AP_Mount/AP_Mount_Xacti.cpp @@ -116,14 +116,7 @@ void AP_Mount_Xacti::update() update_mnt_target(); // send target angles or rates depending on the target type - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - send_target_angles(mnt_target.angle_rad.pitch, mnt_target.angle_rad.yaw, mnt_target.angle_rad.yaw_is_ef); - break; - case MountTargetType::RATE: - send_target_rates(mnt_target.rate_rads.pitch, mnt_target.rate_rads.yaw, mnt_target.rate_rads.yaw_is_ef); - break; - } + send_target_to_gimbal(); } // return true if healthy @@ -344,17 +337,22 @@ bool AP_Mount_Xacti::get_attitude_quaternion(Quaternion& att_quat) } // send target pitch and yaw rates to gimbal -// yaw_is_ef should be true if yaw_rads target is an earth frame rate, false if body_frame -void AP_Mount_Xacti::send_target_rates(float pitch_rads, float yaw_rads, bool yaw_is_ef) +void AP_Mount_Xacti::send_target_rates(const MountRateTarget &rate_rads) { + const float pitch_rads = rate_rads.pitch; + const float yaw_rads = rate_rads.yaw; + // send gimbal rate target to gimbal send_gimbal_control(3, degrees(pitch_rads) * 100, degrees(yaw_rads) * 100); } // send target pitch and yaw angles to gimbal -// yaw_is_ef should be true if yaw_rad target is an earth frame angle, false if body_frame -void AP_Mount_Xacti::send_target_angles(float pitch_rad, float yaw_rad, bool yaw_is_ef) +void AP_Mount_Xacti::send_target_angles(const MountAngleTarget &angle_rad) { + const float pitch_rad = angle_rad.pitch; + const float yaw_rad = angle_rad.yaw; + const bool yaw_is_ef = angle_rad.yaw_is_ef; + // convert yaw to body frame const float yaw_bf_rad = yaw_is_ef ? wrap_PI(yaw_rad - AP::ahrs().get_yaw_rad()) : yaw_rad; diff --git a/libraries/AP_Mount/AP_Mount_Xacti.h b/libraries/AP_Mount/AP_Mount_Xacti.h index 14643a7acc49e..d5c2b98e6c5e5 100644 --- a/libraries/AP_Mount/AP_Mount_Xacti.h +++ b/libraries/AP_Mount/AP_Mount_Xacti.h @@ -86,6 +86,11 @@ class AP_Mount_Xacti : public AP_Mount_Backend // get attitude as a quaternion. returns true on success bool get_attitude_quaternion(Quaternion& att_quat) override; + // Xacti can send either rates or angles + uint8_t natively_supported_mount_target_types() const override { + return NATIVE_ANGLES_AND_RATES_ONLY; + }; + private: // send text prefix string to reduce flash cost @@ -103,12 +108,10 @@ class AP_Mount_Xacti : public AP_Mount_Backend static const char* sensor_mode_str[]; // send target pitch and yaw rates to gimbal - // yaw_is_ef should be true if yaw_rads target is an earth frame rate, false if body_frame - void send_target_rates(float pitch_rads, float yaw_rads, bool yaw_is_ef); + void send_target_rates(const MountRateTarget &rate_rads) override; // send target pitch and yaw angles to gimbal - // yaw_is_ef should be true if yaw_rad target is an earth frame angle, false if body_frame - void send_target_angles(float pitch_rad, float yaw_rad, bool yaw_is_ef); + void send_target_angles(const MountAngleTarget &angle_rad) override; // register backend in detected modules array used to map DroneCAN port and node id to backend void register_backend(); diff --git a/libraries/AP_Mount/AP_Mount_config.h b/libraries/AP_Mount/AP_Mount_config.h index 5519a62468430..6a022d38e0e72 100644 --- a/libraries/AP_Mount/AP_Mount_config.h +++ b/libraries/AP_Mount/AP_Mount_config.h @@ -6,7 +6,7 @@ #include #ifndef HAL_MOUNT_ENABLED -#define HAL_MOUNT_ENABLED 1 +#define HAL_MOUNT_ENABLED 0 #endif #ifndef AP_MOUNT_BACKEND_DEFAULT_ENABLED @@ -62,8 +62,12 @@ #define HAL_MOUNT_XFROBOT_ENABLED AP_MOUNT_BACKEND_DEFAULT_ENABLED && HAL_PROGRAM_SIZE_LIMIT_KB > 1024 #endif +#ifndef AP_MOUNT_POI_LOCK_ENABLED +#define AP_MOUNT_POI_LOCK_ENABLED 0 +#endif + #ifndef AP_MOUNT_POI_TO_LATLONALT_ENABLED -#define AP_MOUNT_POI_TO_LATLONALT_ENABLED HAL_MOUNT_ENABLED && AP_TERRAIN_AVAILABLE && HAL_PROGRAM_SIZE_LIMIT_KB > 1024 +#define AP_MOUNT_POI_TO_LATLONALT_ENABLED HAL_MOUNT_ENABLED && AP_TERRAIN_AVAILABLE && (HAL_PROGRAM_SIZE_LIMIT_KB > 1024 || AP_MOUNT_POI_LOCK_ENABLED) #endif #ifndef HAL_MOUNT_TOPOTEK_ENABLED diff --git a/libraries/AP_Mount/LogStructure.h b/libraries/AP_Mount/LogStructure.h index 7428946061129..c27bf2ee30194 100644 --- a/libraries/AP_Mount/LogStructure.h +++ b/libraries/AP_Mount/LogStructure.h @@ -10,6 +10,7 @@ // @Description: Mount's desired and actual roll, pitch and yaw angles // @Field: TimeUS: Time since system startup // @Field: I: Instance number +// @Field: Mode: Mount mode // @Field: DRoll: Desired roll // @Field: Roll: Actual roll // @Field: DPitch: Desired pitch @@ -24,6 +25,7 @@ struct PACKED log_Mount { LOG_PACKET_HEADER; uint64_t time_us; uint8_t instance; + uint8_t mode; float desired_roll; float actual_roll; float desired_pitch; @@ -38,7 +40,7 @@ struct PACKED log_Mount { #if HAL_MOUNT_ENABLED #define LOG_STRUCTURE_FROM_MOUNT \ { LOG_MOUNT_MSG, sizeof(log_Mount), \ - "MNT", "QBfffffffff","TimeUS,I,DRoll,Roll,DPitch,Pitch,DYawB,YawB,DYawE,YawE,Dist", "s#ddddddddm", "F---------0" }, + "MNT", "QBBfffffffff","TimeUS,I,Mode,DRoll,Roll,DPitch,Pitch,DYawB,YawB,DYawE,YawE,Dist", "s#-ddddddddm", "F----------0" }, #else #define LOG_STRUCTURE_FROM_MOUNT #endif diff --git a/libraries/AP_NavEKF/AP_NavEKF_core_common.cpp b/libraries/AP_NavEKF/AP_NavEKF_core_common.cpp index 5eb04b41cbd3b..ba0cb4cc6a1d7 100644 --- a/libraries/AP_NavEKF/AP_NavEKF_core_common.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF_core_common.cpp @@ -16,9 +16,7 @@ */ #include "AP_NavEKF_core_common.h" -NavEKF_core_common::Matrix24 NavEKF_core_common::KH; NavEKF_core_common::Matrix24 NavEKF_core_common::KHP; -NavEKF_core_common::Matrix24 NavEKF_core_common::nextP; NavEKF_core_common::Vector28 NavEKF_core_common::Kfusion; /* @@ -31,9 +29,7 @@ void NavEKF_core_common::fill_scratch_variables(void) // SITL where they are used without initialisation. These are all // supposed to be scratch variables that are not used between // iterations - fill_nanf(&KH[0][0], sizeof(KH)/sizeof(ftype)); fill_nanf(&KHP[0][0], sizeof(KHP)/sizeof(ftype)); - fill_nanf(&nextP[0][0], sizeof(nextP)/sizeof(ftype)); fill_nanf(&Kfusion[0], sizeof(Kfusion)/sizeof(ftype)); #endif } diff --git a/libraries/AP_NavEKF/AP_NavEKF_core_common.h b/libraries/AP_NavEKF/AP_NavEKF_core_common.h index a236a93287675..7b676e34d69ba 100644 --- a/libraries/AP_NavEKF/AP_NavEKF_core_common.h +++ b/libraries/AP_NavEKF/AP_NavEKF_core_common.h @@ -42,9 +42,7 @@ class NavEKF_core_common { #endif protected: - static Matrix24 KH; // intermediate result used for covariance updates static Matrix24 KHP; // intermediate result used for covariance updates - static Matrix24 nextP; // Predicted covariance matrix before addition of process noise to diagonals static Vector28 Kfusion; // intermediate fusion vector // fill all the common scratch variables with NaN on SITL diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 7b394a083ba6d..5a3ec45b99e7b 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -30,6 +30,9 @@ extern const AP_HAL::HAL& hal; */ #define ENABLE_EKF_TIMING 0 +NavEKF2_core::Matrix24 NavEKF2_core::KH; +NavEKF2_core::Matrix24 NavEKF2_core::nextP; + // constructor NavEKF2_core::NavEKF2_core(NavEKF2 *_frontend) : dal(AP::dal()), @@ -543,6 +546,10 @@ void NavEKF2_core::UpdateFilter(bool predict) #endif fill_scratch_variables(); +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + fill_nanf(&KH[0][0], sizeof(KH)/sizeof(ftype)); // see fill_scratch_variables() + fill_nanf(&nextP[0][0], sizeof(nextP)/sizeof(ftype)); +#endif // TODO - in-flight restart method diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 8ef86948fa862..3020dccd72d85 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -788,6 +788,9 @@ class NavEKF2_core : public NavEKF_core_common static const uint32_t FLOW_BUFFER_LENGTH = 15; static const uint32_t EXTNAV_BUFFER_LENGTH = 15; + static Matrix24 KH; // intermediate result used for covariance updates + static Matrix24 nextP; // Predicted covariance matrix before addition of process noise to diagonals + // Variables bool statesInitialised; // boolean true when filter states have been initialised bool magHealth; // boolean true if magnetometer has passed innovation consistency check diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index f281db4a8366b..9aa9846841ffd 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1245,12 +1245,25 @@ bool NavEKF3::getAirSpdVec(Vector3f &vel) const return false; } -// return the innovation in m/s, innovation variance in (m/s)^2 and age in msec of the last TAS measurement processed -bool NavEKF3::getAirSpdHealthData(float &innovation, float &innovationVariance, uint32_t &age_ms) const +// return the innovation in m/s, innovation variance in (m/s)^2 and age in msec of the last TAS measurement processed for a given sensor instance +bool NavEKF3::getAirSpdHealthData(uint8_t instance, float &innovation, float &innovationVariance, uint32_t &age_ms) const { - if (core) { + if (core == nullptr) { + return false; + } + + // Return primary if it is configured to use the given instance + if (core[primary].getActiveAirspeed() == instance) { return core[primary].getAirSpdHealthData(innovation, innovationVariance, age_ms); } + + // See if another core is using the given instance + for (uint8_t i = 0; i < num_cores; i++) { + if (core[i].getActiveAirspeed() == instance) { + return core[i].getAirSpdHealthData(innovation, innovationVariance, age_ms); + } + } + return false; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 1c383b884fd15..f54814ce56dd5 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -83,9 +83,9 @@ class NavEKF3 { // returns false if estimate is unavailable bool getAirSpdVec(Vector3f &vel) const; - // return the innovation in m/s, innovation variance in (m/s)^2 and age in msec of the last TAS measurement processed - // returns false if the data is unavilable - bool getAirSpdHealthData(float &innovation, float &innovationVariance, uint32_t &age_ms) const; + // return the innovation in m/s, innovation variance in (m/s)^2 and age in msec of the last TAS measurement processed for a given sensor instance + // returns false if the data is unavailable + bool getAirSpdHealthData(uint8_t instance, float &innovation, float &innovationVariance, uint32_t &age_ms) const; // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s // This can be different to the z component of the EKF velocity state because it will fluctuate with height errors and corrections in the EKF diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index 88317f56344da..6d2babb16dddd 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp @@ -149,31 +149,18 @@ void NavEKF3_core::FuseAirspeed() } stateStruct.quat.normalize(); - // correct the covariance P = (I - K*H)*P - // take advantage of the empty columns in KH to reduce the - // number of operations + // correct the covariance P = (I - K*H)*P = P - K*H*P. take advantage of + // the zero elements of H to reduce the number of operations. for (unsigned i = 0; i<=stateIndexLim; i++) { - for (unsigned j = 0; j<=3; j++) { - KH[i][j] = 0.0f; - } - for (unsigned j = 4; j<=6; j++) { - KH[i][j] = Kfusion[i] * H_TAS[j]; - } - for (unsigned j = 7; j<=21; j++) { - KH[i][j] = 0.0f; - } - for (unsigned j = 22; j<=23; j++) { - KH[i][j] = Kfusion[i] * H_TAS[j]; - } - } - for (unsigned j = 0; j<=stateIndexLim; j++) { - for (unsigned i = 0; i<=stateIndexLim; i++) { + // j as the inner loop allows the compiler to hoist the KH product + // to save computation, and do the inner indexing more efficiently. + for (unsigned j = 0; j<=stateIndexLim; j++) { ftype res = 0; - res += KH[i][4] * P[4][j]; - res += KH[i][5] * P[5][j]; - res += KH[i][6] * P[6][j]; - res += KH[i][22] * P[22][j]; - res += KH[i][23] * P[23][j]; + res += (Kfusion[i] * H_TAS[4]) * P[4][j]; + res += (Kfusion[i] * H_TAS[5]) * P[5][j]; + res += (Kfusion[i] * H_TAS[6]) * P[6][j]; + res += (Kfusion[i] * H_TAS[22]) * P[22][j]; + res += (Kfusion[i] * H_TAS[23]) * P[23][j]; KHP[i][j] = res; } } @@ -425,32 +412,22 @@ void NavEKF3_core::FuseSideslip() } stateStruct.quat.normalize(); - // correct the covariance P = (I - K*H)*P - // take advantage of the empty columns in KH to reduce the - // number of operations + // correct the covariance P = (I - K*H)*P = P - K*H*P. take advantage of + // the zero elements of H to reduce the number of operations. for (unsigned i = 0; i<=stateIndexLim; i++) { - for (unsigned j = 0; j<=6; j++) { - KH[i][j] = Kfusion[i] * H_BETA[j]; - } - for (unsigned j = 7; j<=21; j++) { - KH[i][j] = 0.0f; - } - for (unsigned j = 22; j<=23; j++) { - KH[i][j] = Kfusion[i] * H_BETA[j]; - } - } - for (unsigned j = 0; j<=stateIndexLim; j++) { - for (unsigned i = 0; i<=stateIndexLim; i++) { + // j as the inner loop allows the compiler to hoist the KH product + // to save computation, and do the inner indexing more efficiently. + for (unsigned j = 0; j<=stateIndexLim; j++) { ftype res = 0; - res += KH[i][0] * P[0][j]; - res += KH[i][1] * P[1][j]; - res += KH[i][2] * P[2][j]; - res += KH[i][3] * P[3][j]; - res += KH[i][4] * P[4][j]; - res += KH[i][5] * P[5][j]; - res += KH[i][6] * P[6][j]; - res += KH[i][22] * P[22][j]; - res += KH[i][23] * P[23][j]; + res += (Kfusion[i] * H_BETA[0]) * P[0][j]; + res += (Kfusion[i] * H_BETA[1]) * P[1][j]; + res += (Kfusion[i] * H_BETA[2]) * P[2][j]; + res += (Kfusion[i] * H_BETA[3]) * P[3][j]; + res += (Kfusion[i] * H_BETA[4]) * P[4][j]; + res += (Kfusion[i] * H_BETA[5]) * P[5][j]; + res += (Kfusion[i] * H_BETA[6]) * P[6][j]; + res += (Kfusion[i] * H_BETA[22]) * P[22][j]; + res += (Kfusion[i] * H_BETA[23]) * P[23][j]; KHP[i][j] = res; } } @@ -700,32 +677,22 @@ void NavEKF3_core::FuseDragForces() } stateStruct.quat.normalize(); - // correct the covariance P = (I - K*H)*P - // take advantage of the empty columns in KH to reduce the - // number of operations + // correct the covariance P = (I - K*H)*P = P - K*H*P. take advantage of + // the zero elements of H to reduce the number of operations. for (unsigned i = 0; i<=stateIndexLim; i++) { - for (unsigned j = 0; j<=6; j++) { - KH[i][j] = Kfusion[i] * Hfusion[j]; - } - for (unsigned j = 7; j<=21; j++) { - KH[i][j] = 0.0f; - } - for (unsigned j = 22; j<=23; j++) { - KH[i][j] = Kfusion[i] * Hfusion[j]; - } - } - for (unsigned j = 0; j<=stateIndexLim; j++) { - for (unsigned i = 0; i<=stateIndexLim; i++) { + // j as the inner loop allows the compiler to hoist the KH product + // to save computation, and do the inner indexing more efficiently. + for (unsigned j = 0; j<=stateIndexLim; j++) { ftype res = 0; - res += KH[i][0] * P[0][j]; - res += KH[i][1] * P[1][j]; - res += KH[i][2] * P[2][j]; - res += KH[i][3] * P[3][j]; - res += KH[i][4] * P[4][j]; - res += KH[i][5] * P[5][j]; - res += KH[i][6] * P[6][j]; - res += KH[i][22] * P[22][j]; - res += KH[i][23] * P[23][j]; + res += (Kfusion[i] * Hfusion[0]) * P[0][j]; + res += (Kfusion[i] * Hfusion[1]) * P[1][j]; + res += (Kfusion[i] * Hfusion[2]) * P[2][j]; + res += (Kfusion[i] * Hfusion[3]) * P[3][j]; + res += (Kfusion[i] * Hfusion[4]) * P[4][j]; + res += (Kfusion[i] * Hfusion[5]) * P[5][j]; + res += (Kfusion[i] * Hfusion[6]) * P[6][j]; + res += (Kfusion[i] * Hfusion[22]) * P[22][j]; + res += (Kfusion[i] * Hfusion[23]) * P[23][j]; KHP[i][j] = res; } } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index a82a6e2e64154..9159d8a2b70d0 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -582,6 +582,8 @@ void NavEKF3_core::FuseMagnetometer() } Vector24 H_MAG; + // index of H_MAG which is exactly 1, for more efficient computation below + int H_MAG_unit_index; for (uint8_t obsIndex = 0; obsIndex <= 2; obsIndex++) { if (obsIndex == 0) { @@ -597,6 +599,7 @@ void NavEKF3_core::FuseMagnetometer() H_MAG[19] = 1.0f; H_MAG[20] = 0.0f; H_MAG[21] = 0.0f; + H_MAG_unit_index = 19; // calculate Kalman gain const Vector5 SK_MX { @@ -679,6 +682,7 @@ void NavEKF3_core::FuseMagnetometer() H_MAG[19] = 0.0f; H_MAG[20] = 1.0f; H_MAG[21] = 0.0f; + H_MAG_unit_index = 20; // calculate Kalman gain const Vector5 SK_MY { @@ -763,6 +767,7 @@ void NavEKF3_core::FuseMagnetometer() H_MAG[19] = 0.0f; H_MAG[20] = 0.0f; H_MAG[21] = 1.0f; + H_MAG_unit_index = 21; // calculate Kalman gain const Vector5 SK_MZ { @@ -833,36 +838,23 @@ void NavEKF3_core::FuseMagnetometer() // this can be used by other fusion processes to avoid fusing on the same frame as this expensive step magFusePerformed = true; } - // correct the covariance P = (I - K*H)*P - // take advantage of the empty columns in KH to reduce the - // number of operations + // correct the covariance P = (I - K*H)*P = P - K*H*P. take advantage of + // the zero elements of H to reduce the number of operations. for (unsigned i = 0; i<=stateIndexLim; i++) { - for (unsigned j = 0; j<=3; j++) { - KH[i][j] = Kfusion[i] * H_MAG[j]; - } - for (unsigned j = 4; j<=15; j++) { - KH[i][j] = 0.0f; - } - for (unsigned j = 16; j<=21; j++) { - KH[i][j] = Kfusion[i] * H_MAG[j]; - } - for (unsigned j = 22; j<=23; j++) { - KH[i][j] = 0.0f; - } - } - for (unsigned j = 0; j<=stateIndexLim; j++) { - for (unsigned i = 0; i<=stateIndexLim; i++) { + // j as the inner loop allows the compiler to hoist the KH product + // to save computation, and do the inner indexing more efficiently. + for (unsigned j = 0; j<=stateIndexLim; j++) { ftype res = 0; - res += KH[i][0] * P[0][j]; - res += KH[i][1] * P[1][j]; - res += KH[i][2] * P[2][j]; - res += KH[i][3] * P[3][j]; - res += KH[i][16] * P[16][j]; - res += KH[i][17] * P[17][j]; - res += KH[i][18] * P[18][j]; - res += KH[i][19] * P[19][j]; - res += KH[i][20] * P[20][j]; - res += KH[i][21] * P[21][j]; + res += (Kfusion[i] * H_MAG[0]) * P[0][j]; + res += (Kfusion[i] * H_MAG[1]) * P[1][j]; + res += (Kfusion[i] * H_MAG[2]) * P[2][j]; + res += (Kfusion[i] * H_MAG[3]) * P[3][j]; + res += (Kfusion[i] * H_MAG[16]) * P[16][j]; + res += (Kfusion[i] * H_MAG[17]) * P[17][j]; + res += (Kfusion[i] * H_MAG[18]) * P[18][j]; + // one value in H is always 1, and the others not mentioned here + // are zero, so we can skip that H product to save an operation. + res += Kfusion[i] * P[H_MAG_unit_index][j]; KHP[i][j] = res; } } @@ -1197,20 +1189,18 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method) magHealth = true; } - // correct the covariance using P = P - K*H*P taking advantage of the fact that only the first 3 elements in H are non zero - // calculate K*H*P - for (uint8_t row = 0; row <= stateIndexLim; row++) { - for (uint8_t column = 0; column <= 3; column++) { - KH[row][column] = Kfusion[row] * H_YAW[column]; - } - } - for (uint8_t row = 0; row <= stateIndexLim; row++) { - for (uint8_t column = 0; column <= stateIndexLim; column++) { - ftype tmp = KH[row][0] * P[0][column]; - tmp += KH[row][1] * P[1][column]; - tmp += KH[row][2] * P[2][column]; - tmp += KH[row][3] * P[3][column]; - KHP[row][column] = tmp; + // correct the covariance P = (I - K*H)*P = P - K*H*P. take advantage of + // the zero elements of H to reduce the number of operations. + for (unsigned i = 0; i<=stateIndexLim; i++) { + // j as the inner loop allows the compiler to hoist the KH product + // to save computation, and do the inner indexing more efficiently. + for (unsigned j = 0; j<=stateIndexLim; j++) { + ftype res = 0; + res += (Kfusion[i] * H_YAW[0]) * P[0][j]; + res += (Kfusion[i] * H_YAW[1]) * P[1][j]; + res += (Kfusion[i] * H_YAW[2]) * P[2][j]; + res += (Kfusion[i] * H_YAW[3]) * P[3][j]; + KHP[i][j] = res; } } @@ -1378,22 +1368,16 @@ void NavEKF3_core::FuseDeclination(ftype declErr) innovation = -0.5f; } - // correct the covariance P = (I - K*H)*P - // take advantage of the empty columns in KH to reduce the - // number of operations + // correct the covariance P = (I - K*H)*P = P - K*H*P. take advantage of + // the zero elements of H to reduce the number of operations. for (unsigned i = 0; i<=stateIndexLim; i++) { - for (unsigned j = 0; j<=15; j++) { - KH[i][j] = 0.0f; - } - KH[i][16] = Kfusion[i] * H_DECL[16]; - KH[i][17] = Kfusion[i] * H_DECL[17]; - for (unsigned j = 18; j<=23; j++) { - KH[i][j] = 0.0f; - } - } - for (unsigned j = 0; j<=stateIndexLim; j++) { - for (unsigned i = 0; i<=stateIndexLim; i++) { - KHP[i][j] = KH[i][16] * P[16][j] + KH[i][17] * P[17][j]; + // j as the inner loop allows the compiler to hoist the KH product + // to save computation, and do the inner indexing more efficiently. + for (unsigned j = 0; j<=stateIndexLim; j++) { + ftype res = 0; + res += (Kfusion[i] * H_DECL[16]) * P[16][j]; + res += (Kfusion[i] * H_DECL[17]) * P[17][j]; + KHP[i][j] = res; } } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp index ec3e90917044e..d7fdd1dc789b5 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp @@ -702,27 +702,21 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fus flowFusionActive = true; GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u fusing optical flow",(unsigned)imu_index); } - // correct the covariance P = (I - K*H)*P - // take advantage of the empty columns in KH to reduce the - // number of operations - for (uint8_t i = 0; i<=stateIndexLim; i++) { - for (uint8_t j = 0; j<=6; j++) { - KH[i][j] = Kfusion[i] * H_LOS[j]; - } - for (uint8_t j = 7; j<=stateIndexLim; j++) { - KH[i][j] = 0.0f; - } - } - for (uint8_t j = 0; j<=stateIndexLim; j++) { - for (uint8_t i = 0; i<=stateIndexLim; i++) { + + // correct the covariance P = (I - K*H)*P = P - K*H*P. take advantage of + // the zero elements of H to reduce the number of operations. + for (unsigned i = 0; i<=stateIndexLim; i++) { + // j as the inner loop allows the compiler to hoist the KH product + // to save computation, and do the inner indexing more efficiently. + for (unsigned j = 0; j<=stateIndexLim; j++) { ftype res = 0; - res += KH[i][0] * P[0][j]; - res += KH[i][1] * P[1][j]; - res += KH[i][2] * P[2][j]; - res += KH[i][3] * P[3][j]; - res += KH[i][4] * P[4][j]; - res += KH[i][5] * P[5][j]; - res += KH[i][6] * P[6][j]; + res += (Kfusion[i] * H_LOS[0]) * P[0][j]; + res += (Kfusion[i] * H_LOS[1]) * P[1][j]; + res += (Kfusion[i] * H_LOS[2]) * P[2][j]; + res += (Kfusion[i] * H_LOS[3]) * P[3][j]; + res += (Kfusion[i] * H_LOS[4]) * P[4][j]; + res += (Kfusion[i] * H_LOS[5]) * P[5][j]; + res += (Kfusion[i] * H_LOS[6]) * P[6][j]; KHP[i][j] = res; } } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index c239f446d3f75..e3a17c4fb2984 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -1999,27 +1999,21 @@ void NavEKF3_core::FuseBodyVel() bodyVelFusionActive = true; GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u fusing odometry",(unsigned)imu_index); } - // correct the covariance P = (I - K*H)*P - // take advantage of the empty columns in KH to reduce the - // number of operations + + // correct the covariance P = (I - K*H)*P = P - K*H*P. take advantage of + // the zero elements of H to reduce the number of operations. for (unsigned i = 0; i<=stateIndexLim; i++) { - for (unsigned j = 0; j<=6; j++) { - KH[i][j] = Kfusion[i] * H_VEL[j]; - } - for (unsigned j = 7; j<=stateIndexLim; j++) { - KH[i][j] = 0.0f; - } - } - for (unsigned j = 0; j<=stateIndexLim; j++) { - for (unsigned i = 0; i<=stateIndexLim; i++) { + // j as the inner loop allows the compiler to hoist the KH product + // to save computation, and do the inner indexing more efficiently. + for (unsigned j = 0; j<=stateIndexLim; j++) { ftype res = 0; - res += KH[i][0] * P[0][j]; - res += KH[i][1] * P[1][j]; - res += KH[i][2] * P[2][j]; - res += KH[i][3] * P[3][j]; - res += KH[i][4] * P[4][j]; - res += KH[i][5] * P[5][j]; - res += KH[i][6] * P[6][j]; + res += (Kfusion[i] * H_VEL[0]) * P[0][j]; + res += (Kfusion[i] * H_VEL[1]) * P[1][j]; + res += (Kfusion[i] * H_VEL[2]) * P[2][j]; + res += (Kfusion[i] * H_VEL[3]) * P[3][j]; + res += (Kfusion[i] * H_VEL[4]) * P[4][j]; + res += (Kfusion[i] * H_VEL[5]) * P[5][j]; + res += (Kfusion[i] * H_VEL[6]) * P[6][j]; KHP[i][j] = res; } } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp index a32a4cac5e678..3bf421741a460 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp @@ -260,29 +260,20 @@ void NavEKF3_core::FuseRngBcn() // restart the counter rngBcn.lastPassTime_ms = imuSampleTime_ms; - // correct the covariance P = (I - K*H)*P - // take advantage of the empty columns in KH to reduce the - // number of operations + // correct the covariance P = (I - K*H)*P = P - K*H*P. take advantage of + // the zero elements of H to reduce the number of operations. for (unsigned i = 0; i<=stateIndexLim; i++) { - for (unsigned j = 0; j<=6; j++) { - KH[i][j] = 0.0f; - } - for (unsigned j = 7; j<=9; j++) { - KH[i][j] = Kfusion[i] * H_BCN[j]; - } - for (unsigned j = 10; j<=23; j++) { - KH[i][j] = 0.0f; - } - } - for (unsigned j = 0; j<=stateIndexLim; j++) { - for (unsigned i = 0; i<=stateIndexLim; i++) { + // j as the inner loop allows the compiler to hoist the KH product + // to save computation, and do the inner indexing more efficiently. + for (unsigned j = 0; j<=stateIndexLim; j++) { ftype res = 0; - res += KH[i][7] * P[7][j]; - res += KH[i][8] * P[8][j]; - res += KH[i][9] * P[9][j]; + res += (Kfusion[i] * H_BCN[7]) * P[7][j]; + res += (Kfusion[i] * H_BCN[8]) * P[8][j]; + res += (Kfusion[i] * H_BCN[9]) * P[9][j]; KHP[i][j] = res; } } + // Check that we are not going to drive any variances negative and skip the update if so bool healthyFusion = true; for (uint8_t i= 0; i<=stateIndexLim; i++) { @@ -507,18 +498,16 @@ void NavEKF3_core::FuseRngBcnStatic() rngBcn.receiverPos.y -= K_RNG[1] * rngBcn.innov; rngBcn.receiverPos.z -= K_RNG[2] * rngBcn.innov; - // calculate the covariance correction + // correct the covariance P = (I - K*H)*P = P - K*H*P. take advantage of + // the zero elements of H to reduce the number of operations. for (unsigned i = 0; i<=2; i++) { + // j as the inner loop allows the compiler to hoist the KH product + // to save computation, and do the inner indexing more efficiently. for (unsigned j = 0; j<=2; j++) { - KH[i][j] = K_RNG[i] * H_RNG[j]; - } - } - for (unsigned j = 0; j<=2; j++) { - for (unsigned i = 0; i<=2; i++) { ftype res = 0; - res += KH[i][0] * rngBcn.receiverPosCov[0][j]; - res += KH[i][1] * rngBcn.receiverPosCov[1][j]; - res += KH[i][2] * rngBcn.receiverPosCov[2][j]; + res += (K_RNG[i] * H_RNG[0]) * rngBcn.receiverPosCov[0][j]; + res += (K_RNG[i] * H_RNG[1]) * rngBcn.receiverPosCov[1][j]; + res += (K_RNG[i] * H_RNG[2]) * rngBcn.receiverPosCov[2][j]; KHP[i][j] = res; } } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index be82dfb0a8572..db83dd99e15c0 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -244,9 +244,7 @@ void NavEKF3_core::InitialiseVariables() lastKnownPositionD = 0; prevTnb.zero(); memset(&P[0][0], 0, sizeof(P)); - memset(&KH[0][0], 0, sizeof(KH)); memset(&KHP[0][0], 0, sizeof(KHP)); - memset(&nextP[0][0], 0, sizeof(nextP)); flowDataValid = false; rangeDataToFuse = false; #if EK3_FEATURE_OPTFLOW_FUSION @@ -1181,6 +1179,10 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr) } } + // nextP is a temporary only used in this function, and KHP is a temporary + // the same size only used outside of it. save memory by using KHP as nextP. + auto& nextP = KHP; + // calculate the predicted covariance due to inertial sensor error propagation // we calculate the lower diagonal and copy to take advantage of symmetry @@ -2157,38 +2159,25 @@ void NavEKF3_core::calcTiltErrorVariance() // equations generated by quaternion_error_propagation(): in derivation/generate_2.py // only diagonals have been used - // dq0 ... dq3 terms have been zeroed - const ftype PS1 = q0*q1 + q2*q3; - const ftype PS2 = q1*PS1; - const ftype PS4 = sq(q0) - sq(q1) - sq(q2) + sq(q3); - const ftype PS5 = q0*PS4; - const ftype PS6 = 2*PS2 + PS5; - const ftype PS8 = PS1*q2; - const ftype PS10 = PS4*q3; - const ftype PS11 = PS10 + 2*PS8; - const ftype PS12 = PS1*q3; - const ftype PS13 = PS4*q2; - const ftype PS14 = -2*PS12 + PS13; - const ftype PS15 = PS1*q0; - const ftype PS16 = q1*PS4; - const ftype PS17 = 2*PS15 - PS16; - const ftype PS18 = q0*q2 - q1*q3; - const ftype PS19 = PS18*q2; - const ftype PS20 = 2*PS19 + PS5; - const ftype PS22 = q1*PS18; - const ftype PS23 = -PS10 + 2*PS22; - const ftype PS25 = PS18*q3; - const ftype PS26 = PS16 + 2*PS25; - const ftype PS28 = PS18*q0; - const ftype PS29 = -PS13 + 2*PS28; - const ftype PS32 = PS12 + PS28; - const ftype PS33 = PS19 + PS2; - const ftype PS34 = PS15 - PS25; - const ftype PS35 = PS22 - PS8; - - tiltErrorVariance = 4*sq(PS11)*P[2][2] + 4*sq(PS14)*P[3][3] + 4*sq(PS17)*P[0][0] + 4*sq(PS6)*P[1][1]; - tiltErrorVariance += 4*sq(PS20)*P[2][2] + 4*sq(PS23)*P[1][1] + 4*sq(PS26)*P[3][3] + 4*sq(PS29)*P[0][0]; - tiltErrorVariance += 16*sq(PS32)*P[1][1] + 16*sq(PS33)*P[3][3] + 16*sq(PS34)*P[2][2] + 16*sq(PS35)*P[0][0]; + const ftype PS0 = q0*q1 + q2*q3; + const ftype PS1 = PS0*q1; + const ftype PS2 = sq(q0) - sq(q1) - sq(q2) + sq(q3); + const ftype PS3 = PS2*q0; + const ftype PS4 = PS0*q2; + const ftype PS5 = PS2*q3; + const ftype PS6 = PS0*q3; + const ftype PS7 = PS2*q2; + const ftype PS8 = PS0*q0; + const ftype PS9 = PS2*q1; + const ftype PS10 = q0*q2 - q1*q3; + const ftype PS11 = PS10*q2; + const ftype PS12 = PS10*q3; + const ftype PS13 = PS10*q0; + const ftype PS14 = PS10*q1; + + tiltErrorVariance = 4*P[0][0]*sq(2*PS8 - PS9) + 4*P[1][1]*sq(2*PS1 + PS3) + 4*P[2][2]*sq(2*PS4 + PS5) + 4*P[3][3]*sq(-2*PS6 + PS7); + tiltErrorVariance += 4*P[0][0]*sq(2*PS13 - PS7) + 4*P[1][1]*sq(2*PS14 - PS5) + 4*P[2][2]*sq(2*PS11 + PS3) + 4*P[3][3]*sq(2*PS12 + PS9); + tiltErrorVariance += 16*P[0][0]*sq(PS14 - PS4) + 16*P[1][1]*sq(PS13 + PS6) + 16*P[2][2]*sq(-PS12 + PS8) + 16*P[3][3]*sq(PS1 + PS11); tiltErrorVariance = constrain_ftype(tiltErrorVariance, 0.0f, sq(radians(30.0f))); } diff --git a/libraries/AP_NavEKF3/derivation/generate_2.py b/libraries/AP_NavEKF3/derivation/generate_2.py index 55c8866558cd7..fe3b7a3b6f839 100755 --- a/libraries/AP_NavEKF3/derivation/generate_2.py +++ b/libraries/AP_NavEKF3/derivation/generate_2.py @@ -532,10 +532,10 @@ def quaternion_error_propagation(): # remove second order terms # we don't want the error deltas to appear in the final result - J.subs(dq0,0) - J.subs(dq1,0) - J.subs(dq2,0) - J.subs(dq3,0) + J = J.subs(dq0,0) + J = J.subs(dq1,0) + J = J.subs(dq2,0) + J = J.subs(dq3,0) # define covaraince matrix for quaternion states P = create_symmetric_cov_matrix(4) @@ -545,6 +545,11 @@ def quaternion_error_propagation(): # rotate quaternion covariances into rotation vector state space P_rot_vec = J * P_diag * J.transpose() + # zero off diagonals + for i in range(P_rot_vec.shape[0]): + for j in range(P_rot_vec.shape[1]): + if i != j: + P_rot_vec[i, j] = 0 P_rot_vec_simple = cse(P_rot_vec, symbols("PS0:400"), optimizations='basic') quat_code_generator = CodeGenerator("./generated/tilt_error_cov_mat_generated.cpp") diff --git a/libraries/AP_Notify/Display.cpp b/libraries/AP_Notify/Display.cpp index f6b45fc2c0853..95ede141a18ee 100644 --- a/libraries/AP_Notify/Display.cpp +++ b/libraries/AP_Notify/Display.cpp @@ -30,8 +30,6 @@ #include #include -#include - extern const AP_HAL::HAL& hal; // Bit Map @@ -327,6 +325,20 @@ static_assert(ARRAY_SIZE(_font) == 1280, "_font is correct size"); static_assert(ARRAY_SIZE(_font) == 475, "_font is correct size"); #endif +Display_Backend *Display::probe_i2c_display(uint8_t bus, probe_i2c_display_probefn_t probe) +{ + + const auto dev_ptr = hal.i2c_mgr->get_device_ptr(bus, NOTIFY_DISPLAY_I2C_ADDR); + if (dev_ptr == nullptr) { + return nullptr; + } + Display_Backend *ret = probe(*dev_ptr); + if (ret == nullptr) { + delete dev_ptr; + } + return ret; +} + bool Display::init(void) { // exit immediately if already initialised @@ -338,18 +350,11 @@ bool Display::init(void) FOREACH_I2C(i) { switch (pNotify->_display_type) { case DISPLAY_SSD1306: { - const auto dev_ptr = hal.i2c_mgr->get_device_ptr(i, NOTIFY_DISPLAY_I2C_ADDR); - if (dev_ptr == nullptr) { - break; - } - _driver = Display_SSD1306_I2C::probe(*dev_ptr); - if (_driver == nullptr) { - delete dev_ptr; - } + _driver = probe_i2c_display(i, Display_SSD1306_I2C::probe); break; } case DISPLAY_SH1106: { - _driver = Display_SH1106_I2C::probe(std::move(hal.i2c_mgr->get_device(i, NOTIFY_DISPLAY_I2C_ADDR))); + _driver = probe_i2c_display(i, Display_SH1106_I2C::probe); break; } case DISPLAY_SITL: { diff --git a/libraries/AP_Notify/Display.h b/libraries/AP_Notify/Display.h index 6243a3696e6a0..aa0bbbc6f3412 100644 --- a/libraries/AP_Notify/Display.h +++ b/libraries/AP_Notify/Display.h @@ -23,6 +23,10 @@ class Display: public NotifyDevice { void send_text_blocking(const char *text, uint8_t line) override; void release_text(uint8_t line) override; private: + + using probe_i2c_display_probefn_t = Display_Backend* (*)(AP_HAL::Device&); + Display_Backend *probe_i2c_display(uint8_t bus, probe_i2c_display_probefn_t probe); + void draw_char(uint16_t x, uint16_t y, const char c); void draw_text(uint16_t x, uint16_t y, const char *c); void update_all(); diff --git a/libraries/AP_Notify/Display_SH1106_I2C.cpp b/libraries/AP_Notify/Display_SH1106_I2C.cpp index 275945210055f..3ee5a3719d373 100644 --- a/libraries/AP_Notify/Display_SH1106_I2C.cpp +++ b/libraries/AP_Notify/Display_SH1106_I2C.cpp @@ -14,24 +14,12 @@ */ #include "Display_SH1106_I2C.h" -#include - #include -#include +#include -// constructor -Display_SH1106_I2C::Display_SH1106_I2C(AP_HAL::OwnPtr dev) : - _dev(std::move(dev)) +Display_Backend *Display_SH1106_I2C::probe(AP_HAL::Device &_dev) { -} - -Display_SH1106_I2C::~Display_SH1106_I2C() -{ -} - -Display_SH1106_I2C *Display_SH1106_I2C::probe(AP_HAL::OwnPtr dev) -{ - Display_SH1106_I2C *driver = NEW_NOTHROW Display_SH1106_I2C(std::move(dev)); + Display_SH1106_I2C *driver = NEW_NOTHROW Display_SH1106_I2C(_dev); if (!driver || !driver->hw_init()) { delete driver; return nullptr; @@ -67,21 +55,14 @@ bool Display_SH1106_I2C::hw_init() memset(_displaybuffer, 0, SH1106_COLUMNS * SH1106_ROWS_PER_PAGE); - // take i2c bus semaphore - if (!_dev) { - return false; - } - _dev->get_semaphore()->take_blocking(); + WITH_SEMAPHORE(dev.get_semaphore()); // init display - bool success = _dev->transfer((uint8_t *)&init_seq, sizeof(init_seq), nullptr, 0); - - // give back i2c semaphore - _dev->get_semaphore()->give(); + bool success = dev.transfer((uint8_t *)&init_seq, sizeof(init_seq), nullptr, 0); if (success) { _need_hw_update = true; - _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&Display_SH1106_I2C::_timer, void)); + dev.register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&Display_SH1106_I2C::_timer, void)); } return success; @@ -114,12 +95,12 @@ void Display_SH1106_I2C::_timer() // write buffer to display for (uint8_t i = 0; i < (SH1106_ROWS / SH1106_ROWS_PER_PAGE); i++) { command.page = 0xB0 | (i & 0x0F); - _dev->transfer((uint8_t *)&command, sizeof(command), nullptr, 0); + dev.transfer((uint8_t *)&command, sizeof(command), nullptr, 0); memcpy(&display_buffer.db[0], &_displaybuffer[i * SH1106_COLUMNS], SH1106_COLUMNS/2); - _dev->transfer((uint8_t *)&display_buffer, SH1106_COLUMNS/2 + 1, nullptr, 0); + dev.transfer((uint8_t *)&display_buffer, SH1106_COLUMNS/2 + 1, nullptr, 0); memcpy(&display_buffer.db[0], &_displaybuffer[i * SH1106_COLUMNS + SH1106_COLUMNS/2 ], SH1106_COLUMNS/2); - _dev->transfer((uint8_t *)&display_buffer, SH1106_COLUMNS/2 + 1, nullptr, 0); + dev.transfer((uint8_t *)&display_buffer, SH1106_COLUMNS/2 + 1, nullptr, 0); } } diff --git a/libraries/AP_Notify/Display_SH1106_I2C.h b/libraries/AP_Notify/Display_SH1106_I2C.h index 32c36a5862a3c..d805d81436543 100644 --- a/libraries/AP_Notify/Display_SH1106_I2C.h +++ b/libraries/AP_Notify/Display_SH1106_I2C.h @@ -12,7 +12,7 @@ class Display_SH1106_I2C: public Display_Backend { public: - static Display_SH1106_I2C *probe(AP_HAL::OwnPtr dev); + static Display_Backend *probe(AP_HAL::Device &_dev); void hw_update() override; void set_pixel(uint16_t x, uint16_t y) override; @@ -21,8 +21,10 @@ class Display_SH1106_I2C: public Display_Backend { protected: - Display_SH1106_I2C(AP_HAL::OwnPtr dev); - ~Display_SH1106_I2C() override; + Display_SH1106_I2C(AP_HAL::Device &_dev) : + dev{_dev} + { } + ~Display_SH1106_I2C() override {}; private: @@ -30,7 +32,7 @@ class Display_SH1106_I2C: public Display_Backend { void _timer(); - AP_HAL::OwnPtr _dev; + AP_HAL::Device &dev; uint8_t _displaybuffer[SH1106_COLUMNS * SH1106_ROWS_PER_PAGE]; bool _need_hw_update; diff --git a/libraries/AP_Notify/Display_SSD1306_I2C.cpp b/libraries/AP_Notify/Display_SSD1306_I2C.cpp index 919f7e361eaa0..56b7e2ef15903 100644 --- a/libraries/AP_Notify/Display_SSD1306_I2C.cpp +++ b/libraries/AP_Notify/Display_SSD1306_I2C.cpp @@ -28,7 +28,7 @@ Display_SSD1306_I2C::~Display_SSD1306_I2C() } -Display_SSD1306_I2C *Display_SSD1306_I2C::probe(AP_HAL::Device &_dev) +Display_Backend *Display_SSD1306_I2C::probe(AP_HAL::Device &_dev) { Display_SSD1306_I2C *driver = NEW_NOTHROW Display_SSD1306_I2C(_dev); if (!driver || !driver->hw_init()) { diff --git a/libraries/AP_Notify/Display_SSD1306_I2C.h b/libraries/AP_Notify/Display_SSD1306_I2C.h index b74cecebb9a1e..665bc26846f8c 100644 --- a/libraries/AP_Notify/Display_SSD1306_I2C.h +++ b/libraries/AP_Notify/Display_SSD1306_I2C.h @@ -15,7 +15,7 @@ class Display_SSD1306_I2C: public Display_Backend { public: - static Display_SSD1306_I2C *probe(AP_HAL::Device &_dev); + static Display_Backend *probe(AP_HAL::Device &_dev); void hw_update() override; void set_pixel(uint16_t x, uint16_t y) override; diff --git a/libraries/AP_ONVIF/soap/onvif.h b/libraries/AP_ONVIF/soap/onvif.h index 4813b5793b43e..76cbec861a110 100644 --- a/libraries/AP_ONVIF/soap/onvif.h +++ b/libraries/AP_ONVIF/soap/onvif.h @@ -15055,7 +15055,7 @@ class tt__PTZPresetTourPresetDetailOptions /// Pointer to array tt__ReferenceToken of size 0..unbounded. tt__ReferenceToken *PresetToken 0; ///< Multiple elements. /// 

-///   An option to indicate Home postion for tour spots.
+///   An option to indicate Home position for tour spots.
 ///
/// /// Element "Home" of type xs:boolean. diff --git a/libraries/AP_Param/AP_Param.h b/libraries/AP_Param/AP_Param.h index 48bd57ab3b99d..bc99653a7ae61 100644 --- a/libraries/AP_Param/AP_Param.h +++ b/libraries/AP_Param/AP_Param.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -121,13 +122,19 @@ #define AP_PARAM_FRAME_HELI (1<<5) #define AP_PARAM_FRAME_BLIMP (1<<6) -// a variant of offsetof() to work around C++ restrictions. -// this can only be used when the offset of a variable in a object -// is constant and known at compile time -#define AP_VAROFFSET(type, element) (((ptrdiff_t)(&((const type *)1)->element))-1) - -// find the type of a variable given the class and element -#define AP_CLASSTYPE(clazz, element) ((uint8_t)(((const clazz *) 1)->element.vtype)) +// use __builtin_offsetof which is more or less defined by Clang and GCC to work +// on non-standard-layout C++ classes, and works in constexpr. as that isn't +// standard-compliant, it raises "-Winvalid-offsetof" which we globally disable. +// https://github.com/llvm/llvm-project/blob/5fa5ffeb6cb5bc9aa414c02513e44b8405f0e7cc/libcxx/include/__type_traits/datasizeof.h#L51 +// the first comma operator operand makes the compiler see element as used by +// conjuring a class instance and passing element to an unevaluating function. +#define AP_VAROFFSET(clazz, element) ((void)sizeof(std::declval().element), (ptrdiff_t)__builtin_offsetof(clazz, element)) + +// get the internal type of an AP_Param variable given an arbitrary class and an +// element on it. convert the element, which must be of type AP_Param or a +// subclass, into a non-reference type. then get AP_Param's vtype member, which +// is static const but varies depending on the subclass. works in constexpr! +#define AP_CLASSTYPE(clazz, element) ((uint8_t)(std::remove_reference::type::vtype)) // declare a group var_info line #define AP_GROUPINFO_FLAGS(name, idx, clazz, element, def, flags) { name, AP_VAROFFSET(clazz, element), {def_value : def}, flags, idx, AP_CLASSTYPE(clazz, element)} diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp index 4c389fdf83551..d6920538d62ee 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp @@ -16,13 +16,15 @@ */ +#include "AP_PiccoloCAN_config.h" + +#if AP_PICCOLOCAN_ENABLED + #include #include #include "AP_PiccoloCAN.h" -#if HAL_PICCOLO_CAN_ENABLE - #include #include #include @@ -792,4 +794,4 @@ bool AP_PiccoloCAN::pre_arm_check(char* reason, uint8_t reason_len) } -#endif // HAL_PICCOLO_CAN_ENABLE +#endif // AP_PICCOLOCAN_ENABLED diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h index 9869c2f91fa8b..da67155478762 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h @@ -17,6 +17,10 @@ #pragma once +#include "AP_PiccoloCAN_config.h" + +#if AP_PICCOLOCAN_ENABLED + #include #include @@ -31,8 +35,6 @@ #include "AP_PiccoloCAN_Servo.h" #include -#if HAL_PICCOLO_CAN_ENABLE - #define PICCOLO_MSG_RATE_HZ_MIN 1 #define PICCOLO_MSG_RATE_HZ_MAX 500 #define PICCOLO_MSG_RATE_HZ_DEFAULT 50 @@ -137,4 +139,4 @@ class AP_PiccoloCAN : public AP_CANDriver, public AP_ESC_Telem_Backend }; -#endif // HAL_PICCOLO_CAN_ENABLE +#endif // AP_PICCOLOCAN_ENABLED diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Cortex.cpp b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Cortex.cpp index 28a408832d64c..4ecfaf23cac4f 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Cortex.cpp +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Cortex.cpp @@ -17,7 +17,7 @@ #include "AP_PiccoloCAN_Cortex.h" -#if HAL_PICCOLO_CAN_ENABLE +#if AP_PICCOLOCAN_ENABLED // Protocol files for the Cortex controller #include @@ -91,4 +91,4 @@ uint32_t getCortexPacketID(const void* pkt) return (uint32_t) ((frame->id >> 16) & 0xFF); } -#endif // HAL_PICCOLO_CAN_ENABLE +#endif // AP_PICCOLOCAN_ENABLED diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Device.h b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Device.h index 076a19b9fc2aa..781030e9f312f 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Device.h +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Device.h @@ -17,14 +17,13 @@ #pragma once -#include -#include -#include - #include "AP_PiccoloCAN_config.h" +#if AP_PICCOLOCAN_ENABLED -#if HAL_PICCOLO_CAN_ENABLE +#include +#include +#include //! Piccolo message groups form part of the CAN ID of each frame enum class PiccoloCAN_MessageGroup : uint8_t { @@ -76,4 +75,4 @@ class AP_PiccoloCAN_Device uint64_t last_msg_timestamp = 0; }; -#endif // HAL_PICCOLO_CAN_ENABLE +#endif // AP_PICCOLOCAN_ENABLED diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ECU.cpp b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ECU.cpp index c4343a368507a..395cf1f74e462 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ECU.cpp +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ECU.cpp @@ -15,9 +15,11 @@ * Author: Oliver Walters / Currawong Engineering Pty Ltd */ -#include "AP_PiccoloCAN_ECU.h" +#include "AP_PiccoloCAN_config.h" + +#if AP_PICCOLOCAN_ENABLED -#if HAL_PICCOLO_CAN_ENABLE +#include "AP_PiccoloCAN_ECU.h" // Protocol files for the ECU #include @@ -91,4 +93,4 @@ uint32_t getECUPacketID(const void* pkt) return (uint32_t) ((frame->id >> 16) & 0xFF); } -#endif // HAL_PICCOLO_CAN_ENABLE \ No newline at end of file +#endif // AP_PICCOLOCAN_ENABLED diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ECU.h b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ECU.h index e014e7d29559d..7e34bc6a5a5a8 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ECU.h +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ECU.h @@ -15,9 +15,12 @@ * Author: Oliver Walters / Currawong Engineering Pty Ltd */ - #pragma once +#include "AP_PiccoloCAN_config.h" + +#if AP_PICCOLOCAN_ENABLED + #include #include "AP_PiccoloCAN_config.h" @@ -26,8 +29,6 @@ #define PICCOLO_CAN_ECU_ID_DEFAULT 0 -#if HAL_PICCOLO_CAN_ENABLE - /* * Class representing an individual PiccoloCAN ECU */ @@ -37,4 +38,4 @@ class AP_PiccoloCAN_ECU : public AP_PiccoloCAN_Device // TODO }; -#endif // HAL_PICCOLO_CAN_ENABLE +#endif // AP_PICCOLOCAN_ENABLED diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ESC.cpp b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ESC.cpp index 3c9ef3d29318f..70d92c518709d 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ESC.cpp +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ESC.cpp @@ -15,9 +15,11 @@ * Author: Oliver Walters / Currawong Engineering Pty Ltd */ -#include "AP_PiccoloCAN_ESC.h" +#include "AP_PiccoloCAN_config.h" + +#if AP_PICCOLOCAN_ENABLED -#if HAL_PICCOLO_CAN_ENABLE +#include "AP_PiccoloCAN_ESC.h" /* * Decode a received CAN frame. @@ -148,4 +150,4 @@ uint32_t getESCVelocityPacketID(const void* pkt) return (uint32_t) ((frame->id >> 16) & 0xFF); } -#endif // HAL_PICCOLO_CAN_ENABLE +#endif // AP_PICCOLOCAN_ENABLED diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ESC.h b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ESC.h index 9ac96db318d07..9a4c5efffffc9 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ESC.h +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ESC.h @@ -15,9 +15,12 @@ * Author: Oliver Walters / Currawong Engineering Pty Ltd */ - #pragma once +#include "AP_PiccoloCAN_config.h" + +#if AP_PICCOLOCAN_ENABLED + #include #include #include @@ -26,8 +29,6 @@ #include "AP_PiccoloCAN_Device.h" #include "piccolo_protocol/ESCPackets.h" -#if HAL_PICCOLO_CAN_ENABLE - #define PICCOLO_CAN_MAX_NUM_ESC 16 #define PICCOLO_CAN_MAX_GROUP_ESC (PICCOLO_CAN_MAX_NUM_ESC / 4) @@ -74,4 +75,4 @@ class AP_PiccoloCAN_ESC : public AP_PiccoloCAN_Device, public AP_ESC_Telem_Backe } settings; }; -#endif // HAL_PICCOLO_CAN_ENABLE +#endif // AP_PICCOLOCAN_ENABLED diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Servo.cpp b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Servo.cpp index 93cffd3bb6de0..706f88c70c1a5 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Servo.cpp +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Servo.cpp @@ -15,9 +15,11 @@ * Author: Oliver Walters / Currawong Engineering Pty Ltd */ -#include "AP_PiccoloCAN_Servo.h" +#include "AP_PiccoloCAN_config.h" + +#if AP_PICCOLOCAN_ENABLED -#if HAL_PICCOLO_CAN_ENABLE +#include "AP_PiccoloCAN_Servo.h" /* * Decode a recevied CAN frame. @@ -115,4 +117,4 @@ uint32_t getServoPacketID(const void* pkt) return (uint32_t) ((frame->id >> 16) & 0xFF); } -#endif // HAL_PICCOLO_CAN_ENABLE \ No newline at end of file +#endif // AP_PICCOLOCAN_ENABLED diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Servo.h b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Servo.h index d99c82c94c2b7..0ee47f60c31b2 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Servo.h +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Servo.h @@ -17,14 +17,16 @@ #pragma once +#include "AP_PiccoloCAN_config.h" + +#if AP_PICCOLOCAN_ENABLED + #include #include "AP_PiccoloCAN_config.h" #include "AP_PiccoloCAN_Device.h" #include "piccolo_protocol/ServoPackets.h" -#if HAL_PICCOLO_CAN_ENABLE - #define PICCOLO_CAN_MAX_NUM_SERVO 16 #define PICCOLO_CAN_MAX_GROUP_SERVO (PICCOLO_CAN_MAX_NUM_SERVO / 4) @@ -67,4 +69,4 @@ class AP_PiccoloCAN_Servo : public AP_PiccoloCAN_Device } settings; }; -#endif // HAL_PICCOLO_CAN_ENABLE +#endif // AP_PICCOLOCAN_ENABLED diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_config.h b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_config.h index 3733da2689c54..1ef47e233953b 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_config.h +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_config.h @@ -17,8 +17,8 @@ #pragma once -#include +#include -#ifndef HAL_PICCOLO_CAN_ENABLE -#define HAL_PICCOLO_CAN_ENABLE HAL_NUM_CAN_IFACES -#endif +#ifndef AP_PICCOLOCAN_ENABLED +#define AP_PICCOLOCAN_ENABLED HAL_NUM_CAN_IFACES +#endif // AP_PICCOLOCAN_ENABLED diff --git a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp index f978cb07f2b7c..3300394bb186d 100644 --- a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp +++ b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp @@ -2000,6 +2000,10 @@ AP_CRSF_Telem::ScriptedMenu* AP_CRSF_Telem::ScriptedMenu::add_menu(const char* m } ScriptedMenu* menu = NEW_NOTHROW ScriptedMenu(menu_name, size, parent_menu); + if (menu == nullptr) { + return nullptr; + } + menu->id = tail->id == 0 ? SCRIPTED_MENU_START_ID : tail->id + MAX_SCRIPTED_MENU_SIZE + 1; tail->next_menu = menu; diff --git a/libraries/AP_Relay/AP_Relay.cpp b/libraries/AP_Relay/AP_Relay.cpp index d94f764c69946..4fb4476f47829 100644 --- a/libraries/AP_Relay/AP_Relay.cpp +++ b/libraries/AP_Relay/AP_Relay.cpp @@ -344,12 +344,11 @@ void AP_Relay::init() if ((default_state == AP_Relay_Params::DefaultState::OFF) || (default_state == AP_Relay_Params::DefaultState::ON)) { - set_pin_by_instance(instance, (bool)default_state); + set_instance_state(instance, (bool)default_state); } } else { // all functions are supposed to be off by default - // this will need revisiting when we support inversion - set_pin_by_instance(instance, false); + set_instance_state(instance, false); } // Make sure any DroneCAN pin is enabled for streaming @@ -371,13 +370,13 @@ void AP_Relay::set(const AP_Relay_Params::FUNCTION function, const bool value) { continue; } - set_pin_by_instance(instance, value); + set_instance_state(instance, value); } } // set a pins output state by instance and log if required // this is an internal helper, instance must have already been validated to be in range -void AP_Relay::set_pin_by_instance(uint8_t instance, bool value) +void AP_Relay::set_instance_state(uint8_t instance, bool value) { const int16_t pin = _params[instance].pin; if (pin == -1) { @@ -391,14 +390,14 @@ void AP_Relay::set_pin_by_instance(uint8_t instance, bool value) } #endif - const bool initial_value = get_pin(pin); + const bool initial_value = get_pin_state(pin); if (_params[instance].inverted > 0) { value = !value; } if (initial_value != value) { - set_pin(pin, value); + set_pin_state(pin, value); #if HAL_LOGGING_ENABLED // @LoggerMessage: RELY // @Description: Relay state @@ -428,7 +427,7 @@ void AP_Relay::set(const uint8_t instance, const bool value) return; } - set_pin_by_instance(instance, value); + set_instance_state(instance, value); } void AP_Relay::toggle(uint8_t instance) @@ -496,14 +495,14 @@ bool AP_Relay::get(uint8_t instance) const } if (_params[instance].inverted > 0) { - return !get_pin(_params[instance].pin.get()); + return !get_pin_state(_params[instance].pin.get()); } - return get_pin(_params[instance].pin.get()); + return get_pin_state(_params[instance].pin.get()); } // Get relay state from pin number -bool AP_Relay::get_pin(const int16_t pin) const +bool AP_Relay::get_pin_state(const int16_t pin) const { if (pin < 0) { // invalid pin @@ -513,7 +512,7 @@ bool AP_Relay::get_pin(const int16_t pin) const #if AP_RELAY_DRONECAN_ENABLED if (dronecan.valid_pin(pin)) { // Virtual DroneCAN pin - return dronecan.get_pin(pin); + return dronecan.get_pin_state(pin); } #endif @@ -523,7 +522,7 @@ bool AP_Relay::get_pin(const int16_t pin) const } // Set relay state from pin number -void AP_Relay::set_pin(const int16_t pin, const bool value) +void AP_Relay::set_pin_state(const int16_t pin, const bool value) { if (pin < 0) { // invalid pin @@ -533,7 +532,7 @@ void AP_Relay::set_pin(const int16_t pin, const bool value) #if AP_RELAY_DRONECAN_ENABLED if (dronecan.valid_pin(pin)) { // Virtual DroneCAN pin - dronecan.set_pin(pin, value); + dronecan.set_pin_state(pin, value); return; } #endif @@ -543,6 +542,15 @@ void AP_Relay::set_pin(const int16_t pin, const bool value) hal.gpio->write(pin, value); } +// Get GPIO pin from instance +bool AP_Relay::get_pin_by_instance(uint8_t instance, uint8_t &pin) const +{ + if (instance >= ARRAY_SIZE(_params)) { + return false; + } + pin = _params[instance].pin; + return true; +} // see if the relay is enabled bool AP_Relay::enabled(uint8_t instance) const { @@ -591,7 +599,7 @@ uint8_t AP_Relay::DroneCAN::hardpoint_index(const int16_t pin) const } // Set DroneCAN relay state from pin number -void AP_Relay::DroneCAN::set_pin(const int16_t pin, const bool value) +void AP_Relay::DroneCAN::set_pin_state(const int16_t pin, const bool value) { const uint8_t index = hardpoint_index(pin); @@ -617,7 +625,7 @@ void AP_Relay::DroneCAN::set_pin(const int16_t pin, const bool value) } // Get relay state from pin number, this relies on a cached value, assume remote pin is in sync -bool AP_Relay::DroneCAN::get_pin(const int16_t pin) const +bool AP_Relay::DroneCAN::get_pin_state(const int16_t pin) const { const uint8_t index = hardpoint_index(pin); return state[index].value; diff --git a/libraries/AP_Relay/AP_Relay.h b/libraries/AP_Relay/AP_Relay.h index 40e2af62c10a2..45d15073924b9 100644 --- a/libraries/AP_Relay/AP_Relay.h +++ b/libraries/AP_Relay/AP_Relay.h @@ -68,31 +68,37 @@ class AP_Relay { bool send_relay_status(const class GCS_MAVLINK &link) const; + // Set the state of all relays that are configured as the specified function type void set(AP_Relay_Params::FUNCTION function, bool value); // see if the relay is enabled bool enabled(AP_Relay_Params::FUNCTION function) const; + // Get the pin number of the instance (if assigned to a pin) + bool get_pin_by_instance(uint8_t instance, uint8_t &pin) const; + private: static AP_Relay *singleton; AP_Relay_Params _params[AP_RELAY_NUM_RELAYS]; - // Return true is function is valid + // Return true if function is valid bool function_valid(AP_Relay_Params::FUNCTION function) const; + // Set the state of the specified instance, if it is a valid relay void set(uint8_t instance, bool value); void set_defaults(); void convert_params(); - void set_pin_by_instance(uint8_t instance, bool value); + // Internal helper: set the instance state, accounting for configured inversion + void set_instance_state(uint8_t instance, bool value); - // Set relay state from pin number - void set_pin(const int16_t pin, const bool value); + // Internal helper: directly set the state of the specified pin + void set_pin_state(const int16_t pin, const bool value); - // Get relay state from pin number - bool get_pin(const int16_t pin) const; + // Get the state of the specified pin + bool get_pin_state(const int16_t pin) const; #if AP_RELAY_DRONECAN_ENABLED // Virtual DroneCAN pins @@ -108,10 +114,10 @@ class AP_Relay { bool populate_next_command(uint8_t &index, uavcan_equipment_hardpoint_Command &msg) const; // Set DroneCAN relay state from pin number - void set_pin(const int16_t pin, const bool value); + void set_pin_state(const int16_t pin, const bool value); // Get relay state from pin number - bool get_pin(const int16_t pin) const; + bool get_pin_state(const int16_t pin) const; private: diff --git a/libraries/AP_Scripting/AP_Scripting.cpp b/libraries/AP_Scripting/AP_Scripting.cpp index 6c780fa2fa9be..4a3ff370bea28 100644 --- a/libraries/AP_Scripting/AP_Scripting.cpp +++ b/libraries/AP_Scripting/AP_Scripting.cpp @@ -294,15 +294,6 @@ MAV_RESULT AP_Scripting::handle_command_int_packet(const mavlink_command_int_t & } #endif -/* - avoid optimisation of the thread function. This avoids nasty traps - where setjmp/longjmp does not properly handle save/restore of - floating point registers on exceptions. This is an extra protection - over the top of the fix in luaD_rawrunprotected() for the same issue - */ -#pragma GCC push_options -#pragma GCC optimize ("O0") - void AP_Scripting::thread(void) { while (true) { // reset flags @@ -398,7 +389,6 @@ void AP_Scripting::thread(void) { } } } -#pragma GCC pop_options void AP_Scripting::handle_mission_command(const AP_Mission::Mission_Command& cmd_in) { diff --git a/libraries/AP_Scripting/AP_Scripting_CRSFMenu.cpp b/libraries/AP_Scripting/AP_Scripting_CRSFMenu.cpp index afac86e429900..bb2d2196efdd8 100644 --- a/libraries/AP_Scripting/AP_Scripting_CRSFMenu.cpp +++ b/libraries/AP_Scripting/AP_Scripting_CRSFMenu.cpp @@ -10,17 +10,11 @@ AP_CRSF_Telem::ScriptedParameter* CRSFMenu::add_parameter(uint8_t length, const char* data) { - if (menu == nullptr) { - return nullptr; - } return menu->add_parameter(length, data); } AP_CRSF_Telem::ScriptedMenu* CRSFMenu::add_menu(const char* menu_name) { - if (menu == nullptr) { - return nullptr; - } return menu->add_menu(menu_name, 2, menu->id); } @@ -29,17 +23,17 @@ int lua_CRSF_new_menu(lua_State *L) binding_argcheck(L, 1); const char * name = luaL_checkstring(L, 1); - void *ud = lua_newuserdata(L, sizeof(CRSFMenu)); + + // create Lua object first in case of errors so we don't leak the menu + auto *m = new_CRSFMenu(L); AP_CRSF_Telem::ScriptedMenu* menu = AP::crsf_telem()->add_menu(name); if (menu == nullptr) { - return luaL_error(L, "No menu named: %s", name); + return luaL_error(L, "out of memory"); } - new (ud) CRSFMenu(menu); - luaL_getmetatable(L, "CRSFMenu"); - lua_setmetatable(L, -2); + *m = CRSFMenu(menu); return 1; } @@ -107,14 +101,14 @@ int lua_CRSF_add_parameter(lua_State *L) CRSFMenu* ud = check_CRSFMenu(L, 1); size_t len = 0; const char * data = luaL_checklstring(L, 2, &len); + + // create Lua object first in case of errors so we don't leak the param + auto *p = new_CRSFParameter(L); + AP_CRSF_Telem::ScriptedParameter* param = ud->add_parameter(len, data); if (param) { -#if 2 > LUA_MINSTACK - luaL_checkstack(L, 2, nullptr); -#endif - - *new_CRSFParameter(L) = CRSFParameter(ud->menu, param); + *p = CRSFParameter(ud->get_menu(), param); return 1; } return 0; @@ -126,13 +120,14 @@ int lua_CRSF_add_menu(lua_State *L) CRSFMenu* ud = check_CRSFMenu(L, 1); const char * name = luaL_checkstring(L, 2); + + // create Lua object first in case of errors so we don't leak the menu + auto *m = new_CRSFMenu(L); + AP_CRSF_Telem::ScriptedMenu* menu = ud->add_menu(name); if (menu) { -#if 2 > LUA_MINSTACK - luaL_checkstack(L, 2, nullptr); -#endif - *new_CRSFMenu(L) = CRSFMenu(menu); + *m = CRSFMenu(menu); return 1; } return 0; @@ -143,14 +138,14 @@ int lua_CRSF_add_root_menu(lua_State *L) binding_argcheck(L, 2); const char * name = luaL_checkstring(L, 2); + + // create Lua object first in case of errors so we don't leak the menu + auto *m = new_CRSFMenu(L); + AP_CRSF_Telem::ScriptedMenu* menu = AP::crsf_telem()->add_menu(name); if (menu) { -#if 2 > LUA_MINSTACK - luaL_checkstack(L, 2, nullptr); -#endif - - *new_CRSFMenu(L) = CRSFMenu(menu); + *m = CRSFMenu(menu); return 1; } return 0; @@ -162,6 +157,10 @@ int lua_CRSF_param_data(lua_State *L) CRSFParameter * ud = check_CRSFParameter(L, 1); AP_CRSF_Telem::ScriptedParameter* param = ud->get_parameter(); + if (param == nullptr) { + return luaL_error(L, "internal error: %s is null", "CRSFParameter"); + } + lua_pushlstring(L, param->data, param->length); return 1; } diff --git a/libraries/AP_Scripting/AP_Scripting_CRSFMenu.h b/libraries/AP_Scripting/AP_Scripting_CRSFMenu.h index ad9cfbcf354e7..42322c931ce12 100644 --- a/libraries/AP_Scripting/AP_Scripting_CRSFMenu.h +++ b/libraries/AP_Scripting/AP_Scripting_CRSFMenu.h @@ -38,9 +38,12 @@ class CRSFMenu { uint8_t id() const { return menu->id; } const char* name() const { return menu->name; } uint8_t num_params() const { return menu->num_params; } + AP_CRSF_Telem::ScriptedMenu* get_menu() const { return menu; } AP_CRSF_Telem::ScriptedParameter* add_parameter(uint8_t length, const char* data); AP_CRSF_Telem::ScriptedMenu* add_menu(const char* menu_name); + +private: AP_CRSF_Telem::ScriptedMenu* menu; }; diff --git a/libraries/AP_Scripting/AP_Scripting_config.h b/libraries/AP_Scripting/AP_Scripting_config.h index 92a8c81420dbc..6b48febb0062a 100644 --- a/libraries/AP_Scripting/AP_Scripting_config.h +++ b/libraries/AP_Scripting/AP_Scripting_config.h @@ -2,6 +2,7 @@ #include #include +#include #ifndef AP_SCRIPTING_ENABLED #define AP_SCRIPTING_ENABLED (HAL_PROGRAM_SIZE_LIMIT_KB > 1024) @@ -21,7 +22,7 @@ // bindings configuration #ifndef AP_SCRIPTING_BINDING_MOTORS_ENABLED -#define AP_SCRIPTING_BINDING_MOTORS_ENABLED 1 +#define AP_SCRIPTING_BINDING_MOTORS_ENABLED (AP_SCRIPTING_ENABLED && AP_VEHICLE_ENABLED) #endif #ifndef AP_SCRIPTING_BINDING_VEHICLE_ENABLED diff --git a/libraries/AP_Scripting/AP_Scripting_helpers.cpp b/libraries/AP_Scripting/AP_Scripting_helpers.cpp index 92854b168d6de..8943137d39e29 100644 --- a/libraries/AP_Scripting/AP_Scripting_helpers.cpp +++ b/libraries/AP_Scripting/AP_Scripting_helpers.cpp @@ -19,18 +19,14 @@ int lua_new_Parameter(lua_State *L) { name = luaL_checkstring(L, 1); } - // This chunk is the same as the auto generated constructor - void *ud = lua_newuserdata(L, sizeof(Parameter)); - new (ud) Parameter(); - luaL_getmetatable(L, "Parameter"); - lua_setmetatable(L, -2); + auto *p = new_Parameter(L); if (args == 0) { // no arguments, nothing to do return 1; } - if (!static_cast(ud)->init(name)) { + if (!p->init(name)) { return luaL_error(L, "No parameter: %s", name); } @@ -445,11 +441,7 @@ int DroneCAN_Handle::new_handle(lua_State *L) return 0; } - // This chunk is the same as the auto generated constructor - void *ud = lua_newuserdata(L, sizeof(DroneCAN_Handle)); - new (ud) DroneCAN_Handle(); - - auto *h = static_cast(ud); + auto *h = new_DroneCAN_Handle(L); h->dc = dc; h->signature = sig; @@ -458,9 +450,6 @@ int DroneCAN_Handle::new_handle(lua_State *L) h->canfd = send_canfd; #endif - luaL_getmetatable(L, "DroneCAN_Handle"); - lua_setmetatable(L, -2); - return 1; } diff --git a/libraries/AP_Scripting/README.md b/libraries/AP_Scripting/README.md index a13db27a305cc..16c4457ac8187 100644 --- a/libraries/AP_Scripting/README.md +++ b/libraries/AP_Scripting/README.md @@ -2,7 +2,7 @@ ## Enabling Scripting Support in Builds -Scripting is automatically enabled on all boards with at least 1MB of flash space. +Scripting is automatically enabled on all boards with more than 1MB of flash space. The following example enables scripting, builds the ArduPlane firmware for the Cube, and uploads it. ``` diff --git a/libraries/AP_Scripting/applets/RockBlock.lua b/libraries/AP_Scripting/applets/RockBlock.lua index cec3a278a89a6..f8314361ab8b8 100644 --- a/libraries/AP_Scripting/applets/RockBlock.lua +++ b/libraries/AP_Scripting/applets/RockBlock.lua @@ -448,7 +448,7 @@ local function RockblockModem() local _modem_history = {} local _modem_to_send = {} - local _str_recieved = "" + local _str_received = "" -- Get any incoming data function self.rxdata(inchar) @@ -457,30 +457,30 @@ local function RockblockModem() if _modem_history[#_modem_history] == 'AT+SBDRB' and self.in_read_cycle == false and self.rx_len > 0 then -- read buffer may include /r and /n, so need a special cycle to capture all up to the self.rx_len self.in_read_cycle = true - _str_recieved = _str_recieved .. read - elseif self.in_read_cycle and #_str_recieved == self.rx_len + 3 then + _str_received = _str_received .. read + elseif self.in_read_cycle and #_str_received == self.rx_len + 3 then -- get last byte in read cycle - _str_recieved = _str_recieved .. read + _str_received = _str_received .. read self.in_read_cycle = false self.rx_len = 0 - table.insert(_modem_history, _str_recieved) - _str_recieved = "" + table.insert(_modem_history, _str_received) + _str_received = "" if RCK_DEBUG:get() == 1 then gcs:send_text(3, "Rockblock: Modem rx msg: " .. self.nicestring(_modem_history[#_modem_history])) end elseif (read == '\r' or read == '\n') and not self.in_read_cycle then - if #_str_recieved > 0 then - table.insert(_modem_history, _str_recieved) + if #_str_received > 0 then + table.insert(_modem_history, _str_received) if RCK_DEBUG:get() == 1 then gcs:send_text(3, "Rockblock: Modem response: " .. self.nicestring(_modem_history[#_modem_history])) end maybepkt = self.check_cmd_return() end - _str_recieved = "" + _str_received = "" else - _str_recieved = _str_recieved .. read + _str_received = _str_received .. read end return maybepkt end diff --git a/libraries/AP_Scripting/applets/RockBlock.md b/libraries/AP_Scripting/applets/RockBlock.md index 467f2a4a7f806..1caa52c2b56d4 100644 --- a/libraries/AP_Scripting/applets/RockBlock.md +++ b/libraries/AP_Scripting/applets/RockBlock.md @@ -47,4 +47,4 @@ Enables the modem transmission If RCK_FORCEHL=2, this is the number of seconds of no-messages from the GCS until High Latency mode is auto-enabled -When GCS messages are recieved again, High Latency mode is auto-disabled. \ No newline at end of file +When GCS messages are received again, High Latency mode is auto-disabled. \ No newline at end of file diff --git a/libraries/AP_Scripting/applets/arming-checks.lua b/libraries/AP_Scripting/applets/arming-checks.lua index 98b5e48b04e28..c7ac35eab528b 100644 --- a/libraries/AP_Scripting/applets/arming-checks.lua +++ b/libraries/AP_Scripting/applets/arming-checks.lua @@ -10,7 +10,7 @@ -- -- Thanks to @yuri_rage and Peter Barker for help with the Lua and Autotests -SCRIPT_VERSION = "4.7.0-021" +SCRIPT_VERSION = "4.7.0-022" SCRIPT_NAME = "Arming Checks" SCRIPT_NAME_SHORT = "ArmCk" @@ -116,7 +116,8 @@ FOLL_SYSID = Parameter("FOLL_SYSID") RALLY_LIMIT_KM = Parameter("RALLY_LIMIT_KM") if FWVersion:type() == FIRMWARE.COPTER then -- Copter specific paramters - RTL_ALT = Parameter("RTL_ALT") + RTL_ALTITUDE = Parameter("RTL_ALT_M") + RTL_CLIMB_MIN = Parameter("RTL_CLIMB_MIN_M") elseif FWVersion:type() == FIRMWARE.PLANE then -- Plane specific Parameters RTL_ALTITUDE = Parameter("RTL_ALTITUDE") RTL_CLIMB_MIN = Parameter("RTL_CLIMB_MIN") @@ -218,7 +219,7 @@ end -- is the RTL altitude legal? This defaults to 120m (400') as set in ARM_ALT_LEGAL local function rtl_altitude_legal() - -- plane uses RTL_ALTITUDE in m + -- plane uses RTL_ALTITUDE in meters, copter uses RTL_ALT_M also in meters -- RTL_ALTITUDE will be correct for either plane or copter if (RTL_ALTITUDE ~= nil and (RTL_ALTITUDE:get()) > alt_legal_max) then return false @@ -469,14 +470,14 @@ local arming_checks = {} -- ArduCopter specific checks. Note that the index number starts from 1 for FIRMWARE.COPTER (ARM_C_ parameters) if FWVersion:type() == FIRMWARE.COPTER then -- add Copter specific Parameters --[[ - // @Param: ARM_C_RTL_ALT - // @DisplayName: RTL_ALT should be a valid value - // @Description: RTL_ALT should be < 120m (400ft). 3 or less to prevent arming. -1 to disable. + // @Param: ARM_C_RTL_ALT_M + // @DisplayName: RTL_ALT_M should be a valid value + // @Description: RTL_ALT_M should be < 120m (400ft). 3 or less to prevent arming. -1 to disable. // @Values: -1:Disabled,0:Emergency(PreArm),1:Alert(PreArm),2:Critical(PreArm),3:Error(PreArm),4:Warning,5:Notice,6:Info,7:Debug // @User: Standard --]] table.insert(arming_checks, Arming_Check(FIRMWARE.COPTER, 1, - rtl_altitude_legal, "RTL_ALT", "RTL_AL too high", MAV_SEVERITY.ERROR, false, false)) + rtl_altitude_legal, "RTL_ALT", "RTL_ALT_M too high", MAV_SEVERITY.ERROR, false, false)) elseif FWVersion:type() == FIRMWARE.PLANE then -- add Plane specific Parameters -- ArduPlane specific checks. Note that the index number starts from 1 for FIRMWARE.PLANE (ARM_P_ parameters) --[[ diff --git a/libraries/AP_Scripting/applets/arming-checks.md b/libraries/AP_Scripting/applets/arming-checks.md index 6733c47b8a781..c45289f391357 100644 --- a/libraries/AP_Scripting/applets/arming-checks.md +++ b/libraries/AP_Scripting/applets/arming-checks.md @@ -54,9 +54,9 @@ If there is a rally point more than RALLY_LIMIT_KM kilometers from the home loca The following checks only apply if the firmware is ArduCopter -# ARM_C_RTL_ALT +# ARM_C_RTL_ALT_M -This check is to by default warn if the vehicle RTL_ALT has been set to a value > ARM_V_ALT_LEGAL. +This check is to by default warn if the vehicle RTL_ALT_M has been set to a value > ARM_V_ALT_LEGAL. # Plane specific checks diff --git a/libraries/AP_Scripting/applets/crsf-calibrate.lua b/libraries/AP_Scripting/applets/crsf-calibrate.lua index afeb2d0f702ea..a48790ae3e6a7 100644 --- a/libraries/AP_Scripting/applets/crsf-calibrate.lua +++ b/libraries/AP_Scripting/applets/crsf-calibrate.lua @@ -1,159 +1,166 @@ --[[ --- A script to perform ArduPilot calibrations using a custom CRSF menu --- copy this script to the autopilot's "scripts" directory +-- A script to perform ArduPilot calibrations using a custom CRSF menu. +-- This script uses the enhanced crsf_helper.lua library (v6.1+) --]] -SCRIPT_NAME = "ArduPilot Calibration" -SCRIPT_NAME_SHORT = "Calibration" -SCRIPT_VERSION = "0.1" - -MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} -CRSF_EVENT = {PARAMETER_READ=1, PARAMETER_WRITE=2} -CRSF_PARAM_TYPE = { - UINT8 = 0, -- deprecated - INT8 = 1, -- deprecated - UINT16 = 2, -- deprecated - INT16 = 3, -- deprecated - FLOAT = 8, - TEXT_SELECTION = 9, - STRING = 10, - FOLDER = 11, - INFO = 12, - COMMAND = 13, -} +-- Load the CRSF helper library and its shared constants +local crsf_helper = require('crsf_helper') +local MAV_SEVERITY = crsf_helper.MAV_SEVERITY +local CRSF_COMMAND_STATUS = crsf_helper.CRSF_COMMAND_STATUS -CRSF_COMMAND_STATUS = { - READY = 0, -- --> feedback - START = 1, -- <-- input - PROGRESS = 2, -- --> feedback - CONFIRMATION_NEEDED = 3, -- --> feedback - CONFIRM = 4, -- <-- input - CANCEL = 5, -- <-- input - POLL = 6 -- <-- input -} +-- MAVLink command constants +local MAV_CMD_PREFLIGHT_CALIBRATION = 241 +local MAV_CMD_DO_START_MAG_CAL = 42424 +local MAV_CMD_DO_ACCEPT_MAG_CAL = 42425 +local MAV_CMD_DO_CANCEL_MAG_CAL = 42426 -MAV_CMD_PREFLIGHT_CALIBRATION = 241 -MAV_CMD_DO_START_MAG_CAL = 42424 -MAV_CMD_DO_ACCEPT_MAG_CAL = 42425 -MAV_CMD_DO_CANCEL_MAG_CAL = 42426 +-- This script-local variable tracks the state of the multi-step compass calibration +local compass_cal_running = false --- create a CRSF menu float item -function create_float_entry(name, value, min, max, default, dpoint, step, unit) - return string.pack(">BzllllBlz", CRSF_PARAM_TYPE.FLOAT, name, value, min, max, default, dpoint, step, unit) -end +-- #################### +-- # CALLBACK FUNCTIONS +-- #################### + +-- These functions are called by the helper's event loop. +-- They must return the new status and info text for the command item. + +--- Handles the multi-step compass calibration logic. +local function callback_compass_cal(command_action) + if command_action == CRSF_COMMAND_STATUS.START then + -- User pressed "Execute" + if compass_cal_running then + -- Was already running, cancel it first just in case + gcs:run_command_int(MAV_CMD_DO_CANCEL_MAG_CAL, { p3 = 1 }) + end + + -- Start the new calibration + compass_cal_running = true + gcs:run_command_int(MAV_CMD_DO_START_MAG_CAL, { p3 = 1 }) + gcs:send_text(MAV_SEVERITY.INFO, "Compass cal running...") + + -- Tell the helper to update the TX UI to "Accept" + return CRSF_COMMAND_STATUS.CONFIRMATION_NEEDED, "Accept" + + elseif command_action == CRSF_COMMAND_STATUS.CONFIRM then + -- User pressed "Accept" + if compass_cal_running then + gcs:run_command_int(MAV_CMD_DO_ACCEPT_MAG_CAL, { p3 = 1 }) + gcs:send_text(MAV_SEVERITY.INFO, "Compass calibration accepted") + compass_cal_running = false + end + + -- Tell the helper to update the TX UI back to "Execute" + return CRSF_COMMAND_STATUS.READY, "Execute" + + elseif command_action == CRSF_COMMAND_STATUS.CANCEL then + -- User pressed "Cancel" (e.g., long-press back button on TX) + if compass_cal_running then + gcs:run_command_int(MAV_CMD_DO_CANCEL_MAG_CAL, { p3 = 1 }) + gcs:send_text(MAV_SEVERITY.WARNING, "Calibration cancelled") + compass_cal_running = false + end + + -- Tell the helper to update the TX UI back to "Execute" + return CRSF_COMMAND_STATUS.READY, "Execute" + end --- create a CRSF menu text selection item -function create_text_entry(name, options, value, min, max, default, unit) - return string.pack(">BzzBBBBz", CRSF_PARAM_TYPE.TEXT_SELECTION, name, options, value, min, max, default, unit) + -- Default fallback + return CRSF_COMMAND_STATUS.READY, "Execute" end --- create a CRSF menu string item -function create_string_entry(name, value, max) - return string.pack(">BzzB", CRSF_PARAM_TYPE.STRING, name, value, max) +--- Handles the simple, one-shot gyro calibration. +local function callback_gyro_cal(command_action) + if command_action == CRSF_COMMAND_STATUS.START then + gcs:run_command_int(MAV_CMD_PREFLIGHT_CALIBRATION, { p1 = 1 }) + gcs:send_text(MAV_SEVERITY.INFO, "Gyros calibrated") + end + -- Always return to READY state + return CRSF_COMMAND_STATUS.READY, "Execute" end --- create a CRSF menu info item -function create_info_entry(name, info) - return string.pack(">Bzz", CRSF_PARAM_TYPE.INFO, name, info) +--- Handles the simple, one-shot accel calibration. +local function callback_accel_cal(command_action) + if command_action == CRSF_COMMAND_STATUS.START then + gcs:run_command_int(MAV_CMD_PREFLIGHT_CALIBRATION, { p5 = 4 }) + gcs:send_text(MAV_SEVERITY.INFO, "Accels calibrated") + end + return CRSF_COMMAND_STATUS.READY, "Execute" end --- create a CRSF command entry -function create_command_entry(name, status, timeout, info) - timeout = timeout or 10 -- 1s - return string.pack(">BzBBz", CRSF_PARAM_TYPE.COMMAND, name, status, timeout, info) +--- Handles the simple, one-shot "force" accel calibration. +local function callback_force_accel_cal(command_action) + if command_action == CRSF_COMMAND_STATUS.START then + gcs:run_command_int(MAV_CMD_PREFLIGHT_CALIBRATION, { p5 = 76 }) + gcs:send_text(MAV_SEVERITY.INFO, "Accels force calibrated") + end + return CRSF_COMMAND_STATUS.READY, "Execute" end -local compass_command, accel_command, gyro_command, forceaccel_command, forcecompass_command, ahrs_command - -local menu = crsf:add_menu('Calibrate') -local compass_param = create_command_entry("Calibrate Compass", CRSF_COMMAND_STATUS.READY, 50, "Start Calibration") -local accel_param = create_command_entry("Calibrate Accels", CRSF_COMMAND_STATUS.READY, 50, "Calibrate Accels") -local gyro_param = create_command_entry("Calibrate Gyros", CRSF_COMMAND_STATUS.READY, 50, "Calibrate Gyros") -local forceaccel_param = create_command_entry("Forcecal Accels", CRSF_COMMAND_STATUS.READY, 50, "Forcecal Accels") -local forcecompass_param = create_command_entry("Forcecal Compass", CRSF_COMMAND_STATUS.READY, 50, "Forcecal Compass") -local ahrs_param = create_command_entry("Trim AHRS", CRSF_COMMAND_STATUS.READY, 50, "Trim AHRS") - -if menu ~= nil then - compass_command = menu:add_parameter(compass_param) - accel_command = menu:add_parameter(accel_param) - gyro_command = menu:add_parameter(gyro_param) - forceaccel_command = menu:add_parameter(forceaccel_param) - forcecompass_command = menu:add_parameter(forcecompass_param) - ahrs_command = menu:add_parameter(ahrs_param) - gcs:send_text(MAV_SEVERITY.INFO, string.format("Loaded CRSF calibration menu")) +--- Handles the simple, one-shot "force" compass calibration. +local function callback_force_compass_cal(command_action) + if command_action == CRSF_COMMAND_STATUS.START then + gcs:run_command_int(MAV_CMD_PREFLIGHT_CALIBRATION, { p2 = 76 }) + gcs:send_text(MAV_SEVERITY.INFO, "Compass force calibrated") + end + return CRSF_COMMAND_STATUS.READY, "Execute" end -local calibration_running = false - -function update() - local param, payload, events = crsf:get_menu_event(CRSF_EVENT.PARAMETER_WRITE) - if (events & CRSF_EVENT.PARAMETER_WRITE) ~= 0 then - if compass_command ~= nil and param == compass_command:id() then - local command_action = string.unpack(">B", payload) - if command_action == CRSF_COMMAND_STATUS.START then -- start calibration - -- mag cal - if calibration_running then - gcs:run_command_int(MAV_CMD_DO_CANCEL_MAG_CAL, { p3 = 1 }) - end - calibration_running = true - gcs:run_command_int(MAV_CMD_DO_START_MAG_CAL, { p3 = 1 }) - gcs:send_text(MAV_SEVERITY.INFO, "Compass calibration running") - crsf:send_write_response(create_command_entry("Calibrate Compass", CRSF_COMMAND_STATUS.CONFIRMATION_NEEDED, 50, "Accept Calibration")) - elseif command_action == CRSF_COMMAND_STATUS.CONFIRM then -- confirm acceptance - gcs:run_command_int(MAV_CMD_DO_ACCEPT_MAG_CAL, { p3 = 1 }) - gcs:send_text(MAV_SEVERITY.INFO, "Compass calibration accepted") - crsf:send_write_response(compass_param) - elseif command_action == CRSF_COMMAND_STATUS.CANCEL and calibration_running then - gcs:run_command_int(MAV_CMD_DO_CANCEL_MAG_CAL, { p3 = 1 }) - gcs:send_text(MAV_SEVERITY.WARNING, "Calibration cancelled") - calibration_running = false - crsf:send_write_response(compass_param) - end - elseif accel_command ~= nil and param == accel_command:id() then - local command_action = string.unpack(">B", payload) - if command_action == CRSF_COMMAND_STATUS.START then - -- accelcalsimple - gcs:run_command_int(MAV_CMD_PREFLIGHT_CALIBRATION, { p5 = 4 }) - gcs:send_text(MAV_SEVERITY.INFO, "Accels calibrated") - crsf:send_write_response(accel_param) - end - elseif gyro_command ~= nil and param == gyro_command:id() then - local command_action = string.unpack(">B", payload) - if command_action == CRSF_COMMAND_STATUS.START then - -- gyro cal - gcs:run_command_int(MAV_CMD_PREFLIGHT_CALIBRATION, { p1 = 1 }) - gcs:send_text(MAV_SEVERITY.INFO, "Gyros calibrated") - crsf:send_write_response(gyro_param) - end - elseif forceaccel_command ~= nil and param == forceaccel_command:id() then - local command_action = string.unpack(">B", payload) - if command_action == CRSF_COMMAND_STATUS.START then - -- forcecal accel - gcs:run_command_int(MAV_CMD_PREFLIGHT_CALIBRATION, { p5 = 76 }) - gcs:send_text(MAV_SEVERITY.INFO, "Accels force calibrated") - crsf:send_write_response(forceaccel_param) - end - elseif forcecompass_command ~= nil and param == forcecompass_command:id() then - local command_action = string.unpack(">B", payload) - if command_action == CRSF_COMMAND_STATUS.START then - -- forcecal compass - gcs:run_command_int(MAV_CMD_PREFLIGHT_CALIBRATION, { p2 = 76 }) - gcs:send_text(MAV_SEVERITY.INFO, "Compass force calibrated") - crsf:send_write_response(forcecompass_param) - end - elseif ahrs_command ~= nil and param == ahrs_command:id() then - local command_action = string.unpack(">B", payload) - if command_action == CRSF_COMMAND_STATUS.START then - -- ahrs trim - gcs:run_command_int(MAV_CMD_PREFLIGHT_CALIBRATION, { p5 = 2 }) - gcs:send_text(MAV_SEVERITY.INFO, "AHRS trimmed") - crsf:send_write_response(ahrs_param) - end - end - elseif (events & CRSF_EVENT.PARAMETER_READ) ~= 0 then - gcs:send_text(MAV_SEVERITY.INFO, "Parameter read " .. param) +--- Handles the simple, one-shot AHRS trim. +local function callback_ahrs_trim(command_action) + if command_action == CRSF_COMMAND_STATUS.START then + gcs:run_command_int(MAV_CMD_PREFLIGHT_CALIBRATION, { p5 = 2 }) + gcs:send_text(MAV_SEVERITY.INFO, "AHRS trimmed") end - return update, 100 + return CRSF_COMMAND_STATUS.READY, "Execute" end -return update, 5000 \ No newline at end of file +-- #################### +-- # MENU DEFINITION +-- #################### + +-- This table defines the entire menu structure. +-- The helper library will parse this and build the menu. +local menu_definition = { + name = 'Calibrate', + items = { + { + type = 'COMMAND', + name = 'Calibrate Compass', + callback = callback_compass_cal + -- info = "Execute" -- This is the default + }, + { + type = 'COMMAND', + name = 'Calibrate Accels', + callback = callback_accel_cal + }, + { + type = 'COMMAND', + name = 'Calibrate Gyros', + callback = callback_gyro_cal + }, + { + type = 'COMMAND', + name = 'Forcecal Accels', + callback = callback_force_accel_cal + }, + { + type = 'COMMAND', + name = 'Forcecal Compass', + callback = callback_force_compass_cal + }, + { + type = 'COMMAND', + name = 'Trim AHRS', + callback = callback_ahrs_trim + } + } +} + +-- #################### +-- # SCRIPT START +-- #################### + +-- Register the menu definition with the helper. +-- This builds the menu and starts the helper's persistent event loop. +return crsf_helper.register_menu(menu_definition) \ No newline at end of file diff --git a/libraries/AP_Scripting/applets/crsf-calibrate.md b/libraries/AP_Scripting/applets/crsf-calibrate.md index 97c8560ce02bf..99a2b603f32dc 100644 --- a/libraries/AP_Scripting/applets/crsf-calibrate.md +++ b/libraries/AP_Scripting/applets/crsf-calibrate.md @@ -6,5 +6,8 @@ You can calibrate compass, accels, gyros and trim via the menu # Operation -Install the lua script in the APM/scripts directory on the flight -controller's microSD card. Enable scripting via SCR_ENABLE. +- Install the lua script in the APM/scripts directory on the flight +controller's microSD card. +- Install the helper module modules/crsf_helper.lua in the APM/scripts/modules directory on the flight +controller's microSD card. +- Enable scripting via SCR_ENABLE. diff --git a/libraries/AP_Scripting/applets/param-set.lua b/libraries/AP_Scripting/applets/param-set.lua index c16546565699d..cf737ee40eaa5 100644 --- a/libraries/AP_Scripting/applets/param-set.lua +++ b/libraries/AP_Scripting/applets/param-set.lua @@ -140,9 +140,9 @@ parameters_which_can_be_set["LIGHTS_ON"] = true parameters_which_can_be_set["LOG_BITMASK"] = true parameters_which_can_be_set["LOG_DISARMED"] = true parameters_which_can_be_set["LOG_FILE_DSRMROT"] = true -parameters_which_can_be_set["RTL_ALT"] = true +parameters_which_can_be_set["RTL_ALT_M"] = true parameters_which_can_be_set["RTL_LOIT_TIME"] = true -parameters_which_can_be_set["RTL_SPEED"] = true +parameters_which_can_be_set["RTL_SPEED_MS"] = true local function should_set_parameter_id(param_id) if parameters_which_can_be_set[param_id] == nil then diff --git a/libraries/AP_Scripting/applets/param-set.md b/libraries/AP_Scripting/applets/param-set.md index 914c2711cd441..94eda9a6ec39b 100644 --- a/libraries/AP_Scripting/applets/param-set.md +++ b/libraries/AP_Scripting/applets/param-set.md @@ -40,7 +40,7 @@ Only the following parameters are allowed to be set when the script is enabled: - Battery parameters (e.g., `BATT_ARM_MAH`, `BATT_FS_CRT_ACT`, etc.) - `BRD_OPTIONS` - `COMPASS_USE3` -- Geofence and RTL parameters (e.g., `FENCE_TYPE`, `RTL_ALT`) +- Geofence and RTL parameters (e.g., `FENCE_TYPE`, `RTL_ALT_M`) - `LOG_*`, `LIGHTS_ON` (See the full script for the complete whitelist.) diff --git a/libraries/AP_Scripting/applets/plane_precland.lua b/libraries/AP_Scripting/applets/plane_precland.lua index 1bd080b928d4d..eea5052630caf 100644 --- a/libraries/AP_Scripting/applets/plane_precland.lua +++ b/libraries/AP_Scripting/applets/plane_precland.lua @@ -198,15 +198,14 @@ local function update() --[[ log the target and distance --]] - logger.write("PPLD", 'Lat,Lon,Alt,HDist,TDist,RFND,VN,VE', - 'LLffffff', - 'DUmmmmmm', - 'GG------', + logger.write("PPLD", 'Lat,Lon,Alt,HDist,RFND,VN,VE', + 'LLfffff', + 'DUmmmmm', + 'GG-----', new_WP:lat(), new_WP:lng(), new_WP:alt(), xy_dist, - next_WP:get_distance(next_WP), rngfnd_distance_m, target_vel:x(), target_vel:y()) diff --git a/libraries/AP_Scripting/applets/repl.lua b/libraries/AP_Scripting/applets/repl.lua index eedf551aa6e7a..c9d049583467e 100644 --- a/libraries/AP_Scripting/applets/repl.lua +++ b/libraries/AP_Scripting/applets/repl.lua @@ -4,7 +4,7 @@ -- 0-based index of Scripting protocol port to use, or nil to use MAVLink local PORT_IDX = 0 local MAX_HISTORY = 50 -- number of lines of history to keep (must be >= 1) -local VERSION = "v1.0" -- version is convenience for the user +local VERSION = "v1.1" -- version is convenience for the user local port if PORT_IDX == nil then @@ -93,9 +93,9 @@ end -- write the character c to the port, buffering if failed local function writechar(c) - -- buffer character if stuff already in buffer or write fails + -- let writestring buffer character if stuff already in buffer or write fails if tx_buf or port:write(c) == 0 then - tx_buf[#tx_buf+1] = string.char(c) -- massive overhead... + writestring(string.char(c)) -- massive overhead... end end diff --git a/libraries/AP_Scripting/applets/revert_param.md b/libraries/AP_Scripting/applets/revert_param.md index 7d27cee978ce7..0150d34e8c0f4 100644 --- a/libraries/AP_Scripting/applets/revert_param.md +++ b/libraries/AP_Scripting/applets/revert_param.md @@ -48,11 +48,11 @@ The script covers the following parameters on quadplanes: - Q_A_ANG_PIT_P - Q_A_ANG_YAW_P - Q_A_RATE_*_MAX - - Q_P_ACCZ_* - - Q_P_VELZ_* - - Q_P_POSZ_* - - Q_P_VELXY_* - - Q_P_POSXY_* + - Q_P_D_ACC_* + - Q_P_D_VEL_* + - Q_P_D_POS_* + - Q_P_NE_VEL_* + - Q_P_NE_POS_* The script covers the following parameters on copters: @@ -63,11 +63,11 @@ The script covers the following parameters on copters: - ATC_ANG_PIT_P - ATC_ANG_YAW_P - ATC_RATE_*_MAX - - PSC_ACCZ_* - - PSC_VELZ_* - - PSC_POSZ_* - - PSC_VELXY_* - - PSC_POSXY_* + - PSC_D_ACC_* + - PSC_D_VEL_* + - PSC_D_POS_* + - PSC_NE_VEL_* + - PSC_NE_POS_* For fixed wing the following parameters are covered: diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 88474339bb299..09a6e637c636c 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -1830,6 +1830,10 @@ function motors:get_forward() end ---@return number function motors:get_throttle() end +-- get thrust motor input +---@return number +function motors:get_throttle_in() end + -- get throttle motor output ---@return integer ---| '0' # Shut down @@ -2718,10 +2722,22 @@ function vehicle:set_target_angle_and_climbrate(roll_deg, pitch_deg, yaw_deg, cl ---@return boolean -- true if successful function vehicle:set_target_rate_and_throttle(roll_rate_dps, pitch_rate_dps, yaw_rate_dps, throttle) end +-- Set vehicle's roll, pitch, and yaw angles and rates with throttle in guided mode +---@param roll_deg number -- roll angle in degrees from -180 to 180 +---@param pitch_deg number -- pitch angle in degrees from -90 to 90 +---@param yaw_deg number -- yaw angle in degrees from -360 to 360 +---@param roll_rate_dps number -- roll rate in degrees per second +---@param pitch_rate_dps number -- pitch rate in degrees per second +---@param yaw_rate_dps number -- yaw rate in degrees per second +---@param throttle number -- throttle demand 0.0 to 1.0 +---@return boolean -- true on success +function vehicle:set_target_angle_and_rate_and_throttle(roll_deg, pitch_deg, yaw_deg, roll_rate_dps, pitch_rate_dps, yaw_rate_dps, throttle) end + -- Sets the target velocity using a Vector3f object in a guided mode. ---@param vel_ned Vector3f_ud -- North, East, Down meters / second +---@param align_yaw_to_target? boolean -- optionally align the yaw to the target, defaults to false: yaw is not changed. Only used on Copter. ---@return boolean -- true on success -function vehicle:set_target_velocity_NED(vel_ned) end +function vehicle:set_target_velocity_NED(vel_ned, align_yaw_to_target) end -- desc ---@param target_vel Vector3f_ud diff --git a/libraries/AP_Scripting/drivers/mount-viewpro-driver.lua b/libraries/AP_Scripting/drivers/mount-viewpro-driver.lua deleted file mode 100644 index 131cfbc8d1b5d..0000000000000 --- a/libraries/AP_Scripting/drivers/mount-viewpro-driver.lua +++ /dev/null @@ -1,837 +0,0 @@ --- mount-viewpro-driver.lua: Viewpro mount/gimbal driver - ---[[ - How to use - Connect gimbal UART to one of the autopilot's serial ports - Set SERIALx_PROTOCOL = 28 (Scripting) where "x" corresponds to the serial port connected to the gimbal - Set SCR_ENABLE = 1 to enable scripting and reboot the autopilot - Set MNT1_TYPE = 9 (Scripting) to enable the mount/gimbal scripting driver - Set CAM1_TYPE = 7 (Scripting) to enable the camera scripting driver - Set RCx_OPTION = 300 (Scripting1) to allow real-time selection of the video feed and camera control - Copy this script to the autopilot's SD card in the APM/scripts directory and reboot the autopilot - Set VIEP_CAM_SWLOW, VIEP_CAM_SWMID, VIEP_CAM_SWHIGH to which cameras are controlled by the auxiliary switch - 0: No change in camera selection - 1: EO1 - 2: IR thermal - 3: EO1 + IR Picture-in-picture - 4: IR + EO1 Picture-in-picture - 5: Fusion - 6: IR1 13mm - 7: IR2 52mm - Set VIEP_ZOOM_SPEED to control speed of zoom (value between 0 and 7) - Set VIEP_ZOOM_MAX to the maximum zoom. E.g. for a camera with 20x zoom, set to 20 - Optionally set VIEP_DEBUG = 1 or 2 to increase level of debug output to the GCS - - Packet format - byte description notes - 0~2 header 0x55 0xAA 0xDC - 3 body length bit0~5: body length, n=all bytes from byte3 to checksum, min=4, max=63. bits6~7: frame counter - 4 frame id - 5~n+1 data 1st byte is command id? - n+2 checksum XOR of byte3 to n+1 (inclusive) ---]] - ----@diagnostic disable: cast-local-type ----@diagnostic disable: undefined-global - --- parameters -local PARAM_TABLE_KEY = 39 -assert(param:add_table(PARAM_TABLE_KEY, "VIEP_", 6), "could not add param table") -assert(param:add_param(PARAM_TABLE_KEY, 1, "DEBUG", 0), "could not add VIEP_DEBUG param") -assert(param:add_param(PARAM_TABLE_KEY, 2, "CAM_SWLOW", 1), "could not add VIEP_CAM_SWLOW param") -assert(param:add_param(PARAM_TABLE_KEY, 3, "CAM_SWMID", 2), "could not add VIEP_CAM_SWMID param") -assert(param:add_param(PARAM_TABLE_KEY, 4, "CAM_SWHIGH", 6), "could not add VIEP_CAM_CAM_SWHIGH param") -assert(param:add_param(PARAM_TABLE_KEY, 5, "ZOOM_SPEED", 7), "could not add VIEP_ZOOM_SPEED param") -assert(param:add_param(PARAM_TABLE_KEY, 6, "ZOOM_MAX", 20), "could not add VIEP_ZOOM_MAX param") - --- bind parameters to variables -local MNT1_TYPE = Parameter("MNT1_TYPE") -- should be 9:Scripting -local CAM1_TYPE = Parameter("CAM1_TYPE") -- should be 7:Scripting - ---[[ - // @Param: VIEP_DEBUG - // @DisplayName: ViewPro debug - // @Description: ViewPro debug - // @Values: 0:Disabled, 1:Enabled, 2:Enabled including attitude reporting - // @User: Advanced ---]] -local VIEP_DEBUG = Parameter("VIEP_DEBUG") -- debug level. 0:disabled 1:enabled 2:enabled with attitude reporting - ---[[ - // @Param: VIEP_CAM_SWLOW - // @DisplayName: ViewPro Camera For Switch Low - // @Description: Camera selection when switch is in low position - // @Values: 0:No change in camera selection, 1:EO1, 2:IR thermal, 3:EO1 + IR Picture-in-picture, 4:IR + EO1 Picture-in-picture, 5:Fusion, 6:IR1 13mm, 7:IR2 52mm - // @User: Standard ---]] -local VIEP_CAM_SWLOW = Parameter("VIEP_CAM_SWLOW") -- RC swith low position's camera selection - ---[[ - // @Param: VIEP_CAM_SWMID - // @DisplayName: ViewPro Camera For Switch Mid - // @Description: Camera selection when switch is in middle position - // @Values: 0:No change in camera selection, 1:EO1, 2:IR thermal, 3:EO1 + IR Picture-in-picture, 4:IR + EO1 Picture-in-picture, 5:Fusion, 6:IR1 13mm, 7:IR2 52mm - // @User: Standard ---]] -local VIEP_CAM_SWMID = Parameter("VIEP_CAM_SWMID") -- RC swith middle position's camera selection - ---[[ - // @Param: VIEP_CAM_SWHIGH - // @DisplayName: ViewPro Camera For Switch High - // @Description: Camera selection when switch is in high position - // @Values: 0:No change in camera selection, 1:EO1, 2:IR thermal, 3:EO1 + IR Picture-in-picture, 4:IR + EO1 Picture-in-picture, 5:Fusion, 6:IR1 13mm, 7:IR2 52mm - // @User: Standard ---]] -local VIEP_CAM_SWHIGH = Parameter("VIEP_CAM_SWHIGH") -- RC swith high position's camera selection - ---[[ - // @Param: VIEP_ZOOM_SPEED - // @DisplayName: ViewPro Zoom Speed - // @Description: ViewPro Zoom Speed. Higher numbers result in faster zooming - // @Range: 0 7 - // @User: Standard ---]] -local VIEP_ZOOM_SPEED = Parameter("VIEP_ZOOM_SPEED") -- zoom speed from 0 (slow) to 7 (fast) - ---[[ - // @Param: VIEP_ZOOM_MAX - // @DisplayName: ViewPro Zoom Times Max - // @Description: ViewPro Zoom Times Max - // @Range: 0 30 - // @User: Standard ---]] -local VIEP_ZOOM_MAX = Parameter("VIEP_ZOOM_MAX") -- zoom times max - --- global definitions -local CAM_SELECT_RC_OPTION = 300 -- rc channel option used to control which camera/video is used. RCx_OPTION = 300 (scripting1) -local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} -local INIT_INTERVAL_MS = 3000 -- attempt to initialise the gimbal at this interval -local UPDATE_INTERVAL_MS = 100 -- update at 10hz -local MOUNT_INSTANCE = 0 -- always control the first mount/gimbal -local CAMERA_INSTANCE = 0 -- always use the first camera - --- packet parsing definitions -local HEADER1 = 0x55 -- 1st header byte -local HEADER2 = 0xAA -- 2nd header byte -local HEADER3 = 0xDC -- 3rd header byte -local PACKET_LENGTH_MIN = 4 -- serial packet minimum length. used for sanity checks -local PACKET_LENGTH_MAX = 63 -- serial packet maximum length. used for sanity checks - --- parsing state definitions -local PARSE_STATE_WAITING_FOR_HEADER1 = 0 -local PARSE_STATE_WAITING_FOR_HEADER2 = 1 -local PARSE_STATE_WAITING_FOR_HEADER3 = 2 -local PARSE_STATE_WAITING_FOR_LENGTH = 3 -local PARSE_STATE_WAITING_FOR_FRAMEID = 4 -local PARSE_STATE_WAITING_FOR_DATA = 5 - --- received FrameId -local T1_F1_B1_D1_FRAMEID = 0x40 -- includes roll, pitch, yaw angles - --- camera operation commands -local CAM_COMMAND_NO_ACTION = 0x00 -local CAM_COMMAND_STOP_FOCUS_AND_ZOOM = 0x01 -local CAM_COMMAND_ZOOM_OUT = 0x08 -local CAM_COMMAND_ZOOM_IN = 0x09 -local CAM_COMMAND_FOCUS_PLUS = 0x0A -local CAM_COMMAND_FOCUS_MINUS = 0x0B -local CAM_COMMAND_TAKE_PICTURE = 0x13 -local CAM_COMMAND_START_RECORD = 0x14 -local CAM_COMMAND_STOP_RECORD = 0x15 -local CAM_COMMAND_AUTO_FOCUS = 0x19 -local CAM_COMMAND_MANUAL_FOCUS = 0x1A - --- tracking commands -local TRACK_COMMAND_STOP = 0x01 -local TRACK_COMMAND_START = 0x03 -local TRACK_COMMAND2_SET_POINT = 0x0A -local TRACK_COMMAND2_SET_RECT_TOPLEFT = 0x0B -local TRACK_COMMAND2_SET_RECT_BOTTOMRIGHT = 0x0C - --- camera control2 commands -local CAM_COMMAND2_SET_EO_ZOOM = 0x53 - --- hardcoded outgoing messages -local HEARTBEAT_MSG = {0x55,0xAA,0xDC,0x44,0x00,0x00,0x44} - --- local variables and definitions -local uart -- uart object connected to mount -local initialised = false -- true once connection to gimbal has been initialised -local parse_state = PARSE_STATE_WAITING_FOR_HEADER1 -- parse state -local parse_expected_crc = 0 -- incoming messages expected crc. this is checked against actual crc received -local parse_length = 0 -- incoming message parsed length -local parse_frameid = 0 -- incoming message command id -local parse_data_buff = {} -- data buffer holding roll, pitch and yaw angles from gimbal -local parse_data_bytes_recv = 0 -- count of the number of bytes received in the message so far -local last_frame_counter = 0 -- last frame counter sent to gimbal. always between 0 and 3 -local cam_choice = 0 -- last camera choice (see VIEP_CAM_SWLOW/MID/HIGH parameters) -local cam_pic_count = 0 -- last picture count. used to detect trigger pic -local cam_rec_video = false -- last record video state. used to detect record video -local cam_zoom_type = 0 -- last zoom type 1:Rate 2:Pct -local cam_zoom_value = 0 -- last zoom value. If rate, zoom out = -1, hold = 0, zoom in = 1. If Pct then value from 0 to 100 -local cam_focus_type = 0 -- last focus type 1:Rate, 2:Pct, 4:Auto -local cam_focus_value = 0 -- last focus value. If Rate then focus in = -1, focus hold = 0, focus out = 1 -local last_tracking_type = 0 -- last recorded tracking type (0:None, 1:Point, 2:Rectangle) -local last_tracking_p1x = 0 -- last recorded tracking point1 (used for center or top-left) -local last_tracking_p1y = 0 -- last recorded tracking point1 (used for center or top-left) -local last_tracking_p2x = 0 -- last recorded tracking point2 (bottom-right) -local last_tracking_p2y = 0 -- last recorded tracking point2 (bottom-right) -local tracking_active = false -- true when tracking is active (rate and angle controls are disabled) - --- parsing status reporting variables -local last_print_ms = 0 -- system time that debug output was last printed -local bytes_read = 0 -- number of bytes read from gimbal -local bytes_written = 0 -- number of bytes written to gimbal -local bytes_error = 0 -- number of bytes read that could not be parsed -local msg_ignored = 0 -- number of ignored messages (because frame id does not match) - --- debug variables -local last_test_send_md = 0 -- system time that a test message was last sent -local debug_buff = {} -- debug buffer to display bytes from gimbal - --- get lowbyte of a number -function lowbyte(num) - return num & 0xFF -end - --- get highbyte of a number -function highbyte(num) - return (num >> 8) & 0xFF -end - --- get uint16 from two bytes -function uint16_value(hbyte, lbyte) - return ((hbyte & 0xFF) << 8) | (lbyte & 0xFF) -end - --- get int16 from two bytes -function int16_value(hbyte, lbyte) - local uret = uint16_value(hbyte, lbyte) - if uret <= 0x8000 then - return uret - else - return uret - 0x10000 - end -end - --- wrap yaw angle in degrees to value between 0 and 360 -function wrap_360(angle) - local res = math.fmod(angle, 360.0) - if res < 0 then - res = res + 360.0 - end - return res -end - --- wrap yaw angle in degrees to value between -180 and +180 -function wrap_180(angle_deg) - local res = wrap_360(angle_deg) - if res > 180 then - res = res - 360 - end - return res -end - --- calculate crc from existing crc value and new byte -function calc_crc(orig_crc, b) - local crc = (orig_crc ~ b) & 0xFF - return crc -end - --- find and initialise serial port connected to gimbal -function init() - -- check mount parameter - if MNT1_TYPE:get() ~= 9 then - gcs:send_text(MAV_SEVERITY.CRITICAL, "ViewPro: set MNT1_TYPE=9") - do return end - end - - -- check cam type parametr - if CAM1_TYPE:get() ~= 7 then - gcs:send_text(MAV_SEVERITY.CRITICAL, "ViewPro: set CAM1_TYPE=7") - do return end - end - - -- find and init first instance of SERIALx_PROTOCOL = 28 (Scripting) - uart = serial:find_serial(0) - if uart == nil then - gcs:send_text(3, "ViewPro: no SERIALx_PROTOCOL = 28") -- MAV_SEVERITY_ERR - else - uart:begin(115200) - uart:set_flow_control(0) - initialised = true - gcs:send_text(MAV_SEVERITY.INFO, "ViewPro: started") - end -end - --- send hard coded message -function send_msg(msg) - for i=1,#msg do - uart:write(msg[i]) - -- debug - bytes_written = bytes_written + 1 - end -end - --- parse test message -function parse_test_msg(msg) - for i=1,#msg do - parse_byte(msg[i]) - end -end - --- reading incoming packets from gimbal -function read_incoming_packets() - local n_bytes = uart:available() - while n_bytes > 0 do - n_bytes = n_bytes - 1 - parse_byte(uart:read()) - end -end - --- parse a single byte from gimbal -function parse_byte(b) - -- record num bytes for reporting - bytes_read = bytes_read + 1 - - -- debug - if VIEP_DEBUG:get() > 1 then - debug_buff[#debug_buff+1] = b - if #debug_buff >= 10 then - gcs:send_text(MAV_SEVERITY.INFO, string.format("ViewPro: %x %x %x %x %x %x %x %x %x %x", debug_buff[1], debug_buff[2], debug_buff[3], debug_buff[4], debug_buff[5], debug_buff[6], debug_buff[7], debug_buff[8], debug_buff[9], debug_buff[10])) - debug_buff = {} - end - end - - -- waiting for header1 - if parse_state == PARSE_STATE_WAITING_FOR_HEADER1 then - if b == HEADER1 then - parse_state = PARSE_STATE_WAITING_FOR_HEADER2 - parse_expected_crc = 0 - parse_data_bytes_recv = 0 - do return end - end - bytes_error = bytes_error + 1 - end - - -- waiting for header2 - if parse_state == PARSE_STATE_WAITING_FOR_HEADER2 then - if b == HEADER2 then - parse_state = PARSE_STATE_WAITING_FOR_HEADER3 - else - -- unexpected byte so reset parsing state - parse_state = PARSE_STATE_WAITING_FOR_HEADER1 - bytes_error = bytes_error + 1 - end - do return end - end - - -- waiting for header3 - if parse_state == PARSE_STATE_WAITING_FOR_HEADER3 then - if b == HEADER3 then - parse_state = PARSE_STATE_WAITING_FOR_LENGTH - else - -- unexpected byte so reset parsing state - parse_state = PARSE_STATE_WAITING_FOR_HEADER1 - bytes_error = bytes_error + 1 - end - do return end - end - - -- waiting for length - if parse_state == PARSE_STATE_WAITING_FOR_LENGTH then - parse_expected_crc = calc_crc(parse_expected_crc, b) - parse_length = b & 0x3F - if parse_length >= PACKET_LENGTH_MIN and parse_length <= PACKET_LENGTH_MAX then - parse_state = PARSE_STATE_WAITING_FOR_FRAMEID - else - -- unexpected length - parse_state = PARSE_STATE_WAITING_FOR_HEADER1 - bytes_error = bytes_error + 1 - if VIEP_DEBUG:get() > 0 then - gcs:send_text(MAV_SEVERITY.ERROR, string.format("ViewPro: invalid len:%d", parse_length)) - end - end - do return end - end - - -- waiting for command id - if parse_state == PARSE_STATE_WAITING_FOR_FRAMEID then - parse_expected_crc = calc_crc(parse_expected_crc, b) - parse_frameid = b - parse_state = PARSE_STATE_WAITING_FOR_DATA - do return end - end - - -- waiting for data - if parse_state == PARSE_STATE_WAITING_FOR_DATA then - - -- check for crc - if parse_data_bytes_recv >= parse_length - 3 then - if b == parse_expected_crc then - -- crc matched, process packet - local processed = false - - -- T1_F1_B1_D1 - if parse_frameid == T1_F1_B1_D1_FRAMEID then - processed = true - -- T1 holds target info including target lean angles - -- F1 holds tracker sensor status (which camera, tracking vs lost) - -- B1 section holds actual lean angles - -- D1 section holds camera status including zoom level - local servo_status = (parse_data_buff[24] & 0xF0 >> 4) - local roll_deg = int16_value(parse_data_buff[24] & 0x0F, parse_data_buff[25]) * (180.0/4095.0) - 90.0 - local yaw_deg = int16_value(parse_data_buff[26], parse_data_buff[27]) * (360.0 / 65536.0) - local pitch_deg = -int16_value(parse_data_buff[28], parse_data_buff[29]) * (360.0 / 65536.0) - mount:set_attitude_euler(MOUNT_INSTANCE, roll_deg, pitch_deg, yaw_deg) - - if VIEP_DEBUG:get() > 0 then - gcs:send_text(MAV_SEVERITY.INFO, string.format("ViewPro: r:%f p:%f y:%f ss:%x", roll_deg, pitch_deg, yaw_deg, servo_status)) - end - end - - if not processed then - msg_ignored = msg_ignored + 1 - if VIEP_DEBUG:get() > 0 then - gcs:send_text(MAV_SEVERITY.INFO, string.format("ViewPro: ignored frameid:%x", parse_frameid)) - end - end - else - -- crc mismatch - bytes_error = bytes_error + 1 - if VIEP_DEBUG:get() > 0 then - gcs:send_text(MAV_SEVERITY.INFO, string.format("ViewPro: crc exp:%x got:%x", parse_expected_crc, b)) - end - end - parse_state = PARSE_STATE_WAITING_FOR_HEADER1 - do return end - end - - -- add latest byte to crc and buffer - parse_expected_crc = calc_crc(parse_expected_crc, b) - parse_data_bytes_recv = parse_data_bytes_recv + 1 - parse_data_buff[parse_data_bytes_recv] = b - end -end - --- write a byte to the uart and update the checksum -function write_byte(b, checksum) - if b == nil or checksum == nil then - gcs:send_text(3, "ViewPro: failed to write byte") -- MAV_SEVERITY_ERR - return - end - local byte_to_write = b & 0xFF - uart:write(byte_to_write) - bytes_written = bytes_written + 1 - local checksum_ret = (checksum ~ byte_to_write) & 0xFF - return checksum_ret -end - --- calculate length and frame count byte --- length is all bytes after the header including CRC -function calc_length_and_frame_byte(length) - -- increment frame counter - last_frame_counter = last_frame_counter + 1 & 0x03 - return (last_frame_counter << 6 | length & 0x3F) & 0xFF -end - --- send target angles (in degrees) to gimbal --- yaw_angle_deg is always a body-frame angle -function send_target_angles(pitch_angle_deg, yaw_angle_deg) - - -- prepare A1 message, FrameId 0x1A, CmdId 0x0B - local length_and_frame_counter = calc_length_and_frame_byte(0x0C) - local pitch_angle_output = math.floor((-pitch_angle_deg / 360.0 * 65536.0) + 0.5) - local yaw_angle_output = math.floor((yaw_angle_deg / 360.0 * 65536.0) + 0.5) - - write_byte(HEADER1, 0) - write_byte(HEADER2, 0) - write_byte(HEADER3, 0) - local checksum = write_byte(length_and_frame_counter, 0) -- length and frame count - checksum = write_byte(0x1A, checksum) -- 0x1A: A1 FrameId - checksum = write_byte(0x0B, checksum) -- 0x0B: absolute angle - checksum = write_byte(highbyte(yaw_angle_output), checksum) -- yaw angle MSB - checksum = write_byte(lowbyte(yaw_angle_output), checksum) -- yaw angle LSB - checksum = write_byte(highbyte(pitch_angle_output), checksum) -- pitch angle MSB - checksum = write_byte(lowbyte(pitch_angle_output), checksum) -- pitch angle LSB - checksum = write_byte(0, checksum) -- unused - checksum = write_byte(0, checksum) -- unused - checksum = write_byte(0, checksum) -- unused - checksum = write_byte(0, checksum) -- unused - write_byte(checksum, 0) -- checksum -end - --- send target rates (in deg/sec) to gimbal -function send_target_rates(pitch_rate_degs, yaw_rate_degs) - - -- prepare A1 message, FrameId 0x1A, CmdId 0x01 - local length_and_frame_counter = calc_length_and_frame_byte(0x0C) - local pitch_rate_output = math.floor((-pitch_rate_degs * 100.0) + 0.5) - local yaw_rate_output = math.floor((yaw_rate_degs * 100.0) + 0.5) - - write_byte(HEADER1, 0) - write_byte(HEADER2, 0) - write_byte(HEADER3, 0) - local checksum = write_byte(length_and_frame_counter, 0) -- length and frame count - checksum = write_byte(0x1A, checksum) -- 0x1A: A1 FrameId - checksum = write_byte(0x01, checksum) -- 0x01: manual rate angle - checksum = write_byte(highbyte(yaw_rate_output), checksum) -- yaw rate MSB - checksum = write_byte(lowbyte(yaw_rate_output), checksum) -- yaw rate LSB - checksum = write_byte(highbyte(pitch_rate_output), checksum) -- pitch angle MSB - checksum = write_byte(lowbyte(pitch_rate_output), checksum) -- pitch angle LSB - checksum = write_byte(0, checksum) -- unused - checksum = write_byte(0, checksum) -- unused - checksum = write_byte(0, checksum) -- unused - checksum = write_byte(0, checksum) -- unused - write_byte(checksum, 0) -- checksum -end - --- send camera commands -function send_camera_control(camera_choice, cam_command) - - -- prepare C1 message, FrameId 0x1C - -- bits 0~2 : video choose - -- bits 3~5 : zoom speed - -- bits 6~12 : operation command - -- bits 13~15 : LRF (rangefinder) - local length_and_frame_counter = calc_length_and_frame_byte(0x05) - - local video_choose = camera_choice & 0x07 - - local zoom_speed = 0 - if cam_command == CAM_COMMAND_ZOOM_OUT or cam_command == CAM_COMMAND_ZOOM_IN then - zoom_speed = (VIEP_ZOOM_SPEED:get() & 0x07) << 3 - end - - local operation_cmd = ((cam_command & 0xFFFF) & 0x7F) << 6 - local data_bytes = video_choose | zoom_speed | operation_cmd - - -- debug - --gcs:send_text(MAV_SEVERITY.INFO, string.format("ViewPro: camcmd:%x msb:%x lsb:%x", cam_command, highbyte(cmd_shifted), lowbyte(cmd_shifted))) - gcs:send_text(MAV_SEVERITY.INFO, string.format("ViewPro: cam choice:%x", video_choose)) - - write_byte(HEADER1, 0) - write_byte(HEADER2, 0) - write_byte(HEADER3, 0) - local checksum = write_byte(length_and_frame_counter, 0) -- length and frame count - checksum = write_byte(0x1C, checksum) -- 0x1C: C1 FrameId - checksum = write_byte(highbyte(data_bytes), checksum) -- msb - checksum = write_byte(lowbyte(data_bytes), checksum) -- lsb - write_byte(checksum, 0) -- checksum -end - --- send camera commands using C2 message -function send_camera_control2(cam_command, cam_value) - - -- ensure cam_value is an integer - cam_value = math.floor(cam_value + 0.5) - - -- prepare C2 message, FrameId 0x2C - -- byte0 : command (0x50:set brightness, 0x51:set contrast, 0x52: set aperture, 0x53:set EO zoom, 0x54:set focus, 0x55:set ISO, 0x56:set thermal cam digital zoom) - -- byte1~2 : value related to command - local length_and_frame_counter = calc_length_and_frame_byte(0x06) - cam_command = cam_command & 0xFF - local data_bytes = cam_value & 0xFFFF - - write_byte(HEADER1, 0) - write_byte(HEADER2, 0) - write_byte(HEADER3, 0) - local checksum = write_byte(length_and_frame_counter, 0) -- length and frame count - checksum = write_byte(0x2C, checksum) -- 0x2C: C2 FrameId - checksum = write_byte(cam_command, checksum) -- command - checksum = write_byte(highbyte(data_bytes), checksum) -- msb - checksum = write_byte(lowbyte(data_bytes), checksum) -- lsb - write_byte(checksum, 0) -- checksum -end - --- send tracking commands using E1 message -function send_tracking_control(camera_choice, tracking_command, param2) - - -- prepare E1 message, FrameId 0x1E - -- byte0 : tracking source (0x01:EO 1, 0x02: IR, 0x03:EO 2) - -- byte1 : basic command - -- 0x02: Search (Bring up the cross - -- 0x03: Turn on tracking - -- 0x04: Switch tracking point to cross position(take placed by enable tracking) - -- 0X05: AI function ON/OFF - -- 0X08: For AI. Auto track target once identified - -- 0x09: For AI. Change target when click targets identified - -- 0x0A: For AI. Not change target when click identified targets. - -- 0X21: 32× 32 small template - -- 0X22: 64× 64 medium template - -- 0X23: 128× 128 big template - -- 0X24: Self-adapt between small and medium template - -- 0X25: Self-adapt between small and big template - -- 0X26: Self-adapt between medium and big template - -- 0X28: Self-adaption between small, medium and big template - -- byte2 : parameter 2, value related to above command - - local length_and_frame_counter = calc_length_and_frame_byte(0x06) - write_byte(HEADER1, 0) - write_byte(HEADER2, 0) - write_byte(HEADER3, 0) - local checksum = write_byte(length_and_frame_counter, 0) -- length and frame count - checksum = write_byte(0x1E, checksum) -- 0x1E: E1 FrameId - checksum = write_byte(camera_choice, checksum) -- camera choice - checksum = write_byte(tracking_command & 0xFF, checksum) -- tracking command - checksum = write_byte(param2 & 0xFF, checksum) -- param2 - write_byte(checksum, 0) -- checksum -end - --- send tracking commands using E2 message -function send_tracking_control2(tracking_command2, param1, param2) - - -- prepare E2 message, FrameId 0x2E - -- byte0 : tracking source (0x01:EO 1, 0x02: IR, 0x03:EO 2) - -- byte1 : basic command - -- 0x0A: The tracking point moves to the commanded position - -- 0x0B: Rectangular tracking area, top left corner point set - -- 0x0C: Rectangular tracking area, lower right corner point set - -- byte2~3 : Tracking point yaw, 1bit=1pixel, -960~960, 0 is center, negative is left, positive is right - -- byte4~5 : Tracking point pitch, 1bit=1pixel, -540~540, 0 is center, negative is up, positive is down - - param1 = math.floor(param1 + 0.5) & 0xFFFF - param2 = math.floor(param2 + 0.5) & 0xFFFF - local length_and_frame_counter = calc_length_and_frame_byte(0x08) - write_byte(HEADER1, 0) - write_byte(HEADER2, 0) - write_byte(HEADER3, 0) - local checksum = write_byte(length_and_frame_counter, 0) -- length and frame count - checksum = write_byte(0x2E, checksum) -- 0x2E: E2 FrameId - checksum = write_byte(tracking_command2 & 0xFF, checksum) -- tracking command2 - checksum = write_byte(highbyte(param1), checksum) -- param1 msb - checksum = write_byte(lowbyte(param1), checksum) -- param1 lsb - checksum = write_byte(highbyte(param2), checksum) -- param2 msb - checksum = write_byte(lowbyte(param2), checksum) -- param2 lsb - write_byte(checksum, 0) -- checksum -end - --- return camera selection according to RC switch position and VIEW_CAM_SWxxx parameter --- used in C1 message's "video choose" to specify which cameras should be controlled -function get_camera_choice() - local cam_switch_pos = rc:get_aux_cached(CAM_SELECT_RC_OPTION) - if cam_switch_pos == 0 then - return VIEP_CAM_SWLOW:get() - end - if cam_switch_pos == 1 then - return VIEP_CAM_SWMID:get() - end - return VIEP_CAM_SWHIGH:get() -end - --- check for changes in camera state and send messages to gimbal if required -function check_camera_state() - - -- check for change in camera - local curr_cam_choice = get_camera_choice() - if cam_choice ~= curr_cam_choice then - cam_choice = curr_cam_choice - send_camera_control(cam_choice, CAM_COMMAND_NO_ACTION) - end - - -- get latest camera state from AP driver - local cam_state = camera:get_state(CAMERA_INSTANCE) - if not cam_state then - return - end - - -- check for take picture - if cam_state:take_pic_incr() and cam_state:take_pic_incr() ~= cam_pic_count then - cam_pic_count = cam_state:take_pic_incr() - send_camera_control(cam_choice, CAM_COMMAND_TAKE_PICTURE) - if VIEP_DEBUG:get() > 0 then - gcs:send_text(MAV_SEVERITY.INFO, string.format("ViewPro: took pic %u", pic_count)) - end - end - - -- check for start/stop recording video - if cam_state:recording_video() ~= cam_rec_video then - cam_rec_video = cam_state:recording_video() - if cam_rec_video > 0 then - send_camera_control(cam_choice, CAM_COMMAND_START_RECORD) - gcs:send_text(MAV_SEVERITY.INFO, "ViewPro: start recording") - else - send_camera_control(cam_choice, CAM_COMMAND_STOP_RECORD) - gcs:send_text(MAV_SEVERITY.INFO, "ViewPro: stop recording") - end - if VIEP_DEBUG:get() > 0 then - gcs:send_text(MAV_SEVERITY.INFO, "ViewPro: rec video:" .. tostring(cam_rec_video)) - end - end - - -- check zoom - -- zoom out = -1, hold = 0, zoom in = 1 - local zoom_type_changed = cam_state:zoom_type() and (cam_state:zoom_type() ~= cam_zoom_type) - local zoom_value_changed = cam_state:zoom_value() and (cam_state:zoom_value() ~= cam_zoom_value) - if (zoom_type_changed or zoom_value_changed) then - cam_zoom_type = cam_state:zoom_type() - cam_zoom_value = cam_state:zoom_value() - - -- zoom rate - if cam_zoom_type == 1 then - if cam_zoom_value < 0 then - send_camera_control(cam_choice, CAM_COMMAND_ZOOM_OUT) - elseif cam_zoom_value > 0 then - send_camera_control(cam_choice, CAM_COMMAND_ZOOM_IN) - else - send_camera_control(cam_choice, CAM_COMMAND_STOP_FOCUS_AND_ZOOM) - end - end - - -- zoom percent - if cam_zoom_type == 2 then - -- convert zoom percentage (in the range 0 to 100) to value in the range of 0 to VIEW_ZOOM_MAX * 10 - send_camera_control2(CAM_COMMAND2_SET_EO_ZOOM, VIEP_ZOOM_MAX:get() * cam_zoom_value * 0.1) - end - - if VIEP_DEBUG:get() > 0 then - gcs:send_text(MAV_SEVERITY.INFO, "ViewPro: zoom type:" .. tostring(cam_zoom_type) .. " value:" .. tostring(cam_zoom_value)) - end - end - - -- check manual focus - -- focus in = -1, focus hold = 0, focus out = 1 - local focus_type_changed = cam_state:focus_type() and (cam_state:focus_type() ~= cam_focus_type) - local focus_value_changed = cam_state:focus_value() and (cam_state:focus_value() ~= cam_focus_value) - if (focus_type_changed or focus_value_changed) then - cam_focus_type = cam_state:focus_type() - cam_focus_value = cam_state:focus_value() - - -- focus rate - if cam_focus_type == 1 then - if cam_focus_value < 0 then - send_camera_control(cam_choice, CAM_COMMAND_MANUAL_FOCUS) - send_camera_control(cam_choice, CAM_COMMAND_FOCUS_MINUS) - elseif cam_focus_value == 0 then - send_camera_control(cam_choice, CAM_COMMAND_STOP_FOCUS_AND_ZOOM) - elseif cam_focus_value > 0 then - send_camera_control(cam_choice, CAM_COMMAND_MANUAL_FOCUS) - send_camera_control(cam_choice, CAM_COMMAND_FOCUS_PLUS) - end - if VIEP_DEBUG:get() > 0 then - gcs:send_text(MAV_SEVERITY.INFO, "ViewPro: focus:" .. tostring(cam_focus_step)) - end - end - - -- check auto focus - if cam_focus_type == 4 then - send_camera_control(cam_choice, CAM_COMMAND_AUTO_FOCUS) - if VIEP_DEBUG:get() > 0 then - gcs:send_text(MAV_SEVERITY.INFO, "ViewPro: auto focus:" .. tostring(cam_autofocus)) - end - end - end -end - --- check for changes in tracking state and send messages to gimbal if required -function check_tracking_state() - - -- get latest camera state from AP driver - local cam_state = camera:get_state(CAMERA_INSTANCE) - if not cam_state then - return - end - - -- get current camera choice - local curr_cam_choice = get_camera_choice() - - -- check for change in tracking state - local tracking_type_changed = cam_state:tracking_type() ~= last_tracking_type - local tracking_type_p1_changed = (cam_state:tracking_p1():x() ~= last_tracking_p1x) or (cam_state:tracking_p1():y() ~= last_tracking_p1y) - local tracking_type_p2_changed = (cam_state:tracking_p2():x() ~= last_tracking_p2x) or (cam_state:tracking_p2():y() ~= last_tracking_p2y) - last_tracking_type = cam_state:tracking_type() - last_tracking_p1x = cam_state:tracking_p1():x() - last_tracking_p1y = cam_state:tracking_p1():y() - last_tracking_p2x = cam_state:tracking_p2():x() - last_tracking_p2y = cam_state:tracking_p2():y() - - if (last_tracking_type == 0) and tracking_type_changed then - -- turn off tracking - send_tracking_control(curr_cam_choice, TRACK_COMMAND_STOP, 0) - tracking_active = false - gcs:send_text(MAV_SEVERITY.INFO, "ViewPro: tracking OFF") - end - - if (last_tracking_type == 1) and (tracking_type_changed or tracking_type_p1_changed) then - -- turn tracking point on - local yaw_value = (last_tracking_p1x - 0.5) * 960 - local pitch_value = (last_tracking_p1y - 0.5) * 540 - send_tracking_control(curr_cam_choice, TRACK_COMMAND_START, 0) - send_tracking_control2(TRACK_COMMAND2_SET_POINT, yaw_value, pitch_value) - tracking_active = true - gcs:send_text(MAV_SEVERITY.INFO, "ViewPro: tracking point ON") - end - - if (last_tracking_type == 2) and (tracking_type_changed or tracking_type_p1_changed or tracking_type_p2_changed) then - -- turn tracking rectangle on - local yaw_value1 = (last_tracking_p1x - 0.5) * 960 - local pitch_value1 = (last_tracking_p1y - 0.5) * 540 - local yaw_value2 = (last_tracking_p2x - 0.5) * 960 - local pitch_value2 = (last_tracking_p2y - 0.5) * 540 - send_tracking_control(curr_cam_choice, TRACK_COMMAND_START, 0) - send_tracking_control2(TRACK_COMMAND2_SET_RECT_TOPLEFT, yaw_value1, pitch_value1) - send_tracking_control2(TRACK_COMMAND2_SET_RECT_BOTTOMRIGHT, yaw_value2, pitch_value2) - tracking_active = true - gcs:send_text(MAV_SEVERITY.INFO, "ViewPro: tracking rectangle ON") - end -end - --- the main update function that performs a simplified version of RTL -function update() - - -- get current system time - local now_ms = millis() - - -- initialise connection to gimbal - if not initialised then - init() - return update, INIT_INTERVAL_MS - end - - -- status reporting - if (VIEP_DEBUG:get() > 0) and (now_ms - last_print_ms > 5000) then - last_print_ms = now_ms - gcs:send_text(MAV_SEVERITY.INFO, string.format("ViewPro: r:%u w:%u err:%u ign:%u", bytes_read, bytes_written, bytes_error, msg_ignored)) - end - - -- consume incoming bytes - read_incoming_packets() - - -- check camera state and send control messages - check_camera_state() - - -- check tracking state and send tracking control messages - check_tracking_state() - - -- send heartbeat, gimbal should respond with T1+F1+B1+D1 - send_msg(HEARTBEAT_MSG) - - -- request gimbal attitude by sending heartbeat - if now_ms - last_test_send_md > 1000 then - last_test_send_md = now_ms - send_msg(HEARTBEAT_MSG) - end - - if not tracking_active then - -- send target angle to gimbal - local roll_deg, pitch_deg, yaw_deg, yaw_is_ef = mount:get_angle_target(MOUNT_INSTANCE) - if roll_deg and pitch_deg and yaw_deg then - if yaw_is_ef then - yaw_deg = wrap_180(yaw_deg - math.deg(ahrs:get_yaw_rad())) - end - send_target_angles(pitch_deg, yaw_deg) - return update, UPDATE_INTERVAL_MS - end - - -- send target rate to gimbal - local roll_degs, pitch_degs, yaw_degs, _ = mount:get_rate_target(MOUNT_INSTANCE) - if roll_degs and pitch_degs and yaw_degs then - send_target_rates(pitch_degs, yaw_degs) - return update, UPDATE_INTERVAL_MS - end - end - - return update, UPDATE_INTERVAL_MS -end - -return update() diff --git a/libraries/AP_Scripting/drivers/mount-viewpro-driver.md b/libraries/AP_Scripting/drivers/mount-viewpro-driver.md deleted file mode 100644 index e699a743565d5..0000000000000 --- a/libraries/AP_Scripting/drivers/mount-viewpro-driver.md +++ /dev/null @@ -1,25 +0,0 @@ -# Mount Viewpro Driver - -Viewpro gimbal driver lua script - -# How To Use - - Connect gimbal UART to one of the autopilot's serial ports - Set SERIALx_PROTOCOL = 28 (Scripting) where "x" corresponds to the serial port connected to the gimbal - Set SCR_ENABLE = 1 to enable scripting and reboot the autopilot - Set MNT1_TYPE = 9 (Scripting) to enable the mount/gimbal scripting driver - Set CAM1_TYPE = 7 (Scripting) to enable camera control using the scripting driver - Set RCx_OPTION = 300 (Scripting1) to allow real-time selection of the video feed and camera control - Copy this script to the autopilot's SD card in the APM/scripts directory and reboot the autopilot - Set VIEP_CAM_SWLOW, VIEP_CAM_SWMID, VIEP_CAM_SWHIGH to which cameras are controlled by the auxiliary switch - 0: No change in camera selection - 1: EO1 - 2: IR thermal - 3: EO1 + IR Picture-in-picture - 4: IR + EO1 Picture-in-picture - 5: Fusion - 6: IR1 13mm - 7: IR2 52mm - Set VIEP_ZOOM_SPEED to control speed of zoom (value between 0 and 7) - Set VIEP_ZOOM_MAX to the maximum zoom. E.g. for a camera with 20x zoom, set to 20 - Optionally set VIEP_DEBUG = 1 or 2 to increase level of debug output to the GCS diff --git a/libraries/AP_Scripting/examples/config_profiles.lua b/libraries/AP_Scripting/examples/config_profiles.lua index c18cae015f427..143daf0fa409c 100644 --- a/libraries/AP_Scripting/examples/config_profiles.lua +++ b/libraries/AP_Scripting/examples/config_profiles.lua @@ -287,9 +287,9 @@ local parameters_which_can_be_set = { ["LOG_DISARMED"] = true, ["LOG_FILE_DSRMROT"] = true, - ["RTL_ALT"] = true, + ["RTL_ALT_M"] = true, ["RTL_LOIT_TIME"] = true, - ["RTL_SPEED"] = true, + ["RTL_SPEED_MS"] = true, } local msg_pfx = "CFG: " diff --git a/libraries/AP_Scripting/examples/sbus_send.lua b/libraries/AP_Scripting/examples/sbus_send.lua new file mode 100644 index 0000000000000..02c520dd2e009 --- /dev/null +++ b/libraries/AP_Scripting/examples/sbus_send.lua @@ -0,0 +1,198 @@ +--[[ +EXPERIMENTAL EXAMPLE ONLY – NOT FOR FLIGHT CONTROL USE + +This Lua script demonstrates generating an S.Bus-compatible serial +output stream from ArduPilot servo outputs using a scripting task. + +WARNING: +- This implementation has NO timing guarantees and may produce corrupted + frames under high CPU load. +- S.Bus provides only weak error detection (no CRC, parity only). +- Frame corruption or misalignment may result in severe control errors, + including unintended full-throttle outputs. + +This script MUST NOT be used for safety-critical functions such as: +- Motor or rotor control +- ESC input +- Primary flight control paths + +It is provided solely as an example of serial protocol generation from Lua. +]] + +local pwm_vals = {} +local servo_min = assert(param:get("SERVO1_MIN"), "Lua: Could not read SERVO1_MIN") +local servo_max = assert(param:get("SERVO1_MAX"), "Lua: Could not read SERVO1_MAX") +local servo_trim = assert(param:get("SERVO1_TRIM"), "Lua: Could not read SERVO1_TRIM") +local rc_min = assert(param:get("RC3_MIN"), "Lua: Could not read RC3_MIN") +local rc_max = assert(param:get("RC3_MAX"), "Lua: Could not read RC3_MAX") +local rc_trim = assert(param:get("RC3_TRIM"), "Lua: Could not read RC3_TRIM") +local MAV_SEVERITY = { EMERGENCY = 0, ALERT = 1, CRITICAL = 2, ERROR = 3, WARNING = 4, NOTICE = 5, INFO = 6, DEBUG = 7 } +local SCR_INS = 1 + +local function init() + for ch = 1, 16 do + pwm_vals[ch] = 1500 + end +end + +-- debugging +local function notify_pwm_vals(label) + -- Channels 1–8 + gcs:send_text(0, string.format( + "CH1-8 %s: %d, %d, %d, %d, %d, %d, %d, %d", + label, + pwm_vals[1], pwm_vals[2], pwm_vals[3], pwm_vals[4], + pwm_vals[5], pwm_vals[6], pwm_vals[7], pwm_vals[8] + )) + -- Channels 9–16 + gcs:send_text(0, string.format( + "CH9-16 %s: %d, %d, %d, %d, %d, %d, %d, %d", + label, + pwm_vals[9], pwm_vals[10], pwm_vals[11], pwm_vals[12], + pwm_vals[13], pwm_vals[14], pwm_vals[15], pwm_vals[16] + )) +end + +local function read_rcin() + for ch = 1, 16 do + pwm_vals[ch] = rc:get_pwm(ch) or 0 + end +end + +local function pack_sbus() + local frame = { 0x0F } + for _ = 1, 22 do + frame[#frame + 1] = 0x00 + end + frame[#frame + 1] = 0x00 + frame[#frame + 1] = 0x00 + + for ch = 1, 16 do + local v = pwm_vals[ch] + if v < 0 then + v = 0 + elseif v > 2047 then + v = 2047 + end + + local bit_index = (ch - 1) * 11 + local val = v + for _ = 0, 10 do + if (val % 2) == 1 then + local byte_idx = math.floor(bit_index / 8) + 2 + local bit_in_byte = bit_index % 8 + frame[byte_idx] = frame[byte_idx] + (2 ^ bit_in_byte) + end + val = math.floor(val / 2) + bit_index = bit_index + 1 + end + end + + -- set failsafe flag + if not rc:has_valid_input() then + frame[24] = 8 -- FAILSAFE + else + frame[24] = 0 -- NORMAL + end + + -- send + local buf = {} + for i = 1, #frame do + buf[i] = string.char(frame[i]) + end + return table.concat(buf) +end + +local in_mid = rc_trim +local in_half = (rc_max - rc_min) / 2 +local servo_mid = servo_trim +local servo_half = (servo_max - servo_min) / 2 +local SBUS_OFF = 875 -- 1000 − (2000−1000)*200/1600 +local SBUS_SCALE = 1.6 -- 1/(1000/1600) + +--[[ +servo_val: RCIN or PWM +reverse: 1 or -1 +]] +local function convert_servo_to_sbus(servo_val, reverse) + local norm = (servo_val - servo_mid) / servo_half * reverse + local pwm = in_mid + in_half * norm + return math.floor((pwm - SBUS_OFF) * SBUS_SCALE + 0.5) +end + +local function convert_rcin_to_sbus() + -- gcs:send_text(0, string.format("### convert_rcin_to_sbus ENTER")) + for ch = 1, 16 do + local v = pwm_vals[ch] + if v >= SBUS_OFF then + pwm_vals[ch] = math.floor((v - SBUS_OFF) * SBUS_SCALE + 0.5) + else + pwm_vals[ch] = 0 + end + end +end + +-- debugging +local function notify_sbus_frame(frame) + for i = 1, #frame, 10 do + local bytes = {} + for j = i, math.min(i + 9, #frame) do + bytes[#bytes + 1] = string.format("%02X", string.byte(frame, j)) + end + gcs:send_text(0, string.format( + "SBUS %02d-%02d: %s", + i, + math.min(i + 9, #frame), + table.concat(bytes, " ") + )) + end +end + +-- find_serial (0: TELEM1 1: TELEM2) +port = serial:find_serial(SCR_INS) +if not port then + gcs:send_text(0, "ERROR: TELEM2 port not found") + return +else + port:begin(100000) + -- parity: 0=none, 1=odd, 2=even / stop bits: 1 or 2 + port:configure_parity(2) + port:set_stop_bits(2) + port:set_flow_control(0) + gcs:send_text(MAV_SEVERITY.NOTICE, "TELEM2 port opened for writestring()") +end + +-- main loop：70Hz +local function loop() + read_rcin() + -- debugging + notify_pwm_vals("RCIN PWM") + + convert_rcin_to_sbus() + -- debugging + notify_pwm_vals("SBUS CNT") + + local servo1 = SRV_Channels:get_output_pwm_chan(0) + local servo3 = SRV_Channels:get_output_pwm_chan(2) + + local reverse_ch3 = 1 + local reverse_ch2 = -1 + + pwm_vals[3] = convert_servo_to_sbus(servo1, reverse_ch3) + pwm_vals[2] = convert_servo_to_sbus(servo3, reverse_ch2) + + local sbus_frame = pack_sbus() + -- debugging + notify_sbus_frame(sbus_frame) + + if port then + -- ignore returns + local _ = port:writestring(sbus_frame) + end + + return loop, 14 +end + +init() + +return loop, 0 diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index cc2a8d52d5377..ccabfc94877e9 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -362,9 +362,10 @@ singleton AP_Vehicle method set_target_pos_NED boolean Vector3f boolean float -3 singleton AP_Vehicle method set_target_posvel_NED boolean Vector3f Vector3f singleton AP_Vehicle method set_target_posvelaccel_NED boolean Vector3f Vector3f Vector3f boolean float -360 +360 boolean float'skip_check boolean singleton AP_Vehicle method set_target_velaccel_NED boolean Vector3f Vector3f boolean float -360 +360 boolean float'skip_check boolean -singleton AP_Vehicle method set_target_velocity_NED boolean Vector3f +singleton AP_Vehicle manual set_target_velocity_NED lua_AP_Vehicle_set_target_velocity_NED 2 1 singleton AP_Vehicle method set_target_angle_and_climbrate boolean float -180 180 float -90 90 float -360 360 float'skip_check boolean float'skip_check singleton AP_Vehicle method set_target_rate_and_throttle boolean float'skip_check float'skip_check float'skip_check float'skip_check +singleton AP_Vehicle method set_target_angle_and_rate_and_throttle boolean float -180 180 float -90 90 float -360 360 float'skip_check float'skip_check float'skip_check float'skip_check singleton AP_Vehicle method get_circle_radius boolean float'Null singleton AP_Vehicle method set_circle_rate boolean float'skip_check singleton AP_Vehicle method set_steering_and_throttle boolean float -1 1 float -1 1 @@ -793,6 +794,7 @@ singleton AP::motors() method get_pitch_ff float singleton AP::motors() method get_yaw float singleton AP::motors() method get_yaw_ff float singleton AP::motors() method get_throttle float +singleton AP::motors() method get_throttle_in float singleton AP::motors() method get_forward float singleton AP::motors() method get_lateral float singleton AP::motors() method get_spool_state uint8_t diff --git a/libraries/AP_Scripting/generator/gen/generated_cpp_deps.h b/libraries/AP_Scripting/generator/gen/generated_cpp_deps.h index 109fd8b6ed6aa..e2f3d2facc90a 100644 --- a/libraries/AP_Scripting/generator/gen/generated_cpp_deps.h +++ b/libraries/AP_Scripting/generator/gen/generated_cpp_deps.h @@ -72,13 +72,6 @@ uint32_t get_uint32(lua_State *L, int arg_num, uint32_t min_val, uint32_t max_va return lua_unint32; } -void * new_ap_object(lua_State *L, size_t size, const char * name) { - void * ud = lua_newuserdata(L, size); - luaL_getmetatable(L, name); - lua_setmetatable(L, -2); - return ud; -} - void ** check_ap_object(lua_State *L, int arg_num, const char * name) { void ** data = (void **)luaL_checkudata(L, arg_num, name); if (*data == NULL) { diff --git a/libraries/AP_Scripting/generator/gen/generated_cpp_loader.h b/libraries/AP_Scripting/generator/gen/generated_cpp_loader.h index c8bd2ff21e9c6..67c007b0b4001 100644 --- a/libraries/AP_Scripting/generator/gen/generated_cpp_loader.h +++ b/libraries/AP_Scripting/generator/gen/generated_cpp_loader.h @@ -2,6 +2,45 @@ // for inclusion at the end of the generated .cpp +void * new_ap_object(lua_State *L, size_t size, const char * name) { + void * ud = lua_newuserdata(L, size); + if (luaL_newmetatable(L, name)) { // metatable just created, set it up + for (uint32_t i = 0; i < ARRAY_SIZE(ap_object_fun); i++) { + if (strcmp(name, ap_object_fun[i].name) == 0) { + lua_pushcfunction(L, ap_object_fun[i].func); + lua_setfield(L, -2, "__index"); + break; + } + } + // if no name matched, the metatable will be pointless but otherwise + // valid, so it's safe to continue. since this function isn't + // accessible to user code, it's probably not worth raising an error. + } + // correct metatable is on stack regardless of if we just set it up + lua_setmetatable(L, -2); + return ud; +} + +void set_userdata_metatable(lua_State *L, const char * name) { + if (luaL_newmetatable(L, name)) { // metatable just created, set it up + for (uint32_t i = 0; i < ARRAY_SIZE(userdata_fun); i++) { + if (strcmp(name, userdata_fun[i].name) == 0) { + lua_pushcfunction(L, userdata_fun[i].func); + lua_setfield(L, -2, "__index"); + if (userdata_fun[i].operators != nullptr) { + luaL_setfuncs(L, userdata_fun[i].operators, 0); + } + break; + } + } + // if no name matched, the metatable will be pointless but otherwise + // valid, so it's safe to continue. since this function isn't + // accessible to user code, it's probably not worth raising an error. + } + // correct metatable is on stack regardless of if we just set it up + lua_setmetatable(L, -2); +} + static int binding_index(lua_State *L) { const char * name = luaL_checkstring(L, 2); @@ -39,30 +78,6 @@ static int binding_index(lua_State *L) { return 1; } -void load_generated_bindings(lua_State *L) { - luaL_checkstack(L, 5, nullptr); - // userdata metatables - for (uint32_t i = 0; i < ARRAY_SIZE(userdata_fun); i++) { - luaL_newmetatable(L, userdata_fun[i].name); - lua_pushcfunction(L, userdata_fun[i].func); - lua_setfield(L, -2, "__index"); - if (userdata_fun[i].operators != nullptr) { - luaL_setfuncs(L, userdata_fun[i].operators, 0); - } - lua_pop(L, 1); - } - - // ap object metatables - for (uint32_t i = 0; i < ARRAY_SIZE(ap_object_fun); i++) { - luaL_newmetatable(L, ap_object_fun[i].name); - lua_pushcfunction(L, ap_object_fun[i].func); - lua_setfield(L, -2, "__index"); - lua_pop(L, 1); - } - - // singletons and userdata creation funcs are loaded dynamically -} - void load_generated_sandbox(lua_State *L) { lua_createtable(L, 0, 1); lua_pushcfunction(L, binding_index); diff --git a/libraries/AP_Scripting/generator/gen/generated_h_deps.h b/libraries/AP_Scripting/generator/gen/generated_h_deps.h index ac61424f2a788..183a10b1e1458 100644 --- a/libraries/AP_Scripting/generator/gen/generated_h_deps.h +++ b/libraries/AP_Scripting/generator/gen/generated_h_deps.h @@ -2,7 +2,6 @@ // for inclusion by the generated .h -void load_generated_bindings(lua_State *L); void load_generated_sandbox(lua_State *L); int binding_argcheck(lua_State *L, int expected_arg_count); int field_argerror(lua_State *L); @@ -16,3 +15,4 @@ float get_number(lua_State *L, int arg_num, float min_val, float max_val); uint32_t get_uint32(lua_State *L, int arg_num, uint32_t min_val, uint32_t max_val); void * new_ap_object(lua_State *L, size_t size, const char * name); void ** check_ap_object(lua_State *L, int arg_num, const char * name); +void set_userdata_metatable(lua_State *L, const char * name); diff --git a/libraries/AP_Scripting/generator/src/main.c b/libraries/AP_Scripting/generator/src/main.c index 1134db4380a98..46ef333a94282 100644 --- a/libraries/AP_Scripting/generator/src/main.c +++ b/libraries/AP_Scripting/generator/src/main.c @@ -1346,9 +1346,10 @@ void emit_userdata_allocators(void) { // New method used internally fprintf(source, "%s * new_%s(lua_State *L) {\n", node->name, node->sanitized_name); fprintf(source, " void *ud = lua_newuserdata(L, sizeof(%s));\n", node->name); + // convince compiler to remove null check on the new, lua_newuserdata can't return null + fprintf(source, " if (!ud) { __builtin_unreachable(); } // elide constructor null check\n"); fprintf(source, " new (ud) %s();\n", node->name); - fprintf(source, " luaL_getmetatable(L, \"%s\");\n", node->rename ? node->rename : node->name); - fprintf(source, " lua_setmetatable(L, -2);\n"); + fprintf(source, " set_userdata_metatable(L, \"%s\");\n", node->rename ? node->rename : node->name); fprintf(source, " return (%s *)ud;\n", node->name); fprintf(source, "}\n"); diff --git a/libraries/AP_Scripting/lua/src/ldo.c b/libraries/AP_Scripting/lua/src/ldo.c index 6c627dc3a9fe1..0dd43e966b9fa 100644 --- a/libraries/AP_Scripting/lua/src/ldo.c +++ b/libraries/AP_Scripting/lua/src/ldo.c @@ -10,7 +10,7 @@ #include "lprefix.h" -#include +#include #include #include @@ -71,9 +71,9 @@ #else /* }{ */ /* ISO C handling with long jumps */ -#define LUAI_THROW(L,c) longjmp((c)->b, 1) -#define LUAI_TRY(L,c,a) if (setjmp((c)->b) == 0) { a } -#define luai_jmpbuf jmp_buf +#define LUAI_THROW(L,c) ap_longjmp((c)->b, 1) +#define LUAI_TRY(L,c,a) if (ap_setjmp((c)->b) == 0) { a } +#define luai_jmpbuf ap_jmp_buf #endif /* } */ @@ -133,29 +133,21 @@ l_noret luaD_throw (lua_State *L, int errcode) { } } -// remove optimization -#pragma GCC push_options -#pragma GCC optimize ("O0") + int luaD_rawrunprotected (lua_State *L, Pfunc f, void *ud) { unsigned short oldnCcalls = L->nCcalls; struct lua_longjmp lj; lj.status = LUA_OK; lj.previous = L->errorJmp; /* chain new error handler */ L->errorJmp = &lj; -#ifdef ARM_MATH_CM7 - __asm__("vpush {s16-s31}"); -#endif LUAI_TRY(L, &lj, (*f)(L, ud); ); -#ifdef ARM_MATH_CM7 - __asm__("vpop {s16-s31}"); -#endif L->errorJmp = lj.previous; /* restore old error handler */ L->nCcalls = oldnCcalls; return lj.status; } -#pragma GCC pop_options + /* }====================================================== */ diff --git a/libraries/AP_Scripting/lua_bindings.cpp b/libraries/AP_Scripting/lua_bindings.cpp index 35b74bcd5848b..6cd9d69faf64a 100644 --- a/libraries/AP_Scripting/lua_bindings.cpp +++ b/libraries/AP_Scripting/lua_bindings.cpp @@ -274,7 +274,7 @@ int AP_Logger_Write(lua_State *L) { fix_dot_access_never_add_another_call(L, "logger"); // check we have at least 5 arguments passed in - const int args = lua_gettop(L); + const size_t args = lua_gettop(L); if (args < 5) { return luaL_argerror(L, args, "too few arguments"); } @@ -287,76 +287,68 @@ int AP_Logger_Write(lua_State *L) { if (strlen(name) >= LS_NAME_SIZE) { return luaL_error(L, "Name must be 4 or less chars long"); } - uint8_t length = strlen(labels); - if (length >= (LS_LABELS_SIZE - 7)) { // need 7 chars to add 'TimeUS,' + size_t labels_length = strlen(labels); + if (labels_length >= (LS_LABELS_SIZE - 7)) { // need 7 chars to add 'TimeUS,' return luaL_error(L, "labels must be less than 58 chars long"); } // Count the number of commas - uint8_t commas = 1; - for (uint8_t i=0; i= (LS_FORMAT_SIZE - 1)) { // need 1 char to add timestamp + size_t fmt_length = strlen(fmt); + if (fmt_length >= (LS_FORMAT_SIZE - 1)) { // need 1 char to add timestamp return luaL_error(L, "format must be less than 15 chars long"); } // check the number of arguments matches the number of values in the label - if (length != commas) { + if (fmt_length != commas) { return luaL_argerror(L, args, "label does not match format"); } bool have_units = false; - if (args - 6 == length) { - // check if there are enough arguments for units and multiplyers + if (args - 6 == fmt_length) { + // check if there are enough arguments for units and multipliers have_units = true; - } else if (args - 4 != length) { - // check the number of arguments matches the length of the foramt string + } else if (args - 4 != fmt_length) { + // check the number of arguments matches the length of the format string return luaL_argerror(L, args, "format does not match No. of arguments"); } // prepend timestamp to format and labels char label_cat[LS_LABELS_SIZE]; - strcpy(label_cat,"TimeUS,"); - strcat(label_cat,labels); char fmt_cat[LS_FORMAT_SIZE]; - strcpy(fmt_cat,"Q"); - strcat(fmt_cat,fmt); - - // Need to declare these here so they don't go out of scope - char units_cat[LS_FORMAT_SIZE]; - char multipliers_cat[LS_FORMAT_SIZE]; + snprintf(label_cat, sizeof(label_cat), "TimeUS,%s", labels); + snprintf(fmt_cat, sizeof(fmt_cat), "Q%s", fmt); uint8_t field_start = 4; struct AP_Logger::log_write_fmt *f; if (!have_units) { - // ask for a mesage type + // ask for a message type (will duplicate incoming strings if necessary) f = AP_logger->msg_fmt_for_name(name, label_cat, nullptr, nullptr, fmt_cat, true, true); - } else { - // read in units and multiplers strings + // read in units and multipliers strings field_start += 2; const char * units = luaL_checkstring(L, 5); const char * multipliers = luaL_checkstring(L, 6); - if (length != strlen(units)) { + if (fmt_length != strlen(units)) { return luaL_error(L, "units must be same length as format"); } - if (length != strlen(multipliers)) { + if (fmt_length != strlen(multipliers)) { return luaL_error(L, "multipliers must be same length as format"); } - // prepend timestamp to units and multiplyers - strcpy(units_cat,"s"); - strcat(units_cat,units); + // prepend timestamp to units and multipliers + char units_cat[LS_FORMAT_SIZE]; + char multipliers_cat[LS_FORMAT_SIZE]; + snprintf(units_cat, sizeof(units_cat), "s%s", units); + snprintf(multipliers_cat, sizeof(multipliers_cat), "F%s", multipliers); - strcpy(multipliers_cat,"F"); - strcat(multipliers_cat,multipliers); - - // ask for a mesage type + // ask for a message type (will duplicate incoming strings if necessary) f = AP_logger->msg_fmt_for_name(name, label_cat, units_cat, multipliers_cat, fmt_cat, true, true); } @@ -372,8 +364,11 @@ int AP_Logger_Write(lua_State *L) { return luaL_argerror(L, args, "unknown format"); } - // note that luaM_malloc will never return null, it will fault instead - char *buffer = (char*)luaM_malloc(L, msg_len); + // lua buffers are ~512 bytes on stack. in the unlikely event packets get + // expanded past that, this function should be rewritten to use one. + static_assert(LOG_PACKET_MAX_LEN <= sizeof(luaL_Buffer), "packets are too long"); + + char buffer[LOG_PACKET_MAX_LEN]; // constant buffer size optimizes better // add logging headers uint8_t offset = 0; @@ -396,42 +391,21 @@ int AP_Logger_Write(lua_State *L) { // 'q': int64_t // 'a': int16_t[32] case 'b': { // int8_t - int isnum; - const lua_Integer tmp1 = lua_tointegerx(L, arg_index, &isnum); - if (!isnum || (tmp1 < INT8_MIN) || (tmp1 > INT8_MAX)) { - luaM_free(L, buffer); - luaL_argerror(L, arg_index, "argument out of range"); - // no return - } - int8_t tmp = static_cast(tmp1); + int8_t tmp = get_int8_t(L, arg_index); memcpy(&buffer[offset], &tmp, sizeof(int8_t)); offset += sizeof(int8_t); break; } case 'h': // int16_t case 'c': { // int16_t * 100 - int isnum; - const lua_Integer tmp1 = lua_tointegerx(L, arg_index, &isnum); - if (!isnum || (tmp1 < INT16_MIN) || (tmp1 > INT16_MAX)) { - luaM_free(L, buffer); - luaL_argerror(L, arg_index, "argument out of range"); - // no return - } - int16_t tmp = static_cast(tmp1); + int16_t tmp = get_int16_t(L, arg_index); memcpy(&buffer[offset], &tmp, sizeof(int16_t)); offset += sizeof(int16_t); break; } case 'H': // uint16_t case 'C': { // uint16_t * 100 - int isnum; - const lua_Integer tmp1 = lua_tointegerx(L, arg_index, &isnum); - if (!isnum || (tmp1 < 0) || (tmp1 > UINT16_MAX)) { - luaM_free(L, buffer); - luaL_argerror(L, arg_index, "argument out of range"); - // no return - } - uint16_t tmp = static_cast(tmp1); + uint16_t tmp = get_uint16_t(L, arg_index); memcpy(&buffer[offset], &tmp, sizeof(uint16_t)); offset += sizeof(uint16_t); break; @@ -439,26 +413,14 @@ int AP_Logger_Write(lua_State *L) { case 'i': // int32_t case 'L': // int32_t (lat/long) case 'e': { // int32_t * 100 - int isnum; - const lua_Integer tmp1 = lua_tointegerx(L, arg_index, &isnum); - if (!isnum) { - luaM_free(L, buffer); - luaL_argerror(L, arg_index, "argument out of range"); - // no return - } + const lua_Integer tmp1 = luaL_checkinteger(L, arg_index); const int32_t tmp = tmp1; memcpy(&buffer[offset], &tmp, sizeof(int32_t)); offset += sizeof(int32_t); break; } case 'f': { // float - int isnum; - const lua_Number tmp1 = lua_tonumberx(L, arg_index, &isnum); - if (!isnum) { - luaM_free(L, buffer); - luaL_argerror(L, arg_index, "argument out of range"); - // no return - } + const lua_Number tmp1 = luaL_checknumber(L, arg_index); const float tmp = tmp1; memcpy(&buffer[offset], &tmp, sizeof(float)); offset += sizeof(float); @@ -470,57 +432,25 @@ int AP_Logger_Write(lua_State *L) { } case 'M': // uint8_t (flight mode) case 'B': { // uint8_t - int isnum; - lua_Integer tmp1 = lua_tointegerx(L, arg_index, &isnum); - if (!isnum || (tmp1 < 0) || (tmp1 > UINT8_MAX)) { - // Also allow boolean - if (!isnum && lua_isboolean(L, arg_index)) { - tmp1 = lua_toboolean(L, arg_index); - - } else { - luaM_free(L, buffer); - luaL_argerror(L, arg_index, "argument out of range"); - // no return - } + uint8_t tmp; + if (lua_isboolean(L, arg_index)) { // Also allow boolean + tmp = static_cast(lua_toboolean(L, arg_index)); + } else { + tmp = get_uint8_t(L, arg_index); } - uint8_t tmp = static_cast(tmp1); memcpy(&buffer[offset], &tmp, sizeof(uint8_t)); offset += sizeof(uint8_t); break; } case 'I': // uint32_t case 'E': { // uint32_t * 100 - uint32_t tmp; - void * ud = luaL_testudata(L, arg_index, "uint32_t"); - if (ud != nullptr) { - tmp = *static_cast(ud); - } else { - int success; - const lua_Integer v_int = lua_tointegerx(L, arg_index, &success); - if (success) { - tmp = v_int; - } else { - const lua_Number v_float = lua_tonumberx(L, arg_index, &success); - if (!success || (v_float < 0) || (v_float > float(UINT32_MAX))) { - luaM_free(L, buffer); - luaL_argerror(L, arg_index, "argument out of range"); - // no return - } - tmp = v_float; - } - } + uint32_t tmp = coerce_to_uint32_t(L, arg_index); memcpy(&buffer[offset], &tmp, sizeof(uint32_t)); offset += sizeof(uint32_t); break; } case 'Q': { // uint64_t - void * ud = luaL_testudata(L, arg_index, "uint64_t"); - if (ud == nullptr) { - luaM_free(L, buffer); - luaL_argerror(L, arg_index, "argument out of range"); - // no return - } - uint64_t tmp = *static_cast(ud); + uint64_t tmp = coerce_to_uint64_t(L, arg_index); memcpy(&buffer[offset], &tmp, sizeof(uint64_t)); offset += sizeof(uint64_t); break; @@ -534,23 +464,14 @@ int AP_Logger_Write(lua_State *L) { break; } default: { - luaM_free(L, buffer); - luaL_error(L, "%c unsupported format",fmt_cat[index]); - // no return + return luaL_error(L, "%c unsupported format", fmt_cat[index]); } } if (charlen != 0) { size_t slen; - const char *tmp = lua_tolstring(L, arg_index, &slen); - if (tmp == nullptr) { - luaM_free(L, buffer); - luaL_argerror(L, arg_index, "argument out of range"); - // no return - } + const char *tmp = luaL_checklstring(L, arg_index, &slen); if (slen > charlen) { - luaM_free(L, buffer); - luaL_error(L, "arg %d too long for %c format",arg_index,fmt_cat[index]); - // no return + return luaL_error(L, "arg %d too long for %c format", arg_index, fmt_cat[index]); } memcpy(&buffer[offset], tmp, slen); memset(&buffer[offset+slen], 0, charlen-slen); @@ -560,9 +481,7 @@ int AP_Logger_Write(lua_State *L) { AP_logger->Safe_Write_Emit_FMT(f); - AP_logger->WriteBlock(buffer,msg_len); - - luaM_free(L, buffer); + AP_logger->WriteBlock(buffer, msg_len); return 0; } @@ -1168,7 +1087,8 @@ int lua_range_finder_handle_script_msg(lua_State *L) { #endif // AP_RANGEFINDER_ENABLED /* - lua wants to abort, and doesn't have access to a panic function + Lua raised an error outside protected mode. Outside protected mode, our code + doesn't call functions which can raise errors, so this shouldn't happen (tm). */ void lua_abort() { @@ -1176,11 +1096,7 @@ void lua_abort() #if AP_SIM_ENABLED AP_HAL::panic("lua_abort called"); #else - if (!hal.util->get_soft_armed()) { - AP_HAL::panic("lua_abort called"); - } - // abort while flying, all we can do is loop - while (true) { + while (true) { // scripts will stop but the rest of the system will run hal.scheduler->delay(1000); } #endif @@ -1307,4 +1223,38 @@ int lua_gps_inject_data(lua_State *L) #endif // AP_GPS_ENABLED +#if AP_SCRIPTING_BINDING_VEHICLE_ENABLED +int lua_AP_Vehicle_set_target_velocity_NED(lua_State *L) +{ + const int args = lua_gettop(L); + + if (args > 3) { + return luaL_argerror(L, args, "too many arguments"); + } else if (args < 2) { + return luaL_argerror(L, args, "too few arguments"); + } + + AP_Vehicle * ud = check_AP_Vehicle(L); + Vector3f & data_2 = *check_Vector3f(L, 2); + + bool yaw_to_target = false; + + if (args == 3) { + yaw_to_target = static_cast(lua_toboolean(L, 3)); + } +#if AP_SCHEDULER_ENABLED + AP::scheduler().get_semaphore().take_blocking(); +#endif + const bool data = static_cast(ud->set_target_velocity_NED( + data_2, + yaw_to_target)); + +#if AP_SCHEDULER_ENABLED + AP::scheduler().get_semaphore().give(); +#endif + lua_pushboolean(L, data); + return 1; +} +#endif // AP_SCRIPTING_BINDING_VEHICLE_ENABLED + #endif // AP_SCRIPTING_ENABLED diff --git a/libraries/AP_Scripting/lua_bindings.h b/libraries/AP_Scripting/lua_bindings.h index e2ab60b6ff786..89a228334ffd7 100644 --- a/libraries/AP_Scripting/lua_bindings.h +++ b/libraries/AP_Scripting/lua_bindings.h @@ -37,3 +37,4 @@ int lua_range_finder_handle_script_msg(lua_State *L); int lua_GCS_command_int(lua_State *L); int lua_DroneCAN_get_FlexDebug(lua_State *L); int lua_gps_inject_data(lua_State *L); +int lua_AP_Vehicle_set_target_velocity_NED(lua_State *L); diff --git a/libraries/AP_Scripting/lua_scripts.cpp b/libraries/AP_Scripting/lua_scripts.cpp index a53139cc40ad6..5f07b474e7a1b 100644 --- a/libraries/AP_Scripting/lua_scripts.cpp +++ b/libraries/AP_Scripting/lua_scripts.cpp @@ -30,7 +30,6 @@ extern const AP_HAL::HAL& hal; #define ENABLE_DEBUG_MODULE 0 bool lua_scripts::overtime; -jmp_buf lua_scripts::panic_jmp; char *lua_scripts::error_msg_buf; HAL_Semaphore lua_scripts::error_msg_buf_sem; uint8_t lua_scripts::print_error_count; @@ -130,12 +129,6 @@ void lua_scripts::set_and_print_new_error_message(MAV_SEVERITY severity, const c print_error(severity); } -int lua_scripts::atpanic(lua_State *L) { - set_and_print_new_error_message(MAV_SEVERITY_CRITICAL, "Panic: %s", get_error_object_message(L)); - longjmp(panic_jmp, 1); - return 0; -} - // helper for print and log of runtime stats void lua_scripts::update_stats(const char *name, uint32_t run_time, int total_mem, int run_mem) { @@ -166,40 +159,33 @@ void lua_scripts::update_stats(const char *name, uint32_t run_time, int total_me #endif // HAL_LOGGING_ENABLED } -lua_scripts::script_info *lua_scripts::load_script(lua_State *L, char *filename) { +bool lua_scripts::load_script(lua_State *L, script_info *new_script) { + const char *filename = new_script->name; + if (int error = luaL_loadfile(L, filename)) { switch (error) { case LUA_ERRSYNTAX: set_and_print_new_error_message(MAV_SEVERITY_CRITICAL, "Error: %s", get_error_object_message(L)); lua_pop(L, lua_gettop(L)); - return nullptr; + return false; case LUA_ERRMEM: set_and_print_new_error_message(MAV_SEVERITY_CRITICAL, "Insufficent memory loading %s", filename); lua_pop(L, lua_gettop(L)); - return nullptr; + return false; case LUA_ERRFILE: set_and_print_new_error_message(MAV_SEVERITY_CRITICAL, "Unable to load the file: %s", get_error_object_message(L)); lua_pop(L, lua_gettop(L)); - return nullptr; + return false; default: set_and_print_new_error_message(MAV_SEVERITY_CRITICAL, "Unknown error (%d) loading %s", error, filename); lua_pop(L, lua_gettop(L)); - return nullptr; + return false; } } const int loadMem = lua_gc(L, LUA_GCCOUNT, 0) * 1024 + lua_gc(L, LUA_GCCOUNTB, 0); const uint32_t loadStart = AP_HAL::micros(); - script_info *new_script = (script_info *)_heap.allocate(sizeof(script_info)); - if (new_script == nullptr) { - // No memory, shouldn't happen, we even attempted to do a GC - set_and_print_new_error_message(MAV_SEVERITY_CRITICAL, "Insufficent memory loading %s", filename); - lua_pop(L, 1); // we can't use the function we just loaded, so ditch it - return nullptr; - } - - create_sandbox(L); lua_pushvalue(L, -1); // duplicate environment for reference below lua_setupvalue(L, -3, 1); @@ -209,7 +195,6 @@ lua_scripts::script_info *lua_scripts::load_script(lua_State *L, char *filename) update_stats(filename, loadEnd-loadStart, endMem, loadMem); - new_script->name = filename; new_script->env_ref = luaL_ref(L, LUA_REGISTRYINDEX); // store reference to script's environment new_script->run_ref = luaL_ref(L, LUA_REGISTRYINDEX); // store reference to function to run new_script->next_run_ms = AP_HAL::millis64() - 1; // force the script to be stale @@ -227,7 +212,7 @@ lua_scripts::script_info *lua_scripts::load_script(lua_State *L, char *filename) } } - return new_script; + return true; } void lua_scripts::create_sandbox(lua_State *L) { @@ -288,18 +273,36 @@ void lua_scripts::load_all_scripts_in_dir(lua_State *L, const char *dirname) { // FIXME: because chunk name fetching is not working we are allocating and storing an extra string we shouldn't need to size_t size = strlen(dirname) + strlen(de->d_name) + 2; char * filename = (char *) _heap.allocate(size); - if (filename == nullptr) { + script_info *script = (script_info *)_heap.allocate(sizeof(script_info)); + if ((filename == nullptr) || (script == nullptr)) { + // unlikely to be out of memory for these, just ignore the script... + _heap.deallocate(filename); + _heap.deallocate(script); continue; } snprintf(filename, size, "%s/%s", dirname, de->d_name); - // we have something that looks like a lua file, attempt to load it - script_info * script = load_script(L, filename); - if (script == nullptr) { + // provisionally link the script_info into our list so it will be freed + // if there is an uncaught Lua error during loading + script->env_ref = LUA_NOREF; + script->run_ref = LUA_NOREF; + script->crc = 0; // ensure removing it has no effect on the CRC + script->name = filename; + script->next = scripts; + scripts = script; + + // attempt to load the script, may raise Lua error + bool success = load_script(L, script); + // no Lua error, unlink the script_info so we can handle it properly + scripts = script->next; + script->next = nullptr; + + if (!success) { // discard if load failed _heap.deallocate(filename); + _heap.deallocate(script); continue; } - reschedule_script(script); + reschedule_script(script); // reschedule if load succeeded #if HAL_LOGGER_FILE_CONTENTS_ENABLED if (!option_is_set(AP_Scripting::DebugOption::SUPPRESS_SCRIPT_LOG)) { @@ -377,9 +380,10 @@ void lua_scripts::run_next_script(lua_State *L) { // types match the expectations, go ahead and reschedule script->next_run_ms = start_time_ms + (uint64_t)luaL_checknumber(L, -1); lua_pop(L, 1); - int old_ref = script->run_ref; + luaL_unref(L, LUA_REGISTRYINDEX, script->run_ref); + // cannot cause error as we just made a free ref slot above + // so there won't be any need to allocate more script->run_ref = luaL_ref(L, LUA_REGISTRYINDEX); - luaL_unref(L, LUA_REGISTRYINDEX, old_ref); reschedule_script(script); break; } @@ -421,7 +425,7 @@ void lua_scripts::remove_script(lua_State *L, script_info *script) { } if (L != nullptr) { - // state could be null if we are force killing all scripts + // state will be nullptr when we are tearing down luaL_unref(L, LUA_REGISTRYINDEX, script->env_ref); luaL_unref(L, LUA_REGISTRYINDEX, script->run_ref); } @@ -472,42 +476,51 @@ void *lua_scripts::alloc(void *ud, void *ptr, size_t osize, size_t nsize) { } void lua_scripts::run(void) { - bool succeeded_initial_load = false; - if (!_heap.available()) { GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Lua: Unable to allocate a heap"); return; } - // panic should be hooked first - if (setjmp(panic_jmp)) { - if (!succeeded_initial_load) { - return; - } - if (lua_state != nullptr) { - lua_close(lua_state); // shutdown the old state - } - // remove all the old scheduled scripts - for (script_info *script = scripts; script != nullptr; script = scripts) { - remove_script(nullptr, script); - } - scripts = nullptr; - overtime = false; - } - - lua_state = lua_newstate(alloc, NULL); - lua_State *L = lua_state; + lua_State *L = lua_newstate(alloc, NULL); if (L == nullptr) { GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Lua: Couldn't allocate a lua state"); return; } + // initialize the state with a pointer to us for ls_* callback trampolines + // (see ls_object_from_state) + *static_cast(lua_getextraspace(L)) = this; -#ifndef HAL_CONSOLE_DISABLED - const int inital_mem = lua_gc(L, LUA_GCCOUNT, 0) * 1024 + lua_gc(L, LUA_GCCOUNTB, 0); -#endif + // call main engine function in protected mode now that Lua itself is ready. + // this catches any errors raised by the code between here and Lua scripts. + // our current function must not use any Lua API which can raise an error! + lua_pushcfunction(L, &ls_run_engine); + if (lua_pcall(L, 0, 0, 0) != LUA_OK) { // no args, returns, or msg handler + set_and_print_new_error_message(MAV_SEVERITY_CRITICAL, + "Engine Error: %s", get_error_object_message(L)); + } + // we are now finished with Lua, tear everything down + + lua_close(L); // shut down the state + L = nullptr; - lua_atpanic(L, atpanic); - load_generated_bindings(L); + while (scripts != nullptr) { // remove all scripts from the engine list + remove_script(nullptr, scripts); + } + + // free error message + error_msg_buf_sem.take_blocking(); + if (error_msg_buf != nullptr) { + _heap.deallocate(error_msg_buf); + error_msg_buf = nullptr; + } + error_msg_buf_sem.give(); + + // heap is now empty +} + +int lua_scripts::run_engine(lua_State *L) { + // run our scripting engine now that the Lua state is initialized. we are in + // Lua protected mode and can safely call functions that may raise errors. // set up string metatable. we set up one for all scripts that no script has // access to, as it's impossible to set up one per-script and we don't want @@ -521,8 +534,7 @@ void lua_scripts::run(void) { if (option_is_set(AP_Scripting::DebugOption::RUNTIME_MSG)) { const int loaded_mem = lua_gc(L, LUA_GCCOUNT, 0) * 1024 + lua_gc(L, LUA_GCCOUNTB, 0); - GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "Lua: State memory usage: %i + %i\n", - inital_mem, loaded_mem - inital_mem); + GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "Lua: State memory usage: %i\n", loaded_mem); } // Scan the filesystem in an appropriate manner and autostart scripts @@ -543,10 +555,6 @@ void lua_scripts::run(void) { GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Lua: All directory's disabled see SCR_DIR_DISABLE"); } -#ifndef __clang_analyzer__ - succeeded_initial_load = true; -#endif // __clang_analyzer__ - uint32_t expansion_size = 0; while (AP_Scripting::get_singleton()->should_run()) { @@ -636,22 +644,7 @@ void lua_scripts::run(void) { } } - // make sure all scripts have been removed - while (scripts != nullptr) { - remove_script(lua_state, scripts); - } - - if (lua_state != nullptr) { - lua_close(lua_state); // shutdown the old state - lua_state = nullptr; - } - - error_msg_buf_sem.take_blocking(); - if (error_msg_buf != nullptr) { - _heap.deallocate(error_msg_buf); - error_msg_buf = nullptr; - } - error_msg_buf_sem.give(); + return 0; // no results } // Return the file checksums of running and loaded scripts diff --git a/libraries/AP_Scripting/lua_scripts.h b/libraries/AP_Scripting/lua_scripts.h index d08bb6fe59a9a..7e5e7f114ba00 100644 --- a/libraries/AP_Scripting/lua_scripts.h +++ b/libraries/AP_Scripting/lua_scripts.h @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -31,6 +31,14 @@ #include "lua/src/lua.hpp" +class lua_scripts; + +static inline lua_scripts* ls_object_from_state(lua_State *L) { + // the state extra space stores a pointer to the lua_scripts object the + // state is a part of, used for ls_* callback trampolines. + return *static_cast(lua_getextraspace(L)); +} + class lua_scripts { public: @@ -50,6 +58,11 @@ class lua_scripts private: + static int ls_run_engine(lua_State *L) { + return ls_object_from_state(L)->run_engine(L); + }; + int run_engine(lua_State *L); + void create_sandbox(lua_State *L); typedef struct script_info { @@ -61,7 +74,7 @@ class lua_scripts script_info *next; } script_info; - script_info *load_script(lua_State *L, char *filename); + bool load_script(lua_State *L, script_info *new_script); void reset_loop_overtime(lua_State *L); @@ -80,12 +93,6 @@ class lua_scripts // it must be static to be passed to the C API static void hook(lua_State *L, lua_Debug *ar); - // lua panic handler, will jump back to the start of run - static int atpanic(lua_State *L); - static jmp_buf panic_jmp; - - lua_State *lua_state; - const AP_Int32 & _vm_steps; AP_Int8 & _debug_options; @@ -100,7 +107,7 @@ class lua_scripts // helper for print and log of runtime stats void update_stats(const char *name, uint32_t run_time, int total_mem, int run_mem); - // must be static for use in atpanic + // must be static for bindings static void print_error(MAV_SEVERITY severity); static char *error_msg_buf; static HAL_Semaphore error_msg_buf_sem; @@ -113,7 +120,7 @@ class lua_scripts static HAL_Semaphore crc_sem; public: - // must be static for use in atpanic, public to allow bindings to issue none fatal warnings + // must be static and public to allow bindings to issue non-fatal warnings static void set_and_print_new_error_message(MAV_SEVERITY severity, const char *fmt, ...) FMT_PRINTF(2,3); // return last error message, nullptr if none, must use semaphore as this is updated in the scripting thread diff --git a/libraries/AP_Scripting/modules/crsf_helper.lua b/libraries/AP_Scripting/modules/crsf_helper.lua new file mode 100644 index 0000000000000..39f93a561cdec --- /dev/null +++ b/libraries/AP_Scripting/modules/crsf_helper.lua @@ -0,0 +1,335 @@ +-- crsf_helper.lua +-- A reusable helper library to simplify the creation of ArduPilot CRSF menus. +-- This library abstracts away the complexity of binary packing/unpacking and event loop management. +-- Version 6.4: Adds default handling for COMMAND POLL requests in the event loop, +-- ensuring state synchronization even if script callbacks ignore POLL. + +local helper = {} + +-- MAVLink severity levels for GCS messages +helper.MAV_SEVERITY = {INFO = 6, WARNING = 4, ERROR = 3, DEBUG = 7} +local MAV_SEVERITY = helper.MAV_SEVERITY -- local alias + +-- CRSF constants +local CRSF_EVENT = {PARAMETER_READ = 1, PARAMETER_WRITE = 2} +local CRSF_PARAM_TYPE = { + FLOAT = 8, + TEXT_SELECTION = 9, + FOLDER = 11, + INFO = 12, + COMMAND = 13, +} +helper.CRSF_COMMAND_STATUS = { + READY = 0, -- --> feedback + START = 1, -- <-- input + PROGRESS = 2, -- --> feedback + CONFIRMATION_NEEDED = 3, -- --> feedback + CONFIRM = 4, -- <-- input + CANCEL = 5, -- <-- input + POLL = 6 -- <-- input +} +local CRSF_COMMAND_STATUS = helper.CRSF_COMMAND_STATUS -- local alias + +-- These tables are local, respecting the script sandbox. +-- Each script will have its own instance of this state. +local menu_items = {} +local crsf_objects = {} + +-- #################### +-- # PACKING FUNCTIONS +-- #################### + +-- These functions create the binary packed strings required by the low-level CRSF API. + +-- Creates a CRSF menu text selection item +local function create_selection_entry(name, options_table, current_idx, unit) + -- The CRSF spec requires options to be separated by a semicolon ';'. + local options_str = table.concat(options_table, ";") + local zero_based_idx = current_idx - 1 + local min_val = 0 + local max_val = #options_table - 1 + -- The 4th argument is the current value. The 7th is the default value. + -- For our purposes, we'll pack the current value into both slots. + return string.pack(">BzzBBBBz", CRSF_PARAM_TYPE.TEXT_SELECTION, name, options_str, zero_based_idx, min_val, max_val, zero_based_idx, unit or "") +end + +-- Creates a CRSF menu number item (as float) +local function create_number_entry(name, value, min, max, default, dpoint, step, unit) + -- Per CRSF spec, float values are sent as INT32 with a decimal point indicator. + local scale = 10^(dpoint or 0) + local packed_value = math.floor(value * scale + 0.5) + local packed_min = math.floor(min * scale + 0.5) + local packed_max = math.floor(max * scale + 0.5) + local packed_default = math.floor(default * scale + 0.5) + local packed_step = math.floor(step * scale + 0.5) + return string.pack(">BzllllBlz", CRSF_PARAM_TYPE.FLOAT, name, packed_value, packed_min, packed_max, packed_default, dpoint or 0, packed_step, unit or "") +end + +-- Creates a CRSF menu info item +local function create_info_entry(name, info) + return string.pack(">Bzz", CRSF_PARAM_TYPE.INFO, name, info) +end + +-- Creates a CRSF command entry +local function create_command_entry(name, status, info) + status = status or CRSF_COMMAND_STATUS.READY + info = info or "Execute" + local timeout = 50 -- 5s timeout, increased from 1s for long-running cals + return string.pack(">BzBBz", CRSF_PARAM_TYPE.COMMAND, name, status, timeout, info) +end + +-- #################### +-- # MENU PARSING +-- #################### + +-- Recursively parses a menu definition table and builds the CRSF menu structure. +local function parse_menu(menu_definition, parent_menu_obj) + if not menu_definition.items or type(menu_definition.items) ~= "table" then + return + end + + for _, item_def in ipairs(menu_definition.items) do + local param_obj = nil + local packed_data + + if item_def.type == 'MENU' then + param_obj = parent_menu_obj:add_menu(item_def.name) + if param_obj then + parse_menu(item_def, param_obj) -- Recurse into sub-menu + else + gcs:send_text(MAV_SEVERITY.WARNING, "CRSF: Failed to create menu: " .. item_def.name) + end + + elseif item_def.type == 'SELECTION' then + item_def.current_idx = item_def.default -- Store the initial 1-based index + packed_data = create_selection_entry(item_def.name, item_def.options, item_def.current_idx, item_def.unit) + param_obj = parent_menu_obj:add_parameter(packed_data) + + elseif item_def.type == 'NUMBER' then + item_def.current_val = item_def.default -- Store the initial value + packed_data = create_number_entry(item_def.name, item_def.current_val, item_def.min, item_def.max, item_def.default, item_def.dpoint, item_def.step, item_def.unit) + param_obj = parent_menu_obj:add_parameter(packed_data) + + elseif item_def.type == 'COMMAND' then + item_def.status = CRSF_COMMAND_STATUS.READY + item_def.info = item_def.info or "Execute" + packed_data = create_command_entry(item_def.name, item_def.status, item_def.info) + param_obj = parent_menu_obj:add_parameter(packed_data) + + elseif item_def.type == 'INFO' then + packed_data = create_info_entry(item_def.name, item_def.info) + param_obj = parent_menu_obj:add_parameter(packed_data) + end + + if param_obj then + -- Store a reference to the CRSF object to prevent garbage collection + table.insert(crsf_objects, param_obj) + -- Store the CRSF-assigned ID back into our definition table for easy lookup + menu_items[param_obj:id()] = item_def + elseif not param_obj and item_def.type ~= 'MENU' then + gcs:send_text(MAV_SEVERITY.WARNING, "CRSF: Failed to create param: " .. item_def.name) + end + end +end + +-- #################### +-- # EVENT HANDLING +-- #################### +local idle_backoff = 0 + +-- This function runs as an independent loop for each script. +-- It uses the peek/pop API to safely coexist with other menu scripts. +local function event_loop() + -- Add dynamic polling delays based on arming state + local IDLE_DELAY + local ACTIVE_DELAY + + -- Use the correct arming check from the 'arming' object + if arming:is_armed() then + -- Vehicle is ARMED: prioritize flight code, slow down UI polling + IDLE_DELAY = 500 -- 2.0 Hz idle polling + ACTIVE_DELAY = 100 -- 10 Hz active polling + else + -- Vehicle is DISARMED: prioritize UI responsiveness for setup + IDLE_DELAY = 200 -- 5.0 Hz idle polling + ACTIVE_DELAY = 20 -- 50 Hz active polling + end + + -- ## 1. Peek at the event queue to see if there's anything to do ## + local count, param_id, payload, events = crsf:peek_menu_event() + + -- If the queue is empty, reschedule with the (now dynamic) idle delay. + if count == 0 then + idle_backoff = idle_backoff + 1 + if idle_backoff > (1000/ACTIVE_DELAY) then -- no events in a second so switch to idle + return event_loop, IDLE_DELAY + else + return event_loop, ACTIVE_DELAY + end + end + + -- ## 2. Check if the event belongs to this script's menu ## + local item_def = menu_items[param_id] + if not item_def then + -- This event is not for us. Yield and let another script's loop handle it. + return event_loop, ACTIVE_DELAY -- Use active delay as UI might be busy with other script + end + + idle_backoff = 0 -- no longer idling + + -- ## 3. Pop the event from the queue ## + -- This is critical: pop the event before handling it. + crsf:pop_menu_event() + + -- ## 4. Process the event (it's ours) ## + + -- Handle a READ request from the transmitter first. + if (events & CRSF_EVENT.PARAMETER_READ) ~= 0 then + if item_def.type == 'SELECTION' then + local packed_data = create_selection_entry(item_def.name, item_def.options, item_def.current_idx, item_def.unit) + crsf:send_write_response(packed_data) + elseif item_def.type == 'COMMAND' then + -- Respond with the command's *current* state + local packed_data = create_command_entry(item_def.name, item_def.status, item_def.info) + crsf:send_write_response(packed_data) + elseif item_def.type == 'INFO' then + local packed_data = create_info_entry(item_def.name, item_def.info) + crsf:send_write_response(packed_data) + elseif item_def.type == 'NUMBER' then + -- Respond with the number's *current* state + local packed_data = create_number_entry(item_def.name, item_def.current_val, item_def.min, item_def.max, item_def.default, item_def.dpoint, item_def.step, item_def.unit) + crsf:send_write_response(packed_data) + else + -- Send generic response if type not handled explicitly for read + -- Note: send_response() is deprecated, but kept for potential backward compatibility + -- or if a generic case is truly needed. It uses the popped context. + crsf:send_response() + end + end + + -- Handle a WRITE request from the transmitter. + if (events & CRSF_EVENT.PARAMETER_WRITE) ~= 0 then + if not item_def.callback then + -- No callback, but we must still respond if needed + if item_def.type == 'COMMAND' then + -- Respond with current state even if no callback + local packed_data = create_command_entry(item_def.name, item_def.status, item_def.info) + crsf:send_write_response(packed_data) + elseif item_def.type == 'SELECTION' then + local packed_data = string.pack('>B', item_def.current_idx - 1) + crsf:send_write_response(packed_data) + elseif item_def.type == 'NUMBER' then + local scale = 10^(item_def.dpoint or 0) + local packed_value = math.floor(item_def.current_val * scale + 0.5) + local packed_data = string.pack('>l', packed_value) + crsf:send_write_response(packed_data) + end + goto reschedule -- Skip callback logic + end + + local new_value = nil + + -- Determine the new value from the payload and call the user's callback + if item_def.type == 'SELECTION' then + local selected_index_zero_based = string.unpack(">B", payload) + item_def.current_idx = selected_index_zero_based + 1 + new_value = item_def.options[item_def.current_idx] + elseif item_def.type == 'NUMBER' then + local raw_value = string.unpack(">l", payload) + local scale = 10^(item_def.dpoint or 0) + new_value = raw_value / scale + elseif item_def.type == 'COMMAND' then + -- Pass the specific command action to the callback + local command_action = string.unpack(">B", payload) + -- Check for START, CONFIRM, CANCEL, or POLL + if command_action == CRSF_COMMAND_STATUS.POLL or + command_action == CRSF_COMMAND_STATUS.START or + command_action == CRSF_COMMAND_STATUS.CONFIRM or + command_action == CRSF_COMMAND_STATUS.CANCEL then + new_value = command_action + end + end + + if new_value ~= nil then + -- pcall now expects return values for command state + -- For POLL, callback might return nil or fewer than 2 values if unhandled + local success, ret_status, ret_info = pcall(item_def.callback, new_value) + + if not success then + gcs:send_text(MAV_SEVERITY.ERROR, "CRSF Callback Err: " .. tostring(ret_status)) -- ret_status holds error msg on failure + if item_def.type == 'COMMAND' then + -- Reset command to READY on error + item_def.status = CRSF_COMMAND_STATUS.READY + item_def.info = "Error" + end + else + -- Store new state returned from a successful command callback + if item_def.type == 'COMMAND' then + -- Only update state if callback handled the action (returned non-nil status) + -- For POLL, if callback returns nil, status remains unchanged. + if ret_status ~= nil then + item_def.status = ret_status + item_def.info = ret_info or "Execute" -- Use default info if callback only returns status + end + -- If ret_status is nil (e.g., callback ignored POLL), item_def.status/info remain unchanged. + elseif item_def.type == 'NUMBER' then + -- Store the new value + item_def.current_val = new_value + end + end + end + + -- After a write event, we must respond to confirm the new state to the transmitter. + -- For POLL, respond with current state if callback didn't handle it. + -- For other actions, respond with state potentially updated by callback. + if item_def.type == 'COMMAND' then + -- Respond with the current item_def status/info + local packed_data = create_command_entry(item_def.name, item_def.status, item_def.info) + crsf:send_write_response(packed_data) + elseif item_def.type == 'SELECTION' then + -- The value is the 0-based index of the selection + local packed_data = string.pack('>B', item_def.current_idx - 1) + crsf:send_write_response(packed_data) + elseif item_def.type == 'NUMBER' then + -- The value is the 4-byte int32_t + local scale = 10^(item_def.dpoint or 0) + local packed_value = math.floor(item_def.current_val * scale + 0.5) + local packed_data = string.pack('>l', packed_value) + crsf:send_write_response(packed_data) + end + end + + ::reschedule:: + -- ## 5. Reschedule the loop ## + -- Use the (now dynamic) active delay. + return event_loop, ACTIVE_DELAY +end + +-- #################### +-- # PUBLIC API +-- #################### + +--- Initializes a CRSF menu system for the calling script. +-- @param menu_definition (table) The menu definition table for this script. +function helper.register_menu(menu_definition) + if not (menu_definition and menu_definition.name and menu_definition.items) then + gcs:send_text(MAV_SEVERITY.ERROR, "CRSF: Invalid menu definition passed to helper.register_menu().") + return + end + + -- Create the top-level menu for this specific script + local top_level_menu_obj = crsf:add_menu(menu_definition.name) + if top_level_menu_obj then + -- This function now populates the local menu_items and crsf_objects tables + parse_menu(menu_definition, top_level_menu_obj) + gcs:send_text(MAV_SEVERITY.INFO, "CRSF: Built menu '" .. menu_definition.name .. "'") + else + gcs:send_text(MAV_SEVERITY.WARNING, "CRSF: Failed to create top-level menu for '" .. menu_definition.name .. "'") + return -- Do not start the event loop if the menu could not be created + end + + -- Start this script's independent, persistent event loop. + return event_loop, 2000 +end + +return helper \ No newline at end of file diff --git a/libraries/AP_SurfaceDistance/AP_SurfaceDistance.cpp b/libraries/AP_SurfaceDistance/AP_SurfaceDistance.cpp index d31fc4f12488f..d926418e95cdf 100644 --- a/libraries/AP_SurfaceDistance/AP_SurfaceDistance.cpp +++ b/libraries/AP_SurfaceDistance/AP_SurfaceDistance.cpp @@ -163,6 +163,7 @@ void AP_SurfaceDistance::Log_Write(void) const // @Description: Surface distance measurement // @Field: TimeUS: Time since system startup // @Field: I: Instance + // @Field: St: Surface distance status // @FieldBitmaskEnum: St: Surface_Distance_Status // @Field: D: Raw Distance // @Field: FD: Filtered Distance diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp index e571cda0a5bc2..1b208e636ea90 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp @@ -179,6 +179,90 @@ const AP_Param::GroupInfo AP_TemperatureSensor::var_info[] = { AP_SUBGROUPVARPTR(drivers[8], "9_", 27, AP_TemperatureSensor, backend_var_info[8]), #endif +#if AP_TEMPERATURE_SENSOR_MAX_INSTANCES >= 10 + // @Group: 10_ + // @Path: AP_TemperatureSensor_Params.cpp + AP_SUBGROUPINFO(_params[9], "10_", 28, AP_TemperatureSensor, AP_TemperatureSensor_Params), + + // @Group: 10_ + // @Path: AP_TemperatureSensor_Analog.cpp + // @Group: 10_ + // @Path: AP_TemperatureSensor_DroneCAN.cpp + // @Group: 10_ + // @Path: AP_TemperatureSensor_MAX31865.cpp + AP_SUBGROUPVARPTR(drivers[9], "10_", 29, AP_TemperatureSensor, backend_var_info[9]), +#endif + +#if AP_TEMPERATURE_SENSOR_MAX_INSTANCES >= 11 + // @Group: 11_ + // @Path: AP_TemperatureSensor_Params.cpp + AP_SUBGROUPINFO(_params[10], "11_", 30, AP_TemperatureSensor, AP_TemperatureSensor_Params), + + // @Group: 11_ + // @Path: AP_TemperatureSensor_Analog.cpp + // @Group: 11_ + // @Path: AP_TemperatureSensor_DroneCAN.cpp + // @Group: 11_ + // @Path: AP_TemperatureSensor_MAX31865.cpp + AP_SUBGROUPVARPTR(drivers[10], "11_", 31, AP_TemperatureSensor, backend_var_info[10]), +#endif + +#if AP_TEMPERATURE_SENSOR_MAX_INSTANCES >= 12 + // @Group: 12_ + // @Path: AP_TemperatureSensor_Params.cpp + AP_SUBGROUPINFO(_params[11], "12_", 32, AP_TemperatureSensor, AP_TemperatureSensor_Params), + + // @Group: 12_ + // @Path: AP_TemperatureSensor_Analog.cpp + // @Group: 12_ + // @Path: AP_TemperatureSensor_DroneCAN.cpp + // @Group: 12_ + // @Path: AP_TemperatureSensor_MAX31865.cpp + AP_SUBGROUPVARPTR(drivers[11], "12_", 33, AP_TemperatureSensor, backend_var_info[11]), +#endif + +#if AP_TEMPERATURE_SENSOR_MAX_INSTANCES >= 13 + // @Group: 13_ + // @Path: AP_TemperatureSensor_Params.cpp + AP_SUBGROUPINFO(_params[12], "13_", 34, AP_TemperatureSensor, AP_TemperatureSensor_Params), + + // @Group: 13_ + // @Path: AP_TemperatureSensor_Analog.cpp + // @Group: 13_ + // @Path: AP_TemperatureSensor_DroneCAN.cpp + // @Group: 13_ + // @Path: AP_TemperatureSensor_MAX31865.cpp + AP_SUBGROUPVARPTR(drivers[12], "13_", 35, AP_TemperatureSensor, backend_var_info[12]), +#endif + +#if AP_TEMPERATURE_SENSOR_MAX_INSTANCES >= 14 + // @Group: 14_ + // @Path: AP_TemperatureSensor_Params.cpp + AP_SUBGROUPINFO(_params[13], "14_", 36, AP_TemperatureSensor, AP_TemperatureSensor_Params), + + // @Group: 14_ + // @Path: AP_TemperatureSensor_Analog.cpp + // @Group: 14_ + // @Path: AP_TemperatureSensor_DroneCAN.cpp + // @Group: 14_ + // @Path: AP_TemperatureSensor_MAX31865.cpp + AP_SUBGROUPVARPTR(drivers[13], "14_", 37, AP_TemperatureSensor, backend_var_info[13]), +#endif + +#if AP_TEMPERATURE_SENSOR_MAX_INSTANCES >= 15 + // @Group: 15_ + // @Path: AP_TemperatureSensor_Params.cpp + AP_SUBGROUPINFO(_params[14], "15_", 38, AP_TemperatureSensor, AP_TemperatureSensor_Params), + + // @Group: 15_ + // @Path: AP_TemperatureSensor_Analog.cpp + // @Group: 15_ + // @Path: AP_TemperatureSensor_DroneCAN.cpp + // @Group: 15_ + // @Path: AP_TemperatureSensor_MAX31865.cpp + AP_SUBGROUPVARPTR(drivers[14], "15_", 39, AP_TemperatureSensor, backend_var_info[14]), +#endif + AP_GROUPEND }; diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_SHT3x.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_SHT3x.h index ef4751769bc3b..55282fe4b98c5 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_SHT3x.h +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_SHT3x.h @@ -27,9 +27,10 @@ define AP_TEMPERATURE_SENSOR_BACKEND_DEFAULT_ENABLED 1 pbarker@fx:~$ ./waf configure --board=CubeOrange --extra-hwdef=/tmp/extra.hwdef pbarker@fx:~/rc/ardupilot(pr/SHT3x)$ ./waf rover --upload -# when plugging into I2C1 on a CubeOrange: +# when plugging into I2C2 on a CubeOrange: param set TEMP1_TYPE 8 param set TEMP1_BUS 0 +param set TEMP1_ADDR 0x44 this is bad: AP: SHT3x reset failed diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_config.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_config.h index 2314385753916..e53b2219c91a7 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_config.h +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_config.h @@ -40,7 +40,7 @@ #endif #ifndef AP_TEMPERATURE_SENSOR_SHT3X_ENABLED -#define AP_TEMPERATURE_SENSOR_SHT3X_ENABLED AP_TEMPERATURE_SENSOR_ENABLED +#define AP_TEMPERATURE_SENSOR_SHT3X_ENABLED AP_TEMPERATURE_SENSOR_BACKEND_DEFAULT_ENABLED #endif // AP_TEMPERATURE_SENSOR_SHT3X_ENABLED // maximum number of Temperature Sensors diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 0a6027d4bb852..2384bb776cc8f 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -192,10 +192,11 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { virtual 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 is_terrain_alt) { return false; } virtual bool set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel) { return false; } virtual 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) { return false; } - virtual bool set_target_velocity_NED(const Vector3f& vel_ned) { return false; } + virtual bool set_target_velocity_NED(const Vector3f& vel_ned, bool align_yaw_to_target = false) { return false; } virtual 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 yaw_relative) { return false; } virtual 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) { return false; } virtual bool set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps, float throttle) { return false; } + virtual bool set_target_angle_and_rate_and_throttle(float roll_deg, float pitch_deg, float yaw_deg, float roll_rate_degs, float pitch_rate_degs, float yaw_rate_degs, float throttle) { return false; } // command throttle percentage and roll, pitch, yaw target // rates. For use with scripting controllers diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp index 586034edd13e9..3cbdf899fbe05 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp @@ -171,7 +171,7 @@ void AP_Volz_Protocol::loop() } } -// Send postion commands from PWM, cycle through each servo +// Send position commands from PWM, cycle through each servo void AP_Volz_Protocol::send_position_cmd() { diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h index 1fb515eca71e7..c05bb5c48e550 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h @@ -100,7 +100,7 @@ class AP_Volz_Protocol { // Incoming PWM commands from the servo lib uint16_t servo_pwm[NUM_SERVO_CHANNELS]; - // Send postion commands from PWM, cycle through each servo + // Send position commands from PWM, cycle through each servo void send_position_cmd(); uint8_t last_sent_index; diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index b7a5d828f253d..c2784672572e7 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -610,7 +610,7 @@ class GCS_MAVLINK void handle_rally_fetch_point(const mavlink_message_t &msg); void handle_rally_point(const mavlink_message_t &msg) const; #if HAL_MOUNT_ENABLED - virtual void handle_mount_message(const mavlink_message_t &msg); + void handle_mount_message(const mavlink_message_t &msg); #endif void handle_fence_message(const mavlink_message_t &msg); void handle_param_value(const mavlink_message_t &msg); @@ -675,7 +675,13 @@ class GCS_MAVLINK const uint16_t interval_ms = 10000; } _timesync_request; - void handle_statustext(const mavlink_message_t &msg) const; + void handle_statustext(const mavlink_message_t &msg); + struct { + uint8_t last_src_system; + uint8_t last_src_component; + uint8_t last_id; // ID from the mavlink packet + uint8_t msg_id; // ID used in our logs + } statustext_chunking; void handle_named_value(const mavlink_message_t &msg) const; bool telemetry_delayed() const; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 8ad0e64d1675b..66b792d91815b 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -3818,7 +3818,7 @@ void GCS_MAVLINK::send_timesync() ); } -void GCS_MAVLINK::handle_statustext(const mavlink_message_t &msg) const +void GCS_MAVLINK::handle_statustext(const mavlink_message_t &msg) { #if HAL_LOGGING_ENABLED AP_Logger *logger = AP_Logger::get_singleton(); @@ -3828,23 +3828,38 @@ void GCS_MAVLINK::handle_statustext(const mavlink_message_t &msg) const mavlink_statustext_t packet; mavlink_msg_statustext_decode(&msg, &packet); - const uint8_t max_prefix_len = 20; - const uint8_t text_len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1+max_prefix_len; - char text[text_len] = { 'G','C','S',':'}; - uint8_t offset = strlen(text); - if (!gcs().sysid_is_gcs(msg.sysid)) { - offset = hal.util->snprintf(text, - max_prefix_len, - "SRC=%u/%u:", - msg.sysid, - msg.compid); - offset = MIN(offset, max_prefix_len); + const uint8_t max_prefix_len = 14; + const uint8_t text_len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1+max_prefix_len; + if (msg.sysid != statustext_chunking.last_src_system || + msg.compid != statustext_chunking.last_src_system || + packet.id != statustext_chunking.last_id) { + statustext_chunking.last_src_system = msg.sysid; + statustext_chunking.last_src_component = msg.compid; + statustext_chunking.last_id = packet.id; + statustext_chunking.msg_id = AP::logger().get_MSG_id(); + } + + uint8_t offset = 0; + char text[text_len] = {}; + if (packet.chunk_seq == 0) { + // prefix with origin information + if (gcs().sysid_is_gcs(msg.sysid)) { + strncpy(text, "GCS:", ARRAY_SIZE(text)); + offset = strlen(text); + } else { + offset = hal.util->snprintf(text, + max_prefix_len, + "SRC=%u/%u:", + msg.sysid, + msg.compid); + offset = MIN(offset, max_prefix_len); + } } memcpy(&text[offset], packet.text, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN); - logger->Write_Message(text); + logger->Write_MessageChunk(statustext_chunking.msg_id, text, packet.chunk_seq); #endif } diff --git a/libraries/GCS_MAVLink/GCS_FTP.cpp b/libraries/GCS_MAVLink/GCS_FTP.cpp index 6bbeda9a27cbb..3a2b6da99f4c5 100644 --- a/libraries/GCS_MAVLink/GCS_FTP.cpp +++ b/libraries/GCS_MAVLink/GCS_FTP.cpp @@ -448,7 +448,7 @@ bool GCS_FTP::Session::handle_request(Transaction &request, Transaction &reply) // actually open the file fd = AP::FS().open((char *)request.data, - (request.opcode == FTP_OP::CreateFile) ? O_WRONLY|O_CREAT|O_TRUNC : O_WRONLY); + (request.opcode == FTP_OP::CreateFile) ? O_WRONLY|O_CREAT|O_TRUNC : O_WRONLY|O_CREAT); if (fd == -1) { GCS_FTP::error(reply, FTP_ERROR::FailErrno); break; diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 2ac2a636aec26..be3480ed1574e 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -251,6 +251,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = { // @Values{Plane}: 183: AUTOLAND mode // @Values{Plane}: 184: System ID Chirp // @Values{Copter, Rover, Plane, Blimp, Sub}: 185:Mount Roll/Pitch Lock + // @Values{Copter, Rover, Plane, Blimp, Sub}: 186:Mount POI Lock // @Values{Rover}: 201:Roll // @Values{Rover}: 202:Pitch // @Values{Rover}: 207:MainSail @@ -793,7 +794,10 @@ void RC_Channel::init_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos case AUX_FUNC::RETRACT_MOUNT2: case AUX_FUNC::MOUNT_YAW_LOCK: case AUX_FUNC::MOUNT_RP_LOCK: -#endif +#if AP_MOUNT_POI_LOCK_ENABLED + case AUX_FUNC::MOUNT_POI_LOCK: +#endif //AP_MOUNT_POI_LOCK_ENABLED +#endif //HAL_MOUNT_ENABLED #if HAL_LOGGING_ENABLED case AUX_FUNC::LOG_PAUSE: #endif @@ -915,7 +919,7 @@ const RC_Channel::LookupTable RC_Channel::lookuptable[] = { #if HAL_MOUNT_ENABLED { AUX_FUNC::MOUNT_YAW_LOCK, "Mount Yaw Lock"}, { AUX_FUNC::MOUNT_RP_LOCK, "Mount Roll/Pitch Lock"}, -#endif +#endif //HAL_MOUNT_ENABLED #if HAL_LOGGING_ENABLED { AUX_FUNC::LOG_PAUSE, "Pause Stream Logging"}, #endif @@ -943,7 +947,7 @@ const char *RC_Channel::string_for_aux_function(AUX_FUNC function) const return nullptr; } -/* find string for postion */ +/* find string for position */ const char *RC_Channel::string_for_aux_pos(AuxSwitchPos pos) const { switch (pos) { @@ -1562,7 +1566,7 @@ bool RC_Channel::do_aux_function(const AuxFuncTrigger &trigger) #if AP_BATTERY_ENABLED case AUX_FUNC::BATTERY_MPPT_ENABLE: if (ch_flag != AuxSwitchPos::MIDDLE) { - AP::battery().MPPT_set_powered_state_to_all(ch_flag == AuxSwitchPos::HIGH); + AP::battery().set_powered_state_to_all(ch_flag == AuxSwitchPos::HIGH); } break; #endif @@ -1798,11 +1802,43 @@ bool RC_Channel::do_aux_function(const AuxFuncTrigger &trigger) if (mount == nullptr) { break; } - //low is FPV:no ef locks,high is HORIZON lock:roll/pitch ef lock,mid is only pitch ef lock - mount->set_pitch_lock(ch_flag != AuxSwitchPos::LOW); - mount->set_roll_lock(ch_flag == AuxSwitchPos::HIGH); + //low is FPV:no ef locks,high is HORIZON lock:roll/pitch ef lock,middle is only pitch ef lock + switch (ch_flag) { + case AuxSwitchPos::HIGH: + mount->set_roll_lock(true); + mount->set_pitch_lock(true); + break; + case AuxSwitchPos::MIDDLE: + mount->set_roll_lock(false); + mount->set_pitch_lock(true); + break; + case AuxSwitchPos::LOW: + mount->set_roll_lock(false); + mount->set_pitch_lock(false); + break; + } + break; + } +#if AP_MOUNT_POI_LOCK_ENABLED + case AUX_FUNC::MOUNT_POI_LOCK: { + AP_Mount *mount = AP::mount(); + if (mount == nullptr) { + break; + } + switch (ch_flag) { + case AuxSwitchPos::HIGH: + mount->set_poi_lock(); + break; + case AuxSwitchPos::MIDDLE: + mount->suspend_poi_lock(); + break; + case AuxSwitchPos::LOW: + mount->clear_poi_lock(); + break; + } break; } +#endif // AP_MOUNT_POI_LOCK_ENABLED case AUX_FUNC::MOUNT_LRF_ENABLE: { AP_Mount *mount = AP::mount(); diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 55c4409417cab..22f20c766147c 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -369,7 +369,9 @@ class RC_Channel { AUTOLAND = 183, //Fixed Wing AUTOLAND Mode SYSTEMID = 184, // system ID as an aux switch MOUNT_RP_LOCK = 185, // mount lock modes for roll and pitch axes, for all mounts that support it - +#if AP_MOUNT_POI_LOCK_ENABLED + MOUNT_POI_LOCK = 186, // Lock mount target to current ROI seen and switch mount to GPS Targeting mode +#endif // AP_MOUNT_POI_LOCK_ENABLED // inputs from 200 will eventually used to replace RCMAP ROLL = 201, // roll input PITCH = 202, // pitch input diff --git a/libraries/SITL/SIM_AIS.cpp b/libraries/SITL/SIM_AIS.cpp index 371fd77af5dc9..157f3619b28b1 100644 --- a/libraries/SITL/SIM_AIS.cpp +++ b/libraries/SITL/SIM_AIS.cpp @@ -188,25 +188,25 @@ void AIS::update_simulated_vessel(ais_vessel &vessel, const float dt, const Loca } // Report, update rate depends on speed - uint32_t postion_interval; + uint32_t position_interval; // Convert speed over ground from 0.1 knots to cm/s if (vessel.info.velocity > 23.0 * 0.1 * KNOTS_TO_M_PER_SEC * 100.0) { - postion_interval = 2000; // every 2 seconds if faster than 23 knots + position_interval = 2000; // every 2 seconds if faster than 23 knots } else if (vessel.info.velocity > 14.0 * 0.1 * KNOTS_TO_M_PER_SEC * 100.0) { - postion_interval = 6000; // every 6 seconds if faster than 14 knots + position_interval = 6000; // every 6 seconds if faster than 14 knots } else if (vessel.info.velocity > 0.0) { - postion_interval = 10000; // every 10 seconds if moving + position_interval = 10000; // every 10 seconds if moving } else { - postion_interval = 3 * 60 * 1000; // every 3 mins if anchored or moored and moving at less than 3 knots + position_interval = 3 * 60 * 1000; // every 3 mins if anchored or moored and moving at less than 3 knots } // Send position report at internal - if ((vessel.last_position_report_ms == 0) || (now_ms - vessel.last_position_report_ms > postion_interval)) { + if ((vessel.last_position_report_ms == 0) || (now_ms - vessel.last_position_report_ms > position_interval)) { send_position_report(vessel.info); vessel.last_position_report_ms = now_ms; } @@ -233,7 +233,7 @@ void AIS::init_vessel(ais_vessel &vessel, const Location &vehicle_loc, const flo vessel.info.MMSI = rand() & 0x3FFFFFFF; // Set nav status and type - vessel.info.navigational_status = UNDER_WAY; + vessel.info.navigational_status = AIS_NAV_STATUS_UNDER_WAY; vessel.info.type = AIS_TYPE_CARGO; // 90% chance of having valid dimensions diff --git a/libraries/SITL/SIM_AIS.h b/libraries/SITL/SIM_AIS.h index a39e6cdd8a9d5..0b0295e456fe1 100644 --- a/libraries/SITL/SIM_AIS.h +++ b/libraries/SITL/SIM_AIS.h @@ -64,7 +64,7 @@ class AIS : public SerialDevice { struct ais_vessel { mavlink_ais_vessel_t info; // Info about the vessel - uint32_t last_position_report_ms; // last time a postion report message was sent + uint32_t last_position_report_ms; // last time a position report message was sent uint32_t last_static_and_voyage_ms; // last time a static and voyage data message was sent bool active; // true if this vessel should be reported }; diff --git a/libraries/SITL/SIM_AirSim.cpp b/libraries/SITL/SIM_AirSim.cpp index 25c48a7514e6e..8a0d05a455a8a 100644 --- a/libraries/SITL/SIM_AirSim.cpp +++ b/libraries/SITL/SIM_AirSim.cpp @@ -166,7 +166,7 @@ bool AirSim::parse_sensors(const char *json) break; case DATA_FLOAT: - *((float *)key.ptr) = atof(p); + *((float *)key.ptr) = strtof(p, nullptr); break; case DATA_DOUBLE: @@ -246,7 +246,7 @@ bool AirSim::parse_sensors(const char *json) v->data = d; v->length = n+1; } - v->data[n] = atof(p); + v->data[n] = strtof(p, nullptr); n++; p = strchr(p,','); if (!p) { diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 52b337aac6f74..28be7e2baeebb 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -378,6 +378,11 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) is_smoothed = true; } fdm.timestamp_us = time_now_us; + // keep track of the number of frames processed so that the IMUs can follow + if (flightaxis_sync_imus_to_frames) { + fdm.flightaxis_imu_frame_num++; + } + if (fdm.home.lat == 0 && fdm.home.lng == 0) { // initialise home fdm.home = home; diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 4aab342ed7165..13992dfb2b35b 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -224,6 +224,7 @@ class Aircraft { float battery_current; float local_ground_level; // ground level at local position bool lock_step_scheduled; + bool flightaxis_sync_imus_to_frames; // causes the frame counter to be incremented on each timestep, IMUs will then update at the same rate uint32_t last_one_hz_ms; // battery model diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp index b4c721acf6be2..28720caec05fb 100644 --- a/libraries/SITL/SIM_FlightAxis.cpp +++ b/libraries/SITL/SIM_FlightAxis.cpp @@ -44,8 +44,15 @@ const AP_Param::GroupInfo FlightAxis::var_info[] = { // @Bitmask: 1: Swap first 4 and last 4 servos (for quadplane testing) // @Bitmask: 2: Demix heli servos and send roll/pitch/collective/yaw // @Bitmask: 3: Don't print frame rate stats + // @Bitmask: 4: Don't log Dt stats // @User: Advanced AP_GROUPINFO("OPTS", 1, FlightAxis, _options, uint32_t(Option::ResetPosition)), + + // @Param: SAMPLEHZ + // @DisplayName: FlightAxis IMU synthetic sample rate + // @Description: FlightAxis IMU synthetic sample rate + // @User: Advanced + AP_GROUPINFO("SAMPLEHZ", 2, FlightAxis, _samplehz, 2000), AP_GROUPEND }; @@ -117,6 +124,33 @@ static double timestamp_sec() return tval.tv_sec + (tval.tv_usec*1.0e-6); } +#if defined(CYGWIN_BUILD) +#include + +/* + usleep on cygwin is broken for small times: https://stackoverflow.com/questions/6254703/thread-sleep-for-less-than-1-millisecond + the solution is to define our own busy wait sleep + */ +static void us_wait(int64_t microseconds) { + LARGE_INTEGER frequency; + LARGE_INTEGER start_time, current_time; + + QueryPerformanceFrequency(&frequency); // Get the frequency of the performance counter + QueryPerformanceCounter(&start_time); // Get the current time + + // Calculate the number of ticks required for the specified microseconds + int64_t ticks_to_wait = (int64_t)microseconds * frequency.QuadPart / 1000000; + int64_t end_ticks = start_time.QuadPart + ticks_to_wait; + + // Busy-wait until the desired time has elapsed + do { + QueryPerformanceCounter(¤t_time); + } while (current_time.QuadPart < end_ticks); +} +#else +#define us_wait(x) usleep(x) +#endif + FlightAxis::FlightAxis(const char *frame_str) : Aircraft(frame_str) { @@ -124,6 +158,7 @@ FlightAxis::FlightAxis(const char *frame_str) : AP_Param::setup_object_defaults(this, var_info); use_time_sync = false; + flightaxis_sync_imus_to_frames = true; // tell the IMUs to advance on each frame that is processed rate_hz = 250 / target_speedup; if(strstr(frame_str, "helidemix") != nullptr) { _options.set(_options | uint32_t(Option::HeliDemix)); @@ -152,7 +187,7 @@ FlightAxis::FlightAxis(const char *frame_str) : printf("Failed to create socket_creator thread\n"); } } - + /* extremely primitive SOAP parser that assumes the format used by FlightAxis */ @@ -205,12 +240,12 @@ bool FlightAxis::soap_request_start(const char *action, const char *fmt, ...) vasprintf(&req1, fmt, ap); va_end(ap); + // consumer/producer pattern while (sock == nullptr) { - sockcond1.wait_blocking(); - + sock_outsem.wait_blocking(); sock = socknext; socknext = nullptr; - sockcond2.signal(); + sock_insem.signal(); } char *req; @@ -288,41 +323,62 @@ char *FlightAxis::soap_request_end(uint32_t timeout_ms) return strdup(replybuf); } -void FlightAxis::exchange_data(const struct sitl_input &input) +bool FlightAxis::exchange_data(const struct sitl_input &input) { if (!sock && (!controller_started || - is_zero(state.m_flightAxisControllerIsActive) || - !is_zero(state.m_resetButtonHasBeenPressed))) { - printf("Starting controller at %s\n", controller_ip); - // call a restore first. This allows us to connect after the aircraft is changed in RealFlight - soap_request_start("RestoreOriginalControllerDevice", R"( + is_zero(next_state.m_flightAxisControllerIsActive) || + !is_zero(next_state.m_resetButtonHasBeenPressed))) { + start_controller(); + } + + if (!sock) { + send_request_message(input); + } + if (sock) { + bool ret = process_reply_message(); + if (ret) { + us_wait(250); + send_request_message(input); + } + return ret; + } + return false; +} + +void FlightAxis::start_controller() +{ + printf("Starting controller at %s\n", controller_ip); + // call a restore first. This allows us to connect after the aircraft is changed in RealFlight + soap_request_start("RestoreOriginalControllerDevice", R"( 12 )"); - soap_request_end(1000); - if(option_is_set(Option::ResetPosition)) { - soap_request_start("ResetAircraft", R"( + soap_request_end(1000UL); + if(option_is_set(Option::ResetPosition)) { + soap_request_start("ResetAircraft", R"( 12 )"); - soap_request_end(1000); - } - soap_request_start("InjectUAVControllerInterface", R"( + soap_request_end(1000UL); + } + soap_request_start("InjectUAVControllerInterface", R"( 12 )"); - soap_request_end(1000); - activation_frame_counter = frame_counter; - controller_started = true; - } + soap_request_end(1000UL); + activation_frame_counter = frame_counter; + controller_started = true; +} +void FlightAxis::send_request_message(const struct sitl_input &input) +{ // maximum number of servos to send is 12 with new FlightAxis float scaled_servos[12] {}; uint16_t valid_channels = 0; @@ -363,8 +419,8 @@ void FlightAxis::exchange_data(const struct sitl_input &input) scaled_servos[2] = constrain_float(col, 0, 1); } - if (!sock) { - soap_request_start("ExchangeData", R"( + const uint16_t channels = hal.scheduler->is_system_initialized()?4095:0; + soap_request_start("ExchangeData", R"( @@ -387,60 +443,129 @@ void FlightAxis::exchange_data(const struct sitl_input &input) )", - valid_channels, - scaled_servos[0], - scaled_servos[1], - scaled_servos[2], - scaled_servos[3], - scaled_servos[4], - scaled_servos[5], - scaled_servos[6], - scaled_servos[7], - scaled_servos[8], - scaled_servos[9], - scaled_servos[10], - scaled_servos[11]); - } + channels, + scaled_servos[0], + scaled_servos[1], + scaled_servos[2], + scaled_servos[3], + scaled_servos[4], + scaled_servos[5], + scaled_servos[6], + scaled_servos[7], + scaled_servos[8], + scaled_servos[9], + scaled_servos[10], + scaled_servos[11]); +} +bool FlightAxis::process_reply_message() +{ char *reply = nullptr; - if (sock) { - reply = soap_request_end(0); - if (reply == nullptr) { - sock_error_count++; - if (sock_error_count >= 10000 && timestamp_sec() - last_recv_sec > 1) { - printf("socket timeout\n"); - delete sock; - sock = nullptr; - sock_error_count = 0; - last_recv_sec = timestamp_sec(); - } + reply = soap_request_end(1); + if (reply == nullptr) { + sock_error_count++; + if (sock_error_count >= 10000 && timestamp_sec() - last_recv_sec > 1) { + printf("socket timeout\n"); + delete sock; + sock = nullptr; + sock_error_count = 0; + last_recv_sec = timestamp_sec(); } } if (reply) { sock_error_count = 0; last_recv_sec = timestamp_sec(); - double lastt_s = state.m_currentPhysicsTime_SEC; parse_reply(reply); - double dt = state.m_currentPhysicsTime_SEC - lastt_s; - if (dt > 0 && dt < 0.1) { - if (average_frame_time_s < 1.0e-6) { - average_frame_time_s = dt; - } - average_frame_time_s = average_frame_time_s * 0.98 + dt * 0.02; - } - socket_frame_counter++; free(reply); + return true; } + return false; } +bool FlightAxis::wait_for_sample(const struct sitl_input &input) +{ + double dt_seconds = 0; + const float SAMPLE_INTERVAL_S = 1.0f / _samplehz.get(); + const float SAMPLE_INTERVAL_MIN_S = (SAMPLE_INTERVAL_S/2); // smallest interval before moving on to the next + + double lastt_s = state.m_currentPhysicsTime_SEC; + + if (is_zero(prev_state.m_currentPhysicsTime_SEC)) { + if (exchange_data(input)) { + state = prev_state = next_state; + sample_interval_s = SAMPLE_INTERVAL_S; + } + return false; + } + + // get a new frame if we have processed all the whole samples in the previous one + if (state.m_currentPhysicsTime_SEC + SAMPLE_INTERVAL_MIN_S >= next_state.m_currentPhysicsTime_SEC) { + prev_state = next_state; + do { + if (exchange_data(input)) { // updates next_state + state = prev_state; // ensure state is at the beginning of the data range + + if (is_zero(last_time_s)) { + last_time_s = state.m_currentPhysicsTime_SEC - SAMPLE_INTERVAL_S; + } + dt_seconds = next_state.m_currentPhysicsTime_SEC - last_dt_sample_s; + last_delta_time_s = next_state.m_currentPhysicsTime_SEC - last_time_s; + last_dt_sample_s = next_state.m_currentPhysicsTime_SEC; + } + } while (is_zero(dt_seconds)); + // adjust the sample interval so that the number of samples in a frame is an integer + // this prevents very small dt's which can cause havoc with flight control + sample_interval_s = dt_seconds / roundf(dt_seconds / SAMPLE_INTERVAL_S); + } + + double new_time = MIN(next_state.m_currentPhysicsTime_SEC, state.m_currentPhysicsTime_SEC + sample_interval_s); + state = interpolate_frame(next_state, prev_state, new_time); + + socket_frame_counter++; + + double dt = state.m_currentPhysicsTime_SEC - lastt_s; + if (dt > 0 && dt < 0.1) { + if (average_frame_time_s < 1.0e-6) { + average_frame_time_s = dt; + } + average_frame_time_s = average_frame_time_s * 0.98 + dt * 0.02; + } +#if HAL_LOGGING_ENABLED + if (!(option_is_set(Option::NoDtLog))) { + uint64_t time_now = uint64_t(state.m_currentPhysicsTime_SEC * 1.0e6); +// @LoggerMessage: RF +// @Description: RealFlight mode messages +// @Field: TimeUS: Time since system startup +// @Field: Dt: delta time between this frame and the previous frame +// @Field: Fps: frames-per-second implied by the current delta time + AP::logger().WriteStreaming("RF", "TimeUS,Dt,Fps", "QdI", time_now, dt, uint32_t(roundf(1/dt))); + } +#endif + if (last_time_s > 0) { + if (dt_seconds > 0 && dt_seconds < 0.1) { + if (is_zero(average_delta_time_s)) { + average_delta_time_s = dt_seconds; + } + average_delta_time_s = average_delta_time_s * 0.98 + dt_seconds * 0.02; + } + } + + if (is_equal(last_time_s, state.m_currentPhysicsTime_SEC)) { + AP_HAL::panic("Time did not move"); + } + + return true; +} /* update the FlightAxis simulation by one time step */ void FlightAxis::update(const struct sitl_input &input) { - exchange_data(input); + if (!wait_for_sample(input)) { + return; + } double dt_seconds = state.m_currentPhysicsTime_SEC - last_time_s; if (dt_seconds < 0) { @@ -450,27 +575,10 @@ void FlightAxis::update(const struct sitl_input &input) position_offset.zero(); return; } - if (dt_seconds < 0.00001f) { - float delta_time = 0.001; - // don't go past the next expected frame - if (delta_time + extrapolated_s > average_frame_time_s) { - delta_time = average_frame_time_s - extrapolated_s; - } - if (delta_time <= 0) { - return; - } - time_now_us += delta_time * 1.0e6; - extrapolate_sensors(delta_time); - update_position(); - update_mag_field_bf(); - extrapolated_s += delta_time; - report_FPS(); - return; - } - - extrapolated_s = 0; + // initialize timer if (initial_time_s <= 0) { + time_now_us = 1; // prevent time going backwards dt_seconds = 0.001f; initial_time_s = state.m_currentPhysicsTime_SEC - dt_seconds; } @@ -564,6 +672,7 @@ void FlightAxis::update(const struct sitl_input &input) time_advance(); uint64_t new_time_us = (state.m_currentPhysicsTime_SEC - initial_time_s)*1.0e6; if (new_time_us < time_now_us) { + //printf("Time going backwards by %u\n", uint32_t(time_now_us - new_time_us)); uint64_t dt_us = time_now_us - new_time_us; if (dt_us > 500000) { // time going backwards @@ -581,11 +690,13 @@ void FlightAxis::update(const struct sitl_input &input) dt_us = glitch_threshold_us; glitch_count++; } + if (dt_us) { // adjust the frame time to match the dt which will vary over time + adjust_frame_time(1.0e6/dt_us); + } time_now_us += dt_us; } last_time_s = state.m_currentPhysicsTime_SEC; - last_velocity_ef = velocity_ef; // update magnetic field @@ -601,19 +712,55 @@ void FlightAxis::update(const struct sitl_input &input) report_FPS(); } +struct FlightAxis::state FlightAxis::interpolate_frame(struct state& new_state, struct state& old_state, double new_time) +{ + struct state intermediate_state = old_state; + double dt = new_state.m_currentPhysicsTime_SEC - old_state.m_currentPhysicsTime_SEC; + double interval = new_time - old_state.m_currentPhysicsTime_SEC; + +#define INTERPOLATE(name) (intermediate_state.name = (old_state.name + interval * (new_state.name - old_state.name) / dt)) + INTERPOLATE(m_airspeed_MPS); + INTERPOLATE(m_altitudeAGL_MTR); + INTERPOLATE(m_pitchRate_DEGpSEC); + INTERPOLATE(m_rollRate_DEGpSEC); + INTERPOLATE(m_yawRate_DEGpSEC); + INTERPOLATE(m_aircraftPositionX_MTR); + INTERPOLATE(m_aircraftPositionY_MTR); + INTERPOLATE(m_velocityWorldU_MPS); + INTERPOLATE(m_velocityWorldV_MPS); + INTERPOLATE(m_velocityWorldW_MPS); + INTERPOLATE(m_accelerationBodyAX_MPS2); + INTERPOLATE(m_accelerationBodyAY_MPS2); + INTERPOLATE(m_accelerationBodyAZ_MPS2); + INTERPOLATE(m_windX_MPS); + INTERPOLATE(m_windY_MPS); + INTERPOLATE(m_windZ_MPS); + INTERPOLATE(m_propRPM); + INTERPOLATE(m_heliMainRotorRPM); + INTERPOLATE(m_batteryVoltage_VOLTS); + INTERPOLATE(m_batteryCurrentDraw_AMPS); + INTERPOLATE(m_orientationQuaternion_X); + INTERPOLATE(m_orientationQuaternion_Y); + INTERPOLATE(m_orientationQuaternion_Z); + INTERPOLATE(m_orientationQuaternion_W); + intermediate_state.m_currentPhysicsTime_SEC = new_time; + + return intermediate_state; +} + /* report frame rates */ void FlightAxis::report_FPS(void) { - if (frame_counter++ % 1000 == 0) { + if (frame_counter++ % _samplehz == 0) { if (!is_zero(last_frame_count_s)) { uint64_t frames = socket_frame_counter - last_socket_frame_counter; last_socket_frame_counter = socket_frame_counter; double dt = state.m_currentPhysicsTime_SEC - last_frame_count_s; if(!option_is_set(Option::SilenceFPS)) { - printf("%.2f/%.2f FPS avg=%.2f glitches=%u\n", - frames / dt, 1000 / dt, 1.0/average_frame_time_s, unsigned(glitch_count)); + printf("%.2f/%.2f FPS avg=%.2f FPS net=%.2f glitches=%u\n", + frames / dt, _samplehz / dt, 1.0/average_frame_time_s, 1.0/average_delta_time_s, unsigned(glitch_count)); } } else { printf("Initial position %f %f %f\n", position.x, position.y, position.z); @@ -622,33 +769,33 @@ void FlightAxis::report_FPS(void) } } +#include + void FlightAxis::socket_creator(void) { socket_pid = getpid(); while (true) { - { - while (socknext != nullptr) { - sockcond2.wait_blocking(); - } + while (socknext != nullptr) { + sock_insem.wait_blocking(); } auto *sck = NEW_NOTHROW SocketAPM_native(false); if (sck == nullptr) { - usleep(500); + us_wait(500); continue; } /* - don't let the connection take more than 100ms (10Hz). Longer + don't let the connection take more than 10ms (100Hz). Longer than this and we are better off trying for a new socket */ - if (!sck->connect_timeout(controller_ip, controller_port, 100)) { + if (!sck->connect_timeout(controller_ip, controller_port, 10)) { ::printf("connect failed\n"); delete sck; - usleep(5000); + us_wait(500); continue; } sck->set_blocking(false); socknext = sck; - sockcond1.signal(); + sock_outsem.signal(); } } diff --git a/libraries/SITL/SIM_FlightAxis.h b/libraries/SITL/SIM_FlightAxis.h index c434dcb0fe39b..81092f9ae374a 100644 --- a/libraries/SITL/SIM_FlightAxis.h +++ b/libraries/SITL/SIM_FlightAxis.h @@ -94,91 +94,99 @@ class FlightAxis : public Aircraft { double m_orientationQuaternion_W; double m_flightAxisControllerIsActive; double m_resetButtonHasBeenPressed; - } state; + } state, prev_state, next_state; - static const uint16_t num_keys = sizeof(state)/sizeof(double); + static const uint16_t num_keys = sizeof(next_state)/sizeof(double); struct keytable { const char *key; double &ref; } keytable[num_keys] = { - { "item", state.rcin[0] }, - { "item", state.rcin[1] }, - { "item", state.rcin[2] }, - { "item", state.rcin[3] }, - { "item", state.rcin[4] }, - { "item", state.rcin[5] }, - { "item", state.rcin[6] }, - { "item", state.rcin[7] }, - { "item", state.rcin[8] }, - { "item", state.rcin[9] }, - { "item", state.rcin[10] }, - { "item", state.rcin[11] }, - { "m-airspeed-MPS", state.m_airspeed_MPS }, - { "m-altitudeASL-MTR", state.m_altitudeASL_MTR }, - { "m-altitudeAGL-MTR", state.m_altitudeAGL_MTR }, - { "m-groundspeed-MPS", state.m_groundspeed_MPS }, - { "m-pitchRate-DEGpSEC", state.m_pitchRate_DEGpSEC }, - { "m-rollRate-DEGpSEC", state.m_rollRate_DEGpSEC }, - { "m-yawRate-DEGpSEC", state.m_yawRate_DEGpSEC }, - { "m-azimuth-DEG", state.m_azimuth_DEG }, - { "m-inclination-DEG", state.m_inclination_DEG }, - { "m-roll-DEG", state.m_roll_DEG }, - { "m-aircraftPositionX-MTR", state.m_aircraftPositionX_MTR }, - { "m-aircraftPositionY-MTR", state.m_aircraftPositionY_MTR }, - { "m-velocityWorldU-MPS", state.m_velocityWorldU_MPS }, - { "m-velocityWorldV-MPS", state.m_velocityWorldV_MPS }, - { "m-velocityWorldW-MPS", state.m_velocityWorldW_MPS }, - { "m-velocityBodyU-MPS", state.m_velocityBodyU_MPS }, - { "m-velocityBodyV-MPS", state.m_velocityBodyV_MPS }, - { "m-velocityBodyW-MPS", state.m_velocityBodyW_MPS }, - { "m-accelerationWorldAX-MPS2", state.m_accelerationWorldAX_MPS2 }, - { "m-accelerationWorldAY-MPS2", state.m_accelerationWorldAY_MPS2 }, - { "m-accelerationWorldAZ-MPS2", state.m_accelerationWorldAZ_MPS2 }, - { "m-accelerationBodyAX-MPS2", state.m_accelerationBodyAX_MPS2 }, - { "m-accelerationBodyAY-MPS2", state.m_accelerationBodyAY_MPS2 }, - { "m-accelerationBodyAZ-MPS2", state.m_accelerationBodyAZ_MPS2 }, - { "m-windX-MPS", state.m_windX_MPS }, - { "m-windY-MPS", state.m_windY_MPS }, - { "m-windZ-MPS", state.m_windZ_MPS }, - { "m-propRPM", state.m_propRPM }, - { "m-heliMainRotorRPM", state.m_heliMainRotorRPM }, - { "m-batteryVoltage-VOLTS", state.m_batteryVoltage_VOLTS }, - { "m-batteryCurrentDraw-AMPS", state.m_batteryCurrentDraw_AMPS }, - { "m-batteryRemainingCapacity-MAH", state.m_batteryRemainingCapacity_MAH }, - { "m-fuelRemaining-OZ", state.m_fuelRemaining_OZ }, - { "m-isLocked", state.m_isLocked }, - { "m-hasLostComponents", state.m_hasLostComponents }, - { "m-anEngineIsRunning", state.m_anEngineIsRunning }, - { "m-isTouchingGround", state.m_isTouchingGround }, - { "m-currentAircraftStatus", state.m_currentAircraftStatus }, - { "m-currentPhysicsTime-SEC", state.m_currentPhysicsTime_SEC }, - { "m-currentPhysicsSpeedMultiplier", state.m_currentPhysicsSpeedMultiplier }, - { "m-orientationQuaternion-X", state.m_orientationQuaternion_X }, - { "m-orientationQuaternion-Y", state.m_orientationQuaternion_Y }, - { "m-orientationQuaternion-Z", state.m_orientationQuaternion_Z }, - { "m-orientationQuaternion-W", state.m_orientationQuaternion_W }, - { "m-flightAxisControllerIsActive", state.m_flightAxisControllerIsActive }, - { "m-resetButtonHasBeenPressed", state.m_resetButtonHasBeenPressed }, + { "item", next_state.rcin[0] }, + { "item", next_state.rcin[1] }, + { "item", next_state.rcin[2] }, + { "item", next_state.rcin[3] }, + { "item", next_state.rcin[4] }, + { "item", next_state.rcin[5] }, + { "item", next_state.rcin[6] }, + { "item", next_state.rcin[7] }, + { "item", next_state.rcin[8] }, + { "item", next_state.rcin[9] }, + { "item", next_state.rcin[10] }, + { "item", next_state.rcin[11] }, + { "m-airspeed-MPS", next_state.m_airspeed_MPS }, + { "m-altitudeASL-MTR", next_state.m_altitudeASL_MTR }, + { "m-altitudeAGL-MTR", next_state.m_altitudeAGL_MTR }, + { "m-groundspeed-MPS", next_state.m_groundspeed_MPS }, + { "m-pitchRate-DEGpSEC", next_state.m_pitchRate_DEGpSEC }, + { "m-rollRate-DEGpSEC", next_state.m_rollRate_DEGpSEC }, + { "m-yawRate-DEGpSEC", next_state.m_yawRate_DEGpSEC }, + { "m-azimuth-DEG", next_state.m_azimuth_DEG }, + { "m-inclination-DEG", next_state.m_inclination_DEG }, + { "m-roll-DEG", next_state.m_roll_DEG }, + { "m-aircraftPositionX-MTR", next_state.m_aircraftPositionX_MTR }, + { "m-aircraftPositionY-MTR", next_state.m_aircraftPositionY_MTR }, + { "m-velocityWorldU-MPS", next_state.m_velocityWorldU_MPS }, + { "m-velocityWorldV-MPS", next_state.m_velocityWorldV_MPS }, + { "m-velocityWorldW-MPS", next_state.m_velocityWorldW_MPS }, + { "m-velocityBodyU-MPS", next_state.m_velocityBodyU_MPS }, + { "m-velocityBodyV-MPS", next_state.m_velocityBodyV_MPS }, + { "m-velocityBodyW-MPS", next_state.m_velocityBodyW_MPS }, + { "m-accelerationWorldAX-MPS2", next_state.m_accelerationWorldAX_MPS2 }, + { "m-accelerationWorldAY-MPS2", next_state.m_accelerationWorldAY_MPS2 }, + { "m-accelerationWorldAZ-MPS2", next_state.m_accelerationWorldAZ_MPS2 }, + { "m-accelerationBodyAX-MPS2", next_state.m_accelerationBodyAX_MPS2 }, + { "m-accelerationBodyAY-MPS2", next_state.m_accelerationBodyAY_MPS2 }, + { "m-accelerationBodyAZ-MPS2", next_state.m_accelerationBodyAZ_MPS2 }, + { "m-windX-MPS", next_state.m_windX_MPS }, + { "m-windY-MPS", next_state.m_windY_MPS }, + { "m-windZ-MPS", next_state.m_windZ_MPS }, + { "m-propRPM", next_state.m_propRPM }, + { "m-heliMainRotorRPM", next_state.m_heliMainRotorRPM }, + { "m-batteryVoltage-VOLTS", next_state.m_batteryVoltage_VOLTS }, + { "m-batteryCurrentDraw-AMPS", next_state.m_batteryCurrentDraw_AMPS }, + { "m-batteryRemainingCapacity-MAH", next_state.m_batteryRemainingCapacity_MAH }, + { "m-fuelRemaining-OZ", next_state.m_fuelRemaining_OZ }, + { "m-isLocked", next_state.m_isLocked }, + { "m-hasLostComponents", next_state.m_hasLostComponents }, + { "m-anEngineIsRunning", next_state.m_anEngineIsRunning }, + { "m-isTouchingGround", next_state.m_isTouchingGround }, + { "m-currentAircraftStatus", next_state.m_currentAircraftStatus }, + { "m-currentPhysicsTime-SEC", next_state.m_currentPhysicsTime_SEC }, + { "m-currentPhysicsSpeedMultiplier", next_state.m_currentPhysicsSpeedMultiplier }, + { "m-orientationQuaternion-X", next_state.m_orientationQuaternion_X }, + { "m-orientationQuaternion-Y", next_state.m_orientationQuaternion_Y }, + { "m-orientationQuaternion-Z", next_state.m_orientationQuaternion_Z }, + { "m-orientationQuaternion-W", next_state.m_orientationQuaternion_W }, + { "m-flightAxisControllerIsActive", next_state.m_flightAxisControllerIsActive }, + { "m-resetButtonHasBeenPressed", next_state.m_resetButtonHasBeenPressed }, }; private: bool soap_request_start(const char *action, const char *fmt, ...); char *soap_request_end(uint32_t timeout_ms); - void exchange_data(const struct sitl_input &input); + bool exchange_data(const struct sitl_input &input); + void start_controller(); + void send_request_message(const struct sitl_input &input); + bool process_reply_message(); void parse_reply(const char *reply); + bool wait_for_sample(const struct sitl_input &input); void update_loop(void); void report_FPS(void); void socket_creator(void); + struct state interpolate_frame(struct state& new_state, struct state& old_state, double new_time); + AP_Int32 _options; + AP_Int16 _samplehz; enum class Option : uint32_t{ ResetPosition = (1U<<0), Rev4Servos = (1U<<1), HeliDemix = (1U<<2), SilenceFPS = (1U<<3), + NoDtLog = (1U<<4), }; // return true if an option is set @@ -187,9 +195,11 @@ class FlightAxis : public Aircraft { } double average_frame_time_s; - double extrapolated_s; double initial_time_s; double last_time_s; + double last_dt_sample_s; + double last_delta_time_s; + double sample_interval_s; bool controller_started; uint32_t glitch_count; uint64_t frame_counter; @@ -200,10 +210,17 @@ class FlightAxis : public Aircraft { Vector3d position_offset; Vector3f last_velocity_ef; + double next_sample_s; + double average_delta_time_s; + const char *controller_ip = "127.0.0.1"; uint16_t controller_port = 18083; SocketAPM_native *socknext; SocketAPM_native *sock; + + HAL_BinarySemaphore sock_outsem; + HAL_BinarySemaphore sock_insem; + char replybuf[10000]; pid_t socket_pid; uint32_t sock_error_count; diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index af8ba8686bdcd..b9db8a731086b 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -398,46 +398,52 @@ static Motor firefly_motors[] = Motor(AP_MOTORS_MOT_6, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6, -1, 0, 0, 6, 0, -90) }; +typedef struct { + const char *name; + uint8_t num_motors; + Motor *motors; +} FrameTemplate; + /* table of supported frame types. String order is important for partial name matching */ -static Frame supported_frames[] = -{ - Frame("+", 4, quad_plus_motors), - Frame("quad", 4, quad_plus_motors), - Frame("copter", 4, quad_plus_motors), - Frame("x", 4, quad_x_motors), - Frame("bfxrev", 4, quad_bf_x_rev_motors), - Frame("bfx", 4, quad_bf_x_motors), +static const FrameTemplate supported_frame_templates[] = +{ + {"+", 4, quad_plus_motors}, + {"quad", 4, quad_plus_motors}, + {"copter", 4, quad_plus_motors}, + {"x", 4, quad_x_motors}, + {"bfxrev", 4, quad_bf_x_rev_motors}, + {"bfx", 4, quad_bf_x_motors}, #if AP_SIM_FRAME_COPTER_DOTRIACONTA_OCTAQUAD_X_ENABLED - Frame("dotriaconta", 32, dotriaconta_octaquad_x_motors), + {"dotriaconta", 32, dotriaconta_octaquad_x_motors}, #endif // AP_SIM_FRAME_COPTER_DOTRIACONTA_OCTAQUAD_X_ENABLED - Frame("djix", 4, quad_dji_x_motors), - Frame("cwx", 4, quad_cw_x_motors), - Frame("tilthvec", 4, tiltquad_h_vectored_motors), - Frame("hexadeca-octa", 16, hexadeca_octa_motors), - Frame("hexadeca-octa-cwx", 16, hexadeca_octa_cw_x_motors), - Frame("hexax", 6, hexax_motors), - Frame("hexa-cwx", 6, hexa_cw_x_motors), - Frame("hexa-dji", 6, hexa_dji_x_motors), - Frame("hexa", 6, hexa_motors), - Frame("octa-cwx", 8, octa_cw_x_motors), - Frame("octa-dji", 8, octa_dji_x_motors), - Frame("octa-quad-cwx",8, octa_quad_cw_x_motors), - Frame("octa-quad-cor", 8, octa_quad_corotating_motors), - Frame("octa-quad-cw-cor", 8, octa_quad_cw_corotating_motors), - Frame("octa-quad", 8, octa_quad_motors), - Frame("octa", 8, octa_motors), - Frame("deca", 10, deca_motors), - Frame("deca-cwx", 10, deca_cw_x_motors), - Frame("dodeca-hexa", 12, dodeca_hexa_motors), - Frame("tri", 3, tri_motors), - Frame("tilttrivec",3, tilttri_vectored_motors), - Frame("tilttri", 3, tilttri_motors), - Frame("y6", 6, y6_motors), - Frame("firefly", 6, firefly_motors), - Frame("tilt", 4, tiltquad), + {"djix", 4, quad_dji_x_motors}, + {"cwx", 4, quad_cw_x_motors}, + {"tilthvec", 4, tiltquad_h_vectored_motors}, + {"hexadeca-octa", 16, hexadeca_octa_motors}, + {"hexadeca-octa-cwx", 16, hexadeca_octa_cw_x_motors}, + {"hexax", 6, hexax_motors}, + {"hexa-cwx", 6, hexa_cw_x_motors}, + {"hexa-dji", 6, hexa_dji_x_motors}, + {"hexa", 6, hexa_motors}, + {"octa-cwx", 8, octa_cw_x_motors}, + {"octa-dji", 8, octa_dji_x_motors}, + {"octa-quad-cwx",8, octa_quad_cw_x_motors}, + {"octa-quad-cor", 8, octa_quad_corotating_motors}, + {"octa-quad-cw-cor", 8, octa_quad_cw_corotating_motors}, + {"octa-quad", 8, octa_quad_motors}, + {"octa", 8, octa_motors}, + {"deca", 10, deca_motors}, + {"deca-cwx", 10, deca_cw_x_motors}, + {"dodeca-hexa", 12, dodeca_hexa_motors}, + {"tri", 3, tri_motors}, + {"tilttrivec",3, tilttri_vectored_motors}, + {"tilttri", 3, tilttri_motors}, + {"y6", 6, y6_motors}, + {"firefly", 6, firefly_motors}, + {"tilt", 4, tiltquad}, }; // get air density in kg/m^3 @@ -643,14 +649,15 @@ void Frame::init(const char *frame_str, Battery *_battery) } /* - find a frame by name + create a frame by name from its template */ -Frame *Frame::find_frame(const char *name) +Frame *Frame::create_frame(const char *name) { - for (uint8_t i=0; i < ARRAY_SIZE(supported_frames); i++) { + for (uint8_t i=0; i < ARRAY_SIZE(supported_frame_templates); i++) { + auto &tplate = supported_frame_templates[i]; // `template` is a reserved word // do partial name matching to allow for frame variants - if (strncasecmp(name, supported_frames[i].name, strlen(supported_frames[i].name)) == 0) { - return &supported_frames[i]; + if (strncasecmp(name, tplate.name, strlen(tplate.name)) == 0) { + return NEW_NOTHROW Frame(tplate.name, tplate.num_motors, tplate.motors); } } return nullptr; diff --git a/libraries/SITL/SIM_Frame.h b/libraries/SITL/SIM_Frame.h index 1c5dba3c3256c..836178518bbb2 100644 --- a/libraries/SITL/SIM_Frame.h +++ b/libraries/SITL/SIM_Frame.h @@ -45,8 +45,8 @@ class Frame { motors(_motors) {} #if AP_SIM_ENABLED - // find a frame by name - static Frame *find_frame(const char *name); + // create a frame by name from its template + static Frame *create_frame(const char *name); // initialise frame void init(const char *frame_str, Battery *_battery); diff --git a/libraries/SITL/SIM_GPS.cpp b/libraries/SITL/SIM_GPS.cpp index 86fbca187a87a..8ac046593e220 100644 --- a/libraries/SITL/SIM_GPS.cpp +++ b/libraries/SITL/SIM_GPS.cpp @@ -146,6 +146,14 @@ const AP_Param::GroupInfo SIM::GPSParms::var_info[] = { // @Description: GPS heading offset in degrees. how off the simulated GPS heading is from the actual heading // @User: Advanced AP_GROUPINFO("HDG_OFS", 17, GPSParms, heading_offset, 0), + + // @Param: OPTIONS + // @DisplayName: GPS Options + // @Description: GPS Options bitmask + // @Bitmask: 0:UBlox GPS is F9P + // @User: Advanced + AP_GROUPINFO("OPTIONS", 18, GPSParms, options, 0), + AP_GROUPEND }; } diff --git a/libraries/SITL/SIM_GPS_UBLOX.cpp b/libraries/SITL/SIM_GPS_UBLOX.cpp index f53a30a06453f..4ba5c5091f079 100644 --- a/libraries/SITL/SIM_GPS_UBLOX.cpp +++ b/libraries/SITL/SIM_GPS_UBLOX.cpp @@ -162,6 +162,14 @@ void GPS_UBlox::publish(const GPS_Data *d) uint32_t headVeh; uint8_t reserved2[4]; } pvt {}; + struct PACKED ubx_nav_timegps { + uint32_t itow; + int32_t ftow; + uint16_t week; + int8_t leapS; + uint8_t valid; // leapsvalid | weekvalid | tow valid; + uint32_t tAcc; + } timegps {}; const uint8_t SV_COUNT = 10; struct PACKED ubx_nav_svinfo { uint32_t itow; @@ -187,11 +195,10 @@ void GPS_UBlox::publish(const GPS_Data *d) const uint8_t MSG_VELNED = 0x12; const uint8_t MSG_SOL = 0x6; const uint8_t MSG_PVT = 0x7; + const uint8_t MSG_TIMEGPS = 0x20; const uint8_t MSG_SVINFO = 0x30; const uint8_t MSG_RELPOSNED = 0x3c; - uint32_t _next_nav_sv_info_time = 0; - const auto gps_tow = gps_time(); pos.time = gps_tow.ms; @@ -271,6 +278,13 @@ void GPS_UBlox::publish(const GPS_Data *d) pvt.headVeh = 0; memset(pvt.reserved2, '\0', ARRAY_SIZE(pvt.reserved2)); + timegps.itow = gps_tow.ms; + timegps.ftow = 0; // we don't simulate fractional ns + timegps.week = gps_tow.week; + timegps.leapS = 0; + timegps.valid = d->have_lock ? 0x03 : 0x00; // tow valid|week valid + timegps.tAcc = 0; + switch (_sitl->gps[instance].hdg_enabled) { case SITL::SIM::GPS_HEADING_NONE: case SITL::SIM::GPS_HEADING_BASE: @@ -285,7 +299,18 @@ void GPS_UBlox::publish(const GPS_Data *d) send_ubx(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos)); send_ubx(MSG_STATUS, (uint8_t*)&status, sizeof(status)); send_ubx(MSG_VELNED, (uint8_t*)&velned, sizeof(velned)); - send_ubx(MSG_SOL, (uint8_t*)&sol, sizeof(sol)); + const bool is_f9p = (_sitl->gps[instance].options & static_cast(SITL::SIM::GPSOptions::UBX_IS_F9P)) != 0; + + if (is_f9p) { + const uint32_t now_ms = AP_HAL::millis(); + if ((int32_t)(now_ms - _next_timegps_send_ms) >= 0) { + _next_timegps_send_ms = now_ms + 1000; + send_ubx(MSG_TIMEGPS, (uint8_t*)&timegps, sizeof(timegps)); + } + } else { + // F9P and later use TIMEGPS to set week number, older u-blox use SOL + send_ubx(MSG_SOL, (uint8_t*)&sol, sizeof(sol)); + } send_ubx(MSG_DOP, (uint8_t*)&dop, sizeof(dop)); send_ubx(MSG_PVT, (uint8_t*)&pvt, sizeof(pvt)); if (_sitl->gps[instance].hdg_enabled > SITL::SIM::GPS_HEADING_NONE) { diff --git a/libraries/SITL/SIM_GPS_UBLOX.h b/libraries/SITL/SIM_GPS_UBLOX.h index 06f03cf5d8e60..4aaedd766a586 100644 --- a/libraries/SITL/SIM_GPS_UBLOX.h +++ b/libraries/SITL/SIM_GPS_UBLOX.h @@ -53,6 +53,10 @@ class GPS_UBlox : public GPS_Backend { uint32_t flags; }; + uint32_t _next_timegps_send_ms; + uint32_t _next_nav_sv_info_time; + + void update_relposned(ubx_nav_relposned &relposned, uint32_t tow_ms, float yaw_deg); void send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size); }; diff --git a/libraries/SITL/SIM_JSBSim.cpp b/libraries/SITL/SIM_JSBSim.cpp index 3b2fe6f9b00de..c498bf885b3c4 100644 --- a/libraries/SITL/SIM_JSBSim.cpp +++ b/libraries/SITL/SIM_JSBSim.cpp @@ -64,11 +64,6 @@ JSBSim::JSBSim(const char *frame_str) : if (model_name != nullptr) { jsbsim_model = model_name + 1; } - control_port = 5505 + instance*10; - fdm_port = 5504 + instance*10; - - printf("JSBSim backend started: control_port=%u fdm_port=%u\n", - control_port, fdm_port); } /* @@ -76,6 +71,12 @@ JSBSim::JSBSim(const char *frame_str) : */ bool JSBSim::create_templates(void) { + control_port = 5505 + instance*10; + fdm_port = 5504 + instance*10; + + printf("JSBSim backend started: instance=%u control_port=%u fdm_port=%u\n", + instance, control_port, fdm_port); + if (created_templates) { return true; } @@ -489,4 +490,4 @@ void JSBSim::update(const struct sitl_input &input) } // namespace SITL -#endif // AP_SIM_JSBSIM_ENABLED +#endif // AP_SIM_JSBSIM_ENABLED \ No newline at end of file diff --git a/libraries/SITL/SIM_JSON.cpp b/libraries/SITL/SIM_JSON.cpp index f0fa80a036422..52eca0c3cf1ee 100644 --- a/libraries/SITL/SIM_JSON.cpp +++ b/libraries/SITL/SIM_JSON.cpp @@ -161,7 +161,7 @@ bool parse_array(const char *str, T &arr, int count) { return false; } - arr[i] = atof(p); + arr[i] = strtod(p, nullptr); // Move past the number while (*p && *p != ',' && *p != ']') @@ -176,9 +176,9 @@ bool parse_array(const char *str, T &arr, int count) { return true; } -uint32_t JSON::parse_sensors(const char *json) +uint64_t JSON::parse_sensors(const char *json) { - uint32_t received_bitmask = 0; + uint64_t received_bitmask = 0; #if SITL_JSON_DEBUG && AP_FILESYSTEM_FILE_WRITING_ENABLED // it is useful in some environments to be able to get a copy of the raw @@ -223,7 +223,7 @@ uint32_t JSON::parse_sensors(const char *json) } // record the keys that are found - received_bitmask |= 1U << i; + received_bitmask |= 1ULL << i; p += strlen(key.key)+2; switch (key.type) { @@ -233,7 +233,7 @@ uint32_t JSON::parse_sensors(const char *json) break; case DATA_FLOAT: - *((float *)key.ptr) = atof(p); + *((float *)key.ptr) = strtof(p, nullptr); //printf("%s/%s = %f\n", key.section, key.key, *((float *)key.ptr)); break; @@ -307,6 +307,19 @@ void JSON::recv_fdm(const struct sitl_input &input) // Receive sensor packet ssize_t ret = sock.recv(&sensor_buffer[sensor_buffer_len], sizeof(sensor_buffer)-sensor_buffer_len, UDP_TIMEOUT_MS); uint32_t wait_ms = UDP_TIMEOUT_MS; + + if (state.no_lockstep && ret <= 0) { + // in no_lockstep mode do not block waiting for data; advance time using SITL loop rate + if (sitl != nullptr && sitl->loop_rate_hz > 0) { + frame_time_us = (uint32_t)(1000000.0f / sitl->loop_rate_hz); + } else { + frame_time_us = 10000; // fallback to 10ms + } + time_now_us += frame_time_us; + time_advance(); + return; + } + while (ret <= 0) { //printf("No JSON sensor message received - %s\n", strerror(errno)); ret = sock.recv(&sensor_buffer[sensor_buffer_len], sizeof(sensor_buffer)-sensor_buffer_len, UDP_TIMEOUT_MS); @@ -335,7 +348,7 @@ void JSON::recv_fdm(const struct sitl_input &input) return; } - const uint32_t received_bitmask = parse_sensors((const char *)(p1+1)); + const uint64_t received_bitmask = parse_sensors((const char *)(p1+1)); if (received_bitmask == 0) { // did not receive one of the mandatory fields printf("Did not contain all mandatory fields\n"); @@ -353,7 +366,7 @@ void JSON::recv_fdm(const struct sitl_input &input) printf("\nJSON received:\n"); for (uint16_t i=0; iloop_rate_hz, rate_hz-1, rate_hz+1)); } @@ -573,4 +617,4 @@ void JSON::update(const struct sitl_input &input) #endif } -#endif // AP_SIM_JSON_ENABLED +#endif // AP_SIM_JSON_ENABLED \ No newline at end of file diff --git a/libraries/SITL/SIM_JSON.h b/libraries/SITL/SIM_JSON.h index 44148f83f0694..6d76c237972ce 100644 --- a/libraries/SITL/SIM_JSON.h +++ b/libraries/SITL/SIM_JSON.h @@ -35,7 +35,7 @@ class JSON : public Aircraft { return NEW_NOTHROW JSON(frame_str); } - /* Create and set in/out socket for JSON generic simulator */ + /* Create and set in/out socket for JSON generic simulator */ void set_interface_ports(const char* address, const int port_in, const int port_out) override; private: @@ -73,7 +73,7 @@ class JSON : public Aircraft { void output_servos(const struct sitl_input &input); void recv_fdm(const struct sitl_input &input); - uint32_t parse_sensors(const char *json); + uint64_t parse_sensors(const char *json); // buffer for parsing pose data in JSON format uint8_t sensor_buffer[65000]; @@ -91,6 +91,9 @@ class JSON : public Aircraft { struct { double timestamp_s; + double latitude; + double longitude; + double altitude; struct { Vector3f gyro; Vector3f accel_body; @@ -110,6 +113,7 @@ class JSON : public Aircraft { } wind_vane_apparent; float airspeed; bool no_time_sync; + bool no_lockstep; } state; // table to aid parsing of JSON sensor data @@ -119,11 +123,14 @@ class JSON : public Aircraft { void *ptr; enum data_type type; bool required; - } keytable[32] { + } keytable[36] { { "", "timestamp", &state.timestamp_s, DATA_DOUBLE, true }, + { "", "latitude", &state.latitude, DATA_DOUBLE, false }, + { "", "longitude", &state.longitude, DATA_DOUBLE, false }, + { "", "altitude", &state.altitude, DATA_DOUBLE, false }, { "imu", "gyro", &state.imu.gyro, DATA_VECTOR3F, true }, { "imu", "accel_body", &state.imu.accel_body, DATA_VECTOR3F, true }, - { "", "position", &state.position, DATA_VECTOR3D, true }, + { "", "position", &state.position, DATA_VECTOR3D, false }, { "", "attitude", &state.attitude, DATA_VECTOR3F, false }, { "", "quaternion", &state.quaternion, QUATERNION, false }, { "", "velocity", &state.velocity, DATA_VECTOR3F, true }, @@ -138,6 +145,7 @@ class JSON : public Aircraft { {"windvane","speed", &state.wind_vane_apparent.speed, DATA_FLOAT, false}, {"", "airspeed", &state.airspeed, DATA_FLOAT, false}, {"", "no_time_sync", &state.no_time_sync, BOOLEAN, false}, + {"", "no_lockstep", &state.no_lockstep, BOOLEAN, false}, { "rc", "rc_1", &state.rc[0], DATA_FLOAT, false }, { "rc", "rc_2", &state.rc[1], DATA_FLOAT, false }, { "rc", "rc_3", &state.rc[2], DATA_FLOAT, false }, @@ -155,45 +163,51 @@ class JSON : public Aircraft { }; // Enum coresponding to the ordering of keys in the keytable. - enum DataKey { - TIMESTAMP = 1U << 0, - GYRO = 1U << 1, - ACCEL_BODY = 1U << 2, - POSITION = 1U << 3, - EULER_ATT = 1U << 4, - QUAT_ATT = 1U << 5, - VELOCITY = 1U << 6, - RNG_1 = 1U << 7, - RNG_2 = 1U << 8, - RNG_3 = 1U << 9, - RNG_4 = 1U << 10, - RNG_5 = 1U << 11, - RNG_6 = 1U << 12, - WIND_VEL = 1U << 13, - WIND_DIR = 1U << 14, - WIND_SPD = 1U << 15, - AIRSPEED = 1U << 16, - TIME_SYNC = 1U << 17, - RC_1 = 1U << 18, - RC_2 = 1U << 19, - RC_3 = 1U << 20, - RC_4 = 1U << 21, - RC_5 = 1U << 22, - RC_6 = 1U << 23, - RC_7 = 1U << 24, - RC_8 = 1U << 25, - RC_9 = 1U << 26, - RC_10 = 1U << 27, - RC_11 = 1U << 28, - RC_12 = 1U << 29, - BAT_VOLT = 1U << 30, - BAT_AMP = 1U << 31, + enum DataKey : uint64_t { + TIMESTAMP = 0x0000000000000001ULL, // 1ULL << 0 + LATITUDE = 0x0000000000000002ULL, // 1ULL << 1 + LONGITUDE = 0x0000000000000004ULL, // 1ULL << 2 + ALTITUDE = 0x0000000000000008ULL, // 1ULL << 3 + GYRO = 0x0000000000000010ULL, // 1ULL << 4 + ACCEL_BODY = 0x0000000000000020ULL, // 1ULL << 5 + POSITION = 0x0000000000000040ULL, // 1ULL << 6 + EULER_ATT = 0x0000000000000080ULL, // 1ULL << 7 + QUAT_ATT = 0x0000000000000100ULL, // 1ULL << 8 + VELOCITY = 0x0000000000000200ULL, // 1ULL << 9 + RNG_1 = 0x0000000000000400ULL, // 1ULL << 10 + RNG_2 = 0x0000000000000800ULL, // 1ULL << 11 + RNG_3 = 0x0000000000001000ULL, // 1ULL << 12 + RNG_4 = 0x0000000000002000ULL, // 1ULL << 13 + RNG_5 = 0x0000000000004000ULL, // 1ULL << 14 + RNG_6 = 0x0000000000008000ULL, // 1ULL << 15 + WIND_VEL = 0x0000000000010000ULL, // 1ULL << 16 + WIND_DIR = 0x0000000000020000ULL, // 1ULL << 17 + WIND_SPD = 0x0000000000040000ULL, // 1ULL << 18 + AIRSPEED = 0x0000000000080000ULL, // 1ULL << 19 + TIME_SYNC = 0x0000000000100000ULL, // 1ULL << 20 + LOCKSTEP = 0x0000000000200000ULL, // 1ULL << 21 + RC_1 = 0x0000000000400000ULL, // 1ULL << 22 + RC_2 = 0x0000000000800000ULL, // 1ULL << 23 + RC_3 = 0x0000000001000000ULL, // 1ULL << 24 + RC_4 = 0x0000000002000000ULL, // 1ULL << 25 + RC_5 = 0x0000000004000000ULL, // 1ULL << 26 + RC_6 = 0x0000000008000000ULL, // 1ULL << 27 + RC_7 = 0x0000000010000000ULL, // 1ULL << 28 + RC_8 = 0x0000000020000000ULL, // 1ULL << 29 + RC_9 = 0x0000000040000000ULL, // 1ULL << 30 + RC_10 = 0x0000000080000000ULL, // 1ULL << 31 + RC_11 = 0x0000000100000000ULL, // 1ULL << 32 + RC_12 = 0x0000000200000000ULL, // 1ULL << 33 + BAT_VOLT = 0x0000000400000000ULL, // 1ULL << 34 + BAT_AMP = 0x0000000800000000ULL, // 1ULL << 35 }; - uint32_t last_received_bitmask; + uint64_t last_received_bitmask; uint32_t last_debug_ms; + + bool last_no_lockstep; }; } -#endif // AP_SIM_JSON_ENABLED +#endif // AP_SIM_JSON_ENABLED \ No newline at end of file diff --git a/libraries/SITL/SIM_Morse.cpp b/libraries/SITL/SIM_Morse.cpp index 8b3f203f19b79..d32a967f1efef 100644 --- a/libraries/SITL/SIM_Morse.cpp +++ b/libraries/SITL/SIM_Morse.cpp @@ -164,7 +164,7 @@ bool Morse::parse_sensors(const char *json) p += strlen(key.key)+3; switch (key.type) { case DATA_FLOAT: - *((float *)key.ptr) = atof(p); + *((float *)key.ptr) = strtof(p, nullptr); break; case DATA_DOUBLE: @@ -237,7 +237,7 @@ bool Morse::parse_sensors(const char *json) v->data = d; v->length = n+1; } - v->data[n] = atof(p); + v->data[n] = strtof(p, nullptr); n++; p = strchr(p,','); if (!p) { diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index 273c2fc06dad7..30d0cfef7ed62 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -26,9 +26,9 @@ using namespace SITL; MultiCopter::MultiCopter(const char *frame_str) : Aircraft(frame_str) { - frame = Frame::find_frame(frame_str); + frame = Frame::create_frame(frame_str); if (frame == nullptr) { - printf("Frame '%s' not found", frame_str); + printf("Frame '%s' not found or insufficient memory", frame_str); exit(1); } diff --git a/libraries/SITL/SIM_Multicopter.h b/libraries/SITL/SIM_Multicopter.h index 289aac318290f..b1fa4de706d04 100644 --- a/libraries/SITL/SIM_Multicopter.h +++ b/libraries/SITL/SIM_Multicopter.h @@ -31,6 +31,10 @@ class MultiCopter : public Aircraft { public: MultiCopter(const char *frame_str); + ~MultiCopter() { + delete frame; + } + /* update model by one time step */ void update(const struct sitl_input &input) override; diff --git a/libraries/SITL/SIM_PS_RPLidar.cpp b/libraries/SITL/SIM_PS_RPLidar.cpp index 9e573583a6a7d..089d05da9fb31 100644 --- a/libraries/SITL/SIM_PS_RPLidar.cpp +++ b/libraries/SITL/SIM_PS_RPLidar.cpp @@ -18,11 +18,12 @@ #include "SIM_PS_RPLidar.h" -#if AP_SIM_PS_RPLIDARA2_ENABLED +#if AP_SIM_PS_RPLIDARA2_ENABLED || AP_SIM_PS_RPLIDARA1_ENABLED || AP_SIM_PS_RPLIDARS2_ENABLED #include #include #include +#include using namespace SITL; @@ -82,14 +83,44 @@ void PS_RPLidar::update_input() _buflen--; set_inputstate(InputState::WAITING_FOR_PREAMBLE); set_state(State::IDLE); + _scan_mode = ScanMode::SCAN; return; case Command::SCAN: - // consume the command: + // 5-byte scan mode memmove(_buffer, &_buffer[1], _buflen-1); _buflen--; send_response_descriptor(0x05, SendMode::SRMR, DataType::Unknown81); set_inputstate(InputState::WAITING_FOR_PREAMBLE); set_state(State::SCANNING); + _scan_mode = ScanMode::SCAN; + last_scan_output_time_ms = 0; + last_degrees_bf = 0.0f; + return; + case Command::EXPRESS_SCAN: + // Dense Express mode (40 samples / packet) + // + // Request is: A5 82 <5-byte payload> + // Descriptor: A5 5A 54 00 00 40 85 (len=0x54=84 bytes, type=0x85) + memmove(_buffer, &_buffer[1], _buflen-1); + _buflen--; + if (_buflen >= 1) { + const uint8_t payload_size = (uint8_t)_buffer[0]; + const uint8_t total_to_discard = 1 + payload_size + 1; // size + payload + checksum + if (_buflen >= total_to_discard) { + memmove(_buffer, &_buffer[total_to_discard], _buflen - total_to_discard); + _buflen -= total_to_discard; + } else { + _buflen = 0; + } + } else { + _buflen = 0; + } + send_response_descriptor(0x54, SendMode::SRMR, DataType::Unknown85); + set_inputstate(InputState::WAITING_FOR_PREAMBLE); + set_state(State::SCANNING); + _scan_mode = ScanMode::EXPRESS_SCAN_DENSE; + _express_w_i_deg = 0.0f; + last_scan_output_time_ms = 0; return; case Command::GET_HEALTH: { // consume the command: @@ -138,7 +169,13 @@ void PS_RPLidar::update_input() set_inputstate(InputState::RESETTING_START); return; default: - AP_HAL::panic("Bad command received (%02x)", (uint8_t)_buffer[0]); + const uint8_t bad = static_cast(_buffer[0]); + ::fprintf(stderr, "SIM_RPLidar: unknown command 0x%02x, ignoring\n", bad); + // consume this byte and resync: + memmove(_buffer, &_buffer[1], _buflen-1); + _buflen--; + set_inputstate(InputState::WAITING_FOR_PREAMBLE); + return; } case InputState::RESETTING_START: _firmware_info_offset = 0; @@ -179,7 +216,7 @@ void PS_RPLidar::update_output_scan(const Location &location) last_scan_output_time_ms += sample_count/samples_per_ms; for (uint32_t i=0; i 3200 samples/s (40 samples/packet). + const uint32_t packets_per_second = 80; + const uint32_t packet_period_ms = 1000 / packets_per_second; + + const uint32_t time_delta = (now - last_scan_output_time_ms); + const uint32_t packet_count = time_delta / packet_period_ms; + + if (packet_count == 0) { + return; + } + + last_scan_output_time_ms += packet_count * packet_period_ms; + + // 0.1125 degree per sample -> 4.5 degrees per packet + const float sample_delta_deg = 0.1125f; + const float packet_delta_deg = 40.0f * sample_delta_deg; + + for (uint32_t p = 0; p < packet_count; p++) { + uint8_t packet[84] {}; + // Start angle in degrees for this packet + const float w_i_deg = _express_w_i_deg; + + // start_angle_q6 is in 1/64 degrees + const uint16_t start_angle_q6 = (uint16_t)lrintf(w_i_deg * 64.0f); + + packet[2] = start_angle_q6 & 0xFF; + packet[3] = (start_angle_q6 >> 8) & 0xFF; + + // cabins: 40 distances (2 bytes each, little-endian) + uint8_t* cab = &packet[4]; + for (uint32_t k = 0; k < 40; k++) { + // angle of sample k + float angle_deg = w_i_deg + sample_delta_deg * k; + // wrap to [0, 360) + if (angle_deg >= 360.0f) { + angle_deg = fmodf(angle_deg, 360.0f); + } + + float distance = measure_distance_at_angle_bf(location, angle_deg); + if (distance > max_range()) { + distance = 0.0f; + } + + const uint16_t dist_mm = (uint16_t)lrintf(distance * 1000.0f); + cab[0] = dist_mm & 0xFF; + cab[1] = (dist_mm >> 8) & 0xFF; + cab += 2; + } + + // checksum: XOR of bytes [2..83] + uint8_t checksum = 0; + for (uint32_t i = 2; i < sizeof(packet); i++) { + checksum ^= packet[i]; + } + + // sync1/sync2 and checksum nibbles (dense format) + const uint8_t sync1 = 0x0A; + const uint8_t sync2 = 0x05; + + packet[0] = uint8_t((sync1 << 4) | (checksum & 0x0F)); + packet[1] = uint8_t((sync2 << 4) | ((checksum >> 4) & 0x0F)); + + const ssize_t ret = write_to_autopilot((const char*)packet, sizeof(packet)); + if (ret != (ssize_t)sizeof(packet)) { + abort(); + } + + // advance start angle for next packet + _express_w_i_deg += packet_delta_deg; + if (_express_w_i_deg >= 360.0f) { + _express_w_i_deg = fmodf(_express_w_i_deg, 360.0f); + } + } +} + void PS_RPLidar::update_output(const Location &location) { switch (_state) { case State::IDLE: return; case State::SCANNING: - update_output_scan(location); + if (_scan_mode == ScanMode::SCAN) { + update_output_scan(location); + } else { + update_output_express_dense(location); + } return; } } @@ -255,4 +380,4 @@ void PS_RPLidar::send_response_descriptor(uint32_t data_response_length, SendMod } } -#endif // AP_SIM_PS_RPLIDARA2_ENABLED +#endif // AP_SIM_PS_RPLIDARA2_ENABLED || AP_SIM_PS_RPLIDARA1_ENABLED || AP_SIM_PS_RPLIDARS2_ENABLED diff --git a/libraries/SITL/SIM_PS_RPLidar.h b/libraries/SITL/SIM_PS_RPLidar.h index 7e851f6bf14c9..eb662bbbe1b3f 100644 --- a/libraries/SITL/SIM_PS_RPLidar.h +++ b/libraries/SITL/SIM_PS_RPLidar.h @@ -15,7 +15,7 @@ Base class for RPLidar support */ /* - Simulator for the RPLidarA2 proximity sensor + Simulator for the RPLidar proximity sensor ./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:rplidara2 --speedup=1 -l 51.8752066,14.6487840,54.15,0 --map @@ -47,7 +47,7 @@ rc 2 1450 #include "SIM_config.h" -#if AP_SIM_PS_RPLIDARA2_ENABLED || AP_SIM_PS_RPLIDARA1_ENABLED +#if AP_SIM_PS_RPLIDARA2_ENABLED || AP_SIM_PS_RPLIDARA1_ENABLED || AP_SIM_PS_RPLIDARS2_ENABLED #include "SIM_SerialProximitySensor.h" @@ -70,8 +70,13 @@ class PS_RPLidar : public SerialProximitySensor { void update_input(); void update_output(const Location &location); + + // 5-byte scan output void update_output_scan(const Location &location); + // dense express scan output (40 samples per packet) + void update_output_express_dense(const Location &location); + uint32_t last_scan_output_time_ms; float last_degrees_bf; @@ -104,21 +109,23 @@ class PS_RPLidar : public SerialProximitySensor { static const uint8_t PREAMBLE = 0xA5; - enum class Command { + enum class Command : uint8_t { STOP = 0x25, SCAN = 0x20, FORCE_SCAN = 0x21, RESET = 0x40, GET_DEVICE_INFO = 0x50, GET_HEALTH = 0x52, + EXPRESS_SCAN = 0x82, }; void move_preamble_in_buffer(); - enum class DataType { - Unknown04 = 0x04, // uint8_t ?! - Unknown06 = 0x06, // uint8_t ?! - Unknown81 = 0x81, // uint8_t ?! + enum class DataType : uint8_t { + Unknown04 = 0x04, // device info + Unknown06 = 0x06, // health + Unknown81 = 0x81, // scan (5-byte) + Unknown85 = 0x85 // dense express scan }; enum class SendMode { @@ -135,6 +142,16 @@ class PS_RPLidar : public SerialProximitySensor { static const constexpr char *FIRMWARE_INFO = "R12345678901234567890123456789012345678901234567890123456789012"; uint8_t _firmware_info_offset; + // scan type + enum class ScanMode { + SCAN = 0, + EXPRESS_SCAN_DENSE = 1, + }; + ScanMode _scan_mode = ScanMode::SCAN; + + // state for dense express packets (current start angle in degrees) + float _express_w_i_deg = 0.0f; + // methods for sub-classes to implement: virtual uint8_t device_info_model() const = 0; virtual uint8_t max_range() const = 0; @@ -142,4 +159,4 @@ class PS_RPLidar : public SerialProximitySensor { }; -#endif // AP_SIM_PS_RPLIDARA2_ENABLED || AP_SIM_PS_RPLIDARA1_ENABLED +#endif // AP_SIM_PS_RPLIDARA2_ENABLED || AP_SIM_PS_RPLIDARA1_ENABLED || AP_SIM_PS_RPLIDARS2_ENABLED diff --git a/libraries/SITL/SIM_PS_RPLidarS2.h b/libraries/SITL/SIM_PS_RPLidarS2.h new file mode 100644 index 0000000000000..7936c36affd5d --- /dev/null +++ b/libraries/SITL/SIM_PS_RPLidarS2.h @@ -0,0 +1,61 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + Simulator for the RPLidar S2 proximity sensor + +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:rplidars2 --speedup=1 -l 51.8752066,14.6487840,0,0 --map + +param set SERIAL5_PROTOCOL 11 +param set PRX1_TYPE 5 +reboot + +arm throttle +rc 3 1600 + +# for avoidance: +param set DISARM_DELAY 0 +param set AVOID_ENABLE 2 # use proximity sensor +param set AVOID_MARGIN 2.00 # 2m +param set AVOID_BEHAVE 0 # slide +param set OA_DB_OUTPUT 3 +param set OA_TYPE 2 +reboot +mode loiter +script /tmp/post-locations.scr +arm throttle +rc 3 1600 +rc 3 1500 +rc 2 1450 +*/ + +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_PS_RPLIDARS2_ENABLED + +#include "SIM_PS_RPLidar.h" + +namespace SITL { + +class PS_RPLidarS2 : public PS_RPLidar { +public: + uint8_t device_info_model() const override { return 0x71; } + uint8_t max_range() const override { return 50; } +}; + +} + +#endif // AP_SIM_PS_RPLIDARS2_ENABLED diff --git a/libraries/SITL/SIM_QuadPlane.cpp b/libraries/SITL/SIM_QuadPlane.cpp index 70ee81dee4cd0..924e1af4be6b4 100644 --- a/libraries/SITL/SIM_QuadPlane.cpp +++ b/libraries/SITL/SIM_QuadPlane.cpp @@ -83,9 +83,9 @@ QuadPlane::QuadPlane(const char *frame_str) : ground_behavior = GROUND_BEHAVIOR_TAILSITTER; thrust_scale *= 1.5; } - frame = Frame::find_frame(frame_type); + frame = Frame::create_frame(frame_type); if (frame == nullptr) { - printf("Failed to find frame '%s'\n", frame_type); + printf("Failed to find frame '%s' or insufficient memory\n", frame_type); exit(1); } diff --git a/libraries/SITL/SIM_QuadPlane.h b/libraries/SITL/SIM_QuadPlane.h index 1915e5aa97c09..d73d460cda543 100644 --- a/libraries/SITL/SIM_QuadPlane.h +++ b/libraries/SITL/SIM_QuadPlane.h @@ -31,6 +31,10 @@ class QuadPlane : public Plane { public: QuadPlane(const char *frame_str); + ~QuadPlane() { + delete frame; + } + /* update model by one time step */ void update(const struct sitl_input &input) override; diff --git a/libraries/SITL/SIM_Webots.cpp b/libraries/SITL/SIM_Webots.cpp index 9897260e39238..0332230025b04 100644 --- a/libraries/SITL/SIM_Webots.cpp +++ b/libraries/SITL/SIM_Webots.cpp @@ -182,7 +182,7 @@ bool Webots::parse_sensors(const char *json) p += strlen(key.key)+3; switch (key.type) { case DATA_FLOAT: - *((float *)key.ptr) = atof(p); + *((float *)key.ptr) = strtof(p, nullptr); //printf("GOT %s/%s value: %f\n", key.section, key.key, *((float *)key.ptr)); break; @@ -267,7 +267,7 @@ bool Webots::parse_sensors(const char *json) v->data = d; v->length = n+1; } - v->data[n] = atof(p); + v->data[n] = strtof(p, nullptr); n++; p = strchr(p,','); if (!p) { diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index dfbb8ed5b4422..385191a517102 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -44,6 +44,10 @@ #define AP_SIM_PS_RPLIDARA2_ENABLED AP_SIM_SERIALPROXIMITYSENSOR_ENABLED #endif +#ifndef AP_SIM_PS_RPLIDARS2_ENABLED +#define AP_SIM_PS_RPLIDARS2_ENABLED AP_SIM_SERIALPROXIMITYSENSOR_ENABLED +#endif + #ifndef AP_SIM_PS_TERARANGERTOWER_ENABLED #define AP_SIM_PS_TERARANGERTOWER_ENABLED AP_SIM_SERIALPROXIMITYSENSOR_ENABLED #endif // AP_SIM_PS_TERARANGERTOWER_ENABLED diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 2cc3ae52ff789..84b2ae106a8ef 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -59,6 +59,7 @@ class FlightAxis; struct sitl_fdm { // this is the structure passed between FDM models and the main SITL code uint64_t timestamp_us; + uint64_t flightaxis_imu_frame_num; // the sitl frame number that should have a corresponding imu sample Location home; double latitude, longitude; // degrees double altitude; // MSL @@ -171,6 +172,10 @@ class SIM { GPS_HEADING_BASE = 4, // act as an RTK base }; + enum class GPSOptions : uint32_t { + UBX_IS_F9P = 1U << 0, + }; + struct sitl_fdm state; // throttle when motors are active @@ -333,6 +338,7 @@ class SIM { AP_Vector3f vel_err; // Velocity error offsets in NED (x = N, y = E, z = D) AP_Int8 jam; // jamming simulation enable AP_Float heading_offset; // heading offset in degrees + AP_Int32 options; // GPS options bitmask }; GPSParms gps[AP_SIM_MAX_GPS_SENSORS]; diff --git a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.c b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.c index e33a3fab89b92..934fdb5cc0301 100644 --- a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.c +++ b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.c @@ -187,7 +187,7 @@ bool parse_controls(const char *json) switch (key->type) { case DATA_FLOAT: - *((float *)key->ptr) = atof(p); + *((float *)key->ptr) = strtof(p, NULL); #ifdef DEBUG_INPUT_DATA printf("GOT %s/%s\n", key->section, key->key); #endif @@ -347,7 +347,7 @@ bool initialize (int argc, char *argv[]) { // specify drag functor used to simulate air resistance. if (argc > i+1 ) { - dragFactor = atof (argv[i+1]); + dragFactor = strtof (argv[i+1], NULL); printf("drag Factor %f\n",dragFactor); } else diff --git a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_ROVER/ardupilot_SITL_ROVER.c b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_ROVER/ardupilot_SITL_ROVER.c index e68a88b69f1c8..a2b7f49e6cb9e 100644 --- a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_ROVER/ardupilot_SITL_ROVER.c +++ b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_ROVER/ardupilot_SITL_ROVER.c @@ -139,7 +139,7 @@ bool parse_controls(const char *json) switch (key->type) { case DATA_FLOAT: - *((float *)key->ptr) = atof(p); + *((float *)key->ptr) = strtof(p, NULL); #ifdef DEBUG_INPUT_DATA printf("GOT %s/%s\n", key->section, key->key); #endif diff --git a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/ardupilot_SITL_TRICOPTER.c b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/ardupilot_SITL_TRICOPTER.c index 6ca7ad944833a..e4304f96aa460 100644 --- a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/ardupilot_SITL_TRICOPTER.c +++ b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/ardupilot_SITL_TRICOPTER.c @@ -276,7 +276,7 @@ bool parse_controls(const char *json) switch (key->type) { case DATA_FLOAT: - *((float *)key->ptr) = atof(p); + *((float *)key->ptr) = strtof(p, NULL); #ifdef DEBUG_INPUT_DATA printf("GOT %s/%s\n", key->section, key->key); #endif @@ -422,7 +422,7 @@ bool initialize (int argc, char *argv[]) { // specify drag functor used to simulate air resistance. if (argc > i+1 ) { - dragFactor = atof (argv[i+1]); + dragFactor = strtof (argv[i+1], NULL); printf("drag Factor %f\n",dragFactor); } else diff --git a/libraries/SRV_Channel/SRV_Channels.cpp b/libraries/SRV_Channel/SRV_Channels.cpp index 292aa2bcacb77..5bdb8ecda10f4 100644 --- a/libraries/SRV_Channel/SRV_Channels.cpp +++ b/libraries/SRV_Channel/SRV_Channels.cpp @@ -530,7 +530,7 @@ void SRV_Channels::push() ap_dronecan->SRV_push_servos(); break; } -#if HAL_PICCOLO_CAN_ENABLE +#if AP_PICCOLOCAN_ENABLED case AP_CAN::Protocol::PiccoloCAN: { AP_PiccoloCAN *ap_pcan = AP_PiccoloCAN::get_pcan(i); if (ap_pcan == nullptr) { diff --git a/modules/DroneCAN/DSDL b/modules/DroneCAN/DSDL index 1b2118cf35802..04e0e818b06c1 160000 --- a/modules/DroneCAN/DSDL +++ b/modules/DroneCAN/DSDL @@ -1 +1 @@ -Subproject commit 1b2118cf358027453830ef644838a3bedb9411ea +Subproject commit 04e0e818b06c180eb1720fcdf16484d0f12895ee diff --git a/modules/DroneCAN/dronecan_dsdlc b/modules/DroneCAN/dronecan_dsdlc index bd9124715cc7c..de47a8972b7d8 160000 --- a/modules/DroneCAN/dronecan_dsdlc +++ b/modules/DroneCAN/dronecan_dsdlc @@ -1 +1 @@ -Subproject commit bd9124715cc7cbb9bbe3f3270da0edb020507816 +Subproject commit de47a8972b7d8c59e50c3acfab5754db29798caa diff --git a/modules/DroneCAN/libcanard b/modules/DroneCAN/libcanard index b2da417dfcbc0..601ed35467e0a 160000 --- a/modules/DroneCAN/libcanard +++ b/modules/DroneCAN/libcanard @@ -1 +1 @@ -Subproject commit b2da417dfcbc0a4b617aeaa0d680dc357b233172 +Subproject commit 601ed35467e0ac38819df17cd7c918de19f62d58 diff --git a/modules/DroneCAN/pydronecan b/modules/DroneCAN/pydronecan index 1f494e9a56ac9..08cda37aaf295 160000 --- a/modules/DroneCAN/pydronecan +++ b/modules/DroneCAN/pydronecan @@ -1 +1 @@ -Subproject commit 1f494e9a56ac9930f1e11c2f453789414b10d54e +Subproject commit 08cda37aaf2958657399606653b99ceb5a6beae0 diff --git a/modules/mavlink b/modules/mavlink index f5eeecf85b6d9..20974be6447bf 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit f5eeecf85b6d9efb32a5a60e63135310b6f93c8b +Subproject commit 20974be6447bf66a2e3b0912e2870e9389f6ab5a diff --git a/wscript b/wscript index d129e34639913..5d4b6a786de09 100644 --- a/wscript +++ b/wscript @@ -728,7 +728,7 @@ def generate_tasklist(ctx, do_print=True): elif 'iofirmware' in board: task['targets'] = ['iofirmware', 'bootloader'] else: - if boards.is_board_based(board, boards.sitl): + if boards.is_board_based(board, boards.SITLBoard): task['targets'] = vehicles + ['replay'] elif boards.is_board_based(board, boards.LinuxBoard): task['targets'] = vehicles