diff --git a/.editorconfig b/.editorconfig index 2c73f926bdb5ab..2ceadeedf2bd00 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/.github/workflows/cygwin_build.yml b/.github/workflows/cygwin_build.yml index 4157e219af2d6c..4b174721603596 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 diff --git a/.github/workflows/esp32_build.yml b/.github/workflows/esp32_build.yml index f24d8190d5608f..d61477b32dbf36 100644 --- a/.github/workflows/esp32_build.yml +++ b/.github/workflows/esp32_build.yml @@ -1,6 +1,144 @@ 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 diff --git a/.github/workflows/macos_build.yml b/.github/workflows/macos_build.yml index e9e54b8d9b8190..67e736006e902e 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 diff --git a/.github/workflows/test_ccache.yml b/.github/workflows/test_ccache.yml index 3f3ba047dfcf4c..ef1961c363afc9 100644 --- a/.github/workflows/test_ccache.yml +++ b/.github/workflows/test_ccache.yml @@ -1,9 +1,124 @@ 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 @@ -11,7 +126,7 @@ concurrency: jobs: build: runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest + container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.0.29 strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: @@ -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 bf0a4f78dc9526..e9082244ad09a0 100644 --- a/.github/workflows/test_chibios.yml +++ b/.github/workflows/test_chibios.yml @@ -1,10 +1,132 @@ 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 @@ -12,7 +134,7 @@ concurrency: jobs: build: runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest + container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.0.29 strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: @@ -30,35 +152,19 @@ 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 diff --git a/.github/workflows/test_coverage.yml b/.github/workflows/test_coverage.yml index 8feaabfe2da503..b6c5403ac77f90 100644 --- a/.github/workflows/test_coverage.yml +++ b/.github/workflows/test_coverage.yml @@ -15,7 +15,7 @@ jobs: build: runs-on: ubuntu-20.04 container: - image: ardupilot/ardupilot-dev-${{ matrix.type }}:latest + image: ardupilot/ardupilot-dev-${{ matrix.type }}:v0.0.29 options: --privileged strategy: fail-fast: false # don't cancel if a job from the matrix fails @@ -59,8 +59,8 @@ jobs: - name: Configure CAN if: ${{ matrix.config == 'sitltest-can'}} run: | - sudo apt update - sudo apt -y linux-modules-extra-$(uname -r) + 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 diff --git a/.github/workflows/test_dds.yml b/.github/workflows/test_dds.yml index 4ebe109d6ff944..77cdb854fa104e 100644 --- a/.github/workflows/test_dds.yml +++ b/.github/workflows/test_dds.yml @@ -1,9 +1,138 @@ name: test dds -on: [push, pull_request, workflow_dispatch] -# paths: -# - "*" -# - "!README.md" <-- don't rebuild on doc change +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 diff --git a/.github/workflows/test_environment.yml b/.github/workflows/test_environment.yml index ebd3124d3942ae..01bcb5f60dc2da 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: @@ -25,6 +34,8 @@ jobs: name: focal - os: ubuntu name: jammy + - os: ubuntu + name: lunar - os: archlinux name: latest - os: debian @@ -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 3db0df687afc17..943360ca08b701 100644 --- a/.github/workflows/test_linux_sbc.yml +++ b/.github/workflows/test_linux_sbc.yml @@ -1,9 +1,133 @@ 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 @@ -11,7 +135,7 @@ concurrency: jobs: build: runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest + container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.0.29 strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: diff --git a/.github/workflows/test_replay.yml b/.github/workflows/test_replay.yml index fb3d3b2d761e2f..462fa6477e2873 100644 --- a/.github/workflows/test_replay.yml +++ b/.github/workflows/test_replay.yml @@ -1,27 +1,146 @@ 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 @@ -29,7 +148,7 @@ concurrency: jobs: build: runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest + container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.0.29 strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: diff --git a/.github/workflows/test_scripting.yml b/.github/workflows/test_scripting.yml index 67074fe019bde3..8fbc8a10d7d150 100644 --- a/.github/workflows/test_scripting.yml +++ b/.github/workflows/test_scripting.yml @@ -22,7 +22,7 @@ concurrency: jobs: test-scripting: runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-base:latest + container: ardupilot/ardupilot-dev-base:v0.0.29 steps: # git checkout the PR - uses: actions/checkout@v3 diff --git a/.github/workflows/test_scripts.yml b/.github/workflows/test_scripts.yml index 1f73fa05677367..410037f5ba6e63 100644 --- a/.github/workflows/test_scripts.yml +++ b/.github/workflows/test_scripts.yml @@ -9,7 +9,7 @@ concurrency: jobs: build: runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-base:latest + container: ardupilot/ardupilot-dev-base:v0.0.29 strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: diff --git a/.github/workflows/test_sitl_copter.yml b/.github/workflows/test_sitl_copter.yml index 0d76aed9048b33..e72c244ed8aae6 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: @@ -26,7 +161,7 @@ concurrency: jobs: build: runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest + container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.0.29 strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: @@ -72,7 +207,7 @@ jobs: 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 container: - image: ardupilot/ardupilot-dev-base:latest + image: ardupilot/ardupilot-dev-base:v0.0.29 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 @@ -140,7 +275,7 @@ jobs: build-gcc-heli: runs-on: ubuntu-20.04 container: - image: ardupilot/ardupilot-dev-base:latest + image: ardupilot/ardupilot-dev-base:v0.0.29 options: --privileged --cap-add=SYS_PTRACE --security-opt apparmor=unconfined --security-opt seccomp=unconfined steps: # git checkout the PR @@ -175,7 +310,7 @@ 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 + container: ardupilot/ardupilot-dev-base:v0.0.29 strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: diff --git a/.github/workflows/test_sitl_periph.yml b/.github/workflows/test_sitl_periph.yml index 514f814ffa5c55..7c8d8d14150dbf 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: @@ -28,7 +160,7 @@ concurrency: jobs: build-gcc-ap_periph: runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-periph:latest + container: ardupilot/ardupilot-dev-periph:v0.0.29 steps: # git checkout the PR - uses: actions/checkout@v3 @@ -69,7 +201,7 @@ jobs: 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 container: - image: ardupilot/ardupilot-dev-periph:latest + image: ardupilot/ardupilot-dev-periph:v0.0.29 options: --privileged strategy: fail-fast: false # don't cancel if a job from the matrix fails diff --git a/.github/workflows/test_sitl_plane.yml b/.github/workflows/test_sitl_plane.yml index 172aa820d0868b..b04962e8b9f94a 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: @@ -26,7 +162,7 @@ concurrency: jobs: build: runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest + container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.0.29 strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: @@ -72,7 +208,7 @@ jobs: 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 container: - image: ardupilot/ardupilot-dev-base:latest + image: ardupilot/ardupilot-dev-base:v0.0.29 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 diff --git a/.github/workflows/test_sitl_rover.yml b/.github/workflows/test_sitl_rover.yml index 12f9b0ff2c66f5..baf8ae40d9cd60 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: @@ -26,7 +161,7 @@ concurrency: jobs: build: runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest + container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.0.29 strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: @@ -72,7 +207,7 @@ jobs: 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 container: - image: ardupilot/ardupilot-dev-base:latest + image: ardupilot/ardupilot-dev-base:v0.0.29 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 diff --git a/.github/workflows/test_sitl_sub.yml b/.github/workflows/test_sitl_sub.yml index 7f7c7f16e08ed4..d54ee417776c5a 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: @@ -26,7 +164,7 @@ concurrency: jobs: build: runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest + container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.0.29 strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: @@ -72,7 +210,7 @@ jobs: 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 container: - image: ardupilot/ardupilot-dev-base:latest + image: ardupilot/ardupilot-dev-base:v0.0.29 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 diff --git a/.github/workflows/test_sitl_tracker.yml b/.github/workflows/test_sitl_tracker.yml index f31761beab52fa..e57e038d47f3b1 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: @@ -26,7 +164,7 @@ concurrency: jobs: build: runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest + container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.0.29 strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: @@ -72,7 +210,7 @@ jobs: 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 container: - image: ardupilot/ardupilot-dev-base:latest + image: ardupilot/ardupilot-dev-base:v0.0.29 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 diff --git a/.github/workflows/test_size.yml b/.github/workflows/test_size.yml index 3e3f08fa6e5536..26c42d2c67ba7f 100644 --- a/.github/workflows/test_size.yml +++ b/.github/workflows/test_size.yml @@ -1,9 +1,59 @@ 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 @@ -11,7 +61,7 @@ concurrency: jobs: build: runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest + container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.0.29 strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: @@ -23,6 +73,7 @@ jobs: MatekF405, 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!) ] @@ -65,7 +116,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 @@ -138,7 +190,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 +283,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 +305,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 54c660ea6b818a..933ee4f21064f0 100644 --- a/.github/workflows/test_unit_tests.yml +++ b/.github/workflows/test_unit_tests.yml @@ -1,9 +1,98 @@ 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 @@ -12,7 +101,7 @@ jobs: build: runs-on: ubuntu-20.04 container: - image: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest + image: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.0.29 options: --user 1001 strategy: fail-fast: false # don't cancel if a job from the matrix fails diff --git a/.gitignore b/.gitignore index 58913bfed4d3dd..823d39ed5aeb10 100644 --- a/.gitignore +++ b/.gitignore @@ -111,7 +111,8 @@ GRTAGS GTAGS *.apj .gdbinit -/.vscode +.vscode/* +!.vscode/extensions.json /.history Parameters.html Parameters.md @@ -125,6 +126,7 @@ parameters.edn LogMessages.html LogMessages.rst LogMessages.xml +LogMessages.md # JetBrains IDE files .idea/* # CMake diff --git a/.gitmodules b/.gitmodules index ff6918a69b89bd..fbfe76753a51d6 100644 --- a/.gitmodules +++ b/.gitmodules @@ -34,8 +34,8 @@ [submodule "modules/Micro-XRCE-DDS-Client"] path = modules/Micro-XRCE-DDS-Client url = https://github.com/ardupilot/Micro-XRCE-DDS-Client.git - branch = develop + branch = master [submodule "modules/Micro-CDR"] path = modules/Micro-CDR url = https://github.com/ardupilot/Micro-CDR.git - branch = develop + branch = master diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 9cd87421bcf78e..01af44c5848bf7 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -41,7 +41,12 @@ repos: hooks: - id: format-xmllint files: libraries/AP_DDS/dds_xrce_profile.xml -# Disable isort and mypy temporarily due to config conflicts + - repo: https://github.com/psf/black + rev: 23.1.0 + hooks: + - id: black + files: libraries\/AP_DDS\/(wscript|.*\.py)$ + # # Use to sort python imports by name and put system import first. # - repo: https://github.com/pycqa/isort # rev: 5.12.0 diff --git a/.semaphore/semaphore.yml b/.semaphore/semaphore.yml deleted file mode 100644 index 19c75718d99a4d..00000000000000 --- 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 e9cd876a0b91cf..4284ec07442f18 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/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index 02c48c6b94f38e..c5581708ab3584 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -412,7 +412,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 +425,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_long_packet(const mavlink_command_long_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 +444,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_long_packet(packet, msg); } } diff --git a/AntennaTracker/GCS_Mavlink.h b/AntennaTracker/GCS_Mavlink.h index 5dcaaeda496655..d7bfc34a954a80 100644 --- a/AntennaTracker/GCS_Mavlink.h +++ b/AntennaTracker/GCS_Mavlink.h @@ -9,6 +9,8 @@ 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 @@ -16,11 +18,10 @@ class GCS_MAVLINK_Tracker : public GCS_MAVLINK // 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_long_packet(const mavlink_command_long_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/Tracker.cpp b/AntennaTracker/Tracker.cpp index e7e1a384587e7d..f3284e985ec353 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), }; diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp index 191c236294a131..3e296c3fa5a77f 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 1adb76acba8a9a..ff55bece071f3e 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/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 8f7ae7186612ea..1bdbc630bc2a9d 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,17 +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; + 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; } @@ -49,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) @@ -212,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))) { @@ -332,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); @@ -623,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); @@ -790,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; } @@ -845,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 00000000000000..e5f1f49ec74213 --- /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 aicraft 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 00000000000000..e9a879106ebef0 --- /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; +private: + /* + Return true if Copter is ready to handle external control data. + Currently checks mode and arm states. + */ + bool ready_for_external_control(); +}; + +#endif // AP_EXTERNAL_CONTROL_ENABLED diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 1241b5f9b2336d..8bbbc186c19e14 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -178,7 +178,9 @@ 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 AC_PRECLAND_ENABLED SCHED_TASK(update_precland, 400, 50, 69), @@ -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), @@ -446,6 +447,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 @@ -562,6 +583,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 +614,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 @@ -718,7 +744,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 bde520ce01befe..b6d5ce552fb2ac 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -100,6 +100,11 @@ #include "AP_Rally.h" // Rally point library #include "AP_Arming.h" +#include +#if AP_EXTERNAL_CONTROL_ENABLED +#include "AP_ExternalControl_Copter.h" +#endif + #include #if AP_BEACON_ENABLED #include @@ -193,6 +198,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 +234,7 @@ class Copter : public AP_Vehicle { friend class ModeZigZag; friend class ModeAutorotate; friend class ModeTurtle; + friend class AP_ExternalControl_Copter; Copter(void); @@ -319,6 +328,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; @@ -680,6 +695,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); @@ -851,9 +868,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(); diff --git a/ArduCopter/GCS_Copter.h b/ArduCopter/GCS_Copter.h index e00ec46f37b59d..4e419add63084b 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 9250a7928404f2..3b1e612e6c20df 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -579,6 +579,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 @@ -728,18 +736,18 @@ 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: #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); @@ -749,17 +757,17 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i return handle_command_pause_continue(packet); 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_long_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,11 +775,11 @@ 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_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg) { switch(packet.command) { @@ -818,16 +826,6 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ } 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: // param1 : target angle [0-360] // param2 : speed during change [deg per second] @@ -938,23 +936,6 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ 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 - /* Solo user presses Fly button */ case MAV_CMD_SOLO_BTN_FLY_CLICK: { if (copter.failsafe.radio) { @@ -1022,14 +1003,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); + return GCS_MAVLINK::handle_command_long_packet(packet, msg); } } @@ -1061,7 +1036,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 +1064,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 +1238,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 +1339,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 diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index 2c0828aae47657..cb33755d89d7f9 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -30,10 +30,10 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK 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; #if HAL_MOUNT_ENABLED - MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet) override; + MAV_RESULT handle_command_mount(const mavlink_command_long_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_long_packet(const mavlink_command_long_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 +55,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; diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 579a1baa446504..334c4b02934414 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -162,7 +162,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 +449,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_ @@ -1038,6 +1040,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 @@ -1215,6 +1218,15 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = { // @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 diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 15f719224c585e..18a32dd25fc184 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -681,6 +681,7 @@ class ParametersG2 { 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 03d398c67a3c42..f400694b451b38 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) { diff --git a/ArduCopter/RC_Channel.h b/ArduCopter/RC_Channel.h index 576135f91f438d..6d9d9f9426801c 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 9ca8364d6cbc23..dcc974b5829b53 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,5 +1,311 @@ ArduPilot Copter Release Notes: ------------------------------------------------------------------ +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) + - Takeoff throttle max is configurable (see TKOFF_TH_MAX) + - 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-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 diff --git a/ArduCopter/config.h b/ArduCopter/config.h index a260b6cba4ec67..7a7b6900c2fa7d 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -27,6 +27,7 @@ /// change in your local copy of APM_Config.h. /// #include "APM_Config.h" +#include ////////////////////////////////////////////////////////////////////////////// @@ -146,8 +147,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 ////////////////////////////////////////////////////////////////////////////// @@ -189,7 +190,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 ////////////////////////////////////////////////////////////////////////////// @@ -201,7 +202,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 ////////////////////////////////////////////////////////////////////////////// @@ -237,7 +238,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 ////////////////////////////////////////////////////////////////////////////// @@ -249,19 +250,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 ////////////////////////////////////////////////////////////////////////////// @@ -269,17 +270,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 @@ -287,6 +289,7 @@ #else # define MODE_AUTOROTATE_ENABLED DISABLED #endif +#endif ////////////////////////////////////////////////////////////////////////////// // RADIO CONFIGURATION @@ -561,7 +564,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 diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index b3cd317f76e68a..d5c55be60fea14 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -151,6 +151,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 69e4edb3b11735..94077c5088040c 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 516de29c84ce51..d1e0822b200bdb 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -194,6 +194,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 +264,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 diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index dc417f51c6c837..28c78411797a85 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -2,6 +2,7 @@ #include "Copter.h" #include +#include // TODO why is this needed if Copter.h includes this class Parameters; class ParametersG2; @@ -462,6 +463,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(); @@ -483,10 +485,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: @@ -569,7 +567,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 @@ -607,7 +605,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); @@ -960,6 +958,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; } @@ -1318,6 +1320,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, @@ -1717,6 +1723,7 @@ class ModeAvoidADSB : public ModeGuided { }; +#if AP_FOLLOW_ENABLED class ModeFollow : public ModeGuided { public: @@ -1746,6 +1753,7 @@ class ModeFollow : public ModeGuided { uint32_t last_log_ms; // system time of last time desired velocity was logging }; +#endif class ModeZigZag : public Mode { @@ -1781,6 +1789,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); diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 77be3560749f48..cdf41a4f7aa47c 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -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; @@ -502,7 +502,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 +519,7 @@ void ModeAuto::nav_guided_start() // set submode set_submode(SubMode::NAVGUIDED); } -#endif //NAV_GUIDED +#endif //AC_NAV_GUIDED bool ModeAuto::is_landing() const { @@ -647,7 +647,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; @@ -719,7 +719,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; @@ -892,7 +892,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; @@ -1033,7 +1033,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 +1041,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 @@ -1751,7 +1751,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 +1770,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) @@ -1892,7 +1892,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); } @@ -2188,7 +2188,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 +2200,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 +2251,9 @@ bool ModeAuto::resume() return true; } +bool ModeAuto::paused() const +{ + return wp_nav->paused(); +} + #endif diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 55d71f1c63f484..65a353ebc78296 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 c86de8856df188..1c73b807dd3dce 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,18 @@ 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()) { + Vector3p loc = copter.circle_nav->get_center(); + loc.z = 0; + Location circle_center(loc, 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 afa348503b084c..a0649c5956c24f 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 fd313a1c0d0831..4d0f9e3154cfb2 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; } @@ -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_rtl.cpp b/ArduCopter/mode_rtl.cpp index e6ae505e8214a6..795e483e93423a 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -554,4 +554,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_turtle.cpp b/ArduCopter/mode_turtle.cpp index 99c5d2b08d73c1..74dc49421ecff6 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/motor_test.cpp b/ArduCopter/motor_test.cpp index 6efd17d41f5d37..5c5deae8995cd7 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); diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index fd103ee21895f8..589561b8cfe406 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 4e1527e2fc569f..221a92bb9a2700 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 @@ -151,8 +153,10 @@ 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 @@ -196,11 +200,6 @@ 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(); diff --git a/ArduCopter/takeoff_check.cpp b/ArduCopter/takeoff_check.cpp index f3b55ec45fb3d1..48f172a02cc366 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/version.h b/ArduCopter/version.h index 35ee178e09076a..aca789abdc19fa 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 54fdc5592f961c..7ab5e92b8e9901 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,22 +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 (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 ); @@ -129,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) { @@ -304,7 +325,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 @@ -377,6 +398,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 } /* @@ -416,3 +458,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 afce68ae116955..b340ecdfc2f81e 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 d832d8fed3a342..61d76425333278 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)); @@ -511,12 +525,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 +551,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 +568,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 @@ -855,9 +870,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 { @@ -870,7 +906,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; } } @@ -887,4 +923,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 0b51098b43ce00..90484121cb60ba 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -50,7 +50,7 @@ float Plane::calc_speed_scaler(void) speed_scaler = 1; } if (!plane.ahrs.airspeed_sensor_enabled() && - (plane.g2.flight_options & FlightOptions::SURPRESS_TKOFF_SCALING) && + (plane.flight_option_enabled(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 return MIN(speed_scaler, 1.0f) ; } @@ -145,7 +145,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))); } /* @@ -208,23 +208,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 +255,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; @@ -268,7 +275,7 @@ 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; } @@ -300,38 +307,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 +365,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 +389,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 +438,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 +448,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; @@ -505,35 +482,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 +519,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 +546,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 +564,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 +575,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(); } diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index b75509936d17e9..99160eccea0c7e 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); } @@ -711,7 +711,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 @@ -950,7 +950,7 @@ 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) { @@ -963,22 +963,22 @@ 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 - return MAV_RESULT_FAILED; - + 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_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg) { switch(packet.command) { @@ -1102,18 +1102,8 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l 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); + return GCS_MAVLINK::handle_command_long_packet(packet, msg); } } @@ -1316,6 +1306,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 +1316,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 849efe8a17306b..7d391fcc5acbe1 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -11,18 +11,21 @@ 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_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; + MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg) override; MAV_RESULT handle_command_do_set_mission_current(const mavlink_command_long_t &packet) override; void send_position_target_global_int() override; diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index f8b1a3ac576491..5925050cb0795c 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(); + } } @@ -110,6 +113,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 +129,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 +141,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 +222,7 @@ struct PACKED log_AETR { float throttle; float rudder; float flap; + float steering; float speed_scaler; }; @@ -232,6 +236,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(), }; @@ -374,8 +379,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 }, @@ -422,15 +427,17 @@ const struct LogStructure Plane::log_structure[] = { // @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 +450,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 83c1924e4287b2..73f0d4a65a420d 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -38,6 +38,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 @@ -424,8 +432,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 +613,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 +755,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_ @@ -1092,6 +1095,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), @@ -1239,7 +1243,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), @@ -1251,17 +1255,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 db02c44565e8be..3045ff8eda7e02 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; @@ -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; @@ -551,7 +551,7 @@ class ParametersG2 { AC_PID guidedHeading{5000.0, 0.0, 0.0, 0 , 10.0, 5.0, 5.0 , 5.0 , 0.2}; #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 087e86cd414ab2..5369536a434466 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -83,6 +83,10 @@ #include #include // Landing Gear library #include +#include +#if AP_EXTERNAL_CONTROL_ENABLED +#include +#endif #include "GCS_Mavlink.h" #include "GCS_Plane.h" @@ -190,6 +194,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 +215,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 +222,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; } @@ -386,6 +384,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; @@ -401,11 +400,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 @@ -503,6 +504,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 @@ -686,6 +690,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 @@ -767,6 +774,11 @@ class Plane : public AP_Vehicle { 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[]; @@ -859,12 +871,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; @@ -966,7 +977,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); @@ -974,6 +984,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 @@ -1021,11 +1035,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(); @@ -1064,6 +1080,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); @@ -1079,7 +1096,6 @@ 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_servos_old_elevons(void); void set_servos_flaps(void); @@ -1099,6 +1115,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); @@ -1226,6 +1243,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 1da0108f6ce95c..e476982eb85c09 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(); } } } @@ -355,7 +359,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 35663554a51e0f..7ecc352c32aa8b 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 70036d3c84690e..6cdec344fadd06 100644 --- a/ArduPlane/ReleaseNotes.txt +++ b/ArduPlane/ReleaseNotes.txt @@ -1,3 +1,356 @@ +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 + - Paramters 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 ------------------------------ diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 6387edef12aad0..04aa72b2707469 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 */ diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index b6d017a53a7295..2edc79b158e5c3 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -1062,7 +1062,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); @@ -1078,7 +1078,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 ba29c2062d8015..a8c6c890c96cb0 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 8caeae82042799..7f15e0e2663f90 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 e880eef494b3a0..c7389daf49bdca 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/events.cpp b/ArduPlane/events.cpp index 9c2a215608ce78..122b3dc361d211 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 avoide 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 avoide 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/mode.cpp b/ArduPlane/mode.cpp index 63559a50dd9950..a9af6555b7a6a4 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,6 +89,9 @@ 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(); @@ -197,3 +204,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 46e56c284c4a7d..4da472cd063929 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -65,7 +65,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 +79,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 +128,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 +146,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,6 +219,8 @@ class ModeAuto : public Mode bool mode_allows_autotuning() const override { return true; } + bool is_landing() const override; + protected: bool _enter() override; @@ -355,6 +370,8 @@ class ModeManual : public Mode // methods that affect movement of the vehicle in this mode void update() override; + + void run() override; }; @@ -398,6 +415,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_acro.cpp b/ArduPlane/mode_acro.cpp index 5bfeea2f81b920..acf0f497523d80 100644 --- a/ArduPlane/mode_acro.cpp +++ b/ArduPlane/mode_acro.cpp @@ -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 548dd0b2618b8a..6ebf3546ef1690 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -159,3 +159,8 @@ 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); +} diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index 900b084f8ca596..2d712c75987222 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -35,9 +35,67 @@ 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); + + plane.g2.guidedHeading.update_error(error, delta); // push error into AC_PID , possible improvement is to use update_all instead.? + + float i = plane.g2.guidedHeading.get_i(); // get integrator TODO + if (((is_negative(error) && !plane.guided_state.target_heading_limit_low) || (is_positive(error) && !plane.guided_state.target_heading_limit_high))) { + i = plane.g2.guidedHeading.get_i(); + } + + float desired = plane.g2.guidedHeading.get_p() + i + plane.g2.guidedHeading.get_d(); + plane.guided_state.target_heading_limit_low = (desired <= -bank_limit); + plane.guided_state.target_heading_limit_high = (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 e7b07c9db127e9..05ce3a69568572 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 68535f96186ba7..1f8fcd702cd14e 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 d1c69de4e3e0df..b74a4343841611 100644 --- a/ArduPlane/mode_qacro.cpp +++ b/ArduPlane/mode_qacro.cpp @@ -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 d74cf792b5d05c..28bf977128faf2 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 13c1a4c4e65edb..1fe4848296a0c6 100644 --- a/ArduPlane/mode_qhover.cpp +++ b/ArduPlane/mode_qhover.cpp @@ -24,6 +24,13 @@ 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); @@ -32,6 +39,10 @@ void ModeQHover::run() } else { 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 26ef2bf214f33c..e39ad63559ed42 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(); @@ -112,6 +122,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 75d82b48225fab..16e805a40d5c05 100644 --- a/ArduPlane/mode_qrtl.cpp +++ b/ArduPlane/mode_qrtl.cpp @@ -86,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 @@ -165,6 +172,10 @@ void ModeQRTL::run() break; } } + + // Stabilize with fixed wing surfaces + plane.stabilize_roll(); + plane.stabilize_pitch(); } /* @@ -184,7 +195,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; @@ -209,7 +220,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 2c8da2d96885aa..39f168fd736d55 100644 --- a/ArduPlane/mode_qstabilize.cpp +++ b/ArduPlane/mode_qstabilize.cpp @@ -40,15 +40,28 @@ 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; + } + // 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 diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp index fbe368b2bc573b..417eb6170f22ef 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 7414d575065724..b73dfb6fc26506 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 6e1984a7d9527e..bed8ebb65e2ea0 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 @@ -40,7 +41,7 @@ const AP_Param::GroupInfo ModeTakeoff::var_info[] = { // @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 9215fe0aba34d5..23c81fbcc288f8 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 cfc640cab9859e..67dee708b50b7b 100644 --- a/ArduPlane/motor_test.cpp +++ b/ArduPlane/motor_test.cpp @@ -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 16ceb260351585..b60b442646e69d 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 @@ -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(); @@ -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 1a1bf6e873fdc2..3ec890ac3dc3f9 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 5d308773ca172e..5a8a895f1ef6ba 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1546,7 +1546,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; @@ -2366,7 +2367,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 +2407,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 +2474,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 +2490,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; } @@ -2949,15 +2963,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; } @@ -3226,7 +3240,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; @@ -4097,6 +4112,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); @@ -4138,12 +4157,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(); } @@ -4349,7 +4372,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 @@ -4528,4 +4551,31 @@ 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; +} + #endif // HAL_QUADPLANE_ENABLED diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index d87709562c3dc6..ae65c3768e1666 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,15 @@ 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; + private: AP_AHRS &ahrs; @@ -223,9 +233,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); @@ -583,7 +590,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; diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 02fa2f79e8ba4e..7a28ca11f59f1a 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 @@ -221,7 +222,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 +278,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 +439,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 04033d00f665ac..f64f37b276d6b3 100644 --- a/ArduPlane/reverse_thrust.cpp +++ b/ArduPlane/reverse_thrust.cpp @@ -144,7 +144,7 @@ float Plane::get_throttle_input(bool no_deadzone) const float Plane::get_adjusted_throttle_input(bool no_deadzone) const { 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 11f50fabd8494c..ae6f3c69c5fc17 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -376,32 +376,6 @@ void Plane::set_servos_idle(void) 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); - -#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(); - - // 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 @@ -572,7 +546,7 @@ void Plane::set_servos_controlled(void) control_mode == &mode_autotune) { // a manual throttle mode if (!rc().has_valid_input()) { - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_passthru_stabilize ? 0.0 : MAX(min_throttle,0)); } else if (g.throttle_passthru_stabilize) { // manual pass through of throttle while in FBWA or // STABILIZE mode with THR_PASS_STAB set @@ -845,34 +819,7 @@ void Plane::set_servos(void) 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(); } @@ -968,6 +915,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 becuase 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 +962,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 149aa273e36587..6f21b75abc7519 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,6 +218,40 @@ 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) { @@ -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 815aac3c65ed01..3c8fe77d728402 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -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 @@ -880,7 +880,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 +892,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 +907,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, leveling 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 1d91fa49e589b7..4f2ff33c3ba370 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 31aeab02295f07..2b316fe8e0b0ef 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(); diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index 5591fb3b6d1733..0c40e47d1be95f 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -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), diff --git a/ArduPlane/transition.h b/ArduPlane/transition.h index 9ddc1d5b5f5790..b31e45ff040ece 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/version.h b/ArduPlane/version.h index 59de573b2b2254..cdf466f7155fb6 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 1386bd7b7c1c9d..4dd670ccc34a52 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 7eb1589a8178d4..84789a10f664cc 100644 --- a/ArduSub/AP_Arming_Sub.cpp +++ b/ArduSub/AP_Arming_Sub.cpp @@ -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(); diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 918a189eb34130..b8ded91fddc343 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 @@ -346,4 +355,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/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index f078629041daf0..a753490e8eba13 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 @@ -448,7 +448,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; } @@ -460,17 +460,17 @@ bool GCS_MAVLINK_Sub::set_home(const Location& loc, bool _lock) { } -MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_long_t &packet) +MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg) { switch (packet.command) { case MAV_CMD_NAV_LOITER_UNLIM: - if (!sub.set_mode(POSHOLD, ModeReason::GCS_COMMAND)) { + 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)) { + if (!sub.set_mode(Mode::Number::SURFACE, ModeReason::GCS_COMMAND)) { return MAV_RESULT_FAILED; } return MAV_RESULT_ACCEPTED; @@ -483,7 +483,7 @@ 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; @@ -500,7 +500,7 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon return MAV_RESULT_FAILED; case MAV_CMD_MISSION_START: - if (sub.motors.armed() && sub.set_mode(AUTO, ModeReason::GCS_COMMAND)) { + if (sub.motors.armed() && sub.set_mode(Mode::Number::AUTO, ModeReason::GCS_COMMAND)) { return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; @@ -516,7 +516,7 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon return MAV_RESULT_ACCEPTED; default: - return GCS_MAVLINK::handle_command_long_packet(packet); + return GCS_MAVLINK::handle_command_long_packet(packet, msg); } } @@ -588,7 +588,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 +598,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 +606,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 +642,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 +678,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 +696,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 +725,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; @@ -794,7 +804,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 +814,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 +824,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 +833,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 937224027c1949..c1b65dfb910973 100644 --- a/ArduSub/GCS_Mavlink.h +++ b/ArduSub/GCS_Mavlink.h @@ -8,6 +8,7 @@ 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 { @@ -16,12 +17,10 @@ class GCS_MAVLINK_Sub : public GCS_MAVLINK { MAV_RESULT handle_flight_termination(const mavlink_command_long_t &packet) override; - uint8_t sysid_my_gcs() const 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_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg) override; // override sending of scaled_pressure3 to send on-board temperature: void send_scaled_pressure3() override; diff --git a/ArduSub/GCS_Sub.cpp b/ArduSub/GCS_Sub.cpp index d535a823e6b6c0..b57839c9bc842e 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 1dbf759d6ab938..f2649bc8417a92 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 75afeb22ca1c65..e7162c5ebcf1b7 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -409,9 +409,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 +622,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 +701,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 eeff0a26935c14..12e300732e66f7 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -5,6 +5,8 @@ #include #include +#include +#include #if AP_SCRIPTING_ENABLED #include @@ -318,6 +320,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 +350,22 @@ 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}, + { "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}, +}; diff --git a/ArduSub/RC_Channel.h b/ArduSub/RC_Channel.h index 7b7139a1e946bd..e7ee787b509356 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/Sub.cpp b/ArduSub/Sub.cpp index bd73ce41a6bf15..4275ddb7109d46 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,7 +37,8 @@ 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; diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 95205f84b260fc..72f6193542991f 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -71,6 +71,7 @@ #include "Parameters.h" #include "AP_Arming_Sub.h" #include "GCS_Sub.h" +#include "mode.h" #include // Optical Flow library @@ -108,6 +109,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 +202,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 +246,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 +295,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 +425,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 +439,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); @@ -563,8 +515,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,6 +524,8 @@ 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); @@ -608,6 +561,24 @@ 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; public: void mainloop_failsafe_check(); diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index 855f8b0adf6862..f35eac79675ef5 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 @@ -410,7 +410,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 +459,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 +529,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 +548,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 +579,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 +614,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 +629,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 +638,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 +681,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 d09d7e351c129b..4a517afe13adbb 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 959ad69f8e8117..00000000000000 --- 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 a420cfe3149ea7..00000000000000 --- 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 fca637e9bf8f3f..00000000000000 --- 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 3c13e04b231182..00000000000000 --- 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 93e821740a12b4..00000000000000 --- 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_stabilize.cpp b/ArduSub/control_stabilize.cpp deleted file mode 100644 index 356ce2479be1d6..00000000000000 --- 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 407b04ab5a88ec..00000000000000 --- 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 b3c7109f10fda4..170b466966cbdf 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 @@ -55,32 +42,7 @@ enum control_mode_t : uint8_t { #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_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 4b2fa212b7eeb6..5c0d08d32499cf 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 7d89c89cae1ee4..c39e84aebf5319 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 521586923fd8fc..00000000000000 --- 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 bbbd19c3464cc9..5f0f83fe9de98f 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 // ---------------------------------------------------------------------------- @@ -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); @@ -157,28 +158,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 +360,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 +425,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 +561,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) { @@ -595,6 +600,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 +613,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 +636,7 @@ void Sub::handle_jsbutton_release(uint8_t _button, bool shift) { ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_trim()); // 1-indexed } break; +#endif } } diff --git a/ArduSub/mode.cpp b/ArduSub/mode.cpp new file mode 100644 index 00000000000000..323836c2f94abb --- /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 00000000000000..584f5b26d315db --- /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 00000000000000..29b1e61891667c --- /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 00000000000000..3282054d7329ad --- /dev/null +++ b/ArduSub/mode_althold.cpp @@ -0,0 +1,114 @@ +#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 target_yaw_rate = sub.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); + 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 4631e8dcef1809..87ad912976b5cf 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); -} - -// 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; - } + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); } -// 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,20 @@ 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(); + if (sub.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { + sub.camera_mount.set_mode_to_default(); } #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 +417,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 +436,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 +445,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 +468,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 +476,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 +500,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 +526,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 +552,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 +561,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 00000000000000..28be79dbad54cd --- /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 00000000000000..25857bf5c94e64 --- /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 00000000000000..9d6e29892ea91a --- /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 1718de53b421d5..370e1838ce1829 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/control_poshold.cpp b/ArduSub/mode_poshold.cpp similarity index 50% rename from ArduSub/control_poshold.cpp rename to ArduSub/mode_poshold.cpp index 46121952ae054a..8c1b389464c1ab 100644 --- a/ArduSub/control_poshold.cpp +++ b/ArduSub/mode_poshold.cpp @@ -7,47 +7,47 @@ #if POSHOLD_ENABLED == ENABLED // poshold_init - initialise PosHold controller -bool Sub::poshold_init() +bool ModePoshold::init(bool ignore_checks) { // fail to initialise PosHold mode if no GPS lock - if (!position_ok()) { + if (!sub.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); + 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 - pos_control.init_xy_controller_stopping_point(); - pos_control.init_z_controller(); + 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(); - pos_control.relax_z_controller(0.5f); + attitude_control->set_throttle_out(0.5f ,true, g.throttle_filt); + attitude_control->relax_attitude_controllers(); + position_control->relax_z_controller(0.5f); - last_pilot_heading = ahrs.yaw_sensor; + sub.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() +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(); - pos_control.init_xy_controller_stopping_point(); - pos_control.relax_z_controller(0.5f); - last_pilot_heading = ahrs.yaw_sensor; + 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; } @@ -62,15 +62,15 @@ void Sub::poshold_run() float lateral_out = 0; float forward_out = 0; - if (position_ok()) { + if (sub.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(); + position_control->init_xy_controller_stopping_point(); } - translate_pos_control_rp(lateral_out, forward_out); - pos_control.update_xy_controller(); + sub.translate_pos_control_rp(lateral_out, forward_out); + position_control->update_xy_controller(); } else { - pos_control.init_xy_controller_stopping_point(); + position_control->init_xy_controller_stopping_point(); } motors.set_forward(forward_out + pilot_forward); motors.set_lateral(lateral_out + pilot_lateral); @@ -78,32 +78,32 @@ void Sub::poshold_run() // Update attitude // // get pilot's desired yaw rate - float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + float target_yaw_rate = sub.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); + 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); - last_pilot_heading = ahrs.yaw_sensor; - last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading + 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 < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading + 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); - last_pilot_heading = ahrs.yaw_sensor; // update heading to hold + 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, last_pilot_heading, true); + attitude_control->input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, sub.last_pilot_heading, true); } } diff --git a/ArduSub/mode_stabilize.cpp b/ArduSub/mode_stabilize.cpp new file mode 100644 index 00000000000000..cd75be241b6c2c --- /dev/null +++ b/ArduSub/mode_stabilize.cpp @@ -0,0 +1,68 @@ +#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 target_yaw_rate = sub.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); + 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 00000000000000..6bab07a067b860 --- /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 5d33b5052b40e4..9e25f51513e605 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 diff --git a/ArduSub/surface_bottom_detector.cpp b/ArduSub/surface_bottom_detector.cpp index 4a1b4cd215edd2..0338e77c99d4c3 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 7ac209a87f17b1..487abe625e063a 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/wscript b/ArduSub/wscript index de8dc48a7b984e..89da451ee21e7f 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 0143d70da6258a..9124d7acb7d3d1 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** @@ -164,6 +163,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 c72d250cc2dbc8..ea36c2c1f0d873 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; } diff --git a/Blimp/Blimp.cpp b/Blimp/Blimp.cpp index 9c9f457b137162..ff5327006738d9 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), @@ -215,12 +217,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 0c6e7a2627e37e..8923e6bd1ceb38 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; @@ -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 e51a465b3690b9..6e225580385e32 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, @@ -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 c07facca56b98c..830e0abc209431 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 515a2870b6b9c4..5ffabf5ef9f5c7 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/Blimp/GCS_Mavlink.cpp @@ -52,8 +52,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 +102,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, @@ -466,30 +466,17 @@ 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); 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) +MAV_RESULT GCS_MAVLINK_Blimp::handle_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg) { switch (packet.command) { @@ -515,7 +502,7 @@ MAV_RESULT GCS_MAVLINK_Blimp::handle_command_long_packet(const mavlink_command_l return MAV_RESULT_FAILED; default: - return GCS_MAVLINK::handle_command_long_packet(packet); + return GCS_MAVLINK::handle_command_long_packet(packet, msg); } } diff --git a/Blimp/GCS_Mavlink.h b/Blimp/GCS_Mavlink.h index 3e2d67d7d4015c..100d06f0dd46b9 100644 --- a/Blimp/GCS_Mavlink.h +++ b/Blimp/GCS_Mavlink.h @@ -26,9 +26,8 @@ class GCS_MAVLINK_Blimp : public GCS_MAVLINK 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_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg) override; MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet); @@ -74,7 +73,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 6b74b028fd2e2f..cda675619688bc 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,7 +234,6 @@ tuning_max : tune_max logger.WriteBlock(&pkt_tune, sizeof(pkt_tune)); } - // 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 @@ -247,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 @@ -262,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 @@ -278,16 +283,26 @@ const struct LogStructure Blimp::log_structure[] = { // @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 }, + { + 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 @@ -403,8 +418,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 00000000000000..8c6b5d5e7aea7c --- /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 00000000000000..c21fdd57c6f251 --- /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 487d0981146b01..6956f1eda58433 100644 --- a/Blimp/Parameters.cpp +++ b/Blimp/Parameters.cpp @@ -53,15 +53,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 +175,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 +263,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 diff --git a/Blimp/Parameters.h b/Blimp/Parameters.h index c819ad0ab46543..8ec466d62c0c6d 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 39682160ae5dc1..7709867cf25c4a 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 80e53ceda23028..ec37259794cb82 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 10cac9ea72bf7b..9c5908f65fed98 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,100 +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 - -////////////////////////////////////////////////////////////////////////////// -// Parachute release -#ifndef PARACHUTE -# define PARACHUTE HAL_PARACHUTE_ENABLED -#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 - -////////////////////////////////////////////////////////////////////////////// -// Guided mode - control vehicle's position or angles from GCS -#ifndef MODE_GUIDED_ENABLED -# define MODE_GUIDED_ENABLED ENABLED -#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 - -////////////////////////////////////////////////////////////////////////////// -// RADIO CONFIGURATION -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - - ////////////////////////////////////////////////////////////////////////////// // FLIGHT_MODE // - #ifndef FLIGHT_MODE_1 # define FLIGHT_MODE_1 Mode::Number::MANUAL #endif @@ -238,7 +93,6 @@ # define FLIGHT_MODE_6 Mode::Number::MANUAL #endif - ////////////////////////////////////////////////////////////////////////////// // Throttle Failsafe // @@ -246,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 f356e77600f551..0a73072a1aea5f 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 8bc678922f0b5b..fd524b99ab91a5 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 cbd778cd6ca69f..452f624c5b879f 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 511ac6be5bbc84..b7d80bd906100f 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 ed9e60353f0f7d..8a990fbb31b30c 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,72 +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: - // 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; - - }; - static AutoYaw auto_yaw; - // pass-through functions to reduce code churn on conversion; // these are candidates for moving into the Mode base // class. @@ -304,7 +247,6 @@ class ModeLoiter : public Mode private: Vector3f target_pos; float target_yaw; - float loop_period; }; class ModeLand : public Mode @@ -347,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 f6e334839e1d04..640e69ef528df8 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 71e2794c2bbae3..725a702480815a 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 1183b4bd7a1ad1..30a9eb7bbaffff 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 00000000000000..c1b593cd9c9331 --- /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 a333d6cbc29440..ae6011c619fb48 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 c2034774880f41..35ba08b90cce93 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 e7640c44c2aa96..861c057820a5fc 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 cec5103bb53aac..f3d8bdc08e69f1 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 a922ff5c4c1e02..e5bfa27c87be87 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 a45b13a3137502..34cb8c170490e0 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 f0be5d2aaa6090..495c843488054b 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,4 +1,6 @@ -FROM ubuntu:20.04 +ARG BASE_IMAGE="ubuntu" +ARG TAG="22.04" +FROM ${BASE_IMAGE}:${TAG} WORKDIR /ardupilot ARG DEBIAN_FRONTEND=noninteractive diff --git a/README.md b/README.md index 36994430d4fcbc..7b603fc1632425 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 c952f7d9652b55..dc0f717d173e59 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,7 +78,7 @@ bool AP_Arming_Rover::pre_arm_checks(bool report) //are arming checks disabled? if (checks_to_perform == 0) { - return true; + return mandatory_checks(report); } if (rover.g2.sailboat.sail_enabled() && !rover.g2.windvane.enabled()) { @@ -196,9 +196,6 @@ bool AP_Arming_Rover::mode_checks(bool report) if (!rover.control_mode->allows_arming()) { check_failed(report, "Mode not armable"); return false; - } else if (rover.control_mode == &rover.mode_auto && rover.mode_auto.mission.num_commands() <= 1) { - check_failed(report, "AUTO requires mission"); - return false; } return true; } diff --git a/Rover/AP_Rally.cpp b/Rover/AP_Rally.cpp index 1a836159645eff..b0daf506f10856 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 0055dcb1e5e93e..197d6c92797fff 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 92d17426514e46..dc419cfcd39095 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -398,8 +398,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); } @@ -635,7 +637,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,40 +657,27 @@ 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; + 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; + default: - return GCS_MAVLINK::handle_command_int_packet(packet); + return GCS_MAVLINK::handle_command_int_packet(packet, msg); } } -MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_long_t &packet) +MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg) { 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_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; - - 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; - case MAV_CMD_NAV_SET_YAW_SPEED: { // param1 : yaw angle to adjust direction by in centidegress @@ -723,7 +712,7 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_l packet.param4); default: - return GCS_MAVLINK::handle_command_long_packet(packet); + return GCS_MAVLINK::handle_command_long_packet(packet, msg); } } diff --git a/Rover/GCS_Mavlink.h b/Rover/GCS_Mavlink.h index 028c92f497a6b4..3e7ccacc31327e 100644 --- a/Rover/GCS_Mavlink.h +++ b/Rover/GCS_Mavlink.h @@ -16,8 +16,8 @@ class GCS_MAVLINK_Rover : public GCS_MAVLINK 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_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; + MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg) override; MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet); void send_position_target_global_int() override; diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index 792e50153461d6..24783eb2a33e7b 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -27,7 +27,7 @@ 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), @@ -171,7 +171,7 @@ 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), @@ -221,9 +221,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 @@ -515,9 +517,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 @@ -544,7 +548,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), @@ -694,6 +698,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 }; @@ -735,7 +743,7 @@ ParametersG2::ParametersG2(void) #if AP_BEACON_ENABLED beacon(), #endif - motors(rover.ServoRelayEvents, wheel_rate_control), + motors(wheel_rate_control), wheel_rate_control(wheel_encoder), attitude_control(), smart_rtl(), @@ -746,7 +754,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 3c3040ce5304bb..035d16f4d0ffb4 100644 --- a/Rover/Parameters.h +++ b/Rover/Parameters.h @@ -316,13 +316,13 @@ class ParametersG2 { AP_Beacon beacon; #endif - // Motor library - AP_MotorsUGV motors; - // wheel encoders AP_WheelEncoder wheel_encoder; AP_WheelRateControl wheel_rate_control; + // Motor library + AP_MotorsUGV motors; + // steering and throttle controller AR_AttitudeControl attitude_control; @@ -360,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; @@ -379,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; @@ -438,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 e83a9fd0d011a2..e799f38a70fd24 100644 --- a/Rover/RC_Channel.cpp +++ b/Rover/RC_Channel.cpp @@ -62,9 +62,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,10 +214,12 @@ 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: diff --git a/Rover/RC_Channel.h b/Rover/RC_Channel.h index 844b4c1b9069d4..d25d714aa261cd 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 2ef70f1c9e9868..4b6497382b48dd 100644 --- a/Rover/Rover.cpp +++ b/Rover/Rover.cpp @@ -96,7 +96,9 @@ 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), #endif @@ -116,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), @@ -206,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) { @@ -420,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 } diff --git a/Rover/Rover.h b/Rover/Rover.h index f11bae3d600f9d..fd22073c5bbbdb 100644 --- a/Rover/Rover.h +++ b/Rover/Rover.h @@ -43,6 +43,11 @@ #include #include #include +#include +#include +#if AP_EXTERNAL_CONTROL_ENABLED +#include +#endif // Configuration #include "defines.h" @@ -83,6 +88,7 @@ class Rover : public AP_Vehicle { friend class Mode; friend class ModeAcro; friend class ModeAuto; + friend class ModeCircle; friend class ModeGuided; friend class ModeHold; friend class ModeLoiter; @@ -90,7 +96,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; @@ -139,6 +147,11 @@ class Rover : public AP_Vehicle { // Arming/Disarming management class AP_Arming_Rover arming; + // dummy external control implementation +#if AP_EXTERNAL_CONTROL_ENABLED + AP_ExternalControl external_control; +#endif + #if AP_OPTICALFLOW_ENABLED AP_OpticalFlow optflow; #endif @@ -235,7 +248,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; @@ -257,6 +272,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; @@ -359,9 +375,14 @@ 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; 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); diff --git a/Rover/config.h b/Rover/config.h index 77b2981c5fbcc5..7a280e88f3497a 100644 --- a/Rover/config.h +++ b/Rover/config.h @@ -31,13 +31,6 @@ #error XXX #endif -////////////////////////////////////////////////////////////////////////////// -// RALLY POINTS -// -#ifndef AP_RALLY - #define AP_RALLY ENABLED -#endif - ////////////////////////////////////////////////////////////////////////////// // NAVL1 // @@ -67,6 +60,12 @@ # 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 + ////////////////////////////////////////////////////////////////////////////// // Developer Items diff --git a/Rover/mode.cpp b/Rover/mode.cpp index 1509b724367388..842789c1006698 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 761602b6ff501e..2ae7a90b2463d3 100644 --- a/Rover/mode.h +++ b/Rover/mode.h @@ -22,6 +22,7 @@ class Mode #if MODE_DOCK_ENABLED == ENABLED DOCK = 8, #endif + CIRCLE = 9, AUTO = 10, RTL = 11, SMART_RTL = 12, @@ -252,6 +253,12 @@ class ModeAuto : public Mode // 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; } + // 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; @@ -295,6 +302,7 @@ class ModeAuto : public 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 + Auto_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,6 +395,95 @@ 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); + + uint32_t mode_number() const override { return 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 { @@ -690,6 +789,7 @@ class ModeInitializing : public Mode bool _enter() override { return false; }; }; +#if MODE_FOLLOW_ENABLED == ENABLED class ModeFollow : public Mode { public: @@ -724,6 +824,7 @@ class ModeFollow : public Mode float _desired_speed; // desired speed in m/s }; +#endif class ModeSimple : public Mode { diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp index b7cf77163cf0b0..5be1d1eba0a7c1 100644 --- a/Rover/mode_auto.cpp +++ b/Rover/mode_auto.cpp @@ -145,6 +145,10 @@ void ModeAuto::update() case Auto_NavScriptTime: rover.mode_guided.update(); break; + + case Auto_Circle: + rover.g2.mode_circle.update(); + break; } } @@ -158,6 +162,102 @@ 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 Auto_WP: + return g2.wp_nav.wp_bearing_cd() * 0.01f; + case Auto_HeadingAndSpeed: + case Auto_Stop: + return 0.0f; + case Auto_RTL: + return rover.mode_rtl.wp_bearing(); + case Auto_Loiter: + return rover.mode_loiter.wp_bearing(); + case Auto_Guided: + case Auto_NavScriptTime: + return rover.mode_guided.wp_bearing(); + case Auto_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 Auto_WP: + return g2.wp_nav.nav_bearing_cd() * 0.01f; + case Auto_HeadingAndSpeed: + case Auto_Stop: + return 0.0f; + case Auto_RTL: + return rover.mode_rtl.nav_bearing(); + case Auto_Loiter: + return rover.mode_loiter.nav_bearing(); + case Auto_Guided: + case Auto_NavScriptTime: + return rover.mode_guided.nav_bearing(); + case Auto_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 Auto_WP: + return g2.wp_nav.crosstrack_error(); + case Auto_HeadingAndSpeed: + case Auto_Stop: + return 0.0f; + case Auto_RTL: + return rover.mode_rtl.crosstrack_error(); + case Auto_Loiter: + return rover.mode_loiter.crosstrack_error(); + case Auto_Guided: + case Auto_NavScriptTime: + return rover.mode_guided.crosstrack_error(); + case Auto_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 Auto_WP: + return g2.wp_nav.get_lat_accel(); + case Auto_HeadingAndSpeed: + case Auto_Stop: + return 0.0f; + case Auto_RTL: + return rover.mode_rtl.get_desired_lat_accel(); + case Auto_Loiter: + return rover.mode_loiter.get_desired_lat_accel(); + case Auto_Guided: + case Auto_NavScriptTime: + return rover.mode_guided.get_desired_lat_accel(); + case Auto_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 { @@ -175,6 +275,8 @@ float ModeAuto::get_distance_to_destination() const case Auto_Guided: case Auto_NavScriptTime: return rover.mode_guided.get_distance_to_destination(); + case Auto_Circle: + return rover.g2.mode_circle.get_distance_to_destination(); } // this line should never be reached @@ -201,7 +303,9 @@ bool ModeAuto::get_desired_location(Location& destination) const return rover.mode_loiter.get_desired_location(destination); case Auto_Guided: case Auto_NavScriptTime: - return rover.mode_guided.get_desired_location(destination);\ + return rover.mode_guided.get_desired_location(destination); + case Auto_Circle: + return rover.g2.mode_circle.get_desired_location(destination); } // we should never reach here but just in case @@ -242,7 +346,8 @@ bool ModeAuto::reached_destination() const case Auto_Guided: case Auto_NavScriptTime: return rover.mode_guided.reached_destination(); - break; + case Auto_Circle: + return rover.g2.mode_circle.reached_destination(); } // we should never reach here but just in case, return true to allow missions to continue @@ -266,6 +371,8 @@ bool ModeAuto::set_desired_speed(float speed) case Auto_Guided: case Auto_NavScriptTime: return rover.mode_guided.set_desired_speed(speed); + case Auto_Circle: + return rover.g2.mode_circle.set_desired_speed(speed); } return false; } @@ -428,6 +535,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; @@ -570,6 +680,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); @@ -808,6 +921,34 @@ bool ModeAuto::verify_nav_set_yaw_speed() 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 = Auto_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 /********************************************************************************/ diff --git a/Rover/mode_circle.cpp b/Rover/mode_circle.cpp new file mode 100644 index 00000000000000..42d6f63833b79b --- /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 437cb53deb8445..964a81a102a6ba 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_rtl.cpp b/Rover/mode_rtl.cpp index b9a8ee49649e17..e896c5151fc878 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/release-notes.txt b/Rover/release-notes.txt index 4a80f680be59b6..155075b3aced54 100644 --- a/Rover/release-notes.txt +++ b/Rover/release-notes.txt @@ -1,5 +1,277 @@ Rover Release Notes: ------------------------------------------------------------------ +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 diff --git a/Rover/system.cpp b/Rover/system.cpp index bd6ed6b1ccf68e..10cc4a975ba34d 100644 --- a/Rover/system.cpp +++ b/Rover/system.cpp @@ -107,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 @@ -186,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 @@ -221,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) { @@ -228,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 diff --git a/Rover/version.h b/Rover/version.h index 6ec99cb1d6b818..11b034f62f7d68 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 d383905f3a7a79..f42271208073ba 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 c4577cd2312e15..28004b66aaadb1 100644 --- a/Tools/AP_Bootloader/bl_protocol.cpp +++ b/Tools/AP_Bootloader/bl_protocol.cpp @@ -316,12 +316,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 !defined(STM32_OTG2_IS_OTG1) rccResetOTG_FS(); +#endif #if defined(rccResetOTG_HS) rccResetOTG_HS(); #endif diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index af66dca9c15b9f..8f12c807694a40 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 @@ -217,11 +217,28 @@ 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_ESP32_PERIPH 1205 AP_HW_ESP32S3_PERIPH 1206 @@ -231,6 +248,10 @@ AP_HW_CUBEBLACK_PERIPH 1401 AP_HW_PIXRACER_PERIPH 1402 AP_HW_SWBOOMBOARD_PERIPH 1403 +# IDs 5000-5099 reserved for Carbonix +# IDs 5100-5199 reserved for SYPAQ Systems +# 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/custom.cpp b/Tools/AP_Bootloader/custom.cpp new file mode 100644 index 00000000000000..ef5f7ef434d38b --- /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; @@ -464,3 +469,4 @@ void port_setbaud(uint32_t baudrate) #endif } #endif // BOOTLOADER_DEV_LIST + diff --git a/Tools/AP_Bootloader/support.h b/Tools/AP_Bootloader/support.h index 13f3085ba3be92..d0ffe1503f83fa 100644 --- a/Tools/AP_Bootloader/support.h +++ b/Tools/AP_Bootloader/support.h @@ -44,6 +44,8 @@ void led_on(unsigned led); void led_off(unsigned led); void led_toggle(unsigned led); +void custom_startup(void); + // printf to debug uart (or USB) extern "C" { void uprintf(const char *fmt, ...) FMT_PRINTF(1,2); @@ -55,7 +57,6 @@ void led_pulses(uint8_t npulses); typedef struct mcu_des_t { uint16_t mcuid; const char *desc; - char rev; } mcu_des_t; typedef struct mcu_rev_t { diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index 975167cec15f34..c4f71cbdc34dfc 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -100,6 +100,10 @@ void AP_Periph_FW::init() can_start(); +#ifdef HAL_PERIPH_ENABLE_NETWORKING + networking.init(); +#endif + #if HAL_GCS_ENABLED stm32_watchdog_pat(); gcs().init(); @@ -159,6 +163,10 @@ void AP_Periph_FW::init() battery.lib.init(); #endif +#ifdef HAL_PERIPH_ENABLE_RCIN + rcin_init(); +#endif + #if defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_RC_OUT) hal.rcout->init(); #endif @@ -185,7 +193,11 @@ void AP_Periph_FW::init() } } #endif - + +#if AP_KDECAN_ENABLED + kdecan.init(); +#endif + #ifdef HAL_PERIPH_ENABLE_AIRSPEED #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS const bool pins_enabled = ChibiOS::I2CBus::check_select_pins(0x01); @@ -219,7 +231,7 @@ void AP_Periph_FW::init() } #endif -#ifdef HAL_PERIPH_ENABLE_PRX +#if HAL_PROXIMITY_ENABLED if (proximity.get_type(0) != AP_Proximity::Type::None && g.proximity_port >= 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 @@ -413,13 +438,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 @@ -436,6 +461,10 @@ void AP_Periph_FW::update() } #endif +#ifdef HAL_PERIPH_ENABLE_RCIN + rcin_update(); +#endif + static uint32_t fiftyhz_last_update_ms; if (now - fiftyhz_last_update_ms >= 20) { // update at 50Hz @@ -457,12 +486,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.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 diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index d9fa52efb06d97..4fa3f664ccd8ed 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -13,17 +13,27 @@ #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 +#include +#if HAL_WITH_ESC_TELEM +#include +#endif +#include +#include "rc_in.h" #include #if HAL_NMEA_OUTPUT_ENABLED && !(HAL_GCS_ENABLED && defined(HAL_PERIPH_ENABLE_GPS)) @@ -35,6 +45,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 @@ -69,6 +81,9 @@ extern "C" { void can_printf(const char *fmt, ...) FMT_PRINTF(1,2); } +struct CanardInstance; +struct CanardRxTransfer; + class AP_Periph_FW { public: AP_Periph_FW(); @@ -147,6 +162,11 @@ class AP_Periph_FW { AP_Baro baro; #endif +#ifdef HAL_PERIPH_ENABLE_RPM + AP_RPM rpm_sensor; + uint32_t rpm_last_update_ms; +#endif + #ifdef HAL_PERIPH_ENABLE_BATTERY struct AP_Periph_Battery { void handle_battery_failsafe(const char* type_str, const int8_t action) { } @@ -161,7 +181,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 @@ -201,7 +221,7 @@ class AP_Periph_FW { uint32_t last_sample_ms; #endif -#ifdef HAL_PERIPH_ENABLE_PRX +#if HAL_PROXIMITY_ENABLED AP_Proximity proximity; #endif @@ -227,7 +247,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 +280,16 @@ 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 + #if AP_TEMPERATURE_SENSOR_ENABLED AP_TemperatureSensor temperature_sensor; #endif @@ -283,6 +322,10 @@ class AP_Periph_FW { AP_Logger logger; #endif +#ifdef HAL_PERIPH_ENABLE_NETWORKING + AP_Networking networking; +#endif + #if HAL_GCS_ENABLED GCS_Periph _gcs; #endif @@ -301,16 +344,52 @@ class AP_Periph_FW { 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<= 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_DroneCAN), + // @CopyFieldsFrom: CAN_PROTOCOL + GARRAY(can_protocol, 2, "CAN3_PROTOCOL", float(AP_CAN::Protocol::DroneCAN)), #endif #if HAL_CANFD_SUPPORTED @@ -158,11 +152,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 +171,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:Disabled, 1:Show free stack space, 2:Auto Reboot after 15sec, 3:Enable sending stats // @User: Advanced GSCALAR(debug, "DEBUG", 0), @@ -343,13 +335,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 @@ -483,7 +475,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GOBJECT(efi, "EFI", AP_EFI), #endif -#ifdef HAL_PERIPH_ENABLE_PRX +#if HAL_PROXIMITY_ENABLED // @Param: PRX_BAUDRATE // @DisplayName: Proximity Sensor serial baudrate // @Description: Proximity Sensor serial baudrate. @@ -515,7 +507,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { // @Group: PRX // @Path: ../libraries/AP_Proximity/AP_Proximity.cpp GOBJECT(proximity, "PRX", AP_Proximity), -#endif +#endif // HAL_PROXIMITY_ENABLED #if HAL_NMEA_OUTPUT_ENABLED // @Group: NMEA_ @@ -523,6 +515,72 @@ 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: ../libraries/AP_Networking/AP_Networking.cpp + GOBJECT(networking, "NET_", AP_Networking), +#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 + +#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 + AP_VAREND }; diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index 63322b10d467c6..03b9ba62825dfb 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -30,7 +30,7 @@ class Parameters { k_param_hardpoint_id, k_param_hardpoint_rate, k_param_baro_enable, - k_param_esc_number, + k_param_esc_number0, k_param_battery, k_param_debug, k_param_serial_number, @@ -70,6 +70,17 @@ 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, + k_param_rpm_sensor, + k_param_g_rcin, + k_param_sitl, + k_param_ahrs, }; AP_Int16 format_version; @@ -77,7 +88,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 +114,7 @@ class Parameters { AP_Int16 rangefinder_max_rate; #endif -#ifdef HAL_PERIPH_ENABLE_PRX +#if HAL_PROXIMITY_ENABLED AP_Int32 proximity_baud; AP_Int8 proximity_port; AP_Int16 proximity_max_rate; @@ -120,8 +131,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 @@ -169,6 +193,7 @@ class Parameters { #else static constexpr uint8_t can_fdmode = 0; #endif + AP_Int8 node_stats; Parameters() {} }; diff --git a/Tools/AP_Periph/ReleaseNotes.txt b/Tools/AP_Periph/ReleaseNotes.txt index 76666579839791..5165da3b526307 100644 --- a/Tools/AP_Periph/ReleaseNotes.txt +++ b/Tools/AP_Periph/ReleaseNotes.txt @@ -1,3 +1,19 @@ +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 --------------------------- diff --git a/Tools/AP_Periph/adsb.cpp b/Tools/AP_Periph/adsb.cpp index 0e44461eb7ff36..fe80b39081ec89 100644 --- a/Tools/AP_Periph/adsb.cpp +++ b/Tools/AP_Periph/adsb.cpp @@ -23,6 +23,7 @@ #ifdef HAL_PERIPH_ENABLE_ADSB #include +#include extern const AP_HAL::HAL &hal; @@ -52,6 +53,15 @@ void AP_Periph_FW::adsb_init(void) } } +static mavlink_message_t chan_buffer; +mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) { + return &chan_buffer; +} + +static mavlink_status_t chan_status; +mavlink_status_t* mavlink_get_channel_status(uint8_t chan) { + return &chan_status; +} /* update ADSB subsystem @@ -84,4 +94,62 @@ void AP_Periph_FW::adsb_update(void) } } +/* + 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/can.cpp b/Tools/AP_Periph/can.cpp index 1c340a9e12b28f..d35568f4f4c848 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -59,6 +59,8 @@ extern AP_Periph_FW periph; #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 @@ -122,7 +124,7 @@ static struct dronecan_protocol_t { 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 @@ -180,7 +182,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 +208,13 @@ static void readUniqueID(uint8_t* out_uid) /* handle a GET_NODE_INFO request */ -static void handle_get_node_info(CanardInstance* ins, +static void 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; @@ -237,7 +241,7 @@ static void handle_get_node_info(CanardInstance* ins, uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer, !periph.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, @@ -261,7 +265,7 @@ static void handle_get_node_info(CanardInstance* ins, /* handle parameter GetSet request */ -static void handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer) +static void handle_param_getset(CanardInstance* canard_instance, CanardRxTransfer* transfer) { // param fetch all can take a long time, so pat watchdog stm32_watchdog_pat(); @@ -346,7 +350,7 @@ 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()); - canardRequestOrRespond(ins, + canardRequestOrRespond(canard_instance, transfer->source_node_id, UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE, UAVCAN_PROTOCOL_PARAM_GETSET_ID, @@ -368,7 +372,7 @@ static void handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer) /* handle parameter executeopcode request */ -static void handle_param_executeopcode(CanardInstance* ins, CanardRxTransfer* transfer) +static void handle_param_executeopcode(CanardInstance* canard_instance, CanardRxTransfer* transfer) { uavcan_protocol_param_ExecuteOpcodeRequest req; if (uavcan_protocol_param_ExecuteOpcodeRequest_decode(transfer, &req)) { @@ -406,7 +410,7 @@ static void handle_param_executeopcode(CanardInstance* ins, CanardRxTransfer* tr uint8_t buffer[UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_MAX_SIZE] {}; uint16_t total_size = uavcan_protocol_param_ExecuteOpcodeResponse_encode(&pkt, buffer, !periph.canfdout()); - canardRequestOrRespond(ins, + canardRequestOrRespond(canard_instance, transfer->source_node_id, UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE, UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID, @@ -424,15 +428,10 @@ static void handle_param_executeopcode(CanardInstance* ins, CanardRxTransfer* tr ); } -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) +static void handle_begin_firmware_update(CanardInstance* canard_instance, CanardRxTransfer* transfer) { #if HAL_RAM_RESERVE_START >= 256 // setup information on firmware request at start of ram @@ -450,14 +449,14 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* comms->server_node_id = transfer->source_node_id; } memcpy(comms->path, req.image_file_remote_path.path.data, req.image_file_remote_path.path.len); - comms->my_node_id = canardGetLocalNodeID(ins); + 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, + canardRequestOrRespond(canard_instance, transfer->source_node_id, UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE, UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID, @@ -484,16 +483,16 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* // the node_id periph.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) +static void 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) @@ -527,7 +526,7 @@ 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 @@ -553,7 +552,7 @@ static uint32_t buzzer_len_ms; /* handle BeepCommand */ -static void handle_beep_command(CanardInstance* ins, CanardRxTransfer* transfer) +static void handle_beep_command(CanardInstance* canard_instance, CanardRxTransfer* transfer) { uavcan_equipment_indication_BeepCommand req; if (uavcan_equipment_indication_BeepCommand_decode(transfer, &req)) { @@ -565,7 +564,7 @@ static void handle_beep_command(CanardInstance* ins, CanardRxTransfer* transfer) hal.rcout->init(); hal.util->toneAlarm_init(AP_Notify::Notify_Buzz_Builtin); } - buzzer_start_ms = AP_HAL::native_millis(); + 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); @@ -581,7 +580,7 @@ static void handle_beep_command(CanardInstance* ins, CanardRxTransfer* transfer) static void can_buzzer_update(void) { if (buzzer_start_ms != 0) { - uint32_t now = AP_HAL::native_millis(); + 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; @@ -596,7 +595,7 @@ static uint8_t safety_state; /* handle SafetyState */ -static void handle_safety_state(CanardInstance* ins, CanardRxTransfer* transfer) +static void handle_safety_state(CanardInstance* canard_instance, CanardRxTransfer* transfer) { ardupilot_indication_SafetyState req; if (ardupilot_indication_SafetyState_decode(transfer, &req)) { @@ -612,7 +611,7 @@ static void handle_safety_state(CanardInstance* ins, CanardRxTransfer* transfer) /* handle ArmingStatus */ -static void handle_arming_status(CanardInstance* ins, CanardRxTransfer* transfer) +static void handle_arming_status(CanardInstance* canard_instance, CanardRxTransfer* transfer) { uavcan_equipment_safety_ArmingStatus req; if (uavcan_equipment_safety_ArmingStatus_decode(transfer, &req)) { @@ -625,7 +624,7 @@ static void handle_arming_status(CanardInstance* ins, CanardRxTransfer* transfer /* handle gnss::RTCMStream */ -static void handle_RTCMStream(CanardInstance* ins, CanardRxTransfer* transfer) +static void handle_RTCMStream(CanardInstance* canard_instance, CanardRxTransfer* transfer) { uavcan_equipment_gnss_RTCMStream req; if (uavcan_equipment_gnss_RTCMStream_decode(transfer, &req)) { @@ -638,7 +637,7 @@ static void handle_RTCMStream(CanardInstance* ins, CanardRxTransfer* transfer) handle gnss::MovingBaselineData */ #if GPS_MOVING_BASELINE -static void handle_MovingBaselineData(CanardInstance* ins, CanardRxTransfer* transfer) +static void handle_MovingBaselineData(CanardInstance* canard_instance, CanardRxTransfer* transfer) { ardupilot_gnss_MovingBaselineData msg; if (ardupilot_gnss_MovingBaselineData_decode(transfer, &msg)) { @@ -737,7 +736,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) +static void handle_lightscommand(CanardInstance* canard_instance, CanardRxTransfer* transfer) { uavcan_equipment_indication_LightsCommand req; if (uavcan_equipment_indication_LightsCommand_decode(transfer, &req)) { @@ -767,7 +766,7 @@ 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) +static void handle_esc_rawcommand(CanardInstance* canard_instance, CanardRxTransfer* transfer) { uavcan_equipment_esc_RawCommand cmd; if (uavcan_equipment_esc_RawCommand_decode(transfer, &cmd)) { @@ -780,7 +779,7 @@ static void handle_esc_rawcommand(CanardInstance* ins, CanardRxTransfer* transfe periph.last_esc_raw_command_ms = AP_HAL::millis(); } -static void handle_act_command(CanardInstance* ins, CanardRxTransfer* transfer) +static void handle_act_command(CanardInstance* canard_instance, CanardRxTransfer* transfer) { uavcan_equipment_actuator_ArrayCommand cmd; if (uavcan_equipment_actuator_ArrayCommand_decode(transfer, &cmd)) { @@ -802,7 +801,7 @@ static void handle_act_command(CanardInstance* ins, CanardRxTransfer* transfer) #endif // HAL_PERIPH_ENABLE_RC_OUT #if defined(HAL_PERIPH_ENABLE_NOTIFY) -static void handle_notify_state(CanardInstance* ins, CanardRxTransfer* transfer) +static void handle_notify_state(CanardInstance* canard_instance, CanardRxTransfer* transfer) { ardupilot_indication_NotifyState msg; if (ardupilot_indication_NotifyState_decode(transfer, &msg)) { @@ -830,7 +829,7 @@ static void can_safety_LED_update(void) palWriteLine(HAL_GPIO_PIN_SAFE_LED, SAFE_LED_ON); break; case ARDUPILOT_INDICATION_SAFETYSTATE_STATUS_SAFETY_ON: { - uint32_t now = AP_HAL::native_millis(); + uint32_t now = AP_HAL::millis(); if (now - last_update_ms > 100) { last_update_ms = now; static uint8_t led_counter; @@ -859,7 +858,7 @@ static void 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; @@ -880,18 +879,18 @@ static void can_safety_button_update(void) uint8_t buffer[ARDUPILOT_INDICATION_BUTTON_MAX_SIZE] {}; uint16_t total_size = ardupilot_indication_Button_encode(&pkt, buffer, !periph.canfdout()); - canard_broadcast(ARDUPILOT_INDICATION_BUTTON_SIGNATURE, - ARDUPILOT_INDICATION_BUTTON_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); + periph.canard_broadcast(ARDUPILOT_INDICATION_BUTTON_SIGNATURE, + 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, +static void onTransferReceived(CanardInstance* canard_instance, CanardRxTransfer* transfer) { #ifdef HAL_GPIO_PIN_LED_CAN1 @@ -907,21 +906,21 @@ static void onTransferReceived(CanardInstance* ins, * 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: @@ -936,60 +935,66 @@ 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: + periph.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 } @@ -1003,7 +1008,7 @@ 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, +static bool shouldAcceptTransfer(const CanardInstance* canard_instance, uint64_t* out_data_type_signature, uint16_t data_type_id, CanardTransferType transfer_type, @@ -1011,7 +1016,7 @@ static bool shouldAcceptTransfer(const CanardInstance* ins, { (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. @@ -1069,7 +1074,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; @@ -1134,24 +1146,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, @@ -1171,6 +1181,14 @@ 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) @@ -1185,23 +1203,23 @@ 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 deadline = AP_HAL::micros64() + 1000000; // 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_DroneCAN) { + if (periph.can_protocol_cached[_ins.index] != AP_CAN::Protocol::DroneCAN) { continue; } #endif - if (ins.iface->send(txmsg, deadline, 0) <= 0) { + if (_ins.iface->send(txmsg, deadline, 0) <= 0) { sent = false; } } @@ -1214,6 +1232,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; @@ -1221,6 +1242,51 @@ static void processTx(void) } } +#if HAL_ENABLE_SENDING_STATS +static void 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 + static void processRx(void) { AP_HAL::CANFrame rxmsg; @@ -1229,7 +1295,7 @@ static void processRx(void) continue; } #if HAL_NUM_CAN_IFACES >= 2 - if (periph.can_protocol_cached[ins.index] != AP_CANManager::Driver_Type_DroneCAN) { + if (periph.can_protocol_cached[ins.index] != AP_CAN::Protocol::DroneCAN) { continue; } #endif @@ -1257,21 +1323,26 @@ 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 } } @@ -1286,18 +1357,58 @@ static uint16_t pool_peak_percent() static void 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, !periph.canfdout()); - canard_broadcast(UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, - UAVCAN_PROTOCOL_NODESTATUS_ID, - CANARD_TRANSFER_PRIORITY_LOW, - buffer, - len); + periph.canard_broadcast(UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, + UAVCAN_PROTOCOL_NODESTATUS_ID, + CANARD_TRANSFER_PRIORITY_LOW, + buffer, + len); + } +#if HAL_ENABLE_SENDING_STATS + if (periph.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, !periph.canfdout()); + periph.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, !periph.canfdout()); + periph.canard_broadcast(DRONECAN_PROTOCOL_CANSTATS_SIGNATURE, + DRONECAN_PROTOCOL_CANSTATS_ID, + CANARD_TRANSFER_PRIORITY_LOWEST, + buffer, + len); + } + } +#endif } @@ -1382,12 +1493,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 @@ -1407,7 +1518,7 @@ static bool can_do_dna() 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; @@ -1472,7 +1583,7 @@ 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; @@ -1485,12 +1596,12 @@ void AP_Periph_FW::can_start() #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; idisable_interrupts_save(); uint16_t value = pwm_hardpoint.highest_pwm; @@ -1600,7 +1711,7 @@ void AP_Periph_FW::hwesc_telem_update() const HWESC_Telem::HWESC &t = hwesc_telem.get_telem(); uavcan_equipment_esc_Status pkt {}; - pkt.esc_index = g.esc_number; + 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)); @@ -1671,12 +1782,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, !periph.canfdout()); + canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, + UAVCAN_EQUIPMENT_ESC_STATUS_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); + } + } + +} +#endif // HAL_PERIPH_ENABLE_ESC_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; @@ -1704,7 +1849,7 @@ 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 @@ -1713,6 +1858,9 @@ void AP_Periph_FW::can_update() { can_mag_update(); can_gps_update(); +#if AP_UART_MONITOR_ENABLED + send_serial_monitor_data(); +#endif can_battery_update(); can_baro_update(); can_airspeed_update(); @@ -1733,6 +1881,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 @@ -1779,7 +1930,7 @@ void AP_Periph_FW::can_mag_update(void) #if AP_PERIPH_PROBE_CONTINUOUS if (compass.get_count() == 0) { static uint32_t last_probe_ms; - uint32_t now = AP_HAL::native_millis(); + uint32_t now = AP_HAL::millis(); if (now - last_probe_ms >= 1000) { last_probe_ms = now; compass.init(); @@ -1820,7 +1971,7 @@ void AP_Periph_FW::can_mag_update(void) void AP_Periph_FW::can_battery_update(void) { #ifdef HAL_PERIPH_ENABLE_BATTERY - const uint32_t now_ms = AP_HAL::native_millis(); + const uint32_t now_ms = AP_HAL::millis(); if (now_ms - battery.last_can_send_ms < 100) { return; } @@ -2114,9 +2265,11 @@ void AP_Periph_FW::send_moving_baseline_msg() } else #endif { + // we use MEDIUM priority on this data as we need to get all + // the data through for RTK moving baseline yaw to work canard_broadcast(ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE, ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID, - CANARD_TRANSFER_PRIORITY_LOW, + CANARD_TRANSFER_PRIORITY_MEDIUM, &buffer[0], total_size); } @@ -2219,7 +2372,7 @@ void AP_Periph_FW::can_airspeed_update(void) } #if AP_PERIPH_PROBE_CONTINUOUS if (!airspeed.healthy()) { - uint32_t now = AP_HAL::native_millis(); + uint32_t now = AP_HAL::millis(); static uint32_t last_probe_ms; if (now - last_probe_ms >= 1000) { last_probe_ms = now; @@ -2227,7 +2380,7 @@ void AP_Periph_FW::can_airspeed_update(void) } } #endif - uint32_t now = AP_HAL::native_millis(); + uint32_t now = AP_HAL::millis(); if (now - last_airspeed_update_ms < 50) { // max 20Hz data return; @@ -2247,8 +2400,6 @@ void AP_Periph_FW::can_airspeed_update(void) } uavcan_equipment_air_data_RawAirData pkt {}; - pkt.differential_pressure = press; - pkt.static_air_temperature = temp; // unfilled elements are NaN pkt.static_pressure = nanf(""); @@ -2256,6 +2407,21 @@ void AP_Periph_FW::can_airspeed_update(void) 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= 1000) { last_probe_ms = now; @@ -2287,10 +2453,10 @@ void AP_Periph_FW::can_rangefinder_update(void) } } #endif - uint32_t now = AP_HAL::native_millis(); + uint32_t now = AP_HAL::millis(); static uint32_t last_update_ms; if (g.rangefinder_max_rate > 0 && - now - last_update_ms < 1000/g.rangefinder_max_rate) { + now - last_update_ms < uint32_t(1000/g.rangefinder_max_rate)) { // limit to max rate return; } @@ -2355,12 +2521,12 @@ void AP_Periph_FW::can_rangefinder_update(void) void AP_Periph_FW::can_proximity_update() { -#ifdef HAL_PERIPH_ENABLE_PRX +#if HAL_PROXIMITY_ENABLED if (proximity.get_type(0) == AP_Proximity::Type::None) { return; } - uint32_t now = AP_HAL::native_millis(); + 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) { @@ -2419,66 +2585,6 @@ void AP_Periph_FW::can_proximity_update() #endif } -#ifdef HAL_PERIPH_ENABLE_ADSB -/* - 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_LOW, - &buffer[0], - total_size); -} -#endif // HAL_PERIPH_ENABLE_ADSB - #ifdef HAL_PERIPH_ENABLE_EFI /* @@ -2665,7 +2771,6 @@ void AP_Periph_FW::can_efi_update(void) } #endif // HAL_PERIPH_ENABLE_EFI - // printf to CAN LogMessage for debugging void can_printf(const char *fmt, ...) { @@ -2693,11 +2798,11 @@ void can_printf(const char *fmt, ...) uint8_t buffer_packet[UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_MAX_SIZE] {}; const uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer_packet, !periph.canfdout()); - canard_broadcast(UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE, - UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID, - CANARD_TRANSFER_PRIORITY_LOW, - buffer_packet, - len); + periph.canard_broadcast(UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE, + UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID, + CANARD_TRANSFER_PRIORITY_LOW, + buffer_packet, + len); } #else @@ -2711,11 +2816,11 @@ void can_printf(const char *fmt, ...) uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer, !periph.canfdout()); - canard_broadcast(UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE, - UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID, - CANARD_TRANSFER_PRIORITY_LOW, - buffer, - len); + periph.canard_broadcast(UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE, + UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID, + CANARD_TRANSFER_PRIORITY_LOW, + buffer, + len); #endif } diff --git a/Tools/AP_Periph/esc_apd_telem.cpp b/Tools/AP_Periph/esc_apd_telem.cpp new file mode 100644 index 00000000000000..fccc2663312bbb --- /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 = 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 00000000000000..42f2f416ad6112 --- /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/hwing_esc.cpp b/Tools/AP_Periph/hwing_esc.cpp index a79862f3cf6d0e..13d30eb8d68dcb 100644 --- a/Tools/AP_Periph/hwing_esc.cpp +++ b/Tools/AP_Periph/hwing_esc.cpp @@ -39,7 +39,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; diff --git a/Tools/AP_Periph/rc_in.cpp b/Tools/AP_Periph/rc_in.cpp new file mode 100644 index 00000000000000..f5f577adc6e448 --- /dev/null +++ b/Tools/AP_Periph/rc_in.cpp @@ -0,0 +1,180 @@ +/* + 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: RC_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: RC_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: RC1_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: RC1_PORT_OPTIONS + // @DisplayName: RC input port serial options + // @Description: This is the serial port number where SERIALx_PROTOCOL will be set to RC input. + // @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; i. + */ +/* + 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 +#if HAL_PROXIMITY_ENABLED + 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; + 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/wscript b/Tools/AP_Periph/wscript index d6a733b5ffafd0..30f09bb36dfcd6 100644 --- a/Tools/AP_Periph/wscript +++ b/Tools/AP_Periph/wscript @@ -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,7 +70,16 @@ 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', ] bld.ap_stlib( name= 'AP_Periph_libs', diff --git a/Tools/CPUInfo/CPUInfo.cpp b/Tools/CPUInfo/CPUInfo.cpp index 9802c9f93a418c..a20db4924d95d6 100644 --- a/Tools/CPUInfo/CPUInfo.cpp +++ b/Tools/CPUInfo/CPUInfo.cpp @@ -26,6 +26,7 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); // On H750 we want to measure external flash to ram performance #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 100755 index 00000000000000..b33cc18a18bd2b --- /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 00000000000000..54d9c2e446169d --- /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 cce4dddf532196..54dfab7eb55bac 100644 --- a/Tools/GIT_Test/GIT_Success.txt +++ b/Tools/GIT_Test/GIT_Success.txt @@ -173,4 +173,7 @@ 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 \ No newline at end of file diff --git a/Tools/IO_Firmware/iofirmware_dshot_highpolh.bin b/Tools/IO_Firmware/iofirmware_dshot_highpolh.bin new file mode 100755 index 00000000000000..441aa59e79f6c7 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 00000000000000..0bce6c10f1ecd1 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 d148daded7da40..82065f85eb99b0 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 b3036b63edd7aa..9897dce49e79c2 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 100644 index 00000000000000..2c1bd7e9d57eb8 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 100644 index 00000000000000..6276596b429b01 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 100644 index 00000000000000..8a21b6e17ebc70 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 100644 index 00000000000000..1723b089d741cc 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 3f308f141ffa12..12aa45dc089501 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 defcab7926f50f..63e9f7040c6e49 100755 Binary files a/Tools/IO_Firmware/iofirmware_lowpolh.bin and b/Tools/IO_Firmware/iofirmware_lowpolh.bin differ diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp index 8b9d2bd4315111..cbdca233afaf68 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 b258f496e728a0..4e30a6571d35ce 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 d36c3da3fe9e19..9a84053ab81ada 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/ardupilotwaf/ap_library.py b/Tools/ardupilotwaf/ap_library.py index 9c7b42f7749890..b49010ae06001c 100644 --- a/Tools/ardupilotwaf/ap_library.py +++ b/Tools/ardupilotwaf/ap_library.py @@ -270,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") @@ -321,3 +325,4 @@ def configure(cfg): 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 4ef709a99051ce..75c9975b826bd7 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): @@ -302,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): @@ -353,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 @@ -367,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, @@ -378,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 = [] @@ -564,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 d8dae5af9f46da..36fbce5d709ba4 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -47,29 +47,24 @@ 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: @@ -248,16 +243,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 += [ @@ -265,6 +264,7 @@ def configure_env(self, cfg, env): ] env.CXXFLAGS += [ '-fcheck-new', + '-fsingle-precision-constant', ] if cfg.env.DEBUG: @@ -275,6 +275,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', @@ -442,6 +446,7 @@ def configure_env(self, cfg, env): DRONECAN_CXX_WRAPPERS = 1, USE_USER_HELPERS = 1, CANARD_ENABLE_DEADLINE = 1, + CANARD_ALLOCATE_SEM=1 ) @@ -546,7 +551,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() @@ -628,7 +637,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) @@ -638,14 +647,19 @@ 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) + if self.with_can: cfg.define('HAL_NUM_CAN_IFACES', 2) env.DEFINES.update(CANARD_MULTI_IFACE=1, CANARD_IFACE_ALL = 0x3, - CANARD_ENABLE_CANFD = 1) + CANARD_ENABLE_CANFD = 1, + CANARD_ENABLE_ASSERTS = 1) env.CXXFLAGS += [ - '-Werror=float-equal' + '-Werror=float-equal', + '-Werror=missing-declarations', ] if cfg.options.ubsan or cfg.options.ubsan_abort: @@ -692,10 +706,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): @@ -762,6 +775,34 @@ def configure_env(self, cfg, env): '-m32', ] + # whitelist of compilers which we should build with -Werror + gcc_whitelist = frozenset([ + ('11','3','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__ @@ -769,14 +810,24 @@ 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, + 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, @@ -787,18 +838,25 @@ def configure_env(self, cfg, env): 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_NAVEKF2_AVAILABLE = 0, + HAL_NAVEKF3_AVAILABLE = 0, + HAL_PWM_COUNT = 32, + HAL_WITH_ESC_TELEM = 1, ) @@ -827,7 +885,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 += [ @@ -852,6 +910,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') @@ -881,6 +940,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 @@ -910,7 +972,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', @@ -967,6 +1028,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', @@ -988,7 +1050,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: @@ -1001,6 +1063,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' ] @@ -1012,7 +1079,7 @@ def configure_env(self, cfg, env): if cfg.env.SAVE_TEMPS: env.CXXFLAGS += [ '-S', '-save-temps=obj' ] - if cfg.options.disable_watchdog or cfg.env.DEBUG: + if cfg.options.disable_watchdog: cfg.msg("Disabling Watchdog", "yes") env.CFLAGS += [ '-DDISABLE_WATCHDOG' ] env.CXXFLAGS += [ '-DDISABLE_WATCHDOG' ] @@ -1056,13 +1123,14 @@ def configure_env(self, cfg, env): ] # 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'), + ]) if cfg.env.HAL_CANFD_SUPPORTED: env.DEFINES.update(CANARD_ENABLE_CANFD=1) diff --git a/Tools/ardupilotwaf/build_summary.py b/Tools/ardupilotwaf/build_summary.py index d7002543965ce7..6a0abacc827f7c 100644 --- a/Tools/ardupilotwaf/build_summary.py +++ b/Tools/ardupilotwaf/build_summary.py @@ -49,6 +49,7 @@ 'size_bss': 'BSS (B)', 'size_total': 'Total Flash Used (B)', 'size_free_flash': 'Free Flash (B)', + 'ext_flash_used': 'External Flash Used (B)', } def text(label, text=''): @@ -170,14 +171,19 @@ def _build_summary(bld): def _parse_size_output(s, s_all, totals=False): # Get the size of .crash_log to remove it from .bss reporting + # also get external flash size if applicable crash_log_size = None + ext_flash_used = 0 if s_all is not None: lines = s_all.splitlines()[1:] for line in lines: if ".crash_log" in line: row = line.strip().split() crash_log_size = int(row[1]) - break + if ".extflash" in line: + row = line.strip().split() + if int(row[1]) > 0: + ext_flash_used = int(row[1]) import re pattern = re.compile("^.*TOTALS.*$") @@ -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 2c7d35bbc43205..83c41cb81dba7f 100644 --- a/Tools/ardupilotwaf/chibios.py +++ b/Tools/ardupilotwaf/chibios.py @@ -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): @@ -455,7 +457,8 @@ def setup_canmgr_build(cfg): 'DRONECAN_CXX_WRAPPERS=1', 'USE_USER_HELPERS=1', 'CANARD_ENABLE_DEADLINE=1', - 'CANARD_MULTI_IFACE=1' + 'CANARD_MULTI_IFACE=1', + 'CANARD_ALLOCATE_SEM=1' ] if cfg.env.HAL_CANFD_SUPPORTED: @@ -492,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: @@ -647,13 +652,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') @@ -665,7 +679,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')] @@ -673,7 +687,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') @@ -703,6 +717,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 9ec9691c150ae1..0616de6128ed6f 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 d4101be9ec9850..f6e067da3ea237 100644 --- a/Tools/ardupilotwaf/dronecangen.py +++ b/Tools/ardupilotwaf/dronecangen.py @@ -66,9 +66,6 @@ def configure(cfg): """ setup environment for uavcan header generator """ - cfg.load('python') - cfg.check_python_version(minver=(2,7,0)) - env = cfg.env 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' diff --git a/Tools/ardupilotwaf/embed.py b/Tools/ardupilotwaf/embed.py index d833b40c977459..ccc9673985ea10 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 8631abff3c2d3a..20a9528860de18 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 0dec86207594d4..5af076ac3f4b48 100644 --- a/Tools/ardupilotwaf/mavgen.py +++ b/Tools/ardupilotwaf/mavgen.py @@ -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/autotest/ArduPlane_Tests/GUIDEDToAUTO/mission.txt b/Tools/autotest/ArduPlane_Tests/GUIDEDToAUTO/mission.txt new file mode 100644 index 00000000000000..12bb5ed5f65e6b --- /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/LordEAHRS/ap1.txt b/Tools/autotest/ArduPlane_Tests/MicroStrainEAHRS/ap1.txt similarity index 100% rename from Tools/autotest/ArduPlane_Tests/LordEAHRS/ap1.txt rename to Tools/autotest/ArduPlane_Tests/MicroStrainEAHRS/ap1.txt diff --git a/Tools/autotest/antennatracker.py b/Tools/autotest/antennatracker.py index ba452956aafd92..cea2a37fa09ba5 100644 --- a/Tools/autotest/antennatracker.py +++ b/Tools/autotest/antennatracker.py @@ -122,27 +122,21 @@ def SERVOTEST(self): # 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.run_cmd( + 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 - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, - channel, - value, - 0, - 0, - 0, - 0, - 0, - timeout=1) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_SET_SERVO, + p1=channel, + p2=value, + timeout=1, + ) self.wait_servo_channel_value(channel, value) def SCAN(self): diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 3aafe1a4f3a77a..cff00c76d66756 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 @@ -124,19 +123,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 +141,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 +156,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() @@ -709,6 +688,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''' @@ -1120,7 +1140,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) @@ -1139,11 +1159,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() @@ -1168,6 +1191,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, @@ -2088,7 +2126,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) @@ -2428,7 +2466,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 @@ -2582,7 +2620,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() @@ -2650,16 +2704,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") @@ -2667,23 +2717,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): @@ -3629,14 +3690,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) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_PARACHUTE, + p1=2, # release + ) self.wait_statustext('BANG', timeout=60) self.disarm_vehicle(force=True) self.reboot_sitl() @@ -3654,15 +3711,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 - ) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_PARACHUTE, + p1=mavutil.mavlink.PARACHUTE_DISABLE, + ) ok = False try: self.wait_statustext('BANG', timeout=2) @@ -3670,15 +3722,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 - ) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_PARACHUTE, + p1=mavutil.mavlink.PARACHUTE_ENABLE, + ) ok = False try: self.wait_statustext('BANG', timeout=2) @@ -3693,15 +3740,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 - ) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_PARACHUTE, + p1=mavutil.mavlink.PARACHUTE_RELEASE, + ) ok = False try: self.wait_statustext('BANG', timeout=2) @@ -3711,24 +3753,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 - ) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_PARACHUTE, + p1=mavutil.mavlink.PARACHUTE_ENABLE, + ) + self.run_cmd( + 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() @@ -3775,15 +3807,16 @@ def MotorTest(self, timeout=60): # 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) + self.run_cmd( + 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(1, pwm_in, timeout=10) self.wait_servo_channel_value(4, pwm_in, timeout=10) @@ -3796,15 +3829,16 @@ def MotorTest(self, timeout=60): # 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.run_cmd( + 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(1, expected_pwm, timeout=10) self.wait_servo_channel_value(4, expected_pwm, timeout=10) self.wait_statustext("finished motor test") @@ -3852,7 +3886,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)) @@ -4569,7 +4603,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 @@ -4647,6 +4681,7 @@ def PayLoadPlaceMission(self): except Exception as e: self.print_exception_caught(e) + self.disarm_vehicle(force=True) ex = e self.context_pop() @@ -4814,15 +4849,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") @@ -4871,14 +4901,12 @@ def do_yaw_rate(self, yaw_rate): '''yaw aircraft in guided/rate mode''' self.run_cmd( mavutil.mavlink.MAV_CMD_CONDITION_YAW, - 60, # target angle - 0, # degrees/second - 1, # -1 is counter-clockwise, 1 clockwise - 1, # 1 for relative, 0 for absolute - 0, # p5 - 0, # p6 - 0, # p7 - quiet=True) + 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''' @@ -4905,11 +4933,12 @@ 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( + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE, + p1=mount_mode, + p2=0, # stabilize roll (unsupported) + p3=0, # stabilize pitch (unsupported) + ) def Mount(self): '''Test Camera/Antenna Mount''' @@ -4975,14 +5004,16 @@ def Mount(self): 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.run_cmd( + mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW, + p1=-20, # 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(-20, 1, mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING) # point gimbal at specified location @@ -5126,27 +5157,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() @@ -5156,15 +5195,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() @@ -5214,15 +5250,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), @@ -5236,6 +5267,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), @@ -5294,15 +5330,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) @@ -5336,15 +5369,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) @@ -6753,8 +6783,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: @@ -6793,14 +6823,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, @@ -6860,8 +6890,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: @@ -6871,9 +6901,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 @@ -6889,14 +6921,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") @@ -6924,7 +6950,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) @@ -6939,27 +6965,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( @@ -7673,13 +7683,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") @@ -7737,29 +7741,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 @@ -7769,6 +7763,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() @@ -7917,6 +7951,7 @@ def RangeFinderDrivers(self): ("benewake_tf03", 27), ("gyus42v2", 31), ("teraranger_serial", 35), + ("nooploop_tofsense", 37), ] while len(drivers): do_drivers = drivers[0:3] @@ -8321,7 +8356,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)) @@ -8723,13 +8760,10 @@ def MAV_CMD_CONDITION_YAW_absolute(self): target = initial_heading self.run_cmd( 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) @@ -8747,13 +8781,10 @@ def rate_watcher(mav, m): part_way_target = initial_heading + 10 self.run_cmd( 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) @@ -8763,13 +8794,10 @@ def rate_watcher(mav, m): part_way_target = initial_heading + 30 self.run_cmd( 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) @@ -8824,10 +8852,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() @@ -9127,9 +9155,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, @@ -9167,8 +9192,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) @@ -9481,111 +9507,105 @@ 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(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''' @@ -9618,6 +9638,7 @@ def tests1b(self): self.BrakeMode, self.RecordThenPlayMission, self.ThrottleFailsafe, + self.ThrottleFailsafePassthrough, self.GCSFailsafe, self.CustomController, ]) @@ -9632,12 +9653,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, ]) @@ -9763,10 +9786,41 @@ 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') @@ -9799,6 +9853,92 @@ def GuidedYawRate(self): 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 tests2b(self): # this block currently around 9.5mins here '''return list of all tests''' ret = ([ @@ -9819,6 +9959,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, @@ -9854,6 +9995,11 @@ def tests2b(self): # this block currently around 9.5mins here self.IMUConsistency, self.AHRSTrimLand, self.GuidedYawRate, + self.NoArmWithoutMissionItems, + self.RPLidarA1, + self.RPLidarA2, + self.SafetySwitch, + self.BrakeZ, ]) return ret diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 6f426449ab8491..05a415b841cec8 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -20,8 +20,12 @@ from common import NotAchievedException from common import PreconditionFailedException from common import WaitModeTimeout +from common import OldpymavlinkException +from common import Test + from pymavlink.rotmat import Vector3 from pysim import vehicleinfo +from pysim import util import operator @@ -318,78 +322,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 +568,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.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.wait_altitude(new_alt-10, new_alt, timeout=30, relative=True) + self.progress("flying home") self.fly_home_land_and_disarm() def DeepStall(self): @@ -625,18 +771,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) @@ -725,13 +871,9 @@ def DO_CHANGE_SPEED_mavlink(self): 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") @@ -743,13 +885,11 @@ def DO_CHANGE_SPEED_mavlink(self): 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) + p1=1, # groundspeed + p2=new_target_groundspeed, + p3=-1, # throttle / no change + p4=0, # absolute values + ) 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) @@ -760,25 +900,21 @@ def DO_CHANGE_SPEED_mavlink(self): # clear target groundspeed self.run_cmd( 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) + p1=0, # airspeed + p2=new_target_airspeed, + p3=-1, # throttle / no change + p4=0, # absolute values + ) self.wait_airspeed(new_target_airspeed-0.5, new_target_airspeed+0.5) self.progress("Adding some wind, hoping groundspeed increases/decreases") self.set_parameters({ @@ -1169,29 +1305,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 +1431,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 @@ -1844,6 +2004,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 +2048,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) @@ -2825,9 +2984,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 +3074,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 +3082,9 @@ 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 MicroStrainEAHRS(self): + '''Test MicroStrain EAHRS support''' + self.fly_external_AHRS("MicroStrain", 2, "ap1.txt") def get_accelvec(self, m): return Vector3(m.xacc, m.yacc, m.zacc) * 0.001 * 9.81 @@ -2986,13 +3147,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 +3397,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 +3410,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]: @@ -3592,6 +3750,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)) @@ -3676,13 +3835,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 +3914,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) @@ -4342,25 +4509,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 +4553,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 +4597,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 +4617,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 +4637,43 @@ 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 tests(self): '''return list of all tests''' ret = super(AutoTestPlane, self).tests() @@ -4537,7 +4725,7 @@ def tests(self): self.TerrainMission, self.TerrainLoiter, self.VectorNavEAHRS, - self.LordEAHRS, + self.MicroStrainEAHRS, self.Deadreckoning, self.DeadreckoningNoAirSpeed, self.EKFlaneswitch, @@ -4556,6 +4744,7 @@ def tests(self): self.DCMFallback, self.MAVFTP, self.AUTOTUNE, + self.AutotuneFiltering, self.MegaSquirt, self.MSP_DJI, self.SpeedToFly, @@ -4565,12 +4754,17 @@ 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.MAV_CMD_GUIDED_CHANGE_ALTITUDE, ]) return ret diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 58a5f6805c91a0..a6af0aba9c06c6 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -375,15 +375,11 @@ 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% # 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=255, # battery mask + p2=10, # percentage + ) self.run_cmd_reboot() tstart = time.time() while True: @@ -392,14 +388,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) diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index a19bdf335300d5..207bf3e7137357 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 @@ -380,6 +381,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 +401,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"], } @@ -485,6 +488,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: @@ -499,24 +507,31 @@ def run_step(step): 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, @@ -802,7 +817,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(): @@ -1115,6 +1130,7 @@ def format_epilog(self, formatter): 'build.Blimp', 'defaults.Blimp', + 'test.Blimp', 'build.SITLPeriphGPS', 'test.CAN', diff --git a/Tools/autotest/bisect-helper.py b/Tools/autotest/bisect-helper.py index e0da93182ef33d..e0aafe56587335 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, @@ -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 00000000000000..e180ea04882d33 --- /dev/null +++ b/Tools/autotest/blimp.py @@ -0,0 +1,131 @@ +''' +Fly Blimp in SITL + +AP_FLAKE8_CLEAN +''' + +from __future__ import print_function +import os +import shutil + +from pymavlink import mavutil + +from common import AutoTest + +# get location of scripts +testdir = os.path.dirname(os.path.realpath(__file__)) +SITL_START_LOCATION = mavutil.location(-35.362938, 149.165085, 584, 270) + +# Flight mode switch positions are set-up in arducopter.param to be +# switch 1 = Circle +# switch 2 = Land +# switch 3 = RTL +# switch 4 = Auto +# switch 5 = Loiter +# switch 6 = Stabilize + + +class AutoTestBlimp(AutoTest): + @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 Speed(self): + '''test we can move''' + self.change_mode('MANUAL') + self.wait_ready_to_arm() + self.arm_vehicle() + + # make sure we don't drift: + start = self.mav.location() + self.set_rc(2, 2000) + self.wait_distance_to_location(start, 2, 10, timeout=40) + self.disarm_vehicle() + + def tests(self): + '''return list of all tests''' + # ret = super(AutoTestBlimp, self).tests() + ret = [] + ret.extend([ + self.Speed, + ]) + return ret + + def disabled_tests(self): + return { + } diff --git a/Tools/autotest/check_autotest_speedup.py b/Tools/autotest/check_autotest_speedup.py index a601dcc5e99201..81bf13edfe064b 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, diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 6ce9a81cc187e0..09f5f0b84262de 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -37,6 +37,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 +168,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 +202,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 @@ -219,7 +228,8 @@ def close(self): def write(self, data): self.file.write(data) - self.stdout.write(data) + if not self.suppress_stdout: + self.stdout.write(data) def flush(self): self.file.flush() @@ -1650,13 +1660,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', @@ -1865,26 +1889,13 @@ 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) + self.run_cmd( + mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, + p1=1, # reboot autopilot + ) def run_cmd_run_prearms(self): - self.run_cmd(mavutil.mavlink.MAV_CMD_RUN_PREARM_CHECKS, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0) + self.run_cmd(mavutil.mavlink.MAV_CMD_RUN_PREARM_CHECKS) def run_cmd_enable_high_latency(self, new_state): p1 = 0 @@ -1893,13 +1904,7 @@ def run_cmd_enable_high_latency(self, new_state): self.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): @@ -1939,13 +1944,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() @@ -2035,12 +2037,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: @@ -2137,34 +2140,19 @@ def get_sim_parameter_documentation_get_whitelist(self): "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", @@ -2221,7 +2209,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", @@ -2255,7 +2242,6 @@ def get_sim_parameter_documentation_get_whitelist(self): "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", @@ -2276,30 +2262,14 @@ def get_sim_parameter_documentation_get_whitelist(self): "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", @@ -2424,7 +2394,6 @@ def get_sim_parameter_documentation_get_whitelist(self): "SIM_INS_THR_MIN", "SIM_LED_LAYOUT", "SIM_LOOP_DELAY", - "SIM_MAG1_DEVID", "SIM_MAG1_SCALING", "SIM_MAG2_DEVID", "SIM_MAG2_DIA_X", @@ -2484,7 +2453,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", @@ -2785,6 +2753,8 @@ def all_log_format_ids(self): continue if "#if AC_PRECLAND_ENABLED" in line: continue + if "#if OFFBOARD_GUIDED == ENABLED" in line: + continue if "#end" in line: continue if "LOG_COMMON_STRUCTURES" in line: @@ -2961,7 +2931,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: @@ -3366,17 +3336,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: @@ -3449,6 +3421,14 @@ 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): + overridden_message_rates = self.context_get().overridden_message_rates + + if id not in overridden_message_rates: + overridden_message_rates[id] = self.get_message_rate(id) + + self.set_message_rate_hz(id, rate_hz) + def HIGH_LATENCY2_links(self): self.start_subtest("SerialProtocol_MAVLinkHL links") @@ -3463,7 +3443,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, @@ -3873,14 +3853,9 @@ def run_auxfunc(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): self.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): @@ -3950,13 +3925,27 @@ def message_has_field_values(self, m, fieldvalues, verbose=True, epsilon=None): else: equal = got == value + 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 equal: + # 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): @@ -3964,7 +3953,16 @@ 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): + 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): # noqa + if poll: + self.poll_message(message) m = self.assert_receive_message(message, verbose=verbose, very_verbose=very_verbose) self.assert_message_field_values(m, fieldvalues, verbose=verbose, epsilon=epsilon) return m @@ -4215,6 +4213,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''' @@ -4488,16 +4498,17 @@ def load_rally(self, filename): 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): @@ -4524,40 +4535,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 ) @@ -4862,7 +4887,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: @@ -4981,17 +5006,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""" @@ -4999,36 +5030,13 @@ 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_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 { @@ -5044,43 +5052,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, )) @@ -5092,15 +5086,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) @@ -5126,30 +5117,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) @@ -5639,7 +5623,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 @@ -5648,13 +5632,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() @@ -5699,18 +5689,29 @@ def sysid_thismav(self): 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, + ): + + 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() @@ -5737,13 +5738,13 @@ def run_cmd_int(self, 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, @@ -5787,13 +5788,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, @@ -5854,13 +5855,7 @@ def set_current_waypoint_using_mav_cmd_do_set_mission_current( 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, timeout=1, target_sysid=target_sysid, target_compid=target_compid) @@ -5993,13 +5988,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): @@ -6080,15 +6070,10 @@ def run_cmd_do_set_mode(self, custom_mode = self.get_mode_from_mode_mapping(mode) self.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): @@ -6180,13 +6165,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))) @@ -6207,15 +6189,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.""" @@ -6229,15 +6208,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) @@ -6771,7 +6746,7 @@ 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 @@ -7184,6 +7159,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) @@ -7207,6 +7186,10 @@ 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() @@ -7309,6 +7292,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, @@ -7453,6 +7449,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: @@ -7468,6 +7465,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) @@ -7606,6 +7608,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) @@ -7627,6 +7642,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: @@ -7650,6 +7674,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): @@ -7737,14 +7765,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" % @@ -7761,7 +7789,31 @@ 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 @@ -7774,7 +7826,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 @@ -7821,12 +7873,6 @@ 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) ardupilot_alive = False @@ -7836,29 +7882,17 @@ 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: @@ -7878,7 +7912,7 @@ 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 not passed: # implicit reboot after a failed test: + 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 @@ -7906,7 +7940,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") @@ -7954,7 +7988,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): self.start_mavproxy_count += 1 if self.mavproxy is not None: return self.mavproxy @@ -7966,11 +8000,17 @@ 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() + mavproxy = util.start_MAVProxy_SITL( self.vehicleinfo_key(), + master='tcp:127.0.0.1:%u' % self.adjust_ardupilot_port(5762), 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) @@ -7989,7 +8029,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, @@ -7997,7 +8039,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, @@ -8016,15 +8058,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 + + # 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_suplementary_programs(self): + def get_supplementary_programs(self): return self.sup_prog def stop_sup_program(self, instance=None): @@ -8057,23 +8108,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): @@ -8293,14 +8339,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) @@ -8384,11 +8424,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(): @@ -8402,15 +8442,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)) @@ -8440,43 +8477,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: @@ -8490,33 +8519,18 @@ 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 - ) + self.run_cmd( + mavutil.mavlink.MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS, + p1=2, # param1 (compass0) + ) + self.run_cmd( + mavutil.mavlink.MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS, + p1=5, # param1 (compass1) + ) + self.run_cmd( + mavutil.mavlink.MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS, + p1=6, # param1 (compass2) + ) self.progress("zeroed mag parameters") params = [ [("SIM_MAG1_OFS1_X", "COMPASS_OFS_X", 0), @@ -8600,17 +8614,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): @@ -8718,29 +8730,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: @@ -8836,17 +8838,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): @@ -8868,17 +8865,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") @@ -9148,16 +9143,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") @@ -9444,6 +9441,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): @@ -9579,6 +9586,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()) @@ -9605,16 +9614,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) @@ -9627,16 +9631,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) @@ -9726,15 +9725,12 @@ def set_message_rate_hz(self, id, rate_hz, mav=None): 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) + self.run_cmd( + mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, + p1=id, + p2=set_interval, + mav=mav, + ) def send_get_message_interval(self, victim_message, mav=None): if mav is None: @@ -9754,6 +9750,25 @@ def send_get_message_interval(self, victim_message, mav=None): 0, 0) + def get_message_interval(self, victim_message, mav=None): + '''returns message interval in microseconds''' + self.send_get_message_interval(victim_message, mav=mav) + m = self.assert_receive_message('MESSAGE_INTERVAL', timeout=1, mav=mav) + if m.message_id != victim_message: + raise NotAchievedException("Unexpected ID in MESSAGE_INTERVAL") + return m.interval_us + + def set_message_interval(self, victim_message, interval_us, mav=None): + '''sets message interval in microseconds''' + if type(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, @@ -9910,18 +9925,14 @@ def send_poll_message(self, message_id, target_sysid=None, target_compid=None, q mav = self.mav if type(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: @@ -9935,7 +9946,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: @@ -9987,10 +9998,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 @@ -9999,16 +10011,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() @@ -10511,6 +10518,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 @@ -10873,6 +10952,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 @@ -11240,27 +11322,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): @@ -11344,13 +11418,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() @@ -11390,9 +11458,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) @@ -11552,6 +11621,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() @@ -11663,14 +11743,8 @@ 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.assert_prearm_failure("Motors Emergency Stopped", other_prearm_failures_fatal=False) @@ -12001,10 +12075,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 @@ -12021,14 +12096,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) @@ -12095,10 +12166,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() @@ -12249,10 +12321,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() @@ -12523,10 +12596,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 @@ -12595,24 +12669,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=255, # battery mask + p2=96, # percentage + ) m = self.assert_receive_message('BATTERY_STATUS', timeout=1) want_battery_remaining_pct = m.battery_remaining @@ -12717,10 +12788,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 = { @@ -12759,10 +12831,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) @@ -12831,10 +12904,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() @@ -12858,10 +12932,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") @@ -12882,6 +12957,27 @@ 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() + def AHRS_ORIENTATION(self): '''test AHRS_ORIENTATION parameter works''' self.context_push() @@ -12910,23 +13006,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, 'detected'), + (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: @@ -12992,6 +13091,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" % @@ -13010,9 +13110,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: @@ -13021,9 +13123,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") @@ -13176,7 +13287,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])) @@ -13319,25 +13434,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/default_params/blimp.parm b/Tools/autotest/default_params/blimp.parm index 192455b115c7b9..a790eec33454f5 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 d12d6725f63c4a..5b5e58adda58a6 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 00000000000000..38269b2d2aa68e --- /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 08a836dd01f97f..994bf370801ecf 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-winch.parm b/Tools/autotest/default_params/copter-winch.parm new file mode 100644 index 00000000000000..4f2279c00d9927 --- /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/periph.parm b/Tools/autotest/default_params/periph.parm new file mode 100644 index 00000000000000..0e39124d3732ad --- /dev/null +++ b/Tools/autotest/default_params/periph.parm @@ -0,0 +1,19 @@ +# 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 4 + +# 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 diff --git a/Tools/autotest/default_params/plane-jsbsim.parm b/Tools/autotest/default_params/plane-jsbsim.parm index e53e15acd64f26..a77ec1084ef7d1 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 00000000000000..0fe872319e3659 --- /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 00000000000000..2e29723e09d2eb --- /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 00000000000000..3971389f992640 --- /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-periph.parm b/Tools/autotest/default_params/quadplane-periph.parm new file mode 100644 index 00000000000000..dc7a19687b6122 --- /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/helicopter.py b/Tools/autotest/helicopter.py index 4a3ee9b64a42e4..b910b762441ed4 100644 --- a/Tools/autotest/helicopter.py +++ b/Tools/autotest/helicopter.py @@ -152,7 +152,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 +167,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") @@ -199,6 +205,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 +303,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 +401,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", @@ -793,6 +818,7 @@ def tests(self): self.SplineWaypoint, self.AutoRotation, self.ManAutoRotation, + self.governortest, self.FlyEachFrame, self.AirspeedDrivers, self.TurbineStart, diff --git a/Tools/autotest/jsb_sim/runsim.py b/Tools/autotest/jsb_sim/runsim.py deleted file mode 100755 index 416a4e97697d06..00000000000000 --- 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 ce3cf76fed012e..3717b3613f9899 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 00000000000000..d6d1cb78778ba4 --- /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 24ff2106653a02..50c026a21651f3 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 f7d4bd9e715690..d66d557cf3b131 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): diff --git a/Tools/autotest/param_metadata/param.py b/Tools/autotest/param_metadata/param.py index 8120e0ad771088..f7db128834a3bb 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,7 @@ 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', } required_param_fields = [ diff --git a/Tools/autotest/param_metadata/param_parse.py b/Tools/autotest/param_metadata/param_parse.py index fce23c9c10a276..7f5f9c21202dee 100755 --- a/Tools/autotest/param_metadata/param_parse.py +++ b/Tools/autotest/param_metadata/param_parse.py @@ -111,7 +111,7 @@ def lua_applets(): # 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 +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) @@ -352,11 +352,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)) @@ -434,6 +448,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"): @@ -441,6 +458,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 @@ -470,6 +491,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(":") diff --git a/Tools/autotest/pysim/aircraft.py b/Tools/autotest/pysim/aircraft.py deleted file mode 100644 index f3981648733131..00000000000000 --- 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 c76f68f3fc949d..00000000000000 --- 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 2776488c8526aa..cd028f756effa3 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' @@ -403,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, @@ -413,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, @@ -421,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") @@ -479,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: @@ -505,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: @@ -521,8 +543,21 @@ def start_SITL(binary, if not enable_fgview_output: cmd.append("--disable-fgview") + 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)]) + 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 @@ -560,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: @@ -568,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. @@ -603,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) @@ -616,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: @@ -641,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: @@ -707,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 . @@ -802,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: diff --git a/Tools/autotest/pysim/vehicleinfo.py b/Tools/autotest/pysim/vehicleinfo.py index fbcb2b54df0d60..9709dcd917e0d2 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 9168130492cc17..afedf93ce76103 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -388,17 +388,20 @@ def fly_qautotune(self): break self.wait_disarmed() - 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): @@ -894,6 +897,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) @@ -1227,6 +1243,7 @@ def tests(self): self.ICEngine, self.ICEngineMission, self.MidAirDisarmDisallowed, + self.GUIDEDToAUTO, self.BootInAUTO, self.Ship, self.MAV_CMD_NAV_LOITER_TO_ALT, diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 64e83424f12705..e9fd5376daa3c1 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -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 @@ -621,11 +590,61 @@ def ServoRelayEvents(self): raise NotAchievedException( "Pin mask unchanged after relay cmd") self.progress("Pin mask changed after relay command") + self.do_set_relay(0, 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, + }) + self.do_set_relay(0, 1) + self.assert_received_message_field_values('RELAY_STATUS', { + "present": 3, + "on": 1, + }) + self.do_set_relay(1, 1) + self.assert_received_message_field_values('RELAY_STATUS', { + "present": 3, + "on": 3, + }) + self.do_set_relay(0, 0) + self.do_set_relay(1, 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, + }) + self.do_set_relay(5, 1) + self.assert_received_message_field_values('RELAY_STATUS', { + "present": 35, + "on": 32, + }) + self.do_set_relay(0, 1) + self.assert_received_message_field_values('RELAY_STATUS', { + "present": 35, + "on": 33, + }) + self.do_set_relay(5, 0) + self.assert_received_message_field_values('RELAY_STATUS', { + "present": 35, + "on": 1, + }) + + 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 +656,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) @@ -1194,7 +1213,7 @@ def DO_SET_MODE(self): self.do_set_mode_via_command_long("HOLD") self.do_set_mode_via_command_long("MANUAL") - def InitialMode(self): + def RoverInitialMode(self): '''test INITIAL_MODE parameter works''' # from mavproxy_rc.py self.wait_ready_to_arm() @@ -4772,13 +4791,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") @@ -5220,15 +5238,19 @@ 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"] + test_scripts = ["scripting_test.lua", "math.lua", "strings.lua", "mavlink_test.lua"] + success_text = ["Internal tests passed", "Math tests passed", "String tests passed", "Received heartbeat from"] + named_value_float_types = ["test"] messages = [] + named_value_float = [] def my_message_hook(mav, message): - if message.get_type() != 'STATUSTEXT': - return - messages.append(message) + if message.get_type() == 'STATUSTEXT': + messages.append(message) + # also sniff for named value float messages + if message.get_type() == 'NAMED_VALUE_FLOAT': + named_value_float.append(message) self.install_message_hook_context(my_message_hook) self.set_parameters({ @@ -5236,6 +5258,8 @@ def my_message_hook(mav, message): "SCR_HEAP_SIZE": 1024000, "SCR_VM_I_COUNT": 1000000, }) + self.install_test_modules_context() + self.install_mavlink_module_context() for script in test_scripts: self.install_test_script_context(script) self.reboot_sitl() @@ -5253,9 +5277,21 @@ def my_message_hook(mav, message): 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") + raise NotAchievedException("Failed to receive STATUS_TEXT") + else: + self.progress("Success STATUS_TEXT") + + for type in named_value_float_types: + script_success = False + for m in named_value_float: + if type == m.name: + script_success = True + success = script_success and success + if not success: + raise NotAchievedException("Failed to receive NAMED_VALUE_FLOAT") + else: + self.progress("Success NAMED_VALUE_FLOAT") def test_scripting_hello_world(self): self.start_subtest("Scripting hello world") @@ -5910,18 +5946,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_FAILED, + ) def FlashStorage(self): '''Test flash storage (for parameters etc)''' @@ -6180,7 +6212,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) @@ -6281,6 +6314,84 @@ 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 tests(self): '''return list of all tests''' ret = super(AutoTestRover, self).tests() @@ -6294,7 +6405,6 @@ def tests(self): self.DriveRTL, self.SmartRTL, self.DriveSquare, - self.DriveMaxRCIN, self.DriveMission, # self.DriveBrake, # disabled due to frequent failures self.GetBanner, @@ -6315,6 +6425,7 @@ def tests(self): self.SET_ATTITUDE_TARGET, self.SET_POSITION_TARGET_LOCAL_NED, self.MAV_CMD_DO_SET_MISSION_CURRENT, + self.MAV_CMD_DO_CHANGE_SPEED, self.Button, self.Rally, self.Offboard, @@ -6348,12 +6459,16 @@ 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, ]) return ret diff --git a/Tools/autotest/run_in_terminal_window.sh b/Tools/autotest/run_in_terminal_window.sh index 86bd86a2d16200..7a83f1c565dacd 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 00000000000000..99a0bc519868f5 --- /dev/null +++ b/Tools/autotest/run_mission.py @@ -0,0 +1,107 @@ +#!/usr/bin/python3 + +''' +Run a mission in SITL + +AP_FLAKE8_CLEAN +''' + +import common +import os +import sys +import argparse + +from pysim import util + + +class RunMission(common.AutoTest): + 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/sim_vehicle.py b/Tools/autotest/sim_vehicle.py index 6a788cb5bfb76f..61f8a08453354a 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. @@ -415,7 +415,8 @@ def do_build(opts, frame_options): 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"]) @@ -646,13 +647,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, @@ -662,13 +660,24 @@ 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') cmd = ["nice"] cmd_name = "sitl_periph_gps" @@ -691,6 +700,9 @@ def start_CAN_GPS(opts): 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) @@ -771,7 +783,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,)) @@ -860,7 +872,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 @@ -1031,6 +1043,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, @@ -1048,11 +1064,6 @@ 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", @@ -1134,10 +1145,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 (use Tools/scripts/CAN/can_sitl_nodev.sh first)") group_sim.add_option("-A", "--sitl-instance-args", type='string', default=None, @@ -1475,8 +1486,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(",")] @@ -1540,13 +1551,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,)) diff --git a/Tools/autotest/test_build_options.py b/Tools/autotest/test_build_options.py index bb665a20135f7f..839269415554c9 100755 --- a/Tools/autotest/test_build_options.py +++ b/Tools/autotest/test_build_options.py @@ -74,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([])) @@ -294,12 +301,13 @@ 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: - print(f.write(f"{count}/{len(options)} {feature.define}\n")) + f.write(f"{count}/{len(options)} {feature.define}\n") # if feature.define < "WINCH_ENABLED": # count += 1 # continue @@ -316,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/web-firmware/Tools/FilterTool/FileSaver.js b/Tools/autotest/web-firmware/Tools/FilterTool/FileSaver.js deleted file mode 100644 index 5d204aee15a8d7..00000000000000 --- 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 76ded1b765fe86..00000000000000 --- 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 b68bc3b67371b3..00000000000000 --- 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