diff --git a/.dockerignore b/.dockerignore index d7de4ddfc3cde..771ee99cc1411 100644 --- a/.dockerignore +++ b/.dockerignore @@ -28,3 +28,4 @@ mav.* !Tools/environment_install/install-prereqs-ubuntu.sh !Tools/environment_install/install-prereqs-arch.sh !Tools/completion +autotest_result_*_junit.xml diff --git a/.editorconfig b/.editorconfig index 2c73f926bdb5a..2ceadeedf2bd0 100644 --- a/.editorconfig +++ b/.editorconfig @@ -13,8 +13,9 @@ indent_style = space indent_size = 4 end_of_line = lf charset = utf-8 -trim_trailing_whitespace = false # These are the correct rules for APM coding standards, but fixing up old files causes git spam -insert_final_newline = false +# These are the correct rules for APM coding standards, but fixing up old files causes git spam +trim_trailing_whitespace = false +insert_final_newline = true [*.mk] indent_style = tab diff --git a/.git-blame-ignore-revs b/.git-blame-ignore-revs new file mode 100644 index 0000000000000..80c607e02ac4a --- /dev/null +++ b/.git-blame-ignore-revs @@ -0,0 +1,6 @@ +# This file allows ignoring commits in git blame view on Github. +# For more info, see here: +# https://docs.github.com/en/repositories/working-with-files/using-files/viewing-a-file#ignore-commits-in-the-blame-view + +# Tools: ros2: Run ament_black on all files +85172b56467668bee9fa0e68081027b13bc18c4a diff --git a/.github/workflows/cache_cleanup.yml b/.github/workflows/cache_cleanup.yml index cb3ffb5495c6a..6cb2403f2b622 100644 --- a/.github/workflows/cache_cleanup.yml +++ b/.github/workflows/cache_cleanup.yml @@ -7,7 +7,7 @@ on: jobs: cleanup: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 permissions: # `actions:write` permission is required to delete caches # See also: https://docs.github.com/en/rest/actions/cache?apiVersion=2022-11-28#delete-a-github-actions-cache-for-a-repository-using-a-cache-id @@ -15,7 +15,7 @@ jobs: contents: read steps: - name: Check out code - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Cleanup run: | diff --git a/.github/workflows/colcon.yml b/.github/workflows/colcon.yml new file mode 100644 index 0000000000000..e39af18fc1694 --- /dev/null +++ b/.github/workflows/colcon.yml @@ -0,0 +1,206 @@ +name: colcon build/test + +on: + push: + paths-ignore: + # remove not tests vehicles + - 'AntennaTracker/**' + - 'ArduSub/**' + - 'Blimp/**' + - 'Rover/**' + # remove esp32 HAL + - 'libraries/AP_HAL_ESP32/**' + # remove non SITL directories + - 'Tools/AP_Bootloader/**' + - 'Tools/bootloaders/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/mavproxy_modules/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove autotests stuff + - 'Tools/autotest/**' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' + + pull_request: + paths-ignore: + # remove not tests vehicles + - 'AntennaTracker/**' + - 'ArduSub/**' + - 'Rover/**' + - 'Blimp/**' + # remove esp32 HAL + - 'libraries/AP_HAL_ESP32/**' + # remove non SITL directories + - 'Tools/AP_Bootloader/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CPUInfo/**' + - 'Tools/CodeStyle/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/GIT_Test/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/Vicon/**' + - 'Tools/bootloaders/**' + - 'Tools/completion/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/geotag/**' + - 'Tools/gittools/**' + - 'Tools/mavproxy_modules/**' + - 'Tools/simulink/**' + - 'Tools/vagrant/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove autotests stuff + - 'Tools/autotest/**' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' + + workflow_dispatch: + +concurrency: + group: ci-${{github.workflow}}-${{ github.ref }} + cancel-in-progress: true + +jobs: + build-test: + runs-on: ubuntu-22.04 + container: + image: ardupilot/ardupilot-dev-ros:latest + strategy: + fail-fast: false # don't cancel if a job from the matrix fails + steps: + # git checkout the PR + - uses: actions/checkout@v4 + with: + submodules: 'recursive' + path: src/ardupilot + # 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@v3 + with: + path: ~/.ccache + key: ${{github.workflow}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} + restore-keys: ${{github.workflow}}-ccache- # restore ccache from either previous build on this branch or on master + - name: setup ccache + run: | + . src/ardupilot/.github/workflows/ccache.env + # https://ardupilot.org/dev/docs/ros2.html#installation-ubuntu + - name: Pull in repos with vcs + run: | + cd src + wget --progress=dot:giga https://raw.githubusercontent.com/ArduPilot/ardupilot/master/Tools/ros2/ros2.repos + vcs import --recursive --debug --shallow --skip-existing < ros2.repos + - name: Install rosdep dependencies + shell: 'bash' + run: | + apt update + rosdep update + # Workaround for flake8 attribute error + # https://github.com/ArduPilot/ardupilot/pull/24277#issuecomment-1632833433 + python -m pip install flake8==3.7.9 + source /opt/ros/humble/setup.bash + rosdep install --from-paths src --ignore-src --default-yes + - name: Install MAVProxy + run: | + # Install this specific version just to prevent rolling updates. + # These are the latest available + python3 -m pip install MAVProxy==1.8.67 + - name: Install local pymavlink + working-directory: ./src/ardupilot/modules/mavlink/pymavlink + run: | + pip install . -U --user + - name: Build with colcon + shell: 'bash' + run: | + source /opt/ros/humble/setup.bash + colcon build --packages-up-to ardupilot_dds_tests --cmake-args -DBUILD_TESTING=ON + - name: Test with colcon + shell: 'bash' + run: | + source install/setup.bash + colcon test --packages-select ardupilot_dds_tests --event-handlers=console_cohesion+ + - name: Report colcon test results + run: | + colcon test-result --all --verbose + diff --git a/.github/workflows/cygwin_build.yml b/.github/workflows/cygwin_build.yml index 4157e219af2d6..f836b9c09a923 100644 --- a/.github/workflows/cygwin_build.yml +++ b/.github/workflows/cygwin_build.yml @@ -1,6 +1,140 @@ name: Cygwin Build -on: [push, pull_request, workflow_dispatch] +on: + push: + paths-ignore: + # remove other vehicles + - 'AntennaTracker/**' + - 'Blimp/**' + # remove non SITL HAL + - 'libraries/AP_HAL_ChibiOS/**' + - 'libraries/AP_HAL_ESP32/**' + # remove non SITL directories + - 'Tools/AP_Bootloader/**' + - 'Tools/AP_Periph/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CPUInfo/**' + - 'Tools/CodeStyle/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/GIT_Test/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/Vicon/**' + - 'Tools/bootloaders/**' + - 'Tools/completion/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/geotag/**' + - 'Tools/gittools/**' + - 'Tools/mavproxy_modules/**' + - 'Tools/simulink/**' + - 'Tools/vagrant/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove autotest + - 'Tools/autotest/**' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' + + pull_request: + paths-ignore: + # remove other vehicles + - 'AntennaTracker/**' + - 'Blimp/**' + # remove non SITL HAL + - 'libraries/AP_HAL_ChibiOS/**' + - 'libraries/AP_HAL_ESP32/**' + # remove non SITL directories + - 'Tools/AP_Bootloader/**' + - 'Tools/AP_Periph/**' + - 'Tools/bootloaders/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/mavproxy_modules/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove autotest + - 'Tools/autotest/**' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' + + workflow_dispatch: + concurrency: group: ci-${{github.workflow}}-${{ github.ref }} cancel-in-progress: true @@ -10,7 +144,7 @@ jobs: runs-on: 'windows-latest' steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' - name: Prepare ccache timestamp @@ -55,7 +189,7 @@ jobs: shell: C:\cygwin\bin\bash.exe -eo pipefail '{0}' run: >- ln -sf /usr/bin/python3.7 /usr/bin/python && ln -sf /usr/bin/pip3.7 /usr/bin/pip && - python -m pip install --progress-bar off empy pexpect && + python -m pip install --progress-bar off empy==3.3.4 pexpect && python -m pip install --progress-bar off dronecan --upgrade && cp /usr/bin/ccache /usr/local/bin/ && cd /usr/local/bin && ln -s ccache /usr/local/bin/gcc && diff --git a/.github/workflows/esp32_build.yml b/.github/workflows/esp32_build.yml index f24d8190d5608..47ada972560a4 100644 --- a/.github/workflows/esp32_build.yml +++ b/.github/workflows/esp32_build.yml @@ -1,13 +1,151 @@ name: ESP32 Build -on: [push, pull_request, workflow_dispatch] +on: + push: + paths-ignore: + # remove non copter and plane vehicles + - 'AntennaTracker/**' + - 'ArduSub/**' + - 'Blimp/**' + - 'Rover/**' + # remove non esp32 HAL + - 'libraries/AP_HAL_ChibiOS/**' + - 'libraries/AP_HAL_SITL/**' + # remove non SITL directories + - 'Tools/AP_Bootloader/**' + - 'Tools/AP_Periph/**' + - 'Tools/bootloaders/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/mavproxy_modules/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove autotest + - 'Tools/autotest/**' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' + + pull_request: + paths-ignore: + # remove non copter and plane vehicles + - 'AntennaTracker/**' + - 'ArduSub/**' + - 'Blimp/**' + - 'Rover/**' + # remove non esp32 HAL + - 'libraries/AP_HAL_ChibiOS/**' + - 'libraries/AP_HAL_SITL/**' + # remove non SITL directories + - 'Tools/AP_Bootloader/**' + - 'Tools/AP_Periph/**' + - 'Tools/bootloaders/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/mavproxy_modules/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove autotest + - 'Tools/autotest/**' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' + + workflow_dispatch: + concurrency: group: ci-${{github.workflow}}-${{ github.ref }} cancel-in-progress: true jobs: build: - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: @@ -17,7 +155,7 @@ jobs: gcc: [10] steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' - name: Install Prerequisites @@ -62,7 +200,7 @@ jobs: ./install.sh source ./export.sh cd ../.. - python -m pip install --progress-bar off future lxml pymavlink MAVProxy pexpect flake8 geocoder empy dronecan + python -m pip install --progress-bar off future lxml pymavlink MAVProxy pexpect flake8 geocoder empy==3.3.4 dronecan which cmake ./waf configure --board ${{matrix.config}} ./waf plane diff --git a/.github/workflows/macos_build.yml b/.github/workflows/macos_build.yml index e9e54b8d9b819..7dba405b0300e 100644 --- a/.github/workflows/macos_build.yml +++ b/.github/workflows/macos_build.yml @@ -1,6 +1,140 @@ name: Macos Build -on: [push, pull_request, workflow_dispatch] +on: + push: + paths-ignore: + # remove other env install scripts + - 'Tools/environment_install/APM_install.sh' + - 'Tools/environment_install/install-ROS-ubuntu.sh' + - 'Tools/environment_install/install-prereqs-arch.sh' + - 'Tools/environment_install/install-prereqs-ubuntu.sh' + - 'Tools/environment_install/install-prereqs-windows-andAPMSource.ps1' + - 'Tools/environment_install/install-prereqs-windows.ps1' + # remove non esp32 HAL + - 'libraries/AP_HAL_ESP32/**' + # remove non SITL non stm32 directories + - 'Tools/AP_Bootloader/**' + - 'Tools/AP_Periph/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/mavproxy_modules/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove autotests stuff + - 'Tools/autotest/**' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' + + pull_request: + paths-ignore: + # remove other env install scripts + - 'Tools/environment_install/APM_install.sh' + - 'Tools/environment_install/install-ROS-ubuntu.sh' + - 'Tools/environment_install/install-prereqs-arch.sh' + - 'Tools/environment_install/install-prereqs-ubuntu.sh' + - 'Tools/environment_install/install-prereqs-windows-andAPMSource.ps1' + - 'Tools/environment_install/install-prereqs-windows.ps1' + # remove non esp32 HAL + - 'libraries/AP_HAL_ESP32/**' + # remove non SITL non stm32 directories + - 'Tools/AP_Bootloader/**' + - 'Tools/AP_Periph/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/mavproxy_modules/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove autotests stuff + - 'Tools/autotest/**' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' + + workflow_dispatch: + concurrency: group: ci-${{github.workflow}}-${{ github.ref }} cancel-in-progress: true @@ -17,7 +151,7 @@ jobs: ] steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' - name: Install Prerequisites diff --git a/.github/workflows/test_ccache.yml b/.github/workflows/test_ccache.yml index 3f3ba047dfcf4..c00521bc7b9df 100644 --- a/.github/workflows/test_ccache.yml +++ b/.github/workflows/test_ccache.yml @@ -1,17 +1,132 @@ name: test ccache -on: [push, pull_request, workflow_dispatch] -# paths: -# - "*" -# - "!README.md" <-- don't rebuild on doc change +on: + push: + paths-ignore: + # remove other vehicles + - 'AntennaTracker/**' + - 'ArduPlane/**' + - 'ArduSub/**' + - 'Blimp/**' + - 'Rover/**' + # remove non chibios HAL + - 'libraries/AP_HAL_Linux/**' + - 'libraries/AP_HAL_ESP32/**' + - 'libraries/AP_HAL_SITL/**' + # remove non stm directories + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/mavproxy_modules/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Remove autotests stuff + - 'Tools/autotest/**' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' + + pull_request: + paths-ignore: + # remove other vehicles + - 'AntennaTracker/**' + - 'ArduPlane/**' + - 'ArduSub/**' + - 'Blimp/**' + - 'Rover/**' + # remove non chibios HAL + - 'libraries/AP_HAL_Linux/**' + - 'libraries/AP_HAL_ESP32/**' + - 'libraries/AP_HAL_SITL/**' + # remove non stm directories + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/mavproxy_modules/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Remove autotests stuff + - 'Tools/autotest/**' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' + + workflow_dispatch: + concurrency: group: ci-${{github.workflow}}-${{ github.ref }} cancel-in-progress: true jobs: build: - runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest + 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: @@ -21,7 +136,7 @@ jobs: gcc: [10] steps: # git checkout the PR - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' - name: ccache test @@ -29,5 +144,5 @@ jobs: run: | PATH="/usr/lib/ccache:/opt/gcc-arm-none-eabi-${{matrix.gcc}}/bin:$PATH" Tools/scripts/build_tests/test_ccache.py --boards MatekF405,MatekF405-bdshot --min-cache-pct=75 - Tools/scripts/build_tests/test_ccache.py --boards CubeOrange,Durandal --min-cache-pct=75 + Tools/scripts/build_tests/test_ccache.py --boards Durandal,Pixhawk6X --min-cache-pct=70 diff --git a/.github/workflows/test_chibios.yml b/.github/workflows/test_chibios.yml index bf0a4f78dc952..3fe7c730eac02 100644 --- a/.github/workflows/test_chibios.yml +++ b/.github/workflows/test_chibios.yml @@ -1,18 +1,140 @@ name: test chibios -on: [push, pull_request, workflow_dispatch] +on: + push: + paths-ignore: + # remove non chibios HAL + - 'libraries/AP_HAL_Linux/**' + - 'libraries/AP_HAL_ESP32/**' + - 'libraries/AP_HAL_SITL/**' + # remove non stm directories + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/mavproxy_modules/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Remove vehicles autotest we need support of test_build_option.py in the Tools/autotest directory + - 'Tools/autotest/antennatracker.py' + - 'Tools/autotest/arduplane.py' + - 'Tools/autotest/ardusub.py' + - 'Tools/autotest/balancebot.py' + - 'Tools/autotest/location.txt' + - 'Tools/autotest/quadplane.py' + - 'Tools/autotest/rover.py' + - 'Tools/autotest/sailboat.py' + - 'Tools/autotest/swarminit.txt' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' + + pull_request: + paths-ignore: + # remove non chibios HAL + - 'libraries/AP_HAL_Linux/**' + - 'libraries/AP_HAL_ESP32/**' + - 'libraries/AP_HAL_SITL/**' + # remove non stm directories + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/mavproxy_modules/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Remove vehicles autotest we need support of test_build_option.py in the Tools/autotest directory + - 'Tools/autotest/antennatracker.py' + - 'Tools/autotest/arduplane.py' + - 'Tools/autotest/ardusub.py' + - 'Tools/autotest/autotest.py' + - 'Tools/autotest/balancebot.py' + - 'Tools/autotest/common.py' + - 'Tools/autotest/examples.py' + - 'Tools/autotest/quadplane.py' + - 'Tools/autotest/rover.py' + - 'Tools/autotest/sailboat.py' + - 'Tools/autotest/**.txt' + - 'Tools/autotest/logger_metadata/**' + - 'Tools/autotest/param_metadata/**' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' + + workflow_dispatch: -# paths: -# - "*" -# - "!README.md" <-- don't rebuild on doc change concurrency: group: ci-${{github.workflow}}-${{ github.ref }} cancel-in-progress: true jobs: build: - runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest + 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: @@ -30,39 +152,23 @@ jobs: revo-mini, MatekF405-Wing, CubeOrange-ODID, + CubeRedPrimary-bootloader, configure-all, build-options-defaults-test, signing ] toolchain: [ - chibios, # GCC-6 + chibios, #chibios-clang, ] - gcc: [6, 10] + gcc: [10] exclude: - gcc: 10 toolchain: chibios-clang - - gcc: 6 - config: fmuv2-plane - - gcc: 6 - config: revo-mini - - gcc: 6 - config: MatekF405-Wing - - gcc: 6 - config: periph-build - - gcc: 6 - config: CubeOrange-ODID - - gcc: 6 - config: signing - - gcc: 6 - config: fmuv3-bootloader - include: - - config: stm32h7 - toolchain: chibios-py2 steps: # git checkout the PR - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' # Put ccache into github cache for faster build diff --git a/.github/workflows/test_coverage.yml b/.github/workflows/test_coverage.yml index 8feaabfe2da50..dda488770f776 100644 --- a/.github/workflows/test_coverage.yml +++ b/.github/workflows/test_coverage.yml @@ -13,9 +13,9 @@ concurrency: jobs: build: - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 container: - image: ardupilot/ardupilot-dev-${{ matrix.type }}:latest + image: ardupilot/ardupilot-dev-${{ matrix.type }}:v0.1.3 options: --privileged strategy: fail-fast: false # don't cancel if a job from the matrix fails @@ -38,7 +38,7 @@ jobs: type: coverage steps: # git checkout the PR - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -56,14 +56,6 @@ jobs: - name: setup ccache run: | . .github/workflows/ccache.env - - name: Configure CAN - if: ${{ matrix.config == 'sitltest-can'}} - run: | - sudo apt update - sudo apt -y linux-modules-extra-$(uname -r) - sudo modprobe vcan - sudo ip link add dev vcan0 type vcan - sudo ip link set up vcan0 - name: test ${{matrix.config}} ${{ matrix.toolchain }} env: CI_BUILD_TARGET: ${{matrix.config}} @@ -99,7 +91,7 @@ jobs: finish: if: always() needs: build - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Coveralls Finished uses: coverallsapp/github-action@master diff --git a/.github/workflows/test_dds.yml b/.github/workflows/test_dds.yml new file mode 100644 index 0000000000000..c8e63c33eda02 --- /dev/null +++ b/.github/workflows/test_dds.yml @@ -0,0 +1,188 @@ +name: test dds + +on: + push: + paths-ignore: + # remove not tests vehicles + - 'AntennaTracker/**' + - 'ArduSub/**' + - 'Blimp/**' + - 'Rover/**' + # remove esp32 HAL + - 'libraries/AP_HAL_ESP32/**' + # remove non SITL directories + - 'Tools/AP_Bootloader/**' + - 'Tools/bootloaders/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/mavproxy_modules/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove autotests stuff + - 'Tools/autotest/**' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' + + pull_request: + paths-ignore: + # remove not tests vehicles + - 'AntennaTracker/**' + - 'ArduSub/**' + - 'Rover/**' + - 'Blimp/**' + # remove esp32 HAL + - 'libraries/AP_HAL_ESP32/**' + # remove non SITL directories + - 'Tools/AP_Bootloader/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CPUInfo/**' + - 'Tools/CodeStyle/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/GIT_Test/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/Vicon/**' + - 'Tools/bootloaders/**' + - 'Tools/completion/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/geotag/**' + - 'Tools/gittools/**' + - 'Tools/mavproxy_modules/**' + - 'Tools/simulink/**' + - 'Tools/vagrant/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove autotests stuff + - 'Tools/autotest/**' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' + + workflow_dispatch: + +concurrency: + group: ci-${{github.workflow}}-${{ github.ref }} + cancel-in-progress: true + +jobs: + build: + runs-on: ubuntu-22.04 + container: + image: ardupilot/ardupilot-dev-ros:latest + options: --user 1001 + strategy: + fail-fast: false # don't cancel if a job from the matrix fails + matrix: + config: [ + sitl, + stm32h7 + ] + steps: + # git checkout the PR + - uses: actions/checkout@v4 + with: + 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@v3 + with: + path: ~/.ccache + key: ${{github.workflow}}-ccache-${{ matrix.config }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} + restore-keys: ${{github.workflow}}-ccache-${{ matrix.config }}- # restore ccache from either previous build on this branch or on master + - name: setup ccache + run: | + . .github/workflows/ccache.env + - name: test ${{matrix.config}} + env: + CI_BUILD_TARGET: dds-${{matrix.config}} + shell: 'script -q -e -c "bash {0}"' + run: | + git config --global --add safe.directory ${GITHUB_WORKSPACE} + PATH="/github/home/.local/bin:$PATH" + Tools/scripts/build_ci.sh + + - name: Archive buildlog artifacts + uses: actions/upload-artifact@v3 + if: failure() + with: + name: fail-${{matrix.config}} + path: /tmp/buildlogs + retention-days: 14 diff --git a/.github/workflows/test_environment.yml b/.github/workflows/test_environment.yml index 3d029782a6fb7..f724c1db964d4 100644 --- a/.github/workflows/test_environment.yml +++ b/.github/workflows/test_environment.yml @@ -3,6 +3,15 @@ on: schedule: - cron: '0 0 * * 6' # every saturday at midnight workflow_dispatch: + push: + paths: + - '.github/workflows/test_environment.yml' + - 'Tools/environment_install/**' + + pull_request: + paths: + - '.github/workflows/test_environment.yml' + - 'Tools/environment_install/**' concurrency: @@ -11,7 +20,7 @@ concurrency: jobs: build: - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 container: image: ${{matrix.os}}:${{matrix.name}} options: --privileged @@ -19,12 +28,14 @@ jobs: fail-fast: false # don't cancel if a job from the matrix fails matrix: include: - - os: ubuntu - name: bionic - os: ubuntu name: focal - os: ubuntu - name: hirsute + name: jammy + - os: ubuntu + name: lunar + - os: ubuntu + name: mantic - os: archlinux name: latest - os: debian @@ -57,16 +68,16 @@ jobs: software-properties-common ;; *"archlinux"*) - pacman -Sy --noconfirm --needed git sudo + pacman -Syu --noconfirm --needed git sudo ;; esac # git checkout the PR - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' - name: test install environment ${{matrix.os}}.${{matrix.name}} - timeout-minutes: 30 + timeout-minutes: 45 env: DISABLE_MAVNATIVE: True DEBIAN_FRONTEND: noninteractive @@ -104,6 +115,7 @@ jobs: run: | git config --global --add safe.directory ${GITHUB_WORKSPACE} source ~/.bashrc + source $HOME/venv-ardupilot/bin/activate || true git config --global --add safe.directory /__w/ardupilot/ardupilot ./waf configure ./waf rover @@ -117,6 +129,7 @@ jobs: run: | git config --global --add safe.directory ${GITHUB_WORKSPACE} source ~/.bashrc + source $HOME/venv-ardupilot/bin/activate || true case ${{matrix.os}} in *"archlinux"*) export PATH=/opt/gcc-arm-none-eabi-10-2020-q4-major/bin:$PATH diff --git a/.github/workflows/test_linux_sbc.yml b/.github/workflows/test_linux_sbc.yml index 3db0df687afc1..d55a75b3f42c1 100644 --- a/.github/workflows/test_linux_sbc.yml +++ b/.github/workflows/test_linux_sbc.yml @@ -1,17 +1,141 @@ name: test Linux SBC -on: [push, pull_request, workflow_dispatch] -# paths: -# - "*" -# - "!README.md" <-- don't rebuild on doc change +on: + push: + paths-ignore: + # remove non LINUX HAL + - 'libraries/AP_HAL_ChibiOS/**' + - 'libraries/AP_HAL_ESP32/**' + - 'libraries/AP_HAL_SITL/**' + # remove non SITL directories + - 'Tools/AP_Bootloader/**' + - 'Tools/AP_Periph/**' + - 'Tools/bootloaders/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/mavproxy_modules/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard file from Tools/scripts as not used + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove autotests stuff + - 'Tools/autotest/**' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' + + pull_request: + paths-ignore: + # remove non LINUX HAL + - 'libraries/AP_HAL_ChibiOS/**' + - 'libraries/AP_HAL_ESP32/**' + - 'libraries/AP_HAL_SITL/**' + # remove non SITL directories + - 'Tools/AP_Bootloader/**' + - 'Tools/AP_Periph/**' + - 'Tools/bootloaders/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/mavproxy_modules/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove autotests stuff + - 'Tools/autotest/**' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' + + workflow_dispatch: + concurrency: group: ci-${{github.workflow}}-${{ github.ref }} cancel-in-progress: true jobs: build: - runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest + 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: @@ -43,7 +167,7 @@ jobs: steps: # git checkout the PR - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' # Put ccache into github cache for faster build diff --git a/.github/workflows/test_replay.yml b/.github/workflows/test_replay.yml index fb3d3b2d761e2..86cd8ba3b307b 100644 --- a/.github/workflows/test_replay.yml +++ b/.github/workflows/test_replay.yml @@ -1,35 +1,154 @@ name: test replay -on: +on: push: - paths-ignore: # ignore vehicle only changes + paths-ignore: + # remove other vehicles - 'AntennaTracker/**' + - 'ArduCopter/**' - 'ArduPlane/**' - 'ArduSub/**' - - 'Rover/**' - 'Blimp/**' - - 'ArduCopter/**' + - 'Rover/**' + # remove non SITL HAL + - 'libraries/AP_HAL_ChibiOS/**' + - 'libraries/AP_HAL_ESP32/**' + # remove non SITL directories + - 'Tools/AP_Bootloader/**' + - 'Tools/AP_Periph/**' + - 'Tools/bootloaders/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/mavproxy_modules/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove autotests stuff + - 'Tools/autotest/**' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' pull_request: - paths-ignore: # ignore vehicle only changes + paths-ignore: + # remove other vehicles - 'AntennaTracker/**' + - 'ArduCopter/**' - 'ArduPlane/**' - 'ArduSub/**' - - 'Rover/**' - 'Blimp/**' - - 'ArduCopter/**' + - 'Rover/**' + # remove non SITL HAL + - 'libraries/AP_HAL_ChibiOS/**' + - 'libraries/AP_HAL_ESP32/**' + # remove non SITL directories + - 'Tools/AP_Bootloader/**' + - 'Tools/AP_Periph/**' + - 'Tools/bootloaders/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/mavproxy_modules/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove autotests stuff + - 'Tools/autotest/**' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' workflow_dispatch: - concurrency: group: ci-${{github.workflow}}-${{ github.ref }} cancel-in-progress: true jobs: build: - runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest + 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: @@ -42,7 +161,7 @@ jobs: ] steps: # git checkout the PR - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' # Put ccache into github cache for faster build diff --git a/.github/workflows/test_scripting.yml b/.github/workflows/test_scripting.yml index 67074fe019bde..fab7a226deba1 100644 --- a/.github/workflows/test_scripting.yml +++ b/.github/workflows/test_scripting.yml @@ -21,11 +21,11 @@ concurrency: jobs: test-scripting: - runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-base:latest + runs-on: ubuntu-22.04 + container: ardupilot/ardupilot-dev-base:v0.1.3 steps: # git checkout the PR - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' diff --git a/.github/workflows/test_scripts.yml b/.github/workflows/test_scripts.yml index b26699e8b77bc..b2e0c7bc6d139 100644 --- a/.github/workflows/test_scripts.yml +++ b/.github/workflows/test_scripts.yml @@ -8,8 +8,8 @@ concurrency: jobs: build: - runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-base:latest + runs-on: ubuntu-22.04 + container: ardupilot/ardupilot-dev-base:v0.1.3 strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: @@ -17,11 +17,12 @@ jobs: check_autotest_options, param_parse, python-cleanliness, + astyle-cleanliness, validate_board_list, ] steps: # git checkout the PR - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' - name: test ${{matrix.config}} diff --git a/.github/workflows/test_sitl_blimp.yml b/.github/workflows/test_sitl_blimp.yml new file mode 100644 index 0000000000000..c43546273c50c --- /dev/null +++ b/.github/workflows/test_sitl_blimp.yml @@ -0,0 +1,270 @@ +name: test blimp + +on: + push: + paths-ignore: + # remove other vehicles + - 'AntennaTracker/**' + - 'ArduPlane/**' + - 'ArduSub/**' + - 'ArduCopter/**' + - 'Rover/**' + # remove non SITL HAL + - 'libraries/AP_HAL_ChibiOS/**' + - 'libraries/AP_HAL_ESP32/**' + # remove non SITL directories + - 'Tools/AP_Bootloader/**' + - 'Tools/AP_Periph/**' + - 'Tools/bootloaders/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove other vehicles autotest + - 'Tools/autotest/antennatracker.py' + - 'Tools/autotest/arduplane.py' + - 'Tools/autotest/arducopter.py' + - 'Tools/autotest/ardusub.py' + - 'Tools/autotest/balancebot.py' + - 'Tools/autotest/helicopter.py' + - 'Tools/autotest/location.txt' + - 'Tools/autotest/quadplane.py' + - 'Tools/autotest/rover.py' + - 'Tools/autotest/sailboat.py' + - 'Tools/autotest/swarminit.txt' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + + + pull_request: + paths-ignore: + # remove other vehicles + - 'AntennaTracker/**' + - 'ArduPlane/**' + - 'ArduSub/**' + - 'ArduCopter/**' + - 'Rover/**' + # remove non SITL HAL + - 'libraries/AP_HAL_ChibiOS/**' + - 'libraries/AP_HAL_ESP32/**' + # remove non SITL directories + - 'Tools/AP_Bootloader/**' + - 'Tools/AP_Periph/**' + - 'Tools/bootloaders/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove other vehicles autotest + - 'Tools/autotest/antennatracker.py' + - 'Tools/autotest/arduplane.py' + - 'Tools/autotest/arducopter.py' + - 'Tools/autotest/ardusub.py' + - 'Tools/autotest/balancebot.py' + - 'Tools/autotest/helicopter.py' + - 'Tools/autotest/location.txt' + - 'Tools/autotest/quadplane.py' + - 'Tools/autotest/rover.py' + - 'Tools/autotest/sailboat.py' + - 'Tools/autotest/swarminit.txt' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + + workflow_dispatch: + +concurrency: + group: ci-${{github.workflow}}-${{ github.ref }} + cancel-in-progress: true + +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: [ + base, # GCC + clang, + ] + steps: + # git checkout the PR + - uses: actions/checkout@v4 + with: + 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@v3 + with: + path: ~/.ccache + key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} + restore-keys: ${{github.workflow}}-ccache-${{ matrix.toolchain }}- # restore ccache from either previous build on this branch or on master + - name: setup ccache + run: | + . .github/workflows/ccache.env + - name: build blimp ${{ matrix.toolchain }} + shell: bash + run: | + git config --global --add safe.directory ${GITHUB_WORKSPACE} + if [[ ${{ matrix.toolchain }} == "clang" ]]; then + export CC=clang + export CXX=clang++ + fi + PATH="/github/home/.local/bin:$PATH" + ./waf configure --board sitl + ./waf build --target bin/blimp + ccache -s + ccache -z + + autotest: + needs: build # don't try to launch the tests matrix if it doesn't build first, profit from caching for fast build + runs-on: ubuntu-22.04 + container: + image: ardupilot/ardupilot-dev-base:v0.1.3 + options: --privileged --cap-add=SYS_PTRACE --security-opt apparmor=unconfined --security-opt seccomp=unconfined + strategy: + fail-fast: false # don't cancel if a job from the matrix fails + matrix: + config: [ + sitltest-blimp, + ] + + steps: + # git checkout the PR + - uses: actions/checkout@v4 + with: + 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/restore@v3 + with: + path: ~/.ccache + key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}} + restore-keys: ${{github.workflow}}-ccache-base- # restore ccache from either previous build on this branch or on master + - name: setup ccache + run: | + . .github/workflows/ccache.env + - name: test ${{matrix.config}} + env: + CI_BUILD_TARGET: ${{matrix.config}} + shell: bash + run: | + git config --global --add safe.directory ${GITHUB_WORKSPACE} + PATH="/github/home/.local/bin:$PATH" + Tools/scripts/build_ci.sh + + - name: Archive buildlog artifacts + uses: actions/upload-artifact@v3 + if: failure() + with: + name: fail-${{matrix.config}} + path: | + /tmp/buildlogs + /__w/ardupilot/ardupilot/logs + /__w/ardupilot/ardupilot/ap-*.core + /__w/ardupilot/ardupilot/core.* + /__w/ardupilot/ardupilot/dumpstack.sh_* + /__w/ardupilot/ardupilot/dumpcore.sh_* + retention-days: 14 + + - name: Archive .bin artifacts + uses: actions/upload-artifact@v3 + with: + name: BIN-${{matrix.config}} + path: /__w/ardupilot/ardupilot/logs + retention-days: 7 + diff --git a/.github/workflows/test_sitl_copter.yml b/.github/workflows/test_sitl_copter.yml index 0d76aed9048b3..0a379980978e6 100644 --- a/.github/workflows/test_sitl_copter.yml +++ b/.github/workflows/test_sitl_copter.yml @@ -2,20 +2,155 @@ name: test copter on: push: - paths-ignore: # ignore vehicle only changes not for copter + paths-ignore: + # remove other vehicles - 'AntennaTracker/**' - 'ArduPlane/**' - 'ArduSub/**' - - 'Rover/**' - 'Blimp/**' + - 'Rover/**' + # remove non SITL HAL + - 'libraries/AP_HAL_ChibiOS/**' + - 'libraries/AP_HAL_ESP32/**' + # remove non SITL directories + - 'Tools/AP_Bootloader/**' + - 'Tools/AP_Periph/**' + - 'Tools/bootloaders/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove other vehicles autotest + - 'Tools/autotest/antennatracker.py' + - 'Tools/autotest/arduplane.py' + - 'Tools/autotest/ardusub.py' + - 'Tools/autotest/balancebot.py' + - 'Tools/autotest/location.txt' + - 'Tools/autotest/quadplane.py' + - 'Tools/autotest/rover.py' + - 'Tools/autotest/sailboat.py' + - 'Tools/autotest/swarminit.txt' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' + pull_request: - paths-ignore: # ignore vehicle only changes not for copter + paths-ignore: + # remove other vehicles - 'AntennaTracker/**' - 'ArduPlane/**' - 'ArduSub/**' - - 'Rover/**' - 'Blimp/**' + - 'Rover/**' + # remove non SITL HAL + - 'libraries/AP_HAL_ChibiOS/**' + - 'libraries/AP_HAL_ESP32/**' + # remove non SITL directories + - 'Tools/AP_Bootloader/**' + - 'Tools/AP_Periph/**' + - 'Tools/bootloaders/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove other vehicles autotest + - 'Tools/autotest/antennatracker.py' + - 'Tools/autotest/arduplane.py' + - 'Tools/autotest/ardusub.py' + - 'Tools/autotest/balancebot.py' + - 'Tools/autotest/location.txt' + - 'Tools/autotest/quadplane.py' + - 'Tools/autotest/rover.py' + - 'Tools/autotest/sailboat.py' + - 'Tools/autotest/swarminit.txt' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' workflow_dispatch: @@ -25,8 +160,8 @@ concurrency: jobs: build: - runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest + 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: @@ -36,7 +171,7 @@ jobs: ] steps: # git checkout the PR - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -70,9 +205,9 @@ jobs: autotest: needs: build # don't try to launch the tests matrix if it doesn't build first, profit from caching for fast build - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 container: - image: ardupilot/ardupilot-dev-base:latest + image: ardupilot/ardupilot-dev-base:v0.1.3 options: --privileged --cap-add=SYS_PTRACE --security-opt apparmor=unconfined --security-opt seccomp=unconfined strategy: fail-fast: false # don't cancel if a job from the matrix fails @@ -89,7 +224,7 @@ jobs: steps: # git checkout the PR - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -138,13 +273,13 @@ jobs: retention-days: 7 build-gcc-heli: - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 container: - image: ardupilot/ardupilot-dev-base:latest + image: ardupilot/ardupilot-dev-base:v0.1.3 options: --privileged --cap-add=SYS_PTRACE --security-opt apparmor=unconfined --security-opt seccomp=unconfined steps: # git checkout the PR - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -174,8 +309,8 @@ jobs: autotest-heli: needs: build-gcc-heli # don't try to launch the tests matrix if it doesn't build first, profit from caching for fast build - runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-base:latest + runs-on: ubuntu-22.04 + container: ardupilot/ardupilot-dev-base:v0.1.3 strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: @@ -185,7 +320,7 @@ jobs: steps: # git checkout the PR - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' # Put ccache into github cache for faster build diff --git a/.github/workflows/test_sitl_periph.yml b/.github/workflows/test_sitl_periph.yml index 514f814ffa5c5..0f4b5453fbef4 100644 --- a/.github/workflows/test_sitl_periph.yml +++ b/.github/workflows/test_sitl_periph.yml @@ -2,22 +2,154 @@ name: test ap_periph on: push: - paths-ignore: # ignore vehicle only changes + paths-ignore: + # remove other vehicles than copter - 'AntennaTracker/**' - 'ArduPlane/**' - 'ArduSub/**' - - 'Rover/**' - 'Blimp/**' - - 'ArduCopter/**' + - 'Rover/**' + # remove non SITL HAL + - 'libraries/AP_HAL_ChibiOS/**' + - 'libraries/AP_HAL_ESP32/**' + # remove non SITL directories + - 'Tools/bootloaders/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove other vehicles autotest keep only coptertest + - 'Tools/autotest/antennatracker.py' + - 'Tools/autotest/arduplane.py' + - 'Tools/autotest/ardusub.py' + - 'Tools/autotest/balancebot.py' + - 'Tools/autotest/helicopter.py' + - 'Tools/autotest/location.txt' + - 'Tools/autotest/quadplane.py' + - 'Tools/autotest/rover.py' + - 'Tools/autotest/sailboat.py' + - 'Tools/autotest/swarminit.txt' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' pull_request: - paths-ignore: # ignore vehicle only changes + paths-ignore: + # remove other vehicles than copter - 'AntennaTracker/**' - 'ArduPlane/**' - 'ArduSub/**' - - 'Rover/**' - 'Blimp/**' - - 'ArduCopter/**' + - 'Rover/**' + # remove non SITL HAL + - 'libraries/AP_HAL_ChibiOS/**' + - 'libraries/AP_HAL_ESP32/**' + # remove non SITL directories + - 'Tools/bootloaders/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove other vehicles autotest keep only coptertest + - 'Tools/autotest/antennatracker.py' + - 'Tools/autotest/arduplane.py' + - 'Tools/autotest/ardusub.py' + - 'Tools/autotest/balancebot.py' + - 'Tools/autotest/helicopter.py' + - 'Tools/autotest/location.txt' + - 'Tools/autotest/quadplane.py' + - 'Tools/autotest/rover.py' + - 'Tools/autotest/sailboat.py' + - 'Tools/autotest/swarminit.txt' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' workflow_dispatch: @@ -27,11 +159,11 @@ concurrency: jobs: build-gcc-ap_periph: - runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-periph:latest + runs-on: ubuntu-22.04 + container: ardupilot/ardupilot-dev-periph:v0.1.3 steps: # git checkout the PR - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -67,9 +199,9 @@ jobs: autotest-can: needs: build-gcc-ap_periph # don't try to launch the tests matrix if it doesn't build first, profit from caching for fast build - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 container: - image: ardupilot/ardupilot-dev-periph:latest + image: ardupilot/ardupilot-dev-periph:v0.1.3 options: --privileged strategy: fail-fast: false # don't cancel if a job from the matrix fails @@ -80,7 +212,7 @@ jobs: steps: # git checkout the PR - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -98,15 +230,6 @@ jobs: - name: setup ccache run: | . .github/workflows/ccache.env - - name: setup can-utils - run: | - kernel_ver=`uname -r` - if [ "$kernel_ver" = "5.4.0-1032-azure" ] || [ "$kernel_ver" = "5.11.4-051104-generic" ]; then echo "Unsupported Kernel $kernel_ver" && exit 0; fi; - sudo apt-get update - sudo apt-get -y install can-utils iproute2 linux-modules-extra-$(uname -r) - sudo modprobe vcan - sudo ip link add dev vcan0 type vcan - sudo ip link set up vcan0 - name: test ${{matrix.config}} env: CI_BUILD_TARGET: ${{matrix.config}} diff --git a/.github/workflows/test_sitl_plane.yml b/.github/workflows/test_sitl_plane.yml index 172aa820d0868..18b1f0a801901 100644 --- a/.github/workflows/test_sitl_plane.yml +++ b/.github/workflows/test_sitl_plane.yml @@ -1,21 +1,157 @@ name: test plane -on: +on: push: - paths-ignore: # ignore vehicle only changes not for plane + paths-ignore: + # remove other vehicles - 'AntennaTracker/**' + - 'ArduCopter/**' - 'ArduSub/**' - - 'Rover/**' - 'Blimp/**' - - 'ArduCopter/**' + - 'Rover/**' + # remove non SITL HAL + - 'libraries/AP_HAL_ChibiOS/**' + - 'libraries/AP_HAL_ESP32/**' + # remove non SITL directories + - 'Tools/AP_Bootloader/**' + - 'Tools/AP_Periph/**' + - 'Tools/bootloaders/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove other vehicles autotest + - 'Tools/autotest/antennatracker.py' + - 'Tools/autotest/arducopter.py' + - 'Tools/autotest/ardusub.py' + - 'Tools/autotest/balancebot.py' + - 'Tools/autotest/helicopter.py' + - 'Tools/autotest/location.txt' + - 'Tools/autotest/rover.py' + - 'Tools/autotest/sailboat.py' + - 'Tools/autotest/swarminit.txt' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' pull_request: - paths-ignore: # ignore vehicle only changes not for plane + paths-ignore: + # remove other vehicles - 'AntennaTracker/**' + - 'ArduCopter/**' - 'ArduSub/**' - - 'Rover/**' - 'Blimp/**' - - 'ArduCopter/**' + - 'Rover/**' + # remove non SITL HAL + - 'libraries/AP_HAL_ChibiOS/**' + - 'libraries/AP_HAL_ESP32/**' + # remove non SITL directories + - 'Tools/AP_Bootloader/**' + - 'Tools/AP_Periph/**' + - 'Tools/bootloaders/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove other vehicles autotest + - 'Tools/autotest/antennatracker.py' + - 'Tools/autotest/arducopter.py' + - 'Tools/autotest/ardusub.py' + - 'Tools/autotest/balancebot.py' + - 'Tools/autotest/helicopter.py' + - 'Tools/autotest/location.txt' + - 'Tools/autotest/rover.py' + - 'Tools/autotest/sailboat.py' + - 'Tools/autotest/swarminit.txt' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' workflow_dispatch: @@ -25,8 +161,8 @@ concurrency: jobs: build: - runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest + 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: @@ -36,7 +172,7 @@ jobs: ] steps: # git checkout the PR - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -70,9 +206,9 @@ jobs: autotest: needs: build # don't try to launch the tests matrix if it doesn't build first, profit from caching for fast build - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 container: - image: ardupilot/ardupilot-dev-base:latest + image: ardupilot/ardupilot-dev-base:v0.1.3 options: --privileged --cap-add=SYS_PTRACE --security-opt apparmor=unconfined --security-opt seccomp=unconfined strategy: fail-fast: false # don't cancel if a job from the matrix fails @@ -84,7 +220,7 @@ jobs: steps: # git checkout the PR - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' # Put ccache into github cache for faster build diff --git a/.github/workflows/test_sitl_rover.yml b/.github/workflows/test_sitl_rover.yml index 12f9b0ff2c66f..d17c0d154e16e 100644 --- a/.github/workflows/test_sitl_rover.yml +++ b/.github/workflows/test_sitl_rover.yml @@ -1,21 +1,156 @@ name: test rover -on: +on: push: - paths-ignore: # ignore vehicle only changes not for rover + paths-ignore: + # remove other vehicles - 'AntennaTracker/**' + - 'ArduCopter/**' - 'ArduPlane/**' - 'ArduSub/**' - 'Blimp/**' - - 'ArduCopter/**' + # remove non SITL HAL + - 'libraries/AP_HAL_ChibiOS/**' + - 'libraries/AP_HAL_ESP32/**' + # remove non SITL directories + - 'Tools/AP_Bootloader/**' + - 'Tools/AP_Periph/**' + - 'Tools/bootloaders/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove other vehicles autotest + - 'Tools/autotest/antennatracker.py' + - 'Tools/autotest/arducopter.py' + - 'Tools/autotest/arduplane.py' + - 'Tools/autotest/ardusub.py' + - 'Tools/autotest/helicopter.py' + - 'Tools/autotest/location.txt' + - 'Tools/autotest/quadplane.py' + - 'Tools/autotest/swarminit.txt' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' + pull_request: - paths-ignore: # ignore vehicle only changes not for rover + paths-ignore: + # remove other vehicles - 'AntennaTracker/**' + - 'ArduCopter/**' - 'ArduPlane/**' - 'ArduSub/**' - 'Blimp/**' - - 'ArduCopter/**' + # remove non SITL HAL + - 'libraries/AP_HAL_ChibiOS/**' + - 'libraries/AP_HAL_ESP32/**' + # remove non SITL directories + - 'Tools/AP_Bootloader/**' + - 'Tools/AP_Periph/**' + - 'Tools/bootloaders/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove other vehicles autotest + - 'Tools/autotest/antennatracker.py' + - 'Tools/autotest/arducopter.py' + - 'Tools/autotest/arduplane.py' + - 'Tools/autotest/ardusub.py' + - 'Tools/autotest/helicopter.py' + - 'Tools/autotest/location.txt' + - 'Tools/autotest/quadplane.py' + - 'Tools/autotest/swarminit.txt' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' workflow_dispatch: @@ -25,8 +160,8 @@ concurrency: jobs: build: - runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest + 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: @@ -36,7 +171,7 @@ jobs: ] steps: # git checkout the PR - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -70,9 +205,9 @@ jobs: autotest: needs: build # don't try to launch the tests matrix if it doesn't build first, profit from caching for fast build - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 container: - image: ardupilot/ardupilot-dev-base:latest + image: ardupilot/ardupilot-dev-base:v0.1.3 options: --privileged --cap-add=SYS_PTRACE --security-opt apparmor=unconfined --security-opt seccomp=unconfined strategy: fail-fast: false # don't cancel if a job from the matrix fails @@ -85,7 +220,7 @@ jobs: steps: # git checkout the PR - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' # Put ccache into github cache for faster build diff --git a/.github/workflows/test_sitl_sub.yml b/.github/workflows/test_sitl_sub.yml index 7f7c7f16e08ed..65812b31b79e5 100644 --- a/.github/workflows/test_sitl_sub.yml +++ b/.github/workflows/test_sitl_sub.yml @@ -2,20 +2,158 @@ name: test sub on: push: - paths-ignore: # ignore vehicle only changes not for sub + paths-ignore: + # remove other vehicles - 'AntennaTracker/**' + - 'ArduCopter/**' - 'ArduPlane/**' - - 'Rover/**' - 'Blimp/**' - - 'ArduCopter/**' + - 'Rover/**' + # remove non SITL HAL + - 'libraries/AP_HAL_ChibiOS/**' + - 'libraries/AP_HAL_ESP32/**' + # remove non SITL directories + - 'Tools/AP_Bootloader/**' + - 'Tools/AP_Periph/**' + - 'Tools/bootloaders/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove other vehicles autotest + - 'Tools/autotest/antennatracker.py' + - 'Tools/autotest/arducopter.py' + - 'Tools/autotest/arduplane.py' + - 'Tools/autotest/balancebot.py' + - 'Tools/autotest/helicopter.py' + - 'Tools/autotest/location.txt' + - 'Tools/autotest/quadplane.py' + - 'Tools/autotest/rover.py' + - 'Tools/autotest/sailboat.py' + - 'Tools/autotest/swarminit.txt' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' pull_request: - paths-ignore: # ignore vehicle only changes not for sub + paths-ignore: + # remove other vehicles - 'AntennaTracker/**' + - 'ArduCopter/**' - 'ArduPlane/**' - - 'Rover/**' - 'Blimp/**' - - 'ArduCopter/**' + - 'Rover/**' + # remove non SITL HAL + - 'libraries/AP_HAL_ChibiOS/**' + - 'libraries/AP_HAL_ESP32/**' + # remove non SITL directories + - 'Tools/AP_Bootloader/**' + - 'Tools/AP_Periph/**' + - 'Tools/bootloaders/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove other vehicles autotest + - 'Tools/autotest/antennatracker.py' + - 'Tools/autotest/arducopter.py' + - 'Tools/autotest/arduplane.py' + - 'Tools/autotest/balancebot.py' + - 'Tools/autotest/helicopter.py' + - 'Tools/autotest/location.txt' + - 'Tools/autotest/quadplane.py' + - 'Tools/autotest/rover.py' + - 'Tools/autotest/sailboat.py' + - 'Tools/autotest/swarminit.txt' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' workflow_dispatch: @@ -25,8 +163,8 @@ concurrency: jobs: build: - runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest + 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: @@ -36,7 +174,7 @@ jobs: ] steps: # git checkout the PR - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -70,9 +208,9 @@ jobs: autotest: needs: build # don't try to launch the tests matrix if it doesn't build first, profit from caching for fast build - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 container: - image: ardupilot/ardupilot-dev-base:latest + image: ardupilot/ardupilot-dev-base:v0.1.3 options: --privileged --cap-add=SYS_PTRACE --security-opt apparmor=unconfined --security-opt seccomp=unconfined strategy: fail-fast: false # don't cancel if a job from the matrix fails @@ -83,7 +221,7 @@ jobs: steps: # git checkout the PR - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' # Put ccache into github cache for faster build diff --git a/.github/workflows/test_sitl_tracker.yml b/.github/workflows/test_sitl_tracker.yml index f31761beab52f..e9e20eccfd8ae 100644 --- a/.github/workflows/test_sitl_tracker.yml +++ b/.github/workflows/test_sitl_tracker.yml @@ -1,21 +1,159 @@ name: test tracker -on: +on: push: - paths-ignore: # ignore vehicle only changes + paths-ignore: + # remove other vehicles + - 'ArduCopter/**' - 'ArduPlane/**' - 'ArduSub/**' - - 'Rover/**' - 'Blimp/**' - - 'ArduCopter/**' + - 'Rover/**' + # remove non SITL HAL + - 'libraries/AP_HAL_ChibiOS/**' + - 'libraries/AP_HAL_ESP32/**' + # remove non SITL directories + - 'Tools/AP_Bootloader/**' + - 'Tools/AP_Periph/**' + - 'Tools/bootloaders/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove other vehicles autotest + - 'Tools/autotest/arducopter.py' + - 'Tools/autotest/arduplane.py' + - 'Tools/autotest/ardusub.py' + - 'Tools/autotest/balancebot.py' + - 'Tools/autotest/helicopter.py' + - 'Tools/autotest/location.txt' + - 'Tools/autotest/quadplane.py' + - 'Tools/autotest/rover.py' + - 'Tools/autotest/sailboat.py' + - 'Tools/autotest/swarminit.txt' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' pull_request: - paths-ignore: # ignore vehicle only changes + paths-ignore: + # remove other vehicles + - 'ArduCopter/**' - 'ArduPlane/**' - 'ArduSub/**' - - 'Rover/**' - 'Blimp/**' - - 'ArduCopter/**' + - 'Rover/**' + # remove non SITL HAL + - 'libraries/AP_HAL_ChibiOS/**' + - 'libraries/AP_HAL_ESP32/**' + # remove non SITL directories + - 'Tools/AP_Bootloader/**' + - 'Tools/AP_Periph/**' + - 'Tools/bootloaders/**' + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/Frame_params/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/IO_Firmware/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/Replay/**' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # Discard python file from Tools/scripts as not used + - 'Tools/scripts/**.py' + - 'Tools/scripts/build_sizes/**' + - 'Tools/scripts/build_tests/**' + - 'Tools/scripts/CAN/**' + - 'Tools/scripts/signing/**' + # Remove other vehicles autotest + - 'Tools/autotest/arducopter.py' + - 'Tools/autotest/arduplane.py' + - 'Tools/autotest/ardusub.py' + - 'Tools/autotest/balancebot.py' + - 'Tools/autotest/helicopter.py' + - 'Tools/autotest/location.txt' + - 'Tools/autotest/quadplane.py' + - 'Tools/autotest/rover.py' + - 'Tools/autotest/sailboat.py' + - 'Tools/autotest/swarminit.txt' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' workflow_dispatch: @@ -25,8 +163,8 @@ concurrency: jobs: build: - runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest + 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: @@ -36,7 +174,7 @@ jobs: ] steps: # git checkout the PR - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -70,9 +208,9 @@ jobs: autotest: needs: build # don't try to launch the tests matrix if it doesn't build first, profit from caching for fast build - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 container: - image: ardupilot/ardupilot-dev-base:latest + image: ardupilot/ardupilot-dev-base:v0.1.3 options: --privileged --cap-add=SYS_PTRACE --security-opt apparmor=unconfined --security-opt seccomp=unconfined strategy: fail-fast: false # don't cancel if a job from the matrix fails @@ -83,7 +221,7 @@ jobs: steps: # git checkout the PR - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' # Put ccache into github cache for faster build diff --git a/.github/workflows/test_size.yml b/.github/workflows/test_size.yml index 3e3f08fa6e553..f8015bc88a717 100644 --- a/.github/workflows/test_size.yml +++ b/.github/workflows/test_size.yml @@ -1,17 +1,67 @@ name: test size -on: [pull_request] -# paths: -# - "*" -# - "!README.md" <-- don't rebuild on doc change +on: + pull_request: + paths-ignore: # ignore autotest stuffs + - 'Tools/autotest/**' + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.flake8' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.pre-commit-config.yaml' + - './.pydevproject' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove generic tools + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/mavproxy_modules/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # remove non CHIBIOS HAL + - 'libraries/AP_HAL_SITL/**' + - 'libraries/AP_HAL_ESP32/**' + - 'libraries/AP_HAL_Linux/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' + workflow_dispatch: + + concurrency: group: ci-${{github.workflow}}-${{ github.ref }} cancel-in-progress: true jobs: build: - runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest + 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: @@ -21,8 +71,11 @@ jobs: config: [ Durandal, MatekF405, + KakuteF7, + MatekH743-bdshot, 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!) ] @@ -33,7 +86,7 @@ jobs: - config: disco toolchain: chibios steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: ref: ${{ github.event.pull_request.base.ref }} path: base_branch @@ -65,7 +118,8 @@ jobs: if [ "${{matrix.config}}" = "Hitec-Airspeed" ] || [ "${{matrix.config}}" = "f103-GPS" ]; then AP_PERIPH=1 - elif [ "${{matrix.config}}" = "MatekF405-CAN" ]; then + elif [ "${{matrix.config}}" = "MatekF405-CAN" ] || + [ "${{matrix.config}}" = "DrotekP3Pro" ]; then BOOTLOADER=1 fi if [ $BOOTLOADER -eq 1 ]; then @@ -121,7 +175,7 @@ jobs: echo [`date`] Built ${{ github.event.pull_request.base.ref }} with no versions - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: fetch-depth: 0 path: 'pr' @@ -138,7 +192,8 @@ jobs: if [ "${{matrix.config}}" = "Hitec-Airspeed" ] || [ "${{matrix.config}}" = "f103-GPS" ]; then AP_PERIPH=1 - elif [ "${{matrix.config}}" = "MatekF405-CAN" ]; then + elif [ "${{matrix.config}}" = "MatekF405-CAN" ] || + [ "${{matrix.config}}" = "DrotekP3Pro" ]; then BOOTLOADER=1 fi if [ $BOOTLOADER -eq 1 ]; then @@ -230,7 +285,8 @@ jobs: if [ "${{matrix.config}}" = "Hitec-Airspeed" ] || [ "${{matrix.config}}" = "f103-GPS" ]; then AP_PERIPH=1 - elif [ "${{matrix.config}}" = "MatekF405-CAN" ]; then + elif [ "${{matrix.config}}" = "MatekF405-CAN" ] || + [ "${{matrix.config}}" = "DrotekP3Pro" ]; then BOOTLOADER=1 fi if [ $AP_PERIPH -eq 1 ]; then @@ -251,42 +307,3 @@ jobs: shell: bash run: | diff -r $GITHUB_WORKSPACE/base_branch_bin_no_versions $GITHUB_WORKSPACE/pr_bin_no_versions --exclude=*.elf --exclude=*.apj || true - - - name: elf_diff compare with ${{ github.event.pull_request.base.ref }} - shell: bash - run: | - # we don't use weasyprint so manually pull the elf_diff deps reduce install size and time - python3 -m pip install -U --no-deps elf_diff GitPython Jinja2 MarkupSafe PyYAML anytree dict2xml gitdb progressbar2 python-utils setuptools-git smmap - mkdir elf_diff - 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" ]; then - BOOTLOADER=1 - fi - - if [ $AP_PERIPH -eq 1 ]; then - TO_CHECK="AP_Periph" - elif [ $BOOTLOADER -eq 1 ]; then - TO_CHECK="AP_Bootloader" - else - TO_CHECK="arduplane arducopter" - fi - for CHECK in $TO_CHECK; do - python3 -m elf_diff --bin_prefix="$BIN_PREFIX" --html_dir=elf_diff/AP_Periph $GITHUB_WORKSPACE/base_branch_bin/$CHECK $GITHUB_WORKSPACE/pr_bin/$CHECK - done - - zip -r elf_diff.zip elf_diff - - - name: Archive elf_diff output - uses: actions/upload-artifact@v3 - with: - name: ELF_DIFF_${{matrix.config}} - path: elf_diff.zip - retention-days: 14 diff --git a/.github/workflows/test_unit_tests.yml b/.github/workflows/test_unit_tests.yml index 54c660ea6b818..609e30386c1ca 100644 --- a/.github/workflows/test_unit_tests.yml +++ b/.github/workflows/test_unit_tests.yml @@ -1,18 +1,107 @@ name: test unit tests and sitl building -on: [push, pull_request, workflow_dispatch] -# paths: -# - "*" -# - "!README.md" <-- don't rebuild on doc change +on: + push: + paths-ignore: + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove generic tools + - 'Tools/CHDK-Script/**' + - 'Tools/CodeStyle/**' + - 'Tools/completion/**' + - 'Tools/CPUInfo/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/FilterTestTool/**' + - 'Tools/geotag/**' + - 'Tools/GIT_Test/**' + - 'Tools/gittools/**' + - 'Tools/Hello/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/mavproxy_modules/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/simulink/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/vagrant/**' + - 'Tools/Vicon/**' + # remove non SITL HAL + - 'libraries/AP_HAL_ChibiOS/**' + - 'libraries/AP_HAL_ESP32/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' + pull_request: + paths-ignore: + # Remove markdown files as irrelevant + - '**.md' + # Remove dotfile at root directory + - './.dir-locals.el' + - './.dockerignore' + - './.editorconfig' + - './.gitattributes' + - './.github' + - './.gitignore' + - './.valgrind-suppressions' + - './.valgrindrc' + - 'Dockerfile' + - 'Vagrantfile' + - 'Makefile' + # Remove some directories check + - '.vscode/**' + - '.github/ISSUE_TEMPLATE/**' + # Remove generic tools + - 'Tools/CHDK-Script/**' + - 'Tools/CPUInfo/**' + - 'Tools/CodeStyle/**' + - 'Tools/FilterTestTool/**' + - 'Tools/GIT_Test/**' + - 'Tools/Hello/**' + - 'Tools/Linux_HAL_Essentials/**' + - 'Tools/LogAnalyzer/**' + - 'Tools/Pozyx/**' + - 'Tools/PrintVersion.py' + - 'Tools/completion/**' + - 'Tools/debug/**' + - 'Tools/environment_install/**' + - 'Tools/geotag/**' + - 'Tools/gittools/**' + - 'Tools/mavproxy_modules/**' + - 'Tools/simulink/**' + - 'Tools/vagrant/**' + - 'Tools/UDP_Proxy/**' + - 'Tools/Vicon/**' + # remove non SITL HAL + - 'libraries/AP_HAL_ChibiOS/**' + - 'libraries/AP_HAL_ESP32/**' + # Remove change on other workflows + - '.github/workflows/test_environment.yml' + workflow_dispatch: + concurrency: group: ci-${{github.workflow}}-${{ github.ref }} cancel-in-progress: true jobs: build: - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 container: - image: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest + image: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.1.3 options: --user 1001 strategy: fail-fast: false # don't cancel if a job from the matrix fails @@ -28,7 +117,7 @@ jobs: ] steps: # git checkout the PR - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: 'recursive' # Put ccache into github cache for faster build diff --git a/.gitignore b/.gitignore index 58913bfed4d3d..2769d31790419 100644 --- a/.gitignore +++ b/.gitignore @@ -92,6 +92,7 @@ logs/ mav.log mav.log.raw mav.parm +mission.stg /defaults.parm /ArduCopter/defaults.parm /ArduPlane/defaults.parm @@ -111,7 +112,8 @@ GRTAGS GTAGS *.apj .gdbinit -/.vscode +.vscode/* +!.vscode/extensions.json /.history Parameters.html Parameters.md @@ -125,6 +127,7 @@ parameters.edn LogMessages.html LogMessages.rst LogMessages.xml +LogMessages.md # JetBrains IDE files .idea/* # CMake @@ -152,3 +155,14 @@ dumpstack_*out build.tmp.binaries/ tasklist.json modules/esp_idf + +# Ignore Python virtual environments +# from: https://github.com/github/gitignore/blob/4488915eec0b3a45b5c63ead28f286819c0917de/Python.gitignore#L125 +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ +autotest_result_*_junit.xml diff --git a/.gitmodules b/.gitmodules index a6fbca83a4ad0..fbfe76753a51d 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,3 @@ -[submodule "modules/uavcan"] - path = modules/uavcan - url = https://github.com/DroneCAN/libuavcan.git [submodule "modules/waf"] path = modules/waf url = https://github.com/ArduPilot/waf.git @@ -34,3 +31,11 @@ [submodule "modules/DroneCAN/libcanard"] path = modules/DroneCAN/libcanard url = https://github.com/DroneCAN/libcanard +[submodule "modules/Micro-XRCE-DDS-Client"] + path = modules/Micro-XRCE-DDS-Client + url = https://github.com/ardupilot/Micro-XRCE-DDS-Client.git + branch = master +[submodule "modules/Micro-CDR"] + path = modules/Micro-CDR + url = https://github.com/ardupilot/Micro-CDR.git + branch = master diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index ef511f9df7c9f..9c15396246364 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -16,7 +16,7 @@ exclude: | repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.2.0 + rev: v4.4.0 hooks: #- id: trailing-whitespace #- id: end-of-file-fixer @@ -24,6 +24,11 @@ repos: name: Check line ending character (LF) args: ["--fix=lf"] types_or: [python, c, c++, shell] + exclude: | + (?x)^( + libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp | + libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.h + )$ - id: check-added-large-files - id: check-executables-have-shebangs - id: check-shebang-scripts-are-executable @@ -36,10 +41,24 @@ repos: - id: check-xml - id: check-yaml -# Disable isort and mypy temporarily due to config conflicts + - repo: https://github.com/lsst-ts/pre-commit-xmllint + rev: 6f36260b537bf9a42b6ea5262c915ae50786296e + hooks: + - id: format-xmllint + files: libraries/AP_DDS/dds_xrce_profile.xml + - repo: https://github.com/psf/black + rev: 23.7.0 + hooks: + - id: black + files: | + (?x)^( + libraries\/AP_DDS\/(wscript|.*\.py)$ | + Tools/ros2/.*\.py + )$ + # # Use to sort python imports by name and put system import first. # - repo: https://github.com/pycqa/isort -# rev: 5.10.1 +# rev: 5.12.0 # hooks: # - id: isort # args: [--check-only] diff --git a/.semaphore/semaphore.yml b/.semaphore/semaphore.yml deleted file mode 100644 index 19c75718d99a4..0000000000000 --- a/.semaphore/semaphore.yml +++ /dev/null @@ -1,104 +0,0 @@ -version: v1.0 -name: ArduPilot Semaphore CI - -agent: - machine: - type: e1-standard-2 - os_image: ubuntu1804 - -# Auto-cancel both running and queued pipelines on a new push -auto_cancel: - running: - when: "true" - -# Shared accross all tasks -global_job_config: - prologue: - commands: - - checkout - - git submodule update --init --recursive --depth 1 --no-single-branch - - virtualenv --python=python2.7 --system-site-packages python2-env - - VIRTUAL_ENV_DISABLE_PROMPT=1 source python2-env/bin/activate - - sudo apt-get update && sudo apt-get install --no-install-recommends -y lsb-release software-properties-common && sudo apt purge -y gcc g++ - - export SKIP_AP_EXT_ENV=1 - - export SKIP_AP_GRAPHIC_ENV=1 - - export SKIP_AP_COV_ENV=1 - - export SKIP_AP_GIT_CHECK=1 - - ./Tools/environment_install/install-prereqs-ubuntu.sh -y - - mkdir -p ~/.ccache - - echo "base_dir = /home/semaphore/${SEMAPHORE_GIT_DIR}" > ~/.ccache/ccache.conf - - echo "compression = true" >> ~/.ccache/ccache.conf - - echo "compression_level = 6" >> ~/.ccache/ccache.conf - - echo "max_size = 400M" >> ~/.ccache/ccache.conf - - PATH="/home/semaphore/.local/bin:$PATH" - - epilogue: - always: - commands: - - git submodule deinit --force . - -blocks: - - name: "Building" - task: - jobs: - - name: "Linux_boards-1" - commands: - - source /home/semaphore/.profile - - cache restore ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-1,ccache-master-$SEMAPHORE_JOB_NAME-1 - - CI_BUILD_TARGET="navio" ./Tools/scripts/build_ci.sh - - cache store ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-1 ~/.ccache - - cache restore ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-2,ccache-master-$SEMAPHORE_JOB_NAME-2 - - CI_BUILD_TARGET="bbbmini" ./Tools/scripts/build_ci.sh - - cache store ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-2 ~/.ccache - - cache restore ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-3,ccache-master-$SEMAPHORE_JOB_NAME-3 - - CI_BUILD_TARGET="bhat" ./Tools/scripts/build_ci.sh - - cache store ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-3 ~/.ccache - - - name: "SITL" - commands: - - source /home/semaphore/.profile - - cache restore ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-1,ccache-master-$SEMAPHORE_JOB_NAME-1 - - CI_BUILD_TARGET="sitl" ./Tools/scripts/build_ci.sh - - cache store ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-1 ~/.ccache - - cache restore ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-2,ccache-master-$SEMAPHORE_JOB_NAME-2 - - CI_BUILD_TARGET="configure-all" ./Tools/scripts/build_ci.sh - - cache store ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-2 ~/.ccache - - - name: "Linux_boards-2" - commands: - - source /home/semaphore/.profile - - cache restore ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-1,ccache-master-$SEMAPHORE_JOB_NAME-1 - - CI_BUILD_TARGET="bebop" ./Tools/scripts/build_ci.sh - - cache store ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-1 ~/.ccache - - cache restore ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-2,ccache-master-$SEMAPHORE_JOB_NAME-2 - - CI_BUILD_TARGET="linux" ./Tools/scripts/build_ci.sh - - cache store ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-2 ~/.ccache - - cache restore ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-3,ccache-master-$SEMAPHORE_JOB_NAME-3 - - CI_BUILD_TARGET="navio2" ./Tools/scripts/build_ci.sh - - cache store ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-3 ~/.ccache - - - name: "Linux_boards-3" - commands: - - source /home/semaphore/.profile - - cache restore ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-1,ccache-master-$SEMAPHORE_JOB_NAME-1 - - CI_BUILD_TARGET="erlebrain2" ./Tools/scripts/build_ci.sh - - cache store ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-1 ~/.ccache - - cache restore ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-2,ccache-master-$SEMAPHORE_JOB_NAME-2 - - CI_BUILD_TARGET="pxfmini" ./Tools/scripts/build_ci.sh - - cache store ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-2 ~/.ccache - - cache restore ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-3,ccache-master-$SEMAPHORE_JOB_NAME-3 - - CI_BUILD_TARGET="pxf" ./Tools/scripts/build_ci.sh - - cache store ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-3 ~/.ccache - - - name: "Chibios_boards" - commands: - - source /home/semaphore/.profile - - cache restore ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-1,ccache-master-$SEMAPHORE_JOB_NAME-1 - - CI_BUILD_TARGET="fmuv3" ./Tools/scripts/build_ci.sh - - cache store ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-1 ~/.ccache - - cache restore ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-2,ccache-master-$SEMAPHORE_JOB_NAME-2 - - CI_BUILD_TARGET="revo-mini" ./Tools/scripts/build_ci.sh - - cache store ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-2 ~/.ccache - - cache restore ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-3,ccache-master-$SEMAPHORE_JOB_NAME-3 - - CI_BUILD_TARGET="MatekF405-Wing" ./Tools/scripts/build_ci.sh - - cache store ccache-$SEMAPHORE_GIT_BRANCH-$SEMAPHORE_JOB_NAME-3 ~/.ccache \ No newline at end of file diff --git a/.vscode/extensions.json b/.vscode/extensions.json index e9cd876a0b91c..4284ec07442f1 100644 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -1,11 +1,13 @@ { "recommendations": [ + "augustocdias.tasks-shell-input", + "bierner.markdown-mermaid", "ms-vscode.cpptools", "sumneko.lua", "ms-python.python", "ms-python.vscode-pylance", "streetsidesoftware.code-spell-checker", "chiehyu.vscode-astyle", - "ardupilot-org.ardupilot-devenv" + "ardupilot-org.ardupilot-devenv", ] } diff --git a/.vscode/settings.json b/.vscode/settings.default.json similarity index 100% rename from .vscode/settings.json rename to .vscode/settings.default.json diff --git a/AntennaTracker/AntennaTracker.txt b/AntennaTracker/AntennaTracker.txt index f66169dee5093..8ced3605b2246 100644 --- a/AntennaTracker/AntennaTracker.txt +++ b/AntennaTracker/AntennaTracker.txt @@ -109,7 +109,7 @@ around. It might even damage itself. AntennaTracker (like other ardupilot software such as ArduPlane, ArduRover etc) has configuration values that control and tailor its operation, and which are stored in EEPROM on the processor. The configuration is restored from -EEPROM every time the processsor starts. +EEPROM every time the processor starts. You can use MissionPlanner, mavproxy or apm_planner or other mavlink compatible software to check and change the configuration of your AntennaTracker. @@ -219,7 +219,7 @@ the vehicle, cd to the ArduPlane directory and run this: ../Tools/autotest/sim_arduplane.sh -T --aircraft test -The -T flag tells sim_arduplane.sh to start an entenna tracker +The -T flag tells sim_arduplane.sh to start an antenna tracker simulator and also start a virtual antenna tracker in a window. To start the antenna tracker running run "tracker start" in the diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index 02c48c6b94f38..0fbe19edaa8ca 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -274,13 +274,17 @@ static const ap_message STREAM_RAW_SENSORS_msgs[] = { static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { MSG_SYS_STATUS, MSG_POWER_STATUS, +#if HAL_WITH_MCU_MONITORING MSG_MCU_STATUS, +#endif MSG_MEMINFO, MSG_NAV_CONTROLLER_OUTPUT, MSG_GPS_RAW, MSG_GPS_RTK, +#if GPS_MAX_RECEIVERS > 1 MSG_GPS2_RAW, MSG_GPS2_RTK, +#endif }; static const ap_message STREAM_POSITION_msgs[] = { MSG_LOCATION, @@ -299,11 +303,15 @@ static const ap_message STREAM_EXTRA1_msgs[] = { }; static const ap_message STREAM_EXTRA3_msgs[] = { MSG_AHRS, +#if AP_SIM_ENABLED MSG_SIMSTATE, +#endif MSG_SYSTEM_TIME, MSG_AHRS2, +#if COMPASS_CAL_ENABLED MSG_MAG_CAL_REPORT, MSG_MAG_CAL_PROGRESS, +#endif MSG_EKF_STATUS_REPORT, }; static const ap_message STREAM_PARAMS_msgs[] = { @@ -412,7 +420,7 @@ MAV_RESULT GCS_MAVLINK_Tracker::_handle_command_preflight_calibration_baro(const return ret; } -MAV_RESULT GCS_MAVLINK_Tracker::handle_command_component_arm_disarm(const mavlink_command_long_t &packet) +MAV_RESULT GCS_MAVLINK_Tracker::handle_command_component_arm_disarm(const mavlink_command_int_t &packet) { if (is_equal(packet.param1,1.0f)) { tracker.arm_servos(); @@ -425,11 +433,8 @@ MAV_RESULT GCS_MAVLINK_Tracker::handle_command_component_arm_disarm(const mavlin return MAV_RESULT_UNSUPPORTED; } -MAV_RESULT GCS_MAVLINK_Tracker::handle_command_long_packet(const mavlink_command_long_t &packet) +MAV_RESULT GCS_MAVLINK_Tracker::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { - // do command - send_text(MAV_SEVERITY_INFO,"Command received: "); - switch(packet.command) { case MAV_CMD_DO_SET_SERVO: @@ -447,7 +452,7 @@ MAV_RESULT GCS_MAVLINK_Tracker::handle_command_long_packet(const mavlink_command return MAV_RESULT_ACCEPTED; default: - return GCS_MAVLINK::handle_command_long_packet(packet); + return GCS_MAVLINK::handle_command_int_packet(packet, msg); } } diff --git a/AntennaTracker/GCS_Mavlink.h b/AntennaTracker/GCS_Mavlink.h index 5dcaaeda49665..38f7cdb483e71 100644 --- a/AntennaTracker/GCS_Mavlink.h +++ b/AntennaTracker/GCS_Mavlink.h @@ -9,18 +9,19 @@ class GCS_MAVLINK_Tracker : public GCS_MAVLINK using GCS_MAVLINK::GCS_MAVLINK; + uint8_t sysid_my_gcs() const override; + protected: // telem_delay is not used by Tracker but is pure virtual, thus - // this implementaiton. it probably *should* be used by Tracker, + // this implementation. it probably *should* be used by Tracker, // as currently Tracker may brick XBees uint32_t telem_delay() const override { return 0; } - uint8_t sysid_my_gcs() const override; - MAV_RESULT handle_command_component_arm_disarm(const mavlink_command_long_t &packet) override; + MAV_RESULT handle_command_component_arm_disarm(const mavlink_command_int_t &packet) override; MAV_RESULT _handle_command_preflight_calibration_baro(const mavlink_message_t &msg) override; - MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override; + MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; int32_t global_position_int_relative_alt() const override { return 0; // what if we have been picked up and carried somewhere? diff --git a/AntennaTracker/Parameters.cpp b/AntennaTracker/Parameters.cpp index 33cb304aa19ba..2d20396ecfc69 100644 --- a/AntennaTracker/Parameters.cpp +++ b/AntennaTracker/Parameters.cpp @@ -23,6 +23,7 @@ const AP_Param::Info Tracker::var_info[] = { // @DisplayName: Ground station MAVLink system ID // @Description: The identifier of the ground station in the MAVLink protocol. Don't change this unless you also modify the ground station to match. // @Range: 1 255 + // @Increment: 1 // @User: Advanced GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), @@ -130,7 +131,7 @@ const AP_Param::Info Tracker::var_info[] = { // @Param: ONOFF_PITCH_MINT // @DisplayName: Pitch minimum movement time - // @Description: Minimim amount of time in seconds to move in pitch + // @Description: Minimum amount of time in seconds to move in pitch // @Units: s // @Increment: 0.01 // @Range: 0 2 @@ -379,6 +380,33 @@ const AP_Param::Info Tracker::var_info[] = { // @Increment: 0.5 // @User: Advanced + // @Param: PITCH2SRV_PDMX + // @DisplayName: Pitch axis controller PD sum maximum + // @Description: Pitch axis controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output + // @Range: 0 4000 + // @Increment: 10 + // @Units: d% + // @User: Advanced + + // @Param: PITCH2SRV_D_FF + // @DisplayName: Pitch 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.1 + // @Increment: 0.001 + // @User: Advanced + + // @Param: PITCH2SRV_NTF + // @DisplayName: Pitch Target notch filter index + // @Description: Pitch Target notch filter index + // @Range: 1 8 + // @User: Advanced + + // @Param: PITCH2SRV_NEF + // @DisplayName: Pitch Error notch filter index + // @Description: Pitch Error notch filter index + // @Range: 1 8 + // @User: Advanced + GGROUP(pidPitch2Srv, "PITCH2SRV_", AC_PID), // @Param: YAW2SRV_P @@ -448,6 +476,33 @@ const AP_Param::Info Tracker::var_info[] = { // @Increment: 0.5 // @User: Advanced + // @Param: YAW2SRV_PDMX + // @DisplayName: Yaw axis controller PD sum maximum + // @Description: Yaw axis controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output + // @Range: 0 4000 + // @Increment: 10 + // @Units: d% + // @User: Advanced + + // @Param: YAW2SRV_D_FF + // @DisplayName: Yaw 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.1 + // @Increment: 0.001 + // @User: Advanced + + // @Param: YAW2SRV_NTF + // @DisplayName: Yaw Target notch filter index + // @Description: Yaw Target notch filter index + // @Range: 1 8 + // @User: Advanced + + // @Param: YAW2SRV_NEF + // @DisplayName: Yaw Error notch filter index + // @Description: Yaw Error notch filter index + // @Range: 1 8 + // @User: Advanced + GGROUP(pidYaw2Srv, "YAW2SRV_", AC_PID), #if AP_SCRIPTING_ENABLED @@ -525,6 +580,18 @@ const AP_Param::Info Tracker::var_info[] = { // @Path: ../libraries/AP_Logger/AP_Logger.cpp GOBJECT(logger, "LOG", AP_Logger), +#if HAL_NAVEKF2_AVAILABLE + // @Group: EK2_ + // @Path: ../libraries/AP_NavEKF2/AP_NavEKF2.cpp + GOBJECTN(ahrs.EKF2, NavEKF2, "EK2_", NavEKF2), +#endif + +#if HAL_NAVEKF3_AVAILABLE + // @Group: EK3_ + // @Path: ../libraries/AP_NavEKF3/AP_NavEKF3.cpp + GOBJECTN(ahrs.EKF3, NavEKF3, "EK3_", NavEKF3), +#endif + AP_VAREND }; diff --git a/AntennaTracker/Parameters.h b/AntennaTracker/Parameters.h index a3192e24b5594..972c36f52505e 100644 --- a/AntennaTracker/Parameters.h +++ b/AntennaTracker/Parameters.h @@ -130,6 +130,8 @@ class Parameters { k_param_disarm_pwm, k_param_auto_opts, + k_param_NavEKF2, + k_param_NavEKF3, k_param_logger = 253, // 253 - Logging Group diff --git a/AntennaTracker/Tracker.cpp b/AntennaTracker/Tracker.cpp index e7e1a384587e7..fbf3427c501c5 100644 --- a/AntennaTracker/Tracker.cpp +++ b/AntennaTracker/Tracker.cpp @@ -64,7 +64,6 @@ const AP_Scheduler::Task Tracker::scheduler_tasks[] = { SCHED_TASK_CLASS(AP_Logger, &tracker.logger, periodic_tasks, 50, 300, 65), #endif SCHED_TASK_CLASS(AP_InertialSensor, &tracker.ins, periodic, 50, 50, 70), - SCHED_TASK_CLASS(AP_Notify, &tracker.notify, update, 50, 100, 75), SCHED_TASK(one_second_loop, 1, 3900, 80), SCHED_TASK(stats_update, 1, 200, 90), }; @@ -104,6 +103,8 @@ void Tracker::one_second_loop() set_likely_flying(hal.util->get_soft_armed()); AP_Notify::flags.flying = hal.util->get_soft_armed(); + + g.pidYaw2Srv.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); } void Tracker::ten_hz_logging_loop() diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp index 191c236294a13..3e296c3fa5a77 100644 --- a/AntennaTracker/system.cpp +++ b/AntennaTracker/system.cpp @@ -59,9 +59,6 @@ void Tracker::init_ardupilot() // initialise AP_Logger library logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&tracker, &Tracker::Log_Write_Vehicle_Startup_Messages, void)); - // set serial ports non-blocking - serial_manager.set_blocking_writes_all(false); - // initialise rc channels including setting mode rc().convert_options(RC_Channel::AUX_FUNC::ARMDISARM_UNUSED, RC_Channel::AUX_FUNC::ARMDISARM); rc().init(); diff --git a/AntennaTracker/version.h b/AntennaTracker/version.h index 1adb76acba8a9..ff55bece071f3 100644 --- a/AntennaTracker/version.h +++ b/AntennaTracker/version.h @@ -6,13 +6,13 @@ #include "ap_version.h" -#define THISFIRMWARE "AntennaTracker V4.3.0-dev" +#define THISFIRMWARE "AntennaTracker V4.5.0-dev" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,3,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 4,5,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 4 -#define FW_MINOR 3 +#define FW_MINOR 5 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index db60b9cfc754e..bdd901706b50d 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -9,8 +9,6 @@ //#define AC_OAPATHPLANNER_ENABLED DISABLED // disable path planning around obstacles //#define PARACHUTE DISABLED // disable parachute release to save 1k of flash //#define NAV_GUIDED DISABLED // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands -//#define PRECISION_LANDING DISABLED // disable precision landing using companion computer or IRLock sensor -//#define BEACON_ENABLED DISABLED // disable beacon support //#define STATS_ENABLED DISABLED // disable statistics support //#define MODE_ACRO_ENABLED DISABLED // disable acrobatic mode support //#define MODE_AUTO_ENABLED DISABLED // disable auto mode support diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 6f6051f89676c..d2e4838f8ce30 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -1,5 +1,10 @@ #include "Copter.h" +#pragma GCC diagnostic push +#if defined(__clang__) +#pragma GCC diagnostic ignored "-Wbitwise-instead-of-logical" +#endif + bool AP_Arming_Copter::pre_arm_checks(bool display_failure) { const bool passed = run_pre_arm_checks(display_failure); @@ -18,11 +23,12 @@ bool AP_Arming_Copter::run_pre_arm_checks(bool display_failure) // check if motor interlock and either Emergency Stop aux switches are used // at the same time. This cannot be allowed. + bool passed = true; if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_INTERLOCK) && (rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_ESTOP) || rc().find_channel_for_option(RC_Channel::AUX_FUNC::ARM_EMERGENCY_STOP))){ check_failed(display_failure, "Interlock/E-Stop Conflict"); - return false; + passed = false; } // check if motor interlock aux switch is in use @@ -30,23 +36,22 @@ bool AP_Arming_Copter::run_pre_arm_checks(bool display_failure) // otherwise exit immediately. if (copter.ap.using_interlock && copter.ap.motor_interlock_switch) { check_failed(display_failure, "Motor Interlock Enabled"); - return false; - } - - // if we are using motor Estop switch, it must not be in Estop position - if (SRV_Channels::get_emergency_stop()){ - check_failed(display_failure, "Motor Emergency Stopped"); - return false; + passed = false; } if (!disarm_switch_checks(display_failure)) { - return false; + passed = false; } // always check motors char failure_msg[50] {}; if (!copter.motors->arming_checks(ARRAY_SIZE(failure_msg), failure_msg)) { check_failed(display_failure, "Motors: %s", failure_msg); + passed = false; + } + + // If not passed all checks return false + if (!passed) { return false; } @@ -55,6 +60,7 @@ bool AP_Arming_Copter::run_pre_arm_checks(bool display_failure) return mandatory_checks(display_failure); } + // bitwise & ensures all checks are run return parameter_checks(display_failure) & oa_checks(display_failure) & gcs_failsafe_check(display_failure) @@ -218,19 +224,6 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure) } #if FRAME_CONFIG == HELI_FRAME - if (copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_QUAD && - copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_DUAL && - copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Invalid Heli FRAME_CLASS"); - return false; - } - - // check helicopter parameters - if (!copter.motors->parameter_check(display_failure)) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Heli motors checks failed"); - return false; - } - char fail_msg[50]; // check input manager parameters if (!copter.input_manager.parameter_check(fail_msg, ARRAY_SIZE(fail_msg))) { @@ -338,6 +331,7 @@ bool AP_Arming_Copter::rc_calibration_checks(bool display_failure) copter.channel_yaw }; + // bitwise & ensures all checks are run copter.ap.pre_arm_rc_check = rc_checks_copter_sub(display_failure, channels) & AP_Arming::rc_calibration_checks(display_failure); @@ -629,7 +623,7 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method) const char *rc_item = "Throttle"; #endif // check throttle is not too high - skips checks if arming from GCS/scripting in Guided,Guided_NoGPS or Auto - if (!((method == AP_Arming::Method::MAVLINK || method == AP_Arming::Method::SCRIPTING) && (copter.flightmode->mode_number() == Mode::Number::GUIDED || copter.flightmode->mode_number() == Mode::Number::GUIDED_NOGPS || copter.flightmode->mode_number() == Mode::Number::AUTO))) { + if (!((AP_Arming::method_is_GCS(method) || method == AP_Arming::Method::SCRIPTING) && (copter.flightmode->mode_number() == Mode::Number::GUIDED || copter.flightmode->mode_number() == Mode::Number::GUIDED_NOGPS || copter.flightmode->mode_number() == Mode::Number::AUTO))) { // above top of deadband is too always high if (copter.get_pilot_desired_climb_rate(copter.channel_throttle->get_control_in()) > 0.0f) { check_failed(ARMING_CHECK_RC, true, "%s too high", rc_item); @@ -714,7 +708,7 @@ bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_ } #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - gcs().send_text(MAV_SEVERITY_INFO, "Arming motors"); + send_arm_disarm_statustext("Arming motors"); #endif // Remember Orientation @@ -755,8 +749,8 @@ bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_ copter.sprayer.test_pump(false); #endif - // enable output to motors - copter.enable_motor_output(); + // output lowest possible value to motors + copter.motors->output_min(); // finally actually arm the motors copter.motors->armed(true); @@ -796,7 +790,7 @@ bool AP_Arming_Copter::disarm(const AP_Arming::Method method, bool do_disarm_che // do not allow disarm via mavlink if we think we are flying: if (do_disarm_checks && - method == AP_Arming::Method::MAVLINK && + AP_Arming::method_is_GCS(method) && !copter.ap.land_complete) { return false; } @@ -806,7 +800,7 @@ bool AP_Arming_Copter::disarm(const AP_Arming::Method method, bool do_disarm_che } #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - gcs().send_text(MAV_SEVERITY_INFO, "Disarming motors"); + send_arm_disarm_statustext("Disarming motors"); #endif auto &ahrs = AP::ahrs(); @@ -851,3 +845,5 @@ bool AP_Arming_Copter::disarm(const AP_Arming::Method method, bool do_disarm_che return true; } + +#pragma GCC diagnostic pop diff --git a/ArduCopter/AP_ExternalControl_Copter.cpp b/ArduCopter/AP_ExternalControl_Copter.cpp new file mode 100644 index 0000000000000..35353a412618f --- /dev/null +++ b/ArduCopter/AP_ExternalControl_Copter.cpp @@ -0,0 +1,37 @@ +/* + external control library for copter + */ + + +#include "AP_ExternalControl_Copter.h" +#if AP_EXTERNAL_CONTROL_ENABLED + +#include "Copter.h" + +/* + set linear velocity and yaw rate. Pass NaN for yaw_rate_rads to not control yaw + velocity is in earth frame, NED, m/s +*/ +bool AP_ExternalControl_Copter::set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, float yaw_rate_rads) +{ + if (!ready_for_external_control()) { + return false; + } + const float yaw_rate_cds = isnan(yaw_rate_rads)? 0: degrees(yaw_rate_rads)*100; + + // Copter velocity is positive if aircraft is moving up which is opposite the incoming NED frame. + Vector3f velocity_NEU_ms { + linear_velocity.x, + linear_velocity.y, + -linear_velocity.z }; + Vector3f velocity_up_cms = velocity_NEU_ms * 100; + copter.mode_guided.set_velocity(velocity_up_cms, false, 0, !isnan(yaw_rate_rads), yaw_rate_cds); + return true; +} + +bool AP_ExternalControl_Copter::ready_for_external_control() +{ + return copter.flightmode->in_guided_mode() && copter.motors->armed(); +} + +#endif // AP_EXTERNAL_CONTROL_ENABLED diff --git a/ArduCopter/AP_ExternalControl_Copter.h b/ArduCopter/AP_ExternalControl_Copter.h new file mode 100644 index 0000000000000..e7601c5552fc6 --- /dev/null +++ b/ArduCopter/AP_ExternalControl_Copter.h @@ -0,0 +1,26 @@ +/* + external control library for copter + */ +#pragma once + +#include + +#if AP_EXTERNAL_CONTROL_ENABLED + +class AP_ExternalControl_Copter : public AP_ExternalControl { +public: + /* + Set linear velocity and yaw rate. Pass NaN for yaw_rate_rads to not control yaw. + Velocity is in earth frame, NED [m/s]. + Yaw is in earth frame, NED [rad/s]. + */ + bool set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, float yaw_rate_rads) override WARN_IF_UNUSED; +private: + /* + Return true if Copter is ready to handle external control data. + Currently checks mode and arm states. + */ + bool ready_for_external_control() WARN_IF_UNUSED; +}; + +#endif // AP_EXTERNAL_CONTROL_ENABLED diff --git a/ArduCopter/AP_Rally.cpp b/ArduCopter/AP_Rally.cpp index 31027d7909cdf..46b6735f560e7 100644 --- a/ArduCopter/AP_Rally.cpp +++ b/ArduCopter/AP_Rally.cpp @@ -13,6 +13,10 @@ along with this program. If not, see . */ +#include + +#if HAL_RALLY_ENABLED + #include #include "Copter.h" @@ -28,3 +32,5 @@ bool AP_Rally_Copter::is_valid(const Location &rally_point) const #endif return true; } + +#endif // HAL_RALLY_ENABLED diff --git a/ArduCopter/AP_Rally.h b/ArduCopter/AP_Rally.h index 466f5c04b9897..27c3df857a927 100644 --- a/ArduCopter/AP_Rally.h +++ b/ArduCopter/AP_Rally.h @@ -14,6 +14,10 @@ */ #pragma once +#include + +#if HAL_RALLY_ENABLED + #include #include @@ -28,3 +32,5 @@ class AP_Rally_Copter : public AP_Rally private: bool is_valid(const Location &rally_point) const override; }; + +#endif // HAL_RALLY_ENABLED diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 293d1d38f5f09..f5242feb0d95d 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -165,7 +165,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #if HAL_PROXIMITY_ENABLED SCHED_TASK_CLASS(AP_Proximity, &copter.g2.proximity, update, 200, 50, 36), #endif -#if BEACON_ENABLED == ENABLED +#if AP_BEACON_ENABLED SCHED_TASK_CLASS(AP_Beacon, &copter.g2.beacon, update, 400, 50, 39), #endif SCHED_TASK(update_altitude, 10, 100, 42), @@ -178,9 +178,11 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK_CLASS(AC_Sprayer, &copter.sprayer, update, 3, 90, 54), #endif SCHED_TASK(three_hz_loop, 3, 75, 57), +#if AP_SERVORELAYEVENTS_ENABLED SCHED_TASK_CLASS(AP_ServoRelayEvents, &copter.ServoRelayEvents, update_events, 50, 75, 60), +#endif SCHED_TASK_CLASS(AP_Baro, &copter.barometer, accumulate, 50, 90, 63), -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED SCHED_TASK(update_precland, 400, 50, 69), #endif #if FRAME_CONFIG == HELI_FRAME @@ -189,7 +191,6 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #if LOGGING_ENABLED == ENABLED SCHED_TASK(loop_rate_logging, LOOP_RATE, 50, 75), #endif - SCHED_TASK_CLASS(AP_Notify, &copter.notify, update, 50, 90, 78), SCHED_TASK(one_hz_loop, 1, 100, 81), SCHED_TASK(ekf_check, 10, 75, 84), SCHED_TASK(check_vibration, 10, 50, 87), @@ -219,7 +220,9 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #if AP_RPM_ENABLED SCHED_TASK_CLASS(AP_RPM, &copter.rpm_sensor, update, 40, 200, 129), #endif +#if AP_TEMPCALIBRATION_ENABLED SCHED_TASK_CLASS(AP_TempCalibration, &copter.g2.temp_calibration, update, 10, 100, 135), +#endif #if HAL_ADSB_ENABLED SCHED_TASK(avoidance_adsb_update, 10, 100, 138), #endif @@ -446,6 +449,26 @@ bool Copter::has_ekf_failsafed() const #endif // AP_SCRIPTING_ENABLED +// returns true if vehicle is landing. Only used by Lua scripts +bool Copter::is_landing() const +{ + return flightmode->is_landing(); +} + +// returns true if vehicle is taking off. Only used by Lua scripts +bool Copter::is_taking_off() const +{ + return flightmode->is_taking_off(); +} + +bool Copter::current_mode_requires_mission() const +{ +#if MODE_AUTO_ENABLED == ENABLED + return flightmode == &mode_auto; +#else + return false; +#endif +} // rc_loops - reads user input from transmitter/receiver // called at 100hz @@ -550,7 +573,7 @@ void Copter::ten_hz_logging_loop() #if HAL_PROXIMITY_ENABLED g2.proximity.log(); // Write proximity sensor distances #endif -#if BEACON_ENABLED == ENABLED +#if AP_BEACON_ENABLED g2.beacon.log(); #endif } @@ -562,6 +585,11 @@ void Copter::ten_hz_logging_loop() g2.winch.write_log(); } #endif +#if HAL_MOUNT_ENABLED + if (should_log(MASK_LOG_CAMERA)) { + camera_mount.write_log(); + } +#endif } // twentyfive_hz_logging - should be run at 25hz @@ -588,7 +616,7 @@ void Copter::twentyfive_hz_logging() #endif } -// three_hz_loop - 3.3hz loop +// three_hz_loop - 3hz loop void Copter::three_hz_loop() { // check if we've lost contact with the ground station @@ -643,6 +671,13 @@ void Copter::one_hz_loop() #endif AP_Notify::flags.flying = !ap.land_complete; + + // slowly update the PID notches with the average loop rate + attitude_control->set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); + pos_control->get_accel_z_pid().set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); +#if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED + custom_control.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); +#endif } void Copter::init_simple_bearing() @@ -718,7 +753,7 @@ void Copter::update_super_simple_bearing(bool force_update) void Copter::read_AHRS(void) { - // we tell AHRS to skip INS update as we have already done it in fast_loop() + // we tell AHRS to skip INS update as we have already done it in FAST_TASK. ahrs.update(true); } diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 8322ad66e60ff..bb468a39939a5 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -69,6 +69,7 @@ #include // Crop sprayer library #include // ADS-B RF based collision avoidance module library #include // ArduPilot proximity sensor library +#include #include #include @@ -76,12 +77,6 @@ #include "defines.h" #include "config.h" -#if FRAME_CONFIG == HELI_FRAME - #define AC_AttitudeControl_t AC_AttitudeControl_Heli -#else - #define AC_AttitudeControl_t AC_AttitudeControl_Multi -#endif - #if FRAME_CONFIG == HELI_FRAME #define MOTOR_CLASS AP_MotorsHeli #else @@ -99,8 +94,13 @@ #include "AP_Rally.h" // Rally point library #include "AP_Arming.h" -// libraries which are dependent on #defines in defines.h and/or config.h -#if BEACON_ENABLED == ENABLED +#include +#if AP_EXTERNAL_CONTROL_ENABLED +#include "AP_ExternalControl_Copter.h" +#endif + +#include +#if AP_BEACON_ENABLED #include #endif @@ -115,7 +115,7 @@ #if AP_GRIPPER_ENABLED # include #endif -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED # include # include #endif @@ -168,15 +168,14 @@ #error AP_OAPathPlanner relies on AP_FENCE_ENABLED which is disabled #endif -// Local modules -#ifdef USER_PARAMS_ENABLED -#include "UserParameters.h" -#endif -#include "Parameters.h" #if HAL_ADSB_ENABLED #include "avoidance_adsb.h" #endif - +// Local modules +#include "Parameters.h" +#if USER_PARAMS_ENABLED +#include "UserParameters.h" +#endif #include "mode.h" class Copter : public AP_Vehicle { @@ -192,6 +191,9 @@ class Copter : public AP_Vehicle { friend class AP_AdvancedFailsafe_Copter; #endif friend class AP_Arming_Copter; +#if AP_EXTERNAL_CONTROL_ENABLED + friend class AP_ExternalControl_Copter; +#endif friend class ToyMode; friend class RC_Channel_Copter; friend class RC_Channels_Copter; @@ -226,6 +228,10 @@ class Copter : public AP_Vehicle { friend class ModeAutorotate; friend class ModeTurtle; + friend class _AutoTakeoff; + + friend class PayloadPlace; + Copter(void); private: @@ -318,6 +324,12 @@ class Copter : public AP_Vehicle { AP_OpticalFlow optflow; #endif + // external control library +#if AP_EXTERNAL_CONTROL_ENABLED + AP_ExternalControl_Copter external_control; +#endif + + // system time in milliseconds of last recorded yaw reset from ekf uint32_t ekfYawReset_ms; int8_t ekf_primary_core; @@ -467,7 +479,8 @@ class Copter : public AP_Vehicle { // Attitude, Position and Waypoint navigation objects // To-Do: move inertial nav up or other navigation variables down here - AC_AttitudeControl_t *attitude_control; + AC_AttitudeControl *attitude_control; + const struct AP_Param::GroupInfo *attitude_control_var_info; AC_PosControl *pos_control; AC_WPNav *wp_nav; AC_Loiter *loiter_nav; @@ -529,7 +542,7 @@ class Copter : public AP_Vehicle { #endif // Precision Landing -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED AC_PrecLand precland; AC_PrecLand_StateMachine precland_statemachine; #endif @@ -679,6 +692,8 @@ class Copter : public AP_Vehicle { // returns true if the EKF failsafe has triggered bool has_ekf_failsafed() const override; #endif // AP_SCRIPTING_ENABLED + bool is_landing() const override; + bool is_taking_off() const override; void rc_loop(); void throttle_loop(); void update_batt_compass(void); @@ -850,9 +865,13 @@ class Copter : public AP_Vehicle { // called when an attempt to change into a mode is unsuccessful: void mode_change_failed(const Mode *mode, const char *reason); uint8_t get_mode() const override { return (uint8_t)flightmode->mode_number(); } + bool current_mode_requires_mission() const override; void update_flight_mode(); void notify_flight_mode(); + // Check if this mode can be entered from the GCS + bool gcs_mode_enabled(const Mode::Number mode_num); + // mode_land.cpp void set_mode_land_with_pause(ModeReason reason); bool landing_with_GPS(); @@ -891,7 +910,6 @@ class Copter : public AP_Vehicle { void default_dead_zones(); void init_rc_in(); void init_rc_out(); - void enable_motor_output(); void read_radio(); void set_throttle_and_failsafe(uint16_t throttle_pwm); void set_throttle_zero_flag(int16_t throttle_control); diff --git a/ArduCopter/GCS_Copter.cpp b/ArduCopter/GCS_Copter.cpp index 7721e5e74665d..02599b045d798 100644 --- a/ArduCopter/GCS_Copter.cpp +++ b/ArduCopter/GCS_Copter.cpp @@ -114,7 +114,7 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void) } #endif -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED if (copter.precland.enabled()) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; diff --git a/ArduCopter/GCS_Copter.h b/ArduCopter/GCS_Copter.h index e00ec46f37b59..4e419add63084 100644 --- a/ArduCopter/GCS_Copter.h +++ b/ArduCopter/GCS_Copter.h @@ -28,9 +28,10 @@ class GCS_Copter : public GCS bool simple_input_active() const override; bool supersimple_input_active() const override; + uint8_t sysid_this_mav() const override; + protected: - uint8_t sysid_this_mav() const override; // minimum amount of time (in microseconds) that must remain in // the main scheduler loop before we are allowed to send any diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 28aebf0511093..7f86eabc8ef5e 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -311,17 +311,17 @@ void GCS_MAVLINK_Copter::send_pid_tuning() } } +#if AP_WINCH_ENABLED // send winch status message void GCS_MAVLINK_Copter::send_winch_status() const { -#if AP_WINCH_ENABLED AP_Winch *winch = AP::winch(); if (winch == nullptr) { return; } winch->send_status(*this); -#endif } +#endif uint8_t GCS_MAVLINK_Copter::sysid_my_gcs() const { @@ -503,15 +503,21 @@ static const ap_message STREAM_RAW_SENSORS_msgs[] = { static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { MSG_SYS_STATUS, MSG_POWER_STATUS, +#if HAL_WITH_MCU_MONITORING MSG_MCU_STATUS, +#endif MSG_MEMINFO, MSG_CURRENT_WAYPOINT, // MISSION_CURRENT MSG_GPS_RAW, MSG_GPS_RTK, +#if GPS_MAX_RECEIVERS > 1 MSG_GPS2_RAW, MSG_GPS2_RTK, +#endif MSG_NAV_CONTROLLER_OUTPUT, +#if AP_FENCE_ENABLED MSG_FENCE_STATUS, +#endif MSG_POSITION_TARGET_GLOBAL_INT, }; static const ap_message STREAM_POSITION_msgs[] = { @@ -525,7 +531,9 @@ static const ap_message STREAM_RC_CHANNELS_msgs[] = { }; static const ap_message STREAM_EXTRA1_msgs[] = { MSG_ATTITUDE, +#if AP_SIM_ENABLED MSG_SIMSTATE, +#endif MSG_AHRS2, MSG_PID_TUNING // Up to four PID_TUNING messages are sent, depending on GCS_PID_MASK parameter }; @@ -541,19 +549,33 @@ static const ap_message STREAM_EXTRA3_msgs[] = { #if AP_TERRAIN_AVAILABLE MSG_TERRAIN, #endif +#if AP_BATTERY_ENABLED MSG_BATTERY_STATUS, +#endif +#if HAL_MOUNT_ENABLED MSG_GIMBAL_DEVICE_ATTITUDE_STATUS, +#endif +#if AP_OPTICALFLOW_ENABLED MSG_OPTICAL_FLOW, +#endif +#if COMPASS_CAL_ENABLED MSG_MAG_CAL_REPORT, MSG_MAG_CAL_PROGRESS, +#endif MSG_EKF_STATUS_REPORT, MSG_VIBRATION, #if AP_RPM_ENABLED MSG_RPM, #endif +#if HAL_WITH_ESC_TELEM MSG_ESC_TELEMETRY, +#endif +#if HAL_GENERATOR_ENABLED MSG_GENERATOR_STATUS, +#endif +#if AP_WINCH_ENABLED MSG_WINCH_STATUS, +#endif #if HAL_EFI_ENABLED MSG_EFI_STATUS, #endif @@ -563,7 +585,9 @@ static const ap_message STREAM_PARAMS_msgs[] = { }; static const ap_message STREAM_ADSB_msgs[] = { MSG_ADSB_VEHICLE, +#if AP_AIS_ENABLED MSG_AIS_VESSEL, +#endif }; const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = { @@ -579,6 +603,14 @@ const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = { MAV_STREAM_TERMINATOR // must have this at end of stream_entries }; +MISSION_STATE GCS_MAVLINK_Copter::mission_state(const class AP_Mission &mission) const +{ + if (copter.mode_auto.paused()) { + return MISSION_STATE_PAUSED; + } + return GCS_MAVLINK::mission_state(mission); +} + bool GCS_MAVLINK_Copter::handle_guided_request(AP_Mission::Mission_Command &cmd) { #if MODE_AUTO_ENABLED == ENABLED @@ -640,14 +672,14 @@ void GCS_MAVLINK_Copter::handle_command_ack(const mavlink_message_t &msg) */ void GCS_MAVLINK_Copter::handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) { -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED copter.precland.handle_msg(packet, timestamp_ms); #endif } -MAV_RESULT GCS_MAVLINK_Copter::_handle_command_preflight_calibration(const mavlink_command_long_t &packet, const mavlink_message_t &msg) +MAV_RESULT GCS_MAVLINK_Copter::_handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { - if (is_equal(packet.param6,1.0f)) { + if (packet.y == 1) { // compassmot calibration return copter.mavlink_compassmot(*this); } @@ -665,7 +697,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_do_set_roi(const Location &roi_loc return MAV_RESULT_ACCEPTED; } -MAV_RESULT GCS_MAVLINK_Copter::handle_preflight_reboot(const mavlink_command_long_t &packet, const mavlink_message_t &msg) +MAV_RESULT GCS_MAVLINK_Copter::handle_preflight_reboot(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { // reject reboot if user has also specified they want the "Auto" ESC calibration on next reboot if (copter.g.esc_calibrate == (uint8_t)Copter::ESCCalibrationModes::ESCCAL_AUTO) { @@ -728,18 +760,25 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_do_reposition(const mavlink_co #endif } -MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_int_t &packet) +MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { switch(packet.command) { - case MAV_CMD_DO_FOLLOW: + + case MAV_CMD_CONDITION_YAW: + return handle_MAV_CMD_CONDITION_YAW(packet); + + case MAV_CMD_DO_CHANGE_SPEED: + return handle_MAV_CMD_DO_CHANGE_SPEED(packet); + #if MODE_FOLLOW_ENABLED == ENABLED + case MAV_CMD_DO_FOLLOW: // param1: sysid of target to follow if ((packet.param1 > 0) && (packet.param1 <= 255)) { copter.g2.follow.set_target_sysid((uint8_t)packet.param1); return MAV_RESULT_ACCEPTED; } + return MAV_RESULT_DENIED; #endif - return MAV_RESULT_UNSUPPORTED; case MAV_CMD_DO_REPOSITION: return handle_command_int_do_reposition(packet); @@ -748,18 +787,79 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i case MAV_CMD_DO_PAUSE_CONTINUE: return handle_command_pause_continue(packet); + case MAV_CMD_DO_MOTOR_TEST: + return handle_MAV_CMD_DO_MOTOR_TEST(packet); + + case MAV_CMD_NAV_TAKEOFF: + case MAV_CMD_NAV_VTOL_TAKEOFF: + return handle_MAV_CMD_NAV_TAKEOFF(packet); + +#if PARACHUTE == ENABLED + case MAV_CMD_DO_PARACHUTE: + return handle_MAV_CMD_DO_PARACHUTE(packet); +#endif + +#if AC_MAVLINK_SOLO_BUTTON_COMMAND_HANDLING_ENABLED + // Solo user presses pause button + case MAV_CMD_SOLO_BTN_PAUSE_CLICK: + return handle_MAV_CMD_SOLO_BTN_PAUSE_CLICK(packet); + // Solo user presses Fly button: + case MAV_CMD_SOLO_BTN_FLY_HOLD: + return handle_MAV_CMD_SOLO_BTN_FLY_HOLD(packet); + // Solo user holds down Fly button for a couple of seconds + case MAV_CMD_SOLO_BTN_FLY_CLICK: + return handle_MAV_CMD_SOLO_BTN_FLY_CLICK(packet); +#endif + +#if MODE_AUTO_ENABLED == ENABLED + case MAV_CMD_MISSION_START: + return handle_MAV_CMD_MISSION_START(packet); +#endif + +#if AP_WINCH_ENABLED + case MAV_CMD_DO_WINCH: + return handle_MAV_CMD_DO_WINCH(packet); +#endif + + case MAV_CMD_NAV_LOITER_UNLIM: + if (!copter.set_mode(Mode::Number::LOITER, ModeReason::GCS_COMMAND)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; + + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + if (!copter.set_mode(Mode::Number::RTL, ModeReason::GCS_COMMAND)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; + + case MAV_CMD_NAV_VTOL_LAND: + case MAV_CMD_NAV_LAND: + if (!copter.set_mode(Mode::Number::LAND, ModeReason::GCS_COMMAND)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; + +#if MODE_AUTO_ENABLED == ENABLED + case MAV_CMD_DO_LAND_START: + if (copter.mode_auto.jump_to_landing_sequence_auto_RTL(ModeReason::GCS_COMMAND)) { + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; +#endif + default: - return GCS_MAVLINK::handle_command_int_packet(packet); + return GCS_MAVLINK::handle_command_int_packet(packet, msg); } } #if HAL_MOUNT_ENABLED -MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_long_t &packet) +MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { switch (packet.command) { case MAV_CMD_DO_MOUNT_CONTROL: // if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead - if ((copter.camera_mount.get_mount_type() != copter.camera_mount.MountType::Mount_Type_None) && + if ((copter.camera_mount.get_mount_type() != AP_Mount::Type::None) && !copter.camera_mount.has_pan_control()) { copter.flightmode->auto_yaw.set_yaw_angle_rate((float)packet.param3, 0.0f); } @@ -767,68 +867,45 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_long_t default: break; } - return GCS_MAVLINK::handle_command_mount(packet); + return GCS_MAVLINK::handle_command_mount(packet, msg); } #endif -MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_long_t &packet) +MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_NAV_TAKEOFF(const mavlink_command_int_t &packet) { - switch(packet.command) { + if (packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT) { + return MAV_RESULT_DENIED; // meaning some parameters are bad + } - case MAV_CMD_NAV_VTOL_TAKEOFF: - case MAV_CMD_NAV_TAKEOFF: { // param3 : horizontal navigation by pilot acceptable // param4 : yaw angle (not supported) // param5 : latitude (not supported) // param6 : longitude (not supported) // param7 : altitude [metres] - float takeoff_alt = packet.param7 * 100; // Convert m to cm + float takeoff_alt = packet.z * 100; // Convert m to cm if (!copter.flightmode->do_user_takeoff(takeoff_alt, is_zero(packet.param3))) { return MAV_RESULT_FAILED; } return MAV_RESULT_ACCEPTED; - } +} -#if MODE_AUTO_ENABLED == ENABLED - case MAV_CMD_DO_LAND_START: - if (copter.mode_auto.jump_to_landing_sequence_auto_RTL(ModeReason::GCS_COMMAND)) { - return MAV_RESULT_ACCEPTED; - } - return MAV_RESULT_FAILED; +#if AP_MAVLINK_COMMAND_LONG_ENABLED +bool GCS_MAVLINK_Copter::mav_frame_for_command_long(MAV_FRAME &frame, MAV_CMD packet_command) const +{ + if (packet_command == MAV_CMD_NAV_TAKEOFF || + packet_command == MAV_CMD_NAV_VTOL_TAKEOFF) { + frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + return true; + } + return GCS_MAVLINK::mav_frame_for_command_long(frame, packet_command); +} #endif - case MAV_CMD_NAV_LOITER_UNLIM: - if (!copter.set_mode(Mode::Number::LOITER, ModeReason::GCS_COMMAND)) { - return MAV_RESULT_FAILED; - } - return MAV_RESULT_ACCEPTED; - - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - if (!copter.set_mode(Mode::Number::RTL, ModeReason::GCS_COMMAND)) { - return MAV_RESULT_FAILED; - } - return MAV_RESULT_ACCEPTED; - - case MAV_CMD_NAV_VTOL_LAND: - case MAV_CMD_NAV_LAND: - if (!copter.set_mode(Mode::Number::LAND, ModeReason::GCS_COMMAND)) { - return MAV_RESULT_FAILED; - } - return MAV_RESULT_ACCEPTED; -#if MODE_FOLLOW_ENABLED == ENABLED - case MAV_CMD_DO_FOLLOW: - // param1: sysid of target to follow - if ((packet.param1 > 0) && (packet.param1 <= 255)) { - copter.g2.follow.set_target_sysid((uint8_t)packet.param1); - return MAV_RESULT_ACCEPTED; - } - return MAV_RESULT_FAILED; -#endif - - case MAV_CMD_CONDITION_YAW: +MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_CONDITION_YAW(const mavlink_command_int_t &packet) +{ // param1 : target angle [0-360] // param2 : speed during change [deg per second] // param3 : direction (-1:ccw, +1:cw) @@ -844,8 +921,10 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; +} - case MAV_CMD_DO_CHANGE_SPEED: +MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_CHANGE_SPEED(const mavlink_command_int_t &packet) +{ // param1 : Speed type (0 or 1=Ground Speed, 2=Climb Speed, 3=Descent Speed) // param2 : new speed in m/s // param3 : unused @@ -869,9 +948,11 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ } } return MAV_RESULT_FAILED; +} #if MODE_AUTO_ENABLED == ENABLED - case MAV_CMD_MISSION_START: +MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_MISSION_START(const mavlink_command_int_t &packet) +{ if (copter.set_mode(Mode::Number::AUTO, ModeReason::GCS_COMMAND)) { copter.set_auto_armed(true); if (copter.mode_auto.mission.state() != AP_Mission::MISSION_RUNNING) { @@ -880,10 +961,14 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; +} #endif + + #if PARACHUTE == ENABLED - case MAV_CMD_DO_PARACHUTE: +MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_PARACHUTE(const mavlink_command_int_t &packet) +{ // configure or release parachute switch ((uint16_t)packet.param1) { case PARACHUTE_DISABLE: @@ -898,9 +983,11 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; +} #endif - case MAV_CMD_DO_MOTOR_TEST: +MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command_int_t &packet) +{ // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle) // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum) // param3 : throttle (range depends upon param2) @@ -912,10 +999,12 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ (uint8_t)packet.param2, packet.param3, packet.param4, - (uint8_t)packet.param5); + (uint8_t)packet.x); +} #if AP_WINCH_ENABLED - case MAV_CMD_DO_WINCH: +MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_WINCH(const mavlink_command_int_t &packet) +{ // param1 : winch number (ignored) // param2 : action (0=relax, 1=relative length control, 2=rate control). See WINCH_ACTIONS enum. if (!copter.g2.winch.enabled()) { @@ -936,27 +1025,12 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ break; } return MAV_RESULT_FAILED; -#endif - -#if AP_LANDINGGEAR_ENABLED - case MAV_CMD_AIRFRAME_CONFIGURATION: { - // Param 1: Select which gear, not used in ArduPilot - // Param 2: 0 = Deploy, 1 = Retract - // For safety, anything other than 1 will deploy - switch ((uint8_t)packet.param2) { - case 1: - copter.landinggear.set_position(AP_LandingGear::LandingGear_Retract); - return MAV_RESULT_ACCEPTED; - default: - copter.landinggear.set_position(AP_LandingGear::LandingGear_Deploy); - return MAV_RESULT_ACCEPTED; - } - return MAV_RESULT_FAILED; - } -#endif +} +#endif // AP_WINCH_ENABLED - /* Solo user presses Fly button */ - case MAV_CMD_SOLO_BTN_FLY_CLICK: { +#if AC_MAVLINK_SOLO_BUTTON_COMMAND_HANDLING_ENABLED +MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_SOLO_BTN_FLY_CLICK(const mavlink_command_int_t &packet) +{ if (copter.failsafe.radio) { return MAV_RESULT_ACCEPTED; } @@ -966,10 +1040,10 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::GCS_COMMAND); } return MAV_RESULT_ACCEPTED; - } +} - /* Solo user holds down Fly button for a couple of seconds */ - case MAV_CMD_SOLO_BTN_FLY_HOLD: { +MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_SOLO_BTN_FLY_HOLD(const mavlink_command_int_t &packet) +{ if (copter.failsafe.radio) { return MAV_RESULT_ACCEPTED; } @@ -987,10 +1061,10 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ copter.set_mode(Mode::Number::LAND, ModeReason::GCS_COMMAND); } return MAV_RESULT_ACCEPTED; - } +} - /* Solo user presses pause button */ - case MAV_CMD_SOLO_BTN_PAUSE_CLICK: { +MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_SOLO_BTN_PAUSE_CLICK(const mavlink_command_int_t &packet) +{ if (copter.failsafe.radio) { return MAV_RESULT_ACCEPTED; } @@ -1020,18 +1094,8 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ } } return MAV_RESULT_ACCEPTED; - } - - // pause or resume an auto mission - case MAV_CMD_DO_PAUSE_CONTINUE: { - mavlink_command_int_t packet_int; - GCS_MAVLINK_Copter::convert_COMMAND_LONG_to_COMMAND_INT(packet, packet_int); - return handle_command_pause_continue(packet_int); - } - default: - return GCS_MAVLINK::handle_command_long_packet(packet); - } } +#endif // AC_MAVLINK_SOLO_BUTTON_COMMAND_HANDLING_ENABLED MAV_RESULT GCS_MAVLINK_Copter::handle_command_pause_continue(const mavlink_command_int_t &packet) { @@ -1061,7 +1125,7 @@ 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() != copter.camera_mount.MountType::Mount_Type_None) && + if ((copter.camera_mount.get_mount_type() != AP_Mount::Type::None) && !copter.camera_mount.has_pan_control()) { copter.flightmode->auto_yaw.set_yaw_angle_rate( mavlink_msg_mount_control_get_input_c(&msg) * 0.01f, @@ -1089,6 +1153,20 @@ void GCS_MAVLINK_Copter::handle_manual_control_axes(const mavlink_manual_control manual_override(copter.channel_yaw, packet.r, 1000, 2000, tnow); } +// sanity check velocity or acceleration vector components are numbers +// (e.g. not NaN) and below 1000. vec argument units are in meters/second or +// metres/second/second +bool GCS_MAVLINK_Copter::sane_vel_or_acc_vector(const Vector3f &vec) const +{ + for (uint8_t i=0; i<3; i++) { + // consider velocity invalid if any component nan or >1000(m/s or m/s/s) + if (isnan(vec[i]) || fabsf(vec[i]) > 1000) { + return false; + } + } + return true; +} + void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) { #if MODE_GUIDED_ENABLED == ENABLED @@ -1249,8 +1327,13 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) // prepare velocity Vector3f vel_vector; if (!vel_ignore) { - // convert to cm - vel_vector = Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f); + vel_vector = Vector3f{packet.vx, packet.vy, -packet.vz}; + if (!sane_vel_or_acc_vector(vel_vector)) { + // input is not valid so stop + copter.mode_guided.init(true); + return; + } + vel_vector *= 100; // m/s -> cm/s // rotate to body-frame if necessary if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { copter.rotate_body_frame_to_NE(vel_vector.x, vel_vector.y); @@ -1345,8 +1428,13 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) // prepare velocity Vector3f vel_vector; if (!vel_ignore) { - // convert to cm - vel_vector = Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f); + vel_vector = Vector3f{packet.vx, packet.vy, -packet.vz}; + if (!sane_vel_or_acc_vector(vel_vector)) { + // input is not valid so stop + copter.mode_guided.init(true); + return; + } + vel_vector *= 100; // m/s -> cm/s } // prepare acceleration @@ -1424,7 +1512,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) } // end handle mavlink -MAV_RESULT GCS_MAVLINK_Copter::handle_flight_termination(const mavlink_command_long_t &packet) { +MAV_RESULT GCS_MAVLINK_Copter::handle_flight_termination(const mavlink_command_int_t &packet) { #if ADVANCED_FAILSAFE == ENABLED if (GCS_MAVLINK::handle_flight_termination(packet) == MAV_RESULT_ACCEPTED) { return MAV_RESULT_ACCEPTED; diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index 2c0828aae4765..d042d3000ebe0 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -1,6 +1,11 @@ #pragma once #include +#include + +#ifndef AC_MAVLINK_SOLO_BUTTON_COMMAND_HANDLING_ENABLED +#define AC_MAVLINK_SOLO_BUTTON_COMMAND_HANDLING_ENABLED 1 +#endif class GCS_MAVLINK_Copter : public GCS_MAVLINK { @@ -13,7 +18,7 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK uint32_t telem_delay() const override; - MAV_RESULT handle_flight_termination(const mavlink_command_long_t &packet) override; + MAV_RESULT handle_flight_termination(const mavlink_command_int_t &packet) override; uint8_t sysid_my_gcs() const override; bool sysid_enforce() const override; @@ -21,19 +26,18 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK bool params_ready() const override; void send_banner() override; - MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet, const mavlink_message_t &msg) override; + MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; void send_attitude_target() override; void send_position_target_global_int() override; void send_position_target_local_ned() override; MAV_RESULT handle_command_do_set_roi(const Location &roi_loc) override; - MAV_RESULT handle_preflight_reboot(const mavlink_command_long_t &packet, const mavlink_message_t &msg) override; + MAV_RESULT handle_preflight_reboot(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; #if HAL_MOUNT_ENABLED - MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet) override; + MAV_RESULT handle_command_mount(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; #endif - MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet) override; - MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override; + 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); MAV_RESULT handle_command_pause_continue(const mavlink_command_int_t &packet); @@ -55,6 +59,13 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK private: + // sanity check velocity or acceleration vector components are numbers + // (e.g. not NaN) and below 1000. vec argument units are in meters/second or + // metres/second/second + bool sane_vel_or_acc_vector(const Vector3f &vec) const; + + MISSION_STATE mission_state(const class AP_Mission &mission) const override; + void handleMessage(const mavlink_message_t &msg) override; void handle_command_ack(const mavlink_message_t &msg) override; bool handle_guided_request(AP_Mission::Mission_Command &cmd) override; @@ -72,7 +83,9 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK void send_pid_tuning() override; +#if AP_WINCH_ENABLED void send_winch_status() const override; +#endif void send_wind() const; @@ -84,4 +97,28 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK uint8_t high_latency_wind_speed() const override; uint8_t high_latency_wind_direction() const override; #endif // HAL_HIGH_LATENCY2_ENABLED + + + MAV_RESULT handle_MAV_CMD_CONDITION_YAW(const mavlink_command_int_t &packet); + MAV_RESULT handle_MAV_CMD_DO_CHANGE_SPEED(const mavlink_command_int_t &packet); + MAV_RESULT handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command_int_t &packet); + MAV_RESULT handle_MAV_CMD_DO_PARACHUTE(const mavlink_command_int_t &packet); + +#if AC_MAVLINK_SOLO_BUTTON_COMMAND_HANDLING_ENABLED + MAV_RESULT handle_MAV_CMD_SOLO_BTN_FLY_CLICK(const mavlink_command_int_t &packet); + MAV_RESULT handle_MAV_CMD_SOLO_BTN_FLY_HOLD(const mavlink_command_int_t &packet); + MAV_RESULT handle_MAV_CMD_SOLO_BTN_PAUSE_CLICK(const mavlink_command_int_t &packet); +#endif + +#if AP_MAVLINK_COMMAND_LONG_ENABLED + bool mav_frame_for_command_long(MAV_FRAME &frame, MAV_CMD packet_command) const override; +#endif + + MAV_RESULT handle_MAV_CMD_MISSION_START(const mavlink_command_int_t &packet); + MAV_RESULT handle_MAV_CMD_NAV_TAKEOFF(const mavlink_command_int_t &packet); + +#if AP_WINCH_ENABLED + MAV_RESULT handle_MAV_CMD_DO_WINCH(const mavlink_command_int_t &packet); +#endif + }; diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index ba352f2329edd..80a79da030a7c 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -46,6 +46,7 @@ const AP_Param::Info Copter::var_info[] = { // @DisplayName: My ground station number // @Description: Allows restricting radio overrides to only come from my ground station // @Range: 1 255 + // @Increment: 1 // @User: Advanced GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), @@ -162,7 +163,7 @@ const AP_Param::Info Copter::var_info[] = { // @Param: FS_GCS_ENABLE // @DisplayName: Ground Station Failsafe Enable // @Description: Controls whether failsafe will be invoked (and what action to take) when connection with Ground station is lost for at least 5 seconds. See FS_OPTIONS param for additional actions, or for cases allowing Mission continuation, when GCS failsafe is enabled. - // @Values: 0:Disabled/NoAction,1:RTL,2:RTL or Continue with Mission in Auto Mode (Removed in 4.0+-see FS_OPTIONS),3:SmartRTL or RTL,4:SmartRTL or Land,5:Land,6:Auto DO_LAND_START or RTL + // @Values: 0:Disabled/NoAction,1:RTL,2:RTL or Continue with Mission in Auto Mode (Removed in 4.0+-see FS_OPTIONS),3:SmartRTL or RTL,4:SmartRTL or Land,5:Land,6:Auto DO_LAND_START or RTL,7:Brake or Land // @User: Standard GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_DISABLED), @@ -449,9 +450,11 @@ const AP_Param::Info Copter::var_info[] = { GOBJECT(camera, "CAM", AP_Camera), #endif +#if AP_RELAY_ENABLED // @Group: RELAY_ // @Path: ../libraries/AP_Relay/AP_Relay.cpp GOBJECT(relay, "RELAY_", AP_Relay), +#endif #if PARACHUTE == ENABLED // @Group: CHUTE_ @@ -495,11 +498,7 @@ const AP_Param::Info Copter::var_info[] = { // @Group: ATC_ // @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp -#if FRAME_CONFIG == HELI_FRAME - GOBJECTPTR(attitude_control, "ATC_", AC_AttitudeControl_Heli), -#else - GOBJECTPTR(attitude_control, "ATC_", AC_AttitudeControl_Multi), -#endif + GOBJECTVARPTR(attitude_control, "ATC_", &copter.attitude_control_var_info), // @Group: PSC // @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -664,7 +663,7 @@ const AP_Param::Info Copter::var_info[] = { GOBJECT(optflow, "FLOW", AP_OpticalFlow), #endif -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED // @Group: PLND_ // @Path: ../libraries/AC_PrecLand/AC_PrecLand.cpp GOBJECT(precland, "PLND_", AC_PrecLand), @@ -776,7 +775,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Advanced AP_GROUPINFO("DEV_OPTIONS", 7, ParametersG2, dev_options, 0), -#if BEACON_ENABLED == ENABLED +#if AP_BEACON_ENABLED // @Group: BCN // @Path: ../libraries/AP_Beacon/AP_Beacon.cpp AP_SUBGROUPINFO(beacon, "BCN", 14, ParametersG2, AP_Beacon), @@ -836,9 +835,11 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // 18 was used by AP_VisualOdom +#if AP_TEMPCALIBRATION_ENABLED // @Group: TCAL // @Path: ../libraries/AP_TempCalibration/AP_TempCalibration.cpp AP_SUBGROUPINFO(temp_calibration, "TCAL", 19, ParametersG2, AP_TempCalibration), +#endif #if TOY_MODE_ENABLED == ENABLED // @Group: TMODE @@ -890,7 +891,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_SUBGROUPINFO(follow, "FOLL", 27, ParametersG2, AP_Follow), #endif -#ifdef USER_PARAMS_ENABLED +#if USER_PARAMS_ENABLED == ENABLED AP_SUBGROUPINFO(user_parameters, "USR", 28, ParametersG2, UserParameters), #endif @@ -1038,6 +1039,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Description: set which surface to track in surface tracking // @Values: 0:Do not track, 1:Ground, 2:Ceiling // @User: Advanced + // @RebootRequired: True AP_GROUPINFO("SURFTRAK_MODE", 51, ParametersG2, surftrak_mode, (uint8_t)Copter::SurfaceTracking::Surface::GROUND), // @Param: FS_DR_ENABLE @@ -1208,6 +1210,22 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = { // @User: Advanced AP_GROUPINFO("SURFTRAK_TC", 5, ParametersG2, surftrak_tc, 1.0), + // @Param: TKOFF_THR_MAX + // @DisplayName: Takeoff maximum throttle during take-off ramp up + // @Description: Takeoff maximum throttle allowed before controllers assume the aircraft is airborne during the takeoff process. + // @Range: 0.0 0.9 + // @User: Advanced + AP_GROUPINFO("TKOFF_THR_MAX", 6, ParametersG2, takeoff_throttle_max, 0.9), + +#if HAL_WITH_ESC_TELEM && FRAME_CONFIG != HELI_FRAME + // @Param: TKOFF_RPM_MAX + // @DisplayName: Takeoff Check RPM maximum + // @Description: Takeoff is not permitted until motors report no more than this RPM. Set to zero to disable check + // @Range: 0 10000 + // @User: Standard + AP_GROUPINFO("TKOFF_RPM_MAX", 7, ParametersG2, takeoff_rpm_max, 0), +#endif + // ID 62 is reserved for the AP_SUBGROUPEXTENSION AP_GROUPEND @@ -1217,8 +1235,11 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = { constructor for g2 object */ ParametersG2::ParametersG2(void) - : temp_calibration() // this doesn't actually need constructing, but removing it here is problematic syntax-wise -#if BEACON_ENABLED == ENABLED + : command_model_pilot(PILOT_Y_RATE_DEFAULT, PILOT_Y_EXPO_DEFAULT, 0.0f) +#if AP_TEMPCALIBRATION_ENABLED + , temp_calibration() +#endif +#if AP_BEACON_ENABLED , beacon() #endif #if HAL_PROXIMITY_ENABLED @@ -1236,7 +1257,7 @@ ParametersG2::ParametersG2(void) #if MODE_FOLLOW_ENABLED == ENABLED ,follow() #endif -#ifdef USER_PARAMS_ENABLED +#if USER_PARAMS_ENABLED == ENABLED ,user_parameters() #endif #if AUTOTUNE_ENABLED == ENABLED @@ -1263,8 +1284,6 @@ ParametersG2::ParametersG2(void) ,command_model_acro_y(ACRO_Y_RATE_DEFAULT, ACRO_Y_EXPO_DEFAULT, 0.0f) #endif - ,command_model_pilot(PILOT_Y_RATE_DEFAULT, PILOT_Y_EXPO_DEFAULT, 0.0f) - #if WEATHERVANE_ENABLED == ENABLED ,weathervane() #endif @@ -1287,12 +1306,6 @@ ParametersG2::ParametersG2(void) old object. This should be zero for top level parameters. */ const AP_Param::ConversionInfo conversion_table[] = { - // PARAMETER_CONVERSION - Added: Oct-2014 - { Parameters::k_param_log_bitmask_old, 0, AP_PARAM_INT16, "LOG_BITMASK" }, - // PARAMETER_CONVERSION - Added: Jan-2015 - { Parameters::k_param_serial0_baud, 0, AP_PARAM_INT16, "SERIAL0_BAUD" }, - { Parameters::k_param_serial1_baud, 0, AP_PARAM_INT16, "SERIAL1_BAUD" }, - { Parameters::k_param_serial2_baud, 0, AP_PARAM_INT16, "SERIAL2_BAUD" }, // PARAMETER_CONVERSION - Added: Jan-2017 { Parameters::k_param_arming_check_old, 0, AP_PARAM_INT8, "ARMING_CHECK" }, // battery @@ -1363,46 +1376,7 @@ void Copter::load_parameters(void) // handle conversion of PID gains void Copter::convert_pid_parameters(void) { - // conversion info - const AP_Param::ConversionInfo pid_conversion_info[] = { - // PARAMETER_CONVERSION - Added: Apr-2016 - { Parameters::k_param_pid_rate_roll, 0, AP_PARAM_FLOAT, "ATC_RAT_RLL_P" }, - { Parameters::k_param_pid_rate_roll, 1, AP_PARAM_FLOAT, "ATC_RAT_RLL_I" }, - { Parameters::k_param_pid_rate_roll, 2, AP_PARAM_FLOAT, "ATC_RAT_RLL_D" }, - { Parameters::k_param_pid_rate_pitch, 0, AP_PARAM_FLOAT, "ATC_RAT_PIT_P" }, - { Parameters::k_param_pid_rate_pitch, 1, AP_PARAM_FLOAT, "ATC_RAT_PIT_I" }, - { Parameters::k_param_pid_rate_pitch, 2, AP_PARAM_FLOAT, "ATC_RAT_PIT_D" }, - { Parameters::k_param_pid_rate_yaw, 0, AP_PARAM_FLOAT, "ATC_RAT_YAW_P" }, - { Parameters::k_param_pid_rate_yaw, 1, AP_PARAM_FLOAT, "ATC_RAT_YAW_I" }, - { Parameters::k_param_pid_rate_yaw, 2, AP_PARAM_FLOAT, "ATC_RAT_YAW_D" }, -#if FRAME_CONFIG == HELI_FRAME - // PARAMETER_CONVERSION - Added: May-2016 - { Parameters::k_param_pid_rate_roll, 4, AP_PARAM_FLOAT, "ATC_RAT_RLL_VFF" }, - { Parameters::k_param_pid_rate_pitch, 4, AP_PARAM_FLOAT, "ATC_RAT_PIT_VFF" }, - { Parameters::k_param_pid_rate_yaw , 4, AP_PARAM_FLOAT, "ATC_RAT_YAW_VFF" }, -#endif - }; - const AP_Param::ConversionInfo imax_conversion_info[] = { - // PARAMETER_CONVERSION - Added: Apr-2016 - { Parameters::k_param_pid_rate_roll, 5, AP_PARAM_FLOAT, "ATC_RAT_RLL_IMAX" }, - { Parameters::k_param_pid_rate_pitch, 5, AP_PARAM_FLOAT, "ATC_RAT_PIT_IMAX" }, - { Parameters::k_param_pid_rate_yaw, 5, AP_PARAM_FLOAT, "ATC_RAT_YAW_IMAX" }, -#if FRAME_CONFIG == HELI_FRAME - // PARAMETER_CONVERSION - Added: May-2016 - { Parameters::k_param_pid_rate_roll, 7, AP_PARAM_FLOAT, "ATC_RAT_RLL_ILMI" }, - { Parameters::k_param_pid_rate_pitch, 7, AP_PARAM_FLOAT, "ATC_RAT_PIT_ILMI" }, - { Parameters::k_param_pid_rate_yaw, 7, AP_PARAM_FLOAT, "ATC_RAT_YAW_ILMI" }, -#endif - }; - // conversion from Copter-3.3 to Copter-3.4 const AP_Param::ConversionInfo angle_and_filt_conversion_info[] = { - // PARAMETER_CONVERSION - Added: May-2016 - { Parameters::k_param_p_stabilize_roll, 0, AP_PARAM_FLOAT, "ATC_ANG_RLL_P" }, - { Parameters::k_param_p_stabilize_pitch, 0, AP_PARAM_FLOAT, "ATC_ANG_PIT_P" }, - { Parameters::k_param_p_stabilize_yaw, 0, AP_PARAM_FLOAT, "ATC_ANG_YAW_P" }, - // PARAMETER_CONVERSION - Added: Apr-2016 - { Parameters::k_param_pid_rate_roll, 6, AP_PARAM_FLOAT, "ATC_RAT_RLL_FILT" }, - { Parameters::k_param_pid_rate_pitch, 6, AP_PARAM_FLOAT, "ATC_RAT_PIT_FILT" }, // PARAMETER_CONVERSION - Added: Jan-2018 { Parameters::k_param_pid_rate_yaw, 6, AP_PARAM_FLOAT, "ATC_RAT_YAW_FILT" }, { Parameters::k_param_pi_vel_xy, 0, AP_PARAM_FLOAT, "PSC_VELXY_P" }, @@ -1422,11 +1396,6 @@ void Copter::convert_pid_parameters(void) { Parameters::k_param_p_alt_hold, 0, AP_PARAM_FLOAT, "PSC_POSZ_P" }, { Parameters::k_param_p_pos_xy, 0, AP_PARAM_FLOAT, "PSC_POSXY_P" }, }; - const AP_Param::ConversionInfo throttle_conversion_info[] = { - // PARAMETER_CONVERSION - Added: Jun-2016 - { Parameters::k_param_throttle_min, 0, AP_PARAM_FLOAT, "MOT_SPIN_MIN" }, - { Parameters::k_param_throttle_mid, 0, AP_PARAM_FLOAT, "MOT_THST_HOVER" } - }; const AP_Param::ConversionInfo loiter_conversion_info[] = { // PARAMETER_CONVERSION - Added: Apr-2018 { Parameters::k_param_wp_nav, 4, AP_PARAM_FLOAT, "LOIT_SPEED" }, @@ -1435,34 +1404,10 @@ void Copter::convert_pid_parameters(void) { Parameters::k_param_wp_nav, 9, AP_PARAM_FLOAT, "LOIT_BRK_ACCEL" } }; - // PARAMETER_CONVERSION - Added: Apr-2016 - // gains increase by 27% due to attitude controller's switch to use radians instead of centi-degrees - // and motor libraries switch to accept inputs in -1 to +1 range instead of -4500 ~ +4500 - float pid_scaler = 1.27f; - -#if FRAME_CONFIG != HELI_FRAME - // Multicopter x-frame gains are 40% lower because -1 or +1 input to motors now results in maximum rotation - if (g.frame_type == AP_Motors::MOTOR_FRAME_TYPE_X || g.frame_type == AP_Motors::MOTOR_FRAME_TYPE_V || g.frame_type == AP_Motors::MOTOR_FRAME_TYPE_H) { - pid_scaler = 0.9f; - } -#endif - - // scale PID gains - for (const auto &info : pid_conversion_info) { - AP_Param::convert_old_parameter(&info, pid_scaler); - } - // reduce IMAX into -1 ~ +1 range - for (const auto &info : imax_conversion_info) { - AP_Param::convert_old_parameter(&info, 1.0f/4500.0f); - } // convert angle controller gain and filter without scaling for (const auto &info : angle_and_filt_conversion_info) { AP_Param::convert_old_parameter(&info, 1.0f); } - // convert throttle parameters (multicopter only) - for (const auto &info : throttle_conversion_info) { - AP_Param::convert_old_parameter(&info, 0.001f); - } // convert RC_FEEL_RP to ATC_INPUT_TC // PARAMETER_CONVERSION - Added: Mar-2018 const AP_Param::ConversionInfo rc_feel_rp_conversion_info = { Parameters::k_param_rc_feel_rp, 0, AP_PARAM_INT8, "ATC_INPUT_TC" }; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index dd6f6ddaa6da4..e59bf2238788b 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -522,10 +522,12 @@ class ParametersG2 { // ground effect compensation enable/disable AP_Int8 gndeffect_comp_enabled; +#if AP_TEMPCALIBRATION_ENABLED // temperature calibration handling AP_TempCalibration temp_calibration; +#endif -#if BEACON_ENABLED == ENABLED +#if AP_BEACON_ENABLED // beacon (non-GPS positioning) library AP_Beacon beacon; #endif @@ -589,7 +591,7 @@ class ParametersG2 { AP_Follow follow; #endif -#ifdef USER_PARAMS_ENABLED +#if USER_PARAMS_ENABLED == ENABLED // User custom parameters UserParameters user_parameters; #endif @@ -678,8 +680,10 @@ class ParametersG2 { // ramp time of throttle during take-off AP_Float takeoff_throttle_slew_time; + AP_Float takeoff_throttle_max; #if HAL_WITH_ESC_TELEM && FRAME_CONFIG != HELI_FRAME AP_Int16 takeoff_rpm_min; + AP_Int16 takeoff_rpm_max; #endif #if WEATHERVANE_ENABLED == ENABLED diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index afcd65754cad2..f400694b451b3 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -37,9 +37,14 @@ void RC_Channel_Copter::mode_switch_changed(modeswitch_pos_t new_pos) } } +bool RC_Channels_Copter::in_rc_failsafe() const +{ + return copter.failsafe.radio; +} + bool RC_Channels_Copter::has_valid_input() const { - if (copter.failsafe.radio) { + if (in_rc_failsafe()) { return false; } if (copter.failsafe.radio_counter != 0) { @@ -391,7 +396,7 @@ bool RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi break; case AUX_FUNC::PRECISION_LOITER: -#if PRECISION_LANDING == ENABLED && MODE_LOITER_ENABLED == ENABLED +#if AC_PRECLAND_ENABLED && MODE_LOITER_ENABLED == ENABLED switch (ch_flag) { case AuxSwitchPos::HIGH: copter.mode_loiter.set_precision_loiter_enabled(true); diff --git a/ArduCopter/RC_Channel.h b/ArduCopter/RC_Channel.h index 576135f91f438..6d9d9f9426801 100644 --- a/ArduCopter/RC_Channel.h +++ b/ArduCopter/RC_Channel.h @@ -31,6 +31,7 @@ class RC_Channels_Copter : public RC_Channels public: bool has_valid_input() const override; + bool in_rc_failsafe() const override; RC_Channel *get_arming_channel(void) const override; diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 5b5ae54095018..62b8ca786a0e6 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,5 +1,400 @@ ArduPilot Copter Release Notes: ------------------------------------------------------------------ +Copter 4.4.4 05-Dec-2023 +Changes from 4.4.3 +1) Autopilot related enhancement and fixes + - CubeOrange Sim-on-hardware compilation fix + - RADIX2HD supports external I2C compasses + - SpeedyBeeF405v4 support +2) Bug fixes + - DroneCAN battery monitor with cell monitor SoC reporting fix + - NTF_LED_TYPES parameter description fixed (was missing IS31FL3195) + - ProfiLED output fixed in both Notify and Scripting + - Scripting bug that could cause crash if parameters were added in flight + - STAT_BOOTCNT param fix (was not updating in some cases) + - Takeoff RPM check fixed where motors are attached to AUX channels + - don't query hobbywing DroneCAN ESC IDs while armed +------------------------------------------------------------------ +Copter 4.4.3 14-Nov-2023 +Changes from 4.4.3-beta1 +1) AP_GPS: correct uBlox M10 configuration on low flash boards +------------------------------------------------------------------ +Copter 4.4.3-beta1 07-Nov-2023 +Changes from 4.4.2 +1) Autopilot related enhancements and fixes + - BETAFTP-F405 board configuration fixes + - CubeOrangePlus-BG edition ICM45486 IMU setup fixed + - YJUAV_A6SE_H743 support +2) Minor enhancements + - GPS_DRV_OPTION allows using ellipsoid height in more GPS drivers + - Lua script support for fully populating ESC telemetry data +3) Bug fixes + - AK09916 compass being non-responsive fixed + - IxM42xxx IMUs "stuck" gyros fixed + - MAVLink response fixed when no airspeed sensor during preflight calibration + - Notch filtering protection when using uninitialised RPM source in ESC telemetry + - SIYI gimbal driver parsing bug fixed (was causing lost packets) +------------------------------------------------------------------ +Copter 4.4.2 22-Oct-2023 / Copter 4.4.2-beta1 13-Oct-2023 +Changes from 4.4.1 +1) Autopilot related enhancements and fixes + - BETAFPV-F405 support + - MambaF405v2 battery and serial setup corrected + - mRo Control Zero OEM H7 bdshot support + - SpeedyBee-F405-Wing gets VTX power control + - SpeedyBee-F405-Mini support + - T-Motor H743 Mini support +2) EKF3 supports baroless boards +3) GPS-for-yaw allows base GPS to update at only 3Hz +4) INA battery monitor supports config of shunt resistor used (see BATTx_SHUNT) +5) Log VER message includes vehicle type +6) OpenDroneId option to auto-store IDs in persistent flash +7) RC SBUS protection against invalid data in first 4 channels +8) Bug fixes + - BMI088 IMU error value handling fixed to avoid occasional negative spike + - Dev environment CI autotest stability improvements + - OSD correct DisplayPort BF MSP symbols + - OSD option to correct direction arrows for BF font set + - Sensor status reporting to GCS fixed for baroless boards +------------------------------------------------------------------ +Copter 4.4.1 26-Sep-2023 / 4.4.1-beta2 14-Sep-2023 +Changes from 4.4.1-beta1 +1) Autopilot related enhancements + - H750 external flash optimisations for to lower CPU load + - MambaF405Mini fixes to match manufacturer's recommended wiring + - RADIX2 HD support + - YJUAV_A6SE support +2) Bug fixes + - Airbotf4 features minimised to build for 4.4 + - ChibiOS clock fix for 480Mhz H7 boards (affected FDCAN) + - RPI hardware version check fix +------------------------------------------------------------------ +Copter 4.4.1-beta1 05-Sep-2023 +Changes from 4.4.0 +1) Autopilot related fixes and enhancements + - KakuteH7-wing get 8 bit directional dshot channel support + - Luminousbee5 boards defaults updated + - Navigator autopilot GPIOs fix (PWM output was broken) + - Pixhawk6C Serial RTS lines pulled low on startup + - QiotekZealotF427 and QiotekZealotH743 battery monitor default fixed + - SDMODELH7V1 support +2) Driver enhancements + - DroneCAN battery monitors allow reset of battery SoC + - Himark DroneCAN servo support + - Hobbywing DroneCAN ESC support +3) EKF3 high vibration handling improved with EK3_GLITCH_RADIUS option +4) Custom build server gets mission storage on SDCard selection +5) SITL default parameter handling bug fix +------------------------------------------------------------------ +Copter 4.4.0 18-Aug-2023 / 4.4.0-beta5 12-Aug-2023 +Changes from 4.4.0-beta4 +1) Autopilots specific changes + - SIYI N7 support +2) Bug fixes + - DroneCAN airspeed sensor fix to handle missing packets + - DroneCAN GPS RTK injection fix + - Notch filter gyro glitch caused by race condition fixed +------------------------------------------------------------------ +Copter 4.4.0-beta4 27-July-2023 +Changes from 4.4.0-beta3 +1) Autopilots specific changes + - Diatone-Mamba-MK4-H743v2 uses SPL06 baro (was DPS280) + - DMA for I2C disabled on F7 and H7 boards + - Foxeer H743v1 default serial protocol config fixes + - HeeWing-F405 and F405v2 support + - iFlight BlitzF7 support +2) Scripts may take action based on VTOL motor loss +3) Bug fixes + - BLHeli returns battery status requested via MSP (avoids hang when using esc-configurator) + - CRSFv3 rescans at baudrates to avoid RX loss + - EK3_ABIAS_P_NSE param range fix + - Scripting restart memory corruption bug fixed + - Siyi A8/ZR10 driver fix to avoid crash if serial port not configured +------------------------------------------------------------------ +Copter 4.4.0-beta3 03-July-2023 +Changes from 4.4.0-beta2 +1) Autopilots specific changes + - Holybro KakuteH7-Wing support + - JFB100 external watchdog GPIO support added + - Pixhawk1-bdshot support + - Pixhawk6X-bdshot support + - SpeedyBeeF4 loses bdshot support +2) Device drivers + - added LP5562 I2C LED driver + - added IS31FL3195 LED driver +3) TradHeli gets minor fix to RSC servo output range +4) Camera and Gimbal related changes + - DO_SET_ROI_NONE command support added +5) Applet changes + - added QUIK_MAX_REDUCE parameter to VTOL quicktune lua applet +6) Bug fixes + - ADSB sensor loss of transceiver message less spammy + - AutoTune Yaw rate max fixed + - EKF vertical velocity reset fixed on loss of GPS + - GPS pre-arm failure message clarified + - SERVOx_PROTOCOL "SToRM32 Gimbal Serial" value renamed to "Gimbal" because also used by Siyi + - SERIALx_OPTION "Swap" renamed to "SwapTXRX" for clarity + - SBF GPS ellipsoid height fixed + - Ublox M10S GPS auto configuration fixed + - ZigZag mode user takeoff fixed (users could not takeoff in ZigZag mode previously) +------------------------------------------------------------------ +Copter 4.4.0-beta2 05-Jun-2023 +Changes from 4.4.0-beta1 +1) Autopilots specific changes + - FlywooF745 update to motor pin output mapping and baro + - FoxeerH743 support + - JFB100 support + - Mamba-F405v2 supports ICM42688 + - Matek-F405-TE/VTOL support + - Matek-H743 IMU SPI slowed to 1Mhz to avoid init issues + - SpeedyBee-405-Wing support +2) Copter specfic fixes and enhancements + - RTL speed fix so changes to WPNAV_SPEED have no effect if RTL_SPEED is non-zero + - RTL mode accepts do-change-speed commands from GCS +3) AHRS/EKF related fixes and Enhancements + - EKF allocation failure handled to avoid watchdog + - EKF3 accel bias calculation fix and tuning for greater robustness + - Airspeed sensor remains enabled during dead-reckoning (few copters have airspeed sensors) + - Wind speed estimates updates reduced while dead-reckoning +4) Other Enhancements + - Attitude control slew limits always calculated (helps tuning reporting and analysis) + - INA228 and INA238 I2C battery monitor support + - LOG_DISARMED=3 logs while disarmed but discards log if never eventually armed + - LOG_DARM_RATEMAX reduces logging while disarmed + - Serial LEDs threading enhancement to support longer lengths without dshot interference +4) Bug fixes + - Analog battery monitor2 current parameter default fixed + - AutoTune fix for loading Yaw Rate D gains + - BRD_SAFETYOPTION parameter documentation fix (ActiveForSafetyEnable and Disable were reversed) + - Compassmot fix to protect against bad gyro biases from GSF yaw + - ICE engine fix for starting after reaching a specified altitude + - LED thread locking fix to avoid watchdog + - Logging rotation on disarm disabled if Replay logging active (avoids gaps in logs) + - RC input on IOMCU bug fix (RC might not be regained if lost) + - Serial passthrough fixed +5) Custom build server fix to which features are included/excluded +------------------------------------------------------------------ +Copter 4.4.0-beta1 19-Apr-2023 +Changes from 4.3.6 +1) New autopilots supported + - ESP32 + - Flywoo Goku F405S AIO + - Foxeer H743v1 + - MambaF405-2022B + - PixPilot-V3 + - PixSurveyA2 + - rFCU H743 + - ThePeach K1/R1 +2) Autopilot specific changes + - Bi-Directional DShot support for CubeOrangePlus-bdshot, CUAVNora+, MatekF405TE/VTOL-bdshot, MatekL431, Pixhawk6C-bdshot, QioTekZealotH743-bdshot + - Bi-Directional DShot up to 8 channels on MatekH743 + - BlueRobotics Navigator supports baro on I2C bus 6 + - BMP280 baro only for BeastF7, KakuteF4, KakuteF7Mini, MambaF405, MatekF405, Omnibusf4 to reduce code size (aka "flash") + - CSRF and Hott telemetry disabled by default on some low power boards (aka "minimised boards") + - Foxeer Reaper F745 supports external compasses + - OmnibusF4 support for BMI270 IMU + - OmnibusF7V2-bdshot support removed + - KakuteF7 regains displayport, frees up DMA from unused serial port + - KakuteH7v2 gets second battery sensor + - MambaH743v4 supports VTX + - MatekF405-Wing supports InvensenseV3 IMUs + - PixPilot-V6 heater enabled + - Raspberry 64OS startup crash fixed + - ReaperF745AIO serial protocol defaults fixed + - SkystarsH7HD (non-bdshot) removed as users should always use -bdshot version + - Skyviper loses many unnecessary features to save flash + - UBlox GPS only for AtomRCF405NAVI, BeastF7, MatekF405, Omnibusf4 to reduce code size (aka "flash") + - VRBrain-v52 and VRCore-v10 features reduced to save flash +3) Driver enhancements + - ARK RTK GPS support + - BMI088 IMU filtering and timing improved, ignores bad data + - CRSF OSD may display disarmed state after flight mode (enabled using RC_OPTIONS) + - Daiwa winch baud rate obeys SERIALx_BAUD parameter + - EFI supports fuel pressure and ignition voltage reporting and battery failsafe + - ICM45686 IMU support + - ICM20602 uses fast reset instead of full reset on bad temperature sample (avoids occasional very high offset) + - ICM45686 supports fast sampling + - MAX31865 temp sensor support + - MB85RS256TY-32k, PB85RS128C and PB85RS2MC FRAM support + - MMC3416 compass orientation fix + - MPPT battery monitor reliability improvements, enable/disable aux function and less spammy + - Multiple USD-D1-CAN radar support + - NMEA output rate configurable (see NMEA_RATE_MS) + - NMEA output supports PASHR message (see NMEA_MSG_EN) + - OSD supports average resting cell voltage (see OSD_ACRVOLT_xxx params) + - Rockblock satellite modem support + - Serial baud support for 2Mbps (only some hardware supports this speed) + - SF45b lidar filtering reduced (allows detecting smaller obstacles + - SmartAudio 2.0 learns all VTX power levels) + - UAVCAN ESCs report error count using ESC Telemetry + - Unicore GPS (e.g. UM982) support + - VectorNav 100 external AHRS support + - 5 IMUs supported +4) EKF related enhancements + - Baro compensation using wind estimates works when climbing or descending (see BAROx_WCF_UP/DN) + - External AHRS support for enabling only some sensors (e.g. IMU, Baro, Compass) see EAHRS_SENSORS + - Magnetic field tables updated + - Non-compass initial yaw alignment uses GPS course over GSF (mostly affects Planes and Rover) +5) Control and navigation enhancements + - AutoTune of attitude control yaw D gain (set AUTOTUNE_AXES=8) + - Circle moode and Loiter Turns command supports counter-clockwise rotation (set CIRCLE_RATE to negative number) + - DO_SET_ROI_NONE command turns off ROI + - JUMP_TAG mission item support + - Missions can be stored on SD card (see BRD_SD_MISSION) + - NAV_SCRIPT_TIME command accepts floating point arguments + - Pause/Resume returns success if mission is already paused or resumed + - Payload Place enhancements + - Descent speed is configurable (see PLDP_SPEED_DN) + - Manual release supported (detects pilot release of gripper) + - Post release delay is configurable (see PLDP_DELAY) + - Range finder range used to protect against premature release (see PLDP_RNG_MIN) + - Touchdown detection threshold is configurable (see PLDP_THRESH) + - Position controller angle max adjusted inflight with CH6 Tuning knob (set TUNE=59) + - Surface tracking time constant allows tuning response (see SURFTRAK_TC) + - Throttle-Gain boost increases attitude control gains when throttle high (see ATC_THR_G_BOOST) + - Waypoint navigation cornering acceleration is configurable (see WPNAV_ACCEL_C) + - WeatherVane into the wind in Auto and Guided modes (see WVANE_ENABLE) +6) TradHeli specific enhancements + - Manual autorotation support + - Improved collect to yaw compensation +7) Filtering enhancements + - FFT notch can be run based on filtered data + - Warn of motor noise at RPM frequency using FFT + - In-flight FFT can better track low frequency noise + - In-flight FFT logging improved + - IMU data can be read and replayed for FFT analysis +8) Camera and gimbal enhancements + - BMMCC support included in Servo driver + - DJI RS2/RS3-Pro gimbal support + - Dual camera support (see CAM2_TYPE) + - Gimbal/Mount2 can be moved to retracted or neutral position + - Gremsy ZIO support + - IMAGE_START_CAPTURE, SET_CAMERA_ZOOM/FOCUS, VIDEO_START/STOP_CAPTURE command support + - Paramters renamed and rescaled + - CAM_TRIGG_TYPE renamed to CAM1_TYPE and options have changed + - CAM_DURATION renamed to CAM1_DURATION and scaled in seconds + - CAM_FEEDBACK_PIN/POL renamed to CAM1_FEEBAK_PIN/POL + - CAM_MIN_INTERVAL renamed to CAM1_INTRVAL_MIN and scaled in seconds + - CAM_TRIGG_DIST renamed to CAMx_TRIGG_DIST and accepts fractional values + - RunCam2 4k support + - ViewPro camera gimbal support +9) Logging changes + - BARD msg includes 3-axis dynamic pressure useful for baro compensation of wind estimate + - MCU log msg includes main CPU temp and voltage (was part of POWR message) + - RCOut banner message always included in Logs + - SCR message includes memory usage of all running scripts + - CANS message includes CAN bus tx/rx statistics + - OFCA (optical flow calibration log message) units added + - Home location not logged to CMD message + - MOTB message includes throttle output +10) Scripting enhancements + - Copter deadreckoning upgraded to applet + - EFI Skypower driver gets improved telem messages and bug fixes + - Generator throttle control example added + - Heap max increased by allowing heap to be split across multiple underlying OS heaps + - Hexsoon LEDs applet + - Logging from scripts supports more formats + - Parameters can be removed or reordered + - Parameter description support (scripts must be in AP's applet or driver directory) + - Rangefinder driver support + - Runcam_on_arm applet starts recording when vehicle is armed + - Safety switch, E-Stop and motor interlock support + - Scripts can restart all scripts + - Script_Controller applet supports inflight switching of active scripts +11) Custom build server enhancements + - AIS support for displaying nearby boats can be included + - Battery, Camera and Compass drivers can be included/excluded + - EKF3 wind estimation can be included/excluded + - PCA9685, ToshibaLED, PLAY_TUNE notify drivers can be included/excluded + - Preclanding can be included/excluded + - RichenPower generator can be included/excluded + - RC SRXL protocol can be excluded + - SIRF GPSs can be included/excluded +12) Safety related enhancements and fixes + - Arming check for high throttle skipped when arming in Auto mode + - Arming check for servo outputs skipped when SERVOx_FUNCTION is scripting + - Arming check fix if both "All" and other bitmasks are selected (previously only ran the other checks) + - "EK3 sources require RangeFinder" pre-arm check fix when user only sets up 2nd rangefinder (e.g. 1st is disabled) + - Pre-arm check that low and critical battery failsafe thresholds are different + - Pre-arm message fixed if 2nd EKF core unhealthy + - Pre-arm check if reboot required to enabled IMU batch sampling (used for vibe analysis) + - RC failsafe (aka throttle failsafe) option to change to Brake mode + - RC failsafe timeout configurable (see RC_FS_TIMEOUT) + - Takeoff check of motor RPM (see TKOFF_RPM_MIN) + - Turtle mode warns user to raise throttle to arm +13) Minor enhancements + - Boot time reduced by improving parameter conversion efficiency + - BRD_SAFETYENABLE parameter renamed to BRD_SAFETY_DEFLT + - Compass calibration auxiliary switch function (set RCx_OPTION=171) + - Disable IMU3 auxiliary switch function (set RCx_OPTION=110) + - Frame type sent to GCS defaults to multicopter to ease first time setup + - Rangefinder and FS_OPTIONS param conversion code reduced (affects when upgrading from 3.6 or earlier) + - MAVFTP supports file renaming + - MAVLink in-progress reply to some requests for calibration from GCS +14) Bug fixes: + - ADSB telemetry and callsign fixes + - Battery pct reported to GCS limited to 0% to 100% range + - Bi-directional DShot fix on H7 boards after system time wrap (more complete fix than in 4.3.6) + - DisplayPort OSD screen reliability improvement on heavily loaded OSDs especially F4 boards + - DisplayPort OSD artificial horizon better matches actual horizon + - EFI Serial MS bug fix to avoid possible infinite loop + - EKF3 Replay fix when COMPASS_LEARN=3 + - ESC Telemetry external temp reporting fix + - Fence upload works even if Auto mode is excluded from firmware + - FMT messages logged even when Fence is exncluded from firmware (e.g. unselected when using custom build server) + - Guided mode slow yaw fix + - Hardfault avoided if user changes INS_LOG_BAT_CNT while batch sampling running + - ICM20649 temp sensor tolerate increased to avoid unnecessary FIFO reset + - IMU detection bug fix to avoid duplicates + - IMU temp cal fix when using auxiliary IMU + - Message Interval fix for restoring default rate https://github.com/ArduPilot/ardupilot/pull/21947 + - RADIO_STATUS messages slow-down feature never completely stops messages from being sent + - SERVOx_TRIM value output momentarily if SERVOx_FUNCTION is changed from Disabled to RCPassThru, RCIN1, etc. Avoids momentary divide-by-zero + - Scripting file system open fix + - Scripting PWM source deletion crash fix + - MAVFTP fix for low baudrates (4800 baud and lower) + - ModalAI VOXL reset handling fix + - MPU6500 IMU fast sampling rate to 4k (was 1K) + - NMEA GPGGA output fixed for GPS quality, num sats and hdop + - Position control reset avoided even with very uneven main loop rate due to high CPU load + - SingleCopter and CoaxCopter fix to fin centering when using DShot + - SystemID mode fix to write PID log messages + - Terrain offset increased from 15m to 30m (see TERRAIN_OFS_MAX)to reduce chance of "clamping" + - Throttle notch FFT tuning param fix + - VTX protects against pitmode changes when not enabled or vehicle disarmed +15) Developer specific items + - DroneCAN replaces UAVCAN + - FlighAxis simulator rangefinder fixed + - Scripts in applet and drivers directory checked using linter + - Simulator supports main loop timing jitter (see SIM_TIME_JITTER) + - Simulink model and init scripts + - SITL on hardware support (useful to demo servos moving in response to simulated flight) + - SITL parameter definitions added (some, not all) + - Webots 2023a simulator support + - XPlane support for wider range of aircraft +------------------------------------------------------------------ +Copter 4.3.8 24-Aug-2023 / 4.3.8-beta1 12-Aug-2023 +Changes from 4.3.7 +1) Bug fixes + - DroneCAN GPS RTK injection fix + - INAxxx battery monitors allow for battery reset remaining + - Notch filter gyro glitch caused by race condition fixed + - Scripting restart memory corruption bug fixed +------------------------------------------------------------------ +Copter 4.3.7 31-May-2023 / 4.3.7-beta1 24-May-2023 +Changes from 4.3.6 +1) Bug fixes + a) EKF3 accel bias calculations bug fix + b) EKF3 accel bias process noise adjusted for greater robustness + c) GSF yaw numerical stability fix caused by compassmot + d) INS batch sampler fix to avoid watchdog if INS_LOG_BAT_CNT changed without rebooting + e) Memory corruption bug in the STM32H757 (very rare) + f) RC input on IOMCU bug fix (RC might not be regained if lost) +------------------------------------------------------------------ +Copter 4.3.6 05-Apr-2023 / 4.3.6-beta1, 4.3.6--beta2 27-Mar-2023 +Changes from 4.3.5 +1) Bi-directional DShot fix for possible motor stop approx 72min after startup +------------------------------------------------------------------ Copter Copter 4.3.5 14-Mar-2023 / 4.3.5-rc1 01-Mar-2023 Changes from 4.3.4 1) Bug fixes diff --git a/ArduCopter/UserParameters.cpp b/ArduCopter/UserParameters.cpp index 37e61f8ab5eb2..61f1b7122b53e 100644 --- a/ArduCopter/UserParameters.cpp +++ b/ArduCopter/UserParameters.cpp @@ -1,5 +1,7 @@ #include "UserParameters.h" +#include "config.h" +#if USER_PARAMS_ENABLED == ENABLED // "USR" + 13 chars remaining for param name const AP_Param::GroupInfo UserParameters::var_info[] = { @@ -16,3 +18,4 @@ UserParameters::UserParameters() { AP_Param::setup_object_defaults(this, var_info); } +#endif // USER_PARAMS_ENABLED diff --git a/ArduCopter/afs_copter.cpp b/ArduCopter/afs_copter.cpp index 9f669eee8cd65..3bad987a343d3 100644 --- a/ArduCopter/afs_copter.cpp +++ b/ArduCopter/afs_copter.cpp @@ -68,4 +68,9 @@ AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Copter::afs_mode(void) return AP_AdvancedFailsafe::AFS_STABILIZED; } +//to force entering auto mode when datalink loss + void AP_AdvancedFailsafe_Copter::set_mode_auto(void) + { + copter.set_mode(Mode::Number::AUTO,ModeReason::GCS_FAILSAFE); + } #endif // ADVANCED_FAILSAFE diff --git a/ArduCopter/afs_copter.h b/ArduCopter/afs_copter.h index fd733519733c3..b7c8bc8b21c85 100644 --- a/ArduCopter/afs_copter.h +++ b/ArduCopter/afs_copter.h @@ -39,6 +39,9 @@ class AP_AdvancedFailsafe_Copter : public AP_AdvancedFailsafe // return the AFS mapped control mode enum control_mode afs_mode(void) override; + + //to force entering auto mode when datalink loss + void set_mode_auto(void) override; }; #endif // ADVANCED_FAILSAFE diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp index 6853789ee3b84..749a830f4dce6 100644 --- a/ArduCopter/autoyaw.cpp +++ b/ArduCopter/autoyaw.cpp @@ -92,7 +92,7 @@ void Mode::AutoYaw::set_mode(Mode yaw_mode) case Mode::RATE: // initialise target yaw rate to zero - _yaw_rate_cds = 0.0f; + _yaw_rate_cds = 0.0; break; case Mode::CIRCLE: @@ -108,18 +108,15 @@ void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_ds, int8_t di { _last_update_ms = millis(); - _yaw_angle_cd = copter.attitude_control->get_att_target_euler_cd().z; - _yaw_rate_cds = 0.0; - // calculate final angle as relative to vehicle heading or absolute if (relative_angle) { _fixed_yaw_offset_cd = angle_deg * 100.0 * (direction >= 0 ? 1.0 : -1.0); } else { // absolute angle _fixed_yaw_offset_cd = wrap_180_cd(angle_deg * 100.0 - _yaw_angle_cd); - if ( direction < 0 && is_positive(_fixed_yaw_offset_cd) ) { + if (direction < 0 && is_positive(_fixed_yaw_offset_cd)) { _fixed_yaw_offset_cd -= 36000.0; - } else if ( direction > 0 && is_negative(_fixed_yaw_offset_cd) ) { + } else if (direction > 0 && is_negative(_fixed_yaw_offset_cd)) { _fixed_yaw_offset_cd += 36000.0; } } @@ -157,9 +154,7 @@ void Mode::AutoYaw::set_roi(const Location &roi_location) auto_yaw.set_mode_to_default(false); #if HAL_MOUNT_ENABLED // switch off the camera tracking if enabled - if (copter.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { - copter.camera_mount.set_mode_to_default(); - } + copter.camera_mount.clear_roi_target(); #endif // HAL_MOUNT_ENABLED } else { #if HAL_MOUNT_ENABLED @@ -211,14 +206,15 @@ bool Mode::AutoYaw::reached_fixed_yaw_target() return (fabsf(wrap_180_cd(copter.ahrs.yaw_sensor-_yaw_angle_cd)) <= 200); } -// yaw - returns target heading depending upon auto_yaw.mode() -float Mode::AutoYaw::yaw() +// yaw_cd - returns target heading depending upon auto_yaw.mode() +float Mode::AutoYaw::yaw_cd() { switch (_mode) { case Mode::ROI: // point towards a location held in roi - return roi_yaw(); + _yaw_angle_cd = roi_yaw(); + break; case Mode::FIXED: { // keep heading pointing in the direction held in fixed_yaw @@ -229,45 +225,55 @@ float Mode::AutoYaw::yaw() float yaw_angle_step = constrain_float(_fixed_yaw_offset_cd, - dt * _fixed_yaw_slewrate_cds, dt * _fixed_yaw_slewrate_cds); _fixed_yaw_offset_cd -= yaw_angle_step; _yaw_angle_cd += yaw_angle_step; - return _yaw_angle_cd; + break; } case Mode::LOOK_AHEAD: // Commanded Yaw to automatically look ahead. - return look_ahead_yaw(); + _yaw_angle_cd = look_ahead_yaw(); + break; case Mode::RESETTOARMEDYAW: // changes yaw to be same as when quad was armed - return copter.initial_armed_bearing; + _yaw_angle_cd = copter.initial_armed_bearing; + break; case Mode::CIRCLE: #if MODE_CIRCLE_ENABLED if (copter.circle_nav->is_active()) { - return copter.circle_nav->get_yaw(); + _yaw_angle_cd = copter.circle_nav->get_yaw(); } #endif - // return the current attitude target - return wrap_360_cd(copter.attitude_control->get_att_target_euler_cd().z); + break; case Mode::ANGLE_RATE:{ const uint32_t now_ms = millis(); float dt = (now_ms - _last_update_ms) * 0.001; _last_update_ms = now_ms; _yaw_angle_cd += _yaw_rate_cds * dt; - return _yaw_angle_cd; + break; } + case Mode::RATE: + case Mode::WEATHERVANE: + case Mode::PILOT_RATE: + _yaw_angle_cd = copter.attitude_control->get_att_target_euler_cd().z; + break; + case Mode::LOOK_AT_NEXT_WP: default: // point towards next waypoint. // we don't use wp_bearing because we don't want the copter to turn too much during flight - return copter.pos_control->get_yaw_cd(); + _yaw_angle_cd = copter.pos_control->get_yaw_cd(); + break; } + + return _yaw_angle_cd; } // returns yaw rate normally set by SET_POSITION_TARGET mavlink // messages (positive is clockwise, negative is counter clockwise) -float Mode::AutoYaw::rate_cds() const +float Mode::AutoYaw::rate_cds() { switch (_mode) { @@ -277,22 +283,25 @@ float Mode::AutoYaw::rate_cds() const case Mode::LOOK_AHEAD: case Mode::RESETTOARMEDYAW: case Mode::CIRCLE: - return 0.0f; - - case Mode::ANGLE_RATE: - case Mode::RATE: - case Mode::WEATHERVANE: - return _yaw_rate_cds; + _yaw_rate_cds = 0.0f; + break; case Mode::LOOK_AT_NEXT_WP: - return copter.pos_control->get_yaw_rate_cds(); + _yaw_rate_cds = copter.pos_control->get_yaw_rate_cds(); + break; case Mode::PILOT_RATE: - return _pilot_yaw_rate_cds; + _yaw_rate_cds = _pilot_yaw_rate_cds; + break; + + case Mode::ANGLE_RATE: + case Mode::RATE: + case Mode::WEATHERVANE: + break; } // return zero turn rate (this should never happen) - return 0.0f; + return _yaw_rate_cds; } AC_AttitudeControl::HeadingCommand Mode::AutoYaw::get_heading() @@ -315,7 +324,7 @@ AC_AttitudeControl::HeadingCommand Mode::AutoYaw::get_heading() #endif AC_AttitudeControl::HeadingCommand heading; - heading.yaw_angle_cd = yaw(); + heading.yaw_angle_cd = auto_yaw.yaw_cd(); heading.yaw_rate_cds = auto_yaw.rate_cds(); switch (auto_yaw.mode()) { diff --git a/ArduCopter/compassmot.cpp b/ArduCopter/compassmot.cpp index 5e24bd2b407d9..532cbb243a181 100644 --- a/ArduCopter/compassmot.cpp +++ b/ArduCopter/compassmot.cpp @@ -123,7 +123,7 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan) EXPECT_DELAY_MS(5000); // enable motors and pass through throttle - enable_motor_output(); + motors->output_min(); // output lowest possible value to motors motors->armed(true); hal.util->set_soft_armed(true); diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 5a4704b14915d..329678e1b889c 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -27,6 +27,8 @@ /// change in your local copy of APM_Config.h. /// #include "APM_Config.h" +#include +#include ////////////////////////////////////////////////////////////////////////////// @@ -138,12 +140,6 @@ # define AUTOTUNE_ENABLED ENABLED #endif -////////////////////////////////////////////////////////////////////////////// -// Precision Landing with companion computer or IRLock sensor -#ifndef PRECISION_LANDING - # define PRECISION_LANDING ENABLED -#endif - ////////////////////////////////////////////////////////////////////////////// // Parachute release #ifndef PARACHUTE @@ -152,8 +148,8 @@ ////////////////////////////////////////////////////////////////////////////// // Nav-Guided - allows external nav computer to control vehicle -#ifndef NAV_GUIDED - # define NAV_GUIDED !HAL_MINIMIZE_FEATURES +#ifndef AC_NAV_GUIDED + # define AC_NAV_GUIDED ENABLED #endif ////////////////////////////////////////////////////////////////////////////// @@ -195,7 +191,7 @@ ////////////////////////////////////////////////////////////////////////////// // Follow - follow another vehicle or GCS #ifndef MODE_FOLLOW_ENABLED -# define MODE_FOLLOW_ENABLED !HAL_MINIMIZE_FEATURES +# define MODE_FOLLOW_ENABLED AP_FOLLOW_ENABLED #endif ////////////////////////////////////////////////////////////////////////////// @@ -207,7 +203,7 @@ ////////////////////////////////////////////////////////////////////////////// // GuidedNoGPS mode - control vehicle's angles from GCS #ifndef MODE_GUIDED_NOGPS_ENABLED -# define MODE_GUIDED_NOGPS_ENABLED !HAL_MINIMIZE_FEATURES +# define MODE_GUIDED_NOGPS_ENABLED ENABLED #endif ////////////////////////////////////////////////////////////////////////////// @@ -243,7 +239,7 @@ ////////////////////////////////////////////////////////////////////////////// // System ID - conduct system identification tests on vehicle #ifndef MODE_SYSTEMID_ENABLED -# define MODE_SYSTEMID_ENABLED !HAL_MINIMIZE_FEATURES +# define MODE_SYSTEMID_ENABLED ENABLED #endif ////////////////////////////////////////////////////////////////////////////// @@ -255,19 +251,19 @@ ////////////////////////////////////////////////////////////////////////////// // ZigZag - allow vehicle to fly in a zigzag manner with predefined point A B #ifndef MODE_ZIGZAG_ENABLED -# define MODE_ZIGZAG_ENABLED !HAL_MINIMIZE_FEATURES +# define MODE_ZIGZAG_ENABLED ENABLED #endif ////////////////////////////////////////////////////////////////////////////// // Turtle - allow vehicle to be flipped over after a crash #ifndef MODE_TURTLE_ENABLED -# define MODE_TURTLE_ENABLED !HAL_MINIMIZE_FEATURES && !defined(DISABLE_DSHOT) && FRAME_CONFIG != HELI_FRAME +# define MODE_TURTLE_ENABLED HAL_DSHOT_ENABLED && FRAME_CONFIG != HELI_FRAME #endif ////////////////////////////////////////////////////////////////////////////// // Flowhold - use optical flow to hover in place #ifndef MODE_FLOWHOLD_ENABLED -# define MODE_FLOWHOLD_ENABLED !HAL_MINIMIZE_FEATURES && AP_OPTICALFLOW_ENABLED +# define MODE_FLOWHOLD_ENABLED AP_OPTICALFLOW_ENABLED #endif ////////////////////////////////////////////////////////////////////////////// @@ -275,17 +271,18 @@ ////////////////////////////////////////////////////////////////////////////// // Weathervane - allow vehicle to yaw into wind #ifndef WEATHERVANE_ENABLED -# define WEATHERVANE_ENABLED !HAL_MINIMIZE_FEATURES +# define WEATHERVANE_ENABLED ENABLED #endif ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// // Autorotate - autonomous auto-rotation - helicopters only +#ifndef MODE_AUTOROTATE_ENABLED #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if FRAME_CONFIG == HELI_FRAME #ifndef MODE_AUTOROTATE_ENABLED - # define MODE_AUTOROTATE_ENABLED !HAL_MINIMIZE_FEATURES + # define MODE_AUTOROTATE_ENABLED ENABLED #endif #else # define MODE_AUTOROTATE_ENABLED DISABLED @@ -293,12 +290,6 @@ #else # define MODE_AUTOROTATE_ENABLED DISABLED #endif - -////////////////////////////////////////////////////////////////////////////// - -// Beacon support - support for local positioning systems -#ifndef BEACON_ENABLED -# define BEACON_ENABLED !HAL_MINIMIZE_FEATURES #endif ////////////////////////////////////////////////////////////////////////////// @@ -574,7 +565,7 @@ #endif #ifndef AC_OAPATHPLANNER_ENABLED - #define AC_OAPATHPLANNER_ENABLED !HAL_MINIMIZE_FEATURES + #define AC_OAPATHPLANNER_ENABLED ENABLED #endif #if MODE_FOLLOW_ENABLED && !AC_AVOID_ENABLED @@ -648,3 +639,11 @@ #ifndef AC_CUSTOMCONTROL_MULTI_ENABLED #define AC_CUSTOMCONTROL_MULTI_ENABLED FRAME_CONFIG == MULTICOPTER_FRAME && AP_CUSTOMCONTROL_ENABLED #endif + +#ifndef AC_PAYLOAD_PLACE_ENABLED +#define AC_PAYLOAD_PLACE_ENABLED 1 +#endif + +#ifndef USER_PARAMS_ENABLED + #define USER_PARAMS_ENABLED DISABLED +#endif diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index 18a04aec4fdf8..786e56b5a0e52 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -101,7 +101,7 @@ void Copter::thrust_loss_check() { static uint16_t thrust_loss_counter; // number of iterations vehicle may have been crashed - // no-op if suppresed by flight options param + // no-op if suppressed by flight options param if ((copter.g2.flight_options & uint32_t(FlightOptions::DISABLE_THRUST_LOSS_CHECK)) != 0) { return; } @@ -181,7 +181,7 @@ void Copter::thrust_loss_check() // check for a large yaw imbalance, could be due to badly calibrated ESC or misaligned motors void Copter::yaw_imbalance_check() { - // no-op if suppresed by flight options param + // no-op if suppressed by flight options param if ((copter.g2.flight_options & uint32_t(FlightOptions::DISABLE_YAW_IMBALANCE_WARNING)) != 0) { return; } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index b3cd317f76e68..7f1175552537b 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -76,18 +76,6 @@ enum class AirMode { AIRMODE_ENABLED, }; -enum PayloadPlaceStateType { - PayloadPlaceStateType_FlyToLocation, - PayloadPlaceStateType_Descent_Start, - PayloadPlaceStateType_Descent, - PayloadPlaceStateType_Release, - PayloadPlaceStateType_Releasing, - PayloadPlaceStateType_Delay, - PayloadPlaceStateType_Ascent_Start, - PayloadPlaceStateType_Ascent, - PayloadPlaceStateType_Done, -}; - // bit options for DEV_OPTIONS parameter enum DevOptions { DevOptionADSBMAVLink = 1, @@ -151,6 +139,7 @@ enum LoggingParameters { #define FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND 4 #define FS_GCS_ENABLED_ALWAYS_LAND 5 #define FS_GCS_ENABLED_AUTO_RTL_OR_RTL 6 +#define FS_GCS_ENABLED_BRAKE_OR_LAND 7 // EKF failsafe definitions (FS_EKF_ACTION parameter) #define FS_EKF_ACTION_LAND 1 // switch to LAND mode on EKF failsafe diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 69e4edb3b1173..94077c5088040 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -187,6 +187,9 @@ void Copter::failsafe_gcs_on_event(void) case FS_GCS_ENABLED_AUTO_RTL_OR_RTL: desired_action = FailsafeAction::AUTO_DO_LAND_START; break; + case FS_GCS_ENABLED_BRAKE_OR_LAND: + desired_action = FailsafeAction::BRAKE_LAND; + break; default: // if an invalid parameter value is set, the fallback is RTL desired_action = FailsafeAction::RTL; } diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 61b1cbb7561b1..3313ab35509bb 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -25,6 +25,10 @@ Mode::Mode(void) : G_Dt(copter.G_Dt) { }; +#if AC_PAYLOAD_PLACE_ENABLED +PayloadPlace Mode::payload_place; +#endif + // return the static controller object corresponding to supplied mode Mode *Copter::mode_from_mode_num(const Mode::Number mode) { @@ -194,6 +198,52 @@ void Copter::mode_change_failed(const Mode *mode, const char *reason) } } +// Check if this mode can be entered from the GCS +bool Copter::gcs_mode_enabled(const Mode::Number mode_num) +{ + // List of modes that can be blocked, index is bit number in parameter bitmask + static const uint8_t mode_list [] { + (uint8_t)Mode::Number::STABILIZE, + (uint8_t)Mode::Number::ACRO, + (uint8_t)Mode::Number::ALT_HOLD, + (uint8_t)Mode::Number::AUTO, + (uint8_t)Mode::Number::GUIDED, + (uint8_t)Mode::Number::LOITER, + (uint8_t)Mode::Number::CIRCLE, + (uint8_t)Mode::Number::DRIFT, + (uint8_t)Mode::Number::SPORT, + (uint8_t)Mode::Number::FLIP, + (uint8_t)Mode::Number::AUTOTUNE, + (uint8_t)Mode::Number::POSHOLD, + (uint8_t)Mode::Number::BRAKE, + (uint8_t)Mode::Number::THROW, + (uint8_t)Mode::Number::AVOID_ADSB, + (uint8_t)Mode::Number::GUIDED_NOGPS, + (uint8_t)Mode::Number::SMART_RTL, + (uint8_t)Mode::Number::FLOWHOLD, + (uint8_t)Mode::Number::FOLLOW, + (uint8_t)Mode::Number::ZIGZAG, + (uint8_t)Mode::Number::SYSTEMID, + (uint8_t)Mode::Number::AUTOROTATE, + (uint8_t)Mode::Number::AUTO_RTL, + (uint8_t)Mode::Number::TURTLE + }; + + if (!block_GCS_mode_change((uint8_t)mode_num, mode_list, ARRAY_SIZE(mode_list))) { + return true; + } + + // Mode disabled, try and grab a mode name to give a better warning. + Mode *new_flightmode = mode_from_mode_num(mode_num); + if (new_flightmode != nullptr) { + mode_change_failed(new_flightmode, "GCS entry disabled (FLTMODE_GCSBLOCK)"); + } else { + notify_no_such_mode((uint8_t)mode_num); + } + + return false; +} + // set_mode - change flight mode and perform any necessary initialisation // optional force parameter used to force the flight mode change (used only first time mode is set) // returns true if mode was successfully set @@ -218,6 +268,11 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) return true; } + // Check if GCS mode change is disabled via parameter + if ((reason == ModeReason::GCS_COMMAND) && !gcs_mode_enabled(mode)) { + return false; + } + #if MODE_AUTO_ENABLED == ENABLED if (mode == Mode::Number::AUTO_RTL) { // Special case for AUTO RTL, not a true mode, just AUTO in disguise @@ -594,7 +649,7 @@ void Mode::land_run_vertical_control(bool pause_descent) // Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed. cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed)); -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED const bool navigating = pos_control->is_active_xy(); bool doing_precision_landing = !copter.ap.land_repo_active && copter.precland.target_acquired() && navigating; @@ -615,7 +670,7 @@ void Mode::land_run_vertical_control(bool pause_descent) // doing precland but too far away from the obstacle // do not descend cmb_rate = 0.0f; - } else if (target_pos_meas.z > 35.0f && target_pos_meas.z < 200.0f) { + } else if (target_pos_meas.z > 35.0f && target_pos_meas.z < 200.0f && !copter.precland.do_fast_descend()) { // very close to the ground and doing prec land, lets slow down to make sure we land on target // compute desired descent velocity const float precland_acceptable_error_cm = 15.0f; @@ -668,7 +723,7 @@ void Mode::land_run_horizontal_control() AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE); } copter.ap.land_repo_active = true; -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED } else { // no override right now, check if we should allow precland if (copter.precland.allow_precland_after_reposition()) { @@ -681,7 +736,7 @@ void Mode::land_run_horizontal_control() // this variable will be updated if prec land target is in sight and pilot isn't trying to reposition the vehicle copter.ap.prec_land_active = false; -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED copter.ap.prec_land_active = !copter.ap.land_repo_active && copter.precland.target_acquired(); // run precision landing if (copter.ap.prec_land_active) { @@ -738,7 +793,7 @@ void Mode::land_run_horizontal_control() // pause_descent is true if vehicle should not descend void Mode::land_run_normal_or_precland(bool pause_descent) { -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED if (pause_descent || !copter.precland.enabled()) { // we don't want to start descending immediately or prec land is disabled // in both cases just run simple land controllers @@ -753,7 +808,7 @@ void Mode::land_run_normal_or_precland(bool pause_descent) #endif } -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED // Go towards a position commanded by prec land state machine in order to retry landing // The passed in location is expected to be NED and in m void Mode::precland_retry_position(const Vector3f &retry_pos) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 6a28a92195614..fb787077b9059 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -2,12 +2,68 @@ #include "Copter.h" #include +#include // TODO why is this needed if Copter.h includes this class Parameters; class ParametersG2; class GCS_Copter; +// object shared by both Guided and Auto for takeoff. +// position controller controls vehicle but the user can control the yaw. +class _AutoTakeoff { +public: + void run(); + void start(float complete_alt_cm, bool terrain_alt); + bool get_position(Vector3p& completion_pos); + + bool complete; // true when takeoff is complete + +private: + // altitude above-ekf-origin below which auto takeoff does not control horizontal position + bool no_nav_active; + float no_nav_alt_cm; + + // auto takeoff variables + float complete_alt_cm; // completion altitude expressed in cm above ekf origin or above terrain (depending upon auto_takeoff_terrain_alt) + bool terrain_alt; // true if altitudes are above terrain + Vector3p complete_pos; // target takeoff position as offset from ekf origin in cm +}; + +#if AC_PAYLOAD_PLACE_ENABLED +class PayloadPlace { +public: + void run(); + void start_descent(); + bool verify(); + + enum class State : uint8_t { + FlyToLocation, + Descent_Start, + Descent, + Release, + Releasing, + Delay, + Ascent_Start, + Ascent, + Done, + }; + + // these are set by the Mission code: + State state = State::Descent_Start; // records state of payload place + float descent_max_cm; + +private: + + uint32_t descent_established_time_ms; // milliseconds + uint32_t place_start_time_ms; // milliseconds + float descent_thrust_level; + float descent_start_altitude_cm; + float descent_speed_cms; +}; +#endif + class Mode { + friend class PayloadPlace; public: @@ -50,6 +106,8 @@ class Mode { // do not allow copying CLASS_NO_COPY(Mode); + friend class _AutoTakeoff; + // returns a unique number specific to this mode virtual Number mode_number() const = 0; @@ -143,11 +201,16 @@ class Mode { land_run_vertical_control(pause_descent); } +#if AC_PAYLOAD_PLACE_ENABLED + // payload place flight behaviour: + static PayloadPlace payload_place; +#endif + // run normal or precision landing (if enabled) // pause_descent is true if vehicle should not descend void land_run_normal_or_precland(bool pause_descent = false); -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED // Go towards a position commanded by prec land state machine in order to retry landing // The passed in location is expected to be NED and in meters void precland_retry_position(const Vector3f &retry_pos); @@ -178,7 +241,7 @@ class Mode { AC_PosControl *&pos_control; AP_InertialNav &inertial_nav; AP_AHRS &ahrs; - AC_AttitudeControl_t *&attitude_control; + AC_AttitudeControl *&attitude_control; MOTOR_CLASS *&motors; RC_Channel *&channel_roll; RC_Channel *&channel_pitch; @@ -214,21 +277,7 @@ class Mode { virtual bool do_user_takeoff_start(float takeoff_alt_cm); - // method shared by both Guided and Auto for takeoff. - // position controller controls vehicle but the user can control the yaw. - void auto_takeoff_run(); - void auto_takeoff_start(float complete_alt_cm, bool terrain_alt); - bool auto_takeoff_get_position(Vector3p& completion_pos); - - // altitude above-ekf-origin below which auto takeoff does not control horizontal position - static bool auto_takeoff_no_nav_active; - static float auto_takeoff_no_nav_alt_cm; - - // auto takeoff variables - static float auto_takeoff_complete_alt_cm; // completion altitude expressed in cm above ekf origin or above terrain (depending upon auto_takeoff_terrain_alt) - static bool auto_takeoff_terrain_alt; // true if altitudes are above terrain - static bool auto_takeoff_complete; // true when takeoff is complete - static Vector3p auto_takeoff_complete_pos; // target takeoff position as offset from ekf origin in cm + static _AutoTakeoff auto_takeoff; public: // Navigation Yaw control @@ -257,7 +306,6 @@ class Mode { void set_mode(Mode new_mode); Mode default_mode(bool rtl) const; - void set_rate(float new_rate_cds); // set_roi(...): set a "look at" location: @@ -280,11 +328,11 @@ class Mode { private: - // yaw(): main product of AutoYaw; the heading: - float yaw(); + // yaw_cd(): main product of AutoYaw; the heading: + float yaw_cd(); // rate_cds(): desired yaw rate in centidegrees/second: - float rate_cds() const; + float rate_cds(); float look_ahead_yaw(); float roi_yaw() const; @@ -423,10 +471,11 @@ class ModeAltHold : public Mode { }; - class ModeAuto : public Mode { public: + friend class PayloadPlace; // in case wp_run is accidentally required + // inherit constructor using Mode::Mode; Number mode_number() const override { return auto_RTL? Number::AUTO_RTL : Number::AUTO; } @@ -452,7 +501,9 @@ class ModeAuto : public Mode { NAVGUIDED, LOITER, LOITER_TO_ALT, +#if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED && AC_PAYLOAD_PLACE_ENABLED NAV_PAYLOAD_PLACE, +#endif NAV_SCRIPT_TIME, NAV_ATTITUDE_TIME, }; @@ -463,6 +514,7 @@ class ModeAuto : public Mode { // pause continue in auto mode bool pause() override; bool resume() override; + bool paused() const; bool loiter_start(); void rtl_start(); @@ -484,10 +536,6 @@ class ModeAuto : public Mode { bool requires_terrain_failsafe() const override { return true; } - // return true if this flight mode supports user takeoff - // must_nagivate is true if mode must also control horizontal position - virtual bool has_user_takeoff(bool must_navigate) const override { return false; } - void payload_place_start(); // for GCS_MAVLink to call: @@ -550,12 +598,6 @@ class ModeAuto : public Mode { Location loc_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc) const; - void payload_place_run(); - bool payload_place_run_should_run(); - void payload_place_run_hover(); - void payload_place_run_descent(); - void payload_place_run_release(); - SubMode _mode = SubMode::TAKEOFF; // controls which auto controller is run bool shift_alt_to_current_alt(Location& target_loc) const; @@ -570,7 +612,7 @@ class ModeAuto : public Mode { void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd); void do_spline_wp(const AP_Mission::Mission_Command& cmd); void get_spline_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc, Location& dest_loc, Location& next_dest_loc, bool& next_dest_loc_is_spline); -#if NAV_GUIDED == ENABLED +#if AC_NAV_GUIDED == ENABLED void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd); void do_guided_limits(const AP_Mission::Mission_Command& cmd); #endif @@ -608,7 +650,7 @@ class ModeAuto : public Mode { bool verify_nav_wp(const AP_Mission::Mission_Command& cmd); bool verify_circle(const AP_Mission::Mission_Command& cmd); bool verify_spline_wp(const AP_Mission::Mission_Command& cmd); -#if NAV_GUIDED == ENABLED +#if AC_NAV_GUIDED == ENABLED bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd); #endif bool verify_nav_delay(const AP_Mission::Mission_Command& cmd); @@ -643,16 +685,6 @@ class ModeAuto : public Mode { }; State state = State::FlyToLocation; - struct { - PayloadPlaceStateType state = PayloadPlaceStateType_Descent_Start; // records state of payload place - uint32_t descent_established_time_ms; // milliseconds - uint32_t place_start_time_ms; // milliseconds - float descent_thrust_level; - float descent_start_altitude_cm; - float descent_speed_cms; - float descent_max_cm; - } nav_payload_place; - bool waiting_to_start; // true if waiting for vehicle to be armed or EKF origin before starting mission // True if we have entered AUTO to perform a DO_LAND_START landing sequence and we should report as AUTO RTL mode @@ -961,6 +993,10 @@ class ModeFlowHold : public Mode { class ModeGuided : public Mode { public: +#if AP_EXTERNAL_CONTROL_ENABLED + friend class AP_ExternalControl_Copter; +#endif + // inherit constructor using Mode::Mode; Number mode_number() const override { return Number::GUIDED; } @@ -1182,7 +1218,7 @@ class ModeLoiter : public Mode { bool has_user_takeoff(bool must_navigate) const override { return true; } bool allows_autotune() const override { return true; } -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED void set_precision_loiter_enabled(bool value) { _precision_loiter_enabled = value; } #endif @@ -1195,14 +1231,14 @@ class ModeLoiter : public Mode { int32_t wp_bearing() const override; float crosstrack_error() const override { return pos_control->crosstrack_error();} -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED bool do_precision_loiter(); void precision_loiter_xy(); #endif private: -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED bool _precision_loiter_enabled; bool _precision_loiter_active; // true if user has switched on prec loiter #endif @@ -1319,6 +1355,10 @@ class ModeRTL : public Mode { bool use_pilot_yaw() const override; + bool set_speed_xy(float speed_xy_cms) override; + bool set_speed_up(float speed_up_cms) override; + bool set_speed_down(float speed_down_cms) override; + // RTL states enum class SubMode : uint8_t { STARTING, @@ -1718,6 +1758,7 @@ class ModeAvoidADSB : public ModeGuided { }; +#if AP_FOLLOW_ENABLED class ModeFollow : public ModeGuided { public: @@ -1747,6 +1788,7 @@ class ModeFollow : public ModeGuided { uint32_t last_log_ms; // system time of last time desired velocity was logging }; +#endif class ModeZigZag : public Mode { @@ -1782,6 +1824,7 @@ class ModeZigZag : public Mode { bool has_manual_throttle() const override { return false; } bool allows_arming(AP_Arming::Method method) const override { return true; } bool is_autopilot() const override { return true; } + bool has_user_takeoff(bool must_navigate) const override { return true; } // save current position as A or B. If both A and B have been saved move to the one specified void save_or_move_to_destination(Destination ab_dest); @@ -1795,6 +1838,9 @@ class ModeZigZag : public Mode { const char *name() const override { return "ZIGZAG"; } const char *name4() const override { return "ZIGZ"; } + uint32_t wp_distance() const override; + int32_t wp_bearing() const override; + float crosstrack_error() const override; private: diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index b6be6d5cefb16..74bd78a15db91 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -53,7 +53,7 @@ bool ModeAuto::init(bool ignore_checks) // reset flag indicating if pilot has applied roll or pitch inputs during landing copter.ap.land_repo_active = false; -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED // initialise precland state machine copter.precland_statemachine.init(); #endif @@ -136,7 +136,7 @@ void ModeAuto::run() case SubMode::NAVGUIDED: case SubMode::NAV_SCRIPT_TIME: -#if NAV_GUIDED == ENABLED || AP_SCRIPTING_ENABLED +#if AC_NAV_GUIDED == ENABLED || AP_SCRIPTING_ENABLED nav_guided_run(); #endif break; @@ -149,9 +149,11 @@ void ModeAuto::run() loiter_to_alt_run(); break; +#if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED && AC_PAYLOAD_PLACE_ENABLED case SubMode::NAV_PAYLOAD_PLACE: - payload_place_run(); + payload_place.run(); break; +#endif case SubMode::NAV_ATTITUDE_TIME: nav_attitude_time_run(); @@ -350,7 +352,7 @@ void ModeAuto::takeoff_start(const Location& dest_loc) pos_control->init_z_controller(); // initialise alt for WP_NAVALT_MIN and set completion alt - auto_takeoff_start(alt_target_cm, alt_target_terrain); + auto_takeoff.start(alt_target_cm, alt_target_terrain); // set submode set_submode(SubMode::TAKEOFF); @@ -364,7 +366,7 @@ bool ModeAuto::wp_start(const Location& dest_loc) Vector3f stopping_point; if (_mode == SubMode::TAKEOFF) { Vector3p takeoff_complete_pos; - if (auto_takeoff_get_position(takeoff_complete_pos)) { + if (auto_takeoff.get_position(takeoff_complete_pos)) { stopping_point = takeoff_complete_pos.tofloat(); } } @@ -502,7 +504,7 @@ void ModeAuto::circle_start() set_submode(SubMode::CIRCLE); } -#if NAV_GUIDED == ENABLED +#if AC_NAV_GUIDED == ENABLED // auto_nav_guided_start - hand over control to external navigation controller in AUTO mode void ModeAuto::nav_guided_start() { @@ -519,7 +521,7 @@ void ModeAuto::nav_guided_start() // set submode set_submode(SubMode::NAVGUIDED); } -#endif //NAV_GUIDED +#endif //AC_NAV_GUIDED bool ModeAuto::is_landing() const { @@ -536,12 +538,16 @@ bool ModeAuto::is_landing() const bool ModeAuto::is_taking_off() const { - return ((_mode == SubMode::TAKEOFF) && !auto_takeoff_complete); + return ((_mode == SubMode::TAKEOFF) && !auto_takeoff.complete); } +#if AC_PAYLOAD_PLACE_ENABLED // auto_payload_place_start - initialises controller to implement a placing -void ModeAuto::payload_place_start() +void PayloadPlace::start_descent() { + auto *pos_control = copter.pos_control; + auto *wp_nav = copter.wp_nav; + // set horizontal speed and acceleration limits pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); @@ -561,13 +567,11 @@ void ModeAuto::payload_place_start() } // initialise yaw - auto_yaw.set_mode(AutoYaw::Mode::HOLD); - - // set submode - set_submode(SubMode::NAV_PAYLOAD_PLACE); + copter.flightmode->auto_yaw.set_mode(Mode::AutoYaw::Mode::HOLD); - nav_payload_place.state = PayloadPlaceStateType_Descent_Start; + state = PayloadPlace::State::Descent_Start; } +#endif // returns true if pilot's yaw input should be used to adjust vehicle's heading bool ModeAuto::use_pilot_yaw(void) const @@ -647,7 +651,7 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) do_spline_wp(cmd); break; -#if NAV_GUIDED == ENABLED +#if AC_NAV_GUIDED == ENABLED case MAV_CMD_NAV_GUIDED_ENABLE: // 92 accept navigation commands from external nav computer do_nav_guided_enable(cmd); break; @@ -657,9 +661,11 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) do_nav_delay(cmd); break; +#if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED && AC_PAYLOAD_PLACE_ENABLED case MAV_CMD_NAV_PAYLOAD_PLACE: // 94 place at Waypoint do_payload_place(cmd); break; +#endif #if AP_SCRIPTING_ENABLED case MAV_CMD_NAV_SCRIPT_TIME: @@ -719,7 +725,7 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) #endif //AP_FENCE_ENABLED break; -#if NAV_GUIDED == ENABLED +#if AC_NAV_GUIDED == ENABLED case MAV_CMD_DO_GUIDED_LIMITS: // 220 accept guided mode limits do_guided_limits(cmd); break; @@ -865,9 +871,11 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd) cmd_complete = verify_land(); break; +#if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED && AC_PAYLOAD_PLACE_ENABLED case MAV_CMD_NAV_PAYLOAD_PLACE: - cmd_complete = verify_payload_place(); + cmd_complete = payload_place.verify(); break; +#endif case MAV_CMD_NAV_LOITER_UNLIM: cmd_complete = verify_loiter_unlimited(); @@ -892,7 +900,7 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd) cmd_complete = verify_spline_wp(cmd); break; -#if NAV_GUIDED == ENABLED +#if AC_NAV_GUIDED == ENABLED case MAV_CMD_NAV_GUIDED_ENABLE: cmd_complete = verify_nav_guided_enable(cmd); break; @@ -965,7 +973,7 @@ void ModeAuto::takeoff_run() if ((copter.g2.auto_options & (int32_t)Options::AllowTakeOffWithoutRaisingThrottle) != 0) { copter.set_auto_armed(true); } - auto_takeoff_run(); + auto_takeoff.run(); } // auto_wp_run - runs the auto waypoint controller @@ -1033,7 +1041,7 @@ void ModeAuto::circle_run() attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading()); } -#if NAV_GUIDED == ENABLED || AP_SCRIPTING_ENABLED +#if AC_NAV_GUIDED == ENABLED || AP_SCRIPTING_ENABLED // auto_nav_guided_run - allows control by external navigation controller // called by auto_run at 100hz or more void ModeAuto::nav_guided_run() @@ -1041,7 +1049,7 @@ void ModeAuto::nav_guided_run() // call regular guided flight mode run function copter.mode_guided.run(); } -#endif // NAV_GUIDED || AP_SCRIPTING_ENABLED +#endif // AC_NAV_GUIDED || AP_SCRIPTING_ENABLED // auto_loiter_run - loiter in AUTO flight mode // called by auto_run at 100hz or more @@ -1158,122 +1166,130 @@ void ModeAuto::nav_attitude_time_run() pos_control->update_z_controller(); } +#if AC_PAYLOAD_PLACE_ENABLED // auto_payload_place_run - places an object in auto mode // called by auto_run at 100hz or more -void ModeAuto::payload_place_run() +void PayloadPlace::run() { const char* prefix_str = "PayloadPlace:"; - if (!payload_place_run_should_run()) { - zero_throttle_and_relax_ac(); + if (copter.flightmode->is_disarmed_or_landed()) { + copter.flightmode->make_safe_ground_handling(); return; } // set motors to full range - motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); const uint32_t descent_thrust_cal_duration_ms = 2000; // milliseconds const uint32_t placed_check_duration_ms = 500; // how long we have to be below a throttle threshold before considering placed // Vertical thrust is taken from the attitude controller before angle boost is added + auto *attitude_control = copter.attitude_control; const float thrust_level = attitude_control->get_throttle_in(); const uint32_t now_ms = AP_HAL::millis(); // if we discover we've landed then immediately release the load: if (copter.ap.land_complete || copter.ap.land_complete_maybe) { - switch (nav_payload_place.state) { - case PayloadPlaceStateType_FlyToLocation: + switch (state) { + case State::FlyToLocation: // this is handled in wp_run() break; - case PayloadPlaceStateType_Descent_Start: + case State::Descent_Start: // do nothing on this loop break; - case PayloadPlaceStateType_Descent: + case State::Descent: gcs().send_text(MAV_SEVERITY_INFO, "%s landed", prefix_str); - nav_payload_place.state = PayloadPlaceStateType_Release; + state = State::Release; break; - case PayloadPlaceStateType_Release: - case PayloadPlaceStateType_Releasing: - case PayloadPlaceStateType_Delay: - case PayloadPlaceStateType_Ascent_Start: - case PayloadPlaceStateType_Ascent: - case PayloadPlaceStateType_Done: + case State::Release: + case State::Releasing: + case State::Delay: + case State::Ascent_Start: + case State::Ascent: + case State::Done: break; } } #if AP_GRIPPER_ENABLED == ENABLED // if pilot releases load manually: - if (g2.gripper.valid() && g2.gripper.released()) { - switch (nav_payload_place.state) { - case PayloadPlaceStateType_FlyToLocation: - case PayloadPlaceStateType_Descent_Start: - set_submode(SubMode::NAV_PAYLOAD_PLACE); + if (AP::gripper() != nullptr && + AP::gripper()->valid() && AP::gripper()->released()) { + switch (state) { + case State::FlyToLocation: + case State::Descent_Start: gcs().send_text(MAV_SEVERITY_INFO, "%s Manual release", prefix_str); - nav_payload_place.state = PayloadPlaceStateType_Done; + state = State::Done; break; - case PayloadPlaceStateType_Descent: + case State::Descent: gcs().send_text(MAV_SEVERITY_INFO, "%s Manual release", prefix_str); - nav_payload_place.state = PayloadPlaceStateType_Release; + state = State::Release; break; - case PayloadPlaceStateType_Release: - case PayloadPlaceStateType_Releasing: - case PayloadPlaceStateType_Delay: - case PayloadPlaceStateType_Ascent_Start: - case PayloadPlaceStateType_Ascent: - case PayloadPlaceStateType_Done: + case State::Release: + case State::Releasing: + case State::Delay: + case State::Ascent_Start: + case State::Ascent: + case State::Done: break; } } #endif - switch (nav_payload_place.state) { - case PayloadPlaceStateType_FlyToLocation: + auto &inertial_nav = copter.inertial_nav; + auto &g2 = copter.g2; + const auto &g = copter.g; + const auto &wp_nav = copter.wp_nav; + const auto &pos_control = copter.pos_control; + + switch (state) { + case State::FlyToLocation: if (copter.wp_nav->reached_wp_destination()) { - payload_place_start(); + start_descent(); } break; - case PayloadPlaceStateType_Descent_Start: - nav_payload_place.descent_established_time_ms = now_ms; - nav_payload_place.descent_start_altitude_cm = inertial_nav.get_position_z_up_cm(); + case State::Descent_Start: + descent_established_time_ms = now_ms; + descent_start_altitude_cm = inertial_nav.get_position_z_up_cm(); // limiting the decent rate to the limit set in wp_nav is not necessary but done for safety - nav_payload_place.descent_speed_cms = MIN((is_positive(g2.pldp_descent_speed_ms)) ? g2.pldp_descent_speed_ms * 100.0 : abs(g.land_speed), wp_nav->get_default_speed_down()); - nav_payload_place.descent_thrust_level = 1.0; - nav_payload_place.state = PayloadPlaceStateType_Descent; + descent_speed_cms = MIN((is_positive(g2.pldp_descent_speed_ms)) ? g2.pldp_descent_speed_ms * 100.0 : abs(g.land_speed), wp_nav->get_default_speed_down()); + descent_thrust_level = 1.0; + state = State::Descent; FALLTHROUGH; - case PayloadPlaceStateType_Descent: + case State::Descent: // check maximum decent distance - if (!is_zero(nav_payload_place.descent_max_cm) && - nav_payload_place.descent_start_altitude_cm - inertial_nav.get_position_z_up_cm() > nav_payload_place.descent_max_cm) { - nav_payload_place.state = PayloadPlaceStateType_Ascent_Start; + if (!is_zero(descent_max_cm) && + descent_start_altitude_cm - inertial_nav.get_position_z_up_cm() > descent_max_cm) { + state = State::Ascent_Start; gcs().send_text(MAV_SEVERITY_WARNING, "%s Reached maximum descent", prefix_str); break; } // calibrate the decent thrust after aircraft has reached constant decent rate and release if threshold is reached - if (pos_control->get_vel_desired_cms().z > -0.95 * nav_payload_place.descent_speed_cms) { + if (pos_control->get_vel_desired_cms().z > -0.95 * descent_speed_cms) { // decent rate has not reached descent_speed_cms - nav_payload_place.descent_established_time_ms = now_ms; + descent_established_time_ms = now_ms; break; - } else if (now_ms - nav_payload_place.descent_established_time_ms < descent_thrust_cal_duration_ms) { + } else if (now_ms - descent_established_time_ms < descent_thrust_cal_duration_ms) { // record minimum thrust for descent_thrust_cal_duration_ms - nav_payload_place.descent_thrust_level = MIN(nav_payload_place.descent_thrust_level, thrust_level); - nav_payload_place.place_start_time_ms = now_ms; + descent_thrust_level = MIN(descent_thrust_level, thrust_level); + place_start_time_ms = now_ms; break; - } else if (thrust_level > g2.pldp_thrust_placed_fraction * nav_payload_place.descent_thrust_level) { + } else if (thrust_level > g2.pldp_thrust_placed_fraction * descent_thrust_level) { // thrust is above minimum threshold - nav_payload_place.place_start_time_ms = now_ms; + place_start_time_ms = now_ms; break; } else if (is_positive(g2.pldp_range_finder_minimum_m)) { if (!copter.rangefinder_state.enabled) { // abort payload place because rangefinder is not enabled - nav_payload_place.state = PayloadPlaceStateType_Ascent_Start; + state = State::Ascent_Start; gcs().send_text(MAV_SEVERITY_WARNING, "%s PLDP_RNG_MIN set and rangefinder not enabled", prefix_str); break; } else if (copter.rangefinder_alt_ok() && (copter.rangefinder_state.glitch_count == 0) && (copter.rangefinder_state.alt_cm > g2.pldp_range_finder_minimum_m * 100.0)) { // range finder altitude is above minimum - nav_payload_place.place_start_time_ms = now_ms; + place_start_time_ms = now_ms; break; } } @@ -1287,57 +1303,61 @@ void ModeAuto::payload_place_run() // payload touchdown must be detected for 0.5 seconds - if (now_ms - nav_payload_place.place_start_time_ms > placed_check_duration_ms) { - nav_payload_place.state = PayloadPlaceStateType_Release; - gcs().send_text(MAV_SEVERITY_INFO, "%s payload release thrust threshold: %f", prefix_str, static_cast(g2.pldp_thrust_placed_fraction * nav_payload_place.descent_thrust_level)); + if (now_ms - place_start_time_ms > placed_check_duration_ms) { + state = State::Release; + gcs().send_text(MAV_SEVERITY_INFO, "%s payload release thrust threshold: %f", prefix_str, static_cast(g2.pldp_thrust_placed_fraction * descent_thrust_level)); } break; - case PayloadPlaceStateType_Release: + case State::Release: // Reinitialise vertical position controller to remove discontinuity due to touch down of payload pos_control->init_z_controller_no_descent(); #if AP_GRIPPER_ENABLED == ENABLED if (g2.gripper.valid()) { gcs().send_text(MAV_SEVERITY_INFO, "%s Releasing the gripper", prefix_str); g2.gripper.release(); - nav_payload_place.state = PayloadPlaceStateType_Releasing; + state = State::Releasing; } else { - nav_payload_place.state = PayloadPlaceStateType_Delay; + state = State::Delay; } #else - nav_payload_place.state = PayloadPlaceStateType_Delay; + state = State::Delay; #endif break; - case PayloadPlaceStateType_Releasing: + case State::Releasing: #if AP_GRIPPER_ENABLED == ENABLED if (g2.gripper.valid() && !g2.gripper.released()) { break; } #endif - nav_payload_place.state = PayloadPlaceStateType_Delay; + state = State::Delay; FALLTHROUGH; - case PayloadPlaceStateType_Delay: + case State::Delay: // If we get here we have finished releasing the gripper - if (now_ms - nav_payload_place.place_start_time_ms < placed_check_duration_ms + g2.pldp_delay_s * 1000.0) { + if (now_ms - place_start_time_ms < placed_check_duration_ms + g2.pldp_delay_s * 1000.0) { break; } FALLTHROUGH; - case PayloadPlaceStateType_Ascent_Start: { - auto_takeoff_start(nav_payload_place.descent_start_altitude_cm, false); - nav_payload_place.state = PayloadPlaceStateType_Ascent; - } - break; + case State::Ascent_Start: + state = State::Ascent; + FALLTHROUGH; - case PayloadPlaceStateType_Ascent: - if (auto_takeoff_complete) { - nav_payload_place.state = PayloadPlaceStateType_Done; + case State::Ascent: { + // Ascent complete when we are less than 10% of the stopping + // distance from the target altitude stopping distance from + // vel_threshold_fraction * max velocity + const float vel_threshold_fraction = 0.1; + const float stop_distance = 0.5 * sq(vel_threshold_fraction * copter.pos_control->get_max_speed_up_cms()) / copter.pos_control->get_max_accel_z_cmss(); + bool reached_altitude = copter.pos_control->get_pos_target_z_cm() >= descent_start_altitude_cm - stop_distance; + if (reached_altitude) { + state = State::Done; } break; - - case PayloadPlaceStateType_Done: + } + case State::Done: break; default: // this should never happen @@ -1345,61 +1365,35 @@ void ModeAuto::payload_place_run() break; } - switch (nav_payload_place.state) { - case PayloadPlaceStateType_FlyToLocation: + switch (state) { + case State::FlyToLocation: // this should never happen - return wp_run(); - case PayloadPlaceStateType_Descent_Start: - case PayloadPlaceStateType_Descent: - return payload_place_run_descent(); - case PayloadPlaceStateType_Release: - case PayloadPlaceStateType_Releasing: - case PayloadPlaceStateType_Delay: - case PayloadPlaceStateType_Ascent_Start: - return payload_place_run_hover(); - case PayloadPlaceStateType_Ascent: - case PayloadPlaceStateType_Done: - return takeoff_run(); - } -} - -bool ModeAuto::payload_place_run_should_run() -{ - // must be armed - if (!motors->armed()) { - return false; - } - // must be auto-armed - if (!copter.ap.auto_armed) { - return false; - } - // must not be landed - if (copter.ap.land_complete && (nav_payload_place.state == PayloadPlaceStateType_FlyToLocation || nav_payload_place.state == PayloadPlaceStateType_Descent_Start)) { - return false; - } - // interlock must be enabled (i.e. unsafe) - if (!motors->get_interlock()) { - return false; + return copter.mode_auto.wp_run(); + case State::Descent_Start: + case State::Descent: + copter.flightmode->land_run_horizontal_control(); + // update altitude target and call position controller + pos_control->land_at_climb_rate_cm(-descent_speed_cms, true); + pos_control->update_z_controller(); + return; + case State::Release: + case State::Releasing: + case State::Delay: + case State::Ascent_Start: + copter.flightmode->land_run_horizontal_control(); + // update altitude target and call position controller + pos_control->land_at_climb_rate_cm(0.0, false); + pos_control->update_z_controller(); + return; + case State::Ascent: + case State::Done: + float vel = 0.0; + copter.flightmode->land_run_horizontal_control(); + pos_control->input_pos_vel_accel_z(descent_start_altitude_cm, vel, 0.0); + break; } - - return true; -} - -void ModeAuto::payload_place_run_hover() -{ - land_run_horizontal_control(); - // update altitude target and call position controller - pos_control->land_at_climb_rate_cm(0.0, false); - pos_control->update_z_controller(); -} - -void ModeAuto::payload_place_run_descent() -{ - land_run_horizontal_control(); - // update altitude target and call position controller - pos_control->land_at_climb_rate_cm(-nav_payload_place.descent_speed_cms, true); - pos_control->update_z_controller(); } +#endif // sets the target_loc's alt to the vehicle's current alt but does not change target_loc's frame // in the case of terrain altitudes either the terrain database or the rangefinder may be used @@ -1521,8 +1515,10 @@ bool ModeAuto::set_next_wp(const AP_Mission::Mission_Command& current_cmd, const switch (next_cmd.id) { case MAV_CMD_NAV_WAYPOINT: case MAV_CMD_NAV_LOITER_UNLIM: - case MAV_CMD_NAV_LOITER_TIME: - case MAV_CMD_NAV_PAYLOAD_PLACE: { +#if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED + case MAV_CMD_NAV_PAYLOAD_PLACE: +#endif + case MAV_CMD_NAV_LOITER_TIME: { const Location dest_loc = loc_from_cmd(current_cmd, default_loc); const Location next_dest_loc = loc_from_cmd(next_cmd, dest_loc); return wp_nav->set_wp_destination_next_loc(next_dest_loc); @@ -1751,7 +1747,7 @@ void ModeAuto::get_spline_from_cmd(const AP_Mission::Mission_Command& cmd, const } } -#if NAV_GUIDED == ENABLED +#if AC_NAV_GUIDED == ENABLED // do_nav_guided_enable - initiate accepting commands from external nav computer void ModeAuto::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd) { @@ -1770,7 +1766,7 @@ void ModeAuto::do_guided_limits(const AP_Mission::Mission_Command& cmd) cmd.content.guided_limits.alt_max * 100.0f, // convert meters to cm cmd.content.guided_limits.horiz_max * 100.0f); // convert meters to cm } -#endif // NAV_GUIDED +#endif // AC_NAV_GUIDED // do_nav_delay - Delay the next navigation command void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd) @@ -1782,7 +1778,11 @@ void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd) nav_delay_time_max_ms = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds } else { // absolute delay to utc time +#if AP_RTC_ENABLED nav_delay_time_max_ms = AP::rtc().get_time_utc(cmd.content.nav_delay.hour_utc, cmd.content.nav_delay.min_utc, cmd.content.nav_delay.sec_utc, 0); +#else + nav_delay_time_max_ms = 0; +#endif } gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec", (unsigned)(nav_delay_time_max_ms/1000)); } @@ -1850,8 +1850,6 @@ void ModeAuto::do_yaw(const AP_Mission::Mission_Command& cmd) // Do (Now) commands /********************************************************************************/ - - void ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd) { if (cmd.content.speed.target_ms > 0) { @@ -1892,7 +1890,7 @@ void ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd) { #if HAL_MOUNT_ENABLED // if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead - if ((copter.camera_mount.get_mount_type() != copter.camera_mount.MountType::Mount_Type_None) && + if ((copter.camera_mount.get_mount_type() != AP_Mount::Type::None) && !copter.camera_mount.has_pan_control()) { auto_yaw.set_yaw_angle_rate(cmd.content.mount_control.yaw,0.0f); } @@ -1923,13 +1921,14 @@ void ModeAuto::do_winch(const AP_Mission::Mission_Command& cmd) } #endif +#if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED && AC_PAYLOAD_PLACE_ENABLED // do_payload_place - initiate placing procedure void ModeAuto::do_payload_place(const AP_Mission::Mission_Command& cmd) { // if location provided we fly to that location at current altitude if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) { // set state to fly to location - nav_payload_place.state = PayloadPlaceStateType_FlyToLocation; + payload_place.state = PayloadPlace::State::FlyToLocation; // convert cmd to location class Location target_loc(cmd.content.location); @@ -1945,14 +1944,16 @@ void ModeAuto::do_payload_place(const AP_Mission::Mission_Command& cmd) copter.failsafe_terrain_on_event(); return; } - // set submode - set_submode(SubMode::NAV_PAYLOAD_PLACE); } else { // initialise placing controller - payload_place_start(); + payload_place.start_descent(); } - nav_payload_place.descent_max_cm = cmd.p1; + payload_place.descent_max_cm = cmd.p1; + + // set submode + set_submode(SubMode::NAV_PAYLOAD_PLACE); } +#endif // AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED // do_RTL - start Return-to-Launch void ModeAuto::do_RTL(void) @@ -1970,13 +1971,13 @@ bool ModeAuto::verify_takeoff() { #if AP_LANDINGGEAR_ENABLED // if we have reached our destination - if (auto_takeoff_complete) { + if (auto_takeoff.complete) { // retract the landing gear copter.landinggear.retract_after_takeoff(); } #endif - return auto_takeoff_complete; + return auto_takeoff.complete; } // verify_land - returns true if landing has been completed @@ -2022,25 +2023,27 @@ bool ModeAuto::verify_land() return retval; } +#if AC_PAYLOAD_PLACE_ENABLED // verify_payload_place - returns true if placing has been completed -bool ModeAuto::verify_payload_place() -{ - switch (nav_payload_place.state) { - case PayloadPlaceStateType_FlyToLocation: - case PayloadPlaceStateType_Descent_Start: - case PayloadPlaceStateType_Descent: - case PayloadPlaceStateType_Release: - case PayloadPlaceStateType_Releasing: - case PayloadPlaceStateType_Delay: - case PayloadPlaceStateType_Ascent_Start: - case PayloadPlaceStateType_Ascent: +bool PayloadPlace::verify() +{ + switch (state) { + case State::FlyToLocation: + case State::Descent_Start: + case State::Descent: + case State::Release: + case State::Releasing: + case State::Delay: + case State::Ascent_Start: + case State::Ascent: return false; - case PayloadPlaceStateType_Done: + case State::Done: return true; } // should never get here return true; } +#endif bool ModeAuto::verify_loiter_unlimited() { @@ -2188,7 +2191,7 @@ bool ModeAuto::verify_spline_wp(const AP_Mission::Mission_Command& cmd) return false; } -#if NAV_GUIDED == ENABLED +#if AC_NAV_GUIDED == ENABLED // verify_nav_guided - check if we have breached any limits bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) { @@ -2200,7 +2203,7 @@ bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) // check time and position limits return copter.mode_guided.limit_check(); } -#endif // NAV_GUIDED +#endif // AC_NAV_GUIDED // verify_nav_delay - check if we have waited long enough bool ModeAuto::verify_nav_delay(const AP_Mission::Mission_Command& cmd) @@ -2251,4 +2254,9 @@ bool ModeAuto::resume() return true; } +bool ModeAuto::paused() const +{ + return wp_nav->paused(); +} + #endif diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index fb009a1d3421d..85f278ac0d801 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -47,15 +47,15 @@ bool ModeAutorotate::init(bool ignore_checks) // Display message gcs().send_text(MAV_SEVERITY_INFO, "Autorotation initiated"); - // Set all inial flags to on - _flags.entry_initial = 1; - _flags.ss_glide_initial = 1; - _flags.flare_initial = 1; - _flags.touch_down_initial = 1; - _flags.level_initial = 1; - _flags.break_initial = 1; - _flags.straight_ahead_initial = 1; - _flags.bail_out_initial = 1; + // Set all intial flags to on + _flags.entry_initial = true; + _flags.ss_glide_initial = true; + _flags.flare_initial = true; + _flags.touch_down_initial = true; + _flags.level_initial = true; + _flags.break_initial = true; + _flags.straight_ahead_initial = true; + _flags.bail_out_initial = true; _msg_flags.bad_rpm = true; // Setting default starting switches @@ -114,7 +114,7 @@ void ModeAutorotate::run() case Autorotation_Phase::ENTRY: { // Entry phase functions to be run only once - if (_flags.entry_initial == 1) { + if (_flags.entry_initial == true) { #if CONFIG_HAL_BOARD == HAL_BOARD_SITL gcs().send_text(MAV_SEVERITY_INFO, "Entry Phase"); @@ -130,7 +130,7 @@ void ModeAutorotate::run() g2.arot.set_desired_fwd_speed(); // Prevent running the initial entry functions again - _flags.entry_initial = 0; + _flags.entry_initial = false; } @@ -160,7 +160,7 @@ void ModeAutorotate::run() case Autorotation_Phase::SS_GLIDE: { // Steady state glide functions to be run only once - if (_flags.ss_glide_initial == 1) { + if (_flags.ss_glide_initial == true) { #if CONFIG_HAL_BOARD == HAL_BOARD_SITL gcs().send_text(MAV_SEVERITY_INFO, "SS Glide Phase"); @@ -173,11 +173,11 @@ void ModeAutorotate::run() g2.arot.set_desired_fwd_speed(); // Set target head speed in head speed controller - _target_head_speed = HEAD_SPEED_TARGET_RATIO; //Ensure target hs is set to glide in case hs hasent reached target for glide + _target_head_speed = HEAD_SPEED_TARGET_RATIO; //Ensure target hs is set to glide in case hs has not reached target for glide g2.arot.set_target_head_speed(_target_head_speed); // Prevent running the initial glide functions again - _flags.ss_glide_initial = 0; + _flags.ss_glide_initial = false; } // Run airspeed/attitude controller @@ -202,7 +202,7 @@ void ModeAutorotate::run() case Autorotation_Phase::BAIL_OUT: { - if (_flags.bail_out_initial == 1) { + if (_flags.bail_out_initial == true) { // Functions and settings to be done once are done here. #if CONFIG_HAL_BOARD == HAL_BOARD_SITL @@ -243,7 +243,7 @@ void ModeAutorotate::run() motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - _flags.bail_out_initial = 0; + _flags.bail_out_initial = false; } if ((now - _bail_time_start_ms)/1000.0f >= BAILOUT_MOTOR_RAMP_TIME) { diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 55d71f1c63f48..65a353ebc7829 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -61,6 +61,7 @@ void ModeBrake::run() pos_control->set_pos_target_z_from_climb_rate_cm(0.0f); pos_control->update_z_controller(); + // MAV_CMD_SOLO_BTN_PAUSE_CLICK (Solo only) is used to set the timeout. if (_timeout_ms != 0 && millis()-_timeout_start >= _timeout_ms) { if (!copter.set_mode(Mode::Number::LOITER, ModeReason::BRAKE_TIMEOUT)) { copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::BRAKE_TIMEOUT); @@ -68,6 +69,16 @@ void ModeBrake::run() } } +/** + * Set a timeout for the brake mode + * + * @param timeout_ms [in] timeout in milliseconds + * + * @note MAV_CMD_SOLO_BTN_PAUSE_CLICK (Solo only) is used to set the timeout. + * If the timeout is reached, the mode will switch to loiter or alt hold depending on the current mode. + * If timeout_ms is 0, the timeout is disabled. + * +*/ void ModeBrake::timeout_to_loiter_ms(uint32_t timeout_ms) { _timeout_start = millis(); diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index c86de8856df18..bb9a6612f948c 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -1,4 +1,5 @@ #include "Copter.h" +#include #if MODE_CIRCLE_ENABLED == ENABLED @@ -20,6 +21,22 @@ bool ModeCircle::init(bool ignore_checks) // initialise circle controller including setting the circle center based on vehicle speed copter.circle_nav->init(); +#if HAL_MOUNT_ENABLED + AP_Mount *s = AP_Mount::get_singleton(); + + // Check if the CIRCLE_OPTIONS parameter have roi_at_center + if (copter.circle_nav->roi_at_center()) { + const Vector3p &pos { copter.circle_nav->get_center() }; + Location circle_center; + if (!AP::ahrs().get_location_from_origin_offset(circle_center, pos * 0.01)) { + return false; + } + // point at the ground: + circle_center.set_alt_cm(0, Location::AltFrame::ABOVE_TERRAIN); + s->set_roi_target(circle_center); + } +#endif + // set auto yaw circle mode auto_yaw.set_mode(AutoYaw::Mode::CIRCLE); diff --git a/ArduCopter/mode_follow.cpp b/ArduCopter/mode_follow.cpp index afa348503b084..a0649c5956c24 100644 --- a/ArduCopter/mode_follow.cpp +++ b/ArduCopter/mode_follow.cpp @@ -20,6 +20,15 @@ bool ModeFollow::init(const bool ignore_checks) gcs().send_text(MAV_SEVERITY_WARNING, "Set FOLL_ENABLE = 1"); return false; } + +#if HAL_MOUNT_ENABLED + AP_Mount *mount = AP_Mount::get_singleton(); + // follow the lead vehicle using sysid + if (g2.follow.option_is_enabled(AP_Follow::Option::MOUNT_FOLLOW_ON_ENTER) && mount != nullptr) { + mount->set_target_sysid(g2.follow.get_target_sysid()); + } +#endif + // re-use guided mode return ModeGuided::init(ignore_checks); } diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index fd313a1c0d083..9c2914de8b7bf 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -100,7 +100,7 @@ void ModeGuided::run() bool ModeGuided::allows_arming(AP_Arming::Method method) const { // always allow arming from the ground station or scripting - if (method == AP_Arming::Method::MAVLINK || method == AP_Arming::Method::SCRIPTING) { + if (AP_Arming::method_is_GCS(method) || method == AP_Arming::Method::SCRIPTING) { return true; } @@ -153,7 +153,7 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm) pos_control->init_z_controller(); // initialise alt for WP_NAVALT_MIN and set completion alt - auto_takeoff_start(alt_target_cm, alt_target_terrain); + auto_takeoff.start(alt_target_cm, alt_target_terrain); // record takeoff has not completed takeoff_complete = false; @@ -656,8 +656,8 @@ void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_ // called by guided_run at 100hz or more void ModeGuided::takeoff_run() { - auto_takeoff_run(); - if (auto_takeoff_complete && !takeoff_complete) { + auto_takeoff.run(); + if (auto_takeoff.complete && !takeoff_complete) { takeoff_complete = true; #if AP_LANDINGGEAR_ENABLED // optionally retract landing gear @@ -1087,7 +1087,6 @@ uint32_t ModeGuided::wp_distance() const return get_horizontal_distance_cm(inertial_nav.get_position_xy_cm(), guided_pos_target_cm.tofloat().xy()); case SubMode::PosVelAccel: return pos_control->get_pos_error_xy_cm(); - break; default: return 0; } @@ -1102,7 +1101,6 @@ int32_t ModeGuided::wp_bearing() const return get_bearing_cd(inertial_nav.get_position_xy_cm(), guided_pos_target_cm.tofloat().xy()); case SubMode::PosVelAccel: return pos_control->get_bearing_to_target_cd(); - break; case SubMode::TakeOff: case SubMode::Accel: case SubMode::VelAccel: diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 4072e76c8b3a1..377bef5ee9253 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -46,7 +46,7 @@ bool ModeLand::init(bool ignore_checks) copter.fence.auto_disable_fence_for_landing(); #endif -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED // initialise precland state machine copter.precland_statemachine.init(); #endif diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 0198db443e5b4..602aa52859a15 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -34,14 +34,14 @@ bool ModeLoiter::init(bool ignore_checks) pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); pos_control->set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED _precision_loiter_active = false; #endif return true; } -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED bool ModeLoiter::do_precision_loiter() { if (!_precision_loiter_enabled) { @@ -165,7 +165,7 @@ void ModeLoiter::run() // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED bool precision_loiter_old_state = _precision_loiter_active; if (do_precision_loiter()) { precision_loiter_xy(); diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index fc08b39b19e6c..7c74838a3fe74 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -28,7 +28,7 @@ bool ModeRTL::init(bool ignore_checks) // this will be set true if prec land is later active copter.ap.prec_land_active = false; -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED // initialise precland state machine copter.precland_statemachine.init(); #endif @@ -408,9 +408,10 @@ void ModeRTL::build_path() // return target's altitude is updated to a higher altitude that the vehicle can safely return at (frame may also be set) void ModeRTL::compute_return_target() { - // set return target to nearest rally point or home position (Note: alt is absolute) + // set return target to nearest rally point or home position #if HAL_RALLY_ENABLED rtl_path.return_target = copter.rally.calc_best_rally_or_home_location(copter.current_loc, ahrs.get_home().alt); + rtl_path.return_target.change_alt_frame(Location::AltFrame::ABSOLUTE); #else rtl_path.return_target = ahrs.get_home(); #endif @@ -554,4 +555,22 @@ bool ModeRTL::use_pilot_yaw(void) const return allow_yaw_option || land_repositioning || final_landing; } +bool ModeRTL::set_speed_xy(float speed_xy_cms) +{ + copter.wp_nav->set_speed_xy(speed_xy_cms); + return true; +} + +bool ModeRTL::set_speed_up(float speed_up_cms) +{ + copter.wp_nav->set_speed_up(speed_up_cms); + return true; +} + +bool ModeRTL::set_speed_down(float speed_down_cms) +{ + copter.wp_nav->set_speed_down(speed_down_cms); + return true; +} + #endif diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp index 1a942780b8dfc..15dc1eba4f0ed 100644 --- a/ArduCopter/mode_systemid.cpp +++ b/ArduCopter/mode_systemid.cpp @@ -107,7 +107,7 @@ bool ModeSystemId::init(bool ignore_checks) // systemId_exit - clean up systemId controller before exiting void ModeSystemId::exit() { - // reset the feedfoward enabled parameter to the initialized state + // reset the feedforward enabled parameter to the initialized state attitude_control->bf_feedforward(att_bf_feedforward); } @@ -261,11 +261,7 @@ void ModeSystemId::run() attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); // output pilot's throttle - if (copter.is_tradheli()) { - attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt); - } else { - attitude_control->set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt); - } + attitude_control->set_throttle_out(pilot_throttle_scaled, !copter.is_tradheli(), g.throttle_filt); if (log_subsample <= 0) { log_data(); diff --git a/ArduCopter/mode_turtle.cpp b/ArduCopter/mode_turtle.cpp index 99c5d2b08d73c..74dc49421ecff 100644 --- a/ArduCopter/mode_turtle.cpp +++ b/ArduCopter/mode_turtle.cpp @@ -163,7 +163,7 @@ void ModeTurtle::run() Vector2f input{sign_roll, sign_pitch}; motors_input = input.normalized() * 0.5; // we bypass spin min and friends in the deadzone because we only want spin up when the sticks are moved - motors_output = !is_zero(flip_power) ? motors->thrust_to_actuator(flip_power) : 0.0f; + motors_output = !is_zero(flip_power) ? motors->thr_lin.thrust_to_actuator(flip_power) : 0.0f; } // actually write values to the motors diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index ff186ff65ee77..2952158ec615a 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -583,4 +583,17 @@ void ModeZigZag::spray(bool b) #endif } +uint32_t ModeZigZag::wp_distance() const +{ + return is_auto ? wp_nav->get_wp_distance_to_destination() : 0; +} +int32_t ModeZigZag::wp_bearing() const +{ + return is_auto ? wp_nav->get_wp_bearing_to_destination() : 0; +} +float ModeZigZag::crosstrack_error() const +{ + return is_auto ? wp_nav->crosstrack_error() : 0; +} + #endif // MODE_ZIGZAG_ENABLED == ENABLED diff --git a/ArduCopter/motor_test.cpp b/ArduCopter/motor_test.cpp index 13114e1744d27..5c5deae8995cd 100644 --- a/ArduCopter/motor_test.cpp +++ b/ArduCopter/motor_test.cpp @@ -101,6 +101,13 @@ bool Copter::mavlink_motor_control_check(const GCS_MAVLINK &gcs_chan, bool check return false; } + // Check Motor test is allowed + char failure_msg[50] {}; + if (!motors->motor_test_checks(ARRAY_SIZE(failure_msg), failure_msg)) { + gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"%s: %s", mode, failure_msg); + return false; + } + // check rc has been calibrated if (check_rc && !arming.rc_calibration_checks(true)) { gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"%s: RC not calibrated", mode); @@ -153,7 +160,7 @@ MAV_RESULT Copter::mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t EXPECT_DELAY_MS(3000); // enable and arm motors if (!motors->armed()) { - enable_motor_output(); + motors->output_min(); // output lowest possible value to motors motors->armed(true); hal.util->set_soft_armed(true); } diff --git a/ArduCopter/precision_landing.cpp b/ArduCopter/precision_landing.cpp index 076177667806b..ad98c2fa75552 100644 --- a/ArduCopter/precision_landing.cpp +++ b/ArduCopter/precision_landing.cpp @@ -4,7 +4,7 @@ #include "Copter.h" -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED void Copter::init_precland() { diff --git a/ArduCopter/radio.cpp b/ArduCopter/radio.cpp index 0de13687d157e..57043fba91839 100644 --- a/ArduCopter/radio.cpp +++ b/ArduCopter/radio.cpp @@ -77,13 +77,6 @@ void Copter::init_rc_out() } -// enable_motor_output() - enable and output lowest possible value to motors -void Copter::enable_motor_output() -{ - // enable motors - motors->output_min(); -} - void Copter::read_radio() { const uint32_t tnow_ms = millis(); diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index fd103ee21895f..589561b8cfe40 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -6,8 +6,6 @@ void Copter::read_barometer(void) barometer.update(); baro_alt = barometer.get_altitude() * 100.0f; - - motors->set_air_density_ratio(barometer.get_air_density_ratio()); } void Copter::init_rangefinder(void) diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index dae7a40c28811..4c740a2e3b3e1 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -92,7 +92,9 @@ void Copter::init_ardupilot() // motors initialised so parameters can be sent ap.initialised_params = true; +#if AP_RELAY_ENABLED relay.init(); +#endif /* * setup the 'main loop is dead' check. Note that this relies on @@ -132,7 +134,7 @@ void Copter::init_ardupilot() camera.init(); #endif -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED // initialise precision landing init_precland(); #endif @@ -151,15 +153,17 @@ void Copter::init_ardupilot() barometer.set_log_baro_bit(MASK_LOG_IMU); barometer.calibrate(); +#if RANGEFINDER_ENABLED == ENABLED // initialise rangefinder init_rangefinder(); +#endif #if HAL_PROXIMITY_ENABLED // init proximity sensor g2.proximity.init(); #endif -#if BEACON_ENABLED == ENABLED +#if AP_BEACON_ENABLED // init beacons used for non-gps position estimation g2.beacon.init(); #endif @@ -196,20 +200,12 @@ void Copter::init_ardupilot() set_land_complete(true); set_land_complete_maybe(true); - // we don't want writes to the serial port to cause us to pause - // mid-flight, so set the serial ports non-blocking once we are - // ready to fly - serial_manager.set_blocking_writes_all(false); - // enable CPU failsafe failsafe_enable(); ins.set_log_raw_bit(MASK_LOG_IMU_RAW); - // enable output to motors - if (arming.rc_calibration_checks(true)) { - enable_motor_output(); - } + motors->output_min(); // output lowest possible value to motors // attempt to set the intial_mode, else set to STABILIZE if (!set_mode((enum Mode::Number)g.initial_mode.get(), ModeReason::INITIALISED)) { @@ -448,26 +444,24 @@ void Copter::allocate_motors(void) AP_BoardConfig::allocation_error("AP_AHRS_View"); } - const struct AP_Param::GroupInfo *ac_var_info; - #if FRAME_CONFIG != HELI_FRAME if ((AP_Motors::motor_frame_class)g2.frame_class.get() == AP_Motors::MOTOR_FRAME_6DOF_SCRIPTING) { #if AP_SCRIPTING_ENABLED attitude_control = new AC_AttitudeControl_Multi_6DoF(*ahrs_view, aparm, *motors); - ac_var_info = AC_AttitudeControl_Multi_6DoF::var_info; + attitude_control_var_info = AC_AttitudeControl_Multi_6DoF::var_info; #endif // AP_SCRIPTING_ENABLED } else { attitude_control = new AC_AttitudeControl_Multi(*ahrs_view, aparm, *motors); - ac_var_info = AC_AttitudeControl_Multi::var_info; + attitude_control_var_info = AC_AttitudeControl_Multi::var_info; } #else attitude_control = new AC_AttitudeControl_Heli(*ahrs_view, aparm, *motors); - ac_var_info = AC_AttitudeControl_Heli::var_info; + attitude_control_var_info = AC_AttitudeControl_Heli::var_info; #endif if (attitude_control == nullptr) { AP_BoardConfig::allocation_error("AttitudeControl"); } - AP_Param::load_object_from_eeprom(attitude_control, ac_var_info); + AP_Param::load_object_from_eeprom(attitude_control, attitude_control_var_info); pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control); if (pos_control == nullptr) { @@ -528,6 +522,7 @@ void Copter::allocate_motors(void) convert_pid_parameters(); #if FRAME_CONFIG == HELI_FRAME convert_tradheli_parameters(); + motors->heli_motors_param_conversions(); #endif #if HAL_PROXIMITY_ENABLED diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index faa4adfee5660..14fb7d39da7ba 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -1,13 +1,7 @@ #include "Copter.h" Mode::_TakeOff Mode::takeoff; - -bool Mode::auto_takeoff_no_nav_active = false; -float Mode::auto_takeoff_no_nav_alt_cm = 0; -float Mode::auto_takeoff_complete_alt_cm = 0; -bool Mode::auto_takeoff_terrain_alt = false; -bool Mode::auto_takeoff_complete = false; -Vector3p Mode::auto_takeoff_complete_pos; +_AutoTakeoff Mode::auto_takeoff; // This file contains the high-level takeoff logic for Loiter, PosHold, AltHold, Sport modes. // The take-off can be initiated from a GCS NAV_TAKEOFF command which includes a takeoff altitude @@ -90,7 +84,7 @@ void Mode::_TakeOff::do_pilot_takeoff(float& pilot_climb_rate_cm) 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_z_controller(); - if (throttle >= 0.9 || + if (throttle >= MIN(copter.g2.takeoff_throttle_max, 0.9) || (copter.pos_control->get_z_accel_cmss() >= 0.5 * copter.pos_control->get_max_accel_z_cmss()) || (copter.pos_control->get_vel_desired_cms().z >= constrain_float(pilot_climb_rate_cm, copter.pos_control->get_max_speed_up_cms() * 0.1, copter.pos_control->get_max_speed_up_cms() * 0.5)) || (is_positive(take_off_complete_alt - take_off_start_alt) && copter.pos_control->get_pos_target_z_cm() - take_off_start_alt > 0.5 * (take_off_complete_alt - take_off_start_alt))) { @@ -118,20 +112,27 @@ void Mode::_TakeOff::do_pilot_takeoff(float& pilot_climb_rate_cm) // auto_takeoff_run - controls the vertical position controller during the process of taking off in auto modes // auto_takeoff_complete set to true when target altitude is within 10% of the take off altitude and less than 50% max climb rate -void Mode::auto_takeoff_run() +void _AutoTakeoff::run() { + const auto &g2 = copter.g2; + const auto &inertial_nav = copter.inertial_nav; + const auto &wp_nav = copter.wp_nav; + auto *motors = copter.motors; + auto *pos_control = copter.pos_control; + auto *attitude_control = copter.attitude_control; + // if not armed set throttle to zero and exit immediately if (!motors->armed() || !copter.ap.auto_armed) { // do not spool down tradheli when on the ground with motor interlock enabled - make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock()); + copter.flightmode->make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock()); // update auto_takeoff_no_nav_alt_cm - auto_takeoff_no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100; + no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100; return; } // get terrain offset float terr_offset = 0.0f; - if (auto_takeoff_terrain_alt && !wp_nav->get_terrain_offset(terr_offset)) { + if (terrain_alt && !wp_nav->get_terrain_offset(terr_offset)) { // trigger terrain failsafe copter.failsafe_terrain_on_event(); return; @@ -151,7 +152,7 @@ void Mode::auto_takeoff_run() attitude_control->reset_rate_controller_I_terms(); attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0); // update auto_takeoff_no_nav_alt_cm - auto_takeoff_no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100; + no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100; return; } @@ -166,10 +167,10 @@ void Mode::auto_takeoff_run() pos_control->update_xy_controller(); attitude_control->reset_rate_controller_I_terms(); attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0); - if (throttle >= 0.9 || + if (throttle >= MIN(copter.g2.takeoff_throttle_max, 0.9) || (copter.pos_control->get_z_accel_cmss() >= 0.5 * copter.pos_control->get_max_accel_z_cmss()) || (copter.pos_control->get_vel_desired_cms().z >= 0.1 * copter.pos_control->get_max_speed_up_cms()) || - ( auto_takeoff_no_nav_active && (inertial_nav.get_position_z_up_cm() >= auto_takeoff_no_nav_alt_cm))) { + ( no_nav_active && (inertial_nav.get_position_z_up_cm() >= no_nav_alt_cm))) { // throttle > 90% // acceleration > 50% maximum acceleration // velocity > 10% maximum velocity @@ -180,10 +181,10 @@ void Mode::auto_takeoff_run() } // check if we are not navigating because of low altitude - if (auto_takeoff_no_nav_active) { + if (no_nav_active) { // check if vehicle has reached no_nav_alt threshold - if (inertial_nav.get_position_z_up_cm() >= auto_takeoff_no_nav_alt_cm) { - auto_takeoff_no_nav_active = false; + if (inertial_nav.get_position_z_up_cm() >= no_nav_alt_cm) { + no_nav_active = false; } pos_control->relax_velocity_controller_xy(); } else { @@ -194,7 +195,7 @@ void Mode::auto_takeoff_run() pos_control->update_xy_controller(); // command the aircraft to the take off altitude - float pos_z = auto_takeoff_complete_alt_cm + terr_offset; + float pos_z = complete_alt_cm + terr_offset; float vel_z = 0.0; copter.pos_control->input_pos_vel_accel_z(pos_z, vel_z, 0.0); @@ -202,7 +203,7 @@ void Mode::auto_takeoff_run() pos_control->update_z_controller(); // call attitude controller with auto yaw - attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading()); + attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), copter.flightmode->auto_yaw.get_heading()); // takeoff complete when we are less than 1% of the stopping distance from the target altitude // and 10% our maximum climb rate @@ -211,40 +212,42 @@ void Mode::auto_takeoff_run() const float stop_distance = 0.5 * sq(vel_threshold_fraction * copter.pos_control->get_max_speed_up_cms()) / copter.pos_control->get_max_accel_z_cmss(); bool reached_altitude = copter.pos_control->get_pos_target_z_cm() >= pos_z - stop_distance; bool reached_climb_rate = copter.pos_control->get_vel_desired_cms().z < copter.pos_control->get_max_speed_up_cms() * vel_threshold_fraction; - auto_takeoff_complete = reached_altitude && reached_climb_rate; + complete = reached_altitude && reached_climb_rate; // calculate completion for location in case it is needed for a smooth transition to wp_nav - if (auto_takeoff_complete) { - const Vector3p& complete_pos = copter.pos_control->get_pos_target_cm(); - auto_takeoff_complete_pos = Vector3p{complete_pos.x, complete_pos.y, pos_z}; + if (complete) { + const Vector3p& _complete_pos = copter.pos_control->get_pos_target_cm(); + complete_pos = Vector3p{_complete_pos.x, _complete_pos.y, pos_z}; } } -void Mode::auto_takeoff_start(float complete_alt_cm, bool terrain_alt) +void _AutoTakeoff::start(float _complete_alt_cm, bool _terrain_alt) { // auto_takeoff_complete_alt_cm is a problem if equal to auto_takeoff_start_alt_cm - auto_takeoff_complete_alt_cm = complete_alt_cm; - auto_takeoff_terrain_alt = terrain_alt; - auto_takeoff_complete = false; + complete_alt_cm = _complete_alt_cm; + terrain_alt = _terrain_alt; + complete = false; // initialise auto_takeoff_no_nav_alt_cm - auto_takeoff_no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100; - if ((g2.wp_navalt_min > 0) && (is_disarmed_or_landed() || !motors->get_interlock())) { + const auto &g2 = copter.g2; + const auto &inertial_nav = copter.inertial_nav; + no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100; + if ((g2.wp_navalt_min > 0) && (copter.flightmode->is_disarmed_or_landed() || !copter.motors->get_interlock())) { // we are not flying, climb with no navigation to current alt-above-ekf-origin + wp_navalt_min - auto_takeoff_no_nav_active = true; + no_nav_active = true; } else { - auto_takeoff_no_nav_active = false; + no_nav_active = false; } } // return takeoff final position if takeoff has completed successfully -bool Mode::auto_takeoff_get_position(Vector3p& complete_pos) +bool _AutoTakeoff::get_position(Vector3p& _complete_pos) { // only provide location if takeoff has completed - if (!auto_takeoff_complete) { + if (!complete) { return false; } - complete_pos = auto_takeoff_complete_pos; + complete_pos = _complete_pos; return true; } diff --git a/ArduCopter/takeoff_check.cpp b/ArduCopter/takeoff_check.cpp index f3b55ec45fb3d..48f172a02cc36 100644 --- a/ArduCopter/takeoff_check.cpp +++ b/ArduCopter/takeoff_check.cpp @@ -30,7 +30,7 @@ void Copter::takeoff_check() // check ESCs are sending RPM at expected level uint32_t motor_mask = motors->get_motor_mask(); const bool telem_active = AP::esc_telem().is_telemetry_active(motor_mask); - const bool rpm_adequate = AP::esc_telem().are_motors_running(motor_mask, g2.takeoff_rpm_min); + const bool rpm_adequate = AP::esc_telem().are_motors_running(motor_mask, g2.takeoff_rpm_min, g2.takeoff_rpm_max); // if RPM is at the expected level clear block if (telem_active && rpm_adequate) { @@ -49,7 +49,7 @@ void Copter::takeoff_check() if (!telem_active) { gcs().send_text(MAV_SEVERITY_CRITICAL, "%s waiting for ESC RPM", prefix_str); } else if (!rpm_adequate) { - gcs().send_text(MAV_SEVERITY_CRITICAL, "%s ESC RPM too low", prefix_str); + gcs().send_text(MAV_SEVERITY_CRITICAL, "%s ESC RPM out of range", prefix_str); } } #endif diff --git a/ArduCopter/toy_mode.cpp b/ArduCopter/toy_mode.cpp index 922a51727baf7..d212cb4335881 100644 --- a/ArduCopter/toy_mode.cpp +++ b/ArduCopter/toy_mode.cpp @@ -890,9 +890,9 @@ void ToyMode::blink_update(void) // let the TX know we are recording video uint32_t now = AP_HAL::millis(); if (now - last_video_ms < 1000) { - AP_Notify::flags.video_recording = 1; + AP_Notify::flags.video_recording = true; } else { - AP_Notify::flags.video_recording = 0; + AP_Notify::flags.video_recording = false; } if (red_blink_count > 0 && green_blink_count > 0) { @@ -954,7 +954,7 @@ void ToyMode::handle_message(const mavlink_message_t &msg) green_blink_count = 1; last_video_ms = AP_HAL::millis(); // immediately update AP_Notify recording flag - AP_Notify::flags.video_recording = 1; + AP_Notify::flags.video_recording = true; } else if (strncmp(m.name, "WIFICHAN", 10) == 0) { #if HAL_RCINPUT_WITH_AP_RADIO AP_Radio *radio = AP_Radio::get_singleton(); diff --git a/ArduCopter/toy_mode.h b/ArduCopter/toy_mode.h index f955462b91478..13bb12454f9f6 100644 --- a/ArduCopter/toy_mode.h +++ b/ArduCopter/toy_mode.h @@ -1,5 +1,9 @@ #pragma once +#include +#include +#include "mode.h" + /* class to support "toy" mode for simplified user interaction for large volume consumer vehicles diff --git a/ArduCopter/version.h b/ArduCopter/version.h index 35ee178e09076..aca789abdc19f 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -6,13 +6,13 @@ #include "ap_version.h" -#define THISFIRMWARE "ArduCopter V4.4.0-dev" +#define THISFIRMWARE "ArduCopter V4.5.0-dev" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,4,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 4,5,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 4 -#define FW_MINOR 4 +#define FW_MINOR 5 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index 46e52620876dc..b4e868b2ff288 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -14,6 +14,17 @@ const AP_Param::GroupInfo AP_Arming_Plane::var_info[] = { // index 3 was RUDDER and should not be used +#if AP_PLANE_BLACKBOX_LOGGING + // @Param: BBOX_SPD + // @DisplayName: Blackbox speed + // @Description: This is a 3D GPS speed threshold above which we will force arm the vehicle to start logging. WARNING: This should only be used on a vehicle with no propellers attached to the flight controller and when the flight controller is not in control of the vehicle. + // @Units: m/s + // @Increment: 1 + // @Range: 1 20 + // @User: Advanced + AP_GROUPINFO("BBOX_SPD", 4, AP_Arming_Plane, blackbox_speed, 5), +#endif // AP_PLANE_BLACKBOX_LOGGING + AP_GROUPEND }; @@ -42,7 +53,7 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure) } //are arming checks disabled? if (checks_to_perform == 0) { - return true; + return mandatory_checks(display_failure); } if (hal.util->was_watchdog_armed()) { // on watchdog reset bypass arming checks to allow for @@ -91,27 +102,19 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure) ret = false; } + ret &= rc_received_if_enabled_check(display_failure); + #if HAL_QUADPLANE_ENABLED ret &= quadplane_checks(display_failure); #endif - if (plane.control_mode == &plane.mode_auto && plane.mission.num_commands() <= 1) { - check_failed(display_failure, "No mission loaded"); - ret = false; - } - // check adsb avoidance failsafe if (plane.failsafe.adsb) { check_failed(display_failure, "ADSB threat detected"); ret = false; } - if (SRV_Channels::get_emergency_stop()) { - check_failed(display_failure,"Motors Emergency Stopped"); - ret = false; - } - - if (plane.g2.flight_options & FlightOptions::CENTER_THROTTLE_TRIM){ + if (plane.flight_option_enabled(FlightOptions::CENTER_THROTTLE_TRIM)){ int16_t trim = plane.channel_throttle->get_radio_trim(); if (trim < 1250 || trim > 1750) { check_failed(display_failure, "Throttle trim not near center stick(%u)",trim ); @@ -134,6 +137,19 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure) return ret; } +bool AP_Arming_Plane::mandatory_checks(bool display_failure) +{ + bool ret = true; + + ret &= rc_received_if_enabled_check(display_failure); + + // Call parent class checks + ret &= AP_Arming::mandatory_checks(display_failure); + + return ret; +} + + #if HAL_QUADPLANE_ENABLED bool AP_Arming_Plane::quadplane_checks(bool display_failure) { @@ -202,6 +218,11 @@ bool AP_Arming_Plane::quadplane_checks(bool display_failure) ret = false; } + if ((plane.quadplane.tailsitter.enable > 0) && (plane.quadplane.q_fwd_thr_use != QuadPlane::FwdThrUse::OFF)) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, "set Q_FWD_THR_USE to 0"); + ret = false; + } + return ret; } #endif // HAL_QUADPLANE_ENABLED @@ -298,7 +319,7 @@ bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_c // rising edge of delay_arming oneshot delay_arming = true; - gcs().send_text(MAV_SEVERITY_INFO, "Throttle armed"); + send_arm_disarm_statustext("Throttle armed"); return true; } @@ -309,7 +330,7 @@ bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_c bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_checks) { if (do_disarm_checks && - (method == AP_Arming::Method::MAVLINK || + (AP_Arming::method_is_GCS(method) || method == AP_Arming::Method::RUDDER)) { if (plane.is_flying()) { // don't allow mavlink or rudder disarm while flying @@ -358,8 +379,8 @@ bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_chec // re-initialize speed variable used in AUTO and GUIDED for // DO_CHANGE_SPEED commands plane.new_airspeed_cm = -1; - - gcs().send_text(MAV_SEVERITY_INFO, "Throttle disarmed"); + + send_arm_disarm_statustext("Throttle disarmed"); return true; } @@ -382,6 +403,27 @@ void AP_Arming_Plane::update_soft_armed() delay_arming = false; } + +#if AP_PLANE_BLACKBOX_LOGGING + if (blackbox_speed > 0) { + const float speed3d = plane.gps.status() >= AP_GPS::GPS_OK_FIX_3D?plane.gps.velocity().length():0; + const uint32_t now = AP_HAL::millis(); + if (speed3d > blackbox_speed) { + last_over_3dspeed_ms = now; + } + if (!_armed && speed3d > blackbox_speed) { + // force safety on so we don't run motors + hal.rcout->force_safety_on(); + AP_Param::set_by_name("RC_PROTOCOLS", 0); + arm(Method::BLACKBOX, false); + gcs().send_text(MAV_SEVERITY_WARNING, "BlackBox: arming at %.1f m/s", speed3d); + } + if (_armed && now - last_over_3dspeed_ms > 20000U) { + gcs().send_text(MAV_SEVERITY_WARNING, "BlackBox: disarming at %.1f m/s", speed3d); + disarm(Method::BLACKBOX, false); + } + } +#endif } /* @@ -421,3 +463,21 @@ bool AP_Arming_Plane::mission_checks(bool report) #endif return ret; } + +// Checks rc has been received if it is configured to be used +bool AP_Arming_Plane::rc_received_if_enabled_check(bool display_failure) +{ + if (rc().enabled_protocols() == 0) { + // No protocols enabled, will never get RC, don't block arming + return true; + } + + // If RC failsafe is enabled we must receive RC before arming + if ((Plane::ThrFailsafe(plane.g.throttle_fs_enabled.get()) == Plane::ThrFailsafe::Enabled) && + !(rc().has_had_rc_receiver() || rc().has_had_rc_override())) { + check_failed(display_failure, "Waiting for RC"); + return false; + } + + return true; +} diff --git a/ArduPlane/AP_Arming.h b/ArduPlane/AP_Arming.h index afce68ae11695..b340ecdfc2f81 100644 --- a/ArduPlane/AP_Arming.h +++ b/ArduPlane/AP_Arming.h @@ -2,6 +2,10 @@ #include +#ifndef AP_PLANE_BLACKBOX_LOGGING +#define AP_PLANE_BLACKBOX_LOGGING 0 +#endif + /* a plane specific arming class */ @@ -29,6 +33,9 @@ class AP_Arming_Plane : public AP_Arming void update_soft_armed(); bool get_delay_arming() const { return delay_arming; }; + // mandatory checks that cannot be bypassed. This function will only be called if ARMING_CHECK is zero or arming forced + bool mandatory_checks(bool display_failure) override; + protected: bool ins_checks(bool report) override; bool terrain_database_required() const override; @@ -36,10 +43,18 @@ class AP_Arming_Plane : public AP_Arming bool quadplane_checks(bool display_failure); bool mission_checks(bool report) override; + // Checks rc has been received if it is configured to be used + bool rc_received_if_enabled_check(bool display_failure); + private: void change_arm_state(void); // oneshot with duration AP_ARMING_DELAY_MS used by quadplane to delay spoolup after arming: // ignored unless OPTION_DELAY_ARMING or OPTION_TILT_DISARMED is set bool delay_arming; + +#if AP_PLANE_BLACKBOX_LOGGING + AP_Float blackbox_speed; + uint32_t last_over_3dspeed_ms; +#endif }; diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index a724c4af4a0f4..36bc4f5b53e09 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -63,7 +63,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK(check_short_failsafe, 50, 100, 9), SCHED_TASK(update_speed_height, 50, 200, 12), SCHED_TASK(update_throttle_hover, 100, 90, 24), - SCHED_TASK(read_control_switch, 7, 100, 27), + SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&plane.g2.rc_channels, read_mode_switch, 7, 100, 27), SCHED_TASK(update_GPS_50Hz, 50, 300, 30), SCHED_TASK(update_GPS_10Hz, 10, 400, 33), SCHED_TASK(navigate, 10, 150, 36), @@ -77,10 +77,11 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK(ekf_check, 10, 75, 54), SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_receive, 300, 500, 57), SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_send, 300, 750, 60), +#if AP_SERVORELAYEVENTS_ENABLED SCHED_TASK_CLASS(AP_ServoRelayEvents, &plane.ServoRelayEvents, update_events, 50, 150, 63), +#endif SCHED_TASK_CLASS(AP_BattMonitor, &plane.battery, read, 10, 300, 66), SCHED_TASK_CLASS(AP_Baro, &plane.barometer, accumulate, 50, 150, 69), - SCHED_TASK_CLASS(AP_Notify, &plane.notify, update, 50, 300, 72), SCHED_TASK(read_rangefinder, 50, 100, 78), #if AP_ICENGINE_ENABLED SCHED_TASK_CLASS(AP_ICEngine, &plane.g2.ice_control, update, 10, 100, 81), @@ -203,7 +204,13 @@ void Plane::ahrs_update() */ void Plane::update_speed_height(void) { - if (control_mode->does_auto_throttle()) { + bool should_run_tecs = control_mode->does_auto_throttle(); +#if HAL_QUADPLANE_ENABLED + if (quadplane.should_disable_TECS()) { + should_run_tecs = false; + } +#endif + if (should_run_tecs) { // Call TECS 50Hz update. Note that we call this regardless of // throttle suppressed, as this needs to be running for // takeoff detection @@ -238,7 +245,12 @@ void Plane::update_logging10(void) ahrs.Write_AOA_SSA(); } else if (log_faster) { ahrs.Write_AOA_SSA(); - } + } +#if HAL_MOUNT_ENABLED + if (should_log(MASK_LOG_CAMERA)) { + camera_mount.write_log(); + } +#endif } /* @@ -255,7 +267,9 @@ void Plane::update_logging25(void) if (should_log(MASK_LOG_CTUN)) { Log_Write_Control_Tuning(); - AP::ins().write_notch_log_messages(); + if (!should_log(MASK_LOG_NOTCH_FULLRATE)) { + AP::ins().write_notch_log_messages(); + } #if HAL_GYROFFT_ENABLED gyro_fft.write_log_messages(); #endif @@ -303,7 +317,7 @@ void Plane::one_second_loop() adsb.set_max_speed(aparm.airspeed_max); #endif - if (g2.flight_options & FlightOptions::ENABLE_DEFAULT_AIRSPEED) { + if (flight_option_enabled(FlightOptions::ENABLE_DEFAULT_AIRSPEED)) { // use average of min and max airspeed as default airspeed fusion with high variance ahrs.writeDefaultAirSpeed((float)((aparm.airspeed_min + aparm.airspeed_max)/2), (float)((aparm.airspeed_max - aparm.airspeed_min)/2)); @@ -343,6 +357,16 @@ void Plane::one_second_loop() !is_equal(G_Dt, scheduler.get_loop_period_s())) { INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } + + const float loop_rate = AP::scheduler().get_filtered_loop_rate_hz(); +#if HAL_QUADPLANE_ENABLED + if (quadplane.available()) { + quadplane.attitude_control->set_notch_sample_rate(loop_rate); + } +#endif + rollController.set_notch_sample_rate(loop_rate); + pitchController.set_notch_sample_rate(loop_rate); + yawController.set_notch_sample_rate(loop_rate); } void Plane::three_hz_loop() @@ -511,12 +535,6 @@ void Plane::update_alt() { barometer.update(); -#if HAL_QUADPLANE_ENABLED - if (quadplane.available()) { - quadplane.motors->set_air_density_ratio(barometer.get_air_density_ratio()); - } -#endif - // calculate the sink rate. float sink_rate; Vector3f vel; @@ -543,7 +561,14 @@ void Plane::update_alt() } #endif - if (control_mode->does_auto_throttle() && !throttle_suppressed) { + bool should_run_tecs = control_mode->does_auto_throttle(); +#if HAL_QUADPLANE_ENABLED + if (quadplane.should_disable_TECS()) { + should_run_tecs = false; + } +#endif + + if (should_run_tecs && !throttle_suppressed) { float distance_beyond_land_wp = 0; if (flight_stage == AP_FixedWing::FlightStage::LAND && @@ -553,7 +578,7 @@ void Plane::update_alt() tecs_target_alt_cm = relative_target_altitude_cm(); - if (control_mode == &mode_rtl && !rtl.done_climb && (g2.rtl_climb_min > 0 || (plane.g2.flight_options & FlightOptions::CLIMB_BEFORE_TURN))) { + if (control_mode == &mode_rtl && !rtl.done_climb && (g2.rtl_climb_min > 0 || (plane.flight_option_enabled(FlightOptions::CLIMB_BEFORE_TURN)))) { // ensure we do the initial climb in RTL. We add an extra // 10m in the demanded height to push TECS to climb // quickly @@ -821,12 +846,7 @@ bool Plane::get_target_location(Location& target_loc) */ bool Plane::update_target_location(const Location &old_loc, const Location &new_loc) { - if (!old_loc.same_latlon_as(next_WP_loc)) { - return false; - } - ftype alt_diff; - if (!old_loc.get_alt_distance(next_WP_loc, alt_diff) || - !is_zero(alt_diff)) { + if (!old_loc.same_loc_as(next_WP_loc)) { return false; } next_WP_loc = new_loc; @@ -860,9 +880,30 @@ bool Plane::set_land_descent_rate(float descent_rate) #endif return false; } - #endif // AP_SCRIPTING_ENABLED +// returns true if vehicle is landing. +bool Plane::is_landing() const +{ +#if HAL_QUADPLANE_ENABLED + if (plane.quadplane.in_vtol_land_descent()) { + return true; + } +#endif + return control_mode->is_landing(); +} + +// returns true if vehicle is taking off. +bool Plane::is_taking_off() const +{ +#if HAL_QUADPLANE_ENABLED + if (plane.quadplane.in_vtol_takeoff()) { + return true; + } +#endif + return control_mode->is_taking_off(); +} + // correct AHRS pitch for TRIM_PITCH_CD in non-VTOL modes, and return VTOL view in VTOL void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const { @@ -875,7 +916,7 @@ void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const #endif pitch = ahrs.pitch; roll = ahrs.roll; - if (!(g2.flight_options & FlightOptions::OSD_REMOVE_TRIM_PITCH_CD)) { // correct for TRIM_PITCH_CD + if (!(flight_option_enabled(FlightOptions::OSD_REMOVE_TRIM_PITCH_CD))) { // correct for TRIM_PITCH_CD pitch -= g.pitch_trim_cd * 0.01 * DEG_TO_RAD; } } @@ -892,4 +933,10 @@ void Plane::update_current_loc(void) relative_altitude *= -1.0f; } +// check if FLIGHT_OPTION is enabled +bool Plane::flight_option_enabled(FlightOptions flight_option) const +{ + return g2.flight_options & flight_option; +} + AP_HAL_MAIN_CALLBACKS(&plane); diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 0b51098b43ce0..3c59d8a31522b 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -32,7 +32,7 @@ float Plane::calc_speed_scaler(void) speed_scaler = MIN(speed_scaler, new_scaler); // we also decay the integrator to prevent an integrator from before - // we were at low speed persistint at high speed + // we were at low speed persistent at high speed rollController.decay_I(); pitchController.decay_I(); yawController.decay_I(); @@ -49,9 +49,9 @@ float Plane::calc_speed_scaler(void) // no speed estimate and not armed, use a unit scaling speed_scaler = 1; } - if (!plane.ahrs.airspeed_sensor_enabled() && - (plane.g2.flight_options & FlightOptions::SURPRESS_TKOFF_SCALING) && - (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF)) { //scaling is surpressed during climb phase of automatic takeoffs with no airspeed sensor being used due to problems with inaccurate airspeed estimates + if (!plane.ahrs.using_airspeed_sensor() && + (plane.flight_option_enabled(FlightOptions::SURPRESS_TKOFF_SCALING)) && + (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF)) { //scaling is suppressed during climb phase of automatic takeoffs with no airspeed sensor being used due to problems with inaccurate airspeed estimates return MIN(speed_scaler, 1.0f) ; } return speed_scaler; @@ -131,6 +131,16 @@ float Plane::stabilize_roll_get_roll_out() if (!quadplane.use_fw_attitude_controllers()) { // use the VTOL rate for control, to ensure consistency const auto &pid_info = quadplane.attitude_control->get_rate_roll_pid().get_pid_info(); + + // scale FF to angle P + if (quadplane.option_is_set(QuadPlane::OPTION::SCALE_FF_ANGLE_P)) { + const float mc_angR = quadplane.attitude_control->get_angle_roll_p().kP() + * quadplane.attitude_control->get_last_angle_P_scale().x; + if (is_positive(mc_angR)) { + rollController.set_ff_scale(MIN(1.0, 1.0 / (mc_angR * rollController.tau()))); + } + } + const float roll_out = rollController.get_rate_out(degrees(pid_info.target), speed_scaler); /* when slaving fixed wing control to VTOL control we need to decay the integrator to prevent opposing integrators balancing between the two controllers @@ -145,7 +155,7 @@ float Plane::stabilize_roll_get_roll_out() disable_integrator = true; } return rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor, speed_scaler, disable_integrator, - ground_mode && !(plane.g2.flight_options & FlightOptions::DISABLE_GROUND_PID_SUPPRESSION)); + ground_mode && !(plane.flight_option_enabled(FlightOptions::DISABLE_GROUND_PID_SUPPRESSION))); } /* @@ -174,6 +184,16 @@ float Plane::stabilize_pitch_get_pitch_out() if (!quadplane.use_fw_attitude_controllers()) { // use the VTOL rate for control, to ensure consistency const auto &pid_info = quadplane.attitude_control->get_rate_pitch_pid().get_pid_info(); + + // scale FF to angle P + if (quadplane.option_is_set(QuadPlane::OPTION::SCALE_FF_ANGLE_P)) { + const float mc_angP = quadplane.attitude_control->get_angle_pitch_p().kP() + * quadplane.attitude_control->get_last_angle_P_scale().y; + if (is_positive(mc_angP)) { + pitchController.set_ff_scale(MIN(1.0, 1.0 / (mc_angP * pitchController.tau()))); + } + } + const int32_t pitch_out = pitchController.get_rate_out(degrees(pid_info.target), speed_scaler); /* when slaving fixed wing control to VTOL control we need to decay the integrator to prevent opposing integrators balancing between the two controllers @@ -208,23 +228,29 @@ float Plane::stabilize_pitch_get_pitch_out() } return pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, speed_scaler, disable_integrator, - ground_mode && !(plane.g2.flight_options & FlightOptions::DISABLE_GROUND_PID_SUPPRESSION)); + ground_mode && !(plane.flight_option_enabled(FlightOptions::DISABLE_GROUND_PID_SUPPRESSION))); } /* this gives the user control of the aircraft in stabilization modes, only used in Stabilize Mode + to be moved to mode_stabilize.cpp in future */ -void Plane::stabilize_stick_mixing_direct() +void ModeStabilize::stabilize_stick_mixing_direct() { - if (!stick_mixing_enabled()) { + if (!plane.stick_mixing_enabled()) { return; } +#if HAL_QUADPLANE_ENABLED + if (!plane.quadplane.allow_stick_mixing()) { + return; + } +#endif float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron); - aileron = channel_roll->stick_mixing(aileron); + aileron = plane.channel_roll->stick_mixing(aileron); SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron); float elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator); - elevator = channel_pitch->stick_mixing(elevator); + elevator = plane.channel_pitch->stick_mixing(elevator); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator); } @@ -249,6 +275,7 @@ void Plane::stabilize_stick_mixing_fbw() #if QAUTOTUNE_ENABLED control_mode == &mode_qautotune || #endif + !quadplane.allow_stick_mixing() || #endif // HAL_QUADPLANE_ENABLED control_mode == &mode_training) { return; @@ -259,7 +286,7 @@ void Plane::stabilize_stick_mixing_fbw() // non-linear and ends up as 2x the maximum, to ensure that // the user can direct the plane in any direction with stick // mixing. - float roll_input = channel_roll->norm_input(); + float roll_input = channel_roll->norm_input_dz(); if (roll_input > 0.5f) { roll_input = (3*roll_input - 1); } else if (roll_input < -0.5f) { @@ -268,12 +295,12 @@ void Plane::stabilize_stick_mixing_fbw() nav_roll_cd += roll_input * roll_limit_cd; nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); - if ((control_mode == &mode_loiter) && (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL)) { + if ((control_mode == &mode_loiter) && (plane.flight_option_enabled(FlightOptions::ENABLE_LOITER_ALT_CONTROL))) { // loiter is using altitude control based on the pitch stick, don't use it again here return; } - float pitch_input = channel_pitch->norm_input(); + float pitch_input = channel_pitch->norm_input_dz(); if (pitch_input > 0.5f) { pitch_input = (3*pitch_input - 1); } else if (pitch_input < -0.5f) { @@ -300,38 +327,57 @@ void Plane::stabilize_stick_mixing_fbw() */ void Plane::stabilize_yaw() { + bool ground_steering = false; if (landing.is_flaring()) { // in flaring then enable ground steering - steering_control.ground_steering = true; + ground_steering = true; } else { // otherwise use ground steering when no input control and we // are below the GROUND_STEER_ALT - steering_control.ground_steering = (channel_roll->get_control_in() == 0 && + ground_steering = (channel_roll->get_control_in() == 0 && fabsf(relative_altitude) < g.ground_steer_alt); if (!landing.is_ground_steering_allowed()) { // don't use ground steering on landing approach - steering_control.ground_steering = false; + ground_steering = false; } } /* - first calculate steering_control.steering for a nose or tail + first calculate steering for a nose or tail wheel. We use "course hold" mode for the rudder when either performing a flare (when the wings are held level) or when in course hold in FBWA mode (when we are below GROUND_STEER_ALT) */ + float steering_output = 0.0; if (landing.is_flaring() || - (steer_state.hold_course_cd != -1 && steering_control.ground_steering)) { - calc_nav_yaw_course(); - } else if (steering_control.ground_steering) { - calc_nav_yaw_ground(); + (steer_state.hold_course_cd != -1 && ground_steering)) { + steering_output = calc_nav_yaw_course(); + } else if (ground_steering) { + steering_output = calc_nav_yaw_ground(); } /* - now calculate steering_control.rudder for the rudder + now calculate rudder for the rudder */ - calc_nav_yaw_coordinated(); + const float rudder_output = calc_nav_yaw_coordinated(); + + if (!ground_steering) { + // Not doing ground steering, output rudder on steering channel + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder_output); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, rudder_output); + + } else if (!SRV_Channels::function_assigned(SRV_Channel::k_steering)) { + // Ground steering active but no steering output configured, output steering on rudder channel + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, steering_output); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering_output); + + } else { + // Ground steering with both steering and rudder channels + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder_output); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering_output); + } + } /* @@ -339,35 +385,21 @@ void Plane::stabilize_yaw() */ void Plane::stabilize() { - if (control_mode == &mode_manual) { - // reset steering controls - steer_state.locked_course = false; - steer_state.locked_course_err = 0; - return; - } - uint32_t now = AP_HAL::millis(); - bool allow_stick_mixing = true; #if HAL_QUADPLANE_ENABLED if (quadplane.available()) { - quadplane.transition->set_FW_roll_pitch(nav_pitch_cd, nav_roll_cd, allow_stick_mixing); + quadplane.transition->set_FW_roll_pitch(nav_pitch_cd, nav_roll_cd); } #endif if (now - last_stabilize_ms > 2000) { - // if we haven't run the rate controllers for 2 seconds then - // reset the integrators - rollController.reset_I(); - pitchController.reset_I(); - yawController.reset_I(); - - // and reset steering controls - steer_state.locked_course = false; - steer_state.locked_course_err = 0; + // if we haven't run the rate controllers for 2 seconds then reset + control_mode->reset_controllers(); } last_stabilize_ms = now; - if (control_mode == &mode_training) { + if (control_mode == &mode_training || + control_mode == &mode_manual) { plane.control_mode->run(); #if AP_SCRIPTING_ENABLED } else if (nav_scripting_active()) { @@ -377,47 +409,20 @@ void Plane::stabilize() const float elevator = pitchController.get_rate_out(nav_scripting.pitch_rate_dps, speed_scaler); SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator); + float rudder = 0; if (yawController.rate_control_enabled()) { - float rudder = nav_scripting.rudder_offset_pct * 45; + rudder = nav_scripting.rudder_offset_pct * 45; if (nav_scripting.run_yaw_rate_controller) { rudder += yawController.get_rate_out(nav_scripting.yaw_rate_dps, speed_scaler, false); } else { yawController.reset_I(); } - steering_control.rudder = rudder; - } -#endif - } else if (control_mode == &mode_acro) { - plane.control_mode->run(); - } else if (control_mode == &mode_stabilize) { - stabilize_roll(); - stabilize_pitch(); - if (allow_stick_mixing) { - stabilize_stick_mixing_direct(); - } - stabilize_yaw(); -#if HAL_QUADPLANE_ENABLED - } else if (control_mode->is_vtol_mode() && !quadplane.tailsitter.in_vtol_transition(now)) { - // run controlers specific to this mode - plane.control_mode->run(); - - // we also stabilize using fixed wing surfaces - if (plane.control_mode->mode_number() == Mode::Number::QACRO) { - plane.mode_acro.run(); - } else { - stabilize_roll(); - stabilize_pitch(); } + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, rudder); #endif } else { - // Direct stick mixing functionality has been removed, so as not to remove all stick mixing from the user completely - // the old direct option is now used to enable fbw mixing, this is easier than doing a param conversion. - if (allow_stick_mixing && ((g.stick_mixing == StickMixing::FBW) || (g.stick_mixing == StickMixing::DIRECT_REMOVED))) { - stabilize_stick_mixing_fbw(); - } - stabilize_roll(); - stabilize_pitch(); - stabilize_yaw(); + plane.control_mode->run(); } /* @@ -453,14 +458,6 @@ void Plane::calc_throttle() } float commanded_throttle = TECS_controller.get_throttle_demand(); - - // Received an external msg that guides throttle in the last 3 seconds? - if (control_mode->is_guided_mode() && - plane.guided_state.last_forced_throttle_ms > 0 && - millis() - plane.guided_state.last_forced_throttle_ms < 3000) { - commanded_throttle = plane.guided_state.forced_throttle; - } - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, commanded_throttle); } @@ -471,7 +468,7 @@ void Plane::calc_throttle() /* calculate yaw control for coordinated flight */ -void Plane::calc_nav_yaw_coordinated() +int16_t Plane::calc_nav_yaw_coordinated() { const float speed_scaler = get_speed_scaler(); bool disable_integrator = false; @@ -489,7 +486,7 @@ void Plane::calc_nav_yaw_coordinated() // user is doing an AUTOTUNE with yaw rate control const float rudd_expo = rudder_in_expo(true); const float yaw_rate = (rudd_expo/SERVO_MAX) * g.acro_yaw_rate; - // add in the corrdinated turn yaw rate to make it easier to fly while tuning the yaw rate controller + // add in the coordinated turn yaw rate to make it easier to fly while tuning the yaw rate controller const float coordination_yaw_rate = degrees(GRAVITY_MSS * tanf(radians(nav_roll_cd*0.01f))/MAX(aparm.airspeed_min,smoothed_airspeed)); commanded_rudder = yawController.get_rate_out(yaw_rate+coordination_yaw_rate, speed_scaler, false); using_rate_controller = true; @@ -505,35 +502,35 @@ void Plane::calc_nav_yaw_coordinated() commanded_rudder += rudder_in; } - steering_control.rudder = constrain_int16(commanded_rudder, -4500, 4500); - if (!using_rate_controller) { /* When not running the yaw rate controller, we need to reset the rate */ yawController.reset_rate_PID(); } + + return constrain_int16(commanded_rudder, -4500, 4500); } /* calculate yaw control for ground steering with specific course */ -void Plane::calc_nav_yaw_course(void) +int16_t Plane::calc_nav_yaw_course(void) { // holding a specific navigation course on the ground. Used in // auto-takeoff and landing int32_t bearing_error_cd = nav_controller->bearing_error_cd(); - steering_control.steering = steerController.get_steering_out_angle_error(bearing_error_cd); + int16_t steering = steerController.get_steering_out_angle_error(bearing_error_cd); if (stick_mixing_enabled()) { - steering_control.steering = channel_rudder->stick_mixing(steering_control.steering); + steering = channel_rudder->stick_mixing(steering); } - steering_control.steering = constrain_int16(steering_control.steering, -4500, 4500); + return constrain_int16(steering, -4500, 4500); } /* calculate yaw control for ground steering */ -void Plane::calc_nav_yaw_ground(void) +int16_t Plane::calc_nav_yaw_ground(void) { if (gps.ground_speed() < 1 && is_zero(get_throttle_input()) && @@ -542,8 +539,7 @@ void Plane::calc_nav_yaw_ground(void) // manual rudder control while still steer_state.locked_course = false; steer_state.locked_course_err = 0; - steering_control.steering = rudder_input(); - return; + return rudder_input(); } // if we haven't been steering for 1s then clear locked course @@ -570,15 +566,16 @@ void Plane::calc_nav_yaw_ground(void) } } + int16_t steering; if (!steer_state.locked_course) { // use a rate controller at the pilot specified rate - steering_control.steering = steerController.get_steering_out_rate(steer_rate); + steering = steerController.get_steering_out_rate(steer_rate); } else { // use a error controller on the summed error int32_t yaw_error_cd = -ToDeg(steer_state.locked_course_err)*100; - steering_control.steering = steerController.get_steering_out_angle_error(yaw_error_cd); + steering = steerController.get_steering_out_angle_error(yaw_error_cd); } - steering_control.steering = constrain_int16(steering_control.steering, -4500, 4500); + return constrain_int16(steering, -4500, 4500); } @@ -587,17 +584,7 @@ void Plane::calc_nav_yaw_ground(void) */ void Plane::calc_nav_pitch() { - // Calculate the Pitch of the plane - // -------------------------------- int32_t commanded_pitch = TECS_controller.get_pitch_demand(); - - // Received an external msg that guides roll in the last 3 seconds? - if (control_mode->is_guided_mode() && - plane.guided_state.last_forced_rpy_ms.y > 0 && - millis() - plane.guided_state.last_forced_rpy_ms.y < 3000) { - commanded_pitch = plane.guided_state.forced_rpy_cd.y; - } - nav_pitch_cd = constrain_int32(commanded_pitch, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); } @@ -608,43 +595,6 @@ void Plane::calc_nav_pitch() void Plane::calc_nav_roll() { int32_t commanded_roll = nav_controller->nav_roll_cd(); - - // Received an external msg that guides roll in the last 3 seconds? - if (control_mode->is_guided_mode() && - plane.guided_state.last_forced_rpy_ms.x > 0 && - millis() - plane.guided_state.last_forced_rpy_ms.x < 3000) { - commanded_roll = plane.guided_state.forced_rpy_cd.x; -#if OFFBOARD_GUIDED == ENABLED - // guided_state.target_heading is radians at this point between -pi and pi ( defaults to -4 ) - } else if ((control_mode == &mode_guided) && (guided_state.target_heading_type != GUIDED_HEADING_NONE) ) { - uint32_t tnow = AP_HAL::millis(); - float delta = (tnow - guided_state.target_heading_time_ms) * 1e-3f; - guided_state.target_heading_time_ms = tnow; - - float error = 0.0f; - if (guided_state.target_heading_type == GUIDED_HEADING_HEADING) { - error = wrap_PI(guided_state.target_heading - AP::ahrs().yaw); - } else { - Vector2f groundspeed = AP::ahrs().groundspeed_vector(); - error = wrap_PI(guided_state.target_heading - atan2f(-groundspeed.y, -groundspeed.x) + M_PI); - } - - float bank_limit = degrees(atanf(guided_state.target_heading_accel_limit/GRAVITY_MSS)) * 1e2f; - - g2.guidedHeading.update_error(error, delta); // push error into AC_PID , possible improvement is to use update_all instead.? - - float i = g2.guidedHeading.get_i(); // get integrator TODO - if (((is_negative(error) && !guided_state.target_heading_limit_low) || (is_positive(error) && !guided_state.target_heading_limit_high))) { - i = g2.guidedHeading.get_i(); - } - - float desired = g2.guidedHeading.get_p() + i + g2.guidedHeading.get_d(); - guided_state.target_heading_limit_low = (desired <= -bank_limit); - guided_state.target_heading_limit_high = (desired >= bank_limit); - commanded_roll = constrain_float(desired, -bank_limit, bank_limit); -#endif // OFFBOARD_GUIDED == ENABLED - } - nav_roll_cd = constrain_int32(commanded_roll, -roll_limit_cd, roll_limit_cd); update_load_factor(); } @@ -708,11 +658,11 @@ void Plane::update_load_factor(void) nav_roll_cd = constrain_int32(nav_roll_cd, -2500, 2500); roll_limit_cd = MIN(roll_limit_cd, 2500); } else if (max_load_factor < aerodynamic_load_factor) { - // the demanded nav_roll would take us past the aerodymamic + // the demanded nav_roll would take us past the aerodynamic // load limit. Limit our roll to a bank angle that will keep // the load within what the airframe can handle. We always // allow at least 25 degrees of roll however, to ensure the - // aircraft can be maneuvered with a bad airspeed estimate. At + // aircraft can be manoeuvred with a bad airspeed estimate. At // 25 degrees the load factor is 1.1 (10%) int32_t roll_limit = degrees(acosf(sq(1.0f / max_load_factor)))*100; if (roll_limit < 2500) { diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index b75509936d17e..9427b99e0cdac 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -133,7 +133,7 @@ void GCS_MAVLINK_Plane::send_attitude() const float p = ahrs.pitch; float y = ahrs.yaw; - if (!(plane.g2.flight_options & FlightOptions::GCS_REMOVE_TRIM_PITCH_CD)) { + if (!(plane.flight_option_enabled(FlightOptions::GCS_REMOVE_TRIM_PITCH_CD))) { p -= radians(plane.g.pitch_trim_cd * 0.01f); } @@ -157,6 +157,36 @@ void GCS_MAVLINK_Plane::send_attitude() const omega.z); } +void GCS_MAVLINK_Plane::send_attitude_target() +{ +#if HAL_QUADPLANE_ENABLED + // Check if the attitude target is valid for reporting + const uint32_t now = AP_HAL::millis(); + if (now - plane.quadplane.last_att_control_ms > 100) { + return; + } + + const Quaternion quat = plane.quadplane.attitude_control->get_attitude_target_quat(); + const Vector3f& ang_vel = plane.quadplane.attitude_control->get_attitude_target_ang_vel(); + const float throttle = plane.quadplane.attitude_control->get_throttle_in(); + + const float quat_out[4] {quat.q1, quat.q2, quat.q3, quat.q4}; + + const uint16_t typemask = 0; + + mavlink_msg_attitude_target_send( + chan, + now, // time since boot (ms) + typemask, // Bitmask that tells the system what control dimensions should be ignored by the vehicle + quat_out, // Target attitude quaternion [w, x, y, z] order, zero-rotation is [1, 0, 0, 0], unit-length + ang_vel.x, // bodyframe target roll rate (rad/s) + ang_vel.y, // bodyframe target pitch rate (rad/s) + ang_vel.z, // bodyframe yaw rate (rad/s) + throttle); // Collective thrust, normalized to 0 .. 1 + +#endif // HAL_QUADPLANE_ENABLED +} + void GCS_MAVLINK_Plane::send_aoa_ssa() { AP_AHRS &ahrs = AP::ahrs(); @@ -588,15 +618,21 @@ static const ap_message STREAM_RAW_SENSORS_msgs[] = { static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { MSG_SYS_STATUS, MSG_POWER_STATUS, +#if HAL_WITH_MCU_MONITORING MSG_MCU_STATUS, +#endif MSG_MEMINFO, MSG_CURRENT_WAYPOINT, MSG_GPS_RAW, MSG_GPS_RTK, +#if GPS_MAX_RECEIVERS > 1 MSG_GPS2_RAW, MSG_GPS2_RTK, +#endif MSG_NAV_CONTROLLER_OUTPUT, +#if AP_FENCE_ENABLED MSG_FENCE_STATUS, +#endif MSG_POSITION_TARGET_GLOBAL_INT, }; static const ap_message STREAM_POSITION_msgs[] = { @@ -613,7 +649,9 @@ static const ap_message STREAM_RC_CHANNELS_msgs[] = { }; static const ap_message STREAM_EXTRA1_msgs[] = { MSG_ATTITUDE, +#if AP_SIM_ENABLED MSG_SIMSTATE, +#endif MSG_AHRS2, #if AP_RPM_ENABLED MSG_RPM, @@ -621,7 +659,9 @@ static const ap_message STREAM_EXTRA1_msgs[] = { MSG_AOA_SSA, MSG_PID_TUNING, MSG_LANDING, +#if HAL_WITH_ESC_TELEM MSG_ESC_TELEMETRY, +#endif #if HAL_EFI_ENABLED MSG_EFI_STATUS, #endif @@ -641,11 +681,19 @@ static const ap_message STREAM_EXTRA3_msgs[] = { #if AP_TERRAIN_AVAILABLE MSG_TERRAIN, #endif +#if AP_BATTERY_ENABLED MSG_BATTERY_STATUS, +#endif +#if HAL_MOUNT_ENABLED MSG_GIMBAL_DEVICE_ATTITUDE_STATUS, +#endif +#if AP_OPTICALFLOW_ENABLED MSG_OPTICAL_FLOW, +#endif +#if COMPASS_CAL_ENABLED MSG_MAG_CAL_REPORT, MSG_MAG_CAL_PROGRESS, +#endif MSG_EKF_STATUS_REPORT, MSG_VIBRATION, }; @@ -654,7 +702,9 @@ static const ap_message STREAM_PARAMS_msgs[] = { }; static const ap_message STREAM_ADSB_msgs[] = { MSG_ADSB_VEHICLE, +#if AP_AIS_ENABLED MSG_AIS_VESSEL, +#endif }; const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = { @@ -696,7 +746,7 @@ void GCS_MAVLINK_Plane::handle_change_alt_request(AP_Mission::Mission_Command &c } -MAV_RESULT GCS_MAVLINK_Plane::handle_command_preflight_calibration(const mavlink_command_long_t &packet, const mavlink_message_t &msg) +MAV_RESULT GCS_MAVLINK_Plane::handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { plane.in_calibration = true; MAV_RESULT ret = GCS_MAVLINK::handle_command_preflight_calibration(packet, msg); @@ -711,7 +761,7 @@ void GCS_MAVLINK_Plane::packetReceived(const mavlink_status_t &status, #if HAL_ADSB_ENABLED plane.avoidance_adsb.handle_msg(msg); #endif -#if AP_SCRIPTING_ENABLED +#if AP_SCRIPTING_ENABLED && AP_FOLLOW_ENABLED // pass message to follow library plane.g2.follow.handle_msg(msg); #endif @@ -768,7 +818,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com return MAV_RESULT_DENIED; } - if (is_zero(packet.param4)) { + if (isnan(packet.param4) || is_zero(packet.param4)) { requested_position.loiter_ccw = 0; } else { requested_position.loiter_ccw = 1; @@ -917,11 +967,6 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl float new_target_heading = radians(wrap_180(packet.param2)); - // if packet is requesting us to go to the heading we are already going to, we-re already on it. - if ( (is_equal(new_target_heading,plane.guided_state.target_heading))) { // compare two floats as near-enough - return MAV_RESULT_ACCEPTED; - } - // course over ground if ( int(packet.param1) == HEADING_TYPE_COURSE_OVER_GROUND) { // compare as nearest int plane.guided_state.target_heading_type = GUIDED_HEADING_COG; @@ -950,10 +995,13 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl } -MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_int_t &packet) +MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { switch(packet.command) { + case MAV_CMD_DO_AUTOTUNE_ENABLE: + return handle_MAV_CMD_DO_AUTOTUNE_ENABLE(packet); + case MAV_CMD_DO_REPOSITION: return handle_command_int_do_reposition(packet); @@ -963,26 +1011,73 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in case MAV_CMD_GUIDED_CHANGE_HEADING: return handle_command_int_guided_slew_commands(packet); +#if AP_SCRIPTING_ENABLED && AP_FOLLOW_ENABLED case MAV_CMD_DO_FOLLOW: -#if AP_SCRIPTING_ENABLED // param1: sysid of target to follow if ((packet.param1 > 0) && (packet.param1 <= 255)) { plane.g2.follow.set_target_sysid((uint8_t)packet.param1); return MAV_RESULT_ACCEPTED; } + return MAV_RESULT_DENIED; #endif + +#if AP_ICENGINE_ENABLED + case MAV_CMD_DO_ENGINE_CONTROL: + if (!plane.g2.ice_control.engine_control(packet.param1, packet.param2, packet.param3, (uint32_t)packet.param4)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; +#endif + + case MAV_CMD_DO_CHANGE_SPEED: + return handle_command_DO_CHANGE_SPEED(packet); + +#if PARACHUTE == ENABLED + case MAV_CMD_DO_PARACHUTE: + return handle_MAV_CMD_DO_PARACHUTE(packet); +#endif + +#if HAL_QUADPLANE_ENABLED + case MAV_CMD_DO_MOTOR_TEST: + return handle_MAV_CMD_DO_MOTOR_TEST(packet); + + case MAV_CMD_DO_VTOL_TRANSITION: + return handle_command_DO_VTOL_TRANSITION(packet); + + case MAV_CMD_NAV_TAKEOFF: + return handle_command_MAV_CMD_NAV_TAKEOFF(packet); +#endif + + case MAV_CMD_DO_GO_AROUND: + return plane.trigger_land_abort(packet.param1) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED; + + case MAV_CMD_DO_LAND_START: + // attempt to switch to next DO_LAND_START command in the mission + if (plane.mission.jump_to_landing_sequence()) { + plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND); + return MAV_RESULT_ACCEPTED; + } return MAV_RESULT_FAILED; - + + case MAV_CMD_MISSION_START: + plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND); + return MAV_RESULT_ACCEPTED; + + case MAV_CMD_NAV_LOITER_UNLIM: + plane.set_mode(plane.mode_loiter, ModeReason::GCS_COMMAND); + return MAV_RESULT_ACCEPTED; + + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + plane.set_mode(plane.mode_rtl, ModeReason::GCS_COMMAND); + return MAV_RESULT_ACCEPTED; + default: - return GCS_MAVLINK::handle_command_int_packet(packet); + return GCS_MAVLINK::handle_command_int_packet(packet, msg); } } -MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_long_t &packet) +MAV_RESULT GCS_MAVLINK_Plane::handle_command_DO_CHANGE_SPEED(const mavlink_command_int_t &packet) { - switch(packet.command) { - - case MAV_CMD_DO_CHANGE_SPEED: { // if we're in failsafe modes (e.g., RTL, LOITER) or in pilot // controlled modes (e.g., MANUAL, TRAINING) // this command should be ignored since it comes in from GCS @@ -993,58 +1088,75 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l return MAV_RESULT_FAILED; } - AP_Mission::Mission_Command cmd; - if (AP_Mission::mavlink_cmd_long_to_mission_cmd(packet, cmd) != MAV_MISSION_ACCEPTED) { - return MAV_RESULT_DENIED; - } - if (plane.do_change_speed(cmd)) { + if (plane.do_change_speed(packet.param1, packet.param2, packet.param3)) { return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; - } +} - case MAV_CMD_NAV_LOITER_UNLIM: - plane.set_mode(plane.mode_loiter, ModeReason::GCS_COMMAND); - return MAV_RESULT_ACCEPTED; +#if HAL_QUADPLANE_ENABLED +void GCS_MAVLINK_Plane::convert_MAV_CMD_NAV_TAKEOFF_to_COMMAND_INT(const mavlink_command_long_t &in, mavlink_command_int_t &out) +{ + // convert to MAV_FRAME_LOCAL_OFFSET_NED, "NED local tangent frame + // with origin that travels with the vehicle" + out = {}; + out.target_system = in.target_system; + out.target_component = in.target_component; + out.frame = MAV_FRAME_LOCAL_OFFSET_NED; + out.command = in.command; + // out.current = 0; + // out.autocontinue = 0; + // out.param1 = in.param1; // we only use the "z" parameter in this command: + // out.param2 = in.param2; + // out.param3 = in.param3; + // out.param4 = in.param4; + // out.x = 0; // we don't handle positioning when doing takeoffs + // out.y = 0; + out.z = -in.param7; // up -> down +} - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - plane.set_mode(plane.mode_rtl, ModeReason::GCS_COMMAND); - return MAV_RESULT_ACCEPTED; +#if AP_MAVLINK_COMMAND_LONG_ENABLED +void GCS_MAVLINK_Plane::convert_COMMAND_LONG_to_COMMAND_INT(const mavlink_command_long_t &in, mavlink_command_int_t &out, MAV_FRAME frame) +{ + switch (in.command) { + case MAV_CMD_NAV_TAKEOFF: + convert_MAV_CMD_NAV_TAKEOFF_to_COMMAND_INT(in, out); + return; + } + return GCS_MAVLINK::convert_COMMAND_LONG_to_COMMAND_INT(in, out, frame); +} +#endif // AP_MAVLINK_COMMAND_LONG_ENABLED -#if HAL_QUADPLANE_ENABLED - case MAV_CMD_NAV_TAKEOFF: { - // user takeoff only works with quadplane code for now - // param7 : altitude [metres] - float takeoff_alt = packet.param7; - if (plane.quadplane.available() && plane.quadplane.do_user_takeoff(takeoff_alt)) { - return MAV_RESULT_ACCEPTED; - } +MAV_RESULT GCS_MAVLINK_Plane::handle_command_MAV_CMD_NAV_TAKEOFF(const mavlink_command_int_t &packet) +{ + float takeoff_alt = packet.z; + switch (packet.frame) { + case MAV_FRAME_LOCAL_OFFSET_NED: // "NED local tangent frame with origin that travels with the vehicle" + takeoff_alt = -takeoff_alt; // down -> up + break; + default: + return MAV_RESULT_DENIED; // "is supported but has invalid parameters" + } + if (!plane.quadplane.available()) { return MAV_RESULT_FAILED; } -#endif // HAL_QUADPLANE_ENABLED - - case MAV_CMD_MISSION_START: - plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND); - return MAV_RESULT_ACCEPTED; - - case MAV_CMD_DO_LAND_START: - // attempt to switch to next DO_LAND_START command in the mission - if (plane.mission.jump_to_landing_sequence()) { - plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND); - return MAV_RESULT_ACCEPTED; - } + if (!plane.quadplane.do_user_takeoff(takeoff_alt)) { return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; +} +#endif - case MAV_CMD_DO_GO_AROUND: - return plane.trigger_land_abort(packet.param1) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED; - - case MAV_CMD_DO_AUTOTUNE_ENABLE: +MAV_RESULT GCS_MAVLINK_Plane::handle_MAV_CMD_DO_AUTOTUNE_ENABLE(const mavlink_command_int_t &packet) +{ // param1 : enable/disable plane.autotune_enable(!is_zero(packet.param1)); return MAV_RESULT_ACCEPTED; +} #if PARACHUTE == ENABLED - case MAV_CMD_DO_PARACHUTE: +MAV_RESULT GCS_MAVLINK_Plane::handle_MAV_CMD_DO_PARACHUTE(const mavlink_command_int_t &packet) +{ // configure or release parachute switch ((uint16_t)packet.param1) { case PARACHUTE_DISABLE: @@ -1071,10 +1183,13 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l break; } return MAV_RESULT_FAILED; +} #endif + #if HAL_QUADPLANE_ENABLED - case MAV_CMD_DO_MOTOR_TEST: +MAV_RESULT GCS_MAVLINK_Plane::handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command_int_t &packet) +{ // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle) // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum) // param3 : throttle (range depends upon param2) @@ -1085,37 +1200,17 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l (uint8_t)packet.param2, (uint16_t)packet.param3, packet.param4, - (uint8_t)packet.param5); + (uint8_t)packet.x); +} - case MAV_CMD_DO_VTOL_TRANSITION: +MAV_RESULT GCS_MAVLINK_Plane::handle_command_DO_VTOL_TRANSITION(const mavlink_command_int_t &packet) +{ if (!plane.quadplane.handle_do_vtol_transition((enum MAV_VTOL_STATE)packet.param1)) { return MAV_RESULT_FAILED; } return MAV_RESULT_ACCEPTED; -#endif - -#if AP_ICENGINE_ENABLED - case MAV_CMD_DO_ENGINE_CONTROL: - if (!plane.g2.ice_control.engine_control(packet.param1, packet.param2, packet.param3)) { - return MAV_RESULT_FAILED; - } - return MAV_RESULT_ACCEPTED; -#endif - -#if AP_SCRIPTING_ENABLED - case MAV_CMD_DO_FOLLOW: - // param1: sysid of target to follow - if ((packet.param1 > 0) && (packet.param1 <= 255)) { - plane.g2.follow.set_target_sysid((uint8_t)packet.param1); - return MAV_RESULT_ACCEPTED; - } - return MAV_RESULT_FAILED; -#endif - - default: - return GCS_MAVLINK::handle_command_long_packet(packet); - } } +#endif // this is called on receipt of a MANUAL_CONTROL packet and is // expected to call manual_override to override RC input on desired @@ -1300,7 +1395,7 @@ void GCS_MAVLINK_Plane::handleMessage(const mavlink_message_t &msg) } // end switch } // end handle mavlink -MAV_RESULT GCS_MAVLINK_Plane::handle_command_do_set_mission_current(const mavlink_command_long_t &packet) +MAV_RESULT GCS_MAVLINK_Plane::handle_command_do_set_mission_current(const mavlink_command_int_t &packet) { const MAV_RESULT result = GCS_MAVLINK::handle_command_do_set_mission_current(packet); if (result != MAV_RESULT_ACCEPTED) { @@ -1316,6 +1411,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_do_set_mission_current(const mavlin return result; } +#if AP_MAVLINK_MISSION_SET_CURRENT_ENABLED void GCS_MAVLINK_Plane::handle_mission_set_current(AP_Mission &mission, const mavlink_message_t &msg) { // if you change this you must change handle_command_do_set_mission_current @@ -1325,6 +1421,7 @@ void GCS_MAVLINK_Plane::handle_mission_set_current(AP_Mission &mission, const ma plane.mission.resume(); } } +#endif uint64_t GCS_MAVLINK_Plane::capabilities() const { diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index 849efe8a17306..41cb8e6b123f5 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -3,6 +3,7 @@ #include #include #include +#include "quadplane.h" class GCS_MAVLINK_Plane : public GCS_MAVLINK { @@ -11,24 +12,27 @@ class GCS_MAVLINK_Plane : public GCS_MAVLINK using GCS_MAVLINK::GCS_MAVLINK; + uint8_t sysid_my_gcs() const override; + protected: uint32_t telem_delay() const override; +#if AP_MAVLINK_MISSION_SET_CURRENT_ENABLED void handle_mission_set_current(AP_Mission &mission, const mavlink_message_t &msg) override; +#endif - uint8_t sysid_my_gcs() const override; bool sysid_enforce() const override; - MAV_RESULT handle_command_preflight_calibration(const mavlink_command_long_t &packet, const mavlink_message_t &msg) override; - MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet) override; - MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override; - MAV_RESULT handle_command_do_set_mission_current(const mavlink_command_long_t &packet) override; + MAV_RESULT handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; + MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; + MAV_RESULT handle_command_do_set_mission_current(const mavlink_command_int_t &packet) override; void send_position_target_global_int() override; void send_aoa_ssa(); void send_attitude() const override; + void send_attitude_target() override; void send_wind() const; bool persist_streamrates() const override { return true; } @@ -51,7 +55,19 @@ class GCS_MAVLINK_Plane : public GCS_MAVLINK void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override; MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet); MAV_RESULT handle_command_int_guided_slew_commands(const mavlink_command_int_t &packet); - + MAV_RESULT handle_MAV_CMD_DO_AUTOTUNE_ENABLE(const mavlink_command_int_t &packet); + MAV_RESULT handle_command_DO_CHANGE_SPEED(const mavlink_command_int_t &packet); + MAV_RESULT handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command_int_t &packet); + MAV_RESULT handle_MAV_CMD_DO_PARACHUTE(const mavlink_command_int_t &packet); + MAV_RESULT handle_command_DO_VTOL_TRANSITION(const mavlink_command_int_t &packet); + +#if HAL_QUADPLANE_ENABLED +#if AP_MAVLINK_COMMAND_LONG_ENABLED + void convert_MAV_CMD_NAV_TAKEOFF_to_COMMAND_INT(const mavlink_command_long_t &in, mavlink_command_int_t &out); + void convert_COMMAND_LONG_to_COMMAND_INT(const mavlink_command_long_t &in, mavlink_command_int_t &out, MAV_FRAME frame = MAV_FRAME_GLOBAL_RELATIVE_ALT) override; +#endif + MAV_RESULT handle_command_MAV_CMD_NAV_TAKEOFF(const mavlink_command_int_t &packet); +#endif bool try_send_message(enum ap_message id) override; void packetReceived(const mavlink_status_t &status, const mavlink_message_t &msg) override; diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index f8b1a3ac57649..55d5ab4c46b49 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -62,6 +62,9 @@ void Plane::Log_Write_FullRate(void) if (should_log(MASK_LOG_ATTITUDE_FULLRATE)) { Log_Write_Attitude(); } + if (should_log(MASK_LOG_NOTCH_FULLRATE)) { + AP::ins().write_notch_log_messages(); + } } @@ -76,6 +79,7 @@ struct PACKED log_Control_Tuning { float rudder_out; float throttle_dem; float airspeed_estimate; + uint8_t airspeed_estimate_status; float synthetic_airspeed; float EAS2TAS; int32_t groundspeed_undershoot; @@ -85,7 +89,8 @@ struct PACKED log_Control_Tuning { void Plane::Log_Write_Control_Tuning() { float est_airspeed = 0; - ahrs.airspeed_estimate(est_airspeed); + AP_AHRS::AirspeedEstimateType airspeed_estimate_type = AP_AHRS::AirspeedEstimateType::NO_NEW_ESTIMATE; + ahrs.airspeed_estimate(est_airspeed, airspeed_estimate_type); float synthetic_airspeed; if (!ahrs.synthetic_airspeed(synthetic_airspeed)) { @@ -103,6 +108,7 @@ void Plane::Log_Write_Control_Tuning() rudder_out : SRV_Channels::get_output_scaled(SRV_Channel::k_rudder), throttle_dem : TECS_controller.get_throttle_demand(), airspeed_estimate : est_airspeed, + airspeed_estimate_status : (uint8_t)airspeed_estimate_type, synthetic_airspeed : synthetic_airspeed, EAS2TAS : ahrs.get_EAS2TAS(), groundspeed_undershoot : groundspeed_undershoot, @@ -110,6 +116,7 @@ void Plane::Log_Write_Control_Tuning() logger.WriteBlock(&pkt, sizeof(pkt)); } +#if OFFBOARD_GUIDED == ENABLED struct PACKED log_OFG_Guided { LOG_PACKET_HEADER; uint64_t time_us; @@ -125,7 +132,6 @@ struct PACKED log_OFG_Guided { // Write a OFG Guided packet. void Plane::Log_Write_OFG_Guided() { -#if OFFBOARD_GUIDED == ENABLED struct log_OFG_Guided pkt = { LOG_PACKET_HEADER_INIT(LOG_OFG_MSG), time_us : AP_HAL::micros64(), @@ -138,8 +144,8 @@ void Plane::Log_Write_OFG_Guided() target_heading_limit : guided_state.target_heading_accel_limit }; logger.WriteBlock(&pkt, sizeof(pkt)); -#endif } +#endif struct PACKED log_Nav_Tuning { LOG_PACKET_HEADER; @@ -219,6 +225,7 @@ struct PACKED log_AETR { float throttle; float rudder; float flap; + float steering; float speed_scaler; }; @@ -232,6 +239,7 @@ void Plane::Log_Write_AETR() ,throttle : SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) ,rudder : SRV_Channels::get_output_scaled(SRV_Channel::k_rudder) ,flap : SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_flap_auto) + ,steering : SRV_Channels::get_output_scaled(SRV_Channel::k_steering) ,speed_scaler : get_speed_scaler(), }; @@ -298,15 +306,17 @@ const struct LogStructure Plane::log_structure[] = { // @Field: NavPitch: desired pitch // @Field: Pitch: achieved pitch // @Field: ThO: scaled output throttle -// @Field: RdrOut: scaled output rudder +// @Field: RdO: scaled output rudder // @Field: ThD: demanded speed-height-controller throttle // @Field: As: airspeed estimate (or measurement if airspeed sensor healthy and ARSPD_USE>0) -// @Field: SAs: synthetic airspeed measurement derived from non-airspeed sensors, NaN if not available +// @Field: SAs: DCM's airspeed estimate, NaN if not available +// @Field: AsT: airspeed type ( old estimate or source of new estimate) +// @FieldValueEnum: AsT: AP_AHRS::AirspeedEstimateType // @Field: E2T: equivalent to true airspeed ratio // @Field: GU: groundspeed undershoot when flying with minimum groundspeed { LOG_CTUN_MSG, sizeof(log_Control_Tuning), - "CTUN", "Qccccffffffi", "TimeUS,NavRoll,Roll,NavPitch,Pitch,ThO,RdrOut,ThD,As,SAs,E2T,GU", "sdddd---nn-n", "FBBBB---00-B" , true }, + "CTUN", "QccccffffBffi", "TimeUS,NavRoll,Roll,NavPitch,Pitch,ThO,RdO,ThD,As,AsT,SAs,E2T,GU", "sdddd---n-n-n", "FBBBB---000-B" , true }, // @LoggerMessage: NTUN // @Description: Navigation Tuning information - e.g. vehicle destination @@ -374,8 +384,8 @@ const struct LogStructure Plane::log_structure[] = { // @Field: CRt: climb rate // @Field: TMix: transition throttle mix value // @Field: Sscl: speed scalar for tailsitter control surfaces -// @Field: Trn: Transistion state -// @Field: Ast: Q assist active state +// @Field: Trn: Transition state: 0-AirspeedWait,1-Timer,2-Done / TailSitter: 0-FW Wait,1-VTOL Wait,2-Done +// @Field: Ast: Q assist active #if HAL_QUADPLANE_ENABLED { LOG_QTUN_MSG, sizeof(QuadPlane::log_QControl_Tuning), "QTUN", "QffffffeccffBB", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DCRt,CRt,TMix,Sscl,Trn,Ast", "s----mmmnn----", "F----00000-0--" , true }, @@ -391,9 +401,11 @@ const struct LogStructure Plane::log_structure[] = { // @Field: I: integral part of PID // @Field: D: derivative part of PID // @Field: FF: controller feed-forward portion of response +// @Field: DFF: controller derivative feed-forward portion of response // @Field: Dmod: scaler applied to D gain to reduce limit cycling // @Field: SRate: slew rate -// @Field: Limit: 1 if I term is limited due to output saturation +// @Field: Flags: bitmask of PID state flags +// @FieldBitmaskEnum: Flags: log_PID_Flags { LOG_PIQR_MSG, sizeof(log_PID), "PIQR", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS , true }, { LOG_PIQP_MSG, sizeof(log_PID), @@ -413,24 +425,28 @@ const struct LogStructure Plane::log_structure[] = { // @Field: I: integral part of PID // @Field: D: derivative part of PID // @Field: FF: controller feed-forward portion of response +// @Field: DFF: controller derivative feed-forward portion of response // @Field: Dmod: scaler applied to D gain to reduce limit cycling // @Field: SRate: slew rate -// @Field: Limit: 1 if I term is limited due to output saturation +// @Field: Flags: bitmask of PID state flags +// @FieldBitmaskEnum: Flags: log_PID_Flags { LOG_PIDG_MSG, sizeof(log_PID), "PIDG", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS , true }, // @LoggerMessage: AETR // @Description: Normalised pre-mixer control surface outputs // @Field: TimeUS: Time since system startup -// @Field: Ail: Pre-mixer value for aileron output (between -4500 to 4500) -// @Field: Elev: Pre-mixer value for elevator output (between -4500 to 4500) -// @Field: Thr: Pre-mixer value for throttle output (between -4500 to 4500) -// @Field: Rudd: Pre-mixer value for rudder output (between -4500 to 4500) -// @Field: Flap: Pre-mixer value for flaps output (between -4500 to 4500) +// @Field: Ail: Pre-mixer value for aileron output (between -4500 and 4500) +// @Field: Elev: Pre-mixer value for elevator output (between -4500 and 4500) +// @Field: Thr: Pre-mixer value for throttle output (between -100 and 100) +// @Field: Rudd: Pre-mixer value for rudder output (between -4500 and 4500) +// @Field: Flap: Pre-mixer value for flaps output (between 0 and 100) +// @Field: Steer: Pre-mixer value for steering output (between -4500 and 4500) // @Field: SS: Surface movement / airspeed scaling value { LOG_AETR_MSG, sizeof(log_AETR), - "AETR", "Qffffff", "TimeUS,Ail,Elev,Thr,Rudd,Flap,SS", "s------", "F------" , true }, + "AETR", "Qfffffff", "TimeUS,Ail,Elev,Thr,Rudd,Flap,Steer,SS", "s-------", "F-------" , true }, +#if OFFBOARD_GUIDED == ENABLED // @LoggerMessage: OFG // @Description: OFfboard-Guided - an advanced version of GUIDED for companion computers that includes rate/s. // @Field: TimeUS: Time since system startup @@ -443,6 +459,7 @@ const struct LogStructure Plane::log_structure[] = { // @Field: HdgA: target heading lim { LOG_OFG_MSG, sizeof(log_OFG_Guided), "OFG", "QffffBff", "TimeUS,Arsp,ArspA,Alt,AltA,AltF,Hdg,HdgA", "s-------", "F-------" , true }, +#endif }; void Plane::Log_Write_Vehicle_Startup_Messages() diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 83188fe94552b..fbecd0ee40b6c 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -23,6 +23,7 @@ const AP_Param::Info Plane::var_info[] = { // @DisplayName: Ground station MAVLink system ID // @Description: The identifier of the ground station in the MAVLink protocol. Don't change this unless you also modify the ground station to match. // @Range: 1 255 + // @Increment: 1 // @User: Advanced GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), @@ -38,6 +39,14 @@ const AP_Param::Info Plane::var_info[] = { // @User: Standard ASCALAR(autotune_level, "AUTOTUNE_LEVEL", 6), + // @Param: AUTOTUNE_OPTIONS + // @DisplayName: Autotune options bitmask + // @Description: Autotune specific options + // @Bitmask: 0: Disable FLTD update + // @Bitmask: 1: Disable FLTT update + // @User: Advanced + ASCALAR(autotune_options, "AUTOTUNE_OPTIONS", 0), + // @Param: TELEM_DELAY // @DisplayName: Telemetry startup delay // @Description: The amount of time (in seconds) to delay radio telemetry to prevent an Xbee bricking on power up @@ -151,7 +160,7 @@ const AP_Param::Info Plane::var_info[] = { // @Param: TKOFF_TDRAG_SPD1 // @DisplayName: Takeoff tail dragger speed1 - // @Description: This parameter sets the airspeed at which to stop holding the tail down and transition to rudder control of steering on the ground. When TKOFF_TDRAG_SPD1 is reached the pitch of the aircraft will be held level until TKOFF_ROTATE_SPD is reached, at which point the takeoff pitch specified in the mission will be used to "rotate" the pitch for takeoff climb. Set TKOFF_TDRAG_SPD1 to zero to go straight to rotation. This should be set to zero for hand launch and catapult launch. It should also be set to zero for tricycle undercarriages unless you are using the method above to genetly hold the nose wheel down. For tail dragger aircraft it should be set just below the stall speed. + // @Description: This parameter sets the airspeed at which to stop holding the tail down and transition to rudder control of steering on the ground. When TKOFF_TDRAG_SPD1 is reached the pitch of the aircraft will be held level until TKOFF_ROTATE_SPD is reached, at which point the takeoff pitch specified in the mission will be used to "rotate" the pitch for takeoff climb. Set TKOFF_TDRAG_SPD1 to zero to go straight to rotation. This should be set to zero for hand launch and catapult launch. It should also be set to zero for tricycle undercarriages unless you are using the method above to gently hold the nose wheel down. For tail dragger aircraft it should be set just below the stall speed. // @Units: m/s // @Range: 0 30 // @Increment: 0.1 @@ -424,8 +433,8 @@ const AP_Param::Info Plane::var_info[] = { // @Param: FS_LONG_ACTN // @DisplayName: Long failsafe action - // @Description: The action to take on a long (FS_LONG_TIMEOUT seconds) failsafe event. If the aircraft was in a stabilization or manual mode when failsafe started and a long failsafe occurs then it will change to RTL mode if FS_LONG_ACTN is 0 or 1, and will change to FBWA if FS_LONG_ACTN is set to 2. If the aircraft was in an auto mode (such as AUTO or GUIDED) when the failsafe started then it will continue in the auto mode if FS_LONG_ACTN is set to 0, will change to RTL mode if FS_LONG_ACTN is set to 1 and will change to FBWA mode if FS_LONG_ACTN is set to 2. If FS_LONG_ACTION is set to 3, the parachute will be deployed (make sure the chute is configured and enabled). - // @Values: 0:Continue,1:ReturnToLaunch,2:Glide,3:Deploy Parachute + // @Description: The action to take on a long (FS_LONG_TIMEOUT seconds) failsafe event. If the aircraft was in a stabilization or manual mode when failsafe started and a long failsafe occurs then it will change to RTL mode if FS_LONG_ACTN is 0 or 1, and will change to FBWA if FS_LONG_ACTN is set to 2. If the aircraft was in an auto mode (such as AUTO or GUIDED) when the failsafe started then it will continue in the auto mode if FS_LONG_ACTN is set to 0, will change to RTL mode if FS_LONG_ACTN is set to 1 and will change to FBWA mode if FS_LONG_ACTN is set to 2. If FS_LONG_ACTN is set to 3, the parachute will be deployed (make sure the chute is configured and enabled). If FS_LONG_ACTN is set to 4 the aircraft will switch to mode AUTO with the current waypoint if it is not already in mode AUTO, unless it is in the middle of a landing sequence. + // @Values: 0:Continue,1:ReturnToLaunch,2:Glide,3:Deploy Parachute,4:Auto // @User: Standard GSCALAR(fs_action_long, "FS_LONG_ACTN", FS_ACTION_LONG_CONTINUE), @@ -605,17 +614,10 @@ const AP_Param::Info Plane::var_info[] = { // @User: Standard GSCALAR(dspoiler_rud_rate, "DSPOILR_RUD_RATE", DSPOILR_RUD_RATE_DEFAULT), - // @Param: SYS_NUM_RESETS - // @DisplayName: Num Resets - // @Description: Number of APM board resets - // @ReadOnly: True - // @User: Advanced - GSCALAR(num_resets, "SYS_NUM_RESETS", 0), - // @Param: LOG_BITMASK // @DisplayName: Log bitmask // @Description: Bitmap of what on-board log types to enable. This value is made up of the sum of each of the log types you want to be saved. It is usually best just to enable all basic log types by setting this to 65535. - // @Bitmask: 0:Fast Attitude,1:Medium Attitude,2:GPS,3:Performance,4:Control Tuning,5:Navigation Tuning,7:IMU,8:Mission Commands,9:Battery Monitor,10:Compass,11:TECS,12:Camera,13:RC Input-Output,14:Rangefinder,19:Raw IMU,20:Fullrate Attitude,21:Video Stabilization + // @Bitmask: 0:Fast Attitude,1:Medium Attitude,2:GPS,3:Performance,4:Control Tuning,5:Navigation Tuning,7:IMU,8:Mission Commands,9:Battery Monitor,10:Compass,11:TECS,12:Camera,13:RC Input-Output,14:Rangefinder,19:Raw IMU,20:Fullrate Attitude,21:Video Stabilization,22:Fullrate Notch // @User: Advanced GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), @@ -754,9 +756,11 @@ const AP_Param::Info Plane::var_info[] = { // @Path: AP_Arming.cpp,../libraries/AP_Arming/AP_Arming.cpp GOBJECT(arming, "ARMING_", AP_Arming_Plane), +#if AP_RELAY_ENABLED // @Group: RELAY_ // @Path: ../libraries/AP_Relay/AP_Relay.cpp GOBJECT(relay, "RELAY_", AP_Relay), +#endif #if PARACHUTE == ENABLED // @Group: CHUTE_ @@ -797,9 +801,11 @@ const AP_Param::Info Plane::var_info[] = { GOBJECT(quadplane, "Q_", QuadPlane), #endif +#if AP_TUNING_ENABLED // @Group: TUNE_ // @Path: tuning.cpp,../libraries/AP_Tuning/AP_Tuning.cpp GOBJECT(tuning, "TUNE_", AP_Tuning_Plane), +#endif #if HAL_QUADPLANE_ENABLED // @Group: Q_A_ @@ -943,9 +949,11 @@ const AP_Param::Info Plane::var_info[] = { // @Path: ../libraries/AP_Mission/AP_Mission.cpp GOBJECT(mission, "MIS_", AP_Mission), +#if HAL_RALLY_ENABLED // @Group: RALLY_ // @Path: ../libraries/AP_Rally/AP_Rally.cpp GOBJECT(rally, "RALLY_", AP_Rally), +#endif #if HAL_NAVEKF2_AVAILABLE // @Group: EK2_ @@ -1090,6 +1098,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Bitmask: 10: Adjust mid-throttle to be TRIM_THROTTLE in non-auto throttle modes except MANUAL // @Bitmask: 11: Disable suppression of fixed wing rate gains in ground mode // @Bitmask: 12: Enable FBWB style loiter altitude control + // @Bitmask: 13: Indicate takeoff waiting for neutral rudder with flight control surfaces // @User: Advanced AP_GROUPINFO("FLIGHT_OPTIONS", 13, ParametersG2, flight_options, 0), @@ -1237,7 +1246,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8, 8: Servo 9, 9: Servo 10, 10: Servo 11, 11: Servo 12, 12: Servo 13, 13: Servo 14, 14: Servo 15 AP_GROUPINFO("ONESHOT_MASK", 32, ParametersG2, oneshot_mask, 0), -#if AP_SCRIPTING_ENABLED +#if AP_SCRIPTING_ENABLED && AP_FOLLOW_ENABLED // @Group: FOLL // @Path: ../libraries/AP_Follow/AP_Follow.cpp AP_SUBGROUPINFO(follow, "FOLL", 33, ParametersG2, AP_Follow), @@ -1249,17 +1258,12 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Bitmask: 0:Roll,1:Pitch,2:Yaw // @User: Standard AP_GROUPINFO("AUTOTUNE_AXES", 34, ParametersG2, axis_bitmask, 7), - - AP_GROUPEND }; ParametersG2::ParametersG2(void) : unused_integer{1} -#if AP_ICENGINE_ENABLED - ,ice_control(plane.rpm_sensor) -#endif #if HAL_SOARING_ENABLED ,soaring_controller(plane.TECS_controller, plane.aparm) #endif diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index db02c44565e8b..7210bfe777126 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -43,7 +43,7 @@ class Parameters { // k_param_format_version = 0, k_param_software_type, // unused; - k_param_num_resets, + k_param_num_resets, // unused k_param_NavEKF2, k_param_g2, k_param_avoidance_adsb, @@ -356,6 +356,7 @@ class Parameters { k_param_fence, // vehicle fence - unused k_param_acro_yaw_rate, k_param_takeoff_throttle_max_t, + k_param_autotune_options, }; AP_Int16 format_version; @@ -420,7 +421,7 @@ class Parameters { AP_Int8 flight_mode6; AP_Int8 initial_mode; - // Navigational maneuvering limits + // Navigational manoeuvring limits // AP_Int16 alt_offset; AP_Int16 acro_roll_rate; @@ -434,7 +435,6 @@ class Parameters { AP_Float mixing_gain; AP_Int16 mixing_offset; AP_Int16 dspoiler_rud_rate; - AP_Int16 num_resets; AP_Int32 log_bitmask; AP_Int32 RTL_altitude_cm; AP_Int16 pitch_trim_cd; @@ -541,17 +541,17 @@ class ParametersG2 { AP_Int8 crow_flap_options; AP_Int8 crow_flap_aileron_matching; - // Forward throttle battery voltage compenstaion + // Forward throttle battery voltage compensation AP_Float fwd_thr_batt_voltage_max; AP_Float fwd_thr_batt_voltage_min; AP_Int8 fwd_thr_batt_idx; #if OFFBOARD_GUIDED == ENABLED // guided yaw heading PID - AC_PID guidedHeading{5000.0, 0.0, 0.0, 0 , 10.0, 5.0, 5.0 , 5.0 , 0.2}; + AC_PID guidedHeading{5000.0, 0.0, 0.0, 0 , 10.0, 5.0, 5.0 , 5.0 , 0.0}; #endif -#if AP_SCRIPTING_ENABLED +#if AP_SCRIPTING_ENABLED && AP_FOLLOW_ENABLED AP_Follow follow; #endif diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 8d5a08a189298..80e3cb8636ce1 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -83,11 +83,18 @@ #include #include // Landing Gear library #include +#include +#if AP_EXTERNAL_CONTROL_ENABLED +#include +#endif #include "GCS_Mavlink.h" #include "GCS_Plane.h" #include "quadplane.h" +#include +#if AP_TUNING_ENABLED #include "tuning.h" +#endif // Configuration #include "config.h" @@ -190,6 +197,7 @@ class Plane : public AP_Vehicle { // flight modes convenience array AP_Int8 *flight_modes = &g.flight_mode1; + const uint8_t num_flight_modes = 6; AP_FixedWing::Rangefinder_State rangefinder_state; @@ -210,16 +218,6 @@ class Plane : public AP_Vehicle { bool training_manual_roll; // user has manual roll control bool training_manual_pitch; // user has manual pitch control - /* - keep steering and rudder control separated until we update servos, - to allow for a separate wheel servo from rudder servo - */ - struct { - bool ground_steering; // are we doing ground steering? - int16_t steering; // value for nose/tail wheel - int16_t rudder; // value for rudder - } steering_control; - // should throttle be pass-thru in guided? bool guided_throttle_passthru; @@ -227,6 +225,9 @@ class Plane : public AP_Vehicle { // external failsafe boards during baro and airspeed calibration bool in_calibration; + // are we currently in long failsafe but have postponed it in MODE TAKEOFF until min level alt is reached + bool long_failsafe_pending; + // GCS selection GCS_Plane _gcs; // avoid using this; use gcs() GCS_Plane &gcs() { return _gcs; } @@ -244,8 +245,14 @@ class Plane : public AP_Vehicle { AP_OpticalFlow optflow; #endif - // Rally Ponints +#if HAL_RALLY_ENABLED + // Rally Points AP_Rally rally; +#endif + + // returns a Location for a rally point or home; if + // HAL_RALLY_ENABLED is false, just home. + Location calc_best_rally_or_home_location(const Location ¤t_loc, float rtl_home_alt_amsl_cm) const; #if OSD_ENABLED || OSD_PARAM_ENABLED AP_OSD osd; @@ -305,7 +312,7 @@ class Plane : public AP_Vehicle { // Failsafe struct { - // Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold + // Used to track if the value on channel 3 (throttle) has fallen below the failsafe threshold // RC receiver should be set up to output a low throttle value when signal is lost bool rc_failsafe; @@ -380,6 +387,7 @@ class Plane : public AP_Vehicle { // Ground speed // The amount current ground speed is below min ground speed. Centimeters per second int32_t groundspeed_undershoot; + bool groundspeed_undershoot_is_valid; // Difference between current altitude and desired altitude. Centimeters int32_t altitude_error_cm; @@ -395,11 +403,13 @@ class Plane : public AP_Vehicle { struct { uint32_t last_tkoff_arm_time; uint32_t last_check_ms; + uint32_t rudder_takeoff_warn_ms; uint32_t last_report_ms; bool launchTimerStarted; uint8_t accel_event_counter; uint32_t accel_event_ms; uint32_t start_time_ms; + bool waiting_for_rudder_neutral; } takeoff_state; // ground steering controller state @@ -484,11 +494,6 @@ class Plane : public AP_Vehicle { // have we checked for an auto-land? bool checked_for_autoland; - // Altitude threshold to complete a takeoff command in autonomous modes. Centimeters - // are we in idle mode? used for balloon launch to stop servo - // movement until altitude is reached - bool idle_mode; - // are we in VTOL mode in AUTO? bool vtol_mode; @@ -497,6 +502,9 @@ class Plane : public AP_Vehicle { // how much correction have we added for terrain data float terrain_correction; + + // last home altitude for detecting changes + int32_t last_home_alt_cm; } auto_state; #if AP_SCRIPTING_ENABLED @@ -543,8 +551,7 @@ class Plane : public AP_Vehicle { float target_heading_accel_limit; uint32_t target_heading_time_ms; guided_heading_type_t target_heading_type; - bool target_heading_limit_low; - bool target_heading_limit_high; + bool target_heading_limit; #endif // OFFBOARD_GUIDED == ENABLED } guided_state; @@ -579,10 +586,12 @@ class Plane : public AP_Vehicle { // this controls throttle suppression in auto modes bool throttle_suppressed; +#if AP_BATTERY_WATT_MAX_ENABLED // reduce throttle to eliminate battery over-current int8_t throttle_watt_limit_max; int8_t throttle_watt_limit_min; // for reverse thrust uint32_t throttle_watt_limit_timer_ms; +#endif AP_FixedWing::FlightStage flight_stage = AP_FixedWing::FlightStage::NORMAL; @@ -606,7 +615,7 @@ class Plane : public AP_Vehicle { // The instantaneous desired pitch angle. Hundredths of a degree int32_t nav_pitch_cd; - // the aerodymamic load factor. This is calculated from the demanded + // the aerodynamic load factor. This is calculated from the demanded // roll before the roll is clipped, using 1/sqrt(cos(nav_roll)) float aerodynamic_load_factor = 1.0f; @@ -680,6 +689,9 @@ class Plane : public AP_Vehicle { // The amount of time we should stay in a loiter for the Loiter Time command. Milliseconds. uint32_t time_max_ms; + + // current value of loiter radius in metres used by the controller + float radius; } loiter; // Conditional command @@ -720,6 +732,9 @@ class Plane : public AP_Vehicle { // are we trying to follow terrain? bool terrain_following; + // are we waiting to load terrain data to init terrain following + bool terrain_following_pending; + // target altitude above terrain in cm, valid if terrain_following // is set int32_t terrain_alt_cm; @@ -756,11 +771,16 @@ class Plane : public AP_Vehicle { AP_Mount camera_mount; #endif - // Arming/Disarming mangement class + // Arming/Disarming management class AP_Arming_Plane arming; AP_Param param_loader {var_info}; + // dummy implementation of external control +#if AP_EXTERNAL_CONTROL_ENABLED + AP_ExternalControl external_control; +#endif + static const AP_Scheduler::Task scheduler_tasks[]; static const AP_Param::Info var_info[]; @@ -775,8 +795,10 @@ class Plane : public AP_Vehicle { QuadPlane quadplane{ahrs}; #endif +#if AP_TUNING_ENABLED // support for transmitter tuning AP_Tuning_Plane tuning; +#endif static const struct LogStructure log_structure[]; @@ -833,7 +855,7 @@ class Plane : public AP_Vehicle { void reset_offset_altitude(void); void set_offset_altitude_location(const Location &start_loc, const Location &destination_loc); bool above_location_current(const Location &loc); - void setup_terrain_target_alt(Location &loc) const; + void setup_terrain_target_alt(Location &loc); int32_t adjusted_altitude_cm(void); int32_t adjusted_relative_altitude_cm(void); float mission_alt_offset(void); @@ -853,12 +875,11 @@ class Plane : public AP_Vehicle { float stabilize_roll_get_roll_out(); void stabilize_pitch(); float stabilize_pitch_get_pitch_out(); - void stabilize_stick_mixing_direct(); void stabilize_stick_mixing_fbw(); void stabilize_yaw(); - void calc_nav_yaw_coordinated(); - void calc_nav_yaw_course(void); - void calc_nav_yaw_ground(void); + int16_t calc_nav_yaw_coordinated(); + int16_t calc_nav_yaw_course(void); + int16_t calc_nav_yaw_ground(void); // Log.cpp uint32_t last_log_fast_ms; @@ -906,7 +927,6 @@ class Plane : public AP_Vehicle { void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd); void do_loiter_turns(const AP_Mission::Mission_Command& cmd); void do_loiter_time(const AP_Mission::Mission_Command& cmd); - void do_altitude_wait(const AP_Mission::Mission_Command& cmd); void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd); void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd); void do_vtol_takeoff(const AP_Mission::Mission_Command& cmd); @@ -923,22 +943,14 @@ class Plane : public AP_Vehicle { bool verify_command_callback(const AP_Mission::Mission_Command& cmd); float get_wp_radius() const; - void do_nav_delay(const AP_Mission::Mission_Command& cmd); - bool verify_nav_delay(const AP_Mission::Mission_Command& cmd); - bool is_land_command(uint16_t cmd) const; + bool do_change_speed(uint8_t speedtype, float speed_target_ms, float rhtottle_pct); /* return true if in a specific AUTO mission command */ bool in_auto_mission_id(uint16_t command) const; - - // Delay the next navigation command - struct { - uint32_t time_max_ms; - uint32_t time_start_ms; - } nav_delay; - + #if AP_SCRIPTING_ENABLED // nav scripting support void do_nav_script_time(const AP_Mission::Mission_Command& cmd); @@ -960,7 +972,6 @@ class Plane : public AP_Vehicle { // control_modes.cpp void read_control_switch(); uint8_t readSwitch(void) const; - void reset_control_switch(); void autotune_start(void); void autotune_restore(void); void autotune_enable(bool enable); @@ -968,6 +979,10 @@ class Plane : public AP_Vehicle { bool mode_allows_autotuning(void); uint8_t get_mode() const override { return (uint8_t)control_mode->mode_number(); } Mode *mode_from_mode_num(const enum Mode::Number num); + bool current_mode_requires_mission() const override { + return control_mode == &mode_auto; + } + bool autotuning; // events.cpp @@ -1015,11 +1030,13 @@ class Plane : public AP_Vehicle { void update_fly_forward(void); void update_flight_stage(); void set_flight_stage(AP_FixedWing::FlightStage fs); + bool flight_option_enabled(FlightOptions flight_option) const; // navigation.cpp void loiter_angle_reset(void); void loiter_angle_update(void); void navigate(); + void check_home_alt_change(void); void calc_airspeed_errors(); float mode_auto_target_airspeed_cm(); void calc_gndspeed_undershoot(); @@ -1058,6 +1075,7 @@ class Plane : public AP_Vehicle { bool should_log(uint32_t mask); int8_t throttle_percentage(void); void notify_mode(const Mode& mode); + bool gcs_mode_enabled(const Mode::Number mode_num) const; // takeoff.cpp bool auto_takeoff_check(void); @@ -1073,8 +1091,8 @@ class Plane : public AP_Vehicle { // servos.cpp void set_servos_idle(void); void set_servos(); - void set_servos_manual_passthrough(void); void set_servos_controlled(void); + void set_takeoff_expected(void); void set_servos_old_elevons(void); void set_servos_flaps(void); void set_landing_gear(void); @@ -1093,6 +1111,7 @@ class Plane : public AP_Vehicle { void channel_function_mixer(SRV_Channel::Aux_servo_function_t func1_in, SRV_Channel::Aux_servo_function_t func2_in, SRV_Channel::Aux_servo_function_t func1_out, SRV_Channel::Aux_servo_function_t func2_out) const; void flaperon_update(); + void indicate_waiting_for_rud_neutral_to_takeoff(void); // is_flying.cpp void update_is_flying_5Hz(void); @@ -1220,6 +1239,8 @@ class Plane : public AP_Vehicle { public: void failsafe_check(void); + bool is_landing() const override; + bool is_taking_off() const override; #if AP_SCRIPTING_ENABLED bool set_target_location(const Location& target_loc) override; bool get_target_location(Location& target_loc) override; diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index e0435c8253af5..cb426ab21f4a2 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -16,9 +16,14 @@ int8_t RC_Channels_Plane::flight_mode_channel_number() const return plane.g.flight_mode_channel.get(); } +bool RC_Channels_Plane::in_rc_failsafe() const +{ + return (plane.rc_failsafe_active() || plane.failsafe.rc_failsafe); +} + bool RC_Channels_Plane::has_valid_input() const { - if (plane.rc_failsafe_active() || plane.failsafe.rc_failsafe) { + if (in_rc_failsafe()) { return false; } if (plane.failsafe.throttle_counter != 0) { @@ -45,8 +50,7 @@ void RC_Channel_Plane::do_aux_function_change_mode(const Mode::Number number, // return to flight mode switch's flight mode if we are currently // in this mode if (plane.control_mode->mode_number() == number) { -// TODO: rc().reset_mode_switch(); - plane.reset_control_switch(); + rc().reset_mode_switch(); } } } @@ -154,6 +158,7 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option, case AUX_FUNC::RTL: case AUX_FUNC::TAKEOFF: case AUX_FUNC::FBWA: + case AUX_FUNC::AIRBRAKE: #if HAL_QUADPLANE_ENABLED case AUX_FUNC::QRTL: case AUX_FUNC::QSTABILIZE: @@ -171,6 +176,7 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option, case AUX_FUNC::TRIM_TO_CURRENT_SERVO_RC: case AUX_FUNC::EMERGENCY_LANDING_EN: case AUX_FUNC::FW_AUTOTUNE: + case AUX_FUNC::VFWD_THR_OVERRIDE: break; case AUX_FUNC::SOARING: @@ -266,6 +272,15 @@ bool RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit case AUX_FUNC::QSTABILIZE: do_aux_function_change_mode(Mode::Number::QSTABILIZE, ch_flag); break; + + case AUX_FUNC::VFWD_THR_OVERRIDE: { + const bool enable = (ch_flag == AuxSwitchPos::HIGH); + if (enable != plane.quadplane.vfwd_enable_active) { + plane.quadplane.vfwd_enable_active = enable; + gcs().send_text(MAV_SEVERITY_INFO, "QFwdThr: %s", enable?"ON":"OFF"); + } + break; + } #endif case AUX_FUNC::SOARING: @@ -354,7 +369,7 @@ bool RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit break; case AUX_FUNC::MODE_SWITCH_RESET: - plane.reset_control_switch(); + rc().reset_mode_switch(); break; case AUX_FUNC::CRUISE: diff --git a/ArduPlane/RC_Channel.h b/ArduPlane/RC_Channel.h index 35663554a51e0..7ecc352c32aa8 100644 --- a/ArduPlane/RC_Channel.h +++ b/ArduPlane/RC_Channel.h @@ -13,6 +13,9 @@ class RC_Channel_Plane : public RC_Channel AuxSwitchPos ch_flag) override; bool do_aux_function(aux_func_t ch_option, AuxSwitchPos) override; + // called when the mode switch changes position: + void mode_switch_changed(modeswitch_pos_t new_pos) override; + private: void do_aux_function_change_mode(Mode::Number number, @@ -42,10 +45,13 @@ class RC_Channels_Plane : public RC_Channels return &obj_channels[chan]; } + bool in_rc_failsafe() const override; bool has_valid_input() const override; RC_Channel *get_arming_channel(void) const override; + void read_mode_switch() override; + protected: // note that these callbacks are not presently used on Plane: diff --git a/ArduPlane/ReleaseNotes.txt b/ArduPlane/ReleaseNotes.txt index 4d1420f5b95a9..7c1d1f296ad64 100644 --- a/ArduPlane/ReleaseNotes.txt +++ b/ArduPlane/ReleaseNotes.txt @@ -1,3 +1,475 @@ +Release 4.4.4-beta1 5th December 2023 +------------------------------------- + +Changes from 4.4.3: + +- CubeOrange Sim-on-hardware compilation fix +- RADIX2HD supports external I2C compasses +- SpeedyBeeF405v4 support +- DroneCAN battery monitor with cell monitor SoC reporting fix +- ProfiLED output fixed in both Notify and Scripting +- NTF_LED_TYPES parameter description fixed (was missing IS31FL3195) +- Scripting bug that could cause crash if parameters were added in flight +- STAT_BOOTCNT param fix (was not updating in some cases) +- don't query hobbywing DroneCAN ESC IDs while armed +- clamp empy version to prevent build errors + +Release 4.4.3 14th November 2023 +-------------------------------- + +Changes from 4.4.2: + + - fixed setup of ICM45486 IMU on CubeOrangePlus-BG edition + - disable AFSR on IxM42xxx IMUs to prevent gyro bias for "stuck" gyros + - fixed AK09916 compass being non-responsive + - implement GPS_DRV_OPTION for using ellipsoid height in more GPS drivers + - fixed SIYI AP_Mount parsing bug + - configuration fixes for BETAFTP-F405 boards + - fixed origin versus home relative bug in quadplane landing and guided takeoff + - correct mavlink response for no airspeed sensor on preflight calibration + - protect against notch filtering with uninitialised RPM source in ESC telemetry + - allow lua scripts to populate full ESC telemetry data + - added YJUAV_A6SE_H743 support + - fixed uBlox M10 GPS support on boards with 1M flash + +Release 4.4.2 23th October 2023 +------------------------------- + +Changes from 4.4.1 + +- BETAFPV-F405 support +- MambaF405v2 battery and serial setup corrected +- mRo Control Zero OEM H7 bdshot support +- SpeedyBee-F405-Wing gets VTX power control +- SpeedyBee-F405-Mini support +- T-Motor H743 Mini support +- EKF3 supports baroless boards +- INA battery monitor supports config of shunt resistor used (see BATTx_SHUNT) +- BMI088 IMU error value handling fixed to avoid occasional negative spike +- Dev environment CI autotest stability improvements +- OSD correct DisplayPort BF MSP symbols +- OSD option to correct direction arrows for BF font set +- Sensor status reporting to GCS fixed for baroless boards +- added opendroneid option to auto-store IDs in persistent flash +- fixed TECS bug that could cause inability to climb or descend +- fixed race condition when starting TECS controlled mode +- fixed RTL with rally point and terrain follow +- protect against invalid data in SBUS for first 4 channels +- added build type to VER message +- allow moving baseline rover at 3Hz +- use RC deadzones in stick mixing + + +Release 4.4.1 26th September 2023 +--------------------------------- + +No changes from beta2 + +Release 4.4.1-beta2 12th September 2023 +-------------------------------------- + +Changes from 4.4.1-beta1 + +- Airbotf4 features minimised to build for 4.4 +- ChibiOS clock fix for 480Mhz H7 boards (affected FDCAN) +- H750 external flash optimisations for to lower CPU load +- MambaF405Mini fixes to match manufacturer's recommended wiring +- RADIX2 HD support +- RPI hardware version check fix +- YJUAV_A6SE support + +Release 4.4.1-beta1 5th September 2023 +-------------------------------------- + +Changes from 4.4.1 + +- support Himark DroneCAN servos +- support Hobbywing DroneCAN ESCs +- fixed control surface deflection on quadplanes in VTOL takeoff wait +- fixed bug in parameter default handling in SITL +- allow selection of mission sdcard storage on custom.ardupilot.org +- added support for SDMODELH7V1 +- fixed battery monitor default for QiotekZealotF427 and QiotekZealotH743 +- support 8 bit directional dshot channels on KakuteH7-wing +- improved handling of high vibration in EKF3 with new EK3_GLITCH_RADIUS options +- allow reset of battery SoC for DroneCAN battery monitors +- update GPIOs for Navigator board in HAL_Linux +- pull RTS lines low on Pixhawk6C on startup +- added log_file_content in scripting for aerobatics +- added asymmetry factor for skid steering on rovers +- updated defaults for luminousbee5 boards + +Happy flying! + +Release 4.4.0 18th August 2023 +------------------------------ + +No changes from beta5 + +Release 4.4.0-beta5 11th August 2023 +------------------------------------ + +Changes from 4.4.0-beta4 + +- fixed handling of missing DroneCAN airspeed packet +- fixed reset of target altitude in plane GUIDED mode +- added SIYI N7 flight controller +- fixed auto-enable of fence with forced arm +- fixed race condition that caused notch filter gyro glitches +- fixed bug with RTK injection for DroneCAN + +Release 4.4.0 beta4 +------------------- + +Changes from 4.4.0-beta3 + +1) flight controller specific changes + - Diatone-Mamba-MK4-H743v2 uses SPL06 baro (was DPS280) + - DMA for I2C disabled on F7 and H7 boards + - Foxeer H743v1 default serial protocol config fixes + - HeeWing-F405 and F405v2 support + - iFlight BlitzF7 support +2) Scripts may take action based on VTOL motor loss +3) Bug fixes + - BLHeli returns battery status requested via MSP (avoids hang when using esc-configurator) + - CRSFv3 rescans at baudrates to avoid RX loss + - EK3_ABIAS_P_NSE param range fix + - Scripting restart memory corruption bug fixed + - Siyi A8/ZR10 driver fix to avoid crash if serial port not configured + +Release 4.4.0 beta3 +------------------- + +This is the third beta of plane 4.4.0. This includes some important +fixes since beta2 + +1) flight controller specific changes + - Holybro KakuteH7-Wing support + - JFB100 external watchdog GPIO support added + - Pixhawk1-bdshot support + - Pixhawk6X-bdshot support + - SpeedyBeeF4 loses bdshot support + +3) Camera and Gimbal related changes + - DO_SET_ROI_NONE command support added + +4) Bug fixes + - ADSB sensor loss of transceiver message less spammy + - AutoTune Yaw rate max fixed + - EKF vertical velocity reset fixed on loss of GPS + - GPS pre-arm failure message clarified + - SERVOx_PROTOCOL "SToRM32 Gimbal Serial" value renamed to "Gimbal" because also used by Siyi + - SERIALx_OPTION "Swap" renamed to "SwapTXRX" for clarity + - SBF GPS ellipsoid height fixed + - Ublox M10S GPS auto configuration fixed + - ZigZag mode user takeoff fixed (users could not takeoff in ZigZag mode previously) + - fixed memory corruption bug with scripting restart + +5) Device drivers + - added LP5562 I2C LED driver + - added IS31FL3195 LED driver + +6) Applet changes + - added QUIK_MAX_REDUCE parameter to VTOL quicktune lua applet + +7) Plane specific changes + - fixed takeoff mode to ensure climb to takeoff alt before turning + - fixed error in quadplane wait for rudder neutral + - improved handling of forward throttle during VTOL landing + - fixed TECS state reset in VTOL auto modes + - fixed early exit from loiter to alt + - fixed display of started airspeed wait on forward transition + + +Release 4.4.0 beta2 +------------------- + +This is the second beta of plane 4.4.0. This includes some important +fixes since beta1. + +The full list of changes is: + + - added SpeedyBeeF405WING, JSB100 and FoxeerH743 + - added LOG_DISARMED=3 support and LOG_DARM_RATEMAX + - fixed error handling for being out of memory in EKF initialisation + - fixed a bug in RC input handling on the IOMCU + - fixed a bug handling ICE engine start after altitude reached + - adjust EKF3 accel bias process noise for greater robustness + - fixed an EKF3 bug in accel bias calculations + - cope with compassmot impacting GSF yaw numerical stability + - fixed an AUTOTUNE/QAUTOTUNE bug in yaw tuning + - support INA228 and INA238 I2C battery monitors + - always log rate PID slew limiters even when slew limit is zero + - added MambaF4050v2 for new IMU, bdshot and DMA on UART1 + - update for FoxeerH743v1 GA release + - reduced IMU init speed on MatekH743 + - move LED serial processing to its own thread + - fixed parameter documentation for BRD_SAFETYOPTION + - don't reject airspeed using EKF innovation if dead-reckoning + - fixed USB pass-thru on 2nd USB endpoint + +Release 4.3.7 31st May 2023 +--------------------------- + +This stable release is for the 4.3.x stable series and is being done +because of a serious bug that has been found with RC input on boards +that use an IOMCU for RC input (boards with a separate set of 8 "main" +outputs from "aux" outputs). + +The bug was that when RC input is lost and the receiver is one that +uses "no pulses" for loss of RC input then there is a chance that when +the RC link is regained that ArduPilot will not regain RC control and +will continue in RC failsafe. + +The bug is an old one, first introduced in the 4.0.6 release in +September 2020. The bug does not occur often which is why it has been +such a long time before it was noticed. We would like to thank CUAV +for noticing and reporting the bug! + +This release also has some other changes, some of which are to sync +with the Copter 4.3.6 release (which will go to 4.3.7 with this RC +input bug fix) and some are other bugs found since the 4.3.5 plane +release. + +This release skips the 4.3.6 number to sync with copter. + +The full list of changes is: + + - fixed a fault in the INS batch sampler code if you change the INS_LOG_BAT_CNT parameter without rebooting + - fixed the RC input on IOMCU bug explained above + - fixed a bug in ICE engine control if you do a "delay engine start" mission command while flying + - added MCU voltage monitoring for the H757 microcontroller (eg. CubeOrangePlus) + - servo gimbal mount yaw handling fix (only affects 3-axis servo gimbals) + - PiccoloCAN fix for ESC voltage and current scaling + - Gremsy gimbal fix when attached to autopilot's serial3 (or higher) + - added CubeOrangePlus-bdshot build + - fixed a bug in handling bad UART data in the megasquirt serial EFI driver + - added -g option for configuring with debug symbols without full debug (helped with RCIN bug diagnosis) + - fixed airmode switch for QACRO and QSTABILIZE modes + - fixed a rare memory corruption bug in the STM32H757 + - fixed an EKF3 bug in accel bias calculations + - adjust EKF3 accel bias process noise for greater robustness + - cope with compassmot impacting GSF yaw numerical stability + + +Please test and report any issues! + +Release 4.4.0 beta1 +------------------- + +This is the first beta of plane 4.4.0. It is a big release with lots +of changes. Many thanks to all the people who have contributed! + +1) New autopilots supported + - ESP32 + - Flywoo Goku F405S AIO + - Foxeer H743v1 + - MambaF405-2022B + - PixPilot-V3 + - PixSurveyA2 + - rFCU H743 + - ThePeach K1/R1 + +2) Autopilot specific changes + - Bi-Directional DShot support for CubeOrangePlus-bdshot, CUAVNora+, MatekF405TE/VTOL-bdshot, MatekL431, Pixhawk6C-bdshot, QioTekZealotH743-bdshot + - Bi-Directional DShot up to 8 channels on MatekH743 + - BlueRobotics Navigator supports baro on I2C bus 6 + - BMP280 baro only for BeastF7, KakuteF4, KakuteF7Mini, MambaF405, MatekF405, Omnibusf4 to reduce code size (aka "flash") + - CSRF and Hott telemetry disabled by default on some low power boards (aka "minimised boards") + - Foxeer Reaper F745 supports external compasses + - OmnibusF4 support for BMI270 IMU + - OmnibusF7V2-bdshot support removed + - KakuteF7 regains displayport, frees up DMA from unused serial port + - KakuteH7v2 gets second battery sensor + - MambaH743v4 supports VTX + - MatekF405-Wing supports InvensenseV3 IMUs + - PixPilot-V6 heater enabled + - Raspberry 64OS startup crash fixed + - ReaperF745AIO serial protocol defaults fixed + - SkystarsH7HD (non-bdshot) removed as users should always use -bdshot version + - Skyviper loses many unnecessary features to save flash + - UBlox GPS only for AtomRCF405NAVI, BeastF7, MatekF405, Omnibusf4 to reduce code size (aka "flash") + - VRBrain-v52 and VRCore-v10 features reduced to save flash + +3) Driver enhancements + - ARK RTK GPS support + - BMI088 IMU filtering and timing improved, ignores bad data + - CRSF OSD may display disarmed state after flight mode (enabled using RC_OPTIONS) + - Daiwa winch baud rate obeys SERIALx_BAUD parameter + - EFI supports fuel pressure and ignition voltage reporting and battery failsafe + - ICM45686 IMU support + - ICM20602 uses fast reset instead of full reset on bad temperature sample (avoids occasional very high offset) + - ICM45686 supports fast sampling + - MAX31865 temp sensor support + - MB85RS256TY-32k, PB85RS128C and PB85RS2MC FRAM support + - MMC3416 compass orientation fix + - MPPT battery monitor reliability improvements, enable/disable aux function and less spammy + - Multiple USD-D1-CAN radar support + - NMEA output rate configurable (see NMEA_RATE_MS) + - NMEA output supports PASHR message (see NMEA_MSG_EN) + - OSD supports average resting cell voltage (see OSD_ACRVOLT_xxx params) + - Rockblock satellite modem support + - Serial baud support for 2Mbps (only some hardware supports this speed) + - SF45b lidar filtering reduced (allows detecting smaller obstacles + - SmartAudio 2.0 learns all VTX power levels) + - UAVCAN ESCs report error count using ESC Telemetry + - Unicore GPS (e.g. UM982) support + - VectorNav 100 external AHRS support + - 5 IMUs supported + +4) EKF related enhancements + - Baro compensation using wind estimates works when climbing or descending (see BAROx_WCF_UP/DN) + - External AHRS support for enabling only some sensors (e.g. IMU, Baro, Compass) see EAHRS_SENSORS + - Magnetic field tables updated + - Non-compass initial yaw alignment uses GPS course over GSF (mostly affects Planes and Rover) + +5) Control and navigation enhancements + - AutoTune of attitude control yaw D gain (set AUTOTUNE_AXES=8) + - DO_SET_ROI_NONE command turns off ROI + - JUMP_TAG mission item support + - Missions can be stored on SD card (see BRD_SD_MISSION) + - NAV_SCRIPT_TIME command accepts floating point arguments + - Pause/Resume returns success if mission is already paused or resumed + - Payload Place support via lua script in quadplanes + +7) Filtering enhancements + - FFT notch can be run based on filtered data + - Warn of motor noise at RPM frequency using FFT + - In-flight FFT can better track low frequency noise + - In-flight FFT logging improved + - IMU data can be read and replayed for FFT analysis + +8) Camera and gimbal enhancements + - BMMCC support included in Servo driver + - DJI RS2/RS3-Pro gimbal support + - Dual camera support (see CAM2_TYPE) + - Gimbal/Mount2 can be moved to retracted or neutral position + - Gremsy ZIO support + - IMAGE_START_CAPTURE, SET_CAMERA_ZOOM/FOCUS, VIDEO_START/STOP_CAPTURE command support + - Parameters renamed and rescaled + i) CAM_TRIGG_TYPE renamed to CAM1_TYPE and options have changed + ii) CAM_DURATION renamed to CAM1_DURATION and scaled in seconds + iii) CAM_FEEDBACK_PIN/POL renamed to CAM1_FEEBAK_PIN/POL + iv) CAM_MIN_INTERVAL renamed to CAM1_INTRVAL_MIN and scaled in seconds + v) CAM_TRIGG_DIST renamed to CAMx_TRIGG_DIST and accepts fractional values + - RunCam2 4k support + - ViewPro camera gimbal support + +9) Logging changes + - BARO msg includes 3-axis dynamic pressure useful for baro compensation of wind estimate + - MCU log msg includes main CPU temp and voltage (was part of POWR message) + - RCOut banner message always included in Logs + - SCR message includes memory usage of all running scripts + - CANS message includes CAN bus tx/rx statistics + - OFCA (optical flow calibration log message) units added + - Home location not logged to CMD message + - MOTB message includes throttle output + +10) Scripting enhancements + - Generator throttle control example added + - Heap max increased by allowing heap to be split across multiple underlying OS heaps + - Hexsoon LEDs applet + - Logging from scripts supports more formats + - Parameters can be removed or reordered + - Parameter description support (scripts must be in AP's applet or driver directory) + - Rangefinder driver support + - Runcam_on_arm applet starts recording when vehicle is armed + - Safety switch, E-Stop and motor interlock support + - Scripts can restart all scripts + - Script_Controller applet supports inflight switching of active scripts + +11) Custom build server enhancements + - AIS support for displaying nearby boats can be included + - Battery, Camera and Compass drivers can be included/excluded + - EKF3 wind estimation can be included/excluded + - PCA9685, ToshibaLED, PLAY_TUNE notify drivers can be included/excluded + - RichenPower generator can be included/excluded + - RC SRXL protocol can be excluded + - SIRF GPSs can be included/excluded + +12) Safety related enhancements and fixes + - "EK3 sources require RangeFinder" pre-arm check fix when user only sets up 2nd rangefinder (e.g. 1st is disabled) + - Pre-arm check that low and critical battery failsafe thresholds are different + - Pre-arm message fixed if 2nd EKF core unhealthy + - Pre-arm check if reboot required to enabled IMU batch sampling (used for vibe analysis) + +13) Minor enhancements + - Boot time reduced by improving parameter conversion efficiency + - BRD_SAFETYENABLE parameter renamed to BRD_SAFETY_DEFLT + - Compass calibration auxiliary switch function (set RCx_OPTION=171) + - Disable IMU3 auxiliary switch function (set RCx_OPTION=110) + - MAVFTP supports file renaming + - MAVLink in-progress reply to some requests for calibration from GCS + +14) Bug fixes: + - ADSB telemetry and callsign fixes + - Battery pct reported to GCS limited to 0% to 100% range + - Bi-directional DShot fix on H7 boards after system time wrap (more complete fix than in 4.3.6) + - DisplayPort OSD screen reliability improvement on heavily loaded OSDs especially F4 boards + - DisplayPort OSD artificial horizon better matches actual horizon + - EFI Serial MS bug fix to avoid possible infinite loop + - EKF3 Replay fix when COMPASS_LEARN=3 + - ESC Telemetry external temp reporting fix + - Fence upload works even if Auto mode is excluded from firmware + - FMT messages logged even when Fence is exncluded from firmware (e.g. unselected when using custom build server) + - Hardfault avoided if user changes INS_LOG_BAT_CNT while batch sampling running + - ICM20649 temp sensor tolerate increased to avoid unnecessary FIFO reset + - IMU detection bug fix to avoid duplicates + - IMU temp cal fix when using auxiliary IMU + - Message Interval fix for restoring default rate https://github.com/ArduPilot/ardupilot/pull/21947 + - RADIO_STATUS messages slow-down feature never completely stops messages from being sent + - SERVOx_TRIM value output momentarily if SERVOx_FUNCTION is changed from Disabled to RCPassThru, RCIN1, etc. Avoids momentary divide-by-zero + - Scripting file system open fix + - Scripting PWM source deletion crash fix + - MAVFTP fix for low baudrates (4800 baud and lower) + - ModalAI VOXL reset handling fix + - MPU6500 IMU fast sampling rate to 4k (was 1K) + - NMEA GPGGA output fixed for GPS quality, num sats and hdop + - Position control reset avoided even with very uneven main loop rate due to high CPU load + - Terrain offset increased from 15m to 30m (see TERRAIN_OFS_MAX) to reduce chance of "clamping" + - Throttle notch FFT tuning param fix + +15) Developer specific items + - DroneCAN replaces UAVCAN + - FlighAxis simulator rangefinder fixed + - Scripts in applet and drivers directory checked using linter + - Simulator supports main loop timing jitter (see SIM_TIME_JITTER) + - Simulink model and init scripts + - SITL on hardware support (useful to demo servos moving in response to simulated flight) + - SITL parameter definitions added (some, not all) + - Webots 2023a simulator support + - XPlane support for wider range of aircraft + +16) Plane specific changes + - new aerobatics scripting system with flexible schedules + - added plane-3d SITL model + - added quadlane landing abort AUX switch + - added TKOFF_GND_PITCH for taildragger takeoff + - new ACRO_LOCKING=2 mode for quaternion locking with yaw rate controller + - allow yaw rate autotune in modes other than AUTOTUNE + - use a cone for QRTL climb close to home + - added Y4 VTOL config for quadplanes + - added throttle scaling for vectored yaw + - added turn corrdination to yaw AUTOTUNE + - added Q_OPTION for motor tilt when disarmed in fixed wing modes + + +Release 4.3.5 26th March 2023 +------------------------------ + +- fixed 32 bit microsecond wrap in BDShot code + +This release has a single bug fix for a critical bug for anyone using +bi-directional DShot on their vehicle. If using bi-directional DShot +and the vehicle is running its motors when 32 bit microsecond time +wraps at 71 minutes (or multiples of 71 minutes) then the bug that is +fixed in this release has an approximately 1 in 3 chance of causing +the motor to stop. + +Note that the time is the time since boot, not the time since arming, +so even vehicles flying for a short time could be vulnerable if they +sit for a long time on the ground before takeoff. + Release 4.3.4 1st March 2023 ----------------------------- diff --git a/ArduPlane/afs_plane.cpp b/ArduPlane/afs_plane.cpp index 2900d6e767a0f..298d7f5fe8479 100644 --- a/ArduPlane/afs_plane.cpp +++ b/ArduPlane/afs_plane.cpp @@ -103,4 +103,11 @@ AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Plane::afs_mode(void) } return AP_AdvancedFailsafe::AFS_STABILIZED; } + +//to force entering auto mode when datalink loss + void AP_AdvancedFailsafe_Plane::set_mode_auto(void) + { + plane.set_mode(plane.mode_auto,ModeReason::GCS_FAILSAFE); + } + #endif // AP_ADVANCEDFAILSAFE_ENABLED diff --git a/ArduPlane/afs_plane.h b/ArduPlane/afs_plane.h index 142de7f345253..ffd341481c3ff 100644 --- a/ArduPlane/afs_plane.h +++ b/ArduPlane/afs_plane.h @@ -39,6 +39,9 @@ class AP_AdvancedFailsafe_Plane : public AP_AdvancedFailsafe // return the AFS mapped control mode enum control_mode afs_mode(void) override; + + //to force entering auto mode when datalink loss + void set_mode_auto(void) override; }; #endif // AP_ADVANCEDFAILSAFE_ENABLED diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 6387edef12aad..4c3a35f4097ca 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -28,6 +28,27 @@ void Plane::adjust_altitude_target() control_mode->update_target_altitude(); } +void Plane::check_home_alt_change(void) +{ + int32_t home_alt_cm = ahrs.get_home().alt; + if (home_alt_cm != auto_state.last_home_alt_cm && hal.util->get_soft_armed()) { + // cope with home altitude changing + const int32_t alt_change_cm = home_alt_cm - auto_state.last_home_alt_cm; + if (next_WP_loc.terrain_alt) { + /* + next_WP_loc for terrain alt WP are quite strange. They + have terrain_alt=1, but also have relative_alt=0 and + have been calculated to be relative to home. We need to + adjust for the change in home alt + */ + next_WP_loc.alt += alt_change_cm; + } + // reset TECS to force the field elevation estimate to reset + TECS_controller.reset(); + } + auto_state.last_home_alt_cm = home_alt_cm; +} + /* setup for a gradual glide slope to the next waypoint, if appropriate */ @@ -191,6 +212,12 @@ void Plane::set_target_altitude_location(const Location &loc) target_altitude.amsl_cm += home.alt; } #if AP_TERRAIN_AVAILABLE + if (target_altitude.terrain_following_pending) { + /* we didn't get terrain data to init when we started on this + target, retry + */ + setup_terrain_target_alt(next_WP_loc); + } /* if this location has the terrain_alt flag set and we know the terrain altitude of our current location then treat it as a @@ -448,12 +475,16 @@ bool Plane::above_location_current(const Location &loc) modify a destination to be setup for terrain following if TERRAIN_FOLLOW is enabled */ -void Plane::setup_terrain_target_alt(Location &loc) const +void Plane::setup_terrain_target_alt(Location &loc) { #if AP_TERRAIN_AVAILABLE if (terrain_enabled_in_current_mode()) { - loc.change_alt_frame(Location::AltFrame::ABOVE_TERRAIN); + if (!loc.change_alt_frame(Location::AltFrame::ABOVE_TERRAIN)) { + target_altitude.terrain_following_pending = true; + return; + } } + target_altitude.terrain_following_pending = false; #endif } diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 299f5e004ccf1..823f1e4da93bb 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -8,7 +8,11 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) // default to non-VTOL loiter auto_state.vtol_loiter = false; - // log when new commands start +#if AP_TERRAIN_AVAILABLE + plane.target_altitude.terrain_following_pending = false; +#endif + + // log when new commands start if (should_log(MASK_LOG_CMD)) { logger.Write_Mission_Cmd(mission, cmd); } @@ -19,9 +23,6 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) // except in a takeoff auto_state.takeoff_complete = true; - // start non-idle - auto_state.idle_mode = false; - nav_controller->set_data_is_stale(); // reset loiter start time. New command is a new loiter @@ -89,7 +90,6 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) break; case MAV_CMD_NAV_ALTITUDE_WAIT: - do_altitude_wait(cmd); break; #if HAL_QUADPLANE_ENABLED @@ -195,7 +195,8 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_DO_ENGINE_CONTROL: plane.g2.ice_control.engine_control(cmd.content.do_engine_control.start_control, cmd.content.do_engine_control.cold_start, - cmd.content.do_engine_control.height_delay_cm*0.01f); + cmd.content.do_engine_control.height_delay_cm*0.01f, + cmd.content.do_engine_control.allow_disarmed_start); break; #endif @@ -206,7 +207,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) #endif case MAV_CMD_NAV_DELAY: - do_nav_delay(cmd); + mode_auto.do_nav_delay(cmd); break; default: @@ -311,7 +312,7 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret #endif case MAV_CMD_NAV_DELAY: - return verify_nav_delay(cmd); + return mode_auto.verify_nav_delay(cmd); // do commands (always return true) case MAV_CMD_DO_CHANGE_SPEED: @@ -344,7 +345,7 @@ void Plane::do_RTL(int32_t rtl_altitude_AMSL_cm) auto_state.next_wp_crosstrack = false; auto_state.crosstrack = false; prev_WP_loc = current_loc; - next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, rtl_altitude_AMSL_cm); + next_WP_loc = calc_best_rally_or_home_location(current_loc, rtl_altitude_AMSL_cm); setup_terrain_target_alt(next_WP_loc); set_target_altitude_location(next_WP_loc); @@ -360,6 +361,17 @@ void Plane::do_RTL(int32_t rtl_altitude_AMSL_cm) logger.Write_Mode(control_mode->mode_number(), control_mode_reason); } +Location Plane::calc_best_rally_or_home_location(const Location &_current_loc, float rtl_home_alt_amsl_cm) const +{ +#if HAL_RALLY_ENABLED + return plane.rally.calc_best_rally_or_home_location(_current_loc, rtl_home_alt_amsl_cm); +#else + Location destination = plane.home; + destination.set_alt_cm(rtl_home_alt_amsl_cm, Location::AltFrame::ABSOLUTE); + return destination; +#endif +} + /* start a NAV_TAKEOFF command */ @@ -503,12 +515,6 @@ void Plane::do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd) reset_offset_altitude(); } -void Plane::do_altitude_wait(const AP_Mission::Mission_Command& cmd) -{ - // set all servos to trim until we reach altitude or descent speed - auto_state.idle_mode = true; -} - void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) { //set target alt @@ -522,7 +528,7 @@ void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) } // do_nav_delay - Delay the next navigation command -void Plane::do_nav_delay(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd) { nav_delay.time_start_ms = millis(); @@ -531,7 +537,11 @@ void Plane::do_nav_delay(const AP_Mission::Mission_Command& cmd) nav_delay.time_max_ms = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds } else { // absolute delay to utc time +#if AP_RTC_ENABLED nav_delay.time_max_ms = AP::rtc().get_time_utc(cmd.content.nav_delay.hour_utc, cmd.content.nav_delay.min_utc, cmd.content.nav_delay.sec_utc, 0); +#else + nav_delay.time_max_ms = 0; +#endif } gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec", (unsigned)(nav_delay.time_max_ms/1000)); } @@ -541,7 +551,11 @@ void Plane::do_nav_delay(const AP_Mission::Mission_Command& cmd) /********************************************************************************/ bool Plane::verify_takeoff() { - if (ahrs.dcm_yaw_initialised() && steer_state.hold_course_cd == -1) { + bool trust_ahrs_yaw = AP::ahrs().initialised(); +#if AP_AHRS_DCM_ENABLED + trust_ahrs_yaw |= ahrs.dcm_yaw_initialised(); +#endif + if (trust_ahrs_yaw && steer_state.hold_course_cd == -1) { const float min_gps_speed = 5; if (auto_state.takeoff_speed_time_ms == 0 && gps.status() >= AP_GPS::GPS_OK_FIX_3D && @@ -869,9 +883,9 @@ bool Plane::verify_altitude_wait(const AP_Mission::Mission_Command &cmd) } // verify_nav_delay - check if we have waited long enough -bool Plane::verify_nav_delay(const AP_Mission::Mission_Command& cmd) +bool ModeAuto::verify_nav_delay(const AP_Mission::Mission_Command& cmd) { - if (arming.is_armed_and_safety_off()) { + if (AP::arming().is_armed_and_safety_off()) { // don't delay while armed, we need a nav controller running return true; } @@ -935,28 +949,36 @@ void Plane::do_loiter_at_location() bool Plane::do_change_speed(const AP_Mission::Mission_Command& cmd) { - switch (cmd.content.speed.speed_type) - { + return do_change_speed( + (uint8_t)cmd.content.speed.speed_type, + cmd.content.speed.target_ms, + cmd.content.speed.throttle_pct + ); +} + +bool Plane::do_change_speed(uint8_t speedtype, float speed_target_ms, float throttle_pct) +{ + switch (speedtype) { case 0: // Airspeed - if (is_equal(cmd.content.speed.target_ms, -2.0f)) { + if (is_equal(speed_target_ms, -2.0f)) { new_airspeed_cm = -1; // return to default airspeed return true; - } else if ((cmd.content.speed.target_ms >= aparm.airspeed_min.get()) && - (cmd.content.speed.target_ms <= aparm.airspeed_max.get())) { - new_airspeed_cm = cmd.content.speed.target_ms * 100; //new airspeed target for AUTO or GUIDED modes - gcs().send_text(MAV_SEVERITY_INFO, "Set airspeed %u m/s", (unsigned)cmd.content.speed.target_ms); + } else if ((speed_target_ms >= aparm.airspeed_min.get()) && + (speed_target_ms <= aparm.airspeed_max.get())) { + new_airspeed_cm = speed_target_ms * 100; //new airspeed target for AUTO or GUIDED modes + gcs().send_text(MAV_SEVERITY_INFO, "Set airspeed %u m/s", (unsigned)speed_target_ms); return true; } break; case 1: // Ground speed - gcs().send_text(MAV_SEVERITY_INFO, "Set groundspeed %u", (unsigned)cmd.content.speed.target_ms); - aparm.min_gndspeed_cm.set(cmd.content.speed.target_ms * 100); + gcs().send_text(MAV_SEVERITY_INFO, "Set groundspeed %u", (unsigned)speed_target_ms); + aparm.min_gndspeed_cm.set(speed_target_ms * 100); return true; } - if (cmd.content.speed.throttle_pct > 0 && cmd.content.speed.throttle_pct <= 100) { - gcs().send_text(MAV_SEVERITY_INFO, "Set throttle %u", (unsigned)cmd.content.speed.throttle_pct); - aparm.throttle_cruise.set(cmd.content.speed.throttle_pct); + if (throttle_pct > 0 && throttle_pct <= 100) { + gcs().send_text(MAV_SEVERITY_INFO, "Set throttle %u", (unsigned)throttle_pct); + aparm.throttle_cruise.set(throttle_pct); return true; } @@ -1051,7 +1073,7 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd) { // validate that the vehicle is at least the expected distance away from the loiter point // require an angle total of at least 2 centidegrees, due to special casing of 1 centidegree - if (((fabsf(cmd.content.location.get_distance(current_loc) - abs_radius) > 5.0f) && + if (((fabsF(cmd.content.location.get_distance(current_loc) - abs_radius) > 5.0f) && (cmd.content.location.get_distance(current_loc) < abs_radius)) || (labs(loiter.sum_cd) < 2)) { nav_controller->update_loiter(cmd.content.location, abs_radius, direction); @@ -1067,7 +1089,7 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd) const float breakout_direction_rad = radians(vtol_approach_s.approach_direction_deg + (direction > 0 ? 270 : 90)); // breakout when within 5 degrees of the opposite direction - if (fabsf(wrap_PI(ahrs.yaw - breakout_direction_rad)) < radians(5.0f)) { + if (fabsF(wrap_PI(ahrs.yaw - breakout_direction_rad)) < radians(5.0f)) { gcs().send_text(MAV_SEVERITY_INFO, "Starting VTOL land approach path"); vtol_approach_s.approach_stage = APPROACH_LINE; set_next_WP(cmd.content.location); diff --git a/ArduPlane/config.h b/ArduPlane/config.h index ba29c2062d801..a8c6c890c96cb 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -238,7 +238,7 @@ #endif #ifndef OFFBOARD_GUIDED - #define OFFBOARD_GUIDED !HAL_MINIMIZE_FEATURES + #define OFFBOARD_GUIDED 1 #endif ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp index 8caeae8204279..7f15e0e2663f9 100644 --- a/ArduPlane/control_modes.cpp +++ b/ArduPlane/control_modes.cpp @@ -98,61 +98,23 @@ Mode *Plane::mode_from_mode_num(const enum Mode::Number num) return ret; } -void Plane::read_control_switch() +void RC_Channels_Plane::read_mode_switch() { - static bool switch_debouncer; - uint8_t switchPosition = readSwitch(); - - // If switchPosition = 255 this indicates that the mode control channel input was out of range - // If we get this value we do not want to change modes. - if(switchPosition == 255) return; - - if (!rc().has_valid_input()) { - // ignore the mode switch channel if there is no valid RC input - return; - } - - if (millis() - failsafe.last_valid_rc_ms > 100) { + if (millis() - plane.failsafe.last_valid_rc_ms > 100) { // only use signals that are less than 0.1s old. return; } - - if (oldSwitchPosition != switchPosition) { - - if (switch_debouncer == false) { - // this ensures that mode switches only happen if the - // switch changes for 2 reads. This prevents momentary - // spikes in the mode control channel from causing a mode - // switch - switch_debouncer = true; - return; - } - - set_mode_by_number((enum Mode::Number)flight_modes[switchPosition].get(), ModeReason::RC_COMMAND); - - oldSwitchPosition = switchPosition; - } - - switch_debouncer = false; - + RC_Channels::read_mode_switch(); } -uint8_t Plane::readSwitch(void) const +void RC_Channel_Plane::mode_switch_changed(modeswitch_pos_t new_pos) { - uint16_t pulsewidth = RC_Channels::get_radio_in(g.flight_mode_channel - 1); - if (pulsewidth <= 900 || pulsewidth >= 2200) return 255; // This is an error condition - if (pulsewidth <= 1230) return 0; - if (pulsewidth <= 1360) return 1; - if (pulsewidth <= 1490) return 2; - if (pulsewidth <= 1620) return 3; - if (pulsewidth <= 1749) return 4; // Software Manual - return 5; // Hardware Manual -} + if (new_pos < 0 || (uint8_t)new_pos > plane.num_flight_modes) { + // should not have been called + return; + } -void Plane::reset_control_switch() -{ - oldSwitchPosition = 254; - read_control_switch(); + plane.set_mode_by_number((Mode::Number)plane.flight_modes[new_pos].get(), ModeReason::RC_COMMAND); } /* diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index e880eef494b3a..c7389daf49bdc 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -8,6 +8,8 @@ #define MIN_AIRSPEED_MIN 5 // m/s, used for arming check and speed scaling +#define TAKEOFF_RUDDER_WARNING_TIMEOUT 3000 //ms that GCS warning about not returning arming rudder to neutral repeats + // failsafe // ---------------------- enum failsafe_state { @@ -42,6 +44,7 @@ enum failsafe_action_long { FS_ACTION_LONG_RTL = 1, FS_ACTION_LONG_GLIDE = 2, FS_ACTION_LONG_PARACHUTE = 3, + FS_ACTION_LONG_AUTO = 4, }; // type of stick mixing enabled @@ -120,6 +123,7 @@ enum log_messages { #define MASK_LOG_IMU_RAW (1UL<<19) #define MASK_LOG_ATTITUDE_FULLRATE (1U<<20) #define MASK_LOG_VIDEO_STABILISATION (1UL<<21) +#define MASK_LOG_NOTCH_FULLRATE (1UL<<22) enum { CRASH_DETECT_ACTION_BITMASK_DISABLED = 0, @@ -161,7 +165,7 @@ enum FlightOptions { CENTER_THROTTLE_TRIM = (1<<10), DISABLE_GROUND_PID_SUPPRESSION = (1<<11), ENABLE_LOITER_ALT_CONTROL = (1<<12), - + INDICATE_WAITING_FOR_RUDDER_NEUTRAL = (1<<13), }; enum CrowFlapOptions { diff --git a/ArduPlane/ekf_check.cpp b/ArduPlane/ekf_check.cpp index 86b972225ce76..e36e4ef1abcdf 100644 --- a/ArduPlane/ekf_check.cpp +++ b/ArduPlane/ekf_check.cpp @@ -112,7 +112,7 @@ bool Plane::ekf_over_threshold() return false; } - // Get EKF innovations normalised wrt the innovaton test limits. + // Get EKF innovations normalised wrt the innovation test limits. // A value above 1.0 means the EKF has rejected that sensor data float position_variance, vel_variance, height_variance, tas_variance; Vector3f mag_variance; @@ -157,7 +157,7 @@ void Plane::failsafe_ekf_event() ekf_check_state.failsafe_on = true; AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED); - // if not in a VTOL mode requring position, then nothing needs to be done + // if not in a VTOL mode requiring position, then nothing needs to be done #if HAL_QUADPLANE_ENABLED if (!quadplane.in_vtol_posvel_mode()) { return; diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 9c2a215608ce7..f49814aca9b90 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -107,6 +107,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason) { + // This is how to handle a long loss of control signal failsafe. // If the GCS is locked up we allow control to revert to RC RC_Channels::clear_overrides(); @@ -124,6 +125,14 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason case Mode::Number::CIRCLE: case Mode::Number::LOITER: case Mode::Number::THERMAL: + case Mode::Number::TAKEOFF: + if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF && !(g.fs_action_long == FS_ACTION_LONG_GLIDE || g.fs_action_long == FS_ACTION_LONG_PARACHUTE)) { + // don't failsafe if in inital climb of TAKEOFF mode and FS action is not parachute or glide + // long failsafe will be re-called if still in fs after initial climb + long_failsafe_pending = true; + break; + } + if(plane.emergency_landing) { set_mode(mode_fbwa, reason); // emergency landing switch overrides normal action to allow out of range landing break; @@ -131,9 +140,13 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) { #if PARACHUTE == ENABLED parachute_release(); + //stop motors to avoid parachute tangling + plane.arming.disarm(AP_Arming::Method::PARACHUTE_RELEASE, false); #endif } else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) { set_mode(mode_fbwa, reason); + } else if (g.fs_action_long == FS_ACTION_LONG_AUTO) { + set_mode(mode_auto, reason); } else { set_mode(mode_rtl, reason); } @@ -166,24 +179,32 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason case Mode::Number::AVOID_ADSB: case Mode::Number::GUIDED: + if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) { #if PARACHUTE == ENABLED parachute_release(); + //stop motors to avoid parachute tangling + plane.arming.disarm(AP_Arming::Method::PARACHUTE_RELEASE, false); #endif } else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) { set_mode(mode_fbwa, reason); + } else if (g.fs_action_long == FS_ACTION_LONG_AUTO) { + set_mode(mode_auto, reason); } else if (g.fs_action_long == FS_ACTION_LONG_RTL) { set_mode(mode_rtl, reason); } break; case Mode::Number::RTL: + if (g.fs_action_long == FS_ACTION_LONG_AUTO) { + set_mode(mode_auto, reason); + } + break; #if HAL_QUADPLANE_ENABLED case Mode::Number::QLAND: case Mode::Number::QRTL: case Mode::Number::LOITER_ALT_QLAND: #endif - case Mode::Number::TAKEOFF: case Mode::Number::INITIALISING: break; } @@ -204,6 +225,7 @@ void Plane::failsafe_short_off_event(ModeReason reason) void Plane::failsafe_long_off_event(ModeReason reason) { + long_failsafe_pending = false; // We're back in radio contact with RC or GCS if (reason == ModeReason:: GCS_FAILSAFE) { gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe Off"); diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index 28f4abdca677e..640ad65869926 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -75,7 +75,7 @@ void Plane::fence_check() Location loc; if (fence.get_return_rally() != 0 || fence_act == AC_FENCE_ACTION_RTL_AND_LAND) { - loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude_cm()); + loc = calc_best_rally_or_home_location(current_loc, get_RTL_altitude_cm()); } else { //return to fence return point, not a rally point if (fence.get_return_altitude() > 0) { diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index 3b2c27396f59d..ffe578736aff1 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -261,7 +261,7 @@ void Plane::crash_detection_update(void) if (g.takeoff_throttle_min_accel > 0 && !throttle_suppressed) { // if you have an acceleration holding back throttle, but you met the - // accel threshold but still not fying, then you either shook/hit the + // accel threshold but still not flying, then you either shook/hit the // plane or it was a failed launch. crashed = true; crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS; diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index 63559a50dd995..b6c3a2b44fc86 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -32,6 +32,9 @@ bool Mode::enter() // cancel inverted flight plane.auto_state.inverted_flight = false; + + // cancel waiting for rudder neutral + plane.takeoff_state.waiting_for_rudder_neutral = false; // don't cross-track when starting a mission plane.auto_state.next_wp_crosstrack = false; @@ -56,6 +59,7 @@ bool Mode::enter() plane.guided_state.target_heading_type = GUIDED_HEADING_NONE; plane.guided_state.target_airspeed_cm = -1; // same as above, although an airspeed of -1 is rare on plane. plane.guided_state.target_alt = -1; // same as above, although a target alt of -1 is rare on plane. + plane.guided_state.target_alt_time_ms = 0; plane.guided_state.last_target_alt = 0; #endif @@ -85,11 +89,18 @@ bool Mode::enter() // initialize speed variable used in AUTO and GUIDED for DO_CHANGE_SPEED commands plane.new_airspeed_cm = -1; + + // clear postponed long failsafe if mode change (from GCS) occurs before recall of long failsafe + plane.long_failsafe_pending = false; #if HAL_QUADPLANE_ENABLED quadplane.mode_enter(); #endif +#if AP_TERRAIN_AVAILABLE + plane.target_altitude.terrain_following_pending = false; +#endif + bool enter_result = _enter(); if (enter_result) { @@ -102,6 +113,9 @@ bool Mode::enter() plane.adsb.set_is_auto_mode(does_auto_navigation()); #endif + // set the nav controller stale AFTER _enter() so that we can check if we're currently in a loiter during the mode change + plane.nav_controller->set_data_is_stale(); + // reset steering integrator on mode change plane.steerController.reset_I(); @@ -114,6 +128,13 @@ bool Mode::enter() // but it should be harmless to disable the fence temporarily in these situations as well plane.fence.manual_recovery_start(); #endif + //reset mission if in landing sequence, disarmed, not flying, and have changed to a non-autothrottle mode to clear prearm + if (plane.mission.get_in_landing_sequence_flag() && + !plane.is_flying() && !plane.arming.is_armed_and_safety_off() && + !plane.control_mode->does_auto_navigation()) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "In landing sequence: mission reset"); + plane.mission.reset(); + } } return enter_result; @@ -197,3 +218,40 @@ bool Mode::_pre_arm_checks(size_t buflen, char *buffer) const #endif return true; } + +void Mode::run() +{ + // Direct stick mixing functionality has been removed, so as not to remove all stick mixing from the user completely + // the old direct option is now used to enable fbw mixing, this is easier than doing a param conversion. + if ((plane.g.stick_mixing == StickMixing::FBW) || (plane.g.stick_mixing == StickMixing::DIRECT_REMOVED)) { + plane.stabilize_stick_mixing_fbw(); + } + plane.stabilize_roll(); + plane.stabilize_pitch(); + plane.stabilize_yaw(); +} + +// Reset rate and steering controllers +void Mode::reset_controllers() +{ + // reset integrators + plane.rollController.reset_I(); + plane.pitchController.reset_I(); + plane.yawController.reset_I(); + + // reset steering controls + plane.steer_state.locked_course = false; + plane.steer_state.locked_course_err = 0; +} + +bool Mode::is_taking_off() const +{ + return (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF); +} + +// Helper to output to both k_rudder and k_steering servo functions +void Mode::output_rudder_and_steering(float val) +{ + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, val); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, val); +} diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 46e56c284c4a7..2f745074c6f27 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -8,6 +8,7 @@ #include #include "quadplane.h" #include +#include class AC_PosControl; class AC_AttitudeControl_Multi; @@ -65,7 +66,7 @@ class Mode void exit(); // run controllers specific to this mode - virtual void run() {}; + virtual void run(); // returns a unique number specific to this mode virtual Number mode_number() const = 0; @@ -79,6 +80,9 @@ class Mode // returns true if the vehicle can be armed in this mode bool pre_arm_checks(size_t buflen, char *buffer) const; + // Reset rate and steering controllers + void reset_controllers(); + // // methods that sub classes should override to affect movement of the vehicle in this mode // @@ -125,6 +129,13 @@ class Mode // handle a guided target request from GCS virtual bool handle_guided_request(Location target_loc) { return false; } + // true if is landing + virtual bool is_landing() const { return false; } + + // true if is taking + virtual bool is_taking_off() const; + + protected: // subclasses override this to perform checks before entering the mode @@ -136,6 +147,9 @@ class Mode // mode specific pre-arm checks virtual bool _pre_arm_checks(size_t buflen, char *buffer) const; + // Helper to output to both k_rudder and k_steering servo functions + void output_rudder_and_steering(float val); + #if HAL_QUADPLANE_ENABLED // References for convenience, used by QModes AC_PosControl*& pos_control; @@ -206,11 +220,27 @@ class ModeAuto : public Mode bool mode_allows_autotuning() const override { return true; } + bool is_landing() const override; + + void do_nav_delay(const AP_Mission::Mission_Command& cmd); + bool verify_nav_delay(const AP_Mission::Mission_Command& cmd); + + void run() override; + protected: bool _enter() override; void _exit() override; bool _pre_arm_checks(size_t buflen, char *buffer) const override; + +private: + + // Delay the next navigation command + struct { + uint32_t time_max_ms; + uint32_t time_start_ms; + } nav_delay; + }; @@ -355,6 +385,8 @@ class ModeManual : public Mode // methods that affect movement of the vehicle in this mode void update() override; + + void run() override; }; @@ -398,6 +430,12 @@ class ModeStabilize : public Mode // methods that affect movement of the vehicle in this mode void update() override; + + void run() override; + +private: + void stabilize_stick_mixing_direct(); + }; class ModeTraining : public Mode diff --git a/ArduPlane/mode_LoiterAltQLand.cpp b/ArduPlane/mode_LoiterAltQLand.cpp index 0cc8b9457c11e..8fbfd59735cd2 100644 --- a/ArduPlane/mode_LoiterAltQLand.cpp +++ b/ArduPlane/mode_LoiterAltQLand.cpp @@ -10,17 +10,13 @@ bool ModeLoiterAltQLand::_enter() return true; } + // If we were already in a loiter then use that waypoint. Else, use the current point + const bool already_in_a_loiter = plane.nav_controller->reached_loiter_target() && !plane.nav_controller->data_is_stale(); + const Location loiter_wp = already_in_a_loiter ? plane.next_WP_loc : plane.current_loc; + ModeLoiter::_enter(); -#if AP_TERRAIN_AVAILABLE - if (plane.terrain_enabled_in_mode(Mode::Number::QLAND)) { - plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_TERRAIN); - } else { - plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME); - } -#else - plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME); -#endif + handle_guided_request(loiter_wp); switch_qland(); diff --git a/ArduPlane/mode_acro.cpp b/ArduPlane/mode_acro.cpp index 5bfeea2f81b92..c15970b9bdece 100644 --- a/ArduPlane/mode_acro.cpp +++ b/ArduPlane/mode_acro.cpp @@ -68,7 +68,7 @@ void ModeAcro::stabilize() int32_t roll_error_cd = -ToDeg(acro_state.locked_roll_err)*100; plane.nav_roll_cd = ahrs.roll_sensor + roll_error_cd; // try to reduce the integrated angular error to zero. We set - // 'stabilze' to true, which disables the roll integrator + // 'stabilize' to true, which disables the roll integrator SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.rollController.get_servo_out(roll_error_cd, speed_scaler, true, false)); @@ -104,22 +104,24 @@ void ModeAcro::stabilize() SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitchController.get_rate_out(pitch_rate, speed_scaler)); } - plane.steering_control.steering = plane.rudder_input(); - + float rudder_output; if (plane.g.acro_yaw_rate > 0 && plane.yawController.rate_control_enabled()) { // user has asked for yaw rate control with yaw rate scaled by ACRO_YAW_RATE const float rudd_expo = plane.rudder_in_expo(true); const float yaw_rate = (rudd_expo/SERVO_MAX) * plane.g.acro_yaw_rate; - plane.steering_control.steering = plane.steering_control.rudder = plane.yawController.get_rate_out(yaw_rate, speed_scaler, false); - } else if (plane.g2.flight_options & FlightOptions::ACRO_YAW_DAMPER) { + rudder_output = plane.yawController.get_rate_out(yaw_rate, speed_scaler, false); + } else if (plane.flight_option_enabled(FlightOptions::ACRO_YAW_DAMPER)) { // use yaw controller - plane.calc_nav_yaw_coordinated(); + rudder_output = plane.calc_nav_yaw_coordinated(); } else { /* manual rudder */ - plane.steering_control.rudder = plane.steering_control.steering; + rudder_output = plane.rudder_input(); } + + output_rudder_and_steering(rudder_output); + } /* @@ -215,7 +217,7 @@ void ModeAcro::stabilize_quaternion() // call to rate controllers SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.rollController.get_rate_out(desired_rates.x, speed_scaler)); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitchController.get_rate_out(desired_rates.y, speed_scaler)); - plane.steering_control.steering = plane.steering_control.rudder = plane.yawController.get_rate_out(desired_rates.z, speed_scaler, false); + output_rudder_and_steering(plane.yawController.get_rate_out(desired_rates.z, speed_scaler, false)); acro_state.roll_active_last = roll_active; acro_state.pitch_active_last = pitch_active; diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp index 548dd0b2618b8..f1a9f5de5540b 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -159,3 +159,24 @@ bool ModeAuto::_pre_arm_checks(size_t buflen, char *buffer) const // Note that this bypasses the base class checks return true; } + +bool ModeAuto::is_landing() const +{ + return (plane.flight_stage == AP_FixedWing::FlightStage::LAND); +} + +void ModeAuto::run() +{ + if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_ALTITUDE_WAIT) { + // Wiggle servos + plane.set_servos_idle(); + + // Relax attitude control + reset_controllers(); + + } else { + // Normal flight, run base class + Mode::run(); + + } +} diff --git a/ArduPlane/mode_cruise.cpp b/ArduPlane/mode_cruise.cpp index 57d3bcb4bee2d..2d8c4c428c242 100644 --- a/ArduPlane/mode_cruise.cpp +++ b/ArduPlane/mode_cruise.cpp @@ -58,11 +58,17 @@ void ModeCruise::navigate() return; } #endif + + // check if we are moving in the direction of the front of the vehicle + const int32_t ground_course_cd = plane.gps.ground_course_cd(); + const bool moving_forwards = fabsf(wrap_PI(radians(ground_course_cd * 0.01) - plane.ahrs.yaw)) < M_PI_2; + if (!locked_heading && plane.channel_roll->get_control_in() == 0 && plane.rudder_input() == 0 && plane.gps.status() >= AP_GPS::GPS_OK_FIX_2D && plane.gps.ground_speed() >= 3 && + moving_forwards && lock_timer_ms == 0) { // user wants to lock the heading - start the timer lock_timer_ms = millis(); @@ -73,7 +79,7 @@ void ModeCruise::navigate() // from user locked_heading = true; lock_timer_ms = 0; - locked_heading_cd = plane.gps.ground_course_cd(); + locked_heading_cd = ground_course_cd; plane.prev_WP_loc = plane.current_loc; } if (locked_heading) { diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index 900b084f8ca59..a523ddf87065e 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -35,9 +35,62 @@ void ModeGuided::update() return; } #endif - plane.calc_nav_roll(); - plane.calc_nav_pitch(); - plane.calc_throttle(); + + // Received an external msg that guides roll in the last 3 seconds? + if (plane.guided_state.last_forced_rpy_ms.x > 0 && + millis() - plane.guided_state.last_forced_rpy_ms.x < 3000) { + plane.nav_roll_cd = constrain_int32(plane.guided_state.forced_rpy_cd.x, -plane.roll_limit_cd, plane.roll_limit_cd); + plane.update_load_factor(); + +#if OFFBOARD_GUIDED == ENABLED + // guided_state.target_heading is radians at this point between -pi and pi ( defaults to -4 ) + // This function is used in Guided and AvoidADSB, check for guided + } else if ((plane.control_mode == &plane.mode_guided) && (plane.guided_state.target_heading_type != GUIDED_HEADING_NONE) ) { + uint32_t tnow = AP_HAL::millis(); + float delta = (tnow - plane.guided_state.target_heading_time_ms) * 1e-3f; + plane.guided_state.target_heading_time_ms = tnow; + + float error = 0.0f; + if (plane.guided_state.target_heading_type == GUIDED_HEADING_HEADING) { + error = wrap_PI(plane.guided_state.target_heading - AP::ahrs().yaw); + } else { + Vector2f groundspeed = AP::ahrs().groundspeed_vector(); + error = wrap_PI(plane.guided_state.target_heading - atan2f(-groundspeed.y, -groundspeed.x) + M_PI); + } + + float bank_limit = degrees(atanf(plane.guided_state.target_heading_accel_limit/GRAVITY_MSS)) * 1e2f; + bank_limit = MIN(bank_limit, plane.roll_limit_cd); + + // push error into AC_PID + const float desired = plane.g2.guidedHeading.update_error(error, delta, plane.guided_state.target_heading_limit); + + // Check for output saturation + plane.guided_state.target_heading_limit = fabsf(desired) >= bank_limit; + + plane.nav_roll_cd = constrain_int32(desired, -bank_limit, bank_limit); + plane.update_load_factor(); + +#endif // OFFBOARD_GUIDED == ENABLED + } else { + plane.calc_nav_roll(); + } + + if (plane.guided_state.last_forced_rpy_ms.y > 0 && + millis() - plane.guided_state.last_forced_rpy_ms.y < 3000) { + plane.nav_pitch_cd = constrain_int32(plane.guided_state.forced_rpy_cd.y, plane.pitch_limit_min_cd, plane.aparm.pitch_limit_max_cd.get()); + } else { + plane.calc_nav_pitch(); + } + + // Received an external msg that guides throttle in the last 3 seconds? + if (plane.aparm.throttle_cruise > 1 && + plane.guided_state.last_forced_throttle_ms > 0 && + millis() - plane.guided_state.last_forced_throttle_ms < 3000) { + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.guided_state.forced_throttle); + } else { + plane.calc_throttle(); + } + } void ModeGuided::navigate() diff --git a/ArduPlane/mode_loiter.cpp b/ArduPlane/mode_loiter.cpp index e7b07c9db127e..05ce3a6956857 100644 --- a/ArduPlane/mode_loiter.cpp +++ b/ArduPlane/mode_loiter.cpp @@ -9,7 +9,7 @@ bool ModeLoiter::_enter() // make sure the local target altitude is the same as the nav target used for loiter nav // this allows us to do FBWB style stick control /*IGNORE_RETURN(plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, plane.target_altitude.amsl_cm));*/ - if (plane.stick_mixing_enabled() && (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL)) { + if (plane.stick_mixing_enabled() && (plane.flight_option_enabled(FlightOptions::ENABLE_LOITER_ALT_CONTROL))) { plane.set_target_altitude_current(); } @@ -21,7 +21,7 @@ bool ModeLoiter::_enter() void ModeLoiter::update() { plane.calc_nav_roll(); - if (plane.stick_mixing_enabled() && (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL)) { + if (plane.stick_mixing_enabled() && plane.flight_option_enabled(FlightOptions::ENABLE_LOITER_ALT_CONTROL)) { plane.update_fbwb_speed_height(); } else { plane.calc_nav_pitch(); @@ -42,8 +42,7 @@ bool ModeLoiter::isHeadingLinedUp(const Location loiterCenterLoc, const Location // Return true if current heading is aligned to vector to targetLoc. // Tolerance is initially 10 degrees and grows at 10 degrees for each loiter circle completed. - const uint16_t loiterRadius = abs(plane.aparm.loiter_radius); - if (loiterCenterLoc.get_distance(targetLoc) < loiterRadius + loiterRadius*0.05) { + if (loiterCenterLoc.get_distance(targetLoc) < 1.05f * fabsf(plane.loiter.radius)) { /* Whenever next waypoint is within the loiter radius plus 5%, maintaining loiter would prevent us from ever pointing toward the next waypoint. Hence break out of loiter immediately @@ -94,7 +93,7 @@ bool ModeLoiter::isHeadingLinedUp_cd(const int32_t bearing_cd) void ModeLoiter::navigate() { - if (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL) { + if (plane.flight_option_enabled(FlightOptions::ENABLE_LOITER_ALT_CONTROL)) { // update the WP alt from the global target adjusted by update_fbwb_speed_height plane.next_WP_loc.set_alt_cm(plane.target_altitude.amsl_cm, Location::AltFrame::ABSOLUTE); } @@ -112,7 +111,7 @@ void ModeLoiter::navigate() void ModeLoiter::update_target_altitude() { - if (plane.stick_mixing_enabled() && (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL)) { + if (plane.stick_mixing_enabled() && (plane.flight_option_enabled(FlightOptions::ENABLE_LOITER_ALT_CONTROL))) { return; } Mode::update_target_altitude(); diff --git a/ArduPlane/mode_manual.cpp b/ArduPlane/mode_manual.cpp index 68535f96186ba..1f8fcd702cd14 100644 --- a/ArduPlane/mode_manual.cpp +++ b/ArduPlane/mode_manual.cpp @@ -5,9 +5,30 @@ void ModeManual::update() { SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.roll_in_expo(false)); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitch_in_expo(false)); - plane.steering_control.steering = plane.steering_control.rudder = plane.rudder_in_expo(false); + output_rudder_and_steering(plane.rudder_in_expo(false)); + float throttle = plane.get_throttle_input(true); + + +#if HAL_QUADPLANE_ENABLED + if (quadplane.available() && quadplane.option_is_set(QuadPlane::OPTION::IDLE_GOV_MANUAL)) { + // for quadplanes it can be useful to run the idle governor in MANUAL mode + // as it prevents the VTOL motors from running + int8_t min_throttle = plane.aparm.throttle_min.get(); + + // apply idle governor +#if AP_ICENGINE_ENABLED + plane.g2.ice_control.update_idle_governor(min_throttle); +#endif + throttle = MAX(throttle, min_throttle); + } +#endif + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle); plane.nav_roll_cd = ahrs.roll_sensor; plane.nav_pitch_cd = ahrs.pitch_sensor; } +void ModeManual::run() +{ + reset_controllers(); +} diff --git a/ArduPlane/mode_qacro.cpp b/ArduPlane/mode_qacro.cpp index d1c69de4e3e0d..d49e1e0fb6152 100644 --- a/ArduPlane/mode_qacro.cpp +++ b/ArduPlane/mode_qacro.cpp @@ -9,7 +9,7 @@ bool ModeQAcro::_enter() quadplane.transition->force_transition_complete(); attitude_control->relax_attitude_controllers(); - // disable yaw rate time contant to mantain old behaviour + // disable yaw rate time constant to maintain old behaviour quadplane.disable_yaw_rate_time_constant(); IGNORE_RETURN(ahrs.get_quaternion(plane.mode_acro.acro_state.q)); @@ -32,6 +32,13 @@ void ModeQAcro::update() */ void ModeQAcro::run() { + const uint32_t now = AP_HAL::millis(); + if (quadplane.tailsitter.in_vtol_transition(now)) { + // Tailsitters in FW pull up phase of VTOL transition run FW controllers + Mode::run(); + return; + } + if (quadplane.throttle_wait) { quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); attitude_control->set_throttle_out(0, true, 0); @@ -64,6 +71,9 @@ void ModeQAcro::run() // output pilot's throttle without angle boost attitude_control->set_throttle_out(throttle_out, false, 10.0f); } + + // Stabilize with fixed wing surfaces + plane.mode_acro.run(); } #endif diff --git a/ArduPlane/mode_qautotune.cpp b/ArduPlane/mode_qautotune.cpp index d74cf792b5d05..28bf977128faf 100644 --- a/ArduPlane/mode_qautotune.cpp +++ b/ArduPlane/mode_qautotune.cpp @@ -21,9 +21,20 @@ void ModeQAutotune::update() void ModeQAutotune::run() { + const uint32_t now = AP_HAL::millis(); + if (quadplane.tailsitter.in_vtol_transition(now)) { + // Tailsitters in FW pull up phase of VTOL transition run FW controllers + Mode::run(); + return; + } + #if QAUTOTUNE_ENABLED quadplane.qautotune.run(); #endif + + // Stabilize with fixed wing surfaces + plane.stabilize_roll(); + plane.stabilize_pitch(); } void ModeQAutotune::_exit() diff --git a/ArduPlane/mode_qhover.cpp b/ArduPlane/mode_qhover.cpp index 13c1a4c4e65ed..fac2147f93c45 100644 --- a/ArduPlane/mode_qhover.cpp +++ b/ArduPlane/mode_qhover.cpp @@ -24,14 +24,26 @@ void ModeQHover::update() */ void ModeQHover::run() { + const uint32_t now = AP_HAL::millis(); + if (quadplane.tailsitter.in_vtol_transition(now)) { + // Tailsitters in FW pull up phase of VTOL transition run FW controllers + Mode::run(); + return; + } + if (quadplane.throttle_wait) { 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_z_controller(0); } else { + plane.quadplane.assign_tilt_to_fwd_thr(); quadplane.hold_hover(quadplane.get_pilot_desired_climb_rate_cms()); } + + // Stabilize with fixed wing surfaces + plane.stabilize_roll(); + plane.stabilize_pitch(); } #endif diff --git a/ArduPlane/mode_qloiter.cpp b/ArduPlane/mode_qloiter.cpp index 26ef2bf214f33..2eea970c58be3 100644 --- a/ArduPlane/mode_qloiter.cpp +++ b/ArduPlane/mode_qloiter.cpp @@ -28,6 +28,13 @@ void ModeQLoiter::update() // run quadplane loiter controller void ModeQLoiter::run() { + const uint32_t now = AP_HAL::millis(); + if (quadplane.tailsitter.in_vtol_transition(now)) { + // Tailsitters in FW pull up phase of VTOL transition run FW controllers + Mode::run(); + return; + } + if (quadplane.throttle_wait) { quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); attitude_control->set_throttle_out(0, true, 0); @@ -35,6 +42,10 @@ void ModeQLoiter::run() pos_control->relax_z_controller(0); loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); + + // Stabilize with fixed wing surfaces + plane.stabilize_roll(); + plane.stabilize_pitch(); return; } if (!quadplane.motors->armed()) { @@ -45,7 +56,6 @@ void ModeQLoiter::run() loiter_nav->soften_for_landing(); } - const uint32_t now = AP_HAL::millis(); if (now - quadplane.last_loiter_ms > 500) { loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); @@ -73,6 +83,8 @@ void ModeQLoiter::run() plane.nav_roll_cd = loiter_nav->get_roll(); plane.nav_pitch_cd = loiter_nav->get_pitch(); + 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_xy(); } @@ -92,7 +104,7 @@ void ModeQLoiter::run() #if AP_ICENGINE_ENABLED // cut IC engine if enabled if (quadplane.land_icengine_cut != 0) { - plane.g2.ice_control.engine_control(0, 0, 0); + plane.g2.ice_control.engine_control(0, 0, 0, false); } #endif // AP_ICENGINE_ENABLED } @@ -112,6 +124,10 @@ void ModeQLoiter::run() quadplane.set_climb_rate_cms(quadplane.get_pilot_desired_climb_rate_cms()); } quadplane.run_z_controller(); + + // Stabilize with fixed wing surfaces + plane.stabilize_roll(); + plane.stabilize_pitch(); } #endif diff --git a/ArduPlane/mode_qrtl.cpp b/ArduPlane/mode_qrtl.cpp index 98bfe3a28342b..9be1c962bd6a4 100644 --- a/ArduPlane/mode_qrtl.cpp +++ b/ArduPlane/mode_qrtl.cpp @@ -17,8 +17,7 @@ bool ModeQRTL::_enter() int32_t RTL_alt_abs_cm = plane.home.alt + quadplane.qrtl_alt*100UL; if (quadplane.motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) { // VTOL motors are active, either in VTOL flight or assisted flight - Location destination = plane.rally.calc_best_rally_or_home_location(plane.current_loc, RTL_alt_abs_cm); - + Location destination = plane.calc_best_rally_or_home_location(plane.current_loc, RTL_alt_abs_cm); const float dist = plane.current_loc.get_distance(destination); const float radius = get_VTOL_return_radius(); @@ -87,6 +86,13 @@ void ModeQRTL::update() */ void ModeQRTL::run() { + const uint32_t now = AP_HAL::millis(); + if (quadplane.tailsitter.in_vtol_transition(now)) { + // Tailsitters in FW pull up phase of VTOL transition run FW controllers + Mode::run(); + return; + } + switch (submode) { case SubMode::climb: { // request zero velocity @@ -97,6 +103,9 @@ void ModeQRTL::run() // nav roll and pitch are controller by position controller plane.nav_roll_cd = pos_control->get_roll_cd(); plane.nav_pitch_cd = pos_control->get_pitch_cd(); + + 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_xy(); } @@ -117,12 +126,12 @@ void ModeQRTL::run() ftype alt_diff; if (!stopping_loc.get_alt_distance(plane.next_WP_loc, alt_diff) || is_positive(alt_diff)) { - // climb finshed or cant get alt diff, head home + // climb finished or cant get alt diff, head home submode = SubMode::RTL; plane.prev_WP_loc = plane.current_loc; int32_t RTL_alt_abs_cm = plane.home.alt + quadplane.qrtl_alt*100UL; - Location destination = plane.rally.calc_best_rally_or_home_location(plane.current_loc, RTL_alt_abs_cm); + Location destination = plane.calc_best_rally_or_home_location(plane.current_loc, RTL_alt_abs_cm); const float dist = plane.current_loc.get_distance(destination); const float radius = get_VTOL_return_radius(); if (dist < radius) { @@ -166,6 +175,10 @@ void ModeQRTL::run() break; } } + + // Stabilize with fixed wing surfaces + plane.stabilize_roll(); + plane.stabilize_pitch(); } /* @@ -185,7 +198,7 @@ void ModeQRTL::update_target_altitude() initially approach at RTL_ALT_CM, then drop down to QRTL_ALT based on maximum sink rate from TECS, giving time to lose speed before we transition */ - const float radius = MAX(fabsf(plane.aparm.loiter_radius), fabsf(plane.g.rtl_radius)); + const float radius = MAX(fabsf(float(plane.aparm.loiter_radius)), fabsf(float(plane.g.rtl_radius))); const float rtl_alt_delta = MAX(0, plane.g.RTL_altitude_cm*0.01 - plane.quadplane.qrtl_alt); const float sink_time = rtl_alt_delta / MAX(0.6*plane.TECS_controller.get_max_sinkrate(), 1); const float sink_dist = plane.aparm.airspeed_cruise_cm * 0.01 * sink_time; @@ -210,7 +223,7 @@ bool ModeQRTL::allows_throttle_nudging() const // Return the radius from destination at which pure VTOL flight should be used, no transition to FW float ModeQRTL::get_VTOL_return_radius() const { - return MAX(fabsf(plane.aparm.loiter_radius), fabsf(plane.g.rtl_radius)) * 1.5; + return MAX(fabsf(float(plane.aparm.loiter_radius)), fabsf(float(plane.g.rtl_radius))) * 1.5; } #endif diff --git a/ArduPlane/mode_qstabilize.cpp b/ArduPlane/mode_qstabilize.cpp index 2c8da2d96885a..2f78a210fb3c2 100644 --- a/ArduPlane/mode_qstabilize.cpp +++ b/ArduPlane/mode_qstabilize.cpp @@ -40,15 +40,30 @@ void ModeQStabilize::update() // quadplane stabilize mode void ModeQStabilize::run() { + const uint32_t now = AP_HAL::millis(); + if (quadplane.tailsitter.in_vtol_transition(now)) { + // Tailsitters in FW pull up phase of VTOL transition run FW controllers + Mode::run(); + return; + } + + plane.quadplane.assign_tilt_to_fwd_thr(); + // special check for ESC calibration in QSTABILIZE if (quadplane.esc_calibration != 0) { quadplane.run_esc_calibration(); + plane.stabilize_roll(); + plane.stabilize_pitch(); return; } // normal QSTABILIZE mode float pilot_throttle_scaled = quadplane.get_pilot_throttle(); quadplane.hold_stabilize(pilot_throttle_scaled); + + // Stabilize with fixed wing surfaces + plane.stabilize_roll(); + plane.stabilize_pitch(); } // set the desired roll and pitch for a tailsitter @@ -68,7 +83,7 @@ void ModeQStabilize::set_tailsitter_roll_pitch(const float roll_input, const flo plane.quadplane.transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd); } -// set the desired roll and pitch for normal quadplanes, also limited by forward flight limtis +// set the desired roll and pitch for normal quadplanes, also limited by forward flight limits void ModeQStabilize::set_limited_roll_pitch(const float roll_input, const float pitch_input) { plane.nav_roll_cd = roll_input * MIN(plane.roll_limit_cd, plane.quadplane.aparm.angle_max); diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp index fbe368b2bc573..417eb6170f22e 100644 --- a/ArduPlane/mode_rtl.cpp +++ b/ArduPlane/mode_rtl.cpp @@ -47,7 +47,7 @@ void ModeRTL::update() plane.calc_throttle(); bool alt_threshold_reached = false; - if (plane.g2.flight_options & FlightOptions::CLIMB_BEFORE_TURN) { + if (plane.flight_option_enabled(FlightOptions::CLIMB_BEFORE_TURN)) { // Climb to ALT_HOLD_RTL before turning. This overrides RTL_CLIMB_MIN. alt_threshold_reached = plane.current_loc.alt > plane.next_WP_loc.alt; } else if (plane.g2.rtl_climb_min > 0) { diff --git a/ArduPlane/mode_stabilize.cpp b/ArduPlane/mode_stabilize.cpp index 7414d57506572..b73dfb6fc2650 100644 --- a/ArduPlane/mode_stabilize.cpp +++ b/ArduPlane/mode_stabilize.cpp @@ -7,3 +7,10 @@ void ModeStabilize::update() plane.nav_pitch_cd = 0; } +void ModeStabilize::run() +{ + plane.stabilize_roll(); + plane.stabilize_pitch(); + stabilize_stick_mixing_direct(); + plane.stabilize_yaw(); +} diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 6e1984a7d9527..db212361c398e 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -1,5 +1,6 @@ #include "mode.h" #include "Plane.h" +#include /* mode takeoff parameters @@ -34,13 +35,13 @@ const AP_Param::GroupInfo ModeTakeoff::var_info[] = { // @Param: DIST // @DisplayName: Takeoff mode distance - // @Description: This is the distance from the takeoff location where the plane will loiter. The loiter point will be in the direction of takeoff (the direction the plane is facing when the motor starts) + // @Description: This is the distance from the takeoff location where the plane will loiter. The loiter point will be in the direction of takeoff (the direction the plane is facing when the plane begins takeoff) // @Range: 0 500 // @Increment: 1 // @Units: m // @User: Standard AP_GROUPINFO("DIST", 4, ModeTakeoff, target_dist, 200), - + // @Param: GND_PITCH // @DisplayName: Takeoff run pitch demand // @Description: Degrees of pitch angle demanded during the takeoff run before speed reaches TKOFF_ROTATE_SPD. For taildraggers set to 3-point ground pitch angle and use TKOFF_TDRAG_ELEV to prevent nose tipover. For nose-wheel steer aircraft set to the ground pitch angle and if a reduction in nose-wheel load is required as speed rises, use a positive offset in TKOFF_GND_PITCH of up to 5 degrees above the angle on ground, starting at the mesured pitch angle and incrementing in 1 degree steps whilst checking for premature rotation and takeoff with each increment. To increase nose-wheel load, use a negative TKOFF_TDRAG_ELEV and refer to notes on TKOFF_TDRAG_ELEV before making adjustments. @@ -68,61 +69,75 @@ bool ModeTakeoff::_enter() void ModeTakeoff::update() { + // don't setup waypoints if we dont have a valid position and home! + if (!(plane.current_loc.initialised() && AP::ahrs().home_is_set())) { + plane.calc_nav_roll(); + plane.calc_nav_pitch(); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); + return; + } + + const float alt = target_alt; + const float dist = target_dist; if (!takeoff_started) { + const uint16_t altitude = plane.relative_ground_altitude(false,true); + const float direction = degrees(ahrs.yaw); // see if we will skip takeoff as already flying if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && ahrs.groundspeed() > 3) { - gcs().send_text(MAV_SEVERITY_INFO, "Takeoff skipped - circling"); + if (altitude >= alt) { + gcs().send_text(MAV_SEVERITY_INFO, "Above TKOFF alt - loitering"); + plane.next_WP_loc = plane.current_loc; + takeoff_started = true; + plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); + } else { + gcs().send_text(MAV_SEVERITY_INFO, "Climbing to TKOFF alt then loitering"); + plane.next_WP_loc = plane.current_loc; + plane.next_WP_loc.alt += ((alt - altitude) *100); + plane.next_WP_loc.offset_bearing(direction, dist); + takeoff_started = true; + plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); + } + // not flying so do a full takeoff sequence + } else { + // setup target waypoint and alt for loiter at dist and alt from start + + start_loc = plane.current_loc; plane.prev_WP_loc = plane.current_loc; plane.next_WP_loc = plane.current_loc; - takeoff_started = true; - plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); - } - } + plane.next_WP_loc.alt += alt*100.0; + plane.next_WP_loc.offset_bearing(direction, dist); - if (!takeoff_started) { - // setup target location 1.5 times loiter radius from the - // takeoff point, at a height of TKOFF_ALT - const float dist = target_dist; - const float alt = target_alt; - const float direction = degrees(ahrs.yaw); + plane.crash_state.is_crashed = false; - start_loc = plane.current_loc; - plane.prev_WP_loc = plane.current_loc; - plane.next_WP_loc = plane.current_loc; - plane.next_WP_loc.alt += alt*100.0; - plane.next_WP_loc.offset_bearing(direction, dist); - - plane.crash_state.is_crashed = false; + plane.auto_state.takeoff_pitch_cd = level_pitch * 100; - plane.auto_state.takeoff_pitch_cd = level_pitch * 100; + plane.set_flight_stage(AP_FixedWing::FlightStage::TAKEOFF); - plane.set_flight_stage(AP_FixedWing::FlightStage::TAKEOFF); - - if (!plane.throttle_suppressed) { - gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm at %.1fm to %.1f deg", - alt, dist, direction); - takeoff_started = true; + if (!plane.throttle_suppressed) { + gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm for %.1fm heading %.1f deg", + alt, dist, direction); + takeoff_started = true; + } } } // we finish the initial level takeoff if we climb past // TKOFF_LVL_ALT or we pass the target location. The check for // target location prevents us flying forever if we can't climb + // reset the loiter waypoint target to be correct bearing and dist + // from starting location in case original yaw used to set it was off due to EKF + // reset or compass interference from max throttle if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF && (plane.current_loc.alt - start_loc.alt >= level_alt*100 || - start_loc.get_distance(plane.current_loc) >= target_dist)) { - // reached level alt, re-calculate bearing to cope with systems with no compass - // or with poor initial compass - float direction = start_loc.get_bearing_to(plane.current_loc) * 0.01; - float dist_done = start_loc.get_distance(plane.current_loc); - const float dist = target_dist; - - plane.next_WP_loc = plane.current_loc; - plane.next_WP_loc.offset_bearing(direction, MAX(dist-dist_done, 0)); - plane.next_WP_loc.alt = start_loc.alt + target_alt*100.0; + start_loc.get_distance(plane.current_loc) >= dist)) { + // reset the target loiter waypoint using current yaw which should be close to correct starting heading + const float direction = start_loc.get_bearing_to(plane.current_loc) * 0.01; + plane.next_WP_loc = start_loc; + plane.next_WP_loc.offset_bearing(direction, dist); + plane.next_WP_loc.alt += alt*100.0; plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); - + #if AP_FENCE_ENABLED plane.fence.auto_enable_fence_after_takeoff(); #endif @@ -136,6 +151,11 @@ void ModeTakeoff::update() plane.calc_nav_roll(); plane.calc_nav_pitch(); plane.calc_throttle(); + //check if in long failsafe, if it is recall long failsafe now to get fs action via events call + if (plane.long_failsafe_pending) { + plane.long_failsafe_pending = false; + plane.failsafe_long_on_event(FAILSAFE_LONG, ModeReason::MODE_TAKEOFF_FAILSAFE); + } } } diff --git a/ArduPlane/mode_training.cpp b/ArduPlane/mode_training.cpp index 9215fe0aba34d..23c81fbcc288f 100644 --- a/ArduPlane/mode_training.cpp +++ b/ArduPlane/mode_training.cpp @@ -63,5 +63,7 @@ void ModeTraining::run() } } - plane.stabilize_yaw(); + // Always manual rudder control + output_rudder_and_steering(plane.rudder_in_expo(false)); + } diff --git a/ArduPlane/motor_test.cpp b/ArduPlane/motor_test.cpp index cfc640cab9859..38229d2f35134 100644 --- a/ArduPlane/motor_test.cpp +++ b/ArduPlane/motor_test.cpp @@ -67,7 +67,7 @@ void QuadPlane::motor_test_output() // sanity check throttle values if (pwm >= RC_Channel::RC_MIN_LIMIT_PWM && pwm <= RC_Channel::RC_MAX_LIMIT_PWM) { - // turn on motor to specified pwm vlaue + // turn on motor to specified pwm value motors->output_test_seq(motor_test.seq, pwm); } else { motor_test_stop(); @@ -87,6 +87,14 @@ MAV_RESULT QuadPlane::mavlink_motor_test_start(mavlink_channel_t chan, uint8_t m gcs().send_text(MAV_SEVERITY_INFO, "Must be disarmed for motor test"); return MAV_RESULT_FAILED; } + + // Check Motor test is allowed + char failure_msg[50] {}; + if (!motors->motor_test_checks(ARRAY_SIZE(failure_msg), failure_msg)) { + gcs().send_text(MAV_SEVERITY_CRITICAL,"Motor Test: %s", failure_msg); + return MAV_RESULT_FAILED; + } + // if test has not started try to start it if (!motor_test.running) { // start test diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 16ceb26035158..76846b0fe6ed6 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -58,7 +58,7 @@ void Plane::loiter_angle_update(void) } } if (terrain_status_ok && - fabsf(altitude_agl - target_altitude.terrain_alt_cm*0.01) < 5) { + fabsF(altitude_agl - target_altitude.terrain_alt_cm*0.01) < 5) { reached_target_alt = true; } else #endif @@ -73,7 +73,7 @@ void Plane::loiter_angle_update(void) loiter.next_sum_lap_cd = loiter.sum_cd + lap_check_interval_cd; } else if (!loiter.reached_target_alt && labs(loiter.sum_cd) >= loiter.next_sum_lap_cd) { - // check every few laps for scenario where up/downdrafts inhibit you from loitering up/down for too long + // check every few laps for scenario where up/downward inhibit you from loitering up/down for too long loiter.unable_to_acheive_target_alt = labs(current_loc.alt - loiter.start_lap_alt_cm) < 500; loiter.start_lap_alt_cm = current_loc.alt; loiter.next_sum_lap_cd += lap_check_interval_cd; @@ -95,6 +95,8 @@ void Plane::navigate() return; } + check_home_alt_change(); + // waypoint distance from plane // ---------------------------- auto_state.wp_distance = current_loc.get_distance(next_WP_loc); @@ -157,9 +159,9 @@ void Plane::calc_airspeed_errors() // FBW_B/cruise airspeed target if (!failsafe.rc_failsafe && (control_mode == &mode_fbwb || control_mode == &mode_cruise)) { - if (g2.flight_options & FlightOptions::CRUISE_TRIM_AIRSPEED) { + if (flight_option_enabled(FlightOptions::CRUISE_TRIM_AIRSPEED)) { target_airspeed_cm = aparm.airspeed_cruise_cm; - } else if (g2.flight_options & FlightOptions::CRUISE_TRIM_THROTTLE) { + } else if (flight_option_enabled(FlightOptions::CRUISE_TRIM_THROTTLE)) { float control_min = 0.0f; float control_mid = 0.0f; const float control_max = channel_throttle->get_range(); @@ -186,7 +188,7 @@ void Plane::calc_airspeed_errors() get_throttle_input()) + ((int32_t)aparm.airspeed_min * 100); } #if OFFBOARD_GUIDED == ENABLED - } else if (control_mode == &mode_guided && guided_state.target_airspeed_cm > 0.0) { // if offbd guided speed change cmd not set, then this section is skipped + } else if (control_mode == &mode_guided && guided_state.target_airspeed_cm > 0.0) { // if offboard guided speed change cmd not set, then this section is skipped // offboard airspeed demanded uint32_t now = AP_HAL::millis(); float delta = 1e-3f * (now - guided_state.target_airspeed_time_ms); @@ -244,9 +246,10 @@ void Plane::calc_airspeed_errors() // but only when this is faster than the target airspeed commanded // above. if (control_mode->does_auto_throttle() && - aparm.min_gndspeed_cm > 0 && - control_mode != &mode_circle) { - int32_t min_gnd_target_airspeed = airspeed_measured*100 + groundspeed_undershoot; + groundspeed_undershoot_is_valid && + control_mode != &mode_circle) { + float EAS_undershoot = (int32_t)((float)groundspeed_undershoot / ahrs.get_EAS2TAS()); + int32_t min_gnd_target_airspeed = airspeed_measured*100 + EAS_undershoot; if (min_gnd_target_airspeed > target_airspeed_cm) { target_airspeed_cm = min_gnd_target_airspeed; } @@ -276,16 +279,18 @@ void Plane::calc_gndspeed_undershoot() { // Use the component of ground speed in the forward direction // This prevents flyaway if wind takes plane backwards - if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) { - Vector2f gndVel = ahrs.groundspeed_vector(); + Vector3f velNED; + if (ahrs.have_inertial_nav() && ahrs.get_velocity_NED(velNED)) { const Matrix3f &rotMat = ahrs.get_rotation_body_to_ned(); Vector2f yawVect = Vector2f(rotMat.a.x,rotMat.b.x); if (!yawVect.is_zero()) { yawVect.normalize(); - float gndSpdFwd = yawVect * gndVel; - groundspeed_undershoot = (aparm.min_gndspeed_cm > 0) ? (aparm.min_gndspeed_cm - gndSpdFwd*100) : 0; + float gndSpdFwd = yawVect * velNED.xy(); + groundspeed_undershoot_is_valid = aparm.min_gndspeed_cm > 0; + groundspeed_undershoot = groundspeed_undershoot_is_valid ? (aparm.min_gndspeed_cm - gndSpdFwd*100) : 0; } } else { + groundspeed_undershoot_is_valid = false; groundspeed_undershoot = 0; } } @@ -344,6 +349,9 @@ void Plane::update_loiter(uint16_t radius) } } + // the radius actually being used by the controller is required by other functions + loiter.radius = (float)radius; + update_loiter_update_nav(radius); if (loiter.start_time_ms == 0) { diff --git a/ArduPlane/qautotune.h b/ArduPlane/qautotune.h index 1a1bf6e873fdc..3ec890ac3dc3f 100644 --- a/ArduPlane/qautotune.h +++ b/ArduPlane/qautotune.h @@ -8,7 +8,7 @@ #include "quadplane.h" #ifndef QAUTOTUNE_ENABLED - #define QAUTOTUNE_ENABLED HAL_QUADPLANE_ENABLED && !HAL_MINIMIZE_FEATURES + #define QAUTOTUNE_ENABLED HAL_QUADPLANE_ENABLED #endif #if QAUTOTUNE_ENABLED diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index c6c9a08d1f0b1..48259c89a4462 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -33,7 +33,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @DisplayName: Transition time // @Description: Transition time in milliseconds after minimum airspeed is reached // @Units: ms - // @Range: 1 30000 + // @Range: 2000 30000 // @User: Advanced AP_GROUPINFO("TRANSITION_MS", 11, QuadPlane, transition_time_ms, 5000), @@ -167,7 +167,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Param: VFWD_GAIN // @DisplayName: Forward velocity hold gain - // @Description: Controls use of forward motor in vtol modes. If this is zero then the forward motor will not be used for position control in VTOL modes. A value of 0.05 is a good place to start if you want to use the forward motor for position control. No forward motor will be used in QSTABILIZE or QHOVER modes. Use QLOITER for position hold with the forward motor. + // @Description: The use of this parameter is no longer recommended and has been superseded by a method that works in all VTOL modes with the exception of QAUTOTUNE which is controlled by the Q_FWD_THR_USE parameter. This Q_VFD_GAIN parameter controls use of the forward motor in VTOL modes that use the velocity controller. Set to 0 to disable this function. A value of 0.05 is a good place to start if you want to use the forward motor for position control. No forward motor will be used in QSTABILIZE or QHOVER modes. Use with QLOITER for position hold with the forward motor. // @Range: 0 0.5 // @Increment: 0.01 // @User: Standard @@ -269,7 +269,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Bitmask: 8: Mtrs_Only_Qassist-in tailsitters only uses VTOL motors and not flying surfaces for QASSIST // @Bitmask: 10: Disarmed Yaw Tilt-enable motor tilt for yaw when disarmed // @Bitmask: 11: Delay Spoolup-delay VTOL spoolup for 2 seconds after arming - // @Bitmask: 12: Disable Qassist-based on synthetic airspeed even if airspeed sensor is used + // @Bitmask: 12: Disable speed based Qassist when using synthethic airspeed estimates // @Bitmask: 13: Disable Ground Effect Compensation-on baro altitude reports // @Bitmask: 14: Ignore forward flight angle limits-in Qmodes and use Q_ANGLE_MAX exclusively // @Bitmask: 15: ThrLandControl-enable throttle stick control of landing rate @@ -279,6 +279,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Bitmask: 19: CompleteTransition-to fixed wing if Q_TRANS_FAIL timer times out instead of QLAND // @Bitmask: 20: Force RTL mode-forces RTL mode on rc failsafe in VTOL modes overriding bit 5(USE_QRTL) // @Bitmask: 21: Tilt rotor-tilt motors up when disarmed in FW modes (except manual) to prevent ground strikes. + // @Bitmask: 22: Scale FF by the ratio of VTOL/plane angle P gains in VTOL modes rather than reducing VTOL angle P based on airspeed. AP_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0), AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2), @@ -399,7 +400,7 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @Param: ASSIST_ALT // @DisplayName: Quadplane assistance altitude - // @Description: This is the altitude below which quadplane assistance will be triggered. This acts the same way as Q_ASSIST_ANGLE and Q_ASSIST_SPEED, but triggers if the aircraft drops below the given altitude while the VTOL motors are not running. A value of zero disables this feature. The altutude is calculated as being above ground level. The height above ground is given from a Lidar used if available and RNGFND_LANDING=1. Otherwise it comes from terrain data if TERRAIN_FOLLOW=1 and comes from height above home otherwise. + // @Description: This is the altitude below which quadplane assistance will be triggered. This acts the same way as Q_ASSIST_ANGLE and Q_ASSIST_SPEED, but triggers if the aircraft drops below the given altitude while the VTOL motors are not running. A value of zero disables this feature. The altitude is calculated as being above ground level. The height above ground is given from a Lidar used if available and RNGFND_LANDING=1. Otherwise it comes from terrain data if TERRAIN_FOLLOW=1 and comes from height above home otherwise. // @Units: m // @Range: 0 120 // @Increment: 1 @@ -504,6 +505,30 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @User: Standard AP_GROUPINFO("RTL_ALT_MIN", 34, QuadPlane, qrtl_alt_min, 10), + // @Param: FWD_THR_GAIN + // @DisplayName: Q mode fwd throttle gain + // @Description: This parameter sets the gain from forward accel/tilt to forward throttle in certain Q modes. The Q modes this feature operates in is controlled by the Q_FWD_THR_USE parameter. Vehicles using separate forward thrust motors, eg quadplanes, should set this parameter to (all up weight) / (maximum combined thrust of forward motors) with a value of 2 being typical. Vehicles that tilt lifting rotors to provide forward thrust should set this parameter to (all up weight) / (weight lifted by tilting rotors) which for most aircraft can be approximated as (total number of lifting rotors) / (number of lifting rotors that tilt). When using this method of forward throttle control, the forward tilt angle limit is controlled by the Q_FWD_PIT_LIM parameter. + // @Range: 0.0 5.0 + // @Increment: 0.1 + // @User: Standard + AP_GROUPINFO("FWD_THR_GAIN", 35, QuadPlane, q_fwd_thr_gain, 2.0f), + + // @Param: FWD_PIT_LIM + // @DisplayName: Q mode forward pitch limit + // @Description: When forward throttle is being controlled by the Q_FWD_THR_GAIN parameter in Q modes, the vehicle forward (nose down) pitch rotation will be limited to the value specified by this parameter and the any additional forward acceleration required will be produced by use of the forward thrust motor(s) or tilting of moveable rotors. Larger values allow the vehicle to pitch more nose down. Set initially to the amount of nose down pitch required to remove wing lift. + // @Units: deg + // @Range: 0.0 5.0 + // @Increment: 0.1 + // @User: Standard + AP_GROUPINFO("FWD_PIT_LIM", 36, QuadPlane, q_fwd_pitch_lim, 3.0f), + + // @Param: FWD_THR_USE + // @DisplayName: Q mode forward throttle use + // @Description: This parameter determines when the feature that uses forward throttle instead of forward tilt is used. The amount of forward throttle is controlled by the Q_FWD_THR_GAIN parameter. The maximum amount of forward pitch allowed is controlled by the Q_FWD_PIT_LIM parameter. Q_FWD_THR_USE = 0 disables the feature. Q_FWD_THR_USE = 1 enables the feature in all position controlled modes such as QLOITER, QLAND, QRTL and VTOL TAKEOFF. Q_FWD_THR_USE = 2 enables the feature in all Q modes except QAUTOTUNE and QACRO. When enabling the feature, the legacy method of controlling forward throttle use via velocity controller error should be disabled by setting Q_VFWD_GAIN to 0. Do not use this feature with tailsitters. + // @Values: 0:Off,1:On in all position controlled Q modes,2:On in all Q modes except QAUTOTUNE and QACRO + // @User: Standard + AP_GROUPINFO("FWD_THR_USE", 37, QuadPlane, q_fwd_thr_use, uint8_t(FwdThrUse::OFF)), + AP_GROUPEND }; @@ -1194,7 +1219,7 @@ bool QuadPlane::is_flying_vtol(void) const } if (plane.control_mode->is_vtol_man_mode()) { // in manual flight modes only consider aircraft landed when pilot demanded throttle is zero - return is_positive(plane.get_throttle_input()); + return is_positive(get_throttle_input()); } if (in_vtol_mode() && millis() - landing_detect.lower_limit_start_ms > 5000) { // use landing detector @@ -1264,7 +1289,7 @@ float QuadPlane::get_pilot_input_yaw_rate_cds(void) const const auto rudder_in = plane.channel_rudder->get_control_in(); bool manual_air_mode = plane.control_mode->is_vtol_man_throttle() && air_mode_active(); if (!manual_air_mode && - !is_positive(plane.get_throttle_input()) && + !is_positive(get_throttle_input()) && (!plane.control_mode->does_auto_throttle() || motors->limit.throttle_lower) && plane.arming.get_rudder_arming_type() == AP_Arming::RudderArming::ARMDISARM && rudder_in < 0 && @@ -1341,7 +1366,7 @@ float QuadPlane::get_pilot_desired_climb_rate_cms(void) const */ void QuadPlane::init_throttle_wait(void) { - if (plane.get_throttle_input() >= 10 || + if (get_throttle_input() >= 10 || plane.is_flying()) { throttle_wait = false; } else { @@ -1450,7 +1475,7 @@ bool QuadPlane::should_assist(float aspeed, bool have_airspeed) // assistance due to Q_ASSIST_SPEED // if option bit is enabled only allow assist with real airspeed sensor if ((have_airspeed && aspeed < assist_speed) && - (!option_is_set(QuadPlane::OPTION::DISABLE_SYNTHETIC_AIRSPEED_ASSIST) || ahrs.airspeed_sensor_enabled())) { + (!option_is_set(QuadPlane::OPTION::DISABLE_SYNTHETIC_AIRSPEED_ASSIST) || ahrs.using_airspeed_sensor())) { in_angle_assist = false; angle_error_start_ms = 0; return true; @@ -1546,7 +1571,8 @@ void SLT_Transition::update() quadplane.assisted_flight = true; // update transition state for vehicles using airspeed wait if (!in_forced_transition) { - if (transition_state != TRANSITION_AIRSPEED_WAIT) { + const bool show_message = transition_state != TRANSITION_AIRSPEED_WAIT || transition_start_ms == 0; + if (show_message) { gcs().send_text(MAV_SEVERITY_INFO, "Transition started airspeed %.1f", (double)aspeed); } transition_state = TRANSITION_AIRSPEED_WAIT; @@ -1673,14 +1699,15 @@ void SLT_Transition::update() // after airspeed is reached we degrade throttle over the // transition time, but continue to stabilize const uint32_t transition_timer_ms = now - transition_low_airspeed_ms; - if (transition_timer_ms > (unsigned)quadplane.transition_time_ms) { + const float trans_time_ms = constrain_float(quadplane.transition_time_ms,2000,30000); + if (transition_timer_ms > unsigned(trans_time_ms)) { transition_state = TRANSITION_DONE; in_forced_transition = false; transition_start_ms = 0; transition_low_airspeed_ms = 0; gcs().send_text(MAV_SEVERITY_INFO, "Transition done"); } - float trans_time_ms = MAX((float)quadplane.transition_time_ms.get(),1); + float transition_scale = (trans_time_ms - transition_timer_ms) / trans_time_ms; float throttle_scaled = last_throttle * transition_scale; @@ -1893,7 +1920,7 @@ void QuadPlane::update_throttle_suppression(void) consider non-zero throttle to mean that pilot is commanding takeoff unless in a manual thottle mode */ - if (!is_zero(plane.get_throttle_input()) && + if (!is_zero(get_throttle_input()) && (rc().arming_check_throttle() || plane.control_mode->is_vtol_man_throttle() || plane.channel_throttle->norm_input_dz() > 0)) { @@ -2067,6 +2094,9 @@ bool QuadPlane::handle_do_vtol_transition(enum MAV_VTOL_STATE state) const gcs().send_text(MAV_SEVERITY_NOTICE, "Entered VTOL mode"); } plane.auto_state.vtol_mode = true; + // This is a precaution. It should be looked after by the call to QuadPlane::mode_enter(void) on mode entry. + plane.quadplane.q_fwd_throttle = 0.0f; + plane.quadplane.q_fwd_pitch_lim_cd = 100.0f * plane.quadplane.q_fwd_pitch_lim; return true; case MAV_VTOL_STATE_FW: @@ -2222,6 +2252,10 @@ void QuadPlane::run_xy_controller(float accel_limit) pos_control->init_xy_controller(); } pos_control->set_lean_angle_max_cd(MIN(4500, MAX(accel_to_angle(accel_limit)*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_xy(); + } pos_control->update_xy_controller(); } @@ -2366,7 +2400,7 @@ void QuadPlane::vtol_position_controller(void) case QPOS_APPROACH: if (in_vtol_mode()) { - // this means we're not running update_transition() and + // this means we're not running transition update code and // thus not doing qassist checking, force POSITION1 mode // now. We don't expect this to trigger, it is a failsafe // for a logic error @@ -2406,7 +2440,7 @@ void QuadPlane::vtol_position_controller(void) const float aspeed_threshold = MAX(plane.aparm.airspeed_min-2, assist_speed); // run fixed wing navigation - plane.nav_controller->update_waypoint(plane.current_loc, loc); + plane.nav_controller->update_waypoint(plane.auto_state.crosstrack ? plane.prev_WP_loc : plane.current_loc, loc); // use TECS for throttle SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.TECS_controller.get_throttle_demand()); @@ -2473,10 +2507,11 @@ void QuadPlane::vtol_position_controller(void) const uint32_t min_airbrake_ms = 1000; if (poscontrol.get_state() == QPOS_AIRBRAKE && poscontrol.time_since_state_start_ms() > min_airbrake_ms && - (aspeed < aspeed_threshold || - fabsf(degrees(closing_vel.angle(desired_closing_vel))) > 60 || - closing_speed > MAX(desired_closing_speed*1.2, desired_closing_speed+2) || - labs(plane.ahrs.roll_sensor - plane.nav_roll_cd) > attitude_error_threshold_cd || + (aspeed < aspeed_threshold || // too low airspeed + fabsf(degrees(closing_vel.angle(desired_closing_vel))) > 60 || // wrong direction + closing_speed > MAX(desired_closing_speed*1.2, desired_closing_speed+2) || // too fast + closing_speed < desired_closing_speed*0.5 || // too slow ground speed + labs(plane.ahrs.roll_sensor - plane.nav_roll_cd) > attitude_error_threshold_cd || // bad attitude labs(plane.ahrs.pitch_sensor - plane.nav_pitch_cd) > attitude_error_threshold_cd)) { gcs().send_text(MAV_SEVERITY_INFO,"VTOL position1 v=%.1f d=%.1f h=%.1f dc=%.1f", (double)groundspeed, @@ -2488,6 +2523,18 @@ void QuadPlane::vtol_position_controller(void) // switch to vfwd for throttle control vel_forward.integrator = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); + + // adjust the initial forward throttle based on our desired and actual closing speed + // this allows for significant initial forward throttle + // when we have a strong headwind, but low throttle in the usual case where + // we want to slow down ready for POSITION2 + vel_forward.integrator = linear_interpolate(0, vel_forward.integrator, + closing_speed, + 1.2*desired_closing_speed, 0.5*desired_closing_speed); + + // limit our initial forward throttle in POSITION1 to be 0.5 of cruise throttle + vel_forward.integrator = constrain_float(vel_forward.integrator, 0, plane.aparm.throttle_cruise*0.5); + vel_forward.last_ms = now_ms; } @@ -2524,7 +2571,6 @@ void QuadPlane::vtol_position_controller(void) } } - if (poscontrol.get_state() == QPOS_APPROACH) { poscontrol_init_approach(); } @@ -2661,6 +2707,8 @@ void QuadPlane::vtol_position_controller(void) plane.nav_roll_cd = pos_control->get_roll_cd(); plane.nav_pitch_cd = pos_control->get_pitch_cd(); + 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_xy(); } @@ -2714,6 +2762,8 @@ void QuadPlane::vtol_position_controller(void) plane.nav_roll_cd = pos_control->get_roll_cd(); plane.nav_pitch_cd = pos_control->get_pitch_cd(); + 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_xy(); } @@ -2758,6 +2808,8 @@ void QuadPlane::vtol_position_controller(void) plane.nav_roll_cd = pos_control->get_roll_cd(); plane.nav_pitch_cd = pos_control->get_pitch_cd(); + assign_tilt_to_fwd_thr(); + // call attitude controller set_pilot_yaw_rate_time_constant(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, @@ -2807,7 +2859,7 @@ void QuadPlane::vtol_position_controller(void) if (plane.control_mode == &plane.mode_guided || vtol_loiter_auto) { plane.ahrs.get_location(plane.current_loc); int32_t target_altitude_cm; - if (!plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME,target_altitude_cm)) { + if (!plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN,target_altitude_cm)) { break; } if (poscontrol.slow_descent && @@ -2815,7 +2867,7 @@ void QuadPlane::vtol_position_controller(void) // gradually descend as we approach target plane.auto_state.wp_proportion = plane.current_loc.line_path_proportion(plane.prev_WP_loc, plane.next_WP_loc); int32_t prev_alt; - if (plane.prev_WP_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME,prev_alt)) { + if (plane.prev_WP_loc.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN,prev_alt)) { target_altitude_cm = linear_interpolate(prev_alt, target_altitude_cm, plane.auto_state.wp_proportion, @@ -2881,6 +2933,111 @@ void QuadPlane::vtol_position_controller(void) } } +/* + determine which fwd throttle handling method is active + */ +QuadPlane::ActiveFwdThr QuadPlane::get_vfwd_method(void) const +{ + const bool have_fwd_thr_gain = is_positive(q_fwd_thr_gain); + const bool have_vfwd_gain = is_positive(vel_forward.gain); + +#if AP_ICENGINE_ENABLED + const auto ice_state = plane.g2.ice_control.get_state(); + if (ice_state != AP_ICEngine::ICE_DISABLED && ice_state != AP_ICEngine::ICE_RUNNING) { + // we need the engine running for fwd throttle + return ActiveFwdThr::NONE; + } +#endif + +#if QAUTOTUNE_ENABLED + if (plane.control_mode == &plane.mode_qautotune) { + return ActiveFwdThr::NONE; + } +#endif + + if (have_fwd_thr_gain) { + if (vfwd_enable_active) { + // user has used AUX function to activate new method + return ActiveFwdThr::NEW; + } + if (q_fwd_thr_use == FwdThrUse::ALL) { + return ActiveFwdThr::NEW; + } + if (q_fwd_thr_use == FwdThrUse::POSCTRL && pos_control->is_active_xy()) { + return ActiveFwdThr::NEW; + } + } + if (have_vfwd_gain && pos_control->is_active_xy()) { + return ActiveFwdThr::OLD; + } + return ActiveFwdThr::NONE; +} + +/* + map from pitch tilt to fwd throttle when enabled + */ +void QuadPlane::assign_tilt_to_fwd_thr(void) { + + const auto fwd_thr_active = get_vfwd_method(); + if (fwd_thr_active != ActiveFwdThr::NEW) { + q_fwd_throttle = 0.0f; + q_fwd_pitch_lim_cd = 100.0f * q_fwd_pitch_lim; + return; + } + // Handle the case where we are limiting the forward pitch angle to prevent negative wing lift + // and are using the forward thrust motor or tilting rotors to provide the forward acceleration + float fwd_tilt_rad = radians(constrain_float(-0.01f * (float)plane.nav_pitch_cd, 0.0f, 45.0f)); + q_fwd_throttle = MIN(q_fwd_thr_gain * tanf(fwd_tilt_rad), 1.0f); + + // Relax forward tilt limit if the position controller is saturating in the forward direction because + // the forward thrust motor could be failed + const float fwd_tilt_range_cd = (float)aparm.angle_max - 100.0f * q_fwd_pitch_lim; + 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_xy() 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); + // Don't let the forward pitch limit be more than the forward pitch demand before limiting to + // avoid opening up the limit more than necessary + q_fwd_pitch_lim_cd = MIN(q_fwd_pitch_lim_cd, MAX(-(float)plane.nav_pitch_cd, 100.0f * q_fwd_pitch_lim)); + } else { + // take the lesser of the two limits + q_fwd_pitch_lim_cd = (float)aparm.angle_max; + } + + float fwd_thr_scaler; + if (!in_vtol_land_approach()) { + // To prevent forward motor prop strike, reduce throttle to zero when close to ground. + float alt_cutoff = MAX(0,vel_forward_alt_cutoff); + float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing); + fwd_thr_scaler = linear_interpolate(0.0f, 1.0f, height_above_ground, alt_cutoff, alt_cutoff+2); + } else { + // When we are doing horizontal positioning in a VTOL land we always allow the fwd motor + // to run. Otherwise a bad height above landing point estimate could cause the aircraft + // not to be able to approach the landing point + fwd_thr_scaler = 1.0f; + } + q_fwd_throttle *= fwd_thr_scaler; + + // When reducing forward throttle use, relax lower pitch limit to maintain forward + // acceleration capability. + const float nav_pitch_lower_limit_cd = - (int32_t)((float)aparm.angle_max * (1.0f - fwd_thr_scaler) + q_fwd_pitch_lim_cd * fwd_thr_scaler); + + // Diagnostics logging - remove when feature is fully flight tested. + AP::logger().WriteStreaming("FWDT", + "TimeUS,fts,qfplcd,npllcd,npcd,qft", // labels + "Qfffff", // fmt + AP_HAL::micros64(), + (double)fwd_thr_scaler, + (double)q_fwd_pitch_lim_cd, + (double)nav_pitch_lower_limit_cd, + (double)plane.nav_pitch_cd, + (double)q_fwd_throttle); + + plane.nav_pitch_cd = MAX(plane.nav_pitch_cd, (int32_t)nav_pitch_lower_limit_cd); +} /* we want to limit WP speed to a lower speed when more than 20 degrees @@ -2932,6 +3089,10 @@ void QuadPlane::setup_target_position(void) */ void QuadPlane::takeoff_controller(void) { + // reset fixed wing controller to neutral as base output + plane.nav_roll_cd = 0; + plane.nav_pitch_cd = 0; + if (!plane.arming.is_armed_and_safety_off()) { return; } @@ -2949,15 +3110,15 @@ void QuadPlane::takeoff_controller(void) // don't takeoff up until rudder is re-centered after rudder arming if (plane.arming.last_arm_method() == AP_Arming::Method::RUDDER && (takeoff_last_run_ms == 0 || - now - takeoff_last_run_ms < 1000) && + now - takeoff_last_run_ms > 1000) && !plane.seen_neutral_rudder && spool_state <= AP_Motors::DesiredSpoolState::GROUND_IDLE) { // start motor spinning if not spinning already so user sees it is armed set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); takeoff_start_time_ms = now; - if (now - rudder_takeoff_warn_ms > 3000) { - gcs().send_text(MAV_SEVERITY_WARNING, "takeoff wait rudder release"); - rudder_takeoff_warn_ms = now; + if (now - plane.takeoff_state.rudder_takeoff_warn_ms > TAKEOFF_RUDDER_WARNING_TIMEOUT) { + gcs().send_text(MAV_SEVERITY_WARNING, "Takeoff waiting for rudder release"); + plane.takeoff_state.rudder_takeoff_warn_ms = now; } return; } @@ -2993,8 +3154,6 @@ void QuadPlane::takeoff_controller(void) takeoff_last_run_ms = now; if (no_navigation) { - plane.nav_roll_cd = 0; - plane.nav_pitch_cd = 0; pos_control->relax_velocity_controller_xy(); } else { pos_control->set_accel_desired_xy_cmss(zero); @@ -3004,6 +3163,8 @@ void QuadPlane::takeoff_controller(void) // nav roll and pitch are controller by position controller plane.nav_roll_cd = pos_control->get_roll_cd(); plane.nav_pitch_cd = pos_control->get_pitch_cd(); + + assign_tilt_to_fwd_thr(); } run_xy_controller(); @@ -3044,8 +3205,7 @@ void QuadPlane::waypoint_controller(void) const Location &loc = plane.next_WP_loc; const uint32_t now = AP_HAL::millis(); - if (!loc.same_latlon_as(last_auto_target) || - plane.next_WP_loc.alt != last_auto_target.alt || + if (!loc.same_loc_as(last_auto_target) || now - last_loiter_ms > 500) { wp_nav->set_wp_destination(poscontrol.target_cm.tofloat()); last_auto_target = loc; @@ -3062,6 +3222,8 @@ void QuadPlane::waypoint_controller(void) plane.nav_roll_cd = wp_nav->get_roll(); plane.nav_pitch_cd = wp_nav->get_pitch(); + 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_xy(); } @@ -3227,7 +3389,8 @@ bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd) plane.crash_state.is_crashed = false; // also update nav_controller for status output - plane.nav_controller->update_waypoint(plane.current_loc, plane.next_WP_loc); + plane.nav_controller->update_waypoint(plane.auto_state.crosstrack ? plane.prev_WP_loc : plane.current_loc, + plane.next_WP_loc); poscontrol_init_approach(); return true; @@ -3447,7 +3610,7 @@ bool QuadPlane::verify_vtol_land(void) #if AP_ICENGINE_ENABLED // cut IC engine if enabled if (land_icengine_cut != 0) { - plane.g2.ice_control.engine_control(0, 0, 0); + plane.g2.ice_control.engine_control(0, 0, 0, false); } #endif // AP_ICENGINE_ENABLED gcs().send_text(MAV_SEVERITY_INFO,"Land final started"); @@ -3519,6 +3682,11 @@ void QuadPlane::Log_Write_QControl_Tuning() */ float QuadPlane::forward_throttle_pct() { + // handle special case where forward thrust motor is used instead of forward pitch. + if (get_vfwd_method() == ActiveFwdThr::NEW) { + return 100.0f * q_fwd_throttle; + } + /* Unless an RC channel is assigned for manual forward throttle control, we don't use forward throttle in QHOVER or QSTABILIZE as they are the primary @@ -3542,15 +3710,9 @@ float QuadPlane::forward_throttle_pct() } /* - in qautotune mode or modes without a velocity controller + see if the controller should be active */ - bool use_forward_gain = (vel_forward.gain > 0); -#if QAUTOTUNE_ENABLED - if (plane.control_mode == &plane.mode_qautotune) { - use_forward_gain = false; - } -#endif - if (!use_forward_gain) { + if (get_vfwd_method() != ActiveFwdThr::OLD) { return 0; } @@ -3899,7 +4061,7 @@ void QuadPlane::update_throttle_mix(void) if (plane.control_mode->is_vtol_man_throttle()) { // manual throttle - if (!is_positive(plane.get_throttle_input()) && !air_mode_active()) { + if (!is_positive(get_throttle_input()) && !air_mode_active()) { attitude_control->set_throttle_mix_min(); } else { attitude_control->set_throttle_mix_man(); @@ -4098,6 +4260,10 @@ Vector2f QuadPlane::landing_desired_closing_velocity() float tecs_land_airspeed = plane.TECS_controller.get_land_airspeed(); if (is_positive(tecs_land_airspeed)) { land_speed = tecs_land_airspeed; + } else { + // use half way between min airspeed and cruise if + // TECS_LAND_AIRSPEED not set + land_speed = 0.5*(land_speed+plane.aparm.airspeed_min); } target_speed = MIN(target_speed, eas2tas * land_speed); @@ -4139,12 +4305,16 @@ float QuadPlane::get_land_airspeed(void) return approach_speed; } + if (qstate == QPOS_AIRBRAKE) { + // during airbraking ask TECS to slow us to stall speed + return plane.aparm.airspeed_min; + } + // calculate speed based on landing desired velocity Vector2f vel = landing_desired_closing_velocity(); - const Vector3f wind = plane.ahrs.wind_estimate(); + const Vector2f wind = plane.ahrs.wind_estimate().xy(); const float eas2tas = plane.ahrs.get_EAS2TAS(); - vel.x -= wind.x; - vel.y -= wind.y; + vel -= wind; vel /= eas2tas; return vel.length(); } @@ -4350,7 +4520,7 @@ MAV_VTOL_STATE SLT_Transition::get_mav_vtol_state() const } // Set FW roll and pitch limits and keep TECS informed -void SLT_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd, bool& allow_stick_mixing) +void SLT_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd) { if (quadplane.in_vtol_mode() || quadplane.in_vtol_airbrake()) { // not in FW flight @@ -4412,6 +4582,9 @@ void QuadPlane::mode_enter(void) // state for special behaviour guided_wait_takeoff_on_mode_enter = guided_wait_takeoff; guided_wait_takeoff = false; + + q_fwd_throttle = 0.0f; + q_fwd_pitch_lim_cd = 100.0f * q_fwd_pitch_lim; } // Set attitude control yaw rate time constant to pilot input command model value @@ -4529,4 +4702,45 @@ bool QuadPlane::abort_landing(void) return true; } +// Should we allow stick mixing from the pilot +bool QuadPlane::allow_stick_mixing() const +{ + if (!available()) { + // Quadplane not enabled + return true; + } + // Ask transition logic + return transition->allow_stick_mixing(); +} + +/* + return true if we should disable TECS in the current flight state + this ensures that TECS resets when we change height in a VTOL mode + */ +bool QuadPlane::should_disable_TECS() const +{ + if (in_vtol_land_descent()) { + return true; + } + if (plane.control_mode == &plane.mode_guided && + plane.auto_state.vtol_loiter) { + return true; + } + return false; +} + +// Get pilot throttle input with deadzone, this will return 50% throttle in failsafe! +// This is a re-implmentation of Plane::get_throttle_input +// Ignoring the no_deadzone case means we don't need to check for valid RC +// This is handled by Plane::control_failsafe setting of control in +float QuadPlane::get_throttle_input() const +{ + float ret = plane.channel_throttle->get_control_in(); + if (plane.reversed_throttle) { + // RC option for reverse throttle has been set + ret = -ret; + } + return ret; +} + #endif // HAL_QUADPLANE_ENABLED diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index d87709562c3dc..ad5e6251af2df 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -47,6 +47,7 @@ class QuadPlane friend class Tailsitter_Transition; friend class Mode; + friend class ModeManual; friend class ModeAuto; friend class ModeRTL; friend class ModeAvoidADSB; @@ -180,6 +181,18 @@ class QuadPlane */ bool in_vtol_land_descent(void) const; + // Should we allow stick mixing from the pilot + bool allow_stick_mixing() const; + + /* + should we disable the TECS controller? + only called when in an auto-throttle mode + */ + bool should_disable_TECS() const; + + // Get pilot throttle input with deadzone, this will return 50% throttle in failsafe! + float get_throttle_input() const; + private: AP_AHRS &ahrs; @@ -223,9 +236,6 @@ class QuadPlane // check for quadplane assistance needed bool should_assist(float aspeed, bool have_airspeed); - // update transition handling - void update_transition(void); - // check for an EKF yaw reset void check_yaw_reset(void); @@ -388,6 +398,32 @@ class QuadPlane AP_Float acro_pitch_rate; AP_Float acro_yaw_rate; + // gain from forward acceleration to forward throttle + AP_Float q_fwd_thr_gain; + + // limit applied to forward pitch to prevent wing producing negative lift + AP_Float q_fwd_pitch_lim; + + // which fwd throttle handling method is active + enum class ActiveFwdThr : uint8_t { + NONE = 0, + OLD = 1, + NEW = 2, + }; + // override with AUX function + bool vfwd_enable_active; + + // specifies when the feature controlled by q_fwd_thr_gain and q_fwd_pitch_lim is used + enum class FwdThrUse : uint8_t { + OFF = 0, + POSCTRL = 1, + ALL = 2, + }; + AP_Enum q_fwd_thr_use; + + // return which vfwd method to use + ActiveFwdThr get_vfwd_method(void) const; + // time we last got an EKF yaw reset uint32_t ekfYawReset_ms; @@ -404,6 +440,9 @@ class QuadPlane Location last_auto_target; + float q_fwd_throttle; // forward throttle used in q modes + float q_fwd_pitch_lim_cd; // forward pitch limit applied when using q_fwd_throttle + // when did we last run the attitude controller? uint32_t last_att_control_ms; @@ -574,6 +613,7 @@ class QuadPlane TRANS_FAIL_TO_FW=(1<<19), FS_RTL=(1<<20), DISARMED_TILT_UP=(1<<21), + SCALE_FF_ANGLE_P=(1<<22), }; bool option_is_set(OPTION option) const { return (options.get() & int32_t(option)) != 0; @@ -583,7 +623,6 @@ class QuadPlane AP_Float maximum_takeoff_airspeed; uint32_t takeoff_start_time_ms; uint32_t takeoff_time_limit_ms; - uint32_t rudder_takeoff_warn_ms; float last_land_final_agl; @@ -676,6 +715,11 @@ class QuadPlane */ void set_desired_spool_state(AP_Motors::DesiredSpoolState state); + /* + limit forward pitch demand if using rotor tilt or forward flight motor to provide forward acceleration. + */ + void assign_tilt_to_fwd_thr(void); + /* get a scaled Q_WP_SPEED based on direction of movement */ diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 02fa2f79e8ba4..68ee2cb9986f4 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -139,6 +139,7 @@ void Plane::rudder_arm_disarm_check() arming.arm(AP_Arming::Method::RUDDER); rudder_arm_timer = 0; seen_neutral_rudder = false; + takeoff_state.rudder_takeoff_warn_ms = now; } } else { // not at full right rudder @@ -195,7 +196,7 @@ void Plane::read_radio() && channel_throttle->get_control_in() > 50 && stickmixing) { float nudge = (channel_throttle->get_control_in() - 50) * 0.02f; - if (ahrs.airspeed_sensor_enabled()) { + if (ahrs.using_airspeed_sensor()) { airspeed_nudge_cm = (aparm.airspeed_max * 100 - aparm.airspeed_cruise_cm) * nudge; } else { throttle_nudge = (aparm.throttle_max - aparm.throttle_cruise) * nudge; @@ -209,8 +210,10 @@ void Plane::read_radio() quadplane.tailsitter.check_input(); #endif +#if AP_TUNING_ENABLED // check for transmitter tuning changes tuning.check_input(control_mode->mode_number()); +#endif } int16_t Plane::rudder_input(void) @@ -221,7 +224,7 @@ int16_t Plane::rudder_input(void) return 0; } - if ((g2.flight_options & FlightOptions::DIRECT_RUDDER_ONLY) && + if ((flight_option_enabled(FlightOptions::DIRECT_RUDDER_ONLY)) && !(control_mode == &mode_manual || control_mode == &mode_stabilize || control_mode == &mode_acro)) { // the user does not want any input except in these modes return 0; @@ -277,7 +280,10 @@ void Plane::control_failsafe() } } - if (ThrFailsafe(g.throttle_fs_enabled.get()) != ThrFailsafe::Enabled) { + const bool allow_failsafe_bypass = !arming.is_armed() && !is_flying() && (rc().enabled_protocols() != 0); + const bool has_had_input = rc().has_had_rc_receiver() || rc().has_had_rc_override(); + if ((ThrFailsafe(g.throttle_fs_enabled.get()) != ThrFailsafe::Enabled) || (allow_failsafe_bypass && !has_had_input)) { + // If not flying and disarmed don't trigger failsafe until RC has been received for the fist time return; } @@ -435,8 +441,8 @@ bool Plane::throttle_at_zero(void) const /* true if throttle stick is at idle position...if throttle trim has been moved to center stick area in conjunction with sprung throttle, cannot use in_trim, must use rc_min */ - if (((!(g2.flight_options & FlightOptions::CENTER_THROTTLE_TRIM) && channel_throttle->in_trim_dz()) || - (g2.flight_options & FlightOptions::CENTER_THROTTLE_TRIM && channel_throttle->in_min_dz()))) { + if (((!(flight_option_enabled(FlightOptions::CENTER_THROTTLE_TRIM) && channel_throttle->in_trim_dz())) || + (flight_option_enabled(FlightOptions::CENTER_THROTTLE_TRIM)&& channel_throttle->in_min_dz()))) { return true; } return false; diff --git a/ArduPlane/reverse_thrust.cpp b/ArduPlane/reverse_thrust.cpp index 04033d00f665a..16dc514b54764 100644 --- a/ArduPlane/reverse_thrust.cpp +++ b/ArduPlane/reverse_thrust.cpp @@ -125,6 +125,10 @@ bool Plane::have_reverse_thrust(void) const */ float Plane::get_throttle_input(bool no_deadzone) const { + if (!rc().has_valid_input()) { + // Return 0 if there is no valid input + return 0.0; + } float ret; if (no_deadzone) { ret = channel_throttle->get_control_in_zero_dz(); @@ -143,8 +147,12 @@ float Plane::get_throttle_input(bool no_deadzone) const */ float Plane::get_adjusted_throttle_input(bool no_deadzone) const { + if (!rc().has_valid_input()) { + // Return 0 if there is no valid input + return 0.0; + } if ((plane.channel_throttle->get_type() != RC_Channel::ControlType::RANGE) || - (g2.flight_options & FlightOptions::CENTER_THROTTLE_TRIM) == 0) { + (flight_option_enabled(FlightOptions::CENTER_THROTTLE_TRIM)) == 0) { return get_throttle_input(no_deadzone); } float ret = channel_throttle->get_range() * throttle_curve(aparm.throttle_cruise * 0.01, 0, 0.5 + 0.5*channel_throttle->norm_input()); diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 11f50fabd8494..6048aed8042a0 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -134,7 +134,7 @@ bool Plane::suppress_throttle(void) // require 5m/s. This prevents throttle up due to spiky GPS // groundspeed with bad GPS reception #if AP_AIRSPEED_ENABLED - if ((!ahrs.airspeed_sensor_enabled()) || airspeed.get_airspeed() >= 5) { + if ((!ahrs.using_airspeed_sensor()) || airspeed.get_airspeed() >= 5) { // we're moving at more than 5 m/s throttle_suppressed = false; return false; @@ -371,38 +371,18 @@ void Plane::set_servos_idle(void) SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, servo_value); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, servo_value); SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, servo_value); - SRV_Channels::set_output_to_trim(SRV_Channel::k_throttle); - - SRV_Channels::output_ch_all(); -} -/* - pass through channels in manual mode - */ -void Plane::set_servos_manual_passthrough(void) -{ - SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, roll_in_expo(false)); - SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch_in_expo(false)); - SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder_in_expo(false)); - float throttle = get_throttle_input(true); - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 0.0); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0.0); -#if HAL_QUADPLANE_ENABLED - if (quadplane.available() && quadplane.option_is_set(QuadPlane::OPTION::IDLE_GOV_MANUAL)) { - // for quadplanes it can be useful to run the idle governor in MANUAL mode - // as it prevents the VTOL motors from running - int8_t min_throttle = aparm.throttle_min.get(); + SRV_Channels::set_output_to_trim(SRV_Channel::k_throttle); + SRV_Channels::set_output_to_trim(SRV_Channel::k_throttleLeft); + SRV_Channels::set_output_to_trim(SRV_Channel::k_throttleRight); - // apply idle governor -#if AP_ICENGINE_ENABLED - g2.ice_control.update_idle_governor(min_throttle); -#endif - throttle = MAX(throttle, min_throttle); - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle); - } -#endif } + /* Scale the throttle to conpensate for battery voltage drop */ @@ -442,6 +422,7 @@ void Plane::throttle_voltage_comp(int8_t &min_throttle, int8_t &max_throttle) co /* calculate any throttle limits based on the watt limiter */ +#if AP_BATTERY_WATT_MAX_ENABLED void Plane::throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle) { uint32_t now = millis(); @@ -486,6 +467,7 @@ void Plane::throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle) min_throttle = constrain_int16(min_throttle, min_throttle + throttle_watt_limit_min, 0); } } +#endif // #if AP_BATTERY_WATT_MAX_ENABLED /* setup output channels all non-manual modes @@ -511,55 +493,54 @@ void Plane::set_servos_controlled(void) min_throttle = 0; } - bool flight_stage_determines_max_throttle = false; - if (flight_stage == AP_FixedWing::FlightStage::TAKEOFF || - flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING - ) { - flight_stage_determines_max_throttle = true; - } + const bool use_takeoff_throttle_max = #if HAL_QUADPLANE_ENABLED - if (quadplane.in_transition()) { - flight_stage_determines_max_throttle = true; - } + quadplane.in_transition() || #endif - if (flight_stage_determines_max_throttle) { + (flight_stage == AP_FixedWing::FlightStage::TAKEOFF) || + (flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING); + + if (use_takeoff_throttle_max) { if (aparm.takeoff_throttle_max != 0) { - max_throttle = aparm.takeoff_throttle_max; - } else { - max_throttle = aparm.throttle_max; + max_throttle = aparm.takeoff_throttle_max.get(); } } else if (landing.is_flaring()) { min_throttle = 0; } - // conpensate for battery voltage drop + // compensate for battery voltage drop throttle_voltage_comp(min_throttle, max_throttle); +#if AP_BATTERY_WATT_MAX_ENABLED // apply watt limiter throttle_watt_limiter(min_throttle, max_throttle); +#endif - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, - constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), min_throttle, max_throttle)); - if (!arming.is_armed_and_safety_off()) { + // Always set 0 scaled even if overriding to zero pwm. + // This ensures slew limits and other functions using the scaled value pick up in the correct place + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 0.0); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0.0); + if (arming.arming_required() == AP_Arming::Required::YES_ZERO_PWM) { SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::Limit::ZERO_PWM); SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::Limit::ZERO_PWM); SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::Limit::ZERO_PWM); - } else { - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); - SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 0.0); - SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0.0); } } else if (suppress_throttle()) { - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); // default - // throttle is suppressed (above) to zero in final flare in auto mode, but we allow instead thr_min if user prefers, eg turbines: - if (landing.is_flaring() && landing.use_thr_min_during_flare() ) { - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, aparm.throttle_min.get()); - } if (g.throttle_suppress_manual) { // manual pass through of throttle while throttle is suppressed SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true)); + + } else if (landing.is_flaring() && landing.use_thr_min_during_flare() ) { + // throttle is suppressed (above) to zero in final flare in auto mode, but we allow instead thr_min if user prefers, eg turbines: + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, aparm.throttle_min.get()); + + } else { + // default + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); + } #if AP_SCRIPTING_ENABLED } else if (nav_scripting_active()) { @@ -571,9 +552,7 @@ void Plane::set_servos_controlled(void) control_mode == &mode_fbwa || control_mode == &mode_autotune) { // a manual throttle mode - if (!rc().has_valid_input()) { - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); - } else if (g.throttle_passthru_stabilize) { + if (g.throttle_passthru_stabilize) { // manual pass through of throttle while in FBWA or // STABILIZE mode with THR_PASS_STAB set SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true)); @@ -597,8 +576,20 @@ void Plane::set_servos_controlled(void) } SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, fwd_thr); #endif // HAL_QUADPLANE_ENABLED + + } else { + // Apply min/max limits to throttle output from flight mode + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, + constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), min_throttle, max_throttle)); } +} + +/* + Warn AHRS that we might take off soon + */ +void Plane::set_takeoff_expected(void) +{ // let EKF know to start GSF yaw estimator before takeoff movement starts so that yaw angle is better estimated const float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); if (!is_flying() && arming.is_armed()) { @@ -631,7 +622,7 @@ void Plane::set_servos_flaps(void) if (control_mode->does_auto_throttle()) { int16_t flapSpeedSource = 0; - if (ahrs.airspeed_sensor_enabled()) { + if (ahrs.using_airspeed_sensor()) { flapSpeedSource = target_airspeed_cm * 0.01f; } else { flapSpeedSource = aparm.throttle_cruise; @@ -838,42 +829,9 @@ void Plane::set_servos(void) quadplane.update(); #endif - if (control_mode == &mode_auto && auto_state.idle_mode) { - // special handling for balloon launch - set_servos_idle(); - servos_output(); - return; - } - - /* - see if we are doing ground steering. - */ - if (!steering_control.ground_steering) { - // we are not at an altitude for ground steering. Set the nose - // wheel to the rudder just in case the barometer has drifted - // a lot - steering_control.steering = steering_control.rudder; - } else if (!SRV_Channels::function_assigned(SRV_Channel::k_steering)) { - // we are within the ground steering altitude but don't have a - // dedicated steering channel. Set the rudder to the ground - // steering output - steering_control.rudder = steering_control.steering; - } - - // clear ground_steering to ensure manual control if the yaw stabilizer doesn't run - steering_control.ground_steering = false; - - if (control_mode == &mode_training) { - steering_control.rudder = rudder_in_expo(false); - } - - SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, steering_control.rudder); - SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering_control.steering); - - if (control_mode == &mode_manual) { - set_servos_manual_passthrough(); - } else { + if (control_mode != &mode_manual) { set_servos_controlled(); + set_takeoff_expected(); } // setup flap outputs @@ -968,6 +926,23 @@ void Plane::landing_neutral_control_surface_servos(void) } +/* + sets rudder/vtail , and elevon to indicator positions that we are in a rudder arming waiting for neutral stick state +*/ +void Plane::indicate_waiting_for_rud_neutral_to_takeoff(void) +{ + if (takeoff_state.waiting_for_rudder_neutral) { + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0); + channel_function_mixer(SRV_Channel::k_rudder, SRV_Channel::k_elevator, SRV_Channel::k_vtail_right, SRV_Channel::k_vtail_left); + if (!SRV_Channels::function_assigned(SRV_Channel::k_rudder) && !SRV_Channels::function_assigned(SRV_Channel::k_vtail_left)) { + // if no rudder indication possible, neutral elevons during wait because on takeoff stance they are usually both full up + SRV_Channels::set_output_scaled(SRV_Channel::k_elevon_right, 0); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevon_left, 0); + } + } +} + + /* run configured output mixer. This takes calculated servo_out values for each channel and calculates PWM values, then pushes them to @@ -998,6 +973,11 @@ void Plane::servos_output(void) // set control surface servos to neutral landing_neutral_control_surface_servos(); + + // set rudder arm waiting for neutral control throws (rudder neutral, aileron/rt vtail/rt elevon to full right) + if (flight_option_enabled(FlightOptions::INDICATE_WAITING_FOR_RUDDER_NEUTRAL)) { + indicate_waiting_for_rud_neutral_to_takeoff(); + } // support MANUAL_RCMASK if (g2.manual_rc_mask.get() != 0 && control_mode == &mode_manual) { diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 149aa273e3658..7c7066bef06b6 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -42,17 +42,15 @@ void Plane::init_ardupilot() #endif rc().init(); +#if AP_RELAY_ENABLED relay.init(); +#endif // initialise notify system notify.init(); notify_mode(*control_mode); init_rc_out_main(); - - // keep a record of how many resets have happened. This can be - // used to detect in-flight resets - g.num_resets.set_and_save(g.num_resets+1); // init baro barometer.init(); @@ -142,7 +140,7 @@ void Plane::init_ardupilot() // set the correct flight mode // --------------------------- - reset_control_switch(); + rc().reset_mode_switch(); // initialise sensor #if AP_OPTICALFLOW_ENABLED @@ -197,11 +195,6 @@ void Plane::startup_ground(void) // reset last heartbeat time, so we don't trigger failsafe on slow // startup gcs().sysid_myggcs_seen(AP_HAL::millis()); - - // we don't want writes to the serial port to cause us to pause - // mid-flight, so set the serial ports non-blocking once we are - // ready to fly - serial_manager.set_blocking_writes_all(false); } @@ -225,12 +218,46 @@ static bool mode_reason_is_landing_sequence(const ModeReason reason) } #endif // AP_FENCE_ENABLED +// Check if this mode can be entered from the GCS +bool Plane::gcs_mode_enabled(const Mode::Number mode_num) const +{ + // List of modes that can be blocked, index is bit number in parameter bitmask + static const uint8_t mode_list [] { + (uint8_t)Mode::Number::MANUAL, + (uint8_t)Mode::Number::CIRCLE, + (uint8_t)Mode::Number::STABILIZE, + (uint8_t)Mode::Number::TRAINING, + (uint8_t)Mode::Number::ACRO, + (uint8_t)Mode::Number::FLY_BY_WIRE_A, + (uint8_t)Mode::Number::FLY_BY_WIRE_B, + (uint8_t)Mode::Number::CRUISE, + (uint8_t)Mode::Number::AUTOTUNE, + (uint8_t)Mode::Number::AUTO, + (uint8_t)Mode::Number::LOITER, + (uint8_t)Mode::Number::TAKEOFF, + (uint8_t)Mode::Number::AVOID_ADSB, + (uint8_t)Mode::Number::GUIDED, + (uint8_t)Mode::Number::THERMAL, +#if HAL_QUADPLANE_ENABLED + (uint8_t)Mode::Number::QSTABILIZE, + (uint8_t)Mode::Number::QHOVER, + (uint8_t)Mode::Number::QLOITER, + (uint8_t)Mode::Number::QACRO, +#if QAUTOTUNE_ENABLED + (uint8_t)Mode::Number::QAUTOTUNE +#endif +#endif + }; + + return !block_GCS_mode_change((uint8_t)mode_num, mode_list, ARRAY_SIZE(mode_list)); +} + bool Plane::set_mode(Mode &new_mode, const ModeReason reason) { if (control_mode == &new_mode) { // don't switch modes if we are already in the correct mode. - // only make happy noise if using a difent method to switch, this stops beeping for repeated change mode requests from GCS + // only make happy noise if using a different method to switch, this stops beeping for repeated change mode requests from GCS if ((reason != control_mode_reason) && (reason != ModeReason::INITIALISED)) { AP_Notify::events.user_mode_change = 1; } @@ -274,6 +301,12 @@ bool Plane::set_mode(Mode &new_mode, const ModeReason reason) } #endif + // Check if GCS mode change is disabled via parameter + if ((reason == ModeReason::GCS_COMMAND) && !gcs_mode_enabled(new_mode.mode_number())) { + gcs().send_text(MAV_SEVERITY_NOTICE,"Mode change to %s denied, GCS entry disabled (FLTMODE_GCSBLOCK)", new_mode.name()); + return false; + } + // backup current control_mode and previous_mode Mode &old_previous_mode = *previous_mode; Mode &old_mode = *control_mode; diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 815aac3c65ed0..f40fa33d1f26e 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -27,7 +27,7 @@ const AP_Param::GroupInfo Tailsitter::var_info[] = { // @Param: ENABLE // @DisplayName: Enable Tailsitter // @Values: 0:Disable, 1:Enable, 2:Enable Always - // @Description: This enables Tailsitter functionality. A value of 2 forces Qassist active and always stabilize in forward flight with airmode for stabalisation at 0 throttle, for use on vehicles with no control surfaces, vehicle will not arm in forward flight modes, see also Q_OPTIONS "Mtrs_Only_Qassist" + // @Description: This enables Tailsitter functionality. A value of 2 forces Qassist active and always stabilize in forward flight with airmode for stabilisation at 0 throttle, for use on vehicles with no control surfaces, vehicle will not arm in forward flight modes, see also Q_OPTIONS "Mtrs_Only_Qassist" // @User: Standard // @RebootRequired: True AP_GROUPINFO_FLAGS("ENABLE", 1, Tailsitter, enable, 0, AP_PARAM_FLAG_ENABLE), @@ -161,7 +161,7 @@ const AP_Param::GroupInfo Tailsitter::var_info[] = { // @Param: MIN_VO // @DisplayName: Tailsitter Disk loading minimum outflow speed - // @Description: Use in conjunction with disk therory gain scaling and Q_TAILSIT_DSKLD to specify minumum airspeed over control surfaces, this will be used to boost throttle, when decending for example, 0 disables + // @Description: Use in conjunction with disk theory gain scaling and Q_TAILSIT_DSKLD to specify minumum airspeed over control surfaces, this will be used to boost throttle, when descending for example, 0 disables // @Range: 0 15 AP_GROUPINFO("MIN_VO", 22, Tailsitter, disk_loading_min_outflow, 0), @@ -302,11 +302,11 @@ void Tailsitter::output(void) */ if (!is_negative(transition_throttle_vtol)) { // Q_TAILSIT_THR_VT is positive use it until transition is complete - throttle = motors->actuator_to_thrust(MIN(transition_throttle_vtol*0.01,1.0)); + throttle = motors->thr_lin.actuator_to_thrust(MIN(transition_throttle_vtol*0.01,1.0)); } else { throttle = motors->get_throttle_hover(); // work out equivelent motors throttle level for cruise - throttle = MAX(throttle,motors->actuator_to_thrust(plane.aparm.throttle_cruise.get() * 0.01)); + throttle = MAX(throttle,motors->thr_lin.actuator_to_thrust(plane.aparm.throttle_cruise.get() * 0.01)); } SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0.0); @@ -321,7 +321,7 @@ void Tailsitter::output(void) // convert the hover throttle to the same output that would result if used via AP_Motors // apply expo, battery scaling and SPIN min/max. - throttle = motors->thrust_to_actuator(throttle); + throttle = motors->thr_lin.thrust_to_actuator(throttle); // override AP_MotorsTailsitter throttles during back transition @@ -330,7 +330,7 @@ void Tailsitter::output(void) SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, throttle_pwm); SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, throttle_pwm); - // throttle output is not used by AP_Motors so might have diffrent PWM range, set scaled + // throttle output is not used by AP_Motors so might have different PWM range, set scaled SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle * 100.0); } } @@ -366,20 +366,32 @@ void Tailsitter::output(void) if (quadplane.option_is_set(QuadPlane::OPTION::TAILSIT_Q_ASSIST_MOTORS_ONLY)) { // only use motors for Q assist, control surfaces remain under plane control. Zero copter I terms and use plane. // Smoothly relax to zero so there is no step change in output, must also set limit flags so integrator cannot build faster than the relax. - // Assume there is always roll and pitch control surfaces, otherwise motors only assist should not be set. + // Assume there is always roll control surfaces, otherwise motors only assist should not be set. const float dt = quadplane.attitude_control->get_dt(); - quadplane.attitude_control->get_rate_pitch_pid().relax_integrator(0.0, dt, AC_ATTITUDE_RATE_RELAX_TC); - motors->limit.pitch = true; + + // VTOL yaw / FW roll quadplane.attitude_control->get_rate_yaw_pid().relax_integrator(0.0, dt, AC_ATTITUDE_RATE_RELAX_TC); motors->limit.yaw = true; + // VTOL and FW pitch + if (_have_elevator || _have_elevon || _have_v_tail) { + // have pitch control surfaces, use them + quadplane.attitude_control->get_rate_pitch_pid().relax_integrator(0.0, dt, AC_ATTITUDE_RATE_RELAX_TC); + motors->limit.pitch = true; + } else { + // no pitch control surfaces, zero plane I terms and use motors + // We skip the outputting to surfaces for this axis from the copter controller but there are none setup + plane.pitchController.reset_I(); + } + + // VTOL roll / FW yaw if (_have_rudder || _have_v_tail) { // there are yaw control surfaces, zero motor I term quadplane.attitude_control->get_rate_roll_pid().relax_integrator(0.0, dt, AC_ATTITUDE_RATE_RELAX_TC); motors->limit.roll = true; } else { // no yaw control surfaces, zero plane I terms and use motors - // We skip the outputing to surfaces for this axis from the copter controller but there are none setup + // We skip the outputting to surfaces for this axis from the copter controller but there are none setup plane.yawController.reset_I(); } @@ -501,7 +513,7 @@ void Tailsitter::output(void) bool Tailsitter::transition_fw_complete(void) { if (!plane.arming.is_armed_and_safety_off()) { - // instant trainsition when disarmed, no message + // instant transition when disarmed, no message return true; } if (labs(quadplane.ahrs_view->pitch_sensor) > transition_angle_fw*100) { @@ -527,7 +539,7 @@ bool Tailsitter::transition_fw_complete(void) bool Tailsitter::transition_vtol_complete(void) const { if (!plane.arming.is_armed_and_safety_off()) { - // instant trainsition when disarmed, no message + // instant transition when disarmed, no message return true; } // for vectored tailsitters at zero pilot throttle @@ -622,7 +634,7 @@ void Tailsitter::speed_scaling(void) float spd_scaler = 1.0f; float disk_loading_min_throttle = 0.0; - // Scaleing with throttle + // Scaling with throttle float throttle_scaler = throttle_scale_max; if (is_positive(throttle)) { throttle_scaler = constrain_float(hover_throttle / throttle, gain_scaling_min, throttle_scale_max); @@ -880,7 +892,7 @@ bool Tailsitter_Transition::show_vtol_view() const return show_vtol; } -void Tailsitter_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd, bool& allow_stick_mixing) +void Tailsitter_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd) { uint32_t now = AP_HAL::millis(); if (tailsitter.in_vtol_transition(now)) { @@ -892,7 +904,6 @@ void Tailsitter_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& na // multiply by 0.1 to convert (degrees/second * milliseconds) to centi degrees nav_pitch_cd = constrain_float(vtol_transition_initial_pitch + (tailsitter.transition_rate_vtol * dt) * 0.1f, -8500, 8500); nav_roll_cd = 0; - allow_stick_mixing = false; } else if (transition_state == TRANSITION_DONE) { // still in FW, reset transition starting point @@ -908,12 +919,24 @@ void Tailsitter_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& na } else { nav_pitch_cd = pitch_limit_cd; nav_roll_cd = 0; - allow_stick_mixing = false; } } } } +bool Tailsitter_Transition::allow_stick_mixing() const +{ + // Transitioning into VTOL flight, inital pitch up + if (tailsitter.in_vtol_transition()) { + return false; + } + // Transitioning into fixed wing flight, levelling off + if ((transition_state == TRANSITION_DONE) && (fw_limit_start_ms != 0)) { + return false; + } + return true; +} + bool Tailsitter_Transition::set_VTOL_roll_pitch_limit(int32_t& nav_roll_cd, int32_t& nav_pitch_cd) { if (vtol_limit_start_ms == 0) { diff --git a/ArduPlane/tailsitter.h b/ArduPlane/tailsitter.h index 1d91fa49e589b..4f2ff33c3ba37 100644 --- a/ArduPlane/tailsitter.h +++ b/ArduPlane/tailsitter.h @@ -159,7 +159,9 @@ friend class Tailsitter; bool show_vtol_view() const override; - void set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd, bool& allow_stick_mixing) override; + void set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd) override; + + bool allow_stick_mixing() const override; MAV_VTOL_STATE get_mav_vtol_state() const override; diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index 31aeab02295f0..ce2264f8c800c 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -29,6 +29,24 @@ bool Plane::auto_takeoff_check(void) } takeoff_state.last_check_ms = now; + + //check if waiting for rudder neutral after rudder arm + if (plane.arming.last_arm_method() == AP_Arming::Method::RUDDER && + !seen_neutral_rudder) { + // we were armed with rudder but have not seen rudder neutral yet + takeoff_state.waiting_for_rudder_neutral = true; + // warn if we have been waiting a long time + if (now - takeoff_state.rudder_takeoff_warn_ms > TAKEOFF_RUDDER_WARNING_TIMEOUT) { + gcs().send_text(MAV_SEVERITY_WARNING, "Takeoff waiting for rudder release"); + takeoff_state.rudder_takeoff_warn_ms = now; + } + // since we are still waiting, dont takeoff + return false; + } else { + // we did not arm by rudder or rudder has returned to neutral + // make sure we dont indicate we are in the waiting state with servo position indicator + takeoff_state.waiting_for_rudder_neutral = false; + } // Check for bad GPS if (gps.status() < AP_GPS::GPS_OK_FIX_3D) { @@ -36,7 +54,7 @@ bool Plane::auto_takeoff_check(void) return false; } - bool do_takeoff_attitude_check = !(g2.flight_options & FlightOptions::DISABLE_TOFF_ATTITUDE_CHK); + bool do_takeoff_attitude_check = !(flight_option_enabled(FlightOptions::DISABLE_TOFF_ATTITUDE_CHK)); #if HAL_QUADPLANE_ENABLED // disable attitude check on tailsitters do_takeoff_attitude_check = !quadplane.tailsitter.enabled(); @@ -167,7 +185,7 @@ void Plane::takeoff_calc_pitch(void) return; } - if (ahrs.airspeed_sensor_enabled()) { + if (ahrs.using_airspeed_sensor()) { int16_t takeoff_pitch_min_cd = get_takeoff_pitch_min_cd(); calc_nav_pitch(); if (nav_pitch_cd < takeoff_pitch_min_cd) { @@ -220,7 +238,7 @@ int16_t Plane::get_takeoff_pitch_min_cd(void) return auto_state.takeoff_pitch_cd * scalar; } - // are we entering the region where we want to start leveling off before we reach takeoff alt? + // are we entering the region where we want to start levelling off before we reach takeoff alt? if (auto_state.sink_rate < -0.1f) { float sec_to_target = (remaining_height_to_target_cm * 0.01f) / (-auto_state.sink_rate); if (sec_to_target > 0 && @@ -244,7 +262,7 @@ int16_t Plane::get_takeoff_pitch_min_cd(void) */ int8_t Plane::takeoff_tail_hold(void) { - bool in_takeoff = ((control_mode == &mode_auto && !auto_state.takeoff_complete) || + bool in_takeoff = ((plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF) || (control_mode == &mode_fbwa && auto_state.fbwa_tdrag_takeoff_mode)); if (!in_takeoff) { // not in takeoff diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index 5591fb3b6d173..0be2a07845282 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -39,7 +39,7 @@ const AP_Param::GroupInfo Tiltrotor::var_info[] = { // @Param: TYPE // @DisplayName: Tiltrotor type - // @Description: This is the type of tiltrotor when TILT_MASK is non-zero. A continuous tiltrotor can tilt the rotors to any angle on demand. A binary tiltrotor assumes a retract style servo where the servo is either fully forward or fully up. In both cases the servo can't move faster than Q_TILT_RATE. A vectored yaw tiltrotor will use the tilt of the motors to control yaw in hover, Bicopter tiltrottor must use the tailsitter frame class (10) + // @Description: This is the type of tiltrotor when TILT_MASK is non-zero. A continuous tiltrotor can tilt the rotors to any angle on demand. A binary tiltrotor assumes a retract style servo where the servo is either fully forward or fully up. In both cases the servo can't move faster than Q_TILT_RATE. A vectored yaw tiltrotor will use the tilt of the motors to control yaw in hover, Bicopter tiltrotor must use the tailsitter frame class (10) // @Values: 0:Continuous,1:Binary,2:VectoredYaw,3:Bicopter AP_GROUPINFO("TYPE", 5, Tiltrotor, type, TILT_TYPE_CONTINUOUS), @@ -54,7 +54,7 @@ const AP_Param::GroupInfo Tiltrotor::var_info[] = { // @Param: YAW_ANGLE // @DisplayName: Tilt minimum angle for vectored yaw - // @Description: This is the angle of the tilt servos when in VTOL mode and at minimum output. This needs to be set for Q_TILT_TYPE=3 to enable vectored control for yaw of tricopter tilt quadplanes. This is also used to limit the forwards travel of bicopter tilts when in VTOL modes + // @Description: This is the angle of the tilt servos when in VTOL mode and at minimum output (fully back). This needs to be set in addition to Q_TILT_TYPE=2, to enable vectored control for yaw in tilt quadplanes. This is also used to limit the forward travel of bicopter tilts(Q_TILT_TYPE=3) when in VTOL modes. // @Range: 0 30 AP_GROUPINFO("YAW_ANGLE", 7, Tiltrotor, tilt_yaw_angle, 0), @@ -115,7 +115,7 @@ void Tiltrotor::setup() && (type != TILT_TYPE_BICOPTER)); - // check if there are any perminant VTOL motors + // check if there are any permanent VTOL motors for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; ++i) { if (motors->is_motor_enabled(i) && ((tilt_mask & (1U<<1)) == 0)) { // enabled motor not set in tilt mask @@ -193,7 +193,7 @@ void Tiltrotor::slew(float newtilt) SRV_Channels::set_output_scaled(SRV_Channel::k_motor_tilt, 1000 * current_tilt); } -// return the current tilt value that represens forward flight +// return the current tilt value that represents forward flight // tilt wings can sustain forward flight with some amount of wing tilt float Tiltrotor::get_fully_forward_tilt() const { @@ -258,21 +258,25 @@ void Tiltrotor::continuous_update(void) /* we are in a VTOL mode. We need to work out how much tilt is - needed. There are 4 strategies we will use: + needed. There are 5 strategies we will use: - 1) without manual forward throttle control, the angle will be set to zero - in QAUTOTUNE QACRO, QSTABILIZE and QHOVER. This - enables these modes to be used as a safe recovery mode. + 1) With use of a forward throttle controlled by Q_FWD_THR_GAIN in + VTOL modes except Q_AUTOTUNE determined by Q_FWD_THR_USE. We set the angle based on a calculated + forward throttle. - 2) with manual forward throttle control we will set the angle based on - the demanded forward throttle via RC input. + 2) With manual forward throttle control we set the angle based on the + RC input demanded forward throttle for QACRO, QSTABILIZE and QHOVER. - 3) in fixed wing assisted flight or velocity controlled modes we - will set the angle based on the demanded forward throttle, - with a maximum tilt given by Q_TILT_MAX. This relies on - Q_VFWD_GAIN being set. + 3) Without a RC input or calculated forward throttle value, the angle + will be set to zero in QAUTOTUNE, QACRO, QSTABILIZE and QHOVER. + This enables these modes to be used as a safe recovery mode. - 4) if we are in TRANSITION_TIMER mode then we are transitioning + 4) In fixed wing assisted flight or velocity controlled modes we will + set the angle based on the demanded forward throttle, with a maximum + tilt given by Q_TILT_MAX. This relies on Q_FWD_THR_GAIN or Q_VFWD_GAIN + being set. + + 5) if we are in TRANSITION_TIMER mode then we are transitioning to forward flight and should put the rotors all the way forward */ @@ -283,11 +287,23 @@ void Tiltrotor::continuous_update(void) } #endif - // if not in assisted flight and in QACRO, QSTABILIZE or QHOVER mode if (!quadplane.assisted_flight && - (plane.control_mode == &plane.mode_qacro || - plane.control_mode == &plane.mode_qstabilize || - plane.control_mode == &plane.mode_qhover)) { + quadplane.get_vfwd_method() == QuadPlane::ActiveFwdThr::NEW && + quadplane.is_flying_vtol()) + { + // We are using the rotor tilt functionality controlled by Q_FWD_THR_GAIN which can + // operate in all VTOL modes except Q_AUTOTUNE. Forward rotor tilt is used to produce + // forward thrust equivalent to what would have been produced by a forward thrust motor + // set to quadplane.forward_throttle_pct() + const float fwd_g_demand = 0.01f * quadplane.forward_throttle_pct() / plane.quadplane.q_fwd_thr_gain; + const float fwd_tilt_deg = MIN(degrees(atanf(fwd_g_demand)), (float)max_angle_deg); + slew(MIN(fwd_tilt_deg * (1/90.0), get_forward_flight_tilt())); + return; + } else if (!quadplane.assisted_flight && + (plane.control_mode == &plane.mode_qacro || + plane.control_mode == &plane.mode_qstabilize || + plane.control_mode == &plane.mode_qhover)) + { if (quadplane.rc_fwd_thr_ch == nullptr) { // no manual throttle control, set angle to zero slew(0); diff --git a/ArduPlane/transition.h b/ArduPlane/transition.h index 9ddc1d5b5f579..b31e45ff040ec 100644 --- a/ArduPlane/transition.h +++ b/ArduPlane/transition.h @@ -40,7 +40,7 @@ class Transition virtual bool show_vtol_view() const = 0; - virtual void set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd, bool& allow_stick_mixing) {}; + virtual void set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd) {}; virtual bool set_FW_roll_limit(int32_t& roll_limit_cd) { return false; } @@ -56,6 +56,8 @@ class Transition virtual void set_last_fw_pitch(void) {} + virtual bool allow_stick_mixing() const { return true; } + protected: // refences for convenience @@ -87,7 +89,7 @@ class SLT_Transition : public Transition bool show_vtol_view() const override; - void set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd, bool& allow_stick_mixing) override; + void set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd) override; bool set_FW_roll_limit(int32_t& roll_limit_cd) override; diff --git a/ArduPlane/tuning.cpp b/ArduPlane/tuning.cpp index 2218a11ab0a67..be860264fd339 100644 --- a/ArduPlane/tuning.cpp +++ b/ArduPlane/tuning.cpp @@ -1,3 +1,7 @@ +#include + +#if AP_TUNING_ENABLED + #include "Plane.h" /* @@ -90,6 +94,7 @@ const AP_Tuning_Plane::tuning_name AP_Tuning_Plane::tuning_names[] = { { TUNING_PIT_I, "PitchI" }, { TUNING_PIT_D, "PitchD" }, { TUNING_PIT_FF, "PitchFF" }, + { TUNING_Q_FWD_THR, "QModeFwdThr" }, { TUNING_NONE, nullptr } }; @@ -186,6 +191,9 @@ AP_Float *AP_Tuning_Plane::get_param_pointer(uint8_t parm) case TUNING_RATE_YAW_FF: return &plane.quadplane.attitude_control->get_rate_yaw_pid().ff(); + + case TUNING_Q_FWD_THR: + return &plane.quadplane.q_fwd_thr_gain; #endif // HAL_QUADPLANE_ENABLED // fixed wing tuning parameters @@ -305,3 +313,4 @@ void AP_Tuning_Plane::reload_value(uint8_t parm) } } +#endif // AP_TUNING_ENABLED diff --git a/ArduPlane/tuning.h b/ArduPlane/tuning.h index 0a005f7dafdbf..4334455a09657 100644 --- a/ArduPlane/tuning.h +++ b/ArduPlane/tuning.h @@ -1,3 +1,9 @@ +#pragma once + +#include + +#if AP_TUNING_ENABLED + #include /* @@ -70,6 +76,8 @@ class AP_Tuning_Plane : public AP_Tuning TUNING_PIT_I = 55, TUNING_PIT_D = 56, TUNING_PIT_FF = 57, + + TUNING_Q_FWD_THR = 58, }; /* @@ -108,3 +116,5 @@ class AP_Tuning_Plane : public AP_Tuning // mask of what params have been set uint64_t have_set; }; + +#endif // AP_TUNING_ENABLED diff --git a/ArduPlane/version.h b/ArduPlane/version.h index 59de573b2b225..cdf466f7155fb 100644 --- a/ArduPlane/version.h +++ b/ArduPlane/version.h @@ -6,13 +6,13 @@ #include "ap_version.h" -#define THISFIRMWARE "ArduPlane V4.4.0-dev" +#define THISFIRMWARE "ArduPlane V4.5.0-dev" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,4,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 4,5,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 4 -#define FW_MINOR 4 +#define FW_MINOR 5 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV diff --git a/ArduPlane/wscript b/ArduPlane/wscript index 1386bd7b7c1c9..4dd670ccc34a5 100644 --- a/ArduPlane/wscript +++ b/ArduPlane/wscript @@ -28,7 +28,6 @@ def build(bld): 'AP_Devo_Telem', 'AP_OSD', 'AC_AutoTune', - 'AP_KDECAN', 'AP_Follow', ], ) diff --git a/ArduSub/AP_Arming_Sub.cpp b/ArduSub/AP_Arming_Sub.cpp index 7eb1589a8178d..66dfecff2b37b 100644 --- a/ArduSub/AP_Arming_Sub.cpp +++ b/ArduSub/AP_Arming_Sub.cpp @@ -105,7 +105,7 @@ bool AP_Arming_Sub::arm(AP_Arming::Method method, bool do_arming_checks) } #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - gcs().send_text(MAV_SEVERITY_INFO, "Arming motors"); + send_arm_disarm_statustext("Arming motors"); #endif AP_AHRS &ahrs = AP::ahrs(); @@ -134,7 +134,7 @@ bool AP_Arming_Sub::arm(AP_Arming::Method method, bool do_arming_checks) sub.motors.armed(true); // log flight mode in case it was changed while vehicle was disarmed - AP::logger().Write_Mode(sub.control_mode, sub.control_mode_reason); + AP::logger().Write_Mode((uint8_t)sub.control_mode, sub.control_mode_reason); // reenable failsafe sub.mainloop_failsafe_enable(); @@ -161,7 +161,7 @@ bool AP_Arming_Sub::disarm(const AP_Arming::Method method, bool do_disarm_checks } #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - gcs().send_text(MAV_SEVERITY_INFO, "Disarming motors"); + send_arm_disarm_statustext("Disarming motors"); #endif auto &ahrs = AP::ahrs(); diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 918a189eb3413..e8012d2e91f61 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -80,7 +80,6 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = { SCHED_TASK(three_hz_loop, 3, 75, 21), SCHED_TASK(update_turn_counter, 10, 50, 24), SCHED_TASK_CLASS(AP_Baro, &sub.barometer, accumulate, 50, 90, 27), - SCHED_TASK_CLASS(AP_Notify, &sub.notify, update, 50, 90, 30), SCHED_TASK(one_hz_loop, 1, 100, 33), SCHED_TASK_CLASS(GCS, (GCS*)&sub._gcs, update_receive, 400, 180, 36), SCHED_TASK_CLASS(GCS, (GCS*)&sub._gcs, update_send, 400, 550, 39), @@ -102,6 +101,9 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = { #if AP_GRIPPER_ENABLED SCHED_TASK_CLASS(AP_Gripper, &sub.g2.gripper, update, 10, 75, 75), #endif +#if STATS_ENABLED == ENABLED + SCHED_TASK(stats_update, 1, 200, 76), +#endif #ifdef USERHOOK_FASTLOOP SCHED_TASK(userhook_FastLoop, 100, 75, 78), #endif @@ -138,7 +140,7 @@ void Sub::run_rate_controller() pos_control.set_dt(last_loop_time_s); //don't run rate controller in manual or motordetection modes - if (control_mode != MANUAL && control_mode != MOTOR_DETECT) { + if (control_mode != Mode::Number::MANUAL && control_mode != Mode::Number::MOTOR_DETECT) { // run low level rate controllers that only require IMU data and set loop time attitude_control.rate_controller_run(); } @@ -199,7 +201,7 @@ void Sub::ten_hz_logging_loop() if (should_log(MASK_LOG_RCOUT)) { logger.Write_RCOUT(); } - if (should_log(MASK_LOG_NTUN) && (mode_requires_GPS(control_mode) || !mode_has_manual_throttle(control_mode))) { + if (should_log(MASK_LOG_NTUN) && (sub.flightmode->requires_GPS() || !sub.flightmode->has_manual_throttle())) { pos_control.write_log(); } if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) { @@ -208,6 +210,11 @@ void Sub::ten_hz_logging_loop() if (should_log(MASK_LOG_CTUN)) { attitude_control.control_monitor_log(); } +#if HAL_MOUNT_ENABLED + if (should_log(MASK_LOG_CAMERA)) { + camera_mount.write_log(); + } +#endif } // twentyfive_hz_logging_loop @@ -253,7 +260,9 @@ void Sub::three_hz_loop() fence_check(); #endif // AP_FENCE_ENABLED +#if AP_SERVORELAYEVENTS_ENABLED ServoRelayEvents.update_events(); +#endif } // one_hz_loop - runs at 1Hz @@ -282,6 +291,9 @@ void Sub::one_hz_loop() // need to set "likely flying" when armed to allow for compass // learning to run 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_z_pid().set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); } void Sub::read_AHRS() @@ -346,4 +358,15 @@ bool Sub::get_wp_crosstrack_error_m(float &xtrack_error) const return true; } +#if STATS_ENABLED == ENABLED +/* + update AP_Stats +*/ +void Sub::stats_update(void) +{ + g2.stats.set_flying(motors.armed()); + g2.stats.update(); +} +#endif + AP_HAL_MAIN_CALLBACKS(&sub); diff --git a/ArduSub/Attitude.cpp b/ArduSub/Attitude.cpp index 424f853939a22..408d9bfa6b1ca 100644 --- a/ArduSub/Attitude.cpp +++ b/ArduSub/Attitude.cpp @@ -96,8 +96,8 @@ float Sub::get_pilot_desired_climb_rate(float throttle_control) float desired_rate = 0.0f; float mid_stick = channel_throttle->get_control_mid(); - float deadband_top = mid_stick + g.throttle_deadzone; - float deadband_bottom = mid_stick - g.throttle_deadzone; + float deadband_top = mid_stick + g.throttle_deadzone * gain; + float deadband_bottom = mid_stick - g.throttle_deadzone * gain; // ensure a reasonable throttle value throttle_control = constrain_float(throttle_control,0.0f,1000.0f); diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index f078629041daf..aa4c39b3268a7 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -21,10 +21,10 @@ MAV_MODE GCS_MAVLINK_Sub::base_mode() const // the APM flight mode and has a well defined meaning in the // ArduPlane documentation switch (sub.control_mode) { - case AUTO: - case GUIDED: - case CIRCLE: - case POSHOLD: + case Mode::Number::AUTO: + case Mode::Number::GUIDED: + case Mode::Number::CIRCLE: + case Mode::Number::POSHOLD: _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 @@ -50,7 +50,7 @@ MAV_MODE GCS_MAVLINK_Sub::base_mode() const uint32_t GCS_Sub::custom_mode() const { - return sub.control_mode; + return (uint32_t)sub.control_mode; } MAV_STATE GCS_MAVLINK_Sub::vehicle_system_status() const @@ -346,15 +346,21 @@ static const ap_message STREAM_RAW_SENSORS_msgs[] = { static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { MSG_SYS_STATUS, MSG_POWER_STATUS, +#if HAL_WITH_MCU_MONITORING MSG_MCU_STATUS, +#endif MSG_MEMINFO, MSG_CURRENT_WAYPOINT, MSG_GPS_RAW, MSG_GPS_RTK, +#if GPS_MAX_RECEIVERS > 1 MSG_GPS2_RAW, MSG_GPS2_RTK, +#endif MSG_NAV_CONTROLLER_OUTPUT, +#if AP_FENCE_ENABLED MSG_FENCE_STATUS, +#endif MSG_NAMED_FLOAT }; static const ap_message STREAM_POSITION_msgs[] = { @@ -368,7 +374,9 @@ static const ap_message STREAM_RC_CHANNELS_msgs[] = { }; static const ap_message STREAM_EXTRA1_msgs[] = { MSG_ATTITUDE, +#if AP_SIM_ENABLED MSG_SIMSTATE, +#endif MSG_AHRS2, MSG_PID_TUNING }; @@ -383,17 +391,27 @@ static const ap_message STREAM_EXTRA3_msgs[] = { #if AP_TERRAIN_AVAILABLE MSG_TERRAIN, #endif +#if AP_BATTERY_ENABLED MSG_BATTERY_STATUS, +#endif +#if HAL_MOUNT_ENABLED MSG_GIMBAL_DEVICE_ATTITUDE_STATUS, +#endif +#if AP_OPTICALFLOW_ENABLED MSG_OPTICAL_FLOW, +#endif +#if COMPASS_CAL_ENABLED MSG_MAG_CAL_REPORT, MSG_MAG_CAL_PROGRESS, +#endif MSG_EKF_STATUS_REPORT, MSG_VIBRATION, #if AP_RPM_ENABLED MSG_RPM, #endif +#if HAL_WITH_ESC_TELEM MSG_ESC_TELEMETRY, +#endif }; static const ap_message STREAM_PARAMS_msgs[] = { MSG_NEXT_PARAM @@ -431,9 +449,9 @@ MAV_RESULT GCS_MAVLINK_Sub::_handle_command_preflight_calibration_baro(const mav return MAV_RESULT_ACCEPTED; } -MAV_RESULT GCS_MAVLINK_Sub::_handle_command_preflight_calibration(const mavlink_command_long_t &packet, const mavlink_message_t &msg) +MAV_RESULT GCS_MAVLINK_Sub::_handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { - if (is_equal(packet.param6,1.0f)) { + if (packet.y == 1) { // compassmot calibration //result = sub.mavlink_compassmot(chan); gcs().send_text(MAV_SEVERITY_INFO, "#CompassMot calibration not supported"); @@ -448,7 +466,7 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_do_set_roi(const Location &roi_loc) if (!roi_loc.check_latlng()) { return MAV_RESULT_FAILED; } - sub.set_auto_yaw_roi(roi_loc); + sub.mode_auto.set_auto_yaw_roi(roi_loc); return MAV_RESULT_ACCEPTED; } @@ -459,23 +477,51 @@ bool GCS_MAVLINK_Sub::set_home(const Location& loc, bool _lock) { return sub.set_home(loc, _lock); } - -MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_long_t &packet) +MAV_RESULT GCS_MAVLINK_Sub::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { - switch (packet.command) { + switch(packet.command) { + + case MAV_CMD_CONDITION_YAW: + return handle_MAV_CMD_CONDITION_YAW(packet); + + case MAV_CMD_DO_CHANGE_SPEED: + return handle_MAV_CMD_DO_CHANGE_SPEED(packet); + + case MAV_CMD_DO_MOTOR_TEST: + return handle_MAV_CMD_DO_MOTOR_TEST(packet); + + case MAV_CMD_MISSION_START: + return handle_MAV_CMD_MISSION_START(packet); + case MAV_CMD_NAV_LOITER_UNLIM: - if (!sub.set_mode(POSHOLD, ModeReason::GCS_COMMAND)) { + return handle_MAV_CMD_NAV_LOITER_UNLIM(packet); + + case MAV_CMD_NAV_LAND: + return handle_MAV_CMD_NAV_LAND(packet); + + } + + return GCS_MAVLINK::handle_command_int_packet(packet, msg); +} + +MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_NAV_LOITER_UNLIM(const mavlink_command_int_t &packet) +{ + if (!sub.set_mode(Mode::Number::POSHOLD, ModeReason::GCS_COMMAND)) { return MAV_RESULT_FAILED; } return MAV_RESULT_ACCEPTED; +} - case MAV_CMD_NAV_LAND: - if (!sub.set_mode(SURFACE, ModeReason::GCS_COMMAND)) { +MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_NAV_LAND(const mavlink_command_int_t &packet) +{ + if (!sub.set_mode(Mode::Number::SURFACE, ModeReason::GCS_COMMAND)) { return MAV_RESULT_FAILED; } return MAV_RESULT_ACCEPTED; +} - case MAV_CMD_CONDITION_YAW: +MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_CONDITION_YAW(const mavlink_command_int_t &packet) +{ // param1 : target angle [0-360] // param2 : speed during change [deg per second] // param3 : direction (-1:ccw, +1:cw) @@ -483,12 +529,14 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon if ((packet.param1 >= 0.0f) && (packet.param1 <= 360.0f) && (is_zero(packet.param4) || is_equal(packet.param4,1.0f))) { - sub.set_auto_yaw_look_at_heading(packet.param1, packet.param2, (int8_t)packet.param3, (uint8_t)packet.param4); + sub.mode_auto.set_auto_yaw_look_at_heading(packet.param1, packet.param2, (int8_t)packet.param3, (uint8_t)packet.param4); return MAV_RESULT_ACCEPTED; } - return MAV_RESULT_FAILED; + return MAV_RESULT_DENIED; +} - case MAV_CMD_DO_CHANGE_SPEED: +MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_DO_CHANGE_SPEED(const mavlink_command_int_t &packet) +{ // param1 : unused // param2 : new speed in m/s // param3 : unused @@ -498,14 +546,18 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; +} - case MAV_CMD_MISSION_START: - if (sub.motors.armed() && sub.set_mode(AUTO, ModeReason::GCS_COMMAND)) { +MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_MISSION_START(const mavlink_command_int_t &packet) +{ + if (sub.motors.armed() && sub.set_mode(Mode::Number::AUTO, ModeReason::GCS_COMMAND)) { return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; +} - case MAV_CMD_DO_MOTOR_TEST: +MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command_int_t &packet) +{ // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle) // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum) // param3 : throttle (range depends upon param2) @@ -514,14 +566,8 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon return MAV_RESULT_FAILED; } return MAV_RESULT_ACCEPTED; - - default: - return GCS_MAVLINK::handle_command_long_packet(packet); - } } - - void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) { switch (msg.msgid) { @@ -537,7 +583,23 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) break; // only accept control aimed at us } - sub.transform_manual_control_to_rc_override(packet.x,packet.y,packet.z,packet.r,packet.buttons); + sub.transform_manual_control_to_rc_override( + packet.x, + packet.y, + packet.z, + packet.r, + packet.buttons, + packet.buttons2, + packet.enabled_extensions, + packet.s, + packet.t, + packet.aux1, + packet.aux2, + packet.aux3, + packet.aux4, + packet.aux5, + packet.aux6 + ); sub.failsafe.last_pilot_input_ms = AP_HAL::millis(); // a RC override message is considered to be a 'heartbeat' @@ -588,7 +650,7 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) // descend at up to WPNAV_SPEED_DN climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * sub.wp_nav.get_default_speed_down(); } - sub.guided_set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), climb_rate_cms); + sub.mode_guided.guided_set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), climb_rate_cms); break; } @@ -598,7 +660,7 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) mavlink_msg_set_position_target_local_ned_decode(&msg, &packet); // exit if vehicle is not in Guided mode or Auto-Guided mode - if ((sub.control_mode != GUIDED) && !(sub.control_mode == AUTO && sub.auto_mode == Auto_NavGuided)) { + if ((sub.control_mode != Mode::Number::GUIDED) && !(sub.control_mode == Mode::Number::AUTO && sub.auto_mode == Auto_NavGuided)) { break; } @@ -606,34 +668,32 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED && packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED && packet.coordinate_frame != MAV_FRAME_BODY_NED && - packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) { + packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED && + packet.coordinate_frame != MAV_FRAME_BODY_FRD) { break; } bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE; bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; - - /* - * for future use: - * bool force = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE; - * bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE; - * bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE; - */ + bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE; + bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE; // prepare position Vector3f pos_vector; if (!pos_ignore) { // convert to cm pos_vector = Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f); - // rotate to body-frame if necessary + // rotate from body-frame if necessary if (packet.coordinate_frame == MAV_FRAME_BODY_NED || + packet.coordinate_frame == MAV_FRAME_BODY_FRD || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { sub.rotate_body_frame_to_NE(pos_vector.x, pos_vector.y); } // 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_FRD || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { pos_vector += sub.inertial_nav.get_position_neu_cm(); } @@ -644,19 +704,31 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) if (!vel_ignore) { // convert to cm vel_vector = Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f); - // rotate to body-frame if necessary - if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { + // rotate from body-frame if necessary + if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_FRD || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { sub.rotate_body_frame_to_NE(vel_vector.x, vel_vector.y); } } + // prepare yaw + float yaw_cd = 0.0f; + bool yaw_relative = false; + float yaw_rate_cds = 0.0f; + if (!yaw_ignore) { + yaw_cd = ToDeg(packet.yaw) * 100.0f; + yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED; + } + if (!yaw_rate_ignore) { + yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f; + } + // send request if (!pos_ignore && !vel_ignore && acc_ignore) { - sub.guided_set_destination_posvel(pos_vector, vel_vector); + sub.mode_guided.guided_set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); } else if (pos_ignore && !vel_ignore && acc_ignore) { - sub.guided_set_velocity(vel_vector); + sub.mode_guided.guided_set_velocity(vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); } else if (!pos_ignore && vel_ignore && acc_ignore) { - sub.guided_set_destination(pos_vector); + sub.mode_guided.guided_set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); } break; @@ -668,9 +740,9 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) mavlink_msg_set_position_target_global_int_decode(&msg, &packet); // exit if vehicle is not in Guided, Auto-Guided, or Depth Hold modes - if ((sub.control_mode != GUIDED) - && !(sub.control_mode == AUTO && sub.auto_mode == Auto_NavGuided) - && !(sub.control_mode == ALT_HOLD)) { + if ((sub.control_mode != Mode::Number::GUIDED) + && !(sub.control_mode == Mode::Number::AUTO && sub.auto_mode == Auto_NavGuided) + && !(sub.control_mode == Mode::Number::ALT_HOLD)) { break; } @@ -686,7 +758,7 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) * bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE; */ - if (!z_ignore && sub.control_mode == ALT_HOLD) { // Control only target depth when in ALT_HOLD + if (!z_ignore && sub.control_mode == Mode::Number::ALT_HOLD) { // Control only target depth when in ALT_HOLD sub.pos_control.set_pos_target_z_cm(packet.alt*100); break; } @@ -715,11 +787,11 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) } if (!pos_ignore && !vel_ignore && acc_ignore) { - sub.guided_set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f)); + sub.mode_guided.guided_set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f)); } else if (pos_ignore && !vel_ignore && acc_ignore) { - sub.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f)); + sub.mode_guided.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f)); } else if (!pos_ignore && vel_ignore && acc_ignore) { - sub.guided_set_destination(pos_neu_cm); + sub.mode_guided.guided_set_destination(pos_neu_cm); } break; @@ -765,7 +837,7 @@ uint64_t GCS_MAVLINK_Sub::capabilities() const ); } -MAV_RESULT GCS_MAVLINK_Sub::handle_flight_termination(const mavlink_command_long_t &packet) { +MAV_RESULT GCS_MAVLINK_Sub::handle_flight_termination(const mavlink_command_int_t &packet) { if (packet.param1 > 0.5f) { sub.arming.disarm(AP_Arming::Method::TERMINATION); return MAV_RESULT_ACCEPTED; @@ -794,7 +866,7 @@ int16_t GCS_MAVLINK_Sub::high_latency_target_altitude() const UNUSED_RESULT(ahrs.get_location(global_position_current)); //return units are m - if (sub.control_mode == AUTO || sub.control_mode == GUIDED) { + if (sub.control_mode == Mode::Number::AUTO || sub.control_mode == Mode::Number::GUIDED) { return 0.01 * (global_position_current.alt + sub.pos_control.get_pos_error_z_cm()); } return 0; @@ -804,7 +876,7 @@ int16_t GCS_MAVLINK_Sub::high_latency_target_altitude() const uint8_t GCS_MAVLINK_Sub::high_latency_tgt_heading() const { // return units are deg/2 - if (sub.control_mode == AUTO || sub.control_mode == GUIDED) { + if (sub.control_mode == Mode::Number::AUTO || sub.control_mode == Mode::Number::GUIDED) { // need to convert -18000->18000 to 0->360/2 return wrap_360_cd(sub.wp_nav.get_wp_bearing_to_destination()) / 200; } @@ -814,7 +886,7 @@ uint8_t GCS_MAVLINK_Sub::high_latency_tgt_heading() const uint16_t GCS_MAVLINK_Sub::high_latency_tgt_dist() const { // return units are dm - if (sub.control_mode == AUTO || sub.control_mode == GUIDED) { + if (sub.control_mode == Mode::Number::AUTO || sub.control_mode == Mode::Number::GUIDED) { return MIN(sub.wp_nav.get_wp_distance_to_destination() * 0.001, UINT16_MAX); } return 0; @@ -823,7 +895,7 @@ uint16_t GCS_MAVLINK_Sub::high_latency_tgt_dist() const uint8_t GCS_MAVLINK_Sub::high_latency_tgt_airspeed() const { // return units are m/s*5 - if (sub.control_mode == AUTO || sub.control_mode == GUIDED) { + if (sub.control_mode == Mode::Number::AUTO || sub.control_mode == Mode::Number::GUIDED) { return MIN((sub.pos_control.get_vel_desired_cms().length()/100) * 5, UINT8_MAX); } return 0; diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h index 937224027c194..91c62ddff5278 100644 --- a/ArduSub/GCS_Mavlink.h +++ b/ArduSub/GCS_Mavlink.h @@ -8,20 +8,20 @@ class GCS_MAVLINK_Sub : public GCS_MAVLINK { using GCS_MAVLINK::GCS_MAVLINK; + uint8_t sysid_my_gcs() const override; protected: uint32_t telem_delay() const override { return 0; }; - MAV_RESULT handle_flight_termination(const mavlink_command_long_t &packet) override; - - uint8_t sysid_my_gcs() const override; + MAV_RESULT handle_flight_termination(const mavlink_command_int_t &packet) override; MAV_RESULT handle_command_do_set_roi(const Location &roi_loc) override; MAV_RESULT _handle_command_preflight_calibration_baro(const mavlink_message_t &msg) override; - MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet, const mavlink_message_t &msg) override; - MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override; + MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; + + MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; // override sending of scaled_pressure3 to send on-board temperature: void send_scaled_pressure3() override; @@ -52,6 +52,13 @@ class GCS_MAVLINK_Sub : public GCS_MAVLINK { int16_t vfr_hud_throttle() const override; + MAV_RESULT handle_MAV_CMD_CONDITION_YAW(const mavlink_command_int_t &packet); + MAV_RESULT handle_MAV_CMD_MISSION_START(const mavlink_command_int_t &packet); + MAV_RESULT handle_MAV_CMD_DO_CHANGE_SPEED(const mavlink_command_int_t &packet); + MAV_RESULT handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command_int_t &packet); + MAV_RESULT handle_MAV_CMD_NAV_LOITER_UNLIM(const mavlink_command_int_t &packet); + MAV_RESULT handle_MAV_CMD_NAV_LAND(const mavlink_command_int_t &packet); + #if HAL_HIGH_LATENCY2_ENABLED int16_t high_latency_target_altitude() const override; uint8_t high_latency_tgt_heading() const override; diff --git a/ArduSub/GCS_Sub.cpp b/ArduSub/GCS_Sub.cpp index d535a823e6b6c..b57839c9bc842 100644 --- a/ArduSub/GCS_Sub.cpp +++ b/ArduSub/GCS_Sub.cpp @@ -30,12 +30,12 @@ void GCS_Sub::update_vehicle_sensor_status_flags() MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; switch (sub.control_mode) { - case ALT_HOLD: - case AUTO: - case GUIDED: - case CIRCLE: - case SURFACE: - case POSHOLD: + case Mode::Number::ALT_HOLD: + case Mode::Number::AUTO: + case Mode::Number::GUIDED: + case Mode::Number::CIRCLE: + case Mode::Number::SURFACE: + case Mode::Number::POSHOLD: control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index 1dbf759d6ab93..f2649bc8417a9 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -278,7 +278,7 @@ const struct LogStructure Sub::log_structure[] = { void Sub::Log_Write_Vehicle_Startup_Messages() { // only 200(?) bytes are guaranteed by AP_Logger - logger.Write_Mode(control_mode, control_mode_reason); + logger.Write_Mode((uint8_t)control_mode, control_mode_reason); ahrs.Log_Write_Home_And_Origin(); gps.Write_AP_Logger_Log_Startup_messages(); } diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 75afeb22ca1c6..d43b149820f74 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -47,6 +47,8 @@ const AP_Param::Info Sub::var_info[] = { // @Param: SYSID_MYGCS // @DisplayName: My ground station number // @Description: Allows restricting radio overrides to only come from my ground station + // @Range: 1 255 + // @Increment: 1 // @User: Advanced GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), @@ -271,7 +273,7 @@ const AP_Param::Info Sub::var_info[] = { // @Param: JS_THR_GAIN // @DisplayName: Throttle gain scalar - // @Description: Scalar for gain on the throttle channel + // @Description: Scalar for gain on the throttle channel. Gets scaled with the current JS gain // @User: Standard // @Range: 0.5 4.0 GSCALAR(throttle_gain, "JS_THR_GAIN", 1.0f), @@ -348,6 +350,70 @@ const AP_Param::Info Sub::var_info[] = { // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp GGROUP(jbtn_15, "BTN15_", JSButton), + // @Group: BTN16_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_16, "BTN16_", JSButton), + + // @Group: BTN17_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_17, "BTN17_", JSButton), + + // @Group: BTN18_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_18, "BTN18_", JSButton), + + // @Group: BTN19_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_19, "BTN19_", JSButton), + + // @Group: BTN20_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_20, "BTN20_", JSButton), + + // @Group: BTN21_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_21, "BTN21_", JSButton), + + // @Group: BTN22_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_22, "BTN22_", JSButton), + + // @Group: BTN23_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_23, "BTN23_", JSButton), + + // @Group: BTN24_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_24, "BTN24_", JSButton), + + // @Group: BTN25_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_25, "BTN25_", JSButton), + + // @Group: BTN26_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_26, "BTN26_", JSButton), + + // @Group: BTN27_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_27, "BTN27_", JSButton), + + // @Group: BTN28_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_28, "BTN28_", JSButton), + + // @Group: BTN29_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_29, "BTN29_", JSButton), + + // @Group: BTN30_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_30, "BTN30_", JSButton), + + // @Group: BTN31_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_31, "BTN31_", JSButton), + // @Param: RC_SPEED // @DisplayName: ESC Update Speed // @Description: This is the speed in Hertz that your ESCs will receive updates @@ -409,9 +475,11 @@ const AP_Param::Info Sub::var_info[] = { GOBJECT(camera, "CAM", AP_Camera), #endif +#if AP_RELAY_ENABLED // @Group: RELAY_ // @Path: ../libraries/AP_Relay/AP_Relay.cpp GOBJECT(relay, "RELAY_", AP_Relay), +#endif // @Group: COMPASS_ // @Path: ../libraries/AP_Compass/AP_Compass.cpp @@ -620,7 +688,11 @@ const AP_Param::Info Sub::var_info[] = { 2nd group of parameters */ const AP_Param::GroupInfo ParametersG2::var_info[] = { - +#if STATS_ENABLED == ENABLED + // @Group: STAT + // @Path: ../libraries/AP_Stats/AP_Stats.cpp + AP_SUBGROUPINFO(stats, "STAT", 1, ParametersG2, AP_Stats), +#endif #if HAL_PROXIMITY_ENABLED // @Group: PRX // @Path: ../libraries/AP_Proximity/AP_Proximity.cpp @@ -695,22 +767,7 @@ void Sub::load_parameters() AP_Param::set_frame_type_flags(AP_PARAM_FRAME_SUB); convert_old_parameters(); - - AP_Param::set_default_by_name("BRD_SAFETY_DEFLT", 0); - AP_Param::set_default_by_name("ARMING_CHECK", - AP_Arming::ARMING_CHECK_RC | - AP_Arming::ARMING_CHECK_VOLTAGE | - AP_Arming::ARMING_CHECK_BATTERY); - AP_Param::set_default_by_name("CIRCLE_RATE", 2.0f); - AP_Param::set_default_by_name("ATC_ACCEL_Y_MAX", 110000.0f); - AP_Param::set_default_by_name("RC3_TRIM", 1100); - AP_Param::set_default_by_name("COMPASS_OFFS_MAX", 1000); - AP_Param::set_default_by_name("INS_GYR_CAL", 0); - AP_Param::set_default_by_name("MNT1_TYPE", 1); - AP_Param::set_default_by_name("MNT1_DEFLT_MODE", MAV_MOUNT_MODE_RC_TARGETING); - AP_Param::set_default_by_name("MNT1_RC_RATE", 30); - AP_Param::set_default_by_name("RC7_OPTION", 214); // MOUNT1_YAW - AP_Param::set_default_by_name("RC8_OPTION", 213); // MOUNT1_PITCH + AP_Param::set_defaults_from_table(defaults_table, ARRAY_SIZE(defaults_table)); // We should ignore this parameter since ROVs are neutral buoyancy AP_Param::set_by_name("MOT_THST_HOVER", 0.5); diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index eeff0a26935c1..95ca7923ab86f 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -5,6 +5,8 @@ #include #include +#include +#include #if AP_SCRIPTING_ENABLED #include @@ -47,7 +49,7 @@ class Parameters { // Layout version number, always key zero. // k_param_format_version = 0, - k_param_software_type, // unusued + k_param_software_type, // unused k_param_g2, // 2nd block of parameters @@ -147,6 +149,23 @@ class Parameters { k_param_jbtn_14, k_param_jbtn_15, + // 16 more for MANUAL_CONTROL extensions + k_param_jbtn_16, + k_param_jbtn_17, + k_param_jbtn_18, + k_param_jbtn_19, + k_param_jbtn_20, + k_param_jbtn_21, + k_param_jbtn_22, + k_param_jbtn_23, + k_param_jbtn_24, + k_param_jbtn_25, + k_param_jbtn_26, + k_param_jbtn_27, + k_param_jbtn_28, + k_param_jbtn_29, + k_param_jbtn_30, + k_param_jbtn_31, // PID Controllers k_param_p_pos_xy = 126, // deprecated @@ -293,6 +312,23 @@ class Parameters { JSButton jbtn_13; JSButton jbtn_14; JSButton jbtn_15; + // 16 - 31 from manual_control extension + JSButton jbtn_16; + JSButton jbtn_17; + JSButton jbtn_18; + JSButton jbtn_19; + JSButton jbtn_20; + JSButton jbtn_21; + JSButton jbtn_22; + JSButton jbtn_23; + JSButton jbtn_24; + JSButton jbtn_25; + JSButton jbtn_26; + JSButton jbtn_27; + JSButton jbtn_28; + JSButton jbtn_29; + JSButton jbtn_30; + JSButton jbtn_31; // Acro parameters AP_Float acro_rp_p; @@ -318,6 +354,10 @@ class Parameters { class ParametersG2 { public: ParametersG2(void); +#if STATS_ENABLED == ENABLED + // vehicle statistics + AP_Stats stats; +#endif // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; @@ -344,3 +384,43 @@ class ParametersG2 { extern const AP_Param::Info var_info[]; +// Sub-specific default parameters +static const struct AP_Param::defaults_table_struct defaults_table[] = { + { "BRD_SAFETY_DEFLT", 0 }, + { "ARMING_CHECK", AP_Arming::ARMING_CHECK_RC | + AP_Arming::ARMING_CHECK_VOLTAGE | + AP_Arming::ARMING_CHECK_BATTERY}, + { "CIRCLE_RATE", 2.0f}, + { "ATC_ACCEL_Y_MAX", 110000.0f}, + { "ATC_RATE_Y_MAX", 180.0f}, + { "RC3_TRIM", 1100}, + { "COMPASS_OFFS_MAX", 1000}, + { "INS_GYR_CAL", 0}, + { "MNT1_TYPE", 1}, + { "MNT1_DEFLT_MODE", MAV_MOUNT_MODE_RC_TARGETING}, + { "MNT1_RC_RATE", 30}, + { "RC7_OPTION", 214}, // MOUNT1_YAW + { "RC8_OPTION", 213}, // MOUNT1_PITCH + { "MOT_PWM_MIN", 1100}, + { "MOT_PWM_MAX", 1900}, + { "PSC_JERK_Z", 50.0f}, + { "WPNAV_SPEED", 100.0f}, + { "PILOT_SPEED_UP", 100.0f}, + { "PSC_VELXY_P", 6.0f}, + { "EK3_SRC1_VELZ", 0}, +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR + { "BARO_PROBE_EXT", 0}, + { "BATT_MONITOR", 4}, + { "BATT_CAPACITY", 0}, + { "LEAK1_PIN", 27}, + { "SCHED_LOOP_RATE", 200}, + { "SERVO13_FUNCTION", 59}, // k_rcin9, lights 1 + { "SERVO14_FUNCTION", 60}, // k_rcin10, lights 2 + { "SERVO16_FUNCTION", 7}, // k_mount_tilt + { "SERVO16_REVERSED", 1}, +#else + { "BARO_PROBE_EXT", 768}, + { "SERVO9_FUNCTION", 59}, // k_rcin9, lights 1 + { "SERVO10_FUNCTION", 7}, // k_mount_tilt +#endif +}; diff --git a/ArduSub/RC_Channel.h b/ArduSub/RC_Channel.h index 7b7139a1e946b..e7ee787b50935 100644 --- a/ArduSub/RC_Channel.h +++ b/ArduSub/RC_Channel.h @@ -25,6 +25,9 @@ class RC_Channels_Sub : public RC_Channels return &obj_channels[chan]; } + // tell the gimbal code all is good with RC input: + bool in_rc_failsafe() const override { return false; }; + protected: // note that these callbacks are not presently used on Plane: diff --git a/ArduSub/ReleaseNotes.txt b/ArduSub/ReleaseNotes.txt index 937999ce180a4..6d68f5216e7f5 100644 --- a/ArduSub/ReleaseNotes.txt +++ b/ArduSub/ReleaseNotes.txt @@ -704,7 +704,7 @@ Changes from 3.1.4 11) CLI removed from APM1/2 to save flash space, critical functions moved to MAVLink: a) Individual motor tests (see MP's Initial Setup > Optional Hardware > Motor Test) b) compassmot (see MP's Initial Setup > Optional Hardware > Compass/Motor Calib) - c) parameter reset to factory defautls (see MP's Config/Tuning > Full Parameter List > Reset to Default) + c) parameter reset to factory defaults (see MP's Config/Tuning > Full Parameter List > Reset to Default) ------------------------------------------------------------------ ArduCopter 3.1.5 27-May-2014 / 3.1.5-rc2 20-May-2014 Changes from 3.1.5-rc1 diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp index bd73ce41a6bf1..2a90c7f3a685e 100644 --- a/ArduSub/Sub.cpp +++ b/ArduSub/Sub.cpp @@ -25,7 +25,7 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); */ Sub::Sub() : logger(g.log_bitmask), - control_mode(MANUAL), + control_mode(Mode::Number::MANUAL), motors(MAIN_LOOP_RATE), auto_mode(Auto_WP), guided_mode(Guided_WP), @@ -37,12 +37,19 @@ Sub::Sub() wp_nav(inertial_nav, ahrs_view, pos_control, attitude_control), loiter_nav(inertial_nav, ahrs_view, pos_control, attitude_control), circle_nav(inertial_nav, ahrs_view, pos_control), - param_loader(var_info) + param_loader(var_info), + flightmode(&mode_manual) { #if CONFIG_HAL_BOARD != HAL_BOARD_SITL failsafe.pilot_input = true; #endif + if (_singleton != nullptr) { + AP_HAL::panic("Can only be one Sub"); + } + _singleton = this; } +Sub *Sub::_singleton = nullptr; + Sub sub; AP_Vehicle& vehicle = sub; diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 95205f84b260f..482aabfc38892 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -71,6 +71,8 @@ #include "Parameters.h" #include "AP_Arming_Sub.h" #include "GCS_Sub.h" +#include "mode.h" +#include "script_button.h" #include // Optical Flow library @@ -108,6 +110,17 @@ class Sub : public AP_Vehicle { friend class ParametersG2; friend class AP_Arming_Sub; friend class RC_Channels_Sub; + friend class Mode; + friend class ModeManual; + friend class ModeStabilize; + friend class ModeAcro; + friend class ModeAlthold; + friend class ModeGuided; + friend class ModePoshold; + friend class ModeAuto; + friend class ModeCircle; + friend class ModeSurface; + friend class ModeMotordetect; Sub(void); @@ -190,9 +203,9 @@ class Sub : public AP_Vehicle { // This is the state of the flight control system // There are multiple states defined such as STABILIZE, ACRO, - control_mode_t control_mode; + Mode::Number control_mode; - control_mode_t prev_control_mode; + Mode::Number prev_control_mode; #if RCMAP_ENABLED == ENABLED RCMapper rcmap; @@ -234,12 +247,6 @@ class Sub : public AP_Vehicle { AP_Motors6DOF motors; - // Auto - AutoMode auto_mode; // controls which auto controller is run - - // Guided - GuidedMode guided_mode; // controls which controller is run (pos or vel) - // Circle bool circle_pilot_yaw_override; // true if pilot is overriding yaw @@ -289,6 +296,9 @@ class Sub : public AP_Vehicle { // Navigation Yaw control // auto flight mode's yaw mode uint8_t auto_yaw_mode; + + // Parameter to set yaw rate only + bool yaw_rate_only; // Yaw will point at this location if auto_yaw_mode is set to AUTO_YAW_ROI Vector3f roi_WP; @@ -416,61 +426,7 @@ class Sub : public AP_Vehicle { bool verify_wait_delay(); bool verify_within_distance(); bool verify_yaw(); - bool acro_init(void); - void acro_run(); - void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out); - bool althold_init(void); - void althold_run(); - bool auto_init(void); - void auto_run(); - void auto_wp_start(const Vector3f& destination); - void auto_wp_start(const Location& dest_loc); - void auto_wp_run(); - void auto_circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn); - void auto_circle_start(); - void auto_circle_run(); - void auto_nav_guided_start(); - void auto_nav_guided_run(); - bool auto_loiter_start(); - void auto_loiter_run(); - uint8_t get_default_auto_yaw_mode(bool rtl) const; - void set_auto_yaw_mode(uint8_t yaw_mode); - void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle); - void set_auto_yaw_roi(const Location &roi_location); - float get_auto_heading(void); - bool circle_init(void); - void circle_run(); - bool guided_init(bool ignore_checks = false); - void guided_pos_control_start(); - void guided_vel_control_start(); - void guided_posvel_control_start(); - void guided_angle_control_start(); - bool guided_set_destination(const Vector3f& destination); - bool guided_set_destination(const Location& dest_loc); - void guided_set_velocity(const Vector3f& velocity); - bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity); - void guided_set_angle(const Quaternion &q, float climb_rate_cms); - void guided_run(); - void guided_pos_control_run(); - void guided_vel_control_run(); - void guided_posvel_control_run(); - void guided_angle_control_run(); - void guided_limit_clear(); - void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm); - void guided_limit_init_time_and_pos(); - bool guided_limit_check(); - - bool poshold_init(void); - void poshold_run(); - - bool motordetect_init(); - void motordetect_run(); - - bool stabilize_init(void); - void stabilize_run(); - void control_depth(); - bool manual_init(void); - void manual_run(); + void failsafe_sensors_check(void); void failsafe_crash_check(); void failsafe_ekf_check(void); @@ -484,15 +440,12 @@ class Sub : public AP_Vehicle { void mainloop_failsafe_enable(); void mainloop_failsafe_disable(); void fence_check(); - bool set_mode(control_mode_t mode, ModeReason reason); - bool set_mode(const uint8_t mode, const ModeReason reason) override; + bool set_mode(Mode::Number mode, ModeReason reason); + bool set_mode(const uint8_t new_mode, const ModeReason reason) override; uint8_t get_mode() const override { return (uint8_t)control_mode; } void update_flight_mode(); - void exit_mode(control_mode_t old_control_mode, control_mode_t new_control_mode); - bool mode_requires_GPS(control_mode_t mode); - bool mode_has_manual_throttle(control_mode_t mode); - bool mode_allows_arming(control_mode_t mode, bool arming_from_gcs); - void notify_flight_mode(control_mode_t mode); + void exit_mode(Mode::Number old_control_mode, Mode::Number new_control_mode); + void notify_flight_mode(); void read_inertia(); void update_surface_and_bottom_detector(); void set_surfaced(bool at_surface); @@ -502,7 +455,15 @@ class Sub : public AP_Vehicle { void init_rc_out(); void enable_motor_output(); void init_joystick(); - void transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons); + void transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons, uint16_t buttons2, uint8_t enabled_extensions, + int16_t s, + int16_t t, + int16_t aux1, + int16_t aux2, + int16_t aux3, + int16_t aux4, + int16_t aux5, + int16_t aux6); void handle_jsbutton_press(uint8_t button,bool shift=false,bool held=false); void handle_jsbutton_release(uint8_t button, bool shift); JSButton* get_button(uint8_t index); @@ -563,8 +524,7 @@ class Sub : public AP_Vehicle { void failsafe_internal_temperature_check(); void failsafe_terrain_act(void); - bool auto_terrain_recover_start(void); - void auto_terrain_recover_run(void); + void translate_wpnav_rp(float &lateral_out, float &forward_out); void translate_circle_nav_rp(float &lateral_out, float &forward_out); @@ -573,10 +533,12 @@ class Sub : public AP_Vehicle { bool surface_init(void); void surface_run(); + void stats_update(); + uint16_t get_pilot_speed_dn() const; void convert_old_parameters(void); - bool handle_do_motor_test(mavlink_command_long_t command); + bool handle_do_motor_test(mavlink_command_int_t command); bool init_motor_test(); bool verify_motor_test(); @@ -608,9 +570,45 @@ class Sub : public AP_Vehicle { static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1, "_failsafe_priorities is missing the sentinel"); + Mode *mode_from_mode_num(const Mode::Number num); + void exit_mode(Mode *&old_flightmode, Mode *&new_flightmode); + + Mode *flightmode; + ModeManual mode_manual; + ModeStabilize mode_stabilize; + ModeAcro mode_acro; + ModeAlthold mode_althold; + ModeAuto mode_auto; + ModeGuided mode_guided; + ModePoshold mode_poshold; + ModeCircle mode_circle; + ModeSurface mode_surface; + ModeMotordetect mode_motordetect; + + // Auto + AutoSubMode auto_mode; // controls which auto controller is run + GuidedSubMode guided_mode; + +#if AP_SCRIPTING_ENABLED + ScriptButton script_buttons[4]; +#endif // AP_SCRIPTING_ENABLED public: void mainloop_failsafe_check(); + + static Sub *_singleton; + + static Sub *get_singleton() { + return _singleton; + } + +#if AP_SCRIPTING_ENABLED + // For Lua scripting, so index is 1..4, not 0..3 + bool is_button_pressed(uint8_t index); + + // For Lua scripting, so index is 1..4, not 0..3 + uint8_t get_and_clear_button_count(uint8_t index); +#endif // AP_SCRIPTING_ENABLED }; extern const AP_HAL::HAL& hal; diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index 855f8b0adf686..681022a3dd93b 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -124,7 +124,7 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd) // called by mission library in mission.update() bool Sub::verify_command_callback(const AP_Mission::Mission_Command& cmd) { - if (control_mode == AUTO) { + if (control_mode == Mode::Number::AUTO) { bool cmd_complete = verify_command(cmd); // send message to GCS @@ -207,8 +207,8 @@ void Sub::exit_mission() AP_Notify::events.mission_complete = 1; // Try to enter loiter, if that fails, go to depth hold - if (!auto_loiter_start()) { - set_mode(ALT_HOLD, ModeReason::MISSION_END); + if (!mode_auto.auto_loiter_start()) { + set_mode(Mode::Number::ALT_HOLD, ModeReason::MISSION_END); } } @@ -243,7 +243,7 @@ void Sub::do_nav_wp(const AP_Mission::Mission_Command& cmd) loiter_time_max = cmd.p1; // Set wp navigation target - auto_wp_start(target_loc); + mode_auto.auto_wp_start(target_loc); } // do_surface - initiate surface procedure @@ -279,12 +279,12 @@ void Sub::do_surface(const AP_Mission::Mission_Command& cmd) } // Go to wp location - auto_wp_start(target_location); + mode_auto.auto_wp_start(target_location); } void Sub::do_RTL() { - auto_wp_start(ahrs.get_home()); + mode_auto.auto_wp_start(ahrs.get_home()); } // do_loiter_unlimited - start loitering with no end conditions @@ -323,7 +323,7 @@ void Sub::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) } // start way point navigator and provide it the desired location - auto_wp_start(target_loc); + mode_auto.auto_wp_start(target_loc); } // do_circle - initiate moving in a circle @@ -362,7 +362,7 @@ void Sub::do_circle(const AP_Mission::Mission_Command& cmd) const bool circle_direction_ccw = cmd.content.location.loiter_ccw; // move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge - auto_circle_movetoedge_start(circle_center, circle_radius_m, circle_direction_ccw); + mode_auto.auto_circle_movetoedge_start(circle_center, circle_radius_m, circle_direction_ccw); } // do_loiter_time - initiate loitering at a point for a given time period @@ -383,10 +383,10 @@ void Sub::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd) { if (cmd.p1 > 0) { // initialise guided limits - guided_limit_init_time_and_pos(); + mode_auto.guided_limit_init_time_and_pos(); // set navigation target - auto_nav_guided_start(); + mode_auto.auto_nav_guided_start(); } } #endif // NAV_GUIDED @@ -401,7 +401,11 @@ void Sub::do_nav_delay(const AP_Mission::Mission_Command& cmd) nav_delay_time_max_ms = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds } else { // absolute delay to utc time +#if AP_RTC_ENABLED nav_delay_time_max_ms = AP::rtc().get_time_utc(cmd.content.nav_delay.hour_utc, cmd.content.nav_delay.min_utc, cmd.content.nav_delay.sec_utc, 0); +#else + nav_delay_time_max_ms = 0; +#endif } gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec", (unsigned)(nav_delay_time_max_ms/1000)); } @@ -410,7 +414,7 @@ void Sub::do_nav_delay(const AP_Mission::Mission_Command& cmd) // do_guided_limits - pass guided limits to guided controller void Sub::do_guided_limits(const AP_Mission::Mission_Command& cmd) { - guided_limit_set(cmd.p1 * 1000, // convert seconds to ms + mode_guided.guided_limit_set(cmd.p1 * 1000, // convert seconds to ms cmd.content.guided_limits.alt_min * 100.0f, // convert meters to cm cmd.content.guided_limits.alt_max * 100.0f, // convert meters to cm cmd.content.guided_limits.horiz_max * 100.0f); // convert meters to cm @@ -459,7 +463,7 @@ bool Sub::verify_surface(const AP_Mission::Mission_Command& cmd) // TODO get xy target from current wp destination, because current location may be acceptance-radius away from original destination Location target_location(cmd.content.location.lat, cmd.content.location.lng, 0, Location::AltFrame::ABOVE_HOME); - auto_wp_start(target_location); + mode_auto.auto_wp_start(target_location); // advance to next state auto_surface_state = AUTO_SURFACE_STATE_ASCEND; @@ -529,13 +533,13 @@ bool Sub::verify_circle(const AP_Mission::Mission_Command& cmd) } // start circling - auto_circle_start(); + mode_auto.auto_circle_start(); } return false; } // check if we have completed circling - return fabsf(circle_nav.get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1); + return fabsf(sub.circle_nav.get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1); } #if NAV_GUIDED == ENABLED @@ -548,7 +552,7 @@ bool Sub::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) } // check time and position limits - return guided_limit_check(); + return mode_auto.guided_limit_check(); } #endif // NAV_GUIDED @@ -579,7 +583,7 @@ void Sub::do_within_distance(const AP_Mission::Mission_Command& cmd) void Sub::do_yaw(const AP_Mission::Mission_Command& cmd) { - set_auto_yaw_look_at_heading( + sub.mode_auto.set_auto_yaw_look_at_heading( cmd.content.yaw.angle_deg, cmd.content.yaw.turn_rate_dps, cmd.content.yaw.direction, @@ -614,7 +618,7 @@ bool Sub::verify_yaw() { // set yaw mode if it has been changed (the waypoint controller often retakes control of yaw as it executes a new waypoint command) if (auto_yaw_mode != AUTO_YAW_LOOK_AT_HEADING) { - set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING); + sub.mode_auto.set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING); } // check if we are within 2 degrees of the target heading @@ -629,7 +633,7 @@ bool Sub::verify_yaw() bool Sub::do_guided(const AP_Mission::Mission_Command& cmd) { // only process guided waypoint if we are in guided mode - if (control_mode != GUIDED && !(control_mode == AUTO && auto_mode == Auto_NavGuided)) { + if (control_mode != Mode::Number::GUIDED && !(control_mode == Mode::Number::AUTO && auto_mode == Auto_NavGuided)) { return false; } @@ -638,7 +642,7 @@ bool Sub::do_guided(const AP_Mission::Mission_Command& cmd) case MAV_CMD_NAV_WAYPOINT: { // set wp_nav's destination - return guided_set_destination(cmd.content.location); + return sub.mode_guided.guided_set_destination(cmd.content.location); } case MAV_CMD_CONDITION_YAW: @@ -681,7 +685,7 @@ void Sub::do_set_home(const AP_Mission::Mission_Command& cmd) // TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint void Sub::do_roi(const AP_Mission::Mission_Command& cmd) { - set_auto_yaw_roi(cmd.content.location); + sub.mode_auto.set_auto_yaw_roi(cmd.content.location); } // point the camera to a specified angle diff --git a/ArduSub/config.h b/ArduSub/config.h index d09d7e351c129..4a517afe13adb 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -190,6 +190,12 @@ # define LOGGING_ENABLED ENABLED #endif +// Statistics +#ifndef STATS_ENABLED + # define STATS_ENABLED (AP_STATS_ENABLED ? ENABLED : DISABLED) +#endif + + // Default logging bitmask #ifndef DEFAULT_LOG_BITMASK # define DEFAULT_LOG_BITMASK \ diff --git a/ArduSub/control_acro.cpp b/ArduSub/control_acro.cpp deleted file mode 100644 index 959ad69f8e811..0000000000000 --- a/ArduSub/control_acro.cpp +++ /dev/null @@ -1,162 +0,0 @@ -#include "Sub.h" - - -/* - * control_acro.pde - init and run calls for acro flight mode - */ - -// acro_init - initialise acro controller -bool Sub::acro_init() -{ - // set target altitude to zero for reporting - pos_control.set_pos_target_z_cm(0); - - // attitude hold inputs become thrust inputs in acro mode - // set to neutral to prevent chaotic behavior (esp. roll/pitch) - set_neutral_controls(); - - return true; -} - -// acro_run - runs the acro controller -// should be called at 100hz or more -void Sub::acro_run() -{ - float target_roll, target_pitch, target_yaw; - - // if not armed set throttle to zero and exit immediately - if (!motors.armed()) { - 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(); - return; - } - - motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - - // convert the input to the desired body frame rate - get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw); - - // run attitude controller - attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); - - // output pilot's throttle without angle boost - attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt); - - //control_in is range 0-1000 - //radio_in is raw pwm value - motors.set_forward(channel_forward->norm_input()); - motors.set_lateral(channel_lateral->norm_input()); -} - - -// get_pilot_desired_angle_rates - transform pilot's roll pitch and yaw input into a desired lean angle rates -// returns desired angle rates in centi-degrees-per-second -void Sub::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out) -{ - float rate_limit; - Vector3f rate_ef_level, rate_bf_level, rate_bf_request; - - // apply circular limit to pitch and roll inputs - float total_in = norm(pitch_in, roll_in); - - if (total_in > ROLL_PITCH_INPUT_MAX) { - float ratio = (float)ROLL_PITCH_INPUT_MAX / total_in; - roll_in *= ratio; - pitch_in *= ratio; - } - - // calculate roll, pitch rate requests - if (g.acro_expo <= 0) { - rate_bf_request.x = roll_in * g.acro_rp_p; - rate_bf_request.y = pitch_in * g.acro_rp_p; - } else { - // expo variables - float rp_in, rp_in3, rp_out; - - // range check expo - if (g.acro_expo > 1.0f) { - g.acro_expo.set(1.0f); - } - - // roll expo - rp_in = float(roll_in)/ROLL_PITCH_INPUT_MAX; - rp_in3 = rp_in*rp_in*rp_in; - rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in); - rate_bf_request.x = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p; - - // pitch expo - rp_in = float(pitch_in)/ROLL_PITCH_INPUT_MAX; - rp_in3 = rp_in*rp_in*rp_in; - rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in); - rate_bf_request.y = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p; - } - - // calculate yaw rate request - rate_bf_request.z = yaw_in * g.acro_yaw_p; - - // calculate earth frame rate corrections to pull the vehicle back to level while in ACRO mode - - if (g.acro_trainer != ACRO_TRAINER_DISABLED) { - // Calculate trainer mode earth frame rate command for roll - int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor); - rate_ef_level.x = -constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll; - - // Calculate trainer mode earth frame rate command for pitch - int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor); - rate_ef_level.y = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch; - - // Calculate trainer mode earth frame rate command for yaw - rate_ef_level.z = 0; - - // Calculate angle limiting earth frame rate commands - if (g.acro_trainer == ACRO_TRAINER_LIMITED) { - if (roll_angle > aparm.angle_max) { - rate_ef_level.x -= g.acro_balance_roll*(roll_angle-aparm.angle_max); - } else if (roll_angle < -aparm.angle_max) { - rate_ef_level.x -= g.acro_balance_roll*(roll_angle+aparm.angle_max); - } - - if (pitch_angle > aparm.angle_max) { - rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle-aparm.angle_max); - } else if (pitch_angle < -aparm.angle_max) { - rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle+aparm.angle_max); - } - } - - // convert earth-frame level rates to body-frame level rates - attitude_control.euler_rate_to_ang_vel(attitude_control.get_att_target_euler_cd()*radians(0.01f), rate_ef_level, rate_bf_level); - - // combine earth frame rate corrections with rate requests - if (g.acro_trainer == ACRO_TRAINER_LIMITED) { - rate_bf_request.x += rate_bf_level.x; - rate_bf_request.y += rate_bf_level.y; - rate_bf_request.z += rate_bf_level.z; - } else { - float acro_level_mix = constrain_float(1-MAX(MAX(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch(); - - // Scale leveling rates by stick input - rate_bf_level = rate_bf_level*acro_level_mix; - - // Calculate rate limit to prevent change of rate through inverted - rate_limit = fabsf(fabsf(rate_bf_request.x)-fabsf(rate_bf_level.x)); - rate_bf_request.x += rate_bf_level.x; - rate_bf_request.x = constrain_float(rate_bf_request.x, -rate_limit, rate_limit); - - // Calculate rate limit to prevent change of rate through inverted - rate_limit = fabsf(fabsf(rate_bf_request.y)-fabsf(rate_bf_level.y)); - rate_bf_request.y += rate_bf_level.y; - rate_bf_request.y = constrain_float(rate_bf_request.y, -rate_limit, rate_limit); - - // Calculate rate limit to prevent change of rate through inverted - rate_limit = fabsf(fabsf(rate_bf_request.z)-fabsf(rate_bf_level.z)); - rate_bf_request.z += rate_bf_level.z; - rate_bf_request.z = constrain_float(rate_bf_request.z, -rate_limit, rate_limit); - } - } - - // hand back rate request - roll_out = rate_bf_request.x; - pitch_out = rate_bf_request.y; - yaw_out = rate_bf_request.z; -} diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp deleted file mode 100644 index a420cfe3149ea..0000000000000 --- a/ArduSub/control_althold.cpp +++ /dev/null @@ -1,120 +0,0 @@ -#include "Sub.h" - - -/* - * control_althold.pde - init and run calls for althold, flight mode - */ - -// althold_init - initialise althold controller -bool Sub::althold_init() -{ - if(!control_check_barometer()) { - return false; - } - - // initialize vertical maximum speeds and acceleration - // sets the maximum speed up and down returned by position controller - pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - - // initialise position and desired velocity - pos_control.init_z_controller(); - - last_pilot_heading = ahrs.yaw_sensor; - - return true; -} - -// althold_run - runs the althold controller -// should be called at 100hz or more -void Sub::althold_run() -{ - uint32_t tnow = AP_HAL::millis(); - - // initialize vertical speeds and acceleration - pos_control.set_max_speed_accel_z(-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(); - pos_control.relax_z_controller(motors.get_throttle_hover()); - last_pilot_heading = ahrs.yaw_sensor; - return; - } - - motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - - // get pilot desired lean angles - float target_roll, target_pitch; - - // Check if set_attitude_target_no_gps is valid - if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) { - float target_yaw; - Quaternion( - set_attitude_target_no_gps.packet.q - ).to_euler( - target_roll, - target_pitch, - target_yaw - ); - target_roll = degrees(target_roll); - target_pitch = degrees(target_pitch); - target_yaw = degrees(target_yaw); - - attitude_control.input_euler_angle_roll_pitch_yaw(target_roll * 1e2f, target_pitch * 1e2f, target_yaw * 1e2f, true); - return; - } - - get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max_cd()); - - // get pilot's desired yaw rate - float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); - - // 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(target_roll, target_pitch, target_yaw_rate); - last_pilot_heading = ahrs.yaw_sensor; - last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading - - } else { // hold current heading - - // this check is required to prevent bounce back after very fast yaw maneuvers - // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped - if (tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading - target_yaw_rate = 0; // Stop rotation on yaw axis - - // call attitude controller with target yaw rate = 0 to decelerate on yaw axis - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); - last_pilot_heading = ahrs.yaw_sensor; // update heading to hold - - } else { // call attitude controller holding absolute absolute bearing - attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true); - } - } - - control_depth(); - - motors.set_forward(channel_forward->norm_input()); - motors.set_lateral(channel_lateral->norm_input()); -} - -void Sub::control_depth() { - float target_climb_rate_cm_s = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); - target_climb_rate_cm_s = constrain_float(target_climb_rate_cm_s, -get_pilot_speed_dn(), g.pilot_speed_up); - - // desired_climb_rate returns 0 when within the deadzone. - //we allow full control to the pilot, but as soon as there's no input, we handle being at surface/bottom - if (fabsf(target_climb_rate_cm_s) < 0.05f) { - if (ap.at_surface) { - pos_control.set_pos_target_z_cm(MIN(pos_control.get_pos_target_z_cm(), g.surface_depth - 5.0f)); // set target to 5 cm below surface level - } else if (ap.at_bottom) { - pos_control.set_pos_target_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, pos_control.get_pos_target_z_cm())); // set target to 10 cm above bottom - } - } - - pos_control.set_pos_target_z_from_climb_rate_cm(target_climb_rate_cm_s); - pos_control.update_z_controller(); - -} diff --git a/ArduSub/control_circle.cpp b/ArduSub/control_circle.cpp deleted file mode 100644 index fca637e9bf8f3..0000000000000 --- a/ArduSub/control_circle.cpp +++ /dev/null @@ -1,86 +0,0 @@ -#include "Sub.h" - -/* - * control_circle.pde - init and run calls for circle flight mode - */ - -// circle_init - initialise circle controller flight mode -bool Sub::circle_init() -{ - if (!position_ok()) { - return false; - } - - circle_pilot_yaw_override = false; - - // initialize speeds and accelerations - pos_control.set_max_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration()); - pos_control.set_correction_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration()); - pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - - // initialise circle controller including setting the circle center based on vehicle speed - circle_nav.init(); - - return true; -} - -// circle_run - runs the circle flight mode -// should be called at 100hz or more -void Sub::circle_run() -{ - float target_yaw_rate = 0; - float target_climb_rate = 0; - - // update parameters, to allow changing at runtime - pos_control.set_max_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration()); - pos_control.set_max_speed_accel_z(-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()) { - // To-Do: add some initialisation of position controllers - motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); - // Sub vehicles do not stabilize roll/pitch/yaw when disarmed - attitude_control.set_throttle_out(0,true,g.throttle_filt); - attitude_control.relax_attitude_controllers(); - circle_nav.init(); - return; - } - - // process pilot inputs - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); - if (!is_zero(target_yaw_rate)) { - circle_pilot_yaw_override = true; - } - - // get pilot desired climb rate - target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); - - // set motors to full range - motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - - // run circle controller - failsafe_terrain_set_status(circle_nav.update()); - - /////////////////////// - // update xy outputs // - - float lateral_out, forward_out; - translate_circle_nav_rp(lateral_out, forward_out); - - // Send to forward/lateral outputs - motors.set_lateral(lateral_out); - motors.set_forward(forward_out); - - // call attitude controller - if (circle_pilot_yaw_override) { - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); - } else { - attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), circle_nav.get_yaw(), true); - } - - // update altitude target and call position controller - pos_control.set_pos_target_z_from_climb_rate_cm(target_climb_rate); - pos_control.update_z_controller(); -} diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp deleted file mode 100644 index 3c13e04b23118..0000000000000 --- a/ArduSub/control_guided.cpp +++ /dev/null @@ -1,563 +0,0 @@ -#include "Sub.h" - -/* - * Init and run calls for guided flight mode - */ - -#define GUIDED_POSVEL_TIMEOUT_MS 3000 // guided mode's position-velocity controller times out after 3seconds with no new updates -#define GUIDED_ATTITUDE_TIMEOUT_MS 1000 // guided mode's attitude controller times out after 1 second with no new updates - -static Vector3p posvel_pos_target_cm; -static Vector3f posvel_vel_target_cms; -static uint32_t update_time_ms; - -struct { - uint32_t update_time_ms; - float roll_cd; - float pitch_cd; - float yaw_cd; - float climb_rate_cms; -} static guided_angle_state = {0,0.0f, 0.0f, 0.0f, 0.0f}; - -struct Guided_Limit { - uint32_t timeout_ms; // timeout (in seconds) from the time that guided is invoked - float alt_min_cm; // lower altitude limit in cm above home (0 = no limit) - float alt_max_cm; // upper altitude limit in cm above home (0 = no limit) - float horiz_max_cm; // horizontal position limit in cm from where guided mode was initiated (0 = no limit) - uint32_t start_time;// system time in milliseconds that control was handed to the external computer - Vector3f start_pos; // start position as a distance from home in cm. used for checking horiz_max limit -} guided_limit; - -// guided_init - initialise guided controller -bool Sub::guided_init(bool ignore_checks) -{ - if (!position_ok() && !ignore_checks) { - return false; - } - - // start in position control mode - guided_pos_control_start(); - return true; -} - -// initialise guided mode's position controller -void Sub::guided_pos_control_start() -{ - // set to position control mode - guided_mode = Guided_WP; - - // initialise waypoint controller - wp_nav.wp_and_spline_init(); - - // initialise wpnav to stopping point at current altitude - // To-Do: set to current location if disarmed? - // To-Do: set to stopping point altitude? - Vector3f stopping_point; - wp_nav.get_wp_stopping_point(stopping_point); - - // no need to check return status because terrain data is not used - wp_nav.set_wp_destination(stopping_point, false); - - // initialise yaw - set_auto_yaw_mode(get_default_auto_yaw_mode(false)); -} - -// initialise guided mode's velocity controller -void Sub::guided_vel_control_start() -{ - // set guided_mode to velocity controller - guided_mode = Guided_Velocity; - - // initialize vertical maximum speeds and acceleration - pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - - // initialise velocity controller - pos_control.init_z_controller(); - pos_control.init_xy_controller(); -} - -// initialise guided mode's posvel controller -void Sub::guided_posvel_control_start() -{ - // set guided_mode to velocity controller - guided_mode = Guided_PosVel; - - // set vertical speed and acceleration - pos_control.set_max_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); - pos_control.set_correction_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); - - // initialise velocity controller - pos_control.init_z_controller(); - pos_control.init_xy_controller(); - - // pilot always controls yaw - set_auto_yaw_mode(AUTO_YAW_HOLD); -} - -// initialise guided mode's angle controller -void Sub::guided_angle_control_start() -{ - // set guided_mode to velocity controller - guided_mode = Guided_Angle; - - // set vertical speed and acceleration - pos_control.set_max_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); - pos_control.set_correction_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); - - // initialise velocity controller - pos_control.init_z_controller(); - - // initialise targets - guided_angle_state.update_time_ms = AP_HAL::millis(); - guided_angle_state.roll_cd = ahrs.roll_sensor; - guided_angle_state.pitch_cd = ahrs.pitch_sensor; - guided_angle_state.yaw_cd = ahrs.yaw_sensor; - guided_angle_state.climb_rate_cms = 0.0f; - - // pilot always controls yaw - set_auto_yaw_mode(AUTO_YAW_HOLD); -} - -// guided_set_destination - sets guided mode's target destination -// 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 Sub::guided_set_destination(const Vector3f& destination) -{ - // ensure we are in position control mode - if (guided_mode != Guided_WP) { - guided_pos_control_start(); - } - -#if AP_FENCE_ENABLED - // reject destination if outside the fence - const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN); - if (!fence.check_destination_within_fence(dest_loc)) { - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); - // failure is propagated to GCS with NAK - return false; - } -#endif - - // no need to check return status because terrain data is not used - wp_nav.set_wp_destination(destination, false); - - // log target - Log_Write_GuidedTarget(guided_mode, destination, Vector3f()); - return true; -} - -// sets guided mode's target from a Location object -// returns false if destination could not be set (probably caused by missing terrain data) -// or if the fence is enabled and guided waypoint is outside the fence -bool Sub::guided_set_destination(const Location& dest_loc) -{ - // ensure we are in position control mode - if (guided_mode != Guided_WP) { - guided_pos_control_start(); - } - -#if AP_FENCE_ENABLED - // reject destination outside the fence. - // Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails - if (!fence.check_destination_within_fence(dest_loc)) { - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); - // failure is propagated to GCS with NAK - return false; - } -#endif - - if (!wp_nav.set_wp_destination_loc(dest_loc)) { - // failure to set destination can only be because of missing terrain data - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION); - // failure is propagated to GCS with NAK - return false; - } - - // log target - Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f()); - return true; -} - -// guided_set_velocity - sets guided mode's target velocity -void Sub::guided_set_velocity(const Vector3f& velocity) -{ - // check we are in velocity control mode - if (guided_mode != Guided_Velocity) { - guided_vel_control_start(); - } - - update_time_ms = AP_HAL::millis(); - - // set position controller velocity target - pos_control.set_vel_desired_cms(velocity); -} - -// set guided mode posvel target -bool Sub::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity) -{ - // check we are in velocity control mode - if (guided_mode != Guided_PosVel) { - guided_posvel_control_start(); - } - -#if AP_FENCE_ENABLED - // reject destination if outside the fence - const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN); - if (!fence.check_destination_within_fence(dest_loc)) { - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); - // failure is propagated to GCS with NAK - return false; - } -#endif - - update_time_ms = AP_HAL::millis(); - posvel_pos_target_cm = destination.topostype(); - posvel_vel_target_cms = velocity; - - pos_control.input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f()); - float dz = posvel_pos_target_cm.z; - pos_control.input_pos_vel_accel_z(dz, posvel_vel_target_cms.z, 0); - posvel_pos_target_cm.z = dz; - - // log target - Log_Write_GuidedTarget(guided_mode, destination, velocity); - return true; -} - -// set guided mode angle target -void Sub::guided_set_angle(const Quaternion &q, float climb_rate_cms) -{ - // check we are in velocity control mode - if (guided_mode != Guided_Angle) { - guided_angle_control_start(); - } - - // convert quaternion to euler angles - q.to_euler(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd); - guided_angle_state.roll_cd = ToDeg(guided_angle_state.roll_cd) * 100.0f; - guided_angle_state.pitch_cd = ToDeg(guided_angle_state.pitch_cd) * 100.0f; - guided_angle_state.yaw_cd = wrap_180_cd(ToDeg(guided_angle_state.yaw_cd) * 100.0f); - - guided_angle_state.climb_rate_cms = climb_rate_cms; - guided_angle_state.update_time_ms = AP_HAL::millis(); -} - -// guided_run - runs the guided controller -// should be called at 100hz or more -void Sub::guided_run() -{ - // call the correct auto controller - switch (guided_mode) { - - case Guided_WP: - // run position controller - guided_pos_control_run(); - break; - - case Guided_Velocity: - // run velocity controller - guided_vel_control_run(); - break; - - case Guided_PosVel: - // run position-velocity controller - guided_posvel_control_run(); - break; - - case Guided_Angle: - // run angle controller - guided_angle_control_run(); - break; - } -} - -// guided_pos_control_run - runs the guided position controller -// called from guided_run -void Sub::guided_pos_control_run() -{ - // if motors not enabled set throttle to zero and exit immediately - if (!motors.armed()) { - motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); - // Sub vehicles do not stabilize roll/pitch/yaw when disarmed - attitude_control.set_throttle_out(0,true,g.throttle_filt); - attitude_control.relax_attitude_controllers(); - wp_nav.wp_and_spline_init(); - return; - } - - // process pilot's yaw input - float target_yaw_rate = 0; - if (!failsafe.pilot_input) { - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); - if (!is_zero(target_yaw_rate)) { - set_auto_yaw_mode(AUTO_YAW_HOLD); - } - } - - // set motors to full range - motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - - // run waypoint controller - failsafe_terrain_set_status(wp_nav.update_wpnav()); - - float lateral_out, forward_out; - translate_wpnav_rp(lateral_out, forward_out); - - // Send to forward/lateral outputs - motors.set_lateral(lateral_out); - motors.set_forward(forward_out); - - // WP_Nav has set the vertical position control targets - // run the vertical position controller and set output throttle - pos_control.update_z_controller(); - - // call attitude controller - if (auto_yaw_mode == AUTO_YAW_HOLD) { - // roll & pitch & yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); - } else { - // roll, pitch from pilot, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true); - } -} - -// guided_vel_control_run - runs the guided velocity controller -// called from guided_run -void Sub::guided_vel_control_run() -{ - // ifmotors not enabled set throttle to zero and exit immediately - if (!motors.armed()) { - motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); - // Sub vehicles do not stabilize roll/pitch/yaw when disarmed - attitude_control.set_throttle_out(0,true,g.throttle_filt); - attitude_control.relax_attitude_controllers(); - // initialise velocity controller - pos_control.init_z_controller(); - pos_control.init_xy_controller(); - return; - } - - // process pilot's yaw input - float target_yaw_rate = 0; - if (!failsafe.pilot_input) { - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); - if (!is_zero(target_yaw_rate)) { - set_auto_yaw_mode(AUTO_YAW_HOLD); - } - } - - // set motors to full range - motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - - // set velocity to zero if no updates received for 3 seconds - uint32_t tnow = AP_HAL::millis(); - if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control.get_vel_desired_cms().is_zero()) { - pos_control.set_vel_desired_cms(Vector3f(0,0,0)); - } - - pos_control.stop_pos_xy_stabilisation(); - // call velocity controller which includes z axis controller - pos_control.update_xy_controller(); - pos_control.update_z_controller(); - - float lateral_out, forward_out; - translate_pos_control_rp(lateral_out, forward_out); - - // Send to forward/lateral outputs - motors.set_lateral(lateral_out); - motors.set_forward(forward_out); - - // call attitude controller - if (auto_yaw_mode == AUTO_YAW_HOLD) { - // roll & pitch & yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); - } else { - // roll, pitch from pilot, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true); - } -} - -// guided_posvel_control_run - runs the guided posvel controller -// called from guided_run -void Sub::guided_posvel_control_run() -{ - // if motors not enabled set throttle to zero and exit immediately - if (!motors.armed()) { - motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); - // Sub vehicles do not stabilize roll/pitch/yaw when disarmed - attitude_control.set_throttle_out(0,true,g.throttle_filt); - attitude_control.relax_attitude_controllers(); - // initialise velocity controller - pos_control.init_z_controller(); - pos_control.init_xy_controller(); - return; - } - - // process pilot's yaw input - float target_yaw_rate = 0; - - if (!failsafe.pilot_input) { - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); - if (!is_zero(target_yaw_rate)) { - set_auto_yaw_mode(AUTO_YAW_HOLD); - } - } - - // set motors to full range - motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - - // set velocity to zero if no updates received for 3 seconds - uint32_t tnow = AP_HAL::millis(); - if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !posvel_vel_target_cms.is_zero()) { - posvel_vel_target_cms.zero(); - } - - // advance position target using velocity target - posvel_pos_target_cm += (posvel_vel_target_cms * pos_control.get_dt()).topostype(); - - // send position and velocity targets to position controller - pos_control.input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f()); - float pz = posvel_pos_target_cm.z; - pos_control.input_pos_vel_accel_z(pz, posvel_vel_target_cms.z, 0); - posvel_pos_target_cm.z = pz; - - // run position controller - pos_control.update_xy_controller(); - pos_control.update_z_controller(); - - float lateral_out, forward_out; - translate_pos_control_rp(lateral_out, forward_out); - - // Send to forward/lateral outputs - motors.set_lateral(lateral_out); - motors.set_forward(forward_out); - - // call attitude controller - if (auto_yaw_mode == AUTO_YAW_HOLD) { - // roll & pitch & yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); - } else { - // roll, pitch from pilot, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true); - } -} - -// guided_angle_control_run - runs the guided angle controller -// called from guided_run -void Sub::guided_angle_control_run() -{ - // if motors not enabled set throttle to zero and exit immediately - if (!motors.armed()) { - motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); - // Sub vehicles do not stabilize roll/pitch/yaw when disarmed - attitude_control.set_throttle_out(0.0f,true,g.throttle_filt); - attitude_control.relax_attitude_controllers(); - // initialise velocity controller - pos_control.init_z_controller(); - return; - } - - // constrain desired lean angles - float roll_in = guided_angle_state.roll_cd; - float pitch_in = guided_angle_state.pitch_cd; - float total_in = norm(roll_in, pitch_in); - float angle_max = MIN(attitude_control.get_althold_lean_angle_max_cd(), aparm.angle_max); - if (total_in > angle_max) { - float ratio = angle_max / total_in; - roll_in *= ratio; - pitch_in *= ratio; - } - - // wrap yaw request - float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd); - - // constrain climb rate - float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up()); - - // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds - uint32_t tnow = AP_HAL::millis(); - if (tnow - guided_angle_state.update_time_ms > GUIDED_ATTITUDE_TIMEOUT_MS) { - roll_in = 0.0f; - pitch_in = 0.0f; - climb_rate_cms = 0.0f; - } - - // set motors to full range - motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - - // call attitude controller - attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true); - - // call position controller - pos_control.set_pos_target_z_from_climb_rate_cm(climb_rate_cms); - pos_control.update_z_controller(); -} - -// Guided Limit code - -// guided_limit_clear - clear/turn off guided limits -void Sub::guided_limit_clear() -{ - guided_limit.timeout_ms = 0; - guided_limit.alt_min_cm = 0.0f; - guided_limit.alt_max_cm = 0.0f; - guided_limit.horiz_max_cm = 0.0f; -} - -// guided_limit_set - set guided timeout and movement limits -void Sub::guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm) -{ - guided_limit.timeout_ms = timeout_ms; - guided_limit.alt_min_cm = alt_min_cm; - guided_limit.alt_max_cm = alt_max_cm; - guided_limit.horiz_max_cm = horiz_max_cm; -} - -// guided_limit_init_time_and_pos - initialise guided start time and position as reference for limit checking -// only called from AUTO mode's auto_nav_guided_start function -void Sub::guided_limit_init_time_and_pos() -{ - // initialise start time - guided_limit.start_time = AP_HAL::millis(); - - // initialise start position from current position - guided_limit.start_pos = inertial_nav.get_position_neu_cm(); -} - -// guided_limit_check - returns true if guided mode has breached a limit -// used when guided is invoked from the NAV_GUIDED_ENABLE mission command -bool Sub::guided_limit_check() -{ - // check if we have passed the timeout - if ((guided_limit.timeout_ms > 0) && (AP_HAL::millis() - guided_limit.start_time >= guided_limit.timeout_ms)) { - return true; - } - - // get current location - const Vector3f& curr_pos = inertial_nav.get_position_neu_cm(); - - // check if we have gone below min alt - if (!is_zero(guided_limit.alt_min_cm) && (curr_pos.z < guided_limit.alt_min_cm)) { - return true; - } - - // check if we have gone above max alt - if (!is_zero(guided_limit.alt_max_cm) && (curr_pos.z > guided_limit.alt_max_cm)) { - return true; - } - - // check if we have gone beyond horizontal limit - if (guided_limit.horiz_max_cm > 0.0f) { - const float horiz_move = get_horizontal_distance_cm(guided_limit.start_pos.xy(), curr_pos.xy()); - if (horiz_move > guided_limit.horiz_max_cm) { - return true; - } - } - - // if we got this far we must be within limits - return false; -} diff --git a/ArduSub/control_manual.cpp b/ArduSub/control_manual.cpp deleted file mode 100644 index 93e821740a12b..0000000000000 --- a/ArduSub/control_manual.cpp +++ /dev/null @@ -1,36 +0,0 @@ -#include "Sub.h" - -// manual_init - initialise manual controller -bool Sub::manual_init() -{ - // set target altitude to zero for reporting - pos_control.set_pos_target_z_cm(0); - - // attitude hold inputs become thrust inputs in manual mode - // set to neutral to prevent chaotic behavior (esp. roll/pitch) - set_neutral_controls(); - - return true; -} - -// manual_run - runs the manual (passthrough) controller -// should be called at 100hz or more -void Sub::manual_run() -{ - // if not armed set throttle to zero and exit immediately - if (!motors.armed()) { - 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(); - return; - } - - motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - - motors.set_roll(channel_roll->norm_input()); - motors.set_pitch(channel_pitch->norm_input()); - motors.set_yaw(channel_yaw->norm_input() * g.acro_yaw_p / ACRO_YAW_P); - motors.set_throttle(channel_throttle->norm_input()); - motors.set_forward(channel_forward->norm_input()); - motors.set_lateral(channel_lateral->norm_input()); -} diff --git a/ArduSub/control_poshold.cpp b/ArduSub/control_poshold.cpp deleted file mode 100644 index 46121952ae054..0000000000000 --- a/ArduSub/control_poshold.cpp +++ /dev/null @@ -1,113 +0,0 @@ -// ArduSub position hold flight mode -// GPS required -// Jacob Walser August 2016 - -#include "Sub.h" - -#if POSHOLD_ENABLED == ENABLED - -// poshold_init - initialise PosHold controller -bool Sub::poshold_init() -{ - // fail to initialise PosHold mode if no GPS lock - if (!position_ok()) { - return false; - } - - // initialize vertical speeds and acceleration - pos_control.set_max_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration()); - pos_control.set_correction_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration()); - pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - - // initialise position and desired velocity - pos_control.init_xy_controller_stopping_point(); - pos_control.init_z_controller(); - - // Stop all thrusters - attitude_control.set_throttle_out(0.5f ,true, g.throttle_filt); - attitude_control.relax_attitude_controllers(); - pos_control.relax_z_controller(0.5f); - - last_pilot_heading = ahrs.yaw_sensor; - - return true; -} - -// poshold_run - runs the PosHold controller -// should be called at 100hz or more -void Sub::poshold_run() -{ - uint32_t tnow = AP_HAL::millis(); - // When unarmed, disable motors and stabilization - 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.5f ,true, g.throttle_filt); - attitude_control.relax_attitude_controllers(); - pos_control.init_xy_controller_stopping_point(); - pos_control.relax_z_controller(0.5f); - last_pilot_heading = ahrs.yaw_sensor; - return; - } - - // set motors to full range - motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - - /////////////////////// - // update xy outputs // - float pilot_lateral = channel_lateral->norm_input(); - float pilot_forward = channel_forward->norm_input(); - - float lateral_out = 0; - float forward_out = 0; - - if (position_ok()) { - // Allow pilot to reposition the sub - if (fabsf(pilot_lateral) > 0.1 || fabsf(pilot_forward) > 0.1) { - pos_control.init_xy_controller_stopping_point(); - } - translate_pos_control_rp(lateral_out, forward_out); - pos_control.update_xy_controller(); - } else { - pos_control.init_xy_controller_stopping_point(); - } - motors.set_forward(forward_out + pilot_forward); - motors.set_lateral(lateral_out + pilot_lateral); - ///////////////////// - // Update attitude // - - // get pilot's desired yaw rate - float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); - - // convert pilot input to lean angles - // To-Do: convert get_pilot_desired_lean_angles to return angles as floats - float target_roll, target_pitch; - get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); - - // 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(target_roll, target_pitch, target_yaw_rate); - last_pilot_heading = ahrs.yaw_sensor; - last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading - - } else { // hold current heading - - // this check is required to prevent bounce back after very fast yaw maneuvers - // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped - if (tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading - target_yaw_rate = 0; // Stop rotation on yaw axis - - // call attitude controller with target yaw rate = 0 to decelerate on yaw axis - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); - last_pilot_heading = ahrs.yaw_sensor; // update heading to hold - - } else { // call attitude controller holding absolute absolute bearing - attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true); - } - } - - // Update z axis // - control_depth(); -} -#endif // POSHOLD_ENABLED == ENABLED diff --git a/ArduSub/control_stabilize.cpp b/ArduSub/control_stabilize.cpp deleted file mode 100644 index 356ce2479be1d..0000000000000 --- a/ArduSub/control_stabilize.cpp +++ /dev/null @@ -1,69 +0,0 @@ -#include "Sub.h" - -// stabilize_init - initialise stabilize controller -bool Sub::stabilize_init() -{ - // set target altitude to zero for reporting - pos_control.set_pos_target_z_cm(0); - last_pilot_heading = ahrs.yaw_sensor; - - return true; -} - -// stabilize_run - runs the main stabilize controller -// should be called at 100hz or more -void Sub::stabilize_run() -{ - uint32_t tnow = AP_HAL::millis(); - float target_roll, target_pitch; - - // if not armed set throttle to zero and exit immediately - if (!motors.armed()) { - 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(); - last_pilot_heading = ahrs.yaw_sensor; - return; - } - - motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - - // convert pilot input to lean angles - // To-Do: convert get_pilot_desired_lean_angles to return angles as floats - get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); - - // get pilot's desired yaw rate - float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); - - // call attitude controller - // 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(target_roll, target_pitch, target_yaw_rate); - last_pilot_heading = ahrs.yaw_sensor; - last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading - - } else { // hold current heading - - // this check is required to prevent bounce back after very fast yaw maneuvers - // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped - if (tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading - target_yaw_rate = 0; // Stop rotation on yaw axis - - // call attitude controller with target yaw rate = 0 to decelerate on yaw axis - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); - last_pilot_heading = ahrs.yaw_sensor; // update heading to hold - - } else { // call attitude controller holding absolute absolute bearing - attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true); - } - } - - // output pilot's throttle - attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt); - - //control_in is range -1000-1000 - //radio_in is raw pwm value - motors.set_forward(channel_forward->norm_input()); - motors.set_lateral(channel_lateral->norm_input()); -} diff --git a/ArduSub/control_surface.cpp b/ArduSub/control_surface.cpp deleted file mode 100644 index 407b04ab5a88e..0000000000000 --- a/ArduSub/control_surface.cpp +++ /dev/null @@ -1,63 +0,0 @@ -#include "Sub.h" - - -bool Sub::surface_init() -{ - if(!control_check_barometer()) { - return false; - } - - // initialize vertical speeds and acceleration - pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - - // initialise position and desired velocity - pos_control.init_z_controller(); - - return true; - -} - -void Sub::surface_run() -{ - float target_roll, target_pitch; - - // if not armed set throttle to zero and exit immediately - if (!motors.armed()) { - motors.output_min(); - 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(); - pos_control.init_z_controller(); - return; - } - - // Already at surface, hold depth at surface - if (ap.at_surface) { - set_mode(ALT_HOLD, ModeReason::SURFACE_COMPLETE); - } - - // convert pilot input to lean angles - // To-Do: convert get_pilot_desired_lean_angles to return angles as floats - get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); - - // get pilot's desired yaw rate - float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); - - // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); - - // set target climb rate - float cmb_rate = constrain_float(fabsf(wp_nav.get_default_speed_up()), 1, pos_control.get_max_speed_up_cms()); - - // record desired climb rate for logging - desired_climb_rate = cmb_rate; - - // update altitude target and call position controller - pos_control.set_pos_target_z_from_climb_rate_cm(cmb_rate); - pos_control.update_z_controller(); - - // pilot has control for repositioning - motors.set_forward(channel_forward->norm_input()); - motors.set_lateral(channel_lateral->norm_input()); -} diff --git a/ArduSub/defines.h b/ArduSub/defines.h index b3c7109f10fda..87e45820bc982 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -26,21 +26,8 @@ enum autopilot_yaw_mode { AUTO_YAW_LOOK_AT_HEADING = 3, // point towards a particular angle (not pilot input accepted) AUTO_YAW_LOOK_AHEAD = 4, // point in the direction the vehicle is moving AUTO_YAW_RESETTOARMEDYAW = 5, // point towards heading at time motors were armed - AUTO_YAW_CORRECT_XTRACK = 6 // steer the sub in order to correct for crosstrack error during line following -}; - -// Auto Pilot Modes enumeration -enum control_mode_t : uint8_t { - STABILIZE = 0, // manual angle with manual depth/throttle - ACRO = 1, // manual body-frame angular rate with manual depth/throttle - ALT_HOLD = 2, // manual angle with automatic depth/throttle - AUTO = 3, // fully automatic waypoint control using mission commands - GUIDED = 4, // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands - CIRCLE = 7, // automatic circular flight with automatic throttle - SURFACE = 9, // automatically return to surface, pilot maintains horizontal control - POSHOLD = 16, // automatic position hold with manual override, with automatic throttle - MANUAL = 19, // Pass-through input with no stabilization - MOTOR_DETECT = 20 // Automatically detect motors orientation + AUTO_YAW_CORRECT_XTRACK = 6, // steer the sub in order to correct for crosstrack error during line following + AUTO_YAW_RATE = 7 // steer the sub with the desired yaw rate }; // Acro Trainer types @@ -52,35 +39,10 @@ enum control_mode_t : uint8_t { #define WP_YAW_BEHAVIOR_NONE 0 // auto pilot will never control yaw during missions or rtl (except for DO_CONDITIONAL_YAW command received) #define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP 1 // auto pilot will face next waypoint or home during rtl #define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL 2 // auto pilot will face next waypoint except when doing RTL at which time it will stay in it's last -#define WP_YAW_BEHAVIOR_LOOK_AHEAD 3 // auto pilot will look ahead during missions and rtl (primarily meant for traditional helicotpers) +#define WP_YAW_BEHAVIOR_LOOK_AHEAD 3 // auto pilot will look ahead during missions and rtl (primarily meant for traditional helicopters) #define WP_YAW_BEHAVIOR_CORRECT_XTRACK 4 // point towards intermediate position target during line following -// Auto modes -enum AutoMode { - Auto_WP, - Auto_CircleMoveToEdge, - Auto_Circle, - Auto_NavGuided, - Auto_Loiter, - Auto_TerrainRecover -}; - -// Guided modes -enum GuidedMode { - Guided_WP, - Guided_Velocity, - Guided_PosVel, - Guided_Angle, -}; -// RTL states -enum RTLState { - RTL_InitialClimb, - RTL_ReturnHome, - RTL_LoiterAtHome, - RTL_FinalDescent, - RTL_Land -}; // Logging parameters - only 32 messages are available to the vehicle here. enum LoggingParameters { diff --git a/ArduSub/failsafe.cpp b/ArduSub/failsafe.cpp index 4b2fa212b7eeb..5c0d08d32499c 100644 --- a/ArduSub/failsafe.cpp +++ b/ArduSub/failsafe.cpp @@ -88,9 +88,9 @@ void Sub::failsafe_sensors_check() gcs().send_text(MAV_SEVERITY_CRITICAL, "Depth sensor error!"); AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_SENSORS, LogErrorCode::BAD_DEPTH); - if (control_mode == ALT_HOLD || control_mode == SURFACE || mode_requires_GPS(control_mode)) { + if (control_mode == Mode::Number::ALT_HOLD || control_mode == Mode::Number::SURFACE || sub.flightmode->requires_GPS()) { // This should always succeed - if (!set_mode(MANUAL, ModeReason::BAD_DEPTH)) { + if (!set_mode(Mode::Number::MANUAL, ModeReason::BAD_DEPTH)) { // We should never get here arming.disarm(AP_Arming::Method::BADFLOWOFCONTROL); } @@ -156,7 +156,7 @@ void Sub::handle_battery_failsafe(const char* type_str, const int8_t action) switch((Failsafe_Action)action) { case Failsafe_Action_Surface: - set_mode(SURFACE, ModeReason::BATTERY_FAILSAFE); + set_mode(Mode::Number::SURFACE, ModeReason::BATTERY_FAILSAFE); break; case Failsafe_Action_Disarm: arming.disarm(AP_Arming::Method::BATTERYFAILSAFE); @@ -299,7 +299,7 @@ void Sub::failsafe_leak_check() // Handle failsafe action if (failsafe.leak && g.failsafe_leak == FS_LEAK_SURFACE && motors.armed()) { - set_mode(SURFACE, ModeReason::LEAK_FAILSAFE); + set_mode(Mode::Number::SURFACE, ModeReason::LEAK_FAILSAFE); } } @@ -352,11 +352,11 @@ void Sub::failsafe_gcs_check() if (g.failsafe_gcs == FS_GCS_DISARM) { arming.disarm(AP_Arming::Method::GCSFAILSAFE); } else if (g.failsafe_gcs == FS_GCS_HOLD && motors.armed()) { - if (!set_mode(ALT_HOLD, ModeReason::GCS_FAILSAFE)) { + if (!set_mode(Mode::Number::ALT_HOLD, ModeReason::GCS_FAILSAFE)) { arming.disarm(AP_Arming::Method::GCS_FAILSAFE_HOLDFAILED); } } else if (g.failsafe_gcs == FS_GCS_SURFACE && motors.armed()) { - if (!set_mode(SURFACE, ModeReason::GCS_FAILSAFE)) { + if (!set_mode(Mode::Number::SURFACE, ModeReason::GCS_FAILSAFE)) { arming.disarm(AP_Arming::Method::GCS_FAILSAFE_SURFACEFAILED); } } @@ -380,7 +380,7 @@ void Sub::failsafe_crash_check() } // return immediately if we are not in an angle stabilized flight mode - if (control_mode == ACRO || control_mode == MANUAL) { + if (control_mode == Mode::Number::ACRO || control_mode == Mode::Number::MANUAL) { last_crash_check_pass_ms = tnow; failsafe.crash = false; return; @@ -425,7 +425,7 @@ void Sub::failsafe_crash_check() void Sub::failsafe_terrain_check() { // trigger with 5 seconds of failures while in AUTO mode - bool valid_mode = (control_mode == AUTO || control_mode == GUIDED); + bool valid_mode = (control_mode == Mode::Number::AUTO || control_mode == Mode::Number::GUIDED); bool timeout = (failsafe.terrain_last_failure_ms - failsafe.terrain_first_failure_ms) > FS_TERRAIN_TIMEOUT_MS; bool trigger_event = valid_mode && timeout; @@ -470,7 +470,7 @@ void Sub::failsafe_terrain_on_event() AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::FAILSAFE_OCCURRED); // If rangefinder is enabled, we can recover from this failsafe - if (!rangefinder_state.enabled || !auto_terrain_recover_start()) { + if (!rangefinder_state.enabled || !sub.mode_auto.auto_terrain_recover_start()) { failsafe_terrain_act(); } @@ -482,14 +482,14 @@ void Sub::failsafe_terrain_act() { switch (g.failsafe_terrain) { case FS_TERRAIN_HOLD: - if (!set_mode(POSHOLD, ModeReason::TERRAIN_FAILSAFE)) { - set_mode(ALT_HOLD, ModeReason::TERRAIN_FAILSAFE); + if (!set_mode(Mode::Number::POSHOLD, ModeReason::TERRAIN_FAILSAFE)) { + set_mode(Mode::Number::ALT_HOLD, ModeReason::TERRAIN_FAILSAFE); } AP_Notify::events.failsafe_mode_change = 1; break; case FS_TERRAIN_SURFACE: - set_mode(SURFACE, ModeReason::TERRAIN_FAILSAFE); + set_mode(Mode::Number::SURFACE, ModeReason::TERRAIN_FAILSAFE); AP_Notify::events.failsafe_mode_change = 1; break; diff --git a/ArduSub/fence.cpp b/ArduSub/fence.cpp index 7d89c89cae1ee..c39e84aebf531 100644 --- a/ArduSub/fence.cpp +++ b/ArduSub/fence.cpp @@ -16,7 +16,7 @@ void Sub::fence_check() const uint8_t orig_breaches = fence.get_breaches(); // check for new breaches; new_breaches is bitmask of fence types breached - const uint8_t new_breaches = fence.check(); + const uint8_t new_breaches = sub.fence.check(); // if there is a new breach take action if (new_breaches) { diff --git a/ArduSub/flight_mode.cpp b/ArduSub/flight_mode.cpp deleted file mode 100644 index 521586923fd8f..0000000000000 --- a/ArduSub/flight_mode.cpp +++ /dev/null @@ -1,230 +0,0 @@ -#include "Sub.h" - -// change flight mode and perform any necessary initialisation -// returns true if mode was successfully set -bool Sub::set_mode(control_mode_t mode, ModeReason reason) -{ - // boolean to record if flight mode could be set - bool success = false; - - // return immediately if we are already in the desired mode - if (mode == control_mode) { - prev_control_mode = control_mode; - - control_mode_reason = reason; - return true; - } - - switch (mode) { - case ACRO: - success = acro_init(); - break; - - case STABILIZE: - success = stabilize_init(); - break; - - case ALT_HOLD: - success = althold_init(); - break; - - case AUTO: - success = auto_init(); - break; - - case CIRCLE: - success = circle_init(); - break; - - case GUIDED: - success = guided_init(); - break; - - case SURFACE: - success = surface_init(); - break; - -#if POSHOLD_ENABLED == ENABLED - case POSHOLD: - success = poshold_init(); - break; -#endif - - case MANUAL: - success = manual_init(); - break; - - case MOTOR_DETECT: - success = motordetect_init(); - break; - - default: - success = false; - break; - } - - // update flight mode - if (success) { - // perform any cleanup required by previous flight mode - exit_mode(control_mode, mode); - - prev_control_mode = control_mode; - - control_mode = mode; - control_mode_reason = reason; - logger.Write_Mode(control_mode, control_mode_reason); - gcs().send_message(MSG_HEARTBEAT); - - // update notify object - notify_flight_mode(control_mode); - -#if AP_CAMERA_ENABLED - camera.set_is_auto_mode(control_mode == AUTO); -#endif - -#if AP_FENCE_ENABLED - // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover - // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe) - // but it should be harmless to disable the fence temporarily in these situations as well - fence.manual_recovery_start(); -#endif - } else { - // Log error that we failed to enter desired flight mode - AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); - } - - // return success or failure - return success; -} - -bool Sub::set_mode(const uint8_t new_mode, const ModeReason reason) -{ - static_assert(sizeof(control_mode_t) == sizeof(new_mode), "The new mode can't be mapped to the vehicles mode number"); - return sub.set_mode((control_mode_t)new_mode, reason); -} - -// update_flight_mode - calls the appropriate attitude controllers based on flight mode -// called at 100hz or more -void Sub::update_flight_mode() -{ - switch (control_mode) { - case ACRO: - acro_run(); - break; - - case STABILIZE: - stabilize_run(); - break; - - case ALT_HOLD: - althold_run(); - break; - - case AUTO: - auto_run(); - break; - - case CIRCLE: - circle_run(); - break; - - case GUIDED: - guided_run(); - break; - - case SURFACE: - surface_run(); - break; - -#if POSHOLD_ENABLED == ENABLED - case POSHOLD: - poshold_run(); - break; -#endif - - case MANUAL: - manual_run(); - break; - - case MOTOR_DETECT: - motordetect_run(); - break; - - default: - break; - } -} - -// exit_mode - high level call to organise cleanup as a flight mode is exited -void Sub::exit_mode(control_mode_t old_control_mode, control_mode_t new_control_mode) -{ - // stop mission when we leave auto mode - if (old_control_mode == AUTO) { - if (mission.state() == AP_Mission::MISSION_RUNNING) { - mission.stop(); - } -#if HAL_MOUNT_ENABLED - camera_mount.set_mode_to_default(); -#endif // HAL_MOUNT_ENABLED - } -} - -// returns true or false whether mode requires GPS -bool Sub::mode_requires_GPS(control_mode_t mode) -{ - switch (mode) { - case AUTO: - case GUIDED: - case CIRCLE: - case POSHOLD: - return true; - default: - return false; - } - - return false; -} - -// mode_has_manual_throttle - returns true if the flight mode has a manual throttle (i.e. pilot directly controls throttle) -bool Sub::mode_has_manual_throttle(control_mode_t mode) -{ - switch (mode) { - case ACRO: - case STABILIZE: - case MANUAL: - return true; - default: - return false; - } - - return false; -} - -// mode_allows_arming - returns true if vehicle can be armed in the specified mode -// arming_from_gcs should be set to true if the arming request comes from the ground station -bool Sub::mode_allows_arming(control_mode_t mode, bool arming_from_gcs) -{ - return (mode_has_manual_throttle(mode) - || mode == ALT_HOLD - || mode == POSHOLD - || (arming_from_gcs&& mode == GUIDED) - ); -} - -// notify_flight_mode - sets notify object based on flight mode. Only used for OreoLED notify device -void Sub::notify_flight_mode(control_mode_t mode) -{ - switch (mode) { - case AUTO: - case GUIDED: - case CIRCLE: - case SURFACE: - // autopilot modes - AP_Notify::flags.autopilot_mode = true; - break; - default: - // all other are manual flight modes - AP_Notify::flags.autopilot_mode = false; - break; - } -} diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index bbbd19c3464cc..0eeafe94fb403 100644 --- a/ArduSub/joystick.cpp +++ b/ArduSub/joystick.cpp @@ -1,4 +1,5 @@ #include "Sub.h" +#include "mode.h" // Functions that will handle joystick/gamepad input // ---------------------------------------------------------------------------- @@ -16,7 +17,7 @@ int16_t xTrim = 0; int16_t yTrim = 0; int16_t video_switch = 1100; int16_t x_last, y_last, z_last; -uint16_t buttons_prev; +uint32_t buttons_prev; // Servo control output channels // TODO: Allow selecting output channels @@ -34,7 +35,7 @@ void Sub::init_joystick() lights1 = RC_Channels::rc_channel(8)->get_radio_min(); lights2 = RC_Channels::rc_channel(9)->get_radio_min(); - set_mode(MANUAL, ModeReason::RC_COMMAND); // Initialize flight mode + set_mode(Mode::Number::MANUAL, ModeReason::RC_COMMAND); // Initialize flight mode if (g.numGainSettings < 1) { g.numGainSettings.set_and_save(1); @@ -50,7 +51,15 @@ void Sub::init_joystick() gain = constrain_float(gain, 0.1, 1.0); } -void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) +void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons, uint16_t buttons2, uint8_t enabled_extensions, + int16_t s, + int16_t t, + int16_t aux1, + int16_t aux2, + int16_t aux3, + int16_t aux4, + int16_t aux5, + int16_t aux6) { float rpyScale = 0.4*gain; // Scale -1000-1000 to -400-400 with gain @@ -64,17 +73,18 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t cam_tilt = 1500; cam_pan = 1500; + uint32_t all_buttons = buttons | (buttons2 << 16); // Detect if any shift button is pressed - for (uint8_t i = 0 ; i < 16 ; i++) { - if ((buttons & (1 << i)) && get_button(i)->function() == JSButton::button_function_t::k_shift) { + for (uint8_t i = 0 ; i < 32 ; i++) { + if ((all_buttons & (1 << i)) && get_button(i)->function() == JSButton::button_function_t::k_shift) { shift = true; } } // Act if button is pressed // Only act upon pressing button and ignore holding. This provides compatibility with Taranis as joystick. - for (uint8_t i = 0 ; i < 16 ; i++) { - if ((buttons & (1 << i))) { + for (uint8_t i = 0 ; i < 32 ; i++) { + if ((all_buttons & (1 << i))) { handle_jsbutton_press(i,shift,(buttons_prev & (1 << i))); // buttonDebounce = tnow_ms; } else if (buttons_prev & (1 << i)) { @@ -82,7 +92,7 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t } } - buttons_prev = buttons; + buttons_prev = all_buttons; // attitude mode: if (roll_pitch_flag == 1) { @@ -109,8 +119,8 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t xTot = x + xTrim; } - RC_Channels::set_override(0, constrain_int16(pitchTrim + rpyCenter,1100,1900), tnow); // pitch - RC_Channels::set_override(1, constrain_int16(rollTrim + rpyCenter,1100,1900), tnow); // roll + RC_Channels::set_override(0, constrain_int16(s + pitchTrim + rpyCenter,1100,1900), tnow); // pitch + RC_Channels::set_override(1, constrain_int16(t + rollTrim + rpyCenter,1100,1900), tnow); // roll RC_Channels::set_override(2, constrain_int16((zTot)*throttleScale+throttleBase,1100,1900), tnow); // throttle RC_Channels::set_override(3, constrain_int16(r*rpyScale+rpyCenter,1100,1900), tnow); // yaw @@ -157,28 +167,28 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held) break; case JSButton::button_function_t::k_mode_manual: - set_mode(MANUAL, ModeReason::RC_COMMAND); + set_mode(Mode::Number::MANUAL, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_stabilize: - set_mode(STABILIZE, ModeReason::RC_COMMAND); + set_mode(Mode::Number::STABILIZE, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_depth_hold: - set_mode(ALT_HOLD, ModeReason::RC_COMMAND); + set_mode(Mode::Number::ALT_HOLD, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_auto: - set_mode(AUTO, ModeReason::RC_COMMAND); + set_mode(Mode::Number::AUTO, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_guided: - set_mode(GUIDED, ModeReason::RC_COMMAND); + set_mode(Mode::Number::GUIDED, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_circle: - set_mode(CIRCLE, ModeReason::RC_COMMAND); + set_mode(Mode::Number::CIRCLE, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_acro: - set_mode(ACRO, ModeReason::RC_COMMAND); + set_mode(Mode::Number::ACRO, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_poshold: - set_mode(POSHOLD, ModeReason::RC_COMMAND); + set_mode(Mode::Number::POSHOLD, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mount_center: @@ -359,6 +369,7 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held) controls_reset_since_input_hold = !input_hold_engaged; } break; +#if AP_RELAY_ENABLED case JSButton::button_function_t::k_relay_1_on: relay.on(0); break; @@ -423,10 +434,12 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held) relay.on(3); } break; +#endif //////////////////////////////////////////////// // Servo functions // TODO: initialize +#if AP_SERVORELAYEVENTS_ENABLED case JSButton::button_function_t::k_servo_1_inc: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed @@ -557,6 +570,7 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held) ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_trim()); // 1-indexed } break; +#endif // AP_SERVORELAYEVENTS_ENABLED case JSButton::button_function_t::k_roll_pitch_toggle: if (!held) { @@ -588,6 +602,21 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held) case JSButton::button_function_t::k_custom_6: // Not implemented break; + +#if AP_SCRIPTING_ENABLED + case JSButton::button_function_t::k_script_1: + sub.script_buttons[0].press(); + break; + case JSButton::button_function_t::k_script_2: + sub.script_buttons[1].press(); + break; + case JSButton::button_function_t::k_script_3: + sub.script_buttons[2].press(); + break; + case JSButton::button_function_t::k_script_4: + sub.script_buttons[3].press(); + break; +#endif // AP_SCRIPTING_ENABLED } } @@ -595,6 +624,7 @@ void Sub::handle_jsbutton_release(uint8_t _button, bool shift) { // Act based on the function assigned to this button switch (get_button(_button)->function(shift)) { +#if AP_RELAY_ENABLED case JSButton::button_function_t::k_relay_1_momentary: relay.off(0); break; @@ -607,6 +637,8 @@ void Sub::handle_jsbutton_release(uint8_t _button, bool shift) { case JSButton::button_function_t::k_relay_4_momentary: relay.off(3); break; +#endif +#if AP_SERVORELAYEVENTS_ENABLED case JSButton::button_function_t::k_servo_1_min_momentary: case JSButton::button_function_t::k_servo_1_max_momentary: { @@ -628,6 +660,22 @@ void Sub::handle_jsbutton_release(uint8_t _button, bool shift) { ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_trim()); // 1-indexed } break; +#endif + +#if AP_SCRIPTING_ENABLED + case JSButton::button_function_t::k_script_1: + sub.script_buttons[0].release(); + break; + case JSButton::button_function_t::k_script_2: + sub.script_buttons[1].release(); + break; + case JSButton::button_function_t::k_script_3: + sub.script_buttons[2].release(); + break; + case JSButton::button_function_t::k_script_4: + sub.script_buttons[3].release(); + break; +#endif // AP_SCRIPTING_ENABLED } } @@ -667,6 +715,40 @@ JSButton* Sub::get_button(uint8_t index) return &g.jbtn_14; case 15: return &g.jbtn_15; + + // add 16 more cases for 32 buttons with MANUAL_CONTROL extensions + case 16: + return &g.jbtn_16; + case 17: + return &g.jbtn_17; + case 18: + return &g.jbtn_18; + case 19: + return &g.jbtn_19; + case 20: + return &g.jbtn_20; + case 21: + return &g.jbtn_21; + case 22: + return &g.jbtn_22; + case 23: + return &g.jbtn_23; + case 24: + return &g.jbtn_24; + case 25: + return &g.jbtn_25; + case 26: + return &g.jbtn_26; + case 27: + return &g.jbtn_27; + case 28: + return &g.jbtn_28; + case 29: + return &g.jbtn_29; + case 30: + return &g.jbtn_30; + case 31: + return &g.jbtn_31; default: return &g.jbtn_0; } @@ -721,3 +803,15 @@ void Sub::clear_input_hold() zTrim = 0; input_hold_engaged = false; } + +#if AP_SCRIPTING_ENABLED +bool Sub::is_button_pressed(uint8_t index) +{ + return script_buttons[index - 1].is_pressed(); +} + +uint8_t Sub::get_and_clear_button_count(uint8_t index) +{ + return script_buttons[index - 1].get_and_clear_count(); +} +#endif // AP_SCRIPTING_ENABLED diff --git a/ArduSub/mode.cpp b/ArduSub/mode.cpp new file mode 100644 index 0000000000000..323836c2f94ab --- /dev/null +++ b/ArduSub/mode.cpp @@ -0,0 +1,292 @@ +#include "Sub.h" + +/* + constructor for Mode object + */ +Mode::Mode(void) : + g(sub.g), + g2(sub.g2), + inertial_nav(sub.inertial_nav), + ahrs(sub.ahrs), + motors(sub.motors), + channel_roll(sub.channel_roll), + channel_pitch(sub.channel_pitch), + channel_throttle(sub.channel_throttle), + channel_yaw(sub.channel_yaw), + channel_forward(sub.channel_forward), + channel_lateral(sub.channel_lateral), + position_control(&sub.pos_control), + attitude_control(&sub.attitude_control), + G_Dt(sub.G_Dt) +{ }; + +// return the static controller object corresponding to supplied mode +Mode *Sub::mode_from_mode_num(const Mode::Number mode) +{ + Mode *ret = nullptr; + + switch (mode) { + case Mode::Number::MANUAL: + ret = &mode_manual; + break; + case Mode::Number::STABILIZE: + ret = &mode_stabilize; + break; + case Mode::Number::ACRO: + ret = &mode_acro; + break; + case Mode::Number::ALT_HOLD: + ret = &mode_althold; + break; + case Mode::Number::POSHOLD: + ret = &mode_poshold; + break; + case Mode::Number::AUTO: + ret = &mode_auto; + break; + case Mode::Number::GUIDED: + ret = &mode_guided; + break; + case Mode::Number::CIRCLE: + ret = &mode_circle; + break; + case Mode::Number::SURFACE: + ret = &mode_surface; + break; + case Mode::Number::MOTOR_DETECT: + ret = &mode_motordetect; + break; + default: + break; + } + + return ret; +} + + +// set_mode - change flight mode and perform any necessary initialisation +// optional force parameter used to force the flight mode change (used only first time mode is set) +// returns true if mode was successfully set +// Some modes can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately +bool Sub::set_mode(Mode::Number mode, ModeReason reason) +{ + + // return immediately if we are already in the desired mode + if (mode == control_mode) { + control_mode_reason = reason; + return true; + } + + Mode *new_flightmode = mode_from_mode_num((Mode::Number)mode); + if (new_flightmode == nullptr) { + notify_no_such_mode((uint8_t)mode); + return false; + } + + if (new_flightmode->requires_GPS() && + !sub.position_ok()) { + gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s requires position", new_flightmode->name()); + AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); + return false; + } + + // check for valid altitude if old mode did not require it but new one does + // we only want to stop changing modes if it could make things worse + if (!sub.control_check_barometer() && // maybe use ekf_alt_ok() instead? + flightmode->has_manual_throttle() && + !new_flightmode->has_manual_throttle()) { + gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s need alt estimate", new_flightmode->name()); + AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); + return false; + } + + if (!new_flightmode->init(false)) { + gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed %s", new_flightmode->name()); + AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); + return false; + } + + // perform any cleanup required by previous flight mode + exit_mode(flightmode, new_flightmode); + + // store previous flight mode (only used by tradeheli's autorotation) + prev_control_mode = control_mode; + + // update flight mode + flightmode = new_flightmode; + control_mode = mode; + control_mode_reason = reason; + logger.Write_Mode((uint8_t)control_mode, reason); + gcs().send_message(MSG_HEARTBEAT); + + // update notify object + notify_flight_mode(); + + // return success + return true; +} + +// exit_mode - high level call to organise cleanup as a flight mode is exited +void Sub::exit_mode(Mode::Number old_control_mode, Mode::Number new_control_mode) +{ + // stop mission when we leave auto mode + if (old_control_mode == Mode::Number::AUTO) { + if (mission.state() == AP_Mission::MISSION_RUNNING) { + mission.stop(); + } +#if HAL_MOUNT_ENABLED + camera_mount.set_mode_to_default(); +#endif // HAL_MOUNT_ENABLED + } +} + +bool Sub::set_mode(const uint8_t new_mode, const ModeReason reason) +{ + static_assert(sizeof(Mode::Number) == sizeof(new_mode), "The new mode can't be mapped to the vehicles mode number"); + return sub.set_mode(static_cast(new_mode), reason); +} + +// update_flight_mode - calls the appropriate attitude controllers based on flight mode +// called at 100hz or more +void Sub::update_flight_mode() +{ + flightmode->run(); +} + +// exit_mode - high level call to organise cleanup as a flight mode is exited +void Sub::exit_mode(Mode *&old_flightmode, Mode *&new_flightmode){ +#if HAL_MOUNT_ENABLED + camera_mount.set_mode_to_default(); +#endif // HAL_MOUNT_ENABLED +} + +// notify_flight_mode - sets notify object based on current flight mode. Only used for OreoLED notify device +void Sub::notify_flight_mode() +{ + AP_Notify::flags.autopilot_mode = flightmode->is_autopilot(); + AP_Notify::flags.flight_mode = (uint8_t)control_mode; + notify.set_flight_mode_str(flightmode->name4()); +} + + +// get_pilot_desired_angle_rates - transform pilot's roll pitch and yaw input into a desired lean angle rates +// returns desired angle rates in centi-degrees-per-second +void Mode::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out) +{ + float rate_limit; + Vector3f rate_ef_level, rate_bf_level, rate_bf_request; + + // apply circular limit to pitch and roll inputs + float total_in = norm(pitch_in, roll_in); + + if (total_in > ROLL_PITCH_INPUT_MAX) { + float ratio = (float)ROLL_PITCH_INPUT_MAX / total_in; + roll_in *= ratio; + pitch_in *= ratio; + } + + // calculate roll, pitch rate requests + if (g.acro_expo <= 0) { + rate_bf_request.x = roll_in * g.acro_rp_p; + rate_bf_request.y = pitch_in * g.acro_rp_p; + } else { + // expo variables + float rp_in, rp_in3, rp_out; + + // range check expo + if (g.acro_expo > 1.0f) { + g.acro_expo.set(1.0f); + } + + // roll expo + rp_in = float(roll_in)/ROLL_PITCH_INPUT_MAX; + rp_in3 = rp_in*rp_in*rp_in; + rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in); + rate_bf_request.x = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p; + + // pitch expo + rp_in = float(pitch_in)/ROLL_PITCH_INPUT_MAX; + rp_in3 = rp_in*rp_in*rp_in; + rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in); + rate_bf_request.y = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p; + } + + // calculate yaw rate request + rate_bf_request.z = yaw_in * g.acro_yaw_p; + + // calculate earth frame rate corrections to pull the vehicle back to level while in ACRO mode + + if (g.acro_trainer != ACRO_TRAINER_DISABLED) { + // Calculate trainer mode earth frame rate command for roll + int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor); + rate_ef_level.x = -constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll; + + // Calculate trainer mode earth frame rate command for pitch + int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor); + rate_ef_level.y = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch; + + // Calculate trainer mode earth frame rate command for yaw + rate_ef_level.z = 0; + + // Calculate angle limiting earth frame rate commands + if (g.acro_trainer == ACRO_TRAINER_LIMITED) { + if (roll_angle > sub.aparm.angle_max) { + rate_ef_level.x -= g.acro_balance_roll*(roll_angle-sub.aparm.angle_max); + } else if (roll_angle < -sub.aparm.angle_max) { + rate_ef_level.x -= g.acro_balance_roll*(roll_angle+sub.aparm.angle_max); + } + + if (pitch_angle > sub.aparm.angle_max) { + rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle-sub.aparm.angle_max); + } else if (pitch_angle < -sub.aparm.angle_max) { + rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle+sub.aparm.angle_max); + } + } + + // convert earth-frame level rates to body-frame level rates + attitude_control->euler_rate_to_ang_vel(attitude_control->get_att_target_euler_cd()*radians(0.01f), rate_ef_level, rate_bf_level); + + // combine earth frame rate corrections with rate requests + if (g.acro_trainer == ACRO_TRAINER_LIMITED) { + rate_bf_request.x += rate_bf_level.x; + rate_bf_request.y += rate_bf_level.y; + rate_bf_request.z += rate_bf_level.z; + } else { + float acro_level_mix = constrain_float(1-MAX(MAX(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch(); + + // Scale leveling rates by stick input + rate_bf_level = rate_bf_level*acro_level_mix; + + // Calculate rate limit to prevent change of rate through inverted + rate_limit = fabsf(fabsf(rate_bf_request.x)-fabsf(rate_bf_level.x)); + rate_bf_request.x += rate_bf_level.x; + rate_bf_request.x = constrain_float(rate_bf_request.x, -rate_limit, rate_limit); + + // Calculate rate limit to prevent change of rate through inverted + rate_limit = fabsf(fabsf(rate_bf_request.y)-fabsf(rate_bf_level.y)); + rate_bf_request.y += rate_bf_level.y; + rate_bf_request.y = constrain_float(rate_bf_request.y, -rate_limit, rate_limit); + + // Calculate rate limit to prevent change of rate through inverted + rate_limit = fabsf(fabsf(rate_bf_request.z)-fabsf(rate_bf_level.z)); + rate_bf_request.z += rate_bf_level.z; + rate_bf_request.z = constrain_float(rate_bf_request.z, -rate_limit, rate_limit); + } + } + + // hand back rate request + roll_out = rate_bf_request.x; + pitch_out = rate_bf_request.y; + yaw_out = rate_bf_request.z; +} + + +bool Mode::set_mode(Mode::Number mode, ModeReason reason) +{ + return sub.set_mode(mode, reason); +} + +GCS_Sub &Mode::gcs() +{ + return sub.gcs(); +} diff --git a/ArduSub/mode.h b/ArduSub/mode.h new file mode 100644 index 0000000000000..584f5b26d315d --- /dev/null +++ b/ArduSub/mode.h @@ -0,0 +1,449 @@ +#pragma once + +#include "Sub.h" +class Parameters; +class ParametersG2; + +class GCS_Sub; + +// Guided modes +enum GuidedSubMode { + Guided_WP, + Guided_Velocity, + Guided_PosVel, + Guided_Angle, +}; + +// Auto modes +enum AutoSubMode { + Auto_WP, + Auto_CircleMoveToEdge, + Auto_Circle, + Auto_NavGuided, + Auto_Loiter, + Auto_TerrainRecover +}; + +// RTL states +enum RTLState { + RTL_InitialClimb, + RTL_ReturnHome, + RTL_LoiterAtHome, + RTL_FinalDescent, + RTL_Land +}; + +class Mode +{ + +public: + + // Auto Pilot Modes enumeration + enum class Number : uint8_t { + STABILIZE = 0, // manual angle with manual depth/throttle + ACRO = 1, // manual body-frame angular rate with manual depth/throttle + ALT_HOLD = 2, // manual angle with automatic depth/throttle + AUTO = 3, // fully automatic waypoint control using mission commands + GUIDED = 4, // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands + CIRCLE = 7, // automatic circular flight with automatic throttle + SURFACE = 9, // automatically return to surface, pilot maintains horizontal control + POSHOLD = 16, // automatic position hold with manual override, with automatic throttle + MANUAL = 19, // Pass-through input with no stabilization + MOTOR_DETECT = 20 // Automatically detect motors orientation + }; + + // constructor + Mode(void); + + // do not allow copying + CLASS_NO_COPY(Mode); + + // child classes should override these methods + virtual bool init(bool ignore_checks) { return true; } + virtual void run() = 0; + virtual bool requires_GPS() const = 0; + virtual bool has_manual_throttle() const = 0; + virtual bool allows_arming(bool from_gcs) const = 0; + virtual bool is_autopilot() const { return false; } + virtual bool in_guided_mode() const { return false; } + + // return a string for this flightmode + virtual const char *name() const = 0; + virtual const char *name4() const = 0; + + // functions for reporting to GCS + virtual bool get_wp(Location &loc) { return false; } + virtual int32_t wp_bearing() const { return 0; } + virtual uint32_t wp_distance() const { return 0; } + virtual float crosstrack_error() const { return 0.0f; } + + + // pilot input processing + void get_pilot_desired_accelerations(float &right_out, float &front_out) const; + void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out); + + +protected: + + // navigation support functions + virtual void run_autopilot() {} + + // helper functions + bool is_disarmed_or_landed() const; + + // functions to control landing + // in modes that support landing + void land_run_horizontal_control(); + void land_run_vertical_control(bool pause_descent = false); + + // convenience references to avoid code churn in conversion: + Parameters &g; + ParametersG2 &g2; + AP_InertialNav &inertial_nav; + AP_AHRS &ahrs; + AP_Motors6DOF &motors; + RC_Channel *&channel_roll; + RC_Channel *&channel_pitch; + RC_Channel *&channel_throttle; + RC_Channel *&channel_yaw; + RC_Channel *&channel_forward; + RC_Channel *&channel_lateral; + AC_PosControl *position_control; + AC_AttitudeControl_Sub *attitude_control; + // TODO: channels + float &G_Dt; + +public: + // Navigation Yaw control + class AutoYaw + { + + public: + // mode(): current method of determining desired yaw: + autopilot_yaw_mode mode() const + { + return (autopilot_yaw_mode)_mode; + } + void set_mode_to_default(bool rtl); + void set_mode(autopilot_yaw_mode new_mode); + autopilot_yaw_mode default_mode(bool rtl) const; + + void set_rate(float new_rate_cds); + + // set_roi(...): set a "look at" location: + void set_roi(const Location &roi_location); + + void set_fixed_yaw(float angle_deg, + float turn_rate_dps, + int8_t direction, + bool relative_angle); + + private: + + // yaw_cd(): main product of AutoYaw; the heading: + float yaw_cd(); + + // rate_cds(): desired yaw rate in centidegrees/second: + float rate_cds(); + + float look_ahead_yaw(); + float roi_yaw(); + + // auto flight mode's yaw mode + uint8_t _mode = AUTO_YAW_LOOK_AT_NEXT_WP; + + // Yaw will point at this location if mode is set to AUTO_YAW_ROI + Vector3f roi; + + // bearing from current location to the ROI + float _roi_yaw; + + // yaw used for YAW_FIXED yaw_mode + int32_t _fixed_yaw; + + // Deg/s we should turn + int16_t _fixed_yaw_slewrate; + + // heading when in yaw_look_ahead_yaw + float _look_ahead_yaw; + + // used to reduce update rate to 100hz: + uint8_t roi_yaw_counter; + + GuidedSubMode guided_mode; + + }; + static AutoYaw auto_yaw; + + // pass-through functions to reduce code churn on conversion; + // these are candidates for moving into the Mode base + // class. + bool set_mode(Mode::Number mode, ModeReason reason); + GCS_Sub &gcs(); + + // end pass-through functions +}; + +class ModeManual : public Mode +{ + +public: + // inherit constructor + using Mode::Mode; + virtual void run() override; + bool init(bool ignore_checks) override; + bool requires_GPS() const override { return false; } + bool has_manual_throttle() const override { return true; } + bool allows_arming(bool from_gcs) const override { return true; } + bool is_autopilot() const override { return false; } + +protected: + + const char *name() const override { return "MANUAL"; } + const char *name4() const override { return "MANU"; } +}; + + +class ModeAcro : public Mode +{ + +public: + // inherit constructor + using Mode::Mode; + + virtual void run() override; + + bool init(bool ignore_checks) override; + bool requires_GPS() const override { return false; } + bool has_manual_throttle() const override { return true; } + bool allows_arming(bool from_gcs) const override { return true; } + bool is_autopilot() const override { return false; } + +protected: + + const char *name() const override { return "ACRO"; } + const char *name4() const override { return "ACRO"; } +}; + + +class ModeStabilize : public Mode +{ + +public: + // inherit constructor + using Mode::Mode; + + virtual void run() override; + + bool init(bool ignore_checks) override; + bool requires_GPS() const override { return false; } + bool has_manual_throttle() const override { return true; } + bool allows_arming(bool from_gcs) const override { return true; } + bool is_autopilot() const override { return false; } + +protected: + + const char *name() const override { return "STABILIZE"; } + const char *name4() const override { return "STAB"; } +}; + + +class ModeAlthold : public Mode +{ + +public: + // inherit constructor + using Mode::Mode; + + virtual void run() override; + + bool init(bool ignore_checks) override; + bool requires_GPS() const override { return false; } + bool has_manual_throttle() const override { return false; } + bool allows_arming(bool from_gcs) const override { return true; } + bool is_autopilot() const override { return false; } + void control_depth(); + +protected: + + const char *name() const override { return "ALT_HOLD"; } + const char *name4() const override { return "ALTH"; } +}; + + +class ModeGuided : public Mode +{ + +public: + // inherit constructor + using Mode::Mode; + + virtual void run() override; + + bool init(bool ignore_checks) override; + bool requires_GPS() const override { return true; } + bool has_manual_throttle() const override { return false; } + bool allows_arming(bool from_gcs) const override { return true; } + bool is_autopilot() const override { return true; } + bool guided_limit_check(); + void guided_limit_init_time_and_pos(); + void guided_set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads); + void guided_set_angle(const Quaternion&, float); + void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm); + bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity); + bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw); + bool guided_set_destination(const Vector3f& destination); + bool guided_set_destination(const Location&); + bool guided_set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw); + void guided_set_velocity(const Vector3f& velocity); + void guided_set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw); + void guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle); + float get_auto_heading(); + void guided_limit_clear(); + void set_auto_yaw_mode(autopilot_yaw_mode yaw_mode); + +protected: + + const char *name() const override { return "GUIDED"; } + const char *name4() const override { return "GUID"; } + autopilot_yaw_mode get_default_auto_yaw_mode(bool rtl) const; + +private: + void guided_pos_control_run(); + void guided_vel_control_run(); + void guided_posvel_control_run(); + void guided_angle_control_run(); + void guided_takeoff_run(); + void guided_pos_control_start(); + void guided_vel_control_start(); + void guided_posvel_control_start(); + void guided_angle_control_start(); +}; + + + +class ModeAuto : public ModeGuided +{ + +public: + // inherit constructor + using ModeGuided::ModeGuided; + + virtual void run() override; + + bool init(bool ignore_checks) override; + bool requires_GPS() const override { return true; } + bool has_manual_throttle() const override { return false; } + bool allows_arming(bool from_gcs) const override { return true; } + bool is_autopilot() const override { return true; } + bool auto_loiter_start(); + void auto_wp_start(const Vector3f& destination); + void auto_wp_start(const Location& dest_loc); + void auto_circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn); + void auto_circle_start(); + void auto_nav_guided_start(); + void set_auto_yaw_roi(const Location &roi_location); + void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle); + void set_yaw_rate(float turn_rate_dps); + bool auto_terrain_recover_start(); + +protected: + + const char *name() const override { return "AUTO"; } + const char *name4() const override { return "AUTO"; } + +private: + void auto_wp_run(); + void auto_circle_run(); + void auto_nav_guided_run(); + void auto_loiter_run(); + void auto_terrain_recover_run(); +}; + + +class ModePoshold : public ModeAlthold +{ + +public: + // inherit constructor + using ModeAlthold::ModeAlthold; + + virtual void run() override; + + bool init(bool ignore_checks) override; + + bool requires_GPS() const override { return false; } + bool has_manual_throttle() const override { return false; } + bool allows_arming(bool from_gcs) const override { return true; } + bool is_autopilot() const override { return true; } + +protected: + + const char *name() const override { return "POSHOLD"; } + const char *name4() const override { return "POSH"; } +}; + + +class ModeCircle : public Mode +{ + +public: + // inherit constructor + using Mode::Mode; + + virtual void run() override; + + bool init(bool ignore_checks) override; + bool requires_GPS() const override { return true; } + bool has_manual_throttle() const override { return false; } + bool allows_arming(bool from_gcs) const override { return true; } + bool is_autopilot() const override { return true; } + +protected: + + const char *name() const override { return "CIRCLE"; } + const char *name4() const override { return "CIRC"; } +}; + +class ModeSurface : public Mode +{ + +public: + // inherit constructor + using Mode::Mode; + + virtual void run() override; + + bool init(bool ignore_checks) override; + bool requires_GPS() const override { return true; } + bool has_manual_throttle() const override { return false; } + bool allows_arming(bool from_gcs) const override { return true; } + bool is_autopilot() const override { return true; } + +protected: + + const char *name() const override { return "SURFACE"; } + const char *name4() const override { return "SURF"; } +}; + + +class ModeMotordetect : public Mode +{ + +public: + // inherit constructor + using Mode::Mode; + + virtual void run() override; + + bool init(bool ignore_checks) override; + bool requires_GPS() const override { return true; } + bool has_manual_throttle() const override { return false; } + bool allows_arming(bool from_gcs) const override { return true; } + bool is_autopilot() const override { return true; } + +protected: + + const char *name() const override { return "MOTORDETECT"; } + const char *name4() const override { return "DETE"; } +}; diff --git a/ArduSub/mode_acro.cpp b/ArduSub/mode_acro.cpp new file mode 100644 index 0000000000000..29b1e61891667 --- /dev/null +++ b/ArduSub/mode_acro.cpp @@ -0,0 +1,46 @@ +#include "Sub.h" + + + +#include "Sub.h" + + +bool ModeAcro::init(bool ignore_checks) { + // set target altitude to zero for reporting + position_control->set_pos_target_z_cm(0); + + // attitude hold inputs become thrust inputs in acro mode + // set to neutral to prevent chaotic behavior (esp. roll/pitch) + sub.set_neutral_controls(); + + return true; +} + +void ModeAcro::run() +{ + float target_roll, target_pitch, target_yaw; + + // if not armed set throttle to zero and exit immediately + if (!motors.armed()) { + 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(); + return; + } + + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + + // convert the input to the desired body frame rate + get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw); + + // run attitude controller + attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); + + // output pilot's throttle without angle boost + attitude_control->set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt); + + //control_in is range 0-1000 + //radio_in is raw pwm value + motors.set_forward(channel_forward->norm_input()); + motors.set_lateral(channel_lateral->norm_input()); +} diff --git a/ArduSub/mode_althold.cpp b/ArduSub/mode_althold.cpp new file mode 100644 index 0000000000000..9102ff9f802e7 --- /dev/null +++ b/ArduSub/mode_althold.cpp @@ -0,0 +1,115 @@ +#include "Sub.h" + + +bool ModeAlthold::init(bool ignore_checks) { + if(!sub.control_check_barometer()) { + return false; + } + + // initialize vertical maximum speeds and acceleration + // sets the maximum speed up and down returned by position controller + position_control->set_max_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->set_correction_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + + // initialise position and desired velocity + position_control->init_z_controller(); + + sub.last_pilot_heading = ahrs.yaw_sensor; + + return true; +} + +// althold_run - runs the althold controller +// should be called at 100hz or more +void ModeAlthold::run() +{ + uint32_t tnow = AP_HAL::millis(); + + // initialize vertical speeds and acceleration + position_control->set_max_speed_accel_z(-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_z_controller(motors.get_throttle_hover()); + sub.last_pilot_heading = ahrs.yaw_sensor; + return; + } + + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + + // get pilot desired lean angles + float target_roll, target_pitch; + + // Check if set_attitude_target_no_gps is valid + if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) { + float target_yaw; + Quaternion( + sub.set_attitude_target_no_gps.packet.q + ).to_euler( + target_roll, + target_pitch, + target_yaw + ); + target_roll = degrees(target_roll); + target_pitch = degrees(target_pitch); + target_yaw = degrees(target_yaw); + + attitude_control->input_euler_angle_roll_pitch_yaw(target_roll * 1e2f, target_pitch * 1e2f, target_yaw * 1e2f, true); + return; + } + + sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control->get_althold_lean_angle_max_cd()); + + // get pilot's desired yaw rate + float yaw_input = channel_yaw->pwm_to_angle_dz_trim(channel_yaw->get_dead_zone() * sub.gain, channel_yaw->get_radio_trim()); + float target_yaw_rate = sub.get_pilot_desired_yaw_rate(yaw_input); + + // 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(target_roll, target_pitch, target_yaw_rate); + sub.last_pilot_heading = ahrs.yaw_sensor; + sub.last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading + + } else { // hold current heading + + // this check is required to prevent bounce back after very fast yaw maneuvers + // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped + if (tnow < sub.last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading + target_yaw_rate = 0; // Stop rotation on yaw axis + + // call attitude controller with target yaw rate = 0 to decelerate on yaw axis + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); + sub.last_pilot_heading = ahrs.yaw_sensor; // update heading to hold + + } else { // call attitude controller holding absolute absolute bearing + attitude_control->input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, sub.last_pilot_heading, true); + } + } + + control_depth(); + + motors.set_forward(channel_forward->norm_input()); + motors.set_lateral(channel_lateral->norm_input()); +} + +void ModeAlthold::control_depth() { + float target_climb_rate_cm_s = sub.get_pilot_desired_climb_rate(channel_throttle->get_control_in()); + target_climb_rate_cm_s = constrain_float(target_climb_rate_cm_s, -sub.get_pilot_speed_dn(), g.pilot_speed_up); + + // desired_climb_rate returns 0 when within the deadzone. + //we allow full control to the pilot, but as soon as there's no input, we handle being at surface/bottom + if (fabsf(target_climb_rate_cm_s) < 0.05f) { + if (sub.ap.at_surface) { + position_control->set_pos_target_z_cm(MIN(position_control->get_pos_target_z_cm(), g.surface_depth - 5.0f)); // set target to 5 cm below surface level + } else if (sub.ap.at_bottom) { + position_control->set_pos_target_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_target_z_cm())); // set target to 10 cm above bottom + } + } + + position_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate_cm_s); + position_control->update_z_controller(); + +} diff --git a/ArduSub/control_auto.cpp b/ArduSub/mode_auto.cpp similarity index 54% rename from ArduSub/control_auto.cpp rename to ArduSub/mode_auto.cpp index 4631e8dcef180..d947c8ed0e938 100644 --- a/ArduSub/control_auto.cpp +++ b/ArduSub/mode_auto.cpp @@ -7,42 +7,38 @@ * While in the auto flight mode, navigation or do/now commands can be run. * Code in this file implements the navigation commands */ - -// auto_init - initialise auto controller -bool Sub::auto_init() -{ - if (!position_ok() || mission.num_commands() < 2) { +bool ModeAuto::init(bool ignore_checks) { + if (!sub.position_ok() || sub.mission.num_commands() < 2) { return false; } - auto_mode = Auto_Loiter; + sub.auto_mode = Auto_Loiter; // stop ROI from carrying over from previous runs of the mission // To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check - if (auto_yaw_mode == AUTO_YAW_ROI) { + if (sub.auto_yaw_mode == AUTO_YAW_ROI) { set_auto_yaw_mode(AUTO_YAW_HOLD); } // initialise waypoint controller - wp_nav.wp_and_spline_init(); + sub.wp_nav.wp_and_spline_init(); // clear guided limits guided_limit_clear(); // start/resume the mission (based on MIS_RESTART parameter) - mission.start_or_resume(); + sub.mission.start_or_resume(); return true; } // auto_run - runs the appropriate auto controller // according to the current auto_mode -// should be called at 100hz or more -void Sub::auto_run() +void ModeAuto::run() { - mission.update(); + sub.mission.update(); // call the correct auto controller - switch (auto_mode) { + switch (sub.auto_mode) { case Auto_WP: case Auto_CircleMoveToEdge: @@ -70,42 +66,42 @@ void Sub::auto_run() } // auto_wp_start - initialises waypoint controller to implement flying to a particular destination -void Sub::auto_wp_start(const Vector3f& destination) +void ModeAuto::auto_wp_start(const Vector3f& destination) { - auto_mode = Auto_WP; + sub.auto_mode = Auto_WP; // initialise wpnav (no need to check return status because terrain data is not used) - wp_nav.set_wp_destination(destination, false); + sub.wp_nav.set_wp_destination(destination, false); // 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 != AUTO_YAW_ROI) { + if (sub.auto_yaw_mode != AUTO_YAW_ROI) { set_auto_yaw_mode(get_default_auto_yaw_mode(false)); } } // auto_wp_start - initialises waypoint controller to implement flying to a particular destination -void Sub::auto_wp_start(const Location& dest_loc) +void ModeAuto::auto_wp_start(const Location& dest_loc) { - auto_mode = Auto_WP; + sub.auto_mode = Auto_WP; // send target to waypoint controller - if (!wp_nav.set_wp_destination_loc(dest_loc)) { + if (!sub.wp_nav.set_wp_destination_loc(dest_loc)) { // failure to set destination can only be because of missing terrain data - failsafe_terrain_on_event(); + sub.failsafe_terrain_on_event(); return; } // 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 != AUTO_YAW_ROI) { + if (sub.auto_yaw_mode != AUTO_YAW_ROI) { set_auto_yaw_mode(get_default_auto_yaw_mode(false)); } } // auto_wp_run - runs the auto waypoint controller // called by auto_run at 100hz or more -void Sub::auto_wp_run() +void ModeAuto::auto_wp_run() { // if not armed set throttle to zero and exit immediately if (!motors.armed()) { @@ -114,17 +110,17 @@ void Sub::auto_wp_run() // call attitude controller // Sub vehicles do not stabilize roll/pitch/yaw when disarmed 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(); - wp_nav.wp_and_spline_init(); // Reset xy target + attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->relax_attitude_controllers(); + sub.wp_nav.wp_and_spline_init(); // Reset xy target return; } // process pilot's yaw input float target_yaw_rate = 0; - if (!failsafe.pilot_input) { + if (!sub.failsafe.pilot_input) { // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } @@ -137,13 +133,13 @@ void Sub::auto_wp_run() // TODO logic for terrain tracking target going below fence limit // TODO implement waypoint radius individually for each waypoint based on cmd.p2 // TODO fix auto yaw heading to switch to something appropriate when mission complete and switches to loiter - failsafe_terrain_set_status(wp_nav.update_wpnav()); + sub.failsafe_terrain_set_status(sub.wp_nav.update_wpnav()); /////////////////////// // update xy outputs // float lateral_out, forward_out; - translate_wpnav_rp(lateral_out, forward_out); + sub.translate_wpnav_rp(lateral_out, forward_out); // Send to forward/lateral outputs motors.set_lateral(lateral_out); @@ -151,52 +147,52 @@ void Sub::auto_wp_run() // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle - pos_control.update_z_controller(); + position_control->update_z_controller(); //////////////////////////// // update attitude output // // get pilot desired lean angles float target_roll, target_pitch; - get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); + sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, sub.aparm.angle_max); // call attitude controller - if (auto_yaw_mode == AUTO_YAW_HOLD) { + if (sub.auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch & yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); } else { // roll, pitch from pilot, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, get_auto_heading(), true); + attitude_control->input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, get_auto_heading(), true); } } // auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location -// we assume the caller has set the circle's circle with circle_nav.set_center() +// we assume the caller has set the circle's circle with sub.circle_nav.set_center() // we assume the caller has performed all required GPS_ok checks -void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn) +void ModeAuto::auto_circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn) { // set circle center - circle_nav.set_center(circle_center); + sub.circle_nav.set_center(circle_center); // set circle radius if (!is_zero(radius_m)) { - circle_nav.set_radius_cm(radius_m * 100.0f); + sub.circle_nav.set_radius_cm(radius_m * 100.0f); } // set circle direction by using rate - float current_rate = circle_nav.get_rate(); + float current_rate = sub.circle_nav.get_rate(); current_rate = ccw_turn ? -fabsf(current_rate) : fabsf(current_rate); - circle_nav.set_rate(current_rate); + sub.circle_nav.set_rate(current_rate); // check our distance from edge of circle Vector3f circle_edge_neu; - circle_nav.get_closest_point_on_circle(circle_edge_neu); + sub.circle_nav.get_closest_point_on_circle(circle_edge_neu); float dist_to_edge = (inertial_nav.get_position_neu_cm() - circle_edge_neu).length(); // if more than 3m then fly to edge if (dist_to_edge > 300.0f) { // set the state to move to the edge of the circle - auto_mode = Auto_CircleMoveToEdge; + sub.auto_mode = Auto_CircleMoveToEdge; // convert circle_edge_neu to Location Location circle_edge(circle_edge_neu, Location::AltFrame::ABOVE_ORIGIN); @@ -205,14 +201,14 @@ void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radi circle_edge.set_alt_cm(circle_center.alt, circle_center.get_alt_frame()); // initialise wpnav to move to edge of circle - if (!wp_nav.set_wp_destination_loc(circle_edge)) { + if (!sub.wp_nav.set_wp_destination_loc(circle_edge)) { // failure to set destination can only be because of missing terrain data - failsafe_terrain_on_event(); + sub.failsafe_terrain_on_event(); } // if we are outside the circle, point at the edge, otherwise hold yaw - float dist_to_center = get_horizontal_distance_cm(inertial_nav.get_position_xy_cm().topostype(), circle_nav.get_center().xy()); - if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) { + float dist_to_center = get_horizontal_distance_cm(inertial_nav.get_position_xy_cm().topostype(), sub.circle_nav.get_center().xy()); + if (dist_to_center > sub.circle_nav.get_radius() && dist_to_center > 500) { set_auto_yaw_mode(get_default_auto_yaw_mode(false)); } else { // vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle @@ -225,23 +221,23 @@ void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radi // auto_circle_start - initialises controller to fly a circle in AUTO flight mode // assumes that circle_nav object has already been initialised with circle center and radius -void Sub::auto_circle_start() +void ModeAuto::auto_circle_start() { - auto_mode = Auto_Circle; + sub.auto_mode = Auto_Circle; // initialise circle controller - circle_nav.init(circle_nav.get_center(), circle_nav.center_is_terrain_alt(), circle_nav.get_rate()); + sub.circle_nav.init(sub.circle_nav.get_center(), sub.circle_nav.center_is_terrain_alt(), sub.circle_nav.get_rate()); } // auto_circle_run - circle in AUTO flight mode // called by auto_run at 100hz or more -void Sub::auto_circle_run() +void ModeAuto::auto_circle_run() { // call circle controller - failsafe_terrain_set_status(circle_nav.update()); + sub.failsafe_terrain_set_status(sub.circle_nav.update()); float lateral_out, forward_out; - translate_circle_nav_rp(lateral_out, forward_out); + sub.translate_circle_nav_rp(lateral_out, forward_out); // Send to forward/lateral outputs motors.set_lateral(lateral_out); @@ -249,50 +245,47 @@ void Sub::auto_circle_run() // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle - pos_control.update_z_controller(); + position_control->update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), circle_nav.get_yaw(), true); + attitude_control->input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), sub.circle_nav.get_yaw(), true); } #if NAV_GUIDED == ENABLED // auto_nav_guided_start - hand over control to external navigation controller in AUTO mode -void Sub::auto_nav_guided_start() +void ModeAuto::auto_nav_guided_start() { - auto_mode = Auto_NavGuided; - - // call regular guided flight mode initialisation - guided_init(true); - + sub.mode_guided.init(true); + sub.auto_mode = Auto_NavGuided; // initialise guided start time and position as reference for limit checking - guided_limit_init_time_and_pos(); + sub.mode_auto.guided_limit_init_time_and_pos(); } // auto_nav_guided_run - allows control by external navigation controller // called by auto_run at 100hz or more -void Sub::auto_nav_guided_run() +void ModeAuto::auto_nav_guided_run() { // call regular guided flight mode run function - guided_run(); + sub.mode_guided.run(); } #endif // NAV_GUIDED // auto_loiter_start - initialises loitering in auto mode // returns success/failure because this can be called by exit_mission -bool Sub::auto_loiter_start() +bool ModeAuto::auto_loiter_start() { // return failure if GPS is bad - if (!position_ok()) { + if (!sub.position_ok()) { return false; } - auto_mode = Auto_Loiter; + sub.auto_mode = Auto_Loiter; // calculate stopping point Vector3f stopping_point; - wp_nav.get_wp_stopping_point(stopping_point); + sub.wp_nav.get_wp_stopping_point(stopping_point); // initialise waypoint controller target to stopping point - wp_nav.set_wp_destination(stopping_point); + sub.wp_nav.set_wp_destination(stopping_point); // hold yaw at current heading set_auto_yaw_mode(AUTO_YAW_HOLD); @@ -302,35 +295,35 @@ bool Sub::auto_loiter_start() // auto_loiter_run - loiter in AUTO flight mode // called by auto_run at 100hz or more -void Sub::auto_loiter_run() +void ModeAuto::auto_loiter_run() { // if not armed set throttle to zero and exit immediately if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed - attitude_control.set_throttle_out(0,true,g.throttle_filt); - attitude_control.relax_attitude_controllers(); + attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->relax_attitude_controllers(); - wp_nav.wp_and_spline_init(); // Reset xy target + sub.wp_nav.wp_and_spline_init(); // Reset xy target return; } // accept pilot input of yaw float target_yaw_rate = 0; - if (!failsafe.pilot_input) { - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + if (!sub.failsafe.pilot_input) { + target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); } // set motors to full range motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // run waypoint and z-axis position controller - failsafe_terrain_set_status(wp_nav.update_wpnav()); + sub.failsafe_terrain_set_status(sub.wp_nav.update_wpnav()); /////////////////////// // update xy outputs // float lateral_out, forward_out; - translate_wpnav_rp(lateral_out, forward_out); + sub.translate_wpnav_rp(lateral_out, forward_out); // Send to forward/lateral outputs motors.set_lateral(lateral_out); @@ -338,113 +331,41 @@ void Sub::auto_loiter_run() // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle - pos_control.update_z_controller(); + position_control->update_z_controller(); // get pilot desired lean angles float target_roll, target_pitch; - get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); + sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, sub.aparm.angle_max); // roll & pitch & yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); } -// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter -// set rtl parameter to true if this is during an RTL -uint8_t Sub::get_default_auto_yaw_mode(bool rtl) const -{ - switch (g.wp_yaw_behavior) { - - case WP_YAW_BEHAVIOR_NONE: - return AUTO_YAW_HOLD; - break; - - case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL: - if (rtl) { - return AUTO_YAW_HOLD; - } else { - return AUTO_YAW_LOOK_AT_NEXT_WP; - } - break; - - case WP_YAW_BEHAVIOR_LOOK_AHEAD: - return AUTO_YAW_LOOK_AHEAD; - break; - - case WP_YAW_BEHAVIOR_CORRECT_XTRACK: - return AUTO_YAW_CORRECT_XTRACK; - break; - - case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP: - default: - return AUTO_YAW_LOOK_AT_NEXT_WP; - break; - } -} - -// set_auto_yaw_mode - sets the yaw mode for auto -void Sub::set_auto_yaw_mode(uint8_t yaw_mode) -{ - // return immediately if no change - if (auto_yaw_mode == yaw_mode) { - return; - } - auto_yaw_mode = yaw_mode; - - // perform initialisation - switch (auto_yaw_mode) { - - case AUTO_YAW_LOOK_AT_NEXT_WP: - // wpnav will initialise heading when wpnav's set_destination method is called - break; - - case AUTO_YAW_ROI: - // point towards a location held in yaw_look_at_WP - yaw_look_at_WP_bearing = ahrs.yaw_sensor; - break; - - case AUTO_YAW_LOOK_AT_HEADING: - // keep heading pointing in the direction held in yaw_look_at_heading - // caller should set the yaw_look_at_heading - break; - - case AUTO_YAW_LOOK_AHEAD: - // Commanded Yaw to automatically look ahead. - yaw_look_ahead_bearing = ahrs.yaw_sensor; - break; - - case AUTO_YAW_RESETTOARMEDYAW: - // initial_armed_bearing will be set during arming so no init required - break; - } -} // set_auto_yaw_look_at_heading - sets the yaw look at heading for auto mode -void Sub::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle) +void ModeAuto::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle) { - // get current yaw target - int32_t curr_yaw_target = attitude_control.get_att_target_euler_cd().z; + // get current yaw + int32_t curr_yaw_target = attitude_control->get_att_target_euler_cd().z; // get final angle, 1 = Relative, 0 = Absolute if (relative_angle == 0) { // absolute angle - yaw_look_at_heading = wrap_360_cd(angle_deg * 100); + sub.yaw_look_at_heading = wrap_360_cd(angle_deg * 100); } else { // relative angle if (direction < 0) { angle_deg = -angle_deg; } - yaw_look_at_heading = wrap_360_cd((angle_deg*100+curr_yaw_target)); + sub.yaw_look_at_heading = wrap_360_cd((angle_deg * 100 + curr_yaw_target)); } // get turn speed - // TODO actually implement this, right now, yaw_look_at_heading_slew is unused - // see AP_Float _slew_yaw in AC_AttitudeControl if (is_zero(turn_rate_dps)) { // default to regular auto slew rate - yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE; + sub.yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE; } else { - int32_t turn_rate = (wrap_180_cd(yaw_look_at_heading - curr_yaw_target) / 100) / turn_rate_dps; - yaw_look_at_heading_slew = constrain_int32(turn_rate, 1, 360); // deg / sec + sub.yaw_look_at_heading_slew = MIN(turn_rate_dps, AUTO_YAW_SLEW_RATE); // deg / sec } // set yaw mode @@ -453,8 +374,19 @@ void Sub::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int // TO-DO: restore support for clockwise and counter clockwise rotation held in cmd.content.yaw.direction. 1 = clockwise, -1 = counterclockwise } + +// sets the desired yaw rate +void ModeAuto::set_yaw_rate(float turn_rate_dps) +{ + // set sub to desired yaw rate + sub.yaw_look_at_heading_slew = MIN(turn_rate_dps, AUTO_YAW_SLEW_RATE); // deg / sec + + // set yaw mode + set_auto_yaw_mode(AUTO_YAW_RATE); +} + // set_auto_yaw_roi - sets the yaw to look at roi for auto mode -void Sub::set_auto_yaw_roi(const Location &roi_location) +void ModeAuto::set_auto_yaw_roi(const Location &roi_location) { // if location is zero lat, lon and altitude turn off ROI if (roi_location.alt == 0 && roi_location.lat == 0 && roi_location.lng == 0) { @@ -462,20 +394,18 @@ void Sub::set_auto_yaw_roi(const Location &roi_location) set_auto_yaw_mode(get_default_auto_yaw_mode(false)); #if HAL_MOUNT_ENABLED // switch off the camera tracking if enabled - if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { - camera_mount.set_mode_to_default(); - } + sub.camera_mount.clear_roi_target(); #endif // HAL_MOUNT_ENABLED } else { #if HAL_MOUNT_ENABLED // check if mount type requires us to rotate the sub - if (!camera_mount.has_pan_control()) { - if (roi_location.get_vector_from_origin_NEU(roi_WP)) { + if (!sub.camera_mount.has_pan_control()) { + if (roi_location.get_vector_from_origin_NEU(sub.roi_WP)) { set_auto_yaw_mode(AUTO_YAW_ROI); } } // send the command to the camera mount - camera_mount.set_roi_target(roi_location); + sub.camera_mount.set_roi_target(roi_location); // TO-DO: expand handling of the do_nav_roi to support all modes of the MAVLink. Currently we only handle mode 4 (see below) // 0: do nothing @@ -485,70 +415,18 @@ void Sub::set_auto_yaw_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 sub at the location - if (roi_location.get_vector_from_origin_NEU(roi_WP)) { + if (roi_location.get_vector_from_origin_NEU(sub.roi_WP)) { set_auto_yaw_mode(AUTO_YAW_ROI); } #endif // HAL_MOUNT_ENABLED } } -// get_auto_heading - returns target heading depending upon auto_yaw_mode -// 100hz update rate -float Sub::get_auto_heading() -{ - switch (auto_yaw_mode) { - - case AUTO_YAW_ROI: - // point towards a location held in roi_WP - return get_roi_yaw(); - break; - - case AUTO_YAW_LOOK_AT_HEADING: - // keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed - return yaw_look_at_heading; - break; - - case AUTO_YAW_LOOK_AHEAD: - // Commanded Yaw to automatically look ahead. - return get_look_ahead_yaw(); - break; - - case AUTO_YAW_RESETTOARMEDYAW: - // changes yaw to be same as when quad was armed - return initial_armed_bearing; - break; - - case AUTO_YAW_CORRECT_XTRACK: { - // TODO return current yaw if not in appropriate mode - // Bearing of current track (centidegrees) - float track_bearing = get_bearing_cd(wp_nav.get_wp_origin().xy(), wp_nav.get_wp_destination().xy()); - - // Bearing from current position towards intermediate position target (centidegrees) - const Vector2f target_vel_xy{pos_control.get_vel_target_cms().x, pos_control.get_vel_target_cms().y}; - float angle_error = 0.0f; - if (target_vel_xy.length() >= pos_control.get_max_speed_xy_cms() * 0.1f) { - const float desired_angle_cd = degrees(target_vel_xy.angle()) * 100.0f; - angle_error = wrap_180_cd(desired_angle_cd - track_bearing); - } - float angle_limited = constrain_float(angle_error, -g.xtrack_angle_limit * 100.0f, g.xtrack_angle_limit * 100.0f); - return wrap_360_cd(track_bearing + angle_limited); - } - break; - - case AUTO_YAW_LOOK_AT_NEXT_WP: - default: - // point towards next waypoint. - // we don't use wp_bearing because we don't want the vehicle to turn too much during flight - return wp_nav.get_yaw(); - break; - } -} - // Return true if it is possible to recover from a rangefinder failure -bool Sub::auto_terrain_recover_start() +bool ModeAuto::auto_terrain_recover_start() { // Check rangefinder status to see if recovery is possible - switch (rangefinder.status_orient(ROTATION_PITCH_270)) { + switch (sub.rangefinder.status_orient(ROTATION_PITCH_270)) { case RangeFinder::Status::OutOfRangeLow: case RangeFinder::Status::OutOfRangeHigh: @@ -556,7 +434,7 @@ bool Sub::auto_terrain_recover_start() // RangeFinder::Good if just one valid sample was obtained recently, but ::rangefinder_state.alt_healthy // requires several consecutive valid readings for wpnav to accept rangefinder data case RangeFinder::Status::Good: - auto_mode = Auto_TerrainRecover; + sub.auto_mode = Auto_TerrainRecover; break; // Not connected or no data @@ -565,21 +443,21 @@ bool Sub::auto_terrain_recover_start() } // Initialize recovery timeout time - fs_terrain_recover_start_ms = AP_HAL::millis(); + sub.fs_terrain_recover_start_ms = AP_HAL::millis(); // Stop mission - mission.stop(); + sub.mission.stop(); // Reset xy target - loiter_nav.clear_pilot_desired_acceleration(); - loiter_nav.init_target(); + sub.loiter_nav.clear_pilot_desired_acceleration(); + sub.loiter_nav.init_target(); // Reset z axis controller - pos_control.relax_z_controller(motors.get_throttle_hover()); + position_control->relax_z_controller(motors.get_throttle_hover()); // initialize vertical maximum speeds and acceleration - pos_control.set_max_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); - pos_control.set_correction_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); + position_control->set_max_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z()); + position_control->set_correction_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z()); gcs().send_text(MAV_SEVERITY_WARNING, "Attempting auto failsafe recovery"); return true; @@ -588,7 +466,7 @@ bool Sub::auto_terrain_recover_start() // Attempt recovery from terrain failsafe // If recovery is successful resume mission // If recovery fails revert to failsafe action -void Sub::auto_terrain_recover_run() +void ModeAuto::auto_terrain_recover_run() { float target_climb_rate = 0; static uint32_t rangefinder_recovery_ms = 0; @@ -596,23 +474,23 @@ void Sub::auto_terrain_recover_run() // if not armed set throttle to zero and exit immediately if (!motors.armed()) { 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(); + attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->relax_attitude_controllers(); - loiter_nav.init_target(); // Reset xy target - pos_control.relax_z_controller(motors.get_throttle_hover()); // Reset z axis controller + sub.loiter_nav.init_target(); // Reset xy target + position_control->relax_z_controller(motors.get_throttle_hover()); // Reset z axis controller return; } - switch (rangefinder.status_orient(ROTATION_PITCH_270)) { + switch (sub.rangefinder.status_orient(ROTATION_PITCH_270)) { case RangeFinder::Status::OutOfRangeLow: - target_climb_rate = wp_nav.get_default_speed_up(); + target_climb_rate = sub.wp_nav.get_default_speed_up(); rangefinder_recovery_ms = 0; break; case RangeFinder::Status::OutOfRangeHigh: - target_climb_rate = wp_nav.get_default_speed_down(); + target_climb_rate = sub.wp_nav.get_default_speed_down(); rangefinder_recovery_ms = 0; break; @@ -620,21 +498,21 @@ void Sub::auto_terrain_recover_run() target_climb_rate = 0; // Attempt to hold current depth - if (rangefinder_state.alt_healthy) { + if (sub.rangefinder_state.alt_healthy) { // Start timer as soon as rangefinder is healthy if (rangefinder_recovery_ms == 0) { rangefinder_recovery_ms = AP_HAL::millis(); - pos_control.relax_z_controller(motors.get_throttle_hover()); // Reset alt hold targets + position_control->relax_z_controller(motors.get_throttle_hover()); // Reset alt hold targets } // 1.5 seconds of healthy rangefinder means we can resume mission with terrain enabled if (AP_HAL::millis() > rangefinder_recovery_ms + 1500) { gcs().send_text(MAV_SEVERITY_INFO, "Terrain failsafe recovery successful!"); - failsafe_terrain_set_status(true); // Reset failsafe timers - failsafe.terrain = false; // Clear flag - auto_mode = Auto_Loiter; // Switch back to loiter for next iteration - mission.resume(); // Resume mission + sub.failsafe_terrain_set_status(true); // Reset failsafe timers + sub.failsafe.terrain = false; // Clear flag + sub.auto_mode = Auto_Loiter; // Switch back to loiter for next iteration + sub.mission.resume(); // Resume mission rangefinder_recovery_ms = 0; // Reset for subsequent recoveries } @@ -646,25 +524,25 @@ void Sub::auto_terrain_recover_run() // Terrain failsafe recovery has failed, terrain data is not available // and rangefinder is not connected, or has stopped responding gcs().send_text(MAV_SEVERITY_CRITICAL, "Terrain failsafe recovery failure: No Rangefinder!"); - failsafe_terrain_act(); + sub.failsafe_terrain_act(); rangefinder_recovery_ms = 0; return; } // exit on failure (timeout) - if (AP_HAL::millis() > fs_terrain_recover_start_ms + FS_TERRAIN_RECOVER_TIMEOUT_MS) { + if (AP_HAL::millis() > sub.fs_terrain_recover_start_ms + FS_TERRAIN_RECOVER_TIMEOUT_MS) { // Recovery has failed, revert to failsafe action gcs().send_text(MAV_SEVERITY_CRITICAL, "Terrain failsafe recovery timeout!"); - failsafe_terrain_act(); + sub.failsafe_terrain_act(); } // run loiter controller - loiter_nav.update(); + sub.loiter_nav.update(); /////////////////////// // update xy targets // float lateral_out, forward_out; - translate_wpnav_rp(lateral_out, forward_out); + sub.translate_wpnav_rp(lateral_out, forward_out); // Send to forward/lateral outputs motors.set_lateral(lateral_out); @@ -672,8 +550,8 @@ void Sub::auto_terrain_recover_run() ///////////////////// // update z target // - pos_control.set_pos_target_z_from_climb_rate_cm(target_climb_rate); - pos_control.update_z_controller(); + position_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate); + position_control->update_z_controller(); //////////////////////////// // update angular targets // @@ -681,11 +559,11 @@ void Sub::auto_terrain_recover_run() float target_pitch = 0; // convert pilot input to lean angles - // To-Do: convert get_pilot_desired_lean_angles to return angles as floats - get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); + // To-Do: convert sub.get_pilot_desired_lean_angles to return angles as floats + sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, sub.aparm.angle_max); float target_yaw_rate = 0; // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); } diff --git a/ArduSub/mode_circle.cpp b/ArduSub/mode_circle.cpp new file mode 100644 index 0000000000000..28be79dbad54c --- /dev/null +++ b/ArduSub/mode_circle.cpp @@ -0,0 +1,86 @@ +#include "Sub.h" + +/* + * control_circle.pde - init and run calls for circle flight mode + */ + +// circle_init - initialise circle controller flight mode +bool ModeCircle::init(bool ignore_checks) +{ + if (!sub.position_ok()) { + return false; + } + + sub.circle_pilot_yaw_override = false; + + // initialize speeds and accelerations + position_control->set_max_speed_accel_xy(sub.wp_nav.get_default_speed_xy(), sub.wp_nav.get_wp_acceleration()); + position_control->set_correction_speed_accel_xy(sub.wp_nav.get_default_speed_xy(), sub.wp_nav.get_wp_acceleration()); + position_control->set_max_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->set_correction_speed_accel_z(-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(); + + return true; +} + +// circle_run - runs the circle flight mode +// should be called at 100hz or more +void ModeCircle::run() +{ + float target_yaw_rate = 0; + float target_climb_rate = 0; + + // update parameters, to allow changing at runtime + position_control->set_max_speed_accel_xy(sub.wp_nav.get_default_speed_xy(), sub.wp_nav.get_wp_acceleration()); + position_control->set_max_speed_accel_z(-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()) { + // To-Do: add some initialisation of position controllers + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); + // Sub vehicles do not stabilize roll/pitch/yaw when disarmed + attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->relax_attitude_controllers(); + sub.circle_nav.init(); + return; + } + + // process pilot inputs + // get pilot's desired yaw rate + target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + if (!is_zero(target_yaw_rate)) { + sub.circle_pilot_yaw_override = true; + } + + // get pilot desired climb rate + target_climb_rate = sub.get_pilot_desired_climb_rate(channel_throttle->get_control_in()); + + // set motors to full range + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + + // run circle controller + sub.failsafe_terrain_set_status(sub.circle_nav.update()); + + /////////////////////// + // update xy outputs // + + float lateral_out, forward_out; + sub.translate_circle_nav_rp(lateral_out, forward_out); + + // Send to forward/lateral outputs + motors.set_lateral(lateral_out); + motors.set_forward(forward_out); + + // call attitude controller + if (sub.circle_pilot_yaw_override) { + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); + } else { + attitude_control->input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), sub.circle_nav.get_yaw(), true); + } + + // update altitude target and call position controller + position_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate); + position_control->update_z_controller(); +} diff --git a/ArduSub/mode_guided.cpp b/ArduSub/mode_guided.cpp new file mode 100644 index 0000000000000..25857bf5c94e6 --- /dev/null +++ b/ArduSub/mode_guided.cpp @@ -0,0 +1,866 @@ +#include "Sub.h" + +/* + * Init and run calls for guided flight mode + */ + +#define GUIDED_POSVEL_TIMEOUT_MS 3000 // guided mode's position-velocity controller times out after 3seconds with no new updates +#define GUIDED_ATTITUDE_TIMEOUT_MS 1000 // guided mode's attitude controller times out after 1 second with no new updates + +static Vector3p posvel_pos_target_cm; +static Vector3f posvel_vel_target_cms; +static uint32_t update_time_ms; + +struct { + uint32_t update_time_ms; + float roll_cd; + float pitch_cd; + float yaw_cd; + float climb_rate_cms; +} static guided_angle_state = {0,0.0f, 0.0f, 0.0f, 0.0f}; + +struct Guided_Limit { + uint32_t timeout_ms; // timeout (in seconds) from the time that guided is invoked + float alt_min_cm; // lower altitude limit in cm above home (0 = no limit) + float alt_max_cm; // upper altitude limit in cm above home (0 = no limit) + float horiz_max_cm; // horizontal position limit in cm from where guided mode was initiated (0 = no limit) + uint32_t start_time;// system time in milliseconds that control was handed to the external computer + Vector3f start_pos; // start position as a distance from home in cm. used for checking horiz_max limit +} guided_limit; + +// guided_init - initialise guided controller +bool ModeGuided::init(bool ignore_checks) +{ + if (!sub.position_ok() && !ignore_checks) { + return false; + } + + // start in position control mode + guided_pos_control_start(); + return true; +} + +// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter +// set rtl parameter to true if this is during an RTL +autopilot_yaw_mode ModeGuided::get_default_auto_yaw_mode(bool rtl) const +{ + switch (g.wp_yaw_behavior) { + + case WP_YAW_BEHAVIOR_NONE: + return AUTO_YAW_HOLD; + break; + + case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL: + if (rtl) { + return AUTO_YAW_HOLD; + } else { + return AUTO_YAW_LOOK_AT_NEXT_WP; + } + break; + + case WP_YAW_BEHAVIOR_LOOK_AHEAD: + return AUTO_YAW_LOOK_AHEAD; + break; + + case WP_YAW_BEHAVIOR_CORRECT_XTRACK: + return AUTO_YAW_CORRECT_XTRACK; + break; + + case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP: + default: + return AUTO_YAW_LOOK_AT_NEXT_WP; + break; + } +} + + +// initialise guided mode's position controller +void ModeGuided::guided_pos_control_start() +{ + // set to position control mode + sub.guided_mode = Guided_WP; + + // initialise waypoint controller + sub.wp_nav.wp_and_spline_init(); + + // initialise wpnav to stopping point at current altitude + // To-Do: set to current location if disarmed? + // To-Do: set to stopping point altitude? + Vector3f stopping_point; + sub.wp_nav.get_wp_stopping_point(stopping_point); + + // no need to check return status because terrain data is not used + sub.wp_nav.set_wp_destination(stopping_point, false); + + // initialise yaw + set_auto_yaw_mode(get_default_auto_yaw_mode(false)); +} + +// initialise guided mode's velocity controller +void ModeGuided::guided_vel_control_start() +{ + // set guided_mode to velocity controller + sub.guided_mode = Guided_Velocity; + + // initialize vertical maximum speeds and acceleration + position_control->set_max_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->set_correction_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + + // initialise velocity controller + position_control->init_z_controller(); + position_control->init_xy_controller(); + + // pilot always controls yaw + set_auto_yaw_mode(AUTO_YAW_HOLD); +} + +// initialise guided mode's posvel controller +void ModeGuided::guided_posvel_control_start() +{ + // set guided_mode to velocity controller + sub.guided_mode = Guided_PosVel; + + // set vertical speed and acceleration + position_control->set_max_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z()); + position_control->set_correction_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z()); + + // initialise velocity controller + position_control->init_z_controller(); + position_control->init_xy_controller(); + + // pilot always controls yaw + set_auto_yaw_mode(AUTO_YAW_HOLD); +} + +// initialise guided mode's angle controller +void ModeGuided::guided_angle_control_start() +{ + // set guided_mode to velocity controller + sub.guided_mode = Guided_Angle; + + // set vertical speed and acceleration + position_control->set_max_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z()); + position_control->set_correction_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z()); + + // initialise velocity controller + position_control->init_z_controller(); + + // initialise targets + guided_angle_state.update_time_ms = AP_HAL::millis(); + guided_angle_state.roll_cd = ahrs.roll_sensor; + guided_angle_state.pitch_cd = ahrs.pitch_sensor; + guided_angle_state.yaw_cd = ahrs.yaw_sensor; + guided_angle_state.climb_rate_cms = 0.0f; + + // pilot always controls yaw + set_auto_yaw_mode(AUTO_YAW_HOLD); +} + +// guided_set_destination - sets guided mode's target destination +// 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::guided_set_destination(const Vector3f& destination) +{ + // ensure we are in position control mode + if (sub.guided_mode != Guided_WP) { + guided_pos_control_start(); + } + +#if AP_FENCE_ENABLED + // reject destination if outside the fence + const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN); + if (!sub.fence.check_destination_within_fence(dest_loc)) { + AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); + // failure is propagated to GCS with NAK + return false; + } +#endif + + // no need to check return status because terrain data is not used + sub.wp_nav.set_wp_destination(destination, false); + + // log target + sub.Log_Write_GuidedTarget(sub.guided_mode, destination, Vector3f()); + return true; +} + +// sets guided mode's target from a Location object +// returns false if destination could not be set (probably caused by missing terrain data) +// or if the fence is enabled and guided waypoint is outside the fence +bool ModeGuided::guided_set_destination(const Location& dest_loc) +{ + // ensure we are in position control mode + if (sub.guided_mode != Guided_WP) { + guided_pos_control_start(); + } + +#if AP_FENCE_ENABLED + // reject destination outside the fence. + // Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails + if (!sub.fence.check_destination_within_fence(dest_loc)) { + AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); + // failure is propagated to GCS with NAK + return false; + } +#endif + + if (!sub.wp_nav.set_wp_destination_loc(dest_loc)) { + // failure to set destination can only be because of missing terrain data + AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION); + // failure is propagated to GCS with NAK + return false; + } + + // log target + sub.Log_Write_GuidedTarget(sub.guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f()); + return true; +} + +// guided_set_destination - sets guided mode's target destination and target heading +// 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::guided_set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) +{ + // ensure we are in position control mode + if (sub.guided_mode != Guided_WP) { + guided_pos_control_start(); + } + +#if AP_FENCE_ENABLED + // reject destination if outside the fence + const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN); + if (!sub.fence.check_destination_within_fence(dest_loc)) { + AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); + // failure is propagated to GCS with NAK + return false; + } +#endif + + // set yaw state + guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); + + update_time_ms = AP_HAL::millis(); + + // no need to check return status because terrain data is not used + sub.wp_nav.set_wp_destination(destination, false); + + // log target + sub.Log_Write_GuidedTarget(sub.guided_mode, destination, Vector3f()); + return true; +} + +// guided_set_velocity - sets guided mode's target velocity +void ModeGuided::guided_set_velocity(const Vector3f& velocity) +{ + // check we are in velocity control mode + if (sub.guided_mode != Guided_Velocity) { + guided_vel_control_start(); + } + + update_time_ms = AP_HAL::millis(); + + // set position controller velocity target + position_control->set_vel_desired_cms(velocity); +} + +// guided_set_velocity - sets guided mode's target velocity +void ModeGuided::guided_set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) +{ + // check we are in velocity control mode + if (sub.guided_mode != Guided_Velocity) { + guided_vel_control_start(); + } + + // set yaw state + guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); + + update_time_ms = AP_HAL::millis(); + + // set position controller velocity target + position_control->set_vel_desired_cms(velocity); + +} + +// set guided mode posvel target +bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity) +{ + // check we are in velocity control mode + if (sub.guided_mode != Guided_PosVel) { + guided_posvel_control_start(); + } + +#if AP_FENCE_ENABLED + // reject destination if outside the fence + const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN); + if (!sub.fence.check_destination_within_fence(dest_loc)) { + AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); + // failure is propagated to GCS with NAK + return false; + } +#endif + + update_time_ms = AP_HAL::millis(); + posvel_pos_target_cm = destination.topostype(); + posvel_vel_target_cms = velocity; + + position_control->input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f()); + float dz = posvel_pos_target_cm.z; + position_control->input_pos_vel_accel_z(dz, posvel_vel_target_cms.z, 0); + posvel_pos_target_cm.z = dz; + + // log target + sub.Log_Write_GuidedTarget(sub.guided_mode, destination, velocity); + return true; +} + +// set guided mode posvel target +bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) +{ + // check we are in velocity control mode + if (sub.guided_mode != Guided_PosVel) { + guided_posvel_control_start(); + } + + #if AP_FENCE_ENABLED + // reject destination if outside the fence + const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN); + if (!sub.fence.check_destination_within_fence(dest_loc)) { + AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); + // failure is propagated to GCS with NAK + return false; + } + #endif + + // set yaw state + guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); + + update_time_ms = AP_HAL::millis(); + + posvel_pos_target_cm = destination.topostype(); + posvel_vel_target_cms = velocity; + + position_control->input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f()); + float dz = posvel_pos_target_cm.z; + position_control->input_pos_vel_accel_z(dz, posvel_vel_target_cms.z, 0); + posvel_pos_target_cm.z = dz; + + // log target + sub.Log_Write_GuidedTarget(sub.guided_mode, destination, velocity); + return true; +} + +// set guided mode angle target +void ModeGuided::guided_set_angle(const Quaternion &q, float climb_rate_cms) +{ + // check we are in velocity control mode + if (sub.guided_mode != Guided_Angle) { + guided_angle_control_start(); + } + + // convert quaternion to euler angles + q.to_euler(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd); + guided_angle_state.roll_cd = ToDeg(guided_angle_state.roll_cd) * 100.0f; + guided_angle_state.pitch_cd = ToDeg(guided_angle_state.pitch_cd) * 100.0f; + guided_angle_state.yaw_cd = wrap_180_cd(ToDeg(guided_angle_state.yaw_cd) * 100.0f); + + guided_angle_state.climb_rate_cms = climb_rate_cms; + guided_angle_state.update_time_ms = AP_HAL::millis(); +} + +// helper function to set yaw state and targets +void ModeGuided::guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle) +{ + float current_yaw = wrap_2PI(AP::ahrs().get_yaw()); + float euler_yaw_angle; + float yaw_error; + + euler_yaw_angle = wrap_2PI((yaw_cd * 0.01f)); + yaw_error = wrap_PI(euler_yaw_angle - current_yaw); + + int direction = 0; + if (yaw_error < 0){ + direction = -1; + } else { + direction = 1; + } + + /* + case 1: target yaw only + case 2: target yaw and yaw rate + case 3: target yaw rate only + case 4: hold current yaw + */ + if (use_yaw && !use_yaw_rate) { + sub.yaw_rate_only = false; + sub.mode_auto.set_auto_yaw_look_at_heading(yaw_cd * 0.01f, 0.0f, direction, relative_angle); + } else if (use_yaw && use_yaw_rate) { + sub.yaw_rate_only = false; + sub.mode_auto.set_auto_yaw_look_at_heading(yaw_cd * 0.01f, yaw_rate_cds * 0.01f, direction, relative_angle); + } else if (!use_yaw && use_yaw_rate) { + sub.yaw_rate_only = true; + sub.mode_auto.set_yaw_rate(yaw_rate_cds * 0.01f); + } else{ + sub.yaw_rate_only = false; + set_auto_yaw_mode(AUTO_YAW_HOLD); + } +} + +// guided_run - runs the guided controller +// should be called at 100hz or more +void ModeGuided::run() +{ + // call the correct auto controller + switch (sub.guided_mode) { + + case Guided_WP: + // run position controller + guided_pos_control_run(); + break; + + case Guided_Velocity: + // run velocity controller + guided_vel_control_run(); + break; + + case Guided_PosVel: + // run position-velocity controller + guided_posvel_control_run(); + break; + + case Guided_Angle: + // run angle controller + guided_angle_control_run(); + break; + } +} + +// guided_pos_control_run - runs the guided position controller +// called from guided_run +void ModeGuided::guided_pos_control_run() +{ + // if motors not enabled set throttle to zero and exit immediately + if (!motors.armed()) { + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); + // Sub vehicles do not stabilize roll/pitch/yaw when disarmed + attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->relax_attitude_controllers(); + sub.wp_nav.wp_and_spline_init(); + return; + } + + // process pilot's yaw input + float target_yaw_rate = 0; + if (!sub.failsafe.pilot_input) { + // get pilot's desired yaw rate + target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + if (!is_zero(target_yaw_rate)) { + set_auto_yaw_mode(AUTO_YAW_HOLD); + } else{ + if (sub.yaw_rate_only){ + set_auto_yaw_mode(AUTO_YAW_RATE); + } else{ + set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING); + } + } + } + + // set motors to full range + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + + // run waypoint controller + sub.failsafe_terrain_set_status(sub.wp_nav.update_wpnav()); + + float lateral_out, forward_out; + sub.translate_wpnav_rp(lateral_out, forward_out); + + // Send to forward/lateral outputs + motors.set_lateral(lateral_out); + motors.set_forward(forward_out); + + // WP_Nav has set the vertical position control targets + // run the vertical position controller and set output throttle + position_control->update_z_controller(); + + // call attitude controller + if (sub.auto_yaw_mode == AUTO_YAW_HOLD) { + // roll & pitch & yaw rate from pilot + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); + } else if (sub.auto_yaw_mode == AUTO_YAW_LOOK_AT_HEADING) { + // roll, pitch from pilot, yaw & yaw_rate from auto_control + target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0; + attitude_control->input_euler_angle_roll_pitch_slew_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), target_yaw_rate); + } else if (sub.auto_yaw_mode == AUTO_YAW_RATE) { + // roll, pitch from pilot, yaw_rate from auto_control + target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0; + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); + } else { + // roll, pitch from pilot, yaw heading from auto_heading() + attitude_control->input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true); + } +} + +// guided_vel_control_run - runs the guided velocity controller +// called from guided_run +void ModeGuided::guided_vel_control_run() +{ + // ifmotors not enabled set throttle to zero and exit immediately + if (!motors.armed()) { + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); + // Sub vehicles do not stabilize roll/pitch/yaw when disarmed + attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->relax_attitude_controllers(); + // initialise velocity controller + position_control->init_z_controller(); + position_control->init_xy_controller(); + return; + } + + // process pilot's yaw input + float target_yaw_rate = 0; + if (!sub.failsafe.pilot_input) { + // get pilot's desired yaw rate + target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + if (!is_zero(target_yaw_rate)) { + set_auto_yaw_mode(AUTO_YAW_HOLD); + } else{ + if (sub.yaw_rate_only){ + set_auto_yaw_mode(AUTO_YAW_RATE); + } else{ + set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING); + } + } + } + + // set motors to full range + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + + // set velocity to zero if no updates received for 3 seconds + uint32_t tnow = AP_HAL::millis(); + if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !position_control->get_vel_desired_cms().is_zero()) { + position_control->set_vel_desired_cms(Vector3f(0,0,0)); + } + + position_control->stop_pos_xy_stabilisation(); + // call velocity controller which includes z axis controller + position_control->update_xy_controller(); + + position_control->set_pos_target_z_from_climb_rate_cm(position_control->get_vel_desired_cms().z); + position_control->update_z_controller(); + + float lateral_out, forward_out; + sub.translate_pos_control_rp(lateral_out, forward_out); + + // Send to forward/lateral outputs + motors.set_lateral(lateral_out); + motors.set_forward(forward_out); + + // call attitude controller + if (sub.auto_yaw_mode == AUTO_YAW_HOLD) { + // roll & pitch & yaw rate from pilot + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); + } else if (sub.auto_yaw_mode == AUTO_YAW_LOOK_AT_HEADING) { + // roll, pitch from pilot, yaw & yaw_rate from auto_control + target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0; + attitude_control->input_euler_angle_roll_pitch_slew_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), target_yaw_rate); + } else if (sub.auto_yaw_mode == AUTO_YAW_RATE) { + // roll, pitch from pilot, yaw_rate from auto_control + target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0; + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); + } else { + // roll, pitch from pilot, yaw heading from auto_heading() + attitude_control->input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true); + } +} + +// guided_posvel_control_run - runs the guided posvel controller +// called from guided_run +void ModeGuided::guided_posvel_control_run() +{ + // if motors not enabled set throttle to zero and exit immediately + if (!motors.armed()) { + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); + // Sub vehicles do not stabilize roll/pitch/yaw when disarmed + attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->relax_attitude_controllers(); + // initialise velocity controller + position_control->init_z_controller(); + position_control->init_xy_controller(); + return; + } + + // process pilot's yaw input + float target_yaw_rate = 0; + + if (!sub.failsafe.pilot_input) { + // get pilot's desired yaw rate + target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + if (!is_zero(target_yaw_rate)) { + set_auto_yaw_mode(AUTO_YAW_HOLD); + } else{ + if (sub.yaw_rate_only){ + set_auto_yaw_mode(AUTO_YAW_RATE); + } else{ + set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING); + } + } + } + + // set motors to full range + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + + // set velocity to zero if no updates received for 3 seconds + uint32_t tnow = AP_HAL::millis(); + if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !posvel_vel_target_cms.is_zero()) { + posvel_vel_target_cms.zero(); + } + + // advance position target using velocity target + posvel_pos_target_cm += (posvel_vel_target_cms * position_control->get_dt()).topostype(); + + // send position and velocity targets to position controller + position_control->input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f()); + float pz = posvel_pos_target_cm.z; + position_control->input_pos_vel_accel_z(pz, posvel_vel_target_cms.z, 0); + posvel_pos_target_cm.z = pz; + + // run position controller + position_control->update_xy_controller(); + position_control->update_z_controller(); + + float lateral_out, forward_out; + sub.translate_pos_control_rp(lateral_out, forward_out); + + // Send to forward/lateral outputs + motors.set_lateral(lateral_out); + motors.set_forward(forward_out); + + // call attitude controller + if (sub.auto_yaw_mode == AUTO_YAW_HOLD) { + // roll & pitch & yaw rate from pilot + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); + } else if (sub.auto_yaw_mode == AUTO_YAW_LOOK_AT_HEADING) { + // roll, pitch from pilot, yaw & yaw_rate from auto_control + target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0; + attitude_control->input_euler_angle_roll_pitch_slew_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), target_yaw_rate); + } else if (sub.auto_yaw_mode == AUTO_YAW_RATE) { + // roll, pitch from pilot, and yaw_rate from auto_control + target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0; + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); + } else { + // roll, pitch from pilot, yaw heading from auto_heading() + attitude_control->input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true); + } +} + +// guided_angle_control_run - runs the guided angle controller +// called from guided_run +void ModeGuided::guided_angle_control_run() +{ + // if motors not enabled set throttle to zero and exit immediately + if (!motors.armed()) { + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); + // Sub vehicles do not stabilize roll/pitch/yaw when disarmed + attitude_control->set_throttle_out(0.0f,true,g.throttle_filt); + attitude_control->relax_attitude_controllers(); + // initialise velocity controller + position_control->init_z_controller(); + return; + } + + // constrain desired lean angles + float roll_in = guided_angle_state.roll_cd; + float pitch_in = guided_angle_state.pitch_cd; + float total_in = norm(roll_in, pitch_in); + float angle_max = MIN(attitude_control->get_althold_lean_angle_max_cd(), sub.aparm.angle_max); + if (total_in > angle_max) { + float ratio = angle_max / total_in; + roll_in *= ratio; + pitch_in *= ratio; + } + + // wrap yaw request + float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd); + + // constrain climb rate + float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up()); + + // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds + uint32_t tnow = AP_HAL::millis(); + if (tnow - guided_angle_state.update_time_ms > GUIDED_ATTITUDE_TIMEOUT_MS) { + roll_in = 0.0f; + pitch_in = 0.0f; + climb_rate_cms = 0.0f; + } + + // set motors to full range + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + + // call attitude controller + attitude_control->input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true); + + // call position controller + position_control->set_pos_target_z_from_climb_rate_cm(climb_rate_cms); + position_control->update_z_controller(); +} + +// Guided Limit code + +// guided_limit_clear - clear/turn off guided limits +void ModeGuided::guided_limit_clear() +{ + guided_limit.timeout_ms = 0; + guided_limit.alt_min_cm = 0.0f; + guided_limit.alt_max_cm = 0.0f; + guided_limit.horiz_max_cm = 0.0f; +} + + +// set_auto_yaw_mode - sets the yaw mode for auto +void ModeGuided::set_auto_yaw_mode(autopilot_yaw_mode yaw_mode) +{ + // return immediately if no change + if (sub.auto_yaw_mode == yaw_mode) { + return; + } + sub.auto_yaw_mode = yaw_mode; + + // perform initialisation + switch (sub.auto_yaw_mode) { + + case AUTO_YAW_HOLD: + // pilot controls the heading + break; + + case AUTO_YAW_LOOK_AT_NEXT_WP: + // wpnav will initialise heading when wpnav's set_destination method is called + break; + + case AUTO_YAW_ROI: + // point towards a location held in yaw_look_at_WP + sub.yaw_look_at_WP_bearing = ahrs.yaw_sensor; + break; + + case AUTO_YAW_LOOK_AT_HEADING: + // keep heading pointing in the direction held in yaw_look_at_heading + // caller should set the yaw_look_at_heading + break; + + case AUTO_YAW_LOOK_AHEAD: + // Commanded Yaw to automatically look ahead. + sub.yaw_look_ahead_bearing = ahrs.yaw_sensor; + break; + + case AUTO_YAW_RESETTOARMEDYAW: + // initial_armed_bearing will be set during arming so no init required + break; + + case AUTO_YAW_RATE: + // set target yaw rate to yaw_look_at_heading_slew + break; + } +} + +// get_auto_heading - returns target heading depending upon auto_yaw_mode +// 100hz update rate +float ModeGuided::get_auto_heading() +{ + switch (sub.auto_yaw_mode) { + + case AUTO_YAW_ROI: + // point towards a location held in roi_WP + return sub.get_roi_yaw(); + break; + + case AUTO_YAW_LOOK_AT_HEADING: + // keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed + return sub.yaw_look_at_heading; + break; + + case AUTO_YAW_LOOK_AHEAD: + // Commanded Yaw to automatically look ahead. + return sub.get_look_ahead_yaw(); + break; + + case AUTO_YAW_RESETTOARMEDYAW: + // changes yaw to be same as when quad was armed + return sub.initial_armed_bearing; + break; + + case AUTO_YAW_CORRECT_XTRACK: { + // TODO return current yaw if not in appropriate mode + // Bearing of current track (centidegrees) + float track_bearing = get_bearing_cd(sub.wp_nav.get_wp_origin().xy(), sub.wp_nav.get_wp_destination().xy()); + + // Bearing from current position towards intermediate position target (centidegrees) + const Vector2f target_vel_xy{position_control->get_vel_target_cms().x, position_control->get_vel_target_cms().y}; + float angle_error = 0.0f; + if (target_vel_xy.length() >= position_control->get_max_speed_xy_cms() * 0.1f) { + const float desired_angle_cd = degrees(target_vel_xy.angle()) * 100.0f; + angle_error = wrap_180_cd(desired_angle_cd - track_bearing); + } + float angle_limited = constrain_float(angle_error, -g.xtrack_angle_limit * 100.0f, g.xtrack_angle_limit * 100.0f); + return wrap_360_cd(track_bearing + angle_limited); + } + break; + + case AUTO_YAW_LOOK_AT_NEXT_WP: + default: + // point towards next waypoint. + // we don't use wp_bearing because we don't want the vehicle to turn too much during flight + return sub.wp_nav.get_yaw(); + break; + } +} +// guided_limit_set - set guided timeout and movement limits +void ModeGuided::guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm) +{ + guided_limit.timeout_ms = timeout_ms; + guided_limit.alt_min_cm = alt_min_cm; + guided_limit.alt_max_cm = alt_max_cm; + guided_limit.horiz_max_cm = horiz_max_cm; +} + +// guided_limit_init_time_and_pos - initialise guided start time and position as reference for limit checking +// only called from AUTO mode's auto_nav_guided_start function +void ModeGuided::guided_limit_init_time_and_pos() +{ + // initialise start time + guided_limit.start_time = AP_HAL::millis(); + + // initialise start position from current position + guided_limit.start_pos = inertial_nav.get_position_neu_cm(); +} + +// guided_limit_check - returns true if guided mode has breached a limit +// used when guided is invoked from the NAV_GUIDED_ENABLE mission command +bool ModeGuided::guided_limit_check() +{ + // check if we have passed the timeout + if ((guided_limit.timeout_ms > 0) && (AP_HAL::millis() - guided_limit.start_time >= guided_limit.timeout_ms)) { + return true; + } + + // get current location + const Vector3f& curr_pos = inertial_nav.get_position_neu_cm(); + + // check if we have gone below min alt + if (!is_zero(guided_limit.alt_min_cm) && (curr_pos.z < guided_limit.alt_min_cm)) { + return true; + } + + // check if we have gone above max alt + if (!is_zero(guided_limit.alt_max_cm) && (curr_pos.z > guided_limit.alt_max_cm)) { + return true; + } + + // check if we have gone beyond horizontal limit + if (guided_limit.horiz_max_cm > 0.0f) { + const float horiz_move = get_horizontal_distance_cm(guided_limit.start_pos.xy(), curr_pos.xy()); + if (horiz_move > guided_limit.horiz_max_cm) { + return true; + } + } + + // if we got this far we must be within limits + return false; +} diff --git a/ArduSub/mode_manual.cpp b/ArduSub/mode_manual.cpp new file mode 100644 index 0000000000000..9d6e29892ea91 --- /dev/null +++ b/ArduSub/mode_manual.cpp @@ -0,0 +1,35 @@ +#include "Sub.h" + + +bool ModeManual::init(bool ignore_checks) { + // set target altitude to zero for reporting + position_control->set_pos_target_z_cm(0); + + // attitude hold inputs become thrust inputs in manual mode + // set to neutral to prevent chaotic behavior (esp. roll/pitch) + sub.set_neutral_controls(); + + return true; +} + +// manual_run - runs the manual (passthrough) controller +// should be called at 100hz or more +void ModeManual::run() +{ + // if not armed set throttle to zero and exit immediately + if (!sub.motors.armed()) { + sub.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(); + return; + } + + sub.motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + + sub.motors.set_roll(channel_roll->norm_input()); + sub.motors.set_pitch(channel_pitch->norm_input()); + sub.motors.set_yaw(channel_yaw->norm_input() * g.acro_yaw_p / ACRO_YAW_P); + sub.motors.set_throttle(channel_throttle->norm_input()); + sub.motors.set_forward(channel_forward->norm_input()); + sub.motors.set_lateral(channel_lateral->norm_input()); +} diff --git a/ArduSub/control_motordetect.cpp b/ArduSub/mode_motordetect.cpp similarity index 96% rename from ArduSub/control_motordetect.cpp rename to ArduSub/mode_motordetect.cpp index 1718de53b421d..370e1838ce182 100644 --- a/ArduSub/control_motordetect.cpp +++ b/ArduSub/mode_motordetect.cpp @@ -45,7 +45,7 @@ namespace { static int16_t current_direction; } -bool Sub::motordetect_init() +bool ModeMotordetect::init(bool ignore_checks) { current_motor = 0; md_state = STANDBY; @@ -53,7 +53,7 @@ bool Sub::motordetect_init() return true; } -void Sub::motordetect_run() +void ModeMotordetect::run() { // if not armed set throttle to zero and exit immediately if (!motors.armed()) { @@ -167,8 +167,8 @@ void Sub::motordetect_run() break; } case DONE: - control_mode = prev_control_mode; - arming.disarm(AP_Arming::Method::MOTORDETECTDONE); + set_mode(sub.prev_control_mode, ModeReason::MISSION_END); + sub.arming.disarm(AP_Arming::Method::MOTORDETECTDONE); break; } } diff --git a/ArduSub/mode_poshold.cpp b/ArduSub/mode_poshold.cpp new file mode 100644 index 0000000000000..a5cb3d9bfd050 --- /dev/null +++ b/ArduSub/mode_poshold.cpp @@ -0,0 +1,114 @@ +// ArduSub position hold flight mode +// GPS required +// Jacob Walser August 2016 + +#include "Sub.h" + +#if POSHOLD_ENABLED == ENABLED + +// poshold_init - initialise PosHold controller +bool ModePoshold::init(bool ignore_checks) +{ + // fail to initialise PosHold mode if no GPS lock + if (!sub.position_ok()) { + return false; + } + + // initialize vertical speeds and acceleration + position_control->set_max_speed_accel_xy(sub.wp_nav.get_default_speed_xy(), sub.wp_nav.get_wp_acceleration()); + position_control->set_correction_speed_accel_xy(sub.wp_nav.get_default_speed_xy(), sub.wp_nav.get_wp_acceleration()); + position_control->set_max_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->set_correction_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + + // initialise position and desired velocity + position_control->init_xy_controller_stopping_point(); + position_control->init_z_controller(); + + // Stop all thrusters + attitude_control->set_throttle_out(0.5f ,true, g.throttle_filt); + attitude_control->relax_attitude_controllers(); + position_control->relax_z_controller(0.5f); + + sub.last_pilot_heading = ahrs.yaw_sensor; + + return true; +} + +// poshold_run - runs the PosHold controller +// should be called at 100hz or more +void ModePoshold::run() +{ + uint32_t tnow = AP_HAL::millis(); + // When unarmed, disable motors and stabilization + 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.5f ,true, g.throttle_filt); + attitude_control->relax_attitude_controllers(); + position_control->init_xy_controller_stopping_point(); + position_control->relax_z_controller(0.5f); + sub.last_pilot_heading = ahrs.yaw_sensor; + return; + } + + // set motors to full range + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + + /////////////////////// + // update xy outputs // + float pilot_lateral = channel_lateral->norm_input(); + float pilot_forward = channel_forward->norm_input(); + + float lateral_out = 0; + float forward_out = 0; + + if (sub.position_ok()) { + // Allow pilot to reposition the sub + if (fabsf(pilot_lateral) > 0.1 || fabsf(pilot_forward) > 0.1) { + position_control->init_xy_controller_stopping_point(); + } + sub.translate_pos_control_rp(lateral_out, forward_out); + position_control->update_xy_controller(); + } else { + position_control->init_xy_controller_stopping_point(); + } + motors.set_forward(forward_out + pilot_forward); + motors.set_lateral(lateral_out + pilot_lateral); + ///////////////////// + // Update attitude // + + // get pilot's desired yaw rate + float yaw_input = channel_yaw->pwm_to_angle_dz_trim(channel_yaw->get_dead_zone() * sub.gain, channel_yaw->get_radio_trim()); + float target_yaw_rate = sub.get_pilot_desired_yaw_rate(yaw_input); + + // convert pilot input to lean angles + // To-Do: convert get_pilot_desired_lean_angles to return angles as floats + float target_roll, target_pitch; + sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, sub.aparm.angle_max); + + // 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(target_roll, target_pitch, target_yaw_rate); + sub.last_pilot_heading = ahrs.yaw_sensor; + sub.last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading + + } else { // hold current heading + + // this check is required to prevent bounce back after very fast yaw manoeuvres + // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped + if (tnow < sub.last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading + target_yaw_rate = 0; // Stop rotation on yaw axis + + // call attitude controller with target yaw rate = 0 to decelerate on yaw axis + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); + sub.last_pilot_heading = ahrs.yaw_sensor; // update heading to hold + + } else { // call attitude controller holding absolute absolute bearing + attitude_control->input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, sub.last_pilot_heading, true); + } + } + + // Update z axis // + control_depth(); +} +#endif // POSHOLD_ENABLED == ENABLED diff --git a/ArduSub/mode_stabilize.cpp b/ArduSub/mode_stabilize.cpp new file mode 100644 index 0000000000000..52620b8d5d0b6 --- /dev/null +++ b/ArduSub/mode_stabilize.cpp @@ -0,0 +1,69 @@ +#include "Sub.h" + + +bool ModeStabilize::init(bool ignore_checks) { + // set target altitude to zero for reporting + position_control->set_pos_target_z_cm(0); + sub.last_pilot_heading = ahrs.yaw_sensor; + + return true; + return true; +} + +void ModeStabilize::run() +{ + uint32_t tnow = AP_HAL::millis(); + float target_roll, target_pitch; + + // if not armed set throttle to zero and exit immediately + if (!motors.armed()) { + 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; + return; + } + + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + + // convert pilot input to lean angles + // To-Do: convert sub.get_pilot_desired_lean_angles to return angles as floats + // TODO2: move into mode.h + sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, sub.aparm.angle_max); + + // get pilot's desired yaw rate + float yaw_input = channel_yaw->pwm_to_angle_dz_trim(channel_yaw->get_dead_zone() * sub.gain, channel_yaw->get_radio_trim()); + float target_yaw_rate = sub.get_pilot_desired_yaw_rate(yaw_input); + + // call attitude controller + // 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(target_roll, target_pitch, target_yaw_rate); + sub.last_pilot_heading = ahrs.yaw_sensor; + sub.last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading + + } else { // hold current heading + + // this check is required to prevent bounce back after very fast yaw maneuvers + // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped + if (tnow < sub.last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading + target_yaw_rate = 0; // Stop rotation on yaw axis + + // call attitude controller with target yaw rate = 0 to decelerate on yaw axis + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); + sub.last_pilot_heading = ahrs.yaw_sensor; // update heading to hold + + } else { // call attitude controller holding absolute absolute bearing + attitude_control->input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, sub.last_pilot_heading, true); + } + } + + // output pilot's throttle + attitude_control->set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt); + + //control_in is range -1000-1000 + //radio_in is raw pwm value + motors.set_forward(channel_forward->norm_input()); + motors.set_lateral(channel_lateral->norm_input()); +} diff --git a/ArduSub/mode_surface.cpp b/ArduSub/mode_surface.cpp new file mode 100644 index 0000000000000..6bab07a067b86 --- /dev/null +++ b/ArduSub/mode_surface.cpp @@ -0,0 +1,63 @@ +#include "Sub.h" + + +bool ModeSurface::init(bool ignore_checks) +{ + if(!sub.control_check_barometer()) { + return false; + } + + // initialize vertical speeds and acceleration + position_control->set_max_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->set_correction_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + + // initialise position and desired velocity + position_control->init_z_controller(); + + return true; + +} + +void ModeSurface::run() +{ + float target_roll, target_pitch; + + // if not armed set throttle to zero and exit immediately + if (!motors.armed()) { + motors.output_min(); + 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_z_controller(); + return; + } + + // Already at surface, hold depth at surface + if (sub.ap.at_surface) { + set_mode(Mode::Number::ALT_HOLD, ModeReason::SURFACE_COMPLETE); + } + + // convert pilot input to lean angles + // To-Do: convert sub.get_pilot_desired_lean_angles to return angles as floats + sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, sub.aparm.angle_max); + + // get pilot's desired yaw rate + float target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + + // call attitude controller + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); + + // set target climb rate + float cmb_rate = constrain_float(fabsf(sub.wp_nav.get_default_speed_up()), 1, position_control->get_max_speed_up_cms()); + + // record desired climb rate for logging + sub.desired_climb_rate = cmb_rate; + + // update altitude target and call position controller + position_control->set_pos_target_z_from_climb_rate_cm(cmb_rate); + position_control->update_z_controller(); + + // pilot has control for repositioning + motors.set_forward(channel_forward->norm_input()); + motors.set_lateral(channel_lateral->norm_input()); +} diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 5d33b5052b40e..895ade8c8c53b 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -10,7 +10,7 @@ void Sub::enable_motor_output() void Sub::motors_output() { // Motor detection mode controls the thrusters directly - if (control_mode == MOTOR_DETECT){ + if (control_mode == Mode::Number::MOTOR_DETECT){ return; } // check if we are performing the motor test @@ -78,7 +78,7 @@ bool Sub::verify_motor_test() return true; } -bool Sub::handle_do_motor_test(mavlink_command_long_t command) { +bool Sub::handle_do_motor_test(mavlink_command_int_t command) { last_do_motor_test_ms = AP_HAL::millis(); // If we are not already testing motors, initialize test @@ -103,9 +103,9 @@ bool Sub::handle_do_motor_test(mavlink_command_long_t command) { float throttle = command.param3; // float timeout_s = command.param4; // not used // float motor_count = command.param5; // not used - float test_type = command.param6; + const uint32_t test_type = command.y; - if (!is_equal(test_type, (float)MOTOR_TEST_ORDER_BOARD)) { + if (test_type != MOTOR_TEST_ORDER_BOARD) { gcs().send_text(MAV_SEVERITY_WARNING, "bad test type %0.2f", (double)test_type); return false; // test type not supported here } diff --git a/ArduSub/script_button.cpp b/ArduSub/script_button.cpp new file mode 100644 index 0000000000000..fa8f85079636d --- /dev/null +++ b/ArduSub/script_button.cpp @@ -0,0 +1,47 @@ +#include "AP_Scripting/AP_Scripting_config.h" + +#if AP_SCRIPTING_ENABLED + +#include +#include "script_button.h" + +void ScriptButton::press() +{ + if (!pressed) { + pressed = true; + + // The count will max out at 255, but it won't roll over to 0. + if (count < std::numeric_limits::max()) { + count++; + } + } +} + +void ScriptButton::release() +{ + pressed = false; +} + +bool ScriptButton::is_pressed() const +{ + return pressed; +} + +uint8_t ScriptButton::get_count() const +{ + return count; +} + +void ScriptButton::clear_count() +{ + count = 0; +} + +uint8_t ScriptButton::get_and_clear_count() +{ + auto result = get_count(); + clear_count(); + return result; +} + +#endif // AP_SCRIPTING_ENABLED diff --git a/ArduSub/script_button.h b/ArduSub/script_button.h new file mode 100644 index 0000000000000..31a08a2400409 --- /dev/null +++ b/ArduSub/script_button.h @@ -0,0 +1,34 @@ +#pragma once + +#if AP_SCRIPTING_ENABLED + +#include + +// Joystick button object for use in Lua scripts. +// +// Provide 2 ways to use a joystick button: +// is_pressed() returns true if the button is currently (as of the most recent MANUAL_CONTROL msg) pressed +// get_and_clear_count() returns the number of times the button was pressed since the last call +// +class ScriptButton { +public: + ScriptButton(): pressed(false), count(0) {} + + void press(); + + void release(); + + bool is_pressed() const WARN_IF_UNUSED; + + uint8_t get_count() const WARN_IF_UNUSED; + + void clear_count(); + + uint8_t get_and_clear_count(); + +private: + bool pressed; + uint8_t count; +}; + +#endif // AP_SCRIPTING_ENABLED diff --git a/ArduSub/surface_bottom_detector.cpp b/ArduSub/surface_bottom_detector.cpp index 4a1b4cd215edd..0338e77c99d4c 100644 --- a/ArduSub/surface_bottom_detector.cpp +++ b/ArduSub/surface_bottom_detector.cpp @@ -7,7 +7,7 @@ static uint32_t bottom_detector_count = 0; static uint32_t surface_detector_count = 0; static float current_depth = 0; -// checks if we have have hit bottom or surface and updates the ap.at_bottom and ap.at_surface flags +// checks if we have hit bottom or surface and updates the ap.at_bottom and ap.at_surface flags // called at MAIN_LOOP_RATE // ToDo: doesn't need to be called this fast void Sub::update_surface_and_bottom_detector() diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index 7ac209a87f17b..487abe625e063 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -19,6 +19,11 @@ void Sub::init_ardupilot() can_mgr.init(); #endif +#if STATS_ENABLED == ENABLED + // initialise stats module + g2.stats.init(); +#endif + // init cargo gripper #if AP_GRIPPER_ENABLED g2.gripper.init(); @@ -70,7 +75,9 @@ void Sub::init_ardupilot() init_rc_out(); // sets up motors and output to escs init_joystick(); // joystick initialization +#if AP_RELAY_ENABLED relay.init(); +#endif /* * setup the 'main loop is dead' check. Note that this relies on @@ -97,7 +104,7 @@ void Sub::init_ardupilot() #if HAL_MOUNT_ENABLED // initialise camera mount camera_mount.init(); - // This step ncessary so the servo is properly initialized + // This step is necessary so that the servo is properly initialized camera_mount.set_angle_target(0, 0, 0, false); // for some reason the call to set_angle_targets changes the mode to mavlink targeting! camera_mount.set_mode(MAV_MOUNT_MODE_RC_TARGETING); @@ -164,11 +171,6 @@ void Sub::init_ardupilot() g2.scripting.init(); #endif // AP_SCRIPTING_ENABLED - // we don't want writes to the serial port to cause us to pause - // mid-flight, so set the serial ports non-blocking once we are - // ready to fly - serial_manager.set_blocking_writes_all(false); - // enable CPU failsafe mainloop_failsafe_enable(); diff --git a/ArduSub/version.h b/ArduSub/version.h index 5671b5c762863..b163a7bf69b20 100644 --- a/ArduSub/version.h +++ b/ArduSub/version.h @@ -6,13 +6,13 @@ #include "ap_version.h" -#define THISFIRMWARE "ArduSub V4.2.0-dev" +#define THISFIRMWARE "ArduSub V4.5.0-dev" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,2,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 4,5,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 4 -#define FW_MINOR 2 +#define FW_MINOR 5 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV diff --git a/ArduSub/wscript b/ArduSub/wscript index de8dc48a7b984..89da451ee21e7 100644 --- a/ArduSub/wscript +++ b/ArduSub/wscript @@ -18,7 +18,6 @@ def build(bld): 'AP_Beacon', 'AP_TemperatureSensor', 'AP_Arming', - 'AP_KDECAN', 'AP_OSD', ], ) diff --git a/BUILD.md b/BUILD.md index 0143d70da6258..ae32c0a9107e7 100644 --- a/BUILD.md +++ b/BUILD.md @@ -8,18 +8,17 @@ git clone --recursive https://github.com/ArduPilot/ardupilot.git cd ardupilot ``` -Ardupilot is gradually moving from the make-based build system to -[Waf](https://waf.io/). The instructions below should be enough for you to -build Ardupilot, but you can also read more about the build system in the +You can also read more about the build system in the [Waf Book](https://waf.io/book/). -Waf should always be called from the ardupilot's root directory. Differently -from the make-based build, with Waf there's a configure step to choose the -board to be used (default is `sitl`). +waf should always be called from the locally cloned ardupilot root directory for the local branch you are trying to build from. + +**Note** +Do not run `waf` with `sudo`! This leads to permission and environment problems. ## Basic usage ## -There are several commands in the build system for advanced usages, but here we +There are several commands in the build system for advanced usage, but here we list some basic and more used commands as example. * **Build ArduCopter** @@ -89,6 +88,7 @@ list some basic and more used commands as example. ./waf rover # Ground-based rovers and surface boats ./waf sub # ROV and other submarines ./waf antennatracker # Antenna trackers + ./waf AP_Periph # AP Peripheral ``` @@ -164,6 +164,18 @@ list some basic and more used commands as example. ./waf --targets tests/test_math ``` +* **Use clang instead of gcc** + + Currently, gcc is the default on linux, and clang is used for MacOS. + Building with clang on linux can be accomplished by setting the CXX + environment variables during the configure step, e.g.: + + ``` + CXX=clang++ CC=clang ./waf configure --board=sitl + ``` + + Note: Your clang binary names may differ. + * **Other options** It's possible to see all available commands and options: diff --git a/Blimp/AP_Arming.cpp b/Blimp/AP_Arming.cpp index c72d250cc2dbc..23ba7790f9a17 100644 --- a/Blimp/AP_Arming.cpp +++ b/Blimp/AP_Arming.cpp @@ -107,7 +107,7 @@ bool AP_Arming_Blimp::parameter_checks(bool display_failure) // failsafe parameter checks if (blimp.g.failsafe_throttle) { // check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900 - if (blimp.channel_down->get_radio_min() <= blimp.g.failsafe_throttle_value+10 || blimp.g.failsafe_throttle_value < 910) { + if (blimp.channel_up->get_radio_min() <= blimp.g.failsafe_throttle_value+10 || blimp.g.failsafe_throttle_value < 910) { check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check FS_THR_VALUE"); return false; } @@ -312,7 +312,7 @@ bool AP_Arming_Blimp::arm(const AP_Arming::Method method, const bool do_arming_c AP::notify().update(); } - gcs().send_text(MAV_SEVERITY_INFO, "Arming motors"); //MIR kept in - usually only in SITL + send_arm_disarm_statustext("Arming motors"); //MIR kept in - usually only in SITL auto &ahrs = AP::ahrs(); @@ -371,8 +371,7 @@ bool AP_Arming_Blimp::disarm(const AP_Arming::Method method, bool do_disarm_chec return false; } - gcs().send_text(MAV_SEVERITY_INFO, "Disarming motors"); //MIR keeping in - usually only in SITL - + send_arm_disarm_statustext("Disarming motors"); //MIR keeping in - usually only in SITL auto &ahrs = AP::ahrs(); diff --git a/Blimp/Blimp.cpp b/Blimp/Blimp.cpp index 9c9f457b13716..fc03cc4c8fb04 100644 --- a/Blimp/Blimp.cpp +++ b/Blimp/Blimp.cpp @@ -53,7 +53,7 @@ const AP_Scheduler::Task Blimp::scheduler_tasks[] = { FAST_TASK_CLASS(AP_InertialSensor, &blimp.ins, update), // send outputs to the motors library immediately FAST_TASK(motors_output), - // run EKF state estimator (expensive) + // run EKF state estimator (expensive) FAST_TASK(read_AHRS), // Inertial Nav FAST_TASK(read_inertia), @@ -72,7 +72,9 @@ const AP_Scheduler::Task Blimp::scheduler_tasks[] = { SCHED_TASK(arm_motors_check, 10, 50, 18), SCHED_TASK(update_altitude, 10, 100, 21), SCHED_TASK(three_hz_loop, 3, 75, 24), +#if AP_SERVORELAYEVENTS_ENABLED SCHED_TASK_CLASS(AP_ServoRelayEvents, &blimp.ServoRelayEvents, update_events, 50, 75, 27), +#endif SCHED_TASK_CLASS(AP_Baro, &blimp.barometer, accumulate, 50, 90, 30), #if LOGGING_ENABLED == ENABLED SCHED_TASK(full_rate_logging, 50, 50, 33), @@ -207,6 +209,8 @@ void Blimp::one_hz_loop() SRV_Channels::enable_aux_servos(); AP_Notify::flags.flying = !ap.land_complete; + + blimp.pid_pos_yaw.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); } void Blimp::read_AHRS(void) @@ -215,12 +219,27 @@ void Blimp::read_AHRS(void) ahrs.update(true); IGNORE_RETURN(ahrs.get_velocity_NED(vel_ned)); - IGNORE_RETURN(ahrs.get_relative_position_NED_home(pos_ned)); + IGNORE_RETURN(ahrs.get_relative_position_NED_origin(pos_ned)); vel_yaw = ahrs.get_yaw_rate_earth(); Vector2f vel_xy_filtd = vel_xy_filter.apply({vel_ned.x, vel_ned.y}); vel_ned_filtd = {vel_xy_filtd.x, vel_xy_filtd.y, vel_z_filter.apply(vel_ned.z)}; vel_yaw_filtd = vel_yaw_filter.apply(vel_yaw); + + AP::logger().WriteStreaming("VNF", "TimeUS,X,XF,Y,YF,Z,ZF,Yaw,YawF,PX,PY,PZ,PYaw", "Qffffffffffff", + AP_HAL::micros64(), + vel_ned.x, + vel_ned_filtd.x, + vel_ned.y, + vel_ned_filtd.y, + vel_ned.z, + vel_ned_filtd.z, + vel_yaw, + vel_yaw_filtd, + pos_ned.x, + pos_ned.y, + pos_ned.z, + blimp.ahrs.get_yaw()); } // read baro and log control tuning diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index 0c6e7a2627e37..32ac39b5bddf4 100644 --- a/Blimp/Blimp.h +++ b/Blimp/Blimp.h @@ -59,6 +59,7 @@ #include "config.h" #include "Fins.h" +#include "Loiter.h" #include "RC_Channel.h" // RC Channel Library @@ -91,8 +92,10 @@ class Blimp : public AP_Vehicle friend class ModeLand; friend class ModeVelocity; friend class ModeLoiter; + friend class ModeRTL; friend class Fins; + friend class Loiter; Blimp(void); @@ -108,7 +111,7 @@ class Blimp : public AP_Vehicle // primary input control channels RC_Channel *channel_right; RC_Channel *channel_front; - RC_Channel *channel_down; + RC_Channel *channel_up; RC_Channel *channel_yaw; AP_Logger logger; @@ -172,7 +175,7 @@ class Blimp : public AP_Vehicle RCMapper rcmap; - // intertial nav alt when we armed + // inertial nav alt when we armed float arming_altitude_m; // Failsafe @@ -191,6 +194,7 @@ class Blimp : public AP_Vehicle // Motor Output Fins *motors; + Loiter *loiter; int32_t _home_bearing; uint32_t _home_distance; @@ -360,8 +364,8 @@ class Blimp : public AP_Vehicle void Log_Write_Data(LogDataID id, float value); void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max); void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target); - void Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out); - void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z); + + void Log_Write_Vehicle_Startup_Messages(); void log_init(void); void Write_FINI(float right, float front, float down, float yaw); @@ -378,7 +382,7 @@ class Blimp : public AP_Vehicle void notify_flight_mode(); // mode_land.cpp - void set_mode_land_with_pause(ModeReason reason); + void set_mode_land_failsafe(ModeReason reason); bool landing_with_GPS(); // // motors.cpp @@ -430,6 +434,7 @@ class Blimp : public AP_Vehicle ModeLand mode_land; ModeVelocity mode_velocity; ModeLoiter mode_loiter; + ModeRTL mode_rtl; // mode.cpp Mode *mode_from_mode_num(const Mode::Number mode); diff --git a/Blimp/Fins.h b/Blimp/Fins.h index e51a465b3690b..89541eea7cd97 100644 --- a/Blimp/Fins.h +++ b/Blimp/Fins.h @@ -10,6 +10,7 @@ class Fins { public: friend class Blimp; + friend class Loiter; enum motor_frame_class { MOTOR_FRAME_UNDEFINED = 0, @@ -49,7 +50,7 @@ class Fins uint16_t _speed_hz; // speed in hz to send updates to motors float _throttle_avg_max; // last throttle input from set_throttle_avg_max - float _time; //current timestep + float _time; // current timestamp bool _armed; // 0 if disarmed, 1 if armed @@ -70,7 +71,7 @@ class Fins int8_t _num_added; -//MIR This should probably become private in future. + //MIR This should probably become private in future. public: float right_out; //input right movement, negative for left, +1 to -1 float front_out; //input front/forwards movement, negative for backwards, +1 to -1 @@ -95,7 +96,7 @@ class Fins float get_throttle() { //Only for Mavlink - essentially just an indicator of how hard the fins are working. - //Note that this is the unconstrained version, so if the higher level control gives too high input, + //Note that this is the unconstrained version, so if the higher level control gives too high input, //throttle will be displayed as more than 100. return fmaxf(fmaxf(fabsf(down_out),fabsf(front_out)), fmaxf(fabsf(right_out),fabsf(yaw_out))); } diff --git a/Blimp/GCS_Blimp.h b/Blimp/GCS_Blimp.h index c07facca56b98..830e0abc20943 100644 --- a/Blimp/GCS_Blimp.h +++ b/Blimp/GCS_Blimp.h @@ -25,10 +25,10 @@ class GCS_Blimp : public GCS bool vehicle_initialised() const override; -protected: - uint8_t sysid_this_mav() const override; +protected: + // minimum amount of time (in microseconds) that must remain in // the main scheduler loop before we are allowed to send any // mavlink messages. We want to prioritise the main flight diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index 515a2870b6b9c..8eaf6473dd973 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/Blimp/GCS_Mavlink.cpp @@ -2,6 +2,7 @@ #include "GCS_Mavlink.h" #include +#include MAV_TYPE GCS_Blimp::frame_type() const { @@ -52,8 +53,8 @@ void GCS_MAVLINK_Blimp::send_position_target_global_int() } static constexpr uint16_t POSITION_TARGET_TYPEMASK_LAST_BYTE = 0xF000; static constexpr uint16_t 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 | POSITION_TARGET_TYPEMASK_LAST_BYTE; + 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 | POSITION_TARGET_TYPEMASK_LAST_BYTE; mavlink_msg_position_target_global_int_send( chan, @@ -102,11 +103,11 @@ int16_t GCS_MAVLINK_Blimp::vfr_hud_throttle() const */ void GCS_MAVLINK_Blimp::send_pid_tuning() { - if(blimp.control_mode == Mode::Number::MANUAL || blimp.control_mode == Mode::Number::LAND) { + if (blimp.control_mode == Mode::Number::MANUAL || blimp.control_mode == Mode::Number::LAND) { //No PIDs are used in Manual or Land mode. return; } - + static const int8_t axes[] = { PID_SEND::VELX, PID_SEND::VELY, @@ -315,15 +316,21 @@ static const ap_message STREAM_RAW_SENSORS_msgs[] = { static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { MSG_SYS_STATUS, MSG_POWER_STATUS, +#if HAL_WITH_MCU_MONITORING MSG_MCU_STATUS, +#endif MSG_MEMINFO, MSG_CURRENT_WAYPOINT, // MISSION_CURRENT MSG_GPS_RAW, MSG_GPS_RTK, +#if GPS_MAX_RECEIVERS > 1 MSG_GPS2_RAW, MSG_GPS2_RTK, +#endif MSG_NAV_CONTROLLER_OUTPUT, +#if AP_FENCE_ENABLED MSG_FENCE_STATUS, +#endif MSG_POSITION_TARGET_GLOBAL_INT, }; static const ap_message STREAM_POSITION_msgs[] = { @@ -337,7 +344,9 @@ static const ap_message STREAM_RC_CHANNELS_msgs[] = { }; static const ap_message STREAM_EXTRA1_msgs[] = { MSG_ATTITUDE, +#if AP_SIM_ENABLED MSG_SIMSTATE, +#endif MSG_AHRS2, MSG_PID_TUNING // Up to four PID_TUNING messages are sent, depending on GCS_PID_MASK parameter }; @@ -350,25 +359,39 @@ static const ap_message STREAM_EXTRA3_msgs[] = { MSG_WIND, MSG_RANGEFINDER, MSG_DISTANCE_SENSOR, +#if AP_BATTERY_ENABLED MSG_BATTERY_STATUS, +#endif +#if HAL_MOUNT_ENABLED MSG_GIMBAL_DEVICE_ATTITUDE_STATUS, +#endif +#if AP_OPTICALFLOW_ENABLED MSG_OPTICAL_FLOW, +#endif +#if COMPASS_CAL_ENABLED MSG_MAG_CAL_REPORT, MSG_MAG_CAL_PROGRESS, +#endif MSG_EKF_STATUS_REPORT, MSG_VIBRATION, #if AP_RPM_ENABLED MSG_RPM, #endif +#if HAL_WITH_ESC_TELEM MSG_ESC_TELEMETRY, +#endif +#if HAL_GENERATOR_ENABLED MSG_GENERATOR_STATUS, +#endif }; static const ap_message STREAM_PARAMS_msgs[] = { MSG_NEXT_PARAM }; static const ap_message STREAM_ADSB_msgs[] = { MSG_ADSB_VEHICLE, +#if AP_AIS_ENABLED MSG_AIS_VESSEL, +#endif }; const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = { @@ -417,7 +440,7 @@ void GCS_MAVLINK_Blimp::send_banner() send_text(MAV_SEVERITY_INFO, "Frame: %s", blimp.get_frame_string()); } -MAV_RESULT GCS_MAVLINK_Blimp::_handle_command_preflight_calibration(const mavlink_command_long_t &packet, const mavlink_message_t &msg) +MAV_RESULT GCS_MAVLINK_Blimp::_handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { return GCS_MAVLINK::_handle_command_preflight_calibration(packet, msg); } @@ -466,58 +489,28 @@ MAV_RESULT GCS_MAVLINK_Blimp::handle_command_int_do_reposition(const mavlink_com return MAV_RESULT_ACCEPTED; } -MAV_RESULT GCS_MAVLINK_Blimp::handle_command_int_packet(const mavlink_command_int_t &packet) +MAV_RESULT GCS_MAVLINK_Blimp::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { switch (packet.command) { - case MAV_CMD_DO_FOLLOW: - return MAV_RESULT_UNSUPPORTED; - case MAV_CMD_DO_REPOSITION: return handle_command_int_do_reposition(packet); + case MAV_CMD_NAV_TAKEOFF: + return MAV_RESULT_ACCEPTED; default: - return GCS_MAVLINK::handle_command_int_packet(packet); - } -} - -MAV_RESULT GCS_MAVLINK_Blimp::handle_command_mount(const mavlink_command_long_t &packet) -{ - // if the mount doesn't do pan control then yaw the entire vehicle instead: - switch (packet.command) { - default: - break; + return GCS_MAVLINK::handle_command_int_packet(packet, msg); } - return GCS_MAVLINK::handle_command_mount(packet); } -MAV_RESULT GCS_MAVLINK_Blimp::handle_command_long_packet(const mavlink_command_long_t &packet) +#if AP_MAVLINK_COMMAND_LONG_ENABLED +bool GCS_MAVLINK_Blimp::mav_frame_for_command_long(MAV_FRAME &frame, MAV_CMD packet_command) const { - switch (packet.command) { - - case MAV_CMD_NAV_TAKEOFF: { - return MAV_RESULT_ACCEPTED; - } - - case MAV_CMD_CONDITION_YAW: - // param1 : target angle [0-360] - // param2 : speed during change [deg per second] - // param3 : direction (-1:ccw, +1:cw) - // param4 : relative offset (1) or absolute angle (0) - if ((packet.param1 >= 0.0f) && - (packet.param1 <= 360.0f) && - (is_zero(packet.param4) || is_equal(packet.param4,1.0f))) { - // blimp.flightmode->auto_yaw.set_fixed_yaw( - // packet.param1, - // packet.param2, - // (int8_t)packet.param3, - // is_positive(packet.param4)); - return MAV_RESULT_ACCEPTED; - } - return MAV_RESULT_FAILED; - - default: - return GCS_MAVLINK::handle_command_long_packet(packet); + if (packet_command == MAV_CMD_NAV_TAKEOFF) { + frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + return true; } + return GCS_MAVLINK::mav_frame_for_command_long(frame, packet_command); } +#endif void GCS_MAVLINK_Blimp::handleMessage(const mavlink_message_t &msg) { @@ -540,7 +533,7 @@ void GCS_MAVLINK_Blimp::handleMessage(const mavlink_message_t &msg) } // end handle mavlink -MAV_RESULT GCS_MAVLINK_Blimp::handle_flight_termination(const mavlink_command_long_t &packet) +MAV_RESULT GCS_MAVLINK_Blimp::handle_flight_termination(const mavlink_command_int_t &packet) { MAV_RESULT result = MAV_RESULT_FAILED; if (packet.param1 > 0.5f) { diff --git a/Blimp/GCS_Mavlink.h b/Blimp/GCS_Mavlink.h index 3e2d67d7d4015..756dddc3fcf24 100644 --- a/Blimp/GCS_Mavlink.h +++ b/Blimp/GCS_Mavlink.h @@ -13,7 +13,7 @@ class GCS_MAVLINK_Blimp : public GCS_MAVLINK uint32_t telem_delay() const override; - MAV_RESULT handle_flight_termination(const mavlink_command_long_t &packet) override; + MAV_RESULT handle_flight_termination(const mavlink_command_int_t &packet) override; uint8_t sysid_my_gcs() const override; bool sysid_enforce() const override; @@ -21,16 +21,17 @@ class GCS_MAVLINK_Blimp : public GCS_MAVLINK bool params_ready() const override; void send_banner() override; - MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet, const mavlink_message_t &msg) override; + MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; void send_position_target_global_int() override; MAV_RESULT handle_command_do_set_roi(const Location &roi_loc) override; - MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet) override; - MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet) override; - MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override; + 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); +#if AP_MAVLINK_COMMAND_LONG_ENABLED + bool mav_frame_for_command_long(MAV_FRAME &frame, MAV_CMD packet_command) const override; +#endif bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED; bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED; @@ -74,7 +75,7 @@ class GCS_MAVLINK_Blimp : public GCS_MAVLINK POSZ = 7, POSYAW = 8, }; - + #if HAL_HIGH_LATENCY2_ENABLED uint8_t high_latency_wind_speed() const override; uint8_t high_latency_wind_direction() const override; diff --git a/Blimp/Log.cpp b/Blimp/Log.cpp index dab16518e3913..17c3161faba38 100644 --- a/Blimp/Log.cpp +++ b/Blimp/Log.cpp @@ -30,7 +30,7 @@ struct PACKED log_FINO { //Write a fin input packet void Blimp::Write_FINI(float right, float front, float down, float yaw) { - const struct log_FINI pkt{ + const struct log_FINI pkt { LOG_PACKET_HEADER_INIT(LOG_FINI_MSG), time_us : AP_HAL::micros64(), Right : right, @@ -44,7 +44,7 @@ void Blimp::Write_FINI(float right, float front, float down, float yaw) //Write a fin output packet void Blimp::Write_FINO(float *amp, float *off) { - const struct log_FINO pkt{ + const struct log_FINO pkt { LOG_PACKET_HEADER_INIT(LOG_FINO_MSG), time_us : AP_HAL::micros64(), Fin1_Amp : amp[0], @@ -92,7 +92,9 @@ void Blimp::Log_Write_PIDs() // Write an attitude packet void Blimp::Log_Write_Attitude() { - + //Attitude targets are all zero since Blimp doesn't have attitude control, + //but the rest of the log message is useful. + ahrs.Write_Attitude(Vector3f{0,0,0}); } // Write an EKF and POS packet @@ -232,74 +234,6 @@ tuning_max : tune_max logger.WriteBlock(&pkt_tune, sizeof(pkt_tune)); } -struct PACKED log_SysIdD { - LOG_PACKET_HEADER; - uint64_t time_us; - 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; -}; - -// Write an rate packet -void Blimp::Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z) -{ -#if MODE_SYSTEMID_ENABLED == ENABLED - struct log_SysIdD pkt_sidd = { - LOG_PACKET_HEADER_INIT(LOG_SYSIDD_MSG), -time_us : AP_HAL::micros64(), -waveform_time : waveform_time, -waveform_sample : waveform_sample, -waveform_freq : waveform_freq, -angle_x : angle_x, -angle_y : angle_y, -angle_z : angle_z, -accel_x : accel_x, -accel_y : accel_y, -accel_z : accel_z - }; - logger.WriteBlock(&pkt_sidd, sizeof(pkt_sidd)); -#endif -} - -struct PACKED log_SysIdS { - LOG_PACKET_HEADER; - uint64_t time_us; - 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; -}; - -// Write an rate packet -void Blimp::Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out) -{ -#if MODE_SYSTEMID_ENABLED == ENABLED - struct log_SysIdS pkt_sids = { - LOG_PACKET_HEADER_INIT(LOG_SYSIDS_MSG), -time_us : AP_HAL::micros64(), -systemID_axis : systemID_axis, -waveform_magnitude : waveform_magnitude, -frequency_start : frequency_start, -frequency_stop : frequency_stop, -time_fade_in : time_fade_in, -time_const_freq : time_const_freq, -time_record : time_record, -time_fade_out : time_fade_out - }; - logger.WriteBlock(&pkt_sids, sizeof(pkt_sids)); -#endif -} - // type and unit information can be found in // libraries/AP_Logger/Logstructure.h; search for "log_Units" for // units and "Format characters" for field type information @@ -314,8 +248,10 @@ const struct LogStructure Blimp::log_structure[] = { // @Field: D: Down // @Field: Y: Yaw - { LOG_FINI_MSG, sizeof(log_FINI), - "FINI", "Qffff", "TimeUS,R,F,D,Y", "s----", "F----" }, + { + LOG_FINI_MSG, sizeof(log_FINI), + "FINI", "Qffff", "TimeUS,R,F,D,Y", "s----", "F----" + }, // @LoggerMessage: FINO // @Description: Fin output @@ -329,8 +265,10 @@ const struct LogStructure Blimp::log_structure[] = { // @Field: F4A: Fin 4 Amplitude // @Field: F4O: Fin 4 Offset - { LOG_FINO_MSG, sizeof(log_FINO), - "FINO", "Qffffffff", "TimeUS,F1A,F1O,F2A,F2O,F3A,F3O,F4A,F4O", "s--------", "F--------" }, + { + LOG_FINO_MSG, sizeof(log_FINO), + "FINO", "Qffffffff", "TimeUS,F1A,F1O,F2A,F2O,F3A,F3O,F4A,F4O", "s--------", "F--------" + }, // @LoggerMessage: PIDD,PIVN,PIVE,PIVD,PIVY // @Description: Proportional/Integral/Derivative gain values @@ -342,19 +280,31 @@ const struct LogStructure Blimp::log_structure[] = { // @Field: I: integral part of PID // @Field: D: derivative part of PID // @Field: FF: controller feed-forward portion of response + // @Field: DFF: controller derivative feed-forward portion of response // @Field: Dmod: scaler applied to D gain to reduce limit cycling // @Field: SRate: slew rate - // @Field: Limit: 1 if I term is limited due to output saturation - { LOG_PIDD_MSG, sizeof(log_PID), - "PIDD", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS }, - { LOG_PIVN_MSG, sizeof(log_PID), - "PIVN", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS }, - { LOG_PIVE_MSG, sizeof(log_PID), - "PIVE", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS }, - { LOG_PIVD_MSG, sizeof(log_PID), - "PIVD", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS }, - { LOG_PIVY_MSG, sizeof(log_PID), - "PIVY", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS }, + // @Field: Flags: bitmask of PID state flags + // @FieldBitmaskEnum: Flags: log_PID_Flags + { + LOG_PIDD_MSG, sizeof(log_PID), + "PIDD", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS + }, + { + LOG_PIVN_MSG, sizeof(log_PID), + "PIVN", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS + }, + { + LOG_PIVE_MSG, sizeof(log_PID), + "PIVE", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS + }, + { + LOG_PIVD_MSG, sizeof(log_PID), + "PIVD", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS + }, + { + LOG_PIVY_MSG, sizeof(log_PID), + "PIVY", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS + }, // @LoggerMessage: PTUN // @Description: Parameter Tuning information @@ -441,41 +391,6 @@ const struct LogStructure Blimp::log_structure[] = { LOG_DATA_FLOAT_MSG, sizeof(log_Data_Float), "DFLT", "QBf", "TimeUS,Id,Value", "s--", "F--" }, - - // @LoggerMessage: SIDD - // @Description: System ID data - // @Field: TimeUS: Time since system startup - // @Field: Time: Time reference for waveform - // @Field: Targ: Current waveform sample - // @Field: F: Instantaneous waveform frequency - // @Field: Gx: Delta angle, X-Axis - // @Field: Gy: Delta angle, Y-Axis - // @Field: Gz: Delta angle, Z-Axis - // @Field: Ax: Delta velocity, X-Axis - // @Field: Ay: Delta velocity, Y-Axis - // @Field: Az: Delta velocity, Z-Axis - - { - LOG_SYSIDD_MSG, sizeof(log_SysIdD), - "SIDD", "Qfffffffff", "TimeUS,Time,Targ,F,Gx,Gy,Gz,Ax,Ay,Az", "ss-zkkkooo", "F---------" - }, - - // @LoggerMessage: SIDS - // @Description: System ID settings - // @Field: TimeUS: Time since system startup - // @Field: Ax: The axis which is being excited - // @Field: Mag: Magnitude of the chirp waveform - // @Field: FSt: Frequency at the start of chirp - // @Field: FSp: Frequency at the end of chirp - // @Field: TFin: Time to reach maximum amplitude of chirp - // @Field: TC: Time at constant frequency before chirp starts - // @Field: TR: Time taken to complete chirp waveform - // @Field: TFout: Time to reach zero amplitude after chirp finishes - - { - LOG_SYSIDS_MSG, sizeof(log_SysIdS), - "SIDS", "QBfffffff", "TimeUS,Ax,Mag,FSt,FSp,TFin,TC,TR,TFout", "s--ssssss", "F--------" - }, }; void Blimp::Log_Write_Vehicle_Startup_Messages() @@ -505,8 +420,6 @@ void Blimp::Log_Write_Data(LogDataID id, uint16_t value) {} void Blimp::Log_Write_Data(LogDataID id, float value) {} void Blimp::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max) {} void Blimp::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {} -void Blimp::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 Blimp::Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z) {} void Blimp::Log_Write_Vehicle_Startup_Messages() {} void Blimp::log_init(void) {} diff --git a/Blimp/Loiter.cpp b/Blimp/Loiter.cpp new file mode 100644 index 0000000000000..8c6b5d5e7aea7 --- /dev/null +++ b/Blimp/Loiter.cpp @@ -0,0 +1,202 @@ +#include "Blimp.h" + +#define MA 0.99 +#define MO (1-MA) + +void Loiter::run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled) +{ + const float dt = blimp.scheduler.get_last_loop_time_s(); + + float scaler_xz_n; + float xz_out = fabsf(blimp.motors->front_out) + fabsf(blimp.motors->down_out); + if (xz_out > 1) { + scaler_xz_n = 1 / xz_out; + } else { + scaler_xz_n = 1; + } + scaler_xz = scaler_xz*MA + scaler_xz_n*MO; + + float scaler_yyaw_n; + float yyaw_out = fabsf(blimp.motors->right_out) + fabsf(blimp.motors->yaw_out); + if (yyaw_out > 1) { + scaler_yyaw_n = 1 / yyaw_out; + } else { + scaler_yyaw_n = 1; + } + scaler_yyaw = scaler_yyaw*MA + scaler_yyaw_n*MO; + + AP::logger().WriteStreaming("BSC", "TimeUS,xz,yyaw,xzn,yyawn", + "Qffff", + AP_HAL::micros64(), + scaler_xz, scaler_yyaw, scaler_xz_n, scaler_yyaw_n); + + float yaw_ef = blimp.ahrs.get_yaw(); + Vector3f err_xyz = target_pos - blimp.pos_ned; + float err_yaw = wrap_PI(target_yaw - yaw_ef); + + Vector4b zero; + if ((fabsf(err_xyz.x) < blimp.g.pid_dz) || !blimp.motors->_armed || (blimp.g.dis_mask & (1<<(2-1)))) { + zero.x = true; + } + if ((fabsf(err_xyz.y) < blimp.g.pid_dz) || !blimp.motors->_armed || (blimp.g.dis_mask & (1<<(1-1)))) { + zero.y = true; + } + if ((fabsf(err_xyz.z) < blimp.g.pid_dz) || !blimp.motors->_armed || (blimp.g.dis_mask & (1<<(3-1)))) { + zero.z = true; + } + if ((fabsf(err_yaw) < blimp.g.pid_dz) || !blimp.motors->_armed || (blimp.g.dis_mask & (1<<(4-1)))) { + zero.yaw = true; + } + + //Disabled means "don't update PIDs or output anything at all". Zero means actually output zero thrust. I term is limited in either case." + Vector4b limit = zero || axes_disabled; + + Vector3f target_vel_ef; + if (!axes_disabled.x && !axes_disabled.y) target_vel_ef = {blimp.pid_pos_xy.update_all(target_pos, blimp.pos_ned, dt, {(float)limit.x, (float)limit.y, (float)limit.z}), 0}; + if (!axes_disabled.z) { + target_vel_ef.z = blimp.pid_pos_z.update_all(target_pos.z, blimp.pos_ned.z, dt, limit.z); + } + + float target_vel_yaw = 0; + if (!axes_disabled.yaw) { + target_vel_yaw = blimp.pid_pos_yaw.update_error(wrap_PI(target_yaw - yaw_ef), dt, limit.yaw); + blimp.pid_pos_yaw.set_target_rate(target_yaw); + blimp.pid_pos_yaw.set_actual_rate(yaw_ef); + } + + Vector3f target_vel_ef_c{constrain_float(target_vel_ef.x, -blimp.g.max_vel_xy, blimp.g.max_vel_xy), + constrain_float(target_vel_ef.y, -blimp.g.max_vel_xy, blimp.g.max_vel_xy), + constrain_float(target_vel_ef.z, -blimp.g.max_vel_z, blimp.g.max_vel_z)}; + float target_vel_yaw_c = constrain_float(target_vel_yaw, -blimp.g.max_vel_yaw, blimp.g.max_vel_yaw); + + Vector2f target_vel_ef_c_scaled_xy = {target_vel_ef_c.x * scaler_xz, target_vel_ef_c.y * scaler_yyaw}; + Vector2f vel_ned_filtd_scaled_xy = {blimp.vel_ned_filtd.x * scaler_xz, blimp.vel_ned_filtd.y * scaler_yyaw}; + + Vector2f actuator; + if (!axes_disabled.x && !axes_disabled.y) actuator = blimp.pid_vel_xy.update_all(target_vel_ef_c_scaled_xy, vel_ned_filtd_scaled_xy, dt, {(float)limit.x, (float)limit.y}); + float act_down = 0; + if (!axes_disabled.z) { + act_down = blimp.pid_vel_z.update_all(target_vel_ef_c.z * scaler_xz, blimp.vel_ned_filtd.z * scaler_xz, dt, limit.z); + } + blimp.rotate_NE_to_BF(actuator); + float act_yaw = 0; + if (!axes_disabled.yaw) { + act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw_c * scaler_yyaw, blimp.vel_yaw_filtd * scaler_yyaw, dt, limit.yaw); + } + + if (!blimp.motors->armed()) { + blimp.pid_pos_xy.set_integrator(Vector2f(0,0)); + blimp.pid_pos_z.set_integrator(0); + blimp.pid_pos_yaw.set_integrator(0); + blimp.pid_vel_xy.set_integrator(Vector2f(0,0)); + blimp.pid_vel_z.set_integrator(0); + blimp.pid_vel_yaw.set_integrator(0); + target_pos = blimp.pos_ned; + target_yaw = blimp.ahrs.get_yaw(); + } + + if (zero.x) { + blimp.motors->front_out = 0; + } else if (axes_disabled.x); + else { + blimp.motors->front_out = actuator.x; + } + if (zero.y) { + blimp.motors->right_out = 0; + } else if (axes_disabled.y); + else { + blimp.motors->right_out = actuator.y; + } + if (zero.z) { + blimp.motors->down_out = 0; + } else if (axes_disabled.z); + else { + blimp.motors->down_out = act_down; + } + if (zero.yaw) { + blimp.motors->yaw_out = 0; + } else if (axes_disabled.yaw); + else { + blimp.motors->yaw_out = act_yaw; + } + + AP::logger().Write_PSCN(target_pos.x * 100.0, blimp.pos_ned.x * 100.0, 0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0); + AP::logger().Write_PSCE(target_pos.y * 100.0, blimp.pos_ned.y * 100.0, 0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0); + AP::logger().Write_PSCD(-target_pos.z * 100.0, -blimp.pos_ned.z * 100.0, 0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0); +} + +void Loiter::run_vel(Vector3f& target_vel_ef, float& target_vel_yaw, Vector4b axes_disabled) +{ + const float dt = blimp.scheduler.get_last_loop_time_s(); + + Vector4b zero; + if (!blimp.motors->_armed || (blimp.g.dis_mask & (1<<(2-1)))) { + zero.x = true; + } + if (!blimp.motors->_armed || (blimp.g.dis_mask & (1<<(1-1)))) { + zero.y = true; + } + if (!blimp.motors->_armed || (blimp.g.dis_mask & (1<<(3-1)))) { + zero.z = true; + } + if (!blimp.motors->_armed || (blimp.g.dis_mask & (1<<(4-1)))) { + zero.yaw = true; + } + //Disabled means "don't update PIDs or output anything at all". Zero means actually output zero thrust. I term is limited in either case." + Vector4b limit = zero || axes_disabled; + + Vector3f target_vel_ef_c{constrain_float(target_vel_ef.x, -blimp.g.max_vel_xy, blimp.g.max_vel_xy), + constrain_float(target_vel_ef.y, -blimp.g.max_vel_xy, blimp.g.max_vel_xy), + constrain_float(target_vel_ef.z, -blimp.g.max_vel_z, blimp.g.max_vel_z)}; + float target_vel_yaw_c = constrain_float(target_vel_yaw, -blimp.g.max_vel_yaw, blimp.g.max_vel_yaw); + + Vector2f target_vel_ef_c_scaled_xy = {target_vel_ef_c.x * scaler_xz, target_vel_ef_c.y * scaler_yyaw}; + Vector2f vel_ned_filtd_scaled_xy = {blimp.vel_ned_filtd.x * scaler_xz, blimp.vel_ned_filtd.y * scaler_yyaw}; + + Vector2f actuator; + if (!axes_disabled.x && !axes_disabled.y) actuator = blimp.pid_vel_xy.update_all(target_vel_ef_c_scaled_xy, vel_ned_filtd_scaled_xy, dt, {(float)limit.x, (float)limit.y}); + float act_down = 0; + if (!axes_disabled.z) { + act_down = blimp.pid_vel_z.update_all(target_vel_ef_c.z * scaler_xz, blimp.vel_ned_filtd.z * scaler_xz, dt, limit.z); + } + blimp.rotate_NE_to_BF(actuator); + float act_yaw = 0; + if (!axes_disabled.yaw) { + act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw_c * scaler_yyaw, blimp.vel_yaw_filtd * scaler_yyaw, dt, limit.yaw); + } + + if (!blimp.motors->armed()) { + blimp.pid_vel_xy.set_integrator(Vector2f(0,0)); + blimp.pid_vel_z.set_integrator(0); + blimp.pid_vel_yaw.set_integrator(0); + } + + if (zero.x) { + blimp.motors->front_out = 0; + } else if (axes_disabled.x); + else { + blimp.motors->front_out = actuator.x; + } + if (zero.y) { + blimp.motors->right_out = 0; + } else if (axes_disabled.y); + else { + blimp.motors->right_out = actuator.y; + } + if (zero.z) { + blimp.motors->down_out = 0; + } else if (axes_disabled.z); + else { + blimp.motors->down_out = act_down; + } + if (zero.yaw) { + blimp.motors->yaw_out = 0; + } else if (axes_disabled.yaw); + else { + blimp.motors->yaw_out = act_yaw; + } + + AP::logger().Write_PSCN(0.0, blimp.pos_ned.x * 100.0, 0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0); + AP::logger().Write_PSCE(0.0, blimp.pos_ned.y * 100.0, 0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0); + AP::logger().Write_PSCD(0.0, -blimp.pos_ned.z * 100.0, 0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0); +} diff --git a/Blimp/Loiter.h b/Blimp/Loiter.h new file mode 100644 index 0000000000000..c21fdd57c6f25 --- /dev/null +++ b/Blimp/Loiter.h @@ -0,0 +1,59 @@ +#pragma once + +class Vector4b +{ +public: + bool x; + bool y; + bool z; + bool yaw; + + constexpr Vector4b() + : x(0) + , y(0) + , z(0) + , yaw(0) {} + + constexpr Vector4b(const bool x0, const bool y0, const bool z0, const bool yaw0) + : x(x0) + , y(y0) + , z(z0) + , yaw(yaw0) {} + + Vector4b operator &&(const Vector4b &v) + { + Vector4b temp{x && v.x, y && v.y, z && v.z, yaw && v.yaw}; + return temp; + } + + Vector4b operator ||(const Vector4b &v) + { + Vector4b temp{x || v.x, y || v.y, z || v.z, yaw || v.yaw}; + return temp; + } + +}; + + + +class Loiter +{ +public: + friend class Blimp; + friend class Fins; + + float scaler_xz; + float scaler_yyaw; + + //constructor + Loiter(uint16_t loop_rate) + { + scaler_xz = 1; + scaler_yyaw = 1; + }; + + //Run Loiter controller with target position and yaw in global frame. Expects to be called at loop rate. + void run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled); + //Run Loiter controller with target velocity and velocity in global frame. Expects to be called at loop rate. + void run_vel(Vector3f& target_vel, float& target_vel_yaw, Vector4b axes_disabled); +}; diff --git a/Blimp/Parameters.cpp b/Blimp/Parameters.cpp index 487d0981146b0..593e9f26687f7 100644 --- a/Blimp/Parameters.cpp +++ b/Blimp/Parameters.cpp @@ -41,6 +41,7 @@ const AP_Param::Info Blimp::var_info[] = { // @DisplayName: My ground station number // @Description: Allows restricting radio overrides to only come from my ground station // @Range: 1 255 + // @Increment: 1 // @User: Advanced GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), @@ -53,15 +54,6 @@ const AP_Param::Info Blimp::var_info[] = { // @Increment: .5 GSCALAR(throttle_filt, "PILOT_THR_FILT", 0), - // @Param: PILOT_TKOFF_ALT - // @DisplayName: Pilot takeoff altitude - // @Description: Altitude that altitude control modes will climb to when a takeoff is triggered with the throttle stick. - // @User: Standard - // @Units: cm - // @Range: 0.0 1000.0 - // @Increment: 10 - GSCALAR(pilot_takeoff_alt, "PILOT_TKOFF_ALT", PILOT_TKOFF_ALT_DEFAULT), - // @Param: PILOT_THR_BHV // @DisplayName: Throttle stick behavior // @Description: Bitmask containing various throttle stick options. TX with sprung throttle can set PILOT_THR_BHV to "1" so motor feedback when landed starts from mid-stick instead of bottom of stick. @@ -184,7 +176,7 @@ const AP_Param::Info Blimp::var_info[] = { // @Param: LOG_BITMASK // @DisplayName: Log bitmask // @Description: Bitmap of what log types to enable in on-board logger. This value is made up of the sum of each of the log types you want to be saved. On boards supporting microSD cards or other large block-storage devices it is usually best just to enable all basic log types by setting this to 65535. - // @Bitmask: 0:Fast Attitude,1:Medium Attitude,2:GPS,3:System Performance,4:Control Tuning,6:RC Input,7:IMU,9:Battery Monitor,10:RC Output,11:Optical Flow,12:PID,13:Compass + // @Bitmask: 0:Fast Attitude,1:Medium Attitude,2:GPS,3:System Performance,4:Control Tuning,6:RC Input,7:IMU,9:Battery Monitor,10:RC Output,12:PID,13:Compass // @User: Standard GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), @@ -272,12 +264,20 @@ const AP_Param::Info Blimp::var_info[] = { // @Param: DIS_MASK // @DisplayName: Disable output mask - // @Description: Mask for disabling one or more of the 4 output axis in mode Velocity or Loiter + // @Description: Mask for disabling (setting to zero) one or more of the 4 output axis in mode Velocity or Loiter // @Values: 0:All enabled,1:Right,2:Front,4:Down,8:Yaw,3:Down and Yaw only,12:Front & Right only // @Bitmask: 0:Right,1:Front,2:Down,3:Yaw // @User: Standard GSCALAR(dis_mask, "DIS_MASK", 0), + // @Param: PID_DZ + // @DisplayName: Deadzone for the position PIDs + // @Description: Output 0 thrust signal when blimp is within this distance (in meters) of the target position. Warning: If this param is greater than MAX_POS_XY param then the blimp won't move at all in the XY plane in Loiter mode as it does not allow more than a second's lag. Same for the other axes. + // @Units: m + // @Range: 0.1 1 + // @User: Standard + GSCALAR(pid_dz, "PID_DZ", 0), + // @Param: RC_SPEED // @DisplayName: ESC Update Speed // @Description: This is the speed in Hertz that your ESCs will receive updates @@ -722,6 +722,34 @@ const AP_Param::Info Blimp::var_info[] = { // @Range: 0 200 // @Increment: 0.5 // @User: Advanced + + // @Param: POSYAW_PDMX + // @DisplayName: Position (yaw) axis controller PD sum maximum + // @Description: Position (yaw) axis controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output + // @Range: 0 4000 + // @Increment: 10 + // @Units: d% + // @User: Advanced + + // @Param: POSYAW_D_FF + // @DisplayName: Position (yaw) 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.1 + // @Increment: 0.001 + // @User: Advanced + + // @Param: POSYAW_NTF + // @DisplayName: Position (yaw) Target notch filter index + // @Description: Position (yaw) Target notch filter index + // @Range: 1 8 + // @User: Advanced + + // @Param: POSYAW_NEF + // @DisplayName: Position (yaw) Error notch filter index + // @Description: Position (yaw) Error notch filter index + // @Range: 1 8 + // @User: Advanced + GOBJECT(pid_pos_yaw, "POSYAW_", AC_PID), // @Group: diff --git a/Blimp/Parameters.h b/Blimp/Parameters.h index c819ad0ab4654..8ec466d62c0c6 100644 --- a/Blimp/Parameters.h +++ b/Blimp/Parameters.h @@ -87,7 +87,7 @@ class Parameters k_param_log_bitmask, k_param_throttle_filt, k_param_throttle_behavior, - k_param_pilot_takeoff_alt, + k_param_pilot_takeoff_alt, //unused // AP_ADSB Library k_param_adsb, @@ -110,6 +110,7 @@ class Parameters k_param_max_pos_yaw, k_param_simple_mode, k_param_dis_mask, + k_param_pid_dz, // // 90: misc2 @@ -135,7 +136,7 @@ class Parameters k_param_gcs2, k_param_serial_manager, k_param_gcs3, - k_param_gcs_pid_mask, // 126 + k_param_gcs_pid_mask, k_param_gcs4, k_param_gcs5, k_param_gcs6, @@ -213,7 +214,6 @@ class Parameters AP_Float throttle_filt; AP_Int16 throttle_behavior; - AP_Float pilot_takeoff_alt; AP_Int8 failsafe_gcs; // ground station failsafe behavior AP_Int16 gps_hdop_good; // GPS Hdop value at or below this value represent a good position @@ -254,6 +254,7 @@ class Parameters AP_Int8 simple_mode; AP_Int16 dis_mask; + AP_Float pid_dz; AP_Int8 rtl_alt_type; diff --git a/Blimp/RC_Channel.cpp b/Blimp/RC_Channel.cpp index 39682160ae5dc..7709867cf25c4 100644 --- a/Blimp/RC_Channel.cpp +++ b/Blimp/RC_Channel.cpp @@ -36,6 +36,11 @@ void RC_Channel_Blimp::mode_switch_changed(modeswitch_pos_t new_pos) } } +bool RC_Channels_Blimp::in_rc_failsafe() const +{ + return blimp.failsafe.radio; +} + bool RC_Channels_Blimp::has_valid_input() const { if (blimp.failsafe.radio) { @@ -101,7 +106,7 @@ bool RC_Channel_Blimp::do_aux_function(const aux_func_t ch_option, const AuxSwit case AUX_FUNC::SAVE_TRIM: if ((ch_flag == AuxSwitchPos::HIGH) && (blimp.control_mode <= Mode::Number::MANUAL) && - (blimp.channel_down->get_control_in() == 0)) { + (blimp.channel_up->get_control_in() == 0)) { blimp.save_trim(); } break; diff --git a/Blimp/RC_Channel.h b/Blimp/RC_Channel.h index 80e53ceda2302..ec37259794cb8 100644 --- a/Blimp/RC_Channel.h +++ b/Blimp/RC_Channel.h @@ -30,6 +30,7 @@ class RC_Channels_Blimp : public RC_Channels public: bool has_valid_input() const override; + bool in_rc_failsafe() const override; RC_Channel *get_arming_channel(void) const override; diff --git a/Blimp/config.h b/Blimp/config.h index 7eba1cceba324..9c5908f65fed9 100644 --- a/Blimp/config.h +++ b/Blimp/config.h @@ -22,13 +22,6 @@ # define ARMING_DELAY_SEC 2.0f #endif -////////////////////////////////////////////////////////////////////////////// -// FRAME_CONFIG -// -#ifndef FRAME_CONFIG -# define FRAME_CONFIG MULTICOPTER_FRAME -#endif - ////////////////////////////////////////////////////////////////////////////// // PWM control // default RC speed in Hz @@ -36,53 +29,6 @@ # define RC_FAST_SPEED 490 #endif -////////////////////////////////////////////////////////////////////////////// -// Rangefinder -// - -#ifndef RANGEFINDER_ENABLED -# define RANGEFINDER_ENABLED ENABLED -#endif - -#ifndef RANGEFINDER_HEALTH_MAX -# define RANGEFINDER_HEALTH_MAX 3 // number of good reads that indicates a healthy rangefinder -#endif - -#ifndef RANGEFINDER_TIMEOUT_MS -# define RANGEFINDER_TIMEOUT_MS 1000 // rangefinder filter reset if no updates from sensor in 1 second -#endif - -#ifndef RANGEFINDER_GAIN_DEFAULT -# define RANGEFINDER_GAIN_DEFAULT 0.8f // gain for controlling how quickly rangefinder range adjusts target altitude (lower means slower reaction) -#endif - -#ifndef SURFACE_TRACKING_TIMEOUT_MS -# define SURFACE_TRACKING_TIMEOUT_MS 1000 // surface tracking target alt will reset to current rangefinder alt after this many milliseconds without a good rangefinder alt -#endif - -#ifndef RANGEFINDER_WPNAV_FILT_HZ -# define RANGEFINDER_WPNAV_FILT_HZ 0.25f // filter frequency for rangefinder altitude provided to waypoint navigation class -#endif - -#ifndef RANGEFINDER_TILT_CORRECTION // by disable tilt correction for use of range finder data by EKF -# define RANGEFINDER_TILT_CORRECTION ENABLED -#endif - -#ifndef RANGEFINDER_GLITCH_ALT_CM -# define RANGEFINDER_GLITCH_ALT_CM 200 // amount of rangefinder change to be considered a glitch -#endif - -#ifndef RANGEFINDER_GLITCH_NUM_SAMPLES -# define RANGEFINDER_GLITCH_NUM_SAMPLES 3 // number of rangefinder glitches in a row to take new reading -#endif - -////////////////////////////////////////////////////////////////////////////// -// Proximity sensor -// -#ifndef PROXIMITY_ENABLED -# define PROXIMITY_ENABLED ENABLED -#endif - #ifndef MAV_SYSTEM_ID # define MAV_SYSTEM_ID 1 #endif @@ -125,136 +71,9 @@ # define EKF_ORIGIN_MAX_DIST_M 50000 // EKF origin and waypoints (including home) must be within 50km #endif -////////////////////////////////////////////////////////////////////////////// -// OPTICAL_FLOW -#ifndef OPTFLOW -# define OPTFLOW ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// Auto Tuning -#ifndef AUTOTUNE_ENABLED -# define AUTOTUNE_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// Precision Landing with companion computer or IRLock sensor -#ifndef PRECISION_LANDING -# define PRECISION_LANDING ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// Parachute release -#ifndef PARACHUTE -# define PARACHUTE HAL_PARACHUTE_ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// Nav-Guided - allows external nav computer to control vehicle -#ifndef NAV_GUIDED -# define NAV_GUIDED !HAL_MINIMIZE_FEATURES -#endif - -////////////////////////////////////////////////////////////////////////////// -// Acro - fly vehicle in acrobatic mode -#ifndef MODE_ACRO_ENABLED -# define MODE_ACRO_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// Auto mode - allows vehicle to trace waypoints and perform automated actions -#ifndef MODE_AUTO_ENABLED -# define MODE_AUTO_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// Brake mode - bring vehicle to stop -#ifndef MODE_BRAKE_ENABLED -# define MODE_BRAKE_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// Circle - fly vehicle around a central point -#ifndef MODE_CIRCLE_ENABLED -# define MODE_CIRCLE_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// Drift - fly vehicle in altitude-held, coordinated-turn mode -#ifndef MODE_DRIFT_ENABLED -# define MODE_DRIFT_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// flip - fly vehicle in flip in pitch and roll direction mode -#ifndef MODE_FLIP_ENABLED -# define MODE_FLIP_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// Follow - follow another vehicle or GCS -#ifndef MODE_FOLLOW_ENABLED -# define MODE_FOLLOW_ENABLED !HAL_MINIMIZE_FEATURES -#endif - -////////////////////////////////////////////////////////////////////////////// -// Guided mode - control vehicle's position or angles from GCS -#ifndef MODE_GUIDED_ENABLED -# define MODE_GUIDED_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// GuidedNoGPS mode - control vehicle's angles from GCS -#ifndef MODE_GUIDED_NOGPS_ENABLED -# define MODE_GUIDED_NOGPS_ENABLED !HAL_MINIMIZE_FEATURES -#endif - -////////////////////////////////////////////////////////////////////////////// -// Loiter mode - allows vehicle to hold global position -#ifndef MODE_LOITER_ENABLED -# define MODE_LOITER_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// Position Hold - enable holding of global position -#ifndef MODE_POSHOLD_ENABLED -# define MODE_POSHOLD_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// RTL - Return To Launch -#ifndef MODE_RTL_ENABLED -# define MODE_RTL_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// SmartRTL - allows vehicle to retrace a (loop-eliminated) breadcrumb home -#ifndef MODE_SMARTRTL_ENABLED -# define MODE_SMARTRTL_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// Sport - fly vehicle in rate-controlled (earth-frame) mode -#ifndef MODE_SPORT_ENABLED -# define MODE_SPORT_ENABLED !HAL_MINIMIZE_FEATURES -#endif - -////////////////////////////////////////////////////////////////////////////// -// System ID - conduct system identification tests on vehicle -#ifndef MODE_SYSTEMID_ENABLED -# define MODE_SYSTEMID_ENABLED !HAL_MINIMIZE_FEATURES -#endif - -////////////////////////////////////////////////////////////////////////////// -// RADIO CONFIGURATION -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - - ////////////////////////////////////////////////////////////////////////////// // FLIGHT_MODE // - #ifndef FLIGHT_MODE_1 # define FLIGHT_MODE_1 Mode::Number::MANUAL #endif @@ -274,7 +93,6 @@ # define FLIGHT_MODE_6 Mode::Number::MANUAL #endif - ////////////////////////////////////////////////////////////////////////////// // Throttle Failsafe // @@ -282,188 +100,17 @@ # define FS_THR_VALUE_DEFAULT 975 #endif -////////////////////////////////////////////////////////////////////////////// -// Takeoff -// -#ifndef PILOT_TKOFF_ALT_DEFAULT -# define PILOT_TKOFF_ALT_DEFAULT 0 // default final alt above home for pilot initiated takeoff -#endif - - -////////////////////////////////////////////////////////////////////////////// -// Landing -// -#ifndef LAND_SPEED -# define LAND_SPEED 50 // the descent speed for the final stage of landing in cm/s -#endif -#ifndef LAND_REPOSITION_DEFAULT -# define LAND_REPOSITION_DEFAULT 1 // by default the pilot can override roll/pitch during landing -#endif -#ifndef LAND_WITH_DELAY_MS -# define LAND_WITH_DELAY_MS 4000 // default delay (in milliseconds) when a land-with-delay is triggered during a failsafe event -#endif -#ifndef LAND_CANCEL_TRIGGER_THR -# define LAND_CANCEL_TRIGGER_THR 700 // land is cancelled by input throttle above 700 -#endif -#ifndef LAND_RANGEFINDER_MIN_ALT_CM -#define LAND_RANGEFINDER_MIN_ALT_CM 200 -#endif - -////////////////////////////////////////////////////////////////////////////// -// Landing Detector -// -#ifndef LAND_DETECTOR_TRIGGER_SEC -# define LAND_DETECTOR_TRIGGER_SEC 1.0f // number of seconds to detect a landing -#endif -#ifndef LAND_DETECTOR_MAYBE_TRIGGER_SEC -# define LAND_DETECTOR_MAYBE_TRIGGER_SEC 0.2f // number of seconds that means we might be landed (used to reset horizontal position targets to prevent tipping over) -#endif -#ifndef LAND_DETECTOR_ACCEL_LPF_CUTOFF -# define LAND_DETECTOR_ACCEL_LPF_CUTOFF 1.0f // frequency cutoff of land detector accelerometer filter -#endif -#ifndef LAND_DETECTOR_ACCEL_MAX -# define LAND_DETECTOR_ACCEL_MAX 1.0f // vehicle acceleration must be under 1m/s/s -#endif - -////////////////////////////////////////////////////////////////////////////// -// Flight mode definitions -// - -// Acro Mode -#ifndef ACRO_RP_P -# define ACRO_RP_P 4.5f -#endif - -#ifndef ACRO_YAW_P -# define ACRO_YAW_P 4.5f -#endif - -#ifndef ACRO_LEVEL_MAX_ANGLE -# define ACRO_LEVEL_MAX_ANGLE 3000 -#endif - -#ifndef ACRO_BALANCE_ROLL -#define ACRO_BALANCE_ROLL 1.0f -#endif - -#ifndef ACRO_BALANCE_PITCH -#define ACRO_BALANCE_PITCH 1.0f -#endif - -#ifndef ACRO_RP_EXPO_DEFAULT -#define ACRO_RP_EXPO_DEFAULT 0.3f -#endif - -#ifndef ACRO_Y_EXPO_DEFAULT -#define ACRO_Y_EXPO_DEFAULT 0.0f -#endif - -#ifndef ACRO_THR_MID_DEFAULT -#define ACRO_THR_MID_DEFAULT 0.0f -#endif - -// RTL Mode -#ifndef RTL_ALT_FINAL -# define RTL_ALT_FINAL 0 // the altitude 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 -#endif - -#ifndef RTL_ALT_MIN -# define RTL_ALT_MIN 200 // min height above ground for RTL (i.e 2m) -#endif - -#ifndef RTL_CLIMB_MIN_DEFAULT -# define RTL_CLIMB_MIN_DEFAULT 0 // vehicle will always climb this many cm as first stage of RTL -#endif - -#ifndef RTL_ABS_MIN_CLIMB -# define RTL_ABS_MIN_CLIMB 250 // absolute minimum initial climb -#endif - -#ifndef RTL_CONE_SLOPE_DEFAULT -# define RTL_CONE_SLOPE_DEFAULT 3.0f // slope of RTL cone (height / distance). 0 = No cone -#endif - -#ifndef RTL_MIN_CONE_SLOPE -# define RTL_MIN_CONE_SLOPE 0.5f // minimum slope of cone -#endif - -#ifndef RTL_LOITER_TIME -# define RTL_LOITER_TIME 5000 // Time (in milliseconds) to loiter above home before beginning final descent -#endif - -// AUTO Mode -#ifndef WP_YAW_BEHAVIOR_DEFAULT -# define WP_YAW_BEHAVIOR_DEFAULT WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL -#endif - -#ifndef YAW_LOOK_AHEAD_MIN_SPEED -# define YAW_LOOK_AHEAD_MIN_SPEED 100 // minimum ground speed in cm/s required before blimp is aimed at ground course -#endif - -////////////////////////////////////////////////////////////////////////////// -// Stabilize Rate Control -// -#ifndef ROLL_PITCH_YAW_INPUT_MAX -# define ROLL_PITCH_YAW_INPUT_MAX 4500 // roll, pitch and yaw input range -#endif -#ifndef DEFAULT_ANGLE_MAX -# define DEFAULT_ANGLE_MAX 4500 // ANGLE_MAX parameters default value -#endif - -////////////////////////////////////////////////////////////////////////////// -// Stop mode defaults -// -#ifndef BRAKE_MODE_SPEED_Z -# define BRAKE_MODE_SPEED_Z 250 // z-axis speed in cm/s in Brake Mode -#endif -#ifndef BRAKE_MODE_DECEL_RATE -# define BRAKE_MODE_DECEL_RATE 750 // acceleration rate in cm/s/s in Brake Mode -#endif - -////////////////////////////////////////////////////////////////////////////// -// PosHold parameter defaults -// -#ifndef POSHOLD_BRAKE_RATE_DEFAULT -# define POSHOLD_BRAKE_RATE_DEFAULT 8 // default POSHOLD_BRAKE_RATE param value. Rotation rate during braking in deg/sec -#endif -#ifndef POSHOLD_BRAKE_ANGLE_DEFAULT -# define POSHOLD_BRAKE_ANGLE_DEFAULT 3000 // default POSHOLD_BRAKE_ANGLE param value. Max lean angle during braking in centi-degrees -#endif - ////////////////////////////////////////////////////////////////////////////// // Throttle control defaults // - #ifndef THR_DZ_DEFAULT # define THR_DZ_DEFAULT 100 // the deadzone above and below mid throttle while in althold or loiter #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 -#endif -#ifndef PILOT_ACCEL_Z_DEFAULT -# define PILOT_ACCEL_Z_DEFAULT 250 // vertical acceleration in cm/s/s while altitude is under pilot control -#endif - #ifndef AUTO_DISARMING_DELAY # define AUTO_DISARMING_DELAY 10 #endif -////////////////////////////////////////////////////////////////////////////// -// Throw mode configuration -// -#ifndef THROW_HIGH_SPEED -# define THROW_HIGH_SPEED 500.0f // vehicle much reach this total 3D speed in cm/s (or be free falling) -#endif -#ifndef THROW_VERTICAL_SPEED -# define THROW_VERTICAL_SPEED 50.0f // motors start when vehicle reaches this total 3D speed in cm/s -#endif - ////////////////////////////////////////////////////////////////////////////// // Logging control // diff --git a/Blimp/defines.h b/Blimp/defines.h index f356e77600f55..0a73072a1aea5 100644 --- a/Blimp/defines.h +++ b/Blimp/defines.h @@ -10,111 +10,6 @@ #define ENABLE ENABLED #define DISABLE DISABLED -// Autopilot Yaw Mode enumeration -enum autopilot_yaw_mode { - AUTO_YAW_HOLD = 0, // pilot controls the heading - AUTO_YAW_LOOK_AT_NEXT_WP = 1, // point towards next waypoint (no pilot input accepted) - AUTO_YAW_ROI = 2, // point towards a location held in roi (no pilot input accepted) - AUTO_YAW_FIXED = 3, // point towards a particular angle (no pilot input accepted) - AUTO_YAW_LOOK_AHEAD = 4, // point in the direction the blimp is moving - AUTO_YAW_RESETTOARMEDYAW = 5, // point towards heading at time motors were armed - AUTO_YAW_RATE = 6, // turn at a specified rate (held in auto_yaw_rate) - AUTO_YAW_CIRCLE = 7, // use AC_Circle's provided yaw (used during Loiter-Turns commands) -}; - -// Frame types -#define UNDEFINED_FRAME 0 -#define MULTICOPTER_FRAME 1 -#define HELI_FRAME 2 - -// Tuning enumeration -enum tuning_func { - TUNING_NONE = 0, // - TUNING_STABILIZE_ROLL_PITCH_KP = 1, // stabilize roll/pitch angle controller's P term - TUNING_STABILIZE_YAW_KP = 3, // stabilize yaw heading controller's P term - TUNING_RATE_ROLL_PITCH_KP = 4, // body frame roll/pitch rate controller's P term - TUNING_RATE_ROLL_PITCH_KI = 5, // body frame roll/pitch rate controller's I term - TUNING_YAW_RATE_KP = 6, // body frame yaw rate controller's P term - TUNING_THROTTLE_RATE_KP = 7, // throttle rate controller's P term (desired rate to acceleration or motor output) - TUNING_WP_SPEED = 10, // maximum speed to next way point (0 to 10m/s) - TUNING_LOITER_POSITION_KP = 12, // loiter distance controller's P term (position error to speed) - TUNING_HELI_EXTERNAL_GYRO = 13, // TradHeli specific external tail gyro gain - TUNING_ALTITUDE_HOLD_KP = 14, // altitude hold controller's P term (alt error to desired rate) - TUNING_RATE_ROLL_PITCH_KD = 21, // body frame roll/pitch rate controller's D term - TUNING_VEL_XY_KP = 22, // loiter rate controller's P term (speed error to tilt angle) - TUNING_ACRO_RP_KP = 25, // acro controller's P term. converts pilot input to a desired roll, pitch or yaw rate - TUNING_YAW_RATE_KD = 26, // body frame yaw rate controller's D term - TUNING_VEL_XY_KI = 28, // loiter rate controller's I term (speed error to tilt angle) - TUNING_AHRS_YAW_KP = 30, // ahrs's compass effect on yaw angle (0 = very low, 1 = very high) - TUNING_AHRS_KP = 31, // accelerometer effect on roll/pitch angle (0=low) - TUNING_ACCEL_Z_KP = 34, // accel based throttle controller's P term - TUNING_ACCEL_Z_KI = 35, // accel based throttle controller's I term - TUNING_ACCEL_Z_KD = 36, // accel based throttle controller's D term - TUNING_DECLINATION = 38, // compass declination in radians - TUNING_CIRCLE_RATE = 39, // circle turn rate in degrees (hard coded to about 45 degrees in either direction) - TUNING_ACRO_YAW_KP = 40, // acro controller's P term. converts pilot input to a desired roll, pitch or yaw rate - TUNING_RANGEFINDER_GAIN = 41, // rangefinder gain - TUNING_EKF_VERTICAL_POS = 42, // unused - TUNING_EKF_HORIZONTAL_POS = 43, // unused - TUNING_EKF_ACCEL_NOISE = 44, // unused - TUNING_RC_FEEL_RP = 45, // roll-pitch input smoothing - TUNING_RATE_PITCH_KP = 46, // body frame pitch rate controller's P term - TUNING_RATE_PITCH_KI = 47, // body frame pitch rate controller's I term - TUNING_RATE_PITCH_KD = 48, // body frame pitch rate controller's D term - TUNING_RATE_ROLL_KP = 49, // body frame roll rate controller's P term - TUNING_RATE_ROLL_KI = 50, // body frame roll rate controller's I term - TUNING_RATE_ROLL_KD = 51, // body frame roll rate controller's D term - TUNING_RATE_PITCH_FF = 52, // body frame pitch rate controller FF term - TUNING_RATE_ROLL_FF = 53, // body frame roll rate controller FF term - TUNING_RATE_YAW_FF = 54, // body frame yaw rate controller FF term - TUNING_RATE_MOT_YAW_HEADROOM = 55, // motors yaw headroom minimum - TUNING_RATE_YAW_FILT = 56, // yaw rate input filter - UNUSED = 57, // was winch control - TUNING_SYSTEM_ID_MAGNITUDE = 58 // magnitude of the system ID signal -}; - -// Yaw behaviours during missions - possible values for WP_YAW_BEHAVIOR parameter -#define WP_YAW_BEHAVIOR_NONE 0 // auto pilot will never control yaw during missions or rtl (except for DO_CONDITIONAL_YAW command received) -#define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP 1 // auto pilot will face next waypoint or home during rtl -#define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL 2 // auto pilot will face next waypoint except when doing RTL at which time it will stay in it's last -#define WP_YAW_BEHAVIOR_LOOK_AHEAD 3 // auto pilot will look ahead during missions and rtl (primarily meant for traditional helicopters) - -// Auto modes -enum AutoMode { - Auto_TakeOff, - Auto_WP, - Auto_Land, - Auto_RTL, - Auto_CircleMoveToEdge, - Auto_Circle, - Auto_Spline, - Auto_NavGuided, - Auto_Loiter, - Auto_LoiterToAlt, - Auto_NavPayloadPlace, -}; - -// Guided modes -enum GuidedMode { - Guided_TakeOff, - Guided_WP, - Guided_Velocity, - Guided_PosVel, - Guided_Angle, -}; - -enum PayloadPlaceStateType { - PayloadPlaceStateType_FlyToLocation, - PayloadPlaceStateType_Descent_Start, - PayloadPlaceStateType_Descent, - PayloadPlaceStateType_Release, - PayloadPlaceStateType_Releasing, - PayloadPlaceStateType_Delay, - PayloadPlaceStateType_Ascent_Start, - PayloadPlaceStateType_Ascent, - PayloadPlaceStateType_Done, -}; - // bit options for DEV_OPTIONS parameter enum DevOptions { DevOptionADSBMAVLink = 1, @@ -184,8 +79,7 @@ enum LoggingParameters { // EKF failsafe definitions (FS_EKF_ACTION parameter) #define FS_EKF_ACTION_LAND 1 // switch to LAND mode on EKF failsafe -#define FS_EKF_ACTION_ALTHOLD 2 // switch to ALTHOLD mode on EKF failsafe -#define FS_EKF_ACTION_LAND_EVEN_STABILIZE 3 // switch to Land mode on EKF failsafe even if in a manual flight mode like stabilize +#define FS_EKF_ACTION_LAND_EVEN_MANUAL 3 // switch to Land mode on EKF failsafe even if in Manual mode // for PILOT_THR_BHV parameter #define THR_BEHAVE_FEEDBACK_FROM_MID_STICK (1<<0) diff --git a/Blimp/ekf_check.cpp b/Blimp/ekf_check.cpp index 8bc678922f0b5..fd524b99ab91a 100644 --- a/Blimp/ekf_check.cpp +++ b/Blimp/ekf_check.cpp @@ -148,16 +148,16 @@ void Blimp::failsafe_ekf_event() AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED); // does this mode require position? - if (!blimp.flightmode->requires_GPS() && (g.fs_ekf_action != FS_EKF_ACTION_LAND_EVEN_STABILIZE)) { + if (!blimp.flightmode->requires_GPS() && (g.fs_ekf_action != FS_EKF_ACTION_LAND_EVEN_MANUAL)) { return; } // take action based on fs_ekf_action parameter switch (g.fs_ekf_action) { case FS_EKF_ACTION_LAND: - case FS_EKF_ACTION_LAND_EVEN_STABILIZE: + case FS_EKF_ACTION_LAND_EVEN_MANUAL: default: - set_mode_land_with_pause(ModeReason::EKF_FAILSAFE); + set_mode_land_failsafe(ModeReason::EKF_FAILSAFE); break; } } diff --git a/Blimp/events.cpp b/Blimp/events.cpp index cbd778cd6ca69..452f624c5b879 100644 --- a/Blimp/events.cpp +++ b/Blimp/events.cpp @@ -98,8 +98,8 @@ void Blimp::failsafe_gcs_check() const uint32_t gcs_last_seen_ms = gcs().sysid_myggcs_last_seen_time_ms(); if (gcs_last_seen_ms == 0) { - return; - } + return; + } // calc time since last gcs update // note: this only looks at the heartbeat from the device id set by g.sysid_my_gcs @@ -148,7 +148,7 @@ void Blimp::do_failsafe_action(Failsafe_Action action, ModeReason reason) case Failsafe_Action_None: return; case Failsafe_Action_Land: - set_mode_land_with_pause(reason); + set_mode_land_failsafe(reason); break; case Failsafe_Action_Terminate: { arming.disarm(AP_Arming::Method::FAILSAFE_ACTION_TERMINATE); diff --git a/Blimp/mode.cpp b/Blimp/mode.cpp index 511ac6be5bbc8..b7d80bd906100 100644 --- a/Blimp/mode.cpp +++ b/Blimp/mode.cpp @@ -16,9 +16,10 @@ Mode::Mode(void) : inertial_nav(blimp.inertial_nav), ahrs(blimp.ahrs), motors(blimp.motors), + loiter(blimp.loiter), channel_right(blimp.channel_right), channel_front(blimp.channel_front), - channel_down(blimp.channel_down), + channel_up(blimp.channel_up), channel_yaw(blimp.channel_yaw), G_Dt(blimp.G_Dt) { }; @@ -41,6 +42,9 @@ Mode *Blimp::mode_from_mode_num(const Mode::Number mode) case Mode::Number::LOITER: ret = &mode_loiter; break; + case Mode::Number::RTL: + ret = &mode_rtl; + break; default: break; } @@ -136,7 +140,7 @@ void Blimp::update_flight_mode() // exit_mode - high level call to organise cleanup as a flight mode is exited void Blimp::exit_mode(Mode *&old_flightmode, - Mode *&new_flightmode){} + Mode *&new_flightmode) {} // notify_flight_mode - sets notify object based on current flight mode. Only used for OreoLED notify device void Blimp::notify_flight_mode() @@ -152,18 +156,23 @@ void Mode::update_navigation() run_autopilot(); } -// returns desired angle in centi-degrees -void Mode::get_pilot_desired_accelerations(float &right_out, float &front_out) const +// returns desired thrust/acceleration +void Mode::get_pilot_input(Vector3f &pilot, float &yaw) { // throttle failsafe check if (blimp.failsafe.radio || !blimp.ap.rc_receiver_present) { - right_out = 0; - front_out = 0; + pilot.y = 0; + pilot.x = 0; + pilot.z = 0; + yaw = 0; return; } - // fetch roll and pitch inputs - right_out = channel_right->get_control_in(); - front_out = channel_front->get_control_in(); + // fetch pilot inputs + pilot.y = channel_right->get_control_in() / float(RC_SCALE); + pilot.x = channel_front->get_control_in() / float(RC_SCALE); + //TODO: need to make this channel_up instead, and then have it .negative. before being sent to pilot.z -> this is "throttle" channel, so higher = up. + pilot.z = -channel_up->get_control_in() / float(RC_SCALE); + yaw = channel_yaw->get_control_in() / float(RC_SCALE); } bool Mode::is_disarmed_or_landed() const diff --git a/Blimp/mode.h b/Blimp/mode.h index 81ef932059a89..8a990fbb31b30 100644 --- a/Blimp/mode.h +++ b/Blimp/mode.h @@ -17,6 +17,7 @@ class Mode MANUAL = 1, // manual control VELOCITY = 2, // velocity mode LOITER = 3, // loiter mode (position hold) + RTL = 4, // rtl }; // constructor @@ -83,7 +84,7 @@ class Mode void update_navigation(); // pilot input processing - void get_pilot_desired_accelerations(float &right_out, float &front_out) const; + void get_pilot_input(Vector3f &pilot, float &yaw); protected: @@ -104,75 +105,14 @@ class Mode AP_InertialNav &inertial_nav; AP_AHRS &ahrs; Fins *&motors; + Loiter *&loiter; RC_Channel *&channel_right; RC_Channel *&channel_front; - RC_Channel *&channel_down; + RC_Channel *&channel_up; RC_Channel *&channel_yaw; float &G_Dt; public: - // Navigation Yaw control - class AutoYaw - { - - public: - - // yaw(): main product of AutoYaw; the heading: - float yaw(); - - // mode(): current method of determining desired yaw: - autopilot_yaw_mode mode() const - { - return (autopilot_yaw_mode)_mode; - } - void set_mode_to_default(bool rtl); - void set_mode(autopilot_yaw_mode new_mode); - autopilot_yaw_mode default_mode(bool rtl) const; - - // rate_cds(): desired yaw rate in centidegrees/second: - float rate_cds() const; - void set_rate(float new_rate_cds); - - // set_roi(...): set a "look at" location: - void set_roi(const Location &roi_location); - - void set_fixed_yaw(float angle_deg, - float turn_rate_dps, - int8_t direction, - bool relative_angle); - - private: - - float look_ahead_yaw(); - float roi_yaw(); - - // auto flight mode's yaw mode - uint8_t _mode = AUTO_YAW_LOOK_AT_NEXT_WP; - - // Yaw will point at this location if mode is set to AUTO_YAW_ROI - Vector3f roi; - - // bearing from current location to the ROI - float _roi_yaw; - - // yaw used for YAW_FIXED yaw_mode - int32_t _fixed_yaw; - - // Deg/s we should turn - int16_t _fixed_yaw_slewrate; - - // heading when in yaw_look_ahead_yaw - float _look_ahead_yaw; - - // turn rate (in cds) when auto_yaw_mode is set to AUTO_YAW_RATE - float _rate_cds; - - // used to reduce update rate to 100hz: - uint8_t roi_yaw_counter; - - }; - static AutoYaw auto_yaw; - // pass-through functions to reduce code churn on conversion; // these are candidates for moving into the Mode base // class. @@ -307,7 +247,6 @@ class ModeLoiter : public Mode private: Vector3f target_pos; float target_yaw; - float loop_period; }; class ModeLand : public Mode @@ -350,3 +289,43 @@ class ModeLand : public Mode private: }; + +class ModeRTL : public Mode +{ + +public: + // inherit constructor + using Mode::Mode; + + virtual bool init(bool ignore_checks) override; + virtual void run() override; + + bool requires_GPS() const override + { + return true; + } + bool has_manual_throttle() const override + { + return false; + } + bool allows_arming(bool from_gcs) const override + { + return true; + }; + bool is_autopilot() const override + { + return false; + //TODO + } + +protected: + + const char *name() const override + { + return "RTL"; + } + const char *name4() const override + { + return "RTL"; + } +}; diff --git a/Blimp/mode_land.cpp b/Blimp/mode_land.cpp index f6e334839e1d0..640e69ef528df 100644 --- a/Blimp/mode_land.cpp +++ b/Blimp/mode_land.cpp @@ -13,12 +13,11 @@ void ModeLand::run() motors->down_out = 0; } -// set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts -// this is always called from a failsafe so we trigger notification to pilot -void Blimp::set_mode_land_with_pause(ModeReason reason) +// set_mode_land_failsafe - sets mode to LAND +// this is always called from a failsafe so we trigger notification to pilot +void Blimp::set_mode_land_failsafe(ModeReason reason) { set_mode(Mode::Number::LAND, reason); - //TODO: Add pause // alert pilot to mode change AP_Notify::events.failsafe_mode_change = 1; diff --git a/Blimp/mode_loiter.cpp b/Blimp/mode_loiter.cpp index 71e2794c2bbae..725a702480815 100644 --- a/Blimp/mode_loiter.cpp +++ b/Blimp/mode_loiter.cpp @@ -3,66 +3,47 @@ * Init and run calls for loiter flight mode */ +//Number of seconds of movement that the target position can be ahead of actual position. +#define POS_LAG 1 - bool ModeLoiter::init(bool ignore_checks){ +bool ModeLoiter::init(bool ignore_checks) +{ target_pos = blimp.pos_ned; target_yaw = blimp.ahrs.get_yaw(); return true; - } +} //Runs the main loiter controller void ModeLoiter::run() { const float dt = blimp.scheduler.get_last_loop_time_s(); - - Vector3f pilot; - pilot.x = channel_front->get_control_in() / float(RC_SCALE) * g.max_pos_xy * dt; - pilot.y = channel_right->get_control_in() / float(RC_SCALE) * g.max_pos_xy * dt; - pilot.z = channel_down->get_control_in() / float(RC_SCALE) * g.max_pos_z * dt; - float pilot_yaw = channel_yaw->get_control_in() / float(RC_SCALE) * g.max_pos_yaw * dt; - if (g.simple_mode == 0){ + Vector3f pilot; + float pilot_yaw; + get_pilot_input(pilot, pilot_yaw); + pilot.x *= g.max_pos_xy * dt; + pilot.y *= g.max_pos_xy * dt; + pilot.z *= g.max_pos_z * dt; + pilot_yaw *= g.max_pos_yaw * dt; + + if (g.simple_mode == 0) { //If simple mode is disabled, input is in body-frame, thus needs to be rotated. blimp.rotate_BF_to_NE(pilot.xy()); } - target_pos.x += pilot.x; - target_pos.y += pilot.y; - target_pos.z += pilot.z; - target_yaw = wrap_PI(target_yaw + pilot_yaw); - - //Pos controller's output becomes target for velocity controller - Vector3f target_vel_ef{blimp.pid_pos_xy.update_all(target_pos, blimp.pos_ned, dt, {0,0,0}), 0}; - target_vel_ef.z = blimp.pid_pos_z.update_all(target_pos.z, blimp.pos_ned.z, dt); - float yaw_ef = blimp.ahrs.get_yaw(); - float target_vel_yaw = blimp.pid_pos_yaw.update_error(wrap_PI(target_yaw - yaw_ef), dt); - blimp.pid_pos_yaw.set_target_rate(target_yaw); - blimp.pid_pos_yaw.set_actual_rate(yaw_ef); - - Vector3f target_vel_ef_c{constrain_float(target_vel_ef.x, -g.max_vel_xy, g.max_vel_xy), - constrain_float(target_vel_ef.y, -g.max_vel_xy, g.max_vel_xy), - constrain_float(target_vel_ef.z, -g.max_vel_z, g.max_vel_z)}; - float target_vel_yaw_c = constrain_float(target_vel_yaw, -g.max_vel_yaw, g.max_vel_yaw); - - Vector2f actuator = blimp.pid_vel_xy.update_all(target_vel_ef_c, blimp.vel_ned_filtd, dt, {0,0,0}); - float act_down = blimp.pid_vel_z.update_all(target_vel_ef_c.z, blimp.vel_ned_filtd.z, dt); - blimp.rotate_NE_to_BF(actuator); - float act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw_c, blimp.vel_yaw_filtd, dt); - if(!(blimp.g.dis_mask & (1<<(2-1)))){ - motors->front_out = actuator.x; + if (fabsf(target_pos.x-blimp.pos_ned.x) < (g.max_pos_xy*POS_LAG)) { + target_pos.x += pilot.x; } - if(!(blimp.g.dis_mask & (1<<(1-1)))){ - motors->right_out = actuator.y; + if (fabsf(target_pos.y-blimp.pos_ned.y) < (g.max_pos_xy*POS_LAG)) { + target_pos.y += pilot.y; } - if(!(blimp.g.dis_mask & (1<<(3-1)))){ - motors->down_out = act_down; + if (fabsf(target_pos.z-blimp.pos_ned.z) < (g.max_pos_z*POS_LAG)) { + target_pos.z += pilot.z; } - if(!(blimp.g.dis_mask & (1<<(4-1)))){ - motors->yaw_out = act_yaw; + if (fabsf(wrap_PI(target_yaw-ahrs.get_yaw())) < (g.max_pos_yaw*POS_LAG)) { + target_yaw = wrap_PI(target_yaw + pilot_yaw); } - AP::logger().Write_PSCN(target_pos.x * 100.0, blimp.pos_ned.x * 100.0, 0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0); - AP::logger().Write_PSCE(target_pos.y * 100.0, blimp.pos_ned.y * 100.0, 0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0); - AP::logger().Write_PSCD(-target_pos.z * 100.0, -blimp.pos_ned.z * 100.0, 0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0); + blimp.loiter->run(target_pos, target_yaw, Vector4b{false,false,false,false}); } diff --git a/Blimp/mode_manual.cpp b/Blimp/mode_manual.cpp index 1183b4bd7a1ad..30a9eb7bbafff 100644 --- a/Blimp/mode_manual.cpp +++ b/Blimp/mode_manual.cpp @@ -6,8 +6,11 @@ // Runs the main manual controller void ModeManual::run() { - motors->right_out = channel_right->get_control_in() / float(RC_SCALE); - motors->front_out = channel_front->get_control_in() / float(RC_SCALE); - motors->yaw_out = channel_yaw->get_control_in() / float(RC_SCALE); - motors->down_out = channel_down->get_control_in() / float(RC_SCALE); + Vector3f pilot; + float pilot_yaw; + get_pilot_input(pilot, pilot_yaw); + motors->right_out = pilot.y; + motors->front_out = pilot.x; + motors->yaw_out = pilot_yaw; + motors->down_out = pilot.z; } diff --git a/Blimp/mode_rtl.cpp b/Blimp/mode_rtl.cpp new file mode 100644 index 0000000000000..c1b593cd9c933 --- /dev/null +++ b/Blimp/mode_rtl.cpp @@ -0,0 +1,20 @@ +#include "Blimp.h" +/* + * Init and run calls for rtl flight mode + */ + +//Number of seconds of movement that the target position can be ahead of actual position. +#define POS_LAG 1 + +bool ModeRTL::init(bool ignore_checks) +{ + return true; +} + +//Runs the main rtl controller +void ModeRTL::run() +{ + Vector3f target_pos = {0,0,0}; + float target_yaw = 0; + blimp.loiter->run(target_pos, target_yaw, Vector4b{false,false,false,false}); +} diff --git a/Blimp/mode_velocity.cpp b/Blimp/mode_velocity.cpp index a333d6cbc2944..ae6011c619fb4 100644 --- a/Blimp/mode_velocity.cpp +++ b/Blimp/mode_velocity.cpp @@ -8,34 +8,17 @@ // Runs the main velocity controller void ModeVelocity::run() { - const float dt = blimp.scheduler.get_last_loop_time_s(); - Vector3f target_vel; - target_vel.x = channel_front->get_control_in() / float(RC_SCALE) * g.max_vel_xy; - target_vel.y = channel_right->get_control_in() / float(RC_SCALE) * g.max_vel_xy; - blimp.rotate_BF_to_NE(target_vel.xy()); - target_vel.z = channel_down->get_control_in() / float(RC_SCALE) * g.max_vel_z; - float target_vel_yaw = channel_yaw->get_control_in() / float(RC_SCALE) * g.max_vel_yaw; - - Vector2f actuator = blimp.pid_vel_xy.update_all(target_vel, blimp.vel_ned_filtd, dt, {0,0,0}); - blimp.rotate_NE_to_BF(actuator); - float act_down = blimp.pid_vel_z.update_all(target_vel.z, blimp.vel_ned_filtd.z, dt); - float act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw, blimp.vel_yaw_filtd, dt); - - if(!(blimp.g.dis_mask & (1<<(2-1)))){ - motors->front_out = actuator.x; - } - if(!(blimp.g.dis_mask & (1<<(1-1)))){ - motors->right_out = actuator.y; - } - if(!(blimp.g.dis_mask & (1<<(3-1)))){ - motors->down_out = act_down; - } - if(!(blimp.g.dis_mask & (1<<(4-1)))){ - motors->yaw_out = act_yaw; + float target_vel_yaw; + get_pilot_input(target_vel, target_vel_yaw); + target_vel.x *= g.max_vel_xy; + target_vel.y *= g.max_vel_xy; + if (g.simple_mode == 0) { + //If simple mode is disabled, input is in body-frame, thus needs to be rotated. + blimp.rotate_BF_to_NE(target_vel.xy()); } + target_vel.z *= g.max_vel_z; + target_vel_yaw *= g.max_vel_yaw; - AP::logger().Write_PSCN(0.0, blimp.pos_ned.x * 100.0, 0.0, 0.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0); - AP::logger().Write_PSCE(0.0, blimp.pos_ned.y * 100.0, 0.0, 0.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0); - AP::logger().Write_PSCD(0.0, -blimp.pos_ned.z * 100.0, 0.0, 0.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0); + blimp.loiter->run_vel(target_vel, target_vel_yaw, Vector4b{false,false,false,false}); } diff --git a/Blimp/motors.cpp b/Blimp/motors.cpp index c2034774880f4..35ba08b90cce9 100644 --- a/Blimp/motors.cpp +++ b/Blimp/motors.cpp @@ -18,7 +18,7 @@ void Blimp::arm_motors_check() } // ensure throttle is down - if (channel_down->get_control_in() > 0) { //MIR what dow we do with this? + if (channel_up->get_control_in() > 0) { //MIR what dow we do with this? arming_counter = 0; return; } diff --git a/Blimp/radio.cpp b/Blimp/radio.cpp index e7640c44c2aa9..861c057820a5f 100644 --- a/Blimp/radio.cpp +++ b/Blimp/radio.cpp @@ -8,23 +8,23 @@ void Blimp::default_dead_zones() { channel_right->set_default_dead_zone(20); channel_front->set_default_dead_zone(20); - channel_down->set_default_dead_zone(30); + channel_up->set_default_dead_zone(30); channel_yaw->set_default_dead_zone(20); rc().channel(CH_6)->set_default_dead_zone(0); } void Blimp::init_rc_in() { - channel_right = rc().channel(rcmap.roll()-1); - channel_front = rc().channel(rcmap.pitch()-1); - channel_down = rc().channel(rcmap.throttle()-1); - channel_yaw = rc().channel(rcmap.yaw()-1); + channel_right = rc().channel(rcmap.roll()-1); + channel_front = rc().channel(rcmap.pitch()-1); + channel_up = rc().channel(rcmap.throttle()-1); + channel_yaw = rc().channel(rcmap.yaw()-1); // set rc channel ranges channel_right->set_angle(RC_SCALE); channel_front->set_angle(RC_SCALE); channel_yaw->set_angle(RC_SCALE); - channel_down->set_angle(RC_SCALE); + channel_up->set_angle(RC_SCALE); // set default dead zones default_dead_zones(); @@ -58,14 +58,14 @@ void Blimp::read_radio() if (rc().read_input()) { ap.new_radio_frame = true; - set_throttle_and_failsafe(channel_down->get_radio_in()); - set_throttle_zero_flag(channel_down->get_control_in()); + set_throttle_and_failsafe(channel_up->get_radio_in()); + set_throttle_zero_flag(channel_up->get_control_in()); // RC receiver must be attached if we've just got input ap.rc_receiver_present = true; const float dt = (tnow_ms - last_radio_update_ms)*1.0e-3f; - rc_throttle_control_in_filter.apply(channel_down->get_control_in(), dt); + rc_throttle_control_in_filter.apply(channel_up->get_control_in(), dt); last_radio_update_ms = tnow_ms; return; } diff --git a/Blimp/system.cpp b/Blimp/system.cpp index cec5103bb53aa..f3d8bdc08e69f 100644 --- a/Blimp/system.cpp +++ b/Blimp/system.cpp @@ -46,6 +46,7 @@ void Blimp::init_ardupilot() // allocate the motors class allocate_motors(); + loiter = new Loiter(blimp.scheduler.get_loop_rate_hz()); // initialise rc channels including setting mode rc().convert_options(RC_Channel::AUX_FUNC::ARMDISARM_UNUSED, RC_Channel::AUX_FUNC::ARMDISARM); @@ -58,7 +59,9 @@ void Blimp::init_ardupilot() // motors initialised so parameters can be sent ap.initialised_params = true; +#if AP_RELAY_ENABLED relay.init(); +#endif /* * setup the 'main loop is dead' check. Note that this relies on @@ -87,11 +90,6 @@ void Blimp::init_ardupilot() g2.scripting.init(); #endif // AP_SCRIPTING_ENABLED - // we don't want writes to the serial port to cause us to pause - // mid-flight, so set the serial ports non-blocking once we are - // ready to fly - serial_manager.set_blocking_writes_all(false); - ins.set_log_raw_bit(MASK_LOG_IMU_RAW); // setup fin output diff --git a/Blimp/version.h b/Blimp/version.h index a922ff5c4c1e0..e5bfa27c87be8 100644 --- a/Blimp/version.h +++ b/Blimp/version.h @@ -6,14 +6,14 @@ #include "ap_version.h" -#define THISFIRMWARE "Blimp V4.3.0-dev" +#define THISFIRMWARE "Blimp V4.5.0-dev" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,3,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 4,5,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 4 -#define FW_MINOR 3 +#define FW_MINOR 5 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV -#include \ No newline at end of file +#include diff --git a/Blimp/wscript b/Blimp/wscript index a45b13a313750..34cb8c170490e 100644 --- a/Blimp/wscript +++ b/Blimp/wscript @@ -7,9 +7,7 @@ def build(bld): name=vehicle + '_libs', ap_vehicle=vehicle, ap_libraries=bld.ap_common_vehicle_libraries() + [ - 'AC_AttitudeControl', 'AC_InputManager', - 'AC_WPNav', 'AP_InertialNav', 'AP_RCMapper', 'AP_Avoidance', @@ -27,5 +25,4 @@ def build(bld): program_name='blimp', program_groups=['bin', 'blimp'], use=vehicle + '_libs', - defines=['FRAME_CONFIG=MULTICOPTER_FRAME'], ) diff --git a/Dockerfile b/Dockerfile index f0be5d2aaa609..fd24eaff53574 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,10 +1,17 @@ -FROM ubuntu:20.04 +ARG BASE_IMAGE="ubuntu" +ARG TAG="22.04" +FROM ${BASE_IMAGE}:${TAG} WORKDIR /ardupilot ARG DEBIAN_FRONTEND=noninteractive ARG USER_NAME=ardupilot ARG USER_UID=1000 ARG USER_GID=1000 +ARG SKIP_AP_EXT_ENV=1 +ARG SKIP_AP_GRAPHIC_ENV=1 +ARG SKIP_AP_COV_ENV=1 +ARG SKIP_AP_GIT_CHECK=1 + RUN groupadd ${USER_NAME} --gid ${USER_GID}\ && useradd -l -m ${USER_NAME} -u ${USER_UID} -g ${USER_GID} -s /bin/bash @@ -27,7 +34,7 @@ RUN chown -R ${USER_NAME}:${USER_NAME} /${USER_NAME} USER ${USER_NAME} -ENV SKIP_AP_EXT_ENV=1 SKIP_AP_GRAPHIC_ENV=1 SKIP_AP_COV_ENV=1 SKIP_AP_GIT_CHECK=1 +ENV SKIP_AP_EXT_ENV=$SKIP_AP_EXT_ENV SKIP_AP_GRAPHIC_ENV=$SKIP_AP_GRAPHIC_ENV SKIP_AP_COV_ENV=$SKIP_AP_COV_ENV SKIP_AP_GIT_CHECK=$SKIP_AP_GIT_CHECK RUN Tools/environment_install/install-prereqs-ubuntu.sh -y # add waf alias to ardupilot waf to .ardupilot_env diff --git a/README.md b/README.md index 36994430d4fcb..7b603fc163242 100644 --- a/README.md +++ b/README.md @@ -77,7 +77,7 @@ It is continually being expanded to provide support for new emerging vehicle typ The ArduPilot project is licensed under the GNU General Public License, version 3. -- [Overview of license](https://dev.ardupilot.com/wiki/license-gplv3) +- [Overview of license](https://ardupilot.org/dev/docs/license-gplv3.html) - [Full Text](https://github.com/ArduPilot/ardupilot/blob/master/COPYING.txt) diff --git a/Rover/AP_Arming.cpp b/Rover/AP_Arming.cpp index 805f23d720a49..02744eacd1919 100644 --- a/Rover/AP_Arming.cpp +++ b/Rover/AP_Arming.cpp @@ -59,7 +59,7 @@ bool AP_Arming_Rover::gps_checks(bool display_failure) return false; } - // ensure position esetimate is ok + // ensure position estimate is ok if (!rover.ekf_position_ok()) { // vehicle level position estimate checks check_failed(display_failure, "Need Position Estimate"); @@ -78,11 +78,7 @@ bool AP_Arming_Rover::pre_arm_checks(bool report) //are arming checks disabled? if (checks_to_perform == 0) { - return true; - } - if (SRV_Channels::get_emergency_stop()) { - check_failed(report, "Motors Emergency Stopped"); - return false; + return mandatory_checks(report); } if (rover.g2.sailboat.sail_enabled() && !rover.g2.windvane.enabled()) { @@ -134,7 +130,7 @@ bool AP_Arming_Rover::arm(AP_Arming::Method method, const bool do_arming_checks) update_soft_armed(); - gcs().send_text(MAV_SEVERITY_INFO, "Throttle armed"); + send_arm_disarm_statustext("Throttle armed"); return true; } @@ -154,7 +150,7 @@ bool AP_Arming_Rover::disarm(const AP_Arming::Method method, bool do_disarm_chec update_soft_armed(); - gcs().send_text(MAV_SEVERITY_INFO, "Throttle disarmed"); + send_arm_disarm_statustext("Throttle disarmed"); return true; } diff --git a/Rover/AP_ExternalControl_Rover.cpp b/Rover/AP_ExternalControl_Rover.cpp new file mode 100644 index 0000000000000..1c08f92a9b5fd --- /dev/null +++ b/Rover/AP_ExternalControl_Rover.cpp @@ -0,0 +1,37 @@ +/* + external control library for rover + */ + + +#include "AP_ExternalControl_Rover.h" +#if AP_EXTERNAL_CONTROL_ENABLED + +#include "Rover.h" + +/* + set linear velocity and yaw rate. Pass NaN for yaw_rate_rads to not control yaw + velocity is in earth frame, NED, m/s +*/ +bool AP_ExternalControl_Rover::set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, float yaw_rate_rads) +{ + if (!ready_for_external_control()) { + return false; + } + + // rover is commanded in body-frame using FRD convention + auto &ahrs = AP::ahrs(); + Vector3f linear_velocity_body = ahrs.earth_to_body(linear_velocity); + + const float target_speed = linear_velocity_body.x; + const float turn_rate_cds = isnan(yaw_rate_rads)? 0: degrees(yaw_rate_rads)*100; + + rover.mode_guided.set_desired_turn_rate_and_speed(turn_rate_cds, target_speed); + return true; +} + +bool AP_ExternalControl_Rover::ready_for_external_control() +{ + return rover.control_mode->in_guided_mode() && rover.arming.is_armed(); +} + +#endif // AP_EXTERNAL_CONTROL_ENABLED diff --git a/Rover/AP_ExternalControl_Rover.h b/Rover/AP_ExternalControl_Rover.h new file mode 100644 index 0000000000000..434972833e45d --- /dev/null +++ b/Rover/AP_ExternalControl_Rover.h @@ -0,0 +1,26 @@ +/* + external control library for rover + */ +#pragma once + +#include + +#if AP_EXTERNAL_CONTROL_ENABLED + +class AP_ExternalControl_Rover : public AP_ExternalControl { +public: + /* + Set linear velocity and yaw rate. Pass NaN for yaw_rate_rads to not control yaw. + Velocity is in earth frame, NED [m/s]. + Yaw is in earth frame, NED [rad/s]. + */ + bool set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, float yaw_rate_rads)override WARN_IF_UNUSED; +private: + /* + Return true if Rover is ready to handle external control data. + Currently checks mode and arm states. + */ + bool ready_for_external_control() WARN_IF_UNUSED; +}; + +#endif // AP_EXTERNAL_CONTROL_ENABLED diff --git a/Rover/AP_Rally.cpp b/Rover/AP_Rally.cpp index 1a836159645ef..b0daf506f1085 100644 --- a/Rover/AP_Rally.cpp +++ b/Rover/AP_Rally.cpp @@ -13,11 +13,13 @@ along with this program. If not, see . */ -#include +#include "AP_Rally.h" + +#if HAL_RALLY_ENABLED #include "Rover.h" -#include "AP_Rally.h" +#include bool AP_Rally_Rover::is_valid(const Location &rally_point) const { @@ -28,3 +30,5 @@ bool AP_Rally_Rover::is_valid(const Location &rally_point) const #endif return true; } + +#endif // HAL_RALLY_ENABLED diff --git a/Rover/AP_Rally.h b/Rover/AP_Rally.h index 0055dcb1e5e93..197d6c92797ff 100644 --- a/Rover/AP_Rally.h +++ b/Rover/AP_Rally.h @@ -15,7 +15,8 @@ #pragma once #include -#include + +#if HAL_RALLY_ENABLED class AP_Rally_Rover : public AP_Rally { @@ -28,3 +29,5 @@ class AP_Rally_Rover : public AP_Rally private: bool is_valid(const Location &rally_point) const override; }; + +#endif // HAL_RALLY_ENABLED diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index acfa8699b9836..e13e35bebc255 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -53,7 +53,7 @@ MAV_MODE GCS_MAVLINK_Rover::base_mode() const uint32_t GCS_Rover::custom_mode() const { - return rover.control_mode->mode_number(); + return (uint32_t)rover.control_mode->mode_number(); } MAV_STATE GCS_MAVLINK_Rover::vehicle_system_status() const @@ -149,6 +149,7 @@ int16_t GCS_MAVLINK_Rover::vfr_hud_throttle() const return rover.g2.motors.get_throttle(); } +#if AP_RANGEFINDER_ENABLED void GCS_MAVLINK_Rover::send_rangefinder() const { float distance = 0; @@ -178,6 +179,7 @@ void GCS_MAVLINK_Rover::send_rangefinder() const distance, voltage); } +#endif // AP_RANGEFINDER_ENABLED /* send PID tuning message @@ -398,8 +400,10 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id) void GCS_MAVLINK_Rover::packetReceived(const mavlink_status_t &status, const mavlink_message_t &msg) { +#if AP_FOLLOW_ENABLED // pass message to follow library rover.g2.follow.handle_msg(msg); +#endif GCS_MAVLINK::packetReceived(status, msg); } @@ -521,15 +525,21 @@ static const ap_message STREAM_RAW_SENSORS_msgs[] = { static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { MSG_SYS_STATUS, MSG_POWER_STATUS, +#if HAL_WITH_MCU_MONITORING MSG_MCU_STATUS, +#endif MSG_MEMINFO, MSG_CURRENT_WAYPOINT, MSG_GPS_RAW, MSG_GPS_RTK, +#if GPS_MAX_RECEIVERS > 1 MSG_GPS2_RAW, MSG_GPS2_RTK, +#endif MSG_NAV_CONTROLLER_OUTPUT, +#if AP_FENCE_ENABLED MSG_FENCE_STATUS, +#endif MSG_POSITION_TARGET_GLOBAL_INT, }; static const ap_message STREAM_POSITION_msgs[] = { @@ -546,7 +556,9 @@ static const ap_message STREAM_RC_CHANNELS_msgs[] = { }; static const ap_message STREAM_EXTRA1_msgs[] = { MSG_ATTITUDE, +#if AP_SIM_ENABLED MSG_SIMSTATE, +#endif MSG_AHRS2, MSG_PID_TUNING, }; @@ -559,18 +571,28 @@ static const ap_message STREAM_EXTRA3_msgs[] = { MSG_RANGEFINDER, MSG_DISTANCE_SENSOR, MSG_SYSTEM_TIME, +#if AP_BATTERY_ENABLED MSG_BATTERY_STATUS, +#endif +#if HAL_MOUNT_ENABLED MSG_GIMBAL_DEVICE_ATTITUDE_STATUS, +#endif +#if AP_OPTICALFLOW_ENABLED MSG_OPTICAL_FLOW, +#endif +#if COMPASS_CAL_ENABLED MSG_MAG_CAL_REPORT, MSG_MAG_CAL_PROGRESS, +#endif MSG_EKF_STATUS_REPORT, MSG_VIBRATION, #if AP_RPM_ENABLED MSG_RPM, #endif MSG_WHEEL_DISTANCE, +#if HAL_WITH_ESC_TELEM MSG_ESC_TELEMETRY, +#endif #if HAL_EFI_ENABLED MSG_EFI_STATUS, #endif @@ -580,7 +602,9 @@ static const ap_message STREAM_PARAMS_msgs[] = { }; static const ap_message STREAM_ADSB_msgs[] = { MSG_ADSB_VEHICLE, +#if AP_AIS_ENABLED MSG_AIS_VESSEL, +#endif }; const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = { @@ -608,15 +632,15 @@ bool GCS_MAVLINK_Rover::handle_guided_request(AP_Mission::Mission_Command &cmd) return rover.mode_guided.set_desired_location(cmd.content.location); } -MAV_RESULT GCS_MAVLINK_Rover::_handle_command_preflight_calibration(const mavlink_command_long_t &packet, const mavlink_message_t &msg) +MAV_RESULT GCS_MAVLINK_Rover::_handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { - if (is_equal(packet.param6, 1.0f)) { + if (packet.y == 1) { if (rover.g2.windvane.start_direction_calibration()) { return MAV_RESULT_ACCEPTED; } else { return MAV_RESULT_FAILED; } - } else if (is_equal(packet.param6, 2.0f)) { + } else if (packet.y == 2) { if (rover.g2.windvane.start_speed_calibration()) { return MAV_RESULT_ACCEPTED; } else { @@ -635,7 +659,7 @@ bool GCS_MAVLINK_Rover::set_home(const Location& loc, bool _lock) { return rover.set_home(loc, _lock); } -MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_packet(const mavlink_command_int_t &packet) +MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { switch (packet.command) { @@ -655,45 +679,46 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_packet(const mavlink_command_in rover.control_mode->set_reversed(is_equal(packet.param1,1.0f)); return MAV_RESULT_ACCEPTED; - default: - return GCS_MAVLINK::handle_command_int_packet(packet); - } -} - -MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_long_t &packet) -{ - switch (packet.command) { - case MAV_CMD_NAV_RETURN_TO_LAUNCH: if (rover.set_mode(rover.mode_rtl, ModeReason::GCS_COMMAND)) { return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; + case MAV_CMD_DO_MOTOR_TEST: + // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle) + // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum) + // param3 : throttle (range depends upon param2) + // param4 : timeout (in seconds) + return rover.mavlink_motor_test_start(*this, + (AP_MotorsUGV::motor_test_order)packet.param1, + static_cast(packet.param2), + static_cast(packet.param3), + packet.param4); + case MAV_CMD_MISSION_START: if (rover.set_mode(rover.mode_auto, ModeReason::GCS_COMMAND)) { return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; - case MAV_CMD_DO_CHANGE_SPEED: - // param1 : unused - // param2 : new speed in m/s - if (!rover.control_mode->set_desired_speed(packet.param2)) { - return MAV_RESULT_FAILED; - } - return MAV_RESULT_ACCEPTED; +#if AP_MAVLINK_MAV_CMD_NAV_SET_YAW_SPEED_ENABLED + case MAV_CMD_NAV_SET_YAW_SPEED: + send_received_message_deprecation_warning("MAV_CMD_NAV_SET_YAW_SPEED"); + return handle_command_nav_set_yaw_speed(packet, msg); +#endif - case MAV_CMD_DO_SET_REVERSE: - // param1 : Direction (0=Forward, 1=Reverse) - rover.control_mode->set_reversed(is_equal(packet.param1,1.0f)); - return MAV_RESULT_ACCEPTED; + default: + return GCS_MAVLINK::handle_command_int_packet(packet, msg); + } +} - case MAV_CMD_NAV_SET_YAW_SPEED: - { - // param1 : yaw angle to adjust direction by in centidegress - // param2 : Speed - normalized to 0 .. 1 - // param3 : 0 = absolute, 1 = relative +#if AP_MAVLINK_MAV_CMD_NAV_SET_YAW_SPEED_ENABLED +MAV_RESULT GCS_MAVLINK_Rover::handle_command_nav_set_yaw_speed(const mavlink_command_int_t &packet, const mavlink_message_t &msg) +{ + // param1 : yaw angle (may be absolute or relative) + // param2 : Speed - in metres/second + // param3 : 0 = param1 is absolute, 1 = param1 is relative // exit if vehicle is not in Guided mode if (!rover.control_mode->in_guided_mode()) { @@ -709,23 +734,8 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_l rover.mode_guided.set_desired_heading_and_speed(packet.param1 * 100.0f, packet.param2); } return MAV_RESULT_ACCEPTED; - } - - case MAV_CMD_DO_MOTOR_TEST: - // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle) - // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum) - // param3 : throttle (range depends upon param2) - // param4 : timeout (in seconds) - return rover.mavlink_motor_test_start(*this, - (AP_MotorsUGV::motor_test_order)packet.param1, - static_cast(packet.param2), - static_cast(packet.param3), - packet.param4); - - default: - return GCS_MAVLINK::handle_command_long_packet(packet); - } } +#endif MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_do_reposition(const mavlink_command_int_t &packet) { @@ -1063,7 +1073,7 @@ void GCS_MAVLINK_Rover::handle_radio(const mavlink_message_t &msg) */ void GCS_MAVLINK_Rover::handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) { -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED rover.precland.handle_msg(packet, timestamp_ms); #endif } diff --git a/Rover/GCS_Mavlink.h b/Rover/GCS_Mavlink.h index 028c92f497a6b..050109f5ccc3b 100644 --- a/Rover/GCS_Mavlink.h +++ b/Rover/GCS_Mavlink.h @@ -2,6 +2,11 @@ #include + // set 0 in 4.6, remove feature in 4.7: +#ifndef AP_MAVLINK_MAV_CMD_NAV_SET_YAW_SPEED_ENABLED +#define AP_MAVLINK_MAV_CMD_NAV_SET_YAW_SPEED_ENABLED 1 +#endif + class GCS_MAVLINK_Rover : public GCS_MAVLINK { public: @@ -15,10 +20,10 @@ class GCS_MAVLINK_Rover : public GCS_MAVLINK uint8_t sysid_my_gcs() const override; bool sysid_enforce() const override; - MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet, const mavlink_message_t &msg) override; - MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet) override; - MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override; + MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; + 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); + MAV_RESULT handle_command_nav_set_yaw_speed(const mavlink_command_int_t &packet, const mavlink_message_t &msg); void send_position_target_global_int() override; @@ -53,7 +58,9 @@ class GCS_MAVLINK_Rover : public GCS_MAVLINK int16_t vfr_hud_throttle() const override; +#if AP_RANGEFINDER_ENABLED void send_rangefinder() const override; +#endif #if HAL_HIGH_LATENCY2_ENABLED uint8_t high_latency_tgt_heading() const override; diff --git a/Rover/Log.cpp b/Rover/Log.cpp index 6f40ceae0bf59..36403f66d253e 100644 --- a/Rover/Log.cpp +++ b/Rover/Log.cpp @@ -78,8 +78,10 @@ void Rover::Log_Write_Depth() (double)(s->distance()), temp_C); } +#if AP_RANGEFINDER_ENABLED // send water depth and temp to ground station gcs().send_message(MSG_WATER_DEPTH); +#endif } // guided mode logging @@ -241,7 +243,7 @@ void Rover::Log_Write_RC(void) void Rover::Log_Write_Vehicle_Startup_Messages() { // only 200(?) bytes are guaranteed by AP_Logger - logger.Write_Mode(control_mode->mode_number(), control_mode_reason); + logger.Write_Mode((uint8_t)control_mode->mode_number(), control_mode_reason); ahrs.Log_Write_Home_And_Origin(); gps.Write_AP_Logger_Log_Startup_messages(); } diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index 1de13e3f6bf3b..5d4a84fc9b08b 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -27,9 +27,9 @@ const AP_Param::Info Rover::var_info[] = { // @Param: INITIAL_MODE // @DisplayName: Initial driving mode // @Description: This selects the mode to start in on boot. This is useful for when you want to start in AUTO mode on boot without a receiver. Usually used in combination with when AUTO_TRIGGER_PIN or AUTO_KICKSTART. - // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,7:Simple,10:Auto,11:RTL,12:SmartRTL,15:Guided + // @CopyValuesFrom: MODE1 // @User: Advanced - GSCALAR(initial_mode, "INITIAL_MODE", Mode::Number::MANUAL), + GSCALAR(initial_mode, "INITIAL_MODE", (int8_t)Mode::Number::MANUAL), // @Param: SYSID_THISMAV // @DisplayName: MAVLink system ID of this vehicle @@ -42,6 +42,7 @@ const AP_Param::Info Rover::var_info[] = { // @DisplayName: MAVLink ground station ID // @Description: The identifier of the ground station in the MAVLink protocol. Don't change this unless you also modify the ground station to match. // @Range: 1 255 + // @Increment: 1 // @User: Advanced GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), @@ -109,7 +110,7 @@ const AP_Param::Info Rover::var_info[] = { // @Description: What to do on a failsafe event // @Values: 0:Nothing,1:RTL,2:Hold,3:SmartRTL or RTL,4:SmartRTL or Hold // @User: Standard - GSCALAR(fs_action, "FS_ACTION", Failsafe_Action_Hold), + GSCALAR(fs_action, "FS_ACTION", (int8_t)FailsafeAction::Hold), // @Param: FS_TIMEOUT // @DisplayName: Failsafe timeout @@ -171,41 +172,41 @@ const AP_Param::Info Rover::var_info[] = { // @Param: MODE1 // @DisplayName: Mode1 - // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,7:Simple,10:Auto,11:RTL,12:SmartRTL,15:Guided + // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,7:Simple,8:Dock,9:Circle,10:Auto,11:RTL,12:SmartRTL,15:Guided // @User: Standard // @Description: Driving mode for switch position 1 (910 to 1230 and above 2049) - GSCALAR(mode1, "MODE1", Mode::Number::MANUAL), + GSCALAR(mode1, "MODE1", (int8_t)Mode::Number::MANUAL), // @Param: MODE2 // @DisplayName: Mode2 // @Description: Driving mode for switch position 2 (1231 to 1360) // @CopyValuesFrom: MODE1 // @User: Standard - GSCALAR(mode2, "MODE2", Mode::Number::MANUAL), + GSCALAR(mode2, "MODE2", (int8_t)Mode::Number::MANUAL), // @Param: MODE3 // @CopyFieldsFrom: MODE1 // @DisplayName: Mode3 // @Description: Driving mode for switch position 3 (1361 to 1490) - GSCALAR(mode3, "MODE3", Mode::Number::MANUAL), + GSCALAR(mode3, "MODE3", (int8_t)Mode::Number::MANUAL), // @Param: MODE4 // @CopyFieldsFrom: MODE1 // @DisplayName: Mode4 // @Description: Driving mode for switch position 4 (1491 to 1620) - GSCALAR(mode4, "MODE4", Mode::Number::MANUAL), + GSCALAR(mode4, "MODE4", (int8_t)Mode::Number::MANUAL), // @Param: MODE5 // @CopyFieldsFrom: MODE1 // @DisplayName: Mode5 // @Description: Driving mode for switch position 5 (1621 to 1749) - GSCALAR(mode5, "MODE5", Mode::Number::MANUAL), + GSCALAR(mode5, "MODE5", (int8_t)Mode::Number::MANUAL), // @Param: MODE6 // @CopyFieldsFrom: MODE1 // @DisplayName: Mode6 // @Description: Driving mode for switch position 6 (1750 to 2049) - GSCALAR(mode6, "MODE6", Mode::Number::MANUAL), + GSCALAR(mode6, "MODE6", (int8_t)Mode::Number::MANUAL), // variables not in the g class which contain EEPROM saved variables @@ -221,9 +222,11 @@ const AP_Param::Info Rover::var_info[] = { // @Path: ../libraries/AP_Baro/AP_Baro.cpp GOBJECT(barometer, "BARO", AP_Baro), +#if AP_RELAY_ENABLED // @Group: RELAY_ // @Path: ../libraries/AP_Relay/AP_Relay.cpp GOBJECT(relay, "RELAY_", AP_Relay), +#endif // @Group: RCMAP_ // @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp @@ -297,7 +300,7 @@ const AP_Param::Info Rover::var_info[] = { GOBJECT(camera, "CAM", AP_Camera), #endif -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED // @Group: PLND_ // @Path: ../libraries/AC_PrecLand/AC_PrecLand.cpp GOBJECT(precland, "PLND_", AC_PrecLand), @@ -425,9 +428,11 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_SUBGROUPINFO(afs, "AFS_", 5, ParametersG2, AP_AdvancedFailsafe), #endif +#if AP_BEACON_ENABLED // @Group: BCN // @Path: ../libraries/AP_Beacon/AP_Beacon.cpp AP_SUBGROUPINFO(beacon, "BCN", 6, ParametersG2, AP_Beacon), +#endif // 7 was used by AP_VisualOdometry @@ -513,9 +518,11 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Standard AP_GROUPINFO("CRASH_ANGLE", 22, ParametersG2, crash_angle, 0), +#if AP_FOLLOW_ENABLED // @Group: FOLL // @Path: ../libraries/AP_Follow/AP_Follow.cpp AP_SUBGROUPINFO(follow, "FOLL", 23, ParametersG2, AP_Follow), +#endif // @Param: FRAME_TYPE // @DisplayName: Frame Type @@ -542,7 +549,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Path: ../libraries/AP_WheelEncoder/AP_WheelRateControl.cpp AP_SUBGROUPINFO(wheel_rate_control, "WRC", 27, ParametersG2, AP_WheelRateControl), -#if AP_RALLY == ENABLED +#if HAL_RALLY_ENABLED // @Group: RALLY_ // @Path: AP_Rally.cpp,../libraries/AP_Rally/AP_Rally.cpp AP_SUBGROUPINFO(rally, "RALLY_", 28, ParametersG2, AP_Rally_Rover), @@ -692,6 +699,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Standard AP_GROUPINFO("FS_GCS_TIMEOUT", 56, ParametersG2, fs_gcs_timeout, 5), + // @Group: CIRC + // @Path: mode_circle.cpp + AP_SUBGROUPINFO(mode_circle, "CIRC", 57, ParametersG2, ModeCircle), + AP_GROUPEND }; @@ -730,8 +741,10 @@ ParametersG2::ParametersG2(void) #if ADVANCED_FAILSAFE == ENABLED afs(), #endif +#if AP_BEACON_ENABLED beacon(), - motors(rover.ServoRelayEvents, wheel_rate_control), +#endif + motors(wheel_rate_control), wheel_rate_control(wheel_encoder), attitude_control(), smart_rtl(), @@ -742,7 +755,9 @@ ParametersG2::ParametersG2(void) proximity(), #endif avoid(), +#if AP_FOLLOW_ENABLED follow(), +#endif windvane(), pos_control(attitude_control), wp_nav(attitude_control, pos_control), diff --git a/Rover/Parameters.h b/Rover/Parameters.h index 83f718b91e8c3..035d16f4d0ffb 100644 --- a/Rover/Parameters.h +++ b/Rover/Parameters.h @@ -312,15 +312,17 @@ class ParametersG2 { AP_AdvancedFailsafe_Rover afs; #endif +#if AP_BEACON_ENABLED AP_Beacon beacon; - - // Motor library - AP_MotorsUGV motors; +#endif // wheel encoders AP_WheelEncoder wheel_encoder; AP_WheelRateControl wheel_rate_control; + // Motor library + AP_MotorsUGV motors; + // steering and throttle controller AR_AttitudeControl attitude_control; @@ -358,8 +360,10 @@ class ParametersG2 { // pitch/roll angle for crash check AP_Int8 crash_angle; +#if AP_FOLLOW_ENABLED // follow mode library AP_Follow follow; +#endif // frame type for vehicle (used for vectored motor vehicles and custom motor configs) AP_Int8 frame_type; @@ -377,8 +381,10 @@ class ParametersG2 { AP_Gripper gripper; #endif +#if HAL_RALLY_ENABLED // Rally point library AP_Rally_Rover rally; +#endif // Simple mode types AP_Int8 simple_type; @@ -436,6 +442,8 @@ class ParametersG2 { // FS GCS timeout trigger time AP_Float fs_gcs_timeout; + + class ModeCircle mode_circle; }; extern const AP_Param::Info var_info[]; diff --git a/Rover/RC_Channel.cpp b/Rover/RC_Channel.cpp index e83a9fd0d011a..9a89b66d61376 100644 --- a/Rover/RC_Channel.cpp +++ b/Rover/RC_Channel.cpp @@ -33,6 +33,7 @@ void RC_Channel_Rover::init_aux_function(const aux_func_t ch_option, const AuxSw // the following functions do not need initialising: case AUX_FUNC::ACRO: case AUX_FUNC::AUTO: + case AUX_FUNC::CIRCLE: case AUX_FUNC::FOLLOW: case AUX_FUNC::GUIDED: case AUX_FUNC::HOLD: @@ -62,9 +63,14 @@ void RC_Channel_Rover::init_aux_function(const aux_func_t ch_option, const AuxSw } +bool RC_Channels_Rover::in_rc_failsafe() const +{ + return rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE; +} + bool RC_Channels_Rover::has_valid_input() const { - if (rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) { + if (in_rc_failsafe()) { return false; } return true; @@ -209,16 +215,22 @@ bool RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const AuxSwit do_aux_function_change_mode(rover.mode_loiter, ch_flag); break; +#if MODE_FOLLOW_ENABLED == ENABLED // Set mode to Follow case AUX_FUNC::FOLLOW: do_aux_function_change_mode(rover.mode_follow, ch_flag); break; +#endif // set mode to Simple case AUX_FUNC::SIMPLE: do_aux_function_change_mode(rover.mode_simple, ch_flag); break; + case AUX_FUNC::CIRCLE: + do_aux_function_change_mode(rover.g2.mode_circle, ch_flag); + break; + // trigger sailboat tack case AUX_FUNC::SAILBOAT_TACK: // any switch movement interpreted as request to tack diff --git a/Rover/RC_Channel.h b/Rover/RC_Channel.h index 844b4c1b9069d..d25d714aa261c 100644 --- a/Rover/RC_Channel.h +++ b/Rover/RC_Channel.h @@ -32,6 +32,7 @@ class RC_Channels_Rover : public RC_Channels public: + bool in_rc_failsafe() const override; bool has_valid_input() const override; RC_Channel *get_arming_channel(void) const override; diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp index f8a3ef7800666..82b7f46e37139 100644 --- a/Rover/Rover.cpp +++ b/Rover/Rover.cpp @@ -80,7 +80,9 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = { SCHED_TASK(set_servos, 400, 200, 15), SCHED_TASK_CLASS(AP_GPS, &rover.gps, update, 50, 300, 18), SCHED_TASK_CLASS(AP_Baro, &rover.barometer, update, 10, 200, 21), +#if AP_BEACON_ENABLED SCHED_TASK_CLASS(AP_Beacon, &rover.g2.beacon, update, 50, 200, 24), +#endif #if HAL_PROXIMITY_ENABLED SCHED_TASK_CLASS(AP_Proximity, &rover.g2.proximity, update, 50, 200, 27), #endif @@ -94,12 +96,14 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = { SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&rover.g2.rc_channels, read_mode_switch, 7, 200, 57), SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&rover.g2.rc_channels, read_aux_all, 10, 200, 60), SCHED_TASK_CLASS(AP_BattMonitor, &rover.battery, read, 10, 300, 63), +#if AP_SERVORELAYEVENTS_ENABLED SCHED_TASK_CLASS(AP_ServoRelayEvents, &rover.ServoRelayEvents, update_events, 50, 200, 66), +#endif #if AP_GRIPPER_ENABLED SCHED_TASK_CLASS(AP_Gripper, &rover.g2.gripper, update, 10, 75, 69), -#if PRECISION_LANDING == ENABLED - SCHED_TASK(update_precland, 400, 50, 70), #endif +#if AC_PRECLAND_ENABLED + SCHED_TASK(update_precland, 400, 50, 70), #endif #if AP_RPM_ENABLED SCHED_TASK_CLASS(AP_RPM, &rover.rpm_sensor, update, 10, 100, 72), @@ -114,7 +118,6 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = { SCHED_TASK(fence_check, 10, 200, 84), SCHED_TASK(ekf_check, 10, 100, 87), SCHED_TASK_CLASS(ModeSmartRTL, &rover.mode_smartrtl, save_position, 3, 200, 90), - SCHED_TASK_CLASS(AP_Notify, &rover.notify, update, 50, 300, 93), SCHED_TASK(one_second_loop, 1, 1500, 96), #if HAL_SPRAYER_ENABLED SCHED_TASK_CLASS(AC_Sprayer, &rover.g2.sprayer, update, 3, 90, 99), @@ -204,6 +207,14 @@ bool Rover::set_steering_and_throttle(float steering, float throttle) return true; } +// get steering and throttle (-1 to +1) (for use by scripting) +bool Rover::get_steering_and_throttle(float& steering, float& throttle) +{ + steering = g2.motors.get_steering() / 4500.0; + throttle = g2.motors.get_throttle() * 0.01; + return true; +} + // set desired turn rate (degrees/sec) and speed (m/s). Used for scripting bool Rover::set_desired_turn_rate_and_speed(float turn_rate, float speed) { @@ -377,7 +388,9 @@ void Rover::update_logging1(void) if (should_log(MASK_LOG_THR)) { Log_Write_Throttle(); +#if AP_BEACON_ENABLED g2.beacon.log(); +#endif } if (should_log(MASK_LOG_NTUN)) { @@ -416,6 +429,11 @@ void Rover::update_logging2(void) gyro_fft.write_log_messages(); #endif } +#if HAL_MOUNT_ENABLED + if (should_log(MASK_LOG_CAMERA)) { + camera_mount.write_log(); + } +#endif } @@ -450,6 +468,7 @@ void Rover::one_second_loop(void) // send latest param values to wp_nav g2.wp_nav.set_turn_params(g2.turn_radius, g2.motors.have_skid_steering()); g2.pos_control.set_turn_params(g2.turn_radius, g2.motors.have_skid_steering()); + g2.wheel_rate_control.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); } void Rover::update_current_mode(void) diff --git a/Rover/Rover.h b/Rover/Rover.h index b06a25703329c..c6aa6db49702c 100644 --- a/Rover/Rover.h +++ b/Rover/Rover.h @@ -42,6 +42,12 @@ #include #include #include +#include +#include +#include +#if AP_EXTERNAL_CONTROL_ENABLED +#include "AP_ExternalControl_Rover.h" +#endif // Configuration #include "defines.h" @@ -61,10 +67,10 @@ #include "GCS_Mavlink.h" #include "GCS_Rover.h" #include "AP_Rally.h" -#include "RC_Channel.h" // RC Channel Library -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED #include #endif +#include "RC_Channel.h" // RC Channel Library #include "mode.h" @@ -77,11 +83,15 @@ class Rover : public AP_Vehicle { friend class AP_Arming_Rover; #if ADVANCED_FAILSAFE == ENABLED friend class AP_AdvancedFailsafe_Rover; +#endif +#if AP_EXTERNAL_CONTROL_ENABLED + friend class AP_ExternalControl_Rover; #endif friend class GCS_Rover; friend class Mode; friend class ModeAcro; friend class ModeAuto; + friend class ModeCircle; friend class ModeGuided; friend class ModeHold; friend class ModeLoiter; @@ -89,7 +99,9 @@ class Rover : public AP_Vehicle { friend class ModeManual; friend class ModeRTL; friend class ModeSmartRTL; +#if MODE_FOLLOW_ENABLED == ENABLED friend class ModeFollow; +#endif friend class ModeSimple; #if MODE_DOCK_ENABLED == ENABLED friend class ModeDock; @@ -138,6 +150,11 @@ class Rover : public AP_Vehicle { // Arming/Disarming management class AP_Arming_Rover arming; + // external control implementation +#if AP_EXTERNAL_CONTROL_ENABLED + AP_ExternalControl_Rover external_control; +#endif + #if AP_OPTICALFLOW_ENABLED AP_OpticalFlow optflow; #endif @@ -145,7 +162,7 @@ class Rover : public AP_Vehicle { #if OSD_ENABLED || OSD_PARAM_ENABLED AP_OSD osd; #endif -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED AC_PrecLand precland; #endif // GCS handling @@ -234,7 +251,9 @@ class Rover : public AP_Vehicle { ModeSteering mode_steering; ModeRTL mode_rtl; ModeSmartRTL mode_smartrtl; +#if MODE_FOLLOW_ENABLED == ENABLED ModeFollow mode_follow; +#endif ModeSimple mode_simple; #if MODE_DOCK_ENABLED == ENABLED ModeDock mode_dock; @@ -256,6 +275,7 @@ class Rover : public AP_Vehicle { bool set_target_location(const Location& target_loc) override; bool set_target_velocity_NED(const Vector3f& vel_ned) 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 bool set_desired_turn_rate_and_speed(float turn_rate, float speed) override; bool set_desired_speed(float speed) override; @@ -358,9 +378,15 @@ class Rover : public AP_Vehicle { void init_ardupilot() override; void startup_ground(void); void update_ahrs_flyforward(); + bool gcs_mode_enabled(const Mode::Number mode_num) const; bool set_mode(Mode &new_mode, ModeReason reason); bool set_mode(const uint8_t new_mode, ModeReason reason) override; + bool set_mode(Mode::Number new_mode, ModeReason reason); uint8_t get_mode() const override { return (uint8_t)control_mode->mode_number(); } + bool current_mode_requires_mission() const override { + return control_mode == &mode_auto; + } + void startup_INS_ground(void); void notify_mode(const Mode *new_mode); uint8_t check_digital_pin(uint8_t pin); @@ -372,13 +398,13 @@ class Rover : public AP_Vehicle { bool get_wp_bearing_deg(float &bearing) const override; bool get_wp_crosstrack_error_m(float &xtrack_error) const override; - enum Failsafe_Action { - Failsafe_Action_None = 0, - Failsafe_Action_RTL = 1, - Failsafe_Action_Hold = 2, - Failsafe_Action_SmartRTL = 3, - Failsafe_Action_SmartRTL_Hold = 4, - Failsafe_Action_Terminate = 5 + enum class FailsafeAction: int8_t { + None = 0, + RTL = 1, + Hold = 2, + SmartRTL = 3, + SmartRTL_Hold = 4, + Terminate = 5 }; enum class Failsafe_Options : uint32_t { @@ -386,12 +412,12 @@ class Rover : public AP_Vehicle { }; static constexpr int8_t _failsafe_priorities[] = { - Failsafe_Action_Terminate, - Failsafe_Action_Hold, - Failsafe_Action_RTL, - Failsafe_Action_SmartRTL_Hold, - Failsafe_Action_SmartRTL, - Failsafe_Action_None, + (int8_t)FailsafeAction::Terminate, + (int8_t)FailsafeAction::Hold, + (int8_t)FailsafeAction::RTL, + (int8_t)FailsafeAction::SmartRTL_Hold, + (int8_t)FailsafeAction::SmartRTL, + (int8_t)FailsafeAction::None, -1 // the priority list must end with a sentinel of -1 }; static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1, diff --git a/Rover/afs_rover.cpp b/Rover/afs_rover.cpp index e0349eefc0ea8..4265a20887ebb 100644 --- a/Rover/afs_rover.cpp +++ b/Rover/afs_rover.cpp @@ -29,4 +29,9 @@ AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Rover::afs_mode(void) return AP_AdvancedFailsafe::AFS_STABILIZED; } +//to force entering auto mode when datalink loss + void AP_AdvancedFailsafe_Rover::set_mode_auto(void) + { + over.set_mode(rover.mode_auto,ModeReason::GCS_FAILSAFE); + } #endif // ADVANCED_FAILSAFE diff --git a/Rover/afs_rover.h b/Rover/afs_rover.h index 36dd0a0aa9f2f..aa3473ddc39d5 100644 --- a/Rover/afs_rover.h +++ b/Rover/afs_rover.h @@ -38,6 +38,9 @@ class AP_AdvancedFailsafe_Rover : public AP_AdvancedFailsafe // return the AFS mapped control mode enum control_mode afs_mode(void) override; + + //to force entering auto mode when datalink loss + void set_mode_auto(void) override; }; #endif // ADVANCED_FAILSAFE diff --git a/Rover/config.h b/Rover/config.h index 6983f9d122eed..7a280e88f3497 100644 --- a/Rover/config.h +++ b/Rover/config.h @@ -31,19 +31,6 @@ #error XXX #endif -////////////////////////////////////////////////////////////////////////////// -// RALLY POINTS -// -#ifndef AP_RALLY - #define AP_RALLY ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// Precision Landing with companion computer or IRLock sensor -#ifndef PRECISION_LANDING - # define PRECISION_LANDING ENABLED -#endif - ////////////////////////////////////////////////////////////////////////////// // NAVL1 // @@ -70,7 +57,13 @@ ////////////////////////////////////////////////////////////////////////////// // Dock mode - allows vehicle to dock to a docking target #ifndef MODE_DOCK_ENABLED -# define MODE_DOCK_ENABLED PRECISION_LANDING +# define MODE_DOCK_ENABLED AC_PRECLAND_ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Follow mode - allows vehicle to follow target +#ifndef MODE_FOLLOW_ENABLED +# define MODE_FOLLOW_ENABLED AP_FOLLOW_ENABLED #endif diff --git a/Rover/failsafe.cpp b/Rover/failsafe.cpp index 2520c73f66334..da877c2bb7482 100644 --- a/Rover/failsafe.cpp +++ b/Rover/failsafe.cpp @@ -80,29 +80,30 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, const char* type_str, bool o // continue with mission in auto mode gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe - Continuing Auto Mode"); } else { - switch (g.fs_action) { - case Failsafe_Action_None: + switch ((FailsafeAction)g.fs_action.get()) { + case FailsafeAction::None: break; - case Failsafe_Action_RTL: - if (!set_mode(mode_rtl, ModeReason::FAILSAFE)) { - set_mode(mode_hold, ModeReason::FAILSAFE); + case FailsafeAction::SmartRTL: + if (set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) { + break; } - break; - case Failsafe_Action_Hold: - set_mode(mode_hold, ModeReason::FAILSAFE); - break; - case Failsafe_Action_SmartRTL: - if (!set_mode(mode_smartrtl, ModeReason::FAILSAFE)) { - if (!set_mode(mode_rtl, ModeReason::FAILSAFE)) { - set_mode(mode_hold, ModeReason::FAILSAFE); - } + FALLTHROUGH; + case FailsafeAction::RTL: + if (set_mode(mode_rtl, ModeReason::BATTERY_FAILSAFE)) { + break; } + FALLTHROUGH; + case FailsafeAction::Hold: + set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE); break; - case Failsafe_Action_SmartRTL_Hold: + case FailsafeAction::SmartRTL_Hold: if (!set_mode(mode_smartrtl, ModeReason::FAILSAFE)) { set_mode(mode_hold, ModeReason::FAILSAFE); } break; + case FailsafeAction::Terminate: + arming.disarm(AP_Arming::Method::FAILSAFE_ACTION_TERMINATE); + break; } } } @@ -110,28 +111,28 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, const char* type_str, bool o void Rover::handle_battery_failsafe(const char* type_str, const int8_t action) { - switch ((Failsafe_Action)action) { - case Failsafe_Action_None: + switch ((FailsafeAction)action) { + case FailsafeAction::None: break; - case Failsafe_Action_SmartRTL: + case FailsafeAction::SmartRTL: if (set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) { break; } FALLTHROUGH; - case Failsafe_Action_RTL: + case FailsafeAction::RTL: if (set_mode(mode_rtl, ModeReason::BATTERY_FAILSAFE)) { break; } FALLTHROUGH; - case Failsafe_Action_Hold: + case FailsafeAction::Hold: set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE); break; - case Failsafe_Action_SmartRTL_Hold: + case FailsafeAction::SmartRTL_Hold: if (!set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) { set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE); } break; - case Failsafe_Action_Terminate: + case FailsafeAction::Terminate: #if ADVANCED_FAILSAFE == ENABLED char battery_type_str[17]; snprintf(battery_type_str, 17, "%s battery", type_str); diff --git a/Rover/fence.cpp b/Rover/fence.cpp index fba0706bdefd6..7150c88c467cc 100644 --- a/Rover/fence.cpp +++ b/Rover/fence.cpp @@ -18,32 +18,33 @@ void Rover::fence_check() // if there is a new breach take action if (new_breaches) { // if the user wants some kind of response and motors are armed - if (fence.get_action() != Failsafe_Action_None) { + if ((FailsafeAction)fence.get_action() != FailsafeAction::None) { // if within 100m of the fence, it will take the action specified by the FENCE_ACTION parameter if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) { - switch (fence.get_action()) { - case Failsafe_Action_None: + switch ((FailsafeAction)fence.get_action()) { + case FailsafeAction::None: break; - case Failsafe_Action_RTL: - if (!set_mode(mode_rtl, ModeReason::FENCE_BREACHED)) { - set_mode(mode_hold, ModeReason::FENCE_BREACHED); + case FailsafeAction::SmartRTL: + if (set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) { + break; } - break; - case Failsafe_Action_Hold: - set_mode(mode_hold, ModeReason::FENCE_BREACHED); - break; - case Failsafe_Action_SmartRTL: - if (!set_mode(mode_smartrtl, ModeReason::FENCE_BREACHED)) { - if (!set_mode(mode_rtl, ModeReason::FENCE_BREACHED)) { - set_mode(mode_hold, ModeReason::FENCE_BREACHED); - } + FALLTHROUGH; + case FailsafeAction::RTL: + if (set_mode(mode_rtl, ModeReason::BATTERY_FAILSAFE)) { + break; } + FALLTHROUGH; + case FailsafeAction::Hold: + set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE); break; - case Failsafe_Action_SmartRTL_Hold: + case FailsafeAction::SmartRTL_Hold: if (!set_mode(mode_smartrtl, ModeReason::FENCE_BREACHED)) { set_mode(mode_hold, ModeReason::FENCE_BREACHED); } break; + case FailsafeAction::Terminate: + arming.disarm(AP_Arming::Method::FENCEBREACH); + break; } } else { // if more than 100m outside the fence just force to HOLD diff --git a/Rover/mode.cpp b/Rover/mode.cpp index 1509b72436738..842789c100669 100644 --- a/Rover/mode.cpp +++ b/Rover/mode.cpp @@ -531,12 +531,17 @@ Mode *Rover::mode_from_mode_num(const enum Mode::Number num) case Mode::Number::LOITER: ret = &mode_loiter; break; +#if MODE_FOLLOW_ENABLED == ENABLED case Mode::Number::FOLLOW: ret = &mode_follow; break; +#endif case Mode::Number::SIMPLE: ret = &mode_simple; break; + case Mode::Number::CIRCLE: + ret = &g2.mode_circle; + break; case Mode::Number::AUTO: ret = &mode_auto; break; diff --git a/Rover/mode.h b/Rover/mode.h index 761602b6ff501..0d7833de99e12 100644 --- a/Rover/mode.h +++ b/Rover/mode.h @@ -11,7 +11,7 @@ class Mode // Auto Pilot modes // ---------------- - enum Number : uint8_t { + enum class Number : uint8_t { MANUAL = 0, ACRO = 1, STEERING = 3, @@ -22,6 +22,7 @@ class Mode #if MODE_DOCK_ENABLED == ENABLED DOCK = 8, #endif + CIRCLE = 9, AUTO = 10, RTL = 11, SMART_RTL = 12, @@ -42,7 +43,7 @@ class Mode void exit(); // returns a unique number specific to this mode - virtual uint32_t mode_number() const = 0; + virtual Number mode_number() const = 0; // returns short text name (up to 4 bytes) virtual const char *name4() const = 0; @@ -217,7 +218,7 @@ class ModeAcro : public Mode { public: - uint32_t mode_number() const override { return ACRO; } + Number mode_number() const override { return Number::ACRO; } const char *name4() const override { return "ACRO"; } // methods that affect movement of the vehicle in this mode @@ -239,7 +240,7 @@ class ModeAuto : public Mode { public: - uint32_t mode_number() const override { return AUTO; } + Number mode_number() const override { return Number::AUTO; } const char *name4() const override { return "AUTO"; } // methods that affect movement of the vehicle in this mode @@ -250,7 +251,13 @@ class ModeAuto : public Mode bool is_autopilot_mode() const override { return true; } // return if external control is allowed in this mode (Guided or Guided-within-Auto) - bool in_guided_mode() const override { return _submode == Auto_Guided || _submode == Auto_NavScriptTime; } + bool in_guided_mode() const override { return _submode == SubMode::Guided || _submode == SubMode::NavScriptTime; } + + // return heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message) + float wp_bearing() const override; + float nav_bearing() const override; + float crosstrack_error() const override; + float get_desired_lat_accel() const override; // return distance (in meters) to destination float get_distance_to_destination() const override; @@ -287,14 +294,15 @@ class ModeAuto : public Mode bool _enter() override; void _exit() override; - enum AutoSubMode { - Auto_WP, // drive to a given location - Auto_HeadingAndSpeed, // turn to a given heading - Auto_RTL, // perform RTL within auto mode - Auto_Loiter, // perform Loiter within auto mode - Auto_Guided, // handover control to external navigation system from within auto mode - Auto_Stop, // stop the vehicle as quickly as possible - Auto_NavScriptTime, // accept targets from lua scripts while NAV_SCRIPT_TIME commands are executing + enum SubMode: uint8_t { + WP, // drive to a given location + HeadingAndSpeed, // turn to a given heading + RTL, // perform RTL within auto mode + Loiter, // perform Loiter within auto mode + Guided, // handover control to external navigation system from within auto mode + Stop, // stop the vehicle as quickly as possible + NavScriptTime, // accept targets from lua scripts while NAV_SCRIPT_TIME commands are executing + Circle, // circle a given location } _submode; private: @@ -322,6 +330,8 @@ class ModeAuto : public Mode bool verify_loiter_time(const AP_Mission::Mission_Command& cmd); bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd); bool verify_nav_set_yaw_speed(); + bool do_circle(const AP_Mission::Mission_Command& cmd); + bool verify_circle(const AP_Mission::Mission_Command& cmd); void do_wait_delay(const AP_Mission::Mission_Command& cmd); void do_within_distance(const AP_Mission::Mission_Command& cmd); bool verify_wait_delay(); @@ -385,12 +395,104 @@ class ModeAuto : public Mode AP_Mission_ChangeDetector mis_change_detector; }; +class ModeCircle : public Mode +{ +public: + + // need a constructor for parameters + ModeCircle(); + + // Does not allow copies + CLASS_NO_COPY(ModeCircle); + + Number mode_number() const override { return Number::CIRCLE; } + const char *name4() const override { return "CIRC"; } + + // initialise with specific center location, radius (in meters) and direction + // replaces use of _enter when initialised from within Auto mode + bool set_center(const Location& center_loc, float radius_m, bool dir_ccw); + + // methods that affect movement of the vehicle in this mode + void update() override; + + bool is_autopilot_mode() const override { return true; } + + // return desired heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message) + float wp_bearing() const override; + float nav_bearing() const override; + float crosstrack_error() const override { return dist_to_edge_m; } + float get_desired_lat_accel() const override; + + // set desired speed in m/s + bool set_desired_speed(float speed_ms) override; + + // return distance (in meters) to destination + float get_distance_to_destination() const override { return _distance_to_destination; } + + // get or set desired location + bool get_desired_location(Location& destination) const override WARN_IF_UNUSED; + + // return total angle in radians that vehicle has circled + // fabsf is used so that full rotations in either direction are counted + float get_angle_total_rad() const { return fabsf(angle_total_rad); } + + static const struct AP_Param::GroupInfo var_info[]; + +protected: + + AP_Float radius; // circle radius in meters + AP_Float speed; // vehicle speed in m/s. If zero uses WP_SPEED + AP_Int8 direction; // direction 0:clockwise, 1:counter-clockwise + + // initialise mode + bool _enter() override; + + // initialise target_yaw_rad using the vehicle's position and yaw + // if there is no current position estimate target_yaw_rad is set to vehicle yaw + void init_target_yaw_rad(); + + // limit config speed so that lateral acceleration is within limits + // outputs warning to user if speed is reduced + void check_config_speed(); + + // ensure config radius is no smaller then vehicle's TURN_RADIUS + // radius is increased if necessary and warning is output to the user + void check_config_radius(); + + // enum for Direction parameter + enum class Direction { + CW = 0, + CCW = 1 + }; + + // local members + struct { + Location center_loc; // circle center as a Location + Vector2f center_pos; // circle center as an offset (in meters) from the EKF origin + float radius; // circle radius + float speed; // desired speed around circle in m/s + Direction dir; // direction, 0:clockwise, 1:counter-clockwise + } config; + struct { + float speed; // vehicle's target speed around circle in m/s + float yaw_rad; // earth-frame angle of tarrget point on the circle + Vector2p pos; // latest position target sent to position controller + Vector2f vel; // latest velocity target sent to position controller + Vector2f accel; // latest accel target sent to position controller + } target; + float angle_total_rad; // total angle in radians that vehicle has circled + bool reached_edge; // true once vehicle has reached edge of circle + float dist_to_edge_m; // distance to edge of circle in meters (equivalent to crosstrack error) +}; class ModeGuided : public Mode { public: +#if AP_EXTERNAL_CONTROL_ENABLED + friend class AP_ExternalControl_Rover; +#endif - uint32_t mode_number() const override { return GUIDED; } + Number mode_number() const override { return Number::GUIDED; } const char *name4() const override { return "GUID"; } // methods that affect movement of the vehicle in this mode @@ -445,13 +547,13 @@ class ModeGuided : public Mode protected: - enum GuidedMode { - Guided_WP, - Guided_HeadingAndSpeed, - Guided_TurnRateAndSpeed, - Guided_Loiter, - Guided_SteeringAndThrottle, - Guided_Stop + enum class SubMode: uint8_t { + WP, + HeadingAndSpeed, + TurnRateAndSpeed, + Loiter, + SteeringAndThrottle, + Stop }; // enum for GUID_OPTIONS parameter @@ -465,7 +567,7 @@ class ModeGuided : public Mode // scurves provide path planning and object avoidance but cannot handle fast updates to the destination (for fast updates use position controller input shaping) bool use_scurves_for_navigation() const; - GuidedMode _guided_mode; // stores which GUIDED mode the vehicle is in + SubMode _guided_mode; // stores which GUIDED mode the vehicle is in // attitude control bool have_attitude_target; // true if we have a valid attitude target @@ -494,7 +596,7 @@ class ModeHold : public Mode { public: - uint32_t mode_number() const override { return HOLD; } + Number mode_number() const override { return Number::HOLD; } const char *name4() const override { return "HOLD"; } // methods that affect movement of the vehicle in this mode @@ -512,7 +614,7 @@ class ModeLoiter : public Mode { public: - uint32_t mode_number() const override { return LOITER; } + Number mode_number() const override { return Number::LOITER; } const char *name4() const override { return "LOIT"; } // methods that affect movement of the vehicle in this mode @@ -544,7 +646,7 @@ class ModeManual : public Mode { public: - uint32_t mode_number() const override { return MANUAL; } + Number mode_number() const override { return Number::MANUAL; } const char *name4() const override { return "MANU"; } // methods that affect movement of the vehicle in this mode @@ -568,7 +670,7 @@ class ModeRTL : public Mode { public: - uint32_t mode_number() const override { return RTL; } + Number mode_number() const override { return Number::RTL; } const char *name4() const override { return "RTL"; } // methods that affect movement of the vehicle in this mode @@ -603,7 +705,7 @@ class ModeSmartRTL : public Mode { public: - uint32_t mode_number() const override { return SMART_RTL; } + Number mode_number() const override { return Number::SMART_RTL; } const char *name4() const override { return "SRTL"; } // methods that affect movement of the vehicle in this mode @@ -620,7 +722,7 @@ class ModeSmartRTL : public Mode // return distance (in meters) to destination float get_distance_to_destination() const override { return _distance_to_destination; } - bool reached_destination() const override { return smart_rtl_state == SmartRTL_StopAtHome; } + bool reached_destination() const override { return smart_rtl_state == SmartRTLState::StopAtHome; } // set desired speed in m/s bool set_desired_speed(float speed) override; @@ -631,11 +733,11 @@ class ModeSmartRTL : public Mode protected: // Safe RTL states - enum SmartRTLState { - SmartRTL_WaitForPathCleanup, - SmartRTL_PathFollow, - SmartRTL_StopAtHome, - SmartRTL_Failure + enum class SmartRTLState: uint8_t { + WaitForPathCleanup, + PathFollow, + StopAtHome, + Failure } smart_rtl_state; bool _enter() override; @@ -649,7 +751,7 @@ class ModeSteering : public Mode { public: - uint32_t mode_number() const override { return STEERING; } + Number mode_number() const override { return Number::STEERING; } const char *name4() const override { return "STER"; } // methods that affect movement of the vehicle in this mode @@ -674,7 +776,7 @@ class ModeInitializing : public Mode { public: - uint32_t mode_number() const override { return INITIALISING; } + Number mode_number() const override { return Number::INITIALISING; } const char *name4() const override { return "INIT"; } // methods that affect movement of the vehicle in this mode @@ -690,11 +792,12 @@ class ModeInitializing : public Mode bool _enter() override { return false; }; }; +#if MODE_FOLLOW_ENABLED == ENABLED class ModeFollow : public Mode { public: - uint32_t mode_number() const override { return FOLLOW; } + Number mode_number() const override { return Number::FOLLOW; } const char *name4() const override { return "FOLL"; } // methods that affect movement of the vehicle in this mode @@ -724,12 +827,13 @@ class ModeFollow : public Mode float _desired_speed; // desired speed in m/s }; +#endif class ModeSimple : public Mode { public: - uint32_t mode_number() const override { return SIMPLE; } + Number mode_number() const override { return Number::SIMPLE; } const char *name4() const override { return "SMPL"; } // methods that affect movement of the vehicle in this mode @@ -759,7 +863,7 @@ class ModeDock : public Mode // Does not allow copies CLASS_NO_COPY(ModeDock); - uint32_t mode_number() const override { return DOCK; } + Number mode_number() const override { return Number::DOCK; } const char *name4() const override { return "DOCK"; } // methods that affect movement of the vehicle in this mode diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp index e9bdd4c6f09e9..5d2029b8201d2 100644 --- a/Rover/mode_auto.cpp +++ b/Rover/mode_auto.cpp @@ -44,6 +44,12 @@ void ModeAuto::_exit() void ModeAuto::update() { + // check if mission exists (due to being cleared while disarmed in AUTO, + // if no mission, then stop...needs mode change out of AUTO, mission load, + // and change back to AUTO to run a mission at this point + if (!hal.util->get_soft_armed() && mission.num_commands() <= 1) { + start_stop(); + } // start or update mission if (waiting_to_start) { // don't start the mission until we have an origin @@ -60,7 +66,7 @@ void ModeAuto::update() // check for mission changes if (mis_change_detector.check_for_mission_change()) { // if mission is running restart the current command if it is a waypoint command - if ((mission.state() == AP_Mission::MISSION_RUNNING) && (_submode == AutoSubMode::Auto_WP)) { + if ((mission.state() == AP_Mission::MISSION_RUNNING) && (_submode == SubMode::WP)) { if (mission.restart_current_nav_cmd()) { gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto mission changed, restarted command"); } else { @@ -74,28 +80,22 @@ void ModeAuto::update() } switch (_submode) { - case Auto_WP: + case SubMode::WP: { - // check if we've reached the destination - if (!g2.wp_nav.reached_destination() || g2.wp_nav.is_fast_waypoint()) { - // update navigation controller + // boats loiter once the waypoint is reached + bool keep_navigating = true; + if (rover.is_boat() && g2.wp_nav.reached_destination() && !g2.wp_nav.is_fast_waypoint()) { + keep_navigating = !start_loiter(); + } + + // update navigation controller + if (keep_navigating) { navigate_to_waypoint(); - } else { - // we have reached the destination so stay here - if (rover.is_boat()) { - if (!start_loiter()) { - start_stop(); - } - } else { - start_stop(); - } - // update distance to destination - _distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination()); } break; } - case Auto_HeadingAndSpeed: + case SubMode::HeadingAndSpeed: { if (!_reached_heading) { // run steering and throttle controllers @@ -116,15 +116,15 @@ void ModeAuto::update() break; } - case Auto_RTL: + case SubMode::RTL: rover.mode_rtl.update(); break; - case Auto_Loiter: + case SubMode::Loiter: rover.mode_loiter.update(); break; - case Auto_Guided: + case SubMode::Guided: { // send location target to offboard navigation system send_guided_position_target(); @@ -132,13 +132,17 @@ void ModeAuto::update() break; } - case Auto_Stop: + case SubMode::Stop: stop_vehicle(); break; - case Auto_NavScriptTime: + case SubMode::NavScriptTime: rover.mode_guided.update(); break; + + case SubMode::Circle: + rover.g2.mode_circle.update(); + break; } } @@ -152,23 +156,121 @@ void ModeAuto::calc_throttle(float target_speed, bool avoidance_enabled) Mode::calc_throttle(target_speed, avoidance_enabled); } +// return heading (in degrees) to target destination (aka waypoint) +float ModeAuto::wp_bearing() const +{ + switch (_submode) { + case SubMode::WP: + return g2.wp_nav.wp_bearing_cd() * 0.01f; + case SubMode::HeadingAndSpeed: + case SubMode::Stop: + return 0.0f; + case SubMode::RTL: + return rover.mode_rtl.wp_bearing(); + case SubMode::Loiter: + return rover.mode_loiter.wp_bearing(); + case SubMode::Guided: + case SubMode::NavScriptTime: + return rover.mode_guided.wp_bearing(); + case SubMode::Circle: + return rover.g2.mode_circle.wp_bearing(); + } + + // this line should never be reached + return 0.0f; +} + +// return short-term target heading in degrees (i.e. target heading back to line between waypoints) +float ModeAuto::nav_bearing() const +{ + switch (_submode) { + case SubMode::WP: + return g2.wp_nav.nav_bearing_cd() * 0.01f; + case SubMode::HeadingAndSpeed: + case SubMode::Stop: + return 0.0f; + case SubMode::RTL: + return rover.mode_rtl.nav_bearing(); + case SubMode::Loiter: + return rover.mode_loiter.nav_bearing(); + case SubMode::Guided: + case SubMode::NavScriptTime: + return rover.mode_guided.nav_bearing(); + case SubMode::Circle: + return rover.g2.mode_circle.nav_bearing(); + } + + // this line should never be reached + return 0.0f; +} + +// return cross track error (i.e. vehicle's distance from the line between waypoints) +float ModeAuto::crosstrack_error() const +{ + switch (_submode) { + case SubMode::WP: + return g2.wp_nav.crosstrack_error(); + case SubMode::HeadingAndSpeed: + case SubMode::Stop: + return 0.0f; + case SubMode::RTL: + return rover.mode_rtl.crosstrack_error(); + case SubMode::Loiter: + return rover.mode_loiter.crosstrack_error(); + case SubMode::Guided: + case SubMode::NavScriptTime: + return rover.mode_guided.crosstrack_error(); + case SubMode::Circle: + return rover.g2.mode_circle.crosstrack_error(); + } + + // this line should never be reached + return 0.0f; +} + +// return desired lateral acceleration +float ModeAuto::get_desired_lat_accel() const +{ + switch (_submode) { + case SubMode::WP: + return g2.wp_nav.get_lat_accel(); + case SubMode::HeadingAndSpeed: + case SubMode::Stop: + return 0.0f; + case SubMode::RTL: + return rover.mode_rtl.get_desired_lat_accel(); + case SubMode::Loiter: + return rover.mode_loiter.get_desired_lat_accel(); + case SubMode::Guided: + case SubMode::NavScriptTime: + return rover.mode_guided.get_desired_lat_accel(); + case SubMode::Circle: + return rover.g2.mode_circle.get_desired_lat_accel(); + } + + // this line should never be reached + return 0.0f; +} + // return distance (in meters) to destination float ModeAuto::get_distance_to_destination() const { switch (_submode) { - case Auto_WP: + case SubMode::WP: return _distance_to_destination; - case Auto_HeadingAndSpeed: - case Auto_Stop: + case SubMode::HeadingAndSpeed: + case SubMode::Stop: // no valid distance so return zero return 0.0f; - case Auto_RTL: + case SubMode::RTL: return rover.mode_rtl.get_distance_to_destination(); - case Auto_Loiter: + case SubMode::Loiter: return rover.mode_loiter.get_distance_to_destination(); - case Auto_Guided: - case Auto_NavScriptTime: + case SubMode::Guided: + case SubMode::NavScriptTime: return rover.mode_guided.get_distance_to_destination(); + case SubMode::Circle: + return rover.g2.mode_circle.get_distance_to_destination(); } // this line should never be reached @@ -179,23 +281,25 @@ float ModeAuto::get_distance_to_destination() const bool ModeAuto::get_desired_location(Location& destination) const { switch (_submode) { - case Auto_WP: + case SubMode::WP: if (g2.wp_nav.is_destination_valid()) { destination = g2.wp_nav.get_oa_destination(); return true; } return false; - case Auto_HeadingAndSpeed: - case Auto_Stop: + case SubMode::HeadingAndSpeed: + case SubMode::Stop: // no desired location for this submode return false; - case Auto_RTL: + case SubMode::RTL: return rover.mode_rtl.get_desired_location(destination); - case Auto_Loiter: + case SubMode::Loiter: return rover.mode_loiter.get_desired_location(destination); - case Auto_Guided: - case Auto_NavScriptTime: - return rover.mode_guided.get_desired_location(destination);\ + case SubMode::Guided: + case SubMode::NavScriptTime: + return rover.mode_guided.get_desired_location(destination); + case SubMode::Circle: + return rover.g2.mode_circle.get_desired_location(destination); } // we should never reach here but just in case @@ -210,7 +314,7 @@ bool ModeAuto::set_desired_location(const Location &destination, Location next_d return false; } - _submode = Auto_WP; + _submode = SubMode::WP; return true; } @@ -219,24 +323,25 @@ bool ModeAuto::set_desired_location(const Location &destination, Location next_d bool ModeAuto::reached_destination() const { switch (_submode) { - case Auto_WP: + case SubMode::WP: return g2.wp_nav.reached_destination(); break; - case Auto_HeadingAndSpeed: - case Auto_Stop: + case SubMode::HeadingAndSpeed: + case SubMode::Stop: // always return true because this is the safer option to allow missions to continue return true; break; - case Auto_RTL: + case SubMode::RTL: return rover.mode_rtl.reached_destination(); break; - case Auto_Loiter: + case SubMode::Loiter: return rover.mode_loiter.reached_destination(); break; - case Auto_Guided: - case Auto_NavScriptTime: + case SubMode::Guided: + case SubMode::NavScriptTime: return rover.mode_guided.reached_destination(); - break; + case SubMode::Circle: + return rover.g2.mode_circle.reached_destination(); } // we should never reach here but just in case, return true to allow missions to continue @@ -247,19 +352,21 @@ bool ModeAuto::reached_destination() const bool ModeAuto::set_desired_speed(float speed) { switch (_submode) { - case Auto_WP: - case Auto_Stop: + case SubMode::WP: + case SubMode::Stop: return g2.wp_nav.set_speed_max(speed); - case Auto_HeadingAndSpeed: + case SubMode::HeadingAndSpeed: _desired_speed = speed; return true; - case Auto_RTL: + case SubMode::RTL: return rover.mode_rtl.set_desired_speed(speed); - case Auto_Loiter: + case SubMode::Loiter: return rover.mode_loiter.set_desired_speed(speed); - case Auto_Guided: - case Auto_NavScriptTime: + case SubMode::Guided: + case SubMode::NavScriptTime: return rover.mode_guided.set_desired_speed(speed); + case SubMode::Circle: + return rover.g2.mode_circle.set_desired_speed(speed); } return false; } @@ -268,7 +375,7 @@ bool ModeAuto::set_desired_speed(float speed) void ModeAuto::start_RTL() { if (rover.mode_rtl.enter()) { - _submode = Auto_RTL; + _submode = SubMode::RTL; } } @@ -276,7 +383,7 @@ void ModeAuto::start_RTL() bool ModeAuto::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4) { #if AP_SCRIPTING_ENABLED - if (_submode == AutoSubMode::Auto_NavScriptTime) { + if (_submode == SubMode::NavScriptTime) { id = nav_scripting.id; cmd = nav_scripting.command; arg1 = nav_scripting.arg1; @@ -293,7 +400,7 @@ bool ModeAuto::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &a void ModeAuto::nav_script_time_done(uint16_t id) { #if AP_SCRIPTING_ENABLED - if ((_submode == AutoSubMode::Auto_NavScriptTime) && (id == nav_scripting.id)) { + if ((_submode == SubMode::NavScriptTime) && (id == nav_scripting.id)) { nav_scripting.done = true; } #endif @@ -345,7 +452,7 @@ bool ModeAuto::check_trigger(void) bool ModeAuto::start_loiter() { if (rover.mode_loiter.enter()) { - _submode = Auto_Loiter; + _submode = SubMode::Loiter; return true; } return false; @@ -355,7 +462,7 @@ bool ModeAuto::start_loiter() void ModeAuto::start_guided(const Location& loc) { if (rover.mode_guided.enter()) { - _submode = Auto_Guided; + _submode = SubMode::Guided; // initialise guided start time and position as reference for limit checking rover.mode_guided.limit_init_time_and_location(); @@ -374,7 +481,7 @@ void ModeAuto::start_guided(const Location& loc) // start stopping vehicle as quickly as possible void ModeAuto::start_stop() { - _submode = Auto_Stop; + _submode = SubMode::Stop; } // send latest position target to offboard navigation system @@ -422,6 +529,9 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_NAV_LOITER_TIME: // Loiter for specified time return do_nav_wp(cmd, true); + case MAV_CMD_NAV_LOITER_TURNS: + return do_circle(cmd); + case MAV_CMD_NAV_GUIDED_ENABLE: // accept navigation commands from external nav computer do_nav_guided_enable(cmd); break; @@ -564,6 +674,9 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_NAV_LOITER_UNLIM: return verify_loiter_unlimited(cmd); + case MAV_CMD_NAV_LOITER_TURNS: + return verify_circle(cmd); + case MAV_CMD_NAV_LOITER_TIME: return verify_loiter_time(cmd); @@ -669,7 +782,11 @@ void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd) nav_delay_time_max_ms = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds } else { // absolute delay to utc time +#if AP_RTC_ENABLED nav_delay_time_max_ms = AP::rtc().get_time_utc(cmd.content.nav_delay.hour_utc, cmd.content.nav_delay.min_utc, cmd.content.nav_delay.sec_utc, 0); +#else + nav_delay_time_max_ms = 0; +#endif } gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec", (unsigned)(nav_delay_time_max_ms/1000)); } @@ -701,7 +818,7 @@ void ModeAuto::do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd) _desired_speed = constrain_float(cmd.content.set_yaw_speed.speed, -speed_max, speed_max); _desired_yaw_cd = desired_heading_cd; _reached_heading = false; - _submode = Auto_HeadingAndSpeed; + _submode = SubMode::HeadingAndSpeed; } /********************************************************************************/ @@ -777,7 +894,7 @@ bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) { // if we failed to enter guided or this command disables guided // return true so we move to next command - if (_submode != Auto_Guided || cmd.p1 == 0) { + if (_submode != SubMode::Guided || cmd.p1 == 0) { return true; } @@ -795,13 +912,41 @@ bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) // verify_yaw - return true if we have reached the desired heading bool ModeAuto::verify_nav_set_yaw_speed() { - if (_submode == Auto_HeadingAndSpeed) { + if (_submode == SubMode::HeadingAndSpeed) { return _reached_heading; } // we should never reach here but just in case, return true to allow missions to continue return true; } +bool ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd) +{ + // retrieve and sanitize target location + Location circle_center = cmd.content.location; + circle_center.sanitize(rover.current_loc); + + // calculate radius + uint16_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1 + if (cmd.id == MAV_CMD_NAV_LOITER_TURNS && + cmd.type_specific_bits & (1U << 0)) { + // special storage handling allows for larger radii + circle_radius_m *= 10; + } + + // initialise circle mode + if (g2.mode_circle.set_center(circle_center, circle_radius_m, cmd.content.location.loiter_ccw)) { + _submode = SubMode::Circle; + return true; + } + return false; +} + +bool ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd) +{ + // check if we have completed circling + return ((g2.mode_circle.get_angle_total_rad() / M_2PI) >= LOWBYTE(cmd.p1)); +} + /********************************************************************************/ // Condition (May) commands /********************************************************************************/ @@ -884,7 +1029,7 @@ void ModeAuto::do_nav_script_time(const AP_Mission::Mission_Command& cmd) { // call regular guided flight mode initialisation if (rover.mode_guided.enter()) { - _submode = AutoSubMode::Auto_NavScriptTime; + _submode = SubMode::NavScriptTime; nav_scripting.done = false; nav_scripting.id++; nav_scripting.start_ms = millis(); diff --git a/Rover/mode_circle.cpp b/Rover/mode_circle.cpp new file mode 100644 index 0000000000000..42d6f63833b79 --- /dev/null +++ b/Rover/mode_circle.cpp @@ -0,0 +1,273 @@ +#include "Rover.h" + +#define AR_CIRCLE_ACCEL_DEFAULT 1.0 // default acceleration in m/s/s if not specified by user +#define AR_CIRCLE_RADIUS_MIN 0.5 // minimum radius in meters +#define AR_CIRCLE_REACHED_EDGE_DIST 0.2 // vehicle has reached edge if within 0.2m + +const AP_Param::GroupInfo ModeCircle::var_info[] = { + + // @Param: _RADIUS + // @DisplayName: Circle Radius + // @Description: Vehicle will circle the center at this distance + // @Units: m + // @Range: 0 100 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("_RADIUS", 1, ModeCircle, radius, 20), + + // @Param: _SPEED + // @DisplayName: Circle Speed + // @Description: Vehicle will move at this speed around the circle. If set to zero WP_SPEED will be used + // @Units: m/s + // @Range: 0 10 + // @Increment: 0.1 + // @User: Standard + AP_GROUPINFO("_SPEED", 2, ModeCircle, speed, 0), + + // @Param: _DIR + // @DisplayName: Circle Direction + // @Description: Circle Direction + // @Values: 0:Clockwise, 1:Counter-Clockwise + // @User: Standard + AP_GROUPINFO("_DIR", 3, ModeCircle, direction, 0), + + AP_GROUPEND +}; + +ModeCircle::ModeCircle() : Mode() +{ + AP_Param::setup_object_defaults(this, var_info); +} + +// initialise with specific center location, radius (in meters) and direction +// replaces use of _enter when initialised from within Auto mode +bool ModeCircle::set_center(const Location& center_loc, float radius_m, bool dir_ccw) +{ + Vector2f center_pos_cm; + if (!center_loc.get_vector_xy_from_origin_NE(center_pos_cm)) { + return false; + } + if (!_enter()) { + return false; + } + + // convert center position from cm to m + config.center_pos = center_pos_cm * 0.01; + + // set radius + config.radius = MAX(fabsf(radius_m), AR_CIRCLE_RADIUS_MIN); + check_config_radius(); + + // set direction + config.dir = dir_ccw ? Direction::CCW : Direction::CW; + + // set target yaw rad (target point on circle) + init_target_yaw_rad(); + + // record center as location (only used for reporting) + config.center_loc = center_loc; + + return true; +} + +// initialize dock mode +bool ModeCircle::_enter() +{ + // capture starting point and yaw + if (!AP::ahrs().get_relative_position_NE_origin(config.center_pos)) { + return false; + } + config.radius = MAX(fabsf(radius), AR_CIRCLE_RADIUS_MIN); + check_config_radius(); + + config.dir = (direction == 1) ? Direction::CCW : Direction::CW; + config.speed = is_positive(speed) ? speed : g2.wp_nav.get_default_speed(); + target.yaw_rad = AP::ahrs().get_yaw(); + target.speed = 0; + + // check speed around circle does not lead to excessive lateral acceleration + check_config_speed(); + + // calculate speed, accel and jerk limits + // otherwise the vehicle uses wp_nav default speed limit + float atc_accel_max = MIN(g2.attitude_control.get_accel_max(), g2.attitude_control.get_decel_max()); + if (!is_positive(atc_accel_max)) { + atc_accel_max = AR_CIRCLE_ACCEL_DEFAULT; + } + const float accel_max = is_positive(g2.wp_nav.get_default_accel()) ? MIN(g2.wp_nav.get_default_accel(), atc_accel_max) : atc_accel_max; + const float jerk_max = is_positive(g2.wp_nav.get_default_jerk()) ? g2.wp_nav.get_default_jerk() : accel_max; + + // initialise position controller + g2.pos_control.set_limits(config.speed, accel_max, g2.attitude_control.get_turn_lat_accel_max(), jerk_max); + g2.pos_control.init(); + + // initialise angles covered and reached_edge state + angle_total_rad = 0; + reached_edge = false; + dist_to_edge_m = 0; + + return true; +} + +// initialise target_yaw_rad using the vehicle's position and yaw +// if there is no current position estimate target_yaw_rad is set to 0 +void ModeCircle::init_target_yaw_rad() +{ + // if no position estimate use vehicle yaw + Vector2f curr_pos_NE; + if (!AP::ahrs().get_relative_position_NE_origin(curr_pos_NE)) { + target.yaw_rad = AP::ahrs().yaw; + return; + } + + // calc vector from circle center to vehicle + Vector2f center_to_veh = (curr_pos_NE - config.center_pos); + float dist_m = center_to_veh.length(); + + // if current position is exactly at the center of the circle return vehicle yaw + if (is_zero(dist_m)) { + target.yaw_rad = AP::ahrs().yaw; + } else { + target.yaw_rad = center_to_veh.angle(); + } +} + +void ModeCircle::update() +{ + // get current position + Vector2f curr_pos; + const bool position_ok = AP::ahrs().get_relative_position_NE_origin(curr_pos); + + // if no position estimate stop vehicle + if (!position_ok) { + stop_vehicle(); + return; + } + + // check if vehicle has reached edge of circle + const Vector2f center_to_veh = curr_pos - config.center_pos; + _distance_to_destination = center_to_veh.length(); + dist_to_edge_m = fabsf(_distance_to_destination - config.radius); + if (!reached_edge) { + const float dist_thresh_m = MAX(rover.g2.turn_radius, AR_CIRCLE_REACHED_EDGE_DIST); + reached_edge = dist_to_edge_m <= dist_thresh_m; + } + + // accelerate speed up to desired speed + const float speed_max = reached_edge ? config.speed : 0.0; + const float speed_change_max = (g2.pos_control.get_accel_max() * 0.5 * rover.G_Dt); + const float accel_fb = constrain_float(speed_max - target.speed, -speed_change_max, speed_change_max); + target.speed += accel_fb; + + // calculate angular rate and update target angle + const float circumference = 2.0 * M_PI * config.radius; + const float angular_rate_rad = (target.speed / circumference) * M_2PI * (config.dir == Direction::CW ? 1.0 : -1.0); + const float angle_dt = angular_rate_rad * rover.G_Dt; + target.yaw_rad = wrap_PI(target.yaw_rad + angle_dt); + angle_total_rad += angle_dt; + + // calculate target point's position, velocity and acceleration + target.pos = config.center_pos.topostype(); + target.pos.offset_bearing(degrees(target.yaw_rad), config.radius); + + // velocity is perpendicular to angle from the circle's center to the target point on the edge of the circle + target.vel = Vector2f(target.speed, 0); + target.vel.rotate(target.yaw_rad + radians(90)); + + // acceleration is towards center of circle and is speed^2 / radius + target.accel = Vector2f(-sq(target.speed) / config.radius, accel_fb / rover.G_Dt); + target.accel.rotate(target.yaw_rad); + + g2.pos_control.set_pos_vel_accel_target(target.pos, target.vel, target.accel); + g2.pos_control.update(rover.G_Dt); + + // get desired speed and turn rate from pos_control + const float desired_speed = g2.pos_control.get_desired_speed(); + const float desired_turn_rate = g2.pos_control.get_desired_turn_rate_rads(); + + // run steering and throttle controllers + calc_steering_from_turn_rate(desired_turn_rate); + calc_throttle(desired_speed, true); +} + +// return desired heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message) +float ModeCircle::wp_bearing() const +{ + Vector2f curr_pos_NE; + if (!AP::ahrs().get_relative_position_NE_origin(curr_pos_NE)) { + return 0; + } + // calc vector from circle center to vehicle + Vector2f veh_to_center = (config.center_pos - curr_pos_NE); + if (veh_to_center.is_zero()) { + return 0; + } + return degrees(veh_to_center.angle()); +} + +float ModeCircle::nav_bearing() const +{ + // get position error as a vector from the current position to the target position + const Vector2p pos_error = g2.pos_control.get_pos_error(); + if (pos_error.is_zero()) { + return 0; + } + return degrees(pos_error.angle()); +} + +float ModeCircle::get_desired_lat_accel() const +{ + return g2.pos_control.get_desired_lat_accel(); +} + +// set desired speed in m/s +bool ModeCircle::set_desired_speed(float speed_ms) +{ + if (is_positive(speed_ms)) { + config.speed = speed_ms; + + // check desired speed does not lead to excessive lateral acceleration + check_config_speed(); + + // update position controller limits if required + if (config.speed > g2.pos_control.get_speed_max()) { + g2.pos_control.set_limits(config.speed, g2.pos_control.get_accel_max(), g2.pos_control.get_lat_accel_max(), g2.pos_control.get_jerk_max()); + } + return true; + } + + return false; +} + +// return desired location +bool ModeCircle::get_desired_location(Location& destination) const +{ + destination = config.center_loc; + return true; +} + +// limit config speed so that lateral acceleration is within limits +// assumes that config.radius and attitude controller lat accel max have been set +// outputs warning to user if speed is reduced +void ModeCircle::check_config_speed() +{ + // calculate maximum speed based on radius and max lateral acceleration max + const float speed_max = MAX(safe_sqrt(g2.attitude_control.get_turn_lat_accel_max() * config.radius), 0); + + if (config.speed > speed_max) { + config.speed = speed_max; + gcs().send_text(MAV_SEVERITY_WARNING, "Circle: max speed is %4.1f", (double)config.speed); + } +} + +// ensure config radius is no smaller then vehicle's TURN_RADIUS +// assumes that config.radius has been set +// radius is increased if necessary and warning is output to the user +void ModeCircle::check_config_radius() +{ + // ensure radius is at least as large as vehicle's turn radius + if (config.radius < rover.g2.turn_radius) { + config.radius = rover.g2.turn_radius; + gcs().send_text(MAV_SEVERITY_WARNING, "Circle: radius increased to TURN_RADIUS (%4.1f)", (double)rover.g2.turn_radius); + } +} diff --git a/Rover/mode_follow.cpp b/Rover/mode_follow.cpp index 437cb53deb844..964a81a102a6b 100644 --- a/Rover/mode_follow.cpp +++ b/Rover/mode_follow.cpp @@ -1,5 +1,6 @@ #include "Rover.h" +#if MODE_FOLLOW_ENABLED // initialize follow mode bool ModeFollow::_enter() { @@ -94,3 +95,5 @@ bool ModeFollow::set_desired_speed(float speed) _desired_speed = speed; return true; } + +#endif // MODE_FOLLOW_ENABLED diff --git a/Rover/mode_guided.cpp b/Rover/mode_guided.cpp index 76f34367607bb..a1d18cf92d006 100644 --- a/Rover/mode_guided.cpp +++ b/Rover/mode_guided.cpp @@ -22,7 +22,7 @@ bool ModeGuided::_enter() void ModeGuided::update() { switch (_guided_mode) { - case Guided_WP: + case SubMode::WP: { // check if we've reached the destination if (!g2.wp_nav.reached_destination()) { @@ -49,7 +49,7 @@ void ModeGuided::update() break; } - case Guided_HeadingAndSpeed: + case SubMode::HeadingAndSpeed: { // stop vehicle if target not updated within 3 seconds if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) { @@ -73,7 +73,7 @@ void ModeGuided::update() break; } - case Guided_TurnRateAndSpeed: + case SubMode::TurnRateAndSpeed: { // stop vehicle if target not updated within 3 seconds if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) { @@ -101,13 +101,13 @@ void ModeGuided::update() break; } - case Guided_Loiter: + case SubMode::Loiter: { rover.mode_loiter.update(); break; } - case Guided_SteeringAndThrottle: + case SubMode::SteeringAndThrottle: { // handle timeout if (_have_strthr && (AP_HAL::millis() - _strthr_time_ms) > 3000) { @@ -131,7 +131,7 @@ void ModeGuided::update() break; } - case Guided_Stop: + case SubMode::Stop: stop_vehicle(); break; @@ -145,15 +145,15 @@ void ModeGuided::update() float ModeGuided::wp_bearing() const { switch (_guided_mode) { - case Guided_WP: + case SubMode::WP: return g2.wp_nav.wp_bearing_cd() * 0.01f; - case Guided_HeadingAndSpeed: - case Guided_TurnRateAndSpeed: + case SubMode::HeadingAndSpeed: + case SubMode::TurnRateAndSpeed: return 0.0f; - case Guided_Loiter: + case SubMode::Loiter: return rover.mode_loiter.wp_bearing(); - case Guided_SteeringAndThrottle: - case Guided_Stop: + case SubMode::SteeringAndThrottle: + case SubMode::Stop: return 0.0f; } @@ -164,15 +164,15 @@ float ModeGuided::wp_bearing() const float ModeGuided::nav_bearing() const { switch (_guided_mode) { - case Guided_WP: + case SubMode::WP: return g2.wp_nav.nav_bearing_cd() * 0.01f; - case Guided_HeadingAndSpeed: - case Guided_TurnRateAndSpeed: + case SubMode::HeadingAndSpeed: + case SubMode::TurnRateAndSpeed: return 0.0f; - case Guided_Loiter: + case SubMode::Loiter: return rover.mode_loiter.nav_bearing(); - case Guided_SteeringAndThrottle: - case Guided_Stop: + case SubMode::SteeringAndThrottle: + case SubMode::Stop: return 0.0f; } @@ -183,15 +183,15 @@ float ModeGuided::nav_bearing() const float ModeGuided::crosstrack_error() const { switch (_guided_mode) { - case Guided_WP: + case SubMode::WP: return g2.wp_nav.crosstrack_error(); - case Guided_HeadingAndSpeed: - case Guided_TurnRateAndSpeed: + case SubMode::HeadingAndSpeed: + case SubMode::TurnRateAndSpeed: return 0.0f; - case Guided_Loiter: + case SubMode::Loiter: return rover.mode_loiter.crosstrack_error(); - case Guided_SteeringAndThrottle: - case Guided_Stop: + case SubMode::SteeringAndThrottle: + case SubMode::Stop: return 0.0f; } @@ -202,15 +202,15 @@ float ModeGuided::crosstrack_error() const float ModeGuided::get_desired_lat_accel() const { switch (_guided_mode) { - case Guided_WP: + case SubMode::WP: return g2.wp_nav.get_lat_accel(); - case Guided_HeadingAndSpeed: - case Guided_TurnRateAndSpeed: + case SubMode::HeadingAndSpeed: + case SubMode::TurnRateAndSpeed: return 0.0f; - case Guided_Loiter: + case SubMode::Loiter: return rover.mode_loiter.get_desired_lat_accel(); - case Guided_SteeringAndThrottle: - case Guided_Stop: + case SubMode::SteeringAndThrottle: + case SubMode::Stop: return 0.0f; } @@ -222,15 +222,15 @@ float ModeGuided::get_desired_lat_accel() const float ModeGuided::get_distance_to_destination() const { switch (_guided_mode) { - case Guided_WP: + case SubMode::WP: return _distance_to_destination; - case Guided_HeadingAndSpeed: - case Guided_TurnRateAndSpeed: + case SubMode::HeadingAndSpeed: + case SubMode::TurnRateAndSpeed: return 0.0f; - case Guided_Loiter: + case SubMode::Loiter: return rover.mode_loiter.get_distance_to_destination(); - case Guided_SteeringAndThrottle: - case Guided_Stop: + case SubMode::SteeringAndThrottle: + case SubMode::Stop: return 0.0f; } @@ -242,13 +242,13 @@ float ModeGuided::get_distance_to_destination() const bool ModeGuided::reached_destination() const { switch (_guided_mode) { - case Guided_WP: + case SubMode::WP: return g2.wp_nav.reached_destination(); - case Guided_HeadingAndSpeed: - case Guided_TurnRateAndSpeed: - case Guided_Loiter: - case Guided_SteeringAndThrottle: - case Guided_Stop: + case SubMode::HeadingAndSpeed: + case SubMode::TurnRateAndSpeed: + case SubMode::Loiter: + case SubMode::SteeringAndThrottle: + case SubMode::Stop: return true; } @@ -260,16 +260,16 @@ bool ModeGuided::reached_destination() const bool ModeGuided::set_desired_speed(float speed) { switch (_guided_mode) { - case Guided_WP: + case SubMode::WP: return g2.wp_nav.set_speed_max(speed); - case Guided_HeadingAndSpeed: - case Guided_TurnRateAndSpeed: + case SubMode::HeadingAndSpeed: + case SubMode::TurnRateAndSpeed: // speed is set from mavlink message return false; - case Guided_Loiter: + case SubMode::Loiter: return rover.mode_loiter.set_desired_speed(speed); - case Guided_SteeringAndThrottle: - case Guided_Stop: + case SubMode::SteeringAndThrottle: + case SubMode::Stop: // no speed control return false; } @@ -280,21 +280,21 @@ bool ModeGuided::set_desired_speed(float speed) bool ModeGuided::get_desired_location(Location& destination) const { switch (_guided_mode) { - case Guided_WP: + case SubMode::WP: if (g2.wp_nav.is_destination_valid()) { destination = g2.wp_nav.get_oa_destination(); return true; } return false; - case Guided_HeadingAndSpeed: - case Guided_TurnRateAndSpeed: + case SubMode::HeadingAndSpeed: + case SubMode::TurnRateAndSpeed: // not supported in these submodes return false; - case Guided_Loiter: + case SubMode::Loiter: // get destination from loiter return rover.mode_loiter.get_desired_location(destination); - case Guided_SteeringAndThrottle: - case Guided_Stop: + case SubMode::SteeringAndThrottle: + case SubMode::Stop: // no desired location in this submode break; } @@ -320,9 +320,9 @@ bool ModeGuided::set_desired_location(const Location &destination, Location next } // handle guided specific initialisation and logging - _guided_mode = ModeGuided::Guided_WP; + _guided_mode = SubMode::WP; send_notification = true; - rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(destination.lat, destination.lng, 0), Vector3f(g2.wp_nav.get_speed_max(), 0.0f, 0.0f)); + rover.Log_Write_GuidedTarget((uint8_t)_guided_mode, Vector3f(destination.lat, destination.lng, 0), Vector3f(g2.wp_nav.get_speed_max(), 0.0f, 0.0f)); return true; } @@ -330,7 +330,7 @@ bool ModeGuided::set_desired_location(const Location &destination, Location next void ModeGuided::set_desired_heading_and_speed(float yaw_angle_cd, float target_speed) { // initialisation and logging - _guided_mode = ModeGuided::Guided_HeadingAndSpeed; + _guided_mode = SubMode::HeadingAndSpeed; _des_att_time_ms = AP_HAL::millis(); // record targets @@ -339,14 +339,14 @@ void ModeGuided::set_desired_heading_and_speed(float yaw_angle_cd, float target_ have_attitude_target = true; // log new target - rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(_desired_yaw_cd, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f)); + rover.Log_Write_GuidedTarget((uint8_t)_guided_mode, Vector3f(_desired_yaw_cd, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f)); } void ModeGuided::set_desired_heading_delta_and_speed(float yaw_delta_cd, float target_speed) { // handle initialisation - if (_guided_mode != ModeGuided::Guided_HeadingAndSpeed) { - _guided_mode = ModeGuided::Guided_HeadingAndSpeed; + if (_guided_mode != SubMode::HeadingAndSpeed) { + _guided_mode = SubMode::HeadingAndSpeed; _desired_yaw_cd = ahrs.yaw_sensor; } set_desired_heading_and_speed(wrap_180_cd(_desired_yaw_cd + yaw_delta_cd), target_speed); @@ -356,7 +356,7 @@ void ModeGuided::set_desired_heading_delta_and_speed(float yaw_delta_cd, float t void ModeGuided::set_desired_turn_rate_and_speed(float turn_rate_cds, float target_speed) { // handle initialisation - _guided_mode = ModeGuided::Guided_TurnRateAndSpeed; + _guided_mode = SubMode::TurnRateAndSpeed; _des_att_time_ms = AP_HAL::millis(); // record targets @@ -365,13 +365,13 @@ void ModeGuided::set_desired_turn_rate_and_speed(float turn_rate_cds, float targ have_attitude_target = true; // log new target - rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(_desired_yaw_rate_cds, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f)); + rover.Log_Write_GuidedTarget((uint8_t)_guided_mode, Vector3f(_desired_yaw_rate_cds, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f)); } // set steering and throttle (both in the range -1 to +1) void ModeGuided::set_steering_and_throttle(float steering, float throttle) { - _guided_mode = ModeGuided::Guided_SteeringAndThrottle; + _guided_mode = SubMode::SteeringAndThrottle; _strthr_time_ms = AP_HAL::millis(); _strthr_steering = constrain_float(steering, -1.0f, 1.0f); _strthr_throttle = constrain_float(throttle, -1.0f, 1.0f); @@ -381,7 +381,7 @@ void ModeGuided::set_steering_and_throttle(float steering, float throttle) bool ModeGuided::start_loiter() { if (rover.mode_loiter.enter()) { - _guided_mode = Guided_Loiter; + _guided_mode = SubMode::Loiter; return true; } return false; @@ -391,7 +391,7 @@ bool ModeGuided::start_loiter() // start stopping vehicle as quickly as possible void ModeGuided::start_stop() { - _guided_mode = Guided_Stop; + _guided_mode = SubMode::Stop; } // set guided timeout and movement limits diff --git a/Rover/mode_rtl.cpp b/Rover/mode_rtl.cpp index b9a8ee49649e1..e896c5151fc87 100644 --- a/Rover/mode_rtl.cpp +++ b/Rover/mode_rtl.cpp @@ -11,7 +11,7 @@ bool ModeRTL::_enter() g2.wp_nav.init(MAX(0, g2.rtl_speed)); // set target to the closest rally point or home -#if AP_RALLY == ENABLED +#if HAL_RALLY_ENABLED if (!g2.wp_nav.set_desired_location(g2.rally.calc_best_rally_or_home_location(rover.current_loc, ahrs.get_home().alt))) { return false; } diff --git a/Rover/mode_smart_rtl.cpp b/Rover/mode_smart_rtl.cpp index 0d083965e4b32..0080d3569fd8c 100644 --- a/Rover/mode_smart_rtl.cpp +++ b/Rover/mode_smart_rtl.cpp @@ -22,7 +22,7 @@ bool ModeSmartRTL::_enter() } // init state - smart_rtl_state = SmartRTL_WaitForPathCleanup; + smart_rtl_state = SmartRTLState::WaitForPathCleanup; _loitering = false; return true; @@ -31,24 +31,24 @@ bool ModeSmartRTL::_enter() void ModeSmartRTL::update() { switch (smart_rtl_state) { - case SmartRTL_WaitForPathCleanup: + case SmartRTLState::WaitForPathCleanup: // check if return path is computed and if yes, begin journey home if (g2.smart_rtl.request_thorough_cleanup()) { - smart_rtl_state = SmartRTL_PathFollow; + smart_rtl_state = SmartRTLState::PathFollow; _load_point = true; } // Note: this may lead to an unnecessary 20ms slow down of the vehicle (but it is unlikely) stop_vehicle(); break; - case SmartRTL_PathFollow: + case SmartRTLState::PathFollow: // load point if required if (_load_point) { Vector3f dest_NED; if (!g2.smart_rtl.pop_point(dest_NED)) { // if not more points, we have reached home gcs().send_text(MAV_SEVERITY_INFO, "Reached destination"); - smart_rtl_state = SmartRTL_StopAtHome; + smart_rtl_state = SmartRTLState::StopAtHome; break; } else { // peek at the next point. this can fail if the IO task currently has the path semaphore @@ -57,7 +57,7 @@ void ModeSmartRTL::update() if (!g2.wp_nav.set_desired_location_NED(dest_NED, next_dest_NED)) { // this should never happen because the EKF origin should already be set gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination"); - smart_rtl_state = SmartRTL_Failure; + smart_rtl_state = SmartRTLState::Failure; INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } } else { @@ -65,7 +65,7 @@ void ModeSmartRTL::update() if (!g2.wp_nav.set_desired_location_NED(dest_NED)) { // this should never happen because the EKF origin should already be set gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination"); - smart_rtl_state = SmartRTL_Failure; + smart_rtl_state = SmartRTLState::Failure; INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } } @@ -81,8 +81,8 @@ void ModeSmartRTL::update() } break; - case SmartRTL_StopAtHome: - case SmartRTL_Failure: + case SmartRTLState::StopAtHome: + case SmartRTLState::Failure: _reached_destination = true; // we have reached the destination // boats loiters, rovers stop @@ -107,16 +107,16 @@ void ModeSmartRTL::update() bool ModeSmartRTL::get_desired_location(Location& destination) const { switch (smart_rtl_state) { - case SmartRTL_WaitForPathCleanup: + case SmartRTLState::WaitForPathCleanup: return false; - case SmartRTL_PathFollow: + case SmartRTLState::PathFollow: if (g2.wp_nav.is_destination_valid()) { destination = g2.wp_nav.get_destination(); return true; } return false; - case SmartRTL_StopAtHome: - case SmartRTL_Failure: + case SmartRTLState::StopAtHome: + case SmartRTLState::Failure: return false; } // should never reach here but just in case diff --git a/Rover/precision_landing.cpp b/Rover/precision_landing.cpp index 12b33174ebbe5..c62b499b6a3ec 100644 --- a/Rover/precision_landing.cpp +++ b/Rover/precision_landing.cpp @@ -4,7 +4,7 @@ #include "Rover.h" -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED void Rover::init_precland() { diff --git a/Rover/release-notes.txt b/Rover/release-notes.txt index c2bfdaacef754..96c3a1ca40427 100644 --- a/Rover/release-notes.txt +++ b/Rover/release-notes.txt @@ -1,5 +1,370 @@ Rover Release Notes: ------------------------------------------------------------------ +Rover 4.4.0-beta11 05-Dec-2023 +Changes from 4.4.0-beta10 +1) Autopilot related enhancement and fixes + - CubeOrange Sim-on-hardware compilation fix + - RADIX2HD supports external I2C compasses + - SpeedyBeeF405v4 support +2) Bug fixes + - DroneCAN battery monitor with cell monitor SoC reporting fix + - NTF_LED_TYPES parameter description fixed (was missing IS31FL3195) + - ProfiLED output fixed in both Notify and Scripting + - Scripting bug that could cause crash if parameters were added in flight + - STAT_BOOTCNT param fix (was not updating in some cases) + - don't query hobbywing DroneCAN ESC IDs while armed +3) Rover specific changes + - Auto mode keeps navigating while paused at waypoints (reduces overshoot) + - QuikTune script simplification (only tunes rate and speed controllers) + - Torqeedo battery nearly empty reporting fix +------------------------------------------------------------------ +Rover 4.4.0-beta10 14-Nov-2023 +Changes from 4.4.0-beta9 +1) AP_GPS: correct uBlox M10 configuration on low flash boards +------------------------------------------------------------------ +Rover 4.4.0-beta9 07-Nov-2023 +Changes from 4.4.0-beta8 +1) Autopilot related enhancements and fixes + - BETAFTP-F405 board configuration fixes + - CubeOrangePlus-BG edition ICM45486 IMU setup fixed + - YJUAV_A6SE_H743 support +2) Minor enhancements + - GPS_DRV_OPTION allows using ellipsoid height in more GPS drivers + - Lua script support for fully populating ESC telemetry data +3) Bug fixes + - AK09916 compass being non-responsive fixed + - IxM42xxx IMUs "stuck" gyros fixed + - Notch filtering protection when using uninitialised RPM source in ESC telemetry + - SIYI gimbal driver parsing bug fixed (was causing lost packets) +------------------------------------------------------------------ +Rover 4.4.0-beta8 13-Oct-2023 +Changes from 4.4.0-beta7 +1) Autopilot related enhancements and fixes + - BETAFPV-F405 support + - MambaF405v2 battery and serial setup corrected + - mRo Control Zero OEM H7 bdshot support + - SpeedyBee-F405-Wing gets VTX power control + - SpeedyBee-F405-Mini support + - T-Motor H743 Mini support +2) EKF3 supports baroless boards +3) GPS-for-yaw allows base GPS to update at only 3Hz +4) INA battery monitor supports config of shunt resistor used (see BATTx_SHUNT) +5) Log VER message includes vehicle type +6) OpenDroneId option to auto-store IDs in persistent flash +7) RC SBUS protection against invalid data in first 4 channels +8) Bug fixes + - BMI088 IMU error value handling fixed to avoid occasional negative spike + - Dev environment CI autotest stability improvements + - OSD correct DisplayPort BF MSP symbols + - OSD option to correct direction arrows for BF font set + - Sensor status reporting to GCS fixed for baroless boards +------------------------------------------------------------------ +Rover 4.4.0-beta7 14-Sep-2023 +Changes from 4.4.0-beta6 +1) Autopilot related enhancements + - H750 external flash optimisations for to lower CPU load + - MambaF405Mini fixes to match manufacturer's recommended wiring + - RADIX2 HD support + - YJUAV_A6SE support +2) Bug fixes + - Airbotf4 features minimised to build for 4.4 + - ChibiOS clock fix for 480Mhz H7 boards (affected FDCAN) + - RPI hardware version check fix +------------------------------------------------------------------ +Rover 4.4.0-beta6 05-Sep-2023 +Changes from 4.4.0-beta5 +1) Autopilot related fixes and enhancements + - KakuteH7-wing get 8 bit directional dshot channel support + - Luminousbee5 boards defaults updated + - Navigator autopilot GPIOs fix (PWM output was broken) + - Pixhawk6C Serial RTS lines pulled low on startup + - QiotekZealotF427 and QiotekZealotH743 battery monitor default fixed + - SDMODELH7V1 supporta +2) Driver enhancements + - DroneCAN battery monitors allow reset of battery SoC + - Himark DroneCAN servo support + - Hobbywing DroneCAN ESC support +3) Asymmetrical thrust support for skid steering rovers (see MOT_THST_ASYM) +4) EKF3 high vibration handling improved with EK3_GLITCH_RADIUS option +5) Custom build server gets mission storage on SDCard selection +6) SITL default parameter handling bug fix +------------------------------------------------------------------ +Rover 4.4.0-beta5 12-Aug-2023 +Changes from 4.4.0-beta4 +1) Autopilots specific changes + - SIYI N7 support +2) Bug fixes + - DroneCAN airspeed sensor fix to handle missing packets + - DroneCAN GPS RTK injection fix + - Notch filter gyro glitch caused by race condition fixed +------------------------------------------------------------------ +Rover 4.4.0-beta4 27-July-2023 +Changes from 4.4.0-beta3 +1) Autopilots specific changes + - Diatone-Mamba-MK4-H743v2 uses SPL06 baro (was DPS280) + - DMA for I2C disabled on F7 and H7 boards + - Foxeer H743v1 default serial protocol config fixes + - HeeWing-F405 and F405v2 support + - iFlight BlitzF7 support +2) Rover specific enhancements + - QuikTune Lua script + - Circle mode safety improvements including handling when CIRC_SPEED set too high + - Velocity controller I-term build-up avoided when steering reaches limits +3) Bug fixes + - BLHeli returns battery status requested via MSP (avoids hang when using esc-configurator) + - CRSFv3 rescans at baudrates to avoid RX loss + - EK3_ABIAS_P_NSE param range fix + - Scripting restart memory corruption bug fixed + - Siyi A8/ZR10 driver fix to avoid crash if serial port not configured +------------------------------------------------------------------ +Rover 4.4.0-beta3 03-July-2023 +Changes from 4.4.0-beta2 +1) Autopilots specific changes + - Holybro KakuteH7-Wing support + - JFB100 external watchdog GPIO support added + - Pixhawk1-bdshot support + - Pixhawk6X-bdshot support + - SpeedyBeeF4 loses bdshot support +2) Device drivers + - added LP5562 I2C LED driver + - added IS31FL3195 LED driver +3) Circle mode accuracy improvement +4) Camera and Gimbal related changes + - DO_SET_ROI_NONE command support added +5) Bug fixes + - ADSB sensor loss of transceiver message less spammy + - EKF vertical velocity reset fixed on loss of GPS + - GPS pre-arm failure message clarified + - SERVOx_PROTOCOL "SToRM32 Gimbal Serial" value renamed to "Gimbal" because also used by Siyi + - SERIALx_OPTION "Swap" renamed to "SwapTXRX" for clarity + - SBF GPS ellipsoid height fixed + - Ublox M10S GPS auto configuration fixed +------------------------------------------------------------------ +Rover 4.4.0-beta2 05-Jun-2023 +Changes from 4.4.0-beta1 +1) Autopilots specific changes + - FlywooF745 update to motor pin output mapping and baro + - FoxeerH743 support + - JFB100 support + - Mamba-F405v2 supports ICM42688 + - Matek-F405-TE/VTOL support + - Matek-H743 IMU SPI slowed to 1Mhz to avoid init issues + - SpeedyBee-405-Wing support +2) Rover specific changes + - Circle mode and Auto mode LOITER_TURNS support + - Dock mode added to INITIAL_MODE and MODE1 parameter list +3) AHRS/EKF related fixes and Enhancements + - EKF allocation failure handled to avoid watchdog + - EKF3 accel bias calculation fix and tuning for greater robustness + - Airspeed sensor remains enabled during dead-reckoning (few copters have airspeed sensors) + - Wind speed estimates updates reduced while dead-reckoning +4) Other Enhancements + - Attitude control slew limits always calculated (helps tuning reporting and analysis) + - INA228 and INA238 I2C battery monitor support + - LOG_DISARMED=3 logs while disarmed but discards log if never eventually armed + - LOG_DARM_RATEMAX reduces logging while disarmed + - Serial LEDs threading enhancement to support longer lengths without dshot interference +4) Bug fixes + - Analog battery monitor2 current parameter default fixed + - AutoTune fix for loading Yaw Rate D gains + - BRD_SAFETYOPTION parameter documentation fix (ActiveForSafetyEnable and Disable were reversed) + - Compassmot fix to protect against bad gyro biases from GSF yaw + - ICE engine fix for starting after reaching a specified altitude + - LED thread locking fix to avoid watchdog + - Logging rotation on disarm disabled if Replay logging active (avoids gaps in logs) + - RC input on IOMCU bug fix (RC might not be regained if lost) + - Serial passthrough fixed +5) Custom build server fix to which features are included/excluded +------------------------------------------------------------------ +Rover 4.4.0-beta1 19-Apr-2023 +Changes from 4.3.0-beta12 +1) New autopilots supported + - ESP32 + - Flywoo Goku F405S AIO + - Foxeer H743v1 + - MambaF405-2022B + - PixPilot-V3 + - PixSurveyA2 + - rFCU H743 + - ThePeach K1/R1 +2) Autopilot specific changes + - Bi-Directional DShot support for CubeOrangePlus-bdshot, CUAVNora+, MatekF405TE/VTOL-bdshot, MatekL431, Pixhawk6C-bdshot, QioTekZealotH743-bdshot + - Bi-Directional DShot up to 8 channels on MatekH743 + - BlueRobotics Navigator supports baro on I2C bus 6 + - BMP280 baro only for BeastF7, KakuteF4, KakuteF7Mini, MambaF405, MatekF405, Omnibusf4 to reduce code size (aka "flash") + - CSRF and Hott telemetry disabled by default on some low power boards (aka "minimised boards") + - Foxeer Reaper F745 supports external compasses + - OmnibusF4 support for BMI270 IMU + - OmnibusF7V2-bdshot support removed + - KakuteF7 regains displayport, frees up DMA from unused serial port + - KakuteH7v2 gets second battery sensor + - MambaH743v4 supports VTX + - MatekF405-Wing supports InvensenseV3 IMUs + - PixPilot-V6 heater enabled + - Raspberry 64OS startup crash fixed + - ReaperF745AIO serial protocol defaults fixed + - SkystarsH7HD (non-bdshot) removed as users should always use -bdshot version + - Skyviper loses many unnecessary features to save flash + - UBlox GPS only for AtomRCF405NAVI, BeastF7, MatekF405, Omnibusf4 to reduce code size (aka "flash") + - VRBrain-v52 and VRCore-v10 features reduced to save flash +3) Driver enhancements + - ARK RTK GPS support + - BMI088 IMU filtering and timing improved, ignores bad data + - CRSF OSD may display disarmed state after flight mode (enabled using RC_OPTIONS) + - Daiwa winch baud rate obeys SERIALx_BAUD parameter + - EFI supports fuel pressure and ignition voltage reporting and battery failsafe + - ICM45686 IMU support + - ICM20602 uses fast reset instead of full reset on bad temperature sample (avoids occasional very high offset) + - ICM45686 supports fast sampling + - MAX31865 temp sensor support + - MB85RS256TY-32k, PB85RS128C and PB85RS2MC FRAM support + - MMC3416 compass orientation fix + - MPPT battery monitor reliability improvements, enable/disable aux function and less spammy + - Multiple USD-D1-CAN radar support + - NMEA output rate configurable (see NMEA_RATE_MS) + - NMEA output supports PASHR message (see NMEA_MSG_EN) + - OSD supports average resting cell voltage (see OSD_ACRVOLT_xxx params) + - Rockblock satellite modem support + - Serial baud support for 2Mbps (only some hardware supports this speed) + - SF45b lidar filtering reduced (allows detecting smaller obstacles + - SmartAudio 2.0 learns all VTX power levels) + - UAVCAN ESCs report error count using ESC Telemetry + - Unicore GPS (e.g. UM982) support + - VectorNav 100 external AHRS support + - 5 IMUs supported +4) EKF related enhancements + - Baro compensation using wind estimates works when climbing or descending (see BAROx_WCF_UP/DN) + - External AHRS support for enabling only some sensors (e.g. IMU, Baro, Compass) see EAHRS_SENSORS + - Magnetic field tables updated + - Non-compass initial yaw alignment uses GPS course over GSF (mostly affects Planes and Rover) +5) Control and navigation enhancements + - DO_SET_ROI_NONE command turns off ROI + - JUMP_TAG mission item support + - Manual mode steering expo configurable (see MANUAL_STR_EXPO) + - Missions can be stored on SD card (see BRD_SD_MISSION) + - NAV_SCRIPT_TIME command accepts floating point arguments + - Pause/Resume returns success if mission is already paused or resumed +8) Camera and gimbal enhancements + - BMMCC support included in Servo driver + - DJI RS2/RS3-Pro gimbal support + - Dual camera support (see CAM2_TYPE) + - Gimbal/Mount2 can be moved to retracted or neutral position + - Gremsy ZIO support + - IMAGE_START_CAPTURE, SET_CAMERA_ZOOM/FOCUS, VIDEO_START/STOP_CAPTURE command support + - Paramters renamed and rescaled + - CAM_TRIGG_TYPE renamed to CAM1_TYPE and options have changed + - CAM_DURATION renamed to CAM1_DURATION and scaled in seconds + - CAM_FEEDBACK_PIN/POL renamed to CAM1_FEEBAK_PIN/POL + - CAM_MIN_INTERVAL renamed to CAM1_INTRVAL_MIN and scaled in seconds + - CAM_TRIGG_DIST renamed to CAMx_TRIGG_DIST and accepts fractional values + - RunCam2 4k support + - ViewPro camera gimbal support +8) Logging changes + - BARD msg includes 3-axis dynamic pressure useful for baro compensation of wind estimate + - MCU log msg includes main CPU temp and voltage (was part of POWR message) + - RCOut banner message always included in Logs + - SCR message includes memory usage of all running scripts + - CANS message includes CAN bus tx/rx statistics + - Home location not logged to CMD message + - MOTB message includes throttle output +9) Scripting enhancements + - EFI Skypower driver gets improved telem messages and bug fixes + - Generator throttle control example added + - Heap max increased by allowing heap to be split across multiple underlying OS heaps + - Hexsoon LEDs applet + - Logging from scripts supports more formats + - Parameters can be removed or reordered + - Parameter description support (scripts must be in AP's applet or driver directory) + - Rangefinder driver support + - Runcam_on_arm applet starts recording when vehicle is armed + - Safety switch, E-Stop and motor interlock support + - Scripts can restart all scripts + - Script_Controller applet supports inflight switching of active scripts +10) Custom build server enhancements + - AIS support for displaying nearby boats can be included + - Battery, Camera and Compass drivers can be included/excluded + - EKF3 wind estimation can be included/excluded + - PCA9685, ToshibaLED, PLAY_TUNE notify drivers can be included/excluded + - RichenPower generator can be included/excluded + - RC SRXL protocol can be excluded + - SIRF GPSs can be included/excluded +11) Safety related enhancements and fixes + - Arming check for servo outputs skipped when SERVOx_FUNCTION is scripting + - Arming check fix if both "All" and other bitmasks are selected (previously only ran the other checks) + - GCS failsafe timeout is configurable (see FS_GCS_TIMEOUT) + - "EK3 sources require RangeFinder" pre-arm check fix when user only sets up 2nd rangefinder (e.g. 1st is disabled) + - Pre-arm check that low and critical battery failsafe thresholds are different + - Pre-arm message fixed if 2nd EKF core unhealthy + - Pre-arm check if reboot required to enabled IMU batch sampling (used for vibe analysis) + - RC failsafe timeout configurable (see RC_FS_TIMEOUT) +12) Minor enhancements + - Boot time reduced by improving parameter conversion efficiency + - BRD_SAFETYENABLE parameter renamed to BRD_SAFETY_DEFLT + - Compass calibration auxiliary switch function (set RCx_OPTION=171) + - Disable IMU3 auxiliary switch function (set RCx_OPTION=110) + - Rangefinder and FS_OPTIONS param conversion code reduced (affects when upgrading from 3.6 or earlier) + - MAVFTP supports file renaming + - MAVLink in-progress reply to some requests for calibration from GCS +13) Bug fixes: + - ADSB telemetry and callsign fixes + - Battery pct reported to GCS limited to 0% to 100% range + - Bi-directional DShot fix on H7 boards after system time wrap (more complete fix than in 4.3.6) + - DisplayPort OSD screen reliability improvement on heavily loaded OSDs especially F4 boards + - DisplayPort OSD artificial horizon better matches actual horizon + - EFI Serial MS bug fix to avoid possible infinite loop + - EKF3 Replay fix when COMPASS_LEARN=3 + - ESC Telemetry external temp reporting fix + - Fence upload works even if Auto mode is excluded from firmware + - FMT messages logged even when Fence is exncluded from firmware (e.g. unselected when using custom build server) + - Hardfault avoided if user changes INS_LOG_BAT_CNT while batch sampling running + - ICM20649 temp sensor tolerate increased to avoid unnecessary FIFO reset + - IMU detection bug fix to avoid duplicates + - IMU temp cal fix when using auxiliary IMU + - Message Interval fix for restoring default rate https://github.com/ArduPilot/ardupilot/pull/21947 + - RADIO_STATUS messages slow-down feature never completely stops messages from being sent + - SERVOx_TRIM value output momentarily if SERVOx_FUNCTION is changed from Disabled to RCPassThru, RCIN1, etc. Avoids momentary divide-by-zero + - Scripting file system open fix + - Scripting PWM source deletion crash fix + - MAVFTP fix for low baudrates (4800 baud and lower) + - ModalAI VOXL reset handling fix + - MPU6500 IMU fast sampling rate to 4k (was 1K) + - NMEA GPGGA output fixed for GPS quality, num sats and hdop + - Position control reset avoided even with very uneven main loop rate due to high CPU load + - Throttle notch FFT tuning param fix + - VTX protects against pitmode changes when not enabled or vehicle disarmed +14) Developer specific items + - DroneCAN replaces UAVCAN + - FlighAxis simulator rangefinder fixed + - Scripts in applet and drivers directory checked using linter + - Simulator supports main loop timing jitter (see SIM_TIME_JITTER) + - Simulink model and init scripts + - SITL on hardware support (useful to demo servos moving in response to simulated flight) + - SITL parameter definitions added (some, not all) + - Webots 2023a simulator support + - XPlane support for wider range of aircraft +------------------------------------------------------------------ +Rover 4.3.0-beta14 12-Aug-2023 +Changes from 4.3.0-beta13 +1) Bug fixes + - DroneCAN GPS RTK injection fix + - INAxxx battery monitors allow for battery reset remaining + - Notch filter gyro glitch caused by race condition fixed + - Scripting restart memory corruption bug fixed +------------------------------------------------------------------ +Rover 4.3.0-beta13 27-Mar-2023 +Changes from 4.3.0-beta12 +1) Bug fixes + a) EKF3 accel bias calculations bug fix + b) EKF3 accel bias process noise adjusted for greater robustness + c) GSF yaw numerical stability fix caused by compassmot + d) INS batch sampler fix to avoid watchdog if INS_LOG_BAT_CNT changed without rebooting + e) Memory corruption bug in the STM32H757 (very rare) + f) RC input on IOMCU bug fix (RC might not be regained if lost) +------------------------------------------------------------------ +Rover 4.3.0-beta11/beta12 27-Mar-2023 +Changes from 4.3.0-beta10 +1) Bi-directional DShot fix for possible motor stop approx 72min after startup +------------------------------------------------------------------ Rover 4.3.0-beta10 01-Mar-2023 Changes from 4.3.0-beta9 1) Bug fixes diff --git a/Rover/system.cpp b/Rover/system.cpp index c7a17b10d5ac9..492463e8a2345 100644 --- a/Rover/system.cpp +++ b/Rover/system.cpp @@ -75,8 +75,10 @@ void Rover::init_ardupilot() g2.proximity.init(); #endif +#if AP_BEACON_ENABLED // init beacons used for non-gps position estimation g2.beacon.init(); +#endif // and baro for EKF barometer.set_log_baro_bit(MASK_LOG_IMU); @@ -105,7 +107,9 @@ void Rover::init_ardupilot() optflow.init(MASK_LOG_OPTFLOW); #endif // AP_OPTICALFLOW_ENABLED +#if AP_RELAY_ENABLED relay.init(); +#endif #if HAL_MOUNT_ENABLED // initialise camera mount @@ -117,7 +121,7 @@ void Rover::init_ardupilot() camera.init(); #endif -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED // initialise precision landing init_precland(); #endif @@ -184,10 +188,6 @@ void Rover::startup_ground(void) #if AP_SCRIPTING_ENABLED g2.scripting.init(); #endif // AP_SCRIPTING_ENABLED - - // we don't want writes to the serial port to cause us to pause - // so set serial ports non-blocking once we are ready to drive - serial_manager.set_blocking_writes_all(false); } // update the ahrs flyforward setting which can allow @@ -219,6 +219,30 @@ void Rover::update_ahrs_flyforward() ahrs.set_fly_forward(flyforward); } +// Check if this mode can be entered from the GCS +bool Rover::gcs_mode_enabled(const Mode::Number mode_num) const +{ + // List of modes that can be blocked, index is bit number in parameter bitmask + static const uint8_t mode_list [] { + (uint8_t)Mode::Number::MANUAL, + (uint8_t)Mode::Number::ACRO, + (uint8_t)Mode::Number::STEERING, + (uint8_t)Mode::Number::LOITER, + (uint8_t)Mode::Number::FOLLOW, + (uint8_t)Mode::Number::SIMPLE, + (uint8_t)Mode::Number::CIRCLE, + (uint8_t)Mode::Number::AUTO, + (uint8_t)Mode::Number::RTL, + (uint8_t)Mode::Number::SMART_RTL, + (uint8_t)Mode::Number::GUIDED, +#if MODE_DOCK_ENABLED == ENABLED + (uint8_t)Mode::Number::DOCK +#endif + }; + + return !block_GCS_mode_change((uint8_t)mode_num, mode_list, ARRAY_SIZE(mode_list)); +} + bool Rover::set_mode(Mode &new_mode, ModeReason reason) { if (control_mode == &new_mode) { @@ -226,6 +250,12 @@ bool Rover::set_mode(Mode &new_mode, ModeReason reason) return true; } + // 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()); + return false; + } + Mode &old_mode = *control_mode; if (!new_mode.enter()) { // Log error that we failed to enter desired flight mode @@ -251,7 +281,7 @@ bool Rover::set_mode(Mode &new_mode, ModeReason reason) old_mode.exit(); control_mode_reason = reason; - logger.Write_Mode(control_mode->mode_number(), control_mode_reason); + logger.Write_Mode((uint8_t)control_mode->mode_number(), control_mode_reason); gcs().send_message(MSG_HEARTBEAT); notify_mode(control_mode); @@ -261,9 +291,14 @@ bool Rover::set_mode(Mode &new_mode, ModeReason reason) bool Rover::set_mode(const uint8_t new_mode, ModeReason reason) { static_assert(sizeof(Mode::Number) == sizeof(new_mode), "The new mode can't be mapped to the vehicles mode number"); - Mode *mode = rover.mode_from_mode_num((enum Mode::Number)new_mode); + return rover.set_mode(static_cast(new_mode), reason); +} + +bool Rover::set_mode(Mode::Number new_mode, ModeReason reason) +{ + Mode *mode = rover.mode_from_mode_num(new_mode); if (mode == nullptr) { - notify_no_such_mode(new_mode); + notify_no_such_mode((uint8_t)new_mode); return false; } return rover.set_mode(*mode, reason); @@ -287,7 +322,7 @@ void Rover::startup_INS_ground(void) void Rover::notify_mode(const Mode *mode) { AP_Notify::flags.autopilot_mode = mode->is_autopilot_mode(); - notify.flags.flight_mode = mode->mode_number(); + notify.flags.flight_mode = (uint8_t)mode->mode_number(); notify.set_flight_mode_str(mode->name4()); } diff --git a/Rover/version.h b/Rover/version.h index 6ec99cb1d6b81..11b034f62f7d6 100644 --- a/Rover/version.h +++ b/Rover/version.h @@ -6,13 +6,13 @@ #include "ap_version.h" -#define THISFIRMWARE "ArduRover V4.4.0-dev" +#define THISFIRMWARE "ArduRover V4.5.0-dev" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,4,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 4,5,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 4 -#define FW_MINOR 4 +#define FW_MINOR 5 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV diff --git a/Tools/AP_Bootloader/AP_Bootloader.cpp b/Tools/AP_Bootloader/AP_Bootloader.cpp index d383905f3a7a7..f42271208073b 100644 --- a/Tools/AP_Bootloader/AP_Bootloader.cpp +++ b/Tools/AP_Bootloader/AP_Bootloader.cpp @@ -63,6 +63,8 @@ AP_FlashIface_JEDEC ext_flash; int main(void) { + custom_startup(); + #ifdef STM32F427xx if (BOARD_FLASH_SIZE > 1024 && check_limit_flash_1M()) { board_info.fw_size = (1024 - (FLASH_BOOTLOADER_LOAD_KB + APP_START_OFFSET_KB))*1024; @@ -161,6 +163,14 @@ int main(void) } #endif +#if EXT_FLASH_SIZE_MB + while (!ext_flash.init()) { + // keep trying until we get it working + // there's no future without it + chThdSleep(chTimeMS2I(20)); + } +#endif + if (try_boot) { jump_to_app(); } @@ -174,14 +184,6 @@ int main(void) flash_init(); -#if EXT_FLASH_SIZE_MB - while (!ext_flash.init()) { - // keep trying until we get it working - // there's no future without it - chThdSleep(chTimeMS2I(20)); - } -#endif - #if AP_BOOTLOADER_FLASH_FROM_SD_ENABLED if (flash_from_sd()) { jump_to_app(); diff --git a/Tools/AP_Bootloader/bl_protocol.cpp b/Tools/AP_Bootloader/bl_protocol.cpp index c0b7171a2043a..5bc0df7b2877f 100644 --- a/Tools/AP_Bootloader/bl_protocol.cpp +++ b/Tools/AP_Bootloader/bl_protocol.cpp @@ -163,7 +163,7 @@ extern AP_FlashIface_JEDEC ext_flash; /* 1ms timer tick callback */ -static void sys_tick_handler(void *ctx) +static void sys_tick_handler(virtual_timer_t* vt, void *ctx) { chSysLockFromISR(); chVTSetI(&systick_vt, chTimeMS2I(1), sys_tick_handler, nullptr); @@ -240,6 +240,10 @@ do_jump(uint32_t stacktop, uint32_t entrypoint) #define APP_START_ADDRESS (FLASH_LOAD_ADDRESS + (FLASH_BOOTLOADER_LOAD_KB + APP_START_OFFSET_KB)*1024U) #endif +#if !defined(STM32_OTG2_IS_OTG1) +#define STM32_OTG2_IS_OTG1 0 +#endif + void jump_to_app() { @@ -316,12 +320,17 @@ jump_to_app() #elif defined(STM32L4) rccDisableAPB1R1(~0); rccDisableAPB1R2(~0); +#elif defined(STM32L4PLUS) + rccDisableAPB1R1(~0); + rccDisableAPB1R2(~0); #else rccDisableAPB1(~0); #endif rccDisableAPB2(~0); -#if HAL_USE_SERIAL_USB == TRUE +#if HAL_USE_SERIAL_USB == TRUE +#if !STM32_OTG2_IS_OTG1 rccResetOTG_FS(); +#endif #if defined(rccResetOTG_HS) rccResetOTG_HS(); #endif @@ -625,7 +634,7 @@ bootloader(unsigned timeout) led_set(LED_OFF); // erase all sectors - for (uint8_t i = 0; flash_func_sector_size(i) != 0; i++) { + for (uint16_t i = 0; flash_func_sector_size(i) != 0; i++) { #if defined(STM32F7) || defined(STM32H7) if (!flash_func_erase_sector(i, c == PROTO_CHIP_FULL_ERASE)) { #else diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index 748317dd9332e..27bfe00e6a5ad 100644 --- a/Tools/AP_Bootloader/board_types.txt +++ b/Tools/AP_Bootloader/board_types.txt @@ -177,8 +177,8 @@ AP_HW_QioTekAdeptF407 1065 AP_HW_QioTekAdeptF427 1066 AP_HW_FlyingMoonF407 1067 AP_HW_FlyingMoonF427 1068 -AP_HW_CUBERED 1069 -AP_HW_CUBERED_IO 1070 +AP_HW_CUBERED_PRIMARY 1069 +AP_HW_CUBERED_SECONDARY 1070 AP_HW_GreenSight_UltraBlue 1071 AP_HW_GreenSight_microBlue 1072 AP_HW_MAMBAH743_V4 1073 @@ -216,6 +216,48 @@ AP_HW_rFCU 1102 AP_HW_rGNSS 1103 AP_HW_AEROFOX_AIRSPEED_DLVR 1104 AP_HW_KakuteH7-Wing 1105 +AP_HW_SpeedyBeeF405WING 1106 +AP_HW_PixSurveyA-IND 1107 +AP_HW_SPRACINGH7RF 1108 +AP_HW_AEROFOX_GNSS_F9P 1109 +AP_HW_JFB110 1110 +AP_HW_SDMODELH7V1 1111 +AP_HW_FlyingMoonH743 1112 +AP_HW_YJUAV_A6 1113 +AP_HW_YJUAV_A6Nano 1114 +AP_HW_ACNS_CM4PILOT 1115 +AP_HW_ACNS_F405AIO 1116 +AP_HW_BLITZF7 1117 +AP_HW_RADIX2HD 1118 +AP_HW_HEEWING_F405 1119 +AP_HW_PodmanH7 1120 +AP_HW_mRo-M10053 1121 +AP_HW_mRo-M10044 1122 +AP_HW_SIYI_N7 1123 +AP_HW_mRoCZOEM_revG 1124 +AP_HW_BETAFPV_F405 1125 +AP_HW_QioTekAdeptH743 1126 +AP_HW_YJUAV_A6SE 1127 +AP_HW_QioTekAdept_6C 1128 + +AP_HW_PixFlamingoL4R5_V2 1129 +AP_HW_PixFlamingoF427_V1 1130 +AP_HW_PixFlamingoF767_V1 1131 +AP_HW_PixFlamingoH743I 1132 +AP_HW_PixFlamingoH743V 1133 + +AP_HW_AR-F407SmartBat 1134 +AP_HW_SPEEDYBEEF4MINI 1135 +AP_HW_SPEEDYBEEF4V4 1136 +AP_HW_FlywooF405Pro 1137 +AP_HW_TMOTORH7 1138 +AP_HW_MICOAIR405 1139 +AP_HW_PixPilot-C3 1140 +AP_HW_YJUAV_A6SE_H743 1141 +AP_HW_FSO_POWER_STACK 1142 +AP_HW_ATOMRCF405NAVI_DLX 1143 +AP_HW_YJUAV_A6Ultra 1144 + AP_HW_ESP32_PERIPH 1205 AP_HW_ESP32S3_PERIPH 1206 @@ -225,6 +267,22 @@ AP_HW_CUBEBLACK_PERIPH 1401 AP_HW_PIXRACER_PERIPH 1402 AP_HW_SWBOOMBOARD_PERIPH 1403 +AP_HW_VIMDRONES_FLOW 1404 +AP_HW_VIMDRONES_MOSAIC_X5 1405 +AP_HW_VIMDRONES_MOSAIC_H 1406 +AP_HW_VIMDRONES_PERIPH 1407 + +# IDs 5000-5099 reserved for Carbonix +# IDs 5100-5199 reserved for SYPAQ Systems +# IDs 5200-5209 reserved for Airvolute +AP_HW_AIRVOLUTE_DCS2 5200 + +# IDs 5210-5219 reserved for Aocoda-RC +AP_HW_AOCODA-RC-H743DUAL 5210 +AP_HW_AOCODA-RC-F405V3 5211 + +# IDs 6000-6099 reserved for SpektreWorks + # OpenDroneID enabled boards. Use 10000 + the base board ID AP_HW_CubeOrange_ODID 10140 AP_HW_Pixhawk6_ODID 10053 diff --git a/Tools/AP_Bootloader/can.cpp b/Tools/AP_Bootloader/can.cpp index 715070edc6972..24ed583284eba 100644 --- a/Tools/AP_Bootloader/can.cpp +++ b/Tools/AP_Bootloader/can.cpp @@ -47,18 +47,20 @@ static CANConfig cancfg = { CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP, 0 // filled in below }; +// pipelining is not faster when using ChibiOS CAN driver +#define FW_UPDATE_PIPELINE_LEN 1 #else static ChibiOS::CANIface can_iface[HAL_NUM_CAN_IFACES]; #endif #ifndef CAN_APP_VERSION_MAJOR -#define CAN_APP_VERSION_MAJOR 1 +#define CAN_APP_VERSION_MAJOR 2 #endif #ifndef CAN_APP_VERSION_MINOR #define CAN_APP_VERSION_MINOR 0 #endif #ifndef CAN_APP_NODE_NAME -#define CAN_APP_NODE_NAME "org.ardupilot.ap_periph" +#define CAN_APP_NODE_NAME "org.ardupilot." CHIBIOS_BOARD_NAME #endif static uint8_t node_id_allocation_transfer_id; @@ -66,14 +68,30 @@ static uavcan_protocol_NodeStatus node_status; static uint32_t send_next_node_id_allocation_request_at_ms; static uint8_t node_id_allocation_unique_id_offset; +static void processTx(void); + +// keep up to 4 transfers in progress +#ifndef FW_UPDATE_PIPELINE_LEN +#define FW_UPDATE_PIPELINE_LEN 4 +#endif + static struct { - uint64_t ofs; - uint32_t last_ms; + uint32_t rtt_ms; + uint32_t ofs; uint8_t node_id; - uint8_t transfer_id; uint8_t path[sizeof(uavcan_protocol_file_Path::path.data)+1]; uint8_t sector; uint32_t sector_ofs; + uint8_t transfer_id; + uint8_t idx; + struct { + uint8_t tx_id; + uint32_t sent_ms; + uint32_t offset; + bool have_reply; + uavcan_protocol_file_ReadResponse pkt; + } reads[FW_UPDATE_PIPELINE_LEN]; + uint16_t erased_to; } fw_update; /* @@ -150,87 +168,184 @@ static void handle_get_node_info(CanardInstance* ins, /* send a read for a fw update */ -static void send_fw_read(void) +static bool send_fw_read(uint8_t idx) { - uint32_t now = AP_HAL::millis(); - if (now - fw_update.last_ms < 750) { - // the server may still be responding - return; - } - fw_update.last_ms = now; + auto &r = fw_update.reads[idx]; + r.tx_id = fw_update.transfer_id; + r.have_reply = false; + + uavcan_protocol_file_ReadRequest pkt {}; + pkt.path.path.len = strlen((const char *)fw_update.path); + pkt.offset = r.offset; + memcpy(pkt.path.path.data, fw_update.path, pkt.path.path.len); uint8_t buffer[UAVCAN_PROTOCOL_FILE_READ_REQUEST_MAX_SIZE]; - canardEncodeScalar(buffer, 0, 40, &fw_update.ofs); - uint32_t offset = 40; - uint8_t len = strlen((const char *)fw_update.path); - for (uint8_t i=0; i 0) { + // mark it as having been sent + r.sent_ms = AP_HAL::millis(); + return true; + } + return false; } /* - handle response to file read for fw update + send a read for a fw update */ -static void handle_file_read_response(CanardInstance* ins, CanardRxTransfer* transfer) +static void send_fw_reads(void) { - if ((transfer->transfer_id+1)%256 != fw_update.transfer_id || - transfer->source_node_id != fw_update.node_id) { - return; + const uint32_t now = AP_HAL::millis(); + + for (uint8_t i=0; ipayload_len - 2; +} - uint32_t offset = 16; - uint32_t buf32[(len+3)/4]; - uint8_t *buf = (uint8_t *)&buf32[0]; - for (uint16_t i=0; isource_node_id != fw_update.node_id) { + return; } - if (fw_update.sector_ofs+len > sector_size) { - flash_func_erase_sector(fw_update.sector+1); + /* + match the response to a sent request + */ + uint8_t idx = 0; + bool found = false; + for (idx=0; idxtransfer_id) { + found = true; + break; + } } - for (uint16_t i=0; i= flash_func_sector_size(fw_update.sector)) { - fw_update.sector++; - fw_update.sector_ofs -= sector_size; + if (uavcan_protocol_file_ReadResponse_decode(transfer, &fw_update.reads[idx].pkt)) { + return; } - if (len < sizeof(uavcan_protocol_file_ReadResponse::data.data)) { - fw_update.node_id = 0; - flash_write_flush(); - const auto ok = check_good_firmware(); - node_status.vendor_specific_status_code = uint8_t(ok); - if (ok == check_fw_result_t::CHECK_FW_OK) { - jump_to_app(); + fw_update.reads[idx].have_reply = true; + uint32_t rtt = MIN(3000,MAX(AP_HAL::millis() - fw_update.reads[idx].sent_ms, 25)); + fw_update.rtt_ms = uint32_t(0.9 * fw_update.rtt_ms + 0.1 * rtt); + + while (fw_update.reads[fw_update.idx].have_reply) { + auto &r = fw_update.reads[fw_update.idx]; + if (r.offset != fw_update.ofs) { + // bad sequence + r.have_reply = false; + r.sent_ms = 0; + break; + } + const auto &pkt = r.pkt; + const uint16_t len = pkt.data.len; + const uint16_t len_words = (len+3U)/4U; + const uint8_t *buf = (uint8_t *)pkt.data.data; + uint32_t buf32[len_words] {}; + memcpy((uint8_t*)buf32, buf, len); + + if (fw_update.ofs == 0) { + flash_set_keep_unlocked(true); + } + + const uint32_t sector_size = flash_func_sector_size(fw_update.sector); + if (sector_size == 0) { + // firmware is too big + fw_update.node_id = 0; + flash_write_flush(); + flash_set_keep_unlocked(false); + node_status.vendor_specific_status_code = uint8_t(check_fw_result_t::FAIL_REASON_BAD_LENGTH_APP); + break; + } + if (fw_update.sector_ofs == 0) { + erase_to(fw_update.sector); + } + if (fw_update.sector_ofs+len > sector_size) { + erase_to(fw_update.sector+1); + } + if (!flash_write_buffer(fw_update.ofs, buf32, len_words)) { + continue; + } + + fw_update.ofs += len; + fw_update.sector_ofs += len; + if (fw_update.sector_ofs >= flash_func_sector_size(fw_update.sector)) { + fw_update.sector++; + fw_update.sector_ofs -= sector_size; + } + + if (len < sizeof(uavcan_protocol_file_ReadResponse::data.data)) { + fw_update.node_id = 0; + flash_write_flush(); + flash_set_keep_unlocked(false); + const auto ok = check_good_firmware(); + node_status.vendor_specific_status_code = uint8_t(ok); + if (ok == check_fw_result_t::CHECK_FW_OK) { + jump_to_app(); + } + return; } + + r.have_reply = false; + r.sent_ms = 0; + r.offset += FW_UPDATE_PIPELINE_LEN*sizeof(uavcan_protocol_file_ReadResponse::data.data); + send_fw_read(fw_update.idx); + processTx(); + + fw_update.idx = (fw_update.idx + 1) % FW_UPDATE_PIPELINE_LEN; } // show offset number we are flashing in kbyte as crude progress indicator node_status.vendor_specific_status_code = 1 + (fw_update.ofs / 1024U); - - fw_update.last_ms = 0; } /* @@ -238,23 +353,21 @@ static void handle_file_read_response(CanardInstance* ins, CanardRxTransfer* tra */ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* transfer) { - // manual decoding due to TAO bug in libcanard generated code - if (transfer->payload_len < 1 || transfer->payload_len > sizeof(fw_update.path)+1) { - return; - } - if (fw_update.node_id == 0) { - uint32_t offset = 0; - canardDecodeScalar(transfer, 0, 8, false, (void*)&fw_update.node_id); - offset += 8; - for (uint8_t i=0; ipayload_len-1; i++) { - canardDecodeScalar(transfer, offset, 8, false, (void*)&fw_update.path[i]); - offset += 8; + uavcan_protocol_file_BeginFirmwareUpdateRequest pkt; + if (uavcan_protocol_file_BeginFirmwareUpdateRequest_decode(transfer, &pkt)) { + return; } - fw_update.ofs = 0; - fw_update.last_ms = 0; - fw_update.sector = 0; - fw_update.sector_ofs = 0; + if (pkt.image_file_remote_path.path.len > sizeof(fw_update.path)-1) { + return; + } + memset(&fw_update, 0, sizeof(fw_update)); + for (uint8_t i=0; isource_node_id; } @@ -274,8 +387,6 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* CanardResponse, &buffer[0], total_size); - - send_fw_read(); } static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* transfer) @@ -291,35 +402,28 @@ static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* tr return; } - // Copying the unique ID from the message - static const uint8_t UniqueIDBitOffset = 8; - uint8_t received_unique_id[sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data)]; - uint8_t received_unique_id_len = 0; - for (; received_unique_id_len < (transfer->payload_len - (UniqueIDBitOffset / 8U)); received_unique_id_len++) { - const uint8_t bit_offset = (uint8_t)(UniqueIDBitOffset + received_unique_id_len * 8U); - (void) canardDecodeScalar(transfer, bit_offset, 8, false, &received_unique_id[received_unique_id_len]); + struct uavcan_protocol_dynamic_node_id_Allocation msg; + if (uavcan_protocol_dynamic_node_id_Allocation_decode(transfer, &msg)) { + return; } - + // Obtaining the local unique ID uint8_t my_unique_id[sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data)]; readUniqueID(my_unique_id); // Matching the received UID against the local one - if (memcmp(received_unique_id, my_unique_id, received_unique_id_len) != 0) { + if (memcmp(msg.unique_id.data, my_unique_id, msg.unique_id.len) != 0) { node_id_allocation_unique_id_offset = 0; return; // No match, return } - if (received_unique_id_len < sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data)) { + if (msg.unique_id.len < sizeof(msg.unique_id.data)) { // The allocator has confirmed part of unique ID, switching to the next stage and updating the timeout. - node_id_allocation_unique_id_offset = received_unique_id_len; + node_id_allocation_unique_id_offset = msg.unique_id.len; send_next_node_id_allocation_request_at_ms -= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS; } else { // Allocation complete - copying the allocated node ID from the message - uint8_t allocated_node_id = 0; - (void) canardDecodeScalar(transfer, 0, 7, false, &allocated_node_id); - - canardSetLocalNodeID(ins, allocated_node_id); + canardSetLocalNodeID(ins, msg.node_id); } } @@ -617,6 +721,9 @@ bool can_check_update(void) if (comms->magic == APP_BOOTLOADER_COMMS_MAGIC) { can_set_node_id(comms->my_node_id); fw_update.node_id = comms->server_node_id; + for (uint8_t i=0; ipath, sizeof(uavcan_protocol_file_Path::path.data)+1); ret = true; // clear comms region @@ -704,8 +811,8 @@ void can_start() void can_update() { - // do one loop of CAN support. If we are doing a few update then - // loop until it is finished + // do one loop of CAN support. If we are doing a firmware update + // then loop until it is finished do { processTx(); processRx(); @@ -717,8 +824,12 @@ void can_update() process1HzTasks(AP_HAL::micros64()); } if (fw_update.node_id != 0) { - send_fw_read(); + send_fw_reads(); } +#if CH_CFG_ST_FREQUENCY >= 1000000 + // give a bit of time for background processing + chThdSleepMicroseconds(200); +#endif } while (fw_update.node_id != 0); } diff --git a/Tools/AP_Bootloader/custom.cpp b/Tools/AP_Bootloader/custom.cpp new file mode 100644 index 0000000000000..ef5f7ef434d38 --- /dev/null +++ b/Tools/AP_Bootloader/custom.cpp @@ -0,0 +1,52 @@ +/* + custom code for specific boards + */ +#include +#include "ch.h" +#include "hal.h" +#include "support.h" + +#ifdef AP_BOOTLOADER_CUSTOM_HERE4 +/* + reset here4 LEDs +*/ +static void bootloader_custom_Here4(void) +{ + for (uint8_t n=0; n<10; n++) { + const uint8_t num_leds = 4; + const uint32_t min_bits = num_leds*25+50; + const uint8_t num_leading_zeros = 8-min_bits%8 + 50; + const uint32_t output_stream_byte_length = (min_bits+7)/8; + palSetLineMode(HAL_GPIO_PIN_LED_DI, PAL_MODE_OUTPUT_PUSHPULL); + palSetLineMode(HAL_GPIO_PIN_LED_SCK, PAL_MODE_OUTPUT_PUSHPULL); + int l = 100; + while (l--) { + for (uint32_t i=0; i> 16); - mcu_des_t des = mcu_descriptions[STM32_UNKNOWN]; + uint8_t *endp = &revstr[max - 1]; + uint8_t *strp = revstr; for (const auto &desc : mcu_descriptions) { if (mcuid == desc.mcuid) { - des = desc; + // copy the string in: + const char *tmp = desc.desc; + while (strp < endp && *tmp) { + *strp++ = *tmp++; + } break; } } - for (const auto &rev : silicon_revs) { - if (rev.revid == revid) { - des.rev = rev.rev; - } - } - - uint8_t *endp = &revstr[max - 1]; - uint8_t *strp = revstr; - - while (strp < endp && *des.desc) { - *strp++ = *des.desc++; - } - + // comma-separated: if (strp < endp) { *strp++ = ','; } - if (strp < endp) { - *strp++ = des.rev; + for (const auto &rev : silicon_revs) { + if (rev.revid == revid) { + if (strp < endp) { + *strp++ = rev.rev; + } + } } return strp - revstr; @@ -343,6 +353,17 @@ void uprintf(const char *fmt, ...) #endif } +static void thread_sleep_ms(uint32_t ms) +{ + while (ms > 0) { + // don't sleep more than 65 at a time, to cope with 16 bit + // timer + const uint32_t dt = ms > 65? 65: ms; + chThdSleepMilliseconds(dt); + ms -= dt; + } +} + // generate a pulse sequence forever, for debugging void led_pulses(uint8_t npulses) { @@ -350,11 +371,11 @@ void led_pulses(uint8_t npulses) while (true) { for (uint8_t i=0; i= 0) { auto *uart = hal.serial(g.proximity_port); if (uart != nullptr) { @@ -238,6 +250,15 @@ void AP_Periph_FW::init() hwesc_telem.init(hal.serial(HAL_PERIPH_HWESC_SERIAL_PORT)); #endif +#ifdef HAL_PERIPH_ENABLE_ESC_APD + for (uint8_t i = 0; i < ESC_NUMBERS; i++) { + const uint8_t port = g.esc_serial_port[i]; + if (port < SERIALMANAGER_NUM_PORTS) { // skip bad ports + apd_esc_telem[i] = new ESC_APD_Telem (hal.serial(port), g.pole_count[i]); + } + } +#endif + #ifdef HAL_PERIPH_ENABLE_MSP if (g.msp_port >= 0) { msp_init(hal.serial(g.msp_port)); @@ -252,6 +273,10 @@ void AP_Periph_FW::init() nmea.init(); #endif +#ifdef HAL_PERIPH_ENABLE_RPM + rpm_sensor.init(); +#endif + #ifdef HAL_PERIPH_ENABLE_NOTIFY notify.init(); #endif @@ -259,7 +284,7 @@ void AP_Periph_FW::init() #if AP_SCRIPTING_ENABLED scripting.init(); #endif - start_ms = AP_HAL::native_millis(); + start_ms = AP_HAL::millis(); } #if (defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY) && HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY == 8) || defined(HAL_PERIPH_ENABLE_NOTIFY) @@ -277,7 +302,7 @@ void AP_Periph_FW::update_rainbow() if (rainbow_done) { return; } - uint32_t now = AP_HAL::native_millis(); + uint32_t now = AP_HAL::millis(); if (now - start_ms > 1500) { rainbow_done = true; #if defined (HAL_PERIPH_ENABLE_NOTIFY) @@ -361,7 +386,7 @@ void AP_Periph_FW::update() #endif static uint32_t last_led_ms; - uint32_t now = AP_HAL::native_millis(); + uint32_t now = AP_HAL::millis(); if (now - last_led_ms > 1000) { last_led_ms = now; #ifdef HAL_GPIO_PIN_LED @@ -397,10 +422,8 @@ void AP_Periph_FW::update() rcout_init_1Hz(); #endif -#if HAL_GCS_ENABLED - gcs().send_message(MSG_HEARTBEAT); - gcs().send_message(MSG_SYS_STATUS); -#endif + GCS_SEND_MESSAGE(MSG_HEARTBEAT); + GCS_SEND_MESSAGE(MSG_SYS_STATUS); } static uint32_t last_error_ms; @@ -413,13 +436,13 @@ void AP_Periph_FW::update() #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && CH_DBG_ENABLE_STACK_CHECK == TRUE static uint32_t last_debug_ms; - if ((g.debug&(1< 5000) { + if (debug_option_is_set(DebugOptions::SHOW_STACK) && now - last_debug_ms > 5000) { last_debug_ms = now; show_stack_free(); } #endif - if ((g.debug&(1< 15000) { + if (debug_option_is_set(DebugOptions::AUTOREBOOT) && AP_HAL::millis() > 15000) { // attempt reboot with HOLD after 15s periph.prepare_reboot(); #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS @@ -432,10 +455,18 @@ void AP_Periph_FW::update() if (now - battery.last_read_ms >= 100) { // update battery at 10Hz battery.last_read_ms = now; - battery.lib.read(); + battery_lib.read(); } #endif +#ifdef HAL_PERIPH_ENABLE_RCIN + rcin_update(); +#endif + +#ifdef HAL_PERIPH_ENABLE_BATTERY_BALANCE + batt_balance_update(); +#endif + static uint32_t fiftyhz_last_update_ms; if (now - fiftyhz_last_update_ms >= 20) { // update at 50Hz @@ -457,12 +488,23 @@ void AP_Periph_FW::update() temperature_sensor.update(); #endif +#ifdef HAL_PERIPH_ENABLE_RPM + if (now - rpm_last_update_ms >= 100) { + rpm_last_update_ms = now; + rpm_sensor.update(); + } +#endif + #if HAL_LOGGING_ENABLED logger.periodic_tasks(); #endif can_update(); +#ifdef HAL_PERIPH_ENABLE_NETWORKING + networking_periph.update(); +#endif + #if (defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY) && HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY == 8) || defined(HAL_PERIPH_ENABLE_NOTIFY) update_rainbow(); #endif @@ -503,8 +545,8 @@ void AP_Periph_FW::check_for_serial_reboot_cmd(const int8_t serial_index) const char reboot_string_len = sizeof(reboot_string)-1; // -1 is to remove the null termination static uint16_t index[hal.num_serial]; - const int16_t data = uart->read(); - if (data < 0 || data > 0xff) { + uint8_t data; + if (!uart->read(data)) { // read error continue; } diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index d9fa52efb06d9..b0e0839d6f08c 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include @@ -13,17 +14,28 @@ #include #include #include +#include #include #include #include #include "../AP_Bootloader/app_comms.h" #include #include "hwing_esc.h" -#include +#include +#include #include #include #include - +#include +#include +#include +#if HAL_WITH_ESC_TELEM +#include +#endif +#include +#include "rc_in.h" +#include "batt_balance.h" +#include "networking.h" #include #if HAL_NMEA_OUTPUT_ENABLED && !(HAL_GCS_ENABLED && defined(HAL_PERIPH_ENABLE_GPS)) @@ -35,6 +47,8 @@ #include "GCS_MAVLink.h" #endif +#include "esc_apd_telem.h" + #if defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NCP5623_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NCP5623_BGR_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_TOSHIBA_LED_WITHOUT_NOTIFY) #define AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY #endif @@ -54,6 +68,23 @@ #endif #endif +#ifndef AP_PERIPH_SAFETY_SWITCH_ENABLED +#define AP_PERIPH_SAFETY_SWITCH_ENABLED defined(HAL_PERIPH_ENABLE_RC_OUT) +#endif + +#ifndef HAL_PERIPH_CAN_MIRROR +#define HAL_PERIPH_CAN_MIRROR 0 +#endif + +#if defined(HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT) && !defined(HAL_DEBUG_BUILD) && !defined(HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_NON_DEBUG) +/* this checking for reboot can lose bytes on GPS modules and other + * serial devices. It is really only relevent on a debug build if you + * really want it for non-debug build then define + * HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_NON_DEBUG in hwdef.dat + */ +#undef HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT +#endif + #include "Parameters.h" #if CONFIG_HAL_BOARD == HAL_BOARD_SITL @@ -69,6 +100,23 @@ extern "C" { void can_printf(const char *fmt, ...) FMT_PRINTF(1,2); } +struct CanardInstance; +struct CanardRxTransfer; + +#define MAKE_TRANSFER_DESCRIPTOR(data_type_id, transfer_type, src_node_id, dst_node_id) \ + (((uint32_t)(data_type_id)) | (((uint32_t)(transfer_type)) << 16U) | \ + (((uint32_t)(src_node_id)) << 18U) | (((uint32_t)(dst_node_id)) << 25U)) + +#ifndef HAL_CAN_POOL_SIZE +#if HAL_CANFD_SUPPORTED + #define HAL_CAN_POOL_SIZE 16000 +#elif GPS_MOVING_BASELINE + #define HAL_CAN_POOL_SIZE 8000 +#else + #define HAL_CAN_POOL_SIZE 4000 +#endif +#endif + class AP_Periph_FW { public: AP_Periph_FW(); @@ -98,7 +146,11 @@ class AP_Periph_FW { void can_airspeed_update(); void can_rangefinder_update(); void can_battery_update(); + void can_battery_send_cells(uint8_t instance); void can_proximity_update(); + void can_buzzer_update(void); + void can_safety_button_update(void); + void can_safety_LED_update(void); void load_parameters(); void prepare_reboot(); @@ -147,11 +199,15 @@ class AP_Periph_FW { AP_Baro baro; #endif -#ifdef HAL_PERIPH_ENABLE_BATTERY - struct AP_Periph_Battery { - void handle_battery_failsafe(const char* type_str, const int8_t action) { } - AP_BattMonitor lib{0, FUNCTOR_BIND_MEMBER(&AP_Periph_FW::AP_Periph_Battery::handle_battery_failsafe, void, const char*, const int8_t), nullptr}; +#ifdef HAL_PERIPH_ENABLE_RPM + AP_RPM rpm_sensor; + uint32_t rpm_last_update_ms; +#endif +#ifdef HAL_PERIPH_ENABLE_BATTERY + void handle_battery_failsafe(const char* type_str, const int8_t action) { } + AP_BattMonitor battery_lib{0, FUNCTOR_BIND_MEMBER(&AP_Periph_FW::handle_battery_failsafe, void, const char*, const int8_t), nullptr}; + struct { uint32_t last_read_ms; uint32_t last_can_send_ms; } battery; @@ -161,7 +217,7 @@ class AP_Periph_FW { // This allows you to change the protocol and it continues to use the one at boot. // Without this, changing away from UAVCAN causes loss of comms and you can't // change the rest of your params or verify it succeeded. - AP_CANManager::Driver_Type can_protocol_cached[HAL_NUM_CAN_IFACES]; + AP_CAN::Protocol can_protocol_cached[HAL_NUM_CAN_IFACES]; #endif #ifdef HAL_PERIPH_ENABLE_MSP @@ -189,6 +245,7 @@ class AP_Periph_FW { struct { mavlink_message_t msg; mavlink_status_t status; + uint32_t last_heartbeat_ms; } adsb; #endif @@ -201,7 +258,7 @@ class AP_Periph_FW { uint32_t last_sample_ms; #endif -#ifdef HAL_PERIPH_ENABLE_PRX +#ifdef HAL_PERIPH_ENABLE_PROXIMITY AP_Proximity proximity; #endif @@ -227,7 +284,16 @@ class AP_Periph_FW { AP_EFI efi; uint32_t efi_update_ms; #endif + +#if AP_KDECAN_ENABLED + AP_KDECAN kdecan; +#endif +#ifdef HAL_PERIPH_ENABLE_ESC_APD + ESC_APD_Telem *apd_esc_telem[APD_ESC_INSTANCES]; + void apd_esc_telem_update(); +#endif + #ifdef HAL_PERIPH_ENABLE_RC_OUT #if HAL_WITH_ESC_TELEM AP_ESC_Telem esc_telem; @@ -251,6 +317,21 @@ class AP_Periph_FW { void rcout_handle_safety_state(uint8_t safety_state); #endif +#ifdef HAL_PERIPH_ENABLE_RCIN + void rcin_init(); + void rcin_update(); + void can_send_RCInput(uint8_t quality, uint16_t *values, uint8_t nvalues, bool in_failsafe, bool quality_valid); + bool rcin_initialised; + uint32_t rcin_last_sent_RCInput_ms; + const char *rcin_rc_protocol; // protocol currently being decoded + Parameters_RCIN g_rcin; +#endif + +#ifdef HAL_PERIPH_ENABLE_BATTERY_BALANCE + void batt_balance_update(); + BattBalance battery_balance; +#endif + #if AP_TEMPERATURE_SENSOR_ENABLED AP_TemperatureSensor temperature_sensor; #endif @@ -283,6 +364,14 @@ class AP_Periph_FW { AP_Logger logger; #endif +#ifdef HAL_PERIPH_ENABLE_NETWORKING + Networking_Periph networking_periph; +#endif + +#ifdef HAL_PERIPH_ENABLE_RTC + AP_RTC rtc; +#endif + #if HAL_GCS_ENABLED GCS_Periph _gcs; #endif @@ -291,33 +380,140 @@ class AP_Periph_FW { static const AP_Param::Info var_info[]; +#ifdef HAL_PERIPH_ENABLE_EFI + uint32_t last_efi_update_ms; +#endif +#ifdef HAL_PERIPH_ENABLE_MAG uint32_t last_mag_update_ms; +#endif +#ifdef HAL_PERIPH_ENABLE_GPS uint32_t last_gps_update_ms; uint32_t last_gps_yaw_ms; +#endif uint32_t last_relposheading_ms; +#ifdef HAL_PERIPH_ENABLE_BARO uint32_t last_baro_update_ms; +#endif +#ifdef HAL_PERIPH_ENABLE_AIRSPEED uint32_t last_airspeed_update_ms; +#endif +#ifdef HAL_PERIPH_ENABLE_GPS bool saw_gps_lock_once; +#endif static AP_Periph_FW *_singleton; - enum { - DEBUG_SHOW_STACK, - DEBUG_AUTOREBOOT + enum class DebugOptions { + SHOW_STACK = 0, + AUTOREBOOT = 1, + ENABLE_STATS = 2, }; + // check if an option is set + bool debug_option_is_set(const DebugOptions option) const { + return (uint8_t(g.debug.get()) & (1U<delay(10); periph.prepare_reboot(); diff --git a/Tools/AP_Periph/GCS_MAVLink.h b/Tools/AP_Periph/GCS_MAVLink.h index 3d89cca938da5..ce1f17b1ba2ed 100644 --- a/Tools/AP_Periph/GCS_MAVLink.h +++ b/Tools/AP_Periph/GCS_MAVLink.h @@ -31,12 +31,11 @@ class GCS_MAVLINK_Periph : public GCS_MAVLINK uint32_t telem_delay() const override { return 0; } void handleMessage(const mavlink_message_t &msg) override { handle_common_message(msg); } bool handle_guided_request(AP_Mission::Mission_Command &cmd) override { return true; } - MAV_RESULT handle_preflight_reboot(const mavlink_command_long_t &packet, const mavlink_message_t &msg) override; + MAV_RESULT handle_preflight_reboot(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; + uint8_t sysid_my_gcs() const override; protected: - uint8_t sysid_my_gcs() const override; - // Periph information: MAV_MODE base_mode() const override { return (MAV_MODE)MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; } MAV_STATE vehicle_system_status() const override { return MAV_STATE_CALIBRATING; } diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index ea6a4dfe1c18d..27c985a3bf621 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -43,6 +43,10 @@ extern const AP_HAL::HAL &hal; #define AP_PERIPH_BARO_ENABLE_DEFAULT 1 #endif +#ifndef HAL_PERIPH_BATT_HIDE_MASK_DEFAULT +#define HAL_PERIPH_BATT_HIDE_MASK_DEFAULT 0 +#endif + #ifndef AP_PERIPH_EFI_PORT_DEFAULT #define AP_PERIPH_EFI_PORT_DEFAULT 3 #endif @@ -57,6 +61,14 @@ extern const AP_HAL::HAL &hal; #define MAV_SYSTEM_ID HAL_DEFAULT_MAV_SYSTEM_ID #endif +#ifndef APD_ESC_SERIAL_0 + #define APD_ESC_SERIAL_0 -1 +#endif + +#ifndef APD_ESC_SERIAL_1 + #define APD_ESC_SERIAL_1 -1 +#endif + /* * AP_Periph parameter definitions * @@ -95,48 +107,56 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GSCALAR(can_slcan_cport, "CAN_SLCAN_CPORT", 1), #endif +#ifdef HAL_GPIO_PIN_GPIO_CAN1_TERM + // @Param: CAN_TERMINATE + // @DisplayName: Enable CAN software temination in this node + // @Description: Enable CAN software temination in this node + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + // @RebootRequired: True + GARRAY(can_terminate, 0, "CAN_TERMINATE", 0), +#endif + #if HAL_NUM_CAN_IFACES >= 2 // @Param: CAN_PROTOCOL // @DisplayName: Enable use of specific protocol to be used on this port // @Description: Enabling this option starts selected protocol that will use this virtual driver. At least one CAN port must be UAVCAN or else CAN1 gets set to UAVCAN - // @Values: 0:Disabled,1:UAVCAN,4:PiccoloCAN,5:CANTester,6:EFI_NWPMU,7:USD1,8:KDECAN + // @Values: 0:Disabled,1:UAVCAN,4:PiccoloCAN,6:EFI_NWPMU,7:USD1,8:KDECAN // @User: Advanced // @RebootRequired: True - GARRAY(can_protocol, 0, "CAN_PROTOCOL", AP_CANManager::Driver_Type_UAVCAN), + GARRAY(can_protocol, 0, "CAN_PROTOCOL", float(AP_CAN::Protocol::DroneCAN)), // @Param: CAN2_BAUDRATE + // @CopyFieldsFrom: CAN_BAUDRATE // @DisplayName: Bitrate of CAN2 interface - // @Description: Bit rate can be set up to from 10000 to 1000000 - // @Range: 10000 1000000 - // @User: Advanced - // @RebootRequired: True GARRAY(can_baudrate, 1, "CAN2_BAUDRATE", 1000000), // @Param: CAN2_PROTOCOL - // @DisplayName: Enable use of specific protocol to be used on this port - // @Description: Enabling this option starts selected protocol that will use this virtual driver. At least one CAN port must be UAVCAN or else CAN1 gets set to UAVCAN - // @Values: 0:Disabled,1:UAVCAN,4:PiccoloCAN,5:CANTester,6:EFI_NWPMU,7:USD1,8:KDECAN - // @User: Advanced - // @RebootRequired: True - GARRAY(can_protocol, 1, "CAN2_PROTOCOL", AP_CANManager::Driver_Type_UAVCAN), + // @CopyFieldsFrom: CAN_PROTOCOL + GARRAY(can_protocol, 1, "CAN2_PROTOCOL", float(AP_CAN::Protocol::DroneCAN)), + +#ifdef HAL_GPIO_PIN_GPIO_CAN2_TERM + // @Param: CAN2_TERMINATE + // @CopyFieldsFrom: CAN_TERMINATE + GARRAY(can_terminate, 1, "CAN2_TERMINATE", 0), +#endif #endif #if HAL_NUM_CAN_IFACES >= 3 // @Param: CAN3_BAUDRATE // @DisplayName: Bitrate of CAN3 interface - // @Description: Bit rate can be set up to from 10000 to 1000000 - // @Range: 10000 1000000 - // @User: Advanced - // @RebootRequired: True + // @CopyFieldsFrom: CAN_BAUDRATE GARRAY(can_baudrate, 2, "CAN3_BAUDRATE", 1000000), // @Param: CAN3_PROTOCOL - // @DisplayName: Enable use of specific protocol to be used on this port - // @Description: Enabling this option starts selected protocol that will use this virtual driver. At least one CAN port must be UAVCAN or else CAN1 gets set to UAVCAN - // @Values: 0:Disabled,1:UAVCAN,4:PiccoloCAN,5:CANTester,6:EFI_NWPMU,7:USD1,8:KDECAN - // @User: Advanced - // @RebootRequired: True - GARRAY(can_protocol, 2, "CAN3_PROTOCOL", AP_CANManager::Driver_Type_UAVCAN), + // @CopyFieldsFrom: CAN_PROTOCOL + GARRAY(can_protocol, 2, "CAN3_PROTOCOL", float(AP_CAN::Protocol::DroneCAN)), + +#ifdef HAL_GPIO_PIN_GPIO_CAN3_TERM + // @Param: CAN3_TERMINATE + // @CopyFieldsFrom: CAN_TERMINATE + GARRAY(can_terminate, 2, "CAN3_TERMINATE", 0), +#endif #endif #if HAL_CANFD_SUPPORTED @@ -158,11 +178,9 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { #if HAL_NUM_CAN_IFACES >= 2 // @Param: CAN2_FDBAUDRATE + // @CopyFieldsFrom: CAN_FDBAUDRATE // @DisplayName: Set up bitrate for data section on CAN2 // @Description: This sets the bitrate for the data section of CAN2. - // @Values: 1:1M, 2:2M, 4:4M, 5:5M, 8:8M - // @User: Advanced - // @RebootRequired: True GARRAY(can_fdbaudrate, 1, "CAN2_FDBAUDRATE", HAL_CANFD_SUPPORTED), #endif #endif @@ -179,7 +197,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { // @Param: DEBUG // @DisplayName: Debug // @Description: Debug - // @Bitmask: 0:Disabled, 1:Show free stack space, 2:Auto Reboot after 15sec + // @Bitmask: 0:Show free stack space, 1:Auto Reboot after 15sec, 2:Enable sending stats // @User: Advanced GSCALAR(debug, "DEBUG", 0), @@ -231,7 +249,14 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { #ifdef HAL_PERIPH_ENABLE_BATTERY // @Group: BATT // @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp - GOBJECT(battery, "BATT", AP_BattMonitor), + GOBJECT(battery_lib, "BATT", AP_BattMonitor), + + // @Param: BATT_HIDE_MASK + // @DisplayName: Battery hide mask + // @Description: Instance mask of local battery index(es) to prevent transmitting their status over CAN. This is useful for hiding a "battery" instance that is used locally in the peripheral but don't want them to be treated as a battery source(s) to the autopilot. For example, an AP_Periph battery monitor with multiple batteries that monitors each locally for diagnostic or other purposes, but only reports as a single SUM battery monitor to the autopilot. + // @Bitmask: 0:BATT, 1:BATT2, 2:BATT3, 3:BATT4, 4:BATT5, 5:BATT6, 6:BATT7, 7:BATT8, 8:BATT9, 9:BATTA, 10:BATTB, 11:BATTC, 12:BATTD, 13:BATTE, 14:BATTF, 15:BATTG + // @User: Advanced + GSCALAR(battery_hide_mask, "BATT_HIDE_MASK", HAL_PERIPH_BATT_HIDE_MASK_DEFAULT), #endif #ifdef HAL_PERIPH_ENABLE_MAG @@ -343,13 +368,13 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GSCALAR(hardpoint_rate, "HARDPOINT_RATE", 100), #endif -#ifdef HAL_PERIPH_ENABLE_HWESC +#if defined(HAL_PERIPH_ENABLE_HWESC) || defined(HAL_PERIPH_ENABLE_ESC_APD) // @Param: ESC_NUMBER // @DisplayName: ESC number // @Description: This is the ESC number to report as in UAVCAN ESC telemetry feedback packets. // @Increment: 1 // @User: Advanced - GSCALAR(esc_number, "ESC_NUMBER", 0), + GARRAY(esc_number, 0, "ESC_NUMBER", 0), #endif #ifdef HAL_PERIPH_ENABLE_RC_OUT @@ -440,7 +465,9 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { // @Range: 1 255 // @User: Advanced GSCALAR(sysid_this_mav, "SYSID_THISMAV", MAV_SYSTEM_ID), +#endif +#if HAL_GCS_ENABLED || defined(HAL_PERIPH_SHOW_SERIAL_MANAGER_PARAMS) // @Group: SERIAL // @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp GOBJECT(serial_manager, "SERIAL", AP_SerialManager), @@ -483,7 +510,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GOBJECT(efi, "EFI", AP_EFI), #endif -#ifdef HAL_PERIPH_ENABLE_PRX +#ifdef HAL_PERIPH_ENABLE_PROXIMITY // @Param: PRX_BAUDRATE // @DisplayName: Proximity Sensor serial baudrate // @Description: Proximity Sensor serial baudrate. @@ -513,9 +540,9 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { // Proximity driver // @Group: PRX - // @Path: ../libraries/AP_RangeFinder/AP_Proximity.cpp + // @Path: ../libraries/AP_Proximity/AP_Proximity.cpp GOBJECT(proximity, "PRX", AP_Proximity), -#endif +#endif // HAL_PERIPH_ENABLE_PROXIMITY #if HAL_NMEA_OUTPUT_ENABLED // @Group: NMEA_ @@ -523,6 +550,94 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GOBJECT(nmea, "NMEA_", AP_NMEA_Output), #endif +#if AP_KDECAN_ENABLED + // @Group: KDE_ + // @Path: ../libraries/AP_KDECAN/AP_KDECAN.cpp + GOBJECT(kdecan, "KDE_", AP_KDECAN), +#endif + +#if defined(HAL_PERIPH_ENABLE_ESC_APD) + GARRAY(pole_count, 0, "ESC_NUM_POLES", 22), +#endif + +#if defined(HAL_PERIPH_ENABLE_ESC_APD) + // @Param: ESC_APD_SERIAL_1 + // @DisplayName: ESC APD Serial 1 + // @Description: Which serial port to use for APD ESC data + // @Range: 0 6 + // @Increment: 1 + // @User: Advanced + // @RebootRequired: True + GARRAY(esc_serial_port, 0, "ESC_APD_SERIAL_1", APD_ESC_SERIAL_0), + + #if APD_ESC_INSTANCES > 1 + GARRAY(esc_number, 1, "ESC_NUMBER2", 1), + + GARRAY(pole_count, 1, "ESC_NUM_POLES2", 22), + + // @Param: ESC_APD_SERIAL_2 + // @DisplayName: ESC APD Serial 2 + // @Description: Which serial port to use for APD ESC data + // @Range: 0 6 + // @Increment: 1 + // @User: Advanced + // @RebootRequired: True + GARRAY(esc_serial_port, 1, "ESC_APD_SERIAL_2", APD_ESC_SERIAL_1), + #endif +#endif + +#ifdef HAL_PERIPH_ENABLE_NETWORKING + // @Group: NET_ + // @Path: networking.cpp + GOBJECT(networking_periph, "NET_", Networking_Periph), +#endif + +#ifdef HAL_PERIPH_ENABLE_RPM + // @Group: RPM + // @Path: ../libraries/AP_RPM/AP_RPM.cpp + GOBJECT(rpm_sensor, "RPM", AP_RPM), +#endif + +#ifdef HAL_PERIPH_ENABLE_RCIN + // @Group: RC + // @Path: rc_in.cpp + GOBJECT(g_rcin, "RC", Parameters_RCIN), +#endif + +#ifdef HAL_PERIPH_ENABLE_BATTERY_BALANCE + // @Group: BAL + // @Path: batt_balance.cpp + GOBJECT(battery_balance, "BAL", BattBalance), +#endif + + // NOTE: sim parameters should go last +#if AP_SIM_ENABLED + // @Group: SIM_ + // @Path: ../libraries/SITL/SITL.cpp + GOBJECT(sitl, "SIM_", SITL::SIM), + +#if AP_AHRS_ENABLED + // @Group: AHRS_ + // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp + GOBJECT(ahrs, "AHRS_", AP_AHRS), +#endif +#endif // AP_SIM_ENABLED + +#if HAL_PERIPH_CAN_MIRROR + // @Param: CAN_MIRROR_PORTS + // @DisplayName: CAN ports to mirror traffic between + // @Description: Any set ports will participate in blindly mirroring traffic from one port to the other. It is the users responsibility to ensure that no loops exist that cause traffic to be infinitly repeated, and both ports must be running the same baud rates. + // @Bitmask: 0:CAN1, 1:CAN2, 2:CAN3 + // @User: Advanced + GSCALAR(can_mirror_ports, "CAN_MIRROR_PORTS", 0), +#endif // HAL_PERIPH_CAN_MIRROR + +#ifdef HAL_PERIPH_ENABLE_RTC + // @Group: RTC + // @Path: ../libraries/AP_RTC/AP_RTC.cpp + GOBJECT(rtc, "RTC", AP_RTC), +#endif + AP_VAREND }; diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index 63322b10d467c..ff64f16d1e950 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -30,8 +30,8 @@ class Parameters { k_param_hardpoint_id, k_param_hardpoint_rate, k_param_baro_enable, - k_param_esc_number, - k_param_battery, + k_param_esc_number0, + k_param_battery_lib, k_param_debug, k_param_serial_number, k_param_adsb_port, @@ -70,6 +70,24 @@ class Parameters { k_param_proximity_port, k_param_proximity_max_rate, k_param_nmea, + k_param_kdecan, + k_param_pole_count0, + k_param_esc_serial_port0, + k_param_esc_number1, + k_param_pole_count1, + k_param_esc_serial_port1, + k_param_networking_periph, + k_param_rpm_sensor, + k_param_g_rcin, + k_param_sitl, + k_param_ahrs, + k_param_battery_balance, + k_param_battery_hide_mask, + k_param_can_mirror_ports, + k_param_rtc, + k_param_can_terminate0, + k_param_can_terminate1, + k_param_can_terminate2, }; AP_Int16 format_version; @@ -77,7 +95,7 @@ class Parameters { AP_Int32 can_baudrate[HAL_NUM_CAN_IFACES]; #if HAL_NUM_CAN_IFACES >= 2 - AP_Enum can_protocol[HAL_NUM_CAN_IFACES]; + AP_Enum can_protocol[HAL_NUM_CAN_IFACES]; #endif #if AP_CAN_SLCAN_ENABLED @@ -103,7 +121,7 @@ class Parameters { AP_Int16 rangefinder_max_rate; #endif -#ifdef HAL_PERIPH_ENABLE_PRX +#ifdef HAL_PERIPH_ENABLE_PROXIMITY AP_Int32 proximity_baud; AP_Int8 proximity_port; AP_Int16 proximity_max_rate; @@ -120,8 +138,21 @@ class Parameters { AP_Int8 hardpoint_rate; #endif -#ifdef HAL_PERIPH_ENABLE_HWESC - AP_Int8 esc_number; +#if defined(HAL_PERIPH_ENABLE_HWESC) || defined(HAL_PERIPH_ENABLE_ESC_APD) + #if defined ESC_NUMBERS + #error "ESC_NUMBERS should not have been previously defined" + #endif + #if defined(APD_ESC_INSTANCES) + #define ESC_NUMBERS APD_ESC_INSTANCES + #else + #define ESC_NUMBERS 2 + #endif // defined(APD_ESC_INSTANCES) + AP_Int8 esc_number[ESC_NUMBERS]; + AP_Int8 esc_serial_port[ESC_NUMBERS]; +#endif + +#if defined(ESC_NUMBERS) + AP_Int8 pole_count[ESC_NUMBERS]; #endif #ifdef HAL_PERIPH_ENABLE_GPS @@ -158,10 +189,18 @@ class Parameters { AP_Int16 sysid_this_mav; #endif +#ifdef HAL_PERIPH_ENABLE_BATTERY + AP_Int32 battery_hide_mask; +#endif + #ifdef HAL_PERIPH_ENABLE_EFI AP_Int32 efi_baudrate; AP_Int8 efi_port; #endif + +#if HAL_PERIPH_CAN_MIRROR + AP_Int8 can_mirror_ports; +#endif // HAL_PERIPH_CAN_MIRROR #if HAL_CANFD_SUPPORTED AP_Int8 can_fdmode; @@ -169,6 +208,10 @@ class Parameters { #else static constexpr uint8_t can_fdmode = 0; #endif + + AP_Int8 can_terminate[HAL_NUM_CAN_IFACES]; + + AP_Int8 node_stats; Parameters() {} }; diff --git a/Tools/AP_Periph/README.md b/Tools/AP_Periph/README.md index 90e5a62cd05b0..67aba113d923d 100644 --- a/Tools/AP_Periph/README.md +++ b/Tools/AP_Periph/README.md @@ -1,42 +1,50 @@ -# AP_Periph UAVCAN Peripheral Firmware +# AP_Periph DroneCAN Peripheral Firmware -This is an ArduPilot based UAVCAN peripheral firmware. This firmware +This is an ArduPilot based DroneCAN peripheral firmware. This firmware takes advantage of the wide range of sensor drivers in ArduPilot to -make building a UAVCAN peripheral firmware easy. +make building a DroneCAN peripheral firmware easy. The AP_Periph firmware is based on the same ChibiOS hwdef.dat system that is used to define pinouts for STM32 based flight controllers supported by ArduPilot. That means you can add support for a new -UAVCAN peripheral based on the STM32 by just writing a simple +DroneCAN peripheral based on the STM32 by just writing a simple hwdef.dat that defines the pinout of your device. -Currently we have four targets building for AP_Periph firmwares: +We have over 60 build targets building for AP_Periph firmwares. All +ArduPilot supported MCUs can be used, including: - - A STM32F103 128k flash part made by mRobotics (target f103-GPS) - - A STM32F412 512k flash part made by CUAV (target CUAV_GPS) - - A STM32F105 256k flash part (used in ZubaxGNSSv2) - - A STM32F303 256k flash part made by mRobotics (target f303-GPS) + - STM32F1xx + - STM32F3xx + - STM32F4xx + - STM32F7xx + - STM32H7xx + - STM32L4xx + - STM32G4xx More can be added using the hwdef.dat system # Features The AP_Periph firmware can be configured to enable a wide range of -UAVCAN sensor types. Support is included for: +DroneCAN sensor types. Support is included for: - GPS modules (including RTK GPS) - Magnetometers (SPI or I2C) - Barometers (SPI or I2C) - Airspeed sensors (I2C) - Rangefinders (UART or I2C) - - ADSB (Ping ADSB receiver on UART) + - ADSB (uAvionix compatible Ping ADSB receiver on UART) - Battery Monitor (Analog, I2C/SMBus, UART) - LEDs (GPIO, I2C or WS2812 serial) - Safety LED and Safety Switch - Buzzer (tonealarm or simple GPIO) - RC Output (All standard RCOutput protocols) + - RC input + - battery balance monitor + - EFI engines + - Proximity sensors -An AP_Periph UAVCAN firmware supports these UAVCAN features: +An AP_Periph DroneCAN firmware supports these DroneCAN features: - dynamic or static CAN node allocation - firmware upload @@ -56,8 +64,8 @@ Using f103-GPS as an example, build the main firmware like this: - ./waf AP_Periph that will build a file build/f103-GPS/bin/AP_Periph.bin. You can -now load that using the CAN bootloader and either uavcan_gui_tool or -MissionPlanner SLCAN support. +now load that using the CAN bootloader and either dronecan_gui_tool or +MissionPlanner DroneCAN support. # Flashing @@ -87,95 +95,32 @@ the resulting bootloader will be in Tools/bootloaders Firmware targets are automatically built and distributed on the ArduPilot firmware server on firmware.ardupilot.org. These firmwares -can be loaded using Mission Planner or the UAVCAN GUI Tool. Parameters -for peripherals can be changed using the Mission Planner SLCAN support -or using UAVCAN GUI Tools. +can be loaded using Mission Planner or the DroneCAN GUI Tool. Parameters +for peripherals can be changed using the Mission Planner DroneCAN support +or using DroneCAN GUI Tools. # User Bootloader Update The bootloader is automatically stored in ROMFS in the main -firmware. End users can update the bootloader by setting the UAVCAN +firmware. End users can update the bootloader by setting the DroneCAN parameter "FLASH_BOOTLOADER" to 1. After setting it to 1 the node will -respond with a debug text message which can be seen in the UAVCAN GUI +respond with a debug text message which can be seen in the DroneCAN GUI tool to show the result of the flash. # SITL Testing -Currently GPS peripheral build is supported under linux environment, -we simulate a UAVCAN GPS Peripheral on SocketCAN. - -Setup can be done as follows, this is on top of usual setup required -to build ardupilot: - -``` -sudo dpkg --add-architecture i386 -sudo apt-get update -sudo apt-get install -y gcc-multilib g++-multilib -sudo apt-get update -sudo apt-get -y install can-utils iproute2 linux-modules-extra-$(uname -r) -sudo modprobe vcan -sudo ip link add dev vcan0 type vcan -sudo ip link set up vcan0 -``` -Build Commands: -``` -./waf configure --board sitl_periph_gps -./waf AP_Periph -``` -Autotest Command: -``` -Tools/autotest/autotest.py -v Copter build.SITLPeriphGPS test.CAN -``` - - ---- -**Note** - -To run valgrind on AP_Periph binary you might need to get 32 bit version of libc6-dbg which can be simply get using following command for Ubuntu machines: `sudo apt-get install libc6-dbg:i386` - ---- - - -https://github.com/linux-can/can-utils contains a nice set of utility to do CAN related testings on Linux system. I used Ubuntu for this development, for Ubuntu systems you can simply download this tool using `sudo apt-get install can-utils` - -Following are the common commands that can be used while testing or developing: -* Create Virtual CAN Interface: -``` -sudo modprobe vcan -sudo ip link add dev vcan0 type vcan -sudo ip link set up vcan0 -sudo ip link add dev vcan1 type vcan -sudo ip link set up vcan1 -``` -* Route one CANSocket to another -``` -sudo modprobe can-gw -sudo cangw -A -s vcan0 -d vcan1 -e -sudo cangw -A -s vcan1 -d vcan0 -e -``` -* Delete routes -``` -sudo cangw -D -s vcan0 -d vcan1 -e -sudo cangw -D -s vcan1 -d vcan0 -e -``` -* Route SLCAN to VCAN, this allows connecting CAN devices to SITL run via CAN Adapter like the one running in Ardupilot itself. -``` -sudo modprobe slcan -sudo modprobe can-gw -sudo slcan_attach -f -s8 -o /dev/ttyACM0 -sudo slcand ttyACM0 slcan0 -sudo ifconfig slcan0 up -sudo cangw -A -s vcan0 -d slcan0 -e -sudo cangw -A -s slcan0 -d vcan0 -e -``` -* Dump can messages: -``` -sudo candump vcan0 -``` +A wide range of DroneCAN peripherals are supported in the SITL +simulation system. The simplest way of starting a DroneCAN enabled +simulated vehicle is to use sim_vehicle.py. + +For a quadplane use: sim_vehicle.py with the option -f quadplane-can + +For a quadcopter use: sim_vehicle.py with the option -f quad-can # Discussion and Feedback Please join the discussions at these locations: - - https://discuss.ardupilot.org/t/ap-periph-1-0-0-stable-released/49049 - - https://gitter.im/ArduPilot/CANBUS + - https://discuss.ardupilot.org/ + - https://ardupilot.org/discord + diff --git a/Tools/AP_Periph/ReleaseNotes.txt b/Tools/AP_Periph/ReleaseNotes.txt index d265eff2d41ca..252044e1efceb 100644 --- a/Tools/AP_Periph/ReleaseNotes.txt +++ b/Tools/AP_Periph/ReleaseNotes.txt @@ -1,3 +1,45 @@ +Release 1.6.0 8th September 2023 +-------------------------------- + +This is a major release with the following changes: + + - much faster CAN bootloader for faster firmware update + - improved handling of peripherals with 2 or more CAN interfaces + - support most AP_Periph features in SITL testing + - added RC input support + - added battery balance plug support + - support sending RPM over DroneCAN + - support for pitot temperature reporting + +Release 1.5.1 23rd July 2023 +--------------------------- + +This is a major release with the following changes: + +- support serial tunnelling over DroneCAN +- raised CAN priority of MovingBaseline data +- support APD ESC telemetry +- support DroneCAN and CAN statistics reporting +- support KDECAN to DroneCAN translation + +The serial tunnelling support allows for uCenter to be used over +DroneCAN with the serial tunnelling panel in the DroneCAN GUI +tool. This allows for monitoring of uBlox GPS over a telemetry link, +and update of F9P firmware over DroneCAN + +Release 1.5.0 27th Mar 2023 +--------------------------- + +This is a major release with the following changes: + +- fixed airspeed bus default +- limit mag to 25Hz by default +- fixed send rate of GPS yaw +- fixed HW ESC telem temp units +- allow set of port for HW telem +- send GNSS heading message if available +- stop sending old GNSS Fix message + Release 1.4.1 27th Sep 2022 --------------------------- diff --git a/Tools/AP_Periph/adsb.cpp b/Tools/AP_Periph/adsb.cpp index 0e44461eb7ff3..1a1229826bee6 100644 --- a/Tools/AP_Periph/adsb.cpp +++ b/Tools/AP_Periph/adsb.cpp @@ -23,21 +23,10 @@ #ifdef HAL_PERIPH_ENABLE_ADSB #include +#include extern const AP_HAL::HAL &hal; -# if !HAL_GCS_ENABLED - -#include "include/mavlink/v2.0/protocol.h" -#include "include/mavlink/v2.0/mavlink_types.h" -#include "include/mavlink/v2.0/all/mavlink.h" -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wmissing-declarations" -#include "include/mavlink/v2.0/mavlink_helpers.h" -#pragma GCC diagnostic pop - -#endif - /* init ADSB support */ @@ -52,7 +41,6 @@ void AP_Periph_FW::adsb_init(void) } } - /* update ADSB subsystem */ @@ -73,7 +61,7 @@ void AP_Periph_FW::adsb_update(void) const uint8_t c = (uint8_t)uart->read(); // Try to get a new message - if (mavlink_parse_char(MAVLINK_COMM_0, c, &adsb.msg, &adsb.status)) { + if (mavlink_frame_char_buffer(&adsb.msg, &adsb.status, c, &adsb.msg, &adsb.status) == MAVLINK_FRAMING_OK) { if (adsb.msg.msgid == MAVLINK_MSG_ID_ADSB_VEHICLE) { // decode and send as UAVCAN TrafficReport static mavlink_adsb_vehicle_t msg; @@ -82,6 +70,82 @@ void AP_Periph_FW::adsb_update(void) } } } + + /* + some ADSB devices need a heartbeat to get the system ID + */ + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - adsb.last_heartbeat_ms >= 1000) { + adsb.last_heartbeat_ms = now_ms; + mavlink_heartbeat_t heartbeat {}; + mavlink_message_t msg; + heartbeat.type = MAV_TYPE_GENERIC; + heartbeat.autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA; + auto len = mavlink_msg_heartbeat_encode_status(1, + MAV_COMP_ID_PERIPHERAL, + &adsb.status, + &msg, &heartbeat); + + uart->write((uint8_t*)&msg.magic, len); + } +} + +/* + map an ADSB_VEHICLE MAVLink message to a UAVCAN TrafficReport message + */ +void AP_Periph_FW::can_send_ADSB(struct __mavlink_adsb_vehicle_t &msg) +{ + ardupilot_equipment_trafficmonitor_TrafficReport pkt {}; + pkt.timestamp.usec = 0; + pkt.icao_address = msg.ICAO_address; + pkt.tslc = msg.tslc; + pkt.latitude_deg_1e7 = msg.lat; + pkt.longitude_deg_1e7 = msg.lon; + pkt.alt_m = msg.altitude * 1e-3; + + pkt.heading = radians(msg.heading * 1e-2); + + pkt.velocity[0] = cosf(pkt.heading) * msg.hor_velocity * 1e-2; + pkt.velocity[1] = sinf(pkt.heading) * msg.hor_velocity * 1e-2; + pkt.velocity[2] = -msg.ver_velocity * 1e-2; + + pkt.squawk = msg.squawk; + memcpy(pkt.callsign, msg.callsign, MIN(sizeof(msg.callsign),sizeof(pkt.callsign))); + if (msg.flags & 0x8000) { + pkt.source = ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_SOURCE_ADSB_UAT; + } else { + pkt.source = ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_SOURCE_ADSB; + } + + pkt.traffic_type = msg.emitter_type; + + if ((msg.flags & ADSB_FLAGS_VALID_ALTITUDE) != 0 && msg.altitude_type == 0) { + pkt.alt_type = ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ALT_TYPE_PRESSURE_AMSL; + } else if ((msg.flags & ADSB_FLAGS_VALID_ALTITUDE) != 0 && msg.altitude_type == 1) { + pkt.alt_type = ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ALT_TYPE_WGS84; + } else { + pkt.alt_type = ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ALT_TYPE_ALT_UNKNOWN; + } + + pkt.lat_lon_valid = (msg.flags & ADSB_FLAGS_VALID_COORDS) != 0; + pkt.heading_valid = (msg.flags & ADSB_FLAGS_VALID_HEADING) != 0; + pkt.velocity_valid = (msg.flags & ADSB_FLAGS_VALID_VELOCITY) != 0; + pkt.callsign_valid = (msg.flags & ADSB_FLAGS_VALID_CALLSIGN) != 0; + pkt.ident_valid = (msg.flags & ADSB_FLAGS_VALID_SQUAWK) != 0; + pkt.simulated_report = (msg.flags & ADSB_FLAGS_SIMULATED) != 0; + + // these flags are not in common.xml + pkt.vertical_velocity_valid = (msg.flags & 0x0080) != 0; + pkt.baro_valid = (msg.flags & 0x0100) != 0; + + uint8_t buffer[ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_MAX_SIZE] {}; + uint16_t total_size = ardupilot_equipment_trafficmonitor_TrafficReport_encode(&pkt, buffer, !periph.canfdout()); + + canard_broadcast(ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_SIGNATURE, + ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ID, + CANARD_TRANSFER_PRIORITY_LOWEST, + &buffer[0], + total_size); } #endif // HAL_PERIPH_ENABLE_ADSB diff --git a/Tools/AP_Periph/airspeed.cpp b/Tools/AP_Periph/airspeed.cpp new file mode 100644 index 0000000000000..17a4da160731c --- /dev/null +++ b/Tools/AP_Periph/airspeed.cpp @@ -0,0 +1,85 @@ +#include "AP_Periph.h" + +#ifdef HAL_PERIPH_ENABLE_AIRSPEED + +/* + airspeed support + */ + +#include + +#ifndef AP_PERIPH_PROBE_CONTINUOUS +#define AP_PERIPH_PROBE_CONTINUOUS 0 +#endif + +/* + update CAN airspeed + */ +void AP_Periph_FW::can_airspeed_update(void) +{ + if (!airspeed.enabled()) { + return; + } +#if AP_PERIPH_PROBE_CONTINUOUS + if (!airspeed.healthy()) { + uint32_t now = AP_HAL::millis(); + static uint32_t last_probe_ms; + if (now - last_probe_ms >= 1000) { + last_probe_ms = now; + airspeed.allocate(); + } + } +#endif + uint32_t now = AP_HAL::millis(); + if (now - last_airspeed_update_ms < 50) { + // max 20Hz data + return; + } + last_airspeed_update_ms = now; + airspeed.update(); + if (!airspeed.healthy()) { + // don't send any data + return; + } + const float press = airspeed.get_corrected_pressure(); + float temp; + if (!airspeed.get_temperature(temp)) { + temp = nanf(""); + } else { + temp = C_TO_KELVIN(temp); + } + + uavcan_equipment_air_data_RawAirData pkt {}; + + // unfilled elements are NaN + pkt.static_pressure = nanf(""); + pkt.static_pressure_sensor_temperature = nanf(""); + pkt.differential_pressure_sensor_temperature = nanf(""); + pkt.pitot_temperature = nanf(""); + + // populate the elements we have + pkt.differential_pressure = press; + pkt.static_air_temperature = temp; + + // if a Pitot tube temperature sensor is available, use it +#if AP_TEMPERATURE_SENSOR_ENABLED + for (uint8_t i=0; i + +/* + update CAN baro + */ +void AP_Periph_FW::can_baro_update(void) +{ + if (!periph.g.baro_enable) { + return; + } + baro.update(); + if (last_baro_update_ms == baro.get_last_update()) { + return; + } + + last_baro_update_ms = baro.get_last_update(); + if (!baro.healthy()) { + // don't send any data + return; + } + const float press = baro.get_pressure(); + const float temp = baro.get_temperature(); + + { + uavcan_equipment_air_data_StaticPressure pkt {}; + pkt.static_pressure = press; + pkt.static_pressure_variance = 0; // should we make this a parameter? + + uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_MAX_SIZE] {}; + uint16_t total_size = uavcan_equipment_air_data_StaticPressure_encode(&pkt, buffer, !periph.canfdout()); + + canard_broadcast(UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_SIGNATURE, + UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); + } + + { + uavcan_equipment_air_data_StaticTemperature pkt {}; + pkt.static_temperature = C_TO_KELVIN(temp); + pkt.static_temperature_variance = 0; // should we make this a parameter? + + uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_MAX_SIZE] {}; + uint16_t total_size = uavcan_equipment_air_data_StaticTemperature_encode(&pkt, buffer, !periph.canfdout()); + + canard_broadcast(UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_SIGNATURE, + UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); + } +} + +#endif // HAL_PERIPH_ENABLE_BARO diff --git a/Tools/AP_Periph/batt_balance.cpp b/Tools/AP_Periph/batt_balance.cpp new file mode 100644 index 0000000000000..86c7c7a11cd7c --- /dev/null +++ b/Tools/AP_Periph/batt_balance.cpp @@ -0,0 +1,138 @@ +/* + 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 . + */ + +#include "AP_Periph.h" + +#ifdef HAL_PERIPH_ENABLE_BATTERY_BALANCE + +#include + +extern const AP_HAL::HAL &hal; + +#ifndef AP_PERIPH_BATTERY_BALANCE_NUMCELLS_DEFAULT +#define AP_PERIPH_BATTERY_BALANCE_NUMCELLS_DEFAULT 0 +#endif + +#ifndef AP_PERIPH_BATTERY_BALANCE_RATE_DEFAULT +#define AP_PERIPH_BATTERY_BALANCE_RATE_DEFAULT 1 +#endif + +#ifndef AP_PERIPH_BATTERY_BALANCE_CELL1_PIN_DEFAULT +#define AP_PERIPH_BATTERY_BALANCE_CELL1_PIN_DEFAULT 1 +#endif + +#ifndef AP_PERIPH_BATTERY_BALANCE_ID_DEFAULT +#define AP_PERIPH_BATTERY_BALANCE_ID_DEFAULT 0 +#endif + + +const AP_Param::GroupInfo BattBalance::var_info[] { + // @Param: _NUM_CELLS + // @DisplayName: Number of battery cells + // @Description: Number of battery cells to monitor + // @Range: 0 64 + AP_GROUPINFO("_NUM_CELLS", 1, BattBalance, num_cells, AP_PERIPH_BATTERY_BALANCE_NUMCELLS_DEFAULT), + + // @Param: _ID + // @DisplayName: Battery ID + // @Description: Battery ID to match against other batteries + // @Range: 0 127 + AP_GROUPINFO("_ID", 2, BattBalance, id, AP_PERIPH_BATTERY_BALANCE_ID_DEFAULT), + + // @Param: _RATE + // @DisplayName: Send Rate + // @Description: Rate to send cell information + // @Range: 0 20 + AP_GROUPINFO("_RATE", 3, BattBalance, rate, AP_PERIPH_BATTERY_BALANCE_RATE_DEFAULT), + + // @Param: _CELL1_PIN + // @DisplayName: First analog pin + // @Description: Analog pin of the first cell. Later cells must be sequential + // @Range: 0 127 + AP_GROUPINFO("_CELL1_PIN", 4, BattBalance, cell1_pin, AP_PERIPH_BATTERY_BALANCE_CELL1_PIN_DEFAULT), + + AP_GROUPEND +}; + +BattBalance::BattBalance(void) +{ + AP_Param::setup_object_defaults(this, var_info); +} + +void AP_Periph_FW::batt_balance_update() +{ + const int8_t ncell = battery_balance.num_cells; + if (ncell <= 0) { + return; + } + + // allocate cell sources if needed + if (battery_balance.cells == nullptr) { + battery_balance.cells = new AP_HAL::AnalogSource*[ncell]; + if (battery_balance.cells == nullptr) { + return; + } + battery_balance.cells_allocated = ncell; + for (uint8_t i=0; ichannel(battery_balance.cell1_pin + i); + } + } + + const uint32_t now = AP_HAL::millis(); + if (now - battery_balance.last_send_ms < 1000.0/battery_balance.rate.get()) { + return; + } + battery_balance.last_send_ms = now; + + // allocate space for the packet. This is a large + // packet that won't fit on the stack, so dynamically allocate + auto *pkt = new ardupilot_equipment_power_BatteryInfoAux; + uint8_t *buffer = new uint8_t[ARDUPILOT_EQUIPMENT_POWER_BATTERYINFOAUX_MAX_SIZE]; + if (pkt == nullptr || buffer == nullptr) { + delete pkt; + delete [] buffer; + return; + } + + pkt->voltage_cell.len = battery_balance.cells_allocated; + float last_cell = 0; + for (uint8_t i=0; ivoltage_average(); + pkt->voltage_cell.data[i] = v - last_cell; + last_cell = v; + } + pkt->max_current = nanf(""); + pkt->nominal_voltage = nanf(""); + pkt->battery_id = uint8_t(battery_balance.id); + + // encode and send message: + const uint16_t total_size = ardupilot_equipment_power_BatteryInfoAux_encode(pkt, buffer, !periph.canfdout()); + + canard_broadcast(ARDUPILOT_EQUIPMENT_POWER_BATTERYINFOAUX_SIGNATURE, + ARDUPILOT_EQUIPMENT_POWER_BATTERYINFOAUX_ID, + CANARD_TRANSFER_PRIORITY_LOW, + buffer, + total_size); + + delete pkt; + delete [] buffer; +} + +#endif // HAL_PERIPH_ENABLE_BATTERY_BALANCE + diff --git a/Tools/AP_Periph/batt_balance.h b/Tools/AP_Periph/batt_balance.h new file mode 100644 index 0000000000000..961875c660ab7 --- /dev/null +++ b/Tools/AP_Periph/batt_balance.h @@ -0,0 +1,24 @@ +#pragma once + +#ifdef HAL_PERIPH_ENABLE_BATTERY_BALANCE + +class BattBalance { +public: + friend class AP_Periph_FW; + BattBalance(void); + + static const struct AP_Param::GroupInfo var_info[]; + +private: + AP_Int8 num_cells; + AP_Int8 id; + AP_Int8 cell1_pin; + AP_Float rate; + uint32_t last_send_ms; + + AP_HAL::AnalogSource **cells; + uint8_t cells_allocated; +}; + +#endif // HAL_PERIPH_ENABLE_BATTERY_BALANCE + diff --git a/Tools/AP_Periph/battery.cpp b/Tools/AP_Periph/battery.cpp new file mode 100644 index 0000000000000..8f07834d22e66 --- /dev/null +++ b/Tools/AP_Periph/battery.cpp @@ -0,0 +1,128 @@ +#include "AP_Periph.h" + +#ifdef HAL_PERIPH_ENABLE_BATTERY + +/* + battery support + */ + +#include + +extern const AP_HAL::HAL &hal; + +#ifndef AP_PERIPH_BATTERY_MODEL_NAME +#define AP_PERIPH_BATTERY_MODEL_NAME CAN_APP_NODE_NAME +#endif + +/* + update CAN battery monitor + */ +void AP_Periph_FW::can_battery_update(void) +{ + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - battery.last_can_send_ms < 100) { + return; + } + battery.last_can_send_ms = now_ms; + + const uint8_t battery_instances = battery_lib.num_instances(); + for (uint8_t i=0; i= 0) ? serial_number : i+1; + + pkt.voltage = battery_lib.voltage(i); + + float current; + if (battery_lib.current_amps(current, i)) { + pkt.current = current; + } + float temperature; + if (battery_lib.get_temperature(temperature, i)) { + // Battery lib reports temperature in Celsius. + // Convert Celsius to Kelvin for transmission on CAN. + pkt.temperature = C_TO_KELVIN(temperature); + } + + pkt.state_of_health_pct = UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATE_OF_HEALTH_UNKNOWN; + uint8_t percentage = 0; + if (battery_lib.capacity_remaining_pct(percentage, i)) { + pkt.state_of_charge_pct = percentage; + } + pkt.model_instance_id = i+1; + +#if !defined(HAL_PERIPH_BATTERY_SKIP_NAME) + // example model_name: "org.ardupilot.ap_periph SN 123" + hal.util->snprintf((char*)pkt.model_name.data, sizeof(pkt.model_name.data), "%s %ld", AP_PERIPH_BATTERY_MODEL_NAME, (long int)serial_number); + pkt.model_name.len = strnlen((char*)pkt.model_name.data, sizeof(pkt.model_name.data)); +#endif //defined(HAL_PERIPH_BATTERY_SKIP_NAME) + + uint8_t buffer[UAVCAN_EQUIPMENT_POWER_BATTERYINFO_MAX_SIZE] {}; + const uint16_t total_size = uavcan_equipment_power_BatteryInfo_encode(&pkt, buffer, !periph.canfdout()); + + canard_broadcast(UAVCAN_EQUIPMENT_POWER_BATTERYINFO_SIGNATURE, + UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); + + // Send individual cell information if available + if (battery_lib.has_cell_voltages(i)) { + can_battery_send_cells(i); + } + } +} + +/* + send individual cell voltages if available + */ +void AP_Periph_FW::can_battery_send_cells(uint8_t instance) +{ + // allocate space for the packet. This is a large + // packet that won't fit on the stack, so dynamically allocate + auto* pkt = new ardupilot_equipment_power_BatteryInfoAux; + uint8_t* buffer = new uint8_t[ARDUPILOT_EQUIPMENT_POWER_BATTERYINFOAUX_MAX_SIZE]; + if (pkt == nullptr || buffer == nullptr) { + delete pkt; + delete [] buffer; + return; + } + 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; + } + pkt->voltage_cell.data[i] = cell_voltages.cells[i]*0.001; + pkt->voltage_cell.len = i+1; + } + + pkt->max_current = nanf(""); + pkt->nominal_voltage = nanf(""); + + // encode and send message: + const uint16_t total_size = ardupilot_equipment_power_BatteryInfoAux_encode(pkt, buffer, !periph.canfdout()); + + canard_broadcast(ARDUPILOT_EQUIPMENT_POWER_BATTERYINFOAUX_SIGNATURE, + ARDUPILOT_EQUIPMENT_POWER_BATTERYINFOAUX_ID, + CANARD_TRANSFER_PRIORITY_LOW, + buffer, + total_size); + + // Delete temporary buffers + delete pkt; + delete [] buffer; +} + +#endif // HAL_PERIPH_ENABLE_BATTERY + diff --git a/Tools/AP_Periph/buzzer.cpp b/Tools/AP_Periph/buzzer.cpp new file mode 100644 index 0000000000000..c5285e3612d37 --- /dev/null +++ b/Tools/AP_Periph/buzzer.cpp @@ -0,0 +1,55 @@ +#include "AP_Periph.h" + +#if defined(HAL_PERIPH_ENABLE_NOTIFY) || defined(HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY) + +/* + buzzer support + */ + +#include + +extern const AP_HAL::HAL &hal; + +static uint32_t buzzer_start_ms; +static uint32_t buzzer_len_ms; + +/* + handle BeepCommand + */ +void AP_Periph_FW::handle_beep_command(CanardInstance* canard_instance, CanardRxTransfer* transfer) +{ + uavcan_equipment_indication_BeepCommand req; + if (uavcan_equipment_indication_BeepCommand_decode(transfer, &req)) { + return; + } + static bool initialised; + if (!initialised) { + initialised = true; + hal.rcout->init(); + hal.util->toneAlarm_init(AP_Notify::Notify_Buzz_Builtin); + } + buzzer_start_ms = AP_HAL::millis(); + buzzer_len_ms = req.duration*1000; +#ifdef HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY + float volume = constrain_float(periph.g.buzz_volume/100.0f, 0, 1); +#elif defined(HAL_PERIPH_ENABLE_NOTIFY) + float volume = constrain_float(periph.notify.get_buzz_volume()/100.0f, 0, 1); +#endif + hal.util->toneAlarm_set_buzzer_tone(req.frequency, volume, uint32_t(req.duration*1000)); +} + +/* + update buzzer + */ +void AP_Periph_FW::can_buzzer_update(void) +{ + if (buzzer_start_ms != 0) { + uint32_t now = AP_HAL::millis(); + if (now - buzzer_start_ms > buzzer_len_ms) { + hal.util->toneAlarm_set_buzzer_tone(0, 0, 0); + buzzer_start_ms = 0; + } + } +} + +#endif // (HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY) || (HAL_PERIPH_ENABLE_NOTIFY) diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 85a9519a8797e..89611c5cac547 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -20,8 +20,6 @@ #include #include #include "AP_Periph.h" -#include -#include #include #include #include @@ -38,7 +36,7 @@ #include #endif -#define IFACE_ALL ((1U<<(HAL_NUM_CAN_IFACES+1U))-1U) +#define IFACE_ALL ((1U<<(HAL_NUM_CAN_IFACES))-1U) #include "i2c.h" #include @@ -47,7 +45,6 @@ #include #endif - #if CONFIG_HAL_BOARD == HAL_BOARD_SITL extern const HAL_SITL &hal; #else @@ -56,14 +53,6 @@ extern const AP_HAL::HAL &hal; extern AP_Periph_FW periph; -#ifndef HAL_CAN_POOL_SIZE -#if HAL_CANFD_SUPPORTED - #define HAL_CAN_POOL_SIZE 16000 -#else - #define HAL_CAN_POOL_SIZE 4000 -#endif -#endif - #ifndef HAL_PERIPH_LOOP_DELAY_US // delay between can loop updates. This needs to be longer on F4 #if defined(STM32H7) @@ -73,17 +62,13 @@ extern AP_Periph_FW periph; #endif #endif -#ifndef AP_PERIPH_MAG_MAX_RATE -#define AP_PERIPH_MAG_MAX_RATE 25U -#endif - -#define DEBUG_PRINTS 0 #define DEBUG_PKTS 0 -#if DEBUG_PRINTS - # define Debug(fmt, args ...) do {can_printf(fmt "\n", ## args);} while(0) -#else - # define Debug(fmt, args ...) -#endif + +#if HAL_PERIPH_CAN_MIRROR + #ifndef HAL_PERIPH_CAN_MIRROR_QUEUE_SIZE + #define HAL_PERIPH_CAN_MIRROR_QUEUE_SIZE 64 + #endif +#endif //HAL_PERIPH_CAN_MIRROR #ifndef HAL_PERIPH_SUPPORT_LONG_CAN_PRINTF // When enabled, can_printf() strings longer than the droneCAN max text length (90 chars) @@ -102,27 +87,18 @@ static struct instance_t { #elif CONFIG_HAL_BOARD == HAL_BOARD_SITL HALSITL::CANIface* iface; #endif + +#if HAL_PERIPH_CAN_MIRROR + #if HAL_NUM_CAN_IFACES < 2 + #error "Can't use HAL_PERIPH_CAN_MIRROR if there are not at least 2 HAL_NUM_CAN_IFACES" + #endif + ObjectBuffer *mirror_queue; + uint8_t mirror_fail_count; +#endif // HAL_PERIPH_CAN_MIRROR } instances[HAL_NUM_CAN_IFACES]; -static struct dronecan_protocol_t { - CanardInstance canard; - uint32_t canard_memory_pool[HAL_CAN_POOL_SIZE/sizeof(uint32_t)]; - struct tid_map { - uint32_t transfer_desc; - uint8_t tid; - tid_map *next; - } *tid_map_head; - /* - * Variables used for dynamic node ID allocation. - * RTFM at http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation - */ - uint32_t send_next_node_id_allocation_request_at_ms; ///< When the next node ID allocation request should be sent - uint8_t node_id_allocation_unique_id_offset; ///< Depends on the stage of the next request - uint8_t tx_fail_count; - uint8_t dna_interface = 1; -} dronecan; - -#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && defined(HAL_GPIO_PIN_TERMCAN1) + +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && defined(HAL_GPIO_PIN_TERMCAN1) && (HAL_NUM_CAN_IFACES >= 2) static ioline_t can_term_lines[] = { HAL_GPIO_PIN_TERMCAN1 @@ -145,19 +121,11 @@ HAL_GPIO_PIN_TERMCAN1 }; #endif // CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && defined(HAL_GPIO_PIN_TERMCAN1) -#ifndef CAN_APP_NODE_NAME -#define CAN_APP_NODE_NAME "org.ardupilot.ap_periph" -#endif - #ifndef HAL_CAN_DEFAULT_NODE_ID #define HAL_CAN_DEFAULT_NODE_ID CANARD_BROADCAST_NODE_ID #endif uint8_t PreferredNodeID = HAL_CAN_DEFAULT_NODE_ID; -#ifndef AP_PERIPH_BATTERY_MODEL_NAME -#define AP_PERIPH_BATTERY_MODEL_NAME CAN_APP_NODE_NAME -#endif - #ifndef AP_PERIPH_PROBE_CONTINUOUS #define AP_PERIPH_PROBE_CONTINUOUS 0 #endif @@ -180,7 +148,9 @@ SLCAN::CANIface AP_Periph_FW::slcan_interface; * Node status variables */ static uavcan_protocol_NodeStatus node_status; - +#if HAL_ENABLE_SENDING_STATS +static dronecan_protocol_Stats protocol_stats; +#endif /** * Returns a pseudo random integer in a given range */ @@ -204,13 +174,13 @@ static void readUniqueID(uint8_t* out_uid) /* handle a GET_NODE_INFO request */ -static void handle_get_node_info(CanardInstance* ins, - CanardRxTransfer* transfer) +void AP_Periph_FW::handle_get_node_info(CanardInstance* canard_instance, + CanardRxTransfer* transfer) { uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE] {}; uavcan_protocol_GetNodeInfoResponse pkt {}; - node_status.uptime_sec = AP_HAL::native_millis() / 1000U; + node_status.uptime_sec = AP_HAL::millis() / 1000U; pkt.status = node_status; pkt.software_version.major = AP::fwversion().major; @@ -228,16 +198,16 @@ static void handle_get_node_info(CanardInstance* ins, pkt.hardware_version.major = APJ_BOARD_ID >> 8; pkt.hardware_version.minor = APJ_BOARD_ID & 0xFF; - if (periph.g.serial_number > 0) { - hal.util->snprintf((char*)pkt.name.data, sizeof(pkt.name.data), "%s(%u)", CAN_APP_NODE_NAME, (unsigned)periph.g.serial_number); + if (g.serial_number > 0) { + hal.util->snprintf((char*)pkt.name.data, sizeof(pkt.name.data), "%s(%u)", CAN_APP_NODE_NAME, (unsigned)g.serial_number); } else { hal.util->snprintf((char*)pkt.name.data, sizeof(pkt.name.data), "%s", CAN_APP_NODE_NAME); } pkt.name.len = strnlen((char*)pkt.name.data, sizeof(pkt.name.data)); - uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer, !periph.canfdout()); + uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer, !canfdout()); - const int16_t resp_res = canardRequestOrRespond(ins, + const int16_t resp_res = canardRequestOrRespond(canard_instance, transfer->source_node_id, UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, UAVCAN_PROTOCOL_GETNODEINFO_ID, @@ -250,7 +220,7 @@ static void handle_get_node_info(CanardInstance* ins, , IFACE_ALL #endif #if HAL_CANFD_SUPPORTED - , periph.canfdout() + , canfdout() #endif ); if (resp_res <= 0) { @@ -261,7 +231,7 @@ static void handle_get_node_info(CanardInstance* ins, /* handle parameter GetSet request */ -static void handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer) +void AP_Periph_FW::handle_param_getset(CanardInstance* canard_instance, CanardRxTransfer* transfer) { // param fetch all can take a long time, so pat watchdog stm32_watchdog_pat(); @@ -344,9 +314,9 @@ static void handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer) } uint8_t buffer[UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_MAX_SIZE] {}; - uint16_t total_size = uavcan_protocol_param_GetSetResponse_encode(&pkt, buffer, !periph.canfdout()); + uint16_t total_size = uavcan_protocol_param_GetSetResponse_encode(&pkt, buffer, !canfdout()); - canardRequestOrRespond(ins, + canardRequestOrRespond(canard_instance, transfer->source_node_id, UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE, UAVCAN_PROTOCOL_PARAM_GETSET_ID, @@ -359,7 +329,7 @@ static void handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer) , IFACE_ALL #endif #if HAL_CANFD_SUPPORTED - ,periph.canfdout() + ,canfdout() #endif ); @@ -368,7 +338,7 @@ static void handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer) /* handle parameter executeopcode request */ -static void handle_param_executeopcode(CanardInstance* ins, CanardRxTransfer* transfer) +void AP_Periph_FW::handle_param_executeopcode(CanardInstance* canard_instance, CanardRxTransfer* transfer) { uavcan_protocol_param_ExecuteOpcodeRequest req; if (uavcan_protocol_param_ExecuteOpcodeRequest_decode(transfer, &req)) { @@ -380,22 +350,22 @@ static void handle_param_executeopcode(CanardInstance* ins, CanardRxTransfer* tr AP_Param::load_all(); AP_Param::setup_sketch_defaults(); #ifdef HAL_PERIPH_ENABLE_GPS - AP_Param::setup_object_defaults(&periph.gps, periph.gps.var_info); + AP_Param::setup_object_defaults(&gps, gps.var_info); #endif #ifdef HAL_PERIPH_ENABLE_BATTERY - AP_Param::setup_object_defaults(&periph.battery, periph.battery.lib.var_info); + AP_Param::setup_object_defaults(&battery, battery_lib.var_info); #endif #ifdef HAL_PERIPH_ENABLE_MAG - AP_Param::setup_object_defaults(&periph.compass, periph.compass.var_info); + AP_Param::setup_object_defaults(&compass, compass.var_info); #endif #ifdef HAL_PERIPH_ENABLE_BARO - AP_Param::setup_object_defaults(&periph.baro, periph.baro.var_info); + AP_Param::setup_object_defaults(&baro, baro.var_info); #endif #ifdef HAL_PERIPH_ENABLE_AIRSPEED - AP_Param::setup_object_defaults(&periph.airspeed, periph.airspeed.var_info); + AP_Param::setup_object_defaults(&airspeed, airspeed.var_info); #endif #ifdef HAL_PERIPH_ENABLE_RANGEFINDER - AP_Param::setup_object_defaults(&periph.rangefinder, periph.rangefinder.var_info); + AP_Param::setup_object_defaults(&rangefinder, rangefinder.var_info); #endif } @@ -404,9 +374,9 @@ static void handle_param_executeopcode(CanardInstance* ins, CanardRxTransfer* tr pkt.ok = true; uint8_t buffer[UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_MAX_SIZE] {}; - uint16_t total_size = uavcan_protocol_param_ExecuteOpcodeResponse_encode(&pkt, buffer, !periph.canfdout()); + uint16_t total_size = uavcan_protocol_param_ExecuteOpcodeResponse_encode(&pkt, buffer, !canfdout()); - canardRequestOrRespond(ins, + canardRequestOrRespond(canard_instance, transfer->source_node_id, UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE, UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID, @@ -419,20 +389,12 @@ static void handle_param_executeopcode(CanardInstance* ins, CanardRxTransfer* tr , IFACE_ALL #endif #if HAL_CANFD_SUPPORTED - ,periph.canfdout() + ,canfdout() #endif ); } -static void canard_broadcast(uint64_t data_type_signature, - uint16_t data_type_id, - uint8_t priority, - const void* payload, - uint16_t payload_len); -static void processTx(void); -static void processRx(void); - -static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* transfer) +void AP_Periph_FW::handle_begin_firmware_update(CanardInstance* canard_instance, CanardRxTransfer* transfer) { #if HAL_RAM_RESERVE_START >= 256 // setup information on firmware request at start of ram @@ -440,28 +402,24 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* memset(comms, 0, sizeof(struct app_bootloader_comms)); comms->magic = APP_BOOTLOADER_COMMS_MAGIC; - // manual decoding due to TAO bug in libcanard generated code - if (transfer->payload_len < 1 || transfer->payload_len > sizeof(comms->path)+1) { + uavcan_protocol_file_BeginFirmwareUpdateRequest req; + if (uavcan_protocol_file_BeginFirmwareUpdateRequest_decode(transfer, &req)) { return; } - uint32_t offset = 0; - canardDecodeScalar(transfer, 0, 8, false, (void*)&comms->server_node_id); - offset += 8; - for (uint8_t i=0; ipayload_len-1; i++) { - canardDecodeScalar(transfer, offset, 8, false, (void*)&comms->path[i]); - offset += 8; - } + + comms->server_node_id = req.source_node_id; if (comms->server_node_id == 0) { comms->server_node_id = transfer->source_node_id; } - comms->my_node_id = canardGetLocalNodeID(ins); + memcpy(comms->path, req.image_file_remote_path.path.data, req.image_file_remote_path.path.len); + comms->my_node_id = canardGetLocalNodeID(canard_instance); uint8_t buffer[UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_MAX_SIZE] {}; uavcan_protocol_file_BeginFirmwareUpdateResponse reply {}; reply.error = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_OK; - uint32_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, buffer, !periph.canfdout()); - canardRequestOrRespond(ins, + uint32_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, buffer, !canfdout()); + canardRequestOrRespond(canard_instance, transfer->source_node_id, UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE, UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID, @@ -474,7 +432,7 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* ,IFACE_ALL #endif #if HAL_CANFD_SUPPORTED - ,periph.canfdout() + ,canfdout() #endif ); uint8_t count = 50; @@ -486,18 +444,18 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* // instant reboot, with backup register used to give bootloader // the node_id - periph.prepare_reboot(); + prepare_reboot(); #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS - set_fast_reboot((rtc_boot_magic)(RTC_BOOT_CANBL | canardGetLocalNodeID(ins))); + set_fast_reboot((rtc_boot_magic)(RTC_BOOT_CANBL | canardGetLocalNodeID(canard_instance))); NVIC_SystemReset(); #endif } -static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* transfer) +void AP_Periph_FW::handle_allocation_response(CanardInstance* canard_instance, CanardRxTransfer* transfer) { // Rule C - updating the randomized time interval dronecan.send_next_node_id_allocation_request_at_ms = - AP_HAL::native_millis() + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS + + AP_HAL::millis() + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS + get_random_range(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS); if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID) @@ -531,19 +489,19 @@ static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* tr printf("Matching allocation response: %d\n", msg.unique_id.len); } else { // Allocation complete - copying the allocated node ID from the message - canardSetLocalNodeID(ins, msg.node_id); + canardSetLocalNodeID(canard_instance, msg.node_id); printf("IF%d Node ID allocated: %d\n", dronecan.dna_interface, msg.node_id); #if defined(HAL_PERIPH_ENABLE_GPS) && (HAL_NUM_CAN_IFACES >= 2) && GPS_MOVING_BASELINE - if (periph.g.gps_mb_only_can_port) { + if (g.gps_mb_only_can_port) { // we need to assign the unallocated port to be used for Moving Baseline only - periph.gps_mb_can_port = (dronecan.dna_interface+1)%HAL_NUM_CAN_IFACES; + gps_mb_can_port = (dronecan.dna_interface+1)%HAL_NUM_CAN_IFACES; if (canardGetLocalNodeID(&dronecan.canard) == CANARD_BROADCAST_NODE_ID) { // copy node id from the primary iface canardSetLocalNodeID(&dronecan.canard, msg.node_id); #ifdef HAL_GPIO_PIN_TERMCAN1 // also terminate the line as we don't have any other device on this port - palWriteLine(can_term_lines[periph.gps_mb_can_port], 1); + palWriteLine(can_term_lines[gps_mb_can_port], 1); #endif } } @@ -551,48 +509,6 @@ static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* tr } } -#if defined(HAL_PERIPH_ENABLE_NOTIFY) || defined(HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY) -static uint32_t buzzer_start_ms; -static uint32_t buzzer_len_ms; -/* - handle BeepCommand - */ -static void handle_beep_command(CanardInstance* ins, CanardRxTransfer* transfer) -{ - uavcan_equipment_indication_BeepCommand req; - if (uavcan_equipment_indication_BeepCommand_decode(transfer, &req)) { - return; - } - static bool initialised; - if (!initialised) { - initialised = true; - hal.rcout->init(); - hal.util->toneAlarm_init(AP_Notify::Notify_Buzz_Builtin); - } - buzzer_start_ms = AP_HAL::native_millis(); - buzzer_len_ms = req.duration*1000; -#ifdef HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY - float volume = constrain_float(periph.g.buzz_volume/100.0f, 0, 1); -#elif defined(HAL_PERIPH_ENABLE_NOTIFY) - float volume = constrain_float(periph.notify.get_buzz_volume()/100.0f, 0, 1); -#endif - hal.util->toneAlarm_set_buzzer_tone(req.frequency, volume, uint32_t(req.duration*1000)); -} - -/* - update buzzer - */ -static void can_buzzer_update(void) -{ - if (buzzer_start_ms != 0) { - uint32_t now = AP_HAL::native_millis(); - if (now - buzzer_start_ms > buzzer_len_ms) { - hal.util->toneAlarm_set_buzzer_tone(0, 0, 0); - buzzer_start_ms = 0; - } - } -} -#endif // (HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY) || (HAL_PERIPH_ENABLE_NOTIFY) #if defined(HAL_GPIO_PIN_SAFE_LED) || defined(HAL_PERIPH_ENABLE_RC_OUT) static uint8_t safety_state; @@ -600,15 +516,15 @@ static uint8_t safety_state; /* handle SafetyState */ -static void handle_safety_state(CanardInstance* ins, CanardRxTransfer* transfer) +void AP_Periph_FW::handle_safety_state(CanardInstance* canard_instance, CanardRxTransfer* transfer) { ardupilot_indication_SafetyState req; if (ardupilot_indication_SafetyState_decode(transfer, &req)) { return; } safety_state = req.status; -#ifdef HAL_PERIPH_ENABLE_RC_OUT - periph.rcout_handle_safety_state(safety_state); +#if AP_PERIPH_SAFETY_SWITCH_ENABLED + rcout_handle_safety_state(safety_state); #endif } #endif // HAL_GPIO_PIN_SAFE_LED @@ -616,7 +532,7 @@ static void handle_safety_state(CanardInstance* ins, CanardRxTransfer* transfer) /* handle ArmingStatus */ -static void handle_arming_status(CanardInstance* ins, CanardRxTransfer* transfer) +void AP_Periph_FW::handle_arming_status(CanardInstance* canard_instance, CanardRxTransfer* transfer) { uavcan_equipment_safety_ArmingStatus req; if (uavcan_equipment_safety_ArmingStatus_decode(transfer, &req)) { @@ -625,44 +541,15 @@ static void handle_arming_status(CanardInstance* ins, CanardRxTransfer* transfer hal.util->set_soft_armed(req.status == UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_STATUS_FULLY_ARMED); } -#ifdef HAL_PERIPH_ENABLE_GPS -/* - handle gnss::RTCMStream - */ -static void handle_RTCMStream(CanardInstance* ins, CanardRxTransfer* transfer) -{ - uavcan_equipment_gnss_RTCMStream req; - if (uavcan_equipment_gnss_RTCMStream_decode(transfer, &req)) { - return; - } - periph.gps.handle_gps_rtcm_fragment(0, req.data.data, req.data.len); -} - -/* - handle gnss::MovingBaselineData -*/ -#if GPS_MOVING_BASELINE -static void handle_MovingBaselineData(CanardInstance* ins, CanardRxTransfer* transfer) -{ - ardupilot_gnss_MovingBaselineData msg; - if (ardupilot_gnss_MovingBaselineData_decode(transfer, &msg)) { - return; - } - periph.gps.inject_MBL_data(msg.data.data, msg.data.len); - Debug("MovingBaselineData: len=%u\n", msg.data.len); -} -#endif // GPS_MOVING_BASELINE - -#endif // HAL_PERIPH_ENABLE_GPS #if defined(AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NOTIFY) -static void set_rgb_led(uint8_t red, uint8_t green, uint8_t blue) +void AP_Periph_FW::set_rgb_led(uint8_t red, uint8_t green, uint8_t blue) { #ifdef HAL_PERIPH_ENABLE_NOTIFY - periph.notify.handle_rgb(red, green, blue); + notify.handle_rgb(red, green, blue); #ifdef HAL_PERIPH_ENABLE_RC_OUT - periph.rcout_has_new_data_to_update = true; + rcout_has_new_data_to_update = true; #endif // HAL_PERIPH_ENABLE_RC_OUT #endif // HAL_PERIPH_ENABLE_NOTIFY @@ -741,7 +628,7 @@ static void set_rgb_led(uint8_t red, uint8_t green, uint8_t blue) /* handle lightscommand */ -static void handle_lightscommand(CanardInstance* ins, CanardRxTransfer* transfer) +void AP_Periph_FW::handle_lightscommand(CanardInstance* canard_instance, CanardRxTransfer* transfer) { uavcan_equipment_indication_LightsCommand req; if (uavcan_equipment_indication_LightsCommand_decode(transfer, &req)) { @@ -755,9 +642,9 @@ static void handle_lightscommand(CanardInstance* ins, CanardRxTransfer* transfer uint8_t green = (cmd.color.green>>1U)<<3U; uint8_t blue = cmd.color.blue<<3U; #ifdef HAL_PERIPH_ENABLE_NOTIFY - const int8_t brightness = periph.notify.get_rgb_led_brightness_percent(); + const int8_t brightness = notify.get_rgb_led_brightness_percent(); #elif defined(AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY) - const int8_t brightness = periph.g.led_brightness; + const int8_t brightness = g.led_brightness; #endif if (brightness != 100 && brightness >= 0) { const float scale = brightness * 0.01; @@ -771,57 +658,34 @@ static void handle_lightscommand(CanardInstance* ins, CanardRxTransfer* transfer #endif // AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY #ifdef HAL_PERIPH_ENABLE_RC_OUT -static void handle_esc_rawcommand(CanardInstance* ins, CanardRxTransfer* transfer) +void AP_Periph_FW::handle_esc_rawcommand(CanardInstance* canard_instance, CanardRxTransfer* transfer) { uavcan_equipment_esc_RawCommand cmd; if (uavcan_equipment_esc_RawCommand_decode(transfer, &cmd)) { return; } - periph.rcout_esc(cmd.cmd.data, cmd.cmd.len); + rcout_esc(cmd.cmd.data, cmd.cmd.len); // Update internal copy for disabling output to ESC when CAN packets are lost - periph.last_esc_num_channels = cmd.cmd.len; - periph.last_esc_raw_command_ms = AP_HAL::millis(); + last_esc_num_channels = cmd.cmd.len; + last_esc_raw_command_ms = AP_HAL::millis(); } -static void handle_act_command(CanardInstance* ins, CanardRxTransfer* transfer) +void AP_Periph_FW::handle_act_command(CanardInstance* canard_instance, CanardRxTransfer* transfer) { - // manual decoding due to TAO bug in libcanard generated code - if (transfer->payload_len < 1 || transfer->payload_len > UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_MAX_SIZE+1) { + uavcan_equipment_actuator_ArrayCommand cmd; + if (uavcan_equipment_actuator_ArrayCommand_decode(transfer, &cmd)) { return; } - const uint8_t data_count = (transfer->payload_len / UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_MAX_SIZE); - uavcan_equipment_actuator_Command data[data_count] {}; - - uint32_t offset = 0; - for (uint8_t i=0; i 100) { last_update_ms = now; static uint8_t led_counter; @@ -882,11 +746,11 @@ static void can_safety_LED_update(void) /* update safety button */ -static void can_safety_button_update(void) +void AP_Periph_FW::can_safety_button_update(void) { static uint32_t last_update_ms; static uint8_t counter; - uint32_t now = AP_HAL::native_millis(); + uint32_t now = AP_HAL::millis(); // send at 10Hz when pressed if (palReadLine(HAL_GPIO_PIN_SAFE_BUTTON) != HAL_SAFE_BUTTON_ON) { counter = 0; @@ -905,51 +769,56 @@ static void can_safety_button_update(void) pkt.press_time = counter; uint8_t buffer[ARDUPILOT_INDICATION_BUTTON_MAX_SIZE] {}; - uint16_t total_size = ardupilot_indication_Button_encode(&pkt, buffer, !periph.canfdout()); + uint16_t total_size = ardupilot_indication_Button_encode(&pkt, buffer, !canfdout()); canard_broadcast(ARDUPILOT_INDICATION_BUTTON_SIGNATURE, - ARDUPILOT_INDICATION_BUTTON_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); + ARDUPILOT_INDICATION_BUTTON_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); } #endif // HAL_GPIO_PIN_SAFE_BUTTON /** * This callback is invoked by the library when a new message or request or response is received. */ -static void onTransferReceived(CanardInstance* ins, - CanardRxTransfer* transfer) +void AP_Periph_FW::onTransferReceived(CanardInstance* canard_instance, + CanardRxTransfer* transfer) { #ifdef HAL_GPIO_PIN_LED_CAN1 palToggleLine(HAL_GPIO_PIN_LED_CAN1); #endif +#if HAL_CANFD_SUPPORTED + // enable tao for decoding when not on CANFD + transfer->tao = !transfer->canfd; +#endif + /* * Dynamic node ID allocation protocol. * Taking this branch only if we don't have a node ID, ignoring otherwise. */ - if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID) { + if (canardGetLocalNodeID(canard_instance) == CANARD_BROADCAST_NODE_ID) { if (transfer->transfer_type == CanardTransferTypeBroadcast && transfer->data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID) { - handle_allocation_response(ins, transfer); + handle_allocation_response(canard_instance, transfer); } return; } switch (transfer->data_type_id) { case UAVCAN_PROTOCOL_GETNODEINFO_ID: - handle_get_node_info(ins, transfer); + handle_get_node_info(canard_instance, transfer); break; case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID: - handle_begin_firmware_update(ins, transfer); + handle_begin_firmware_update(canard_instance, transfer); break; case UAVCAN_PROTOCOL_RESTARTNODE_ID: printf("RestartNode\n"); hal.scheduler->delay(10); - periph.prepare_reboot(); + prepare_reboot(); #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS NVIC_SystemReset(); #elif CONFIG_HAL_BOARD == HAL_BOARD_SITL @@ -958,65 +827,81 @@ static void onTransferReceived(CanardInstance* ins, break; case UAVCAN_PROTOCOL_PARAM_GETSET_ID: - handle_param_getset(ins, transfer); + handle_param_getset(canard_instance, transfer); break; case UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID: - handle_param_executeopcode(ins, transfer); + handle_param_executeopcode(canard_instance, transfer); break; #if defined(HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY) || defined (HAL_PERIPH_ENABLE_NOTIFY) case UAVCAN_EQUIPMENT_INDICATION_BEEPCOMMAND_ID: - handle_beep_command(ins, transfer); + handle_beep_command(canard_instance, transfer); break; #endif #if defined(HAL_GPIO_PIN_SAFE_LED) || defined(HAL_PERIPH_ENABLE_RC_OUT) case ARDUPILOT_INDICATION_SAFETYSTATE_ID: - handle_safety_state(ins, transfer); + handle_safety_state(canard_instance, transfer); break; #endif case UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_ID: - handle_arming_status(ins, transfer); + handle_arming_status(canard_instance, transfer); break; #ifdef HAL_PERIPH_ENABLE_GPS case UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_ID: - handle_RTCMStream(ins, transfer); + handle_RTCMStream(canard_instance, transfer); break; #if GPS_MOVING_BASELINE case ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID: - handle_MovingBaselineData(ins, transfer); + handle_MovingBaselineData(canard_instance, transfer); break; #endif +#endif // HAL_PERIPH_ENABLE_GPS + +#if AP_UART_MONITOR_ENABLED + case UAVCAN_TUNNEL_TARGETTED_ID: + handle_tunnel_Targetted(canard_instance, transfer); + break; #endif - + #if defined(AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NOTIFY) case UAVCAN_EQUIPMENT_INDICATION_LIGHTSCOMMAND_ID: - handle_lightscommand(ins, transfer); + handle_lightscommand(canard_instance, transfer); break; #endif #ifdef HAL_PERIPH_ENABLE_RC_OUT case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID: - handle_esc_rawcommand(ins, transfer); + handle_esc_rawcommand(canard_instance, transfer); break; case UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_ID: - handle_act_command(ins, transfer); + handle_act_command(canard_instance, transfer); break; #endif #ifdef HAL_PERIPH_ENABLE_NOTIFY case ARDUPILOT_INDICATION_NOTIFYSTATE_ID: - handle_notify_state(ins, transfer); + handle_notify_state(canard_instance, transfer); break; #endif } } +/** + * This callback is invoked by the library when a new message or request or response is received. + */ +static void onTransferReceived_trampoline(CanardInstance* canard_instance, + CanardRxTransfer* transfer) +{ + AP_Periph_FW *fw = (AP_Periph_FW *)canard_instance->user_reference; + fw->onTransferReceived(canard_instance, transfer); +} + /** * This callback is invoked by the library when it detects beginning of a new transfer on the bus that can be received @@ -1025,15 +910,15 @@ static void onTransferReceived(CanardInstance* ins, * If the callback returns false, the library will ignore the transfer. * All transfers that are addressed to other nodes are always ignored. */ -static bool shouldAcceptTransfer(const CanardInstance* ins, - uint64_t* out_data_type_signature, - uint16_t data_type_id, - CanardTransferType transfer_type, - uint8_t source_node_id) +bool AP_Periph_FW::shouldAcceptTransfer(const CanardInstance* canard_instance, + uint64_t* out_data_type_signature, + uint16_t data_type_id, + CanardTransferType transfer_type, + uint8_t source_node_id) { (void)source_node_id; - if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID) + if (canardGetLocalNodeID(canard_instance) == CANARD_BROADCAST_NODE_ID) { /* * If we're in the process of allocation of dynamic node ID, accept only relevant transfers. @@ -1091,7 +976,14 @@ static bool shouldAcceptTransfer(const CanardInstance* ins, *out_data_type_signature = ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE; return true; #endif +#endif // HAL_PERIPH_ENABLE_GPS + +#if AP_UART_MONITOR_ENABLED + case UAVCAN_TUNNEL_TARGETTED_ID: + *out_data_type_signature = UAVCAN_TUNNEL_TARGETTED_SIGNATURE; + return true; #endif + #ifdef HAL_PERIPH_ENABLE_RC_OUT case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID: *out_data_type_signature = UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE; @@ -1113,16 +1005,22 @@ static bool shouldAcceptTransfer(const CanardInstance* ins, return false; } -static void cleanup_stale_transactions(uint64_t ×tamp_usec) +static bool shouldAcceptTransfer_trampoline(const CanardInstance* canard_instance, + uint64_t* out_data_type_signature, + uint16_t data_type_id, + CanardTransferType transfer_type, + uint8_t source_node_id) { - canardCleanupStaleTransfers(&dronecan.canard, timestamp_usec); + AP_Periph_FW *fw = (AP_Periph_FW *)canard_instance->user_reference; + return fw->shouldAcceptTransfer(canard_instance, out_data_type_signature, data_type_id, transfer_type, source_node_id); } -#define MAKE_TRANSFER_DESCRIPTOR(data_type_id, transfer_type, src_node_id, dst_node_id) \ - (((uint32_t)(data_type_id)) | (((uint32_t)(transfer_type)) << 16U) | \ - (((uint32_t)(src_node_id)) << 18U) | (((uint32_t)(dst_node_id)) << 25U)) +void AP_Periph_FW::cleanup_stale_transactions(uint64_t ×tamp_usec) +{ + canardCleanupStaleTransfers(&dronecan.canard, timestamp_usec); +} -static uint8_t* get_tid_ptr(uint32_t transfer_desc) +uint8_t *AP_Periph_FW::get_tid_ptr(uint32_t transfer_desc) { // check head if (!dronecan.tid_map_head) { @@ -1156,24 +1054,22 @@ static uint8_t* get_tid_ptr(uint32_t transfer_desc) return &tid_map_ptr->next->tid; } -static void canard_broadcast(uint64_t data_type_signature, - uint16_t data_type_id, - uint8_t priority, - const void* payload, - uint16_t payload_len) +bool AP_Periph_FW::canard_broadcast(uint64_t data_type_signature, + uint16_t data_type_id, + uint8_t priority, + const void* payload, + uint16_t payload_len) { if (canardGetLocalNodeID(&dronecan.canard) == CANARD_BROADCAST_NODE_ID) { - return; + return false; } uint8_t *tid_ptr = get_tid_ptr(MAKE_TRANSFER_DESCRIPTOR(data_type_signature, data_type_id, 0, CANARD_BROADCAST_NODE_ID)); if (tid_ptr == nullptr) { - return; + return false; } -#if DEBUG_PKTS - const int16_t res = -#endif - canardBroadcast(&dronecan.canard, + + const int16_t res = canardBroadcast(&dronecan.canard, data_type_signature, data_type_id, tid_ptr, @@ -1184,7 +1080,7 @@ static void canard_broadcast(uint64_t data_type_signature, , IFACE_ALL // send over all ifaces #endif #if HAL_CANFD_SUPPORTED - , periph.canfdout() + , canfdout() #endif ); @@ -1193,11 +1089,19 @@ static void canard_broadcast(uint64_t data_type_signature, can_printf("Tx error %d\n", res); } #endif +#if HAL_ENABLE_SENDING_STATS + if (res <= 0) { + protocol_stats.tx_errors++; + } else { + protocol_stats.tx_frames += res; + } +#endif + return res > 0; } -static void processTx(void) +void AP_Periph_FW::processTx(void) { - for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&dronecan.canard)) != NULL;) { + for (CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&dronecan.canard)) != NULL;) { AP_HAL::CANFrame txmsg {}; txmsg.dlc = AP_HAL::CANFrame::dataLengthToDlc(txf->data_len); memcpy(txmsg.data, txf->data, txf->data_len); @@ -1207,24 +1111,40 @@ static void processTx(void) #endif // push message with 1s timeout bool sent = true; - const uint64_t deadline = AP_HAL::native_micros64() + 1000000; + const uint64_t now_us = AP_HAL::micros64(); + const uint64_t deadline = now_us + 1000000U; // try sending to all interfaces - for (auto &ins : instances) { - if (ins.iface == NULL) { + for (auto &_ins : instances) { + if (_ins.iface == NULL) { continue; } #if CANARD_MULTI_IFACE - if (!(txf->iface_mask & (1U<iface_mask & (1U<<_ins.index))) { continue; } #endif #if HAL_NUM_CAN_IFACES >= 2 - if (periph.can_protocol_cached[ins.index] != AP_CANManager::Driver_Type_UAVCAN) { + if (can_protocol_cached[_ins.index] != AP_CAN::Protocol::DroneCAN) { continue; } #endif - if (ins.iface->send(txmsg, deadline, 0) <= 0) { - sent = false; + if (_ins.iface->send(txmsg, deadline, 0) <= 0) { + /* + We were not able to queue the frame for + sending. Only mark the send as failing if the + interface is active. We consider an interface as + active if it has had a successful transmit in the + last 2 seconds + */ + volatile const auto *stats = _ins.iface->get_statistics(); + uint64_t last_transmit_us = stats->last_transmit_us; + if (stats == nullptr || AP_HAL::micros64() - last_transmit_us < 2000000UL) { + sent = false; + } + } else { +#if CANARD_MULTI_IFACE + txf->iface_mask &= ~(1U<<_ins.index); +#endif } } if (sent) { @@ -1236,6 +1156,9 @@ static void processTx(void) if (dronecan.tx_fail_count < 8) { dronecan.tx_fail_count++; } else { +#if HAL_ENABLE_SENDING_STATS + protocol_stats.tx_errors++; +#endif canardPopTxQueue(&dronecan.canard); } break; @@ -1243,7 +1166,52 @@ static void processTx(void) } } -static void processRx(void) +#if HAL_ENABLE_SENDING_STATS +void AP_Periph_FW::update_rx_protocol_stats(int16_t res) +{ + switch (res) { + case CANARD_OK: + protocol_stats.rx_frames++; + break; + case -CANARD_ERROR_OUT_OF_MEMORY: + protocol_stats.rx_error_oom++; + break; + case -CANARD_ERROR_INTERNAL: + protocol_stats.rx_error_internal++; + break; + case -CANARD_ERROR_RX_INCOMPATIBLE_PACKET: + protocol_stats.rx_ignored_not_wanted++; + break; + case -CANARD_ERROR_RX_WRONG_ADDRESS: + protocol_stats.rx_ignored_wrong_address++; + break; + case -CANARD_ERROR_RX_NOT_WANTED: + protocol_stats.rx_ignored_not_wanted++; + break; + case -CANARD_ERROR_RX_MISSED_START: + protocol_stats.rx_error_missed_start++; + break; + case -CANARD_ERROR_RX_WRONG_TOGGLE: + protocol_stats.rx_error_wrong_toggle++; + break; + case -CANARD_ERROR_RX_UNEXPECTED_TID: + protocol_stats.rx_ignored_unexpected_tid++; + break; + case -CANARD_ERROR_RX_SHORT_FRAME: + protocol_stats.rx_error_short_frame++; + break; + case -CANARD_ERROR_RX_BAD_CRC: + protocol_stats.rx_error_bad_crc++; + break; + default: + // mark all other errors as internal + protocol_stats.rx_error_internal++; + break; + } +} +#endif + +void AP_Periph_FW::processRx(void) { AP_HAL::CANFrame rxmsg; for (auto &ins : instances) { @@ -1251,7 +1219,7 @@ static void processRx(void) continue; } #if HAL_NUM_CAN_IFACES >= 2 - if (periph.can_protocol_cached[ins.index] != AP_CANManager::Driver_Type_UAVCAN) { + if (can_protocol_cached[ins.index] != AP_CAN::Protocol::DroneCAN) { continue; } #endif @@ -1270,6 +1238,17 @@ static void processRx(void) if (ins.iface->receive(rxmsg, timestamp, flags) <= 0) { break; } +#if HAL_PERIPH_CAN_MIRROR + for (auto &other_instance : instances) { + if (other_instance.mirror_queue == nullptr) { // we aren't mirroring here, or failed on memory + continue; + } + if (other_instance.index == ins.index) { // don't self add + continue; + } + other_instance.mirror_queue->push(rxmsg); + } +#endif // HAL_PERIPH_CAN_MIRROR rx_frame.data_len = AP_HAL::CANFrame::dlcToDataLength(rxmsg.dlc); memcpy(rx_frame.data, rxmsg.data, rx_frame.data_len); #if HAL_CANFD_SUPPORTED @@ -1279,54 +1258,133 @@ static void processRx(void) #if CANARD_MULTI_IFACE rx_frame.iface_id = ins.index; #endif -#if DEBUG_PKTS - const int16_t res = -#endif - canardHandleRxFrame(&dronecan.canard, &rx_frame, timestamp); -#if DEBUG_PKTS - if (res < 0 && - res != -CANARD_ERROR_RX_NOT_WANTED && - res != -CANARD_ERROR_RX_WRONG_ADDRESS && - res != -CANARD_ERROR_RX_MISSED_START) { - printf("Rx error %d, IF%d %lx: ", res, ins.index, rx_frame.id); - for (uint8_t i = 0; i < rx_frame.data_len; i++) { - printf("%02x", rx_frame.data[i]); + + const int16_t res = canardHandleRxFrame(&dronecan.canard, &rx_frame, timestamp); +#if HAL_ENABLE_SENDING_STATS + if (res == -CANARD_ERROR_RX_MISSED_START) { + // this might remaining frames from a message that we don't accept, so check + uint64_t dummy_signature; + if (shouldAcceptTransfer(&dronecan.canard, + &dummy_signature, + extractDataType(rx_frame.id), + extractTransferType(rx_frame.id), + 1)) { // doesn't matter what we pass here + update_rx_protocol_stats(res); + } else { + protocol_stats.rx_ignored_not_wanted++; } - printf("\n"); + } else { + update_rx_protocol_stats(res); } +#else + (void)res; #endif } } } -static uint16_t pool_peak_percent() +#if HAL_PERIPH_CAN_MIRROR +void AP_Periph_FW::processMirror(void) +{ + const uint64_t deadline = AP_HAL::micros64() + 1000000; + + for (auto &ins : instances) { + if (ins.iface == nullptr || ins.mirror_queue == nullptr) { // can't send on a null interface + continue; + } + + const uint32_t pending = ins.mirror_queue->available(); + for (uint32_t i = 0; i < pending; i++) { // limit how long we can loop + AP_HAL::CANFrame txmsg {}; + + if (!ins.mirror_queue->peek(txmsg)) { + break; + } + + if (ins.iface->send(txmsg, deadline, 0) <= 0) { + if (ins.mirror_fail_count < 8) { + ins.mirror_fail_count++; + } else { + ins.mirror_queue->pop(); + } + break; + } else { + ins.mirror_fail_count = 0; + ins.mirror_queue->pop(); + } + } + } +} +#endif // HAL_PERIPH_CAN_MIRROR + +uint16_t AP_Periph_FW::pool_peak_percent() { const CanardPoolAllocatorStatistics stats = canardGetPoolAllocatorStatistics(&dronecan.canard); const uint16_t peak_percent = (uint16_t)(100U * stats.peak_usage_blocks / stats.capacity_blocks); return peak_percent; } -static void node_status_send(void) +void AP_Periph_FW::node_status_send(void) { - uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE]; - node_status.uptime_sec = AP_HAL::millis() / 1000U; + { + uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE]; + node_status.uptime_sec = AP_HAL::millis() / 1000U; - node_status.vendor_specific_status_code = hal.util->available_memory(); + node_status.vendor_specific_status_code = hal.util->available_memory(); - uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer, !periph.canfdout()); + uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer, !canfdout()); - canard_broadcast(UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, - UAVCAN_PROTOCOL_NODESTATUS_ID, - CANARD_TRANSFER_PRIORITY_LOW, - buffer, - len); + canard_broadcast(UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, + UAVCAN_PROTOCOL_NODESTATUS_ID, + CANARD_TRANSFER_PRIORITY_LOW, + buffer, + len); + } +#if HAL_ENABLE_SENDING_STATS + if (debug_option_is_set(AP_Periph_FW::DebugOptions::ENABLE_STATS)) { + { + uint8_t buffer[DRONECAN_PROTOCOL_STATS_MAX_SIZE]; + uint32_t len = dronecan_protocol_Stats_encode(&protocol_stats, buffer, !canfdout()); + canard_broadcast(DRONECAN_PROTOCOL_STATS_SIGNATURE, + DRONECAN_PROTOCOL_STATS_ID, + CANARD_TRANSFER_PRIORITY_LOWEST, + buffer, + len); + } + for (auto &ins : instances) { + uint8_t buffer[DRONECAN_PROTOCOL_CANSTATS_MAX_SIZE]; + dronecan_protocol_CanStats can_stats; + const AP_HAL::CANIface::bus_stats_t *bus_stats = ins.iface->get_statistics(); + if (bus_stats == nullptr) { + return; + } + can_stats.interface = ins.index; + can_stats.tx_requests = bus_stats->tx_requests; + can_stats.tx_rejected = bus_stats->tx_rejected; + can_stats.tx_overflow = bus_stats->tx_overflow; + can_stats.tx_success = bus_stats->tx_success; + can_stats.tx_timedout = bus_stats->tx_timedout; + can_stats.tx_abort = bus_stats->tx_abort; + can_stats.rx_received = bus_stats->rx_received; + can_stats.rx_overflow = bus_stats->rx_overflow; + can_stats.rx_errors = bus_stats->rx_errors; + can_stats.busoff_errors = bus_stats->num_busoff_err; + uint32_t len = dronecan_protocol_CanStats_encode(&can_stats, buffer, !canfdout()); + canard_broadcast(DRONECAN_PROTOCOL_CANSTATS_SIGNATURE, + DRONECAN_PROTOCOL_CANSTATS_ID, + CANARD_TRANSFER_PRIORITY_LOWEST, + buffer, + len); + } + } +#endif } /** * This function is called at 1 Hz rate from the main loop. */ -static void process1HzTasks(uint64_t timestamp_usec) +void AP_Periph_FW::process1HzTasks(uint64_t timestamp_usec) { /* * Purging transfers that are no longer transmitted. This will occasionally free up some memory. @@ -1353,9 +1411,9 @@ static void process1HzTasks(uint64_t timestamp_usec) node_status_send(); #if !defined(HAL_NO_FLASH_SUPPORT) && !defined(HAL_NO_ROMFS_SUPPORT) - if (periph.g.flash_bootloader.get()) { - const uint8_t flash_bl = periph.g.flash_bootloader.get(); - periph.g.flash_bootloader.set_and_save_ifchanged(0); + if (g.flash_bootloader.get()) { + const uint8_t flash_bl = g.flash_bootloader.get(); + g.flash_bootloader.set_and_save_ifchanged(0); if (flash_bl == 42) { // magic developer value to test watchdog support with main loop lockup while (true) { @@ -1404,12 +1462,12 @@ static void process1HzTasks(uint64_t timestamp_usec) #if 0 // test code for watchdog reset - if (AP_HAL::native_millis() > 15000) { + if (AP_HAL::millis() > 15000) { while (true) ; } #endif #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS - if (AP_HAL::native_millis() > 30000) { + if (AP_HAL::millis() > 30000) { // use RTC to mark that we have been running fine for // 30s. This is used along with watchdog resets to ensure the // user has a chance to load a fixed firmware @@ -1422,14 +1480,15 @@ static void process1HzTasks(uint64_t timestamp_usec) wait for dynamic allocation of node ID */ bool AP_Periph_FW::no_iface_finished_dna = true; -static bool can_do_dna() + +bool AP_Periph_FW::can_do_dna() { if (canardGetLocalNodeID(&dronecan.canard) != CANARD_BROADCAST_NODE_ID) { AP_Periph_FW::no_iface_finished_dna = false; return true; } - const uint32_t now = AP_HAL::native_millis(); + const uint32_t now = AP_HAL::millis(); static uint8_t node_id_allocation_transfer_id = 0; @@ -1477,6 +1536,7 @@ static bool can_do_dna() ,(1U << dronecan.dna_interface) #endif #if HAL_CANFD_SUPPORTED + // always send allocation request as non-FD ,false #endif ); @@ -1493,29 +1553,39 @@ void AP_Periph_FW::can_start() { node_status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK; node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_INITIALIZATION; - node_status.uptime_sec = AP_HAL::native_millis() / 1000U; + node_status.uptime_sec = AP_HAL::millis() / 1000U; if (g.can_node >= 0 && g.can_node < 128) { PreferredNodeID = g.can_node; } #if !defined(HAL_NO_FLASH_SUPPORT) && !defined(HAL_NO_ROMFS_SUPPORT) - periph.g.flash_bootloader.set_and_save_ifchanged(0); + g.flash_bootloader.set_and_save_ifchanged(0); #endif #if AP_PERIPH_ENFORCE_AT_LEAST_ONE_PORT_IS_UAVCAN_1MHz && HAL_NUM_CAN_IFACES >= 2 bool has_uavcan_at_1MHz = false; for (uint8_t i=0; i (HAL_PERIPH_CAN_MIRROR_QUEUE_SIZE); + } +#endif //HAL_PERIPH_CAN_MIRROR #if HAL_NUM_CAN_IFACES >= 2 can_protocol_cached[i] = g.can_protocol[i]; CANSensor::set_periph(i, can_protocol_cached[i], can_iface_periph[i]); @@ -1544,100 +1619,20 @@ void AP_Periph_FW::can_start() instances[slcan_selected_index].iface = (AP_HAL::CANIface*)&slcan_interface; // ensure there's a serial port mapped to SLCAN - if (!periph.serial_manager.have_serial(AP_SerialManager::SerialProtocol_SLCAN, 0)) { - periph.serial_manager.set_protocol_and_baud(SERIALMANAGER_NUM_PORTS-1, AP_SerialManager::SerialProtocol_SLCAN, 1500000); + if (!serial_manager.have_serial(AP_SerialManager::SerialProtocol_SLCAN, 0)) { + serial_manager.set_protocol_and_baud(SERIALMANAGER_NUM_PORTS-1, AP_SerialManager::SerialProtocol_SLCAN, 1500000); } } #endif canardInit(&dronecan.canard, (uint8_t *)dronecan.canard_memory_pool, sizeof(dronecan.canard_memory_pool), - onTransferReceived, shouldAcceptTransfer, NULL); + onTransferReceived_trampoline, shouldAcceptTransfer_trampoline, this); if (PreferredNodeID != CANARD_BROADCAST_NODE_ID) { canardSetLocalNodeID(&dronecan.canard, PreferredNodeID); } } -#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT -void AP_Periph_FW::pwm_hardpoint_init() -{ - hal.gpio->attach_interrupt( - PWM_HARDPOINT_PIN, - FUNCTOR_BIND_MEMBER(&AP_Periph_FW::pwm_irq_handler, void, uint8_t, bool, uint32_t), AP_HAL::GPIO::INTERRUPT_BOTH); - -} - -/* - called on PWM pin transition - */ -void AP_Periph_FW::pwm_irq_handler(uint8_t pin, bool pin_state, uint32_t timestamp) -{ - if (pin_state == 0 && pwm_hardpoint.last_state == 1 && pwm_hardpoint.last_ts_us != 0) { - uint32_t width = timestamp - pwm_hardpoint.last_ts_us; - if (width > 500 && width < 2500) { - pwm_hardpoint.pwm_value = width; - if (width > pwm_hardpoint.highest_pwm) { - pwm_hardpoint.highest_pwm = width; - } - } - } - pwm_hardpoint.last_state = pin_state; - pwm_hardpoint.last_ts_us = timestamp; -} - -void AP_Periph_FW::pwm_hardpoint_update() -{ - uint32_t now = AP_HAL::native_millis(); - // send at 10Hz - void *save = hal.scheduler->disable_interrupts_save(); - uint16_t value = pwm_hardpoint.highest_pwm; - pwm_hardpoint.highest_pwm = 0; - hal.scheduler->restore_interrupts(save); - float rate = g.hardpoint_rate; - rate = constrain_float(rate, 10, 100); - if (value > 0 && now - pwm_hardpoint.last_send_ms >= 1000U/rate) { - pwm_hardpoint.last_send_ms = now; - uavcan_equipment_hardpoint_Command cmd {}; - cmd.hardpoint_id = g.hardpoint_id; - cmd.command = value; - - uint8_t buffer[UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_hardpoint_Command_encode(&cmd, buffer, !periph.canfdout()); - canard_broadcast(UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_SIGNATURE, - UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); - } -} -#endif // HAL_PERIPH_ENABLE_PWM_HARDPOINT - -#ifdef HAL_PERIPH_ENABLE_HWESC -void AP_Periph_FW::hwesc_telem_update() -{ - if (!hwesc_telem.update()) { - return; - } - const HWESC_Telem::HWESC &t = hwesc_telem.get_telem(); - - uavcan_equipment_esc_Status pkt {}; - pkt.esc_index = g.esc_number; - pkt.voltage = t.voltage; - pkt.current = t.current; - pkt.temperature = C_TO_KELVIN(MAX(t.mos_temperature, t.cap_temperature)); - pkt.rpm = t.rpm; - pkt.power_rating_pct = t.phase_current; - pkt.error_count = t.error_count; - - uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer, !periph.canfdout()); - canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, - UAVCAN_EQUIPMENT_ESC_STATUS_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); -} -#endif // HAL_PERIPH_ENABLE_HWESC #ifdef HAL_PERIPH_ENABLE_RC_OUT #if HAL_WITH_ESC_TELEM @@ -1683,7 +1678,7 @@ void AP_Periph_FW::esc_telem_update() pkt.error_count = 0; uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer, !periph.canfdout()); + uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer, !canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, UAVCAN_EQUIPMENT_ESC_STATUS_ID, CANARD_TRANSFER_PRIORITY_LOW, @@ -1692,12 +1687,46 @@ void AP_Periph_FW::esc_telem_update() } } #endif // HAL_WITH_ESC_TELEM -#endif // HAL_PERIPH_ENABLE_RC_OUT +#ifdef HAL_PERIPH_ENABLE_ESC_APD +void AP_Periph_FW::apd_esc_telem_update() +{ + for(uint8_t i = 0; i < ARRAY_SIZE(apd_esc_telem); i++) { + if (apd_esc_telem[i] == nullptr) { + continue; + } + ESC_APD_Telem &esc = *apd_esc_telem[i]; + + if (esc.update()) { + const ESC_APD_Telem::telem &t = esc.get_telem(); + + uavcan_equipment_esc_Status pkt {}; + static_assert(APD_ESC_INSTANCES <= ARRAY_SIZE(g.esc_number), "There must be an ESC instance number for each APD ESC"); + pkt.esc_index = g.esc_number[i]; + pkt.voltage = t.voltage; + pkt.current = t.current; + pkt.temperature = t.temperature; + pkt.rpm = t.rpm; + pkt.power_rating_pct = t.power_rating_pct; + pkt.error_count = t.error_count; + + uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {}; + uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer, !canfdout()); + canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, + UAVCAN_EQUIPMENT_ESC_STATUS_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); + } + } + +} +#endif // HAL_PERIPH_ENABLE_ESC_APD +#endif // HAL_PERIPH_ENABLE_RC_OUT void AP_Periph_FW::can_update() { - const uint32_t now = AP_HAL::native_millis(); + const uint32_t now = AP_HAL::millis(); const uint32_t led_pattern = 0xAAAA; const uint32_t led_change_period = 50; static uint8_t led_idx = 0; @@ -1725,20 +1754,37 @@ void AP_Periph_FW::can_update() static uint32_t last_1Hz_ms; if (now - last_1Hz_ms >= 1000) { last_1Hz_ms = now; - process1HzTasks(AP_HAL::native_micros64()); + process1HzTasks(AP_HAL::micros64()); } #if CONFIG_HAL_BOARD == HAL_BOARD_SITL if (!hal.run_in_maintenance_mode()) #endif { +#ifdef HAL_PERIPH_ENABLE_MAG can_mag_update(); +#endif +#ifdef HAL_PERIPH_ENABLE_GPS can_gps_update(); +#endif +#if AP_UART_MONITOR_ENABLED + send_serial_monitor_data(); +#endif +#ifdef HAL_PERIPH_ENABLE_BATTERY can_battery_update(); +#endif +#ifdef HAL_PERIPH_ENABLE_BARO can_baro_update(); +#endif +#ifdef HAL_PERIPH_ENABLE_AIRSPEED can_airspeed_update(); +#endif +#ifdef HAL_PERIPH_ENABLE_RANGEFINDER can_rangefinder_update(); +#endif +#ifdef HAL_PERIPH_ENABLE_PROXIMITY can_proximity_update(); +#endif #if defined(HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY) || defined (HAL_PERIPH_ENABLE_NOTIFY) can_buzzer_update(); #endif @@ -1754,6 +1800,9 @@ void AP_Periph_FW::can_update() #ifdef HAL_PERIPH_ENABLE_HWESC hwesc_telem_update(); #endif +#ifdef HAL_PERIPH_ENABLE_ESC_APD + apd_esc_telem_update(); +#endif #ifdef HAL_PERIPH_ENABLE_MSP msp_sensor_update(); #endif @@ -1767,920 +1816,20 @@ void AP_Periph_FW::can_update() const uint32_t now_us = AP_HAL::micros(); while ((AP_HAL::micros() - now_us) < 1000) { hal.scheduler->delay_microseconds(HAL_PERIPH_LOOP_DELAY_US); - processTx(); - processRx(); - } -} - -/* - update CAN magnetometer - */ -void AP_Periph_FW::can_mag_update(void) -{ -#ifdef HAL_PERIPH_ENABLE_MAG - if (!compass.available()) { - return; - } - -#if AP_PERIPH_MAG_MAX_RATE > 0 - // don't flood the bus with very high rate magnetometers - const uint32_t now_ms = AP_HAL::millis(); - if (now_ms - last_mag_update_ms < (1000U / AP_PERIPH_MAG_MAX_RATE)) { - return; - } -#endif - compass.read(); -#if AP_PERIPH_PROBE_CONTINUOUS - if (compass.get_count() == 0) { - static uint32_t last_probe_ms; - uint32_t now = AP_HAL::native_millis(); - if (now - last_probe_ms >= 1000) { - last_probe_ms = now; - compass.init(); - } - } +#if HAL_CANFD_SUPPORTED + // allow for user enabling/disabling CANFD at runtime + dronecan.canard.tao_disabled = g.can_fdmode == 1; #endif - if (last_mag_update_ms == compass.last_update_ms()) { - return; - } - if (!compass.healthy()) { - return; - } - - last_mag_update_ms = compass.last_update_ms(); - const Vector3f &field = compass.get_field(); - uavcan_equipment_ahrs_MagneticFieldStrength pkt {}; - - // the canard dsdl compiler doesn't understand float16 - for (uint8_t i=0; i<3; i++) { - pkt.magnetic_field_ga[i] = field[i] * 0.001; + processTx(); + processRx(); +#if HAL_PERIPH_CAN_MIRROR + processMirror(); +#endif // HAL_PERIPH_CAN_MIRROR } - - uint8_t buffer[UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_ahrs_MagneticFieldStrength_encode(&pkt, buffer, !periph.canfdout()); - - canard_broadcast(UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_SIGNATURE, - UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); -#endif // HAL_PERIPH_ENABLE_MAG } -/* - update CAN battery monitor - */ -void AP_Periph_FW::can_battery_update(void) -{ -#ifdef HAL_PERIPH_ENABLE_BATTERY - const uint32_t now_ms = AP_HAL::native_millis(); - if (now_ms - battery.last_can_send_ms < 100) { - return; - } - battery.last_can_send_ms = now_ms; - - const uint8_t battery_instances = battery.lib.num_instances(); - for (uint8_t i=0; i= 0) ? serial_number : i+1; - - pkt.voltage = battery.lib.voltage(i); - - float current; - if (battery.lib.current_amps(current, i)) { - pkt.current = current; - } - float temperature; - if (battery.lib.get_temperature(temperature, i)) { - // Battery lib reports temperature in Celsius. - // Convert Celsius to Kelvin for transmission on CAN. - pkt.temperature = C_TO_KELVIN(temperature); - } - - pkt.state_of_health_pct = UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATE_OF_HEALTH_UNKNOWN; - uint8_t percentage = 0; - if (battery.lib.capacity_remaining_pct(percentage, i)) { - pkt.state_of_charge_pct = percentage; - } - pkt.model_instance_id = i+1; - -#if !defined(HAL_PERIPH_BATTERY_SKIP_NAME) - // example model_name: "org.ardupilot.ap_periph SN 123" - hal.util->snprintf((char*)pkt.model_name.data, sizeof(pkt.model_name.data), "%s %ld", AP_PERIPH_BATTERY_MODEL_NAME, (long int)serial_number); - pkt.model_name.len = strnlen((char*)pkt.model_name.data, sizeof(pkt.model_name.data)); -#endif //defined(HAL_PERIPH_BATTERY_SKIP_NAME) - - uint8_t buffer[UAVCAN_EQUIPMENT_POWER_BATTERYINFO_MAX_SIZE] {}; - const uint16_t total_size = uavcan_equipment_power_BatteryInfo_encode(&pkt, buffer, !periph.canfdout()); - - canard_broadcast(UAVCAN_EQUIPMENT_POWER_BATTERYINFO_SIGNATURE, - UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); - } -#endif -} - -#ifdef HAL_PERIPH_ENABLE_GPS -/* - convert large values to NaN for float16 - */ -static void check_float16_range(float *v, uint8_t len) -{ - for (uint8_t i=0; i= f16max) { - v[i] = nanf(""); - } - } -} -#endif - -/* - update CAN GPS - */ -void AP_Periph_FW::can_gps_update(void) -{ -#ifdef HAL_PERIPH_ENABLE_GPS - if (gps.get_type(0) == AP_GPS::GPS_Type::GPS_TYPE_NONE) { - return; - } - gps.update(); - send_moving_baseline_msg(); - send_relposheading_msg(); - if (last_gps_update_ms == gps.last_message_time_ms()) { - return; - } - last_gps_update_ms = gps.last_message_time_ms(); - - { - /* - send Fix2 packet - */ - uavcan_equipment_gnss_Fix2 pkt {}; - const Location &loc = gps.location(); - const Vector3f &vel = gps.velocity(); - if (gps.status() < AP_GPS::GPS_OK_FIX_2D && !saw_gps_lock_once) { - pkt.timestamp.usec = AP_HAL::micros64(); - pkt.gnss_timestamp.usec = 0; - } else { - saw_gps_lock_once = true; - pkt.timestamp.usec = gps.time_epoch_usec(); - pkt.gnss_timestamp.usec = gps.last_message_epoch_usec(); - } - if (pkt.gnss_timestamp.usec == 0) { - pkt.gnss_time_standard = UAVCAN_EQUIPMENT_GNSS_FIX_GNSS_TIME_STANDARD_NONE; - } else { - pkt.gnss_time_standard = UAVCAN_EQUIPMENT_GNSS_FIX_GNSS_TIME_STANDARD_UTC; - } - pkt.longitude_deg_1e8 = uint64_t(loc.lng) * 10ULL; - pkt.latitude_deg_1e8 = uint64_t(loc.lat) * 10ULL; - pkt.height_ellipsoid_mm = loc.alt * 10; - pkt.height_msl_mm = loc.alt * 10; - float undulation; - if (gps.get_undulation(undulation)) { - pkt.height_ellipsoid_mm -= undulation*1000; - } - for (uint8_t i=0; i<3; i++) { - pkt.ned_velocity[i] = vel[i]; - } - pkt.sats_used = gps.num_sats(); - switch (gps.status()) { - case AP_GPS::GPS_Status::NO_GPS: - case AP_GPS::GPS_Status::NO_FIX: - pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_NO_FIX; - pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE; - pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER; - break; - case AP_GPS::GPS_Status::GPS_OK_FIX_2D: - pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_2D_FIX; - pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE; - pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER; - break; - case AP_GPS::GPS_Status::GPS_OK_FIX_3D: - pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX; - pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE; - pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER; - break; - case AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS: - pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX; - pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_DGPS; - pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_SBAS; - break; - case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT: - pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX; - pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_RTK; - pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FLOAT; - break; - case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED: - pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX; - pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_RTK; - pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FIXED; - break; - } - - pkt.covariance.len = 6; - - float hacc; - if (gps.horizontal_accuracy(hacc)) { - pkt.covariance.data[0] = pkt.covariance.data[1] = sq(hacc); - } - - float vacc; - if (gps.vertical_accuracy(vacc)) { - pkt.covariance.data[2] = sq(vacc); - } - - float sacc; - if (gps.speed_accuracy(sacc)) { - float vc3 = sq(sacc); - pkt.covariance.data[3] = pkt.covariance.data[4] = pkt.covariance.data[5] = vc3; - } - - check_float16_range(pkt.covariance.data, pkt.covariance.len); - - uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_FIX2_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_gnss_Fix2_encode(&pkt, buffer, !periph.canfdout()); - - canard_broadcast(UAVCAN_EQUIPMENT_GNSS_FIX2_SIGNATURE, - UAVCAN_EQUIPMENT_GNSS_FIX2_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); - } - - /* - send aux packet - */ - { - uavcan_equipment_gnss_Auxiliary aux {}; - aux.hdop = gps.get_hdop() * 0.01; - aux.vdop = gps.get_vdop() * 0.01; - - uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_AUXILIARY_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_gnss_Auxiliary_encode(&aux, buffer, !periph.canfdout()); - canard_broadcast(UAVCAN_EQUIPMENT_GNSS_AUXILIARY_SIGNATURE, - UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); - } - - // send the gnss status packet - { - ardupilot_gnss_Status status {}; - - status.healthy = gps.is_healthy(); - if (gps.logging_present() && gps.logging_enabled() && !gps.logging_failed()) { - status.status |= ARDUPILOT_GNSS_STATUS_STATUS_LOGGING; - } - uint8_t idx; // unused - if (status.healthy && !gps.first_unconfigured_gps(idx)) { - status.status |= ARDUPILOT_GNSS_STATUS_STATUS_ARMABLE; - } - - uint32_t error_codes; - if (gps.get_error_codes(error_codes)) { - status.error_codes = error_codes; - } - - uint8_t buffer[ARDUPILOT_GNSS_STATUS_MAX_SIZE] {}; - const uint16_t total_size = ardupilot_gnss_Status_encode(&status, buffer, !periph.canfdout()); - canard_broadcast(ARDUPILOT_GNSS_STATUS_SIGNATURE, - ARDUPILOT_GNSS_STATUS_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); - - } - - // send Heading message if we are not sending RelPosHeading messages and have yaw - if (gps.have_gps_yaw() && last_relposheading_ms == 0) { - float yaw_deg, yaw_acc_deg; - uint32_t yaw_time_ms; - if (gps.gps_yaw_deg(yaw_deg, yaw_acc_deg, yaw_time_ms) && yaw_time_ms != last_gps_yaw_ms) { - last_gps_yaw_ms = yaw_time_ms; - - ardupilot_gnss_Heading heading {}; - heading.heading_valid = true; - heading.heading_accuracy_valid = is_positive(yaw_acc_deg); - heading.heading_rad = radians(yaw_deg); - heading.heading_accuracy_rad = radians(yaw_acc_deg); - uint8_t buffer[ARDUPILOT_GNSS_HEADING_MAX_SIZE] {}; - const uint16_t total_size = ardupilot_gnss_Heading_encode(&heading, buffer, !periph.canfdout()); - canard_broadcast(ARDUPILOT_GNSS_HEADING_SIGNATURE, - ARDUPILOT_GNSS_HEADING_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); - } - } -#endif // HAL_PERIPH_ENABLE_GPS -} - - -void AP_Periph_FW::send_moving_baseline_msg() -{ -#if defined(HAL_PERIPH_ENABLE_GPS) && GPS_MOVING_BASELINE - const uint8_t *data = nullptr; - uint16_t len = 0; - if (!gps.get_RTCMV3(data, len)) { - return; - } - if (len == 0 || data == nullptr) { - return; - } - // send the packet from Moving Base to be used RelPosHeading calc by GPS module - ardupilot_gnss_MovingBaselineData mbldata {}; - // get the data from the moving base - static_assert(sizeof(ardupilot_gnss_MovingBaselineData::data.data) == RTCM3_MAX_PACKET_LEN, "Size of Moving Base data is wrong"); - mbldata.data.len = len; - memcpy(mbldata.data.data, data, len); - uint8_t buffer[ARDUPILOT_GNSS_MOVINGBASELINEDATA_MAX_SIZE] {}; - const uint16_t total_size = ardupilot_gnss_MovingBaselineData_encode(&mbldata, buffer, !periph.canfdout()); - -#if HAL_NUM_CAN_IFACES >= 2 - if (gps_mb_can_port != -1 && (gps_mb_can_port < HAL_NUM_CAN_IFACES)) { - uint8_t *tid_ptr = get_tid_ptr(MAKE_TRANSFER_DESCRIPTOR(ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE, ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID, 0, CANARD_BROADCAST_NODE_ID)); - canardBroadcast(&dronecan.canard, - ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE, - ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID, - tid_ptr, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size -#if CANARD_MULTI_IFACE - ,(1U<= 1000) { - last_probe_ms = now; - airspeed.allocate(); - } - } -#endif - uint32_t now = AP_HAL::native_millis(); - if (now - last_airspeed_update_ms < 50) { - // max 20Hz data - return; - } - last_airspeed_update_ms = now; - airspeed.update(); - if (!airspeed.healthy()) { - // don't send any data - return; - } - const float press = airspeed.get_corrected_pressure(); - float temp; - if (!airspeed.get_temperature(temp)) { - temp = nanf(""); - } else { - temp = C_TO_KELVIN(temp); - } - - uavcan_equipment_air_data_RawAirData pkt {}; - pkt.differential_pressure = press; - pkt.static_air_temperature = temp; - - // unfilled elements are NaN - pkt.static_pressure = nanf(""); - pkt.static_pressure_sensor_temperature = nanf(""); - pkt.differential_pressure_sensor_temperature = nanf(""); - pkt.pitot_temperature = nanf(""); - - uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_air_data_RawAirData_encode(&pkt, buffer, !periph.canfdout()); - - canard_broadcast(UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_SIGNATURE, - UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); -#endif // HAL_PERIPH_ENABLE_AIRSPEED -} - - -/* - update CAN rangefinder - */ -void AP_Periph_FW::can_rangefinder_update(void) -{ -#ifdef HAL_PERIPH_ENABLE_RANGEFINDER - if (rangefinder.get_type(0) == RangeFinder::Type::NONE) { - return; - } -#if AP_PERIPH_PROBE_CONTINUOUS - if (rangefinder.num_sensors() == 0) { - uint32_t now = AP_HAL::native_millis(); - static uint32_t last_probe_ms; - if (now - last_probe_ms >= 1000) { - last_probe_ms = now; - rangefinder.init(ROTATION_NONE); - } - } -#endif - uint32_t now = AP_HAL::native_millis(); - static uint32_t last_update_ms; - if (g.rangefinder_max_rate > 0 && - now - last_update_ms < 1000/g.rangefinder_max_rate) { - // limit to max rate - return; - } - last_update_ms = now; - rangefinder.update(); - RangeFinder::Status status = rangefinder.status_orient(ROTATION_NONE); - if (status <= RangeFinder::Status::NoData) { - // don't send any data - return; - } - const uint32_t sample_ms = rangefinder.last_reading_ms(ROTATION_NONE); - if (last_sample_ms == sample_ms) { - return; - } - last_sample_ms = sample_ms; - - uint16_t dist_cm = rangefinder.distance_cm_orient(ROTATION_NONE); - uavcan_equipment_range_sensor_Measurement pkt {}; - pkt.sensor_id = rangefinder.get_address(0); - switch (status) { - case RangeFinder::Status::OutOfRangeLow: - pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_CLOSE; - break; - case RangeFinder::Status::OutOfRangeHigh: - pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_FAR; - break; - case RangeFinder::Status::Good: - pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_VALID_RANGE; - break; - default: - pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_UNDEFINED; - break; - } - switch (rangefinder.get_mav_distance_sensor_type_orient(ROTATION_NONE)) { - case MAV_DISTANCE_SENSOR_LASER: - pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_LIDAR; - break; - case MAV_DISTANCE_SENSOR_ULTRASOUND: - pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_SONAR; - break; - case MAV_DISTANCE_SENSOR_RADAR: - pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_RADAR; - break; - default: - pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_UNDEFINED; - break; - } - - pkt.range = dist_cm * 0.01; - - uint8_t buffer[UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_range_sensor_Measurement_encode(&pkt, buffer, !periph.canfdout()); - - canard_broadcast(UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE, - UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); -#endif // HAL_PERIPH_ENABLE_RANGEFINDER -} - - -void AP_Periph_FW::can_proximity_update() -{ -#ifdef HAL_PERIPH_ENABLE_PRX - if (proximity.get_type(0) == AP_Proximity::Type::None) { - return; - } - - uint32_t now = AP_HAL::native_millis(); - static uint32_t last_update_ms; - if (g.proximity_max_rate > 0 && - now - last_update_ms < 1000/g.proximity_max_rate) { - // limit to max rate - return; - } - last_update_ms = now; - proximity.update(); - AP_Proximity::Status status = proximity.get_status(); - if (status <= AP_Proximity::Status::NoData) { - // don't send any data - return; - } - - ardupilot_equipment_proximity_sensor_Proximity pkt {}; - - const uint8_t obstacle_count = proximity.get_obstacle_count(); - - // if no objects return - if (obstacle_count == 0) { - return; - } - - // calculate maximum roll, pitch values from objects - for (uint8_t i=0; i + +#ifndef AP_PERIPH_MAG_MAX_RATE +#define AP_PERIPH_MAG_MAX_RATE 25U +#endif + +#ifndef AP_PERIPH_PROBE_CONTINUOUS +#define AP_PERIPH_PROBE_CONTINUOUS 0 +#endif + +/* + update CAN magnetometer + */ +void AP_Periph_FW::can_mag_update(void) +{ + if (!compass.available()) { + return; + } + +#if AP_PERIPH_MAG_MAX_RATE > 0 + // don't flood the bus with very high rate magnetometers + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - last_mag_update_ms < (1000U / AP_PERIPH_MAG_MAX_RATE)) { + return; + } +#endif + + compass.read(); + +#if AP_PERIPH_PROBE_CONTINUOUS + if (compass.get_count() == 0) { + static uint32_t last_probe_ms; + uint32_t now = AP_HAL::millis(); + if (now - last_probe_ms >= 1000) { + last_probe_ms = now; + compass.init(); + } + } +#endif + + if (last_mag_update_ms == compass.last_update_ms()) { + return; + } + if (!compass.healthy()) { + return; + } + + last_mag_update_ms = compass.last_update_ms(); + const Vector3f &field = compass.get_field(); + uavcan_equipment_ahrs_MagneticFieldStrength pkt {}; + + // the canard dsdl compiler doesn't understand float16 + for (uint8_t i=0; i<3; i++) { + pkt.magnetic_field_ga[i] = field[i] * 0.001; + } + + uint8_t buffer[UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_MAX_SIZE] {}; + uint16_t total_size = uavcan_equipment_ahrs_MagneticFieldStrength_encode(&pkt, buffer, !periph.canfdout()); + + canard_broadcast(UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_SIGNATURE, + UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); +} + +#endif // HAL_PERIPH_ENABLE_MAG diff --git a/Tools/AP_Periph/efi.cpp b/Tools/AP_Periph/efi.cpp new file mode 100644 index 0000000000000..d1bb114cf633d --- /dev/null +++ b/Tools/AP_Periph/efi.cpp @@ -0,0 +1,207 @@ +#include "AP_Periph.h" + +#ifdef HAL_PERIPH_ENABLE_EFI + +/* + EFI support + */ + +#include + +#ifndef AP_PERIPH_EFI_MAX_RATE +// default to 2x the AP_Vehicle rate +#define AP_PERIPH_EFI_MAX_RATE 100U +#endif + +/* + update CAN EFI + */ +void AP_Periph_FW::can_efi_update(void) +{ + if (!efi.enabled()) { + return; + } + +#if AP_PERIPH_EFI_MAX_RATE > 0 + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - last_efi_update_ms < (1000U / AP_PERIPH_EFI_MAX_RATE)) { + return; + } + last_efi_update_ms = now_ms; +#endif + + efi.update(); + const uint32_t update_ms = efi.get_last_update_ms(); + if (!efi.is_healthy() || efi_update_ms == update_ms) { + return; + } + efi_update_ms = update_ms; + EFI_State state; + efi.get_state(state); + + { + /* + send status packet + */ + uavcan_equipment_ice_reciprocating_Status pkt {}; + + // state maps 1:1 from Engine_State + pkt.state = uint8_t(state.engine_state); + + switch (state.crankshaft_sensor_status) { + case Crankshaft_Sensor_Status::NOT_SUPPORTED: + break; + case Crankshaft_Sensor_Status::OK: + pkt.flags |= UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_CRANKSHAFT_SENSOR_ERROR_SUPPORTED; + break; + case Crankshaft_Sensor_Status::ERROR: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_CRANKSHAFT_SENSOR_ERROR_SUPPORTED | + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_CRANKSHAFT_SENSOR_ERROR; + break; + } + + switch (state.temperature_status) { + case Temperature_Status::NOT_SUPPORTED: + break; + case Temperature_Status::OK: + pkt.flags |= UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_SUPPORTED; + break; + case Temperature_Status::BELOW_NOMINAL: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_SUPPORTED | + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_BELOW_NOMINAL; + break; + case Temperature_Status::ABOVE_NOMINAL: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_SUPPORTED | + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_ABOVE_NOMINAL; + break; + case Temperature_Status::OVERHEATING: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_SUPPORTED | + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_OVERHEATING; + break; + case Temperature_Status::EGT_ABOVE_NOMINAL: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_SUPPORTED | + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_EGT_ABOVE_NOMINAL; + break; + } + + switch (state.fuel_pressure_status) { + case Fuel_Pressure_Status::NOT_SUPPORTED: + break; + case Fuel_Pressure_Status::OK: + pkt.flags |= UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_SUPPORTED; + break; + case Fuel_Pressure_Status::BELOW_NOMINAL: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_SUPPORTED | + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_BELOW_NOMINAL; + break; + case Fuel_Pressure_Status::ABOVE_NOMINAL: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_SUPPORTED | + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_ABOVE_NOMINAL; + break; + } + + switch (state.oil_pressure_status) { + case Oil_Pressure_Status::NOT_SUPPORTED: + break; + case Oil_Pressure_Status::OK: + pkt.flags |= UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_SUPPORTED; + break; + case Oil_Pressure_Status::BELOW_NOMINAL: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_SUPPORTED | + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_BELOW_NOMINAL; + break; + case Oil_Pressure_Status::ABOVE_NOMINAL: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_SUPPORTED | + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_ABOVE_NOMINAL; + break; + } + + switch (state.detonation_status) { + case Detonation_Status::NOT_SUPPORTED: + break; + case Detonation_Status::NOT_OBSERVED: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DETONATION_SUPPORTED; + break; + case Detonation_Status::OBSERVED: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DETONATION_SUPPORTED | + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DETONATION_OBSERVED; + break; + } + + switch (state.misfire_status) { + case Misfire_Status::NOT_SUPPORTED: + break; + case Misfire_Status::NOT_OBSERVED: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_MISFIRE_SUPPORTED; + break; + case Misfire_Status::OBSERVED: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_MISFIRE_SUPPORTED | + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_MISFIRE_OBSERVED; + break; + } + + switch (state.debris_status) { + case Debris_Status::NOT_SUPPORTED: + break; + case Debris_Status::NOT_DETECTED: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DEBRIS_SUPPORTED; + break; + case Debris_Status::DETECTED: + pkt.flags |= + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DEBRIS_SUPPORTED | + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DEBRIS_DETECTED; + break; + } + + pkt.engine_load_percent = state.engine_load_percent; + pkt.engine_speed_rpm = state.engine_speed_rpm; + pkt.spark_dwell_time_ms = state.spark_dwell_time_ms; + pkt.atmospheric_pressure_kpa = state.atmospheric_pressure_kpa; + pkt.intake_manifold_pressure_kpa = state.intake_manifold_pressure_kpa; + pkt.intake_manifold_temperature = state.intake_manifold_temperature; + pkt.coolant_temperature = state.coolant_temperature; + pkt.oil_pressure = state.oil_pressure; + pkt.oil_temperature = state.oil_temperature; + pkt.fuel_pressure = state.fuel_pressure; + pkt.fuel_consumption_rate_cm3pm = state.fuel_consumption_rate_cm3pm; + pkt.estimated_consumed_fuel_volume_cm3 = state.estimated_consumed_fuel_volume_cm3; + pkt.throttle_position_percent = state.throttle_position_percent; + pkt.ecu_index = state.ecu_index; + pkt.spark_plug_usage = uint8_t(state.spark_plug_usage); + + // assume single set of cylinder status + pkt.cylinder_status.len = 1; + auto &c = pkt.cylinder_status.data[0]; + const auto &state_c = state.cylinder_status; + c.ignition_timing_deg = state_c.ignition_timing_deg; + c.injection_time_ms = state_c.injection_time_ms; + c.cylinder_head_temperature = state_c.cylinder_head_temperature; + c.exhaust_gas_temperature = state_c.exhaust_gas_temperature; + c.lambda_coefficient = state_c.lambda_coefficient; + + uint8_t buffer[UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_MAX_SIZE] {}; + const uint16_t total_size = uavcan_equipment_ice_reciprocating_Status_encode(&pkt, buffer, !canfdout()); + + canard_broadcast(UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_SIGNATURE, + UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); + } +} + +#endif // HAL_PERIPH_ENABLE_EFI diff --git a/Tools/AP_Periph/esc_apd_telem.cpp b/Tools/AP_Periph/esc_apd_telem.cpp new file mode 100644 index 0000000000000..ed438d60f36c5 --- /dev/null +++ b/Tools/AP_Periph/esc_apd_telem.cpp @@ -0,0 +1,97 @@ +/* + ESC Telemetry for the APD HV Pro ESC + + Protocol is here: https://docs.powerdrives.net/products/hv_pro/uart-telemetry-output + */ +#include "esc_apd_telem.h" +#include +#include +#include +#include + +#ifdef HAL_PERIPH_ENABLE_ESC_APD + +extern const AP_HAL::HAL& hal; + +#define TELEM_HEADER 0x9B +#define TELEM_LEN 0x16 + +ESC_APD_Telem::ESC_APD_Telem (AP_HAL::UARTDriver *_uart, float num_poles) : + pole_count(num_poles), + uart(_uart) { + uart->begin(115200); +} + +bool ESC_APD_Telem::update() { + uint32_t n = uart->available(); + if (n == 0) { + return false; + } + + // don't read too much in one loop to prevent too high CPU load + if (n > 50) { + n = 50; + } + + bool ret = false; + + while (n--) { + uint8_t b = uart->read(); + received.bytes[len++] = b; + + // check the packet size first + if ((size_t)len >= sizeof(received.packet)) { + // we have a full packet, check the stop byte + if (received.packet.stop == 65535) { + // valid stop byte, check the CRC + if (crc_fletcher16(received.bytes, 18) == received.packet.checksum) { + // valid packet, copy the data we need and reset length + decoded.voltage = le16toh(received.packet.voltage) * 1e-2f; + decoded.temperature = convert_temperature(le16toh(received.packet.temperature)); + decoded.current = ((int16_t)le16toh(received.packet.bus_current)) * (1 / 12.5f); + decoded.rpm = le32toh(received.packet.erpm) / pole_count; + decoded.power_rating_pct = le16toh(received.packet.motor_duty) * 1e-2f; + ret = true; + len = 0; + } else { + // we have an invalid packet, shift it back a byte + shift_buffer(); + } + } else { + // invalid stop byte, we've lost sync, shift the packet by 1 byte + shift_buffer(); + } + + } + } + return ret; +} + +// shift the decode buffer left by 1 byte, and rewind the progress +void ESC_APD_Telem::shift_buffer(void) { + memmove(received.bytes, received.bytes + 1, sizeof(received.bytes) - 1); + len--; +} + +// convert the raw ESC temperature to a useful value (in Kelvin) +// based on the 1.1 example C code found here https://docs.powerdrives.net/products/hv_pro/uart-telemetry-output +float ESC_APD_Telem::convert_temperature(uint16_t raw) const { + const float series_resistor = 10000; + const float nominal_resistance = 10000; + const float nominal_temperature = 25; + const float b_coefficent = 3455; + + + const float Rntc = series_resistor / ((4096 / float(raw)) - 1); + + float temperature = Rntc / nominal_resistance; // (R/Ro) + temperature = logf(temperature); // ln(R/Ro) + temperature /= b_coefficent; // 1/B * ln(R/Ro) + temperature += 1 / C_TO_KELVIN(nominal_temperature); // + (1/To) + temperature = 1 / temperature; // invert + + // the example code rejected anything below 0C, or above 200C, the 200C clamp makes some sense, the below 0C is harder to accept + return temperature; +} + +#endif // HAL_PERIPH_ENABLE_ESC_APD diff --git a/Tools/AP_Periph/esc_apd_telem.h b/Tools/AP_Periph/esc_apd_telem.h new file mode 100644 index 0000000000000..42f2f416ad611 --- /dev/null +++ b/Tools/AP_Periph/esc_apd_telem.h @@ -0,0 +1,61 @@ +/* + ESC Telemetry for APD ESC. + */ + +#pragma once + +#include + +#ifdef HAL_PERIPH_ENABLE_ESC_APD + +class ESC_APD_Telem { +public: + ESC_APD_Telem (AP_HAL::UARTDriver *_uart, float num_poles); + bool update(); + + CLASS_NO_COPY(ESC_APD_Telem); + + struct telem { + uint32_t error_count; + float voltage; + float current; + float temperature; // kelvin + int32_t rpm; + uint8_t power_rating_pct; + }; + + const telem &get_telem(void) { + return decoded; + } + +private: + AP_HAL::UARTDriver *uart; + + union { + struct PACKED { + uint16_t voltage; + uint16_t temperature; + int16_t bus_current; + uint16_t reserved0; + uint32_t erpm; + uint16_t input_duty; + uint16_t motor_duty; + uint16_t reserved1; + uint16_t checksum; // 16 bit fletcher checksum + uint16_t stop; // should always be 65535 on a valid packet + } packet; + uint8_t bytes[22]; + } received; + static_assert(sizeof(received.packet) == sizeof(received.bytes), "The packet must be the same size as the raw buffer"); + + uint8_t len; + + struct telem decoded; + + float pole_count; + + float convert_temperature(uint16_t raw) const; + void shift_buffer(void); +}; + +#endif // HAL_PERIPH_ENABLE_ESC_APD diff --git a/Tools/AP_Periph/gps.cpp b/Tools/AP_Periph/gps.cpp new file mode 100644 index 0000000000000..e8262e2efb434 --- /dev/null +++ b/Tools/AP_Periph/gps.cpp @@ -0,0 +1,322 @@ +#include "AP_Periph.h" + +#ifdef HAL_PERIPH_ENABLE_GPS + +/* + GPS support + */ + +#include +#include + +#define DEBUG_PRINTS 0 + +#if DEBUG_PRINTS + # define Debug(fmt, args ...) do {can_printf(fmt "\n", ## args);} while(0) +#else + # define Debug(fmt, args ...) +#endif + +/* + handle gnss::RTCMStream + */ +void AP_Periph_FW::handle_RTCMStream(CanardInstance* canard_instance, CanardRxTransfer* transfer) +{ + uavcan_equipment_gnss_RTCMStream req; + if (uavcan_equipment_gnss_RTCMStream_decode(transfer, &req)) { + return; + } + gps.handle_gps_rtcm_fragment(0, req.data.data, req.data.len); +} + +/* + handle gnss::MovingBaselineData +*/ +#if GPS_MOVING_BASELINE +void AP_Periph_FW::handle_MovingBaselineData(CanardInstance* canard_instance, CanardRxTransfer* transfer) +{ + ardupilot_gnss_MovingBaselineData msg; + if (ardupilot_gnss_MovingBaselineData_decode(transfer, &msg)) { + return; + } + gps.inject_MBL_data(msg.data.data, msg.data.len); + Debug("MovingBaselineData: len=%u\n", msg.data.len); +} +#endif // GPS_MOVING_BASELINE + +/* + convert large values to NaN for float16 + */ +static void check_float16_range(float *v, uint8_t len) +{ + for (uint8_t i=0; i= f16max) { + v[i] = nanf(""); + } + } +} + +/* + update CAN GPS + */ +void AP_Periph_FW::can_gps_update(void) +{ + if (gps.get_type(0) == AP_GPS::GPS_Type::GPS_TYPE_NONE) { + return; + } + gps.update(); + send_moving_baseline_msg(); + send_relposheading_msg(); + if (last_gps_update_ms == gps.last_message_time_ms()) { + return; + } + last_gps_update_ms = gps.last_message_time_ms(); + + { + /* + send Fix2 packet + */ + uavcan_equipment_gnss_Fix2 pkt {}; + const Location &loc = gps.location(); + const Vector3f &vel = gps.velocity(); + if (gps.status() < AP_GPS::GPS_OK_FIX_2D && !saw_gps_lock_once) { + pkt.timestamp.usec = AP_HAL::micros64(); + pkt.gnss_timestamp.usec = 0; + } else { + saw_gps_lock_once = true; + pkt.timestamp.usec = gps.time_epoch_usec(); + pkt.gnss_timestamp.usec = gps.last_message_epoch_usec(); + } + if (pkt.gnss_timestamp.usec == 0) { + pkt.gnss_time_standard = UAVCAN_EQUIPMENT_GNSS_FIX_GNSS_TIME_STANDARD_NONE; + } else { + pkt.gnss_time_standard = UAVCAN_EQUIPMENT_GNSS_FIX_GNSS_TIME_STANDARD_UTC; + } + pkt.longitude_deg_1e8 = uint64_t(loc.lng) * 10ULL; + pkt.latitude_deg_1e8 = uint64_t(loc.lat) * 10ULL; + pkt.height_ellipsoid_mm = loc.alt * 10; + pkt.height_msl_mm = loc.alt * 10; + float undulation; + if (gps.get_undulation(undulation)) { + pkt.height_ellipsoid_mm -= undulation*1000; + } + for (uint8_t i=0; i<3; i++) { + pkt.ned_velocity[i] = vel[i]; + } + pkt.sats_used = gps.num_sats(); + switch (gps.status()) { + case AP_GPS::GPS_Status::NO_GPS: + case AP_GPS::GPS_Status::NO_FIX: + pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_NO_FIX; + pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE; + pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_2D: + pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_2D_FIX; + pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE; + pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_3D: + pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX; + pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE; + pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS: + pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX; + pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_DGPS; + pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_SBAS; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT: + pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX; + pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_RTK; + pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FLOAT; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED: + pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX; + pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_RTK; + pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FIXED; + break; + } + + pkt.covariance.len = 6; + + float hacc; + if (gps.horizontal_accuracy(hacc)) { + pkt.covariance.data[0] = pkt.covariance.data[1] = sq(hacc); + } + + float vacc; + if (gps.vertical_accuracy(vacc)) { + pkt.covariance.data[2] = sq(vacc); + } + + float sacc; + if (gps.speed_accuracy(sacc)) { + float vc3 = sq(sacc); + pkt.covariance.data[3] = pkt.covariance.data[4] = pkt.covariance.data[5] = vc3; + } + + check_float16_range(pkt.covariance.data, pkt.covariance.len); + + uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_FIX2_MAX_SIZE] {}; + uint16_t total_size = uavcan_equipment_gnss_Fix2_encode(&pkt, buffer, !canfdout()); + + canard_broadcast(UAVCAN_EQUIPMENT_GNSS_FIX2_SIGNATURE, + UAVCAN_EQUIPMENT_GNSS_FIX2_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); + } + + /* + send aux packet + */ + { + uavcan_equipment_gnss_Auxiliary aux {}; + aux.hdop = gps.get_hdop() * 0.01; + aux.vdop = gps.get_vdop() * 0.01; + + uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_AUXILIARY_MAX_SIZE] {}; + uint16_t total_size = uavcan_equipment_gnss_Auxiliary_encode(&aux, buffer, !canfdout()); + canard_broadcast(UAVCAN_EQUIPMENT_GNSS_AUXILIARY_SIGNATURE, + UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); + } + + // send the gnss status packet + { + ardupilot_gnss_Status status {}; + + status.healthy = gps.is_healthy(); + if (gps.logging_present() && gps.logging_enabled() && !gps.logging_failed()) { + status.status |= ARDUPILOT_GNSS_STATUS_STATUS_LOGGING; + } + uint8_t idx; // unused + if (status.healthy && !gps.first_unconfigured_gps(idx)) { + status.status |= ARDUPILOT_GNSS_STATUS_STATUS_ARMABLE; + } + + uint32_t error_codes; + if (gps.get_error_codes(error_codes)) { + status.error_codes = error_codes; + } + + uint8_t buffer[ARDUPILOT_GNSS_STATUS_MAX_SIZE] {}; + const uint16_t total_size = ardupilot_gnss_Status_encode(&status, buffer, !canfdout()); + canard_broadcast(ARDUPILOT_GNSS_STATUS_SIGNATURE, + ARDUPILOT_GNSS_STATUS_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); + + } + + // send Heading message if we are not sending RelPosHeading messages and have yaw + if (gps.have_gps_yaw() && last_relposheading_ms == 0) { + float yaw_deg, yaw_acc_deg; + uint32_t yaw_time_ms; + if (gps.gps_yaw_deg(yaw_deg, yaw_acc_deg, yaw_time_ms) && yaw_time_ms != last_gps_yaw_ms) { + last_gps_yaw_ms = yaw_time_ms; + + ardupilot_gnss_Heading heading {}; + heading.heading_valid = true; + heading.heading_accuracy_valid = is_positive(yaw_acc_deg); + heading.heading_rad = radians(yaw_deg); + heading.heading_accuracy_rad = radians(yaw_acc_deg); + uint8_t buffer[ARDUPILOT_GNSS_HEADING_MAX_SIZE] {}; + const uint16_t total_size = ardupilot_gnss_Heading_encode(&heading, buffer, !canfdout()); + canard_broadcast(ARDUPILOT_GNSS_HEADING_SIGNATURE, + ARDUPILOT_GNSS_HEADING_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); + } + } +} + + +void AP_Periph_FW::send_moving_baseline_msg() +{ +#if GPS_MOVING_BASELINE + const uint8_t *data = nullptr; + uint16_t len = 0; + if (!gps.get_RTCMV3(data, len)) { + return; + } + if (len == 0 || data == nullptr) { + return; + } + // send the packet from Moving Base to be used RelPosHeading calc by GPS module + ardupilot_gnss_MovingBaselineData mbldata {}; + // get the data from the moving base + static_assert(sizeof(ardupilot_gnss_MovingBaselineData::data.data) == RTCM3_MAX_PACKET_LEN, "Size of Moving Base data is wrong"); + mbldata.data.len = len; + memcpy(mbldata.data.data, data, len); + uint8_t buffer[ARDUPILOT_GNSS_MOVINGBASELINEDATA_MAX_SIZE] {}; + const uint16_t total_size = ardupilot_gnss_MovingBaselineData_encode(&mbldata, buffer, !canfdout()); + +#if HAL_NUM_CAN_IFACES >= 2 + if (gps_mb_can_port != -1 && (gps_mb_can_port < HAL_NUM_CAN_IFACES)) { + uint8_t *tid_ptr = get_tid_ptr(MAKE_TRANSFER_DESCRIPTOR(ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE, ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID, 0, CANARD_BROADCAST_NODE_ID)); + canardBroadcast(&dronecan.canard, + ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE, + ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID, + tid_ptr, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size +#if CANARD_MULTI_IFACE + ,(1U< + +extern const AP_HAL::HAL &hal; + +/* + hardpoint support + */ + +#include + +void AP_Periph_FW::pwm_hardpoint_init() +{ + hal.gpio->attach_interrupt( + PWM_HARDPOINT_PIN, + FUNCTOR_BIND_MEMBER(&AP_Periph_FW::pwm_irq_handler, void, uint8_t, bool, uint32_t), AP_HAL::GPIO::INTERRUPT_BOTH); +} + +/* + called on PWM pin transition + */ +void AP_Periph_FW::pwm_irq_handler(uint8_t pin, bool pin_state, uint32_t timestamp) +{ + if (pin_state == 0 && pwm_hardpoint.last_state == 1 && pwm_hardpoint.last_ts_us != 0) { + uint32_t width = timestamp - pwm_hardpoint.last_ts_us; + if (width > 500 && width < 2500) { + pwm_hardpoint.pwm_value = width; + if (width > pwm_hardpoint.highest_pwm) { + pwm_hardpoint.highest_pwm = width; + } + } + } + pwm_hardpoint.last_state = pin_state; + pwm_hardpoint.last_ts_us = timestamp; +} + +void AP_Periph_FW::pwm_hardpoint_update() +{ + uint32_t now = AP_HAL::millis(); + // send at 10Hz + void *save = hal.scheduler->disable_interrupts_save(); + uint16_t value = pwm_hardpoint.highest_pwm; + pwm_hardpoint.highest_pwm = 0; + hal.scheduler->restore_interrupts(save); + float rate = g.hardpoint_rate; + rate = constrain_float(rate, 10, 100); + if (value > 0 && now - pwm_hardpoint.last_send_ms >= 1000U/rate) { + pwm_hardpoint.last_send_ms = now; + uavcan_equipment_hardpoint_Command cmd {}; + cmd.hardpoint_id = g.hardpoint_id; + cmd.command = value; + + uint8_t buffer[UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_MAX_SIZE] {}; + uint16_t total_size = uavcan_equipment_hardpoint_Command_encode(&cmd, buffer, !canfdout()); + canard_broadcast(UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_SIGNATURE, + UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); + } +} + +#endif // HAL_PERIPH_ENABLE_PWM_HARDPOINT diff --git a/Tools/AP_Periph/hwing_esc.cpp b/Tools/AP_Periph/hwing_esc.cpp index a79862f3cf6d0..871c2e0517c45 100644 --- a/Tools/AP_Periph/hwing_esc.cpp +++ b/Tools/AP_Periph/hwing_esc.cpp @@ -6,8 +6,11 @@ This protocol only allows for one ESC per UART RX line, so using a CAN node per ESC works well. */ + +#include "AP_Periph.h" #include "hwing_esc.h" #include +#include #ifdef HAL_PERIPH_ENABLE_HWESC @@ -39,7 +42,7 @@ bool HWESC_Telem::update() } // we expect at least 50ms idle between frames - uint32_t now = AP_HAL::native_millis(); + uint32_t now = AP_HAL::millis(); bool frame_gap = (now - last_read_ms) > 10; last_read_ms = now; @@ -143,5 +146,30 @@ bool HWESC_Telem::parse_packet(void) return true; } +void AP_Periph_FW::hwesc_telem_update() +{ + if (!hwesc_telem.update()) { + return; + } + const HWESC_Telem::HWESC &t = hwesc_telem.get_telem(); + + uavcan_equipment_esc_Status pkt {}; + pkt.esc_index = g.esc_number[0]; // only supports a single ESC + pkt.voltage = t.voltage; + pkt.current = t.current; + pkt.temperature = C_TO_KELVIN(MAX(t.mos_temperature, t.cap_temperature)); + pkt.rpm = t.rpm; + pkt.power_rating_pct = t.phase_current; + pkt.error_count = t.error_count; + + uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {}; + uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer, !canfdout()); + canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, + UAVCAN_EQUIPMENT_ESC_STATUS_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); +} + #endif // HAL_PERIPH_ENABLE_HWESC diff --git a/Tools/AP_Periph/networking.cpp b/Tools/AP_Periph/networking.cpp new file mode 100644 index 0000000000000..8ac256850cd9b --- /dev/null +++ b/Tools/AP_Periph/networking.cpp @@ -0,0 +1,167 @@ +/* + 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 . + */ + +#include "AP_Periph.h" + +#ifdef HAL_PERIPH_ENABLE_NETWORKING + +const AP_Param::GroupInfo Networking_Periph::var_info[] { + // @Group: + // @Path: ../../libraries/AP_Networking/AP_Networking.cpp + AP_SUBGROUPINFO(networking_lib, "", 1, Networking_Periph, AP_Networking), + + /* + the NET_Pn_ parameters need to be here as otherwise we + are too deep in the parameter tree + */ + +#if AP_NETWORKING_NUM_PORTS > 0 + // @Group: P1_ + // @Path: ../../libraries/AP_Networking/AP_Networking_port.cpp + AP_SUBGROUPINFO(networking_lib.ports[0], "P1_", 2, Networking_Periph, AP_Networking::Port), +#endif + +#if AP_NETWORKING_NUM_PORTS > 1 + // @Group: P2_ + // @Path: ../../libraries/AP_Networking/AP_Networking_port.cpp + AP_SUBGROUPINFO(networking_lib.ports[1], "P2_", 3, Networking_Periph, AP_Networking::Port), +#endif + +#if AP_NETWORKING_NUM_PORTS > 2 + // @Group: P3_ + // @Path: ../../libraries/AP_Networking/AP_Networking_port.cpp + AP_SUBGROUPINFO(networking_lib.ports[2], "P3_", 4, Networking_Periph, AP_Networking::Port), +#endif + +#if AP_NETWORKING_NUM_PORTS > 3 + // @Group: P4_ + // @Path: ../../libraries/AP_Networking/AP_Networking_port.cpp + AP_SUBGROUPINFO(networking_lib.ports[3], "P4_", 5, Networking_Periph, AP_Networking::Port), +#endif + +#if AP_NETWORKING_NUM_PORTS > 4 + // @Group: P5_ + // @Path: ../../libraries/AP_Networking/AP_Networking_port.cpp + AP_SUBGROUPINFO(networking_lib.ports[4], "P5_", 6, Networking_Periph, AP_Networking::Port), +#endif + +#if AP_NETWORKING_NUM_PORTS > 5 + // @Group: P6_ + // @Path: ../../libraries/AP_Networking/AP_Networking_port.cpp + AP_SUBGROUPINFO(networking_lib.ports[5], "P6_", 7, Networking_Periph, AP_Networking::Port), +#endif + +#if AP_NETWORKING_NUM_PORTS > 6 + // @Group: P7_ + // @Path: ../../libraries/AP_Networking/AP_Networking_port.cpp + AP_SUBGROUPINFO(networking_lib.ports[6], "P7_", 8, Networking_Periph, AP_Networking::Port), +#endif + +#if AP_NETWORKING_NUM_PORTS > 7 + // @Group: P8_ + // @Path: ../../libraries/AP_Networking/AP_Networking_port.cpp + AP_SUBGROUPINFO(networking_lib.ports[7], "P8_", 9, Networking_Periph, AP_Networking::Port), +#endif + +#if AP_NETWORKING_NUM_PORTS > 8 + // @Group: P9_ + // @Path: ../../libraries/AP_Networking/AP_Networking_port.cpp + AP_SUBGROUPINFO(networking_lib.ports[8], "P9_", 10, Networking_Periph, AP_Networking::Port), +#endif + + + +#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 0 + // @Group: PASS1_ + // @Path: networking_passthru.cpp + AP_SUBGROUPINFO(passthru[0], "PASS1_", 11, Networking_Periph, Passthru), +#endif + +#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 1 + // @Group: PASS2_ + // @Path: networking_passthru.cpp + AP_SUBGROUPINFO(passthru[1], "PASS2_", 12, Networking_Periph, Passthru), +#endif + +#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 2 + // @Group: PASS3_ + // @Path: networking_passthru.cpp + AP_SUBGROUPINFO(passthru[2], "PASS3_", 13, Networking_Periph, Passthru), +#endif + +#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 3 + // @Group: PASS4_ + // @Path: networking_passthru.cpp + AP_SUBGROUPINFO(passthru[3], "PASS4_", 14, Networking_Periph, Passthru), +#endif + +#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 4 + // @Group: PASS5_ + // @Path: networking_passthru.cpp + AP_SUBGROUPINFO(passthru[4], "PASS5_", 15, Networking_Periph, Passthru), +#endif + +#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 5 + // @Group: PASS6_ + // @Path: networking_passthru.cpp + AP_SUBGROUPINFO(passthru[5], "PASS6_", 16, Networking_Periph, Passthru), +#endif + +#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 6 + // @Group: PASS7_ + // @Path: networking_passthru.cpp + AP_SUBGROUPINFO(passthru[6], "PASS7_", 17, Networking_Periph, Passthru), +#endif + +#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 7 + // @Group: PASS8_ + // @Path: networking_passthru.cpp + AP_SUBGROUPINFO(passthru[7], "PASS8_", 18, Networking_Periph, Passthru), +#endif + +#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 8 + // @Group: PASS9_ + // @Path: networking_passthru.cpp + AP_SUBGROUPINFO(passthru[8], "PASS9_", 19, Networking_Periph, Passthru), +#endif + + AP_GROUPEND +}; + + +void Networking_Periph::init(void) +{ + networking_lib.init(); + +#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 0 + for (auto &p : passthru) { + p.init(); + } +#endif +} + +void Networking_Periph::update(void) +{ + networking_lib.update(); + +#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 0 + for (auto &p : passthru) { + p.update(); + } +#endif +} + +#endif // HAL_PERIPH_ENABLE_NETWORKING + diff --git a/Tools/AP_Periph/networking.h b/Tools/AP_Periph/networking.h new file mode 100644 index 0000000000000..6b7f857fdcde8 --- /dev/null +++ b/Tools/AP_Periph/networking.h @@ -0,0 +1,60 @@ +#pragma once + +#include + +#ifdef HAL_PERIPH_ENABLE_NETWORKING + +#include + +#ifndef HAL_PERIPH_NETWORK_NUM_PASSTHRU +#define HAL_PERIPH_NETWORK_NUM_PASSTHRU 2 +#endif + +class Networking_Periph { +public: + Networking_Periph() { + AP_Param::setup_object_defaults(this, var_info); + } + + static const struct AP_Param::GroupInfo var_info[]; + + void init(); + void update(); + +private: + +#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 0 + class Passthru { + public: + friend class Networking_Periph; + + CLASS_NO_COPY(Passthru); + + Passthru() { + AP_Param::setup_object_defaults(this, var_info); + } + + void init(); + void update(); + + static const struct AP_Param::GroupInfo var_info[]; + + private: + AP_Int8 enabled; + AP_Int8 ep1; + AP_Int8 ep2; + AP_Int32 baud1; + AP_Int32 baud2; + AP_Int32 options1; + AP_Int32 options2; + + AP_HAL::UARTDriver *port1; + AP_HAL::UARTDriver *port2; + } passthru[HAL_PERIPH_NETWORK_NUM_PASSTHRU]; +#endif // HAL_PERIPH_NETWORK_NUM_PASSTHRU + + AP_Networking networking_lib; +}; + +#endif // HAL_PERIPH_ENABLE_BATTERY_BALANCE + diff --git a/Tools/AP_Periph/networking_passthru.cpp b/Tools/AP_Periph/networking_passthru.cpp new file mode 100644 index 0000000000000..3d2f65a1cad7b --- /dev/null +++ b/Tools/AP_Periph/networking_passthru.cpp @@ -0,0 +1,135 @@ +/* + 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 . + */ + +#include "AP_Periph.h" + +#if defined(HAL_PERIPH_ENABLE_NETWORKING) && HAL_PERIPH_NETWORK_NUM_PASSTHRU > 0 + +#include + +const AP_Param::GroupInfo Networking_Periph::Passthru::var_info[] = { + // @Param: ENABLE + // @DisplayName: Enable Passthrough + // @Description: Enable Passthrough of any UART, Network, or CAN ports to any UART, Network, or CAN ports. + // @Values: 0:Disabled, 1:Enabled + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO_FLAGS("ENABLE", 1, Networking_Periph::Passthru, enabled, 0, AP_PARAM_FLAG_ENABLE), + + // @Param: EP1 + // @DisplayName: Endpoint 1 + // @Description: Passthrough Endpoint 1. This can be a serial port UART, a Network port, or a CAN port. The selected port will route to Endport 2. + // @Values: -1:Disabled, 0:Serial0(usually USB), 1:Serial1, 2:Serial2, 3:Serial3, 4:Serial4, 5:Serial5, 6:Serial6, 7:Serial7, 8:Serial8, 9:Serial9, 21:Network Port1, 22:Network Port2, 23:Network Port3, 24:Network Port4, 25:Network Port5, 26:Network Port6, 27:Network Port7, 28:Network Port8, 29:Network Port9, 41:CAN1 Port1, 42:CAN1 Port2, 43:CAN1 Port3, 44:CAN1 Port4, 45:CAN1 Port5, 46:CAN1 Port6, 47:CAN1 Port7, 48:CAN1 Port8, 49:CAN1 Port9, 51:CAN2 Port1, 52:CAN2 Port2, 53:CAN2 Port3, 54:CAN2 Port4, 55:CAN2 Port5, 56:CAN2 Port6, 57:CAN2 Port7, 58:CAN2 Port8, 59:CAN2 Port9 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("EP1", 2, Networking_Periph::Passthru, ep1, -1), + + // @Param: EP2 + // @DisplayName: Endpoint 2 + // @Description: Passthrough Endpoint 2. This can be a serial port UART, a Network port, or a CAN port. The selected port will route to Endport 1. + // @CopyFieldsFrom: NET_PASS1_EP1 + AP_GROUPINFO("EP2", 3, Networking_Periph::Passthru, ep2, -1), + + // @Param: BAUD1 + // @DisplayName: Endpoint 1 Baud Rate + // @Description: The baud rate used for Endpoint 1. Only applies to serial ports. + // @CopyFieldsFrom: SERIAL1_BAUD + AP_GROUPINFO("BAUD1", 4, Networking_Periph::Passthru, baud1, 115200), + + // @Param: BAUD2 + // @DisplayName: Endpoint 2 Baud Rate + // @Description: The baud rate used for Endpoint 2. Only applies to serial ports. + // @CopyFieldsFrom: SERIAL1_BAUD + AP_GROUPINFO("BAUD2", 5, Networking_Periph::Passthru, baud2, 115200), + + // @Param: OPT1 + // @DisplayName: Serial Port Options EP1 + // @Description: Control over UART options for Endpoint 1. Only applies to serial ports. + // @CopyFieldsFrom: SERIAL1_OPTIONS + AP_GROUPINFO("OPT1", 6, Networking_Periph::Passthru, options1, 0), + + // @Param: OPT2 + // @DisplayName: Serial Port Options EP2 + // @Description: Control over UART options for Endpoint 2. Only applies to serial ports. + // @CopyFieldsFrom: SERIAL1_OPTIONS + AP_GROUPINFO("OPT2", 7, Networking_Periph::Passthru, options2, 0), + + AP_GROUPEND +}; + +void Networking_Periph::Passthru::init() +{ + if (enabled == 0) { + // Feature is disabled + return; + } + + if (port1 != nullptr || port2 != nullptr) { + // The ports have already been initialized, nothing to do. + return; + } + + if (ep1 <= -1 || ep2 <= -1 || ep1 == ep2) { + // end points are not set or are the same. Can't route to self + return; + } + + port1 = AP::serialmanager().get_serial_by_id(ep1); + port2 = AP::serialmanager().get_serial_by_id(ep2); + + if (port1 != nullptr && port2 != nullptr) { + port1->set_options(options1); + port1->begin(baud1); + + port2->set_options(options2); + port2->begin(baud2); + } +} + +void Networking_Periph::Passthru::update() +{ + if (enabled == 0 || port1 == nullptr || port2 == nullptr) { + return; + } + + // Fastest possible connection is 3Mbps serial port, which is roughly 300KB/s payload and we service this at <= 1kHz + // Raising this any higher just causes excess stack usage which never gets used. + uint8_t buf[300]; + + // read from port1, and write to port2 + auto avail = port1->available(); + if (avail > 0) { + auto space = port2->txspace(); + const uint32_t n = MIN(space, sizeof(buf)); + const auto nbytes = port1->read(buf, n); + if (nbytes > 0) { + port2->write(buf, nbytes); + } + } + + // read from port2, and write to port1 + avail = port2->available(); + if (avail > 0) { + auto space = port1->txspace(); + const uint32_t n = MIN(space, sizeof(buf)); + const auto nbytes = port2->read(buf, n); + if (nbytes > 0) { + port1->write(buf, nbytes); + } + } +} + +#endif // defined(HAL_PERIPH_ENABLE_NETWORKING) && HAL_PERIPH_NETWORK_NUM_PASSTHRU > 0 + diff --git a/Tools/AP_Periph/proximity.cpp b/Tools/AP_Periph/proximity.cpp new file mode 100644 index 0000000000000..2c88d3fc0f98a --- /dev/null +++ b/Tools/AP_Periph/proximity.cpp @@ -0,0 +1,75 @@ +#include "AP_Periph.h" + +#ifdef HAL_PERIPH_ENABLE_PROXIMITY + +/* + proximity support + */ + +#include + +void AP_Periph_FW::can_proximity_update() +{ + if (proximity.get_type(0) == AP_Proximity::Type::None) { + return; + } + + uint32_t now = AP_HAL::millis(); + static uint32_t last_update_ms; + if (g.proximity_max_rate > 0 && + now - last_update_ms < 1000/g.proximity_max_rate) { + // limit to max rate + return; + } + last_update_ms = now; + proximity.update(); + AP_Proximity::Status status = proximity.get_status(); + if (status <= AP_Proximity::Status::NoData) { + // don't send any data + return; + } + + ardupilot_equipment_proximity_sensor_Proximity pkt {}; + + const uint8_t obstacle_count = proximity.get_obstacle_count(); + + // if no objects return + if (obstacle_count == 0) { + return; + } + + // calculate maximum roll, pitch values from objects + for (uint8_t i=0; i + +#ifndef AP_PERIPH_PROBE_CONTINUOUS +#define AP_PERIPH_PROBE_CONTINUOUS 0 +#endif + +/* + update CAN rangefinder + */ +void AP_Periph_FW::can_rangefinder_update(void) +{ + if (rangefinder.get_type(0) == RangeFinder::Type::NONE) { + return; + } +#if AP_PERIPH_PROBE_CONTINUOUS + if (rangefinder.num_sensors() == 0) { + uint32_t now = AP_HAL::millis(); + static uint32_t last_probe_ms; + if (now - last_probe_ms >= 1000) { + last_probe_ms = now; + rangefinder.init(ROTATION_NONE); + } + } +#endif + uint32_t now = AP_HAL::millis(); + static uint32_t last_update_ms; + if (g.rangefinder_max_rate > 0 && + now - last_update_ms < uint32_t(1000/g.rangefinder_max_rate)) { + // limit to max rate + return; + } + last_update_ms = now; + rangefinder.update(); + RangeFinder::Status status = rangefinder.status_orient(ROTATION_NONE); + if (status <= RangeFinder::Status::NoData) { + // don't send any data + return; + } + const uint32_t sample_ms = rangefinder.last_reading_ms(ROTATION_NONE); + if (last_sample_ms == sample_ms) { + return; + } + last_sample_ms = sample_ms; + + uint16_t dist_cm = rangefinder.distance_cm_orient(ROTATION_NONE); + uavcan_equipment_range_sensor_Measurement pkt {}; + pkt.sensor_id = rangefinder.get_address(0); + switch (status) { + case RangeFinder::Status::OutOfRangeLow: + pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_CLOSE; + break; + case RangeFinder::Status::OutOfRangeHigh: + pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_FAR; + break; + case RangeFinder::Status::Good: + pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_VALID_RANGE; + break; + default: + pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_UNDEFINED; + break; + } + switch (rangefinder.get_mav_distance_sensor_type_orient(ROTATION_NONE)) { + case MAV_DISTANCE_SENSOR_LASER: + pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_LIDAR; + break; + case MAV_DISTANCE_SENSOR_ULTRASOUND: + pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_SONAR; + break; + case MAV_DISTANCE_SENSOR_RADAR: + pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_RADAR; + break; + default: + pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_UNDEFINED; + break; + } + + pkt.range = dist_cm * 0.01; + + uint8_t buffer[UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE] {}; + uint16_t total_size = uavcan_equipment_range_sensor_Measurement_encode(&pkt, buffer, !periph.canfdout()); + + canard_broadcast(UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE, + UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); +} + +#endif // HAL_PERIPH_ENABLE_RANGEFINDER diff --git a/Tools/AP_Periph/rc_in.cpp b/Tools/AP_Periph/rc_in.cpp new file mode 100644 index 0000000000000..63a584d684b82 --- /dev/null +++ b/Tools/AP_Periph/rc_in.cpp @@ -0,0 +1,179 @@ +/* + 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 . + */ + +#include + +#ifdef HAL_PERIPH_ENABLE_RCIN + +#ifndef AP_PERIPH_RC1_PORT_DEFAULT +#define AP_PERIPH_RC1_PORT_DEFAULT -1 +#endif + +#ifndef AP_PERIPH_RC1_PORT_OPTIONS_DEFAULT +#define AP_PERIPH_RC1_PORT_OPTIONS_DEFAULT 0 +#endif + +#include +#include "AP_Periph.h" +#include + +extern const AP_HAL::HAL &hal; + +const AP_Param::GroupInfo Parameters_RCIN::var_info[] { + // RC_PROTOCOLS copied from RC_Channel/RC_Channels_Varinfo.h + // @Param: _PROTOCOLS + // @DisplayName: RC protocols enabled + // @Description: Bitmask of enabled RC protocols. Allows narrowing the protocol detection to only specific types of RC receivers which can avoid issues with incorrect detection. Set to 1 to enable all protocols. + // @User: Advanced + // @Bitmask: 0:All,1:PPM,2:IBUS,3:SBUS,4:SBUS_NI,5:DSM,6:SUMD,7:SRXL,8:SRXL2,9:CRSF,10:ST24,11:FPORT,12:FPORT2,13:FastSBUS + AP_GROUPINFO("_PROTOCOLS", 1, Parameters_RCIN, rcin_protocols, 1), + + // RC_PROTOCOLS copied from RC_Channel/RC_Channels_Varinfo.h + // @Param: _MSGRATE + // @DisplayName: DroneCAN RC Message rate + // @Description: Rate at which RC input is sent via DroneCAN + // @User: Advanced + // @Increment: 1 + // @Range: 0 255 + // @Units: Hz + AP_GROUPINFO("_MSGRATE", 2, Parameters_RCIN, rcin_rate_hz, 50), + + // @Param: 1_PORT + // @DisplayName: RC input port + // @Description: This is the serial port number where SERIALx_PROTOCOL will be set to RC input. + // @Range: 0 10 + // @Increment: 1 + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("_PORT", 3, Parameters_RCIN, rcin1_port, AP_PERIPH_RC1_PORT_DEFAULT), + + // @Param: 1_PORT_OPTIONS + // @DisplayName: RC input port serial options + // @Description: Control over UART options. The InvertRX option controls invert of the receive pin. The InvertTX option controls invert of the transmit pin. The HalfDuplex option controls half-duplex (onewire) mode, where both transmit and receive is done on the transmit wire. The Swap option allows the RX and TX pins to be swapped on STM32F7 based boards. + // @Bitmask: 0:InvertRX, 1:InvertTX, 2:HalfDuplex, 3:SwapTXRX, 4: RX_PullDown, 5: RX_PullUp, 6: TX_PullDown, 7: TX_PullUp, 8: RX_NoDMA, 9: TX_NoDMA, 10: Don't forward mavlink to/from, 11: DisableFIFO, 12: Ignore Streamrate + AP_GROUPINFO("1_PORT_OPTIONS", 4, Parameters_RCIN, rcin1_port_options, AP_PERIPH_RC1_PORT_OPTIONS_DEFAULT), + // @RebootRequired: True + + AP_GROUPEND +}; + +Parameters_RCIN::Parameters_RCIN(void) +{ + AP_Param::setup_object_defaults(this, var_info); +} + +void AP_Periph_FW::rcin_init() +{ + if (g_rcin.rcin1_port < 0) { + return; + } + + // init uart for serial RC + auto *uart = hal.serial(g_rcin.rcin1_port); + if (uart == nullptr) { + return; + } + + uart->set_options(g_rcin.rcin1_port_options); + + serial_manager.set_protocol_and_baud( + g_rcin.rcin1_port, + AP_SerialManager::SerialProtocol_RCIN, + 115200 // baud doesn't matter; RC Protocol autobauds + ); + + auto &rc = AP::RC(); + rc.init(); + rc.set_rc_protocols(g_rcin.rcin_protocols); + rc.add_uart(uart); + + rcin_initialised = true; +} + +void AP_Periph_FW::rcin_update() +{ + if (!rcin_initialised) { + return; + } + + auto &rc = AP::RC(); + if (!rc.new_input()) { + return; + } + + // log discovered protocols: + auto new_rc_protocol = rc.protocol_name(); + if (new_rc_protocol != rcin_rc_protocol) { + can_printf("Decoding (%s)", new_rc_protocol); + rcin_rc_protocol = new_rc_protocol; + } + + // decimate the input to a parameterized rate + const uint8_t rate_hz = g_rcin.rcin_rate_hz; + if (rate_hz == 0) { + return; + } + + const auto now_ms = AP_HAL::millis(); + const auto interval_ms = 1000U / rate_hz; + if (now_ms - rcin_last_sent_RCInput_ms < interval_ms) { + return; + } + rcin_last_sent_RCInput_ms = now_ms; + + // extract data and send CAN packet: + const uint8_t num_channels = rc.num_channels(); + uint16_t channels[MAX_RCIN_CHANNELS]; + rc.read(channels, num_channels); + const int16_t rssi = rc.get_RSSI(); + + can_send_RCInput((uint8_t)rssi, channels, num_channels, rc.failsafe_active(), rssi > 0 && rssi <256); +} + +/* + send an RCInput CAN message + */ +void AP_Periph_FW::can_send_RCInput(uint8_t quality, uint16_t *values, uint8_t nvalues, bool in_failsafe, bool quality_valid) +{ + uint16_t status = 0; + if (quality_valid) { + status |= DRONECAN_SENSORS_RC_RCINPUT_STATUS_QUALITY_VALID; + } + if (in_failsafe) { + status |= DRONECAN_SENSORS_RC_RCINPUT_STATUS_FAILSAFE; + } + + // assemble packet + dronecan_sensors_rc_RCInput pkt {}; + pkt.quality = quality; + pkt.status = status; + pkt.rcin.len = nvalues; + for (uint8_t i=0; iforce_safety_on(); +#else + hal.rcout->force_safety_off(); +#endif #if HAL_WITH_ESC_TELEM && !HAL_GCS_ENABLED if (g.esc_telem_port >= 0) { @@ -132,7 +136,8 @@ void AP_Periph_FW::rcout_update() const bool has_esc_rawcommand_timed_out = esc_timeout_ms != 0 && ((now_ms - last_esc_raw_command_ms) >= esc_timeout_ms); if (last_esc_num_channels > 0 && has_esc_rawcommand_timed_out) { // If we've seen ESCs previously, and a timeout has occurred, then zero the outputs - int16_t esc_output[last_esc_num_channels] {}; + int16_t esc_output[last_esc_num_channels]; + memset(esc_output, 0, sizeof(esc_output)); rcout_esc(esc_output, last_esc_num_channels); // register that the output has been changed diff --git a/Tools/AP_Periph/serial_tunnel.cpp b/Tools/AP_Periph/serial_tunnel.cpp new file mode 100644 index 0000000000000..cdba23b41c46d --- /dev/null +++ b/Tools/AP_Periph/serial_tunnel.cpp @@ -0,0 +1,214 @@ +/* + 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 . + */ +/* + handle tunnelling of serial data over DroneCAN + */ + +#include +#include "AP_Periph.h" + +#if AP_UART_MONITOR_ENABLED + +#include + +extern const AP_HAL::HAL &hal; + +#define TUNNEL_LOCK_KEY 0xf2e460e4U + +#ifndef TUNNEL_DEBUG +#define TUNNEL_DEBUG 0 +#endif + +#if TUNNEL_DEBUG +# define debug(fmt, args...) can_printf(fmt "\n", ##args) +#else +# define debug(fmt, args...) +#endif + +/* + get the default port to tunnel if the client requests port -1 + */ +int8_t AP_Periph_FW::get_default_tunnel_serial_port(void) const +{ + int8_t uart_num = -1; +#ifdef HAL_PERIPH_ENABLE_GPS + if (uart_num == -1) { + uart_num = g.gps_port; + } +#endif +#ifdef HAL_PERIPH_ENABLE_RANGEFINDER + if (uart_num == -1) { + uart_num = g.rangefinder_port; + } +#endif +#ifdef HAL_PERIPH_ENABLE_ADSB + if (uart_num == -1) { + uart_num = g.adsb_port; + } +#endif +#ifdef HAL_PERIPH_ENABLE_PROXIMITY + if (uart_num == -1) { + uart_num = g.proximity_port; + } +#endif + return uart_num; +} + +/* + handle tunnel data + */ +void AP_Periph_FW::handle_tunnel_Targetted(CanardInstance* canard_ins, CanardRxTransfer* transfer) +{ + uavcan_tunnel_Targetted pkt; + if (uavcan_tunnel_Targetted_decode(transfer, &pkt)) { + return; + } + if (pkt.target_node != canardGetLocalNodeID(canard_ins)) { + return; + } + if (uart_monitor.buffer == nullptr) { + uart_monitor.buffer = new ByteBuffer(1024); + if (uart_monitor.buffer == nullptr) { + return; + } + } + int8_t uart_num = pkt.serial_id; + if (uart_num == -1) { + uart_num = get_default_tunnel_serial_port(); + } + if (uart_num < 0) { + return; + } + auto *uart = hal.serial(uart_num); + if (uart == nullptr) { + return; + } + if (uart_monitor.uart_num != uart_num && uart_monitor.uart != nullptr) { + // remove monitor from previous uart + hal.serial(uart_monitor.uart_num)->set_monitor_read_buffer(nullptr); + } + uart_monitor.uart_num = uart_num; + if (uart != uart_monitor.uart) { + // change of uart or expired, clear old data + uart_monitor.buffer->clear(); + uart_monitor.uart = uart; + uart_monitor.baudrate = 0; + } + if (uart_monitor.uart == nullptr) { + return; + } + /* + allow for locked state to change at any time, so users can + switch between locked and unlocked while connected + */ + const bool was_locked = uart_monitor.locked; + uart_monitor.locked = (pkt.options & UAVCAN_TUNNEL_TARGETTED_OPTION_LOCK_PORT) != 0; + if (uart_monitor.locked) { + uart_monitor.uart->lock_port(TUNNEL_LOCK_KEY, TUNNEL_LOCK_KEY); + } else { + uart_monitor.uart->lock_port(0,0); + } + uart_monitor.node_id = transfer->source_node_id; + uart_monitor.protocol = pkt.protocol.protocol; + if (pkt.baudrate != uart_monitor.baudrate || !was_locked) { + if (uart_monitor.locked && pkt.baudrate != 0) { + // ensure we have enough buffer space for a uBlox fw update and fast uCenter data + uart_monitor.uart->begin_locked(pkt.baudrate, 2048, 2048, TUNNEL_LOCK_KEY); + debug("begin_locked %u", unsigned(pkt.baudrate)); + } + uart_monitor.baudrate = pkt.baudrate; + } + uart_monitor.uart->set_monitor_read_buffer(uart_monitor.buffer); + uart_monitor.last_request_ms = AP_HAL::millis(); + + // write to device + if (pkt.buffer.len > 0) { + if (uart_monitor.locked) { + debug("write_locked %u", unsigned(pkt.buffer.len)); + uart_monitor.uart->write_locked(pkt.buffer.data, pkt.buffer.len, TUNNEL_LOCK_KEY); + } else { + uart_monitor.uart->write(pkt.buffer.data, pkt.buffer.len); + } + } else { + debug("locked keepalive"); + } +} + +/* + send tunnelled serial data + */ +void AP_Periph_FW::send_serial_monitor_data() +{ + if (uart_monitor.uart == nullptr || + uart_monitor.node_id == 0 || + uart_monitor.buffer == nullptr) { + return; + } + const uint32_t last_req_ms = uart_monitor.last_request_ms; + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - last_req_ms >= 3000) { + // stop sending and unlock, but don't release the buffer + if (uart_monitor.locked) { + debug("unlock"); + uart_monitor.uart->lock_port(0, 0); + } + uart_monitor.uart = nullptr; + return; + } + if (uart_monitor.locked) { + /* + when the port is locked nobody is reading the uart so the + monitor doesn't fill. We read here to ensure it fills + */ + uint8_t buf[120]; + for (uint8_t i=0; i<8; i++) { + if (uart_monitor.uart->read_locked(buf, sizeof(buf), TUNNEL_LOCK_KEY) <= 0) { + break; + } + } + } + uint8_t sends = 8; + while (uart_monitor.buffer->available() > 0 && sends-- > 0) { + uint32_t n; + const uint8_t *buf = uart_monitor.buffer->readptr(n); + if (n == 0) { + return; + } + // broadcast data as tunnel packets, can be used for uCenter debug and device fw update + uavcan_tunnel_Targetted pkt {}; + n = MIN(n, sizeof(pkt.buffer.data)); + pkt.target_node = uart_monitor.node_id; + pkt.protocol.protocol = uart_monitor.protocol; + pkt.buffer.len = n; + pkt.baudrate = uart_monitor.baudrate; + pkt.serial_id = uart_monitor.uart_num; + memcpy(pkt.buffer.data, buf, n); + + uint8_t buffer[UAVCAN_TUNNEL_TARGETTED_MAX_SIZE] {}; + const uint16_t total_size = uavcan_tunnel_Targetted_encode(&pkt, buffer, !canfdout()); + + debug("read %u", unsigned(n)); + + if (!canard_broadcast(UAVCAN_TUNNEL_TARGETTED_SIGNATURE, + UAVCAN_TUNNEL_TARGETTED_ID, + CANARD_TRANSFER_PRIORITY_MEDIUM, + &buffer[0], + total_size)) { + break; + } + uart_monitor.buffer->advance(n); + } +} +#endif // AP_UART_MONITOR_ENABLED diff --git a/Tools/AP_Periph/version.h b/Tools/AP_Periph/version.h index 84aad587c28b2..090841e1d57fd 100644 --- a/Tools/AP_Periph/version.h +++ b/Tools/AP_Periph/version.h @@ -2,15 +2,17 @@ #include -#define THISFIRMWARE "AP_Periph V1.5.0-dev" +#define THISFIRMWARE "AP_Periph V1.7.0-dev" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 1,5,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 1,7,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 1 -#define FW_MINOR 5 +#define FW_MINOR 7 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV + + diff --git a/Tools/AP_Periph/wscript b/Tools/AP_Periph/wscript index d6a733b5ffafd..da728bdcf4e28 100644 --- a/Tools/AP_Periph/wscript +++ b/Tools/AP_Periph/wscript @@ -7,7 +7,7 @@ import sys try: import em except ImportError: - print("you need to install empy with 'python -m pip install empy'") + print("you need to install empy with 'python -m pip install empy==3.3.4'") sys.exit(1) try: @@ -38,11 +38,13 @@ def build(bld): 'AP_BoardConfig', 'AP_BattMonitor', 'AP_CANManager', + 'AP_KDECAN', 'AP_Param', 'StorageManager', 'AP_FlashStorage', 'AP_RAMTRON', 'AP_GPS', + 'AP_Networking', 'AP_SerialManager', 'AP_RTC', 'AP_Compass', @@ -68,8 +70,19 @@ def build(bld): 'AP_Stats', 'AP_EFI', 'AP_CheckFirmware', + 'AP_RPM', 'AP_Proximity', + 'AP_RCProtocol', + 'AP_AHRS', + 'AP_Terrain', + 'AP_Torqeedo', + 'AP_Volz_Protocol', + 'AP_SBusOut', + 'AP_RobotisServo', + 'AP_FETtecOneWire', + 'GCS_MAVLink', ] + bld.ap_stlib( name= 'AP_Periph_libs', dynamic_source='modules/DroneCAN/libcanard/dsdlc_generated/src/**.c', diff --git a/Tools/CPUInfo/CPUInfo.cpp b/Tools/CPUInfo/CPUInfo.cpp index 73d1c6a45a9cf..a20db4924d95d 100644 --- a/Tools/CPUInfo/CPUInfo.cpp +++ b/Tools/CPUInfo/CPUInfo.cpp @@ -25,7 +25,8 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); #if CONFIG_HAL_BOARD != HAL_BOARD_LINUX // On H750 we want to measure external flash to ram performance -#if defined(EXT_FLASH_SIZE_MB) && defined(STM32H7) +#if defined(EXT_FLASH_SIZE_MB) && EXT_FLASH_SIZE_MB>0 && defined(STM32H7) +#include "ch.h" #define DISABLE_CACHES #endif @@ -44,7 +45,9 @@ AP_ESC_Telem telem; void setup() { #ifdef DISABLE_CACHES +#if !HAL_XIP_ENABLED // can't disable DCache in memory-mapped mode SCB_DisableDCache(); +#endif SCB_DisableICache(); #endif ekf.init(); @@ -97,6 +100,7 @@ volatile uint8_t mbuf1[128], mbuf2[128]; volatile uint64_t v_64 = 1; volatile uint64_t v_out_64 = 1; +#pragma GCC diagnostic error "-Wframe-larger-than=2000" static void show_timings(void) { diff --git a/Tools/CPUInfo/output-PixFlamingo.txt b/Tools/CPUInfo/output-PixFlamingo.txt new file mode 100644 index 0000000000000..b33cc18a18bd2 --- /dev/null +++ b/Tools/CPUInfo/output-PixFlamingo.txt @@ -0,0 +1,71 @@ +SYSCLK 120MHz +Type sizes: +char : 1 +short : 2 +int : 4 +long : 4 +long long : 8 +bool : 1 +void* : 4 +printing NaN: nan +printing +Inf: inf +printing -Inf: -inf + +Operation timings: +Note: timings for some operations are very data dependent +nop 0.0105 usec/call +micros() 0.9710 usec/call +micros16() 0.1179 usec/call +millis() 2.0830 usec/call +millis16() 2.1672 usec/call +micros64() 1.0273 usec/call +fadd 0.0648 usec/call +fsub 0.0630 usec/call +fmul 0.0656 usec/call +fdiv /= 0.1738 usec/call +fdiv 2/x 0.1668 usec/call +dadd 0.8870 usec/call +dsub 0.8592 usec/call +dmul 0.4650 usec/call +ddiv 0.5958 usec/call +sinf() 1.3428 usec/call +cosf() 1.2680 usec/call +arm_sin_f32() 0.4202 usec/call +arm_cos_f32() 0.4472 usec/call +tanf() 2.0798 usec/call +acosf() 1.3822 usec/call +asinf() 1.4354 usec/call +atan2f() 2.0736 usec/call +sqrtf() 0.1790 usec/call +sin() 1.3470 usec/call +cos() 1.2674 usec/call +tan() 2.0820 usec/call +acos() 1.3848 usec/call +asin() 1.4472 usec/call +atan2() 2.0726 usec/call +sqrt() 0.1834 usec/call +arm_sqrt_f32() 0.2216 usec/call +sq() 0.0490 usec/call +powf(v,2) 0.0478 usec/call +powf(v,3.1) 6.9318 usec/call +EKF 48.7400 usec/call +iadd8 0.0854 usec/call +isub8 0.0850 usec/call +imul8 0.0820 usec/call +idiv8 0.0870 usec/call +iadd16 0.0584 usec/call +isub16 0.0630 usec/call +imul16 0.0602 usec/call +idiv16 0.1202 usec/call +iadd32 0.0478 usec/call +isub32 0.0544 usec/call +imul32 0.0496 usec/call +idiv32 0.0884 usec/call +iadd64 0.0990 usec/call +isub64 0.1028 usec/call +imul64 0.1518 usec/call +idiv64 0.9588 usec/call +memcpy128 0.9161 usec/call +memset128 1.0854 usec/call +delay(1) 1018.43 usec/call +SEM 2.2314 usec/call diff --git a/Tools/CPUInfo/output-esp32-classic.txt b/Tools/CPUInfo/output-esp32-classic.txt new file mode 100644 index 0000000000000..54d9c2e446169 --- /dev/null +++ b/Tools/CPUInfo/output-esp32-classic.txt @@ -0,0 +1,94 @@ +rst:0x10 (RTCWDT_RTC_RESET),boot:0x13 (SPI_FAST_FLASH_BOOT) +configsip: 0, SPIWP:0xee +clk_drv:0x00,q_drv:0x00,d_drv:0x00,cs0_drv:0x00,hd_drv:0x00,wp_drv:0x00 +mode:DIO, clock div:1 +load:0x3fff0030,len:5232 +load:0x40078000,len:14264 +load:0x40080400,len:4244 +entry 0x40080664 +I (334) cpu_start: Pro cpu up. +I (334) cpu_start: Application information: +I (334) cpu_start: ELF file SHA256: 69635451ea60f4bd... +I (337) cpu_start: ESP-IDF: v4.2.4-209-g527a23d63f +I (344) cpu_start: Starting app cpu, entry point is 0x40081c50 +I (0) cpu_start: App cpu up. +I (354) heap_init: Initializing. RAM available for dynamic allocation: +I (361) heap_init: At 3FFAE6E0 len 00001920 (6 KiB): DRAM +I (367) heap_init: At 3FFBF160 len 00020EA0 (131 KiB): DRAM +I (373) heap_init: At 3FFE0440 len 00003AE0 (14 KiB): D/IRAM +I (379) heap_init: At 3FFE4350 len 0001BCB0 (111 KiB): D/IRAM +I (386) heap_init: At 400954F8 len 0000AB08 (42 KiB): IRAM +I (392) cpu_start: Pro cpu start user code +I (410) spi_flash: detected chip: generic +I (410) spi_flash: flash io: qio +I (410) esp_core_dump_uart: Init core dump to UART +I (413) cpu_start: Starting scheduler on PRO CPU. +I (0) cpu_start: Starting scheduler on APP CPU. +SYSCLK 0MHz +Type sizes: +char : 1 +short : 2 +int : 4 +long : 4 +long long : 8 +bool : 1 +void* : 4 +printing NaN: nan +printing +Inf: inf +printing -Inf: -inf + +Operation timings: +Note: timings for some operations are very data dependent +nop 0.0049 usec/call +micros() 0.5432 usec/call +micros16() 0.5979 usec/call +millis() 0.9198 usec/call +millis16() 0.9713 usec/call +micros64() 0.5536 usec/call +fadd 0.0634 usec/call +fsub 0.0624 usec/call +fmul 0.0624 usec/call +fdiv /= 0.2504 usec/call +fdiv 2/x 0.2428 usec/call +dadd 0.2940 usec/call +dsub 0.3306 usec/call +dmul 0.2638 usec/call +ddiv 0.2430 usec/call +sinf() 0.9574 usec/call +cosf() 0.8554 usec/call +tanf() 1.4338 usec/call +acosf() 2.1064 usec/call +asinf() 1.7080 usec/call +atan2f() 3.0798 usec/call +sqrtf() 0.4264 usec/call +sin() 0.9496 usec/call +cos() 0.8504 usec/call +tan() 1.4314 usec/call +acos() 12.1722 usec/call +asin() 11.5936 usec/call +atan2() 3.8804 usec/call +sqrt() 0.4268 usec/call +sq() 0.0516 usec/call +powf(v,2) 0.0516 usec/call +powf(v,3.1) 3.0752 usec/call +EKF 16.2000 usec/call +iadd8 0.0674 usec/call +isub8 0.0676 usec/call +imul8 0.0718 usec/call +idiv8 0.0862 usec/call +iadd16 0.0672 usec/call +isub16 0.0676 usec/call +imul16 0.0718 usec/call +idiv16 0.0852 usec/call +iadd32 0.0610 usec/call +isub32 0.0614 usec/call +imul32 0.0612 usec/call +idiv32 0.0654 usec/call +iadd64 0.1232 usec/call +isub64 0.1232 usec/call +imul64 0.1712 usec/call +idiv64 0.3848 usec/call +memcpy128 0.5094 usec/call +memset128 0.3917 usec/call +delay(1) 164.9920 usec/call +SEM 6.1196 usec/call diff --git a/Tools/GIT_Test/GIT_Success.txt b/Tools/GIT_Test/GIT_Success.txt index cce4dddf53219..9f00eea02a121 100644 --- a/Tools/GIT_Test/GIT_Success.txt +++ b/Tools/GIT_Test/GIT_Success.txt @@ -173,4 +173,8 @@ Ava Falkner Emre Can Suiçmez Yacine Thabet - 57 Greg Poulos -Torre Orazio \ No newline at end of file +Torre Orazio +seba czapnik +Ramy Gad +Matthew R. Cunningham +prathap devendran (Tamil) india, 11-23 diff --git a/Tools/IO_Firmware/iofirmware_dshot_highpolh.bin b/Tools/IO_Firmware/iofirmware_dshot_highpolh.bin new file mode 100755 index 0000000000000..83a7e2b629969 Binary files /dev/null and b/Tools/IO_Firmware/iofirmware_dshot_highpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_dshot_lowpolh.bin b/Tools/IO_Firmware/iofirmware_dshot_lowpolh.bin new file mode 100755 index 0000000000000..dbc520edd07a6 Binary files /dev/null and b/Tools/IO_Firmware/iofirmware_dshot_lowpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_f103_8MHz_highpolh.bin b/Tools/IO_Firmware/iofirmware_f103_8MHz_highpolh.bin index 6e4fbea04f5f5..a722bc5bd2ee9 100755 Binary files a/Tools/IO_Firmware/iofirmware_f103_8MHz_highpolh.bin and b/Tools/IO_Firmware/iofirmware_f103_8MHz_highpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_f103_8MHz_lowpolh.bin b/Tools/IO_Firmware/iofirmware_f103_8MHz_lowpolh.bin index 97759cc3419c3..cdb40f2fab870 100755 Binary files a/Tools/IO_Firmware/iofirmware_f103_8MHz_lowpolh.bin and b/Tools/IO_Firmware/iofirmware_f103_8MHz_lowpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_f103_dshot_highpolh.bin b/Tools/IO_Firmware/iofirmware_f103_dshot_highpolh.bin new file mode 100755 index 0000000000000..3d993264ce869 Binary files /dev/null and b/Tools/IO_Firmware/iofirmware_f103_dshot_highpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_f103_dshot_lowpolh.bin b/Tools/IO_Firmware/iofirmware_f103_dshot_lowpolh.bin new file mode 100755 index 0000000000000..ef5755a62abef Binary files /dev/null and b/Tools/IO_Firmware/iofirmware_f103_dshot_lowpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_f103_highpolh.bin b/Tools/IO_Firmware/iofirmware_f103_highpolh.bin new file mode 100755 index 0000000000000..89d0449fb1256 Binary files /dev/null and b/Tools/IO_Firmware/iofirmware_f103_highpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_f103_lowpolh.bin b/Tools/IO_Firmware/iofirmware_f103_lowpolh.bin new file mode 100755 index 0000000000000..2179b5cbc5efb Binary files /dev/null and b/Tools/IO_Firmware/iofirmware_f103_lowpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_highpolh.bin b/Tools/IO_Firmware/iofirmware_highpolh.bin index 414d51ef0e554..4e2b62dec6a67 100755 Binary files a/Tools/IO_Firmware/iofirmware_highpolh.bin and b/Tools/IO_Firmware/iofirmware_highpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_lowpolh.bin b/Tools/IO_Firmware/iofirmware_lowpolh.bin index 114b8101dad94..3ba40363bcd4a 100755 Binary files a/Tools/IO_Firmware/iofirmware_lowpolh.bin and b/Tools/IO_Firmware/iofirmware_lowpolh.bin differ diff --git a/Tools/LogAnalyzer/DataflashLog.py b/Tools/LogAnalyzer/DataflashLog.py index 38fc82b21943d..a96cca589cecd 100644 --- a/Tools/LogAnalyzer/DataflashLog.py +++ b/Tools/LogAnalyzer/DataflashLog.py @@ -169,7 +169,7 @@ def to_class(self): _pack_=True, ) - if type(self.types[0]) == str: + if isinstance(self.types[0], str): fieldtypes = [i for i in self.types] else: fieldtypes = [chr(i) for i in self.types] @@ -756,7 +756,7 @@ def read_text(self, f, ignoreBadlines): else: raise Exception("") else: - if not tokens[0] in self.formats: + if tokens[0] not in self.formats: raise ValueError("Unknown Format {}".format(tokens[0])) e = self.formats[tokens[0]](*tokens[1:]) self.process(lineNumber, e) diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp index 8b9d2bd431511..cbdca233afaf6 100644 --- a/Tools/Replay/LR_MsgHandler.cpp +++ b/Tools/Replay/LR_MsgHandler.cpp @@ -289,6 +289,12 @@ void LR_MsgHandler_REPH::process_message(uint8_t *msgbytes) AP::dal().handle_message(msg, ekf2, ekf3); } +void LR_MsgHandler_RSLL::process_message(uint8_t *msgbytes) +{ + MSG_CREATE(RSLL, msgbytes); + AP::dal().handle_message(msg, ekf2, ekf3); +} + void LR_MsgHandler_REVH::process_message(uint8_t *msgbytes) { MSG_CREATE(REVH, msgbytes); diff --git a/Tools/Replay/LR_MsgHandler.h b/Tools/Replay/LR_MsgHandler.h index b258f496e728a..4e30a6571d35c 100644 --- a/Tools/Replay/LR_MsgHandler.h +++ b/Tools/Replay/LR_MsgHandler.h @@ -57,6 +57,12 @@ class LR_MsgHandler_REPH : public LR_MsgHandler_EKF void process_message(uint8_t *msg) override; }; +class LR_MsgHandler_RSLL : public LR_MsgHandler_EKF +{ + using LR_MsgHandler_EKF::LR_MsgHandler_EKF; + void process_message(uint8_t *msg) override; +}; + class LR_MsgHandler_REVH : public LR_MsgHandler_EKF { using LR_MsgHandler_EKF::LR_MsgHandler_EKF; diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index d36c3da3fe9e1..9a84053ab81ad 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -118,6 +118,8 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f) msgparser[f.type] = new LR_MsgHandler_ROFH(formats[f.type], ekf2, ekf3); } else if (streq(name, "REPH")) { msgparser[f.type] = new LR_MsgHandler_REPH(formats[f.type], ekf2, ekf3); + } else if (streq(name, "RSLL")) { + msgparser[f.type] = new LR_MsgHandler_RSLL(formats[f.type], ekf2, ekf3); } else if (streq(name, "REVH")) { msgparser[f.type] = new LR_MsgHandler_REVH(formats[f.type], ekf2, ekf3); } else if (streq(name, "RWOH")) { diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp index 63c0cfba19106..a55a33a799d1b 100644 --- a/Tools/Replay/Replay.cpp +++ b/Tools/Replay/Replay.cpp @@ -100,8 +100,10 @@ const struct AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { }; GCS_Dummy _gcs; +#if AP_ADVANCEDFAILSAFE_ENABLED AP_AdvancedFailsafe *AP::advancedfailsafe() { return nullptr; } bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate, const char *reason) { return false; } +#endif // dummy method to avoid linking AP_Avoidance // AP_Avoidance *AP::ap_avoidance() { return nullptr; } diff --git a/Tools/Replay/check_replay.py b/Tools/Replay/check_replay.py index 32a49ea5556a1..dbb0a0bf0203c 100755 --- a/Tools/Replay/check_replay.py +++ b/Tools/Replay/check_replay.py @@ -40,7 +40,7 @@ def check_log(logfile, progress=print, ekf2_only=False, ekf3_only=False, verbose if not hasattr(m,'C'): continue mtype = m.get_type() - if not mtype in counts: + if mtype not in counts: counts[mtype] = 0 base_counts[mtype] = 0 core = m.C diff --git a/Tools/ardupilotwaf/ap_library.py b/Tools/ardupilotwaf/ap_library.py index d8fdd4c05ca10..b49010ae06001 100644 --- a/Tools/ardupilotwaf/ap_library.py +++ b/Tools/ardupilotwaf/ap_library.py @@ -239,6 +239,25 @@ def __str__(self): def keyword(self): return 'Checking included headers' +def custom_flags_check(tgen): + ''' + check for tasks marked as having custom cpp or c flags + a library can do this by setting AP_LIB_EXTRA_CXXFLAGS and AP_LIB_EXTRA_CFLAGS + + For example add this is the configure section of the library, using AP_DDS as an example: + + cfg.env.AP_LIB_EXTRA_CXXFLAGS['AP_DDS'] = ['-DSOME_CXX_FLAG'] + cfg.env.AP_LIB_EXTRA_CFLAGS['AP_DDS'] = ['-DSOME_C_FLAG'] + ''' + if not tgen.name.startswith("objs/"): + return + libname = tgen.name[5:] + if libname in tgen.env.AP_LIB_EXTRA_CXXFLAGS: + tgen.env.CXXFLAGS.extend(tgen.env.AP_LIB_EXTRA_CXXFLAGS[libname]) + if libname in tgen.env.AP_LIB_EXTRA_CFLAGS: + tgen.env.CFLAGS.extend(tgen.env.AP_LIB_EXTRA_CFLAGS[libname]) + + def double_precision_check(tasks): '''check for tasks marked as double precision''' @@ -251,11 +270,15 @@ def double_precision_check(tasks): double_tasks.append([library, s]) src = str(t.inputs[0]).split('/')[-2:] - if src in double_tasks: - single_precision_option='-fsingle-precision-constant' + double_library = t.env.DOUBLE_PRECISION_LIBRARIES.get(src[0],False) + + if double_library or src in double_tasks: t.env.CXXFLAGS = t.env.CXXFLAGS[:] - if single_precision_option in t.env.CXXFLAGS: - t.env.CXXFLAGS.remove(single_precision_option) + for opt in ['-fsingle-precision-constant', '-cl-single-precision-constant']: + try: + t.env.CXXFLAGS.remove(opt) + except ValueError: + pass t.env.CXXFLAGS.append("-DALLOW_DOUBLE_MATH_FUNCTIONS") @@ -284,6 +307,7 @@ def ap_library_register_for_check(self): if not hasattr(self, 'compiled_tasks'): return + custom_flags_check(self) double_precision_check(self.compiled_tasks) if self.env.ENABLE_ONVIF: gsoap_library_check(self.bld, self.compiled_tasks) @@ -298,4 +322,7 @@ def ap_library_register_for_check(self): def configure(cfg): cfg.env.AP_LIBRARIES_OBJECTS_KW = dict() cfg.env.AP_LIB_EXTRA_SOURCES = dict() + cfg.env.AP_LIB_EXTRA_CXXFLAGS = dict() + cfg.env.AP_LIB_EXTRA_CFLAGS = dict() cfg.env.DOUBLE_PRECISION_SOURCES = dict() + cfg.env.DOUBLE_PRECISION_LIBRARIES = dict() diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py index 0f9fa3d46e99d..75c9975b826bd 100644 --- a/Tools/ardupilotwaf/ardupilotwaf.py +++ b/Tools/ardupilotwaf/ardupilotwaf.py @@ -34,6 +34,7 @@ 'AP_HAL', 'AP_HAL_Empty', 'AP_InertialSensor', + 'AP_KDECAN', 'AP_Math', 'AP_Mission', 'AP_DAL', @@ -63,6 +64,7 @@ 'AP_Module', 'AP_Button', 'AP_ICEngine', + 'AP_Networking', 'AP_Frsky_Telem', 'AP_FlashStorage', 'AP_Relay', @@ -113,6 +115,7 @@ 'AP_AIS', 'AP_OpenDroneID', 'AP_CheckFirmware', + 'AP_ExternalControl', ] def get_legacy_defines(sketch_name, bld): @@ -134,6 +137,7 @@ def get_legacy_defines(sketch_name, bld): IGNORED_AP_LIBRARIES = [ 'doc', 'AP_Scripting', # this gets explicitly included when it is needed and should otherwise never be globbed in + 'AP_DDS', ] @@ -301,6 +305,8 @@ def ap_program(bld, for group in program_groups: _grouped_programs.setdefault(group, {}).update({tg.name : tg}) + return tg + @conf def ap_example(bld, **kw): @@ -324,6 +330,9 @@ def ap_stlib(bld, **kw): for l in kw['ap_libraries']: bld.ap_library(l, kw['ap_vehicle']) + if 'dynamic_source' not in kw: + kw['dynamic_source'] = 'modules/DroneCAN/libcanard/dsdlc_generated/src/**.c' + kw['features'] = kw.get('features', []) + ['cxx', 'cxxstlib'] kw['target'] = kw['name'] kw['source'] = [] @@ -349,7 +358,7 @@ def ap_stlib_target(self): self.target = '#%s' % os.path.join('lib', self.target) @conf -def ap_find_tests(bld, use=[]): +def ap_find_tests(bld, use=[], DOUBLE_PRECISION_SOURCES=[]): if not bld.env.HAS_GTEST: return @@ -363,7 +372,7 @@ def ap_find_tests(bld, use=[]): includes = [bld.srcnode.abspath() + '/tests/'] for f in bld.path.ant_glob(incl='*.cpp'): - ap_program( + t = ap_program( bld, features=features, includes=includes, @@ -374,6 +383,16 @@ def ap_find_tests(bld, use=[]): use_legacy_defines=False, cxxflags=['-Wno-undef'], ) + filename = os.path.basename(f.abspath()) + if filename in DOUBLE_PRECISION_SOURCES: + t.env.CXXFLAGS = t.env.CXXFLAGS[:] + single_precision_option='-fsingle-precision-constant' + if single_precision_option in t.env.CXXFLAGS: + t.env.CXXFLAGS.remove(single_precision_option) + single_precision_option='-cl-single-precision-constant' + if single_precision_option in t.env.CXXFLAGS: + t.env.CXXFLAGS.remove(single_precision_option) + t.env.CXXFLAGS.append("-DALLOW_DOUBLE_MATH_FUNCTIONS") _versions = [] @@ -560,6 +579,11 @@ def options(opt): help='''Specify the port to be used with the --upload option. For example a port of /dev/ttyS10 indicates that serial port 10 shuld be used. ''') + g.add_option('--upload-force', + action='store_true', + help='''Override board type check and continue loading. Same as using uploader.py --force. +''') + g = opt.ap_groups['check'] g.add_option('--check-verbose', diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index a4e30077fec6d..e18d3137065f3 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -47,34 +47,43 @@ def srcpath(path): env.SRCROOT = srcpath('') self.configure_env(cfg, env) + # Setup scripting: env.DEFINES.update( - AP_SCRIPTING_ENABLED = 0, + LUA_32BITS = 1, ) - # Setup scripting, had to defer this to allow checking board size - if ((not cfg.options.disable_scripting) and - (not cfg.env.DISABLE_SCRIPTING) and - ((cfg.env.BOARD_FLASH_SIZE is None) or - (cfg.env.BOARD_FLASH_SIZE == []) or - (cfg.env.BOARD_FLASH_SIZE > 1024))): + env.AP_LIBRARIES += [ + 'AP_Scripting', + 'AP_Scripting/lua/src', + ] + if cfg.options.enable_scripting: env.DEFINES.update( AP_SCRIPTING_ENABLED = 1, - LUA_32BITS = 1, - ) - - env.AP_LIBRARIES += [ - 'AP_Scripting', - 'AP_Scripting/lua/src', - ] - - else: - cfg.options.disable_scripting = True + ) + elif cfg.options.disable_scripting: + env.DEFINES.update( + AP_SCRIPTING_ENABLED = 0, + ) # allow GCS disable for AP_DAL example if cfg.options.no_gcs: env.CXXFLAGS += ['-DHAL_GCS_ENABLED=0'] + # configurations for XRCE-DDS + if cfg.options.enable_dds: + cfg.recurse('libraries/AP_DDS') + env.ENABLE_DDS = True + env.AP_LIBRARIES += [ + 'AP_DDS' + ] + env.DEFINES.update(AP_DDS_ENABLED = 1) + # check for microxrceddsgen + cfg.find_program('microxrceddsgen',mandatory=True) + else: + env.ENABLE_DDS = False + env.DEFINES.update(AP_DDS_ENABLED = 0) + # setup for supporting onvif cam control if cfg.options.enable_onvif: cfg.recurse('libraries/AP_ONVIF') @@ -139,6 +148,12 @@ def srcpath(path): ) cfg.msg("Enabled custom controller", 'no', color='YELLOW') + if cfg.options.disable_networking: + env.CXXFLAGS += ['-DAP_NETWORKING_ENABLED=0'] + + if cfg.options.enable_networking_tests: + env.CXXFLAGS += ['-DAP_NETWORKING_TESTS_ENABLED=1'] + d = env.get_merged_dict() # Always prepend so that arguments passed in the command line get # the priority. @@ -234,16 +249,20 @@ def configure_env(self, cfg, env): if 'clang' in cfg.env.COMPILER_CC: env.CFLAGS += [ '-fcolor-diagnostics', - '-Wno-gnu-designator', '-Wno-inconsistent-missing-override', '-Wno-mismatched-tags', '-Wno-gnu-variable-sized-type-not-at-end', '-Werror=implicit-fallthrough', + '-cl-single-precision-constant', + ] + env.CXXFLAGS += [ + '-cl-single-precision-constant', ] else: env.CFLAGS += [ '-Wno-format-contains-nul', + '-fsingle-precision-constant', # force const vals to be float , not double. so 100.0 means 100.0f ] if self.cc_version_gte(cfg, 7, 4): env.CXXFLAGS += [ @@ -251,6 +270,7 @@ def configure_env(self, cfg, env): ] env.CXXFLAGS += [ '-fcheck-new', + '-fsingle-precision-constant', ] if cfg.env.DEBUG: @@ -261,6 +281,10 @@ def configure_env(self, cfg, env): env.DEFINES.update( HAL_DEBUG_BUILD = 1, ) + elif cfg.options.debug_symbols: + env.CFLAGS += [ + '-g', + ] if cfg.env.COVERAGE: env.CFLAGS += [ '-fprofile-arcs', @@ -416,14 +440,19 @@ def configure_env(self, cfg, env): if self.with_can and not cfg.env.AP_PERIPH: env.AP_LIBRARIES += [ - 'AP_UAVCAN', - 'modules/uavcan/libuavcan/src/**/*.cpp' + 'AP_DroneCAN', + 'modules/DroneCAN/libcanard/*.c', ] + if cfg.options.enable_dronecan_tests: + env.DEFINES.update( + AP_TEST_DRONECAN_DRIVERS = 1 + ) env.DEFINES.update( - UAVCAN_CPP_VERSION = 'UAVCAN_CPP03', - UAVCAN_NO_ASSERTIONS = 1, - UAVCAN_NULLPTR = 'nullptr' + DRONECAN_CXX_WRAPPERS = 1, + USE_USER_HELPERS = 1, + CANARD_ENABLE_DEADLINE = 1, + CANARD_ALLOCATE_SEM=1 ) @@ -433,8 +462,8 @@ def configure_env(self, cfg, env): # We always want to use PRI format macros cfg.define('__STDC_FORMAT_MACROS', 1) - if cfg.options.disable_ekf2: - env.CXXFLAGS += ['-DHAL_NAVEKF2_AVAILABLE=0'] + if cfg.options.enable_ekf2: + env.CXXFLAGS += ['-DHAL_NAVEKF2_AVAILABLE=1'] if cfg.options.disable_ekf3: env.CXXFLAGS += ['-DHAL_NAVEKF3_AVAILABLE=0'] @@ -485,6 +514,7 @@ def pre_build(self, bld): def build(self, bld): bld.ap_version_append_str('GIT_VERSION', bld.git_head_hash(short=True)) bld.ap_version_append_int('GIT_VERSION_INT', int("0x" + bld.git_head_hash(short=True), base=16)) + bld.ap_version_append_str('AP_BUILD_ROOT', bld.srcnode.abspath()) import time ltime = time.localtime() if bld.env.build_dates: @@ -528,7 +558,11 @@ def add_dynamic_boards_esp32(): continue hwdef = os.path.join(dirname, d, 'hwdef.dat') if os.path.exists(hwdef): - newclass = type(d, (esp32,), {'name': d}) + mcu_esp32s3 = True if (d[0:7] == "esp32s3") else False + if mcu_esp32s3: + newclass = type(d, (esp32s3,), {'name': d}) + else: + newclass = type(d, (esp32,), {'name': d}) def get_boards_names(): add_dynamic_boards_chibios() @@ -587,7 +621,7 @@ def get_board(ctx): ''' % ctx.env.BOARD) boards = _board_classes.keys() - if not ctx.env.BOARD in boards: + if ctx.env.BOARD not in boards: ctx.fatal("Invalid board '%s': choices are %s" % (ctx.env.BOARD, ', '.join(sorted(boards, key=str.lower)))) _board = _board_classes[ctx.env.BOARD]() return _board @@ -599,10 +633,7 @@ def get_board(ctx): class sitl(Board): def __init__(self): - if Utils.unversioned_sys_platform().startswith("linux"): - self.with_can = True - else: - self.with_can = False + self.with_can = True def configure_env(self, cfg, env): super(sitl, self).configure_env(cfg, env) @@ -610,7 +641,7 @@ def configure_env(self, cfg, env): CONFIG_HAL_BOARD = 'HAL_BOARD_SITL', CONFIG_HAL_BOARD_SUBTYPE = 'HAL_BOARD_SUBTYPE_NONE', AP_SCRIPTING_CHECKS = 1, # SITL should always do runtime scripting checks - HAL_PROBE_EXTERNAL_I2C_BAROS = 1, + AP_BARO_PROBE_EXTERNAL_I2C_BUSES = 1, ) cfg.define('AP_SIM_ENABLED', 1) @@ -620,15 +651,38 @@ def configure_env(self, cfg, env): cfg.define('AP_OPENDRONEID_ENABLED', 1) cfg.define('AP_SIGNED_FIRMWARE', 0) + cfg.define('AP_NOTIFY_LP5562_BUS', 2) + cfg.define('AP_NOTIFY_LP5562_ADDR', 0x30) + + 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) - cfg.define('UAVCAN_EXCEPTIONS', 0) - cfg.define('UAVCAN_SUPPORT_CANFD', 1) + env.DEFINES.update(CANARD_MULTI_IFACE=1, + CANARD_IFACE_ALL = 0x3, + CANARD_ENABLE_CANFD = 1, + CANARD_ENABLE_ASSERTS = 1) + if not cfg.options.force_32bit: + # needed for cygwin + env.CXXFLAGS += [ '-DCANARD_64_BIT=1' ] + env.CFLAGS += [ '-DCANARD_64_BIT=1' ] + if Utils.unversioned_sys_platform().startswith("linux"): + cfg.define('HAL_CAN_WITH_SOCKETCAN', 1) + else: + cfg.define('HAL_CAN_WITH_SOCKETCAN', 0) env.CXXFLAGS += [ - '-Werror=float-equal' + '-Werror=float-equal', + '-Werror=missing-declarations', ] + if not cfg.options.disable_networking: + env.CXXFLAGS += ['-DAP_NETWORKING_ENABLED=1'] + if cfg.options.ubsan or cfg.options.ubsan_abort: env.CXXFLAGS += [ "-fsanitize=undefined", @@ -673,10 +727,9 @@ def configure_env(self, cfg, env): 'AP_CSVReader', ] - if not cfg.env.AP_PERIPH: - env.AP_LIBRARIES += [ - 'SITL', - ] + env.AP_LIBRARIES += [ + 'SITL', + ] if cfg.options.enable_sfml: if not cfg.check_SFML(env): @@ -731,7 +784,7 @@ def configure_env(self, cfg, env): '-fno-slp-vectorize' # compiler bug when trying to use SLP ] - if cfg.options.sitl_32bit: + if cfg.options.force_32bit: # 32bit platform flags env.CXXFLAGS += [ '-m32', @@ -743,6 +796,35 @@ def configure_env(self, cfg, env): '-m32', ] + # whitelist of compilers which we should build with -Werror + gcc_whitelist = frozenset([ + ('11','3','0'), + ('11','4','0'), + ('12','1','0'), + ]) + + # initialise werr_enabled from defaults: + werr_enabled = bool('g++' in cfg.env.COMPILER_CXX and cfg.env.CC_VERSION in gcc_whitelist) + + # now process overrides to that default: + if (cfg.options.Werror is not None and + cfg.options.Werror == cfg.options.disable_Werror): + cfg.fatal("Asked to both enable and disable Werror") + + if cfg.options.Werror is not None: + werr_enabled = cfg.options.Werror + elif cfg.options.disable_Werror is not None: + werr_enabled = not cfg.options.disable_Werror + + if werr_enabled: + cfg.msg("Enabling -Werror", "yes") + if '-Werror' not in env.CXXFLAGS: + env.CXXFLAGS += [ '-Werror' ] + else: + cfg.msg("Enabling -Werror", "no") + if '-Werror' in env.CXXFLAGS: + env.CXXFLAGS.remove('-Werror') + def get_name(self): return self.__class__.__name__ @@ -750,48 +832,63 @@ def get_name(self): class sitl_periph_gps(sitl): def configure_env(self, cfg, env): cfg.env.AP_PERIPH = 1 - cfg.env.DISABLE_SCRIPTING = 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"', - AP_AIRSPEED_ENABLED = 0, + CAN_APP_NODE_NAME = '"org.ardupilot.ap_periph"', HAL_PERIPH_ENABLE_GPS = 1, + HAL_PERIPH_ENABLE_AIRSPEED = 1, + HAL_PERIPH_ENABLE_MAG = 1, + HAL_PERIPH_ENABLE_BARO = 1, + HAL_PERIPH_ENABLE_RANGEFINDER = 1, + HAL_PERIPH_ENABLE_BATTERY = 1, + HAL_PERIPH_ENABLE_EFI = 1, + HAL_PERIPH_ENABLE_RPM = 1, + HAL_PERIPH_ENABLE_RC_OUT = 1, + HAL_PERIPH_ENABLE_ADSB = 1, + AP_ICENGINE_ENABLED = 0, + AP_AIRSPEED_ENABLED = 1, + AP_AIRSPEED_AUTOCAL_ENABLE = 0, + AP_AHRS_ENABLED = 1, + AP_UART_MONITOR_ENABLED = 1, HAL_CAN_DEFAULT_NODE_ID = 0, HAL_RAM_RESERVE_START = 0, APJ_BOARD_ID = 100, HAL_GCS_ENABLED = 0, + HAL_MAVLINK_BINDINGS_ENABLED = 1, HAL_LOGGING_ENABLED = 0, HAL_LOGGING_MAVLINK_ENABLED = 0, AP_MISSION_ENABLED = 0, HAL_RALLY_ENABLED = 0, AP_SCHEDULER_ENABLED = 0, + CANARD_ENABLE_TAO_OPTION = 1, + AP_RCPROTOCOL_ENABLED = 0, CANARD_ENABLE_CANFD = 1, CANARD_MULTI_IFACE = 1, HAL_CANMANAGER_ENABLED = 0, COMPASS_CAL_ENABLED = 0, COMPASS_MOT_ENABLED = 0, COMPASS_LEARN_ENABLED = 0, - AP_BATTERY_ESC_ENABLED = 0, + AP_BATTERY_ESC_ENABLED = 1, HAL_EXTERNAL_AHRS_ENABLED = 0, HAL_GENERATOR_ENABLED = 0, AP_STATS_ENABLED = 0, HAL_SUPPORT_RCOUT_SERIAL = 0, AP_CAN_SLCAN_ENABLED = 0, + HAL_PROXIMITY_ENABLED = 0, + AP_SCRIPTING_ENABLED = 0, + HAL_NAVEKF3_AVAILABLE = 0, + HAL_PWM_COUNT = 32, + HAL_WITH_ESC_TELEM = 1, + AP_RTC_ENABLED = 0, ) - # libcanard is written for 32bit platforms - env.CXXFLAGS += [ - '-m32', - ] - env.CFLAGS += [ - '-m32', - ] - env.LDFLAGS += [ - '-m32', - ] - + try: + env.CXXFLAGS.remove('-DHAL_NAVEKF2_AVAILABLE=1') + except ValueError: + pass + env.CXXFLAGS += ['-DHAL_NAVEKF2_AVAILABLE=0'] class esp32(Board): abstract = True @@ -818,7 +915,7 @@ def expand_path(p): env.DEFINES.update( ENABLE_HEAP = 0, CONFIG_HAL_BOARD_SUBTYPE = 'HAL_BOARD_SUBTYPE_ESP32_%s' % tt.upper() , - ALLOW_DOUBLE_MATH_FUNCTIONS = '1', + HAL_HAVE_HARDWARE_DOUBLE = '1', ) env.AP_LIBRARIES += [ @@ -843,6 +940,7 @@ def expand_path(p): '-Wno-sign-compare', '-fno-inline-functions', '-mlongcalls', + '-fno-threadsafe-statics', '-DCYGWIN_BUILD'] env.CXXFLAGS.remove('-Werror=undef') env.CXXFLAGS.remove('-Werror=shadow') @@ -872,6 +970,9 @@ def build(self, bld): def get_name(self): return self.__class__.__name__ +class esp32s3(esp32): + abstract = True + toolchain = 'xtensa-esp32s3-elf' class chibios(Board): abstract = True @@ -901,7 +1002,6 @@ def configure_env(self, cfg, env): env.CFLAGS += cfg.env.CPU_FLAGS + [ '-Wlogical-op', '-Wframe-larger-than=1300', - '-fsingle-precision-constant', '-Wno-attributes', '-fno-exceptions', '-Wall', @@ -958,6 +1058,7 @@ def configure_env(self, cfg, env): bldnode = cfg.bldnode.make_node(self.name) env.BUILDROOT = bldnode.make_node('').abspath() + env.LINKFLAGS = cfg.env.CPU_FLAGS + [ '-fomit-frame-pointer', '-falign-functions=16', @@ -979,7 +1080,7 @@ def configure_env(self, cfg, env): '-L%s' % env.BUILDROOT, '-L%s' % cfg.srcnode.make_node('modules/ChibiOS/os/common/startup/ARMCMx/compilers/GCC/ld/').abspath(), '-L%s' % cfg.srcnode.make_node('libraries/AP_HAL_ChibiOS/hwdef/common/').abspath(), - '-Wl,-Map,Linker.map,--cref,--gc-sections,--no-warn-mismatch,--library-path=/ld,--script=ldscript.ld,--defsym=__process_stack_size__=%s,--defsym=__main_stack_size__=%s' % (cfg.env.PROCESS_STACK, cfg.env.MAIN_STACK) + '-Wl,-Map,Linker.map,%s--cref,--gc-sections,--no-warn-mismatch,--library-path=/ld,--script=ldscript.ld,--defsym=__process_stack_size__=%s,--defsym=__main_stack_size__=%s' % ("--print-memory-usage," if cfg.env.EXT_FLASH_SIZE_MB > 0 and cfg.env.INT_FLASH_PRIMARY == 0 else "", cfg.env.PROCESS_STACK, cfg.env.MAIN_STACK) ] if cfg.env.DEBUG: @@ -992,6 +1093,11 @@ def configure_env(self, cfg, env): '-g3', ] + if cfg.env.COMPILER_CXX == "g++": + if not self.cc_version_gte(cfg, 10, 2): + # require at least 10.2 compiler + cfg.fatal("ChibiOS build requires g++ version 10.2.1 or later, found %s" % '.'.join(cfg.env.CC_VERSION)) + if cfg.env.ENABLE_ASSERTS: cfg.msg("Enabling ChibiOS asserts", "yes") env.CFLAGS += [ '-DHAL_CHIBIOS_ENABLE_ASSERTS' ] @@ -1043,28 +1149,28 @@ def configure_env(self, cfg, env): ] env.INCLUDES += [ - cfg.srcnode.find_dir('libraries/AP_GyroFFT/CMSIS_5/include').abspath() + cfg.srcnode.find_dir('libraries/AP_GyroFFT/CMSIS_5/include').abspath(), + cfg.srcnode.find_dir('modules/ChibiOS/ext/lwip/src/include/compat/posix').abspath() ] # whitelist of compilers which we should build with -Werror - gcc_whitelist = [ + gcc_whitelist = frozenset([ ('4','9','3'), ('6','3','1'), ('9','2','1'), ('9','3','1'), ('10','2','1'), - ] + ('11','3','0'), + ('11','4','0'), + ]) - if cfg.env.AP_PERIPH: - if cfg.env.HAL_CANFD_SUPPORTED: - env.DEFINES.update(CANARD_ENABLE_CANFD=1) - else: - env.DEFINES.update(CANARD_ENABLE_TAO_OPTION=1) - if not cfg.options.bootloader: - if int(cfg.env.HAL_NUM_CAN_IFACES) > 1: - env.DEFINES.update(CANARD_MULTI_IFACE=1) - else: - env.DEFINES.update(CANARD_MULTI_IFACE=0) + if cfg.env.HAL_CANFD_SUPPORTED: + env.DEFINES.update(CANARD_ENABLE_CANFD=1) + else: + env.DEFINES.update(CANARD_ENABLE_TAO_OPTION=1) + if not cfg.options.bootloader and cfg.env.HAL_NUM_CAN_IFACES: + if int(cfg.env.HAL_NUM_CAN_IFACES) >= 1: + env.DEFINES.update(CANARD_IFACE_ALL=(1< 0: + ext_flash_used = int(row[1]) import re pattern = re.compile("^.*TOTALS.*$") lines = s.splitlines()[1:] l = [] for line in lines: - if pattern.match(line) or totals==False: + if pattern.match(line) or totals is False: row = line.strip().split() # check if crash_log wasn't found @@ -203,8 +209,9 @@ def _parse_size_output(s, s_all, totals=False): size_data=int(row[1]), size_bss=size_bss, # Total Flash Cost = Data + Text - size_total=int(row[0]) + int(row[1]), + size_total=int(row[0]) + int(row[1]) - ext_flash_used, size_free_flash=size_free_flash, + ext_flash_used= ext_flash_used if ext_flash_used else None, )) return l @@ -285,4 +292,5 @@ def configure(cfg): 'size_bss', 'size_total', 'size_free_flash', + 'ext_flash_used', ] diff --git a/Tools/ardupilotwaf/chibios.py b/Tools/ardupilotwaf/chibios.py index 89e2c9df8694d..a827af26f777f 100644 --- a/Tools/ardupilotwaf/chibios.py +++ b/Tools/ardupilotwaf/chibios.py @@ -31,7 +31,7 @@ def _load_dynamic_env_data(bld): # relative paths from the make build are relative to BUILDROOT d = os.path.join(bld.env.BUILDROOT, d) d = os.path.normpath(d) - if not d in idirs2: + if d not in idirs2: idirs2.append(d) _dynamic_env_data['include_dirs'] = idirs2 @@ -64,11 +64,13 @@ def run(self): if not self.wsl2_prereq_checks(): return print("If this takes takes too long here, try power-cycling your hardware\n") - cmd = "{} '{}/uploader.py' '{}'".format('python.exe', upload_tools, src.abspath()) + cmd = "{} -u '{}/uploader.py' '{}'".format('python.exe', upload_tools, src.abspath()) else: cmd = "{} '{}/uploader.py' '{}'".format(self.env.get_flat('PYTHON'), upload_tools, src.abspath()) if upload_port is not None: cmd += " '--port' '%s'" % upload_port + if self.generator.bld.options.upload_force: + cmd += " '--force'" return self.exec_command(cmd) def wsl2_prereq_checks(self): @@ -96,7 +98,7 @@ def wsl2_prereq_checks(self): except subprocess.CalledProcessError: #if where.exe can't find the file it returns a non-zero result which throws this exception where_python = "" - if not where_python or not "\Python\Python" in where_python or "python.exe" not in where_python: + if not where_python or "\Python\Python" not in where_python or "python.exe" not in where_python: print(self.get_full_wsl2_error_msg("Windows python.exe not found")) return False return True @@ -112,7 +114,7 @@ def get_full_wsl2_error_msg(self, error_msg): and make sure to add it to your path during the installation. Once installed, run this command in Powershell or Command Prompt to install some packages: - pip.exe install empy pyserial + pip.exe install empy==3.3.4 pyserial **************************************** **************************************** """ % error_msg) @@ -348,7 +350,7 @@ def run(self): class build_abin(Task.Task): '''build an abin file for skyviper firmware upload via web UI''' color='CYAN' - run_str='${TOOLS_SCRIPTS}/make_abin.sh ${SRC}.bin ${SRC}.abin' + run_str='${TOOLS_SCRIPTS}/make_abin.sh ${SRC} ${TGT}' always_run = True def keyword(self): return "Generating" @@ -400,7 +402,7 @@ def chibios_firmware(self): if self.env.BUILD_ABIN: abin_target = self.bld.bldnode.find_or_declare('bin/' + link_output.change_ext('.abin').name) - abin_task = self.create_task('build_abin', src=link_output, tgt=abin_target) + abin_task = self.create_task('build_abin', src=bin_target, tgt=abin_target) abin_task.set_run_after(generate_apj_task) cleanup_task = self.create_task('build_normalized_bins', src=bin_target) @@ -442,17 +444,22 @@ def setup_canmgr_build(cfg): the build based on the presence of CAN pins in hwdef.dat except for AP_Periph builds''' env = cfg.env env.AP_LIBRARIES += [ - 'AP_UAVCAN', - 'modules/uavcan/libuavcan/src/**/*.cpp', + 'AP_DroneCAN', + 'modules/DroneCAN/libcanard/*.c', + ] + env.INCLUDES += [ + cfg.srcnode.find_dir('modules/DroneCAN/libcanard').abspath(), ] - env.CFLAGS += ['-DHAL_CAN_IFACES=2'] - env.DEFINES += [ - 'UAVCAN_CPP_VERSION=UAVCAN_CPP03', - 'UAVCAN_NO_ASSERTIONS=1', - 'UAVCAN_NULLPTR=nullptr' - ] + if not env.AP_PERIPH: + env.DEFINES += [ + 'DRONECAN_CXX_WRAPPERS=1', + 'USE_USER_HELPERS=1', + 'CANARD_ENABLE_DEADLINE=1', + 'CANARD_MULTI_IFACE=1', + 'CANARD_ALLOCATE_SEM=1' + ] if cfg.env.HAL_CANFD_SUPPORTED: env.DEFINES += ['UAVCAN_SUPPORT_CANFD=1'] @@ -488,6 +495,8 @@ def load_env_vars(env): else: env[k] = v print("env set %s=%s" % (k, v)) + if env.DEBUG or env.DEBUG_SYMBOLS: + env.CHIBIOS_BUILD_FLAGS += ' ENABLE_DEBUG_SYMBOLS=yes' if env.ENABLE_ASSERTS: env.CHIBIOS_BUILD_FLAGS += ' ENABLE_ASSERTS=yes' if env.ENABLE_MALLOC_GUARD: @@ -496,6 +505,8 @@ def load_env_vars(env): env.CHIBIOS_BUILD_FLAGS += ' ENABLE_STATS=yes' if env.ENABLE_DFU_BOOT and env.BOOTLOADER: env.CHIBIOS_BUILD_FLAGS += ' USE_ASXOPT=-DCRT0_ENTRY_HOOK=TRUE' + if env.AP_BOARD_START_TIME: + env.CHIBIOS_BUILD_FLAGS += ' AP_BOARD_START_TIME=0x%x' % env.AP_BOARD_START_TIME def setup_optimization(env): @@ -567,6 +578,8 @@ def bldpath(path): load_env_vars(cfg.env) if env.HAL_NUM_CAN_IFACES and not env.AP_PERIPH: setup_canmgr_build(cfg) + if env.HAL_NUM_CAN_IFACES and env.AP_PERIPH and int(env.HAL_NUM_CAN_IFACES)>1 and not env.BOOTLOADER: + env.DEFINES += [ 'CANARD_MULTI_IFACE=1' ] setup_optimization(cfg.env) def generate_hwdef_h(env): @@ -641,13 +654,22 @@ def build(bld): bld( # create the file modules/ChibiOS/include_dirs - rule="touch Makefile && BUILDDIR=${BUILDDIR_REL} CRASHCATCHER=${CC_ROOT_REL} CHIBIOS=${CH_ROOT_REL} AP_HAL=${AP_HAL_REL} ${CHIBIOS_BUILD_FLAGS} ${CHIBIOS_BOARD_NAME} ${MAKE} pass -f '${BOARD_MK}'", + rule="touch Makefile && BUILDDIR=${BUILDDIR_REL} BUILDROOT=${BUILDROOT} CRASHCATCHER=${CC_ROOT_REL} CHIBIOS=${CH_ROOT_REL} AP_HAL=${AP_HAL_REL} ${CHIBIOS_BUILD_FLAGS} ${CHIBIOS_BOARD_NAME} ${MAKE} pass -f '${BOARD_MK}'", group='dynamic_sources', target=bld.bldnode.find_or_declare('modules/ChibiOS/include_dirs') ) + bld( + # create the file modules/ChibiOS/include_dirs + rule="echo // BUILD_FLAGS: ${BUILDDIR_REL} ${BUILDROOT} ${CC_ROOT_REL} ${CH_ROOT_REL} ${AP_HAL_REL} ${CHIBIOS_BUILD_FLAGS} ${CHIBIOS_BOARD_NAME} ${HAL_MAX_STACK_FRAME_SIZE} > chibios_flags.h", + group='dynamic_sources', + target=bld.bldnode.find_or_declare('chibios_flags.h') + ) + common_src = [bld.bldnode.find_or_declare('hwdef.h'), bld.bldnode.find_or_declare('hw.dat'), + bld.bldnode.find_or_declare('ldscript.ld'), + bld.bldnode.find_or_declare('common.ld'), bld.bldnode.find_or_declare('modules/ChibiOS/include_dirs')] common_src += bld.path.ant_glob('libraries/AP_HAL_ChibiOS/hwdef/common/*.[ch]') common_src += bld.path.ant_glob('libraries/AP_HAL_ChibiOS/hwdef/common/*.mk') @@ -659,7 +681,7 @@ def build(bld): if bld.env.ENABLE_CRASHDUMP: ch_task = bld( # build libch.a from ChibiOS sources and hwdef.h - rule="BUILDDIR='${BUILDDIR_REL}' CRASHCATCHER='${CC_ROOT_REL}' CHIBIOS='${CH_ROOT_REL}' AP_HAL=${AP_HAL_REL} ${CHIBIOS_BUILD_FLAGS} ${CHIBIOS_BOARD_NAME} ${HAL_MAX_STACK_FRAME_SIZE} '${MAKE}' -j%u lib -f '${BOARD_MK}'" % bld.options.jobs, + rule="BUILDDIR='${BUILDDIR_REL}' BUILDROOT='${BUILDROOT}' CRASHCATCHER='${CC_ROOT_REL}' CHIBIOS='${CH_ROOT_REL}' AP_HAL=${AP_HAL_REL} ${CHIBIOS_BUILD_FLAGS} ${CHIBIOS_BOARD_NAME} ${HAL_MAX_STACK_FRAME_SIZE} '${MAKE}' -j%u lib -f '${BOARD_MK}'" % bld.options.jobs, group='dynamic_sources', source=common_src, target=[bld.bldnode.find_or_declare('modules/ChibiOS/libch.a'), bld.bldnode.find_or_declare('modules/ChibiOS/libcc.a')] @@ -667,7 +689,7 @@ def build(bld): else: ch_task = bld( # build libch.a from ChibiOS sources and hwdef.h - rule="BUILDDIR='${BUILDDIR_REL}' CHIBIOS='${CH_ROOT_REL}' AP_HAL=${AP_HAL_REL} ${CHIBIOS_BUILD_FLAGS} ${CHIBIOS_BOARD_NAME} ${HAL_MAX_STACK_FRAME_SIZE} '${MAKE}' -j%u lib -f '${BOARD_MK}'" % bld.options.jobs, + rule="BUILDDIR='${BUILDDIR_REL}' BUILDROOT='${BUILDROOT}' CHIBIOS='${CH_ROOT_REL}' AP_HAL=${AP_HAL_REL} ${CHIBIOS_BUILD_FLAGS} ${CHIBIOS_BOARD_NAME} ${HAL_MAX_STACK_FRAME_SIZE} '${MAKE}' -j%u lib -f '${BOARD_MK}'" % bld.options.jobs, group='dynamic_sources', source=common_src, target=bld.bldnode.find_or_declare('modules/ChibiOS/libch.a') @@ -697,6 +719,7 @@ def build(bld): 'fopen', 'fflush', 'fwrite', 'fread', 'fputs', 'fgets', 'clearerr', 'fseek', 'ferror', 'fclose', 'tmpfile', 'getc', 'ungetc', 'feof', 'ftell', 'freopen', 'remove', 'vfprintf', 'fscanf', - '_gettimeofday', '_times', '_times_r', '_gettimeofday_r', 'time', 'clock' ] + '_gettimeofday', '_times', '_times_r', '_gettimeofday_r', 'time', 'clock', + '_sbrk', '_sbrk_r', '_malloc_r', '_calloc_r', '_free_r'] for w in wraplist: bld.env.LINKFLAGS += ['-Wl,--wrap,%s' % w] diff --git a/Tools/ardupilotwaf/cmake.py b/Tools/ardupilotwaf/cmake.py index 9ec9691c150ae..0616de6128ed6 100644 --- a/Tools/ardupilotwaf/cmake.py +++ b/Tools/ardupilotwaf/cmake.py @@ -23,7 +23,7 @@ the configuration to set a minimum version required for cmake. Example:: def configure(cfg): - cfg.CMAKE_MIN_VERSION = '3.5.2' + cfg.env.CMAKE_MIN_VERSION = '3.5.2' cfg.load('cmake') Usage example:: diff --git a/Tools/ardupilotwaf/dronecangen.py b/Tools/ardupilotwaf/dronecangen.py index dd1d4fcd65c38..f6e067da3ea23 100644 --- a/Tools/ardupilotwaf/dronecangen.py +++ b/Tools/ardupilotwaf/dronecangen.py @@ -19,7 +19,7 @@ def run(self): python = self.env.get_flat('PYTHON') out = self.env.get_flat('OUTPUT_DIR') src = self.env.get_flat('SRC') - dsdlc = self.env.get_flat("DSDL_COMPILER") + dsdlc = self.env.get_flat("DC_DSDL_COMPILER") ret = self.exec_command(['{}'.format(python), '{}'.format(dsdlc), @@ -66,10 +66,7 @@ def configure(cfg): """ setup environment for uavcan header generator """ - cfg.load('python') - cfg.check_python_version(minver=(2,7,0)) - env = cfg.env - env.DSDL_COMPILER_DIR = cfg.srcnode.make_node('modules/DroneCAN/dronecan_dsdlc/').abspath() - env.DSDL_COMPILER = env.DSDL_COMPILER_DIR + '/dronecan_dsdlc.py' - cfg.msg('DSDL compiler', env.DSDL_COMPILER) + env.DC_DSDL_COMPILER_DIR = cfg.srcnode.make_node('modules/DroneCAN/dronecan_dsdlc/').abspath() + env.DC_DSDL_COMPILER = env.DC_DSDL_COMPILER_DIR + '/dronecan_dsdlc.py' + cfg.msg('DC_DSDL compiler', env.DC_DSDL_COMPILER) diff --git a/Tools/ardupilotwaf/embed.py b/Tools/ardupilotwaf/embed.py index d833b40c97745..ccc9673985ea1 100755 --- a/Tools/ardupilotwaf/embed.py +++ b/Tools/ardupilotwaf/embed.py @@ -34,7 +34,7 @@ def embed_file(out, f, idx, embedded_name, uncompressed): print("Padded %u bytes for %s to %u" % (pad, embedded_name, len(contents))) crc = crc32(bytearray(contents)) - write_encode(out, 'static const uint8_t ap_romfs_%u[] = {' % idx) + write_encode(out, '__EXTFLASHFUNC__ static const uint8_t ap_romfs_%u[] = {' % idx) compressed = tempfile.NamedTemporaryFile() if uncompressed: diff --git a/Tools/ardupilotwaf/esp32.py b/Tools/ardupilotwaf/esp32.py index 8631abff3c2d3..20a9528860de1 100644 --- a/Tools/ardupilotwaf/esp32.py +++ b/Tools/ardupilotwaf/esp32.py @@ -18,7 +18,8 @@ import subprocess def configure(cfg): - + mcu_esp32s3 = True if (cfg.variant[0:7] == "esp32s3") else False + target = "esp32s3" if mcu_esp32s3 else "esp32" bldnode = cfg.bldnode.make_node(cfg.variant) def srcpath(path): return cfg.srcnode.make_node(path).abspath() @@ -30,13 +31,13 @@ def bldpath(path): #define env and location for the cmake esp32 file env = cfg.env - env.AP_HAL_ESP32 = srcpath('libraries/AP_HAL_ESP32/targets/esp-idf') + env.AP_HAL_ESP32 = srcpath('libraries/AP_HAL_ESP32/targets/'+target+'/esp-idf') env.AP_PROGRAM_FEATURES += ['esp32_ap_program'] env.ESP_IDF_PREFIX_REL = 'esp-idf' prefix_node = bldnode.make_node(env.ESP_IDF_PREFIX_REL) - + env.ESP32_TARGET = target env.BUILDROOT = bldpath('') env.SRCROOT = srcpath('') env.APJ_TOOL = srcpath('Tools/scripts/apj_tool.py') @@ -63,10 +64,11 @@ def pre_build(self): lib_vars['ARDUPILOT_CMD'] = self.cmd lib_vars['ARDUPILOT_LIB'] = self.bldnode.find_or_declare('lib/').abspath() lib_vars['ARDUPILOT_BIN'] = self.bldnode.find_or_declare('lib/bin').abspath() + target = self.env.ESP32_TARGET esp_idf = self.cmake( name='esp-idf', cmake_vars=lib_vars, - cmake_src='libraries/AP_HAL_ESP32/targets/esp-idf', + cmake_src='libraries/AP_HAL_ESP32/targets/'+target+'/esp-idf', cmake_bld='esp-idf_build', ) diff --git a/Tools/ardupilotwaf/mavgen.py b/Tools/ardupilotwaf/mavgen.py index 0dec86207594d..f246d768d5a3f 100644 --- a/Tools/ardupilotwaf/mavgen.py +++ b/Tools/ardupilotwaf/mavgen.py @@ -44,7 +44,7 @@ def scan(self): node.parent.path_from(entry_point.parent), path ) - if not path in names: + if path not in names: names.append(path) return nodes, names @@ -94,8 +94,5 @@ def configure(cfg): """ setup environment for mavlink header generator """ - cfg.load('python') - cfg.check_python_version(minver=(2,7,0)) - env = cfg.env env.MAVLINK_DIR = cfg.srcnode.make_node('modules/mavlink/').abspath() diff --git a/Tools/ardupilotwaf/toolchain.py b/Tools/ardupilotwaf/toolchain.py index 5d242a4fc6c38..eb86084307cb2 100644 --- a/Tools/ardupilotwaf/toolchain.py +++ b/Tools/ardupilotwaf/toolchain.py @@ -119,7 +119,7 @@ def _set_pkgconfig_crosscompilation_wrapper(cfg): @conf def new_validate_cfg(kw): - if not 'path' in kw: + if 'path' not in kw: if not cfg.env.PKGCONFIG: cfg.find_program('%s-pkg-config' % cfg.env.TOOLCHAIN, var='PKGCONFIG') kw['path'] = cfg.env.PKGCONFIG diff --git a/Tools/ardupilotwaf/uavcangen.py b/Tools/ardupilotwaf/uavcangen.py deleted file mode 100644 index 0ca74c947b6e5..0000000000000 --- a/Tools/ardupilotwaf/uavcangen.py +++ /dev/null @@ -1,76 +0,0 @@ -# encoding: utf-8 - -""" -generate DSDLC headers for uavcan -""" - -from waflib import Logs, Task, Utils, Node -from waflib.TaskGen import feature, before_method, extension -import os -import os.path -from xml.etree import ElementTree as et - -class uavcangen(Task.Task): - """generate uavcan header files""" - color = 'BLUE' - before = 'cxx c' - - def run(self): - python = self.env.get_flat('PYTHON') - out = self.env.get_flat('OUTPUT_DIR') - src = self.env.get_flat('SRC') - dsdlc = self.env.get_flat("DSDL_COMPILER") - - ret = self.exec_command(['{}'.format(python), - '{}'.format(dsdlc), - '-O{}'.format(out)] + [x.abspath() for x in self.inputs]) - - if ret != 0: - # ignore if there was a signal to the interpreter rather - # than a real error in the script. Some environments use a - # signed and some an unsigned return for this - if ret > 128 or ret < 0: - Logs.warn('uavcangen crashed with code: {}'.format(ret)) - ret = 0 - else: - Logs.error('uavcangen returned {} error code'.format(ret)) - return ret - - def post_run(self): - super(uavcangen, self).post_run() - for header in self.generator.output_dir.ant_glob("*.hpp **/*.hpp", remove=False): - header.sig = header.cache_sig = self.cache_sig - -def options(opt): - opt.load('python') - -@feature('uavcangen') -@before_method('process_source') -def process_uavcangen(self): - if not hasattr(self, 'output_dir'): - self.bld.fatal('uavcangen: missing option output_dir') - - inputs = self.to_nodes(self.source) - outputs = [] - - self.source = [] - - if not isinstance(self.output_dir, Node.Node): - self.output_dir = self.bld.bldnode.find_or_declare(self.output_dir) - - task = self.create_task('uavcangen', inputs, outputs) - task.env['OUTPUT_DIR'] = self.output_dir.abspath() - - task.env.env = dict(os.environ) - -def configure(cfg): - """ - setup environment for uavcan header generator - """ - cfg.load('python') - cfg.check_python_version(minver=(2,7,0)) - - env = cfg.env - env.DSDL_COMPILER_DIR = cfg.srcnode.make_node('modules/uavcan/libuavcan/dsdl_compiler').abspath() - env.DSDL_COMPILER = env.DSDL_COMPILER_DIR + '/libuavcan_dsdlc' - cfg.msg('DSDL compiler', env.DSDL_COMPILER) diff --git a/Tools/autotest/ArduCopter_Tests/PayLoadPlaceMission/copter_payload_place.txt b/Tools/autotest/ArduCopter_Tests/PayloadPlaceMission/copter_payload_place.txt similarity index 100% rename from Tools/autotest/ArduCopter_Tests/PayLoadPlaceMission/copter_payload_place.txt rename to Tools/autotest/ArduCopter_Tests/PayloadPlaceMission/copter_payload_place.txt diff --git a/Tools/autotest/ArduPlane_Tests/GUIDEDToAUTO/mission.txt b/Tools/autotest/ArduPlane_Tests/GUIDEDToAUTO/mission.txt new file mode 100644 index 0000000000000..12bb5ed5f65e6 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/GUIDEDToAUTO/mission.txt @@ -0,0 +1,36 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -27.272924 151.290848 10.000000 1 +1 0 10 84 0.000000 0.000000 0.000000 0.000000 -27.272924 151.290848 10.000000 1 +2 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.272705 151.298172 100.000000 1 +3 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.277561 151.337250 100.000000 1 +4 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.281748 151.335953 100.000000 1 +5 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.275724 151.289932 100.000000 1 +6 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.297457 151.285629 100.000000 1 +7 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.308109 151.354279 100.000000 1 +8 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.330292 151.374268 90.000000 1 +9 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.330435 151.375977 70.000000 1 +10 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.332638 151.376099 70.000000 1 +11 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.334694 151.376144 70.000000 1 +12 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.333776 151.374146 70.000000 1 +13 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.331438 151.378159 70.000000 1 +14 0 0 177 9.000000 4.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +15 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.334566 151.375366 40.000000 1 +16 0 0 178 0.000000 20.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +17 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.334840 151.377234 40.000000 1 +18 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.333933 151.376849 35.000000 1 +19 0 10 85 0.000000 0.000000 0.000000 0.000000 -27.332676 151.376511 0.000000 1 +20 0 10 84 0.000000 0.000000 0.000000 0.000000 -27.332659 151.376511 35.000000 1 +21 0 0 178 0.000000 24.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +22 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.332201 151.376511 100.000000 1 +23 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.330381 151.374191 100.000000 1 +24 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.308224 151.354538 100.000000 1 +25 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.297340 151.285385 100.000000 1 +26 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.275724 151.289932 100.000000 1 +27 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.281631 151.335953 100.000000 1 +28 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.277679 151.337128 100.000000 1 +29 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.272587 151.298294 100.000000 1 +30 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.271564 151.291473 30.000000 1 +31 0 0 178 0.000000 20.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +32 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.271054 151.290211 25.000000 1 +33 0 10 16 0.000000 0.000000 0.000000 0.000000 -27.273310 151.290222 15.000000 1 +34 0 10 85 0.000000 0.000000 0.000000 0.000000 -27.274887 151.289918 0.000000 1 diff --git a/Tools/autotest/ArduPlane_Tests/MAV_CMD_DO_GO_AROUND/mission.txt b/Tools/autotest/ArduPlane_Tests/MAV_CMD_DO_GO_AROUND/mission.txt new file mode 100644 index 0000000000000..41e2938f9c9d6 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/MAV_CMD_DO_GO_AROUND/mission.txt @@ -0,0 +1,8 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165238 584.089966 1 +1 0 3 22 0.000000 0.000000 0.000000 0.000000 -35.361438 149.165031 50.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361505 149.163723 50.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365845 149.164175 52.219997 1 +4 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.368656 149.165976 54.410000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.366148 149.165741 50.849998 1 +6 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.362920 149.165127 50.000000 1 diff --git a/Tools/autotest/ArduPlane_Tests/LordEAHRS/ap1.txt b/Tools/autotest/ArduPlane_Tests/MicroStrainEAHRS5/ap1.txt similarity index 100% rename from Tools/autotest/ArduPlane_Tests/LordEAHRS/ap1.txt rename to Tools/autotest/ArduPlane_Tests/MicroStrainEAHRS5/ap1.txt diff --git a/Tools/autotest/ArduPlane_Tests/MicroStrainEAHRS7/ap1.txt b/Tools/autotest/ArduPlane_Tests/MicroStrainEAHRS7/ap1.txt new file mode 100644 index 0000000000000..ab2be0d1a4547 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/MicroStrainEAHRS7/ap1.txt @@ -0,0 +1,7 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.090027 1 +1 0 3 22 0.000000 0.000000 0.000000 0.000000 -35.361553 149.163956 20.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364540 149.162857 50.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.367333 149.163164 28.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.366814 149.165878 28.000000 1 +5 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.362947 149.165179 0.000000 1 diff --git a/Tools/autotest/Generic_Missions/QuadPlaneDalbyRTL.txt b/Tools/autotest/Generic_Missions/QuadPlaneDalbyRTL.txt new file mode 100644 index 0000000000000..e88bb0c7fd6c0 --- /dev/null +++ b/Tools/autotest/Generic_Missions/QuadPlaneDalbyRTL.txt @@ -0,0 +1,6 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -27.274439 151.290064 343.209991 1 +1 0 3 189 0.000000 0.000000 0.000000 0.000000 -27.270617 151.283268 28.590000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -27.272859 151.286018 28.789999 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -27.273316 151.288023 30.000000 1 +4 0 3 85 0.000000 0.000000 0.000000 0.000000 -27.273771 151.289905 0.000000 1 diff --git a/Tools/autotest/antennatracker.py b/Tools/autotest/antennatracker.py index ba452956aafd9..b06df26492126 100644 --- a/Tools/autotest/antennatracker.py +++ b/Tools/autotest/antennatracker.py @@ -14,15 +14,15 @@ from pymavlink import mavextra from pymavlink import mavutil -from common import AutoTest -from common import NotAchievedException +import vehicle_test_suite +from vehicle_test_suite import NotAchievedException # get location of scripts testdir = os.path.dirname(os.path.realpath(__file__)) SITL_START_LOCATION = mavutil.location(-27.274439, 151.290064, 343, 8.7) -class AutoTestTracker(AutoTest): +class AutoTestTracker(vehicle_test_suite.TestSuite): def log_name(self): return "AntennaTracker" @@ -116,34 +116,36 @@ def MANUAL(self): self.set_rc(chan, pwm) self.wait_servo_channel_value(chan, pwm) - def SERVOTEST(self): + def MAV_CMD_DO_SET_SERVO(self): '''Test SERVOTEST mode''' self.change_mode(0) # "MANUAL" # magically changes to SERVOTEST (3) - for value in 1900, 1200: - channel = 1 - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, - channel, - value, - 0, - 0, - 0, - 0, - 0, - timeout=1) - self.wait_servo_channel_value(channel, value) - for value in 1300, 1670: - channel = 2 - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, - channel, - value, - 0, - 0, - 0, - 0, - 0, - timeout=1) - self.wait_servo_channel_value(channel, value) + for method in self.run_cmd, self.run_cmd_int: + for value in 1900, 1200: + channel = 1 + method( + mavutil.mavlink.MAV_CMD_DO_SET_SERVO, + p1=channel, + p2=value, + timeout=1, + ) + self.wait_servo_channel_value(channel, value) + for value in 1300, 1670: + channel = 2 + method( + mavutil.mavlink.MAV_CMD_DO_SET_SERVO, + p1=channel, + p2=value, + timeout=1, + ) + self.wait_servo_channel_value(channel, value) + + def MAV_CMD_MISSION_START(self): + '''test MAV_CMD_MISSION_START mavlink command''' + for method in self.run_cmd, self.run_cmd_int: + self.change_mode(0) # "MANUAL" + method(mavutil.mavlink.MAV_CMD_MISSION_START) + self.wait_mode("AUTO") def SCAN(self): '''Test SCAN mode''' @@ -172,7 +174,8 @@ def tests(self): ret.extend([ self.GUIDED, self.MANUAL, - self.SERVOTEST, + self.MAV_CMD_DO_SET_SERVO, + self.MAV_CMD_MISSION_START, self.NMEAOutput, self.SCAN, ]) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 5e30a0bb97599..f8afa254b4cec 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -11,7 +11,6 @@ import shutil import time import numpy -import operator from pymavlink import quaternion from pymavlink import mavutil @@ -21,10 +20,11 @@ from pysim import util from pysim import vehicleinfo -from common import AutoTest -from common import NotAchievedException, AutoTestTimeoutException, PreconditionFailedException -from common import Test -from common import MAV_POS_TARGET_TYPE_MASK +import vehicle_test_suite + +from vehicle_test_suite import NotAchievedException, AutoTestTimeoutException, PreconditionFailedException +from vehicle_test_suite import Test +from vehicle_test_suite import MAV_POS_TARGET_TYPE_MASK from pymavlink.rotmat import Vector3 @@ -41,7 +41,7 @@ # switch 6 = Stabilize -class AutoTestCopter(AutoTest): +class AutoTestCopter(vehicle_test_suite.TestSuite): @staticmethod def get_not_armable_mode_list(): return ["AUTO", "AUTOTUNE", "BRAKE", "CIRCLE", "FLIP", "LAND", "RTL", "SMART_RTL", "AVOID_ADSB", "FOLLOW"] @@ -124,19 +124,6 @@ def get_disarm_delay(self): def set_autodisarm_delay(self, delay): self.set_parameter("DISARM_DELAY", delay) - def user_takeoff(self, alt_min=30, timeout=30, max_err=5): - '''takeoff using mavlink takeoff command''' - self.run_cmd(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, - 0, # param1 - 0, # param2 - 0, # param3 - 0, # param4 - 0, # param5 - 0, # param6 - alt_min # param7 - ) - self.wait_for_alt(alt_min, timeout=timeout, max_err=max_err) - def takeoff(self, alt_min=30, takeoff_throttle=1700, @@ -155,17 +142,10 @@ def takeoff(self, self.user_takeoff(alt_min=alt_min, timeout=timeout, max_err=max_err) else: self.set_rc(3, takeoff_throttle) - self.wait_for_alt(alt_min=alt_min, timeout=timeout, max_err=max_err) + self.wait_altitude(alt_min-1, alt_min+max_err, relative=True, timeout=timeout) self.hover() self.progress("TAKEOFF COMPLETE") - def wait_for_alt(self, alt_min=30, timeout=30, max_err=5): - """Wait for minimum altitude to be reached.""" - self.wait_altitude(alt_min - 1, - (alt_min + max_err), - relative=True, - timeout=timeout) - def land_and_disarm(self, timeout=60): """Land the quad.""" self.progress("STARTING LANDING") @@ -177,7 +157,7 @@ def wait_landed_and_disarmed(self, min_alt=6, timeout=60): m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) alt = m.relative_alt / 1000.0 # mm -> m if alt > min_alt: - self.wait_for_alt(min_alt, timeout=timeout) + self.wait_altitude(min_alt-1, min_alt+5, relative=True, timeout=timeout) # self.wait_statustext("SIM Hit ground", timeout=timeout) self.wait_disarmed() @@ -412,13 +392,13 @@ def RecordThenPlayMission(self, side=50, timeout=300): self.land_and_disarm() # enter RTL mode and wait for the vehicle to disarm - def do_RTL(self, distance_min=None, check_alt=True, distance_max=10, timeout=250): + def do_RTL(self, distance_min=None, check_alt=True, distance_max=10, timeout=250, quiet=False): """Enter RTL mode and wait for the vehicle to disarm at Home.""" self.change_mode("RTL") self.hover() - self.wait_rtl_complete(check_alt=check_alt, distance_max=distance_max, timeout=timeout) + self.wait_rtl_complete(check_alt=check_alt, distance_max=distance_max, timeout=timeout, quiet=True) - def wait_rtl_complete(self, check_alt=True, distance_max=10, timeout=250): + def wait_rtl_complete(self, check_alt=True, distance_max=10, timeout=250, quiet=False): """Wait for RTL to reach home and disarm""" self.progress("Waiting RTL to reach Home and disarm") tstart = self.get_sim_time() @@ -435,8 +415,9 @@ def wait_rtl_complete(self, check_alt=True, distance_max=10, timeout=250): else: if distance_valid: home = "HOME" - self.progress("Alt: %.02f HomeDist: %.02f %s" % - (alt, home_distance, home)) + if not quiet: + self.progress("Alt: %.02f HomeDist: %.02f %s" % + (alt, home_distance, home)) # our post-condition is that we are disarmed: if not self.armed(): @@ -708,6 +689,47 @@ def ThrottleFailsafe(self, side=60, timeout=360): self.set_parameter('FS_THR_ENABLE', 0) self.reboot_sitl() + def ThrottleFailsafePassthrough(self): + '''check servo passthrough on RC failsafe. Make sure it doesn't glitch to the bad RC input value''' + channel = 7 + trim_value = 1450 + self.set_parameters({ + 'RC%u_MIN' % channel: 1000, + 'RC%u_MAX' % channel: 2000, + 'SERVO%u_MIN' % channel: 1000, + 'SERVO%u_MAX' % channel: 2000, + 'SERVO%u_TRIM' % channel: trim_value, + 'SERVO%u_FUNCTION' % channel: 146, # scaled passthrough for channel 7 + 'FS_THR_ENABLE': 1, + 'RC_FS_TIMEOUT': 10, + 'SERVO_RC_FS_MSK': 1 << (channel-1), + }) + + self.reboot_sitl() + + self.context_set_message_rate_hz('SERVO_OUTPUT_RAW', 200) + + self.set_rc(channel, 1799) + expected_servo_output_value = 1778 # 1778 because of weird trim + self.wait_servo_channel_value(channel, expected_servo_output_value) + # receiver goes into failsafe with wild override values: + + def ensure_SERVO_values_never_input(mav, m): + if m.get_type() != "SERVO_OUTPUT_RAW": + return + value = getattr(m, "servo%u_raw" % channel) + if value != expected_servo_output_value and value != trim_value: + raise NotAchievedException("Bad servo value %u received" % value) + + self.install_message_hook_context(ensure_SERVO_values_never_input) + self.progress("Forcing receiver into failsafe") + self.set_rc_from_map({ + 3: 800, + channel: 1300, + }) + self.wait_servo_channel_value(channel, trim_value) + self.delay_sim_time(10) + # Tests all actions and logic behind the GCS failsafe def GCSFailsafe(self, side=60, timeout=360): '''Test GCS Failsafe''' @@ -1119,7 +1141,7 @@ def VibrationFailsafe(self): self.change_mode("LAND") # check vehicle descends to 2m or less within 40 seconds - self.wait_altitude(-5, 2, timeout=40, relative=True) + self.wait_altitude(-5, 2, timeout=50, relative=True) # force disarm of vehicle (it will likely not automatically disarm) self.disarm_vehicle(force=True) @@ -1138,11 +1160,14 @@ def test_takeoff_check_mode(self, mode, user_takeoff=False): self.context_collect('STATUSTEXT') self.arm_vehicle() if user_takeoff: - self.run_cmd(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 10) + self.run_cmd( + mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, + p7=10, + ) else: self.set_rc(3, 1700) # we may never see ourselves as armed in a heartbeat - self.wait_statustext("Takeoff blocked: ESC RPM too low", check_context=True) + self.wait_statustext("Takeoff blocked: ESC RPM out of range", check_context=True) self.context_pop() self.zero_throttle() self.disarm_vehicle() @@ -1167,6 +1192,21 @@ def TakeoffCheck(self): self.test_takeoff_check_mode("POSHOLD") # self.test_takeoff_check_mode("SPORT") + self.set_parameters({ + "AHRS_EKF_TYPE": 10, + 'SIM_ESC_TELEM': 1, + 'TKOFF_RPM_MIN': 1, + 'TKOFF_RPM_MAX': 3, + }) + self.test_takeoff_check_mode("STABILIZE") + self.test_takeoff_check_mode("ACRO") + self.test_takeoff_check_mode("LOITER") + self.test_takeoff_check_mode("ALT_HOLD") + # self.test_takeoff_check_mode("FLOWHOLD") + self.test_takeoff_check_mode("GUIDED", True) + self.test_takeoff_check_mode("POSHOLD") + # self.test_takeoff_check_mode("SPORT") + def assert_dataflash_message_field_level_at(self, mtype, field, @@ -2087,7 +2127,7 @@ def ModeFlip(self): self.progress("Regaining altitude") self.change_mode('ALT_HOLD') - self.wait_for_alt(20, max_err=40) + self.wait_altitude(19, 60, relative=True) self.progress("Flipping in pitch") self.set_rc(2, 1700) @@ -2427,7 +2467,7 @@ def AutoTuneSwitch(self): raise NotAchievedException("AUTOTUNE gains not present in pilot testing") # land without changing mode self.set_rc(3, 1000) - self.wait_for_alt(0) + self.wait_altitude(-1, 5, relative=True) self.wait_disarmed() # Check gains are still there after disarm if (rlld == self.get_parameter("ATC_RAT_RLL_D") or @@ -2581,7 +2621,23 @@ def CANGPSCopterMission(self): "CAN_P1_DRIVER": 1, "GPS_TYPE": 9, "GPS_TYPE2": 9, - "SIM_GPS2_DISABLE": 0, + # disable simulated GPS, so only via DroneCAN + "SIM_GPS_DISABLE": 1, + "SIM_GPS2_DISABLE": 1, + # this ensures we use DroneCAN baro and compass + "SIM_BARO_COUNT" : 0, + "SIM_MAG1_DEVID" : 0, + "SIM_MAG2_DEVID" : 0, + "SIM_MAG3_DEVID" : 0, + "COMPASS_USE2" : 0, + "COMPASS_USE3" : 0, + # use DroneCAN rangefinder + "RNGFND1_TYPE" : 24, + "RNGFND1_MAX_CM" : 11000, + # use DroneCAN battery monitoring, and enforce with a arming voltage + "BATT_MONITOR" : 8, + "BATT_ARM_VOLT" : 12.0, + "SIM_SPEEDUP": 2, }) self.context_push() @@ -2590,8 +2646,8 @@ def CANGPSCopterMission(self): self.reboot_sitl() # Test UAVCAN GPS ordering working - gps1_det_text = self.wait_text("GPS 1: specified as UAVCAN.*", regex=True, check_context=True) - gps2_det_text = self.wait_text("GPS 2: specified as UAVCAN.*", regex=True, check_context=True) + gps1_det_text = self.wait_text("GPS 1: specified as DroneCAN.*", regex=True, check_context=True) + gps2_det_text = self.wait_text("GPS 2: specified as DroneCAN.*", regex=True, check_context=True) gps1_nodeid = int(gps1_det_text.split('-')[1]) gps2_nodeid = int(gps2_det_text.split('-')[1]) if gps1_nodeid is None or gps2_nodeid is None: @@ -2621,11 +2677,11 @@ def CANGPSCopterMission(self): gps1_det_text = None gps2_det_text = None try: - gps1_det_text = self.wait_text("GPS 1: specified as UAVCAN.*", regex=True, check_context=True) + gps1_det_text = self.wait_text("GPS 1: specified as DroneCAN.*", regex=True, check_context=True) except AutoTestTimeoutException: pass try: - gps2_det_text = self.wait_text("GPS 2: specified as UAVCAN.*", regex=True, check_context=True) + gps2_det_text = self.wait_text("GPS 2: specified as DroneCAN.*", regex=True, check_context=True) except AutoTestTimeoutException: pass @@ -2649,16 +2705,12 @@ def CANGPSCopterMission(self): raise NotAchievedException("Failed ordering for requested CASE:", case) if len(case[4]): self.context_collect('STATUSTEXT') - self.run_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 1, # ARM - 0, - 0, - 0, - 0, - 0, - 0, - timeout=10, - want_result=mavutil.mavlink.MAV_RESULT_FAILED) + self.run_cmd( + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + p1=1, # ARM + timeout=10, + want_result=mavutil.mavlink.MAV_RESULT_FAILED, + ) self.wait_statustext(case[4], check_context=True) self.context_stop_collecting('STATUSTEXT') self.progress("############################### All GPS Order Cases Tests Passed") @@ -2666,23 +2718,34 @@ def CANGPSCopterMission(self): self.set_parameter("ARMING_CHECK", 1) self.stop_sup_program(instance=0) self.start_sup_program(instance=0, args="-M") + self.stop_sup_program(instance=1) + self.start_sup_program(instance=1, args="-M") self.delay_sim_time(2) self.context_collect('STATUSTEXT') - self.run_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 1, # ARM - 0, - 0, - 0, - 0, - 0, - 0, - timeout=10, - want_result=mavutil.mavlink.MAV_RESULT_FAILED) - self.wait_statustext("Node {} unhealthy".format(gps1_nodeid), check_context=True) + self.run_cmd( + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + p1=1, # ARM + timeout=10, + want_result=mavutil.mavlink.MAV_RESULT_FAILED, + ) + self.wait_statustext(".*Node .* unhealthy", check_context=True, regex=True) self.stop_sup_program(instance=0) self.start_sup_program(instance=0) + self.stop_sup_program(instance=1) + self.start_sup_program(instance=1) self.context_stop_collecting('STATUSTEXT') self.context_pop() + + self.set_parameters({ + # use DroneCAN ESCs for flight + "CAN_D1_UC_ESC_BM" : 0x0f, + # this stops us using local servo output, guaranteeing we are + # flying on DroneCAN ESCs + "SIM_CAN_SRV_MSK" : 0xFF, + # we can do the flight faster + "SIM_SPEEDUP" : 5, + }) + self.CopterMission() def TakeoffAlt(self): @@ -3163,6 +3226,71 @@ def FlyMissionTwice(self): self.wait_disarmed() self.delay_sim_time(20) + def FlyMissionTwiceWithReset(self): + '''Fly a mission twice in a row without changing modes in between. + Allow the mission to complete, then reset the mission state machine and restart the mission.''' + + self.upload_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 20), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 20, 0, 20), + (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0), + ]) + + num_wp = self.get_mission_count() + self.set_parameter("AUTO_OPTIONS", 3) + self.change_mode('AUTO') + self.wait_ready_to_arm() + + for i in 1, 2: + self.progress("run %u" % i) + # Use the "Reset Mission" param of DO_SET_MISSION_CURRENT to reset mission state machine + self.set_current_waypoint_using_mav_cmd_do_set_mission_current(seq=0, reset=1) + self.arm_vehicle() + self.wait_waypoint(num_wp-1, num_wp-1) + self.wait_disarmed() + self.delay_sim_time(20) + + def MissionIndexValidity(self): + '''Confirm that attempting to select an invalid mission item is rejected.''' + + self.upload_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 20), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 20, 0, 20), + (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0), + ]) + + num_wp = self.get_mission_count() + accepted_indices = [0, 1, num_wp-1] + denied_indices = [-1, num_wp] + + for seq in accepted_indices: + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_MISSION_CURRENT, + p1=seq, + timeout=1, + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED) + + for seq in denied_indices: + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_MISSION_CURRENT, + p1=seq, + timeout=1, + want_result=mavutil.mavlink.MAV_RESULT_DENIED) + + def InvalidJumpTags(self): + '''Verify the behaviour when selecting invalid jump tags.''' + + MAX_TAG_NUM = 65535 + # Jump tag is not present, so expect FAILED + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_JUMP_TAG, + p1=MAX_TAG_NUM, + timeout=1, + want_result=mavutil.mavlink.MAV_RESULT_FAILED) + + # Jump tag is too big, so expect DENIED + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_JUMP_TAG, + p1=MAX_TAG_NUM+1, + timeout=1, + want_result=mavutil.mavlink.MAV_RESULT_DENIED) + def GPSViconSwitching(self): """Fly GPS and Vicon switching test""" self.customise_SITL_commandline(["--uartF=sim:vicon:"]) @@ -3604,8 +3732,8 @@ def test_rangefinder_switchover(self): if ex is not None: raise ex - def Parachute(self): - '''Test Parachute Functionality''' + def _Parachute(self, command): + '''Test Parachute Functionality using specific mavlink command''' self.set_rc(9, 1000) self.set_parameters({ "CHUTE_ENABLED": 1, @@ -3628,14 +3756,10 @@ def Parachute(self): self.progress("Test triggering with mavlink message") self.takeoff(20) - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_PARACHUTE, - 2, # release - 0, - 0, - 0, - 0, - 0, - 0) + command( + mavutil.mavlink.MAV_CMD_DO_PARACHUTE, + p1=2, # release + ) self.wait_statustext('BANG', timeout=60) self.disarm_vehicle(force=True) self.reboot_sitl() @@ -3653,15 +3777,10 @@ def Parachute(self): self.progress("Test mavlink triggering") self.takeoff(20) - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_PARACHUTE, - mavutil.mavlink.PARACHUTE_DISABLE, # param1 - 0, # param2 - 0, # param3 - 0, # param4 - 0, # param5 - 0, # param6 - 0 # param7 - ) + command( + mavutil.mavlink.MAV_CMD_DO_PARACHUTE, + p1=mavutil.mavlink.PARACHUTE_DISABLE, + ) ok = False try: self.wait_statustext('BANG', timeout=2) @@ -3669,15 +3788,10 @@ def Parachute(self): ok = True if not ok: raise NotAchievedException("Disabled parachute fired") - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_PARACHUTE, - mavutil.mavlink.PARACHUTE_ENABLE, # param1 - 0, # param2 - 0, # param3 - 0, # param4 - 0, # param5 - 0, # param6 - 0 # param7 - ) + command( + mavutil.mavlink.MAV_CMD_DO_PARACHUTE, + p1=mavutil.mavlink.PARACHUTE_ENABLE, + ) ok = False try: self.wait_statustext('BANG', timeout=2) @@ -3692,15 +3806,10 @@ def Parachute(self): # parachute should not fire if you go from disabled to release: self.takeoff(20) - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_PARACHUTE, - mavutil.mavlink.PARACHUTE_RELEASE, - 0, # param2 - 0, # param3 - 0, # param4 - 0, # param5 - 0, # param6 - 0 # param7 - ) + command( + mavutil.mavlink.MAV_CMD_DO_PARACHUTE, + p1=mavutil.mavlink.PARACHUTE_RELEASE, + ) ok = False try: self.wait_statustext('BANG', timeout=2) @@ -3710,24 +3819,14 @@ def Parachute(self): raise NotAchievedException("Parachute fired when going straight from disabled to release") # now enable then release parachute: - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_PARACHUTE, - mavutil.mavlink.PARACHUTE_ENABLE, # param1 - 0, # param2 - 0, # param3 - 0, # param4 - 0, # param5 - 0, # param6 - 0 # param7 - ) - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_PARACHUTE, - mavutil.mavlink.PARACHUTE_RELEASE, - 0, # param2 - 0, # param3 - 0, # param4 - 0, # param5 - 0, # param6 - 0 # param7 - ) + command( + mavutil.mavlink.MAV_CMD_DO_PARACHUTE, + p1=mavutil.mavlink.PARACHUTE_ENABLE, + ) + command( + mavutil.mavlink.MAV_CMD_DO_PARACHUTE, + p1=mavutil.mavlink.PARACHUTE_RELEASE, + ) self.wait_statustext('BANG! Parachute deployed', timeout=2) self.disarm_vehicle(force=True) self.reboot_sitl() @@ -3767,47 +3866,10 @@ def Parachute(self): self.disarm_vehicle(force=True) self.reboot_sitl() - def MotorTest(self, timeout=60): - '''Run Motor Tests''' - self.start_subtest("Testing PWM output") - pwm_in = 1300 - # default frame is "+" - start motor of 2 is "B", which is - # motor 1... see - # https://ardupilot.org/copter/docs/connect-escs-and-motors.html - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, - 2, # start motor - mavutil.mavlink.MOTOR_TEST_THROTTLE_PWM, - pwm_in, # pwm-to-output - 2, # timeout in seconds - 2, # number of motors to output - 0, # compass learning - 0, - timeout=timeout) - # long timeouts here because there's a pause before we start motors - self.wait_servo_channel_value(1, pwm_in, timeout=10) - self.wait_servo_channel_value(4, pwm_in, timeout=10) - self.wait_statustext("finished motor test") - self.end_subtest("Testing PWM output") - - self.start_subtest("Testing percentage output") - percentage = 90.1 - # since MOT_SPIN_MIN and MOT_SPIN_MAX are not set, the RC3 - # min/max are used. - expected_pwm = 1000 + (self.get_parameter("RC3_MAX") - self.get_parameter("RC3_MIN")) * percentage/100.0 - self.progress("expected pwm=%f" % expected_pwm) - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, - 2, # start motor - mavutil.mavlink.MOTOR_TEST_THROTTLE_PERCENT, - percentage, # pwm-to-output - 2, # timeout in seconds - 2, # number of motors to output - 0, # compass learning - 0, - timeout=timeout) - self.wait_servo_channel_value(1, expected_pwm, timeout=10) - self.wait_servo_channel_value(4, expected_pwm, timeout=10) - self.wait_statustext("finished motor test") - self.end_subtest("Testing percentage output") + def Parachute(self): + '''Test Parachute Functionality''' + self._Parachute(self.run_cmd) + self._Parachute(self.run_cmd_int) def PrecisionLanding(self): """Use PrecLand backends precision messages to land aircraft.""" @@ -3851,7 +3913,7 @@ def PrecisionLanding(self): new_pos = self.mav.location() delta = self.get_distance(target, new_pos) self.progress("Landed %f metres from target position" % delta) - max_delta = 1 + max_delta = 1.5 if delta > max_delta: raise NotAchievedException("Did not land close enough to target position (%fm > %fm" % (delta, max_delta)) @@ -4370,7 +4432,7 @@ def fly_guided_move_local(self, x, y, z_up, timeout=100): MAV_POS_TARGET_TYPE_MASK.POS_ONLY | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE, # mask specifying use-only-x-y-z x, # x y, # y - -z_up,# z + -z_up, # z 0, # vx 0, # vy 0, # vz @@ -4568,7 +4630,7 @@ def precision_loiter_to_pos(self, x, y, z, timeout=40): # determine if we've successfully navigated to close to # where we should be: dist = math.sqrt(delta_ef.x * delta_ef.x + delta_ef.y * delta_ef.y) - dist_max = 0.15 + dist_max = 1 self.progress("dist=%f want <%f" % (dist, dist_max)) if dist < dist_max: # success! We've gotten within our target distance @@ -4603,7 +4665,7 @@ def precision_loiter_to_pos(self, x, y, z, timeout=40): 0.01 # size of target in radians, Y-axis ) - def PayLoadPlaceMission(self): + def PayloadPlaceMission(self): """Test payload placing in auto.""" self.context_push() @@ -4646,6 +4708,7 @@ def PayLoadPlaceMission(self): except Exception as e: self.print_exception_caught(e) + self.disarm_vehicle(force=True) ex = e self.context_pop() @@ -4699,6 +4762,74 @@ def Weathervane(self): self.wait_heading(100, accuracy=8, timeout=100) self.do_RTL() + def _DO_WINCH(self, command): + self.context_push() + self.load_default_params_file("copter-winch.parm") + self.reboot_sitl() + self.wait_ready_to_arm() + + self.start_subtest("starts relaxed") + self.wait_servo_channel_value(9, 0) + + self.start_subtest("rate control") + command( + mavutil.mavlink.MAV_CMD_DO_WINCH, + p1=1, # instance number + p2=mavutil.mavlink.WINCH_RATE_CONTROL, # command + p3=0, # length to release + p4=1, # rate in m/s + ) + self.wait_servo_channel_value(9, 1900) + + self.start_subtest("relax") + command( + mavutil.mavlink.MAV_CMD_DO_WINCH, + p1=1, # instance number + p2=mavutil.mavlink.WINCH_RELAXED, # command + p3=0, # length to release + p4=1, # rate in m/s + ) + self.wait_servo_channel_value(9, 0) + + self.start_subtest("hold but zero output") + command( + mavutil.mavlink.MAV_CMD_DO_WINCH, + p1=1, # instance number + p2=mavutil.mavlink.WINCH_RATE_CONTROL, # command + p3=0, # length to release + p4=0, # rate in m/s + ) + self.wait_servo_channel_value(9, 1500) + + self.start_subtest("relax") + command( + mavutil.mavlink.MAV_CMD_DO_WINCH, + p1=1, # instance number + p2=mavutil.mavlink.WINCH_RELAXED, # command + p3=0, # length to release + p4=1, # rate in m/s + ) + self.wait_servo_channel_value(9, 0) + + self.start_subtest("position") + command( + mavutil.mavlink.MAV_CMD_DO_WINCH, + p1=1, # instance number + p2=mavutil.mavlink.WINCH_RELATIVE_LENGTH_CONTROL, # command + p3=2, # length to release + p4=1, # rate in m/s + ) + self.wait_servo_channel_value(9, 1900) + self.wait_servo_channel_value(9, 1500, timeout=60) + + self.context_pop() + self.reboot_sitl() + + def DO_WINCH(self): + '''test mavlink DO_WINCH command''' + self._DO_WINCH(self.run_cmd_int) + self._DO_WINCH(self.run_cmd) + def GuidedSubModeChange(self): """"Ensure we can move around in guided after a takeoff command.""" @@ -4813,15 +4944,10 @@ def ManualThrottleModeChange(self): self.progress("Check setting an invalid mode") self.run_cmd( mavutil.mavlink.MAV_CMD_DO_SET_MODE, - mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, - 126, - 0, - 0, - 0, - 0, - 0, + p1=mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, + p2=126, want_result=mavutil.mavlink.MAV_RESULT_FAILED, - timeout=1 + timeout=1, ) self.set_rc(3, 1000) self.run_cmd_do_set_mode("ACRO") @@ -4848,8 +4974,7 @@ def test_mount_pitch(self, despitch, despitch_tolerance, mount_mode, timeout=10, (mount_pitch, despitch)) if success_start == 0: success_start = now - continue - if now - success_start > hold: + if now - success_start >= hold: self.progress("Mount pitch achieved") return @@ -4866,6 +4991,17 @@ def do_pitch(self, pitch): 0, # yaw rate (rad/s) 0.5) # thrust, 0 to 1, translated to a climb/descent rate + def do_yaw_rate(self, yaw_rate): + '''yaw aircraft in guided/rate mode''' + self.run_cmd( + mavutil.mavlink.MAV_CMD_CONDITION_YAW, + p1=60, # target angle + p2=0, # degrees/second + p3=1, # -1 is counter-clockwise, 1 clockwise + p4=1, # 1 for relative, 0 for absolute + quiet=True, + ) + def setup_servo_mount(self, roll_servo=5, pitch_servo=6, yaw_servo=7): '''configure a rpy servo mount; caller responsible for required rebooting''' self.progress("Setting up servo mount") @@ -4891,11 +5027,107 @@ def get_mount_roll_pitch_yaw_deg(self): def set_mount_mode(self, mount_mode): '''set mount mode''' - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE, - mount_mode, - 0, # stabilize roll (unsupported) - 0, # stabilize pitch (unsupported) - 0, 0, 0, 0) + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE, + p1=mount_mode, + p2=0, # stabilize roll (unsupported) + p3=0, # stabilize pitch (unsupported) + ) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE, + p1=mount_mode, + p2=0, # stabilize roll (unsupported) + p3=0, # stabilize pitch (unsupported) + ) + + def test_mount_rc_targetting(self): + '''called in multipleplaces to make sure that mount RC targetting works''' + try: + self.context_push() + self.set_parameters({ + 'RC6_OPTION': 0, + 'RC11_OPTION': 212, # MOUNT1_ROLL + 'RC12_OPTION': 213, # MOUNT1_PITCH + 'RC13_OPTION': 214, # MOUNT1_YAW + 'RC12_MIN': 1100, + 'RC12_MAX': 1900, + 'RC12_TRIM': 1500, + 'MNT1_PITCH_MIN': -45, + 'MNT1_PITCH_MAX': 45, + }) + self.progress("Testing RC angular control") + # default RC min=1100 max=1900 + self.set_rc_from_map({ + 11: 1500, + 12: 1500, + 13: 1500, + }) + self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.progress("Testing RC input down 1/4 of its range in the output, should be down 1/4 range in output") + rc12_in = 1400 + rc12_min = 1100 # default + rc12_max = 1900 # default + mpitch_min = -45.0 + mpitch_max = 45.0 + expected_pitch = (float(rc12_in-rc12_min)/float(rc12_max-rc12_min) * (mpitch_max-mpitch_min)) + mpitch_min + self.progress("expected mount pitch: %f" % expected_pitch) + if expected_pitch != -11.25: + raise NotAchievedException("Calculation wrong - defaults changed?!") + self.set_rc(12, rc12_in) + self.test_mount_pitch(-11.25, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.set_rc(12, 1800) + self.test_mount_pitch(33.75, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.set_rc_from_map({ + 11: 1500, + 12: 1500, + 13: 1500, + }) + + try: + self.context_push() + self.set_parameters({ + "RC12_MIN": 1000, + "RC12_MAX": 2000, + "MNT1_PITCH_MIN": -90, + "MNT1_PITCH_MAX": 10, + }) + self.set_rc(12, 1000) + self.test_mount_pitch(-90.00, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.set_rc(12, 2000) + self.test_mount_pitch(10.00, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.set_rc(12, 1500) + self.test_mount_pitch(-40.00, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + finally: + self.context_pop() + + self.set_rc(12, 1500) + + self.progress("Testing RC rate control") + self.set_parameter('MNT1_RC_RATE', 10) + self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.set_rc(12, 1300) + self.test_mount_pitch(-5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.test_mount_pitch(-10, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.test_mount_pitch(-15, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.test_mount_pitch(-20, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.set_rc(12, 1700) + self.test_mount_pitch(-15, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.test_mount_pitch(-10, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.test_mount_pitch(-5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.test_mount_pitch(5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + + self.progress("Reverting to angle mode") + self.set_parameter('MNT1_RC_RATE', 0) + self.set_rc(12, 1500) + self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + + self.context_pop() + + except Exception as e: + self.print_exception_caught(e) + self.context_pop() + raise e def Mount(self): '''Test Camera/Antenna Mount''' @@ -4954,22 +5186,25 @@ def Mount(self): self.progress("Pitching vehicle") self.do_pitch(despitch) self.wait_pitch(despitch, despitch_tolerance) - self.test_mount_pitch(-despitch, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) # point gimbal at specified angle self.progress("Point gimbal using GIMBAL_MANAGER_PITCHYAW (ANGLE)") self.do_pitch(0) # level vehicle self.wait_pitch(0, despitch_tolerance) self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING) - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW, - -20, # pitch angle in degrees - 0, # yaw angle in degrees - 0, # pitch rate in degrees (NaN to ignore) - 0, # yaw rate in degrees (NaN to ignore) - 0, # flags (0=Body-frame, 16/GIMBAL_MANAGER_FLAGS_YAW_LOCK=Earth Frame) - 0, # unused - 0) # gimbal id - self.test_mount_pitch(-20, 1, mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING) + for (method, angle) in (self.run_cmd, -20), (self.run_cmd_int, -30): + method( + mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW, + p1=angle, # pitch angle in degrees + p2=0, # yaw angle in degrees + p3=0, # pitch rate in degrees (NaN to ignore) + p4=0, # yaw rate in degrees (NaN to ignore) + p5=0, # flags (0=Body-frame, 16/GIMBAL_MANAGER_FLAGS_YAW_LOCK=Earth Frame) + p6=0, # unused + p7=0, # gimbal id + ) + self.test_mount_pitch(angle, 1, mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING) # point gimbal at specified location self.progress("Point gimbal at Location using MOUNT_CONTROL (GPS)") @@ -4997,9 +5232,6 @@ def Mount(self): ) self.test_mount_pitch(-52, 5, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT) - # now test RC targetting - self.progress("Testing mount RC targetting") - # this is a one-off; ArduCopter *will* time out this directive! self.progress("Levelling aircraft") self.mav.mav.set_attitude_target_send( @@ -5013,94 +5245,13 @@ def Mount(self): 0, # yaw rate (rad/s) 0.5) # thrust, 0 to 1, translated to a climb/descent rate - self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - - try: - self.context_push() - self.set_parameters({ - 'RC6_OPTION': 0, - 'RC11_OPTION': 212, # MOUNT1_ROLL - 'RC12_OPTION': 213, # MOUNT1_PITCH - 'RC13_OPTION': 214, # MOUNT1_YAW - 'RC12_MIN': 1100, - 'RC12_MAX': 1900, - 'RC12_TRIM': 1500, - 'MNT1_PITCH_MIN': -45, - 'MNT1_PITCH_MAX': 45, - }) - self.progress("Testing RC angular control") - # default RC min=1100 max=1900 - self.set_rc_from_map({ - 11: 1500, - 12: 1500, - 13: 1500, - }) - self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.progress("Testing RC input down 1/4 of its range in the output, should be down 1/4 range in output") - rc12_in = 1400 - rc12_min = 1100 # default - rc12_max = 1900 # default - mpitch_min = -45.0 - mpitch_max = 45.0 - expected_pitch = (float(rc12_in-rc12_min)/float(rc12_max-rc12_min) * (mpitch_max-mpitch_min)) + mpitch_min - self.progress("expected mount pitch: %f" % expected_pitch) - if expected_pitch != -11.25: - raise NotAchievedException("Calculation wrong - defaults changed?!") - self.set_rc(12, rc12_in) - self.test_mount_pitch(-11.25, 0.01, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.set_rc(12, 1800) - self.test_mount_pitch(33.75, 0.01, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.set_rc_from_map({ - 11: 1500, - 12: 1500, - 13: 1500, - }) - - try: - self.context_push() - self.set_parameters({ - "RC12_MIN": 1000, - "RC12_MAX": 2000, - "MNT1_PITCH_MIN": -90, - "MNT1_PITCH_MAX": 10, - }) - self.set_rc(12, 1000) - self.test_mount_pitch(-90.00, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.set_rc(12, 2000) - self.test_mount_pitch(10.00, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.set_rc(12, 1500) - self.test_mount_pitch(-40.00, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - finally: - self.context_pop() - - self.set_rc(12, 1500) - - self.progress("Testing RC rate control") - self.set_parameter('MNT1_RC_RATE', 10) - self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.set_rc(12, 1300) - self.test_mount_pitch(-5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_pitch(-10, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_pitch(-15, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_pitch(-20, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.set_rc(12, 1700) - self.test_mount_pitch(-15, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_pitch(-10, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_pitch(-5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_pitch(5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - - self.progress("Reverting to angle mode") - self.set_parameter('MNT1_RC_RATE', 0) - self.set_rc(12, 1500) - self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.wait_groundspeed(0, 1) - self.context_pop() + # now test RC targetting + self.progress("Testing mount RC targetting") - except Exception as e: - self.print_exception_caught(e) - self.context_pop() - raise e + self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.test_mount_rc_targetting() self.progress("Testing mount ROI behaviour") self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) @@ -5112,27 +5263,35 @@ def Mount(self): 20) roi_alt = 0 self.progress("Using MAV_CMD_DO_SET_ROI_LOCATION") - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION, - 0, - 0, - 0, - 0, - roi_lat, - roi_lon, - roi_alt, - ) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION, + p5=roi_lat, + p6=roi_lon, + p7=roi_alt, + ) + self.test_mount_pitch(-52, 5, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT) + self.progress("Using MAV_CMD_DO_SET_ROI_LOCATION") + # start by pointing the gimbal elsewhere with a + # known-working command: + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION, + p5=roi_lat + 1, + p6=roi_lon + 1, + p7=roi_alt, + ) + # now point it with command_int: + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION, + p5=int(roi_lat * 1e7), + p6=int(roi_lon * 1e7), + p7=roi_alt, + frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + ) self.test_mount_pitch(-52, 5, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT) self.progress("Using MAV_CMD_DO_SET_ROI_NONE") - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI_NONE, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - ) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI_NONE) + self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_SET_ROI_NONE) self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) start = self.mav.location() @@ -5142,15 +5301,12 @@ def Mount(self): -200) roi_alt = 0 self.progress("Using MAV_CMD_DO_SET_ROI") - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI, - 0, - 0, - 0, - 0, - roi_lat, - roi_lon, - roi_alt, - ) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_SET_ROI, + p5=roi_lat, + p6=roi_lon, + p7=roi_alt, + ) self.test_mount_pitch(-7.5, 1, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT) start = self.mav.location() @@ -5200,15 +5356,10 @@ def Mount(self): 20) roi_alt = 0 self.progress("Using MAV_CMD_DO_SET_ROI_SYSID") - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI_SYSID, - self.mav.source_system, - 0, - 0, - 0, - 0, - 0, - 0, - ) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_SET_ROI_SYSID, + p1=self.mav.source_system, + ) self.mav.mav.global_position_int_send( 0, # time boot ms int(roi_lat * 1e7), @@ -5222,6 +5373,11 @@ def Mount(self): ) self.test_mount_pitch(-89, 5, mavutil.mavlink.MAV_MOUNT_MODE_SYSID_TARGET, hold=2) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI_NONE) + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_SET_ROI_SYSID, + p1=self.mav.source_system, + ) self.mav.mav.global_position_int_send( 0, # time boot ms int(roi_lat * 1e7), @@ -5251,6 +5407,193 @@ def Mount(self): if ex is not None: raise ex + def assert_mount_rpy(self, r, p, y, tolerance=1): + '''assert mount atttiude in degrees''' + got_r, got_p, got_y = self.get_mount_roll_pitch_yaw_deg() + for (want, got, name) in (r, got_r, "roll"), (p, got_p, "pitch"), (y, got_y, "yaw"): + if abs(want - got) > tolerance: + raise NotAchievedException("%s incorrect; want=%f got=%f" % + (name, want, got)) + + def neutralise_gimbal(self): + '''put mount into neutralise mode, assert it is at zero angles''' + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, + p7=mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL, + ) + self.test_mount_pitch(0, 0, mavutil.mavlink.MAV_MOUNT_MODE_RETRACT) + + def MAV_CMD_DO_MOUNT_CONTROL(self): + '''test MAV_CMD_DO_MOUNT_CONTROL mavlink command''' + + # setup mount parameters + self.context_push() + self.setup_servo_mount() + self.reboot_sitl() # to handle MNT_TYPE changing + + takeoff_loc = self.mav.location() + + self.takeoff(20, mode='GUIDED') + self.guided_achieve_heading(315) + + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, + p7=mavutil.mavlink.MAV_MOUNT_MODE_RETRACT, + ) + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, + p7=mavutil.mavlink.MAV_MOUNT_MODE_RETRACT, + ) + + for method in self.run_cmd, self.run_cmd_int: + self.start_subtest("MAV_MOUNT_MODE_GPS_POINT") + + self.progress("start=%s" % str(takeoff_loc)) + t = self.offset_location_ne(takeoff_loc, 20, 0) + self.progress("targetting=%s" % str(t)) + + # this command is *weird* as the lat/lng is *always* 1e7, + # even when transported via COMMAND_LONG! + x = int(t.lat * 1e7) + y = int(t.lng * 1e7) + method( + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, + p4=0, # this is a relative altitude! + p5=x, + p6=y, + p7=mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT, + ) + self.test_mount_pitch(-45, 5, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT) + self.neutralise_gimbal() + + self.start_subtest("MAV_MOUNT_MODE_HOME_LOCATION") + method( + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, + p7=mavutil.mavlink.MAV_MOUNT_MODE_HOME_LOCATION, + ) + self.test_mount_pitch(-90, 5, mavutil.mavlink.MAV_MOUNT_MODE_HOME_LOCATION) + self.neutralise_gimbal() + + # try an invalid mount mode. Note that this is asserting we + # are receiving a result code which is actually incorrect; + # this should be MAV_RESULT_DENIED + self.start_subtest("Invalid mode") + method( + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, + p7=87, + want_result=mavutil.mavlink.MAV_RESULT_FAILED, + ) + + self.start_subtest("MAV_MOUNT_MODE_MAVLINK_TARGETING") + r = 15 + p = 20 + y = 30 + method( + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, + p1=p, + p2=r, + p3=y, + p7=mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, + ) + self.delay_sim_time(2) + self.assert_mount_rpy(r, p, y) + self.neutralise_gimbal() + + self.start_subtest("MAV_MOUNT_MODE_RC_TARGETING") + method( + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, + p7=mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, + ) + self.test_mount_rc_targetting() + + self.start_subtest("MAV_MOUNT_MODE_RETRACT") + self.context_push() + retract_r = 13 + retract_p = 23 + retract_y = 33 + self.set_parameters({ + "MNT1_RETRACT_X": retract_r, + "MNT1_RETRACT_Y": retract_p, + "MNT1_RETRACT_Z": retract_y, + }) + method( + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, + p7=mavutil.mavlink.MAV_MOUNT_MODE_RETRACT, + ) + self.delay_sim_time(3) + self.assert_mount_rpy(retract_r, retract_p, retract_y) + self.context_pop() + + self.do_RTL() + + self.context_pop() + self.reboot_sitl() + + def MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE(self): + '''test MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE mavlink command''' + # setup mount parameters + self.context_push() + self.setup_servo_mount() + self.reboot_sitl() # to handle MNT_TYPE changing + + self.context_set_message_rate_hz('GIMBAL_MANAGER_STATUS', 10) + self.assert_received_message_field_values('GIMBAL_MANAGER_STATUS', { + "gimbal_device_id": 1, + "primary_control_sysid": 0, + "primary_control_compid": 0, + }) + + for method in self.run_cmd, self.run_cmd_int: + self.start_subtest("set_sysid-compid") + method( + mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, + p1=37, + p2=38, + ) + self.assert_received_message_field_values('GIMBAL_MANAGER_STATUS', { + "gimbal_device_id": 1, + "primary_control_sysid": 37, + "primary_control_compid": 38, + }) + + self.start_subtest("leave unchanged") + method(mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, p1=-1) + self.assert_received_message_field_values('GIMBAL_MANAGER_STATUS', { + "gimbal_device_id": 1, + "primary_control_sysid": 37, + "primary_control_compid": 38, + }) + + # ardupilot currently handles this incorrectly: + # self.start_subtest("self-controlled") + # method(mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, p1=-2) + # self.assert_received_message_field_values('GIMBAL_MANAGER_STATUS', { + # "gimbal_device_id": 1, + # "primary_control_sysid": 1, + # "primary_control_compid": 1, + # }) + + self.start_subtest("release control") + method( + mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, + p1=self.mav.source_system, + p2=self.mav.source_component, + ) + self.assert_received_message_field_values('GIMBAL_MANAGER_STATUS', { + "gimbal_device_id": 1, + "primary_control_sysid": self.mav.source_system, + "primary_control_compid": self.mav.source_component, + }) + method(mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, p1=-3) + self.assert_received_message_field_values('GIMBAL_MANAGER_STATUS', { + "gimbal_device_id": 1, + "primary_control_sysid": 0, + "primary_control_compid": 0, + }) + + self.context_pop() + self.reboot_sitl() + def MountYawVehicleForMountROI(self): '''Test Camera/Antenna Mount vehicle yawing for ROI''' self.context_push() @@ -5280,15 +5623,12 @@ def MountYawVehicleForMountROI(self): -100) roi_alt = 0 self.progress("Using MAV_CMD_DO_SET_ROI") - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI, - 0, - 0, - 0, - 0, - roi_lat, - roi_lon, - roi_alt, - ) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_SET_ROI, + p5=roi_lat, + p6=roi_lon, + p7=roi_alt, + ) self.progress("Waiting for vehicle to point towards ROI") self.wait_heading(225, timeout=600, minimum_duration=2) @@ -5322,15 +5662,12 @@ def MountYawVehicleForMountROI(self): bearing = self.bearing_to(there) self.wait_heading(bearing, timeout=600, minimum_duration=2) - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI, - 0, - 0, - 0, - 0, - roi_lat, - roi_lon, - roi_alt, - ) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_SET_ROI, + p5=roi_lat, + p6=roi_lon, + p7=roi_alt, + ) self.progress("Wait for vehicle to point sssse due to moving") self.wait_heading(170, timeout=600, minimum_duration=1) @@ -5623,25 +5960,64 @@ def extract_median_FTN1_PkAvg_from_current_onboard_log(self, tstart, tend): freqs.append(m.PkAvg) return numpy.median(numpy.asarray(freqs)), len(freqs) - def ThrottleGainBoost(self): - """Use PD and Angle P boost for anti-gravity.""" - # basic gyro sample rate test - self.progress("Flying with Throttle-Gain Boost") + def PIDNotches(self): + """Use dynamic harmonic notch to control motor noise.""" + self.progress("Flying with PID notches") self.context_push() ex = None try: - # magic tridge EKF type that dramatically speeds up the test self.set_parameters({ + "FILT1_TYPE": 1, "AHRS_EKF_TYPE": 10, - "EK2_ENABLE": 0, - "EK3_ENABLE": 0, - "INS_FAST_SAMPLE": 0, - "LOG_BITMASK": 959, + "INS_LOG_BAT_MASK": 3, + "INS_LOG_BAT_OPT": 0, + "INS_GYRO_FILTER": 100, # set the gyro filter high so we can observe behaviour + "LOG_BITMASK": 65535, "LOG_DISARMED": 0, - "ATC_THR_G_BOOST": 5.0, + "SIM_VIB_FREQ_X": 120, # roll + "SIM_VIB_FREQ_Y": 120, # pitch + "SIM_VIB_FREQ_Z": 180, # yaw + "FILT1_NOTCH_FREQ": 120, + "ATC_RAT_RLL_NEF": 1, + "ATC_RAT_PIT_NEF": 1, + "ATC_RAT_YAW_NEF": 1, + "SIM_GYR1_RND": 5, }) - + self.reboot_sitl() + + self.takeoff(10, mode="ALT_HOLD") + + freq, hover_throttle, peakdb1 = self.hover_and_check_matched_frequency_with_fft(5, 20, 350, reverse=True) + + except Exception as e: + self.print_exception_caught(e) + ex = e + + self.context_pop() + + if ex is not None: + raise ex + + def ThrottleGainBoost(self): + """Use PD and Angle P boost for anti-gravity.""" + # basic gyro sample rate test + self.progress("Flying with Throttle-Gain Boost") + self.context_push() + + ex = None + try: + # magic tridge EKF type that dramatically speeds up the test + self.set_parameters({ + "AHRS_EKF_TYPE": 10, + "EK2_ENABLE": 0, + "EK3_ENABLE": 0, + "INS_FAST_SAMPLE": 0, + "LOG_BITMASK": 959, + "LOG_DISARMED": 0, + "ATC_THR_G_BOOST": 5.0, + }) + self.reboot_sitl() self.takeoff(10, mode="ALT_HOLD") @@ -6739,8 +7115,8 @@ def OBSTACLE_DISTANCE_3D(self): self.context_pop() self.reboot_sitl() - def fly_proximity_avoidance_test_corners(self): - self.start_subtest("Corners") + def AC_Avoidance_Proximity(self): + '''Test proximity avoidance slide behaviour''' self.context_push() ex = None try: @@ -6779,14 +7155,14 @@ def ProximitySensors(self): }) sensors = [ # tuples of name, prx_type ('sf45b', 8, { - mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 285, - mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 256, - mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 1131, - mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 1283, - mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 625, - mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 968, - mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 760, - mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 762, + mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 270, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 258, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 1146, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 632, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 629, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 972, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 774, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 774, }), ('rplidara2', 5, { mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 277, @@ -6826,6 +7202,7 @@ def ProximitySensors(self): failed = False wants = [] gots = [] + epsilon = 20 while True: if self.get_sim_time_cached() - tstart > 30: raise AutoTestTimeoutException("Failed to get distances") @@ -6838,7 +7215,7 @@ def ProximitySensors(self): want = expected_distances_copy[m.orientation] wants.append(want) gots.append(got) - if abs(want - got) > 5: + if abs(want - got) > epsilon: failed = True del expected_distances_copy[m.orientation] if failed: @@ -6846,8 +7223,8 @@ def ProximitySensors(self): "Distance too great (%s) (want=%s != got=%s)" % (name, wants, gots)) - def fly_proximity_avoidance_test_alt_no_avoid(self): - self.start_subtest("Alt-no-avoid") + def AC_Avoidance_Proximity_AVOID_ALT_MIN(self): + '''Test proximity avoidance with AVOID_ALT_MIN''' self.context_push() ex = None try: @@ -6857,9 +7234,11 @@ def fly_proximity_avoidance_test_alt_no_avoid(self): }) self.set_analog_rangefinder_parameters() self.reboot_sitl() - tstart = self.get_sim_time() + self.change_mode('LOITER') self.wait_ekf_happy() + + tstart = self.get_sim_time() while True: if self.armed(): break @@ -6875,14 +7254,8 @@ def fly_proximity_avoidance_test_alt_no_avoid(self): mavutil.mavlink.MAV_SENSOR_ROTATION_NONE, # orientation 255 # covariance ) - self.send_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 1, # ARM - 0, - 0, - 0, - 0, - 0, - 0) + self.send_mavlink_arm_command() + self.takeoff(15, mode='LOITER') self.progress("Poking vehicle; should avoid") @@ -6910,7 +7283,7 @@ def shove(a, b): if self.get_sim_time_cached() - tstart > 10: break vel = self.get_body_frame_velocity() - if vel.length() > 0.3: + if vel.length() > 0.5: raise NotAchievedException("Moved too much (%s)" % (str(vel),)) shove(None, None) @@ -6925,27 +7298,11 @@ def shove(a, b): if ex is not None: raise ex - def AC_Avoidance_Proximity(self): - '''Test proximity avoidance slide behaviour''' - self.fly_proximity_avoidance_test_alt_no_avoid() - self.fly_proximity_avoidance_test_corners() - def AC_Avoidance_Fence(self): '''Test fence avoidance slide behaviour''' - self.context_push() - ex = None - try: - self.load_fence("copter-avoidance-fence.txt") - self.set_parameter("FENCE_ENABLE", 1) - self.check_avoidance_corners() - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() - self.clear_fence() - self.disarm_vehicle(force=True) - if ex is not None: - raise ex + self.load_fence("copter-avoidance-fence.txt") + self.set_parameter("FENCE_ENABLE", 1) + self.check_avoidance_corners() def global_position_int_for_location(self, loc, time_boot, heading=0): return self.mav.mav.global_position_int_encode( @@ -7321,6 +7678,12 @@ def RichenPower(self): raise NotAchievedException("Did not find expected GEN message") def IE24(self): + '''Test IntelligentEnergy 2.4kWh generator with V1 and V2 telemetry protocols''' + protocol_ver = (1, 2) + for ver in protocol_ver: + self.run_IE24(ver) + + def run_IE24(self, proto_ver): '''Test IntelligentEnergy 2.4kWh generator''' elec_battery_instance = 2 fuel_battery_instance = 1 @@ -7330,14 +7693,14 @@ def IE24(self): "GEN_TYPE": 2, "BATT%u_MONITOR" % (fuel_battery_instance + 1): 18, # fuel-based generator "BATT%u_MONITOR" % (elec_battery_instance + 1): 17, - "SIM_IE24_ENABLE": 1, + "SIM_IE24_ENABLE": proto_ver, "LOG_DISARMED": 1, }) self.customise_SITL_commandline(["--uartF=sim:ie24"]) - self.start_subtest("ensure that BATTERY_STATUS for electrical generator message looks right") - self.start_subsubtest("Checking original voltage (electrical)") + self.start_subtest("Protocol %i: ensure that BATTERY_STATUS for electrical generator message looks right" % proto_ver) + self.start_subsubtest("Protocol %i: Checking original voltage (electrical)" % proto_ver) # ArduPilot spits out essentially uninitialised battery # messages until we read things fromthe battery: self.delay_sim_time(30) @@ -7355,13 +7718,13 @@ def IE24(self): "battery_remaining": original_elec_m.battery_remaining - 1, }, instance=elec_battery_instance) - self.start_subtest("ensure that BATTERY_STATUS for fuel generator message looks right") - self.start_subsubtest("Checking original voltage (fuel)") + self.start_subtest("Protocol %i: ensure that BATTERY_STATUS for fuel generator message looks right" % proto_ver) + self.start_subsubtest("Protocol %i: Checking original voltage (fuel)" % proto_ver) # ArduPilot spits out essentially uninitialised battery # messages until we read things fromthe battery: if original_fuel_m.battery_remaining <= 90: raise NotAchievedException("Bad original percentage (want=>%f got %f" % (90, original_fuel_m.battery_remaining)) - self.start_subsubtest("Ensure percentage is counting down") + self.start_subsubtest("Protocol %i: Ensure percentage is counting down" % proto_ver) self.wait_message_field_values('BATTERY_STATUS', { "battery_remaining": original_fuel_m.battery_remaining - 1, }, instance=fuel_battery_instance) @@ -7371,7 +7734,7 @@ def IE24(self): self.disarm_vehicle() # Test for pre-arm check fail when state is not running - self.start_subtest("If you haven't taken off generator error should cause instant failsafe and disarm") + self.start_subtest("Protocol %i: Without takeoff generator error should cause failsafe and disarm" % proto_ver) self.set_parameter("SIM_IE24_STATE", 8) self.wait_statustext("Status not running", timeout=40) self.try_arm(result=False, @@ -7379,7 +7742,7 @@ def IE24(self): self.set_parameter("SIM_IE24_STATE", 2) # Explicitly set state to running # Test that error code does result in failsafe - self.start_subtest("If you haven't taken off generator error should cause instant failsafe and disarm") + self.start_subtest("Protocol %i: Without taken off generator error should cause failsafe and disarm" % proto_ver) self.change_mode("STABILIZE") self.set_parameter("DISARM_DELAY", 0) self.arm_vehicle() @@ -7423,6 +7786,38 @@ def AuxFunctionsInMission(self): self.change_mode('AUTO') self.wait_rtl_complete() + def MAV_CMD_AIRFRAME_CONFIGURATION(self): + '''deploy/retract landing gear using mavlink command''' + self.context_push() + self.set_parameters({ + "LGR_ENABLE": 1, + "SERVO10_FUNCTION": 29, + "SERVO10_MIN": 1001, + "SERVO10_MAX": 1999, + }) + self.reboot_sitl() + + # starts loose: + self.wait_servo_channel_value(10, 0) + + # 0 is down: + self.start_subtest("Put gear down") + self.run_cmd(mavutil.mavlink.MAV_CMD_AIRFRAME_CONFIGURATION, p2=0) + self.wait_servo_channel_value(10, 1999) + + # 1 is up: + self.start_subtest("Put gear up") + self.run_cmd_int(mavutil.mavlink.MAV_CMD_AIRFRAME_CONFIGURATION, p2=1) + self.wait_servo_channel_value(10, 1001) + + # 0 is down: + self.start_subtest("Put gear down") + self.run_cmd(mavutil.mavlink.MAV_CMD_AIRFRAME_CONFIGURATION, p2=0) + self.wait_servo_channel_value(10, 1999) + + self.context_pop() + self.reboot_sitl() + def WatchAlts(self): '''Ensure we can monitor different altitudes''' self.takeoff(30, mode='GUIDED') @@ -7659,13 +8054,7 @@ def fly_rangefinder_mavlink_distance_sensor(self): self.run_cmd( mavutil.mavlink.MAV_CMD_AIRFRAME_CONFIGURATION, - 0, - 0, # deploy - 0, - 0, - 0, - 0, - 0 + p2=0, # deploy ) self.context_collect("STATUSTEXT") @@ -7723,29 +8112,19 @@ def fly_rangefinder_mavlink_distance_sensor(self): def GSF(self): '''test the Gaussian Sum filter''' - ex = None self.context_push() - try: - self.set_parameter("EK2_ENABLE", 1) - self.reboot_sitl() - self.takeoff(20, mode='LOITER') - self.set_rc(2, 1400) - self.delay_sim_time(5) - self.set_rc(2, 1500) - self.progress("Path: %s" % self.current_onboard_log_filepath()) - dfreader = self.dfreader_for_current_onboard_log() - self.do_RTL() - except Exception as e: - self.progress("Caught exception: %s" % - self.get_exception_stacktrace(e)) - ex = e - + self.set_parameter("EK2_ENABLE", 1) + self.reboot_sitl() + self.takeoff(20, mode='LOITER') + self.set_rc(2, 1400) + self.delay_sim_time(5) + self.set_rc(2, 1500) + self.progress("Path: %s" % self.current_onboard_log_filepath()) + dfreader = self.dfreader_for_current_onboard_log() + self.do_RTL() self.context_pop() self.reboot_sitl() - if ex is not None: - raise ex - # ensure log messages present want = set(["XKY0", "XKY1", "NKY0", "NKY1"]) still_want = want @@ -7755,6 +8134,46 @@ def GSF(self): raise NotAchievedException("Did not get %s" % want) still_want.remove(m.get_type()) + def GSF_reset(self): + '''test the Gaussian Sum filter based Emergency reset''' + self.context_push() + self.set_parameters({ + "COMPASS_ORIENT": 4, # yaw 180 + "COMPASS_USE2": 0, # disable backup compasses to avoid pre-arm failures + "COMPASS_USE3": 0, + }) + self.reboot_sitl() + self.change_mode('GUIDED') + self.wait_ready_to_arm() + + # record starting position + startpos = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True) + self.progress("startpos=%s" % str(startpos)) + + # arm vehicle and takeoff to at least 5m + self.arm_vehicle() + expected_alt = 5 + self.user_takeoff(alt_min=expected_alt) + + # watch for emergency yaw reset + self.wait_text("EKF3 IMU. emergency yaw reset", timeout=5, regex=True) + + # record how far vehicle flew off + endpos = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True) + delta_x = endpos.x - startpos.x + delta_y = endpos.y - startpos.y + dist_m = math.sqrt(delta_x*delta_x + delta_y*delta_y) + self.progress("GSF yaw reset triggered at %f meters" % dist_m) + + self.do_RTL() + self.context_pop() + self.reboot_sitl() + + # ensure vehicle did not fly too far + dist_m_max = 8 + if dist_m > dist_m_max: + raise NotAchievedException("GSF reset failed, vehicle flew too far (%f > %f)" % (dist_m, dist_m_max)) + def fly_rangefinder_mavlink(self): self.fly_rangefinder_mavlink_distance_sensor() @@ -7903,6 +8322,7 @@ def RangeFinderDrivers(self): ("benewake_tf03", 27), ("gyus42v2", 31), ("teraranger_serial", 35), + ("nooploop_tofsense", 37), ] while len(drivers): do_drivers = drivers[0:3] @@ -8307,7 +8727,9 @@ def FlyEachFrame(self): 'heli-compound': "wrong binary, different takeoff regime", 'heli-dual': "wrong binary, different takeoff regime", 'heli': "wrong binary, different takeoff regime", + 'heli-gas': "wrong binary, different takeoff regime", 'heli-blade360': "wrong binary, different takeoff regime", + "quad-can" : "needs CAN periph", } for frame in sorted(copter_vinfo_options["frames"].keys()): self.start_subtest("Testing frame (%s)" % str(frame)) @@ -8326,7 +8748,7 @@ def FlyEachFrame(self): # to carry the path to the JSON. actual_model = model.split(":")[0] defaults = self.model_defaults_filepath(actual_model) - if type(defaults) != list: + if not isinstance(defaults, list): defaults = [defaults] self.customise_SITL_commandline( ["--defaults", ','.join(defaults), ], @@ -8338,13 +8760,14 @@ def FlyEachFrame(self): def verify_yaw(mav, m): if m.get_type() != 'ATTITUDE': return - yawspeed_thresh_rads = math.radians(10) + yawspeed_thresh_rads = math.radians(20) if m.yawspeed > yawspeed_thresh_rads: raise NotAchievedException("Excessive yaw on takeoff: %f deg/s > %f deg/s (frame=%s)" % (math.degrees(m.yawspeed), math.degrees(yawspeed_thresh_rads), frame)) - self.install_message_hook(verify_yaw) + self.context_push() + self.install_message_hook_context(verify_yaw) self.takeoff(10) - self.remove_message_hook(verify_yaw) + self.context_pop() self.hover() self.change_mode('ALT_HOLD') self.delay_sim_time(1) @@ -8360,13 +8783,14 @@ def verify_rollpitch(mav, m): if m.roll > roll_thresh_rad: raise NotAchievedException("Excessive roll %f deg > %f deg" % (math.degrees(m.roll), math.degrees(roll_thresh_rad))) - self.install_message_hook(verify_rollpitch) + self.context_push() + self.install_message_hook_context(verify_rollpitch) for i in range(5): self.set_rc(4, 2000) self.delay_sim_time(0.5) self.set_rc(4, 1500) self.delay_sim_time(5) - self.remove_message_hook(verify_rollpitch) + self.context_pop() self.do_RTL() @@ -8698,7 +9122,7 @@ def GroundEffectCompensation_takeOffExpected(self): raise NotAchievedException("Was expecting takeoff for longer than expected; got=%f want<=%f" % (duration, want_lt)) - def MAV_CMD_CONDITION_YAW_absolute(self): + def _MAV_CMD_CONDITION_YAW(self, command): self.start_subtest("absolute") self.takeoff(20, mode='GUIDED') @@ -8707,17 +9131,15 @@ def MAV_CMD_CONDITION_YAW_absolute(self): self.progress("Ensuring initial heading is steady") target = initial_heading - self.run_cmd( + command( mavutil.mavlink.MAV_CMD_CONDITION_YAW, - target, # target angle - 10, # degrees/second - 1, # -1 is counter-clockwise, 1 clockwise - 0, # 1 for relative, 0 for absolute - 0, # p5 - 0, # p6 - 0, # p7 + p1=target, # target angle + p2=10, # degrees/second + p3=1, # -1 is counter-clockwise, 1 clockwise + p4=0, # 1 for relative, 0 for absolute ) self.wait_heading(target, minimum_duration=2, timeout=50) + self.wait_yaw_speed(0) degsecond = 2 @@ -8731,15 +9153,12 @@ def rate_watcher(mav, m): self.progress("Yaw CW 60 degrees") target = initial_heading + 60 part_way_target = initial_heading + 10 - self.run_cmd( + command( mavutil.mavlink.MAV_CMD_CONDITION_YAW, - target, # target angle - degsecond, # degrees/second - 1, # -1 is counter-clockwise, 1 clockwise - 0, # 1 for relative, 0 for absolute - 0, # p5 - 0, # p6 - 0, # p7 + p1=target, # target angle + p2=degsecond, # degrees/second + p3=1, # -1 is counter-clockwise, 1 clockwise + p4=0, # 1 for relative, 0 for absolute ) self.wait_heading(part_way_target) self.wait_heading(target, minimum_duration=2) @@ -8747,28 +9166,27 @@ def rate_watcher(mav, m): self.progress("Yaw CCW 60 degrees") target = initial_heading part_way_target = initial_heading + 30 - self.run_cmd( + command( mavutil.mavlink.MAV_CMD_CONDITION_YAW, - target, # target angle - degsecond, # degrees/second - -1, # -1 is counter-clockwise, 1 clockwise - 0, # 1 for relative, 0 for absolute - 0, # p5 - 0, # p6 - 0, # p7 + p1=target, # target angle + p2=degsecond, # degrees/second + p3=-1, # -1 is counter-clockwise, 1 clockwise + p4=0, # 1 for relative, 0 for absolute ) self.wait_heading(part_way_target) self.wait_heading(target, minimum_duration=2) - self.do_RTL() - - def MAV_CMD_CONDITION_YAW_relative(self): - pass + self.disarm_vehicle(force=True) + self.reboot_sitl() def MAV_CMD_CONDITION_YAW(self): - '''Test response to MAV_CMD_CONDITION_YAW''' - self.MAV_CMD_CONDITION_YAW_absolute() - self.MAV_CMD_CONDITION_YAW_relative() + '''Test response to MAV_CMD_CONDITION_YAW via mavlink''' + self.context_push() + self._MAV_CMD_CONDITION_YAW(self.run_cmd_int) + self.context_pop() + self.context_push() + self._MAV_CMD_CONDITION_YAW(self.run_cmd) + self.context_pop() def GroundEffectCompensation_touchDownExpected(self): '''Test EKF's handling of touchdown-expected''' @@ -8810,10 +9228,10 @@ def RefindGPS(self): '''Refind the GPS and attempt to RTL rather than continue to land''' # https://github.com/ArduPilot/ardupilot/issues/14236 self.progress("arm the vehicle and takeoff in Guided") - self.takeoff(20, mode='GUIDED') + self.takeoff(50, mode='GUIDED') self.progress("fly 50m North (or whatever)") old_pos = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) - self.fly_guided_move_global_relative_alt(50, 0, 20) + self.fly_guided_move_global_relative_alt(50, 0, 50) self.set_parameter('GPS_TYPE', 0) self.drain_mav() tstart = self.get_sim_time() @@ -9113,9 +9531,6 @@ def DO_CHANGE_SPEED(self): def AUTO_LAND_TO_BRAKE(self): '''ensure terrain altitude is taken into account when braking''' - self.load_mission('mission.txt') - home_loc = self.get_home_tuple_from_mission("mission.txt") - self.set_parameters({ "PLND_ACC_P_NSE": 2.500000, "PLND_ALT_MAX": 8.000000, @@ -9153,8 +9568,9 @@ def AUTO_LAND_TO_BRAKE(self): self.set_analog_rangefinder_parameters() + self.load_mission('mission.txt') self.customise_SITL_commandline([ - "--home", "%s,%s,%s,%s" % home_loc + "--home", self.sitl_home_string_from_mission("mission.txt"), ]) self.set_parameter('AUTO_OPTIONS', 3) @@ -9467,115 +9883,109 @@ def IMUConsistency(self): def Sprayer(self): """Test sprayer functionality.""" self.context_push() - ex = None - try: - rc_ch = 9 - pump_ch = 5 - spinner_ch = 6 - pump_ch_min = 1050 - pump_ch_trim = 1520 - pump_ch_max = 1950 - spinner_ch_min = 975 - spinner_ch_trim = 1510 - spinner_ch_max = 1975 - self.set_parameters({ - "SPRAY_ENABLE": 1, + rc_ch = 9 + pump_ch = 5 + spinner_ch = 6 + pump_ch_min = 1050 + pump_ch_trim = 1520 + pump_ch_max = 1950 + spinner_ch_min = 975 + spinner_ch_trim = 1510 + spinner_ch_max = 1975 - "SERVO%u_FUNCTION" % pump_ch: 22, - "SERVO%u_MIN" % pump_ch: pump_ch_min, - "SERVO%u_TRIM" % pump_ch: pump_ch_trim, - "SERVO%u_MAX" % pump_ch: pump_ch_max, + self.set_parameters({ + "SPRAY_ENABLE": 1, - "SERVO%u_FUNCTION" % spinner_ch: 23, - "SERVO%u_MIN" % spinner_ch: spinner_ch_min, - "SERVO%u_TRIM" % spinner_ch: spinner_ch_trim, - "SERVO%u_MAX" % spinner_ch: spinner_ch_max, + "SERVO%u_FUNCTION" % pump_ch: 22, + "SERVO%u_MIN" % pump_ch: pump_ch_min, + "SERVO%u_TRIM" % pump_ch: pump_ch_trim, + "SERVO%u_MAX" % pump_ch: pump_ch_max, - "SIM_SPR_ENABLE": 1, - "SIM_SPR_PUMP": pump_ch, - "SIM_SPR_SPIN": spinner_ch, + "SERVO%u_FUNCTION" % spinner_ch: 23, + "SERVO%u_MIN" % spinner_ch: spinner_ch_min, + "SERVO%u_TRIM" % spinner_ch: spinner_ch_trim, + "SERVO%u_MAX" % spinner_ch: spinner_ch_max, - "RC%u_OPTION" % rc_ch: 15, - "LOG_DISARMED": 1, - }) + "SIM_SPR_ENABLE": 1, + "SIM_SPR_PUMP": pump_ch, + "SIM_SPR_SPIN": spinner_ch, - self.reboot_sitl() + "RC%u_OPTION" % rc_ch: 15, + "LOG_DISARMED": 1, + }) - self.wait_ready_to_arm() - self.arm_vehicle() + self.reboot_sitl() + + self.wait_ready_to_arm() + self.arm_vehicle() + + self.progress("test bootup state - it's zero-output!") + self.wait_servo_channel_value(spinner_ch, 0) + self.wait_servo_channel_value(pump_ch, 0) + + self.progress("Enable sprayer") + self.set_rc(rc_ch, 2000) + + self.progress("Testing zero-speed state") + self.wait_servo_channel_value(spinner_ch, spinner_ch_min) + self.wait_servo_channel_value(pump_ch, pump_ch_min) + + self.progress("Testing turning it off") + self.set_rc(rc_ch, 1000) + self.wait_servo_channel_value(spinner_ch, spinner_ch_min) + self.wait_servo_channel_value(pump_ch, pump_ch_min) + + self.progress("Testing turning it back on") + self.set_rc(rc_ch, 2000) + self.wait_servo_channel_value(spinner_ch, spinner_ch_min) + self.wait_servo_channel_value(pump_ch, pump_ch_min) + + self.takeoff(30, mode='LOITER') + + self.progress("Testing speed-ramping") + self.set_rc(1, 1700) # start driving forward + + # this is somewhat empirical... + self.wait_servo_channel_value( + pump_ch, + 1458, + timeout=60, + comparator=lambda x, y : abs(x-y) < 5 + ) + + self.progress("Turning it off again") + self.set_rc(rc_ch, 1000) + self.wait_servo_channel_value(spinner_ch, spinner_ch_min) + self.wait_servo_channel_value(pump_ch, pump_ch_min) + + self.start_subtest("Checking mavlink commands") + self.progress("Starting Sprayer") + self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_SPRAYER, p1=1) + + self.progress("Testing speed-ramping") + self.wait_servo_channel_value( + pump_ch, + 1458, + timeout=60, + comparator=lambda x, y : abs(x-y) < 5 + ) + + self.start_subtest("Stopping Sprayer") + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SPRAYER, p1=0) + + self.wait_servo_channel_value(pump_ch, pump_ch_min) - self.progress("test bootup state - it's zero-output!") - self.wait_servo_channel_value(spinner_ch, 0) - self.wait_servo_channel_value(pump_ch, 0) - - self.progress("Enable sprayer") - self.set_rc(rc_ch, 2000) - - self.progress("Testing zero-speed state") - self.wait_servo_channel_value(spinner_ch, spinner_ch_min) - self.wait_servo_channel_value(pump_ch, pump_ch_min) - - self.progress("Testing turning it off") - self.set_rc(rc_ch, 1000) - self.wait_servo_channel_value(spinner_ch, spinner_ch_min) - self.wait_servo_channel_value(pump_ch, pump_ch_min) - - self.progress("Testing turning it back on") - self.set_rc(rc_ch, 2000) - self.wait_servo_channel_value(spinner_ch, spinner_ch_min) - self.wait_servo_channel_value(pump_ch, pump_ch_min) - - self.takeoff(30, mode='LOITER') - - self.progress("Testing speed-ramping") - self.set_rc(1, 1700) # start driving forward - - # this is somewhat empirical... - self.wait_servo_channel_value(pump_ch, 1458, timeout=60) - - self.progress("Turning it off again") - self.set_rc(rc_ch, 1000) - self.wait_servo_channel_value(spinner_ch, spinner_ch_min) - self.wait_servo_channel_value(pump_ch, pump_ch_min) - - self.start_subtest("Checking mavlink commands") - self.progress("Starting Sprayer") - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SPRAYER, - 1, # p1 - 0, # p2 - 0, # p3 - 0, # p4 - 0, # p5 - 0, # p6 - 0) # p7 - - self.progress("Testing speed-ramping") - self.wait_servo_channel_value(pump_ch, 1458, timeout=60, comparator=operator.gt) - self.start_subtest("Stopping Sprayer") - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SPRAYER, - 0, # p1 - 0, # p2 - 0, # p3 - 0, # p4 - 0, # p5 - 0, # p6 - 0) # p7 - self.wait_servo_channel_value(pump_ch, pump_ch_min) - - self.progress("Sprayer OK") - except Exception as e: - self.print_exception_caught(e) - ex = e self.context_pop() + self.disarm_vehicle(force=True) self.reboot_sitl() - if ex: - raise ex + + self.progress("Sprayer OK") def tests1a(self): '''return list of all tests''' - ret = super(AutoTestCopter, self).tests() # about 5 mins and ~20 initial tests from autotest/common.py + ret = super(AutoTestCopter, self).tests() # about 5 mins and ~20 initial tests from autotest/vehicle_test_suite.py ret.extend([ self.NavDelayTakeoffAbsTime, self.NavDelayAbsTime, @@ -9583,7 +9993,7 @@ def tests1a(self): self.GuidedSubModeChange, self.MAV_CMD_CONDITION_YAW, self.LoiterToAlt, - self.PayLoadPlaceMission, + self.PayloadPlaceMission, self.PrecisionLoiterCompanion, self.Landing, self.PrecisionLanding, @@ -9604,6 +10014,7 @@ def tests1b(self): self.BrakeMode, self.RecordThenPlayMission, self.ThrottleFailsafe, + self.ThrottleFailsafePassthrough, self.GCSFailsafe, self.CustomController, ]) @@ -9618,12 +10029,14 @@ def tests1c(self): self.StabilityPatch, self.OBSTACLE_DISTANCE_3D, self.AC_Avoidance_Proximity, + self.AC_Avoidance_Proximity_AVOID_ALT_MIN, self.AC_Avoidance_Fence, self.AC_Avoidance_Beacon, self.BaroWindCorrection, self.SetpointGlobalPos, self.ThrowDoubleDrop, self.SetpointGlobalVel, + self.SetpointBadVel, self.SplineTerrain, self.TakeoffCheck, ]) @@ -9673,6 +10086,8 @@ def tests1e(self): self.RTLSpeed, self.Mount, self.MountYawVehicleForMountROI, + self.MAV_CMD_DO_MOUNT_CONTROL, + self.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, self.Button, self.ShipTakeoff, self.RangeFinder, @@ -9697,6 +10112,11 @@ def tests1e(self): self.IE24, self.MAVLandedStateTakeoff, self.Weathervane, + self.MAV_CMD_AIRFRAME_CONFIGURATION, + self.MAV_CMD_NAV_LOITER_UNLIM, + self.MAV_CMD_NAV_RETURN_TO_LAUNCH, + self.MAV_CMD_NAV_VTOL_LAND, + self.clear_roi, ]) return ret @@ -9728,7 +10148,7 @@ def ScriptMountPOI(self): self.setup_servo_mount() self.reboot_sitl() self.set_rc(6, 1300) - self.install_example_script_context('mount-poi.lua') + self.install_applet_script_context('mount-poi.lua') self.reboot_sitl() self.wait_ready_to_arm() self.context_collect('STATUSTEXT') @@ -9749,10 +10169,436 @@ def AHRSTrimLand(self): self.reboot_sitl() self.wait_ready_to_arm() self.takeoff(alt_min=20, mode='LOITER') - self.land_and_disarm() + self.do_RTL() self.context_pop() self.reboot_sitl() + def turn_safety_x(self, value): + self.mav.mav.set_mode_send( + self.mav.target_system, + mavutil.mavlink.MAV_MODE_FLAG_DECODE_POSITION_SAFETY, + value) + + def turn_safety_off(self): + self.turn_safety_x(0) + + def turn_safety_on(self): + self.turn_safety_x(1) + + def SafetySwitch(self): + '''test safety switch behaviour''' + self.wait_ready_to_arm() + + self.turn_safety_on() + self.assert_prearm_failure("safety switch") + + self.turn_safety_off() + self.wait_ready_to_arm() + + self.takeoff(2, mode='LOITER') + self.turn_safety_on() + + self.wait_servo_channel_value(1, 0) + self.turn_safety_off() + + self.change_mode('LAND') + self.wait_disarmed() + + def GuidedYawRate(self): + '''ensuer guided yaw rate is not affected by rate of sewt-attitude messages''' + self.takeoff(30, mode='GUIDED') + rates = {} + for rate in 1, 10: + # command huge yaw rate for a while + tstart = self.get_sim_time() + interval = 1/rate + yawspeed_rads_sum = 0 + yawspeed_rads_count = 0 + last_sent = 0 + while True: + self.drain_mav() + tnow = self.get_sim_time_cached() + if tnow - last_sent > interval: + self.do_yaw_rate(60) # this is... unlikely + last_sent = tnow + if tnow - tstart < 5: # let it spin up to speed first + continue + yawspeed_rads_sum += self.mav.messages['ATTITUDE'].yawspeed + yawspeed_rads_count += 1 + if tnow - tstart > 15: # 10 seconds of measurements + break + yawspeed_degs = math.degrees(yawspeed_rads_sum / yawspeed_rads_count) + rates[rate] = yawspeed_degs + self.progress("Input rate %u hz: average yaw rate %f deg/s" % (rate, yawspeed_degs)) + + if rates[10] < rates[1] * 0.95: + raise NotAchievedException("Guided yaw rate slower for higher rate updates") + + self.do_RTL() + + def test_rplidar(self, sim_device_name, expected_distances): + '''plonks a Copter with a RPLidarA2 in the middle of a simulated field + of posts and checks that the measurements are what we expect.''' + self.set_parameters({ + "SERIAL5_PROTOCOL": 11, + "PRX1_TYPE": 5, + }) + self.customise_SITL_commandline([ + "--uartF=sim:%s:" % sim_device_name, + "--home", "51.8752066,14.6487840,0,0", # SITL has "posts" here + ]) + + self.wait_ready_to_arm() + + wanting_distances = copy.copy(expected_distances) + tstart = self.get_sim_time() + timeout = 60 + while True: + now = self.get_sim_time_cached() + if now - tstart > timeout: + raise NotAchievedException("Did not get all distances") + m = self.mav.recv_match(type="DISTANCE_SENSOR", + blocking=True, + timeout=1) + if m is None: + continue + self.progress("Got (%s)" % str(m)) + if m.orientation not in wanting_distances: + continue + if abs(m.current_distance - wanting_distances[m.orientation]) > 5: + self.progress("Wrong distance orient=%u want=%u got=%u" % + (m.orientation, + wanting_distances[m.orientation], + m.current_distance)) + continue + self.progress("Correct distance for orient %u (want=%u got=%u)" % + (m.orientation, + wanting_distances[m.orientation], + m.current_distance)) + del wanting_distances[m.orientation] + if len(wanting_distances.items()) == 0: + break + + def RPLidarA2(self): + '''test Raspberry Pi Lidar A2''' + expected_distances = { + mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 276, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 256, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 1130, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 1286, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 626, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 971, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 762, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 792, + } + + self.test_rplidar("rplidara2", expected_distances) + + def RPLidarA1(self): + '''test Raspberry Pi Lidar A1''' + return # we don't send distances when too long? + expected_distances = { + mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 276, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 256, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 800, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 800, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 626, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 800, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 762, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 792, + } + + self.test_rplidar("rplidara1", expected_distances) + + def BrakeZ(self): + '''check jerk limit correct in Brake mode''' + self.set_parameter('PSC_JERK_Z', 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) + + self.wait_for_local_velocity(vx=vx, vy=vy, vz_up=vz_up, timeout=10) + self.change_mode('BRAKE') + self.wait_for_local_velocity(vx=0, vy=0, vz_up=0, timeout=10) + self.land_and_disarm() + + def MISSION_START(self): + '''test mavlink command MAV_CMD_MISSION_START''' + self.upload_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 200), + (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0), + ]) + for command in self.run_cmd, self.run_cmd_int: + self.change_mode('LOITER') + self.set_current_waypoint(1) + self.wait_ready_to_arm() + self.arm_vehicle() + self.change_mode('AUTO') + command(mavutil.mavlink.MAV_CMD_MISSION_START) + self.wait_altitude(20, 1000, relative=True) + self.change_mode('RTL') + self.wait_disarmed() + + def DO_CHANGE_SPEED_in_guided(self): + '''test Copter DO_CHANGE_SPEED handling in guided mode''' + self.takeoff(20, mode='GUIDED') + + new_loc = self.mav.location() + new_loc_offset_n = 2000 + new_loc_offset_e = 0 + self.location_offset_ne(new_loc, new_loc_offset_n, new_loc_offset_e) + + second_loc_offset_n = -1000 + second_loc_offset_e = 0 + second_loc = self.mav.location() + self.location_offset_ne(second_loc, second_loc_offset_n, second_loc_offset_e) + + # for run_cmd we fly away from home + for (tloc, command) in (new_loc, self.run_cmd), (second_loc, self.run_cmd_int): + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_REPOSITION, + p1=-1, # "default" + p2=0, # flags; none supplied here + p3=0, # loiter radius for planes, zero ignored + p4=float("nan"), # nan means do whatever you want to do + p5=int(tloc.lat * 1e7), + p6=int(tloc.lng * 1e7), + p7=tloc.alt, + frame=mavutil.mavlink.MAV_FRAME_GLOBAL, + ) + for speed in [2, 10, 4]: + command( + mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, + p1=1, # groundspeed, + p2=speed, + p3=-1, # throttle, -1 is no-change + p4=0, # absolute value, not relative + ) + self.wait_groundspeed(speed-0.2, speed+0.2, minimum_duration=10, timeout=20) + + # we've made random changes to vehicle guided speeds above; + # reboot vehicle to reset those: + self.disarm_vehicle(force=True) + self.reboot_sitl() + + def _MAV_CMD_DO_FLIGHTTERMINATION(self, command): + self.set_parameters({ + "SYSID_MYGCS": self.mav.source_system, + "DISARM_DELAY": 0, + }) + self.wait_ready_to_arm() + self.arm_vehicle() + self.context_collect('STATUSTEXT') + command(mavutil.mavlink.MAV_CMD_DO_FLIGHTTERMINATION, p1=1) + self.wait_disarmed() + self.reboot_sitl() + + def MAV_CMD_DO_FLIGHTTERMINATION(self): + '''test MAV_CMD_DO_FLIGHTTERMINATION works on Copter''' + self._MAV_CMD_DO_FLIGHTTERMINATION(self.run_cmd) + self._MAV_CMD_DO_FLIGHTTERMINATION(self.run_cmd_int) + + def MAV_CMD_NAV_LOITER_UNLIM(self): + '''ensure MAV_CMD_NAV_LOITER_UNLIM via mavlink works''' + for command in self.run_cmd, self.run_cmd_int: + self.change_mode('STABILIZE') + command(mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM) + self.wait_mode('LOITER') + + def MAV_CMD_NAV_RETURN_TO_LAUNCH(self): + '''ensure MAV_CMD_NAV_RETURN_TO_LAUNCH via mavlink works''' + for command in self.run_cmd, self.run_cmd_int: + self.change_mode('STABILIZE') + command(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH) + self.wait_mode('RTL') + + def MAV_CMD_NAV_VTOL_LAND(self): + '''ensure MAV_CMD_NAV_LAND via mavlink works''' + for command in self.run_cmd, self.run_cmd_int: + self.change_mode('STABILIZE') + command(mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND) + self.wait_mode('LAND') + self.change_mode('STABILIZE') + command(mavutil.mavlink.MAV_CMD_NAV_LAND) + self.wait_mode('LAND') + + def clear_roi(self): + '''ensure three commands that clear ROI are equivalent''' + + self.upload_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 20), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 20), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 200, 0, 20), # directly North, i.e. 0 degrees + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 400, 0, 20), # directly North, i.e. 0 degrees + ]) + + self.set_parameter("AUTO_OPTIONS", 3) + self.change_mode('AUTO') + self.wait_ready_to_arm() + self.arm_vehicle() + home_loc = self.mav.location() + + cmd_ids = [ + mavutil.mavlink.MAV_CMD_DO_SET_ROI, + mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION, + mavutil.mavlink.MAV_CMD_DO_SET_ROI_NONE, + ] + for command in self.run_cmd, self.run_cmd_int: + for cmd_id in cmd_ids: + self.wait_waypoint(2, 2) + + # Set an ROI at the Home location, expect to point at Home + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION, p5=home_loc.lat, p6=home_loc.lng, p7=home_loc.alt) + self.wait_heading(180) + + # Clear the ROI, expect to point at the next Waypoint + self.progress("Clear ROI using %s(%d)" % (command.__name__, cmd_id)) + command(cmd_id) + self.wait_heading(0) + + self.wait_waypoint(4, 4) + self.set_current_waypoint_using_mav_cmd_do_set_mission_current(seq=2) + + self.land_and_disarm() + + def start_flying_simple_rehome_mission(self, items): + '''uploads items, changes mode to auto, waits ready to arm and arms + vehicle. If the first item it a takeoff you can expect the + vehicle to fly after this method returns + ''' + + self.upload_simple_relhome_mission(items) + + self.set_parameter("AUTO_OPTIONS", 3) + self.change_mode('AUTO') + self.wait_ready_to_arm() + + self.arm_vehicle() + + def _MAV_CMD_DO_LAND_START(self, run_cmd): + alt = 5 + self.start_flying_simple_rehome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, alt), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 200, 0, alt), + (mavutil.mavlink.MAV_CMD_NAV_LAND, 0, 0, 0), + (mavutil.mavlink.MAV_CMD_DO_LAND_START, 0, 0, alt), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 200, 2000, alt), + (mavutil.mavlink.MAV_CMD_NAV_LAND, 0, 0, 0), + ]) + + self.wait_current_waypoint(2) + run_cmd(mavutil.mavlink.MAV_CMD_DO_LAND_START) + self.wait_current_waypoint(5) + # we pretend to be in RTL mode while doing this: + self.wait_mode("AUTO_RTL") + self.do_RTL() + + def MAV_CMD_DO_LAND_START(self): + '''test handling of mavlink-received MAV_CMD_DO_LAND_START command''' + self._MAV_CMD_DO_LAND_START(self.run_cmd) + self.zero_throttle() + self._MAV_CMD_DO_LAND_START(self.run_cmd_int) + + def _MAV_CMD_SET_EKF_SOURCE_SET(self, run_cmd): + run_cmd( + mavutil.mavlink.MAV_CMD_SET_EKF_SOURCE_SET, + 17, + want_result=mavutil.mavlink.MAV_RESULT_DENIED, + ) + + self.change_mode('LOITER') + self.wait_ready_to_arm() + + run_cmd(mavutil.mavlink.MAV_CMD_SET_EKF_SOURCE_SET, 2) + + self.assert_prearm_failure('Need Position Estimate') + run_cmd(mavutil.mavlink.MAV_CMD_SET_EKF_SOURCE_SET, 1) + + self.wait_ready_to_arm() + + def MAV_CMD_SET_EKF_SOURCE_SET(self): + '''test setting of source sets using mavlink command''' + self._MAV_CMD_SET_EKF_SOURCE_SET(self.run_cmd) + self._MAV_CMD_SET_EKF_SOURCE_SET(self.run_cmd_int) + + def MAV_CMD_NAV_TAKEOFF(self): + '''test issuing takeoff command via mavlink''' + self.change_mode('GUIDED') + self.wait_ready_to_arm() + + self.arm_vehicle() + self.run_cmd(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, p7=5) + self.wait_altitude(4.5, 5.5, minimum_duration=5, relative=True) + self.change_mode('LAND') + self.wait_disarmed() + + self.start_subtest("Check NAV_TAKEOFF is above home location, not current location") + # reset home 20 metres above current location + current_alt_abs = self.get_altitude(relative=False) + + loc = self.mav.location() + + home_z_ofs = 20 + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_SET_HOME, + p5=loc.lat, + p6=loc.lng, + p7=current_alt_abs + home_z_ofs, + ) + + self.change_mode('GUIDED') + self.arm_vehicle() + takeoff_alt = 5 + self.run_cmd(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, p7=takeoff_alt) + self.wait_altitude( + current_alt_abs + home_z_ofs + takeoff_alt - 0.5, + current_alt_abs + home_z_ofs + takeoff_alt + 0.5, + minimum_duration=5, + relative=False, + ) + self.change_mode('LAND') + self.wait_disarmed() + + self.reboot_sitl() # unlock home position + + def MAV_CMD_NAV_TAKEOFF_command_int(self): + '''test issuing takeoff command via mavlink and command_int''' + self.change_mode('GUIDED') + self.wait_ready_to_arm() + + self.start_subtest("Check NAV_TAKEOFF is above home location, not current location") + # reset home 20 metres above current location + current_alt_abs = self.get_altitude(relative=False) + + loc = self.mav.location() + + home_z_ofs = 20 + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_SET_HOME, + p5=loc.lat, + p6=loc.lng, + p7=current_alt_abs + home_z_ofs, + ) + + self.change_mode('GUIDED') + self.arm_vehicle() + takeoff_alt = 5 + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, + p7=takeoff_alt, + frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + ) + self.wait_altitude( + current_alt_abs + home_z_ofs + takeoff_alt - 0.5, + current_alt_abs + home_z_ofs + takeoff_alt + 0.5, + minimum_duration=5, + relative=False, + ) + self.change_mode('LAND') + self.wait_disarmed() + + self.reboot_sitl() # unlock home position + def tests2b(self): # this block currently around 9.5mins here '''return list of all tests''' ret = ([ @@ -9760,6 +10606,7 @@ def tests2b(self): # this block currently around 9.5mins here Test(self.DynamicNotches, attempts=4), self.PositionWhenGPSIsZero, Test(self.DynamicRpmNotches, attempts=4), + self.PIDNotches, self.RefindGPS, Test(self.GyroFFT, attempts=1, speedup=8), Test(self.GyroFFTHarmonic, attempts=4, speedup=8), @@ -9773,6 +10620,7 @@ def tests2b(self): # this block currently around 9.5mins here self.AltEstimation, self.EKFSource, self.GSF, + self.GSF_reset, self.AP_Avoidance, self.SMART_RTL, self.RTL_TO_RALLY, @@ -9788,10 +10636,12 @@ def tests2b(self): # this block currently around 9.5mins here self.GroundEffectCompensation_touchDownExpected, self.GroundEffectCompensation_takeOffExpected, self.DO_CHANGE_SPEED, + self.MISSION_START, self.AUTO_LAND_TO_BRAKE, self.WPNAV_SPEED, self.WPNAV_SPEED_UP, self.WPNAV_SPEED_DN, + self.DO_WINCH, self.SensorErrorFlags, self.GPSForYaw, self.DefaultIntervalsFromFiles, @@ -9805,14 +10655,30 @@ def tests2b(self): # this block currently around 9.5mins here self.ThrottleGainBoost, self.ScriptMountPOI, self.FlyMissionTwice, + self.FlyMissionTwiceWithReset, + self.MissionIndexValidity, + self.InvalidJumpTags, self.IMUConsistency, self.AHRSTrimLand, + self.GuidedYawRate, + self.NoArmWithoutMissionItems, + self.DO_CHANGE_SPEED_in_guided, + self.RPLidarA1, + self.RPLidarA2, + self.SafetySwitch, + self.BrakeZ, + self.MAV_CMD_DO_FLIGHTTERMINATION, + self.MAV_CMD_DO_LAND_START, + self.MAV_CMD_SET_EKF_SOURCE_SET, + self.MAV_CMD_NAV_TAKEOFF, + self.MAV_CMD_NAV_TAKEOFF_command_int, ]) return ret def testcan(self): ret = ([ self.CANGPSCopterMission, + self.TestLogDownloadMAVProxyCAN, ]) return ret diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 6f426449ab849..fb78e2c186b53 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -8,20 +8,25 @@ import math import os import signal -import sys import time from pymavlink import quaternion from pymavlink import mavextra from pymavlink import mavutil -from common import AutoTest -from common import AutoTestTimeoutException -from common import NotAchievedException -from common import PreconditionFailedException -from common import WaitModeTimeout from pymavlink.rotmat import Vector3 + +import vehicle_test_suite + +from vehicle_test_suite import AutoTestTimeoutException +from vehicle_test_suite import NotAchievedException +from vehicle_test_suite import OldpymavlinkException +from vehicle_test_suite import PreconditionFailedException +from vehicle_test_suite import Test +from vehicle_test_suite import WaitModeTimeout + from pysim import vehicleinfo +from pysim import util import operator @@ -31,7 +36,7 @@ WIND = "0,180,0.2" # speed,direction,variance -class AutoTestPlane(AutoTest): +class AutoTestPlane(vehicle_test_suite.TestSuite): @staticmethod def get_not_armable_mode_list(): return [] @@ -90,9 +95,26 @@ def get_disarm_delay(self): def set_autodisarm_delay(self, delay): self.set_parameter("LAND_DISARMDELAY", delay) - def takeoff(self, alt=150, alt_max=None, relative=True): + def takeoff(self, alt=150, alt_max=None, relative=True, mode=None, timeout=None): """Takeoff to altitude.""" + if mode == "TAKEOFF": + return self.takeoff_in_TAKEOFF(alt=alt, relative=relative, timeout=timeout) + + return self.takeoff_in_FBWA(alt=alt, alt_max=alt_max, relative=relative, timeout=timeout) + + def takeoff_in_TAKEOFF(self, alt=150, relative=True, mode=None, alt_epsilon=2, timeout=None): + if relative is not True: + raise ValueError("Only relative alt supported ATM") + self.change_mode("TAKEOFF") + self.context_push() + self.set_parameter('TKOFF_ALT', alt) + self.wait_ready_to_arm() + self.arm_vehicle() + self.wait_altitude(alt-alt_epsilon, alt+alt_epsilon, relative=True, timeout=timeout) + self.context_pop() + + def takeoff_in_FBWA(self, alt=150, alt_max=None, relative=True, mode=None, timeout=30): if alt_max is None: alt_max = alt + 30 @@ -125,7 +147,7 @@ def takeoff(self, alt=150, alt_max=None, relative=True): }) # gain a bit of altitude - self.wait_altitude(alt, alt_max, timeout=30, relative=relative) + self.wait_altitude(alt, alt_max, timeout=timeout, relative=relative) # level off self.set_rc(2, 1500) @@ -168,16 +190,17 @@ def NeedEKFToArm(self): self.context_collect("STATUSTEXT") tstart = self.get_sim_time() success = False - while not success: - if self.get_sim_time_cached() - tstart > 60: - raise NotAchievedException("Did not get correct failure reason") - self.run_cmd_run_prearms() - try: - self.wait_statustext(".*AHRS: not using configured AHRS type.*", timeout=1, check_context=True, regex=True) - success = True - continue - except AutoTestTimeoutException: - pass + for run_cmd in self.run_cmd, self.run_cmd_int: + while not success: + if self.get_sim_time_cached() - tstart > 60: + raise NotAchievedException("Did not get correct failure reason") + run_cmd(mavutil.mavlink.MAV_CMD_RUN_PREARM_CHECKS) + try: + self.wait_statustext(".*AHRS: not using configured AHRS type.*", timeout=1, check_context=True, regex=True) + success = True + continue + except AutoTestTimeoutException: + pass self.set_parameter("SIM_GPS_DISABLE", 0) self.wait_ready_to_arm() @@ -318,78 +341,83 @@ def inside_loop(self, count=1): def set_attitude_target(self, tolerance=10): """Test setting of attitude target in guided mode.""" self.change_mode("GUIDED") -# self.set_parameter("STALL_PREVENTION", 0) - state_roll_over = "roll-over" - state_stabilize_roll = "stabilize-roll" - state_hold = "hold" - state_roll_back = "roll-back" - state_done = "done" - - tstart = self.get_sim_time() + steps = [{"name": "roll-over", "roll": 60, "pitch": 0, "yaw": 0, "throttle": 0, "type_mask": 0b10000001}, + {"name": "roll-back", "roll": 0, "pitch": 0, "yaw": 0, "throttle": 0, "type_mask": 0b10000001}, + {"name": "pitch-up+throttle", "roll": 0, "pitch": 20, "yaw": 0, "throttle": 1, "type_mask": 0b11000010}, + {"name": "pitch-back", "roll": 0, "pitch": 0, "yaw": 0, "throttle": 0, "type_mask": 0b10000010}] + state_wait = "wait" + state_hold = "hold" try: - state = state_roll_over - while state != state_done: - - m = self.mav.recv_match(type='ATTITUDE', - blocking=True, - timeout=0.1) - now = self.get_sim_time_cached() - if now - tstart > 20: - raise AutoTestTimeoutException("Manuevers not completed") - if m is None: - continue - - r = math.degrees(m.roll) - if state == state_roll_over: - target_roll_degrees = 60 - if abs(r - target_roll_degrees) < tolerance: - state = state_stabilize_roll - stabilize_start = now - elif state == state_stabilize_roll: - # just give it a little time to sort it self out - if now - stabilize_start > 2: - state = state_hold - hold_start = now - elif state == state_hold: - target_roll_degrees = 60 - if now - hold_start > tolerance: - state = state_roll_back - if abs(r - target_roll_degrees) > tolerance: - raise NotAchievedException("Failed to hold attitude") - elif state == state_roll_back: - target_roll_degrees = 0 - if abs(r - target_roll_degrees) < tolerance: - state = state_done - else: - raise ValueError("Unknown state %s" % str(state)) - - m_nav = self.mav.messages['NAV_CONTROLLER_OUTPUT'] - self.progress("%s Roll: %f desired=%f set=%f" % - (state, r, m_nav.nav_roll, target_roll_degrees)) - - time_boot_millis = 0 # FIXME - target_system = 1 # FIXME - target_component = 1 # FIXME - type_mask = 0b10000001 ^ 0xFF # FIXME - # attitude in radians: - q = quaternion.Quaternion([math.radians(target_roll_degrees), - 0, - 0]) - roll_rate_radians = 0.5 - pitch_rate_radians = 0 - yaw_rate_radians = 0 - thrust = 1.0 - self.mav.mav.set_attitude_target_send(time_boot_millis, - target_system, - target_component, - type_mask, - q, - roll_rate_radians, - pitch_rate_radians, - yaw_rate_radians, - thrust) + for step in steps: + step_start = self.get_sim_time_cached() + state = state_wait + state_start = self.get_sim_time_cached() + while True: + m = self.mav.recv_match(type='ATTITUDE', + blocking=True, + timeout=0.1) + now = self.get_sim_time_cached() + if now - step_start > 30: + raise AutoTestTimeoutException("Manuevers not completed") + if m is None: + continue + + angle_error = 0 + if (step["type_mask"] & 0b00000001) or (step["type_mask"] == 0b10000000): + angle_error += abs(math.degrees(m.roll) - step["roll"]) + + if (step["type_mask"] & 0b00000010) or (step["type_mask"] == 0b10000000): + angle_error += abs(math.degrees(m.pitch) - step["pitch"]) + + if (step["type_mask"] & 0b00000100) or (step["type_mask"] == 0b10000000): + # Strictly we should angle wrap, by plane doesn't support yaw correctly anyway so its not tested here + angle_error += abs(math.degrees(m.yaw) - step["yaw"]) + + # Note were not checking throttle, however the SITL plane needs full throttle to meet the + # target pitch attitude, Pitch test will fail without throttle override + + if state == state_wait: + # Reduced tolerance for initial trigger + if angle_error < (tolerance * 0.25): + state = state_hold + state_start = now + + # Allow 10 seconds to reach attitude + if (now - state_start) > 10: + raise NotAchievedException(step["name"] + ": Failed to get to set attitude") + + elif state == state_hold: + # Give 2 seconds to stabilize + if (now - state_start) > 2 and not (angle_error < tolerance): + raise NotAchievedException(step["name"] + ": Failed to hold set attitude") + + # Hold for 10 seconds + if (now - state_start) > 12: + # move onto next step + self.progress("%s Done" % (step["name"])) + break + + self.progress("%s %s error: %f" % (step["name"], state, angle_error)) + + time_boot_millis = 0 # FIXME + target_system = 1 # FIXME + target_component = 1 # FIXME + type_mask = step["type_mask"] ^ 0xFF # FIXME + # attitude in radians: + q = quaternion.Quaternion([math.radians(step["roll"]), + math.radians(step["pitch"]), + math.radians(step["yaw"])]) + self.mav.mav.set_attitude_target_send(time_boot_millis, + target_system, + target_component, + type_mask, + q, + 0, # roll rate, not used in AP + 0, # pitch rate, not used in AP + 0, # yaw rate, not used in AP + step["throttle"]) except Exception as e: self.change_mode('FBWA') self.set_rc(3, 1700) @@ -559,19 +587,156 @@ def DO_REPOSITION(self): self.location_offset_ne(loc, 500, 500) new_alt = 100 + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_REPOSITION, + p5=int(loc.lat * 1e7), + p6=int(loc.lng * 1e7), + p7=new_alt, # alt + frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + ) + self.wait_altitude(new_alt-10, new_alt, timeout=30, relative=True) + + self.install_terrain_handlers_context() + + self.location_offset_ne(loc, 500, 500) + terrain_height_wanted = 150 self.run_cmd_int( mavutil.mavlink.MAV_CMD_DO_REPOSITION, 0, 0, 0, 0, - int(loc.lat * 1e7), - int(loc.lng * 1e7), - new_alt, # alt - frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + int(loc.lat*1e7), + int(loc.lng*1e7), + terrain_height_wanted, # alt + frame=mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, + ) + + # move to specific terrain-relative altitude and hold for seconds + tstart = self.get_sim_time_cached() + achieve_start = None + tr = None + while True: + if self.get_sim_time_cached() - tstart > 120: + raise NotAchievedException("Did not move to correct terrain alt") + + m = self.mav.recv_match(type='TERRAIN_REPORT', + blocking=True, + timeout=1) + tr = m + terrain_height_achieved = m.current_height + self.progress("terrain_alt=%f want=%f" % + (terrain_height_achieved, terrain_height_wanted)) + if m is None: + continue + if abs(terrain_height_wanted - terrain_height_achieved) > 5: + if achieve_start is not None: + self.progress("Achieve stop") + achieve_start = None + elif achieve_start is None: + self.progress("Achieve start") + achieve_start = self.get_sim_time_cached() + if achieve_start is not None: + if self.get_sim_time_cached() - achieve_start > 10: + break + m = self.mav.recv_match(type='GLOBAL_POSITION_INT', + blocking=True, + timeout=1) + self.progress("TR: %s" % tr) + self.progress("GPI: %s" % m) + min_delta = 4 + delta = abs(m.relative_alt/1000.0 - tr.current_height) + if abs(delta < min_delta): + raise NotAchievedException("Expected altitude delta (want=%f got=%f)" % + (min_delta, delta)) + + self.fly_home_land_and_disarm(timeout=180) + + def ExternalPositionEstimate(self): + '''Test mavlink EXTERNAL_POSITION_ESTIMATE command''' + if not hasattr(mavutil.mavlink, 'MAV_CMD_EXTERNAL_POSITION_ESTIMATE'): + raise OldpymavlinkException("pymavlink too old; upgrade pymavlink to get MAV_CMD_EXTERNAL_POSITION_ESTIMATE") # noqa + self.change_mode("TAKEOFF") + self.wait_ready_to_arm() + self.arm_vehicle() + self.wait_altitude(48, 52, relative=True) + + loc = self.mav.location() + self.location_offset_ne(loc, 2000, 2000) + + # setting external position fail while we have GPS lock + self.progress("set new position with GPS") + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_EXTERNAL_POSITION_ESTIMATE, + p1=self.get_sim_time()-1, # transmit time + p2=0.5, # processing delay + p3=50, # accuracy + p5=int(loc.lat * 1e7), + p6=int(loc.lng * 1e7), + p7=float("NaN"), # alt + frame=mavutil.mavlink.MAV_FRAME_GLOBAL, + want_result=mavutil.mavlink.MAV_RESULT_FAILED, ) - self.wait_altitude(new_alt-10, new_alt, timeout=30, relative=True) + self.progress("disable the GPS") + self.run_auxfunc( + 65, + 2, + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED + ) + + # fly for a bit to get into non-aiding state + self.progress("waiting 20 seconds") + tstart = self.get_sim_time() + while self.get_sim_time() < tstart + 20: + self.wait_heartbeat() + + self.progress("getting base position") + gpi = self.mav.recv_match( + type='GLOBAL_POSITION_INT', + blocking=True, + timeout=5 + ) + loc = mavutil.location(gpi.lat*1e-7, gpi.lon*1e-7, 0, 0) + + self.progress("set new position with no GPS") + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_EXTERNAL_POSITION_ESTIMATE, + p1=self.get_sim_time()-1, # transmit time + p2=0.5, # processing delay + p3=50, # accuracy + p5=gpi.lat+1, + p6=gpi.lon+1, + p7=float("NaN"), # alt + frame=mavutil.mavlink.MAV_FRAME_GLOBAL, + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED + ) + + self.progress("waiting 3 seconds") + tstart = self.get_sim_time() + while self.get_sim_time() < tstart + 3: + self.wait_heartbeat() + + gpi2 = self.mav.recv_match( + type='GLOBAL_POSITION_INT', + blocking=True, + timeout=5 + ) + loc2 = mavutil.location(gpi2.lat*1e-7, gpi2.lon*1e-7, 0, 0) + dist = self.get_distance(loc, loc2) + + self.progress("dist is %.1f" % dist) + if dist > 200: + raise NotAchievedException("Position error dist=%.1f" % dist) + + self.progress("re-enable the GPS") + self.run_auxfunc( + 65, + 0, + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED + ) + + self.progress("flying home") self.fly_home_land_and_disarm() def DeepStall(self): @@ -625,18 +790,18 @@ def fly_deepstall_relative(self): self.change_mode("AUTO") self.wait_ready_to_arm() self.arm_vehicle() - self.progress("Waiting for deepstall messages") - - # note that the following two don't necessarily happen in this - # order, but at very high speedups we may miss the elevator - # PWM if we first look for the text (due to the get_sim_time() - # in wait_servo_channel_value) - self.context_collect('STATUSTEXT') + self.wait_current_waypoint(4) # assume elevator is on channel 2: self.wait_servo_channel_value(2, deepstall_elevator_pwm, timeout=240) - self.wait_text("Deepstall: Entry: ", check_context=True) + self.progress("Waiting for stage DEEPSTALL_STAGE_LAND") + self.assert_receive_message( + 'DEEPSTALL', + condition='DEEPSTALL.stage==6', + timeout=240, + ) + self.progress("Reached stage DEEPSTALL_STAGE_LAND") self.disarm_wait(timeout=120) self.set_current_waypoint(0, check_afterwards=False) @@ -674,18 +839,36 @@ def SmartBattery(self): if not self.current_onboard_log_contains_message("BCL2"): raise NotAchievedException("Expected BCL2 message") - def DO_CHANGE_SPEED(self): - '''Test DO_CHANGE_SPEED command/item''' + def context_push_do_change_speed(self): # the following lines ensure we revert these parameter values # - DO_CHANGE_AIRSPEED is a permanent vehicle change! + self.context_push() self.set_parameters({ "TRIM_ARSPD_CM": self.get_parameter("TRIM_ARSPD_CM"), "MIN_GNDSPD_CM": self.get_parameter("MIN_GNDSPD_CM"), + "TRIM_THROTTLE": self.get_parameter("TRIM_THROTTLE"), + }) + + def DO_CHANGE_SPEED(self): + '''Test DO_CHANGE_SPEED command/item''' + self.set_parameters({ "RTL_AUTOLAND": 1, }) - self.DO_CHANGE_SPEED_mavlink() + self.context_push_do_change_speed() + self.DO_CHANGE_SPEED_mavlink_long() + self.context_pop() + + self.set_current_waypoint(1) + self.zero_throttle() + + self.context_push_do_change_speed() + self.DO_CHANGE_SPEED_mavlink_int() + self.context_pop() + + self.context_push_do_change_speed() self.DO_CHANGE_SPEED_mission() + self.context_pop() def DO_CHANGE_SPEED_mission(self): '''test DO_CHANGE_SPEED as a mission item''' @@ -715,23 +898,25 @@ def DO_CHANGE_SPEED_mission(self): self.fly_home_land_and_disarm() - def DO_CHANGE_SPEED_mavlink(self): + def DO_CHANGE_SPEED_mavlink_int(self): + self.DO_CHANGE_SPEED_mavlink(self.run_cmd_int) + + def DO_CHANGE_SPEED_mavlink_long(self): + self.DO_CHANGE_SPEED_mavlink(self.run_cmd) + + def DO_CHANGE_SPEED_mavlink(self, run_cmd_method): '''test DO_CHANGE_SPEED as a mavlink command''' self.progress("Takeoff") - self.takeoff(alt=100) + self.takeoff(alt=100, mode="TAKEOFF", timeout=120) self.set_rc(3, 1500) # ensure we know what the airspeed is: self.progress("Entering guided and flying somewhere constant") self.change_mode("GUIDED") self.run_cmd_int( mavutil.mavlink.MAV_CMD_DO_REPOSITION, - 0, - 0, - 0, - 0, - 12345, # lat* 1e7 - 12345, # lon* 1e7 - 100 # alt + p5=12345, # lat* 1e7 + p6=12345, # lon* 1e7 + p7=100 # alt ) self.delay_sim_time(10) self.progress("Ensuring initial speed is known and relatively constant") @@ -739,47 +924,43 @@ def DO_CHANGE_SPEED_mavlink(self): timeout = 15 self.wait_airspeed(initial_speed-1, initial_speed+1, minimum_duration=5, timeout=timeout) - self.progress("Setting groundspeed") - new_target_groundspeed = initial_speed + 5 - self.run_cmd( - mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, - 1, # groundspeed - new_target_groundspeed, - -1, # throttle / no change - 0, # absolute values - 0, - 0, - 0) - self.wait_groundspeed(new_target_groundspeed-0.5, new_target_groundspeed+0.5, timeout=40) - self.progress("Adding some wind, ensuring groundspeed holds") - self.set_parameter("SIM_WIND_SPD", 5) - self.delay_sim_time(5) - self.wait_groundspeed(new_target_groundspeed-0.5, new_target_groundspeed+0.5, timeout=40) - self.set_parameter("SIM_WIND_SPD", 0) + self.start_subtest("Setting groundspeed") + for new_target_groundspeed in initial_speed + 5, initial_speed + 2: + run_cmd_method( + mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, + p1=1, # groundspeed + p2=new_target_groundspeed, + p3=-1, # throttle / no change + p4=0, # absolute values + ) + self.wait_groundspeed(new_target_groundspeed-2, new_target_groundspeed+2, timeout=80, minimum_duration=5) + self.progress("Adding some wind, ensuring groundspeed holds") + self.set_parameter("SIM_WIND_SPD", 5) + self.delay_sim_time(5) + self.wait_groundspeed(new_target_groundspeed-2, new_target_groundspeed+2, timeout=40, minimum_duration=5) + self.set_parameter("SIM_WIND_SPD", 0) # clear target groundspeed - self.run_cmd( + run_cmd_method( mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, - 1, # groundspeed - 0, - -1, # throttle / no change - 0, # absolute values - 0, - 0, - 0) + p1=1, # groundspeed + p2=0, + p3=-1, # throttle / no change + p4=0, # absolute values + ) - self.progress("Setting airspeed") - new_target_airspeed = initial_speed + 5 - self.run_cmd( - mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, - 0, # airspeed - new_target_airspeed, - -1, # throttle / no change - 0, # absolute values - 0, - 0, - 0) - self.wait_airspeed(new_target_airspeed-0.5, new_target_airspeed+0.5) + self.start_subtest("Setting airspeed") + for new_target_airspeed in initial_speed - 5, initial_speed + 5: + run_cmd_method( + mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, + p1=0, # airspeed + p2=new_target_airspeed, + p3=-1, # throttle / no change + p4=0, # absolute values + ) + self.wait_airspeed(new_target_airspeed-2, new_target_airspeed+2, minimum_duration=5) + + self.context_push() self.progress("Adding some wind, hoping groundspeed increases/decreases") self.set_parameters({ "SIM_WIND_SPD": 7, @@ -797,6 +978,39 @@ def DO_CHANGE_SPEED_mavlink(self): self.progress("groundspeed and airspeed should be different (have=%f want=%f)" % (delta, want_delta)) if delta > want_delta: break + self.context_pop() + + # cancel minimum groundspeed: + run_cmd_method( + mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, + p1=0, # groundspeed + p2=-2, # return to default + p3=0, # throttle / no change + p4=0, # absolute values + ) + # cancel airspeed: + run_cmd_method( + mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, + p1=1, # airspeed + p2=-2, # return to default + p3=0, # throttle / no change + p4=0, # absolute values + ) + + self.start_subtest("Setting throttle") + self.set_parameter('ARSPD_USE', 0) # setting throttle only effective without airspeed + for (set_throttle, expected_throttle) in (97, 79), (60, 51), (95, 77): + run_cmd_method( + mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, + p1=3, # throttle + p2=0, + p3=set_throttle, # throttle / no change + p4=0, # absolute values + ) + self.wait_message_field_values('VFR_HUD', { + "throttle": expected_throttle, + }, minimum_duration=5, epsilon=2) + self.fly_home_land_and_disarm(timeout=240) def fly_home_land_and_disarm(self, timeout=120): @@ -994,7 +1208,7 @@ def ThrottleFailsafe(self): self.context_collect("HEARTBEAT") self.set_parameter("SIM_RC_FAIL", 2) # throttle-to-950 self.wait_mode('RTL') # long failsafe - if (not self.get_mode_from_mode_mapping("CIRCLE") in + if (self.get_mode_from_mode_mapping("CIRCLE") not in [x.custom_mode for x in self.context_stop_collecting("HEARTBEAT")]): raise NotAchievedException("Did not go via circle mode") self.progress("Ensure we've had our throttle squashed to 950") @@ -1032,7 +1246,7 @@ def ThrottleFailsafe(self): self.context_collect("HEARTBEAT") self.set_parameter("SIM_RC_FAIL", 1) # no-pulses self.wait_mode('RTL') # long failsafe - if (not self.get_mode_from_mode_mapping("CIRCLE") in + if (self.get_mode_from_mode_mapping("CIRCLE") not in [x.custom_mode for x in self.context_stop_collecting("HEARTBEAT")]): raise NotAchievedException("Did not go via circle mode") self.do_timesync_roundtrip() @@ -1169,29 +1383,28 @@ def GCSFailsafe(self): '''Ensure Long-Failsafe works on GCS loss''' self.start_subtest("Test Failsafe: RTL") self.load_sample_mission() - self.set_parameter("RTL_AUTOLAND", 1) - self.change_mode("AUTO") - self.takeoff() self.set_parameters({ "FS_GCS_ENABL": 1, "FS_LONG_ACTN": 1, + "RTL_AUTOLAND": 1, + "SYSID_MYGCS": self.mav.source_system, }) + self.takeoff() + self.change_mode('LOITER') self.progress("Disconnecting GCS") self.set_heartbeat_rate(0) - self.wait_mode("RTL", timeout=5) + self.wait_mode("RTL", timeout=10) self.set_heartbeat_rate(self.speedup) self.end_subtest("Completed RTL Failsafe test") self.start_subtest("Test Failsafe: FBWA Glide") self.set_parameters({ - "RTL_AUTOLAND": 1, "FS_LONG_ACTN": 2, }) - self.change_mode("AUTO") - self.takeoff() + self.change_mode('AUTO') self.progress("Disconnecting GCS") self.set_heartbeat_rate(0) - self.wait_mode("FBWA", timeout=5) + self.wait_mode("FBWA", timeout=10) self.set_heartbeat_rate(self.speedup) self.end_subtest("Completed FBWA Failsafe test") @@ -1296,6 +1509,31 @@ def wait_circling_point_with_radius(self, loc, want_radius, epsilon=5.0, min_cir on_radius_start_heading = None circle_time_start = 0 + def MODE_SWITCH_RESET(self): + '''test the MODE_SWITCH_RESET auxiliary function''' + self.set_parameters({ + "RC9_OPTION": 96, + }) + + self.progress("Using RC to change modes") + self.set_rc(8, 1500) + self.wait_mode('FBWA') + + self.progress("Killing RC to engage RC failsafe") + self.set_parameter('SIM_RC_FAIL', 1) + self.wait_mode('RTL') + + self.progress("Reinstating RC") + self.set_parameter('SIM_RC_FAIL', 0) + + self.progress("Ensuring we don't automatically revert mode") + self.delay_sim_time(2) + self.assert_mode_is('RTL') + + self.progress("Ensuring MODE_SWITCH_RESET switch resets to pre-failsafe mode") + self.set_rc(9, 2000) + self.wait_mode('FBWA') + def FenceStatic(self): '''Test Basic Fence Functionality''' ex = None @@ -1600,6 +1838,115 @@ def FenceRetRally(self): self.do_fence_disable() # Disable fence so we can land self.fly_home_land_and_disarm() # Pack it up, we're going home. + def TerrainRally(self): + """ Tests terrain follow with a rally point """ + self.context_push() + self.install_terrain_handlers_context() + + def terrain_following_above_80m(mav, m): + if m.get_type() == 'TERRAIN_REPORT': + if m.current_height < 50: + raise NotAchievedException( + "TERRAIN_REPORT.current_height below 50m %fm" % m.current_height) + if m.get_type() == 'VFR_HUD': + if m.groundspeed < 2: + raise NotAchievedException("hit ground") + + def terrain_wait_path(loc1, loc2, steps): + '''wait till we have terrain for N steps from loc1 to loc2''' + tstart = self.get_sim_time_cached() + self.progress("Waiting for terrain data") + while True: + now = self.get_sim_time_cached() + if now - tstart > 60: + raise NotAchievedException("Did not get correct required terrain") + for i in range(steps): + lat = loc1.lat + i * (loc2.lat-loc1.lat)/steps + lon = loc1.lng + i * (loc2.lng-loc1.lng)/steps + self.mav.mav.terrain_check_send(int(lat*1.0e7), int(lon*1.0e7)) + + report = self.assert_receive_message('TERRAIN_REPORT', timeout=60) + self.progress("Terrain pending=%u" % report.pending) + if report.pending == 0: + break + self.progress("Got required terrain") + + self.wait_ready_to_arm() + self.homeloc = self.mav.location() + + guided_loc = mavutil.location(-35.39723762, 149.07284612, 99.0, 0) + rally_loc = mavutil.location(-35.3654952000, 149.1558698000, 100, 0) + + terrain_wait_path(self.homeloc, rally_loc, 10) + + # set a rally point to the west of home + self.upload_rally_points_from_locations([rally_loc]) + + self.set_parameter("TKOFF_ALT", 100) + self.change_mode("TAKEOFF") + self.wait_ready_to_arm() + self.arm_vehicle() + self.set_parameter("TERRAIN_FOLLOW", 1) + self.wait_altitude(90, 120, timeout=30, relative=True) + self.progress("Done takeoff") + + self.install_message_hook_context(terrain_following_above_80m) + + self.change_mode("GUIDED") + self.do_reposition(guided_loc, frame=mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT) + self.progress("Flying to guided location") + self.wait_location(guided_loc, + accuracy=200, + target_altitude=None, + timeout=600) + + self.progress("Reached guided location") + self.set_parameter("RALLY_LIMIT_KM", 50) + self.change_mode("RTL") + self.progress("Flying to rally point") + self.wait_location(rally_loc, + accuracy=200, + target_altitude=None, + timeout=600) + self.progress("Reached rally point with TERRAIN_FOLLOW") + + # Fly back to guided location + self.change_mode("GUIDED") + self.do_reposition(guided_loc, frame=mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT) + self.progress("Flying to back to guided location") + + # Disable terrain following and re-load rally point with relative to terrain altitude + self.set_parameter("TERRAIN_FOLLOW", 0) + + rally_item = [self.create_MISSION_ITEM_INT( + mavutil.mavlink.MAV_CMD_NAV_RALLY_POINT, + x=int(rally_loc.lat*1e7), + y=int(rally_loc.lng*1e7), + z=rally_loc.alt, + frame=mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, + mission_type=mavutil.mavlink.MAV_MISSION_TYPE_RALLY + )] + self.correct_wp_seq_numbers(rally_item) + self.check_rally_upload_download(rally_item) + + # Once back at guided location re-trigger RTL + self.wait_location(guided_loc, + accuracy=200, + target_altitude=None, + timeout=600) + + self.change_mode("RTL") + self.progress("Flying to rally point") + self.wait_location(rally_loc, + accuracy=200, + target_altitude=None, + timeout=600) + self.progress("Reached rally point with terrain alt frame") + + self.context_pop() + self.disarm_vehicle(force=True) + self.reboot_sitl() + def Parachute(self): '''Test Parachute''' self.set_rc(9, 1000) @@ -1844,6 +2191,7 @@ def AIRSPEED_AUTOCAL(self): self.fly_home_land_and_disarm() def deadreckoning_main(self, disable_airspeed_sensor=False): + self.reboot_sitl() self.wait_ready_to_arm() self.gpi = None self.simstate = None @@ -1887,13 +2235,11 @@ def validate_global_position_int_against_simstate(mav, m): self.location_offset_ne(loc, 500, 500) self.run_cmd_int( mavutil.mavlink.MAV_CMD_DO_REPOSITION, - 0, - mavutil.mavlink.MAV_DO_REPOSITION_FLAGS_CHANGE_MODE, - 0, - 0, - int(loc.lat * 1e7), - int(loc.lng * 1e7), - 100, # alt + p1=0, + p2=mavutil.mavlink.MAV_DO_REPOSITION_FLAGS_CHANGE_MODE, + p5=int(loc.lat * 1e7), + p6=int(loc.lng * 1e7), + p7=100, # alt frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, ) self.wait_location(loc, accuracy=100) @@ -2414,11 +2760,6 @@ def Soaring(self): self.load_mission('CMAC-soar.txt', strict=False) - self.set_current_waypoint(1) - self.change_mode('AUTO') - self.wait_ready_to_arm() - self.arm_vehicle() - # Enable thermalling RC rc_chan = 0 for i in range(8): @@ -2432,15 +2773,22 @@ def Soaring(self): self.set_rc_from_map({ rc_chan: 1900, - 3: 1500, # Use trim airspeed. }) + self.set_parameters({ + "SOAR_VSPEED": 0.55, + "SOAR_MIN_THML_S": 25, + }) + + self.set_current_waypoint(1) + self.change_mode('AUTO') + self.wait_ready_to_arm() + self.arm_vehicle() + # Wait to detect thermal self.progress("Waiting for thermal") self.wait_mode('THERMAL', timeout=600) - self.set_parameter("SOAR_VSPEED", 0.6) - # Wait to climb to SOAR_ALT_MAX self.progress("Waiting for climb to max altitude") alt_max = self.get_parameter('SOAR_ALT_MAX') @@ -2825,9 +3173,11 @@ def fly_external_AHRS(self, sim, eahrs_type, mission): self.reboot_sitl() self.delay_sim_time(5) self.progress("Running accelcal") - self.run_cmd(mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, - 0, 0, 0, 0, 4, 0, 0, - timeout=5) + self.run_cmd( + mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, + p5=4, + timeout=5, + ) self.wait_ready_to_arm() self.arm_vehicle() @@ -2913,7 +3263,7 @@ def WindEstimates(self): self.wait_and_maintain_wind_estimate( 5, 45, speed_tolerance=1, - timeout=20 + timeout=30 ) self.fly_home_land_and_disarm() @@ -2921,9 +3271,13 @@ def VectorNavEAHRS(self): '''Test VectorNav EAHRS support''' self.fly_external_AHRS("VectorNav", 1, "ap1.txt") - def LordEAHRS(self): - '''Test LORD Microstrain EAHRS support''' - self.fly_external_AHRS("LORD", 2, "ap1.txt") + def MicroStrainEAHRS5(self): + '''Test MicroStrain EAHRS series 5 support''' + self.fly_external_AHRS("MicroStrain5", 2, "ap1.txt") + + def MicroStrainEAHRS7(self): + '''Test MicroStrain EAHRS series 7 support''' + self.fly_external_AHRS("MicroStrain7", 7, "ap1.txt") def get_accelvec(self, m): return Vector3(m.xacc, m.yacc, m.zacc) * 0.001 * 9.81 @@ -2986,13 +3340,17 @@ def IMUTempCal(self): self.set_parameter("SIM_IMUT_FIXED", 12) self.progress("Running accel cal") - self.run_cmd(mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, - 0, 0, 0, 0, 4, 0, 0, - timeout=5) + self.run_cmd( + mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, + p5=4, + timeout=5, + ) self.progress("Running gyro cal") - self.run_cmd(mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, - 0, 0, 0, 0, 1, 0, 0, - timeout=5) + self.run_cmd( + mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, + p5=1, + timeout=5, + ) self.set_parameters({ "SIM_IMUT_FIXED": 0, "INS_TCAL1_ENABLE": 2, @@ -3232,13 +3590,9 @@ def fail_speed(): loc = self.mav.location() self.run_cmd_int( mavutil.mavlink.MAV_CMD_DO_REPOSITION, - 0, - 0, - 0, - 0, - int(loc.lat * 1e7), - int(loc.lng * 1e7), - 50 # alt + p5=int(loc.lat * 1e7), + p6=int(loc.lng * 1e7), + p7=50, # alt ) self.delay_sim_time(5) # create an airspeed sensor error by freezing to the @@ -3249,13 +3603,10 @@ def fail_speed(): self.set_parameter("SIM_ARSPD_FAIL", m.airspeed) self.run_cmd( mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, - 0, # airspeed - 30, - -1, # throttle / no change - 0, # absolute values - 0, - 0, - 0 + p1=0, # airspeed + p2=30, + p3=-1, # throttle / no change + p4=0, # absolute values ) self.wait_statustext(text="EKF3 lane switch", timeout=30, the_function=fail_speed, check_context=True) if self.lane_switches != [1, 0, 1, 0, 1]: @@ -3557,29 +3908,36 @@ def attempt_fence_breached_disable(start_mode, end_mode, expected_mode, action): attempt_fence_breached_disable(start_mode="FBWA", end_mode="FBWA", expected_mode="GUIDED", action=6) attempt_fence_breached_disable(start_mode="FBWA", end_mode="FBWA", expected_mode="GUIDED", action=7) - def MAV_DO_AUX_FUNCTION(self): + def _MAV_CMD_DO_AUX_FUNCTION(self, run_cmd): '''Test triggering Auxiliary Functions via mavlink''' self.context_collect('STATUSTEXT') - self.run_auxfunc(64, 2) # 64 == reverse throttle + self.run_auxfunc(64, 2, run_cmd=run_cmd) # 64 == reverse throttle self.wait_statustext("RevThrottle: ENABLE", check_context=True) - self.run_auxfunc(64, 0) + self.run_auxfunc(64, 0, run_cmd=run_cmd) self.wait_statustext("RevThrottle: DISABLE", check_context=True) - self.run_auxfunc(65, 2) # 65 == GPS_DISABLE + self.run_auxfunc(65, 2, run_cmd=run_cmd) # 65 == GPS_DISABLE self.start_subtest("Bad auxfunc") self.run_auxfunc( 65231, 2, - want_result=mavutil.mavlink.MAV_RESULT_FAILED + want_result=mavutil.mavlink.MAV_RESULT_FAILED, + run_cmd=run_cmd, ) self.start_subtest("Bad switchpos") self.run_auxfunc( 62, 17, - want_result=mavutil.mavlink.MAV_RESULT_DENIED + want_result=mavutil.mavlink.MAV_RESULT_DENIED, + run_cmd=run_cmd, ) + def MAV_CMD_DO_AUX_FUNCTION(self): + '''Test triggering Auxiliary Functions via mavlink''' + self._MAV_CMD_DO_AUX_FUNCTION(run_cmd=self.run_cmd) + self._MAV_CMD_DO_AUX_FUNCTION(run_cmd=self.run_cmd_int) + def FlyEachFrame(self): '''Fly each supported internal frame''' vinfo = vehicleinfo.VehicleInfo() @@ -3592,6 +3950,7 @@ def FlyEachFrame(self): "quadplane-tilttrivec": "loses attitude control and crashes", "plane-ice" : "needs ICE control channel for ignition", "quadplane-ice" : "needs ICE control channel for ignition", + "quadplane-can" : "needs CAN periph", } for frame in sorted(vinfo_options["frames"].keys()): self.start_subtest("Testing frame (%s)" % str(frame)) @@ -3610,7 +3969,7 @@ def FlyEachFrame(self): # to carry the path to the JSON. actual_model = model.split(":")[0] defaults = self.model_defaults_filepath(actual_model) - if type(defaults) != list: + if not isinstance(defaults, list): defaults = [defaults] self.customise_SITL_commandline( ["--defaults", ','.join(defaults), ], @@ -3676,13 +4035,9 @@ def WatchdogHome(self): new_home.altitude = new_home.altitude + 300000 # 300 metres self.run_cmd_int( mavutil.mavlink.MAV_CMD_DO_SET_HOME, - 0, # p1, - 0, # p2, - 0, # p3, - 0, # p4, - new_home.latitude, - new_home.longitude, - new_home.altitude/1000.0, # mm => m + p5=new_home.latitude, + p6=new_home.longitude, + p7=new_home.altitude/1000.0, # mm => m ) old_bootcount = self.get_parameter('STAT_BOOTCNT') self.progress("Forcing watchdog reset") @@ -3759,6 +4114,18 @@ def AUTOTUNE(self): self.change_mode('FBWA') self.fly_home_land_and_disarm(timeout=tdelta+240) + def AutotuneFiltering(self): + '''Test AutoTune mode with filter updates disabled''' + self.set_parameters({ + "AUTOTUNE_OPTIONS": 3, + # some filtering is required for autotune to complete + "RLL_RATE_FLTD": 10, + "PTCH_RATE_FLTD": 10, + "RLL_RATE_FLTT": 20, + "PTCH_RATE_FLTT": 20, + }) + self.AUTOTUNE() + def LandingDrift(self): '''Circuit with baro drift''' self.customise_SITL_commandline([], wipe=True) @@ -3835,24 +4202,85 @@ def ForcedDCM(self): self.fly_home_land_and_disarm() - def MegaSquirt(self): - '''Test MegaSquirt EFI''' + def EFITest(self, efi_type, name, sim_name, check_fuel_flow=True): + '''method to be called by EFI tests''' + self.start_subtest("EFI Test for (%s)" % name) self.assert_not_receiving_message('EFI_STATUS') self.set_parameters({ - 'SIM_EFI_TYPE': 1, - 'EFI_TYPE': 1, + 'SIM_EFI_TYPE': efi_type, + 'EFI_TYPE': efi_type, 'SERIAL5_PROTOCOL': 24, + 'RPM1_TYPE': 10, }) - self.customise_SITL_commandline(["--uartF=sim:megasquirt"]) - self.delay_sim_time(5) - m = self.assert_receive_message('EFI_STATUS') - mavutil.dump_message_verbose(sys.stdout, m) - if m.throttle_out != 0: - raise NotAchievedException("Expected zero throttle") - if m.health != 1: - raise NotAchievedException("Not healthy") - if m.intake_manifold_temperature < 20: - raise NotAchievedException("Bad intake manifold temperature") + + self.customise_SITL_commandline( + ["--uartF=sim:%s" % sim_name, + ], + ) + self.wait_ready_to_arm() + + baro_m = self.assert_receive_message("SCALED_PRESSURE") + self.progress(self.dump_message_verbose(baro_m)) + baro_temperature = baro_m.temperature / 100.0 # cDeg->deg + m = self.assert_received_message_field_values("EFI_STATUS", { + "throttle_out": 0, + "health": 1, + }, very_verbose=1) + + if abs(baro_temperature - m.intake_manifold_temperature) > 1: + raise NotAchievedException( + "Bad intake manifold temperature (want=%f got=%f)" % + (baro_temperature, m.intake_manifold_temperature)) + + self.arm_vehicle() + + self.set_rc(3, 1300) + + tstart = self.get_sim_time() + while True: + now = self.get_sim_time_cached() + if now - tstart > 10: + raise NotAchievedException("RPM1 and EFI_STATUS.rpm did not match") + rpm_m = self.assert_receive_message("RPM", verbose=1) + want_rpm = 1000 + if rpm_m.rpm1 < want_rpm: + continue + + m = self.assert_receive_message("EFI_STATUS", verbose=1) + if abs(m.rpm - rpm_m.rpm1) > 100: + continue + + break + + self.progress("now we're started, check a few more values") + # note that megasquirt drver doesn't send throttle, so throttle_out is zero! + m = self.assert_received_message_field_values("EFI_STATUS", { + "health": 1, + }, very_verbose=1) + m = self.wait_message_field_values("EFI_STATUS", { + "throttle_position": 31, + "intake_manifold_temperature": 28, + }, very_verbose=1, epsilon=2) + + if check_fuel_flow: + if abs(m.fuel_flow - 0.2) < 0.0001: + raise NotAchievedException("Expected fuel flow") + + self.set_rc(3, 1000) + + # need to force disarm as the is_flying flag can trigger with the engine running + self.disarm_vehicle(force=True) + + def MegaSquirt(self): + '''test MegaSquirt driver''' + self.EFITest( + 1, "MegaSquirt", "megasquirt", + check_fuel_flow=False, + ) + + def Hirth(self): + '''Test Hirth EFI''' + self.EFITest(8, "Hirth", "hirth") def GlideSlopeThresh(self): '''Test rebuild glide slope if above and climbing''' @@ -4342,25 +4770,13 @@ def MissionJumpTags_missing_jump_target(self, target_system=1, target_component= self.progress("Checking incorrect tag behaviour") self.run_cmd( mavutil.mavlink.MAV_CMD_DO_JUMP_TAG, - jump_target + 1, # p1 - 0, # p2 - 0, # p3 - 0, # p4 - 0, # p5 - 0, # p6 - 0, # p7 + p1=jump_target + 1, want_result=mavutil.mavlink.MAV_RESULT_FAILED ) self.progress("Checking correct tag behaviour") self.run_cmd( mavutil.mavlink.MAV_CMD_DO_JUMP_TAG, - jump_target, # p1 - 0, # p2 - 0, # p3 - 0, # p4 - 0, # p5 - 0, # p6 - 0, # p7 + p1=jump_target, ) self.assert_current_waypoint(4) @@ -4398,13 +4814,7 @@ def MissionJumpTags_jump_tag_at_end_of_mission(self, target_system=1, target_com self.arm_vehicle() self.run_cmd( mavutil.mavlink.MAV_CMD_DO_JUMP_TAG, - 17, # p1 - 0, # p2 - 0, # p3 - 0, # p4 - 0, # p5 - 0, # p6 - 0, # p7 + p1=17, want_result=mavutil.mavlink.MAV_RESULT_FAILED ) self.disarm_vehicle() @@ -4448,8 +4858,10 @@ def AltResetBadGPS(self): # reboot to clear potentially bad state def trigger_airspeed_cal(self): - self.run_cmd(mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, - 0, 0, 1, 0, 0, 0, 0) + self.run_cmd( + mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, + p3=1, + ) def AirspeedCal(self): '''test Airspeed calibration''' @@ -4466,7 +4878,7 @@ def AirspeedCal(self): self.start_subtest('0 airspeed sensors') self.set_parameter('ARSPD_TYPE', 0) self.reboot_sitl() - self.wait_statustext('No airspeed sensor present or enabled', check_context=True) + self.wait_statustext('No airspeed sensor', check_context=True) self.trigger_airspeed_cal() self.delay_sim_time(5) if self.statustext_in_collections('Airspeed 1 calibrated'): @@ -4486,6 +4898,412 @@ def AirspeedCal(self): self.reboot_sitl() + def RunMissionScript(self): + '''Test run_mission.py script''' + script = os.path.join('Tools', 'autotest', 'run_mission.py') + self.stop_SITL() + util.run_cmd([ + util.reltopdir(script), + self.binary, + 'plane', + self.generic_mission_filepath_for_filename("flaps.txt"), + ]) + self.start_SITL() + + def MAV_CMD_GUIDED_CHANGE_ALTITUDE(self): + '''test handling of MAV_CMD_GUIDED_CHANGE_ALTITUDE''' + self.takeoff(30, relative=True) + self.change_mode('GUIDED') + for alt in 50, 70: + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_GUIDED_CHANGE_ALTITUDE, + p7=alt, + frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + ) + self.wait_altitude(alt-1, alt+1, timeout=30, relative=True) + + # test for #24535 + self.change_mode('LOITER') + self.delay_sim_time(5) + self.change_mode('GUIDED') + self.wait_altitude( + alt-3, # NOTE: reuse of alt from above loop! + alt+3, + minimum_duration=10, + timeout=30, + relative=True, + ) + self.fly_home_land_and_disarm() + + def _MAV_CMD_PREFLIGHT_CALIBRATION(self, command): + self.context_push() + self.start_subtest("Denied when armed") + self.wait_ready_to_arm() + self.arm_vehicle() + command( + mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, + p1=1, + want_result=mavutil.mavlink.MAV_RESULT_FAILED, + ) + self.disarm_vehicle() + + self.context_collect('STATUSTEXT') + + self.start_subtest("gyro cal") + command( + mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, + p1=1, + ) + + self.start_subtest("baro cal") + command( + mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, + p3=1, + ) + self.wait_statustext('Barometer calibration complete', check_context=True) + + # accelcal skipped here, it is checked elsewhere + + self.start_subtest("ins trim") + command( + mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, + p5=2, + ) + + # enforced delay between cals: + self.delay_sim_time(5) + + self.start_subtest("simple accel cal") + command( + mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, + p5=4, + ) + # simple gyro cal makes the GPS units go unhealthy as they are + # not maintaining their update rate (gyro cal is synchronous + # in the main loop). Usually ~30 seconds to recover... + self.wait_gps_sys_status_not_present_or_enabled_and_healthy(timeout=60) + + self.start_subtest("force save accels") + command( + mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, + p5=76, + ) + + self.start_subtest("force save compasses") + command( + mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, + p2=76, + ) + + self.context_pop() + + def MAV_CMD_PREFLIGHT_CALIBRATION(self): + '''test MAV_CMD_PREFLIGHT_CALIBRATION mavlink handling''' + self._MAV_CMD_PREFLIGHT_CALIBRATION(self.run_cmd) + self._MAV_CMD_PREFLIGHT_CALIBRATION(self.run_cmd_int) + + def MAV_CMD_DO_INVERTED_FLIGHT(self): + '''fly upside-down mission item''' + alt = 30 + wps = self.create_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, alt), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 400, 0, alt), + self.create_MISSION_ITEM_INT( + mavutil.mavlink.MAV_CMD_DO_INVERTED_FLIGHT, + p1=1, + ), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 800, 0, alt), + self.create_MISSION_ITEM_INT( + mavutil.mavlink.MAV_CMD_DO_INVERTED_FLIGHT, + p1=0, + ), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1200, 0, alt), + (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0), + ]) + self.check_mission_upload_download(wps) + + self.change_mode('AUTO') + self.wait_ready_to_arm() + + self.arm_vehicle() + + self.wait_current_waypoint(2) # upright flight + self.wait_message_field_values("NAV_CONTROLLER_OUTPUT", { + "nav_roll": 0, + "nav_pitch": 0, + }, epsilon=10) + + def check_altitude(mav, m): + global initial_airspeed_threshold_reached + m_type = m.get_type() + if m_type != 'GLOBAL_POSITION_INT': + return + if abs(30 - m.relative_alt * 0.001) > 15: + raise NotAchievedException("Bad altitude while flying inverted") + + self.context_push() + self.install_message_hook_context(check_altitude) + + self.wait_current_waypoint(4) # inverted flight + self.wait_message_field_values("NAV_CONTROLLER_OUTPUT", { + "nav_roll": 180, + "nav_pitch": 9, + }, epsilon=10,) + + self.wait_current_waypoint(6) # upright flight + self.wait_message_field_values("NAV_CONTROLLER_OUTPUT", { + "nav_roll": 0, + "nav_pitch": 0, + }, epsilon=10) + + self.context_pop() # remove the check_altitude call + + self.wait_current_waypoint(7) + + self.fly_home_land_and_disarm() + + def MAV_CMD_DO_AUTOTUNE_ENABLE(self): + '''test enabling autotune via mavlink''' + self.context_collect('STATUSTEXT') + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_AUTOTUNE_ENABLE, p1=1) + self.wait_statustext('Started autotune', check_context=True) + self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_AUTOTUNE_ENABLE, p1=0) + self.wait_statustext('Stopped autotune', check_context=True) + + def DO_PARACHUTE(self): + '''test triggering parachute via mavlink''' + self.set_parameters({ + "CHUTE_ENABLED": 1, + "CHUTE_TYPE": 10, + "SERVO9_FUNCTION": 27, + "SIM_PARA_ENABLE": 1, + "SIM_PARA_PIN": 9, + "FS_LONG_ACTN": 3, + }) + for command in self.run_cmd, self.run_cmd_int: + self.wait_servo_channel_value(9, 1100) + self.wait_ready_to_arm() + self.arm_vehicle() + command( + mavutil.mavlink.MAV_CMD_DO_PARACHUTE, + p1=mavutil.mavlink.PARACHUTE_RELEASE, + ) + self.wait_servo_channel_value(9, 1300) + self.disarm_vehicle() + self.reboot_sitl() + + def _MAV_CMD_DO_GO_AROUND(self, command): + self.load_mission("mission.txt") + self.set_parameter("RTL_AUTOLAND", 3) + self.change_mode('AUTO') + self.wait_ready_to_arm() + self.arm_vehicle() + self.wait_current_waypoint(6) + command(mavutil.mavlink.MAV_CMD_DO_GO_AROUND, p1=150) + self.wait_current_waypoint(5) + self.wait_altitude(135, 165, relative=True) + self.wait_disarmed(timeout=300) + + def MAV_CMD_DO_GO_AROUND(self): + '''test MAV_CMD_DO_GO_AROUND as a mavlink command''' + self._MAV_CMD_DO_GO_AROUND(self.run_cmd) + self._MAV_CMD_DO_GO_AROUND(self.run_cmd_int) + + def _MAV_CMD_DO_FLIGHTTERMINATION(self, command): + self.set_parameters({ + "AFS_ENABLE": 1, + "SYSID_MYGCS": self.mav.source_system, + "AFS_TERM_ACTION": 42, + }) + self.wait_ready_to_arm() + self.arm_vehicle() + self.context_collect('STATUSTEXT') + command(mavutil.mavlink.MAV_CMD_DO_FLIGHTTERMINATION, p1=1) + self.wait_disarmed() + self.wait_text('Terminating due to GCS request', check_context=True) + self.reboot_sitl() + + def MAV_CMD_DO_FLIGHTTERMINATION(self): + '''test MAV_CMD_DO_FLIGHTTERMINATION works on Plane''' + self._MAV_CMD_DO_FLIGHTTERMINATION(self.run_cmd) + self._MAV_CMD_DO_FLIGHTTERMINATION(self.run_cmd_int) + + def MAV_CMD_DO_LAND_START(self): + '''test MAV_CMD_DO_LAND_START as mavlink command''' + self.set_parameters({ + "RTL_AUTOLAND": 3, + }) + self.upload_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30), + (mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM, 0, 0, 30), + self.create_MISSION_ITEM_INT( + mavutil.mavlink.MAV_CMD_DO_LAND_START, + ), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 800, 0, 0), + ]) + + self.change_mode('AUTO') + self.wait_ready_to_arm() + + self.arm_vehicle() + + self.start_subtest("DO_LAND_START as COMMAND_LONG") + self.wait_current_waypoint(2) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_LAND_START) + self.wait_current_waypoint(4) + + self.start_subtest("DO_LAND_START as COMMAND_INT") + self.set_current_waypoint(2) + self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_LAND_START) + self.wait_current_waypoint(4) + + self.fly_home_land_and_disarm() + + def MAV_CMD_NAV_ALTITUDE_WAIT(self): + '''test MAV_CMD_NAV_ALTITUDE_WAIT mission item, wiggling only''' + + # Load a single waypoint + self.upload_simple_relhome_mission([ + self.create_MISSION_ITEM_INT( + mavutil.mavlink.MAV_CMD_NAV_ALTITUDE_WAIT, + p1=1000, # 1000m alt threshold, this should not trigger + p2=10, # 10m/s descent rate, this should not trigger + p3=10 # servo wiggle every 10 seconds + ) + ]) + + def look_for_wiggle(mav, m): + if m.get_type() == 'SERVO_OUTPUT_RAW': + # Throttle must be zero + if m.servo3_raw != 1000: + raise NotAchievedException( + "Throttle must be 0 in altitude wait, got %f" % m.servo3_raw) + # Aileron, elevator and rudder must all be the same + # However, aileron is revered, so we must un-reverse it + value = 1500 - (m.servo1_raw - 1500) + if (m.servo2_raw != value) or (m.servo4_raw != value): + raise NotAchievedException( + "Aileron, elevator and rudder must be the same") + + # Start mission + self.change_mode('AUTO') + self.wait_ready_to_arm() + self.arm_vehicle() + + # Check outputs + self.context_push() + self.install_message_hook_context(look_for_wiggle) + + # Wait for a bit to let message hook sample + self.delay_sim_time(60) + + self.context_pop() + + # If the mission item completes as there is no other waypoints we will end up in RTL + if not self.mode_is('AUTO'): + raise NotAchievedException("Must still be in AUTO") + + self.disarm_vehicle() + + def start_flying_simple_rehome_mission(self, items): + '''uploads items, changes mode to auto, waits ready to arm and arms + vehicle. If the first item it a takeoff you can expect the + vehicle to fly after this method returns + ''' + + self.upload_simple_relhome_mission(items) + + self.change_mode('AUTO') + self.wait_ready_to_arm() + + self.arm_vehicle() + + def InteractTest(self): + '''just takeoff''' + + if self.mavproxy is None: + raise NotAchievedException("Must be started with --map") + + self.start_flying_simple_rehome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 800, 0, 0), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 800, 800, 0), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 400, 0), + ]) + + self.wait_current_waypoint(4) + + self.set_parameter('SIM_SPEEDUP', 1) + + self.mavproxy.interact() + + def MAV_CMD_MISSION_START(self): + '''test MAV_CMD_MISSION_START starts AUTO''' + self.upload_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 800, 0, 0), + ]) + for run_cmd in self.run_cmd, self.run_cmd_int: + self.change_mode('LOITER') + run_cmd(mavutil.mavlink.MAV_CMD_MISSION_START) + self.wait_mode('AUTO') + + def MAV_CMD_NAV_LOITER_UNLIM(self): + '''test receiving MAV_CMD_NAV_LOITER_UNLIM from GCS''' + self.takeoff(10) + self.run_cmd(mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM) + self.wait_mode('LOITER') + self.change_mode('GUIDED') + self.run_cmd_int(mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM) + self.wait_mode('LOITER') + self.fly_home_land_and_disarm() + + def MAV_CMD_NAV_RETURN_TO_LAUNCH(self): + '''test receiving MAV_CMD_NAV_RETURN_TO_LAUNCH from GCS''' + self.set_parameter('RTL_AUTOLAND', 1) + self.start_flying_simple_rehome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30), + (mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM, 0, 0, 30), + self.create_MISSION_ITEM_INT( + mavutil.mavlink.MAV_CMD_DO_LAND_START, + ), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 800, 0, 0), + ]) + + for i in self.run_cmd, self.run_cmd_int: + self.wait_current_waypoint(2) + self.run_cmd(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH) + self.wait_current_waypoint(4) + self.set_current_waypoint(2) + self.fly_home_land_and_disarm() + + def location_from_ADSB_VEHICLE(self, m): + '''return a mavutil.location extracted from an ADSB_VEHICLE mavlink + message''' + if m.altitude_type != mavutil.mavlink.ADSB_ALTITUDE_TYPE_GEOMETRIC: + raise ValueError("Expected geometric alt") + return mavutil.location( + m.lat*1e-7, + m.lon*1e-7, + m.altitude/1000.0585, # mm -> m + m.heading * 0.01 # centidegrees -> degrees + ) + + def SagetechMXS(self): + '''test Sagetech MXS ADSB device driver''' + sim_name = "sagetech_mxs" + self.set_parameters({ + "SERIAL5_PROTOCOL": 35, + "ADSB_TYPE": 4, # Sagetech-MXS + "SIM_ADSB_TYPES": 8, # Sagetech-MXS + "SIM_ADSB_COUNT": 5, + }) + self.customise_SITL_commandline(["--serial5=sim:%s" % sim_name]) + m = self.assert_receive_message("ADSB_VEHICLE") + adsb_vehicle_loc = self.location_from_ADSB_VEHICLE(m) + self.progress("ADSB Vehicle at loc %s" % str(adsb_vehicle_loc)) + home = self.home_position_as_mav_location() + self.assert_distance(home, adsb_vehicle_loc, 0, 10000) + def tests(self): '''return list of all tests''' ret = super(AutoTestPlane, self).tests() @@ -4504,6 +5322,7 @@ def tests(self): self.TestGripperMission, self.Parachute, self.ParachuteSinkRate, + self.DO_PARACHUTE, self.PitotBlockage, self.AIRSPEED_AUTOCAL, self.RangeFinder, @@ -4537,7 +5356,8 @@ def tests(self): self.TerrainMission, self.TerrainLoiter, self.VectorNavEAHRS, - self.LordEAHRS, + self.MicroStrainEAHRS5, + self.MicroStrainEAHRS7, self.Deadreckoning, self.DeadreckoningNoAirSpeed, self.EKFlaneswitch, @@ -4545,7 +5365,7 @@ def tests(self): self.RTL_CLIMB_MIN, self.ClimbBeforeTurn, self.IMUTempCal, - self.MAV_DO_AUX_FUNCTION, + self.MAV_CMD_DO_AUX_FUNCTION, self.SmartBattery, self.FlyEachFrame, self.RCDisableAirspeedUse, @@ -4556,7 +5376,9 @@ def tests(self): self.DCMFallback, self.MAVFTP, self.AUTOTUNE, + self.AutotuneFiltering, self.MegaSquirt, + self.Hirth, self.MSP_DJI, self.SpeedToFly, self.GlideSlopeThresh, @@ -4565,16 +5387,35 @@ def tests(self): self.EmbeddedParamParser, self.AerobaticsScripting, self.MANUAL_CONTROL, + self.RunMissionScript, self.WindEstimates, self.AltResetBadGPS, self.AirspeedCal, self.MissionJumpTags, - self.GCSFailsafe, + Test(self.GCSFailsafe, speedup=8), self.SDCardWPTest, + self.NoArmWithoutMissionItems, + self.MODE_SWITCH_RESET, + self.ExternalPositionEstimate, + self.SagetechMXS, + self.MAV_CMD_GUIDED_CHANGE_ALTITUDE, + self.MAV_CMD_PREFLIGHT_CALIBRATION, + self.MAV_CMD_DO_INVERTED_FLIGHT, + self.MAV_CMD_DO_AUTOTUNE_ENABLE, + self.MAV_CMD_DO_GO_AROUND, + self.MAV_CMD_DO_FLIGHTTERMINATION, + self.MAV_CMD_DO_LAND_START, + self.MAV_CMD_NAV_ALTITUDE_WAIT, + self.InteractTest, + self.MAV_CMD_MISSION_START, + self.TerrainRally, + self.MAV_CMD_NAV_LOITER_UNLIM, + self.MAV_CMD_NAV_RETURN_TO_LAUNCH, ]) return ret def disabled_tests(self): return { "LandingDrift": "Flapping test. See https://github.com/ArduPilot/ardupilot/issues/20054", + "InteractTest": "requires user interaction", } diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 58a5f6805c91a..40eb081cfb382 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -11,9 +11,9 @@ from pymavlink import mavutil -from common import AutoTest -from common import NotAchievedException -from common import AutoTestTimeoutException +import vehicle_test_suite +from vehicle_test_suite import NotAchievedException +from vehicle_test_suite import AutoTestTimeoutException if sys.version_info[0] < 3: ConnectionResetError = AutoTestTimeoutException @@ -33,7 +33,7 @@ class Joystick(): Lateral = 6 -class AutoTestSub(AutoTest): +class AutoTestSub(vehicle_test_suite.TestSuite): @staticmethod def get_not_armable_mode_list(): return [] @@ -317,12 +317,14 @@ def DiveMission(self): def GripperMission(self): '''Test gripper mission items''' self.load_mission("sub-gripper-mission.txt") - self.change_mode('GUIDED') self.wait_ready_to_arm() self.arm_vehicle() self.change_mode('AUTO') + self.wait_waypoint(1, 2, max_dist=5) self.wait_statustext("Gripper Grabbed", timeout=60) + self.wait_waypoint(1, 4, max_dist=5) self.wait_statustext("Gripper Released", timeout=60) + self.wait_waypoint(1, 6, max_dist=5) self.disarm_vehicle() def SET_POSITION_TARGET_GLOBAL_INT(self): @@ -373,17 +375,13 @@ def SET_POSITION_TARGET_GLOBAL_INT(self): def reboot_sitl(self): """Reboot SITL instance and wait it to reconnect.""" - # out battery is reset to full on reboot. So reduce it to 10% + # our battery is reset to full on reboot. So reduce it to 10% # and wait for it to go above 50. - self.run_cmd(mavutil.mavlink.MAV_CMD_BATTERY_RESET, - 255, # battery mask - 10, # percentage - 0, - 0, - 0, - 0, - 0, - 0) + self.run_cmd( + mavutil.mavlink.MAV_CMD_BATTERY_RESET, + p1=65535, # battery mask + p2=10, # percentage + ) self.run_cmd_reboot() tstart = time.time() while True: @@ -392,14 +390,10 @@ def reboot_sitl(self): # ask for the message: batt = None try: - self.send_cmd(mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE, - mavutil.mavlink.MAVLINK_MSG_ID_BATTERY_STATUS, - 0, - 0, - 0, - 0, - 0, - 0) + self.send_cmd( + mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE, + p1=mavutil.mavlink.MAVLINK_MSG_ID_BATTERY_STATUS, + ) batt = self.mav.recv_match(type='BATTERY_STATUS', blocking=True, timeout=1) @@ -433,6 +427,96 @@ def disabled_tests(self): }) return ret + def MAV_CMD_NAV_LOITER_UNLIM(self): + '''test handling of MAV_CMD_NAV_LOITER_UNLIM received via mavlink''' + for cmd in self.run_cmd, self.run_cmd_int: + self.change_mode('CIRCLE') + cmd(mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM) + self.assert_mode('POSHOLD') + + def MAV_CMD_NAV_LAND(self): + '''test handling of MAV_CMD_NAV_LAND received via mavlink''' + for cmd in self.run_cmd, self.run_cmd_int: + self.change_mode('CIRCLE') + cmd(mavutil.mavlink.MAV_CMD_NAV_LAND) + self.assert_mode('SURFACE') + + def MAV_CMD_MISSION_START(self): + '''test handling of MAV_CMD_NAV_LAND received via mavlink''' + self.upload_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2000, 0, 0), + (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0), + ]) + + self.wait_ready_to_arm() + self.arm_vehicle() + for cmd in self.run_cmd, self.run_cmd_int: + self.change_mode('CIRCLE') + cmd(mavutil.mavlink.MAV_CMD_MISSION_START) + self.assert_mode('AUTO') + self.disarm_vehicle() + + def MAV_CMD_DO_CHANGE_SPEED(self): + '''ensure vehicle changes speeds when DO_CHANGE_SPEED received''' + items = [ + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, -3), # Dive so we have constrat drag + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2000, 0, -1), + (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0), + ] + self.upload_simple_relhome_mission(items) + self.wait_ready_to_arm() + self.arm_vehicle() + self.run_cmd(mavutil.mavlink.MAV_CMD_MISSION_START) + self.progress("SENT MISSION START") + self.wait_mode('AUTO') + self.wait_current_waypoint(2) # wait after we finish diving to 3m + for run_cmd in self.run_cmd, self.run_cmd_int: + for speed in [1, 1.5, 0.5]: + run_cmd(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, p2=speed) + self.wait_groundspeed(speed-0.2, speed+0.2, minimum_duration=2, timeout=60) + self.disarm_vehicle() + + def _MAV_CMD_CONDITION_YAW(self, run_cmd): + self.arm_vehicle() + self.change_mode('GUIDED') + for angle in 5, 30, 60, 10: + angular_rate = 10 + direction = 1 + relative_or_absolute = 0 + run_cmd( + mavutil.mavlink.MAV_CMD_CONDITION_YAW, + p1=angle, + p2=angular_rate, + p3=direction, + p4=relative_or_absolute, # 1 for relative, 0 for absolute + ) + self.wait_heading(angle, minimum_duration=2) + + self.start_subtest('Relative angle') + run_cmd( + mavutil.mavlink.MAV_CMD_CONDITION_YAW, + p1=0, + p2=10, + p3=1, + p4=0, # 1 for relative, 0 for absolute + ) + self.wait_heading(0, minimum_duration=2) + run_cmd( + mavutil.mavlink.MAV_CMD_CONDITION_YAW, + p1=20, + p2=10, + p3=1, + p4=1, # 1 for relative, 0 for absolute + ) + self.wait_heading(20, minimum_duration=2) + + self.disarm_vehicle() + + def MAV_CMD_CONDITION_YAW(self): + '''ensure vehicle yaws according to GCS command''' + self._MAV_CMD_CONDITION_YAW(self.run_cmd) + self._MAV_CMD_CONDITION_YAW(self.run_cmd_int) + def tests(self): '''return list of all tests''' ret = super(AutoTestSub, self).tests() @@ -448,6 +532,12 @@ def tests(self): self.MotorThrustHoverParameterIgnore, self.SET_POSITION_TARGET_GLOBAL_INT, self.TestLogDownloadMAVProxy, + self.TestLogDownloadMAVProxyNetwork, + self.MAV_CMD_NAV_LOITER_UNLIM, + self.MAV_CMD_NAV_LAND, + self.MAV_CMD_MISSION_START, + self.MAV_CMD_DO_CHANGE_SPEED, + self.MAV_CMD_CONDITION_YAW, ]) return ret diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index e6ece2b37aa74..616411b7165a8 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 """ ArduPilot automatic test suite. @@ -21,6 +21,7 @@ import time import traceback +import blimp import rover import arducopter import arduplane @@ -33,10 +34,9 @@ import examples from pysim import util -from pymavlink import mavutil from pymavlink.generator import mavtemplate -from common import Test +from vehicle_test_suite import Test tester = None @@ -58,47 +58,6 @@ def buildlogs_path(path): return os.path.join(*bits) -def get_default_params(atype, binary): - """Get default parameters.""" - # use rover simulator so SITL is not starved of input - HOME = mavutil.location(40.071374969556928, - -105.22978898137808, - 1583.702759, - 246) - if "plane" in binary or "rover" in binary: - frame = "rover" - else: - frame = "+" - - home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading) - mavproxy_master = 'tcp:127.0.0.1:5760' - sitl = util.start_SITL(binary, - wipe=True, - model=frame, - home=home, - speedup=10, - unhide_parameters=True) - mavproxy = util.start_MAVProxy_SITL(atype, - master=mavproxy_master) - print("Dumping defaults") - idx = mavproxy.expect([r'Saved [0-9]+ parameters to (\S+)']) - if idx == 0: - # we need to restart it after eeprom erase - util.pexpect_close(mavproxy) - util.pexpect_close(sitl) - sitl = util.start_SITL(binary, model=frame, home=home, speedup=10) - mavproxy = util.start_MAVProxy_SITL(atype, - master=mavproxy_master) - mavproxy.expect(r'Saved [0-9]+ parameters to (\S+)') - parmfile = mavproxy.match.group(1) - dest = buildlogs_path('%s-defaults.parm' % atype) - shutil.copy(parmfile, dest) - util.pexpect_close(mavproxy) - util.pexpect_close(sitl) - print("Saved defaults for %s to %s" % (atype, dest)) - return True - - def build_all_filepath(): """Get build_all.sh path.""" return util.reltopdir('Tools/scripts/build_all.sh') @@ -139,7 +98,7 @@ def build_binaries(): def build_examples(**kwargs): """Build examples.""" - for target in 'fmuv2', 'Pixhawk1', 'navio', 'linux': + for target in 'Pixhawk1', 'navio', 'linux': print("Running build.examples for %s" % target) try: util.build_examples(target, **kwargs) @@ -227,7 +186,9 @@ def all_vehicles(): 'Rover', 'AntennaTracker', 'ArduSub', - 'Blimp') + 'Blimp', + 'AP_Periph', + ) def build_parameters(): @@ -380,6 +341,7 @@ def find_specific_test_to_run(step): tester_class_map = { + "test.Blimp": blimp.AutoTestBlimp, "test.Copter": arducopter.AutoTestCopter, "test.CopterTests1a": arducopter.AutoTestCopterTests1a, # 8m43s "test.CopterTests1b": arducopter.AutoTestCopterTests1b, # 8m5s @@ -399,8 +361,9 @@ def find_specific_test_to_run(step): "test.CAN": arducopter.AutoTestCAN, } -suplementary_test_binary_map = { - "test.CAN": ["sitl_periph_gps.AP_Periph", "sitl_periph_gps.AP_Periph.1"], +supplementary_test_binary_map = { + "test.CAN": ["sitl_periph_gps:AP_Periph:0:Tools/autotest/default_params/periph.parm,Tools/autotest/default_params/quad-periph.parm", # noqa: E501 + "sitl_periph_gps:AP_Periph:1:Tools/autotest/default_params/periph.parm"], } @@ -417,11 +380,11 @@ def run_specific_test(step, *args, **kwargs): # print("Got %s" % str(tester)) for a in tester.tests(): - if type(a) != Test: + if not isinstance(a, Test): a = Test(a) print("Got %s" % (a.name)) if a.name == test: - return (tester.autotest(tests=[a], allow_skips=False), tester) + return tester.autotest(tests=[a], allow_skips=False, step_name=step), tester print("Failed to find test %s on %s" % (test, testname)) sys.exit(1) @@ -444,10 +407,11 @@ def run_step(step): "postype_single": opts.postype_single, "extra_configure_args": opts.waf_configure_args, "coverage": opts.coverage, - "sitl_32bit" : opts.sitl_32bit, + "force_32bit" : opts.force_32bit, "ubsan" : opts.ubsan, "ubsan_abort" : opts.ubsan_abort, "num_aux_imus" : opts.num_aux_imus, + "dronecan_tests" : opts.dronecan_tests, } if opts.Werror: @@ -484,6 +448,11 @@ def run_step(step): return util.build_replay(board='SITL') if vehicle_binary is not None: + try: + binary = binary_path(step, debug=opts.debug) + os.unlink(binary) + except (FileNotFoundError, ValueError): + pass if len(vehicle_binary.split(".")) == 1: return util.build_SITL(vehicle_binary, **build_opts) else: @@ -495,27 +464,30 @@ def run_step(step): binary = binary_path(step, debug=opts.debug) - if step.startswith("defaults"): - vehicle = step[9:] - return get_default_params(vehicle, binary) + # see if we need any supplementary binaries supplementary_binaries = [] - if step in suplementary_test_binary_map: - for supplementary_test_binary in suplementary_test_binary_map[step]: - config_name = supplementary_test_binary.split('.')[0] - binary_name = supplementary_test_binary.split('.')[1] - instance_num = 0 - if len(supplementary_test_binary.split('.')) >= 3: - instance_num = int(supplementary_test_binary.split('.')[2]) - supplementary_binaries.append([util.reltopdir(os.path.join('build', - config_name, - 'bin', - binary_name)), - '-I {}'.format(instance_num)]) - # we are running in conjunction with a supplementary app - # can't have speedup - opts.speedup = 1.0 - else: - supplementary_binaries = [] + for k in supplementary_test_binary_map.keys(): + if step.startswith(k): + # this test needs to use supplementary binaries + for supplementary_test_binary in supplementary_test_binary_map[k]: + a = supplementary_test_binary.split(':') + if len(a) != 4: + raise ValueError("Bad supplementary_test_binary %s" % supplementary_test_binary) + config_name = a[0] + binary_name = a[1] + instance_num = int(a[2]) + param_file = a[3].split(",") + bin_path = util.reltopdir(os.path.join('build', config_name, 'bin', binary_name)) + customisation = '-I {}'.format(instance_num) + sup_binary = {"binary" : bin_path, + "customisation" : customisation, + "param_file" : param_file} + supplementary_binaries.append(sup_binary) + # we are running in conjunction with a supplementary app + # can't have speedup + opts.speedup = 1.0 + break + fly_opts = { "viewerip": opts.viewerip, "use_map": opts.map, @@ -536,6 +508,7 @@ def run_step(step): "sup_binaries": supplementary_binaries, "reset_after_every_test": opts.reset_after_every_test, "build_opts": copy.copy(build_opts), + "generate_junit": opts.junit, } if opts.speedup is not None: fly_opts["speedup"] = opts.speedup @@ -546,7 +519,7 @@ def run_step(step): global tester tester = tester_class_map[step](binary, **fly_opts) # run the test and return its result and the tester itself - return (tester.autotest(), tester) + return tester.autotest(None, step_name=step), tester # handle "test.Copter.CPUFailsafe" etc: specific_test_to_run = find_specific_test_to_run(step) @@ -608,11 +581,7 @@ class TestResults(object): def __init__(self): """Init test results class.""" self.date = time.asctime() - self.githash = util.run_cmd('git rev-parse HEAD', - output=True, - directory=util.reltopdir('.')).strip() - if sys.version_info.major >= 3: - self.githash = self.githash.decode('utf-8') + self.githash = util.get_git_hash() self.tests = [] self.files = [] self.images = [] @@ -690,9 +659,10 @@ def write_fullresults(): results.addglob("GPX track", '*.gpx') # results common to all vehicles: - vehicle_files = [('{vehicle} defaults', '{vehicle}-defaults.parm'), - ('{vehicle} core', '{vehicle}.core'), - ('{vehicle} ELF', '{vehicle}.elf'), ] + vehicle_files = [ + ('{vehicle} core', '{vehicle}.core'), + ('{vehicle} ELF', '{vehicle}.elf'), + ] vehicle_globs = [('{vehicle} log', '{vehicle}-*.BIN'), ] for vehicle in all_vehicles(): subs = {'vehicle': vehicle} @@ -713,6 +683,7 @@ def write_fullresults(): results.addglob('APM:Copter documentation', 'docs/ArduCopter/index.html') results.addglob('APM:Rover documentation', 'docs/Rover/index.html') results.addglob('APM:Sub documentation', 'docs/ArduSub/index.html') + results.addglob('APM:Blimp documentation', 'docs/Blimp/index.html') results.addglobimage("Flight Track", '*.png') write_webresults(results) @@ -748,7 +719,7 @@ def run_tests(steps): try: success = run_step(step) testinstance = None - if type(success) == tuple: + if isinstance(success, tuple): (success, testinstance) = success if success: results.add(step, 'PASSED', @@ -801,7 +772,7 @@ def run_tests(steps): return passed -vehicle_list = ['Sub', 'Copter', 'Plane', 'Tracker', 'Rover', 'QuadPlane', 'BalanceBot', 'Helicopter', 'Sailboat'] +vehicle_list = ['Sub', 'Copter', 'Plane', 'Tracker', 'Rover', 'QuadPlane', 'BalanceBot', 'Helicopter', 'Sailboat', 'Blimp'] def list_subtests(): @@ -834,7 +805,7 @@ def list_subtests_for_vehicle(vehicle_type): subtests = tester.tests() sorted_list = [] for subtest in subtests: - if type(subtest) != Test: + if not isinstance(subtest, Test): subtest = Test(subtest) sorted_list.append([subtest.name, subtest.description]) sorted_list.sort() @@ -912,6 +883,10 @@ def format_epilog(self, formatter): action='store_true', default=False, help='configure with --Werror') + parser.add_option("--junit", + default=False, + action='store_true', + help='Generate Junit XML tests report') group_build = optparse.OptionGroup(parser, "Build options") group_build.add_option("--no-configure", @@ -958,10 +933,10 @@ def format_epilog(self, formatter): action="store_true", dest="ekf_single", help="force single precision EKF") - group_build.add_option("--sitl-32bit", + group_build.add_option("--force-32bit", default=False, action='store_true', - dest="sitl_32bit", + dest="force_32bit", help="compile sitl using 32-bit") group_build.add_option("", "--ubsan", default=False, @@ -978,6 +953,11 @@ def format_epilog(self, formatter): default=0, type='int', help='number of auxiliary IMUs to simulate') + group_build.add_option("--enable-dronecan-tests", + default=False, + action='store_true', + dest="dronecan_tests", + help="enable dronecan tests") parser.add_option_group(group_build) group_sim = optparse.OptionGroup(parser, "Simulation options") @@ -1082,33 +1062,28 @@ def format_epilog(self, formatter): 'run.examples', 'build.Plane', - 'defaults.Plane', 'test.Plane', 'test.QuadPlane', 'build.Rover', - 'defaults.Rover', 'test.Rover', 'test.BalanceBot', 'test.Sailboat', 'build.Copter', - 'defaults.Copter', 'test.Copter', 'build.Helicopter', 'test.Helicopter', 'build.Tracker', - 'defaults.Tracker', 'test.Tracker', 'build.Sub', - 'defaults.Sub', 'test.Sub', 'build.Blimp', - 'defaults.Blimp', + 'test.Blimp', 'build.SITLPeriphGPS', 'test.CAN', @@ -1149,11 +1124,6 @@ def format_epilog(self, formatter): "drive.balancebot": "test.BalanceBot", "fly.CopterAVC": "test.Helicopter", "test.AntennaTracker": "test.Tracker", - "defaults.ArduCopter": "defaults.Copter", - "defaults.ArduPlane": "defaults.Plane", - "defaults.ArduSub": "defaults.Sub", - "defaults.APMrover2": "defaults.Rover", - "defaults.AntennaTracker": "defaults.Tracker", "fly.ArduCopterTests1a": "test.CopterTests1a", "fly.ArduCopterTests1b": "test.CopterTests1b", "fly.ArduCopterTests1c": "test.CopterTests1c", diff --git a/Tools/autotest/balancebot.py b/Tools/autotest/balancebot.py index efeb4875109aa..de99e7542ec3e 100644 --- a/Tools/autotest/balancebot.py +++ b/Tools/autotest/balancebot.py @@ -10,9 +10,9 @@ import os from rover import AutoTestRover -from common import AutoTest -from common import NotAchievedException +import vehicle_test_suite +from vehicle_test_suite import NotAchievedException # get location of scripts testdir = os.path.dirname(os.path.realpath(__file__)) @@ -105,13 +105,13 @@ def tests(self): '''note that while AutoTestBalanceBot inherits from Rover we don't inherit Rover's tests!''' - ret = AutoTest.tests(self) + ret = vehicle_test_suite.TestSuite.tests(self) ret.extend([ self.DriveRTL, self.DriveMission, self.TestWheelEncoder, - self.GetBanner, + self.MAV_CMD_DO_SEND_BANNER, self.DO_SET_MODE, self.ServoRelayEvents, ]) diff --git a/Tools/autotest/bisect-helper.py b/Tools/autotest/bisect-helper.py index e0da93182ef33..3858a53fb6b3a 100755 --- a/Tools/autotest/bisect-helper.py +++ b/Tools/autotest/bisect-helper.py @@ -2,6 +2,10 @@ '''A helper script for bisecting common problems when working with ArduPilot +When running bisections, you should + +export SITL_PANIC_EXIT=1 + Bisect between a commit which builds and one which doesn't, finding the first commit which broke the build with a specific failure: @@ -115,7 +119,6 @@ def run_program(self, prefix, cmd_list): '''run cmd_list, spewing and setting output in self''' self.progress("Running (%s)" % " ".join(cmd_list)) p = subprocess.Popen(cmd_list, - bufsize=1, stdin=None, close_fds=True, stdout=subprocess.PIPE, @@ -130,7 +133,7 @@ def run_program(self, prefix, cmd_list): # select not available on Windows... probably... time.sleep(0.1) continue - if type(x) == bytes: + if isinstance(x, bytes): x = x.decode('utf-8') self.program_output += x x = x.rstrip() @@ -184,6 +187,10 @@ def __init__(self, opts): super(BisectBuild, self).__init__(opts) def run(self): + if self.opts.build_failure_string is None: + self.progress("--build-failure-string is required when using --build") + self.exit_abort() + self.update_submodules() self.build() # may exit with skip or fail self.exit_pass() @@ -273,7 +280,7 @@ def run(self): if code == self.exit_fail_code(): with open("/tmp/fail-counts", "a") as f: - print("Failed on run %u" % (i+1,), file=f) + f.write("Failed on run %u\n" % (i+1,)) if ignore: self.progress("Ignoring this run") continue @@ -282,6 +289,8 @@ def run(self): self.git_reset() + self.progress("Exit code is %u" % code) + sys.exit(code) @@ -294,8 +303,7 @@ def run(self): help="Help bisect a build failure") parser.add_option("--build-failure-string", type='string', - default=None, - help="If supplied, must be present in" + help="Must be present in" "build output to count as a failure") group_autotest = optparse.OptionGroup(parser, "Run-AutoTest Options") diff --git a/Tools/autotest/blimp.py b/Tools/autotest/blimp.py new file mode 100644 index 0000000000000..661c4a516286c --- /dev/null +++ b/Tools/autotest/blimp.py @@ -0,0 +1,262 @@ +''' +Fly Blimp in SITL + +AP_FLAKE8_CLEAN +''' + +from __future__ import print_function +import os +import shutil + +from pymavlink import mavutil + +from vehicle_test_suite import TestSuite + +# get location of scripts +testdir = os.path.dirname(os.path.realpath(__file__)) +SITL_START_LOCATION = mavutil.location(-35.362938, 149.165085, 584, 0) + +# Flight mode switch positions are set-up in blimp.parm to be +# switch 1 = Land +# switch 2 = Manual +# switch 3 = Velocity +# switch 4 = Loiter +# switch 5 = Manual +# switch 6 = Manual + + +class AutoTestBlimp(TestSuite): + @staticmethod + def get_not_armable_mode_list(): + return [] + + @staticmethod + def get_not_disarmed_settable_modes_list(): + return [] + + @staticmethod + def get_no_position_not_settable_modes_list(): + return [] + + @staticmethod + def get_position_armable_modes_list(): + return [] + + @staticmethod + def get_normal_armable_modes_list(): + return [] + + def log_name(self): + return "Blimp" + + def default_mode(self): + return "MANUAL" + + def test_filepath(self): + return os.path.realpath(__file__) + + def default_speedup(self): + return 100 + + def set_current_test_name(self, name): + self.current_test_name_directory = "Blimp_Tests/" + name + "/" + + def sitl_start_location(self): + return SITL_START_LOCATION + + def sitl_streamrate(self): + return 5 + + def vehicleinfo_key(self): + return 'Blimp' + + def default_frame(self): + return "Blimp" + + def apply_defaultfile_parameters(self): + # Blimp passes in a defaults_filepath in place of applying + # parameters afterwards. + pass + + def defaults_filepath(self): + return self.model_defaults_filepath(self.frame) + + def wait_disarmed_default_wait_time(self): + return 120 + + def close(self): + super(AutoTestBlimp, self).close() + + # [2014/05/07] FC Because I'm doing a cross machine build + # (source is on host, build is on guest VM) I cannot hard link + # This flag tells me that I need to copy the data out + if self.copy_tlog: + shutil.copy(self.logfile, self.buildlog) + + def is_blimp(self): + return True + + def get_stick_arming_channel(self): + return int(self.get_parameter("RCMAP_YAW")) + + def get_disarm_delay(self): + return int(self.get_parameter("DISARM_DELAY")) + + def set_autodisarm_delay(self, delay): + self.set_parameter("DISARM_DELAY", delay) + + def FlyManual(self): + '''test manual mode''' + self.change_mode('MANUAL') + self.wait_ready_to_arm() + self.arm_vehicle() + + acc = 0.5 + + # make sure we don't drift: + bl = self.mav.location() + tl = self.offset_location_ne(location=bl, metres_north=2, metres_east=0) + ttl = self.offset_location_ne(location=bl, metres_north=4, metres_east=0) + tr = self.offset_location_ne(location=bl, metres_north=4, metres_east=2) + ttr = self.offset_location_ne(location=bl, metres_north=4, metres_east=4) + + if self.mavproxy is not None: + self.mavproxy.send(f"map icon {bl.lat} {bl.lng} flag\n") + self.mavproxy.send(f"map icon {tl.lat} {tl.lng} flag\n") + self.mavproxy.send(f"map icon {ttl.lat} {ttl.lng} flag\n") + self.mavproxy.send(f"map icon {tr.lat} {tr.lng} flag\n") + self.mavproxy.send(f"map icon {ttr.lat} {ttr.lng} flag\n") + + self.set_rc(2, 2000) + self.wait_distance_to_location(tl, 0, acc, timeout=10) + self.set_rc(2, 1500) + self.wait_distance_to_location(ttl, 0, acc, timeout=15) + self.set_rc(1, 2000) + self.wait_distance_to_location(tr, 0, acc, timeout=10) + self.set_rc(1, 1500) + self.wait_distance_to_location(ttr, 0, acc, timeout=15) + self.change_mode('RTL') + self.wait_distance_to_location(bl, 0, 0.5, timeout=30, minimum_duration=5) # make sure it can hold position + self.change_mode('MANUAL') + + self.wait_distance_to_location(bl, 0, acc, timeout=5) # make sure we haven't moved from the spot + + self.set_rc(3, 2000) + self.wait_altitude(5, 5.5, relative=True, timeout=15) + self.set_rc(3, 1500) + + self.wait_distance_to_location(bl, 0, acc, timeout=5) # make sure we haven't moved from the spot + + self.set_rc(4, 1000) + self.wait_heading(340, accuracy=5, timeout=5) # short timeout to check yawrate + self.set_rc(4, 1500) + + self.wait_distance_to_location(bl, 0, acc, timeout=5) # make sure we haven't moved from the spot + + self.set_rc(3, 1000) + self.wait_altitude(0, 0.5, relative=True, timeout=20) + self.set_rc(3, 1500) + + self.wait_distance_to_location(bl, 0, acc, timeout=5) # make sure we haven't moved from the spot + + self.set_rc(4, 2000) + self.wait_heading(135, accuracy=5, timeout=10) # short timeout to check yawrate + self.set_rc(4, 1500) + + self.wait_distance_to_location(bl, 0, acc, timeout=5) # make sure we haven't moved from the spot + + self.disarm_vehicle() + + def FlyLoiter(self): + '''test loiter mode''' + + self.change_mode('LOITER') + self.wait_ready_to_arm() + self.arm_vehicle() + + siz = 5 + tim = 60 + + # make sure we don't drift: + bl = self.mav.location() + tl = self.offset_location_ne(location=bl, metres_north=siz, metres_east=0) + tr = self.offset_location_ne(location=bl, metres_north=siz, metres_east=siz) + br = self.offset_location_ne(location=bl, metres_north=0, metres_east=siz) + + print("Locations are:") + print("bottom left ", bl.lat, bl.lng) + print("top left ", tl.lat, tl.lng) + print("top right ", tr.lat, tr.lng) + print("bottom right ", br.lat, br.lng) + + if self.mavproxy is not None: + self.mavproxy.send(f"map icon {bl.lat} {bl.lng} flag\n") + self.mavproxy.send(f"map icon {tl.lat} {tl.lng} flag\n") + self.mavproxy.send(f"map icon {tr.lat} {tr.lng} flag\n") + self.mavproxy.send(f"map icon {br.lat} {br.lng} flag\n") + + self.set_parameter("SIMPLE_MODE", 1) + + self.set_rc(2, 2000) + self.wait_distance_to_location(tl, 0, 0.2, timeout=tim) + self.set_rc(2, 1500) + + self.set_rc(1, 2000) + self.wait_distance_to_location(tr, 0, 0.5, timeout=tim) + self.set_rc(1, 1500) + + self.set_rc(2, 1000) + self.wait_distance_to_location(br, 0, 0.5, timeout=tim) + self.set_rc(2, 1500) + + self.set_rc(1, 1000) + self.wait_distance_to_location(bl, 0, 0.5, timeout=tim) + self.set_rc(1, 1500) + + fin = self.mav.location() + + self.set_rc(4, 1700) + self.wait_heading(135, accuracy=2, timeout=tim) + self.set_rc(4, 1500) + + self.wait_distance_to_location(fin, 0, 0.15, timeout=5) # make sure we haven't moved from the spot + + self.set_rc(3, 2000) + self.wait_altitude(5, 5.5, relative=True, timeout=60) + self.set_rc(3, 1000) + self.wait_altitude(0, 0.5, relative=True, timeout=60) + self.set_rc(3, 1500) + + self.wait_distance_to_location(fin, 0, 0.15, timeout=5) # make sure we haven't moved from the spot + + self.set_rc(4, 1300) + self.wait_heading(0, accuracy=2, timeout=tim) + self.set_rc(4, 1500) + + self.wait_distance_to_location(fin, 0, 0.15, timeout=5) # make sure we haven't moved from the spot + + self.disarm_vehicle() + + def PREFLIGHT_Pressure(self): + '''test triggering pressure calibration with mavlink command''' + # as airspeed is not instantiated on Blimp we expect to + # instantly get back an accepted. + self.run_cmd( + mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, + p3=1, # p3, baro + ) + + def tests(self): + '''return list of all tests''' + # ret = super(AutoTestBlimp, self).tests() + ret = [] + ret.extend([ + self.FlyManual, + self.FlyLoiter, + self.PREFLIGHT_Pressure, + ]) + return ret + + def disabled_tests(self): + return { + } diff --git a/Tools/autotest/check_autotest_speedup.py b/Tools/autotest/check_autotest_speedup.py index a601dcc5e9920..621726c8677c7 100755 --- a/Tools/autotest/check_autotest_speedup.py +++ b/Tools/autotest/check_autotest_speedup.py @@ -38,7 +38,6 @@ def run_program(self, prefix, cmd_list): '''run cmd_list, spewing and setting output in self''' self.progress("Running (%s)" % " ".join(cmd_list)) p = subprocess.Popen(cmd_list, - bufsize=1, stdin=None, close_fds=True, stdout=subprocess.PIPE, @@ -53,7 +52,7 @@ def run_program(self, prefix, cmd_list): # select not available on Windows... probably... time.sleep(0.1) continue - if type(x) == bytes: + if isinstance(x, bytes): x = x.decode('utf-8') self.program_output += x x = x.rstrip() diff --git a/Tools/autotest/default_params/blimp.parm b/Tools/autotest/default_params/blimp.parm index 192455b115c7b..a790eec33454f 100644 --- a/Tools/autotest/default_params/blimp.parm +++ b/Tools/autotest/default_params/blimp.parm @@ -38,6 +38,18 @@ RC7_TRIM 1500 RC8_MAX 2000 RC8_MIN 1000 RC8_TRIM 1500 +SERVO1_MAX 2200 +SERVO1_MIN 500 +SERVO1_TRIM 1350 +SERVO2_MAX 2200 +SERVO2_MIN 500 +SERVO2_TRIM 1350 +SERVO3_MAX 2200 +SERVO3_MIN 500 +SERVO3_TRIM 1350 +SERVO4_MAX 2200 +SERVO4_MIN 500 +SERVO4_TRIM 1350 # setting servo functions for the four fins SERVO1_FUNCTION 33 @@ -76,16 +88,18 @@ INS_ACC3SCAL_Z 1.000 ARMING_RUDDER 0 GCS_PID_MASK 255 +SIM_SERVO_SPEED 0.06 +LOG_BITMASK 65535 # default PID params for position and velocity-controlled modes -MAX_POS_XY 0.3 +MAX_POS_XY 0.15 MAX_POS_YAW 0.3 MAX_POS_Z 0.15 -MAX_VEL_XY 0.4 -MAX_VEL_YAW 0.5 +MAX_VEL_XY 0.2 +MAX_VEL_YAW 0.4 MAX_VEL_Z 0.2 -VELXY_D 0.0 +VELXY_D 1.0 VELXY_FF 0.0 VELXY_FLTD 3.0 VELXY_FLTE 3.0 @@ -98,7 +112,7 @@ VELYAW_FLTD 3.0 VELYAW_FLTE 3.0 VELYAW_I 0.8 VELYAW_IMAX 5.0 -VELYAW_P 15.0 +VELYAW_P 10.0 VELZ_D 0.0 VELZ_FF 0.0 VELZ_FLTD 3.0 diff --git a/Tools/autotest/default_params/copter-heli-dual.parm b/Tools/autotest/default_params/copter-heli-dual.parm index d12d6725f63c4..5b5e58adda58a 100644 --- a/Tools/autotest/default_params/copter-heli-dual.parm +++ b/Tools/autotest/default_params/copter-heli-dual.parm @@ -1,9 +1,9 @@ FRAME_CLASS 11 ATC_ANG_PIT_P 4.5 ATC_ANG_YAW_P 4.5 -ATC_RAT_PIT_D 0.0012 -ATC_RAT_PIT_P 0.001 -ATC_RAT_PIT_FF 0.17 +ATC_RAT_PIT_D 0.0005 +ATC_RAT_PIT_P 0.02 +ATC_RAT_PIT_FF 0.0 ATC_RAT_YAW_D 0.0015 ATC_RAT_YAW_P 0.14685 ATC_HOVR_ROL_TRM 0 diff --git a/Tools/autotest/default_params/copter-heli-gas.parm b/Tools/autotest/default_params/copter-heli-gas.parm new file mode 100644 index 0000000000000..38269b2d2aa68 --- /dev/null +++ b/Tools/autotest/default_params/copter-heli-gas.parm @@ -0,0 +1,9 @@ +H_RSC_IDLE 14 +H_RSC_MODE 3 +H_RSC_RAMP_TIME 10 +H_RSC_RUNUP_TIME 15 +H_RSC_THRCRV_0 27 +H_RSC_THRCRV_25 32 +H_RSC_THRCRV_50 45 +H_RSC_THRCRV_75 75 +H_RSC_THRCRV_100 100 \ No newline at end of file diff --git a/Tools/autotest/default_params/copter-heli.parm b/Tools/autotest/default_params/copter-heli.parm index 08a836dd01f97..994bf370801ec 100644 --- a/Tools/autotest/default_params/copter-heli.parm +++ b/Tools/autotest/default_params/copter-heli.parm @@ -70,7 +70,7 @@ ATC_ACCEL_Y_MAX 60000 ATC_HOVR_ROL_TRM 320 H_COL_MAX 1740 H_COL_MIN 1460 -H_COL_ANG_MAX 10 +H_COL_ANG_MAX 12 H_COL_ANG_MIN -2 H_RSC_MODE 2 H_RSC_SETPOINT 66 diff --git a/Tools/autotest/default_params/copter-tri.parm b/Tools/autotest/default_params/copter-tri.parm index 0f861b7706470..16927c554186f 100644 --- a/Tools/autotest/default_params/copter-tri.parm +++ b/Tools/autotest/default_params/copter-tri.parm @@ -1,6 +1,6 @@ SERVO7_MIN 1000 SERVO7_MAX 2000 -SERVO7_TRIM 1360 +SERVO7_TRIM 1500 FRAME_CLASS 7 ATC_RAT_YAW_FLTE 5.0 MOT_YAW_SV_ANGLE 60 diff --git a/Tools/autotest/default_params/copter-winch.parm b/Tools/autotest/default_params/copter-winch.parm new file mode 100644 index 0000000000000..4f2279c00d992 --- /dev/null +++ b/Tools/autotest/default_params/copter-winch.parm @@ -0,0 +1,6 @@ +# servo winch type +# winch connected to servo output channel 9 +# rc input chnanel 9 used by pilot to control winch +WINCH_TYPE 1 +SERVO9_FUNCTION 88 +RC9_OPTION 45 diff --git a/Tools/autotest/default_params/copter.parm b/Tools/autotest/default_params/copter.parm index 7f41498a4ece8..17e5c25b26d97 100644 --- a/Tools/autotest/default_params/copter.parm +++ b/Tools/autotest/default_params/copter.parm @@ -2,8 +2,8 @@ FRAME_TYPE 0 FS_THR_ENABLE 1 ARSPD_PIN 1 ARSPD_BUS 2 -ATC_RAT_YAW_P 0.09 -ATC_RAT_YAW_I 0.009 +ATC_RAT_YAW_P 0.3 +ATC_RAT_YAW_I 0.02 BATT_MONITOR 4 COMPASS_OFS_X 5 COMPASS_OFS_Y 13 diff --git a/Tools/autotest/default_params/periph.parm b/Tools/autotest/default_params/periph.parm new file mode 100644 index 0000000000000..506d302c6676c --- /dev/null +++ b/Tools/autotest/default_params/periph.parm @@ -0,0 +1,24 @@ +# parameters for SITL peripheral + +GPS_TYPE 1 +COMPASS_ENABLE 1 +BARO_ENABLE 1 +ARSPD_TYPE 100 +RNGFND1_TYPE 100 +RNGFND1_MAX_CM 12000 +BATT_MONITOR 16 +BATT_I2C_BUS 2 + +# by default disable motors/servos, overridden in vehicle specific parameters +OUT1_FUNCTION -1 +OUT2_FUNCTION -1 +OUT3_FUNCTION -1 +OUT4_FUNCTION -1 +OUT5_FUNCTION -1 +OUT6_FUNCTION -1 +OUT7_FUNCTION -1 +OUT8_FUNCTION -1 + +# enable some simulated ADSB vehicles +SIM_ADSB_COUNT 4 +ADSB_PORT 2 diff --git a/Tools/autotest/default_params/plane-jsbsim.parm b/Tools/autotest/default_params/plane-jsbsim.parm index e53e15acd64f2..a77ec1084ef7d 100644 --- a/Tools/autotest/default_params/plane-jsbsim.parm +++ b/Tools/autotest/default_params/plane-jsbsim.parm @@ -7,6 +7,7 @@ TRIM_THROTTLE 50 LIM_PITCH_MIN -2000 LIM_PITCH_MAX 2500 LIM_ROLL_CD 6500 +LAND_DISARMDELAY 3 LAND_PITCH_CD 100 LAND_FLARE_SEC 3 ARSPD_USE 1 diff --git a/Tools/autotest/default_params/quad-can.parm b/Tools/autotest/default_params/quad-can.parm new file mode 100644 index 0000000000000..0fe872319e365 --- /dev/null +++ b/Tools/autotest/default_params/quad-can.parm @@ -0,0 +1,8 @@ +CAN_P1_DRIVER 1 +CAN_D1_UC_ESC_BM 0x0F +SIM_CAN_SRV_MSK 0xFf +SIM_VIB_MOT_MAX 270 +GPS_TYPE 9 +RNGFND1_TYPE 24 +RNGFND1_MAX_CM 11000 +BATT_MONITOR 8 diff --git a/Tools/autotest/default_params/quad-periph.parm b/Tools/autotest/default_params/quad-periph.parm new file mode 100644 index 0000000000000..2e29723e09d2e --- /dev/null +++ b/Tools/autotest/default_params/quad-periph.parm @@ -0,0 +1,17 @@ +# extra parameters for SITL peripheral quadplane + +SIM_CAN_SRV_MSK 0x0f + +# ESCs +OUT1_FUNCTION 33 +OUT1_MIN 1000 +OUT1_MAX 2000 +OUT2_FUNCTION 34 +OUT2_MIN 1000 +OUT2_MAX 2000 +OUT3_FUNCTION 35 +OUT3_MIN 1000 +OUT3_MAX 2000 +OUT4_FUNCTION 36 +OUT4_MIN 1000 +OUT4_MAX 2000 diff --git a/Tools/autotest/default_params/quadplane-can.parm b/Tools/autotest/default_params/quadplane-can.parm new file mode 100644 index 0000000000000..3971389f99264 --- /dev/null +++ b/Tools/autotest/default_params/quadplane-can.parm @@ -0,0 +1,13 @@ +CAN_P1_DRIVER 1 +CAN_D1_UC_ESC_BM 0xF0 +CAN_D1_UC_ESC_OF 4 +CAN_D1_UC_SRV_BM 0x0F +CAN_D1_UC_OPTION 16 +SIM_CAN_SRV_MSK 0xfff +SIM_VIB_MOT_MAX 270 +GPS_TYPE 9 +ARSPD_TYPE 8 +RNGFND1_TYPE 24 +RNGFND1_MAX_CM 11000 +RNGFND_LANDING 1 +BATT_MONITOR 8 diff --git a/Tools/autotest/default_params/quadplane-ice.parm b/Tools/autotest/default_params/quadplane-ice.parm index 9b645e2e46336..1ed7ff8b84072 100644 --- a/Tools/autotest/default_params/quadplane-ice.parm +++ b/Tools/autotest/default_params/quadplane-ice.parm @@ -1 +1,2 @@ Q_OPTIONS 64 +ICE_RPM_THRESH 50 # idles at 70 (1% thrust) diff --git a/Tools/autotest/default_params/quadplane-periph.parm b/Tools/autotest/default_params/quadplane-periph.parm new file mode 100644 index 0000000000000..dc7a19687b612 --- /dev/null +++ b/Tools/autotest/default_params/quadplane-periph.parm @@ -0,0 +1,23 @@ +# extra parameters for SITL peripheral quadplane + +SIM_CAN_SRV_MSK 0xff + +# control surfaces +OUT1_FUNCTION 51 +OUT2_FUNCTION 52 +OUT3_FUNCTION 53 +OUT4_FUNCTION 54 + +# ESCs +OUT5_FUNCTION 33 +OUT5_MIN 1000 +OUT5_MAX 2000 +OUT6_FUNCTION 34 +OUT6_MIN 1000 +OUT6_MAX 2000 +OUT7_FUNCTION 35 +OUT7_MIN 1000 +OUT7_MAX 2000 +OUT8_FUNCTION 36 +OUT8_MIN 1000 +OUT8_MAX 2000 diff --git a/Tools/autotest/fakepos.py b/Tools/autotest/fakepos.py index 8e23a15e29c3f..9793dc7822f9f 100755 --- a/Tools/autotest/fakepos.py +++ b/Tools/autotest/fakepos.py @@ -51,6 +51,7 @@ def kt2mps(x): def mps2kt(x): return x / 0.514444444 + udp = udp_out("127.0.0.1:5501") latitude = -35 diff --git a/Tools/autotest/helicopter.py b/Tools/autotest/helicopter.py index 4a3ee9b64a42e..0da15f1c91e4f 100644 --- a/Tools/autotest/helicopter.py +++ b/Tools/autotest/helicopter.py @@ -5,9 +5,11 @@ ''' from __future__ import print_function + from arducopter import AutoTestCopter -from common import AutoTest -from common import NotAchievedException, AutoTestTimeoutException + +import vehicle_test_suite +from vehicle_test_suite import NotAchievedException, AutoTestTimeoutException from pymavlink import mavutil from pysim import vehicleinfo @@ -152,7 +154,13 @@ def takeoff(self, self.progress("Raising rotor speed") self.set_rc(8, 2000) self.progress("wait for rotor runup to complete") - self.wait_servo_channel_value(8, 1659, timeout=10) + if self.get_parameter("H_RSC_MODE") == 4: + self.context_collect('STATUSTEXT') + self.wait_statustext("Governor Engaged", check_context=True) + elif self.get_parameter("H_RSC_MODE") == 3: + self.wait_rpm(1, 1300, 1400) + else: + self.wait_servo_channel_value(8, 1659, timeout=10) # wait for motor runup self.delay_sim_time(20) @@ -161,7 +169,7 @@ def takeoff(self, self.user_takeoff(alt_min=alt_min) else: self.set_rc(3, takeoff_throttle) - self.wait_for_alt(alt_min=alt_min, timeout=timeout) + self.wait_altitude(alt_min-1, alt_min+5, relative=True, timeout=timeout) self.hover() self.progress("TAKEOFF COMPLETE") @@ -188,7 +196,7 @@ def FlyEachFrame(self): # to carry the path to the JSON. actual_model = model.split(":")[0] defaults = self.model_defaults_filepath(actual_model) - if type(defaults) != list: + if not isinstance(defaults, list): defaults = [defaults] self.customise_SITL_commandline( ["--defaults", ','.join(defaults), ], @@ -199,6 +207,18 @@ def FlyEachFrame(self): self.do_RTL() self.set_rc(8, 1000) + def governortest(self): + '''Test Heli Internal Throttle Curve and Governor''' + self.customise_SITL_commandline( + ["--defaults", ','.join(self.model_defaults_filepath('heli-gas')), ], + model="heli-gas", + wipe=True, + ) + self.set_parameter("H_RSC_MODE", 4) + self.takeoff(10) + self.do_RTL() + self.set_rc(8, 1000) + def hover(self): self.progress("Setting hover collective") self.set_rc(3, 1500) @@ -285,7 +305,7 @@ def StabilizeTakeOff(self): if abs(m.relative_alt) > 100: raise NotAchievedException("Took off prematurely") self.progress("Pushing throttle past half-way") - self.set_rc(3, 1600) + self.set_rc(3, 1650) self.progress("Monitoring takeoff") self.wait_altitude(6.9, 8, relative=True) @@ -383,20 +403,27 @@ def ManAutoRotation(self, timeout=600): self.context_collect('STATUSTEXT') self.change_mode('STABILIZE') self.progress("Triggering manual autorotation by disabling interlock") - self.set_rc(3, 1300) + self.set_rc(3, 1000) self.set_rc(8, 1000) - self.wait_servo_channel_value(8, 1200, timeout=3) + self.wait_servo_channel_value(8, 1199, timeout=3) self.progress("channel 8 set to autorotation window") + # wait to establish autorotation + self.delay_sim_time(2) + self.set_rc(8, 2000) self.wait_servo_channel_value(8, 1659, timeout=AROT_RAMP_TIME * 1.1) + # give time for engine to power up + self.set_rc(3, 1400) + self.delay_sim_time(2) + self.progress("in-flight power recovery") - self.set_rc(3, 1700) + self.set_rc(3, 1500) self.delay_sim_time(5) # initiate autorotation again - self.set_rc(3, 1200) + self.set_rc(3, 1000) self.set_rc(8, 1000) self.wait_statustext(r"SIM Hit ground at ([0-9.]+) m/s", @@ -531,11 +558,7 @@ def scurve_nasty_mission(self, target_system=1, target_component=1): copy.copy(wp5_by_three), self.mission_item_rtl(target_system=target_system, target_component=target_component), ]) - # renumber the items: - count = 0 - for item in ret: - item.seq = count - count += 1 + self.correct_wp_seq_numbers(ret) return ret def scurve_nasty_up_mission(self, target_system=1, target_component=1): @@ -608,11 +631,7 @@ def scurve_nasty_up_mission(self, target_system=1, target_component=1): wp7, self.mission_item_rtl(target_system=target_system, target_component=target_component), ]) - # renumber the items: - count = 0 - for item in ret: - item.seq = count - count += 1 + self.correct_wp_seq_numbers(ret) return ret def fly_mission_points(self, points): @@ -782,9 +801,50 @@ def TurbineStart(self, timeout=200): self.set_rc(8, 1000) self.disarm_vehicle() + def PIDNotches(self): + """Use dynamic harmonic notch to control motor noise.""" + self.progress("Flying with PID notches") + self.context_push() + + ex = None + try: + self.set_parameters({ + "FILT1_TYPE": 1, + "FILT2_TYPE": 1, + "AHRS_EKF_TYPE": 10, + "INS_LOG_BAT_MASK": 3, + "INS_LOG_BAT_OPT": 0, + "INS_GYRO_FILTER": 100, # set the gyro filter high so we can observe behaviour + "LOG_BITMASK": 65535, + "LOG_DISARMED": 0, + "SIM_VIB_FREQ_X": 120, # roll + "SIM_VIB_FREQ_Y": 120, # pitch + "SIM_VIB_FREQ_Z": 180, # yaw + "FILT1_NOTCH_FREQ": 120, + "FILT2_NOTCH_FREQ": 180, + "ATC_RAT_RLL_NEF": 1, + "ATC_RAT_PIT_NEF": 1, + "ATC_RAT_YAW_NEF": 2, + "SIM_GYR1_RND": 5, + }) + self.reboot_sitl() + + self.takeoff(10, mode="ALT_HOLD") + + freq, hover_throttle, peakdb1 = self.hover_and_check_matched_frequency_with_fft(5, 20, 350, reverse=True) + + except Exception as e: + self.print_exception_caught(e) + ex = e + + self.context_pop() + + if ex is not None: + raise ex + def tests(self): '''return list of all tests''' - ret = AutoTest.tests(self) + ret = vehicle_test_suite.TestSuite.tests(self) ret.extend([ self.AVCMission, self.RotorRunup, @@ -793,10 +853,12 @@ def tests(self): self.SplineWaypoint, self.AutoRotation, self.ManAutoRotation, + self.governortest, self.FlyEachFrame, self.AirspeedDrivers, self.TurbineStart, self.NastyMission, + self.PIDNotches, ]) return ret diff --git a/Tools/autotest/jsb_sim/runsim.py b/Tools/autotest/jsb_sim/runsim.py deleted file mode 100755 index 416a4e97697d0..0000000000000 --- a/Tools/autotest/jsb_sim/runsim.py +++ /dev/null @@ -1,385 +0,0 @@ -#!/usr/bin/env python -""" - Run a jsbsim model as a child process. -""" -from __future__ import print_function -import atexit -import errno -import fdpexpect -import math -import os -import select -import signal -import socket -import struct -import sys -import time - -import pexpect -from pymavlink import fgFDM - -from .. pysim import util - - -class control_state(object): - def __init__(self): - self.aileron = 0 - self.elevator = 0 - self.throttle = 0 - self.rudder = 0 - self.ground_height = 0 - -sitl_state = control_state() - - -def interpret_address(addrstr): - """Interpret a IP:port string.""" - a = addrstr.split(':') - a[1] = int(a[1]) - return tuple(a) - - -def jsb_set(variable, value): - """Set a JSBSim variable.""" - global jsb_console - jsb_console.send('set %s %s\r\n' % (variable, value)) - - -def setup_template(home): - """Setup aircraft/Rascal/reset.xml .""" - global opts - v = home.split(',') - if len(v) != 4: - print("home should be lat,lng,alt,hdg - '%s'" % home) - sys.exit(1) - latitude = float(v[0]) - longitude = float(v[1]) - altitude = float(v[2]) - heading = float(v[3]) - sitl_state.ground_height = altitude - template = os.path.join('aircraft', 'Rascal', 'reset_template.xml') - reset = os.path.join('aircraft', 'Rascal', 'reset.xml') - xml = open(template).read() % {'LATITUDE': str(latitude), - 'LONGITUDE': str(longitude), - 'HEADING': str(heading)} - open(reset, mode='w').write(xml) - print("Wrote %s" % reset) - - baseport = int(opts.simout.split(':')[1]) - - template = os.path.join('jsb_sim', 'fgout_template.xml') - out = os.path.join('jsb_sim', 'fgout.xml') - xml = open(template).read() % {'FGOUTPORT': str(baseport+3)} - open(out, mode='w').write(xml) - print("Wrote %s" % out) - - template = os.path.join('jsb_sim', 'rascal_test_template.xml') - out = os.path.join('jsb_sim', 'rascal_test.xml') - xml = open(template).read() % {'JSBCONSOLEPORT': str(baseport+4)} - open(out, mode='w').write(xml) - print("Wrote %s" % out) - - -def process_sitl_input(buf): - """Process control changes from SITL sim.""" - control = list(struct.unpack('<14H', buf)) - pwm = control[:11] - (speed, direction, turbulance) = control[11:] - - global wind - wind.speed = speed*0.01 - wind.direction = direction*0.01 - wind.turbulance = turbulance*0.01 - - aileron = (pwm[0]-1500)/500.0 - elevator = (pwm[1]-1500)/500.0 - throttle = (pwm[2]-1000)/1000.0 - if opts.revthr: - throttle = 1.0 - throttle - rudder = (pwm[3]-1500)/500.0 - - if opts.elevon: - # fake an elevon plane - ch1 = aileron - ch2 = elevator - aileron = (ch2-ch1)/2.0 - # the minus does away with the need for RC2_REVERSED=-1 - elevator = -(ch2+ch1)/2.0 - - if opts.vtail: - # fake an elevon plane - ch1 = elevator - ch2 = rudder - # this matches VTAIL_OUTPUT==2 - elevator = (ch2-ch1)/2.0 - rudder = (ch2+ch1)/2.0 - - buf = '' - if aileron != sitl_state.aileron: - buf += 'set fcs/aileron-cmd-norm %s\n' % aileron - sitl_state.aileron = aileron - if elevator != sitl_state.elevator: - buf += 'set fcs/elevator-cmd-norm %s\n' % elevator - sitl_state.elevator = elevator - if rudder != sitl_state.rudder: - buf += 'set fcs/rudder-cmd-norm %s\n' % rudder - sitl_state.rudder = rudder - if throttle != sitl_state.throttle: - buf += 'set fcs/throttle-cmd-norm %s\n' % throttle - sitl_state.throttle = throttle - buf += 'step\n' - global jsb_console - jsb_console.send(buf) - - -def update_wind(wind): - """Update wind simulation.""" - (speed, direction) = wind.current() - jsb_set('atmosphere/psiw-rad', math.radians(direction)) - jsb_set('atmosphere/wind-mag-fps', speed/0.3048) - - -def process_jsb_input(buf, simtime): - """Process FG FDM input from JSBSim.""" - global fdm, fg_out, sim_out - fdm.parse(buf) - if fg_out: - try: - agl = fdm.get('agl', units='meters') - fdm.set('altitude', agl+sitl_state.ground_height, units='meters') - fdm.set('rpm', sitl_state.throttle*1000) - fg_out.send(fdm.pack()) - except socket.error as e: - if e.errno not in [errno.ECONNREFUSED]: - raise - - timestamp = int(simtime*1.0e6) - - simbuf = struct.pack(' 0.1: - update_wind(wind) - last_wind_update = tnow - - if tnow - last_report > 3: - print("FPS %u asl=%.1f agl=%.1f roll=%.1f pitch=%.1f a=(%.2f %.2f %.2f) AR=%.1f" % ( - frame_count / (time.time() - last_report), - fdm.get('altitude', units='meters'), - fdm.get('agl', units='meters'), - fdm.get('phi', units='degrees'), - fdm.get('theta', units='degrees'), - fdm.get('A_X_pilot', units='mpss'), - fdm.get('A_Y_pilot', units='mpss'), - fdm.get('A_Z_pilot', units='mpss'), - achieved_rate)) - - frame_count = 0 - last_report = time.time() - - if new_frame: - now = time.time() - if now < last_wall_time + scaled_frame_time: - time.sleep(last_wall_time+scaled_frame_time - now) - now = time.time() - - if now > last_wall_time and now - last_wall_time < 0.1: - rate = 1.0/(now - last_wall_time) - achieved_rate = (0.98*achieved_rate) + (0.02*rate) - if achieved_rate < opts.rate*opts.speedup: - scaled_frame_time *= 0.999 - else: - scaled_frame_time *= 1.001 - - last_wall_time = now - - -def exit_handler(): - """Exit the sim.""" - print("running exit handler") - signal.signal(signal.SIGINT, signal.SIG_IGN) - signal.signal(signal.SIGTERM, signal.SIG_IGN) - # JSBSim really doesn't like to die ... - if getattr(jsb, 'pid', None) is not None: - os.kill(jsb.pid, signal.SIGKILL) - jsb_console.send('quit\n') - jsb.close(force=True) - util.pexpect_close_all() - sys.exit(1) - -signal.signal(signal.SIGINT, exit_handler) -signal.signal(signal.SIGTERM, exit_handler) - -try: - main_loop() -except Exception as ex: - print(ex) - exit_handler() - raise diff --git a/Tools/autotest/locations.txt b/Tools/autotest/locations.txt index ce3cf76fed012..3717b3613f989 100644 --- a/Tools/autotest/locations.txt +++ b/Tools/autotest/locations.txt @@ -56,6 +56,7 @@ Elvenes=68.871422,17.986690,17,256 Kawachi=35.879129,140.339683,7,0 SpringValley=-35.280252,149.005821,597.3,5 SpringValley2=-35.28240059,149.00542037,582,10 +SpringValley3=-35.28240515,149.00716878,579,12.6 Pyramid=29.9764,31.1339,0,0 AAMEastField=39.842288,-105.212928,1809,106 HachinoheMine=40.4539496,141.5419051,56,270 diff --git a/Tools/autotest/logger_metadata/emit_md.py b/Tools/autotest/logger_metadata/emit_md.py new file mode 100644 index 0000000000000..d6d1cb78778ba --- /dev/null +++ b/Tools/autotest/logger_metadata/emit_md.py @@ -0,0 +1,82 @@ +import os +import time +import emitter + +class MDEmitter(emitter.Emitter): + def preface(self): + if os.getenv('BRDOC') is not None: + now = time.strftime('%Y-%m-%dT%H:%M:%S%z') + now = now[:-2] + ':' + now[-2:] + return '\n'.join(( + '+++', + 'title = "Onboard Log Messages"', + 'description = "Message listing for DataFlash autopilot logs."', + f'date = {now}', + 'template = "docs/page.html"', + 'sort_by = "weight"', + 'weight = 30', + 'draft = false', + '[extra]', + 'toc = true', + 'top = false', + '+++\n', + '', + 'This is a list of log messages which may be present in DataFlash (`.bin`) ' + 'logs produced and stored onboard ArduSub vehicles (see [Log Parameters]' + '(../parameters/#log-parameters) for creation details). ' + 'It is possible to [add a new message]' + '(https://ardupilot.org/dev/docs/code-overview-adding-a-new-log-message.html) ' + 'by modifying the firmware.\n', + 'DataFlash logs can be downloaded and analysed ' + '[from a computer](http://www.ardusub.com/reference/data-logging.html#downloading) ' + 'or [through BlueOS]' + '(@/software/onboard/BlueOS-1.1/advanced-usage/index.md#log-browser).\n' + )) + + return """ + + +

Onboard Message Log Messages

+
+ +

This is a list of log messages which may be present in logs produced and stored onboard ArduPilot vehicles.

+ + +[toc exclude="Onboard Message Log Messages"] + +""" + def postface(self): + return "" + + def start(self): + self.fh = open("LogMessages.md", mode='w') + print(self.preface(), file=self.fh) + + def emit(self, doccos, enumerations=None): + self.start() + for docco in doccos: + print(f'## {docco.name}', file=self.fh) + desc = '' + if docco.description is not None: + desc += docco.description + if docco.url is not None: + desc += f' ([Read more...]({docco.url}))' + print(desc, file=self.fh) + print("\n|FieldName|Description|\n|---|---|", file=self.fh) + for f in docco.fields_order: + if "description" in docco.fields[f]: + fdesc = docco.fields[f]["description"] + else: + fdesc = "" + print(f'|{f}|{fdesc}|', file=self.fh) + print("", file=self.fh) + self.stop() + + def stop(self): + print(self.postface(), file=self.fh) + self.fh.close() diff --git a/Tools/autotest/logger_metadata/enum_parse.py b/Tools/autotest/logger_metadata/enum_parse.py index 24ff2106653a0..50c026a21651f 100755 --- a/Tools/autotest/logger_metadata/enum_parse.py +++ b/Tools/autotest/logger_metadata/enum_parse.py @@ -72,6 +72,11 @@ def match_enum_line(self, line): # Match: " FRED = 17, // optional comment" m = re.match("\s*([A-Z0-9_a-z]+) *= *(\w+) *,?(?: *// *(.*) *)?$", line) + if m is not None: + return (None, None, None) + # Match: " FRED = FOO(17), // optional comment" + m = re.match("\s*([A-Z0-9_a-z]+) *= *(\w+) *\\( *(\w+) *\\) *,?(?: *// *(.*) *)?$", + line) if m is not None: return (None, None, None) diff --git a/Tools/autotest/logger_metadata/parse.py b/Tools/autotest/logger_metadata/parse.py index f7d4bd9e71569..ea58198ea2bcc 100755 --- a/Tools/autotest/logger_metadata/parse.py +++ b/Tools/autotest/logger_metadata/parse.py @@ -11,6 +11,7 @@ import emit_html import emit_rst import emit_xml +import emit_md import enum_parse from enum_parse import EnumDocco @@ -50,6 +51,7 @@ def __init__(self, vehicle): emit_html.HTMLEmitter(), emit_rst.RSTEmitter(), emit_xml.XMLEmitter(), + emit_md.MDEmitter(), ] class Docco(object): @@ -184,7 +186,7 @@ def emit_output(self): # expand things like PIDR,PIDQ,PIDA into multiple doccos new_doccos = [] for docco in self.doccos: - if type(docco.name) == list: + if isinstance(docco.name, list): for name in docco.name: tmpdocco = copy.copy(docco) tmpdocco.name = name diff --git a/Tools/autotest/param_metadata/param.py b/Tools/autotest/param_metadata/param.py index 8120e0ad77108..3cc5266a47ecc 100644 --- a/Tools/autotest/param_metadata/param.py +++ b/Tools/autotest/param_metadata/param.py @@ -4,6 +4,9 @@ def __init__(self, name, real_path): self.name = name self.real_path = real_path + def change_name(self, name): + self.name = name + class Vehicle(object): def __init__(self, name, path, reference=None): @@ -47,6 +50,7 @@ def has_param(self, pname): 'Volatile', 'ReadOnly', 'Calibration', + 'Vector3Parameter', ] # Follow SI units conventions from: @@ -124,6 +128,8 @@ def has_param(self, pname): 'RPM' : 'Revolutions Per Minute', 'kg/m/m' : 'kilograms per square meter', # metre is the SI unit name, meter is the american spelling of it 'kg/m/m/m': 'kilograms per cubic meter', + 'litres' : 'litres', + 'Ohm' : 'Ohm', } required_param_fields = [ diff --git a/Tools/autotest/param_metadata/param_parse.py b/Tools/autotest/param_metadata/param_parse.py index 057eaf00077d2..fdb1465e29cc4 100755 --- a/Tools/autotest/param_metadata/param_parse.py +++ b/Tools/autotest/param_metadata/param_parse.py @@ -12,7 +12,6 @@ import os import re import sys -import glob from argparse import ArgumentParser from param import (Library, Parameter, Vehicle, known_group_fields, @@ -91,27 +90,31 @@ def debug(str_to_print): def lua_applets(): '''return list of Library objects for lua applets and drivers''' lua_lib = Library("", reference="Lua Script", not_rst=True, check_duplicates=True) - patterns = ["libraries/AP_Scripting/applets/*.lua", "libraries/AP_Scripting/drivers/*.lua"] + dirs = ["libraries/AP_Scripting/applets", "libraries/AP_Scripting/drivers"] paths = [] - for p in patterns: - debug("Adding lua paths %s" % p) - luafiles = glob.glob(os.path.join(apm_path, p)) - for f in luafiles: - # the library is expected to have the path as a relative path from within - # a vehicle directory - f = f.replace(apm_path, "../") - paths.append(f) + for d in dirs: + for root, dirs, files in os.walk(os.path.join(apm_path, d)): + for file in files: + if not file.endswith(".lua"): + continue + f = os.path.join(root, file) + debug("Adding lua path %s" % f) + # the library is expected to have the path as a relative path from within + # a vehicle directory + f = f.replace(apm_path, "../") + paths.append(f) setattr(lua_lib, "Path", ','.join(paths)) return lua_lib libraries = [] -# AP_Vehicle also has parameters rooted at "", but isn't referenced -# from the vehicle in any way: -ap_vehicle_lib = Library("") # the "" is tacked onto the front of param name -setattr(ap_vehicle_lib, "Path", os.path.join('..', 'libraries', 'AP_Vehicle', 'AP_Vehicle.cpp')) -libraries.append(ap_vehicle_lib) +if args.vehicle != "AP_Periph": + # AP_Vehicle also has parameters rooted at "", but isn't referenced + # from the vehicle in any way: + ap_vehicle_lib = Library("", reference="VEHICLE") # the "" is tacked onto the front of param name + setattr(ap_vehicle_lib, "Path", os.path.join('..', 'libraries', 'AP_Vehicle', 'AP_Vehicle.cpp')) + libraries.append(ap_vehicle_lib) libraries.append(lua_applets()) @@ -300,6 +303,7 @@ def process_library(vehicle, library, pathprefix=None): # a parameter is considered to be vehicle-specific if # there does not exist a Values: or Values{VehicleName} # for that vehicle but @Values{OtherVehicle} exists. + seen_values_or_bitmask_for_this_vehicle = False seen_values_or_bitmask_for_other_vehicle = False for field in fields: only_for_vehicles = field[1].split(",") @@ -313,11 +317,26 @@ def process_library(vehicle, library, pathprefix=None): error("tagged param: unknown parameter metadata field '%s'" % field[0]) continue if vehicle.name not in only_for_vehicles: - if len(only_for_vehicles) and field[0] in ['Values', 'Bitmask']: + if len(only_for_vehicles) and field[0] in documentation_tags_which_are_comma_separated_nv_pairs: seen_values_or_bitmask_for_other_vehicle = True continue + + append_value = False + if field[0] in documentation_tags_which_are_comma_separated_nv_pairs: + if vehicle.name in only_for_vehicles: + if seen_values_or_bitmask_for_this_vehicle: + append_value = hasattr(p, field[0]) + seen_values_or_bitmask_for_this_vehicle = True + else: + if seen_values_or_bitmask_for_this_vehicle: + continue + append_value = hasattr(p, field[0]) + value = re.sub('@PREFIX@', library.name, field[2]) - setattr(p, field[0], value) + if append_value: + setattr(p, field[0], getattr(p, field[0]) + ',' + value) + else: + setattr(p, field[0], value) if (getattr(p, 'Values', None) is not None and getattr(p, 'Bitmask', None) is not None): @@ -334,11 +353,25 @@ def process_library(vehicle, library, pathprefix=None): # applicable for this vehicle. continue - p.path = path # Add path. Later deleted - only used for duplicates - if library.check_duplicates and library.has_param(p.name): - error("Duplicate parameter %s in %s" % (p.name, library.name)) - continue - library.params.append(p) + if getattr(p, 'Vector3Parameter', None) is not None: + params_to_add = [] + for axis in 'X', 'Y', 'Z': + new_p = copy.copy(p) + new_p.change_name(p.name + "_" + axis) + for a in ["Description"]: + if hasattr(new_p, a): + current = getattr(new_p, a) + setattr(new_p, a, current + " (%s-axis)" % axis) + params_to_add.append(new_p) + else: + params_to_add = [p] + + for p in params_to_add: + p.path = path # Add path. Later deleted - only used for duplicates + if library.check_duplicates and library.has_param(p.name): + error("Duplicate parameter %s in %s" % (p.name, library.name)) + continue + library.params.append(p) group_matches = prog_groups.findall(p_text) debug("Found %u groups" % len(group_matches)) @@ -416,6 +449,9 @@ def clean_param(param): new_valueList.append(":".join([start, end])) param.Values = ",".join(new_valueList) + if hasattr(param, "Vector3Parameter"): + delattr(param, "Vector3Parameter") + def do_copy_values(vehicle_params, libraries, param): if not hasattr(param, "CopyValuesFrom"): @@ -423,6 +459,10 @@ def do_copy_values(vehicle_params, libraries, param): # so go and find the values... wanted_name = param.CopyValuesFrom + if hasattr(param, 'Vector3Parameter'): + suffix = param.name[-2:] + wanted_name += suffix + del param.CopyValuesFrom for x in vehicle_params: name = x.name @@ -452,6 +492,11 @@ def do_copy_fields(vehicle_params, libraries, param): # so go and find the values... wanted_name = param.CopyFieldsFrom del param.CopyFieldsFrom + + if hasattr(param, 'Vector3Parameter'): + suffix = param.name[-2:] + wanted_name += suffix + for x in vehicle_params: name = x.name (v, name) = name.split(":") @@ -516,7 +561,7 @@ def validate(param, is_library=False): i = i.replace(" ", "") values.append(i.partition(":")[0]) if (len(values) != len(set(values))): - error("Duplicate values found") + error("Duplicate values found" + str({x for x in values if values.count(x) > 1})) # Validate units if (hasattr(param, "Units")): if (param.__dict__["Units"] != "") and (param.__dict__["Units"] not in known_units): diff --git a/Tools/autotest/pysim/aircraft.py b/Tools/autotest/pysim/aircraft.py deleted file mode 100644 index f398164873313..0000000000000 --- a/Tools/autotest/pysim/aircraft.py +++ /dev/null @@ -1,112 +0,0 @@ -import math -import random -import time -import util - -from pymavlink.rotmat import Vector3, Matrix3 - - -class Aircraft(object): - """A basic aircraft class.""" - def __init__(self): - self.home_latitude = 0 - self.home_longitude = 0 - self.home_altitude = 0 - self.ground_level = 0 - self.frame_height = 0.0 - - self.latitude = self.home_latitude - self.longitude = self.home_longitude - self.altitude = self.home_altitude - - self.dcm = Matrix3() - - # rotation rate in body frame - self.gyro = Vector3(0, 0, 0) # rad/s - - self.velocity = Vector3(0, 0, 0) # m/s, North, East, Down - self.position = Vector3(0, 0, 0) # m North, East, Down - self.mass = 0.0 - self.update_frequency = 50 # in Hz - self.gravity = 9.80665 # m/s/s - self.accelerometer = Vector3(0, 0, -self.gravity) - - self.wind = util.Wind('0,0,0') - self.time_base = time.time() - self.time_now = self.time_base + 100*1.0e-6 - - self.gyro_noise = math.radians(0.1) - self.accel_noise = 0.3 - - def on_ground(self, position=None): - """Return true if we are on the ground.""" - if position is None: - position = self.position - return (-position.z) + self.home_altitude <= self.ground_level + self.frame_height - - def update_position(self): - """Update lat/lon/alt from position.""" - - bearing = math.degrees(math.atan2(self.position.y, self.position.x)) - distance = math.sqrt(self.position.x**2 + self.position.y**2) - - (self.latitude, self.longitude) = util.gps_newpos(self.home_latitude, self.home_longitude, - bearing, distance) - - self.altitude = self.home_altitude - self.position.z - - velocity_body = self.dcm.transposed() * self.velocity - - self.accelerometer = self.accel_body.copy() - - def set_yaw_degrees(self, yaw_degrees): - """Rotate to the given yaw.""" - (roll, pitch, yaw) = self.dcm.to_euler() - yaw = math.radians(yaw_degrees) - self.dcm.from_euler(roll, pitch, yaw) - - def time_advance(self, deltat): - """Advance time by deltat in seconds.""" - self.time_now += deltat - - def setup_frame_time(self, rate, speedup): - """Setup frame_time calculation.""" - self.rate = rate - self.speedup = speedup - self.frame_time = 1.0/rate - self.scaled_frame_time = self.frame_time/speedup - self.last_wall_time = time.time() - self.achieved_rate = rate - - def adjust_frame_time(self, rate): - """Adjust frame_time calculation.""" - self.rate = rate - self.frame_time = 1.0/rate - self.scaled_frame_time = self.frame_time/self.speedup - - def sync_frame_time(self): - """Try to synchronise simulation time with wall clock time, taking - into account desired speedup.""" - now = time.time() - if now < self.last_wall_time + self.scaled_frame_time: - time.sleep(self.last_wall_time+self.scaled_frame_time - now) - now = time.time() - - if now > self.last_wall_time and now - self.last_wall_time < 0.1: - rate = 1.0/(now - self.last_wall_time) - self.achieved_rate = (0.98*self.achieved_rate) + (0.02*rate) - if self.achieved_rate < self.rate*self.speedup: - self.scaled_frame_time *= 0.999 - else: - self.scaled_frame_time *= 1.001 - - self.last_wall_time = now - - def add_noise(self, throttle): - """Add noise based on throttle level (from 0..1).""" - self.gyro += Vector3(random.gauss(0, 1), - random.gauss(0, 1), - random.gauss(0, 1)) * throttle * self.gyro_noise - self.accel_body += Vector3(random.gauss(0, 1), - random.gauss(0, 1), - random.gauss(0, 1)) * throttle * self.accel_noise diff --git a/Tools/autotest/pysim/testwind.py b/Tools/autotest/pysim/testwind.py deleted file mode 100755 index c76f68f3fc949..0000000000000 --- a/Tools/autotest/pysim/testwind.py +++ /dev/null @@ -1,20 +0,0 @@ -#!/usr/bin/env python -""" -simple test of wind generation code -""" -from __future__ import print_function -import time -import util -from pymavlink.rotmat import Vector3 - -wind = util.Wind('7,90,0.1') - -t0 = time.time() -velocity = Vector3(0, 0, 0) - -t = 0 -deltat = 0.01 - -while t < 60: - print("%.4f %f" % (t, wind.drag(velocity, deltat=deltat).length())) - t += deltat diff --git a/Tools/autotest/pysim/util.py b/Tools/autotest/pysim/util.py index 68b278138cdc5..beed41e777c75 100644 --- a/Tools/autotest/pysim/util.py +++ b/Tools/autotest/pysim/util.py @@ -7,7 +7,6 @@ import atexit import math import os -import random import re import shlex import signal @@ -15,11 +14,9 @@ import sys import tempfile import time -from math import acos, atan2, cos, pi, sqrt -import pexpect -from pymavlink.rotmat import Vector3, Matrix3 +import pexpect if sys.version_info[0] >= 3: ENCODING = 'ascii' @@ -112,12 +109,13 @@ def waf_configure(board, coverage=False, ekf_single=False, postype_single=False, - sitl_32bit=False, + force_32bit=False, extra_args=[], extra_hwdef=None, ubsan=False, ubsan_abort=False, num_aux_imus=0, + dronecan_tests=False, extra_defines={}): cmd_configure = [relwaf(), "configure", "--board", board] if debug: @@ -130,14 +128,16 @@ def waf_configure(board, cmd_configure.append('--ekf-single') if postype_single: cmd_configure.append('--postype-single') - if sitl_32bit: - cmd_configure.append('--sitl-32bit') + if force_32bit: + cmd_configure.append('--force-32bit') if ubsan: cmd_configure.append('--ubsan') if ubsan_abort: cmd_configure.append('--ubsan-abort') if num_aux_imus > 0: cmd_configure.append('--num-aux-imus=%u' % num_aux_imus) + if dronecan_tests: + cmd_configure.append('--enable-dronecan-tests') if extra_hwdef is not None: cmd_configure.extend(['--extra-hwdef', extra_hwdef]) for nv in extra_defines.items(): @@ -174,10 +174,11 @@ def build_SITL( j=None, math_check_indexes=False, postype_single=False, - sitl_32bit=False, + force_32bit=False, ubsan=False, ubsan_abort=False, num_aux_imus=0, + dronecan_tests=False, ): # first configure @@ -189,11 +190,12 @@ def build_SITL( ekf_single=ekf_single, postype_single=postype_single, coverage=coverage, - sitl_32bit=sitl_32bit, + force_32bit=force_32bit, ubsan=ubsan, ubsan_abort=ubsan_abort, extra_defines=extra_defines, num_aux_imus=num_aux_imus, + dronecan_tests=dronecan_tests, extra_args=extra_configure_args,) # then clean @@ -209,7 +211,8 @@ def build_SITL( def build_examples(board, j=None, debug=False, clean=False, configure=True, math_check_indexes=False, coverage=False, - ekf_single=False, postype_single=False, sitl_32bit=False, ubsan=False, ubsan_abort=False, num_aux_imus=0, + ekf_single=False, postype_single=False, force_32bit=False, ubsan=False, ubsan_abort=False, + num_aux_imus=0, dronecan_tests=False, extra_configure_args=[]): # first configure if configure: @@ -220,10 +223,11 @@ def build_examples(board, j=None, debug=False, clean=False, configure=True, math ekf_single=ekf_single, postype_single=postype_single, coverage=coverage, - sitl_32bit=sitl_32bit, + force_32bit=force_32bit, ubsan=ubsan, ubsan_abort=ubsan_abort, - extra_args=extra_configure_args) + extra_args=extra_configure_args, + dronecan_tests=dronecan_tests) # then clean if clean: @@ -258,10 +262,11 @@ def build_tests(board, coverage=False, ekf_single=False, postype_single=False, - sitl_32bit=False, + force_32bit=False, ubsan=False, ubsan_abort=False, num_aux_imus=0, + dronecan_tests=False, extra_configure_args=[]): # first configure @@ -273,10 +278,12 @@ def build_tests(board, ekf_single=ekf_single, postype_single=postype_single, coverage=coverage, - sitl_32bit=sitl_32bit, + force_32bit=force_32bit, ubsan=ubsan, ubsan_abort=ubsan_abort, - extra_args=extra_configure_args) + num_aux_imus=num_aux_imus, + dronecan_tests=dronecan_tests, + extra_args=extra_configure_args,) # then clean if clean: @@ -393,6 +400,34 @@ def isalive(self): return True +class PSpawnStdPrettyPrinter(object): + '''a fake filehandle-like object which prefixes a string to all lines + before printing to stdout/stderr. To be used to pass to + pexpect.spawn's logfile argument + ''' + def __init__(self, output=sys.stdout, prefix="stdout"): + self.output = output + self.prefix = prefix + self.buffer = "" + + def close(self): + self.print_prefixed_line(self.buffer) + + def write(self, data): + self.buffer += data + lines = self.buffer.split("\n") + self.buffer = lines[-1] + lines.pop() + for line in lines: + self.print_prefixed_line(line) + + def print_prefixed_line(self, line): + print("%s: %s" % (self.prefix, line), file=self.output) + + def flush(self): + pass + + def start_SITL(binary, valgrind=False, callgrind=False, @@ -403,6 +438,7 @@ def start_SITL(binary, home=None, model=None, speedup=1, + sim_rate_hz=None, defaults_filepath=None, unhide_parameters=False, gdbserver=False, @@ -411,7 +447,8 @@ def start_SITL(binary, customisations=[], lldb=False, enable_fgview_output=False, - supplementary=False): + supplementary=False, + stdout_prefix=None): if model is None and not supplementary: raise ValueError("model must not be None") @@ -469,7 +506,7 @@ def start_SITL(binary, '-d', '-m', '-S', 'ardupilot-gdb', - 'gdb', '-x', '/tmp/x.gdb', binary, '--args']) + 'gdb', '--cd', os.getcwd(), '-x', '/tmp/x.gdb', binary, '--args']) elif lldb: f = open("/tmp/x.lldb", "w") for breakingpoint in breakpoints: @@ -495,15 +532,10 @@ def start_SITL(binary, if home is not None: cmd.extend(['--home', home]) cmd.extend(['--model', model]) - if speedup != 1: + if speedup is not None and speedup != 1: cmd.extend(['--speedup', str(speedup)]) - if defaults_filepath is not None: - if type(defaults_filepath) == list: - defaults = [reltopdir(path) for path in defaults_filepath] - if len(defaults): - cmd.extend(['--defaults', ",".join(defaults)]) - else: - cmd.extend(['--defaults', reltopdir(defaults_filepath)]) + if sim_rate_hz is not None: + cmd.extend(['--rate', str(sim_rate_hz)]) if unhide_parameters: cmd.extend(['--unhide-groups']) # somewhere for MAVProxy to connect to: @@ -511,8 +543,21 @@ def start_SITL(binary, if not enable_fgview_output: cmd.append("--disable-fgview") + if defaults_filepath is not None: + if isinstance(defaults_filepath, list): + defaults = [reltopdir(path) for path in defaults_filepath] + if len(defaults): + cmd.extend(['--defaults', ",".join(defaults)]) + else: + cmd.extend(['--defaults', reltopdir(defaults_filepath)]) + cmd.extend(customisations) + pexpect_logfile_prefix = stdout_prefix + if pexpect_logfile_prefix is None: + pexpect_logfile_prefix = os.path.basename(binary) + pexpect_logfile = PSpawnStdPrettyPrinter(prefix=pexpect_logfile_prefix) + if (gdb or lldb) and sys.platform == "darwin" and os.getenv('DISPLAY'): global windowID # on MacOS record the window IDs so we can close them later @@ -550,7 +595,7 @@ def start_SITL(binary, # AP gets a redirect-stdout-to-filehandle option. So, in the # meantime, return a dummy: return pexpect.spawn("true", ["true"], - logfile=sys.stdout, + logfile=pexpect_logfile, encoding=ENCODING, timeout=5) else: @@ -558,10 +603,8 @@ def start_SITL(binary, first = cmd[0] rest = cmd[1:] - child = pexpect.spawn(first, rest, logfile=sys.stdout, encoding=ENCODING, timeout=5) + child = pexpect.spawn(first, rest, logfile=pexpect_logfile, encoding=ENCODING, timeout=5) pexpect_autoclose(child) - # give time for parameters to properly setup - time.sleep(3) if gdb or lldb: # if we run GDB we do so in an xterm. "Waiting for # connection" is never going to appear on xterm's output. @@ -593,11 +636,15 @@ def MAVProxy_version(): def start_MAVProxy_SITL(atype, aircraft=None, setup=False, - master='tcp:127.0.0.1:5762', + master=None, options=[], + sitl_rcin_port=5501, pexpect_timeout=60, logfile=sys.stdout): """Launch mavproxy connected to a SITL instance.""" + if master is None: + raise ValueError("Expected a master") + local_mp_modules_dir = os.path.abspath( os.path.join(__file__, '..', '..', '..', 'mavproxy_modules')) env = dict(os.environ) @@ -606,11 +653,11 @@ def start_MAVProxy_SITL(atype, if old is not None: env['PYTHONPATH'] += os.path.pathsep + old - import pexpect global close_list cmd = [] cmd.append(mavproxy_cmd()) cmd.extend(['--master', master]) + cmd.extend(['--sitl', "localhost:%u" % sitl_rcin_port]) if setup: cmd.append('--setup') if aircraft is None: @@ -631,8 +678,6 @@ def start_MAVProxy_SITL(atype, def expect_setup_callback(e, callback): """Setup a callback that is called once a second while waiting for patterns.""" - import pexpect - def _expect_callback(pattern, timeout=e.timeout): tstart = time.time() while time.time() < tstart + timeout: @@ -697,51 +742,6 @@ def check_parent(parent_pid=None): sys.exit(1) -def EarthRatesToBodyRates(dcm, earth_rates): - """Convert the angular velocities from earth frame to - body frame. Thanks to James Goppert for the formula - - all inputs and outputs are in radians - - returns a gyro vector in body frame, in rad/s . - """ - from math import sin, cos - - (phi, theta, psi) = dcm.to_euler() - phiDot = earth_rates.x - thetaDot = earth_rates.y - psiDot = earth_rates.z - - p = phiDot - psiDot * sin(theta) - q = cos(phi) * thetaDot + sin(phi) * psiDot * cos(theta) - r = cos(phi) * psiDot * cos(theta) - sin(phi) * thetaDot - return Vector3(p, q, r) - - -def BodyRatesToEarthRates(dcm, gyro): - """Convert the angular velocities from body frame to - earth frame. - - all inputs and outputs are in radians/s - - returns a earth rate vector. - """ - from math import sin, cos, tan, fabs - - p = gyro.x - q = gyro.y - r = gyro.z - - (phi, theta, psi) = dcm.to_euler() - - phiDot = p + tan(theta) * (q * sin(phi) + r * cos(phi)) - thetaDot = q * cos(phi) - r * sin(phi) - if fabs(cos(theta)) < 1.0e-20: - theta += 1.0e-10 - psiDot = (q * sin(phi) + r * cos(phi)) / cos(theta) - return Vector3(phiDot, thetaDot, psiDot) - - def gps_newpos(lat, lon, bearing, distance): """Extrapolate latitude/longitude given a heading and distance thanks to http://www.movable-type.co.uk/scripts/latlong.html . @@ -792,132 +792,6 @@ def gps_bearing(lat1, lon1, lat2, lon2): return bearing -class Wind(object): - """A wind generation object.""" - def __init__(self, windstring, cross_section=0.1): - a = windstring.split(',') - if len(a) != 3: - raise RuntimeError("Expected wind in speed,direction,turbulance form, not %s" % windstring) - self.speed = float(a[0]) # m/s - self.direction = float(a[1]) # direction the wind is going in - self.turbulance = float(a[2]) # turbulance factor (standard deviation) - - # the cross-section of the aircraft to wind. This is multiplied by the - # difference in the wind and the velocity of the aircraft to give the acceleration - self.cross_section = cross_section - - # the time constant for the turbulance - the average period of the - # changes over time - self.turbulance_time_constant = 5.0 - - # wind time record - self.tlast = time.time() - - # initial turbulance multiplier - self.turbulance_mul = 1.0 - - def current(self, deltat=None): - """Return current wind speed and direction as a tuple - speed is in m/s, direction in degrees.""" - if deltat is None: - tnow = time.time() - deltat = tnow - self.tlast - self.tlast = tnow - - # update turbulance random walk - w_delta = math.sqrt(deltat) * (1.0 - random.gauss(1.0, self.turbulance)) - w_delta -= (self.turbulance_mul - 1.0) * (deltat / self.turbulance_time_constant) - self.turbulance_mul += w_delta - speed = self.speed * math.fabs(self.turbulance_mul) - return speed, self.direction - - # Calculate drag. - def drag(self, velocity, deltat=None): - """Return current wind force in Earth frame. The velocity parameter is - a Vector3 of the current velocity of the aircraft in earth frame, m/s .""" - from math import radians - - # (m/s, degrees) : wind vector as a magnitude and angle. - (speed, direction) = self.current(deltat=deltat) - # speed = self.speed - # direction = self.direction - - # Get the wind vector. - w = toVec(speed, radians(direction)) - - obj_speed = velocity.length() - - # Compute the angle between the object vector and wind vector by taking - # the dot product and dividing by the magnitudes. - d = w.length() * obj_speed - if d == 0: - alpha = 0 - else: - alpha = acos((w * velocity) / d) - - # Get the relative wind speed and angle from the object. Note that the - # relative wind speed includes the velocity of the object; i.e., there - # is a headwind equivalent to the object's speed even if there is no - # absolute wind. - (rel_speed, beta) = apparent_wind(speed, obj_speed, alpha) - - # Return the vector of the relative wind, relative to the coordinate - # system. - relWindVec = toVec(rel_speed, beta + atan2(velocity.y, velocity.x)) - - # Combine them to get the acceleration vector. - return Vector3(acc(relWindVec.x, drag_force(self, relWindVec.x)), acc(relWindVec.y, drag_force(self, relWindVec.y)), 0) - - -def apparent_wind(wind_sp, obj_speed, alpha): - """http://en.wikipedia.org/wiki/Apparent_wind - - Returns apparent wind speed and angle of apparent wind. Alpha is the angle - between the object and the true wind. alpha of 0 rads is a headwind; pi a - tailwind. Speeds should always be positive.""" - delta = wind_sp * cos(alpha) - x = wind_sp**2 + obj_speed**2 + 2 * obj_speed * delta - rel_speed = sqrt(x) - if rel_speed == 0: - beta = pi - else: - beta = acos((delta + obj_speed) / rel_speed) - - return rel_speed, beta - - -def drag_force(wind, sp): - """See http://en.wikipedia.org/wiki/Drag_equation - - Drag equation is F(a) = cl * p/2 * v^2 * a, where cl : drag coefficient - (let's assume it's low, .e.g., 0.2), p : density of air (assume about 1 - kg/m^3, the density just over 1500m elevation), v : relative speed of wind - (to the body), a : area acted on (this is captured by the cross_section - parameter). - - So then we have - F(a) = 0.2 * 1/2 * v^2 * cross_section = 0.1 * v^2 * cross_section.""" - return (sp**2.0) * 0.1 * wind.cross_section - - -def acc(val, mag): - """ Function to make the force vector. relWindVec is the direction the apparent - wind comes *from*. We want to compute the accleration vector in the direction - the wind blows to.""" - if val == 0: - return mag - else: - return (val / abs(val)) * (0 - mag) - - -def toVec(magnitude, angle): - """Converts a magnitude and angle (radians) to a vector in the xy plane.""" - v = Vector3(magnitude, 0, 0) - m = Matrix3() - m.from_euler(0, 0, angle) - return m.transposed() * v - - def constrain(value, minv, maxv): """Constrain a value to a range.""" if value < minv: @@ -941,6 +815,14 @@ def load_local_module(fname): return ret +def get_git_hash(short=False): + short_v = "--short=8 " if short else "" + githash = run_cmd(f'git rev-parse {short_v}HEAD', output=True, directory=reltopdir('.')).strip() + if sys.version_info.major >= 3: + githash = githash.decode('utf-8') + return githash + + if __name__ == "__main__": import doctest doctest.testmod() diff --git a/Tools/autotest/pysim/vehicleinfo.py b/Tools/autotest/pysim/vehicleinfo.py index fbcb2b54df0d6..9709dcd917e0d 100644 --- a/Tools/autotest/pysim/vehicleinfo.py +++ b/Tools/autotest/pysim/vehicleinfo.py @@ -149,6 +149,11 @@ def __init__(self): "waf_target": "bin/arducopter-heli", "default_params_filename": "default_params/copter-heli.parm", }, + "heli-gas": { + "waf_target": "bin/arducopter-heli", + "default_params_filename": ["default_params/copter-heli.parm", + "default_params/copter-heli-gas.parm"], + }, "heli-dual": { "waf_target": "bin/arducopter-heli", "default_params_filename": ["default_params/copter-heli.parm", @@ -183,6 +188,11 @@ def __init__(self): "default_params_filename": ["default_params/copter.parm", "models/Callisto.param"], }, + "quad-can": { + "waf_target": "bin/arducopter", + "default_params_filename": ["default_params/copter.parm", "default_params/quad-can.parm"], + "periph_params_filename": ["default_params/periph.parm", "default_params/quad-periph.parm"], + }, }, }, "Helicopter": { @@ -192,6 +202,11 @@ def __init__(self): "waf_target": "bin/arducopter-heli", "default_params_filename": "default_params/copter-heli.parm", }, + "heli-gas": { + "waf_target": "bin/arducopter-heli", + "default_params_filename": ["default_params/copter-heli.parm", + "default_params/copter-heli-gas.parm"], + }, "heli-dual": { "waf_target": "bin/arducopter-heli", "default_params_filename": ["default_params/copter-heli.parm", @@ -250,6 +265,11 @@ def __init__(self): "waf_target": "bin/arduplane", "default_params_filename": ["default_params/quadplane.parm", "default_params/plane-ice.parm", "default_params/quadplane-ice.parm"], }, + "quadplane-can": { + "waf_target": "bin/arduplane", + "default_params_filename": ["default_params/quadplane.parm", "default_params/quadplane-can.parm"], + "periph_params_filename": ["default_params/periph.parm", "default_params/quadplane-periph.parm"], + }, "firefly": { "waf_target": "bin/arduplane", "default_params_filename": "default_params/firefly.parm", @@ -409,6 +429,7 @@ def __init__(self): "gps": { "configure_target": "sitl_periph_gps", "waf_target": "bin/AP_Periph", + "default_params_filename": "default_params/periph.parm", }, } }, diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 9168130492cc1..ebb0c2f9cf859 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -11,9 +11,11 @@ import math from pymavlink import mavutil +from pymavlink.rotmat import Vector3 -from common import AutoTest -from common import AutoTestTimeoutException, NotAchievedException, PreconditionFailedException +import vehicle_test_suite +from vehicle_test_suite import Test +from vehicle_test_suite import AutoTestTimeoutException, NotAchievedException, PreconditionFailedException import operator @@ -24,7 +26,7 @@ SITL_START_LOCATION = mavutil.location(-27.274439, 151.290064, 343, 8.7) -class AutoTestQuadPlane(AutoTest): +class AutoTestQuadPlane(vehicle_test_suite.TestSuite): @staticmethod def get_not_armable_mode_list(): @@ -169,6 +171,10 @@ def AirMode(self): self.wait_ready_to_arm() self.start_subtest("Verify that arming with switch does not spin motors in other modes") + # disable compass magnetic field arming check that is triggered by the simulated lean of vehicle + # this is required because adjusting the AHRS_TRIM values only affects the IMU and not external compasses + arming_magthresh = self.get_parameter("ARMING_MAGTHRESH") + self.set_parameter("ARMING_MAGTHRESH", 0) # introduce a large attitude error to verify that stabilization is not active ahrs_trim_x = self.get_parameter("AHRS_TRIM_X") self.set_parameter("AHRS_TRIM_X", math.radians(-60)) @@ -207,8 +213,9 @@ def AirMode(self): self.progress("Waiting for Motor1 to stop") self.wait_servo_channel_value(5, min_pwm, comparator=operator.le) self.wait_ready_to_arm() - # remove attitude error + # remove attitude error and reinstance compass arming check self.set_parameter("AHRS_TRIM_X", ahrs_trim_x) + self.set_parameter("ARMING_MAGTHRESH", arming_magthresh) self.start_subtest("verify that AIRMODE auxswitch turns airmode on/off while armed") """set RC7_OPTION to AIRMODE""" @@ -345,60 +352,107 @@ def EXTENDED_SYS_STATE_SLT(self): self.change_mode("QLAND") self.wait_altitude(0, 2, relative=True, timeout=60) self.wait_extended_sys_state(mavutil.mavlink.MAV_VTOL_STATE_MC, - mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND) + mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND, + timeout=30) self.mav.motors_disarmed_wait() def EXTENDED_SYS_STATE(self): '''Check extended sys state works''' self.EXTENDED_SYS_STATE_SLT() - def fly_qautotune(self): - self.change_mode("QHOVER") - self.wait_ready_to_arm() - self.arm_vehicle() - self.set_rc(3, 1800) - self.wait_altitude(30, - 40, - relative=True, - timeout=30) + def QAUTOTUNE(self): + '''test Plane QAutoTune mode''' + + # this is a list of all parameters modified by QAUTOTUNE. Set + # them so that when the context is popped we get the original + # values back: + parameter_values = self.get_parameters([ + "Q_A_RAT_RLL_P", + "Q_A_RAT_RLL_I", + "Q_A_RAT_RLL_D", + "Q_A_ANG_RLL_P", + "Q_A_ACCEL_R_MAX", + "Q_A_RAT_PIT_P", + "Q_A_RAT_PIT_I", + "Q_A_RAT_PIT_D", + "Q_A_ANG_PIT_P", + "Q_A_ACCEL_P_MAX", + "Q_A_RAT_YAW_P", + "Q_A_RAT_YAW_I", + "Q_A_RAT_YAW_FLTE", + "Q_A_ANG_YAW_P", + "Q_A_ACCEL_Y_MAX", + ]) + self.set_parameters(parameter_values) + + self.takeoff(15, mode='GUIDED') self.set_rc(3, 1500) + self.change_mode("QLOITER") self.change_mode("QAUTOTUNE") tstart = self.get_sim_time() - sim_time_expected = 5000 - deadline = tstart + sim_time_expected - while self.get_sim_time_cached() < deadline: + self.context_collect('STATUSTEXT') + while True: now = self.get_sim_time_cached() - m = self.mav.recv_match(type='STATUSTEXT', - blocking=True, - timeout=1) - if m is None: + if now - tstart > 5000: + raise NotAchievedException("Did not get success message") + try: + self.wait_text("AutoTune: Success", timeout=1, check_context=True) + except AutoTestTimeoutException: continue - self.progress("STATUSTEXT (%u<%u): %s" % (now, deadline, m.text)) - if "AutoTune: Success" in m.text: - break + # got success message + break self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart)) + self.context_clear_collection('STATUSTEXT') + + self.progress("Landing to save gains") self.set_rc(3, 1200) - self.wait_altitude(-5, 1, relative=True, timeout=30) - while self.get_sim_time_cached() < deadline: - self.mavproxy.send('disarm\n') + self.wait_speed_vector( + Vector3(float('nan'), float('nan'), 1.4), + timeout=5, + ) + self.wait_speed_vector( + Vector3(0.0, 0.0, 0.0), + timeout=20, + ) + distance = self.distance_to_home() + if distance > 20: + raise NotAchievedException("wandered from home (distance=%f)" % + (distance,)) + self.set_rc(3, 1000) + tstart = self.get_sim_time() + while True: + now = self.get_sim_time_cached() + if now - tstart > 500: + raise NotAchievedException("Did not get success message") + self.send_mavlink_disarm_command() try: - self.wait_text("AutoTune: Saved gains for Roll Pitch Yaw", timeout=0.5) + self.wait_text( + "AutoTune: Saved gains for Roll Pitch Yaw.*", + timeout=0.5, + check_context=True, + regex=True, + ) except AutoTestTimeoutException: continue break + self.wait_disarmed() + self.reboot_sitl() # far from home - def takeoff(self, height, mode): + def takeoff(self, height, mode, timeout=30): """climb to specified height and set throttle to 1500""" self.set_current_waypoint(0, check_afterwards=False) self.change_mode(mode) self.wait_ready_to_arm() self.arm_vehicle() + if mode == 'GUIDED': + self.user_takeoff(alt_min=height, timeout=timeout) + return self.set_rc(3, 1800) self.wait_altitude(height, height+5, relative=True, - timeout=30) + timeout=timeout) self.set_rc(3, 1500) def do_RTL(self): @@ -408,14 +462,24 @@ def do_RTL(self): self.zero_throttle() def fly_home_land_and_disarm(self, timeout=30): - self.set_parameter("LAND_TYPE", 0) - filename = "flaps.txt" + self.context_push() + self.change_mode('LOITER') + self.set_parameter('RTL_AUTOLAND', 2) + filename = "QuadPlaneDalbyRTL.txt" self.progress("Using %s to fly home" % filename) - self.load_mission(filename) - self.change_mode("AUTO") - self.set_current_waypoint(7) + self.load_generic_mission(filename) + self.send_cmd_do_set_mode("RTL") + self.wait_mode('AUTO') + self.wait_current_waypoint(4) + self.wait_statustext('Land descend started') + self.wait_statustext('Land final started', timeout=60) self.wait_disarmed(timeout=timeout) + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_MISSION) + # the following command is accepted, but doesn't actually + # work! Should be able to remove check_afterwards! self.set_current_waypoint(0, check_afterwards=False) + self.change_mode('MANUAL') + self.context_pop() def wait_level_flight(self, accuracy=5, timeout=30): """Wait for level flight.""" @@ -682,6 +746,40 @@ def PilotYaw(self): self.set_rc(4, 1500) self.do_RTL() + def FwdThrInVTOL(self): + '''test use of fwd motor throttle into wind''' + self.set_parameters({"SIM_WIND_SPD": 25, # need very strong wind for this test + "SIM_WIND_DIR": 360, + "Q_WVANE_ENABLE": 1, + "Q_WVANE_GAIN": 1, + "STICK_MIXING": 0, + "Q_FWD_THR_USE": 2, + "SIM_ENGINE_FAIL": 2}) # we want to fail the forward thrust motor only + + self.takeoff(10, mode="QLOITER") + self.set_rc(2, 1000) + self.delay_sim_time(10) + # Check that it is using some forward throttle + fwd_thr_pwm = self.get_servo_channel_value(3) + if fwd_thr_pwm < 1150 : + raise NotAchievedException("fwd motor pwm command low, want >= 1150 got %f" % (fwd_thr_pwm)) + # check that pitch is on limit + m = self.mav.recv_match(type='ATTITUDE', blocking=True) + pitch = math.degrees(m.pitch) + if abs(pitch + 3.0) > 0.5 : + raise NotAchievedException("pitch should be -3.0 +- 0.5 deg, got %f" % (pitch)) + self.set_rc(2, 1500) + self.delay_sim_time(5) + loc1 = self.mav.location() + self.set_parameter("SIM_ENGINE_MUL", 0) # simulate a complete loss of forward motor thrust + self.delay_sim_time(20) + self.change_mode('QLAND') + self.wait_disarmed(timeout=60) + loc2 = self.mav.location() + position_drift = self.get_distance(loc1, loc2) + if position_drift > 5.0 : + raise NotAchievedException("position drift high, want < 5.0 m got %f m" % (position_drift)) + def Weathervane(self): '''test nose-into-wind functionality''' # We test nose into wind code paths and yaw direction in copter autotest, @@ -894,6 +992,19 @@ def LoiterAltQLand_Terrain(self, self.reset_SITL_commandline() self.context_pop() + def GUIDEDToAUTO(self): + '''Test using GUIDED mode for takeoff before shifting to auto''' + self.load_mission("mission.txt") + self.takeoff(30, mode='GUIDED') + + # extra checks would go here + self.assert_not_receiving_message('CAMERA_FEEDBACK') + + self.change_mode('AUTO') + self.wait_current_waypoint(3) + self.change_mode('QRTL') + self.wait_disarmed(timeout=240) + def Tailsitter(self): '''tailsitter test''' self.set_parameter('Q_FRAME_CLASS', 10) @@ -916,19 +1027,24 @@ def Tailsitter(self): raise NotAchievedException("Changed throttle output on mode change to QHOVER") self.disarm_vehicle() - def ICEngine(self): - '''Test ICE Engine support''' - rc_engine_start_chan = 11 + def setup_ICEngine_vehicle(self, start_chan): + '''restarts SITL with an IC Engine setup''' self.set_parameters({ - 'ICE_START_CHAN': rc_engine_start_chan, + 'ICE_START_CHAN': start_chan, }) - model = "quadplane-ice" + model = "quadplane-ice" self.customise_SITL_commandline( [], model=model, defaults_filepath=self.model_defaults_filepath(model), - wipe=False) + wipe=False, + ) + + def ICEngine(self): + '''Test ICE Engine support''' + rc_engine_start_chan = 11 + self.setup_ICEngine_vehicle(start_chan=rc_engine_start_chan) self.wait_ready_to_arm() self.wait_rpm(1, 0, 0, minimum_duration=1) @@ -946,7 +1062,7 @@ def ICEngine(self): self.wait_rpm(1, 6500, 7500, minimum_duration=30, timeout=40) self.progress("Setting min-throttle") self.set_rc(3, 1000) - self.wait_rpm(1, 300, 400, minimum_duration=1) + self.wait_rpm(1, 65, 75, minimum_duration=1) self.progress("Setting engine-start RC switch to LOW") self.set_rc(rc_engine_start_chan, 1000) self.wait_rpm(1, 0, 0, minimum_duration=1) @@ -967,18 +1083,8 @@ def ICEngine(self): def ICEngineMission(self): '''Test ICE Engine Mission support''' rc_engine_start_chan = 11 - self.set_parameters({ - 'ICE_START_CHAN': rc_engine_start_chan, - }) - model = "quadplane-ice" + self.setup_ICEngine_vehicle(start_chan=rc_engine_start_chan) - self.customise_SITL_commandline( - [], - model=model, - defaults_filepath=self.model_defaults_filepath(model), - wipe=False) - - self.reboot_sitl() self.load_mission("mission.txt") self.wait_ready_to_arm() self.set_rc(rc_engine_start_chan, 2000) @@ -986,6 +1092,81 @@ def ICEngineMission(self): self.change_mode('AUTO') self.wait_disarmed(timeout=300) + def MAV_CMD_DO_ENGINE_CONTROL(self): + '''test MAV_CMD_DO_ENGINE_CONTROL mavlink command''' + + expected_idle_rpm_min = 65 + expected_idle_rpm_max = 75 + expected_starter_rpm_min = 345 + expected_starter_rpm_max = 355 + + rc_engine_start_chan = 11 + self.setup_ICEngine_vehicle(start_chan=rc_engine_start_chan) + + self.wait_ready_to_arm() + + for method in self.run_cmd, self.run_cmd_int: + self.change_mode('MANUAL') + self.set_rc(rc_engine_start_chan, 1500) # allow motor to run + self.wait_rpm(1, 0, 0, minimum_duration=1) + self.arm_vehicle() + self.wait_rpm(1, 0, 0, minimum_duration=1) + self.start_subtest("Start motor") + method(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=1) + self.wait_rpm(1, expected_starter_rpm_min, expected_starter_rpm_max) + self.wait_rpm(1, expected_idle_rpm_min, expected_idle_rpm_max, minimum_duration=10) + + # starting the motor while it is running is failure + # (probably wrong, but that's how this works): + self.start_subtest("try start motor again") + self.context_collect('STATUSTEXT') + method(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=1, want_result=mavutil.mavlink.MAV_RESULT_FAILED) + self.wait_statustext("already running", check_context=True) + self.context_stop_collecting('STATUSTEXT') + # shouldn't affect run state: + self.wait_rpm(1, expected_idle_rpm_min, expected_idle_rpm_max, minimum_duration=1) + + self.start_subtest("Stop motor") + method(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=0) + self.wait_rpm(1, 0, 0, minimum_duration=1) + + self.start_subtest("Stop motor (again)") + method(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=0) + self.wait_rpm(1, 0, 0, minimum_duration=1) + + self.start_subtest("Check start chan control disable") + old_start_channel_value = self.get_rc_channel_value(rc_engine_start_chan) + self.set_rc(rc_engine_start_chan, 1000) + self.context_collect('STATUSTEXT') + method(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=1, want_result=mavutil.mavlink.MAV_RESULT_FAILED) + self.wait_statustext("start control disabled", check_context=True) + self.context_stop_collecting('STATUSTEXT') + self.set_rc(rc_engine_start_chan, old_start_channel_value) + self.wait_rpm(1, 0, 0, minimum_duration=1) + + self.start_subtest("test start-at-height") + self.wait_rpm(1, 0, 0, minimum_duration=1) + self.context_collect('STATUSTEXT') + method( + mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, + p1=1, # start + p3=15.5, # ... at 15.5 metres + ) + self.wait_statustext("height set to 15.5m", check_context=True) + self.wait_rpm(1, 0, 0, minimum_duration=2) + + self.takeoff(20, mode='GUIDED') + self.wait_rpm(1, expected_starter_rpm_min, expected_starter_rpm_max, minimum_duration=1) + self.wait_statustext("Engine running", check_context=True) + self.context_stop_collecting('STATUSTEXT') + + # stop the motor again: + method(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=0) + self.wait_rpm(1, 0, 0, minimum_duration=1) + + self.change_mode('QLAND') + self.wait_disarmed() + def Ship(self): '''Ensure we can take off from simulated ship''' self.context_push() @@ -1208,16 +1389,178 @@ def RCDisableAirspeedUse(self): self.context_pop() self.reboot_sitl() + def mission_MAV_CMD_DO_VTOL_TRANSITION(self): + '''mission item forces transition''' + wps = self.create_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 200, 0, 30), + self.create_MISSION_ITEM_INT( + mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION, + p1=mavutil.mavlink.MAV_VTOL_STATE_MC + ), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 300, 200, 30), + self.create_MISSION_ITEM_INT( + mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION, + p1=mavutil.mavlink.MAV_VTOL_STATE_FW + ), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 100, 200, 30), + (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0), + ]) + self.check_mission_upload_download(wps) + + self.change_mode('AUTO') + self.wait_ready_to_arm() + + self.arm_vehicle() + self.wait_current_waypoint(4) + self.wait_servo_channel_value(5, 1200, comparator=operator.gt) + self.wait_current_waypoint(6) + self.wait_servo_channel_value(5, 1000, comparator=operator.eq, timeout=90) + + self.fly_home_land_and_disarm() + + def mavlink_MAV_CMD_DO_VTOL_TRANSITION(self): + '''mavlink command forces transition during mission''' + wps = self.create_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2000, 0, 30), + (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0), + ]) + self.check_mission_upload_download(wps) + + self.change_mode('AUTO') + self.wait_ready_to_arm() + + self.arm_vehicle() + self.wait_current_waypoint(2) + self.wait_servo_channel_value(5, 1000, comparator=operator.eq, timeout=90) + + for command in self.run_cmd, self.run_cmd_int: + command(mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION, p1=mavutil.mavlink.MAV_VTOL_STATE_MC) + self.wait_servo_channel_value(5, 1200, comparator=operator.gt, timeout=300) + command(mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION, p1=mavutil.mavlink.MAV_VTOL_STATE_FW) + self.wait_servo_channel_value(5, 1000, comparator=operator.eq, timeout=90) + + self.fly_home_land_and_disarm() + + def MAV_CMD_NAV_TAKEOFF(self): + '''test issuing takeoff command via mavlink''' + self.change_mode('GUIDED') + self.wait_ready_to_arm() + + self.arm_vehicle() + self.run_cmd(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, p7=5) + self.wait_altitude(4.5, 5.5, minimum_duration=5, relative=True) + self.change_mode('QLAND') + self.wait_disarmed() + + self.start_subtest("Check NAV_TAKEOFF is above current location, not home location") + self.change_mode('GUIDED') + self.wait_ready_to_arm() + + # reset home 20 metres above current location + current_alt_abs = self.get_altitude(relative=False) + + loc = self.mav.location() + + home_z_ofs = 20 + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_SET_HOME, + p5=loc.lat, + p6=loc.lng, + p7=current_alt_abs + home_z_ofs, + ) + + self.arm_vehicle() + takeoff_alt = 5 + self.run_cmd(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, p7=takeoff_alt) + self.wait_altitude( + current_alt_abs + takeoff_alt - 0.5, + current_alt_abs + takeoff_alt + 0.5, + minimum_duration=5, + relative=False, + ) + self.change_mode('QLAND') + self.wait_disarmed() + + self.reboot_sitl() # unlock home position + + def Q_GUIDED_MODE(self): + '''test moving in VTOL mode with SET_POSITION_TARGET_GLOBAL_INT''' + self.set_parameter('Q_GUIDED_MODE', 1) + self.change_mode('GUIDED') + self.wait_ready_to_arm() + self.arm_vehicle() + self.run_cmd(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, p7=15) + self.wait_altitude(14, 16, relative=True) + + loc = self.mav.location() + self.location_offset_ne(loc, 50, 50) + + # set position target + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_REPOSITION, + 0, + 1, # reposition flags; 1 means "change to guided" + 0, + 0, + int(loc.lat * 1e7), + int(loc.lng * 1e7), + 30, # alt + frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + ) + self.wait_location(loc, timeout=120) + + self.fly_home_land_and_disarm() + + def DCMClimbRate(self): + '''Test the climb rate measurement in DCM with and without GPS''' + self.wait_ready_to_arm() + + self.change_mode('QHOVER') + self.arm_vehicle() + self.set_rc(3, 2000) + self.wait_altitude(30, 50, relative=True) + + # Start Descending + self.set_rc(3, 1000) + self.wait_climbrate(-5, -0.5, timeout=10) + + # Switch to DCM + self.set_parameter('AHRS_EKF_TYPE', 0) + self.delay_sim_time(5) + + # Start Climbing + self.set_rc(3, 2000) + self.wait_climbrate(0.5, 5, timeout=10) + + # Kill any GPSs + self.set_parameters({ + 'SIM_GPS_DISABLE': 1, + 'SIM_GPS2_DISABLE': 1, + }) + self.delay_sim_time(5) + + # Start Descending + self.set_rc(3, 1000) + self.wait_climbrate(-5, -0.5, timeout=10) + + # Force disarm + self.disarm_vehicle(force=True) + def tests(self): '''return list of all tests''' ret = super(AutoTestQuadPlane, self).tests() ret.extend([ + self.FwdThrInVTOL, self.AirMode, self.TestMotorMask, self.PilotYaw, self.ParameterChecks, - self.LogDownload, + self.QAUTOTUNE, + self.TestLogDownload, + self.TestLogDownloadWrap, self.EXTENDED_SYS_STATE, self.Mission, self.Weathervane, @@ -1226,13 +1569,26 @@ def tests(self): self.Tailsitter, self.ICEngine, self.ICEngineMission, + self.MAV_CMD_DO_ENGINE_CONTROL, self.MidAirDisarmDisallowed, + self.GUIDEDToAUTO, self.BootInAUTO, self.Ship, self.MAV_CMD_NAV_LOITER_TO_ALT, self.LoiterAltQLand, self.VTOLLandSpiral, self.VTOLQuicktune, + Test(self.MotorTest, kwargs={ # tests motors 4 and 2 + "mot1_servo_chan": 8, # quad-x second motor cw from f-r + "mot4_servo_chan": 6, # quad-x third motor cw from f-r + "wait_finish_text": False, + "quadplane": True, + }), self.RCDisableAirspeedUse, + self.mission_MAV_CMD_DO_VTOL_TRANSITION, + self.mavlink_MAV_CMD_DO_VTOL_TRANSITION, + self.MAV_CMD_NAV_TAKEOFF, + self.Q_GUIDED_MODE, + self.DCMClimbRate, ]) return ret diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 78789d4ff8e78..f1eeb76c61801 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -14,13 +14,13 @@ import sys import time -from common import AutoTest +import vehicle_test_suite + from pysim import util -from common import AutoTestTimeoutException -from common import MsgRcvTimeoutException -from common import NotAchievedException -from common import PreconditionFailedException +from vehicle_test_suite import AutoTestTimeoutException +from vehicle_test_suite import NotAchievedException +from vehicle_test_suite import PreconditionFailedException from pymavlink import mavextra from pymavlink import mavutil @@ -34,7 +34,7 @@ 246) -class AutoTestRover(AutoTest): +class AutoTestRover(vehicle_test_suite.TestSuite): @staticmethod def get_not_armable_mode_list(): return ["RTL", "SMART_RTL"] @@ -320,27 +320,13 @@ def Sprayer(self): self.start_subtest("Checking mavlink commands") self.change_mode("MANUAL") self.progress("Starting Sprayer") - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SPRAYER, - 1, # p1 - 0, # p2 - 0, # p3 - 0, # p4 - 0, # p5 - 0, # p6 - 0) # p7 + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SPRAYER, p1=1) self.progress("Testing speed-ramping") self.set_rc(3, 1700) # start driving forward self.wait_servo_channel_value(pump_ch, 1690, timeout=60, comparator=operator.gt) self.start_subtest("Stopping Sprayer") - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SPRAYER, - 0, # p1 - 0, # p2 - 0, # p3 - 0, # p4 - 0, # p5 - 0, # p6 - 0) # p7 + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SPRAYER, p1=0) self.wait_servo_channel_value(pump_ch, pump_ch_min) self.set_rc(3, 1000) # start driving forward @@ -356,38 +342,21 @@ def Sprayer(self): def DriveMaxRCIN(self, timeout=30): """Drive rover at max RC inputs""" - self.context_push() - ex = None - - try: - self.progress("Testing max RC inputs") - self.change_mode("MANUAL") - - self.wait_ready_to_arm() - self.arm_vehicle() - - self.set_rc(3, 2000) - self.set_rc(1, 1000) + self.progress("Testing max RC inputs") + self.change_mode("MANUAL") - tstart = self.get_sim_time() - while self.get_sim_time_cached() - tstart < timeout: - m = self.mav.recv_match(type='VFR_HUD', blocking=True, timeout=1) - if m is not None: - self.progress("Current speed: %f" % m.groundspeed) + self.wait_ready_to_arm() + self.arm_vehicle() - # reduce throttle - self.set_rc(3, 1500) - self.set_rc(1, 1500) + self.set_rc(3, 2000) + self.set_rc(1, 1000) - except Exception as e: - self.print_exception_caught(e) - ex = e + tstart = self.get_sim_time() + while self.get_sim_time_cached() - tstart < timeout: + m = self.assert_receive_message('VFR_HUD') + self.progress("Current speed: %f" % m.groundspeed) self.disarm_vehicle() - self.context_pop() - - if ex: - raise ex ################################################# # AUTOTEST ALL @@ -420,34 +389,18 @@ def GripperMission(self): self.wait_statustext("Mission Complete", timeout=60, check_context=True) self.disarm_vehicle() - def GetBanner(self): + def _MAV_CMD_DO_SEND_BANNER(self, run_cmd): '''Get Banner''' - target_sysid = self.sysid_thismav() - target_compid = 1 - self.mav.mav.command_long_send( - target_sysid, - target_compid, - mavutil.mavlink.MAV_CMD_DO_SEND_BANNER, - 1, # confirmation - 1, # send it - 0, - 0, - 0, - 0, - 0, - 0) - start = time.time() - while True: - m = self.mav.recv_match(type='STATUSTEXT', - blocking=True, - timeout=1) - if m is not None and "ArduRover" in m.text: - self.progress("banner received: %s" % m.text) - return - if time.time() - start > 10: - break + self.context_push() + self.context_collect('STATUSTEXT') + run_cmd(mavutil.mavlink.MAV_CMD_DO_SEND_BANNER) + self.wait_statustext("ArduRover", timeout=1, check_context=True) + self.context_pop() - raise MsgRcvTimeoutException("banner not received") + def MAV_CMD_DO_SEND_BANNER(self): + '''test MAV_CMD_DO_SEND_BANNER''' + self._MAV_CMD_DO_SEND_BANNER(self.run_cmd) + self._MAV_CMD_DO_SEND_BANNER(self.run_cmd_int) def drive_brake_get_stopping_distance(self, speed): '''measure our stopping distance''' @@ -613,19 +566,113 @@ def AC_Avoidance(self): def ServoRelayEvents(self): '''Test ServoRelayEvents''' - self.do_set_relay(0, 0) - off = self.get_parameter("SIM_PIN_MASK") - self.do_set_relay(0, 1) - on = self.get_parameter("SIM_PIN_MASK") - if on == off: - raise NotAchievedException( - "Pin mask unchanged after relay cmd") - self.progress("Pin mask changed after relay command") + for method in self.run_cmd, self.run_cmd_int: + self.context_push() + method(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, p1=0, p2=0) + off = self.get_parameter("SIM_PIN_MASK") + method(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, p1=0, p2=1) + on = self.get_parameter("SIM_PIN_MASK") + if on == off: + raise NotAchievedException( + "Pin mask unchanged after relay cmd") + self.progress("Pin mask changed after relay command") + method(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, p1=0, p2=0) + + self.set_message_rate_hz("RELAY_STATUS", 10) + + # default configuration for relays in sim have one relay: + self.assert_received_message_field_values('RELAY_STATUS', { + "present": 3, + "on": 0, + }) + method(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, p1=0, p2=1) + self.assert_received_message_field_values('RELAY_STATUS', { + "present": 3, + "on": 1, + }) + method(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, p1=1, p2=1) + self.assert_received_message_field_values('RELAY_STATUS', { + "present": 3, + "on": 3, + }) + method(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, p1=0, p2=0) + method(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, p1=1, p2=0) + self.assert_received_message_field_values('RELAY_STATUS', { + "present": 3, + "on": 0, + }) + + # add another servo: + self.set_parameter("RELAY_PIN6", 14) + self.assert_received_message_field_values('RELAY_STATUS', { + "present": 35, + "on": 0, + }) + method(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, p1=5, p2=1) + self.assert_received_message_field_values('RELAY_STATUS', { + "present": 35, + "on": 32, + }) + method(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, p1=0, p2=1) + self.assert_received_message_field_values('RELAY_STATUS', { + "present": 35, + "on": 33, + }) + method(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, p1=5, p2=0) + self.assert_received_message_field_values('RELAY_STATUS', { + "present": 35, + "on": 1, + }) + + self.start_subtest("test MAV_CMD_DO_REPEAT_RELAY") + self.context_push() + self.set_parameter("SIM_SPEEDUP", 1) + method( + mavutil.mavlink.MAV_CMD_DO_REPEAT_RELAY, + p1=0, # servo 1 + p2=5, # 5 times + p3=0.5, # 1 second between being on + ) + for value in 0, 1, 0, 1, 0, 1, 0, 1: + self.wait_message_field_values('RELAY_STATUS', { + "on": value, + }) + self.context_pop() + self.delay_sim_time(3) + self.assert_received_message_field_values('RELAY_STATUS', { + "on": 1, # back to initial state + }) + self.context_pop() + + self.start_subtest("test MAV_CMD_DO_SET_SERVO") + for value in 1678, 2300, 0: + method(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=13, p2=value) + self.wait_servo_channel_value(13, value) + + self.start_subtest("test MAV_CMD_DO_REPEAT_SERVO") + + self.context_push() + self.set_parameter("SIM_SPEEDUP", 1) + trim = self.get_parameter("SERVO13_TRIM") + value = 2000 + method( + mavutil.mavlink.MAV_CMD_DO_REPEAT_SERVO, + p1=12, # servo12 + p2=value, # pwm + p3=5, # count + p4=0.5, # cycle time (1 second between high and high) + ) + for value in trim, value, trim, value, trim, value, trim, value: + self.wait_servo_channel_value(12, value) + self.context_pop() + + self.set_message_rate_hz("RELAY_STATUS", 0) def MAVProxy_SetModeUsingSwitch(self): """Set modes via mavproxy switch""" + port = self.sitl_rcin_port(offset=1) self.customise_SITL_commandline([ - "--rc-in-port", "5502", + "--rc-in-port", str(port), ]) ex = None try: @@ -637,7 +684,7 @@ def MAVProxy_SetModeUsingSwitch(self): (4, 'AUTO'), (5, 'AUTO'), # non-existant mode, should stay in RTL (6, 'MANUAL')] - mavproxy = self.start_mavproxy() + mavproxy = self.start_mavproxy(sitl_rcin_port=port) for (num, expected) in fnoo: mavproxy.send('switch %u\n' % num) self.wait_mode(expected) @@ -1193,8 +1240,10 @@ def DO_SET_MODE(self): '''Set mode via MAV_COMMAND_DO_SET_MODE''' self.do_set_mode_via_command_long("HOLD") self.do_set_mode_via_command_long("MANUAL") + self.do_set_mode_via_command_int("HOLD") + self.do_set_mode_via_command_int("MANUAL") - def InitialMode(self): + def RoverInitialMode(self): '''test INITIAL_MODE parameter works''' # from mavproxy_rc.py self.wait_ready_to_arm() @@ -1290,22 +1339,27 @@ def SYSID_ENFORCE(self): def Rally(self): '''Test Rally Points''' - self.load_rally("rover-test-rally.txt") + self.load_rally_using_mavproxy("rover-test-rally.txt") + self.assert_parameter_value('RALLY_TOTAL', 2) self.wait_ready_to_arm() self.arm_vehicle() + # calculate early to avoid round-trips while vehicle is moving: + accuracy = self.get_parameter("WP_RADIUS") + self.reach_heading_manual(10) self.reach_distance_manual(50) self.change_mode("RTL") + # location copied in from rover-test-rally.txt: loc = mavutil.location(40.071553, -105.229401, 0, 0) - accuracy = self.get_parameter("WP_RADIUS") - self.wait_location(loc, accuracy=accuracy, minimum_duration=10) + + self.wait_location(loc, accuracy=accuracy, minimum_duration=10, timeout=45) self.disarm_vehicle() def fence_with_bad_frame(self, target_system=1, target_component=1): @@ -4768,13 +4822,12 @@ def MotorTest(self): self.wait_ready_to_arm() self.run_cmd( mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, - 1, # p1 - motor instance - mavutil.mavlink.MOTOR_TEST_THROTTLE_PWM, # p2 - throttle type - magic_throttle_value, # p3 - throttle - 5, # p4 - timeout - 1, # p5 - motor count - 0, # p6 - test order (see MOTOR_TEST_ORDER) - 0, # p7 + p1=1, # motor instance + p2=mavutil.mavlink.MOTOR_TEST_THROTTLE_PWM, # throttle type + p3=magic_throttle_value, # throttle + p4=5, # timeout + p5=1, # motor count + p6=0, # test order (see MOTOR_TEST_ORDER) ) self.wait_armed() self.progress("Waiting for magic throttle value") @@ -5216,43 +5269,44 @@ def test_scripting_internal_test(self): self.context_push() - test_scripts = ["scripting_test.lua", "math.lua", "strings.lua"] - success_text = ["Internal tests passed", "Math tests passed", "String tests passed"] - - messages = [] - - def my_message_hook(mav, message): - if message.get_type() != 'STATUSTEXT': - return - messages.append(message) - - self.install_message_hook_context(my_message_hook) self.set_parameters({ "SCR_ENABLE": 1, "SCR_HEAP_SIZE": 1024000, "SCR_VM_I_COUNT": 1000000, }) - for script in test_scripts: + self.install_test_modules_context() + self.install_mavlink_module_context() + for script in [ + "scripting_test.lua", + "math.lua", + "strings.lua", + "mavlink_test.lua", + ]: self.install_test_script_context(script) + + self.context_collect('STATUSTEXT') + self.context_collect('NAMED_VALUE_FLOAT') + self.reboot_sitl() - self.delay_sim_time(10) + for success_text in [ + "Internal tests passed", + "Math tests passed", + "String tests passed", + "Received heartbeat from" + ]: + self.wait_statustext(success_text, check_context=True) + + for success_nvf in [ + "test", + ]: + self.assert_received_message_field_values("NAMED_VALUE_FLOAT", { + "name": success_nvf, + }, check_context=True) self.context_pop() self.reboot_sitl() - # check all messages to see if we got our message - success = True - for text in success_text: - script_success = False - for m in messages: - if text in m.text: - script_success = True - success = script_success and success - self.progress("Success") - if not success: - raise NotAchievedException("Scripting internal test failed") - def test_scripting_hello_world(self): self.start_subtest("Scripting hello world") @@ -5548,7 +5602,10 @@ def SendToComponents(self): 0, 0, 0) + self.progress("Sending control message") + self.context_push() + self.context_collect('COMMAND_LONG') self.mav.mav.digicam_control_send( 1, # target_system 1, # target_component @@ -5563,21 +5620,56 @@ def SendToComponents(self): ) self.mav.mav.srcSystem = old_srcSystem - self.progress("Expecting a command long") - tstart = self.get_sim_time_cached() - while True: - now = self.get_sim_time_cached() - if now - tstart > 2: - raise NotAchievedException("Did not receive digicam_control message") - m = self.mav.recv_match(type='COMMAND_LONG', blocking=True, timeout=0.1) - self.progress("Message: %s" % str(m)) - if m is None: - continue - if m.command != mavutil.mavlink.MAV_CMD_DO_DIGICAM_CONTROL: - raise NotAchievedException("Did not get correct command") - if m.param6 != 17: - raise NotAchievedException("Did not get correct command_id") - break + self.assert_received_message_field_values('COMMAND_LONG', { + 'command': mavutil.mavlink.MAV_CMD_DO_DIGICAM_CONTROL, + 'param6': 17, + }, timeout=2, check_context=True) + self.context_pop() + + # test sending via commands: + for run_cmd in self.run_cmd, self.run_cmd_int: + self.progress("Sending control command") + self.context_push() + self.context_collect('COMMAND_LONG') + run_cmd(mavutil.mavlink.MAV_CMD_DO_DIGICAM_CONTROL, + p1=1, # start or keep it up + p2=1, # zoom_pos + p3=0, # zoom_step + p4=0, # focus_lock + p5=0, # 1 shot or start filming + p6=37, # command id (de-dupe field) + ) + + self.assert_received_message_field_values('COMMAND_LONG', { + 'command': mavutil.mavlink.MAV_CMD_DO_DIGICAM_CONTROL, + 'param6': 37, + }, timeout=2, check_context=True) + + self.context_pop() + + # test sending via commands: + for run_cmd in self.run_cmd, self.run_cmd_int: + self.progress("Sending configure command") + self.context_push() + self.context_collect('COMMAND_LONG') + run_cmd(mavutil.mavlink.MAV_CMD_DO_DIGICAM_CONFIGURE, + p1=1, + p2=1, + p3=0, + p4=0, + p5=12, + p6=37 + ) + + self.assert_received_message_field_values('COMMAND_LONG', { + 'command': mavutil.mavlink.MAV_CMD_DO_DIGICAM_CONFIGURE, + 'param5': 12, + 'param6': 37, + }, timeout=2, check_context=True) + + self.context_pop() + + self.mav.mav.srcSystem = old_srcSystem def SkidSteer(self): '''Check skid-steering''' @@ -5719,6 +5811,40 @@ def SET_ATTITUDE_TARGET(self, target_sysid=None, target_compid=1): break self.disarm_vehicle() + def SET_ATTITUDE_TARGET_heading(self, target_sysid=None, target_compid=1): + '''Test handling of SET_ATTITUDE_TARGET''' + self.change_mode('GUIDED') + self.wait_ready_to_arm() + self.arm_vehicle() + + for angle in 0, 290, 70, 180, 0: + self.SET_ATTITUDE_TARGET_heading_test_target(angle, target_sysid, target_compid) + self.disarm_vehicle() + + def SET_ATTITUDE_TARGET_heading_test_target(self, angle, target_sysid, target_compid): + if target_sysid is None: + target_sysid = self.sysid_thismav() + + def poke_set_attitude(value, target): + self.mav.mav.set_attitude_target_send( + 0, # time_boot_ms + target_sysid, + target_compid, + mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE | + mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE | + mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE, + mavextra.euler_to_quat([ + math.radians(0), + math.radians(0), + math.radians(angle) + ]), # att + 0, # roll rate (rad/s) + 0, # pitch rate + 0, # yaw rate + 1) # thrust + + self.wait_heading(angle, called_function=poke_set_attitude, minimum_duration=5) + def SET_POSITION_TARGET_LOCAL_NED(self, target_sysid=None, target_compid=1): '''Test handling of SET_POSITION_TARGET_LOCAL_NED''' if target_sysid is None: @@ -5906,18 +6032,14 @@ def MAV_CMD_DO_SET_MISSION_CURRENT(self, target_sysid=None, target_compid=1): self.set_current_waypoint_using_mav_cmd_do_set_mission_current(2) - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_MISSION_CURRENT, - 17, - 0, - 0, - 0, - 0, - 0, - 0, - timeout=1, - target_sysid=target_sysid, - target_compid=target_compid, - want_result=mavutil.mavlink.MAV_RESULT_FAILED) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_SET_MISSION_CURRENT, + p1=17, + timeout=1, + target_sysid=target_sysid, + target_compid=target_compid, + want_result=mavutil.mavlink.MAV_RESULT_DENIED, + ) def FlashStorage(self): '''Test flash storage (for parameters etc)''' @@ -6071,8 +6193,9 @@ def EStopAtBoot(self): }) self.set_rc(9, 2000) self.reboot_sitl() - self.delay_sim_time(10) - self.assert_prearm_failure("Motors Emergency Stopped") + self.assert_prearm_failure( + "Motors Emergency Stopped", + other_prearm_failures_fatal=False) self.context_pop() self.reboot_sitl() @@ -6175,7 +6298,8 @@ def AutoDock(self): def PrivateChannel(self): '''test the serial option bit specifying a mavlink channel as private''' global mav2 - mav2 = mavutil.mavlink_connection("tcp:localhost:5763", + port = self.adjust_ardupilot_port(5763) + mav2 = mavutil.mavlink_connection("tcp:localhost:%u" % port, robust_parsing=True, source_system=7, source_component=7) @@ -6276,6 +6400,217 @@ def printmessage(mav, m): # both the vehicle and this tests's special heartbeat raise NotAchievedException("Got heartbeat on private channel from non-vehicle") + def MAV_CMD_DO_SET_REVERSE(self): + '''test MAV_CMD_DO_SET_REVERSE command''' + self.change_mode('GUIDED') + self.wait_ready_to_arm() + self.arm_vehicle() + + here = self.mav.location() + target_loc = self.offset_location_ne(here, 2000, 0) + self.send_guided_mission_item(target_loc) + + self.wait_groundspeed(3, 100, minimum_duration=5) + + for method in self.run_cmd, self.run_cmd_int: + self.progress("Forwards!") + method(mavutil.mavlink.MAV_CMD_DO_SET_REVERSE, p1=0) + self.wait_heading(0) + + self.progress("Backwards!") + method(mavutil.mavlink.MAV_CMD_DO_SET_REVERSE, p1=1) + self.wait_heading(180) + + self.progress("Forwards!") + method(mavutil.mavlink.MAV_CMD_DO_SET_REVERSE, p1=0) + self.wait_heading(0) + + self.disarm_vehicle() + + def MAV_CMD_NAV_RETURN_TO_LAUNCH(self): + '''test MAV_CMD_NAV_RETURN_TO_LAUNCH mavlink command''' + self.change_mode('GUIDED') + self.wait_ready_to_arm() + self.arm_vehicle() + + here = self.mav.location() + target_loc = self.offset_location_ne(here, 2000, 0) + self.send_guided_mission_item(target_loc) + self.wait_distance_to_home(20, 100) + + self.run_cmd(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH) + self.wait_mode('RTL') + + self.change_mode('GUIDED') + + self.run_cmd_int(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH) + self.wait_mode('RTL') + + self.wait_distance_to_home(0, 5, timeout=30) + self.disarm_vehicle() + + def MAV_CMD_DO_CHANGE_SPEED(self): + '''test MAV_CMD_NAV_RETURN_TO_LAUNCH mavlink command''' + self.change_mode('GUIDED') + self.wait_ready_to_arm() + self.arm_vehicle() + + original_loc = self.mav.location() + here = original_loc + target_loc = self.offset_location_ne(here, 2000, 0) + self.send_guided_mission_item(target_loc) + self.wait_distance_to_home(20, 100) + + speeds = 3, 7, 12, 4 + + for speed in speeds: + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, p2=speed) + self.wait_groundspeed(speed-0.5, speed+0.5, minimum_duration=5) + + self.send_guided_mission_item(original_loc) + + for speed in speeds: + self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, p2=speed) + self.wait_groundspeed(speed-0.5, speed+0.5, minimum_duration=5) + + self.change_mode('RTL') + + self.wait_distance_to_home(0, 5, timeout=30) + self.disarm_vehicle() + + def MAV_CMD_MISSION_START(self): + '''simple test for starting missing using this command''' + # home and 1 waypoint a long way away: + self.upload_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2000, 0, 0), + ]) + self.change_mode('AUTO') + self.wait_ready_to_arm() + self.arm_vehicle() + for method in self.run_cmd, self.run_cmd_int: + self.change_mode('MANUAL') + self.wait_groundspeed(0, 1) + method(mavutil.mavlink.MAV_CMD_MISSION_START) + self.wait_mode('AUTO') + self.wait_groundspeed(3, 100) + self.disarm_vehicle() + + def MAV_CMD_NAV_SET_YAW_SPEED(self): + '''tests for MAV_CMD_NAV_SET_YAW_SPEED guided-mode command''' + self.change_mode('GUIDED') + self.wait_ready_to_arm() + self.arm_vehicle() + + for method in self.run_cmd, self.run_cmd_int: + self.change_mode('MANUAL') + self.wait_groundspeed(0, 1) + self.change_mode('GUIDED') + self.start_subtest("Absolute angles") + for (heading, speed) in (10, 5), (190, 10), (0, 2), (135, 6): + def cf(*args, **kwargs): + method( + mavutil.mavlink.MAV_CMD_NAV_SET_YAW_SPEED, + p1=heading, + p2=speed, + p3=0, # zero is absolute-angles + ) + self.wait_groundspeed(speed-0.5, speed+0.5, called_function=cf, minimum_duration=2) + self.wait_heading(heading-0.5, heading+0.5, called_function=cf, minimum_duration=2) + + self.start_subtest("relative angles") + original_angle = 90 + method( + mavutil.mavlink.MAV_CMD_NAV_SET_YAW_SPEED, + p1=original_angle, + p2=5, + p3=0, # zero is absolute-angles + ) + self.wait_groundspeed(4, 6) + self.wait_heading(original_angle-0.5, original_angle+0.5) + + expected_angle = original_angle + for (angle_delta, speed) in (5, 6), (-30, 2), (180, 7): + method( + mavutil.mavlink.MAV_CMD_NAV_SET_YAW_SPEED, + p1=angle_delta, + p2=speed, + p3=1, # one is relative-angles + ) + + def cf(*args, **kwargs): + method( + mavutil.mavlink.MAV_CMD_NAV_SET_YAW_SPEED, + p1=0, + p2=speed, + p3=1, # one is absolute-angles + ) + expected_angle += angle_delta + if expected_angle < 0: + expected_angle += 360 + if expected_angle > 360: + expected_angle -= 360 + self.wait_groundspeed(speed-0.5, speed+0.5, called_function=cf, minimum_duration=2) + self.wait_heading(expected_angle, called_function=cf, minimum_duration=2) + self.do_RTL() + self.disarm_vehicle() + + def _MAV_CMD_GET_HOME_POSITION(self, run_cmd): + '''test handling of mavlink command MAV_CMD_GET_HOME_POSITION''' + self.context_collect('HOME_POSITION') + run_cmd(mavutil.mavlink.MAV_CMD_GET_HOME_POSITION) + self.assert_receive_message('HOME_POSITION', check_context=True) + + def MAV_CMD_GET_HOME_POSITION(self): + '''test handling of mavlink command MAV_CMD_GET_HOME_POSITION''' + self.change_mode('LOITER') + self.wait_ready_to_arm() + self._MAV_CMD_GET_HOME_POSITION(self.run_cmd) + self._MAV_CMD_GET_HOME_POSITION(self.run_cmd_int) + + def MAV_CMD_DO_FENCE_ENABLE(self): + '''ensure MAV_CMD_DO_FENCE_ENABLE mavlink command works''' + here = self.mav.location() + + self.upload_fences_from_locations( + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, + [ + [ # east + self.offset_location_ne(here, -50, 20), # bl + self.offset_location_ne(here, 50, 20), # br + self.offset_location_ne(here, 50, 40), # tr + self.offset_location_ne(here, -50, 40), # tl, + ], [ # over the top of the vehicle + self.offset_location_ne(here, -50, -50), # bl + self.offset_location_ne(here, -50, 50), # br + self.offset_location_ne(here, 50, 50), # tr + self.offset_location_ne(here, 50, -50), # tl, + ] + ] + ) + + # enable: + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=1) + self.assert_fence_enabled() + + # disable + self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=0) + self.assert_fence_disabled() + + def MAV_CMD_BATTERY_RESET(self): + '''manipulate battery levels with MAV_CMD_BATTERY_RESET''' + for (run_cmd, value) in (self.run_cmd, 56), (self.run_cmd_int, 97): + run_cmd( + mavutil.mavlink.MAV_CMD_BATTERY_RESET, + p1=65535, # battery mask + p2=value, + ) + self.assert_received_message_field_values('BATTERY_STATUS', { + "battery_remaining": value, + }, { + "poll": True, + }) + def tests(self): '''return list of all tests''' ret = super(AutoTestRover, self).tests() @@ -6289,10 +6624,9 @@ def tests(self): self.DriveRTL, self.SmartRTL, self.DriveSquare, - self.DriveMaxRCIN, self.DriveMission, # self.DriveBrake, # disabled due to frequent failures - self.GetBanner, + self.MAV_CMD_DO_SEND_BANNER, self.DO_SET_MODE, self.MAVProxy_DO_SET_MODE, self.ServoRelayEvents, @@ -6305,11 +6639,16 @@ def tests(self): self.Gripper, self.GripperMission, self.SET_MESSAGE_INTERVAL, + self.MESSAGE_INTERVAL_COMMAND_INT, self.REQUEST_MESSAGE, self.SYSID_ENFORCE, self.SET_ATTITUDE_TARGET, + self.SET_ATTITUDE_TARGET_heading, self.SET_POSITION_TARGET_LOCAL_NED, self.MAV_CMD_DO_SET_MISSION_CURRENT, + self.MAV_CMD_DO_CHANGE_SPEED, + self.MAV_CMD_MISSION_START, + self.MAV_CMD_NAV_SET_YAW_SPEED, self.Button, self.Rally, self.Offboard, @@ -6343,12 +6682,19 @@ def tests(self): self.DepthFinder, self.ChangeModeByNumber, self.EStopAtBoot, + self.MAV_CMD_NAV_RETURN_TO_LAUNCH, self.StickMixingAuto, self.AutoDock, self.PrivateChannel, self.GCSFailsafe, - self.InitialMode, + self.RoverInitialMode, self.DriveMaxRCIN, + self.NoArmWithoutMissionItems, + self.CompassPrearms, + self.MAV_CMD_DO_SET_REVERSE, + self.MAV_CMD_GET_HOME_POSITION, + self.MAV_CMD_DO_FENCE_ENABLE, + self.MAV_CMD_BATTERY_RESET, ]) return ret diff --git a/Tools/autotest/run_in_terminal_window.sh b/Tools/autotest/run_in_terminal_window.sh index 86bd86a2d1620..7a83f1c565dac 100755 --- a/Tools/autotest/run_in_terminal_window.sh +++ b/Tools/autotest/run_in_terminal_window.sh @@ -43,7 +43,7 @@ elif [ -n "$DISPLAY" -a -n "$(which gnome-terminal)" ]; then gnome-terminal -e "$*" elif [ -n "$STY" ]; then # We are running inside of screen, try to start it there - screen -X screen -t "$name" $* + screen -X screen -t "$name" bash -c "cd $PWD; $*" else filename="/tmp/$name.log" echo "RiTW: Window access not found, logging to $filename" diff --git a/Tools/autotest/run_mission.py b/Tools/autotest/run_mission.py new file mode 100755 index 0000000000000..53721d76ee2c1 --- /dev/null +++ b/Tools/autotest/run_mission.py @@ -0,0 +1,107 @@ +#!/usr/bin/python3 + +''' +Run a mission in SITL + +AP_FLAKE8_CLEAN +''' + +import vehicle_test_suite +import os +import sys +import argparse + +from pysim import util + + +class RunMission(vehicle_test_suite.TestSuite): + def __init__(self, vehicle_binary, model, mission_filepath, speedup=None, sim_rate_hz=None): + super(RunMission, self).__init__(vehicle_binary) + self.mission_filepath = mission_filepath + self.model = model + self.speedup = speedup + self.sim_rate_hz = sim_rate_hz + + if self.speedup is None: + self.speedup = 100 + + def vehicleinfo_key(self): + '''magically guess vehicleinfo_key from filepath''' + path = self.binary.lower() + if "plane" in path: + return "ArduPlane" + if "copter" in path: + return "ArduCopter" + raise ValueError("Can't determine vehicleinfo_key from binary path") + + def run(self): + self.start_SITL( + binary=self.binary, + model=self.model, + sitl_home=self.sitl_home_string_from_mission_filepath(self.mission_filepath), + speedup=self.speedup, + sim_rate_hz=self.sim_rate_hz, + defaults_filepath=self.model_defaults_filepath(self.model), + ) + self.get_mavlink_connection_going() + + # hack; Plane defaults are annoying... we should do better + # here somehow. + if self.vehicleinfo_key() == "ArduPlane": + self.set_parameter("RTL_AUTOLAND", 1) + + self.load_mission_from_filepath(self.mission_filepath, strict=False) + self.change_mode('AUTO') + self.set_streamrate(1) + self.wait_ready_to_arm() + self.arm_vehicle() + self.wait_disarmed(timeout=600) + self.stop_SITL() + + +if __name__ == "__main__": + ''' main program ''' + os.environ['PYTHONUNBUFFERED'] = '1' + + if sys.platform != "darwin": + os.putenv('TMPDIR', util.reltopdir('tmp')) + + parser = argparse.ArgumentParser("run_mission.py") + parser.add_argument( + 'vehicle_binary', + type=str, + help='vehicle binary to use' + ) + parser.add_argument( + 'model', + type=str, + help='vehicle model to use' + ) + parser.add_argument( + 'mission_filepath', + type=str, + help='mission file path' + ) + parser.add_argument( + '--speedup', + type=int, + help='simulation speedup', + default=None, + ) + parser.add_argument( + '--sim-rate-hz', + type=int, + help='simulation physics rate', + default=None, + ) + + args = parser.parse_args() + + x = RunMission( + args.vehicle_binary, + args.model, + args.mission_filepath, + speedup=args.speedup, + sim_rate_hz=args.sim_rate_hz + ) + x.run() diff --git a/Tools/autotest/sailboat.py b/Tools/autotest/sailboat.py index a67fe0a5ad8ea..4c85ae33a4afe 100644 --- a/Tools/autotest/sailboat.py +++ b/Tools/autotest/sailboat.py @@ -11,8 +11,8 @@ from rover import AutoTestRover -from common import AutoTestTimeoutException -from common import PreconditionFailedException +from vehicle_test_suite import AutoTestTimeoutException +from vehicle_test_suite import PreconditionFailedException # get location of scripts testdir = os.path.dirname(os.path.realpath(__file__)) diff --git a/Tools/autotest/sim_vehicle.py b/Tools/autotest/sim_vehicle.py index 3f80bbd1819ed..065b5265c1c53 100755 --- a/Tools/autotest/sim_vehicle.py +++ b/Tools/autotest/sim_vehicle.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 """ Framework to start a simulated vehicle and connect it to MAVProxy. @@ -378,8 +378,8 @@ def do_build(opts, frame_options): if opts.math_check_indexes: cmd_configure.append("--enable-math-check-indexes") - if opts.disable_ekf2: - cmd_configure.append("--disable-ekf2") + if opts.enable_ekf2: + cmd_configure.append("--enable-ekf2") if opts.disable_ekf3: cmd_configure.append("--disable-ekf3") @@ -393,8 +393,8 @@ def do_build(opts, frame_options): if opts.ekf_single: cmd_configure.append("--ekf-single") - if opts.sitl_32bit: - cmd_configure.append("--sitl-32bit") + if opts.force_32bit: + cmd_configure.append("--force-32bit") if opts.ubsan: cmd_configure.append("--ubsan") @@ -408,11 +408,21 @@ def do_build(opts, frame_options): for nv in opts.define: cmd_configure.append("--define=%s" % nv) + if opts.enable_dds: + cmd_configure.append("--enable-dds") + + if opts.disable_networking: + cmd_configure.append("--disable-networking") + + if opts.enable_networking_tests: + cmd_configure.append("--enable-networking-tests") + pieces = [shlex.split(x) for x in opts.waf_configure_args] for piece in pieces: cmd_configure.extend(piece) - run_cmd_blocking("Configure waf", cmd_configure, check=True) + if not cmd_opts.no_configure: + run_cmd_blocking("Configure waf", cmd_configure, check=True) if opts.clean: run_cmd_blocking("Building clean", [waf_light, "clean"]) @@ -643,13 +653,10 @@ def start_antenna_tracker(opts): oldpwd = os.getcwd() os.chdir(vehicledir) tracker_uarta = "tcp:127.0.0.1:" + str(5760 + 10 * tracker_instance) - if cmd_opts.build_system == "waf": - binary_basedir = "build/sitl" - exe = os.path.join(root_dir, - binary_basedir, - "bin/antennatracker") - else: - exe = os.path.join(vehicledir, "AntennaTracker.elf") + binary_basedir = "build/sitl" + exe = os.path.join(root_dir, + binary_basedir, + "bin/antennatracker") run_in_terminal_window("AntennaTracker", ["nice", exe, @@ -659,16 +666,50 @@ def start_antenna_tracker(opts): os.chdir(oldpwd) -def start_CAN_GPS(opts): - """Compile and run the sitl_periph_gps""" +def start_CAN_Periph(opts, frame_info): + """Compile and run the sitl_periph""" global can_uarta progress("Preparing sitl_periph_gps") options = vinfo.options["sitl_periph_gps"]['frames']['gps'] - do_build(opts, options) + defaults_path = frame_info.get('periph_params_filename', None) + if defaults_path is None: + defaults_path = options.get('default_params_filename', None) + + if not isinstance(defaults_path, list): + defaults_path = [defaults_path] + + # add in path and make a comma separated list + defaults_path = ','.join([util.relcurdir(os.path.join(autotest_dir, p)) for p in defaults_path]) + + if not cmd_opts.no_rebuild: + do_build(opts, options) exe = os.path.join(root_dir, 'build/sitl_periph_gps', 'bin/AP_Periph') - run_in_terminal_window("sitl_periph_gps", - ["nice", exe]) + cmd = ["nice"] + cmd_name = "sitl_periph_gps" + if opts.valgrind: + cmd_name += " (valgrind)" + cmd.append("valgrind") + # adding this option allows valgrind to cope with the overload + # of operator new + cmd.append("--soname-synonyms=somalloc=nouserintercepts") + cmd.append("--track-origins=yes") + if opts.gdb or opts.gdb_stopped: + cmd_name += " (gdb)" + cmd.append("gdb") + gdb_commands_file = tempfile.NamedTemporaryFile(mode='w', delete=False) + atexit.register(os.unlink, gdb_commands_file.name) + gdb_commands_file.write("set pagination off\n") + if not opts.gdb_stopped: + gdb_commands_file.write("r\n") + gdb_commands_file.close() + cmd.extend(["-x", gdb_commands_file.name]) + cmd.append("--args") + cmd.append(exe) + if defaults_path is not None: + cmd.append("--defaults") + cmd.append(defaults_path) + run_in_terminal_window(cmd_name, cmd) def start_vehicle(binary, opts, stuff, spawns=None): @@ -748,7 +789,7 @@ def start_vehicle(binary, opts, stuff, spawns=None): print("The parameter file (%s) does not exist" % (x,)) sys.exit(1) path = ",".join(paths) - if cmd_opts.count > 1: + if cmd_opts.count > 1 or opts.auto_sysid: # we are in a subdirectory when using -n path = os.path.join("..", path) progress("Using defaults from (%s)" % (path,)) @@ -837,7 +878,7 @@ def start_mavproxy(opts, stuff): for i in instances: if not opts.no_extra_ports: - ports = [p + 10 * i for p in [14550, 14551]] + ports = [14550 + 10 * i] for port in ports: if under_vagrant(): # We're running inside of a vagrant guest; forward our @@ -954,6 +995,20 @@ def generate_frame_help(): return ret +# map from some vehicle aliases back to directory names. APMrover2 +# was the old name / directory name for Rover. +vehicle_map = { + "APMrover2": "Rover", + "Copter": "ArduCopter", + "Plane": "ArduPlane", + "Sub": "ArduSub", + "Blimp" : "Blimp", + "Rover": "Rover", +} +# add lower-case equivalents too +for k in list(vehicle_map.keys()): + vehicle_map[k.lower()] = vehicle_map[k] + # define and run parser parser = CompatOptionParser( "sim_vehicle.py", @@ -965,15 +1020,10 @@ def generate_frame_help(): "simulate ArduPlane") vehicle_choices = list(vinfo.options.keys()) -# add an alias for people with too much m -vehicle_choices.append("APMrover2") -vehicle_choices.append("Copter") # should change to ArduCopter at some stage -vehicle_choices.append("Plane") # should change to ArduPlane at some stage -vehicle_choices.append("Sub") # should change to Sub at some stage -vehicle_choices.append("copter") # should change to ArduCopter at some stage -vehicle_choices.append("plane") # should change to ArduPlane at some stage -vehicle_choices.append("sub") # should change to Sub at some stage -vehicle_choices.append("blimp") # should change to Blimp at some stage + +# add vehicle aliases to argument parser options: +for c in vehicle_map.keys(): + vehicle_choices.append(c) parser.add_option("-v", "--vehicle", type='choice', @@ -999,6 +1049,10 @@ def generate_frame_help(): action='store_true', default=False, help="don't rebuild before starting ardupilot") +group_build.add_option("--no-configure", + action='store_true', + default=False, + help="don't run waf configure before building") group_build.add_option("-D", "--debug", action='store_true', default=False, @@ -1016,20 +1070,15 @@ def generate_frame_help(): default=None, type='string', help="override SITL build target") -group_build.add_option("-s", "--build-system", - default="waf", - type='choice', - choices=["make", "waf"], - help="build system to use") group_build.add_option("--enable-math-check-indexes", default=False, action="store_true", dest="math_check_indexes", help="enable checking of math indexes") -group_build.add_option("", "--sitl-32bit", +group_build.add_option("", "--force-32bit", default=False, action='store_true', - dest="sitl_32bit", + dest="force_32bit", help="compile sitl using 32-bit") group_build.add_option("", "--configure-define", default=[], @@ -1102,10 +1151,10 @@ def generate_frame_help(): group_sim.add_option("", "--enable-onvif", action="store_true", help="enable onvif camera control sim using AntennaTracker") -group_sim.add_option("", "--can-gps", +group_sim.add_option("", "--can-peripherals", action='store_true', default=False, - help="start a DroneCAN GPS instance (use Tools/scripts/CAN/can_sitl_nodev.sh first)") + help="start a DroneCAN peripheral instance") group_sim.add_option("-A", "--sitl-instance-args", type='string', default=None, @@ -1246,7 +1295,7 @@ def generate_frame_help(): group_sim.add_option("--fram-storage", action='store_true', help="use fram storage emulation") -group_sim.add_option("--disable-ekf2", +group_sim.add_option("--enable-ekf2", action='store_true', help="disable EKF2 in build") group_sim.add_option("--disable-ekf3", @@ -1281,6 +1330,13 @@ def generate_frame_help(): type=str, default="127.0.0.1", help="IP address of the simulator. Defaults to localhost") +group_sim.add_option("--enable-dds", action='store_true', + help="Enable the dds client to connect with ROS2/DDS") +group_sim.add_option("--disable-networking", action='store_true', + help="Disable networking APIs") +group_sim.add_option("--enable-networking-tests", action='store_true', + help="Enable networking tests") + parser.add_option_group(group_sim) @@ -1395,22 +1451,10 @@ def generate_frame_help(): break cwd = os.path.dirname(cwd) -# map from some vehicle aliases back to canonical names. APMrover2 -# was the old name / directory name for Rover. -vehicle_map = { - "APMrover2": "Rover", - "Copter": "ArduCopter", # will switch eventually - "Plane": "ArduPlane", # will switch eventually - "Sub": "ArduSub", # will switch eventually - "copter": "ArduCopter", # will switch eventually - "plane": "ArduPlane", # will switch eventually - "sub": "ArduSub", # will switch eventually - "blimp" : "Blimp", # will switch eventually -} if cmd_opts.vehicle in vehicle_map: - progress("%s is now known as %s" % - (cmd_opts.vehicle, vehicle_map[cmd_opts.vehicle])) cmd_opts.vehicle = vehicle_map[cmd_opts.vehicle] +elif cmd_opts.vehicle.lower() in vehicle_map: + cmd_opts.vehicle = vehicle_map[cmd_opts.vehicle.lower()] # try to validate vehicle if cmd_opts.vehicle not in vinfo.options: @@ -1452,8 +1496,8 @@ def generate_frame_help(): if cmd_opts.tracker: start_antenna_tracker(cmd_opts) -if cmd_opts.can_gps: - start_CAN_GPS(cmd_opts) +if cmd_opts.can_peripherals or frame_infos.get('periph_params_filename', None) is not None: + start_CAN_Periph(cmd_opts, frame_infos) if cmd_opts.custom_location: location = [(float)(x) for x in cmd_opts.custom_location.split(",")] @@ -1517,13 +1561,11 @@ def generate_frame_help(): if cmd_opts.vehicle_binary is not None: vehicle_binary = cmd_opts.vehicle_binary - elif cmd_opts.build_system == "waf": + else: binary_basedir = "build/sitl" vehicle_binary = os.path.join(root_dir, binary_basedir, frame_infos["waf_target"]) - else: - vehicle_binary = os.path.join(vehicle_dir, cmd_opts.vehicle + ".elf") if not os.path.exists(vehicle_binary): print("Vehicle binary (%s) does not exist" % (vehicle_binary,)) @@ -1585,7 +1627,7 @@ def generate_frame_help(): entities[i][k] = v config['entities'] = list(entities.values()) env = Environment(loader=FileSystemLoader(os.path.join(autotest_dir, 'template'))) - mission = env.get_template('scrimmage.xml').render(**config) + mission = env.get_template('scrimmage.xml.j2').render(**config) tmp = mkstemp() atexit.register(os.remove, tmp[1]) diff --git a/Tools/autotest/template/scrimmage.xml b/Tools/autotest/template/scrimmage.xml.j2 similarity index 100% rename from Tools/autotest/template/scrimmage.xml rename to Tools/autotest/template/scrimmage.xml.j2 diff --git a/Tools/autotest/test_build_options.py b/Tools/autotest/test_build_options.py index 50e9626d0e664..839269415554c 100755 --- a/Tools/autotest/test_build_options.py +++ b/Tools/autotest/test_build_options.py @@ -23,9 +23,13 @@ import fnmatch import optparse import os +import sys from pysim import util +sys.path.insert(1, os.path.join(os.path.dirname(__file__), '..', 'scripts')) +import extract_features # noqa + class TestBuildOptionsResult(object): '''object to return results from a comparison''' @@ -70,7 +74,14 @@ def must_have_defines_for_board(self, board): 'AP_COMPASS_AK8963_ENABLED', 'AP_COMPASS_AK09916_ENABLED', 'AP_COMPASS_ICM20948_ENABLED', - ]) + ]), + "CubeBlack": frozenset([ + 'AP_BARO_MS56XX_ENABLED', + 'AP_COMPASS_LSM303D_ENABLED', + 'AP_COMPASS_AK8963_ENABLED', + 'AP_COMPASS_AK09916_ENABLED', + 'AP_COMPASS_ICM20948_ENABLED', + ]), } return must_have_defines.get(board, frozenset([])) @@ -160,6 +171,29 @@ def test_disable_feature(self, feature, options): self.test_compile_with_defines(defines) + # if the feature is truly disabled then extract_features.py + # should say so: + for target in self.build_targets: + path = self.target_to_elf_path(target) + extracter = extract_features.ExtractFeatures(path) + (compiled_in_feature_defines, not_compiled_in_feature_defines) = extracter.extract() + for define in defines: + # the following defines are known not to work on some + # or all vehicles: + feature_define_whitelist = set([ + 'AP_RANGEFINDER_ENABLED', # only at vehicle level ATM + 'AC_AVOID_ENABLED', # Rover doesn't obey this + 'AC_OAPATHPLANNER_ENABLED', # Rover doesn't obey this + 'BEACON_ENABLED', # Rover doesn't obey this (should also be AP_BEACON_ENABLED) + 'WINCH_ENABLED', # Copter doesn't use this; should use AP_WINCH_ENABLED + ]) + if define in compiled_in_feature_defines: + error = f"feature gated by {define} still compiled into ({target}); extract_features.py bug?" + if define in feature_define_whitelist: + print("warn: " + error) + else: + raise ValueError(error) + def test_enable_feature(self, feature, options): defines = self.get_enable_defines(feature, options) @@ -195,9 +229,9 @@ def test_compile_with_defines(self, defines): (t,)) raise - def find_build_sizes(self): - '''returns a hash with size of all build targets''' - ret = {} + def target_to_path(self, target, extension=None): + '''given a build target (e.g. copter), return expected path to .bin + file for that target''' target_to_binpath = { "copter": "arducopter", "plane": "arduplane", @@ -206,8 +240,26 @@ def find_build_sizes(self): "sub": "ardusub", "blimp": "blimp", } + filename = target_to_binpath[target] + if extension is not None: + filename += "." + extension + return os.path.join("build", self.board(), "bin", filename) + + def target_to_bin_path(self, target): + '''given a build target (e.g. copter), return expected path to .bin + file for that target''' + return self.target_to_path(target, 'bin') + + def target_to_elf_path(self, target): + '''given a build target (e.g. copter), return expected path to .elf + file for that target''' + return self.target_to_path(target) + + def find_build_sizes(self): + '''returns a hash with size of all build targets''' + ret = {} for target in self.build_targets: - path = os.path.join("build", self.board(), "bin", "%s.bin" % target_to_binpath[target]) + path = self.target_to_bin_path(target) ret[target] = os.path.getsize(path) return ret @@ -249,13 +301,20 @@ def disable_in_turn_check_sizes(self, feature, sizes_nothing_disabled): def run_disable_in_turn(self): options = self.get_build_options_from_ardupilot_tree() - if self.match_glob is not None: - options = list(filter(lambda x : fnmatch.fnmatch(x.define, self.match_glob), options)) count = 1 for feature in sorted(options, key=lambda x : x.define): + if self.match_glob is not None: + if not fnmatch.fnmatch(feature.define, self.match_glob): + continue + with open("/tmp/run-disable-in-turn-progress", "w") as f: + f.write(f"{count}/{len(options)} {feature.define}\n") + # if feature.define < "WINCH_ENABLED": + # count += 1 + # continue if feature.define in self.must_have_defines_for_board(self._board): self.progress("Feature %s(%s) (%u/%u) is a MUST-HAVE" % (feature.label, feature.define, count, len(options))) + count += 1 continue self.progress("Disabling feature %s(%s) (%u/%u)" % (feature.label, feature.define, count, len(options))) @@ -265,12 +324,15 @@ def run_disable_in_turn(self): def run_enable_in_turn(self): options = self.get_build_options_from_ardupilot_tree() - if self.match_glob is not None: - options = list(filter(lambda x : fnmatch.fnmatch(x.define, self.match_glob), options)) count = 1 for feature in options: + if self.match_glob is not None: + if not fnmatch.fnmatch(feature.define, self.match_glob): + continue self.progress("Enabling feature %s(%s) (%u/%u)" % (feature.label, feature.define, count, len(options))) + with open("/tmp/run-enable-in-turn-progress", "w") as f: + f.write(f"{count}/{len(options)} {feature.define}\n") self.test_enable_feature(feature, options) count += 1 diff --git a/Tools/autotest/common.py b/Tools/autotest/vehicle_test_suite.py similarity index 88% rename from Tools/autotest/common.py rename to Tools/autotest/vehicle_test_suite.py index d5369983377e4..b94581334ae6f 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -18,6 +18,12 @@ import sys import time import traceback +from datetime import datetime +from typing import List +from typing import Tuple +from typing import Dict +import importlib.util + import pexpect import fnmatch import operator @@ -28,6 +34,7 @@ import tempfile import threading import enum +from pathlib import Path from MAVProxy.modules.lib import mp_util from MAVProxy.modules.lib import mp_elevation @@ -37,6 +44,7 @@ from pymavlink import mavextra from pymavlink.rotmat import Vector3 from pymavlink import quaternion +from pymavlink.generator import mavgen from pysim import util, vehicleinfo @@ -167,6 +175,11 @@ class NotAchievedException(ErrorException): pass +class OldpymavlinkException(ErrorException): + """Thrown when a new feature is required from pymavlink""" + pass + + class YawSpeedNotAchievedException(NotAchievedException): """Thrown when fails to achieve given yaw speed.""" pass @@ -196,11 +209,14 @@ def __init__(self): self.heartbeat_interval_ms = 1000 self.original_heartbeat_interval_ms = None self.installed_scripts = [] + self.installed_modules = [] + self.overridden_message_rates = {} # https://stackoverflow.com/questions/616645/how-do-i-duplicate-sys-stdout-to-a-log-file-in-python class TeeBoth(object): - def __init__(self, name, mode, mavproxy_logfile): + def __init__(self, name, mode, mavproxy_logfile, suppress_stdout=False): + self.suppress_stdout = suppress_stdout self.file = open(name, mode) self.stdout = sys.stdout self.stderr = sys.stderr @@ -218,8 +234,11 @@ def close(self): self.file = None def write(self, data): + if isinstance(data, bytes): + data = data.decode('ascii') self.file.write(data) - self.stdout.write(data) + if not self.suppress_stdout: + self.stdout.write(data) def flush(self): self.file.flush() @@ -1436,12 +1455,13 @@ def __init__(self, lat, lon, alt, yaw): class Test(object): '''a test definition - information about a test''' - def __init__(self, function, attempts=1, speedup=None): + def __init__(self, function, kwargs={}, attempts=1, speedup=None): self.name = function.__name__ self.description = function.__doc__ if self.description is None: raise ValueError("%s is missing a docstring" % self.name) self.function = function + self.kwargs = kwargs self.attempts = attempts self.speedup = speedup @@ -1453,23 +1473,25 @@ def __init__(self, test): self.reason = None self.exception = None self.debug_filename = None + self.time_elapsed = 0.0 # self.passed = False def __str__(self): ret = " %s (%s)" % (self.test.name, self.test.description) if self.passed: - return ret + " OK" + return f"{ret} OK" if self.reason is not None: - ret += " (" + self.reason + ")" + ret += f" ({self.reason} )" if self.exception is not None: - ret += " (" + str(self.exception) + ")" + ret += f" ({str(self.exception)})" if self.debug_filename is not None: - ret += " (see " + self.debug_filename + ")" - + ret += f" (see {self.debug_filename})" + if self.time_elapsed is not None: + ret += f" (duration {self.time_elapsed} s)" return ret -class AutoTest(ABC): +class TestSuite(ABC): """Base abstract class. It implements the common function for all vehicle types. """ @@ -1494,10 +1516,12 @@ def __init__(self, replay=False, sup_binaries=[], reset_after_every_test=False, - sitl_32bit=False, + force_32bit=False, ubsan=False, ubsan_abort=False, num_aux_imus=0, + dronecan_tests=False, + generate_junit=False, build_opts={}): self.start_time = time.time() @@ -1521,11 +1545,19 @@ def __init__(self, self.speedup = self.default_speedup() self.sup_binaries = sup_binaries self.reset_after_every_test = reset_after_every_test - self.sitl_32bit = sitl_32bit + self.force_32bit = force_32bit self.ubsan = ubsan self.ubsan_abort = ubsan_abort self.build_opts = build_opts self.num_aux_imus = num_aux_imus + self.generate_junit = generate_junit + if generate_junit: + try: + spec = importlib.util.find_spec("junitparser") + if spec is None: + raise ImportError + except ImportError as e: + raise ImportError(f"Junit export need junitparser package.\n {e} \nTry: python3 -m pip install junitparser") self.mavproxy = None self._mavproxy = None # for auto-cleanup on failed tests @@ -1578,6 +1610,8 @@ def __init__(self, offline=self.terrain_in_offline_mode ) self.terrain_data_messages_sent = 0 # count of messages back + self.dronecan_tests = dronecan_tests + self.statustext_id = 1 def __del__(self): if self.rc_thread is not None: @@ -1648,13 +1682,27 @@ def sitl_streamrate(self): """Allow subclasses to override SITL streamrate.""" return 10 + def adjust_ardupilot_port(self, port): + '''adjust port in case we do not wish to use the default range (5760 and 5501 etc)''' + return port + + def spare_network_port(self, offset=0): + '''returns a network port which should be able to be bound''' + if offset > 2: + raise ValueError("offset too large") + return 8000 + offset + def autotest_connection_string_to_ardupilot(self): - return "tcp:127.0.0.1:5760" + return "tcp:127.0.0.1:%u" % self.adjust_ardupilot_port(5760) + + def sitl_rcin_port(self, offset=0): + if offset > 2: + raise ValueError("offset too large") + return 5501 + offset def mavproxy_options(self): """Returns options to be passed to MAVProxy.""" ret = [ - '--sitl=127.0.0.1:5502', '--streamrate=%u' % self.sitl_streamrate(), '--target-system=%u' % self.sysid_thismav(), '--target-component=1', @@ -1863,41 +1911,21 @@ def reboot_check_valgrind_log(self): shutil.move(valgrind_log, backup_valgrind_log) def run_cmd_reboot(self): - self.run_cmd(mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, - 1, # confirmation - 1, # reboot autopilot - 0, - 0, - 0, - 0, - 0, - 0) - - def run_cmd_run_prearms(self): - self.run_cmd(mavutil.mavlink.MAV_CMD_RUN_PREARM_CHECKS, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0) - - def run_cmd_enable_high_latency(self, new_state): + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, + p1=1, # reboot autopilot + ) + + def run_cmd_enable_high_latency(self, new_state, run_cmd=None): + if run_cmd is None: + run_cmd = self.run_cmd p1 = 0 if new_state: p1 = 1 - self.run_cmd( + run_cmd( mavutil.mavlink.MAV_CMD_CONTROL_HIGH_LATENCY, - p1, # p1 - enable/disable - 0, # p2 - 0, # p3 - 0, # p4 - 0, # p5 - 0, # p6 - 0, # p7 + p1=p1, # enable/disable ) def reboot_sitl_mav(self, required_bootcount=None, force=False): @@ -1937,13 +1965,10 @@ def reboot_sitl_mav(self, required_bootcount=None, force=False): p6 = 20190226 # magic force-reboot value self.send_cmd( mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, - 1, - 1, - 0, - 0, - 0, - p6, - 0) + p1=1, + p2=1, + p6=p6, + ) do_context = True if do_context: self.context_push() @@ -1984,7 +2009,8 @@ def reboot_sitl(self, required_bootcount=None, force=False): self.progress("Rebooting SITL") self.reboot_sitl_mav(required_bootcount=required_bootcount, force=force) self.do_heartbeats(force=True) - self.assert_simstate_location_is_at_startup_location() + if self.frame != 'sailboat': # sailboats drift with wind! + self.assert_simstate_location_is_at_startup_location() def reboot_sitl_mavproxy(self, required_bootcount=None): """Reboot SITL instance using MAVProxy and wait for it to reconnect.""" @@ -2032,12 +2058,13 @@ def scripting_restart(self): self.progress("Restarting Scripting") self.run_cmd_int( mavutil.mavlink.MAV_CMD_SCRIPTING, - mavutil.mavlink.SCRIPTING_CMD_STOP_AND_RESTART, - 0, 0, 0, 0, 0, 0, - timeout=5) + p1=mavutil.mavlink.SCRIPTING_CMD_STOP_AND_RESTART, + timeout=5, + ) def set_streamrate(self, streamrate, timeout=20, stream=mavutil.mavlink.MAV_DATA_STREAM_ALL): '''set MAV_DATA_STREAM_ALL; timeout is wallclock time''' + self.do_timesync_roundtrip(timeout_in_wallclock=True) tstart = time.time() while True: if time.time() - tstart > timeout: @@ -2130,41 +2157,11 @@ def test_adsb_send_threatening_adsb_message(self, here, offset_ne=None): def get_sim_parameter_documentation_get_whitelist(self): # common parameters ret = set([ - "SIM_ACC1_BIAS_X", - "SIM_ACC1_BIAS_Y", - "SIM_ACC1_BIAS_Z", "SIM_ACC1_RND", - "SIM_ACC1_SCAL_X", - "SIM_ACC1_SCAL_Y", - "SIM_ACC1_SCAL_Z", - "SIM_ACC2_BIAS_X", - "SIM_ACC2_BIAS_Y", - "SIM_ACC2_BIAS_Z", "SIM_ACC2_RND", - "SIM_ACC2_SCAL_X", - "SIM_ACC2_SCAL_Y", - "SIM_ACC2_SCAL_Z", - "SIM_ACC3_BIAS_X", - "SIM_ACC3_BIAS_Y", - "SIM_ACC3_BIAS_Z", "SIM_ACC3_RND", - "SIM_ACC3_SCAL_X", - "SIM_ACC3_SCAL_Y", - "SIM_ACC3_SCAL_Z", "SIM_ACC4_RND", - "SIM_ACC4_SCAL_X", - "SIM_ACC4_SCAL_Y", - "SIM_ACC4_SCAL_Z", - "SIM_ACC4_BIAS_X", - "SIM_ACC4_BIAS_Y", - "SIM_ACC4_BIAS_Z", "SIM_ACC5_RND", - "SIM_ACC5_SCAL_X", - "SIM_ACC5_SCAL_Y", - "SIM_ACC5_SCAL_Z", - "SIM_ACC5_BIAS_X", - "SIM_ACC5_BIAS_Y", - "SIM_ACC5_BIAS_Z", "SIM_ACC_FILE_RW", "SIM_ACC_TRIM_X", "SIM_ACC_TRIM_Y", @@ -2173,19 +2170,10 @@ def get_sim_parameter_documentation_get_whitelist(self): "SIM_ADSB_COUNT", "SIM_ADSB_RADIUS", "SIM_ADSB_TX", - "SIM_ARSPD2_FAIL", - "SIM_ARSPD2_FAILP", "SIM_ARSPD2_OFS", - "SIM_ARSPD2_PITOT", - "SIM_ARSPD2_RATIO", "SIM_ARSPD2_RND", - "SIM_ARSPD2_SIGN", - "SIM_ARSPD_FAILP", "SIM_ARSPD_OFS", - "SIM_ARSPD_PITOT", - "SIM_ARSPD_RATIO", "SIM_ARSPD_RND", - "SIM_ARSPD_SIGN", "SIM_BAR2_DELAY", "SIM_BAR2_DISABLE", "SIM_BAR2_DRIFT", @@ -2218,7 +2206,6 @@ def get_sim_parameter_documentation_get_whitelist(self): "SIM_BARO_WCF_RGT", "SIM_BARO_WCF_UP", "SIM_BATT_CAP_AH", - "SIM_BATT_VOLTAGE", "SIM_BAUDLIMIT_EN", "SIM_DRIFT_SPEED", "SIM_DRIFT_TIME", @@ -2226,86 +2213,19 @@ def get_sim_parameter_documentation_get_whitelist(self): "SIM_ENGINE_FAIL", "SIM_ENGINE_MUL", "SIM_ESC_ARM_RPM", - "SIM_FLOW_DELAY", - "SIM_FLOW_ENABLE", - "SIM_FLOW_POS_X", - "SIM_FLOW_POS_Y", - "SIM_FLOW_POS_Z", - "SIM_FLOW_RATE", - "SIM_FLOW_RND", "SIM_FTOWESC_ENA", "SIM_FTOWESC_POW", "SIM_GND_BEHAV", - "SIM_GPS2_ACC", - "SIM_GPS2_ALT_OFS", - "SIM_GPS2_BYTELOS", - "SIM_GPS2_DRFTALT", - "SIM_GPS2_GLTCH_X", - "SIM_GPS2_GLTCH_Y", - "SIM_GPS2_GLTCH_Z", - "SIM_GPS2_HDG", - "SIM_GPS2_HZ", - "SIM_GPS2_LAG_MS", - "SIM_GPS2_LCKTIME", - "SIM_GPS2_NOISE", - "SIM_GPS2_NUMSATS", - "SIM_GPS2_POS_X", - "SIM_GPS2_POS_Y", - "SIM_GPS2_POS_Z", - "SIM_GPS2_TYPE", - "SIM_GPS2_VERR_X", - "SIM_GPS2_VERR_Y", - "SIM_GPS2_VERR_Z", - "SIM_GPS_ACC", - "SIM_GPS_ALT_OFS", - "SIM_GPS_BYTELOSS", - "SIM_GPS_DRIFTALT", - "SIM_GPS_GLITCH_X", - "SIM_GPS_GLITCH_Y", - "SIM_GPS_GLITCH_Z", - "SIM_GPS_HDG", - "SIM_GPS_HZ", - "SIM_GPS_LAG_MS", - "SIM_GPS_LOCKTIME", - "SIM_GPS_LOG_NUM", - "SIM_GPS_NOISE", - "SIM_GPS_NUMSATS", - "SIM_GPS_POS_X", - "SIM_GPS_POS_Y", - "SIM_GPS_POS_Z", - "SIM_GPS_TYPE", - "SIM_GPS_VERR_X", - "SIM_GPS_VERR_Y", - "SIM_GPS_VERR_Z", "SIM_GYR1_RND", - "SIM_GYR1_SCALE_X", - "SIM_GYR1_SCALE_Y", - "SIM_GYR1_SCALE_Z", "SIM_GYR2_RND", - "SIM_GYR2_SCALE_X", - "SIM_GYR2_SCALE_Y", - "SIM_GYR2_SCALE_Z", "SIM_GYR3_RND", - "SIM_GYR3_SCALE_X", - "SIM_GYR3_SCALE_Y", - "SIM_GYR3_SCALE_Z", "SIM_GYR4_RND", - "SIM_GYR4_SCALE_X", - "SIM_GYR4_SCALE_Y", - "SIM_GYR4_SCALE_Z", "SIM_GYR5_RND", - "SIM_GYR5_SCALE_X", - "SIM_GYR5_SCALE_Y", - "SIM_GYR5_SCALE_Z", "SIM_GYR_FAIL_MSK", "SIM_GYR_FILE_RW", "SIM_IE24_ENABLE", "SIM_IE24_ERROR", "SIM_IE24_STATE", - "SIM_IMU_COUNT", - "SIM_IMU_POS_X", - "SIM_IMU_POS_Y", - "SIM_IMU_POS_Z", "SIM_IMUT1_ACC1_X", "SIM_IMUT1_ACC1_Y", "SIM_IMUT1_ACC1_Z", @@ -2415,13 +2335,9 @@ def get_sim_parameter_documentation_get_whitelist(self): "SIM_IMUT_FIXED", "SIM_IMUT_START", "SIM_IMUT_TCONST", - "SIM_INIT_ALT_OFS", - "SIM_INIT_LAT_OFS", - "SIM_INIT_LON_OFS", "SIM_INS_THR_MIN", "SIM_LED_LAYOUT", "SIM_LOOP_DELAY", - "SIM_MAG1_DEVID", "SIM_MAG1_SCALING", "SIM_MAG2_DEVID", "SIM_MAG2_DIA_X", @@ -2481,7 +2397,6 @@ def get_sim_parameter_documentation_get_whitelist(self): "SIM_RC_CHANCOUNT", "SIM_RICH_CTRL", "SIM_RICH_ENABLE", - "SIM_SAFETY_STATE", "SIM_SERVO_SPEED", "SIM_SHIP_DSIZE", "SIM_SHIP_ENABLE", @@ -2624,7 +2539,7 @@ def test_parameter_documentation_get_all_parameters(self): def find_format_defines(self, lines): ret = {} for line in lines: - if type(line) == bytes: + if isinstance(line, bytes): line = line.decode("utf-8") m = re.match(r'#define (\w+_(?:LABELS|FMT|UNITS|MULTS))\s+(".*")', line) if m is None: @@ -2681,7 +2596,7 @@ def all_log_format_ids(self): for line in open(f).readlines(): if debug: print("line: %s" % line) - if type(line) == bytes: + if isinstance(line, bytes): line = line.decode("utf-8") line = re.sub("//.*", "", line) # trim comments if re.match(r"\s*$", line): @@ -2760,7 +2675,7 @@ def all_log_format_ids(self): linestate_within = 90 linestate = linestate_none for line in open(filepath, 'rb').readlines(): - if type(line) == bytes: + if isinstance(line, bytes): line = line.decode("utf-8") line = re.sub("//.*", "", line) # trim comments if re.match(r"\s*$", line): @@ -2780,7 +2695,9 @@ def all_log_format_ids(self): continue if "#if FRAME_CONFIG == HELI_FRAME" in line: continue - if "#if PRECISION_LANDING == ENABLED" in line: + if "#if AC_PRECLAND_ENABLED" in line: + continue + if "#if OFFBOARD_GUIDED == ENABLED" in line: continue if "#end" in line: continue @@ -2858,7 +2775,7 @@ def all_log_format_ids(self): continue count = 0 for line in open(filepath, 'rb').readlines(): - if type(line) == bytes: + if isinstance(line, bytes): line = line.decode("utf-8") if state == state_outside: if (re.match(r"\s*AP::logger\(\)[.]Write(?:Streaming)?\(", line) or @@ -2958,7 +2875,7 @@ def LoggerDocumentation(self): REPLAY_MSGS = ['RFRH', 'RFRF', 'REV2', 'RSO2', 'RWA2', 'REV3', 'RSO3', 'RWA3', 'RMGI', 'REY3', 'RFRN', 'RISH', 'RISI', 'RISJ', 'RBRH', 'RBRI', 'RRNH', 'RRNI', 'RGPH', 'RGPI', 'RGPJ', 'RASH', 'RASI', 'RBCH', 'RBCI', 'RVOH', 'RMGH', - 'ROFH', 'REPH', 'REVH', 'RWOH', 'RBOH'] + 'ROFH', 'REPH', 'REVH', 'RWOH', 'RBOH', 'RSLL'] docco_ids = {} for thing in tree.logformat: @@ -3228,7 +3145,7 @@ def message_hook(self, mav, msg): """Called as each mavlink msg is received.""" # print("msg: %s" % str(msg)) if msg.get_type() == 'STATUSTEXT': - self.progress("AP: %s" % msg.text) + self.progress("AP: %s" % msg.text, send_statustext=False) self.write_msg_to_tlog(msg) @@ -3363,17 +3280,19 @@ def do_timesync_roundtrip(self, quiet=False, timeout_in_wallclock=False): self.progress("Received: %s" % str(m)) if m is None: continue - if m.tc1 == 0: - self.progress("this is a timesync request, which we don't answer") - continue if m.ts1 % 1000 != self.mav.source_system: self.progress("this isn't a response to our timesync (%s)" % (m.ts1 % 1000)) continue + if m.tc1 == 0: + # this should also not happen: + self.progress("this is a timesync request, which we don't answer") + continue if int(m.ts1 / 1000) != self.timesync_number: self.progress("this isn't the one we just sent") continue if m.get_srcSystem() != self.mav.target_system: - self.progress("response from system other than our target") + self.progress("response from system other than our target (want=%u got=%u" % + (self.mav.target_system, m.get_srcSystem())) continue # no component check ATM because we send broadcast... # if m.get_srcComponent() != self.mav.target_component: @@ -3446,6 +3365,17 @@ def HIGH_LATENCY2(self): raise NotAchievedException("Air Temperature not received from HIGH_LATENCY2") self.HIGH_LATENCY2_links() + def context_set_message_rate_hz(self, id, rate_hz, run_cmd=None): + if run_cmd is None: + run_cmd = self.run_cmd + + overridden_message_rates = self.context_get().overridden_message_rates + + if id not in overridden_message_rates: + overridden_message_rates[id] = self.measure_message_rate(id) + + self.set_message_rate_hz(id, rate_hz, run_cmd=run_cmd) + def HIGH_LATENCY2_links(self): self.start_subtest("SerialProtocol_MAVLinkHL links") @@ -3460,7 +3390,7 @@ def HIGH_LATENCY2_links(self): self.reboot_sitl() mav2 = mavutil.mavlink_connection( - "tcp:localhost:5763", + "tcp:localhost:%u" % self.adjust_ardupilot_port(5763), robust_parsing=True, source_system=7, source_component=7, @@ -3471,16 +3401,17 @@ def HIGH_LATENCY2_links(self): self.assert_not_receive_message('HIGH_LATENCY2', mav=mav, timeout=10) self.start_subsubtest("Get HIGH_LATENCY2 upon link enabled only on HL link") - self.run_cmd_enable_high_latency(True) - self.assert_receive_message("HIGH_LATENCY2", mav=mav2, timeout=10) - self.assert_not_receive_message("HIGH_LATENCY2", mav=self.mav, timeout=10) - - self.start_subsubtest("Not get HIGH_LATENCY2 upon HL disable") - self.run_cmd_enable_high_latency(False) - self.delay_sim_time(10) - self.assert_not_receive_message('HIGH_LATENCY2', mav=self.mav, timeout=10) - self.drain_mav(mav2) - self.assert_not_receive_message('HIGH_LATENCY2', mav=mav2, timeout=10) + for run_cmd in self.run_cmd, self.run_cmd_int: + self.run_cmd_enable_high_latency(True, run_cmd=run_cmd) + self.assert_receive_message("HIGH_LATENCY2", mav=mav2, timeout=10) + self.assert_not_receive_message("HIGH_LATENCY2", mav=self.mav, timeout=10) + + self.start_subsubtest("Not get HIGH_LATENCY2 upon HL disable") + self.run_cmd_enable_high_latency(False, run_cmd=run_cmd) + self.delay_sim_time(10) + self.assert_not_receive_message('HIGH_LATENCY2', mav=self.mav, timeout=10) + self.drain_mav(mav2) + self.assert_not_receive_message('HIGH_LATENCY2', mav=mav2, timeout=10) self.start_subsubtest("Stream rate adjustments") self.run_cmd_enable_high_latency(True) @@ -3526,34 +3457,13 @@ def HIGH_LATENCY2_links(self): if ex is not None: raise ex - def LogDownload(self): - '''Test Onboard Log Download''' - if self.is_tracker(): - # tracker starts armed, which is annoying - return - self.progress("Ensuring we have contents we care about") - self.set_parameter("LOG_FILE_DSRMROT", 1) - self.set_parameter("LOG_DISARMED", 0) - self.reboot_sitl() - original_log_list = self.log_list() - for i in range(0, 10): - self.wait_ready_to_arm() - self.arm_vehicle() - self.delay_sim_time(1) - self.disarm_vehicle() - new_log_list = self.log_list() - new_log_count = len(new_log_list) - len(original_log_list) - if new_log_count != 10: - raise NotAchievedException("Expected exactly 10 new logs got %u (%s) to (%s)" % - (new_log_count, original_log_list, new_log_list)) - self.progress("Directory contents: %s" % str(new_log_list)) - + def download_full_log_list(self, print_logs=True): tstart = self.get_sim_time() self.mav.mav.log_request_list_send(self.sysid_thismav(), 1, # target component 0, - 0xff) - logs = [] + 0xffff) + logs = {} last_id = None num_logs = None while True: @@ -3563,10 +3473,11 @@ def LogDownload(self): m = self.mav.recv_match(type='LOG_ENTRY', blocking=True, timeout=1) - self.progress("Received (%s)" % str(m)) + if print_logs: + self.progress("Received (%s)" % str(m)) if m is None: continue - logs.append(m) + logs[m.id] = m if last_id is None: if m.num_logs == 0: # caller to guarantee this works: @@ -3593,7 +3504,134 @@ def LogDownload(self): timeout=2) if m is not None: raise NotAchievedException("Received extra LOG_ENTRY?!") + return logs + def TestLogDownloadWrap(self): + """Test log wrapping.""" + if self.is_tracker(): + # tracker starts armed, which is annoying + return + self.progress("Ensuring we have contents we care about") + self.set_parameter("LOG_FILE_DSRMROT", 1) + self.set_parameter("LOG_DISARMED", 0) + self.reboot_sitl() + logspath = Path("logs") + + def create_num_logs(num_logs, logsdir, clear_logsdir=True): + if clear_logsdir: + shutil.rmtree(logsdir, ignore_errors=True) + logsdir.mkdir() + lastlogfile_path = logsdir / Path("LASTLOG.TXT") + self.progress(f"Add LASTLOG.TXT file with counter at {num_logs}") + with open(lastlogfile_path, 'w') as lastlogfile: + lastlogfile.write(f"{num_logs}\n") + self.progress(f"Create fakelogs from 1 to {num_logs} on logs directory") + for ii in range(1, num_logs + 1): + new_log = logsdir / Path(f"{str(ii).zfill(8)}.BIN") + with open(new_log, 'w+') as logfile: + logfile.write(f"I AM LOG {ii}\n") + logfile.write('1' * ii) + + def verify_logs(test_log_num): + try: + wrap = False + offset = 0 + max_logs_num = int(self.get_parameter("LOG_MAX_FILES")) + if test_log_num > max_logs_num: + wrap = True + offset = test_log_num - max_logs_num + test_log_num = max_logs_num + logs_dict = self.download_full_log_list(print_logs=False) + if len(logs_dict) != test_log_num: + raise NotAchievedException( + f"Didn't get the full log list, expect {test_log_num} got {len(logs_dict)}") + self.progress("Checking logs size are matching") + start_log = offset if wrap else 1 + for ii in range(start_log, test_log_num + 1 - offset): + log_i = logspath / Path(f"{str(ii + offset).zfill(8)}.BIN") + if logs_dict[ii].size != log_i.stat().st_size: + logs_dict = self.download_full_log_list(print_logs=False) + # sometimes we don't have finish writing the log, so get it again prevent failure + if logs_dict[ii].size != log_i.stat().st_size: + raise NotAchievedException( + f"Log{ii} size mismatch : {logs_dict[ii].size} vs {log_i.stat().st_size}" + ) + if wrap: + self.progress("Checking wrapped logs size are matching") + for ii in range(1, offset): + log_i = logspath / Path(f"{str(ii).zfill(8)}.BIN") + if logs_dict[test_log_num + 1 - offset + ii].size != log_i.stat().st_size: + self.progress(f"{logs_dict[test_log_num + 1 - offset + ii]}") + raise NotAchievedException( + f"Log{test_log_num + 1 - offset + ii} size mismatch :" + f" {logs_dict[test_log_num + 1 - offset + ii].size} vs {log_i.stat().st_size}" + ) + except NotAchievedException as e: + shutil.rmtree(logspath, ignore_errors=True) + logspath.mkdir() + with open(logspath / Path("LASTLOG.TXT"), 'w') as lastlogfile: + lastlogfile.write("1\n") + raise e + + def add_and_verify_log(test_log_num): + self.wait_ready_to_arm() + self.arm_vehicle() + self.delay_sim_time(1) + self.disarm_vehicle() + self.delay_sim_time(10) + verify_logs(test_log_num + 1) + + def create_and_verify_logs(test_log_num, clear_logsdir=True): + self.progress(f"Test {test_log_num} logs") + create_num_logs(test_log_num, logspath, clear_logsdir) + self.reboot_sitl() + verify_logs(test_log_num) + self.start_subsubtest("Adding one more log") + add_and_verify_log(test_log_num) + + self.start_subtest("Checking log list match with filesystem info") + create_and_verify_logs(500) + create_and_verify_logs(10) + create_and_verify_logs(1) + + self.start_subtest("Change LOG_MAX_FILES and Checking log list match with filesystem info") + self.set_parameter("LOG_MAX_FILES", 250) + create_and_verify_logs(250) + self.set_parameter("LOG_MAX_FILES", 1) + create_and_verify_logs(1) + + self.start_subtest("Change LOG_MAX_FILES, don't clear old logs and Checking log list match with filesystem info") + self.set_parameter("LOG_MAX_FILES", 500) + create_and_verify_logs(500) + self.set_parameter("LOG_MAX_FILES", 250) + create_and_verify_logs(250, clear_logsdir=False) + + # cleanup + shutil.rmtree(logspath, ignore_errors=True) + + def TestLogDownload(self): + """Test Onboard Log Download.""" + if self.is_tracker(): + # tracker starts armed, which is annoying + return + self.progress("Ensuring we have contents we care about") + self.set_parameter("LOG_FILE_DSRMROT", 1) + self.set_parameter("LOG_DISARMED", 0) + self.reboot_sitl() + original_log_list = self.log_list() + for i in range(0, 10): + self.wait_ready_to_arm() + self.arm_vehicle() + self.delay_sim_time(1) + self.disarm_vehicle() + new_log_list = self.log_list() + new_log_count = len(new_log_list) - len(original_log_list) + if new_log_count != 10: + raise NotAchievedException("Expected exactly 10 new logs got %u (%s) to (%s)" % + (new_log_count, original_log_list, new_log_list)) + self.progress("Directory contents: %s" % str(new_log_list)) + + self.download_full_log_list() log_id = 5 ofs = 6 count = 2 @@ -3812,6 +3850,13 @@ def save_wp(self, ch=7): self.set_rc(ch, 1000) self.delay_sim_time(1) + def correct_wp_seq_numbers(self, wps): + # renumber the items: + count = 0 + for item in wps: + item.seq = count + count += 1 + def create_simple_relhome_mission(self, items_in, target_system=1, target_component=1): '''takes a list of (type, n, e, alt) items. Creates a mission in absolute frame using alt as relative-to-home and n and e as @@ -3822,34 +3867,24 @@ def create_simple_relhome_mission(self, items_in, target_system=1, target_compon items.extend(items_in) seq = 0 ret = [] - for (t, n, e, alt) in items: + for item in items: + if not isinstance(item, tuple): + # hope this is a mission item... + item.seq = seq + seq += 1 + ret.append(item) + continue + (t, n, e, alt) = item lat = 0 lng = 0 if n != 0 or e != 0: loc = self.home_relative_loc_ne(n, e) lat = loc.lat lng = loc.lng - p1 = 0 frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT if not self.ardupilot_stores_frame_for_cmd(t): frame = mavutil.mavlink.MAV_FRAME_GLOBAL - ret.append(self.mav.mav.mission_item_int_encode( - target_system, - target_component, - seq, # seq - frame, - t, - 0, # current - 0, # autocontinue - p1, # p1 - 0, # p2 - 0, # p3 - 0, # p4 - int(lat*1e7), # latitude - int(lng*1e7), # longitude - alt, # altitude - mavutil.mavlink.MAV_MISSION_TYPE_MISSION), - ) + ret.append(self.create_MISSION_ITEM_INT(t, seq=seq, frame=frame, x=int(lat*1e7), y=int(lng*1e7), z=alt)) seq += 1 return ret @@ -3867,17 +3902,15 @@ def get_mission_count(self): def run_auxfunc(self, function, level, + run_cmd=None, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): - self.run_cmd( + if run_cmd is None: + run_cmd = self.run_cmd + run_cmd( mavutil.mavlink.MAV_CMD_DO_AUX_FUNCTION, - function, # p1 - level, # p2 - 0, # p3 - 0, # p4 - 0, # p5 - 0, # p6 - 0, # p7 - want_result=want_result + p1=function, + p2=level, + want_result=want_result, ) def assert_mission_count(self, expected): @@ -3902,21 +3935,23 @@ def log_list(self): self.progress("log list: %s" % str(ret)) return ret - def assert_parameter_values(self, parameters): + def assert_parameter_values(self, parameters, epsilon=None): names = parameters.keys() got = self.get_parameters(names) for name in names: - if got[name] != parameters[name]: + equal = got[name] == parameters[name] + if epsilon is not None: + delta = abs(got[name] - parameters[name]) + equal = delta <= epsilon + if not equal: raise NotAchievedException("parameter %s want=%f got=%f" % (name, parameters[name], got[name])) self.progress("%s has expected value %f" % (name, got[name])) - def assert_parameter_value(self, parameter, required): - got = self.get_parameter(parameter) - if got != required: - raise NotAchievedException("%s has unexpected value; want=%f got=%f" % - (parameter, required, got)) - self.progress("%s has value %f" % (parameter, required)) + def assert_parameter_value(self, parameter, required, **kwargs): + self.assert_parameter_values({ + parameter: required, + }, **kwargs) def assert_reach_imu_temperature(self, target, timeout): '''wait to reach a target temperature''' @@ -3937,23 +3972,43 @@ def assert_reach_imu_temperature(self, target, timeout): if not temp_ok: raise NotAchievedException("target temperature") + def message_has_field_values_field_values_equal(self, fieldname, value, got, epsilon=None): + if isinstance(value, float): + if math.isnan(value) or math.isnan(got): + return math.isnan(value) and math.isnan(got) + + if type(value) is not str and epsilon is not None: + return abs(got - value) <= epsilon + + return got == value + def message_has_field_values(self, m, fieldvalues, verbose=True, epsilon=None): for (fieldname, value) in fieldvalues.items(): got = getattr(m, fieldname) - if math.isnan(value) or math.isnan(got): - equal = math.isnan(value) and math.isnan(got) - elif epsilon is not None: - equal = abs(got - value) <= epsilon - else: - equal = got == value - if not equal: + value_string = value + got_string = got + enum_name = m.fieldenums_by_name.get(fieldname, None) + if enum_name is not None: + enum = mavutil.mavlink.enums[enum_name] + if value not in enum: + raise ValueError("Expected value %s not in enum %s" % (value, enum_name)) + if got not in enum: + raise ValueError("Received value %s not in enum %s" % (value, enum_name)) + value_string = "%s (%s)" % (value, enum[value].name) + got_string = "%s (%s)" % (got, enum[got].name) + + if not self.message_has_field_values_field_values_equal( + fieldname, value, got, epsilon=epsilon + ): + # see if this is an enumerated field: + self.progress(self.dump_message_verbose(m)) self.progress("Expected %s.%s to be %s, got %s" % - (m.get_type(), fieldname, value, got)) + (m.get_type(), fieldname, value_string, got_string)) return False if verbose: self.progress("%s.%s has expected value %s" % - (m.get_type(), fieldname, value)) + (m.get_type(), fieldname, value_string)) return True def assert_message_field_values(self, m, fieldvalues, verbose=True, epsilon=None): @@ -3961,19 +4016,68 @@ def assert_message_field_values(self, m, fieldvalues, verbose=True, epsilon=None return raise NotAchievedException("Did not get expected field values") - def assert_received_message_field_values(self, message, fieldvalues, verbose=True, very_verbose=False, epsilon=None): - m = self.assert_receive_message(message, verbose=verbose, very_verbose=very_verbose) + def assert_cached_message_field_values(self, message, fieldvalues, verbose=True, very_verbose=False, epsilon=None): + '''checks the most-recently received instance of message to ensure it + has the correct field values''' + m = self.get_cached_message(message) + self.assert_message_field_values(m, fieldvalues, verbose=verbose, epsilon=epsilon) + return m + + def assert_received_message_field_values(self, + message, + fieldvalues, + verbose=True, + very_verbose=False, + epsilon=None, + poll=False, + timeout=None, + check_context=False, + ): + if poll: + self.poll_message(message) + m = self.assert_receive_message( + message, + verbose=verbose, + very_verbose=very_verbose, + timeout=timeout, + check_context=check_context + ) self.assert_message_field_values(m, fieldvalues, verbose=verbose, epsilon=epsilon) return m - def wait_message_field_values(self, message, fieldvalues, timeout=10, epsilon=None, instance=None): + # FIXME: try to use wait_and_maintain here? + def wait_message_field_values(self, + message, + fieldvalues, + timeout=10, + epsilon=None, + instance=None, + minimum_duration=None, + verbose=False, + very_verbose=False, + ): + tstart = self.get_sim_time_cached() + pass_start = None while True: - if self.get_sim_time_cached() - tstart > timeout: + now = self.get_sim_time_cached() + if now - tstart > timeout: raise NotAchievedException("Field never reached values") - m = self.assert_receive_message(message, instance=instance) - if self.message_has_field_values(m, fieldvalues, epsilon=epsilon): + m = self.assert_receive_message( + message, + instance=instance, + verbose=verbose, + very_verbose=very_verbose, + ) + if self.message_has_field_values(m, fieldvalues, epsilon=epsilon, verbose=verbose): + if minimum_duration is not None: + if pass_start is None: + pass_start = now + continue + if now - pass_start < minimum_duration: + continue return m + pass_start = None def onboard_logging_not_log_disarmed(self): self.start_subtest("Test LOG_DISARMED-is-false behaviour") @@ -4164,6 +4268,133 @@ def TestLogDownloadMAVProxy(self, upload_logs=False): self.mavproxy_unload_module(mavproxy, 'log') self.stop_mavproxy(mavproxy) + def TestLogDownloadMAVProxyNetwork(self, upload_logs=False): + """Download latest log over network port""" + self.context_push() + self.set_parameters({ + "NET_ENABLED": 1, + "LOG_DARM_RATEMAX": 2, # make small logs + # UDP client + "NET_P1_TYPE": 1, + "NET_P1_PROTOCOL": 2, + "NET_P1_PORT": 16001, + "NET_P1_IP0": 127, + "NET_P1_IP1": 0, + "NET_P1_IP2": 0, + "NET_P1_IP3": 1, + # UDP server + "NET_P2_TYPE": 2, + "NET_P2_PROTOCOL": 2, + "NET_P2_PORT": 16002, + "NET_P2_IP0": 0, + "NET_P2_IP1": 0, + "NET_P2_IP2": 0, + "NET_P2_IP3": 0, + # TCP client + "NET_P3_TYPE": 3, + "NET_P3_PROTOCOL": 2, + "NET_P3_PORT": 16003, + "NET_P3_IP0": 127, + "NET_P3_IP1": 0, + "NET_P3_IP2": 0, + "NET_P3_IP3": 1, + # TCP server + "NET_P4_TYPE": 4, + "NET_P4_PROTOCOL": 2, + "NET_P4_PORT": 16004, + "NET_P4_IP0": 0, + "NET_P4_IP1": 0, + "NET_P4_IP2": 0, + "NET_P4_IP3": 0, + }) + self.reboot_sitl() + endpoints = [('UDPClient', ':16001') , + ('UDPServer', 'udpout:127.0.0.1:16002'), + ('TCPClient', 'tcpin:0.0.0.0:16003'), + ('TCPServer', 'tcp:127.0.0.1:16004')] + for name, e in endpoints: + self.progress("Downloading log with %s %s" % (name, e)) + filename = "MAVProxy-downloaded-net-log-%s.BIN" % name + + mavproxy = self.start_mavproxy(master=e) + self.mavproxy_load_module(mavproxy, 'log') + self.wait_heartbeat() + mavproxy.send("log list\n") + mavproxy.expect("numLogs") + mavproxy.send("log download latest %s\n" % filename) + mavproxy.expect("Finished downloading", timeout=120) + self.mavproxy_unload_module(mavproxy, 'log') + self.stop_mavproxy(mavproxy) + + self.set_parameters({ + # multicast UDP client + "NET_P1_TYPE": 1, + "NET_P1_PROTOCOL": 2, + "NET_P1_PORT": 14550, + "NET_P1_IP0": 239, + "NET_P1_IP1": 255, + "NET_P1_IP2": 145, + "NET_P1_IP3": 50, + # Broadcast UDP client + "NET_P2_TYPE": 1, + "NET_P2_PROTOCOL": 2, + "NET_P2_PORT": 16005, + "NET_P2_IP0": 255, + "NET_P2_IP1": 255, + "NET_P2_IP2": 255, + "NET_P2_IP3": 255, + }) + self.reboot_sitl() + endpoints = [('UDPMulticast', 'mcast:') , + ('UDPBroadcast', ':16005')] + for name, e in endpoints: + self.progress("Downloading log with %s %s" % (name, e)) + filename = "MAVProxy-downloaded-net-log-%s.BIN" % name + + mavproxy = self.start_mavproxy(master=e) + self.mavproxy_load_module(mavproxy, 'log') + self.wait_heartbeat() + mavproxy.send("log list\n") + mavproxy.expect("numLogs") + mavproxy.send("log download latest %s\n" % filename) + mavproxy.expect("Finished downloading", timeout=120) + self.mavproxy_unload_module(mavproxy, 'log') + self.stop_mavproxy(mavproxy) + + self.context_pop() + + def TestLogDownloadMAVProxyCAN(self, upload_logs=False): + """Download latest log over CAN serial port""" + self.context_push() + self.set_parameters({ + "CAN_P1_DRIVER": 1, + "LOG_DISARMED": 1, + }) + self.reboot_sitl() + self.set_parameters({ + "CAN_D1_UC_SER_EN": 1, + "CAN_D1_UC_S1_NOD": 125, + "CAN_D1_UC_S1_IDX": 4, + "CAN_D1_UC_S1_BD": 57600, + "CAN_D1_UC_S1_PRO": 2, + }) + self.reboot_sitl() + filename = "MAVProxy-downloaded-can-log.BIN" + # port 15550 is in SITL_Periph_State.h as SERIAL4 udpclient:127.0.0.1:15550 + mavproxy = self.start_mavproxy(master=':15550') + mavproxy.expect("Detected vehicle") + self.mavproxy_load_module(mavproxy, 'log') + mavproxy.send("log list\n") + mavproxy.expect("numLogs") + self.wait_heartbeat() + self.wait_heartbeat() + mavproxy.send("set shownoise 0\n") + mavproxy.send("log download latest %s\n" % filename) + mavproxy.expect("Finished downloading", timeout=120) + self.mavproxy_unload_module(mavproxy, 'log') + self.stop_mavproxy(mavproxy) + self.context_pop() + def show_gps_and_sim_positions(self, on_off): """Allow to display gps and actual position on map.""" if on_off is True: @@ -4212,6 +4443,18 @@ def install_test_script_context(self, scriptname): self.install_test_script(scriptname) self.context_get().installed_scripts.append(scriptname) + def install_test_modules_context(self): + '''installs test modules which will be removed when the context goes + away''' + self.install_test_modules() + self.context_get().installed_modules.append("test") + + def install_mavlink_module_context(self): + '''installs mavlink module which will be removed when the context goes + away''' + self.install_mavlink_module() + self.context_get().installed_modules.append("mavlink") + def install_applet_script_context(self, scriptname): '''installs an applet script which will be removed when the context goes away''' @@ -4339,15 +4582,25 @@ def assert_not_receive_message(self, message, timeout=1, mav=None, condition=Non def assert_receive_message(self, type, - timeout=1, + timeout=None, verbose=False, very_verbose=False, mav=None, condition=None, delay_fn=None, - instance=None): + instance=None, + check_context=False): + if timeout is None: + timeout = 1 if mav is None: mav = self.mav + + if check_context: + collection = self.context_collection(type) + if len(collection) > 0: + # return the most-recently-received message: + return collection[-1] + m = None tstart = time.time() # timeout in wallclock while True: @@ -4357,8 +4610,10 @@ def assert_receive_message(self, continue if m is not None: break - if time.time() - tstart > timeout: - raise NotAchievedException("Did not get %s" % type) + elapsed_time = time.time() - tstart + if elapsed_time > timeout: + raise NotAchievedException("Did not get %s after %s seconds" % + (type, elapsed_time)) if delay_fn is not None: delay_fn() if verbose: @@ -4473,28 +4728,30 @@ def assert_rally_content_same(self, f1, f2): raise ValueError("count %u not handled" % count) self.progress("Rally content same") - def load_rally(self, filename): + def load_rally_using_mavproxy(self, filename): """Load rally points from a file to flight controller.""" self.progress("Loading rally points (%s)" % filename) path = os.path.join(testdir, self.current_test_name_directory, filename) mavproxy = self.start_mavproxy() mavproxy.send('rally load %s\n' % path) mavproxy.expect("Loaded") + self.delay_sim_time(10) # allow transfer to complete self.stop_mavproxy(mavproxy) def load_sample_mission(self): self.load_mission(self.sample_mission_filename()) + def generic_mission_filepath_for_filename(self, filename): + return os.path.join(testdir, "Generic_Missions", filename) + def load_generic_mission(self, filename, strict=True): return self.load_mission_from_filepath( - os.path.join(testdir, "Generic_Missions"), - filename, + self.generic_mission_filepath_for_filename(filename), strict=strict) def load_mission(self, filename, strict=True): return self.load_mission_from_filepath( - self.current_test_name_directory, - filename, + os.path.join(testdir, self.current_test_name_directory, filename), strict=strict) def wp_to_mission_item_int(self, wp): @@ -4521,40 +4778,54 @@ def wp_to_mission_item_int(self, wp): wp.z) return wp_int - def mission_from_filepath(self, filepath, filename, target_system=1, target_component=1): + def mission_from_filepath(self, filepath, target_system=1, target_component=1): '''returns a list of mission-item-ints from filepath''' - self.progress("Loading mission (%s)" % filename) - path = os.path.join(testdir, filepath, filename) + print("filepath: %s" % filepath) + self.progress("Loading mission (%s)" % os.path.basename(filepath)) wploader = mavwp.MAVWPLoader( target_system=target_system, target_component=target_component ) - wploader.load(path) + wploader.load(filepath) return [self.wp_to_mission_item_int(x) for x in wploader.wpoints] + def sitl_home_string_from_mission(self, filename): + '''return a string of the form "lat,lng,yaw,alt" from the home + location in a mission file''' + return "%s,%s,%s,%s" % self.get_home_tuple_from_mission(filename) + + def sitl_home_string_from_mission_filepath(self, filepath): + '''return a string of the form "lat,lng,yaw,alt" from the home + location in a mission file''' + return "%s,%s,%s,%s" % self.get_home_tuple_from_mission_filepath(filepath) + def get_home_tuple_from_mission(self, filename): '''gets item 0 from the mission file, returns a tuple suitable for passing to customise_SITL_commandline as --home. Yaw will be 0, so the caller may want to fill that in ''' - items = self.mission_from_filepath( - self.current_test_name_directory, - filename, + return self.get_home_tuple_from_mission_filepath( + os.path.join(testdir, self.current_test_name_directory, filename) ) + + def get_home_tuple_from_mission_filepath(self, filepath): + '''gets item 0 from the mission file, returns a tuple suitable for + passing to customise_SITL_commandline as --home. Yaw will be + 0, so the caller may want to fill that in + ''' + items = self.mission_from_filepath(filepath) home_item = items[0] return (home_item.x * 1e-7, home_item.y * 1e-7, home_item.z, 0) # TODO: rename the following to "upload_mission_from_filepath" def load_mission_from_filepath(self, filepath, - filename, target_system=1, target_component=1, strict=True, reset_current_wp=True): wpoints_int = self.mission_from_filepath( filepath, - filename, target_system=target_system, target_component=target_component ) @@ -4731,7 +5002,6 @@ def check_mission_item_upload_download(self, items, itype, mission_type, strict= self.upload_using_mission_protocol(mission_type, items) self.progress("check %s upload/download: download items" % itype) downloaded_items = self.download_using_mission_protocol(mission_type) - self.progress("Downloaded items: (%s)" % str(downloaded_items)) if len(items) != len(downloaded_items): raise NotAchievedException("Did not download same number of items as uploaded want=%u got=%u" % (len(items), len(downloaded_items))) @@ -4739,6 +5009,8 @@ def check_mission_item_upload_download(self, items, itype, mission_type, strict= self.check_fence_items_same(items, downloaded_items, strict=strict) elif mission_type == mavutil.mavlink.MAV_MISSION_TYPE_MISSION: self.check_mission_waypoint_items_same(items, downloaded_items, strict=strict) + elif mission_type == mavutil.mavlink.MAV_MISSION_TYPE_RALLY: + self.check_mission_waypoint_items_same(items, downloaded_items, strict=strict) else: raise NotAchievedException("Unhandled") @@ -4755,6 +5027,13 @@ def check_mission_upload_download(self, items, strict=True): mavutil.mavlink.MAV_MISSION_TYPE_MISSION, strict=strict) + def check_rally_upload_download(self, items): + self.check_mission_item_upload_download( + items, + "rally", + mavutil.mavlink.MAV_MISSION_TYPE_RALLY + ) + def check_dflog_message_rates(self, log_filepath, message_rates): reader = self.dfreader_for_path(log_filepath) @@ -4825,7 +5104,7 @@ def rc_defaults(self): def set_rc_from_map(self, _map, timeout=20): map_copy = _map.copy() for v in map_copy.values(): - if type(v) != int: + if not isinstance(v, int): raise NotAchievedException("RC values must be integers") self.rc_queue.put(map_copy) @@ -4859,7 +5138,7 @@ def set_rc_from_map(self, _map, timeout=20): def rc_thread_main(self): chan16 = [1000] * 16 - sitl_output = mavutil.mavudp("127.0.0.1:5501", input=False) + sitl_output = mavutil.mavudp("127.0.0.1:%u" % self.sitl_rcin_port(), input=False) buf = None while True: @@ -4978,17 +5257,23 @@ def arming_test_mission(self): else: return None - def set_safetyswitch_on(self): - self.set_safetyswitch(1) + def set_safetyswitch_on(self, **kwargs): + self.set_safetyswitch(1, **kwargs) - def set_safetyswitch_off(self): - self.set_safetyswitch(0) + def set_safetyswitch_off(self, **kwargs): + self.set_safetyswitch(0, **kwargs) def set_safetyswitch(self, value, target_system=1, target_component=1): self.mav.mav.set_mode_send( target_system, mavutil.mavlink.MAV_MODE_FLAG_DECODE_POSITION_SAFETY, value) + self.wait_sensor_state( + mavutil.mavlink.MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS, + True, not value, True, + verbose=True, + timeout=30 + ) def armed(self): """Return true if vehicle is armed and safetyoff""" @@ -4996,36 +5281,19 @@ def armed(self): return self.mav.motors_armed() def send_mavlink_arm_command(self): - target_sysid = 1 - target_compid = 1 - self.mav.mav.command_long_send( - target_sysid, - target_compid, + self.send_cmd( mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 1, # confirmation - 1, # ARM - 0, - 0, - 0, - 0, - 0, - 0) + p1=1, # ARM + ) + + def send_mavlink_disarm_command(self): + self.send_cmd( + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + p1=0, # DISARM + ) def send_mavlink_run_prearms_command(self): - target_sysid = 1 - target_compid = 1 - self.mav.mav.command_long_send( - target_sysid, - target_compid, - mavutil.mavlink.MAV_CMD_RUN_PREARM_CHECKS, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0) + self.send_cmd(mavutil.mavlink.MAV_CMD_RUN_PREARM_CHECKS) def analog_rangefinder_parameters(self): return { @@ -5041,43 +5309,29 @@ def set_analog_rangefinder_parameters(self): def send_debug_trap(self, timeout=6000): self.progress("Sending trap to autopilot") - self.run_cmd(mavutil.mavlink.MAV_CMD_DEBUG_TRAP, - 32451, # magic number to trap - 0, - 0, - 0, - 0, - 0, - 0, - timeout=timeout) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DEBUG_TRAP, + p1=32451, # magic number to trap + timeout=timeout, + ) def try_arm(self, result=True, expect_msg=None, timeout=60): """Send Arming command, wait for the expected result and statustext.""" self.progress("Try arming and wait for expected result") self.drain_mav() - self.run_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 1, # ARM - 0, - 0, - 0, - 0, - 0, - 0, - want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED if result else mavutil.mavlink.MAV_RESULT_FAILED, - timeout=timeout) + self.run_cmd( + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + p1=1, # ARM + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED if result else mavutil.mavlink.MAV_RESULT_FAILED, + timeout=timeout, + ) if expect_msg is not None: self.wait_statustext( expect_msg, timeout=timeout, the_function=lambda: self.send_cmd( mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 1, # ARM - 0, - 0, - 0, - 0, - 0, - 0, + p1=1, # ARM target_sysid=None, target_compid=None, )) @@ -5089,15 +5343,12 @@ def arm_vehicle(self, timeout=20, force=False): if force: p2 = 2989 try: - self.run_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 1, # ARM - p2, - 0, - 0, - 0, - 0, - 0, - timeout=timeout) + self.run_cmd( + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + p1=1, # ARM + p2=p2, + timeout=timeout, + ) except ValueError as e: # statustexts are queued; give it a second to arrive: self.delay_sim_time(5) @@ -5123,30 +5374,23 @@ def disarm_vehicle(self, timeout=60, force=False): p2 = 0 if force: p2 = 21196 # magic force disarm value - self.run_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 0, # DISARM - p2, - 0, - 0, - 0, - 0, - 0, - timeout=timeout) + self.run_cmd( + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + p1=0, # DISARM + p2=p2, + timeout=timeout, + ) self.wait_disarmed() def disarm_vehicle_expect_fail(self): '''disarm, checking first that non-forced disarm fails, then doing a forced disarm''' self.progress("Disarm - expect to fail") - self.run_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 0, # DISARM - 0, - 0, - 0, - 0, - 0, - 0, - timeout=10, - want_result=mavutil.mavlink.MAV_RESULT_FAILED) + self.run_cmd( + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + p1=0, # DISARM + timeout=10, + want_result=mavutil.mavlink.MAV_RESULT_FAILED, + ) self.progress("Disarm - forced") self.disarm_vehicle(force=True) @@ -5521,7 +5765,7 @@ def get_parameter(self, *args, **kwargs): def send_get_parameter_direct(self, name): encname = name - if sys.version_info.major >= 3 and type(encname) != bytes: + if sys.version_info.major >= 3 and not isinstance(encname, bytes): encname = bytes(encname, 'ascii') self.mav.mav.param_request_read_send(self.sysid_thismav(), 1, @@ -5636,7 +5880,7 @@ def context_stop_collecting(self, msg_type): del context.collections[msg_type] return ret - def context_pop(self): + def context_pop(self, process_interaction_allowed=True): """Set parameters to origin values in reverse order.""" dead = self.contexts.pop() # remove hooks first; these hooks can raise exceptions which @@ -5645,13 +5889,19 @@ def context_pop(self): self.remove_message_hook(hook) for script in dead.installed_scripts: self.remove_installed_script(script) + for (message_id, interval_us) in dead.overridden_message_rates.items(): + self.set_message_interval(message_id, interval_us) + for module in dead.installed_modules: + print("Removing module (%s)" % module) + self.remove_installed_modules(module) if dead.sitl_commandline_customised and len(self.contexts): self.contexts[-1].sitl_commandline_customised = True dead_parameters_dict = {} for p in dead.parameters: dead_parameters_dict[p[0]] = p[1] - self.set_parameters(dead_parameters_dict, add_to_context=False) + if process_interaction_allowed: + self.set_parameters(dead_parameters_dict, add_to_context=False) if getattr(self, "old_binary", None) is not None: self.stop_SITL() @@ -5694,20 +5944,72 @@ def __exit__(self, type, value, traceback): def sysid_thismav(self): return 1 + def create_MISSION_ITEM_INT( + self, + t, + p1=0, + p2=0, + p3=0, + p4=0, + x=0, + y=0, + z=0, + frame=mavutil.mavlink.MAV_FRAME_GLOBAL, + autocontinue=0, + current=0, + target_system=1, + target_component=1, + seq=0, + mission_type=mavutil.mavlink.MAV_MISSION_TYPE_MISSION, + ): + return self.mav.mav.mission_item_int_encode( + target_system, + target_component, + seq, # seq + frame, + t, + current, # current + autocontinue, # autocontinue + p1, # p1 + p2, # p2 + p3, # p3 + p4, # p4 + x, # latitude + y, # longitude + z, # altitude + mission_type + ) + def run_cmd_int(self, command, - p1, - p2, - p3, - p4, - x, - y, - z, + p1=0, + p2=0, + p3=0, + p4=0, + x=0, + y=0, + z=0, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED, timeout=10, target_sysid=None, target_compid=None, - frame=mavutil.mavlink.MAV_FRAME_GLOBAL_INT): + frame=mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + p5=None, + p6=None, + p7=None, + quiet=False, + mav=None, + ): + + if mav is None: + mav = self.mav + + if p5 is not None: + x = p5 + if p6 is not None: + y = p6 + if p7 is not None: + z = p7 if target_sysid is None: target_sysid = self.sysid_thismav() @@ -5717,30 +6019,48 @@ def run_cmd_int(self, self.get_sim_time() # required for timeout in run_cmd_get_ack to work """Send a MAVLink command int.""" - self.mav.mav.command_int_send(target_sysid, - target_compid, - frame, - command, - 0, # current - 0, # autocontinue - p1, - p2, - p3, - p4, - x, - y, - z) - self.run_cmd_get_ack(command, want_result, timeout) + if not quiet: + try: + command_name = mavutil.mavlink.enums["MAV_CMD"][command].name + except KeyError: + command_name = "UNKNOWNu" + self.progress("Sending COMMAND_INT to (%u,%u) (%s=%u) (p1=%f p2=%f p3=%f p4=%f p5=%u p6=%u p7=%f)" % + ( + target_sysid, + target_compid, + command_name, + command, + p1, + p2, + p3, + p4, + x, + y, + z)) + mav.mav.command_int_send(target_sysid, + target_compid, + frame, + command, + 0, # current + 0, # autocontinue + p1, + p2, + p3, + p4, + x, + y, + z) + self.run_cmd_get_ack(command, want_result, timeout, mav=mav) def send_cmd(self, command, - p1, - p2, - p3, - p4, - p5, - p6, - p7, + p1=0, + p2=0, + p3=0, + p4=0, + p5=0, + p6=0, + p7=0, target_sysid=None, target_compid=None, mav=None, @@ -5753,16 +6073,17 @@ def send_cmd(self, target_sysid = self.sysid_thismav() if target_compid is None: target_compid = 1 - try: - command_name = mavutil.mavlink.enums["MAV_CMD"][command].name - except KeyError: - command_name = "UNKNOWN=%u" % command if not quiet: - self.progress("Sending COMMAND_LONG to (%u,%u) (%s) (p1=%f p2=%f p3=%f p4=%f p5=%f p6=%f p7=%f)" % + try: + command_name = mavutil.mavlink.enums["MAV_CMD"][command].name + except KeyError: + command_name = "UNKNOWN" + self.progress("Sending COMMAND_LONG to (%u,%u) (%s=%u) (p1=%f p2=%f p3=%f p4=%f p5=%f p6=%f p7=%f)" % ( target_sysid, target_compid, command_name, + command, p1, p2, p3, @@ -5784,13 +6105,13 @@ def send_cmd(self, def run_cmd(self, command, - p1, - p2, - p3, - p4, - p5, - p6, - p7, + p1=0, + p2=0, + p3=0, + p4=0, + p5=0, + p6=0, + p7=0, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED, target_sysid=None, target_compid=None, @@ -5811,6 +6132,7 @@ def run_cmd(self, target_sysid=target_sysid, target_compid=target_compid, mav=mav, + quiet=quiet, ) self.run_cmd_get_ack(command, want_result, timeout, quiet=quiet, mav=mav) @@ -5847,16 +6169,12 @@ def run_cmd_get_ack(self, command, want_result, timeout, quiet=False, mav=None, def set_current_waypoint_using_mav_cmd_do_set_mission_current( self, seq, + reset=0, target_sysid=1, target_compid=1): self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_MISSION_CURRENT, - seq, - 0, - 0, - 0, - 0, - 0, - 0, + p1=seq, + p2=reset, timeout=1, target_sysid=target_sysid, target_compid=target_compid) @@ -5886,7 +6204,7 @@ def verify_parameter_values(self, parameter_stuff, max_delta=0.0): for param in parameter_stuff: fetched_value = self.get_parameter(param) wanted_value = parameter_stuff[param] - if type(wanted_value) == tuple: + if isinstance(wanted_value, tuple): max_delta = wanted_value[1] wanted_value = wanted_value[0] if abs(fetched_value - wanted_value) > max_delta: @@ -5907,14 +6225,14 @@ def longitude_scale(lat): @staticmethod def get_distance(loc1, loc2): """Get ground distance between two locations.""" - return AutoTest.get_distance_accurate(loc1, loc2) + return TestSuite.get_distance_accurate(loc1, loc2) # dlat = loc2.lat - loc1.lat # try: # dlong = loc2.lng - loc1.lng # except AttributeError: # dlong = loc2.lon - loc1.lon - # return math.sqrt((dlat*dlat) + (dlong*dlong)*AutoTest.longitude_scale(loc2.lat)) * 1.113195e5 + # return math.sqrt((dlat*dlat) + (dlong*dlong)*TestSuite.longitude_scale(loc2.lat)) * 1.113195e5 @staticmethod def get_distance_accurate(loc1, loc2): @@ -5928,6 +6246,14 @@ def get_distance_accurate(loc1, loc2): return mp_util.gps_distance(loc1.lat, lon1, loc2.lat, lon2) + def assert_distance(self, loc1, loc2, min_distance, max_distance): + dist = self.get_distance_accurate(loc1, loc2) + if dist < min_distance or dist > max_distance: + raise NotAchievedException("Expected distance %f to be between %f and %f" % + (dist, min_distance, max_distance)) + self.progress("Distance %f is between %f and %f" % + (dist, min_distance, max_distance)) + @staticmethod def get_latlon_attr(loc, attrs): '''return any found latitude attribute from loc''' @@ -5943,23 +6269,23 @@ def get_latlon_attr(loc, attrs): @staticmethod def get_lat_attr(loc): '''return any found latitude attribute from loc''' - return AutoTest.get_latlon_attr(loc, ["lat", "latitude"]) + return TestSuite.get_latlon_attr(loc, ["lat", "latitude"]) @staticmethod def get_lon_attr(loc): '''return any found latitude attribute from loc''' - return AutoTest.get_latlon_attr(loc, ["lng", "lon", "longitude"]) + return TestSuite.get_latlon_attr(loc, ["lng", "lon", "longitude"]) @staticmethod def get_distance_int(loc1, loc2): """Get ground distance between two locations in the normal "int" form - lat/lon multiplied by 1e7""" - loc1_lat = AutoTest.get_lat_attr(loc1) - loc2_lat = AutoTest.get_lat_attr(loc2) - loc1_lon = AutoTest.get_lon_attr(loc1) - loc2_lon = AutoTest.get_lon_attr(loc2) + loc1_lat = TestSuite.get_lat_attr(loc1) + loc2_lat = TestSuite.get_lat_attr(loc2) + loc1_lon = TestSuite.get_lon_attr(loc1) + loc2_lon = TestSuite.get_lon_attr(loc2) - return AutoTest.get_distance_accurate( + return TestSuite.get_distance_accurate( mavutil.location(loc1_lat*1e-7, loc1_lon*1e-7), mavutil.location(loc2_lat*1e-7, loc2_lon*1e-7)) @@ -5989,13 +6315,8 @@ def get_bearing(loc1, loc2): def send_cmd_do_set_mode(self, mode): self.send_cmd( mavutil.mavlink.MAV_CMD_DO_SET_MODE, - mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, - self.get_mode_from_mode_mapping(mode), - 0, - 0, - 0, - 0, - 0 + p1=mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, + p2=self.get_mode_from_mode_mapping(mode), ) def assert_mode(self, mode): @@ -6046,6 +6367,58 @@ def get_autopilot_capabilities(self): m = self.assert_receive_message('AUTOPILOT_VERSION', timeout=10) return m.capabilities + def decode_flight_sw_version(self, flight_sw_version: int): + """ Decode 32 bit flight_sw_version mavlink parameter + corresponds to encoding in ardupilot GCS_MAVLINK::send_autopilot_version.""" + fw_type_id = (flight_sw_version >> 0) % 256 + patch = (flight_sw_version >> 8) % 256 + minor = (flight_sw_version >> 16) % 256 + major = (flight_sw_version >> 24) % 256 + if fw_type_id == 0: + fw_type = "dev" + elif fw_type_id == 64: + fw_type = "alpha" + elif fw_type_id == 128: + fw_type = "beta" + elif fw_type_id == 192: + fw_type = "rc" + elif fw_type_id == 255: + fw_type = "official" + else: + fw_type = "undefined" + return major, minor, patch, fw_type + + def get_autopilot_firmware_version(self): + self.mav.mav.command_long_send(self.sysid_thismav(), + 1, + mavutil.mavlink.MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES, + 0, # confirmation + 1, # 1: Request autopilot version + 0, + 0, + 0, + 0, + 0, + 0) + m = self.assert_receive_message('AUTOPILOT_VERSION', timeout=10) + self.fcu_firmware_version = self.decode_flight_sw_version(m.flight_sw_version) + + def hex_values_to_int(hex_values): + # Convert ascii codes to characters + hex_chars = [chr(int(hex_value)) for hex_value in hex_values] + # Convert hex characters to integers, handle \x00 case + int_values = [0 if hex_char == '\x00' else int(hex_char, 16) for hex_char in hex_chars] + return int_values + + fcu_hash_to_hex = "" + for i in hex_values_to_int(m.flight_custom_version): + fcu_hash_to_hex += f"{i:x}" + self.fcu_firmware_hash = fcu_hash_to_hex + self.progress(f"Firmware Version {self.fcu_firmware_version}") + self.progress(f"Firmware hash {self.fcu_firmware_hash}") + self.githash = util.get_git_hash(short=True) + self.progress(f"Git hash {self.githash}") + def GetCapabilities(self): '''Get Capabilities''' self.assert_capability(mavutil.mavlink.MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT) @@ -6071,23 +6444,21 @@ def get_mode_from_mode_mapping(self, mode): def run_cmd_do_set_mode(self, mode, timeout=30, + run_cmd=None, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): + if run_cmd is None: + run_cmd = self.run_cmd base_mode = mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED custom_mode = self.get_mode_from_mode_mapping(mode) - self.run_cmd( + run_cmd( mavutil.mavlink.MAV_CMD_DO_SET_MODE, - base_mode, - custom_mode, - 0, - 0, - 0, - 0, - 0, + p1=base_mode, + p2=custom_mode, want_result=want_result, - timeout=timeout + timeout=timeout, ) - def do_set_mode_via_command_long(self, mode, timeout=30): + def do_set_mode_via_command_XYZZY(self, mode, run_cmd, timeout=30): """Set mode with a command long message.""" tstart = self.get_sim_time() want_custom_mode = self.get_mode_from_mode_mapping(mode) @@ -6095,12 +6466,18 @@ def do_set_mode_via_command_long(self, mode, timeout=30): remaining = timeout - (self.get_sim_time_cached() - tstart) if remaining <= 0: raise AutoTestTimeoutException("Failed to change mode") - self.run_cmd_do_set_mode(mode, timeout=10) + self.run_cmd_do_set_mode(mode, run_cmd=run_cmd, timeout=10) m = self.wait_heartbeat() self.progress("Got mode=%u want=%u" % (m.custom_mode, want_custom_mode)) if m.custom_mode == want_custom_mode: return + def do_set_mode_via_command_long(self, mode, timeout=30): + self.do_set_mode_via_command_XYZZY(mode, self.run_cmd, timeout=timeout) + + def do_set_mode_via_command_int(self, mode, timeout=30): + self.do_set_mode_via_command_XYZZY(mode, self.run_cmd_int, timeout=timeout) + def mavproxy_do_set_mode_via_command_long(self, mavproxy, mode, timeout=30): """Set mode with a command long message with Mavproxy.""" base_mode = mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED @@ -6176,13 +6553,10 @@ def guided_achieve_heading(self, heading, accuracy=None): raise NotAchievedException("Did not achieve heading") self.run_cmd( mavutil.mavlink.MAV_CMD_CONDITION_YAW, - heading, # target angle - 10, # degrees/second - 1, # -1 is counter-clockwise, 1 clockwise - 0, # 1 for relative, 0 for absolute - 0, # p5 - 0, # p6 - 0, # p7 + p1=heading, # target angle + p2=10, # degrees/second + p3=1, # -1 is counter-clockwise, 1 clockwise + p4=0, # 1 for relative, 0 for absolute ) m = self.mav.recv_match(type='VFR_HUD', blocking=True) self.progress("heading=%d want=%d" % (m.heading, int(heading))) @@ -6203,15 +6577,12 @@ def assert_heading(self, heading, accuracy=1): def do_set_relay(self, relay_num, on_off, timeout=10): """Set relay with a command long message.""" self.progress("Set relay %d to %d" % (relay_num, on_off)) - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, - relay_num, - on_off, - 0, - 0, - 0, - 0, - 0, - timeout=timeout) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_SET_RELAY, + p1=relay_num, + p2=on_off, + timeout=timeout, + ) def do_set_relay_mavproxy(self, relay_num, on_off): """Set relay with mavproxy.""" @@ -6225,15 +6596,11 @@ def do_fence_en_or_dis_able(self, value, want_result=mavutil.mavlink.MAV_RESULT_ p1 = 1 else: p1 = 0 - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, - p1, # param1 - 0, # param2 - 0, # param3 - 0, # param4 - 0, # param5 - 0, # param6 - 0, # param7 - want_result=want_result) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, + p1=p1, # param1 + want_result=want_result, + ) def do_fence_enable(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): self.do_fence_en_or_dis_able(True, want_result=want_result) @@ -6379,10 +6746,13 @@ def validator(value2, target2=None): **kwargs ) - def wait_altitude(self, altitude_min, altitude_max, relative=False, timeout=30, **kwargs): + def wait_altitude(self, altitude_min, altitude_max, relative=False, timeout=None, **kwargs): """Wait for a given altitude range.""" assert altitude_min <= altitude_max, "Minimum altitude should be less than maximum altitude." + if timeout is None: + timeout = 30 + def validator(value2, target2=None): if altitude_min <= value2 <= altitude_max: return True @@ -6440,6 +6810,10 @@ def validator(value2, target2=None): **kwargs ) + def groundspeed(self): + m = self.assert_receive_message('VFR_HUD') + return m.groundspeed + def wait_groundspeed(self, speed_min, speed_max, timeout=30, **kwargs): self.wait_vfr_hud_speed("groundspeed", speed_min, speed_max, timeout=timeout, **kwargs) @@ -6509,7 +6883,7 @@ def validator(value2, target2): ) def wait_and_maintain(self, value_name, target, current_value_getter, validator=None, accuracy=2.0, timeout=30, **kwargs): - if type(target) is Vector3: + if isinstance(target, Vector3): return self.wait_and_maintain_vector( value_name, target, @@ -6540,6 +6914,7 @@ def wait_and_maintain_vector(self, achieving_duration_start = None sum_of_achieved_values = Vector3() last_value = Vector3() + last_fail_print = 0 count_of_achieved_values = 0 called_function = kwargs.get("called_function", None) minimum_duration = kwargs.get("minimum_duration", 0) @@ -6549,7 +6924,15 @@ def wait_and_maintain_vector(self, self.progress("Waiting for %s=(%s)" % (value_name, str(target))) last_print_time = 0 - while self.get_sim_time_cached() < tstart + timeout: # if we failed to received message with the getter the sim time isn't updated # noqa + while True: # if we failed to received message with the getter the sim time isn't updated # noqa + now = self.get_sim_time_cached() + if now - tstart > timeout: + raise AutoTestTimeoutException( + "Failed to attain %s want %s, reached %s" % + (value_name, + str(target), + str(sum_of_achieved_values / count_of_achieved_values) if count_of_achieved_values != 0 else str(last_value))) # noqa + last_value = current_value_getter() if called_function is not None: called_function(last_value, target) @@ -6586,11 +6969,10 @@ def wait_and_maintain_vector(self, achieving_duration_start = None sum_of_achieved_values.zero() count_of_achieved_values = 0 - raise AutoTestTimeoutException( - "Failed to attain %s want %s, reached %s" % - (value_name, - str(target), - str(sum_of_achieved_values / count_of_achieved_values) if count_of_achieved_values != 0 else str(last_value))) + if now - last_fail_print > 1: + self.progress("Waiting for (%s), got %s" % + (target, last_value)) + last_fail_print = now def validate_kwargs(self, kwargs, valid={}): for key in kwargs: @@ -6633,7 +7015,10 @@ def wait_and_maintain_range(self, while self.get_sim_time_cached() < tstart + timeout: # if we failed to received message with the getter the sim time isn't updated # noqa last_value = current_value_getter() if called_function is not None: - called_function(last_value, target) + if print_diagnostics_as_target_not_range: + called_function(last_value, target) + else: + called_function(last_value, minimum, maximum) if validator is not None: if print_diagnostics_as_target_not_range: is_value_valid = validator(last_value, target) @@ -6662,7 +7047,7 @@ def wait_and_maintain_range(self, achieved_duration_bit) ) else: - if type(last_value) is float: + if isinstance(last_value, float): self.progress( "%s=%0.2f (%s between %s and %s)%s" % (value_name, @@ -6767,11 +7152,12 @@ def get_speed_vector(self, timeout=1): return Vector3(msg.vx, msg.vy, msg.vz) """Wait for a given speed vector.""" - def wait_speed_vector(self, speed_vector, accuracy=0.2, timeout=30, **kwargs): + def wait_speed_vector(self, speed_vector, accuracy=0.3, timeout=30, **kwargs): def validator(value2, target2): - return (math.fabs(value2.x - target2.x) <= accuracy and - math.fabs(value2.y - target2.y) <= accuracy and - math.fabs(value2.z - target2.z) <= accuracy) + for (want, got) in (target2.x, value2.x), (target2.y, value2.y), (target2.z, value2.z): + if want != float("nan") and (math.fabs(got - want) > accuracy): + return False + return True self.wait_and_maintain( value_name="SpeedVector", @@ -7071,6 +7457,35 @@ def assert_rc_channel_value(self, channel, value): raise NotAchievedException("Expected %s to be %u got %u" % (channel, value, m_value)) + def do_reposition(self, + loc, + frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT): + '''send a DO_REPOSITION command for a location''' + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_REPOSITION, + 0, + 0, + 0, + 0, + int(loc.lat*1e7), # lat* 1e7 + int(loc.lng*1e7), # lon* 1e7 + loc.alt, + frame=frame + ) + + def add_rally_point(self, loc, seq, total): + '''add a rally point at the given location''' + self.mav.mav.rally_point_send(1, # target system + 0, # target component + seq, # sequence number + total, # total count + int(loc.lat * 1e7), + int(loc.lng * 1e7), + loc.alt, # relative alt + 0, # "break" alt?! + 0, # "land dir" + 0) # flags + def wait_location(self, loc, accuracy=5.0, @@ -7180,6 +7595,10 @@ def wait_waypoint(self, raise WaitWaypointTimeout("Timed out waiting for waypoint %u of %u" % (wpnum_end, wpnum_end)) + def get_cached_message(self, message_type): + '''returns the most-recently received instance of message_type''' + return self.mav.messages[message_type] + def mode_is(self, mode, cached=False, drain_mav=True): if not cached: self.wait_heartbeat(drain_mav=drain_mav) @@ -7203,9 +7622,13 @@ def wait_mode(self, mode, timeout=60): raise WaitModeTimeout("Did not change mode") self.progress("Got mode %s" % mode) + def assert_mode_is(self, mode): + if not self.mode_is(mode): + raise NotAchievedException("Expected mode %s" % str(mode)) + def wait_gps_sys_status_not_present_or_enabled_and_healthy(self, timeout=30): self.progress("Waiting for GPS health") - tstart = self.get_sim_time_cached() + tstart = self.get_sim_time() while True: now = self.get_sim_time_cached() if now - tstart > timeout: @@ -7225,7 +7648,8 @@ def wait_gps_sys_status_not_present_or_enabled_and_healthy(self, timeout=30): if (not (m.onboard_control_sensors_health & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_GPS)): self.progress("GPS not healthy") continue - self.progress("GPS healthy") + self.progress("GPS healthy after %f/%f seconds" % + ((now - tstart), timeout)) return def assert_sensor_state(self, sensor, present=True, enabled=True, healthy=True, verbose=False): @@ -7305,6 +7729,19 @@ def assert_fence_disabled(self, timeout=2): # Check fence is not enabled self.assert_not_receiving_message('FENCE_STATUS', timeout=timeout) + def NoArmWithoutMissionItems(self): + '''ensure we can't arm in auto mode without mission items present''' + # load a trivial mission + items = [] + items.append((mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1000, 0, 20000),) + items.append((mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0)) + self.upload_simple_relhome_mission(items) + + self.change_mode('AUTO') + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_ALL) + self.assert_prearm_failure('Mode requires mission', + other_prearm_failures_fatal=False) + def assert_prearm_failure(self, expected_statustext, timeout=5, @@ -7449,6 +7886,7 @@ def wait_ekf_flags(self, required_value, error_bits, timeout=30): self.progress("Waiting for EKF value %u" % required_value) last_print_time = 0 tstart = self.get_sim_time() + m = None while timeout is None or self.get_sim_time_cached() < tstart + timeout: m = self.mav.recv_match(type='EKF_STATUS_REPORT', blocking=True, timeout=timeout) if m is None: @@ -7464,6 +7902,11 @@ def wait_ekf_flags(self, required_value, error_bits, timeout=30): if everything_ok: self.progress("EKF Flags OK") return True + m_str = str(m) + if m is not None: + m_str = self.dump_message_verbose(m) + self.progress("Last EKF_STATUS_REPORT message:") + self.progress(m_str) raise AutoTestTimeoutException("Failed to get EKF.flags=%u" % required_value) @@ -7602,6 +8045,19 @@ def install_script(self, source, scriptname, install_name=None): self.progress("Copying (%s) to (%s)" % (source, dest)) shutil.copy(source, dest) + def install_test_modules(self): + source = os.path.join(self.rootdir(), "libraries", "AP_Scripting", "tests", "modules", "test") + dest = os.path.join("scripts", "modules", "test") + self.progress("Copying (%s) to (%s)" % (source, dest)) + shutil.copytree(source, dest) + + def install_mavlink_module(self): + dest = os.path.join("scripts", "modules", "mavlink") + ardupilotmega_xml = os.path.join(self.rootdir(), "modules", "mavlink", + "message_definitions", "v1.0", "ardupilotmega.xml") + mavgen.mavgen(mavgen.Opts(output=dest, wire_protocol='2.0', language='Lua'), [ardupilotmega_xml]) + self.progress("Installed mavlink module") + def install_example_script(self, scriptname): source = self.script_example_source_path(scriptname) self.install_script(source, scriptname) @@ -7623,6 +8079,15 @@ def remove_installed_script(self, scriptname): except OSError: pass + def remove_installed_modules(self, modulename): + dest = os.path.join("scripts", "modules", modulename) + try: + shutil.rmtree(dest) + except IOError: + pass + except OSError: + pass + def get_mavlink_connection_going(self): # get a mavlink connection going try: @@ -7646,6 +8111,10 @@ def get_mavlink_connection_going(self): self.mav.mav.set_send_callback(self.send_message_hook, self) self.mav.idle_hooks.append(self.idle_hook) + # we need to wait for a heartbeat here. If we don't then + # self.mav.target_system will be zero because it hasn't + # "locked on" to a target system yet. + self.wait_heartbeat() self.set_streamrate(self.sitl_streamrate()) def show_test_timings_key_sorter(self, t): @@ -7676,7 +8145,14 @@ def send_statustext(self, text): text = bytes(text, "ascii") elif 'unicode' in str(type(text)): text = text.encode('ascii') - self.mav.mav.statustext_send(mavutil.mavlink.MAV_SEVERITY_WARNING, text) + seq = 0 + while len(text): + self.mav.mav.statustext_send(mavutil.mavlink.MAV_SEVERITY_WARNING, text[:50], id=self.statustext_id, chunk_seq=seq) + text = text[50:] + seq += 1 + self.statustext_id += 1 + if self.statustext_id > 255: + self.statustext_id = 1 def get_stacktrace(self): return ''.join(traceback.format_stack()) @@ -7733,14 +8209,14 @@ def check_logs(self, name): util.run_cmd('/bin/cp build/sitl/bin/* %s' % to_dir, directory=util.reltopdir('.')) - def run_one_test(self, test, interact=False): + def run_one_test(self, test, interact=False, suppress_stdout=False): '''new-style run-one-test used by run_tests''' for i in range(0, test.attempts-1): - result = self.run_one_test_attempt(test, interact=interact, attempt=i+2) + result = self.run_one_test_attempt(test, interact=interact, attempt=i+2, suppress_stdout=suppress_stdout) if result.passed: return result self.progress("Run attempt failed. Retrying") - return self.run_one_test_attempt(test, interact=interact, attempt=1) + return self.run_one_test_attempt(test, interact=interact, attempt=1, suppress_stdout=suppress_stdout) def print_exception_caught(self, e, send_statustext=True): self.progress("Exception caught: %s" % @@ -7757,11 +8233,36 @@ def progress_file_content(self, filepath): for line in f: self.progress(line.rstrip()) - def run_one_test_attempt(self, test, interact=False, attempt=1): + def dump_process_status(self, result): + '''used to show where the SITL process is upto. Often caused when + we've lost contact''' + + if self.sitl.isalive(): + self.progress("pexpect says it is alive") + for tool in "dumpstack.sh", "dumpcore.sh": + tool_filepath = os.path.join(self.rootdir(), 'Tools', 'scripts', tool) + if util.run_cmd([tool_filepath, str(self.sitl.pid)]) != 0: + reason = "Failed %s" % (tool,) + self.progress(reason) + result.reason = reason + result.passed = False + else: + self.progress("pexpect says it is dead") + + # try dumping the process status file for more information: + status_filepath = "/proc/%u/status" % self.sitl.pid + self.progress("Checking for status filepath (%s)" % status_filepath) + if os.path.exists(status_filepath): + self.progress_file_content(status_filepath) + else: + self.progress("... does not exist") + + def run_one_test_attempt(self, test, interact=False, attempt=1, suppress_stdout=False): '''called by run_one_test to actually run the test in a retry loop''' name = test.name desc = test.description test_function = test.function + test_kwargs = test.kwargs if attempt != 1: self.progress("RETRYING %s" % name) test_output_filename = self.buildlogs_path("%s-%s-retry-%u.txt" % @@ -7770,7 +8271,7 @@ def run_one_test_attempt(self, test, interact=False, attempt=1): test_output_filename = self.buildlogs_path("%s-%s.txt" % (self.log_name(), name)) - tee = TeeBoth(test_output_filename, 'w', self.mavproxy_logfile) + tee = TeeBoth(test_output_filename, 'w', self.mavproxy_logfile, suppress_stdout=suppress_stdout) start_message_hooks = self.mav.message_hooks @@ -7798,7 +8299,7 @@ def run_one_test_attempt(self, test, interact=False, attempt=1): orig_speedup = self.get_parameter("SIM_SPEEDUP") self.set_parameter("SIM_SPEEDUP", test.speedup) - test_function() + test_function(**test_kwargs) except Exception as e: self.print_exception_caught(e) ex = e @@ -7817,13 +8318,8 @@ def run_one_test_attempt(self, test, interact=False, attempt=1): if ex is not None: passed = False - try: - self.context_pop() - except Exception as e: - self.print_exception_caught(e, send_statustext=False) - passed = False - result = Result(test) + result.time_elapsed = self.test_timings[desc] ardupilot_alive = False try: @@ -7832,29 +8328,20 @@ def run_one_test_attempt(self, test, interact=False, attempt=1): except Exception: # process is dead self.progress("No heartbeat after test", send_statustext=False) - if self.sitl.isalive(): - self.progress("pexpect says it is alive") - for tool in "dumpstack.sh", "dumpcore.sh": - tool_filepath = os.path.join(self.rootdir(), 'Tools', 'scripts', tool) - if util.run_cmd([tool_filepath, str(self.sitl.pid)]) != 0: - self.progress("Failed %s" % (tool,)) - result.description - result.passed = False - return result - else: - self.progress("pexpect says it is dead") - - # try dumping the process status file for more information: - status_filepath = "/proc/%u/status" % self.sitl.pid - self.progress("Checking for status filepath (%s)" % status_filepath) - if os.path.exists(status_filepath): - self.progress_file_content(status_filepath) - else: - self.progress("... does not exist") + self.dump_process_status(result) passed = False reset_needed = True + try: + self.context_pop(process_interaction_allowed=ardupilot_alive) + except Exception as e: + self.print_exception_caught(e, send_statustext=False) + passed = False + + # if we haven't already reset ArduPilot because it's dead, + # then ensure the vehicle was disarmed at the end of the test. + # If it wasn't then the test is considered failed: if ardupilot_alive and self.armed() and not self.is_tracker(): if ex is None: ex = ArmedAtEndOfTestException("Still armed at end of test") @@ -7871,6 +8358,9 @@ def run_one_test_attempt(self, test, interact=False, attempt=1): self.progress("Force-rebooting SITL") self.reboot_sitl() # that'll learn it passed = False + elif ardupilot_alive and not passed: # implicit reboot after a failed test: + self.progress("Test failed but ArduPilot process alive; rebooting") + self.reboot_sitl() # that'll learn it if self._mavproxy is not None: self.progress("Stopping auto-started mavproxy") @@ -7896,7 +8386,7 @@ def run_one_test_attempt(self, test, interact=False, attempt=1): # pop off old contexts to clean up message hooks etc while len(self.contexts) > old_contexts_length: try: - self.context_pop() + self.context_pop(process_interaction_allowed=ardupilot_alive) except Exception as e: self.print_exception_caught(e, send_statustext=False) self.progress("Done popping extra contexts") @@ -7944,7 +8434,7 @@ def run_one_test_attempt(self, test, interact=False, attempt=1): def defaults_filepath(self): return None - def start_mavproxy(self): + def start_mavproxy(self, sitl_rcin_port=None, master=None): self.start_mavproxy_count += 1 if self.mavproxy is not None: return self.mavproxy @@ -7956,11 +8446,20 @@ def start_mavproxy(self): if self.valgrind or self.callgrind: pexpect_timeout *= 10 + if sitl_rcin_port is None: + sitl_rcin_port = self.sitl_rcin_port() + + if master is None: + master = 'tcp:127.0.0.1:%u' % self.adjust_ardupilot_port(5762) + mavproxy = util.start_MAVProxy_SITL( self.vehicleinfo_key(), + master=master, logfile=self.mavproxy_logfile, options=self.mavproxy_options(), - pexpect_timeout=pexpect_timeout) + pexpect_timeout=pexpect_timeout, + sitl_rcin_port=sitl_rcin_port, + ) mavproxy.expect(r'Telemetry log: (\S+)\r\n') self.logfile = mavproxy.match.group(1) self.progress("LOGFILE %s" % self.logfile) @@ -7979,7 +8478,9 @@ def stop_mavproxy(self, mavproxy): util.pexpect_close(mavproxy) self._mavproxy = None - def start_SITL(self, binary=None, **sitl_args): + def start_SITL(self, binary=None, sitl_home=None, **sitl_args): + if sitl_home is None: + sitl_home = self.sitl_home() start_sitl_args = { "breakpoints": self.breakpoints, "disable_breakpoints": self.disable_breakpoints, @@ -7987,7 +8488,7 @@ def start_SITL(self, binary=None, **sitl_args): "gdb_no_tui": self.gdb_no_tui, "gdbserver": self.gdbserver, "lldb": self.lldb, - "home": self.sitl_home(), + "home": sitl_home, "speedup": self.speedup, "valgrind": self.valgrind, "callgrind": self.callgrind, @@ -8006,15 +8507,24 @@ def start_SITL(self, binary=None, **sitl_args): self.sitl = util.start_SITL(binary, **start_sitl_args) self.expect_list_add(self.sitl) self.sup_prog = [] + count = 0 for sup_binary in self.sup_binaries: self.progress("Starting Supplementary Program ", sup_binary) - start_sitl_args["customisations"] = [sup_binary[1]] + start_sitl_args["customisations"] = [sup_binary['customisation']] start_sitl_args["supplementary"] = True - sup_prog_link = util.start_SITL(sup_binary[0], **start_sitl_args) + start_sitl_args["stdout_prefix"] = "%s-%u" % (os.path.basename(sup_binary['binary']), count) + start_sitl_args["defaults_filepath"] = sup_binary['param_file'] + sup_prog_link = util.start_SITL(sup_binary['binary'], **start_sitl_args) self.sup_prog.append(sup_prog_link) self.expect_list_add(sup_prog_link) + count += 1 - def get_suplementary_programs(self): + # mavlink will have disconnected here. Explicitly reconnect, + # or the first packet we send will be lost: + if self.mav is not None: + self.mav.reconnect() + + def get_supplementary_programs(self): return self.sup_prog def stop_sup_program(self, instance=None): @@ -8047,23 +8557,18 @@ def start_sup_program(self, instance=None, args=None): "callgrind": self.callgrind, "wipe": True, } - if instance is None: - for sup_binary in self.sup_binaries: - start_sitl_args["customisations"] = [sup_binary[1]] - if args is not None: - start_sitl_args["customisations"] = [sup_binary[1], args] - start_sitl_args["supplementary"] = True - sup_prog_link = util.start_SITL(sup_binary[0], **start_sitl_args) - self.sup_prog.append(sup_prog_link) # add to list - self.expect_list_add(sup_prog_link) # add to expect list - else: - # start only the instance passed - start_sitl_args["customisations"] = [self.sup_binaries[instance][1]] + for i in range(len(self.sup_binaries)): + if instance is not None and instance != i: + continue + sup_binary = self.sup_binaries[i] + start_sitl_args["customisations"] = [sup_binary['customisation']] if args is not None: - start_sitl_args["customisations"] = [self.sup_binaries[instance][1], args] + start_sitl_args["customisations"] = [sup_binary['customisation'], args] start_sitl_args["supplementary"] = True - sup_prog_link = util.start_SITL(self.sup_binaries[instance][0], **start_sitl_args) - self.sup_prog[instance] = sup_prog_link # add to list + start_sitl_args["defaults_filepath"] = sup_binary['param_file'] + sup_prog_link = util.start_SITL(sup_binary['binary'], **start_sitl_args) + time.sleep(1) + self.sup_prog[i] = sup_prog_link # add to list self.expect_list_add(sup_prog_link) # add to expect list def sitl_is_running(self): @@ -8105,6 +8610,7 @@ def init(self): # recv_match and those will not be in self.mav.messages until # you do this! self.wait_heartbeat() + self.get_autopilot_firmware_version() self.progress("Sim time: %f" % (self.get_sim_time(),)) self.apply_default_parameters() @@ -8283,14 +8789,8 @@ def poll_home_position(self, quiet=True, timeout=30): try: self.run_cmd( mavutil.mavlink.MAV_CMD_GET_HOME_POSITION, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - quiet=quiet) + quiet=quiet, + ) except ValueError: continue m = self.mav.messages.get("HOME_POSITION", None) @@ -8374,11 +8874,11 @@ def SetHome(self): start_loc = self.sitl_start_location() self.progress("SITL start loc: %s" % str(start_loc)) delta = abs(orig_home.latitude * 1.0e-7 - start_loc.lat) - if delta > 0.000001: + if delta > 0.000006: raise ValueError("homes differ in lat got=%f vs want=%f delta=%f" % (orig_home.latitude * 1.0e-7, start_loc.lat, delta)) delta = abs(orig_home.longitude * 1.0e-7 - start_loc.lng) - if delta > 0.000001: + if delta > 0.000006: raise ValueError("homes differ in lon got=%f vs want=%f delta=%f" % (orig_home.longitude * 1.0e-7, start_loc.lng, delta)) if self.is_rover(): @@ -8392,15 +8892,12 @@ def SetHome(self): new_y = orig_home.longitude + 2000 new_z = orig_home.altitude + 300000 # 300 metres print("new home: %s %s %s" % (str(new_x), str(new_y), str(new_z))) - self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_SET_HOME, - 0, # p1, - 0, # p2, - 0, # p3, - 0, # p4, - new_x, - new_y, - new_z/1000.0, # mm => m - ) + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_SET_HOME, + p5=new_x, + p6=new_y, + p7=new_z/1000.0, # mm => m + ) home = self.poll_home_position() self.progress("home: %s" % str(home)) @@ -8430,43 +8927,35 @@ def SetHome(self): self.progress("Waiting for EKF to start") self.wait_ready_to_arm() self.progress("now use lat=0, lon=0 to reset home to current location") - self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_SET_HOME, - 0, # p1, - 0, # p2, - 0, # p3, - 0, # p4, - 0, # lat - 0, # lon - new_z/1000.0, # mm => m - ) + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_SET_HOME, + p5=0, # lat + p6=0, # lon + p7=new_z/1000.0, # mm => m + ) home = self.poll_home_position() self.progress("home: %s" % str(home)) if self.distance_to_home(use_cached_home=True) > 1: raise NotAchievedException("Setting home to current location did not work") self.progress("Setting home elsewhere again") - self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_SET_HOME, - 0, # p1, - 0, # p2, - 0, # p3, - 0, # p4, - new_x, - new_y, - new_z/1000.0, # mm => m - ) + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_SET_HOME, + p5=new_x, + p6=new_y, + p7=new_z/1000.0, # mm => m + ) if self.distance_to_home() < 10: raise NotAchievedException("Setting home to location did not work") self.progress("use param1=1 to reset home to current location") - self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_SET_HOME, - 1, # p1, - 0, # p2, - 0, # p3, - 0, # p4, - 37, # lat - 21, # lon - new_z/1000.0, # mm => m - ) + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_SET_HOME, + p1=1, # use current location + p5=37, # lat + p6=21, # lon + p7=new_z/1000.0, # mm => m + ) home = self.poll_home_position() self.progress("home: %s" % str(home)) if self.distance_to_home() > 1: @@ -8480,34 +8969,16 @@ def SetHome(self): def zero_mag_offset_parameters(self, compass_count=3): self.progress("Zeroing Mag OFS parameters") self.get_sim_time() - self.run_cmd(mavutil.mavlink.MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS, - 2, # param1 (compass0) - 0, # param2 - 0, # param3 - 0, # param4 - 0, # param5 - 0, # param6 - 0 # param7 - ) - self.run_cmd(mavutil.mavlink.MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS, - 5, # param1 (compass1) - 0, # param2 - 0, # param3 - 0, # param4 - 0, # param5 - 0, # param6 - 0 # param7 - ) - self.run_cmd(mavutil.mavlink.MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS, - 6, # param1 (compass2) - 0, # param2 - 0, # param3 - 0, # param4 - 0, # param5 - 0, # param6 - 0 # param7 - ) + zero_offset_parameters_hash = {} + for num in "", "2", "3": + for axis in "X", "Y", "Z": + name = "COMPASS_OFS%s_%s" % (num, axis) + zero_offset_parameters_hash[name] = 0 + self.set_parameters(zero_offset_parameters_hash) + # force-save the calibration values: + self.run_cmd_int(mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, p2=76) self.progress("zeroed mag parameters") + params = [ [("SIM_MAG1_OFS1_X", "COMPASS_OFS_X", 0), ("SIM_MAG1_OFS1_Y", "COMPASS_OFS_Y", 0), @@ -8590,17 +9061,15 @@ def reset_pos_and_start_magcal(mavproxy, tmask): mavproxy.send("sitl_stop\n") mavproxy.send("sitl_attitude 0 0 0\n") self.get_sim_time() - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_START_MAG_CAL, - tmask, # p1: mag_mask - 0, # p2: retry - 0, # p3: autosave - 0, # p4: delay - 0, # param5 - 0, # param6 - 0, # param7 - want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED, - timeout=20, - ) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_START_MAG_CAL, + p1=tmask, # p1: mag_mask + p2=0, # retry + p3=0, # autosave + p4=0, # delay + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED, + timeout=20, + ) mavproxy.send("sitl_magcal\n") def do_prep_mag_cal_test(mavproxy, params): @@ -8708,29 +9177,19 @@ def do_test_mag_cal(mavproxy, params, compass_tnumber): self.progress("Calibration progress compass ID %d: %s%%" % (cid, str(reached_pct[cid]))) if cid == 0 and 13 <= reached_pct[0] <= 15: self.progress("Request again to start calibration, it shouldn't restart from 0") - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_START_MAG_CAL, - target_mask, - 0, - 0, - 0, - 0, - 0, - 0, - want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED, - timeout=20, - ) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_START_MAG_CAL, + p1=target_mask, + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED, + timeout=20, + ) if reached_pct[0] > 30: - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_CANCEL_MAG_CAL, - target_mask, # p1: mag_mask - 0, # param2 - 0, # param3 - 0, # param4 - 0, # param5 - 0, # param6 - 0, # param7 - want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED, - ) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_CANCEL_MAG_CAL, + p1=target_mask, + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED, + ) if tstop is None: tstop = self.get_sim_time_cached() if tstop is not None: @@ -8826,17 +9285,12 @@ def do_test_mag_cal(mavproxy, params, compass_tnumber): self.check_zeros_mag_orient() self.progress("Send acceptation and check value") self.wait_heartbeat() - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_ACCEPT_MAG_CAL, - target_mask, # p1: mag_mask - 0, - 0, - 0, - 0, - 0, - 0, - want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED, - timeout=20, - ) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_ACCEPT_MAG_CAL, + p1=target_mask, # p1: mag_mask + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED, + timeout=20, + ) self.check_mag_parameters(params, compass_tnumber) self.verify_parameter_values({"COMPASS_ORIENT": self.get_parameter("SIM_MAG1_ORIENT")}) for count in range(2, compass_tnumber + 1): @@ -8858,17 +9312,15 @@ def do_test_mag_cal(mavproxy, params, compass_tnumber): self.set_parameter("COMPASS_ORIENT%d" % count, self.get_parameter("SIM_MAG%d_ORIENT" % count)) self.arm_vehicle() self.progress("Test calibration rejection when armed") - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_START_MAG_CAL, - target_mask, # p1: mag_mask - 0, # p2: retry - 0, # p3: autosave - 0, # p4: delay - 0, # param5 - 0, # param6 - 0, # param7 - want_result=mavutil.mavlink.MAV_RESULT_FAILED, - timeout=20, - ) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_START_MAG_CAL, + p1=target_mask, # p1: mag_mask + p2=0, # retry + p3=0, # autosave + p4=0, # delay + want_result=mavutil.mavlink.MAV_RESULT_FAILED, + timeout=20, + ) self.disarm_vehicle() self.mavproxy_unload_module(mavproxy, "relay") self.mavproxy_unload_module(mavproxy, "sitl_calibration") @@ -9138,16 +9590,18 @@ def FixedYawCalibration(self): ss = self.assert_receive_message('SIMSTATE', timeout=1, verbose=True) - self.run_cmd(mavutil.mavlink.MAV_CMD_FIXED_MAG_CAL_YAW, - math.degrees(ss.yaw), # param1 - 0, # param2 - 0, # param3 - 0, # param4 + self.run_cmd( + mavutil.mavlink.MAV_CMD_FIXED_MAG_CAL_YAW, + p1=math.degrees(ss.yaw), + ) + self.verify_parameter_values(wanted) - 0, # param5 - 0, # param6 - 0 # param7 - ) + # run same command but as command_int: + self.zero_mag_offset_parameters() + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_FIXED_MAG_CAL_YAW, + p1=math.degrees(ss.yaw), + ) self.verify_parameter_values(wanted) self.progress("Rebooting and making sure we could arm with these values") @@ -9434,6 +9888,16 @@ def ArmFeatures(self): self.progress("default disarm_vehicle() call") self.disarm_vehicle() + self.start_subtest("Arm/disarm vehicle with COMMAND_INT") + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + p1=1, # ARM + ) + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + p1=0, # DISARM + ) + self.progress("arm with mavproxy") mavproxy = self.start_mavproxy() if not self.mavproxy_arm_vehicle(mavproxy): @@ -9569,6 +10033,8 @@ def ArmFeatures(self): self.set_rc(interlock_channel, 1000) self.start_subtest("Test all mode arming") + self.wait_ready_to_arm() + if self.arming_test_mission() is not None: self.load_mission(self.arming_test_mission()) @@ -9595,16 +10061,11 @@ def ArmFeatures(self): else: self.progress("Not armable mode : %s" % mode) self.change_mode(mode) - self.run_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 1, # ARM - 0, - 0, - 0, - 0, - 0, - 0, - want_result=mavutil.mavlink.MAV_RESULT_FAILED - ) + self.run_cmd( + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + p1=1, # ARM + want_result=mavutil.mavlink.MAV_RESULT_FAILED, + ) self.progress("PASS not able to arm in mode : %s" % mode) if mode in self.get_position_armable_modes_list(): self.progress("Armable mode needing Position : %s" % mode) @@ -9617,16 +10078,11 @@ def ArmFeatures(self): self.progress("Not armable mode without Position : %s" % mode) self.wait_gps_disable() self.change_mode(mode) - self.run_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 1, # ARM - 0, - 0, - 0, - 0, - 0, - 0, - want_result=mavutil.mavlink.MAV_RESULT_FAILED - ) + self.run_cmd( + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + p1=1, # ARM + want_result=mavutil.mavlink.MAV_RESULT_FAILED, + ) self.set_parameter("SIM_GPS_DISABLE", 0) self.wait_ekf_happy() # EKF may stay unhappy for a while self.progress("PASS not able to arm without Position in mode : %s" % mode) @@ -9684,7 +10140,7 @@ def ArmFeatures(self): self.progress("ALL PASS") # TODO : Test arming magic; - def get_message_rate(self, victim_message, timeout=10, mav=None): + def measure_message_rate(self, victim_message, timeout=10, mav=None): if mav is None: mav = self.mav tstart = self.get_sim_time() @@ -9708,28 +10164,40 @@ def get_message_rate(self, victim_message, timeout=10, mav=None): def rate_to_interval_us(self, rate): return 1/float(rate)*1000000.0 - def set_message_rate_hz(self, id, rate_hz, mav=None): + def interval_us_to_rate(self, interval): + if interval == 0: + raise ValueError("Zero interval is infinite rate") + return 1000000.0/float(interval) + + def set_message_rate_hz(self, id, rate_hz, mav=None, run_cmd=None): '''set a message rate in Hz; 0 for original, -1 to disable''' - if type(id) == str: + if run_cmd is None: + run_cmd = self.run_cmd + if isinstance(id, str): id = eval("mavutil.mavlink.MAVLINK_MSG_ID_%s" % id) if rate_hz == 0 or rate_hz == -1: set_interval = rate_hz else: set_interval = self.rate_to_interval_us(rate_hz) - self.run_cmd(mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, - id, - set_interval, - 0, - 0, - 0, - 0, - 0, - mav=mav) + run_cmd( + mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, + p1=id, + p2=set_interval, + mav=mav, + ) + + def get_message_rate_hz(self, id, mav=None, run_cmd=None): + '''return rate message is being sent, in Hz''' + if run_cmd is None: + run_cmd = self.run_cmd + + interval = self.get_message_interval(id, mav=mav, run_cmd=run_cmd) + return self.interval_us_to_rate(interval) def send_get_message_interval(self, victim_message, mav=None): if mav is None: mav = self.mav - if type(victim_message) == str: + if isinstance(victim_message, str): victim_message = eval("mavutil.mavlink.MAVLINK_MSG_ID_%s" % victim_message) mav.mav.command_long_send( 1, @@ -9744,6 +10212,32 @@ def send_get_message_interval(self, victim_message, mav=None): 0, 0) + def get_message_interval(self, victim_message, mav=None, run_cmd=None): + '''returns message interval in microseconds''' + if run_cmd is None: + run_cmd = self.run_cmd + + self.send_get_message_interval(victim_message, mav=mav) + m = self.assert_receive_message('MESSAGE_INTERVAL', timeout=1, mav=mav) + + if isinstance(victim_message, str): + victim_message = eval("mavutil.mavlink.MAVLINK_MSG_ID_%s" % victim_message) + if m.message_id != victim_message: + raise NotAchievedException(f"Unexpected ID in MESSAGE_INTERVAL (want={victim_message}, got={m.message_id}") + + return m.interval_us + + def set_message_interval(self, victim_message, interval_us, mav=None): + '''sets message interval in microseconds''' + if isinstance(victim_message, str): + victim_message = eval("mavutil.mavlink.MAVLINK_MSG_ID_%s" % victim_message) + self.run_cmd( + mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, + p1=victim_message, + p2=interval_us, + mav=mav, + ) + def test_rate(self, desc, in_rate, @@ -9760,7 +10254,7 @@ def test_rate(self, self.set_message_rate_hz(victim_message, in_rate, mav=mav) - new_measured_rate = self.get_message_rate(victim_message, timeout=message_rate_sample_period, mav=mav) + new_measured_rate = self.measure_message_rate(victim_message, timeout=message_rate_sample_period, mav=mav) self.progress( "Measured rate: %f (want %f)" % (round(new_measured_rate, ndigits=ndigits), @@ -9804,6 +10298,22 @@ def SET_MESSAGE_INTERVAL(self): self.start_subtest('Many-message tests') self.test_set_message_interval_many() + def MESSAGE_INTERVAL_COMMAND_INT(self): + '''Test MAV_CMD_SET_MESSAGE_INTERVAL works as COMMAND_INT''' + original_rate = round(self.measure_message_rate("VFR_HUD", 20)) + self.context_set_message_rate_hz('VFR_HUD', original_rate*2, run_cmd=self.run_cmd_int) + if abs(original_rate*2 - round(self.get_message_rate_hz("VFR_HUD", run_cmd=self.run_cmd_int))) > 1: + raise NotAchievedException("Did not set rate") + + self.start_subtest("Use REQUEST_MESSAGE via COMMAND_INT") + # 148 is AUTOPILOT_VERSION: + self.context_collect('AUTOPILOT_VERSION') + self.run_cmd_int(mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE, 148) + self.delay_sim_time(2) + count = len(self.context_collection('AUTOPILOT_VERSION')) + if count != 1: + raise NotAchievedException(f"Did not get single AUTOPILOT_VERSION message (count={count}") + def test_set_message_interval_many(self): messages = [ 'CAMERA_FEEDBACK', @@ -9832,7 +10342,7 @@ def assert_message_rate_hz(self, message, want_rate, sample_period=20, ndigits=0 if mav is None: mav = self.mav self.drain_mav(mav) - rate = round(self.get_message_rate(message, sample_period, mav=mav), ndigits=ndigits) + rate = round(self.measure_message_rate(message, sample_period, mav=mav), ndigits=ndigits) self.progress("%s: Want=%f got=%f" % (message, round(want_rate, ndigits=ndigits), round(rate, ndigits=ndigits))) if rate != want_rate: raise NotAchievedException("Did not get expected rate (want=%f got=%f)" % (want_rate, rate)) @@ -9840,7 +10350,7 @@ def assert_message_rate_hz(self, message, want_rate, sample_period=20, ndigits=0 def test_set_message_interval_basic(self): ex = None try: - rate = round(self.get_message_rate("VFR_HUD", 20)) + rate = round(self.measure_message_rate("VFR_HUD", 20)) self.progress("Initial rate: %u" % rate) self.test_rate("Test set to %u" % (rate/2,), rate/2, rate/2, victim_message="VFR_HUD") @@ -9850,7 +10360,7 @@ def test_set_message_interval_basic(self): self.test_rate("Resetting original rate", 0, rate) self.progress("try getting a message which is not ordinarily streamed out") - rate = round(self.get_message_rate("CAMERA_FEEDBACK", 20)) + rate = round(self.measure_message_rate("CAMERA_FEEDBACK", 20)) if rate != 0: raise PreconditionFailedException("Already getting CAMERA_FEEDBACK") self.progress("try various message rates") @@ -9867,7 +10377,7 @@ def test_set_message_interval_basic(self): want_rate = self.get_parameter("SCHED_LOOP_RATE") * 0.8 self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_CAMERA_FEEDBACK, want_rate) - rate = round(self.get_message_rate("CAMERA_FEEDBACK", 20)) + rate = round(self.measure_message_rate("CAMERA_FEEDBACK", 20)) self.set_parameter("SIM_SPEEDUP", old_speedup) self.progress("Want=%f got=%f" % (want_rate, rate)) if abs(rate - want_rate) > 2: @@ -9898,25 +10408,21 @@ def test_set_message_interval_basic(self): def send_poll_message(self, message_id, target_sysid=None, target_compid=None, quiet=False, mav=None): if mav is None: mav = self.mav - if type(message_id) == str: + if isinstance(message_id, str): message_id = eval("mavutil.mavlink.MAVLINK_MSG_ID_%s" % message_id) - self.send_cmd(mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE, - message_id, - 0, - 0, - 0, - 0, - 0, - 0, - target_sysid=target_sysid, - target_compid=target_compid, - quiet=quiet, - mav=mav) + self.send_cmd( + mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE, + p1=message_id, + target_sysid=target_sysid, + target_compid=target_compid, + quiet=quiet, + mav=mav, + ) def poll_message(self, message_id, timeout=10, quiet=False, mav=None): if mav is None: mav = self.mav - if type(message_id) == str: + if isinstance(message_id, str): message_id = eval("mavutil.mavlink.MAVLINK_MSG_ID_%s" % message_id) tstart = self.get_sim_time() # required for timeout in run_cmd_get_ack to work self.send_poll_message(message_id, quiet=quiet, mav=mav) @@ -9925,7 +10431,7 @@ def poll_message(self, message_id, timeout=10, quiet=False, mav=None): mavutil.mavlink.MAV_RESULT_ACCEPTED, timeout, quiet=quiet, - mav=mav + mav=mav, ) while True: if self.get_sim_time_cached() - tstart > timeout: @@ -9966,7 +10472,7 @@ def REQUEST_MESSAGE(self, timeout=60): '''Test MAV_CMD_REQUEST_MESSAGE''' self.set_parameter("CAM1_TYPE", 1) # Camera with servo trigger self.reboot_sitl() # needed for CAM1_TYPE to take effect - rate = round(self.get_message_rate("CAMERA_FEEDBACK", 10)) + rate = round(self.measure_message_rate("CAMERA_FEEDBACK", 10)) if rate != 0: raise PreconditionFailedException("Receiving camera feedback") self.poll_message("CAMERA_FEEDBACK") @@ -9977,10 +10483,11 @@ def clear_mission(self, mission_type, target_system=1, target_component=1): ''' if mission_type == mavutil.mavlink.MAV_MISSION_TYPE_ALL: # recurse - if not self.is_tracker() and not self.is_plane(): + if not self.is_tracker() and not self.is_blimp(): self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE) - self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_MISSION) - if not self.is_sub() and not self.is_tracker(): + if not self.is_blimp(): + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_MISSION) + if not self.is_sub() and not self.is_tracker() and not self.is_blimp(): self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_RALLY) self.last_wp_load = time.time() return @@ -9989,16 +10496,11 @@ def clear_mission(self, mission_type, target_system=1, target_component=1): target_component, 0, mission_type) - m = self.assert_receive_message('MISSION_ACK', timeout=5) - if m.target_system != self.mav.mav.srcSystem: - raise NotAchievedException("ACK not targetted at correct system want=%u got=%u" % - (self.mav.mav.srcSystem, m.target_system)) - if m.target_component != self.mav.mav.srcComponent: - raise NotAchievedException("ACK not targetted at correct component want=%u got=%u" % - (self.mav.mav.srcComponent, m.target_component)) - if m.type != mavutil.mavlink.MAV_MISSION_ACCEPTED: - raise NotAchievedException("Expected MAV_MISSION_ACCEPTED got %s" % - (mavutil.mavlink.enums["MAV_MISSION_RESULT"][m.type].name,)) + self.assert_received_message_field_values('MISSION_ACK', { + "target_system": self.mav.mav.srcSystem, + "target_component": self.mav.mav.srcComponent, + "type": mavutil.mavlink.MAV_MISSION_ACCEPTED, + }) if mission_type == mavutil.mavlink.MAV_MISSION_TYPE_MISSION: self.last_wp_load = time.time() @@ -10126,62 +10628,40 @@ def GripperType(self, gripper_type): self.set_rc(9, 2000) self.wait_text("Gripper load grabb", check_context=True) self.progress("Test gripper with Mavlink cmd") + + self.context_collect('STATUSTEXT') self.progress("Releasing load") - self.wait_text("Gripper load releas", - the_function=lambda: self.mav.mav.command_long_send(1, - 1, - mavutil.mavlink.MAV_CMD_DO_GRIPPER, - 0, - 1, - mavutil.mavlink.GRIPPER_ACTION_RELEASE, - 0, - 0, - 0, - 0, - 0, - )) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_GRIPPER, + p1=1, + p2=mavutil.mavlink.GRIPPER_ACTION_RELEASE + ) + self.wait_text("Gripper load releas", check_context=True) self.progress("Grabbing load") - self.wait_text("Gripper load grabb", - the_function=lambda: self.mav.mav.command_long_send(1, - 1, - mavutil.mavlink.MAV_CMD_DO_GRIPPER, - 0, - 1, - mavutil.mavlink.GRIPPER_ACTION_GRAB, - 0, - 0, - 0, - 0, - 0, - )) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_GRIPPER, + p1=1, + p2=mavutil.mavlink.GRIPPER_ACTION_GRAB + ) + self.wait_text("Gripper load grabb", check_context=True) + + self.context_clear_collection('STATUSTEXT') self.progress("Releasing load") - self.wait_text("Gripper load releas", - the_function=lambda: self.mav.mav.command_long_send(1, - 1, - mavutil.mavlink.MAV_CMD_DO_GRIPPER, - 0, - 1, - mavutil.mavlink.GRIPPER_ACTION_RELEASE, - 0, - 0, - 0, - 0, - 0, - )) + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_GRIPPER, + p1=1, + p2=mavutil.mavlink.GRIPPER_ACTION_RELEASE + ) + self.wait_text("Gripper load releas", check_context=True) + self.progress("Grabbing load") - self.wait_text("Gripper load grabb", - the_function=lambda: self.mav.mav.command_long_send(1, - 1, - mavutil.mavlink.MAV_CMD_DO_GRIPPER, - 0, - 1, - mavutil.mavlink.GRIPPER_ACTION_GRAB, - 0, - 0, - 0, - 0, - 0, - )) + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_GRIPPER, + p1=1, + p2=mavutil.mavlink.GRIPPER_ACTION_GRAB + ) + self.wait_text("Gripper load grabb", check_context=True) + self.context_pop() self.reboot_sitl() @@ -10501,6 +10981,78 @@ def send_yaw_rate(rate, target=None): self.do_RTL(distance_min=0, distance_max=wp_accuracy) self.disarm_vehicle() + def SetpointBadVel(self, timeout=30): + '''try feeding in a very, very bad velocity and make sure it is ignored''' + self.takeoff(mode='GUIDED') + # following values from a real log: + target_speed = Vector3(-3.6019095525029597e+30, + 1.7796490496925177e-41, + 3.0557017120313744e-26) + + self.progress("Feeding in bad global data, hoping we don't move") + + def send_speed_vector_global_int(vector , mav_frame): + self.mav.mav.set_position_target_global_int_send( + 0, # timestamp + self.sysid_thismav(), # target system_id + 1, # target component id + mav_frame, + MAV_POS_TARGET_TYPE_MASK.POS_IGNORE | + MAV_POS_TARGET_TYPE_MASK.ACC_IGNORE | + MAV_POS_TARGET_TYPE_MASK.YAW_IGNORE | + MAV_POS_TARGET_TYPE_MASK.YAW_RATE_IGNORE, + 0, + 0, + 0, + vector.x, # vx + vector.y, # vy + vector.z, # vz + 0, # afx + 0, # afy + 0, # afz + 0, # yaw + 0, # yawrate + ) + self.wait_speed_vector( + Vector3(0, 0, 0), + timeout=timeout, + called_function=lambda plop, empty: send_speed_vector_global_int(target_speed, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT), # noqa + minimum_duration=10 + ) + + self.progress("Feeding in bad local data, hoping we don't move") + + def send_speed_vector_local_ned(vector , mav_frame): + self.mav.mav.set_position_target_local_ned_send( + 0, # timestamp + self.sysid_thismav(), # target system_id + 1, # target component id + mav_frame, + MAV_POS_TARGET_TYPE_MASK.POS_IGNORE | + MAV_POS_TARGET_TYPE_MASK.ACC_IGNORE | + MAV_POS_TARGET_TYPE_MASK.YAW_IGNORE | + MAV_POS_TARGET_TYPE_MASK.YAW_RATE_IGNORE, + 0, + 0, + 0, + vector.x, # vx + vector.y, # vy + vector.z, # vz + 0, # afx + 0, # afy + 0, # afz + 0, # yaw + 0, # yawrate + ) + self.wait_speed_vector( + Vector3(0, 0, 0), + timeout=timeout, + called_function=lambda plop, empty: send_speed_vector_local_ned(target_speed, mavutil.mavlink.MAV_FRAME_LOCAL_NED), # noqa + minimum_duration=10 + ) + + self.do_RTL() + def SetpointGlobalVel(self, timeout=30): """Test set position message in guided mode.""" # Disable heading and yaw rate test on rover type @@ -10863,6 +11415,9 @@ def send_yaw_rate_vel(rate, vector, mav_frame): self.do_RTL(distance_min=0, distance_max=wp_accuracy) self.disarm_vehicle() + def is_blimp(self): + return False + def is_copter(self): return False @@ -10900,7 +11455,7 @@ def upload_fences_from_locations(self, seq = 0 items = [] for locs in list_of_list_of_locs: - if type(locs) == dict: + if isinstance(locs, dict): # circular fence if vertex_type == mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION: v = mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION @@ -10948,6 +11503,22 @@ def upload_fences_from_locations(self, self.check_fence_upload_download(items) + def rally_MISSION_ITEM_INT_from_loc(self, loc): + return self.create_MISSION_ITEM_INT( + mavutil.mavlink.MAV_CMD_NAV_RALLY_POINT, + x=int(loc.lat*1e7), + y=int(loc.lng*1e7), + z=loc.alt, + frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + mission_type=mavutil.mavlink.MAV_MISSION_TYPE_RALLY + ) + + def upload_rally_points_from_locations(self, rally_point_locs): + '''takes a sequence of locations, sets vehicle rally points to those locations''' + items = [self.rally_MISSION_ITEM_INT_from_loc(x) for x in rally_point_locs] + self.correct_wp_seq_numbers(items) + self.check_rally_upload_download(items) + def wait_for_initial_mode(self): '''wait until we get a heartbeat with an expected initial mode (the one specified in the vehicle constructor)''' @@ -11031,7 +11602,7 @@ def assert_current_onboard_log_contains_message(self, messagetype): if not self.current_onboard_log_contains_message(messagetype): raise NotAchievedException("Current onboard log does not contain message %s" % messagetype) - def run_tests(self, tests): + def run_tests(self, tests) -> List[Result]: """Autotest vehicle in SITL.""" if self.run_tests_called: raise ValueError("run_tests called twice") @@ -11230,27 +11801,19 @@ 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.run_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 1, # ARM - 0, - 0, - 0, - 0, - 0, - 0, - timeout=4, - want_result=mavutil.mavlink.MAV_RESULT_FAILED) + self.run_cmd( + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + p1=1, # ARM + timeout=4, + want_result=mavutil.mavlink.MAV_RESULT_FAILED, + ) self.context_pop() - self.run_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 1, # ARM - 0, - 0, - 0, - 0, - 0, - 0, - timeout=4, - want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED) + self.run_cmd( + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + p1=1, # ARM + timeout=4, + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED, + ) self.disarm_vehicle() def assert_not_receiving_message(self, message, timeout=1, mav=None): @@ -11334,13 +11897,7 @@ def AdvancedFailsafe(self): self.context_collect("STATUSTEXT") self.run_cmd( mavutil.mavlink.MAV_CMD_DO_FLIGHTTERMINATION, - 1, # terminate - 0, - 0, - 0, - 0, - 0, - 0, + p1=1, # terminate ) self.wait_statustext("Terminating due to GCS request", check_context=True) self.context_pop() @@ -11380,9 +11937,10 @@ def NMEAOutput(self): '''Test AHRS NMEA Output can be read by out NMEA GPS''' self.set_parameter("SERIAL5_PROTOCOL", 20) # serial5 is NMEA output self.set_parameter("GPS_TYPE2", 5) # GPS2 is NMEA + port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartE=tcp:6735", # GPS2 is NMEA.... - "--uartF=tcpclient:127.0.0.1:6735", # serial5 spews to localhost:6735 + "--uartE=tcp:%u" % port, # GPS2 is NMEA.... + "--uartF=tcpclient:127.0.0.1:%u" % port, # serial5 spews to localhost port ]) self.do_timesync_roundtrip() self.wait_gps_fix_type_gte(3) @@ -11542,6 +12100,17 @@ def ahrstrim_preflight_cal(self): self.progress("Correct value %.4f for %s error %.2f%%" % (v, pname, error_pct)) + def user_takeoff(self, alt_min=30, timeout=30, max_err=5): + '''takeoff using mavlink takeoff command''' + self.run_cmd( + mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, + p7=alt_min, # param7 + ) + self.wait_altitude(alt_min - 1, + (alt_min + max_err), + relative=True, + timeout=timeout) + def ahrstrim_attitude_correctness(self): self.wait_ready_to_arm() HOME = self.sitl_start_location() @@ -11653,19 +12222,15 @@ def Button(self): # try to arm the vehicle: self.run_cmd( mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 1, # ARM - 0, - 0, - 0, - 0, - 0, - 0, - want_result=mavutil.mavlink.MAV_RESULT_FAILED + p1=1, # ARM + want_result=mavutil.mavlink.MAV_RESULT_FAILED, ) - self.wait_statustext("PreArm: Motors Emergency Stopped", check_context=True) + self.assert_prearm_failure("Motors Emergency Stopped", + other_prearm_failures_fatal=False) self.reboot_sitl() - self.delay_sim_time(10) - self.assert_prearm_failure("Motors Emergency Stopped") + self.assert_prearm_failure( + "Motors Emergency Stopped", + other_prearm_failures_fatal=False) self.context_pop() self.reboot_sitl() @@ -11989,10 +12554,11 @@ def FRSkyPassThroughStatustext(self): "RPM1_TYPE": 10, # enable RPM output "TERRAIN_ENABLE": 0, }) + port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:6735" # serial5 spews to localhost:6735 + "--uartF=tcp:%u" % port # serial5 spews to localhost port ]) - frsky = FRSkyPassThrough(("127.0.0.1", 6735), + frsky = FRSkyPassThrough(("127.0.0.1", port), get_time=self.get_sim_time_cached) # waiting until we are ready to arm should ensure our wanted @@ -12009,14 +12575,10 @@ def FRSkyPassThroughStatustext(self): self.context_collect('STATUSTEXT') command = mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION - self.send_cmd(command, - 0, # p1 - 0, # p2 - 1, # p3, baro - 0, # p4 - 0, # p5 - 0, # p6 - 0) # p7 + self.send_cmd( + command, + p3=1, # p3, baro + ) # this is a test for asynchronous handling of mavlink messages: self.run_cmd_get_ack(command, mavutil.mavlink.MAV_RESULT_IN_PROGRESS, 2) self.run_cmd_get_ack(command, mavutil.mavlink.MAV_RESULT_ACCEPTED, 5) @@ -12083,10 +12645,11 @@ def FRSkyPassThroughSensorIDs(self): "SERIAL5_PROTOCOL": 10, # serial5 is FRSky passthrough "RPM1_TYPE": 10, # enable RPM output }) + port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:6735" # serial5 spews to localhost:6735 + "--uartF=tcp:%u" % port # serial5 spews to localhost port ]) - frsky = FRSkyPassThrough(("127.0.0.1", 6735), + frsky = FRSkyPassThrough(("127.0.0.1", port), get_time=self.get_sim_time_cached) self.wait_ready_to_arm() @@ -12237,10 +12800,11 @@ def run_cmd_via_mavlite_get_ack(self, frsky, sport_to_mavlite, command, want_res def FRSkyMAVlite(self): '''Test FrSky MAVlite serial output''' self.set_parameter("SERIAL5_PROTOCOL", 10) # serial5 is FRSky passthrough + port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:6735" # serial5 spews to localhost:6735 + "--uartF=tcp:%u" % port # serial5 spews to localhost port ]) - frsky = FRSkyPassThrough(("127.0.0.1", 6735)) + frsky = FRSkyPassThrough(("127.0.0.1", port)) frsky.connect() sport_to_mavlite = SPortToMAVlite() @@ -12511,10 +13075,11 @@ def FRSkySPort(self): '''Test FrSky SPort mode''' self.set_parameter("SERIAL5_PROTOCOL", 4) # serial5 is FRSky sport self.set_parameter("RPM1_TYPE", 10) # enable SITL RPM sensor + port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:6735" # serial5 spews to localhost:6735 + "--uartF=tcp:%u" % port # serial5 spews to localhost port ]) - frsky = FRSkySPort(("127.0.0.1", 6735), verbose=True) + frsky = FRSkySPort(("127.0.0.1", port), verbose=True) self.wait_ready_to_arm() # we need to start the engine to get some RPM readings, we do it for plane only @@ -12583,24 +13148,21 @@ def FRSkySPort(self): def FRSkyD(self): '''Test FrSkyD serial output''' self.set_parameter("SERIAL5_PROTOCOL", 3) # serial5 is FRSky output + port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:6735" # serial5 spews to localhost:6735 + "--uartF=tcp:%u" % port # serial5 spews to localhost port ]) - frsky = FRSkyD(("127.0.0.1", 6735)) + frsky = FRSkyD(("127.0.0.1", port)) self.wait_ready_to_arm() m = self.assert_receive_message('GLOBAL_POSITION_INT', timeout=1) gpi_abs_alt = int((m.alt+500) / 1000) # mm -> m # grab a battery-remaining percentage - self.run_cmd(mavutil.mavlink.MAV_CMD_BATTERY_RESET, - 255, # battery mask - 96, # percentage - 0, - 0, - 0, - 0, - 0, - 0) + self.run_cmd( + mavutil.mavlink.MAV_CMD_BATTERY_RESET, + p1=65535, # battery mask + p2=96, # percentage + ) m = self.assert_receive_message('BATTERY_STATUS', timeout=1) want_battery_remaining_pct = m.battery_remaining @@ -12705,10 +13267,11 @@ def test_ltm_s(self, ltm): def LTM(self): '''Test LTM serial output''' self.set_parameter("SERIAL5_PROTOCOL", 25) # serial5 is LTM output + port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:6735" # serial5 spews to localhost:6735 + "--uartF=tcp:%u" % port # serial5 spews to localhost port ]) - ltm = LTM(("127.0.0.1", 6735)) + ltm = LTM(("127.0.0.1", port)) self.wait_ready_to_arm() wants = { @@ -12747,10 +13310,11 @@ def DEVO(self): '''Test DEVO serial output''' self.context_push() self.set_parameter("SERIAL5_PROTOCOL", 17) # serial5 is DEVO output + port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:6735" # serial5 spews to localhost:6735 + "--uartF=tcp:%u" % port # serial5 spews to localhost port ]) - devo = DEVO(("127.0.0.1", 6735)) + devo = DEVO(("127.0.0.1", port)) self.wait_ready_to_arm() m = self.assert_receive_message('GLOBAL_POSITION_INT', timeout=1) @@ -12819,10 +13383,11 @@ def MSP_DJI(self): '''Test MSP DJI serial output''' self.set_parameter("SERIAL5_PROTOCOL", 33) # serial5 is MSP DJI output self.set_parameter("MSP_OPTIONS", 1) # telemetry (unpolled) mode + port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:6735" # serial5 spews to localhost:6735 + "--uartF=tcp:%u" % port # serial5 spews to localhost port ]) - msp = MSP_DJI(("127.0.0.1", 6735)) + msp = MSP_DJI(("127.0.0.1", port)) self.wait_ready_to_arm() tstart = self.get_sim_time() @@ -12846,10 +13411,11 @@ def CRSF(self): ex = None try: self.set_parameter("SERIAL5_PROTOCOL", 23) # serial5 is RCIN input + port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:6735" # serial5 reads from to localhost:6735 + "--uartF=tcp:%u" % port # serial5 reads from to localhost port ]) - crsf = CRSF(("127.0.0.1", 6735)) + crsf = CRSF(("127.0.0.1", port)) crsf.connect() self.progress("Writing vtx_frame") @@ -12870,6 +13436,33 @@ def CRSF(self): if ex is not None: raise ex + def CompassPrearms(self): + '''test compass prearm checks''' + self.wait_ready_to_arm() + # XY are checked specially: + for axis in 'X', 'Y': # ArduPilot only checks these two axes + self.context_push() + self.set_parameter(f"COMPASS_OFS2_{axis}", 1000) + self.assert_prearm_failure("Compasses inconsistent") + self.context_pop() + self.wait_ready_to_arm() + + # now test the total anglular difference: + self.context_push() + self.set_parameters({ + "COMPASS_OFS2_X": 1000, + "COMPASS_OFS2_Y": -1000, + "COMPASS_OFS2_Z": -10000, + }) + self.assert_prearm_failure("Compasses inconsistent") + self.context_pop() + self.wait_ready_to_arm() + # the following line papers over a probably problem with the + # EKF recovering from bad compass offsets. Without it, the + # EKF will maintain a 10-degree offset from the true compass + # heading seemingly indefinitely. + self.reboot_sitl() + def AHRS_ORIENTATION(self): '''test AHRS_ORIENTATION parameter works''' self.context_push() @@ -12898,23 +13491,26 @@ def GPSTypes(self): # if gps_type is None we auto-detect sim_gps = [ # (0, "NONE"), - (1, "UBLOX", None, "u-blox"), - (5, "NMEA", 5, "NMEA"), - (6, "SBP", None, "SBP"), - # (7, "SBP2", 9, "SBP2"), # broken, "waiting for config data" - (8, "NOVA", 15, "NOVA"), # no attempt to auto-detect this in AP_GPS + (1, "UBLOX", None, "u-blox", 5, 'detected'), + (5, "NMEA", 5, "NMEA", 5, 'detected'), + (6, "SBP", None, "SBP", 5, 'detected'), + # (7, "SBP2", 9, "SBP2", 5), # broken, "waiting for config data" + (8, "NOVA", 15, "NOVA", 5, 'detected'), # no attempt to auto-detect this in AP_GPS + (11, "GSOF", 11, "GSOF", 5, 'specified'), # no attempt to auto-detect this in AP_GPS + (19, "MSP", 19, "MSP", 32, 'specified'), # no attempt to auto-detect this in AP_GPS # (9, "FILE"), ] self.context_collect("STATUSTEXT") - for (sim_gps_type, name, gps_type, detect_name) in sim_gps: + for (sim_gps_type, name, gps_type, detect_name, serial_protocol, detect_prefix) in sim_gps: self.start_subtest("Checking GPS type %s" % name) self.set_parameter("SIM_GPS_TYPE", sim_gps_type) + self.set_parameter("SERIAL3_PROTOCOL", serial_protocol) if gps_type is None: gps_type = 1 # auto-detect self.set_parameter("GPS_TYPE", gps_type) self.context_clear_collection('STATUSTEXT') self.reboot_sitl() - self.wait_statustext("detected as %s" % detect_name, check_context=True) + self.wait_statustext("%s as %s" % (detect_prefix, detect_name), check_context=True) n = self.poll_home_position(timeout=120) distance = self.get_distance_int(orig, n) if distance > 1: @@ -12980,6 +13576,7 @@ def MultipleGPS(self): (msg, m.alt, gpi_alt)) introduced_error = 10 # in metres self.set_parameter("SIM_GPS2_ALT_OFS", introduced_error) + self.do_timesync_roundtrip() m = self.assert_receive_message("GPS2_RAW") if abs((m.alt-introduced_error*1000) - gpi_alt) > 100: raise NotAchievedException("skewed Alt (%s) discrepancy; %d+%d vs %d" % @@ -12998,9 +13595,11 @@ def MultipleGPS(self): if abs(new_gpi_alt2 - m.alt) > 100: raise NotAchievedException("Failover not detected") - def fetch_file_via_ftp(self, path): + def fetch_file_via_ftp(self, path, timeout=20): '''returns the content of the FTP'able file at path''' + self.progress("Retrieving (%s) using MAVProxy" % path) mavproxy = self.start_mavproxy() + mavproxy.expect("Saved .* parameters to") ex = None tmpfile = tempfile.NamedTemporaryFile(mode='r', delete=False) try: @@ -13009,9 +13608,18 @@ def fetch_file_via_ftp(self, path): mavproxy.send("ftp set debug 1\n") # so we get the "Terminated session" message mavproxy.send("ftp get %s %s\n" % (path, tmpfile.name)) mavproxy.expect("Getting") - self.delay_sim_time(2) - mavproxy.send("ftp status\n") - mavproxy.expect("No transfer in progress") + tstart = self.get_sim_time() + while True: + now = self.get_sim_time() + if now - tstart > timeout: + raise NotAchievedException("expected complete transfer") + self.progress("Polling status") + mavproxy.send("ftp status\n") + try: + mavproxy.expect("No transfer in progress", timeout=1) + break + except Exception: + continue # terminate the connection, or it may still be in progress the next time an FTP is attempted: mavproxy.send("ftp cancel\n") mavproxy.expect("Terminated session") @@ -13056,7 +13664,7 @@ def write_content_to_filepath(self, content, filepath): '''write biunary content to filepath''' with open(filepath, "wb") as f: if sys.version_info.major >= 3: - if type(content) != bytes: + if not isinstance(content, bytes): raise NotAchievedException("Want bytes to write_content_to_filepath") f.write(content) f.close() @@ -13120,6 +13728,77 @@ def EmbeddedParamParser(self): self.customise_SITL_commandline([], binary=binary_with_defaults) self.assert_parameter_values(param_values) + def _MotorTest(self, + command, + timeout=60, + mot1_servo_chan=1, + mot4_servo_chan=4, + wait_finish_text=True, + quadplane=False): + '''Run Motor Tests (with specific mavlink message)''' + self.start_subtest("Testing PWM output") + pwm_in = 1300 + # default frame is "+" - start motor of 2 is "B", which is + # motor 1... see + # https://ardupilot.org/copter/docs/connect-escs-and-motors.html + command( + mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, + p1=2, # start motor + p2=mavutil.mavlink.MOTOR_TEST_THROTTLE_PWM, + p3=pwm_in, # pwm-to-output + p4=2, # timeout in seconds + p5=2, # number of motors to output + p6=0, # compass learning + timeout=timeout, + ) + # long timeouts here because there's a pause before we start motors + self.wait_servo_channel_value(mot1_servo_chan, pwm_in, timeout=10) + self.wait_servo_channel_value(mot4_servo_chan, pwm_in, timeout=10) + if wait_finish_text: + self.wait_statustext("finished motor test") + self.wait_disarmed() + # wait_disarmed is not sufficient here; it's actually the + # *motors* being armed which causes the problem, not the + # vehicle's arm state! Could we use SYS_STATUS here instead? + self.delay_sim_time(10) + self.end_subtest("Testing PWM output") + + self.start_subtest("Testing percentage output") + percentage = 90.1 + # since MOT_SPIN_MIN and MOT_SPIN_MAX are not set, the RC3 + # min/max are used. + expected_pwm = 1000 + (self.get_parameter("RC3_MAX") - self.get_parameter("RC3_MIN")) * percentage/100.0 + # quadplane doesn't use the expect value - it wants 1900 + # rather than the calculated 1901... + if quadplane: + expected_pwm = 1900 + self.progress("expected pwm=%f" % expected_pwm) + command( + mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, + p1=2, # start motor + p2=mavutil.mavlink.MOTOR_TEST_THROTTLE_PERCENT, + p3=percentage, # pwm-to-output + p4=2, # timeout in seconds + p5=2, # number of motors to output + p6=0, # compass learning + timeout=timeout, + ) + self.wait_servo_channel_value(mot1_servo_chan, expected_pwm, timeout=10) + self.wait_servo_channel_value(mot4_servo_chan, expected_pwm, timeout=10) + if wait_finish_text: + self.wait_statustext("finished motor test") + self.wait_disarmed() + # wait_disarmed is not sufficient here; it's actually the + # *motors* being armed which causes the problem, not the + # vehicle's arm state! Could we use SYS_STATUS here instead? + self.delay_sim_time(10) + self.end_subtest("Testing percentage output") + + def MotorTest(self, timeout=60, **kwargs): + '''Run Motor Tests''' # common to Copter and QuadPlane + self._MotorTest(self.run_cmd, **kwargs) + self._MotorTest(self.run_cmd_int, **kwargs) + def tests(self): return [ self.PIDTuning, @@ -13149,13 +13828,13 @@ def post_tests_announcements(self): print("Had to force-reset SITL %u times" % (self.forced_post_test_sitl_reboots,)) - def autotest(self, tests=None, allow_skips=True): + def autotest(self, tests=None, allow_skips=True, step_name=None): """Autotest used by ArduPilot autotest CI.""" if tests is None: tests = self.tests() all_tests = [] for test in tests: - if type(test) != Test: + if not isinstance(test, Test): test = Test(test) all_tests.append(test) @@ -13164,7 +13843,11 @@ def autotest(self, tests=None, allow_skips=True): disabled = {} skip_list = [] tests = [] + seen_test_name = set() for test in all_tests: + if test.name in seen_test_name: + raise ValueError("Duplicate test name %s" % test.name) + seen_test_name.add(test.name) if test.name in disabled: self.progress("##### %s is skipped: %s" % (test, disabled[test.name])) skip_list.append((test, disabled[test.name])) @@ -13186,9 +13869,49 @@ def autotest(self, tests=None, allow_skips=True): print(str(failure)) self.post_tests_announcements() + if self.generate_junit: + if step_name is None: + step_name = "Unknown_step_name" + step_name.replace(".", "_") + self.create_junit_report(step_name, results, skip_list) return len(self.fail_list) == 0 + def create_junit_report(self, test_name: str, results: List[Result], skip_list: List[Tuple[Test, Dict[str, str]]]) -> None: + """Generate Junit report from the autotest results""" + from junitparser import TestCase, TestSuite, JUnitXml, Skipped, Failure + frame = self.vehicleinfo_key() + xml_filename = f"autotest_result_{frame}_{test_name}_junit.xml" + self.progress(f"Writing test result in jUnit format to {xml_filename}\n") + + suite = TestSuite(f"Autotest {frame} {test_name}") + suite.timestamp = datetime.now().replace(microsecond=0).isoformat() + for result in results: + case = TestCase(f"{result.test.name}", f"{frame}", result.time_elapsed) + # f"{result.test.description}" + # case.file ## TODO : add file properties to match test location + if not result.passed: + case.result = [Failure(f"see {result.debug_filename}", f"{result.exception}")] + suite.add_testcase(case) + for skipped in skip_list: + (test, reason) = skipped + case = TestCase(f"{test.name}", f"{test.function}") + case.result = [Skipped(f"Skipped : {reason}")] + + suite.add_property("Firmware Version Major", self.fcu_firmware_version[0]) + suite.add_property("Firmware Version Minor", self.fcu_firmware_version[1]) + suite.add_property("Firmware Version Rev", self.fcu_firmware_version[2]) + suite.add_property("Firmware hash", self.fcu_firmware_hash) + suite.add_property("Git hash", self.githash) + mavproxy_version = util.MAVProxy_version() + suite.add_property("Mavproxy Version Major", mavproxy_version[0]) + suite.add_property("Mavproxy Version Minor", mavproxy_version[1]) + suite.add_property("Mavproxy Version Rev", mavproxy_version[2]) + + xml = JUnitXml() + xml.add_testsuite(suite) + xml.write(xml_filename, pretty=True) + def mavfft_fttd(self, sensor_type, sensor_instance, since, until): '''display fft for raw ACC data in current logfile''' @@ -13307,25 +14030,17 @@ def load_default_params_file(self, filename): def send_pause_command(self): '''pause AUTO/GUIDED modes''' - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_PAUSE_CONTINUE, - 0, # 0: pause, 1: continue - 0, # param2 - 0, # param3 - 0, # param4 - 0, # param5 - 0, # param6 - 0) # param7 + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_PAUSE_CONTINUE, + p1=0, # 0: pause, 1: continue + ) def send_resume_command(self): '''resume AUTO/GUIDED modes''' - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_PAUSE_CONTINUE, - 1, # 0: pause, 1: continue - 0, # param2 - 0, # param3 - 0, # param4 - 0, # param5 - 0, # param6 - 0) # param7 + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_PAUSE_CONTINUE, + p1=1, # 0: pause, 1: continue + ) def enum_state_name(self, enum_name, state, pretrim=None): e = mavutil.mavlink.enums[enum_name] diff --git a/Tools/autotest/web-firmware/Tools/FilterTool/FileSaver.js b/Tools/autotest/web-firmware/Tools/FilterTool/FileSaver.js deleted file mode 100644 index 5d204aee15a8d..0000000000000 --- a/Tools/autotest/web-firmware/Tools/FilterTool/FileSaver.js +++ /dev/null @@ -1,171 +0,0 @@ -/* -* FileSaver.js -* A saveAs() FileSaver implementation. -* -* By Eli Grey, http://eligrey.com -* -* License : https://github.com/eligrey/FileSaver.js/blob/master/LICENSE.md (MIT) -* source : http://purl.eligrey.com/github/FileSaver.js -*/ - -// The one and only way of getting global scope in all environments -// https://stackoverflow.com/q/3277182/1008999 -var _global = typeof window === 'object' && window.window === window - ? window : typeof self === 'object' && self.self === self - ? self : typeof global === 'object' && global.global === global - ? global - : this - -function bom (blob, opts) { - if (typeof opts === 'undefined') opts = { autoBom: false } - else if (typeof opts !== 'object') { - console.warn('Deprecated: Expected third argument to be a object') - opts = { autoBom: !opts } - } - - // prepend BOM for UTF-8 XML and text/* types (including HTML) - // note: your browser will automatically convert UTF-16 U+FEFF to EF BB BF - if (opts.autoBom && /^\s*(?:text\/\S*|application\/xml|\S*\/\S*\+xml)\s*;.*charset\s*=\s*utf-8/i.test(blob.type)) { - return new Blob([String.fromCharCode(0xFEFF), blob], { type: blob.type }) - } - return blob -} - -function download (url, name, opts) { - var xhr = new XMLHttpRequest() - xhr.open('GET', url) - xhr.responseType = 'blob' - xhr.onload = function () { - saveAs(xhr.response, name, opts) - } - xhr.onerror = function () { - console.error('could not download file') - } - xhr.send() -} - -function corsEnabled (url) { - var xhr = new XMLHttpRequest() - // use sync to avoid popup blocker - xhr.open('HEAD', url, false) - try { - xhr.send() - } catch (e) {} - return xhr.status >= 200 && xhr.status <= 299 -} - -// `a.click()` doesn't work for all browsers (#465) -function click (node) { - try { - node.dispatchEvent(new MouseEvent('click')) - } catch (e) { - var evt = document.createEvent('MouseEvents') - evt.initMouseEvent('click', true, true, window, 0, 0, 0, 80, - 20, false, false, false, false, 0, null) - node.dispatchEvent(evt) - } -} - -// Detect WebView inside a native macOS app by ruling out all browsers -// We just need to check for 'Safari' because all other browsers (besides Firefox) include that too -// https://www.whatismybrowser.com/guides/the-latest-user-agent/macos -var isMacOSWebView = _global.navigator && /Macintosh/.test(navigator.userAgent) && /AppleWebKit/.test(navigator.userAgent) && !/Safari/.test(navigator.userAgent) - -var saveAs = _global.saveAs || ( - // probably in some web worker - (typeof window !== 'object' || window !== _global) - ? function saveAs () { /* noop */ } - - // Use download attribute first if possible (#193 Lumia mobile) unless this is a macOS WebView - : ('download' in HTMLAnchorElement.prototype && !isMacOSWebView) - ? function saveAs (blob, name, opts) { - var URL = _global.URL || _global.webkitURL - var a = document.createElement('a') - name = name || blob.name || 'download' - - a.download = name - a.rel = 'noopener' // tabnabbing - - // TODO: detect chrome extensions & packaged apps - // a.target = '_blank' - - if (typeof blob === 'string') { - // Support regular links - a.href = blob - if (a.origin !== location.origin) { - corsEnabled(a.href) - ? download(blob, name, opts) - : click(a, a.target = '_blank') - } else { - click(a) - } - } else { - // Support blobs - a.href = URL.createObjectURL(blob) - setTimeout(function () { URL.revokeObjectURL(a.href) }, 4E4) // 40s - setTimeout(function () { click(a) }, 0) - } - } - - // Use msSaveOrOpenBlob as a second approach - : 'msSaveOrOpenBlob' in navigator - ? function saveAs (blob, name, opts) { - name = name || blob.name || 'download' - - if (typeof blob === 'string') { - if (corsEnabled(blob)) { - download(blob, name, opts) - } else { - var a = document.createElement('a') - a.href = blob - a.target = '_blank' - setTimeout(function () { click(a) }) - } - } else { - navigator.msSaveOrOpenBlob(bom(blob, opts), name) - } - } - - // Fallback to using FileReader and a popup - : function saveAs (blob, name, opts, popup) { - // Open a popup immediately do go around popup blocker - // Mostly only available on user interaction and the fileReader is async so... - popup = popup || open('', '_blank') - if (popup) { - popup.document.title = - popup.document.body.innerText = 'downloading...' - } - - if (typeof blob === 'string') return download(blob, name, opts) - - var force = blob.type === 'application/octet-stream' - var isSafari = /constructor/i.test(_global.HTMLElement) || _global.safari - var isChromeIOS = /CriOS\/[\d]+/.test(navigator.userAgent) - - if ((isChromeIOS || (force && isSafari) || isMacOSWebView) && typeof FileReader !== 'undefined') { - // Safari doesn't allow downloading of blob URLs - var reader = new FileReader() - reader.onloadend = function () { - var url = reader.result - url = isChromeIOS ? url : url.replace(/^data:[^;]*;/, 'data:attachment/file;') - if (popup) popup.location.href = url - else location = url - popup = null // reverse-tabnabbing #460 - } - reader.readAsDataURL(blob) - } else { - var URL = _global.URL || _global.webkitURL - var url = URL.createObjectURL(blob) - if (popup) popup.location = url - else location.href = url - popup = null // reverse-tabnabbing #460 - setTimeout(function () { URL.revokeObjectURL(url) }, 4E4) // 40s - } - } -) - -_global.saveAs = saveAs.saveAs = saveAs - -if (typeof module !== 'undefined') { - module.exports = saveAs; -} diff --git a/Tools/autotest/web-firmware/Tools/FilterTool/app.py b/Tools/autotest/web-firmware/Tools/FilterTool/app.py deleted file mode 100644 index 76ded1b765fe8..0000000000000 --- a/Tools/autotest/web-firmware/Tools/FilterTool/app.py +++ /dev/null @@ -1,16 +0,0 @@ -import os -from flask import Flask -from flask import render_template - -# A flask app to allow hosting filter tool locally - -this_path = os.path.dirname(os.path.realpath(__file__)) - -app = Flask(__name__, template_folder=this_path, static_folder=this_path) - -@app.route('/') -def index(): - return render_template('index.html') - -if __name__ == "__main__": - app.run() diff --git a/Tools/autotest/web-firmware/Tools/FilterTool/filters.js b/Tools/autotest/web-firmware/Tools/FilterTool/filters.js deleted file mode 100644 index b68bc3b67371b..0000000000000 --- a/Tools/autotest/web-firmware/Tools/FilterTool/filters.js +++ /dev/null @@ -1,1196 +0,0 @@ -function calc_lowpass_alpha_dt(dt, cutoff_freq) -{ - if (dt <= 0.0 || cutoff_freq <= 0.0) { - return 1.0; - } - var rc = 1.0/(Math.PI*2*cutoff_freq); - return dt/(dt+rc); -} - -function PID(sample_rate,kP,kI,kD,filtE,filtD) { - - this._kP = kP; - this._kI = kI; - this._kD = kD; - - this._dt = 1.0/sample_rate; - this.E_alpha = calc_lowpass_alpha_dt(this._dt,filtE) - this.D_alpha = calc_lowpass_alpha_dt(this._dt,filtD) - - this._error = 0.0; - this._derivative = 0.0; - this._integrator = 0.0; - - this.reset = function(sample) { - this._error = 0.0; - this._derivative = 0.0; - this._integrator = 0.0; - } - - this.apply = function(error) { - - var error_last = this._error; - this._error += this.E_alpha * (error - this._error); - - var derivative = (this._error - error_last) / this._dt; - this._derivative += this.D_alpha * (derivative - this._derivative) - - this._integrator += this._error * this._kI * this._dt; - - var P_out = this._error * this._kP; - var D_out = this._derivative * this._kD; - - return P_out + this._integrator + D_out; - } - return this; -} - -function LPF_1P(sample_rate,cutoff) { - this.reset = function(sample) { - this.value = sample; - } - if (cutoff <= 0) { - this.apply = function(sample) { - return sample; - } - return this; - } - this.alpha = calc_lowpass_alpha_dt(1.0/sample_rate,cutoff) - this.value = 0.0; - this.apply = function(sample) { - this.value += this.alpha * (sample - this.value); - return this.value; - } - return this; -} - -function DigitalBiquadFilter(sample_freq, cutoff_freq) { - this.delay_element_1 = 0; - this.delay_element_2 = 0; - this.cutoff_freq = cutoff_freq; - - if (cutoff_freq <= 0) { - // zero cutoff means pass-thru - this.reset = function(sample) { - } - this.apply = function(sample) { - return sample; - } - return this; - } - - var fr = sample_freq/cutoff_freq; - var ohm = Math.tan(Math.PI/fr); - var c = 1.0+2.0*Math.cos(Math.PI/4.0)*ohm + ohm*ohm; - - this.b0 = ohm*ohm/c; - this.b1 = 2.0*this.b0; - this.b2 = this.b0; - this.a1 = 2.0*(ohm*ohm-1.0)/c; - this.a2 = (1.0-2.0*Math.cos(Math.PI/4.0)*ohm+ohm*ohm)/c; - this.initialised = false; - - this.apply = function(sample) { - if (!this.initialised) { - this.reset(sample); - this.initialised = true; - } - var delay_element_0 = sample - this.delay_element_1 * this.a1 - this.delay_element_2 * this.a2; - var output = delay_element_0 * this.b0 + this.delay_element_1 * this.b1 + this.delay_element_2 * this.b2; - - this.delay_element_2 = this.delay_element_1; - this.delay_element_1 = delay_element_0; - return output; - } - - this.reset = function(sample) { - this.delay_element_1 = this.delay_element_2 = sample * (1.0 / (1 + this.a1 + this.a2)); - } - - return this; -} - -function sq(v) { - return v*v; -} - -function constrain_float(v,vmin,vmax) { - if (v < vmin) { - return vmin; - } - if (v > vmax) { - return vmax; - } - return v; -} - -function NotchFilter(sample_freq,center_freq_hz,bandwidth_hz,attenuation_dB) { - this.sample_freq = sample_freq; - this.center_freq_hz = center_freq_hz; - this.bandwidth_hz = bandwidth_hz; - this.attenuation_dB = attenuation_dB; - this.need_reset = true; - this.initialised = false; - - this.calculate_A_and_Q = function() { - this.A = Math.pow(10.0, -this.attenuation_dB / 40.0); - if (this.center_freq_hz > 0.5 * this.bandwidth_hz) { - var octaves = Math.log2(this.center_freq_hz / (this.center_freq_hz - this.bandwidth_hz / 2.0)) * 2.0; - this.Q = Math.sqrt(Math.pow(2.0, octaves)) / (Math.pow(2.0, octaves) - 1.0); - } else { - this.Q = 0.0; - } - } - - this.init_with_A_and_Q = function() { - if ((this.center_freq_hz > 0.0) && (this.center_freq_hz < 0.5 * this.sample_freq) && (this.Q > 0.0)) { - var omega = 2.0 * Math.PI * this.center_freq_hz / this.sample_freq; - var alpha = Math.sin(omega) / (2 * this.Q); - this.b0 = 1.0 + alpha*sq(this.A); - this.b1 = -2.0 * Math.cos(omega); - this.b2 = 1.0 - alpha*sq(this.A); - this.a0_inv = 1.0/(1.0 + alpha); - this.a1 = this.b1; - this.a2 = 1.0 - alpha; - this.initialised = true; - } else { - this.initialised = false; - } - } - - // check center frequency is in the allowable range - if ((center_freq_hz > 0.5 * bandwidth_hz) && (center_freq_hz < 0.5 * sample_freq)) { - this.calculate_A_and_Q(); - this.init_with_A_and_Q(); - } else { - this.initialised = false; - } - - this.apply = function(sample) { - if (!this.initialised || this.need_reset) { - // if we have not been initialised when return the input - // sample as output and update delayed samples - this.signal1 = sample; - this.signal2 = sample; - this.ntchsig = sample; - this.ntchsig1 = sample; - this.ntchsig2 = sample; - this.need_reset = false; - return sample; - } - this.ntchsig2 = this.ntchsig1; - this.ntchsig1 = this.ntchsig; - this.ntchsig = sample; - var output = (this.ntchsig*this.b0 + this.ntchsig1*this.b1 + this.ntchsig2*this.b2 - this.signal1*this.a1 - this.signal2*this.a2) * this.a0_inv; - this.signal2 = this.signal1; - this.signal1 = output; - return output; - } - - this.reset = function(sample) { - this.need_reset = true; - this.apply(sample); - } - - return this; -} - -function HarmonicNotchFilter(sample_freq,enable,mode,freq,bw,att,ref,fm_rat,hmncs,opts) { - this.notches = [] - var chained = 1; - var dbl = false; - var triple = false; - var composite_notches = 1; - if (opts & 1) { - dbl = true; - composite_notches = 2; - } else if (opts & 16) { - triple = true; - composite_notches = 3; - } - - this.reset = function(sample) { - for (n in this.notches) { - this.notches[n].reset(sample); - } - } - - if (enable <= 0) { - this.apply = function(sample) { - return sample; - } - return this; - } - - if (mode == 0) { - // fixed notch - } - if (mode == 1) { - var motors_throttle = Math.max(0,get_form("Throttle")); - var throttle_freq = freq * Math.max(fm_rat,Math.sqrt(motors_throttle / ref)); - freq = throttle_freq; - } - if (mode == 2) { - var rpm = get_form("RPM1"); - freq = Math.max(rpm/60.0,freq) * ref; - } - if (mode == 5) { - var rpm = get_form("RPM2"); - freq = Math.max(rpm/60.0,freq) * ref; - } - if (mode == 3) { - if (opts & 2) { - chained = get_form("NUM_MOTORS"); - } - var rpm = get_form("ESC_RPM"); - freq = Math.max(rpm/60.0,freq) * ref; - } - for (var n=0;n<8;n++) { - var fmul = n+1; - if (hmncs & (1< 1) { - var notch_center_double; - // only enable the filter if its center frequency is below the nyquist frequency - notch_center_double = notch_center * (1.0 - notch_spread); - if (notch_center_double < nyquist_limit) { - this.notches.push(new NotchFilter(sample_freq,notch_center_double,bandwidth_hz/composite_notches,att)); - } - // only enable the filter if its center frequency is below the nyquist frequency - notch_center_double = notch_center * (1.0 + notch_spread); - if (notch_center_double < nyquist_limit) { - this.notches.push(new NotchFilter(sample_freq,notch_center_double,bandwidth_hz/composite_notches,att)); - } - } - } - } - } - this.apply = function(sample) { - for (n in this.notches) { - sample = this.notches[n].apply(sample); - } - return sample; - } -} - -function get_form(vname) { - var v = parseFloat(document.getElementById(vname).value); - setCookie(vname, v); - return v; -} - -function run_filters(filters,freq,sample_rate,samples,fast_filters = null,fast_sample_rate = null) { - - for (var j=0;j= best_fit_offset) { - var index = i - best_fit_offset; - X.data[index][0] = Math.sin(t * kt); - X.data[index][1] = Math.cos(t * kt); - y.data[index][0] = output; - } - } - - // Z = a*sin(t*kt + p) + O - var ABO = ML.MatrixLib.solve(X, y); - - var amplitude = Math.sqrt(ABO.get(0,0)*ABO.get(0,0) + ABO.get(1,0)*ABO.get(1,0)); - var phase = Math.atan2(ABO.get(1,0),ABO.get(0,0)) * (-180.0 / Math.PI); - // var DC_offset = ABO.get(2,0); - - return [amplitude,phase]; -} - -var chart_attenuation; -var chart_phase; -var freq_log_scale; - -function get_filters(sample_rate) { - var filters = [] - filters.push(new HarmonicNotchFilter(sample_rate, - get_form("INS_HNTCH_ENABLE"), - get_form("INS_HNTCH_MODE"), - get_form("INS_HNTCH_FREQ"), - get_form("INS_HNTCH_BW"), - get_form("INS_HNTCH_ATT"), - get_form("INS_HNTCH_REF"), - get_form("INS_HNTCH_FM_RAT"), - get_form("INS_HNTCH_HMNCS"), - get_form("INS_HNTCH_OPTS"))); - filters.push(new HarmonicNotchFilter(sample_rate, - get_form("INS_HNTC2_ENABLE"), - get_form("INS_HNTC2_MODE"), - get_form("INS_HNTC2_FREQ"), - get_form("INS_HNTC2_BW"), - get_form("INS_HNTC2_ATT"), - get_form("INS_HNTC2_REF"), - get_form("INS_HNTC2_FM_RAT"), - get_form("INS_HNTC2_HMNCS"), - get_form("INS_HNTC2_OPTS"))); - filters.push(new DigitalBiquadFilter(sample_rate,get_form("INS_GYRO_FILTER"))); - - return filters; -} - -function calculate_filter() { - var sample_rate = get_form("GyroSampleRate"); - var freq_max = get_form("MaxFreq"); - var samples = 1000; - var freq_step = 0.1; - var filters = get_filters(sample_rate); - - var use_dB = document.getElementById("ScaleLog").checked; - setCookie("Scale", use_dB ? "Log" : "Linear"); - var use_RPM = document.getElementById("freq_Scale_RPM").checked; - setCookie("feq_unit", use_RPM ? "RPM" : "Hz"); - var unwrap_pahse = document.getElementById("ScaleUnWrap").checked; - setCookie("PhaseScale", unwrap_pahse ? "unwrap" : "wrap"); - var attenuation = [] - var phase_lag = [] - var min_phase_lag = 0.0; - var max_phase_lag = 0.0; - var phase_wrap = 0.0; - var min_atten = 0.0; - var max_atten = 1.0; - var last_phase = 0.0; - var atten_string = "Magnitude"; - if (use_dB) { - max_atten = 0; - min_atten = -10; - atten_string = "Magnitude (dB)"; - } - var freq_string = "Frequency (Hz)"; - if (use_RPM) { - freq_string = "Frequency (RPM)"; - } - - // start at zero - attenuation.push({x:0, y:1}); - phase_lag.push({x:0, y:0}); - - for (freq=freq_step; freq<=freq_max; freq+=freq_step) { - var v = run_filters(filters, freq, sample_rate, samples); - var aten = v[0]; - var phase = v[1] + phase_wrap; - if (use_dB) { - // show power in decibels - aten = 20 * Math.log10(aten); - } - var freq_value = freq; - if (use_RPM) { - freq_value *= 60; - } - if (unwrap_pahse) { - var phase_diff = phase - last_phase; - if (phase_diff > 180) { - phase_wrap -= 360.0; - phase -= 360.0; - } else if (phase_diff < -180) { - phase_wrap += 360.0; - phase += 360.0; - } - } - attenuation.push({x:freq_value, y:aten}); - phase_lag.push({x:freq_value, y:-phase}); - - min_atten = Math.min(min_atten, aten); - max_atten = Math.max(max_atten, aten); - min_phase_lag = Math.min(min_phase_lag, phase) - max_phase_lag = Math.max(max_phase_lag, phase) - last_phase = phase; - } - - if (unwrap_pahse) { - min_phase_lag = Math.floor((min_phase_lag-10)/10)*10; - min_phase_lag = Math.min(Math.max(-get_form("MaxPhaseLag"), min_phase_lag), 0); - max_phase_lag = Math.ceil((max_phase_lag+10)/10)*10; - max_phase_lag = Math.min(get_form("MaxPhaseLag"), max_phase_lag); - } else { - min_phase_lag = -180; - max_phase_lag = 180; - } - - if (use_RPM) { - freq_max *= 60.0; - } - - var freq_log = document.getElementById("freq_ScaleLog").checked; - setCookie("feq_scale", freq_log ? "Log" : "Linear"); - if ((freq_log_scale != null) && (freq_log_scale != freq_log)) { - // Scale changed, no easy way to update, delete chart and re-draw - chart_attenuation.clear(); - chart_attenuation.destroy(); - chart_attenuation = null; - chart_phase.clear(); - chart_phase.destroy(); - chart_phase = null; - } - freq_log_scale = freq_log; - - if (chart_attenuation) { - chart_attenuation.data.datasets[0].data = attenuation; - chart_attenuation.options.scales.xAxes[0].ticks.max = freq_max; - chart_attenuation.options.scales.xAxes[0].scaleLabel.labelString = freq_string - chart_attenuation.options.scales.yAxes[0].ticks.min = min_atten - chart_attenuation.options.scales.yAxes[0].ticks.max = max_atten; - chart_attenuation.options.scales.yAxes[0].scaleLabel.labelString = atten_string; - chart_attenuation.update(); - } else { - chart_attenuation = new Chart("Attenuation", { - type : "scatter", - data: { - datasets: [ - { - label: 'Magnitude', - yAxisID: 'Magnitude', - pointRadius: 0, - hitRadius: 8, - borderColor: "rgba(0,0,255,1.0)", - pointBackgroundColor: "rgba(0,0,255,1.0)", - data: attenuation, - showLine: true, - fill: false - }, - ] - }, - options: { - aspectRatio: 3, - legend: {display: false}, - scales: { - yAxes: [ - { - scaleLabel: { display: true, labelString: atten_string }, - id: 'Magnitude', - ticks: {min:min_atten, max:max_atten} - }, - ], - xAxes: [ - { - type: (freq_log ? "logarithmic" : "linear"), - scaleLabel: { display: true, labelString: freq_string }, - ticks: - { - min:0.0, - max:freq_max, - callback: function(value, index, ticks) { - return value; - }, - } - } - ], - }, - tooltips: { - callbacks: { - label: function(tooltipItem, data) { - // round tooltip to two decimal places - return tooltipItem.xLabel.toFixed(2) + ', ' + tooltipItem.yLabel.toFixed(2); - } - } - } - } - }); - } - - - if (chart_phase) { - chart_phase.data.datasets[0].data = phase_lag; - chart_phase.options.scales.xAxes[0].ticks.max = freq_max; - chart_phase.options.scales.xAxes[0].scaleLabel.labelString = freq_string - chart_phase.options.scales.yAxes[0].ticks.min = -max_phase_lag; - chart_phase.options.scales.yAxes[0].ticks.max = -min_phase_lag; - chart_phase.update(); - } else { - chart_phase = new Chart("Phase", { - type : "scatter", - data: { - datasets: [ - { - label: 'Phase', - yAxisID: 'Phase', - pointRadius: 0, - hitRadius: 8, - borderColor: "rgba(255,0,0,1.0)", - pointBackgroundColor: "rgba(255,0,0,1.0)", - data: phase_lag, - showLine: true, - fill: false - } - ] - }, - options: { - aspectRatio: 3, - legend: {display: false}, - scales: { - yAxes: [ - { - scaleLabel: { display: true, labelString: "Phase (deg)" }, - id: 'Phase', - ticks: {min:-max_phase_lag, max:-min_phase_lag} - } - ], - xAxes: [ - { - type: (freq_log ? "logarithmic" : "linear"), - scaleLabel: { display: true, labelString: freq_string }, - ticks: - { - min:0.0, - max:freq_max, - callback: function(value, index, ticks) { - return value; - }, - } - } - ], - }, - tooltips: { - callbacks: { - label: function(tooltipItem, data) { - // round tooltip to two decimal places - return tooltipItem.xLabel.toFixed(2) + ', ' + tooltipItem.yLabel.toFixed(2); - } - } - } - } - }); - } -} - -var PID_attenuation; -var PID_phase; -var PID_freq_log_scale; - -function calculate_pid(axis_id) { - //var sample_rate = get_form("GyroSampleRate"); - var PID_rate = get_form("SCHED_LOOP_RATE") - var filters = [] - var freq_max = get_form("PID_MaxFreq"); - var samples = 1000; - var freq_step = 0.1; - - // default to roll axis - var axis_prefix = "ATC_RAT_RLL_"; - if (axis_id == "CalculatePitch") { - var axis_prefix = "ATC_RAT_PIT_"; - document.getElementById("PID_title").innerHTML = "Pitch axis"; - } else if (axis_id == "CalculateYaw") { - var axis_prefix = "ATC_RAT_YAW_"; - document.getElementById("PID_title").innerHTML = "Yaw axis"; - } else { - document.getElementById("PID_title").innerHTML = "Roll axis"; - } - - filters.push(new PID(PID_rate, - get_form(axis_prefix + "P"), - get_form(axis_prefix + "I"), - get_form(axis_prefix + "D"), - get_form(axis_prefix + "FLTE"), - get_form(axis_prefix + "FLTD"))); - - var use_dB = document.getElementById("PID_ScaleLog").checked; - setCookie("PID_Scale", use_dB ? "Log" : "Linear"); - var use_RPM = document.getElementById("PID_freq_Scale_RPM").checked; - setCookie("PID_feq_unit", use_RPM ? "RPM" : "Hz"); - var unwrap_pahse = document.getElementById("PID_ScaleUnWrap").checked; - setCookie("PID_PhaseScale", unwrap_pahse ? "unwrap" : "wrap"); - var attenuation = [] - var phase_lag = [] - var min_phase_lag = 0.0; - var max_phase_lag = 0.0; - var phase_wrap = 0.0; - var min_atten = Infinity; - var max_atten = -Infinity; - var last_phase = 0.0; - var atten_string = "Gain"; - if (use_dB) { - max_atten = 0; - min_atten = -10; - atten_string = "Gain (dB)"; - } - var freq_string = "Frequency (Hz)"; - if (use_RPM) { - freq_string = "Frequency (RPM)"; - } - - var fast_filters = null; - var fast_sample_rate = null; - if (document.getElementById("PID_filtering_Post").checked) { - fast_sample_rate = get_form("GyroSampleRate"); - fast_filters = get_filters(fast_sample_rate); - } - setCookie("filtering", fast_filters == null ? "Pre" : "Post"); - - - for (freq=freq_step; freq<=freq_max; freq+=freq_step) { - var v = run_filters(filters, freq, PID_rate, samples, fast_filters, fast_sample_rate); - var aten = v[0]; - var phase = v[1] + phase_wrap; - if (use_dB) { - // show power in decibels - aten = 20 * Math.log10(aten); - } - var freq_value = freq; - if (use_RPM) { - freq_value *= 60; - } - if (unwrap_pahse) { - var phase_diff = phase - last_phase; - if (phase_diff > 180) { - phase_wrap -= 360.0; - phase -= 360.0; - } else if (phase_diff < -180) { - phase_wrap += 360.0; - phase += 360.0; - } - } - attenuation.push({x:freq_value, y:aten}); - phase_lag.push({x:freq_value, y:-phase}); - - min_atten = Math.min(min_atten, aten); - max_atten = Math.max(max_atten, aten); - min_phase_lag = Math.min(min_phase_lag, phase) - max_phase_lag = Math.max(max_phase_lag, phase) - last_phase = phase; - } - - if (use_RPM) { - freq_max *= 60.0; - } - - var mean_atten = (min_atten + max_atten) * 0.5; - var atten_range = Math.max((max_atten - min_atten) * 0.5 * 1.1, 1.0); - min_atten = mean_atten - atten_range; - max_atten = mean_atten + atten_range; - - if (unwrap_pahse) { - min_phase_lag = Math.floor((min_phase_lag-10)/10)*10; - min_phase_lag = Math.min(Math.max(-get_form("PID_MaxPhaseLag"), min_phase_lag), 0); - max_phase_lag = Math.ceil((max_phase_lag+10)/10)*10; - max_phase_lag = Math.min(get_form("PID_MaxPhaseLag"), max_phase_lag); - } else { - min_phase_lag = -180; - max_phase_lag = 180; - } - - var freq_log = document.getElementById("PID_freq_ScaleLog").checked; - setCookie("PID_feq_scale", use_dB ? "Log" : "Linear"); - if ((PID_freq_log_scale != null) && (PID_freq_log_scale != freq_log)) { - // Scale changed, no easy way to update, delete chart and re-draw - PID_attenuation.clear(); - PID_attenuation.destroy(); - PID_attenuation = null; - PID_phase.clear(); - PID_phase.destroy(); - PID_phase = null; - } - PID_freq_log_scale = freq_log; - - if (PID_attenuation) { - PID_attenuation.data.datasets[0].data = attenuation; - PID_attenuation.options.scales.xAxes[0].ticks.max = freq_max; - PID_attenuation.options.scales.xAxes[0].scaleLabel.labelString = freq_string - PID_attenuation.options.scales.yAxes[0].ticks.min = min_atten - PID_attenuation.options.scales.yAxes[0].ticks.max = max_atten; - PID_attenuation.options.scales.yAxes[0].scaleLabel.labelString = atten_string; - PID_attenuation.update(); - } else { - PID_attenuation = new Chart("PID_Attenuation", { - type : "scatter", - data: { - datasets: [ - { - label: 'Gain', - yAxisID: 'Gain', - pointRadius: 0, - hitRadius: 8, - borderColor: "rgba(0,0,255,1.0)", - pointBackgroundColor: "rgba(0,0,255,1.0)", - data: attenuation, - showLine: true, - fill: false - }, - ] - }, - options: { - aspectRatio: 3, - legend: {display: false}, - scales: { - yAxes: [ - { - scaleLabel: { display: true, labelString: atten_string }, - id: 'Gain', - position: 'left', - ticks: {min:min_atten, max:max_atten} - }, - ], - xAxes: [ - { - type: (freq_log ? "logarithmic" : "linear"), - scaleLabel: { display: true, labelString: freq_string }, - ticks: - { - min:0.0, - max:freq_max, - callback: function(value, index, ticks) { - return value; - }, - } - } - ], - }, - tooltips: { - callbacks: { - label: function(tooltipItem, data) { - // round tooltip to two decimal places - return tooltipItem.xLabel.toFixed(2) + ', ' + tooltipItem.yLabel.toFixed(2); - } - } - } - } - }); - } - - - if (PID_phase) { - PID_phase.data.datasets[0].data = phase_lag; - PID_phase.options.scales.xAxes[0].ticks.max = freq_max; - PID_phase.options.scales.xAxes[0].scaleLabel.labelString = freq_string - PID_phase.options.scales.yAxes[0].ticks.min = -max_phase_lag; - PID_phase.options.scales.yAxes[0].ticks.max = -min_phase_lag; - PID_phase.update(); - } else { - PID_phase = new Chart("PID_Phase", { - type : "scatter", - data: { - datasets: [ - { - label: 'PhaseLag', - yAxisID: 'PhaseLag', - pointRadius: 0, - hitRadius: 8, - borderColor: "rgba(255,0,0,1.0)", - pointBackgroundColor: "rgba(255,0,0,1.0)", - data: phase_lag, - showLine: true, - fill: false - } - ] - }, - options: { - aspectRatio: 3, - legend: {display: false}, - scales: { - yAxes: [ - { - scaleLabel: { display: true, labelString: "Phase (deg)" }, - id: 'PhaseLag', - ticks: {min:-max_phase_lag, max:-min_phase_lag} - } - ], - xAxes: [ - { - type: (freq_log ? "logarithmic" : "linear"), - scaleLabel: { display: true, labelString: freq_string }, - ticks: - { - min:0.0, - max:freq_max, - callback: function(value, index, ticks) { - return value; - }, - } - } - ], - }, - tooltips: { - callbacks: { - label: function(tooltipItem, data) { - // round tooltip to two decimal places - return tooltipItem.xLabel.toFixed(2) + ', ' + tooltipItem.yLabel.toFixed(2); - } - } - } - } - }); - } -} - -function load() { - var url_string = (window.location.href).toLowerCase(); - if (url_string.indexOf('?') == -1) { - // no query params, load from cookies - load_cookies(); - return; - } - - // populate from query's - var params = new URL(url_string).searchParams; - var sections = ["params", "PID_params"]; - for (var j = 0; j -1 ? cookie.substr(0, eqPos) : cookie; - document.cookie = name + "=;expires=Thu, 01 Jan 1970 00:00:00 GMT"; - } -} - -function save_parameters() { - var params = ""; - var inputs = document.forms["params"].getElementsByTagName("input"); - for (const v in inputs) { - var name = "" + inputs[v].name; - if (name.startsWith("INS_")) { - var value = inputs[v].value; - params += name + "," + value + "\n"; - } - } - var blob = new Blob([params], { type: "text/plain;charset=utf-8" }); - saveAs(blob, "filter.param"); -} - -async function load_parameters(file) { - var text = await file.text(); - var lines = text.split('\n'); - for (i in lines) { - var line = lines[i]; - line = line.replace("Q_A_RAT_","ATC_RAT_"); - v = line.split(/[\s,=\t]+/); - if (v.length >= 2) { - var vname = v[0]; - var value = v[1]; - var fvar = document.getElementById(vname); - if (fvar) { - fvar.value = value; - console.log("set " + vname + "=" + value); - } - } - } - fill_docs(); - update_all_hidden(); - calculate_filter(); -} - -function fill_docs() -{ - var inputs = document.forms["params"].getElementsByTagName("input"); - for (const v in inputs) { - var name = inputs[v].name; - var doc = document.getElementById(name + ".doc"); - if (!doc) { - continue; - } - if (inputs[v].onchange == null) { - inputs[v].onchange = fill_docs; - } - var value = parseFloat(inputs[v].value); - if (name.endsWith("_ENABLE")) { - if (value >= 1) { - doc.innerHTML = "Enabled"; - } else { - doc.innerHTML = "Disabled"; - } - } else if (name.endsWith("_MODE")) { - switch (Math.floor(value)) { - case 0: - doc.innerHTML = "Fixed notch"; - break; - case 1: - doc.innerHTML = "Throttle"; - break; - case 2: - doc.innerHTML = "RPM Sensor 1"; - break; - case 3: - doc.innerHTML = "ESC Telemetry"; - break; - case 4: - doc.innerHTML = "Dynamic FFT"; - break; - case 5: - doc.innerHTML = "RPM Sensor 2"; - break; - default: - doc.innerHTML = "INVALID"; - break; - } - } else if (name.endsWith("_OPTS")) { - var ival = Math.floor(value); - var bits = []; - if (ival & 1) { - bits.push("Double Notch"); - } - if (ival & 2) { - bits.push("Dynamic Harmonic"); - } - if (ival & 4) { - bits.push("Loop Rate"); - } - if (ival & 8) { - bits.push("All IMUs Rate"); - } - if ((ival & 16) && (ival & 1) == 0) { - bits.push("Triple Notch"); - } - doc.innerHTML = bits.join(", "); - } else if (name.endsWith("_HMNCS")) { - var ival = Math.floor(value); - var bits = []; - if (ival & 1) { - bits.push("Fundamental"); - } - if (ival & 2) { - bits.push("1st Harmonic"); - } - if (ival & 4) { - bits.push("2nd Harmonic"); - } - if (ival & 8) { - bits.push("3rd Harmonic"); - } - if (ival & 16) { - bits.push("4th Harmonic"); - } - if (ival & 32) { - bits.push("5th Harmonic"); - } - if (ival & 64) { - bits.push("6th Harmonic"); - } - doc.innerHTML = bits.join(", "); - } - - } -} - -// update all hidden params, to be called at init -function update_all_hidden() -{ - var enable_params = ["INS_HNTCH_ENABLE", "INS_HNTC2_ENABLE"]; - for (var i=-0;i