diff --git a/.editorconfig b/.editorconfig index 2ceadeedf2bd00..2c73f926bdb5ab 100644 --- a/.editorconfig +++ b/.editorconfig @@ -13,9 +13,8 @@ indent_style = space indent_size = 4 end_of_line = lf charset = utf-8 -# 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 +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 [*.mk] indent_style = tab diff --git a/.github/workflows/cache_cleanup.yml b/.github/workflows/cache_cleanup.yml index dfc73eebb223c2..cb3ffb5495c6ac 100644 --- a/.github/workflows/cache_cleanup.yml +++ b/.github/workflows/cache_cleanup.yml @@ -7,7 +7,7 @@ on: jobs: cleanup: - runs-on: ubuntu-22.04 + runs-on: ubuntu-latest permissions: # `actions:write` permission is required to delete caches # See also: https://docs.github.com/en/rest/actions/cache?apiVersion=2022-11-28#delete-a-github-actions-cache-for-a-repository-using-a-cache-id diff --git a/.github/workflows/cygwin_build.yml b/.github/workflows/cygwin_build.yml index 4b174721603596..4157e219af2d6c 100644 --- a/.github/workflows/cygwin_build.yml +++ b/.github/workflows/cygwin_build.yml @@ -1,140 +1,6 @@ name: Cygwin Build -on: - push: - paths-ignore: - # remove other vehicles - - 'AntennaTracker/**' - - 'Blimp/**' - # remove non SITL HAL - - 'libraries/AP_HAL_ChibiOS/**' - - 'libraries/AP_HAL_ESP32/**' - # remove non SITL directories - - 'Tools/AP_Bootloader/**' - - 'Tools/AP_Periph/**' - - 'Tools/CHDK-Script/**' - - 'Tools/CPUInfo/**' - - 'Tools/CodeStyle/**' - - 'Tools/FilterTestTool/**' - - 'Tools/Frame_params/**' - - 'Tools/GIT_Test/**' - - 'Tools/Hello/**' - - 'Tools/IO_Firmware/**' - - 'Tools/Linux_HAL_Essentials/**' - - 'Tools/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: - +on: [push, pull_request, 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 adad8f8e0c3e5a..f24d8190d5608f 100644 --- a/.github/workflows/esp32_build.yml +++ b/.github/workflows/esp32_build.yml @@ -1,151 +1,13 @@ name: ESP32 Build -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: - +on: [push, pull_request, workflow_dispatch] concurrency: group: ci-${{github.workflow}}-${{ github.ref }} cancel-in-progress: true jobs: build: - runs-on: ubuntu-22.04 + runs-on: ubuntu-20.04 strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: diff --git a/.github/workflows/macos_build.yml b/.github/workflows/macos_build.yml index 67e736006e902e..e9e54b8d9b8190 100644 --- a/.github/workflows/macos_build.yml +++ b/.github/workflows/macos_build.yml @@ -1,140 +1,6 @@ name: Macos Build -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: - +on: [push, pull_request, 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 fabff75687c479..3f3ba047dfcf4c 100644 --- a/.github/workflows/test_ccache.yml +++ b/.github/workflows/test_ccache.yml @@ -1,132 +1,17 @@ name: test ccache -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: - +on: [push, pull_request, workflow_dispatch] +# paths: +# - "*" +# - "!README.md" <-- don't rebuild on doc change concurrency: group: ci-${{github.workflow}}-${{ github.ref }} cancel-in-progress: true jobs: build: - runs-on: ubuntu-22.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.1.0 + runs-on: ubuntu-20.04 + container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: @@ -144,5 +29,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 Durandal,Pixhawk6X --min-cache-pct=70 + Tools/scripts/build_tests/test_ccache.py --boards CubeOrange,Durandal --min-cache-pct=75 diff --git a/.github/workflows/test_chibios.yml b/.github/workflows/test_chibios.yml index 97a405eec79f04..bf0a4f78dc9526 100644 --- a/.github/workflows/test_chibios.yml +++ b/.github/workflows/test_chibios.yml @@ -1,140 +1,18 @@ name: test chibios -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: +on: [push, pull_request, workflow_dispatch] +# paths: +# - "*" +# - "!README.md" <-- don't rebuild on doc change concurrency: group: ci-${{github.workflow}}-${{ github.ref }} cancel-in-progress: true jobs: build: - runs-on: ubuntu-22.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.1.0 + runs-on: ubuntu-20.04 + container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: @@ -152,19 +30,35 @@ jobs: revo-mini, MatekF405-Wing, CubeOrange-ODID, - CubeRedPrimary-bootloader, configure-all, build-options-defaults-test, signing ] toolchain: [ - chibios, + chibios, # GCC-6 #chibios-clang, ] - gcc: [10] + gcc: [6, 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 508e2cb99cf39f..8feaabfe2da503 100644 --- a/.github/workflows/test_coverage.yml +++ b/.github/workflows/test_coverage.yml @@ -13,9 +13,9 @@ concurrency: jobs: build: - runs-on: ubuntu-22.04 + runs-on: ubuntu-20.04 container: - image: ardupilot/ardupilot-dev-${{ matrix.type }}:v0.1.0 + image: ardupilot/ardupilot-dev-${{ matrix.type }}:latest options: --privileged strategy: fail-fast: false # don't cancel if a job from the matrix fails @@ -56,6 +56,14 @@ jobs: - name: setup ccache run: | . .github/workflows/ccache.env + - name: Configure CAN + if: ${{ matrix.config == 'sitltest-can'}} + run: | + sudo apt update + sudo apt -y linux-modules-extra-$(uname -r) + sudo modprobe vcan + sudo ip link add dev vcan0 type vcan + sudo ip link set up vcan0 - name: test ${{matrix.config}} ${{ matrix.toolchain }} env: CI_BUILD_TARGET: ${{matrix.config}} @@ -91,7 +99,7 @@ jobs: finish: if: always() needs: build - runs-on: ubuntu-22.04 + runs-on: ubuntu-latest steps: - name: Coveralls Finished uses: coverallsapp/github-action@master diff --git a/.github/workflows/test_dds.yml b/.github/workflows/test_dds.yml deleted file mode 100644 index b4487d47315970..00000000000000 --- a/.github/workflows/test_dds.yml +++ /dev/null @@ -1,188 +0,0 @@ -name: test dds - -on: - push: - paths-ignore: - # remove not tests vehicles - - 'AntennaTracker/**' - - 'ArduSub/**' - - 'Blimp/**' - - 'Rover/**' - # remove esp32 HAL - - 'libraries/AP_HAL_ESP32/**' - # remove non SITL directories - - 'Tools/AP_Bootloader/**' - - 'Tools/bootloaders/**' - - 'Tools/CHDK-Script/**' - - 'Tools/CodeStyle/**' - - 'Tools/completion/**' - - 'Tools/CPUInfo/**' - - 'Tools/debug/**' - - 'Tools/environment_install/**' - - 'Tools/FilterTestTool/**' - - 'Tools/Frame_params/**' - - 'Tools/geotag/**' - - 'Tools/GIT_Test/**' - - 'Tools/gittools/**' - - 'Tools/Hello/**' - - 'Tools/IO_Firmware/**' - - 'Tools/LogAnalyzer/**' - - 'Tools/mavproxy_modules/**' - - 'Tools/Pozyx/**' - - 'Tools/PrintVersion.py' - - 'Tools/Replay/**' - - 'Tools/simulink/**' - - 'Tools/UDP_Proxy/**' - - 'Tools/vagrant/**' - - 'Tools/Vicon/**' - # Discard python file from Tools/scripts as not used - - 'Tools/scripts/**.py' - - 'Tools/scripts/build_sizes/**' - - 'Tools/scripts/build_tests/**' - - 'Tools/scripts/CAN/**' - - 'Tools/scripts/signing/**' - # Remove autotests stuff - - 'Tools/autotest/**' - # Remove markdown files as irrelevant - - '**.md' - # Remove dotfile at root directory - - './.dir-locals.el' - - './.dockerignore' - - './.editorconfig' - - './.flake8' - - './.gitattributes' - - './.github' - - './.gitignore' - - './.pre-commit-config.yaml' - - './.pydevproject' - - './.valgrind-suppressions' - - './.valgrindrc' - - 'Dockerfile' - - 'Vagrantfile' - - 'Makefile' - # Remove some directories check - - '.vscode/**' - - '.github/ISSUE_TEMPLATE/**' - # Remove change on other workflows - - '.github/workflows/test_environment.yml' - - pull_request: - paths-ignore: - # remove not tests vehicles - - 'AntennaTracker/**' - - 'ArduSub/**' - - 'Rover/**' - - 'Blimp/**' - # remove esp32 HAL - - 'libraries/AP_HAL_ESP32/**' - # remove non SITL directories - - 'Tools/AP_Bootloader/**' - - 'Tools/CHDK-Script/**' - - 'Tools/CPUInfo/**' - - 'Tools/CodeStyle/**' - - 'Tools/FilterTestTool/**' - - 'Tools/Frame_params/**' - - 'Tools/GIT_Test/**' - - 'Tools/Hello/**' - - 'Tools/IO_Firmware/**' - - 'Tools/LogAnalyzer/**' - - 'Tools/Pozyx/**' - - 'Tools/PrintVersion.py' - - 'Tools/Replay/**' - - 'Tools/UDP_Proxy/**' - - 'Tools/Vicon/**' - - 'Tools/bootloaders/**' - - 'Tools/completion/**' - - 'Tools/debug/**' - - 'Tools/environment_install/**' - - 'Tools/geotag/**' - - 'Tools/gittools/**' - - 'Tools/mavproxy_modules/**' - - 'Tools/simulink/**' - - 'Tools/vagrant/**' - # Discard python file from Tools/scripts as not used - - 'Tools/scripts/**.py' - - 'Tools/scripts/build_sizes/**' - - 'Tools/scripts/build_tests/**' - - 'Tools/scripts/CAN/**' - - 'Tools/scripts/signing/**' - # Remove autotests stuff - - 'Tools/autotest/**' - # Remove markdown files as irrelevant - - '**.md' - # Remove dotfile at root directory - - './.dir-locals.el' - - './.dockerignore' - - './.editorconfig' - - './.flake8' - - './.gitattributes' - - './.github' - - './.gitignore' - - './.pre-commit-config.yaml' - - './.pydevproject' - - './.valgrind-suppressions' - - './.valgrindrc' - - 'Dockerfile' - - 'Vagrantfile' - - 'Makefile' - # Remove some directories check - - '.vscode/**' - - '.github/ISSUE_TEMPLATE/**' - # Remove change on other workflows - - '.github/workflows/test_environment.yml' - - workflow_dispatch: - -concurrency: - group: ci-${{github.workflow}}-${{ github.ref }} - cancel-in-progress: true - -jobs: - build: - runs-on: ubuntu-22.04 - container: - image: ardupilot/ardupilot-dev-ros:latest - options: --user 1001 - strategy: - fail-fast: false # don't cancel if a job from the matrix fails - matrix: - config: [ - sitl, - stm32h7 - ] - steps: - # git checkout the PR - - uses: actions/checkout@v3 - with: - submodules: 'recursive' - # Put ccache into github cache for faster build - - name: Prepare ccache timestamp - id: ccache_cache_timestamp - run: | - NOW=$(date -u +"%F-%T") - echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - - name: ccache cache files - uses: actions/cache@v3 - with: - path: ~/.ccache - key: ${{github.workflow}}-ccache-${{ matrix.config }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} - restore-keys: ${{github.workflow}}-ccache-${{ matrix.config }}- # restore ccache from either previous build on this branch or on master - - name: setup ccache - run: | - . .github/workflows/ccache.env - - name: test ${{matrix.config}} - env: - CI_BUILD_TARGET: dds-${{matrix.config}} - shell: 'script -q -e -c "bash {0}"' - run: | - git config --global --add safe.directory ${GITHUB_WORKSPACE} - PATH="/github/home/.local/bin:$PATH" - Tools/scripts/build_ci.sh - - - name: Archive buildlog artifacts - uses: actions/upload-artifact@v3 - if: failure() - with: - name: fail-${{matrix.config}} - path: /tmp/buildlogs - retention-days: 14 diff --git a/.github/workflows/test_environment.yml b/.github/workflows/test_environment.yml index c7ee632a2d9e0b..3d029782a6fb7a 100644 --- a/.github/workflows/test_environment.yml +++ b/.github/workflows/test_environment.yml @@ -3,15 +3,6 @@ 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: @@ -20,7 +11,7 @@ concurrency: jobs: build: - runs-on: ubuntu-22.04 + runs-on: ubuntu-20.04 container: image: ${{matrix.os}}:${{matrix.name}} options: --privileged @@ -29,13 +20,11 @@ jobs: matrix: include: - os: ubuntu - name: focal - - os: ubuntu - name: jammy + name: bionic - os: ubuntu - name: lunar + name: focal - os: ubuntu - name: mantic + name: hirsute - os: archlinux name: latest - os: debian @@ -68,7 +57,7 @@ jobs: software-properties-common ;; *"archlinux"*) - pacman -Syu --noconfirm --needed git sudo + pacman -Sy --noconfirm --needed git sudo ;; esac @@ -77,7 +66,7 @@ jobs: with: submodules: 'recursive' - name: test install environment ${{matrix.os}}.${{matrix.name}} - timeout-minutes: 45 + timeout-minutes: 30 env: DISABLE_MAVNATIVE: True DEBIAN_FRONTEND: noninteractive @@ -115,7 +104,6 @@ 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 @@ -129,7 +117,6 @@ 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 1c2259f3532a18..3db0df687afc17 100644 --- a/.github/workflows/test_linux_sbc.yml +++ b/.github/workflows/test_linux_sbc.yml @@ -1,141 +1,17 @@ name: test Linux SBC -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: - +on: [push, pull_request, workflow_dispatch] +# paths: +# - "*" +# - "!README.md" <-- don't rebuild on doc change concurrency: group: ci-${{github.workflow}}-${{ github.ref }} cancel-in-progress: true jobs: build: - runs-on: ubuntu-22.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.1.0 + runs-on: ubuntu-20.04 + container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest 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 1cd0b1c6afb60a..fb3d3b2d761e2f 100644 --- a/.github/workflows/test_replay.yml +++ b/.github/workflows/test_replay.yml @@ -1,154 +1,35 @@ name: test replay -on: +on: push: - paths-ignore: - # remove other vehicles + paths-ignore: # ignore vehicle only changes - 'AntennaTracker/**' - - 'ArduCopter/**' - 'ArduPlane/**' - 'ArduSub/**' - - '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/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' + - 'Blimp/**' + - 'ArduCopter/**' pull_request: - paths-ignore: - # remove other vehicles + paths-ignore: # ignore vehicle only changes - 'AntennaTracker/**' - - 'ArduCopter/**' - 'ArduPlane/**' - 'ArduSub/**' - - '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/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' + - 'Blimp/**' + - 'ArduCopter/**' workflow_dispatch: + concurrency: group: ci-${{github.workflow}}-${{ github.ref }} cancel-in-progress: true jobs: build: - runs-on: ubuntu-22.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.1.0 + runs-on: ubuntu-20.04 + container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest 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 c8d4b17c8255b3..67074fe019bde3 100644 --- a/.github/workflows/test_scripting.yml +++ b/.github/workflows/test_scripting.yml @@ -21,8 +21,8 @@ concurrency: jobs: test-scripting: - runs-on: ubuntu-22.04 - container: ardupilot/ardupilot-dev-base:v0.1.0 + runs-on: ubuntu-20.04 + container: ardupilot/ardupilot-dev-base:latest steps: # git checkout the PR - uses: actions/checkout@v3 diff --git a/.github/workflows/test_scripts.yml b/.github/workflows/test_scripts.yml index 1728f5facd1dbd..b26699e8b77bcc 100644 --- a/.github/workflows/test_scripts.yml +++ b/.github/workflows/test_scripts.yml @@ -8,8 +8,8 @@ concurrency: jobs: build: - runs-on: ubuntu-22.04 - container: ardupilot/ardupilot-dev-base:v0.1.0 + runs-on: ubuntu-20.04 + container: ardupilot/ardupilot-dev-base:latest strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: @@ -17,7 +17,6 @@ jobs: check_autotest_options, param_parse, python-cleanliness, - astyle-cleanliness, validate_board_list, ] steps: diff --git a/.github/workflows/test_sitl_blimp.yml b/.github/workflows/test_sitl_blimp.yml deleted file mode 100644 index 7343f4b81ebde6..00000000000000 --- a/.github/workflows/test_sitl_blimp.yml +++ /dev/null @@ -1,270 +0,0 @@ -name: test blimp - -on: - push: - paths-ignore: - # remove other vehicles - - 'AntennaTracker/**' - - 'ArduPlane/**' - - 'ArduSub/**' - - 'ArduCopter/**' - - 'Rover/**' - # remove non SITL HAL - - 'libraries/AP_HAL_ChibiOS/**' - - 'libraries/AP_HAL_ESP32/**' - # remove non SITL directories - - 'Tools/AP_Bootloader/**' - - 'Tools/AP_Periph/**' - - 'Tools/bootloaders/**' - - 'Tools/CHDK-Script/**' - - 'Tools/CodeStyle/**' - - 'Tools/completion/**' - - 'Tools/CPUInfo/**' - - 'Tools/debug/**' - - 'Tools/environment_install/**' - - 'Tools/FilterTestTool/**' - - 'Tools/Frame_params/**' - - 'Tools/geotag/**' - - 'Tools/GIT_Test/**' - - 'Tools/gittools/**' - - 'Tools/Hello/**' - - 'Tools/IO_Firmware/**' - - 'Tools/Linux_HAL_Essentials/**' - - 'Tools/LogAnalyzer/**' - - 'Tools/Pozyx/**' - - 'Tools/PrintVersion.py' - - 'Tools/Replay/**' - - 'Tools/simulink/**' - - 'Tools/UDP_Proxy/**' - - 'Tools/vagrant/**' - - 'Tools/Vicon/**' - # Discard python file from Tools/scripts as not used - - 'Tools/scripts/**.py' - - 'Tools/scripts/build_sizes/**' - - 'Tools/scripts/build_tests/**' - - 'Tools/scripts/CAN/**' - - 'Tools/scripts/signing/**' - # Remove other vehicles autotest - - 'Tools/autotest/antennatracker.py' - - 'Tools/autotest/arduplane.py' - - 'Tools/autotest/arducopter.py' - - 'Tools/autotest/ardusub.py' - - 'Tools/autotest/balancebot.py' - - 'Tools/autotest/helicopter.py' - - 'Tools/autotest/location.txt' - - 'Tools/autotest/quadplane.py' - - 'Tools/autotest/rover.py' - - 'Tools/autotest/sailboat.py' - - 'Tools/autotest/swarminit.txt' - # Remove markdown files as irrelevant - - '**.md' - # Remove dotfile at root directory - - './.dir-locals.el' - - './.dockerignore' - - './.editorconfig' - - './.flake8' - - './.gitattributes' - - './.github' - - './.gitignore' - - './.pre-commit-config.yaml' - - './.pydevproject' - - './.valgrind-suppressions' - - './.valgrindrc' - - 'Dockerfile' - - 'Vagrantfile' - - 'Makefile' - # Remove some directories check - - '.vscode/**' - - '.github/ISSUE_TEMPLATE/**' - - - pull_request: - paths-ignore: - # remove other vehicles - - 'AntennaTracker/**' - - 'ArduPlane/**' - - 'ArduSub/**' - - 'ArduCopter/**' - - 'Rover/**' - # remove non SITL HAL - - 'libraries/AP_HAL_ChibiOS/**' - - 'libraries/AP_HAL_ESP32/**' - # remove non SITL directories - - 'Tools/AP_Bootloader/**' - - 'Tools/AP_Periph/**' - - 'Tools/bootloaders/**' - - 'Tools/CHDK-Script/**' - - 'Tools/CodeStyle/**' - - 'Tools/completion/**' - - 'Tools/CPUInfo/**' - - 'Tools/debug/**' - - 'Tools/environment_install/**' - - 'Tools/FilterTestTool/**' - - 'Tools/Frame_params/**' - - 'Tools/geotag/**' - - 'Tools/GIT_Test/**' - - 'Tools/gittools/**' - - 'Tools/Hello/**' - - 'Tools/IO_Firmware/**' - - 'Tools/Linux_HAL_Essentials/**' - - 'Tools/LogAnalyzer/**' - - 'Tools/Pozyx/**' - - 'Tools/PrintVersion.py' - - 'Tools/Replay/**' - - 'Tools/simulink/**' - - 'Tools/UDP_Proxy/**' - - 'Tools/vagrant/**' - - 'Tools/Vicon/**' - # Discard python file from Tools/scripts as not used - - 'Tools/scripts/**.py' - - 'Tools/scripts/build_sizes/**' - - 'Tools/scripts/build_tests/**' - - 'Tools/scripts/CAN/**' - - 'Tools/scripts/signing/**' - # Remove other vehicles autotest - - 'Tools/autotest/antennatracker.py' - - 'Tools/autotest/arduplane.py' - - 'Tools/autotest/arducopter.py' - - 'Tools/autotest/ardusub.py' - - 'Tools/autotest/balancebot.py' - - 'Tools/autotest/helicopter.py' - - 'Tools/autotest/location.txt' - - 'Tools/autotest/quadplane.py' - - 'Tools/autotest/rover.py' - - 'Tools/autotest/sailboat.py' - - 'Tools/autotest/swarminit.txt' - # Remove markdown files as irrelevant - - '**.md' - # Remove dotfile at root directory - - './.dir-locals.el' - - './.dockerignore' - - './.editorconfig' - - './.flake8' - - './.gitattributes' - - './.github' - - './.gitignore' - - './.pre-commit-config.yaml' - - './.pydevproject' - - './.valgrind-suppressions' - - './.valgrindrc' - - 'Dockerfile' - - 'Vagrantfile' - - 'Makefile' - # Remove some directories check - - '.vscode/**' - - '.github/ISSUE_TEMPLATE/**' - - workflow_dispatch: - -concurrency: - group: ci-${{github.workflow}}-${{ github.ref }} - cancel-in-progress: true - -jobs: - build: - runs-on: ubuntu-20.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.0.29 - strategy: - fail-fast: false # don't cancel if a job from the matrix fails - matrix: - toolchain: [ - base, # GCC - clang, - ] - steps: - # git checkout the PR - - uses: actions/checkout@v3 - with: - submodules: 'recursive' - # Put ccache into github cache for faster build - - name: Prepare ccache timestamp - id: ccache_cache_timestamp - run: | - NOW=$(date -u +"%F-%T") - echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - - name: ccache cache files - uses: actions/cache@v3 - with: - path: ~/.ccache - key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} - restore-keys: ${{github.workflow}}-ccache-${{ matrix.toolchain }}- # restore ccache from either previous build on this branch or on master - - name: setup ccache - run: | - . .github/workflows/ccache.env - - name: build blimp ${{ matrix.toolchain }} - shell: bash - run: | - git config --global --add safe.directory ${GITHUB_WORKSPACE} - if [[ ${{ matrix.toolchain }} == "clang" ]]; then - export CC=clang - export CXX=clang++ - fi - PATH="/github/home/.local/bin:$PATH" - ./waf configure --board sitl - ./waf build --target bin/blimp - ccache -s - ccache -z - - autotest: - needs: build # don't try to launch the tests matrix if it doesn't build first, profit from caching for fast build - runs-on: ubuntu-20.04 - container: - 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 - matrix: - config: [ - sitltest-blimp, - ] - - steps: - # git checkout the PR - - uses: actions/checkout@v3 - with: - submodules: 'recursive' - # Put ccache into github cache for faster build - - name: Prepare ccache timestamp - id: ccache_cache_timestamp - run: | - NOW=$(date -u +"%F-%T") - echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - - name: ccache cache files - uses: actions/cache/restore@v3 - with: - path: ~/.ccache - key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}} - restore-keys: ${{github.workflow}}-ccache-base- # restore ccache from either previous build on this branch or on master - - name: setup ccache - run: | - . .github/workflows/ccache.env - - name: test ${{matrix.config}} - env: - CI_BUILD_TARGET: ${{matrix.config}} - shell: bash - run: | - git config --global --add safe.directory ${GITHUB_WORKSPACE} - PATH="/github/home/.local/bin:$PATH" - Tools/scripts/build_ci.sh - - - name: Archive buildlog artifacts - uses: actions/upload-artifact@v3 - if: failure() - with: - name: fail-${{matrix.config}} - path: | - /tmp/buildlogs - /__w/ardupilot/ardupilot/logs - /__w/ardupilot/ardupilot/ap-*.core - /__w/ardupilot/ardupilot/core.* - /__w/ardupilot/ardupilot/dumpstack.sh_* - /__w/ardupilot/ardupilot/dumpcore.sh_* - retention-days: 14 - - - name: Archive .bin artifacts - uses: actions/upload-artifact@v3 - with: - name: BIN-${{matrix.config}} - path: /__w/ardupilot/ardupilot/logs - retention-days: 7 - diff --git a/.github/workflows/test_sitl_copter.yml b/.github/workflows/test_sitl_copter.yml index 9783a04c3d9acd..0d76aed9048b33 100644 --- a/.github/workflows/test_sitl_copter.yml +++ b/.github/workflows/test_sitl_copter.yml @@ -2,155 +2,20 @@ name: test copter on: push: - paths-ignore: - # remove other vehicles + paths-ignore: # ignore vehicle only changes not for copter - 'AntennaTracker/**' - 'ArduPlane/**' - 'ArduSub/**' - - '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' - + - 'Blimp/**' pull_request: - paths-ignore: - # remove other vehicles + paths-ignore: # ignore vehicle only changes not for copter - 'AntennaTracker/**' - 'ArduPlane/**' - 'ArduSub/**' - - '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' + - 'Blimp/**' workflow_dispatch: @@ -160,8 +25,8 @@ concurrency: jobs: build: - runs-on: ubuntu-22.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.1.0 + runs-on: ubuntu-20.04 + container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: @@ -205,9 +70,9 @@ jobs: autotest: needs: build # don't try to launch the tests matrix if it doesn't build first, profit from caching for fast build - runs-on: ubuntu-22.04 + runs-on: ubuntu-20.04 container: - image: ardupilot/ardupilot-dev-base:v0.1.0 + image: ardupilot/ardupilot-dev-base:latest 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 @@ -273,9 +138,9 @@ jobs: retention-days: 7 build-gcc-heli: - runs-on: ubuntu-22.04 + runs-on: ubuntu-20.04 container: - image: ardupilot/ardupilot-dev-base:v0.1.0 + image: ardupilot/ardupilot-dev-base:latest options: --privileged --cap-add=SYS_PTRACE --security-opt apparmor=unconfined --security-opt seccomp=unconfined steps: # git checkout the PR @@ -309,8 +174,8 @@ jobs: autotest-heli: needs: build-gcc-heli # don't try to launch the tests matrix if it doesn't build first, profit from caching for fast build - runs-on: ubuntu-22.04 - container: ardupilot/ardupilot-dev-base:v0.1.0 + runs-on: ubuntu-20.04 + container: ardupilot/ardupilot-dev-base:latest 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 d3bd91ff2c4fdf..514f814ffa5c55 100644 --- a/.github/workflows/test_sitl_periph.yml +++ b/.github/workflows/test_sitl_periph.yml @@ -2,154 +2,22 @@ name: test ap_periph on: push: - paths-ignore: - # remove other vehicles than copter + paths-ignore: # ignore vehicle only changes - 'AntennaTracker/**' - 'ArduPlane/**' - 'ArduSub/**' - - 'Blimp/**' - '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' + - 'Blimp/**' + - 'ArduCopter/**' pull_request: - paths-ignore: - # remove other vehicles than copter + paths-ignore: # ignore vehicle only changes - 'AntennaTracker/**' - 'ArduPlane/**' - 'ArduSub/**' - - 'Blimp/**' - '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' + - 'Blimp/**' + - 'ArduCopter/**' workflow_dispatch: @@ -159,8 +27,8 @@ concurrency: jobs: build-gcc-ap_periph: - runs-on: ubuntu-22.04 - container: ardupilot/ardupilot-dev-periph:v0.1.0 + runs-on: ubuntu-20.04 + container: ardupilot/ardupilot-dev-periph:latest steps: # git checkout the PR - uses: actions/checkout@v3 @@ -199,9 +67,9 @@ jobs: autotest-can: needs: build-gcc-ap_periph # don't try to launch the tests matrix if it doesn't build first, profit from caching for fast build - runs-on: ubuntu-22.04 + runs-on: ubuntu-20.04 container: - image: ardupilot/ardupilot-dev-periph:v0.1.0 + image: ardupilot/ardupilot-dev-periph:latest options: --privileged strategy: fail-fast: false # don't cancel if a job from the matrix fails @@ -230,6 +98,15 @@ jobs: - name: setup ccache run: | . .github/workflows/ccache.env + - name: setup can-utils + run: | + kernel_ver=`uname -r` + if [ "$kernel_ver" = "5.4.0-1032-azure" ] || [ "$kernel_ver" = "5.11.4-051104-generic" ]; then echo "Unsupported Kernel $kernel_ver" && exit 0; fi; + sudo apt-get update + sudo apt-get -y install can-utils iproute2 linux-modules-extra-$(uname -r) + sudo modprobe vcan + sudo ip link add dev vcan0 type vcan + sudo ip link set up vcan0 - name: test ${{matrix.config}} env: CI_BUILD_TARGET: ${{matrix.config}} diff --git a/.github/workflows/test_sitl_plane.yml b/.github/workflows/test_sitl_plane.yml index 24a48476c102a7..172aa820d0868b 100644 --- a/.github/workflows/test_sitl_plane.yml +++ b/.github/workflows/test_sitl_plane.yml @@ -1,157 +1,21 @@ name: test plane -on: +on: push: - paths-ignore: - # remove other vehicles + paths-ignore: # ignore vehicle only changes not for plane - 'AntennaTracker/**' - - 'ArduCopter/**' - 'ArduSub/**' - - '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/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' + - 'Blimp/**' + - 'ArduCopter/**' pull_request: - paths-ignore: - # remove other vehicles + paths-ignore: # ignore vehicle only changes not for plane - 'AntennaTracker/**' - - 'ArduCopter/**' - 'ArduSub/**' - - '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/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' + - 'Blimp/**' + - 'ArduCopter/**' workflow_dispatch: @@ -161,8 +25,8 @@ concurrency: jobs: build: - runs-on: ubuntu-22.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.1.0 + runs-on: ubuntu-20.04 + container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: @@ -206,9 +70,9 @@ jobs: autotest: needs: build # don't try to launch the tests matrix if it doesn't build first, profit from caching for fast build - runs-on: ubuntu-22.04 + runs-on: ubuntu-20.04 container: - image: ardupilot/ardupilot-dev-base:v0.1.0 + image: ardupilot/ardupilot-dev-base:latest 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 5ca2fa60ace722..12f9b0ff2c66f5 100644 --- a/.github/workflows/test_sitl_rover.yml +++ b/.github/workflows/test_sitl_rover.yml @@ -1,156 +1,21 @@ name: test rover -on: +on: push: - paths-ignore: - # remove other vehicles + paths-ignore: # ignore vehicle only changes not for rover - 'AntennaTracker/**' - - 'ArduCopter/**' - 'ArduPlane/**' - 'ArduSub/**' - '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/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' - + - 'ArduCopter/**' pull_request: - paths-ignore: - # remove other vehicles + paths-ignore: # ignore vehicle only changes not for rover - 'AntennaTracker/**' - - 'ArduCopter/**' - 'ArduPlane/**' - 'ArduSub/**' - '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/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' + - 'ArduCopter/**' workflow_dispatch: @@ -160,8 +25,8 @@ concurrency: jobs: build: - runs-on: ubuntu-22.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.1.0 + runs-on: ubuntu-20.04 + container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: @@ -205,9 +70,9 @@ jobs: autotest: needs: build # don't try to launch the tests matrix if it doesn't build first, profit from caching for fast build - runs-on: ubuntu-22.04 + runs-on: ubuntu-20.04 container: - image: ardupilot/ardupilot-dev-base:v0.1.0 + image: ardupilot/ardupilot-dev-base:latest 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 57d69d575d2f45..7f7c7f16e08ed4 100644 --- a/.github/workflows/test_sitl_sub.yml +++ b/.github/workflows/test_sitl_sub.yml @@ -2,158 +2,20 @@ name: test sub on: push: - paths-ignore: - # remove other vehicles + paths-ignore: # ignore vehicle only changes not for sub - 'AntennaTracker/**' - - 'ArduCopter/**' - 'ArduPlane/**' - - '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/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' + - 'Blimp/**' + - 'ArduCopter/**' pull_request: - paths-ignore: - # remove other vehicles + paths-ignore: # ignore vehicle only changes not for sub - 'AntennaTracker/**' - - 'ArduCopter/**' - 'ArduPlane/**' - - '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/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' + - 'Blimp/**' + - 'ArduCopter/**' workflow_dispatch: @@ -163,8 +25,8 @@ concurrency: jobs: build: - runs-on: ubuntu-22.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.1.0 + runs-on: ubuntu-20.04 + container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: @@ -208,9 +70,9 @@ jobs: autotest: needs: build # don't try to launch the tests matrix if it doesn't build first, profit from caching for fast build - runs-on: ubuntu-22.04 + runs-on: ubuntu-20.04 container: - image: ardupilot/ardupilot-dev-base:v0.1.0 + image: ardupilot/ardupilot-dev-base:latest 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 c869fb2d66a6a1..f31761beab52fa 100644 --- a/.github/workflows/test_sitl_tracker.yml +++ b/.github/workflows/test_sitl_tracker.yml @@ -1,159 +1,21 @@ name: test tracker -on: +on: push: - paths-ignore: - # remove other vehicles - - 'ArduCopter/**' + paths-ignore: # ignore vehicle only changes - 'ArduPlane/**' - 'ArduSub/**' - - '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/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' + - 'Blimp/**' + - 'ArduCopter/**' pull_request: - paths-ignore: - # remove other vehicles - - 'ArduCopter/**' + paths-ignore: # ignore vehicle only changes - 'ArduPlane/**' - 'ArduSub/**' - - '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/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' + - 'Blimp/**' + - 'ArduCopter/**' workflow_dispatch: @@ -163,8 +25,8 @@ concurrency: jobs: build: - runs-on: ubuntu-22.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.1.0 + runs-on: ubuntu-20.04 + container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: @@ -208,9 +70,9 @@ jobs: autotest: needs: build # don't try to launch the tests matrix if it doesn't build first, profit from caching for fast build - runs-on: ubuntu-22.04 + runs-on: ubuntu-20.04 container: - image: ardupilot/ardupilot-dev-base:v0.1.0 + image: ardupilot/ardupilot-dev-base:latest 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 84bb5bb154ec81..bdaa2e2083147d 100644 --- a/.github/workflows/test_size.yml +++ b/.github/workflows/test_size.yml @@ -1,67 +1,17 @@ name: test size -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: - - +on: [pull_request] +# paths: +# - "*" +# - "!README.md" <-- don't rebuild on doc change concurrency: group: ci-${{github.workflow}}-${{ github.ref }} cancel-in-progress: true jobs: build: - runs-on: ubuntu-22.04 - container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.1.0 + runs-on: ubuntu-20.04 + container: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest strategy: fail-fast: false # don't cancel if a job from the matrix fails matrix: @@ -71,11 +21,8 @@ jobs: config: [ Durandal, MatekF405, - KakuteF7, - MatekH743-bdshot, Pixhawk1-1M, MatekF405-CAN, # see special "build bootloader" code below - DrotekP3Pro, # see special "build bootloader" code below Hitec-Airspeed, # see special code for Periph below (3 places!) f103-GPS # see special code for Periph below (3 places!) ] @@ -118,8 +65,7 @@ jobs: if [ "${{matrix.config}}" = "Hitec-Airspeed" ] || [ "${{matrix.config}}" = "f103-GPS" ]; then AP_PERIPH=1 - elif [ "${{matrix.config}}" = "MatekF405-CAN" ] || - [ "${{matrix.config}}" = "DrotekP3Pro" ]; then + elif [ "${{matrix.config}}" = "MatekF405-CAN" ]; then BOOTLOADER=1 fi if [ $BOOTLOADER -eq 1 ]; then @@ -192,8 +138,7 @@ jobs: if [ "${{matrix.config}}" = "Hitec-Airspeed" ] || [ "${{matrix.config}}" = "f103-GPS" ]; then AP_PERIPH=1 - elif [ "${{matrix.config}}" = "MatekF405-CAN" ] || - [ "${{matrix.config}}" = "DrotekP3Pro" ]; then + elif [ "${{matrix.config}}" = "MatekF405-CAN" ]; then BOOTLOADER=1 fi if [ $BOOTLOADER -eq 1 ]; then @@ -285,8 +230,7 @@ jobs: if [ "${{matrix.config}}" = "Hitec-Airspeed" ] || [ "${{matrix.config}}" = "f103-GPS" ]; then AP_PERIPH=1 - elif [ "${{matrix.config}}" = "MatekF405-CAN" ] || - [ "${{matrix.config}}" = "DrotekP3Pro" ]; then + elif [ "${{matrix.config}}" = "MatekF405-CAN" ]; then BOOTLOADER=1 fi if [ $AP_PERIPH -eq 1 ]; then diff --git a/.github/workflows/test_unit_tests.yml b/.github/workflows/test_unit_tests.yml index f843f07960b87a..54c660ea6b818a 100644 --- a/.github/workflows/test_unit_tests.yml +++ b/.github/workflows/test_unit_tests.yml @@ -1,107 +1,18 @@ name: test unit tests and sitl building -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: - +on: [push, pull_request, workflow_dispatch] +# paths: +# - "*" +# - "!README.md" <-- don't rebuild on doc change concurrency: group: ci-${{github.workflow}}-${{ github.ref }} cancel-in-progress: true jobs: build: - runs-on: ubuntu-22.04 + runs-on: ubuntu-20.04 container: - image: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:v0.1.0 + image: ardupilot/ardupilot-dev-${{ matrix.toolchain }}:latest options: --user 1001 strategy: fail-fast: false # don't cancel if a job from the matrix fails diff --git a/.gitignore b/.gitignore index 42d336cd2a66e4..58913bfed4d3dd 100644 --- a/.gitignore +++ b/.gitignore @@ -111,8 +111,7 @@ GRTAGS GTAGS *.apj .gdbinit -.vscode/* -!.vscode/extensions.json +/.vscode /.history Parameters.html Parameters.md @@ -126,7 +125,6 @@ parameters.edn LogMessages.html LogMessages.rst LogMessages.xml -LogMessages.md # JetBrains IDE files .idea/* # CMake @@ -154,13 +152,3 @@ dumpstack_*out build.tmp.binaries/ tasklist.json modules/esp_idf - -# Ignore Python virtual environments -# from: https://github.com/github/gitignore/blob/4488915eec0b3a45b5c63ead28f286819c0917de/Python.gitignore#L125 -.env -.venv -env/ -venv/ -ENV/ -env.bak/ -venv.bak/ diff --git a/.gitmodules b/.gitmodules index fbfe76753a51d6..a6fbca83a4ad0b 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,6 @@ +[submodule "modules/uavcan"] + path = modules/uavcan + url = https://github.com/DroneCAN/libuavcan.git [submodule "modules/waf"] path = modules/waf url = https://github.com/ArduPilot/waf.git @@ -31,11 +34,3 @@ [submodule "modules/DroneCAN/libcanard"] path = modules/DroneCAN/libcanard url = https://github.com/DroneCAN/libcanard -[submodule "modules/Micro-XRCE-DDS-Client"] - path = modules/Micro-XRCE-DDS-Client - url = https://github.com/ardupilot/Micro-XRCE-DDS-Client.git - branch = master -[submodule "modules/Micro-CDR"] - path = modules/Micro-CDR - url = https://github.com/ardupilot/Micro-CDR.git - branch = master diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 44def5fb51b4fe..ef511f9df7c9f6 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -16,7 +16,7 @@ exclude: | repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.4.0 + rev: v4.2.0 hooks: #- id: trailing-whitespace #- id: end-of-file-fixer @@ -24,11 +24,6 @@ repos: name: Check line ending character (LF) args: ["--fix=lf"] types_or: [python, c, c++, shell] - exclude: | - (?x)^( - libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp | - libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.h - )$ - id: check-added-large-files - id: check-executables-have-shebangs - id: check-shebang-scripts-are-executable @@ -41,20 +36,10 @@ repos: - id: check-xml - id: check-yaml - - repo: https://github.com/lsst-ts/pre-commit-xmllint - rev: 6f36260b537bf9a42b6ea5262c915ae50786296e - hooks: - - id: format-xmllint - files: libraries/AP_DDS/dds_xrce_profile.xml - - repo: https://github.com/psf/black - rev: 23.7.0 - hooks: - - id: black - files: libraries\/AP_DDS\/(wscript|.*\.py)$ - +# Disable isort and mypy temporarily due to config conflicts # # Use to sort python imports by name and put system import first. # - repo: https://github.com/pycqa/isort -# rev: 5.12.0 +# rev: 5.10.1 # hooks: # - id: isort # args: [--check-only] diff --git a/.semaphore/semaphore.yml b/.semaphore/semaphore.yml new file mode 100644 index 00000000000000..19c75718d99a4d --- /dev/null +++ b/.semaphore/semaphore.yml @@ -0,0 +1,104 @@ +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 4284ec07442f18..e9cd876a0b91cf 100644 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -1,13 +1,11 @@ { "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.default.json b/.vscode/settings.json similarity index 100% rename from .vscode/settings.default.json rename to .vscode/settings.json diff --git a/AntennaTracker/AntennaTracker.txt b/AntennaTracker/AntennaTracker.txt index 8ced3605b22465..f66169dee5093a 100644 --- a/AntennaTracker/AntennaTracker.txt +++ b/AntennaTracker/AntennaTracker.txt @@ -109,7 +109,7 @@ around. It might even damage itself. AntennaTracker (like other ardupilot software such as ArduPlane, ArduRover etc) has configuration values that control and tailor its operation, and which are stored in EEPROM on the processor. The configuration is restored from -EEPROM every time the processor starts. +EEPROM every time the processsor starts. You can use MissionPlanner, mavproxy or apm_planner or other mavlink compatible software to check and change the configuration of your AntennaTracker. @@ -219,7 +219,7 @@ the vehicle, cd to the ArduPlane directory and run this: ../Tools/autotest/sim_arduplane.sh -T --aircraft test -The -T flag tells sim_arduplane.sh to start an antenna tracker +The -T flag tells sim_arduplane.sh to start an entenna tracker simulator and also start a virtual antenna tracker in a window. To start the antenna tracker running run "tracker start" in the diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index d38d8af77516d5..02c48c6b94f38e 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -302,10 +302,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = { MSG_SIMSTATE, MSG_SYSTEM_TIME, MSG_AHRS2, -#if COMPASS_CAL_ENABLED MSG_MAG_CAL_REPORT, MSG_MAG_CAL_PROGRESS, -#endif MSG_EKF_STATUS_REPORT, }; static const ap_message STREAM_PARAMS_msgs[] = { @@ -414,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_int_t &packet) +MAV_RESULT GCS_MAVLINK_Tracker::handle_command_component_arm_disarm(const mavlink_command_long_t &packet) { if (is_equal(packet.param1,1.0f)) { tracker.arm_servos(); @@ -427,8 +425,11 @@ MAV_RESULT GCS_MAVLINK_Tracker::handle_command_component_arm_disarm(const mavlin return MAV_RESULT_UNSUPPORTED; } -MAV_RESULT GCS_MAVLINK_Tracker::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) +MAV_RESULT GCS_MAVLINK_Tracker::handle_command_long_packet(const mavlink_command_long_t &packet) { + // do command + send_text(MAV_SEVERITY_INFO,"Command received: "); + switch(packet.command) { case MAV_CMD_DO_SET_SERVO: @@ -446,7 +447,7 @@ MAV_RESULT GCS_MAVLINK_Tracker::handle_command_int_packet(const mavlink_command_ return MAV_RESULT_ACCEPTED; default: - return GCS_MAVLINK::handle_command_int_packet(packet, msg); + return GCS_MAVLINK::handle_command_long_packet(packet); } } diff --git a/AntennaTracker/GCS_Mavlink.h b/AntennaTracker/GCS_Mavlink.h index 38f7cdb483e712..5b7796e86a6679 100644 --- a/AntennaTracker/GCS_Mavlink.h +++ b/AntennaTracker/GCS_Mavlink.h @@ -14,14 +14,14 @@ class GCS_MAVLINK_Tracker : public GCS_MAVLINK protected: // telem_delay is not used by Tracker but is pure virtual, thus - // this implementation. it probably *should* be used by Tracker, + // this implementaiton. it probably *should* be used by Tracker, // as currently Tracker may brick XBees uint32_t telem_delay() const override { return 0; } - MAV_RESULT handle_command_component_arm_disarm(const mavlink_command_int_t &packet) override; + MAV_RESULT handle_command_component_arm_disarm(const mavlink_command_long_t &packet) override; MAV_RESULT _handle_command_preflight_calibration_baro(const mavlink_message_t &msg) override; - MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; + MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override; int32_t global_position_int_relative_alt() const override { return 0; // what if we have been picked up and carried somewhere? diff --git a/AntennaTracker/Parameters.cpp b/AntennaTracker/Parameters.cpp index e1757fba32e4cf..33cb304aa19ba1 100644 --- a/AntennaTracker/Parameters.cpp +++ b/AntennaTracker/Parameters.cpp @@ -130,7 +130,7 @@ const AP_Param::Info Tracker::var_info[] = { // @Param: ONOFF_PITCH_MINT // @DisplayName: Pitch minimum movement time - // @Description: Minimum amount of time in seconds to move in pitch + // @Description: Minimim amount of time in seconds to move in pitch // @Units: s // @Increment: 0.01 // @Range: 0 2 @@ -379,14 +379,6 @@ const AP_Param::Info Tracker::var_info[] = { // @Increment: 0.5 // @User: Advanced - // @Param: PITCH2SRV_PDMX - // @DisplayName: Pitch axis controller PD sum maximum - // @Description: Pitch axis controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output - // @Range: 0 4000 - // @Increment: 10 - // @Units: d% - // @User: Advanced - GGROUP(pidPitch2Srv, "PITCH2SRV_", AC_PID), // @Param: YAW2SRV_P @@ -456,14 +448,6 @@ const AP_Param::Info Tracker::var_info[] = { // @Increment: 0.5 // @User: Advanced - // @Param: YAW2SRV_PDMX - // @DisplayName: Yaw axis controller PD sum maximum - // @Description: Yaw axis controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output - // @Range: 0 4000 - // @Increment: 10 - // @Units: d% - // @User: Advanced - GGROUP(pidYaw2Srv, "YAW2SRV_", AC_PID), #if AP_SCRIPTING_ENABLED @@ -541,18 +525,6 @@ const AP_Param::Info Tracker::var_info[] = { // @Path: ../libraries/AP_Logger/AP_Logger.cpp GOBJECT(logger, "LOG", AP_Logger), -#if HAL_NAVEKF2_AVAILABLE - // @Group: EK2_ - // @Path: ../libraries/AP_NavEKF2/AP_NavEKF2.cpp - GOBJECTN(ahrs.EKF2, NavEKF2, "EK2_", NavEKF2), -#endif - -#if HAL_NAVEKF3_AVAILABLE - // @Group: EK3_ - // @Path: ../libraries/AP_NavEKF3/AP_NavEKF3.cpp - GOBJECTN(ahrs.EKF3, NavEKF3, "EK3_", NavEKF3), -#endif - AP_VAREND }; diff --git a/AntennaTracker/Parameters.h b/AntennaTracker/Parameters.h index 972c36f52505e8..a3192e24b55944 100644 --- a/AntennaTracker/Parameters.h +++ b/AntennaTracker/Parameters.h @@ -130,8 +130,6 @@ class Parameters { k_param_disarm_pwm, k_param_auto_opts, - k_param_NavEKF2, - k_param_NavEKF3, k_param_logger = 253, // 253 - Logging Group diff --git a/AntennaTracker/Tracker.cpp b/AntennaTracker/Tracker.cpp index f3284e985ec353..e7e1a384587e7d 100644 --- a/AntennaTracker/Tracker.cpp +++ b/AntennaTracker/Tracker.cpp @@ -64,6 +64,7 @@ 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 3e296c3fa5a77f..191c236294a131 100644 --- a/AntennaTracker/system.cpp +++ b/AntennaTracker/system.cpp @@ -59,6 +59,9 @@ 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 ff55bece071f3e..1adb76acba8a9a 100644 --- a/AntennaTracker/version.h +++ b/AntennaTracker/version.h @@ -6,13 +6,13 @@ #include "ap_version.h" -#define THISFIRMWARE "AntennaTracker V4.5.0-dev" +#define THISFIRMWARE "AntennaTracker V4.3.0-dev" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,5,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 4,3,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 4 -#define FW_MINOR 5 +#define FW_MINOR 3 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index bdd901706b50de..db60b9cfc754ed 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -9,6 +9,8 @@ //#define AC_OAPATHPLANNER_ENABLED DISABLED // disable path planning around obstacles //#define PARACHUTE DISABLED // disable parachute release to save 1k of flash //#define NAV_GUIDED DISABLED // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands +//#define PRECISION_LANDING DISABLED // disable precision landing using companion computer or IRLock sensor +//#define BEACON_ENABLED DISABLED // disable beacon support //#define STATS_ENABLED DISABLED // disable statistics support //#define MODE_ACRO_ENABLED DISABLED // disable acrobatic mode support //#define MODE_AUTO_ENABLED DISABLED // disable auto mode support diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 1bdbc630bc2a9d..6f6051f89676cc 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -1,10 +1,5 @@ #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); @@ -23,12 +18,11 @@ 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"); - passed = false; + return false; } // check if motor interlock aux switch is in use @@ -36,22 +30,23 @@ 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"); - passed = false; + return false; + } + + // if we are using motor Estop switch, it must not be in Estop position + if (SRV_Channels::get_emergency_stop()){ + check_failed(display_failure, "Motor Emergency Stopped"); + return false; } if (!disarm_switch_checks(display_failure)) { - passed = false; + return 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; } @@ -60,7 +55,6 @@ 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) @@ -224,6 +218,19 @@ 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))) { @@ -331,7 +338,6 @@ 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 +629,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 (!((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))) { + 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))) { // 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); @@ -749,8 +755,8 @@ bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_ copter.sprayer.test_pump(false); #endif - // output lowest possible value to motors - copter.motors->output_min(); + // enable output to motors + copter.enable_motor_output(); // finally actually arm the motors copter.motors->armed(true); @@ -790,7 +796,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 && - AP_Arming::method_is_GCS(method) && + method == AP_Arming::Method::MAVLINK && !copter.ap.land_complete) { return false; } @@ -845,5 +851,3 @@ 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 deleted file mode 100644 index 35353a412618f6..00000000000000 --- a/ArduCopter/AP_ExternalControl_Copter.cpp +++ /dev/null @@ -1,37 +0,0 @@ -/* - external control library for copter - */ - - -#include "AP_ExternalControl_Copter.h" -#if AP_EXTERNAL_CONTROL_ENABLED - -#include "Copter.h" - -/* - set linear velocity and yaw rate. Pass NaN for yaw_rate_rads to not control yaw - velocity is in earth frame, NED, m/s -*/ -bool AP_ExternalControl_Copter::set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, float yaw_rate_rads) -{ - if (!ready_for_external_control()) { - return false; - } - const float yaw_rate_cds = isnan(yaw_rate_rads)? 0: degrees(yaw_rate_rads)*100; - - // Copter velocity is positive if aircraft is moving up which is opposite the incoming NED frame. - Vector3f velocity_NEU_ms { - linear_velocity.x, - linear_velocity.y, - -linear_velocity.z }; - Vector3f velocity_up_cms = velocity_NEU_ms * 100; - copter.mode_guided.set_velocity(velocity_up_cms, false, 0, !isnan(yaw_rate_rads), yaw_rate_cds); - return true; -} - -bool AP_ExternalControl_Copter::ready_for_external_control() -{ - return copter.flightmode->in_guided_mode() && copter.motors->armed(); -} - -#endif // AP_EXTERNAL_CONTROL_ENABLED diff --git a/ArduCopter/AP_ExternalControl_Copter.h b/ArduCopter/AP_ExternalControl_Copter.h deleted file mode 100644 index e9a879106ebef0..00000000000000 --- a/ArduCopter/AP_ExternalControl_Copter.h +++ /dev/null @@ -1,26 +0,0 @@ -/* - 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/AP_Rally.cpp b/ArduCopter/AP_Rally.cpp index 46b6735f560e7d..31027d7909cdf5 100644 --- a/ArduCopter/AP_Rally.cpp +++ b/ArduCopter/AP_Rally.cpp @@ -13,10 +13,6 @@ along with this program. If not, see . */ -#include - -#if HAL_RALLY_ENABLED - #include #include "Copter.h" @@ -32,5 +28,3 @@ bool AP_Rally_Copter::is_valid(const Location &rally_point) const #endif return true; } - -#endif // HAL_RALLY_ENABLED diff --git a/ArduCopter/AP_Rally.h b/ArduCopter/AP_Rally.h index 27c3df857a9271..466f5c04b9897b 100644 --- a/ArduCopter/AP_Rally.h +++ b/ArduCopter/AP_Rally.h @@ -14,10 +14,6 @@ */ #pragma once -#include - -#if HAL_RALLY_ENABLED - #include #include @@ -32,5 +28,3 @@ class AP_Rally_Copter : public AP_Rally private: bool is_valid(const Location &rally_point) const override; }; - -#endif // HAL_RALLY_ENABLED diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 8bbbc186c19e14..293d1d38f5f094 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -165,7 +165,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #if HAL_PROXIMITY_ENABLED SCHED_TASK_CLASS(AP_Proximity, &copter.g2.proximity, update, 200, 50, 36), #endif -#if AP_BEACON_ENABLED +#if BEACON_ENABLED == ENABLED SCHED_TASK_CLASS(AP_Beacon, &copter.g2.beacon, update, 400, 50, 39), #endif SCHED_TASK(update_altitude, 10, 100, 42), @@ -178,11 +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 +#if PRECISION_LANDING == ENABLED SCHED_TASK(update_precland, 400, 50, 69), #endif #if FRAME_CONFIG == HELI_FRAME @@ -191,6 +189,7 @@ 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), @@ -447,26 +446,6 @@ 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 @@ -571,7 +550,7 @@ void Copter::ten_hz_logging_loop() #if HAL_PROXIMITY_ENABLED g2.proximity.log(); // Write proximity sensor distances #endif -#if AP_BEACON_ENABLED +#if BEACON_ENABLED == ENABLED g2.beacon.log(); #endif } @@ -583,11 +562,6 @@ 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 @@ -614,7 +588,7 @@ void Copter::twentyfive_hz_logging() #endif } -// three_hz_loop - 3hz loop +// three_hz_loop - 3.3hz loop void Copter::three_hz_loop() { // check if we've lost contact with the ground station @@ -744,7 +718,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_TASK. + // we tell AHRS to skip INS update as we have already done it in fast_loop() ahrs.update(true); } diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index bb3942a8f4822c..8322ad66e60ffb 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -69,7 +69,6 @@ #include // Crop sprayer library #include // ADS-B RF based collision avoidance module library #include // ArduPilot proximity sensor library -#include #include #include @@ -100,13 +99,8 @@ #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 +// libraries which are dependent on #defines in defines.h and/or config.h +#if BEACON_ENABLED == ENABLED #include #endif @@ -121,7 +115,7 @@ #if AP_GRIPPER_ENABLED # include #endif -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED # include # include #endif @@ -198,9 +192,6 @@ 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; @@ -327,12 +318,6 @@ 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; @@ -544,7 +529,7 @@ class Copter : public AP_Vehicle { #endif // Precision Landing -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED AC_PrecLand precland; AC_PrecLand_StateMachine precland_statemachine; #endif @@ -694,8 +679,6 @@ 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); @@ -867,13 +850,9 @@ 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(); @@ -912,6 +891,7 @@ class Copter : public AP_Vehicle { void default_dead_zones(); void init_rc_in(); void init_rc_out(); + void enable_motor_output(); void read_radio(); void set_throttle_and_failsafe(uint16_t throttle_pwm); void set_throttle_zero_flag(int16_t throttle_control); diff --git a/ArduCopter/GCS_Copter.cpp b/ArduCopter/GCS_Copter.cpp index 02599b045d798f..7721e5e74665d9 100644 --- a/ArduCopter/GCS_Copter.cpp +++ b/ArduCopter/GCS_Copter.cpp @@ -114,7 +114,7 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void) } #endif -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED if (copter.precland.enabled()) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 7c74d377dc2750..28aebf0511093f 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -511,9 +511,7 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { MSG_GPS2_RAW, MSG_GPS2_RTK, MSG_NAV_CONTROLLER_OUTPUT, -#if AP_FENCE_ENABLED MSG_FENCE_STATUS, -#endif MSG_POSITION_TARGET_GLOBAL_INT, }; static const ap_message STREAM_POSITION_msgs[] = { @@ -546,10 +544,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = { MSG_BATTERY_STATUS, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_OPTICAL_FLOW, -#if COMPASS_CAL_ENABLED MSG_MAG_CAL_REPORT, MSG_MAG_CAL_PROGRESS, -#endif MSG_EKF_STATUS_REPORT, MSG_VIBRATION, #if AP_RPM_ENABLED @@ -583,14 +579,6 @@ 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 @@ -652,14 +640,14 @@ void GCS_MAVLINK_Copter::handle_command_ack(const mavlink_message_t &msg) */ void GCS_MAVLINK_Copter::handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) { -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED copter.precland.handle_msg(packet, timestamp_ms); #endif } -MAV_RESULT GCS_MAVLINK_Copter::_handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) +MAV_RESULT GCS_MAVLINK_Copter::_handle_command_preflight_calibration(const mavlink_command_long_t &packet, const mavlink_message_t &msg) { - if (packet.y == 1) { + if (is_equal(packet.param6,1.0f)) { // compassmot calibration return copter.mavlink_compassmot(*this); } @@ -677,7 +665,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_do_set_roi(const Location &roi_loc return MAV_RESULT_ACCEPTED; } -MAV_RESULT GCS_MAVLINK_Copter::handle_preflight_reboot(const mavlink_command_int_t &packet, const mavlink_message_t &msg) +MAV_RESULT GCS_MAVLINK_Copter::handle_preflight_reboot(const mavlink_command_long_t &packet, const mavlink_message_t &msg) { // reject reboot if user has also specified they want the "Auto" ESC calibration on next reboot if (copter.g.esc_calibrate == (uint8_t)Copter::ESCCalibrationModes::ESCCAL_AUTO) { @@ -740,25 +728,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, const mavlink_message_t &msg) +MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_int_t &packet) { switch(packet.command) { - - case MAV_CMD_CONDITION_YAW: - return handle_MAV_CMD_CONDITION_YAW(packet); - - case MAV_CMD_DO_CHANGE_SPEED: - return handle_MAV_CMD_DO_CHANGE_SPEED(packet); - -#if MODE_FOLLOW_ENABLED == ENABLED case MAV_CMD_DO_FOLLOW: +#if MODE_FOLLOW_ENABLED == ENABLED // 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); @@ -767,75 +748,18 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i case MAV_CMD_DO_PAUSE_CONTINUE: return handle_command_pause_continue(packet); - case MAV_CMD_DO_MOTOR_TEST: - return handle_MAV_CMD_DO_MOTOR_TEST(packet); - -#if PARACHUTE == ENABLED - case MAV_CMD_DO_PARACHUTE: - return handle_MAV_CMD_DO_PARACHUTE(packet); -#endif - -#if AC_MAVLINK_SOLO_BUTTON_COMMAND_HANDLING_ENABLED - // Solo user presses pause button - case MAV_CMD_SOLO_BTN_PAUSE_CLICK: - return handle_MAV_CMD_SOLO_BTN_PAUSE_CLICK(packet); - // Solo user presses Fly button: - case MAV_CMD_SOLO_BTN_FLY_HOLD: - return handle_MAV_CMD_SOLO_BTN_FLY_HOLD(packet); - // Solo user holds down Fly button for a couple of seconds - case MAV_CMD_SOLO_BTN_FLY_CLICK: - return handle_MAV_CMD_SOLO_BTN_FLY_CLICK(packet); -#endif - -#if MODE_AUTO_ENABLED == ENABLED - case MAV_CMD_MISSION_START: - return handle_MAV_CMD_MISSION_START(packet); -#endif - -#if AP_WINCH_ENABLED - case MAV_CMD_DO_WINCH: - return handle_MAV_CMD_DO_WINCH(packet); -#endif - - case MAV_CMD_NAV_LOITER_UNLIM: - if (!copter.set_mode(Mode::Number::LOITER, ModeReason::GCS_COMMAND)) { - return MAV_RESULT_FAILED; - } - return MAV_RESULT_ACCEPTED; - - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - if (!copter.set_mode(Mode::Number::RTL, ModeReason::GCS_COMMAND)) { - return MAV_RESULT_FAILED; - } - return MAV_RESULT_ACCEPTED; - - case MAV_CMD_NAV_VTOL_LAND: - case MAV_CMD_NAV_LAND: - if (!copter.set_mode(Mode::Number::LAND, ModeReason::GCS_COMMAND)) { - return MAV_RESULT_FAILED; - } - return MAV_RESULT_ACCEPTED; - -#if MODE_AUTO_ENABLED == ENABLED - case MAV_CMD_DO_LAND_START: - if (copter.mode_auto.jump_to_landing_sequence_auto_RTL(ModeReason::GCS_COMMAND)) { - return MAV_RESULT_ACCEPTED; - } - return MAV_RESULT_FAILED; -#endif - default: - return GCS_MAVLINK::handle_command_int_packet(packet, msg); + return GCS_MAVLINK::handle_command_int_packet(packet); } } #if HAL_MOUNT_ENABLED -MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_int_t &packet, const mavlink_message_t &msg) +MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_long_t &packet) { 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() != AP_Mount::Type::None) && + if ((copter.camera_mount.get_mount_type() != copter.camera_mount.MountType::Mount_Type_None) && !copter.camera_mount.has_pan_control()) { copter.flightmode->auto_yaw.set_yaw_angle_rate((float)packet.param3, 0.0f); } @@ -843,11 +767,11 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_int_t default: break; } - return GCS_MAVLINK::handle_command_mount(packet, msg); + return GCS_MAVLINK::handle_command_mount(packet); } #endif -MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg) +MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_long_t &packet) { switch(packet.command) { @@ -867,13 +791,44 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ return MAV_RESULT_ACCEPTED; } - default: - return GCS_MAVLINK::handle_command_long_packet(packet, msg); - } -} +#if MODE_AUTO_ENABLED == ENABLED + case MAV_CMD_DO_LAND_START: + if (copter.mode_auto.jump_to_landing_sequence_auto_RTL(ModeReason::GCS_COMMAND)) { + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; +#endif -MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_CONDITION_YAW(const mavlink_command_int_t &packet) -{ + case MAV_CMD_NAV_LOITER_UNLIM: + if (!copter.set_mode(Mode::Number::LOITER, ModeReason::GCS_COMMAND)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; + + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + if (!copter.set_mode(Mode::Number::RTL, ModeReason::GCS_COMMAND)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; + + case MAV_CMD_NAV_VTOL_LAND: + case MAV_CMD_NAV_LAND: + if (!copter.set_mode(Mode::Number::LAND, ModeReason::GCS_COMMAND)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; + +#if MODE_FOLLOW_ENABLED == ENABLED + case MAV_CMD_DO_FOLLOW: + // param1: sysid of target to follow + if ((packet.param1 > 0) && (packet.param1 <= 255)) { + copter.g2.follow.set_target_sysid((uint8_t)packet.param1); + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; +#endif + + case MAV_CMD_CONDITION_YAW: // param1 : target angle [0-360] // param2 : speed during change [deg per second] // param3 : direction (-1:ccw, +1:cw) @@ -889,10 +844,8 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_CONDITION_YAW(const mavlink_comman return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; -} -MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_CHANGE_SPEED(const mavlink_command_int_t &packet) -{ + case MAV_CMD_DO_CHANGE_SPEED: // param1 : Speed type (0 or 1=Ground Speed, 2=Climb Speed, 3=Descent Speed) // param2 : new speed in m/s // param3 : unused @@ -916,11 +869,9 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_CHANGE_SPEED(const mavlink_comm } } return MAV_RESULT_FAILED; -} #if MODE_AUTO_ENABLED == ENABLED -MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_MISSION_START(const mavlink_command_int_t &packet) -{ + case MAV_CMD_MISSION_START: if (copter.set_mode(Mode::Number::AUTO, ModeReason::GCS_COMMAND)) { copter.set_auto_armed(true); if (copter.mode_auto.mission.state() != AP_Mission::MISSION_RUNNING) { @@ -929,14 +880,10 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_MISSION_START(const mavlink_comman return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; -} #endif - - #if PARACHUTE == ENABLED -MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_PARACHUTE(const mavlink_command_int_t &packet) -{ + case MAV_CMD_DO_PARACHUTE: // configure or release parachute switch ((uint16_t)packet.param1) { case PARACHUTE_DISABLE: @@ -951,11 +898,9 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_PARACHUTE(const mavlink_command return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; -} #endif -MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command_int_t &packet) -{ + case MAV_CMD_DO_MOTOR_TEST: // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle) // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum) // param3 : throttle (range depends upon param2) @@ -967,12 +912,10 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_comman (uint8_t)packet.param2, packet.param3, packet.param4, - (uint8_t)packet.x); -} + (uint8_t)packet.param5); #if AP_WINCH_ENABLED -MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_WINCH(const mavlink_command_int_t &packet) -{ + case MAV_CMD_DO_WINCH: // param1 : winch number (ignored) // param2 : action (0=relax, 1=relative length control, 2=rate control). See WINCH_ACTIONS enum. if (!copter.g2.winch.enabled()) { @@ -993,12 +936,27 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_WINCH(const mavlink_command_int break; } return MAV_RESULT_FAILED; -} -#endif // AP_WINCH_ENABLED +#endif -#if AC_MAVLINK_SOLO_BUTTON_COMMAND_HANDLING_ENABLED -MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_SOLO_BTN_FLY_CLICK(const mavlink_command_int_t &packet) -{ +#if 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) { return MAV_RESULT_ACCEPTED; } @@ -1008,10 +966,10 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_SOLO_BTN_FLY_CLICK(const mavlink_c copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::GCS_COMMAND); } return MAV_RESULT_ACCEPTED; -} + } -MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_SOLO_BTN_FLY_HOLD(const mavlink_command_int_t &packet) -{ + /* Solo user holds down Fly button for a couple of seconds */ + case MAV_CMD_SOLO_BTN_FLY_HOLD: { if (copter.failsafe.radio) { return MAV_RESULT_ACCEPTED; } @@ -1029,10 +987,10 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_SOLO_BTN_FLY_HOLD(const mavlink_co copter.set_mode(Mode::Number::LAND, ModeReason::GCS_COMMAND); } return MAV_RESULT_ACCEPTED; -} + } -MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_SOLO_BTN_PAUSE_CLICK(const mavlink_command_int_t &packet) -{ + /* Solo user presses pause button */ + case MAV_CMD_SOLO_BTN_PAUSE_CLICK: { if (copter.failsafe.radio) { return MAV_RESULT_ACCEPTED; } @@ -1062,8 +1020,18 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_SOLO_BTN_PAUSE_CLICK(const mavlink } } return MAV_RESULT_ACCEPTED; + } + + // pause or resume an auto mission + case MAV_CMD_DO_PAUSE_CONTINUE: { + mavlink_command_int_t packet_int; + GCS_MAVLINK_Copter::convert_COMMAND_LONG_to_COMMAND_INT(packet, packet_int); + return handle_command_pause_continue(packet_int); + } + default: + return GCS_MAVLINK::handle_command_long_packet(packet); + } } -#endif // AC_MAVLINK_SOLO_BUTTON_COMMAND_HANDLING_ENABLED MAV_RESULT GCS_MAVLINK_Copter::handle_command_pause_continue(const mavlink_command_int_t &packet) { @@ -1093,7 +1061,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() != AP_Mount::Type::None) && + if ((copter.camera_mount.get_mount_type() != copter.camera_mount.MountType::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, @@ -1121,20 +1089,6 @@ 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 @@ -1295,13 +1249,8 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) // prepare velocity Vector3f vel_vector; if (!vel_ignore) { - 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 + // 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) { copter.rotate_body_frame_to_NE(vel_vector.x, vel_vector.y); @@ -1396,13 +1345,8 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) // prepare velocity Vector3f vel_vector; if (!vel_ignore) { - 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 + // convert to cm + vel_vector = Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f); } // prepare acceleration @@ -1480,7 +1424,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) } // end handle mavlink -MAV_RESULT GCS_MAVLINK_Copter::handle_flight_termination(const mavlink_command_int_t &packet) { +MAV_RESULT GCS_MAVLINK_Copter::handle_flight_termination(const mavlink_command_long_t &packet) { #if ADVANCED_FAILSAFE == ENABLED if (GCS_MAVLINK::handle_flight_termination(packet) == MAV_RESULT_ACCEPTED) { return MAV_RESULT_ACCEPTED; diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index eda1fd42c65cb5..2c0828aae47657 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -1,11 +1,6 @@ #pragma once #include -#include - -#ifndef AC_MAVLINK_SOLO_BUTTON_COMMAND_HANDLING_ENABLED -#define AC_MAVLINK_SOLO_BUTTON_COMMAND_HANDLING_ENABLED 1 -#endif class GCS_MAVLINK_Copter : public GCS_MAVLINK { @@ -18,7 +13,7 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK uint32_t telem_delay() const override; - MAV_RESULT handle_flight_termination(const mavlink_command_int_t &packet) override; + MAV_RESULT handle_flight_termination(const mavlink_command_long_t &packet) override; uint8_t sysid_my_gcs() const override; bool sysid_enforce() const override; @@ -26,19 +21,19 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK bool params_ready() const override; void send_banner() override; - MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; + MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet, const mavlink_message_t &msg) override; void send_attitude_target() override; void send_position_target_global_int() override; void send_position_target_local_ned() override; MAV_RESULT handle_command_do_set_roi(const Location &roi_loc) override; - MAV_RESULT handle_preflight_reboot(const mavlink_command_int_t &packet, const mavlink_message_t &msg) 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_int_t &packet, const mavlink_message_t &msg) override; + MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet) override; #endif - 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_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_do_reposition(const mavlink_command_int_t &packet); MAV_RESULT handle_command_pause_continue(const mavlink_command_int_t &packet); @@ -60,13 +55,6 @@ 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; @@ -96,23 +84,4 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK uint8_t high_latency_wind_speed() const override; uint8_t high_latency_wind_direction() const override; #endif // HAL_HIGH_LATENCY2_ENABLED - - - MAV_RESULT handle_MAV_CMD_CONDITION_YAW(const mavlink_command_int_t &packet); - MAV_RESULT handle_MAV_CMD_DO_CHANGE_SPEED(const mavlink_command_int_t &packet); - MAV_RESULT handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command_int_t &packet); - MAV_RESULT handle_MAV_CMD_DO_PARACHUTE(const mavlink_command_int_t &packet); - -#if AC_MAVLINK_SOLO_BUTTON_COMMAND_HANDLING_ENABLED - MAV_RESULT handle_MAV_CMD_SOLO_BTN_FLY_CLICK(const mavlink_command_int_t &packet); - MAV_RESULT handle_MAV_CMD_SOLO_BTN_FLY_HOLD(const mavlink_command_int_t &packet); - MAV_RESULT handle_MAV_CMD_SOLO_BTN_PAUSE_CLICK(const mavlink_command_int_t &packet); -#endif - - MAV_RESULT handle_MAV_CMD_MISSION_START(const mavlink_command_int_t &packet); - -#if AP_WINCH_ENABLED - MAV_RESULT handle_MAV_CMD_DO_WINCH(const mavlink_command_int_t &packet); -#endif - }; diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 334c4b02934414..ba352f2329edd8 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,7:Brake or Land + // @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 // @User: Standard GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_DISABLED), @@ -449,11 +449,9 @@ 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_ @@ -666,7 +664,7 @@ const AP_Param::Info Copter::var_info[] = { GOBJECT(optflow, "FLOW", AP_OpticalFlow), #endif -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED // @Group: PLND_ // @Path: ../libraries/AC_PrecLand/AC_PrecLand.cpp GOBJECT(precland, "PLND_", AC_PrecLand), @@ -778,7 +776,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Advanced AP_GROUPINFO("DEV_OPTIONS", 7, ParametersG2, dev_options, 0), -#if AP_BEACON_ENABLED +#if BEACON_ENABLED == ENABLED // @Group: BCN // @Path: ../libraries/AP_Beacon/AP_Beacon.cpp AP_SUBGROUPINFO(beacon, "BCN", 14, ParametersG2, AP_Beacon), @@ -1040,7 +1038,6 @@ 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 @@ -1211,22 +1208,6 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = { // @User: Advanced AP_GROUPINFO("SURFTRAK_TC", 5, ParametersG2, surftrak_tc, 1.0), - // @Param: TKOFF_THR_MAX - // @DisplayName: Takeoff maximum throttle during take-off ramp up - // @Description: Takeoff maximum throttle allowed before controllers assume the aircraft is airborne during the takeoff process. - // @Range: 0.0 0.9 - // @User: Advanced - AP_GROUPINFO("TKOFF_THR_MAX", 6, ParametersG2, takeoff_throttle_max, 0.9), - -#if HAL_WITH_ESC_TELEM && FRAME_CONFIG != HELI_FRAME - // @Param: TKOFF_RPM_MAX - // @DisplayName: Takeoff Check RPM maximum - // @Description: Takeoff is not permitted until motors report no more than this RPM. Set to zero to disable check - // @Range: 0 10000 - // @User: Standard - AP_GROUPINFO("TKOFF_RPM_MAX", 7, ParametersG2, takeoff_rpm_max, 0), -#endif - // ID 62 is reserved for the AP_SUBGROUPEXTENSION AP_GROUPEND @@ -1237,7 +1218,7 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = { */ ParametersG2::ParametersG2(void) : temp_calibration() // this doesn't actually need constructing, but removing it here is problematic syntax-wise -#if AP_BEACON_ENABLED +#if BEACON_ENABLED == ENABLED , beacon() #endif #if HAL_PROXIMITY_ENABLED @@ -1306,6 +1287,12 @@ ParametersG2::ParametersG2(void) old object. This should be zero for top level parameters. */ const AP_Param::ConversionInfo conversion_table[] = { + // PARAMETER_CONVERSION - Added: Oct-2014 + { Parameters::k_param_log_bitmask_old, 0, AP_PARAM_INT16, "LOG_BITMASK" }, + // PARAMETER_CONVERSION - Added: Jan-2015 + { Parameters::k_param_serial0_baud, 0, AP_PARAM_INT16, "SERIAL0_BAUD" }, + { Parameters::k_param_serial1_baud, 0, AP_PARAM_INT16, "SERIAL1_BAUD" }, + { Parameters::k_param_serial2_baud, 0, AP_PARAM_INT16, "SERIAL2_BAUD" }, // PARAMETER_CONVERSION - Added: Jan-2017 { Parameters::k_param_arming_check_old, 0, AP_PARAM_INT8, "ARMING_CHECK" }, // battery @@ -1376,7 +1363,46 @@ void Copter::load_parameters(void) // handle conversion of PID gains void Copter::convert_pid_parameters(void) { + // conversion info + const AP_Param::ConversionInfo pid_conversion_info[] = { + // PARAMETER_CONVERSION - Added: Apr-2016 + { Parameters::k_param_pid_rate_roll, 0, AP_PARAM_FLOAT, "ATC_RAT_RLL_P" }, + { Parameters::k_param_pid_rate_roll, 1, AP_PARAM_FLOAT, "ATC_RAT_RLL_I" }, + { Parameters::k_param_pid_rate_roll, 2, AP_PARAM_FLOAT, "ATC_RAT_RLL_D" }, + { Parameters::k_param_pid_rate_pitch, 0, AP_PARAM_FLOAT, "ATC_RAT_PIT_P" }, + { Parameters::k_param_pid_rate_pitch, 1, AP_PARAM_FLOAT, "ATC_RAT_PIT_I" }, + { Parameters::k_param_pid_rate_pitch, 2, AP_PARAM_FLOAT, "ATC_RAT_PIT_D" }, + { Parameters::k_param_pid_rate_yaw, 0, AP_PARAM_FLOAT, "ATC_RAT_YAW_P" }, + { Parameters::k_param_pid_rate_yaw, 1, AP_PARAM_FLOAT, "ATC_RAT_YAW_I" }, + { Parameters::k_param_pid_rate_yaw, 2, AP_PARAM_FLOAT, "ATC_RAT_YAW_D" }, +#if FRAME_CONFIG == HELI_FRAME + // PARAMETER_CONVERSION - Added: May-2016 + { Parameters::k_param_pid_rate_roll, 4, AP_PARAM_FLOAT, "ATC_RAT_RLL_VFF" }, + { Parameters::k_param_pid_rate_pitch, 4, AP_PARAM_FLOAT, "ATC_RAT_PIT_VFF" }, + { Parameters::k_param_pid_rate_yaw , 4, AP_PARAM_FLOAT, "ATC_RAT_YAW_VFF" }, +#endif + }; + const AP_Param::ConversionInfo imax_conversion_info[] = { + // PARAMETER_CONVERSION - Added: Apr-2016 + { Parameters::k_param_pid_rate_roll, 5, AP_PARAM_FLOAT, "ATC_RAT_RLL_IMAX" }, + { Parameters::k_param_pid_rate_pitch, 5, AP_PARAM_FLOAT, "ATC_RAT_PIT_IMAX" }, + { Parameters::k_param_pid_rate_yaw, 5, AP_PARAM_FLOAT, "ATC_RAT_YAW_IMAX" }, +#if FRAME_CONFIG == HELI_FRAME + // PARAMETER_CONVERSION - Added: May-2016 + { Parameters::k_param_pid_rate_roll, 7, AP_PARAM_FLOAT, "ATC_RAT_RLL_ILMI" }, + { Parameters::k_param_pid_rate_pitch, 7, AP_PARAM_FLOAT, "ATC_RAT_PIT_ILMI" }, + { Parameters::k_param_pid_rate_yaw, 7, AP_PARAM_FLOAT, "ATC_RAT_YAW_ILMI" }, +#endif + }; + // conversion from Copter-3.3 to Copter-3.4 const AP_Param::ConversionInfo angle_and_filt_conversion_info[] = { + // PARAMETER_CONVERSION - Added: May-2016 + { Parameters::k_param_p_stabilize_roll, 0, AP_PARAM_FLOAT, "ATC_ANG_RLL_P" }, + { Parameters::k_param_p_stabilize_pitch, 0, AP_PARAM_FLOAT, "ATC_ANG_PIT_P" }, + { Parameters::k_param_p_stabilize_yaw, 0, AP_PARAM_FLOAT, "ATC_ANG_YAW_P" }, + // PARAMETER_CONVERSION - Added: Apr-2016 + { Parameters::k_param_pid_rate_roll, 6, AP_PARAM_FLOAT, "ATC_RAT_RLL_FILT" }, + { Parameters::k_param_pid_rate_pitch, 6, AP_PARAM_FLOAT, "ATC_RAT_PIT_FILT" }, // PARAMETER_CONVERSION - Added: Jan-2018 { Parameters::k_param_pid_rate_yaw, 6, AP_PARAM_FLOAT, "ATC_RAT_YAW_FILT" }, { Parameters::k_param_pi_vel_xy, 0, AP_PARAM_FLOAT, "PSC_VELXY_P" }, @@ -1396,6 +1422,11 @@ void Copter::convert_pid_parameters(void) { Parameters::k_param_p_alt_hold, 0, AP_PARAM_FLOAT, "PSC_POSZ_P" }, { Parameters::k_param_p_pos_xy, 0, AP_PARAM_FLOAT, "PSC_POSXY_P" }, }; + const AP_Param::ConversionInfo throttle_conversion_info[] = { + // PARAMETER_CONVERSION - Added: Jun-2016 + { Parameters::k_param_throttle_min, 0, AP_PARAM_FLOAT, "MOT_SPIN_MIN" }, + { Parameters::k_param_throttle_mid, 0, AP_PARAM_FLOAT, "MOT_THST_HOVER" } + }; const AP_Param::ConversionInfo loiter_conversion_info[] = { // PARAMETER_CONVERSION - Added: Apr-2018 { Parameters::k_param_wp_nav, 4, AP_PARAM_FLOAT, "LOIT_SPEED" }, @@ -1404,10 +1435,34 @@ void Copter::convert_pid_parameters(void) { Parameters::k_param_wp_nav, 9, AP_PARAM_FLOAT, "LOIT_BRK_ACCEL" } }; + // PARAMETER_CONVERSION - Added: Apr-2016 + // gains increase by 27% due to attitude controller's switch to use radians instead of centi-degrees + // and motor libraries switch to accept inputs in -1 to +1 range instead of -4500 ~ +4500 + float pid_scaler = 1.27f; + +#if FRAME_CONFIG != HELI_FRAME + // Multicopter x-frame gains are 40% lower because -1 or +1 input to motors now results in maximum rotation + if (g.frame_type == AP_Motors::MOTOR_FRAME_TYPE_X || g.frame_type == AP_Motors::MOTOR_FRAME_TYPE_V || g.frame_type == AP_Motors::MOTOR_FRAME_TYPE_H) { + pid_scaler = 0.9f; + } +#endif + + // scale PID gains + for (const auto &info : pid_conversion_info) { + AP_Param::convert_old_parameter(&info, pid_scaler); + } + // reduce IMAX into -1 ~ +1 range + for (const auto &info : imax_conversion_info) { + AP_Param::convert_old_parameter(&info, 1.0f/4500.0f); + } // convert angle controller gain and filter without scaling for (const auto &info : angle_and_filt_conversion_info) { AP_Param::convert_old_parameter(&info, 1.0f); } + // convert throttle parameters (multicopter only) + for (const auto &info : throttle_conversion_info) { + AP_Param::convert_old_parameter(&info, 0.001f); + } // convert RC_FEEL_RP to ATC_INPUT_TC // PARAMETER_CONVERSION - Added: Mar-2018 const AP_Param::ConversionInfo rc_feel_rp_conversion_info = { Parameters::k_param_rc_feel_rp, 0, AP_PARAM_INT8, "ATC_INPUT_TC" }; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 18a32dd25fc184..dd6f6ddaa6da48 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -525,7 +525,7 @@ class ParametersG2 { // temperature calibration handling AP_TempCalibration temp_calibration; -#if AP_BEACON_ENABLED +#if BEACON_ENABLED == ENABLED // beacon (non-GPS positioning) library AP_Beacon beacon; #endif @@ -678,10 +678,8 @@ class ParametersG2 { // ramp time of throttle during take-off AP_Float takeoff_throttle_slew_time; - AP_Float takeoff_throttle_max; #if HAL_WITH_ESC_TELEM && FRAME_CONFIG != HELI_FRAME AP_Int16 takeoff_rpm_min; - AP_Int16 takeoff_rpm_max; #endif #if WEATHERVANE_ENABLED == ENABLED diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index f400694b451b38..afcd65754cad28 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -37,14 +37,9 @@ 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 (in_rc_failsafe()) { + if (copter.failsafe.radio) { return false; } if (copter.failsafe.radio_counter != 0) { @@ -396,7 +391,7 @@ bool RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi break; case AUX_FUNC::PRECISION_LOITER: -#if AC_PRECLAND_ENABLED && MODE_LOITER_ENABLED == ENABLED +#if PRECISION_LANDING == ENABLED && MODE_LOITER_ENABLED == ENABLED switch (ch_flag) { case AuxSwitchPos::HIGH: copter.mode_loiter.set_precision_loiter_enabled(true); diff --git a/ArduCopter/RC_Channel.h b/ArduCopter/RC_Channel.h index 6d9d9f9426801c..576135f91f438d 100644 --- a/ArduCopter/RC_Channel.h +++ b/ArduCopter/RC_Channel.h @@ -31,7 +31,6 @@ 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 b3c7b19c2e972e..5377f3f8586461 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -10,12 +10,8 @@ Changes from 4.4.1 - SpeedyBee-F405-Mini support - T-Motor H743 Mini support 2) EKF3 supports baroless boards -3) GPS-for-yaw allows base GPS to update at only 3Hz -4) INA battery monitor supports config of shunt resistor used (see BATTx_SHUNT) -5) Log VER message includes vehicle type -6) OpenDroneId option to auto-store IDs in persistent flash -7) RC SBUS protection against invalid data in first 4 channels -8) Bug fixes +3) INA battery monitor supports config of shunt resistor used (see BATTx_SHUNT) +4) Bug fixes - BMI088 IMU error value handling fixed to avoid occasional negative spike - Dev environment CI autotest stability improvements - OSD correct DisplayPort BF MSP symbols @@ -339,24 +335,6 @@ Changes from 4.3.6 - Webots 2023a simulator support - XPlane support for wider range of aircraft ------------------------------------------------------------------ -Copter 4.3.8 24-Aug-2023 / 4.3.8-beta1 12-Aug-2023 -Changes from 4.3.7 -1) Bug fixes - - DroneCAN GPS RTK injection fix - - INAxxx battery monitors allow for battery reset remaining - - Notch filter gyro glitch caused by race condition fixed - - Scripting restart memory corruption bug fixed ------------------------------------------------------------------- -Copter 4.3.7 31-May-2023 / 4.3.7-beta1 24-May-2023 -Changes from 4.3.6 -1) Bug fixes - a) EKF3 accel bias calculations bug fix - b) EKF3 accel bias process noise adjusted for greater robustness - c) GSF yaw numerical stability fix caused by compassmot - d) INS batch sampler fix to avoid watchdog if INS_LOG_BAT_CNT changed without rebooting - e) Memory corruption bug in the STM32H757 (very rare) - f) RC input on IOMCU bug fix (RC might not be regained if lost) ------------------------------------------------------------------- Copter 4.3.6 05-Apr-2023 / 4.3.6-beta1, 4.3.6--beta2 27-Mar-2023 Changes from 4.3.5 1) Bi-directional DShot fix for possible motor stop approx 72min after startup diff --git a/ArduCopter/afs_copter.cpp b/ArduCopter/afs_copter.cpp index 3bad987a343d3e..9f669eee8cd656 100644 --- a/ArduCopter/afs_copter.cpp +++ b/ArduCopter/afs_copter.cpp @@ -68,9 +68,4 @@ AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Copter::afs_mode(void) return AP_AdvancedFailsafe::AFS_STABILIZED; } -//to force entering auto mode when datalink loss - void AP_AdvancedFailsafe_Copter::set_mode_auto(void) - { - copter.set_mode(Mode::Number::AUTO,ModeReason::GCS_FAILSAFE); - } #endif // ADVANCED_FAILSAFE diff --git a/ArduCopter/afs_copter.h b/ArduCopter/afs_copter.h index b7c8bc8b21c854..fd733519733c38 100644 --- a/ArduCopter/afs_copter.h +++ b/ArduCopter/afs_copter.h @@ -39,9 +39,6 @@ class AP_AdvancedFailsafe_Copter : public AP_AdvancedFailsafe // return the AFS mapped control mode enum control_mode afs_mode(void) override; - - //to force entering auto mode when datalink loss - void set_mode_auto(void) override; }; #endif // ADVANCED_FAILSAFE diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp index 3254dfde78946c..6853789ee3b843 100644 --- a/ArduCopter/autoyaw.cpp +++ b/ArduCopter/autoyaw.cpp @@ -92,7 +92,7 @@ void Mode::AutoYaw::set_mode(Mode yaw_mode) case Mode::RATE: // initialise target yaw rate to zero - _yaw_rate_cds = 0.0; + _yaw_rate_cds = 0.0f; break; case Mode::CIRCLE: @@ -108,6 +108,9 @@ void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_ds, int8_t di { _last_update_ms = millis(); + _yaw_angle_cd = copter.attitude_control->get_att_target_euler_cd().z; + _yaw_rate_cds = 0.0; + // calculate final angle as relative to vehicle heading or absolute if (relative_angle) { _fixed_yaw_offset_cd = angle_deg * 100.0 * (direction >= 0 ? 1.0 : -1.0); @@ -208,15 +211,14 @@ bool Mode::AutoYaw::reached_fixed_yaw_target() return (fabsf(wrap_180_cd(copter.ahrs.yaw_sensor-_yaw_angle_cd)) <= 200); } -// yaw_cd - returns target heading depending upon auto_yaw.mode() -float Mode::AutoYaw::yaw_cd() +// yaw - returns target heading depending upon auto_yaw.mode() +float Mode::AutoYaw::yaw() { switch (_mode) { case Mode::ROI: // point towards a location held in roi - _yaw_angle_cd = roi_yaw(); - break; + return roi_yaw(); case Mode::FIXED: { // keep heading pointing in the direction held in fixed_yaw @@ -227,55 +229,45 @@ float Mode::AutoYaw::yaw_cd() float yaw_angle_step = constrain_float(_fixed_yaw_offset_cd, - dt * _fixed_yaw_slewrate_cds, dt * _fixed_yaw_slewrate_cds); _fixed_yaw_offset_cd -= yaw_angle_step; _yaw_angle_cd += yaw_angle_step; - break; + return _yaw_angle_cd; } case Mode::LOOK_AHEAD: // Commanded Yaw to automatically look ahead. - _yaw_angle_cd = look_ahead_yaw(); - break; + return look_ahead_yaw(); case Mode::RESETTOARMEDYAW: // changes yaw to be same as when quad was armed - _yaw_angle_cd = copter.initial_armed_bearing; - break; + return copter.initial_armed_bearing; case Mode::CIRCLE: #if MODE_CIRCLE_ENABLED if (copter.circle_nav->is_active()) { - _yaw_angle_cd = copter.circle_nav->get_yaw(); + return copter.circle_nav->get_yaw(); } #endif - break; + // return the current attitude target + return wrap_360_cd(copter.attitude_control->get_att_target_euler_cd().z); case Mode::ANGLE_RATE:{ const uint32_t now_ms = millis(); float dt = (now_ms - _last_update_ms) * 0.001; _last_update_ms = now_ms; _yaw_angle_cd += _yaw_rate_cds * dt; - break; + return _yaw_angle_cd; } - case Mode::RATE: - case Mode::WEATHERVANE: - case Mode::PILOT_RATE: - _yaw_angle_cd = copter.attitude_control->get_att_target_euler_cd().z; - break; - case Mode::LOOK_AT_NEXT_WP: default: // point towards next waypoint. // we don't use wp_bearing because we don't want the copter to turn too much during flight - _yaw_angle_cd = copter.pos_control->get_yaw_cd(); - break; + return copter.pos_control->get_yaw_cd(); } - - return _yaw_angle_cd; } // returns yaw rate normally set by SET_POSITION_TARGET mavlink // messages (positive is clockwise, negative is counter clockwise) -float Mode::AutoYaw::rate_cds() +float Mode::AutoYaw::rate_cds() const { switch (_mode) { @@ -285,25 +277,22 @@ float Mode::AutoYaw::rate_cds() case Mode::LOOK_AHEAD: case Mode::RESETTOARMEDYAW: case Mode::CIRCLE: - _yaw_rate_cds = 0.0f; - break; - - case Mode::LOOK_AT_NEXT_WP: - _yaw_rate_cds = copter.pos_control->get_yaw_rate_cds(); - break; - - case Mode::PILOT_RATE: - _yaw_rate_cds = _pilot_yaw_rate_cds; - break; + return 0.0f; case Mode::ANGLE_RATE: case Mode::RATE: case Mode::WEATHERVANE: - break; + return _yaw_rate_cds; + + case Mode::LOOK_AT_NEXT_WP: + return copter.pos_control->get_yaw_rate_cds(); + + case Mode::PILOT_RATE: + return _pilot_yaw_rate_cds; } // return zero turn rate (this should never happen) - return _yaw_rate_cds; + return 0.0f; } AC_AttitudeControl::HeadingCommand Mode::AutoYaw::get_heading() @@ -326,7 +315,7 @@ AC_AttitudeControl::HeadingCommand Mode::AutoYaw::get_heading() #endif AC_AttitudeControl::HeadingCommand heading; - heading.yaw_angle_cd = auto_yaw.yaw_cd(); + heading.yaw_angle_cd = yaw(); heading.yaw_rate_cds = auto_yaw.rate_cds(); switch (auto_yaw.mode()) { diff --git a/ArduCopter/compassmot.cpp b/ArduCopter/compassmot.cpp index 532cbb243a1811..5e24bd2b407d9c 100644 --- a/ArduCopter/compassmot.cpp +++ b/ArduCopter/compassmot.cpp @@ -123,7 +123,7 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan) EXPECT_DELAY_MS(5000); // enable motors and pass through throttle - motors->output_min(); // output lowest possible value to motors + enable_motor_output(); motors->armed(true); hal.util->set_soft_armed(true); diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 7a7b6900c2fa7d..5a4704b14915d1 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -27,7 +27,6 @@ /// change in your local copy of APM_Config.h. /// #include "APM_Config.h" -#include ////////////////////////////////////////////////////////////////////////////// @@ -139,6 +138,12 @@ # define AUTOTUNE_ENABLED ENABLED #endif +////////////////////////////////////////////////////////////////////////////// +// Precision Landing with companion computer or IRLock sensor +#ifndef PRECISION_LANDING + # define PRECISION_LANDING ENABLED +#endif + ////////////////////////////////////////////////////////////////////////////// // Parachute release #ifndef PARACHUTE @@ -147,8 +152,8 @@ ////////////////////////////////////////////////////////////////////////////// // Nav-Guided - allows external nav computer to control vehicle -#ifndef AC_NAV_GUIDED - # define AC_NAV_GUIDED ENABLED +#ifndef NAV_GUIDED + # define NAV_GUIDED !HAL_MINIMIZE_FEATURES #endif ////////////////////////////////////////////////////////////////////////////// @@ -190,7 +195,7 @@ ////////////////////////////////////////////////////////////////////////////// // Follow - follow another vehicle or GCS #ifndef MODE_FOLLOW_ENABLED -# define MODE_FOLLOW_ENABLED AP_FOLLOW_ENABLED +# define MODE_FOLLOW_ENABLED !HAL_MINIMIZE_FEATURES #endif ////////////////////////////////////////////////////////////////////////////// @@ -202,7 +207,7 @@ ////////////////////////////////////////////////////////////////////////////// // GuidedNoGPS mode - control vehicle's angles from GCS #ifndef MODE_GUIDED_NOGPS_ENABLED -# define MODE_GUIDED_NOGPS_ENABLED ENABLED +# define MODE_GUIDED_NOGPS_ENABLED !HAL_MINIMIZE_FEATURES #endif ////////////////////////////////////////////////////////////////////////////// @@ -238,7 +243,7 @@ ////////////////////////////////////////////////////////////////////////////// // System ID - conduct system identification tests on vehicle #ifndef MODE_SYSTEMID_ENABLED -# define MODE_SYSTEMID_ENABLED ENABLED +# define MODE_SYSTEMID_ENABLED !HAL_MINIMIZE_FEATURES #endif ////////////////////////////////////////////////////////////////////////////// @@ -250,19 +255,19 @@ ////////////////////////////////////////////////////////////////////////////// // ZigZag - allow vehicle to fly in a zigzag manner with predefined point A B #ifndef MODE_ZIGZAG_ENABLED -# define MODE_ZIGZAG_ENABLED ENABLED +# define MODE_ZIGZAG_ENABLED !HAL_MINIMIZE_FEATURES #endif ////////////////////////////////////////////////////////////////////////////// // Turtle - allow vehicle to be flipped over after a crash #ifndef MODE_TURTLE_ENABLED -# define MODE_TURTLE_ENABLED HAL_DSHOT_ENABLED && FRAME_CONFIG != HELI_FRAME +# define MODE_TURTLE_ENABLED !HAL_MINIMIZE_FEATURES && !defined(DISABLE_DSHOT) && FRAME_CONFIG != HELI_FRAME #endif ////////////////////////////////////////////////////////////////////////////// // Flowhold - use optical flow to hover in place #ifndef MODE_FLOWHOLD_ENABLED -# define MODE_FLOWHOLD_ENABLED AP_OPTICALFLOW_ENABLED +# define MODE_FLOWHOLD_ENABLED !HAL_MINIMIZE_FEATURES && AP_OPTICALFLOW_ENABLED #endif ////////////////////////////////////////////////////////////////////////////// @@ -270,18 +275,17 @@ ////////////////////////////////////////////////////////////////////////////// // Weathervane - allow vehicle to yaw into wind #ifndef WEATHERVANE_ENABLED -# define WEATHERVANE_ENABLED ENABLED +# define WEATHERVANE_ENABLED !HAL_MINIMIZE_FEATURES #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 ENABLED + # define MODE_AUTOROTATE_ENABLED !HAL_MINIMIZE_FEATURES #endif #else # define MODE_AUTOROTATE_ENABLED DISABLED @@ -289,6 +293,12 @@ #else # define MODE_AUTOROTATE_ENABLED DISABLED #endif + +////////////////////////////////////////////////////////////////////////////// + +// Beacon support - support for local positioning systems +#ifndef BEACON_ENABLED +# define BEACON_ENABLED !HAL_MINIMIZE_FEATURES #endif ////////////////////////////////////////////////////////////////////////////// @@ -564,7 +574,7 @@ #endif #ifndef AC_OAPATHPLANNER_ENABLED - #define AC_OAPATHPLANNER_ENABLED ENABLED + #define AC_OAPATHPLANNER_ENABLED !HAL_MINIMIZE_FEATURES #endif #if MODE_FOLLOW_ENABLED && !AC_AVOID_ENABLED diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index 786e56b5a0e522..18a04aec4fdf8d 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -101,7 +101,7 @@ void Copter::thrust_loss_check() { static uint16_t thrust_loss_counter; // number of iterations vehicle may have been crashed - // no-op if suppressed by flight options param + // no-op if suppresed by flight options param if ((copter.g2.flight_options & uint32_t(FlightOptions::DISABLE_THRUST_LOSS_CHECK)) != 0) { return; } @@ -181,7 +181,7 @@ void Copter::thrust_loss_check() // check for a large yaw imbalance, could be due to badly calibrated ESC or misaligned motors void Copter::yaw_imbalance_check() { - // no-op if suppressed by flight options param + // no-op if suppresed by flight options param if ((copter.g2.flight_options & uint32_t(FlightOptions::DISABLE_YAW_IMBALANCE_WARNING)) != 0) { return; } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index d5c55be60fea14..b3cd317f76e68a 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -151,7 +151,6 @@ 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 94077c5088040c..69e4edb3b11735 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -187,9 +187,6 @@ 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 888bb87025fe5e..61b1cbb7561b11 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -194,52 +194,6 @@ 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 @@ -264,11 +218,6 @@ 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 @@ -645,7 +594,7 @@ void Mode::land_run_vertical_control(bool pause_descent) // Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed. cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed)); -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED const bool navigating = pos_control->is_active_xy(); bool doing_precision_landing = !copter.ap.land_repo_active && copter.precland.target_acquired() && navigating; @@ -666,7 +615,7 @@ void Mode::land_run_vertical_control(bool pause_descent) // doing precland but too far away from the obstacle // do not descend cmb_rate = 0.0f; - } else if (target_pos_meas.z > 35.0f && target_pos_meas.z < 200.0f && !copter.precland.do_fast_descend()) { + } else if (target_pos_meas.z > 35.0f && target_pos_meas.z < 200.0f) { // very close to the ground and doing prec land, lets slow down to make sure we land on target // compute desired descent velocity const float precland_acceptable_error_cm = 15.0f; @@ -719,7 +668,7 @@ void Mode::land_run_horizontal_control() AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE); } copter.ap.land_repo_active = true; -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED } else { // no override right now, check if we should allow precland if (copter.precland.allow_precland_after_reposition()) { @@ -732,7 +681,7 @@ void Mode::land_run_horizontal_control() // this variable will be updated if prec land target is in sight and pilot isn't trying to reposition the vehicle copter.ap.prec_land_active = false; -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED copter.ap.prec_land_active = !copter.ap.land_repo_active && copter.precland.target_acquired(); // run precision landing if (copter.ap.prec_land_active) { @@ -789,7 +738,7 @@ void Mode::land_run_horizontal_control() // pause_descent is true if vehicle should not descend void Mode::land_run_normal_or_precland(bool pause_descent) { -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED if (pause_descent || !copter.precland.enabled()) { // we don't want to start descending immediately or prec land is disabled // in both cases just run simple land controllers @@ -804,7 +753,7 @@ void Mode::land_run_normal_or_precland(bool pause_descent) #endif } -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED // Go towards a position commanded by prec land state machine in order to retry landing // The passed in location is expected to be NED and in m void Mode::precland_retry_position(const Vector3f &retry_pos) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 28c78411797a85..feb2e1095285cb 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -2,7 +2,6 @@ #include "Copter.h" #include -#include // TODO why is this needed if Copter.h includes this class Parameters; class ParametersG2; @@ -148,7 +147,7 @@ class Mode { // pause_descent is true if vehicle should not descend void land_run_normal_or_precland(bool pause_descent = false); -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED // Go towards a position commanded by prec land state machine in order to retry landing // The passed in location is expected to be NED and in meters void precland_retry_position(const Vector3f &retry_pos); @@ -258,6 +257,7 @@ class Mode { void set_mode(Mode new_mode); Mode default_mode(bool rtl) const; + void set_rate(float new_rate_cds); // set_roi(...): set a "look at" location: @@ -280,11 +280,11 @@ class Mode { private: - // yaw_cd(): main product of AutoYaw; the heading: - float yaw_cd(); + // yaw(): main product of AutoYaw; the heading: + float yaw(); // rate_cds(): desired yaw rate in centidegrees/second: - float rate_cds(); + float rate_cds() const; float look_ahead_yaw(); float roi_yaw() const; @@ -463,7 +463,6 @@ class ModeAuto : public Mode { // pause continue in auto mode bool pause() override; bool resume() override; - bool paused() const; bool loiter_start(); void rtl_start(); @@ -485,6 +484,10 @@ 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: @@ -567,7 +570,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 AC_NAV_GUIDED == ENABLED +#if 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 @@ -605,7 +608,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 AC_NAV_GUIDED == ENABLED +#if 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); @@ -958,10 +961,6 @@ 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; } @@ -1183,7 +1182,7 @@ class ModeLoiter : public Mode { bool has_user_takeoff(bool must_navigate) const override { return true; } bool allows_autotune() const override { return true; } -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED void set_precision_loiter_enabled(bool value) { _precision_loiter_enabled = value; } #endif @@ -1196,14 +1195,14 @@ class ModeLoiter : public Mode { int32_t wp_bearing() const override; float crosstrack_error() const override { return pos_control->crosstrack_error();} -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED bool do_precision_loiter(); void precision_loiter_xy(); #endif private: -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED bool _precision_loiter_enabled; bool _precision_loiter_active; // true if user has switched on prec loiter #endif @@ -1723,7 +1722,6 @@ class ModeAvoidADSB : public ModeGuided { }; -#if AP_FOLLOW_ENABLED class ModeFollow : public ModeGuided { public: @@ -1753,7 +1751,6 @@ class ModeFollow : public ModeGuided { uint32_t last_log_ms; // system time of last time desired velocity was logging }; -#endif class ModeZigZag : public Mode { @@ -1803,9 +1800,6 @@ class ModeZigZag : public Mode { const char *name() const override { return "ZIGZAG"; } const char *name4() const override { return "ZIGZ"; } - uint32_t wp_distance() const override; - int32_t wp_bearing() const override; - float crosstrack_error() const override; private: diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index c1972dbf000ed0..b6be6d5cefb169 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -53,7 +53,7 @@ bool ModeAuto::init(bool ignore_checks) // reset flag indicating if pilot has applied roll or pitch inputs during landing copter.ap.land_repo_active = false; -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED // initialise precland state machine copter.precland_statemachine.init(); #endif @@ -136,7 +136,7 @@ void ModeAuto::run() case SubMode::NAVGUIDED: case SubMode::NAV_SCRIPT_TIME: -#if AC_NAV_GUIDED == ENABLED || AP_SCRIPTING_ENABLED +#if NAV_GUIDED == ENABLED || AP_SCRIPTING_ENABLED nav_guided_run(); #endif break; @@ -502,7 +502,7 @@ void ModeAuto::circle_start() set_submode(SubMode::CIRCLE); } -#if AC_NAV_GUIDED == ENABLED +#if 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 //AC_NAV_GUIDED +#endif //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 AC_NAV_GUIDED == ENABLED +#if 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 AC_NAV_GUIDED == ENABLED +#if 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 AC_NAV_GUIDED == ENABLED +#if 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 AC_NAV_GUIDED == ENABLED || AP_SCRIPTING_ENABLED +#if 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 // AC_NAV_GUIDED || AP_SCRIPTING_ENABLED +#endif // 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 AC_NAV_GUIDED == ENABLED +#if 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 // AC_NAV_GUIDED +#endif // NAV_GUIDED // do_nav_delay - Delay the next navigation command void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd) @@ -1782,11 +1782,7 @@ void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd) nav_delay_time_max_ms = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds } else { // absolute delay to utc time -#if AP_RTC_ENABLED nav_delay_time_max_ms = AP::rtc().get_time_utc(cmd.content.nav_delay.hour_utc, cmd.content.nav_delay.min_utc, cmd.content.nav_delay.sec_utc, 0); -#else - nav_delay_time_max_ms = 0; -#endif } gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec", (unsigned)(nav_delay_time_max_ms/1000)); } @@ -1896,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() != AP_Mount::Type::None) && + if ((copter.camera_mount.get_mount_type() != copter.camera_mount.MountType::Mount_Type_None) && !copter.camera_mount.has_pan_control()) { auto_yaw.set_yaw_angle_rate(cmd.content.mount_control.yaw,0.0f); } @@ -2192,7 +2188,7 @@ bool ModeAuto::verify_spline_wp(const AP_Mission::Mission_Command& cmd) return false; } -#if AC_NAV_GUIDED == ENABLED +#if 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) { @@ -2204,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 // AC_NAV_GUIDED +#endif // NAV_GUIDED // verify_nav_delay - check if we have waited long enough bool ModeAuto::verify_nav_delay(const AP_Mission::Mission_Command& cmd) @@ -2255,9 +2251,4 @@ bool ModeAuto::resume() return true; } -bool ModeAuto::paused() const -{ - return wp_nav->paused(); -} - #endif diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index 414401c2e9a571..fb009a1d3421d7 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -47,7 +47,7 @@ bool ModeAutorotate::init(bool ignore_checks) // Display message gcs().send_text(MAV_SEVERITY_INFO, "Autorotation initiated"); - // Set all intial flags to on + // Set all inial flags to on _flags.entry_initial = 1; _flags.ss_glide_initial = 1; _flags.flare_initial = 1; @@ -173,7 +173,7 @@ void ModeAutorotate::run() g2.arot.set_desired_fwd_speed(); // Set target head speed in head speed controller - _target_head_speed = HEAD_SPEED_TARGET_RATIO; //Ensure target hs is set to glide in case hs has not reached target for glide + _target_head_speed = HEAD_SPEED_TARGET_RATIO; //Ensure target hs is set to glide in case hs hasent reached target for glide g2.arot.set_target_head_speed(_target_head_speed); // Prevent running the initial glide functions again diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 65a353ebc78296..55d71f1c63f484 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -61,7 +61,6 @@ 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); @@ -69,16 +68,6 @@ 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 bb9a6612f948c5..c86de8856df188 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -1,5 +1,4 @@ #include "Copter.h" -#include #if MODE_CIRCLE_ENABLED == ENABLED @@ -21,22 +20,6 @@ bool ModeCircle::init(bool ignore_checks) // initialise circle controller including setting the circle center based on vehicle speed copter.circle_nav->init(); -#if HAL_MOUNT_ENABLED - AP_Mount *s = AP_Mount::get_singleton(); - - // Check if the CIRCLE_OPTIONS parameter have roi_at_center - if (copter.circle_nav->roi_at_center()) { - const Vector3p &pos { copter.circle_nav->get_center() }; - Location circle_center; - if (!AP::ahrs().get_location_from_origin_offset(circle_center, pos * 0.01)) { - return false; - } - // point at the ground: - circle_center.set_alt_cm(0, Location::AltFrame::ABOVE_TERRAIN); - s->set_roi_target(circle_center); - } -#endif - // set auto yaw circle mode auto_yaw.set_mode(AutoYaw::Mode::CIRCLE); diff --git a/ArduCopter/mode_follow.cpp b/ArduCopter/mode_follow.cpp index a0649c5956c24f..afa348503b084c 100644 --- a/ArduCopter/mode_follow.cpp +++ b/ArduCopter/mode_follow.cpp @@ -20,15 +20,6 @@ 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 4d0f9e3154cfb2..fd313a1c0d0831 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 (AP_Arming::method_is_GCS(method) || method == AP_Arming::Method::SCRIPTING) { + if (method == AP_Arming::Method::MAVLINK || method == AP_Arming::Method::SCRIPTING) { return true; } @@ -1087,6 +1087,7 @@ 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; } @@ -1101,6 +1102,7 @@ int32_t ModeGuided::wp_bearing() const return get_bearing_cd(inertial_nav.get_position_xy_cm(), guided_pos_target_cm.tofloat().xy()); case SubMode::PosVelAccel: return pos_control->get_bearing_to_target_cd(); + break; case SubMode::TakeOff: case SubMode::Accel: case SubMode::VelAccel: diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 377bef5ee9253b..4072e76c8b3a10 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -46,7 +46,7 @@ bool ModeLand::init(bool ignore_checks) copter.fence.auto_disable_fence_for_landing(); #endif -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED // initialise precland state machine copter.precland_statemachine.init(); #endif diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 602aa52859a151..0198db443e5b4d 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -34,14 +34,14 @@ bool ModeLoiter::init(bool ignore_checks) pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); pos_control->set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED _precision_loiter_active = false; #endif return true; } -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED bool ModeLoiter::do_precision_loiter() { if (!_precision_loiter_enabled) { @@ -165,7 +165,7 @@ void ModeLoiter::run() // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED bool precision_loiter_old_state = _precision_loiter_active; if (do_precision_loiter()) { precision_loiter_xy(); diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 795e483e93423a..baa806768b883b 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -28,7 +28,7 @@ bool ModeRTL::init(bool ignore_checks) // this will be set true if prec land is later active copter.ap.prec_land_active = false; -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED // initialise precland state machine copter.precland_statemachine.init(); #endif diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp index 15dc1eba4f0ede..1a942780b8dfc0 100644 --- a/ArduCopter/mode_systemid.cpp +++ b/ArduCopter/mode_systemid.cpp @@ -107,7 +107,7 @@ bool ModeSystemId::init(bool ignore_checks) // systemId_exit - clean up systemId controller before exiting void ModeSystemId::exit() { - // reset the feedforward enabled parameter to the initialized state + // reset the feedfoward enabled parameter to the initialized state attitude_control->bf_feedforward(att_bf_feedforward); } @@ -261,7 +261,11 @@ void ModeSystemId::run() attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); // output pilot's throttle - attitude_control->set_throttle_out(pilot_throttle_scaled, !copter.is_tradheli(), g.throttle_filt); + if (copter.is_tradheli()) { + attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt); + } else { + attitude_control->set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt); + } if (log_subsample <= 0) { log_data(); diff --git a/ArduCopter/mode_turtle.cpp b/ArduCopter/mode_turtle.cpp index 74dc49421ecff6..99c5d2b08d73c1 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->thr_lin.thrust_to_actuator(flip_power) : 0.0f; + motors_output = !is_zero(flip_power) ? motors->thrust_to_actuator(flip_power) : 0.0f; } // actually write values to the motors diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index 2952158ec615a6..ff186ff65ee777 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -583,17 +583,4 @@ void ModeZigZag::spray(bool b) #endif } -uint32_t ModeZigZag::wp_distance() const -{ - return is_auto ? wp_nav->get_wp_distance_to_destination() : 0; -} -int32_t ModeZigZag::wp_bearing() const -{ - return is_auto ? wp_nav->get_wp_bearing_to_destination() : 0; -} -float ModeZigZag::crosstrack_error() const -{ - return is_auto ? wp_nav->crosstrack_error() : 0; -} - #endif // MODE_ZIGZAG_ENABLED == ENABLED diff --git a/ArduCopter/motor_test.cpp b/ArduCopter/motor_test.cpp index 5c5deae8995cd7..13114e1744d27c 100644 --- a/ArduCopter/motor_test.cpp +++ b/ArduCopter/motor_test.cpp @@ -101,13 +101,6 @@ 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); @@ -160,7 +153,7 @@ MAV_RESULT Copter::mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t EXPECT_DELAY_MS(3000); // enable and arm motors if (!motors->armed()) { - motors->output_min(); // output lowest possible value to motors + enable_motor_output(); motors->armed(true); hal.util->set_soft_armed(true); } diff --git a/ArduCopter/precision_landing.cpp b/ArduCopter/precision_landing.cpp index ad98c2fa75552d..076177667806bd 100644 --- a/ArduCopter/precision_landing.cpp +++ b/ArduCopter/precision_landing.cpp @@ -4,7 +4,7 @@ #include "Copter.h" -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED void Copter::init_precland() { diff --git a/ArduCopter/radio.cpp b/ArduCopter/radio.cpp index 57043fba918396..0de13687d157ea 100644 --- a/ArduCopter/radio.cpp +++ b/ArduCopter/radio.cpp @@ -77,6 +77,13 @@ void Copter::init_rc_out() } +// enable_motor_output() - enable and output lowest possible value to motors +void Copter::enable_motor_output() +{ + // enable motors + motors->output_min(); +} + void Copter::read_radio() { const uint32_t tnow_ms = millis(); diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 589561b8cfe406..fd103ee21895f8 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -6,6 +6,8 @@ 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 221a92bb9a2700..dae7a40c28811b 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -92,9 +92,7 @@ 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 @@ -134,7 +132,7 @@ void Copter::init_ardupilot() camera.init(); #endif -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED // initialise precision landing init_precland(); #endif @@ -153,17 +151,15 @@ void Copter::init_ardupilot() barometer.set_log_baro_bit(MASK_LOG_IMU); barometer.calibrate(); -#if RANGEFINDER_ENABLED == ENABLED // initialise rangefinder init_rangefinder(); -#endif #if HAL_PROXIMITY_ENABLED // init proximity sensor g2.proximity.init(); #endif -#if AP_BEACON_ENABLED +#if BEACON_ENABLED == ENABLED // init beacons used for non-gps position estimation g2.beacon.init(); #endif @@ -200,12 +196,20 @@ void Copter::init_ardupilot() set_land_complete(true); set_land_complete_maybe(true); + // we don't want writes to the serial port to cause us to pause + // mid-flight, so set the serial ports non-blocking once we are + // ready to fly + serial_manager.set_blocking_writes_all(false); + // enable CPU failsafe failsafe_enable(); ins.set_log_raw_bit(MASK_LOG_IMU_RAW); - motors->output_min(); // output lowest possible value to motors + // enable output to motors + if (arming.rc_calibration_checks(true)) { + enable_motor_output(); + } // attempt to set the intial_mode, else set to STABILIZE if (!set_mode((enum Mode::Number)g.initial_mode.get(), ModeReason::INITIALISED)) { diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 790936ac6bb946..faa4adfee56609 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -90,7 +90,7 @@ void Mode::_TakeOff::do_pilot_takeoff(float& pilot_climb_rate_cm) copter.attitude_control->set_throttle_out(throttle, true, 0.0); // tell position controller to reset alt target and reset I terms copter.pos_control->init_z_controller(); - if (throttle >= MIN(copter.g2.takeoff_throttle_max, 0.9) || + if (throttle >= 0.9 || (copter.pos_control->get_z_accel_cmss() >= 0.5 * copter.pos_control->get_max_accel_z_cmss()) || (copter.pos_control->get_vel_desired_cms().z >= constrain_float(pilot_climb_rate_cm, copter.pos_control->get_max_speed_up_cms() * 0.1, copter.pos_control->get_max_speed_up_cms() * 0.5)) || (is_positive(take_off_complete_alt - take_off_start_alt) && copter.pos_control->get_pos_target_z_cm() - take_off_start_alt > 0.5 * (take_off_complete_alt - take_off_start_alt))) { @@ -166,7 +166,7 @@ void Mode::auto_takeoff_run() pos_control->update_xy_controller(); attitude_control->reset_rate_controller_I_terms(); attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0); - if (throttle >= MIN(copter.g2.takeoff_throttle_max, 0.9) || + if (throttle >= 0.9 || (copter.pos_control->get_z_accel_cmss() >= 0.5 * copter.pos_control->get_max_accel_z_cmss()) || (copter.pos_control->get_vel_desired_cms().z >= 0.1 * copter.pos_control->get_max_speed_up_cms()) || ( auto_takeoff_no_nav_active && (inertial_nav.get_position_z_up_cm() >= auto_takeoff_no_nav_alt_cm))) { diff --git a/ArduCopter/takeoff_check.cpp b/ArduCopter/takeoff_check.cpp index 48f172a02cc366..f3b55ec45fb3d1 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, g2.takeoff_rpm_max); + const bool rpm_adequate = AP::esc_telem().are_motors_running(motor_mask, g2.takeoff_rpm_min); // 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 out of range", prefix_str); + gcs().send_text(MAV_SEVERITY_CRITICAL, "%s ESC RPM too low", prefix_str); } } #endif diff --git a/ArduCopter/version.h b/ArduCopter/version.h index aca789abdc19fa..30dbeb8ae228eb 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -6,14 +6,14 @@ #include "ap_version.h" -#define THISFIRMWARE "ArduCopter V4.5.0-dev" +#define THISFIRMWARE "ArduCopter V4.4.2-beta1" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,5,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 4,4,2,FIRMWARE_VERSION_TYPE_BETA #define FW_MAJOR 4 -#define FW_MINOR 5 -#define FW_PATCH 0 -#define FW_TYPE FIRMWARE_VERSION_TYPE_DEV +#define FW_MINOR 4 +#define FW_PATCH 2 +#define FW_TYPE FIRMWARE_VERSION_TYPE_BETA #include diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index 60ee3d4692c12d..46e52620876dc3 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -14,17 +14,6 @@ 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 }; @@ -53,7 +42,7 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure) } //are arming checks disabled? if (checks_to_perform == 0) { - return mandatory_checks(display_failure); + return true; } if (hal.util->was_watchdog_armed()) { // on watchdog reset bypass arming checks to allow for @@ -102,19 +91,27 @@ 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.flight_option_enabled(FlightOptions::CENTER_THROTTLE_TRIM)){ + if (SRV_Channels::get_emergency_stop()) { + check_failed(display_failure,"Motors Emergency Stopped"); + ret = false; + } + + if (plane.g2.flight_options & 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 ); @@ -137,19 +134,6 @@ 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) { @@ -218,11 +202,6 @@ bool AP_Arming_Plane::quadplane_checks(bool display_failure) ret = false; } - if ((plane.quadplane.tailsitter.enable > 0) && (plane.quadplane.q_fwd_thr_use != QuadPlane::FwdThrUse::OFF)) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, "set Q_FWD_THR_USE to 0"); - ret = false; - } - return ret; } #endif // HAL_QUADPLANE_ENABLED @@ -330,7 +309,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 && - (AP_Arming::method_is_GCS(method) || + (method == AP_Arming::Method::MAVLINK || method == AP_Arming::Method::RUDDER)) { if (plane.is_flying()) { // don't allow mavlink or rudder disarm while flying @@ -403,27 +382,6 @@ 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 } /* @@ -463,21 +421,3 @@ 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 b340ecdfc2f81e..afce68ae116955 100644 --- a/ArduPlane/AP_Arming.h +++ b/ArduPlane/AP_Arming.h @@ -2,10 +2,6 @@ #include -#ifndef AP_PLANE_BLACKBOX_LOGGING -#define AP_PLANE_BLACKBOX_LOGGING 0 -#endif - /* a plane specific arming class */ @@ -33,9 +29,6 @@ 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; @@ -43,18 +36,10 @@ 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 61d76425333278..ba79c7f3e2a3e3 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_CLASS(RC_Channels, (RC_Channels*)&plane.g2.rc_channels, read_mode_switch, 7, 100, 27), + SCHED_TASK(read_control_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,11 +77,10 @@ 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), @@ -245,12 +244,7 @@ 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 + } } /* @@ -267,9 +261,7 @@ void Plane::update_logging25(void) if (should_log(MASK_LOG_CTUN)) { Log_Write_Control_Tuning(); - if (!should_log(MASK_LOG_NOTCH_FULLRATE)) { - AP::ins().write_notch_log_messages(); - } + AP::ins().write_notch_log_messages(); #if HAL_GYROFFT_ENABLED gyro_fft.write_log_messages(); #endif @@ -317,7 +309,7 @@ void Plane::one_second_loop() adsb.set_max_speed(aparm.airspeed_max); #endif - if (flight_option_enabled(FlightOptions::ENABLE_DEFAULT_AIRSPEED)) { + if (g2.flight_options & 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)); @@ -525,6 +517,12 @@ 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; @@ -568,7 +566,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.flight_option_enabled(FlightOptions::CLIMB_BEFORE_TURN)))) { + if (control_mode == &mode_rtl && !rtl.done_climb && (g2.rtl_climb_min > 0 || (plane.g2.flight_options & 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 @@ -836,7 +834,12 @@ bool Plane::get_target_location(Location& target_loc) */ bool Plane::update_target_location(const Location &old_loc, const Location &new_loc) { - if (!old_loc.same_loc_as(next_WP_loc)) { + if (!old_loc.same_latlon_as(next_WP_loc)) { + return false; + } + ftype alt_diff; + if (!old_loc.get_alt_distance(next_WP_loc, alt_diff) || + !is_zero(alt_diff)) { return false; } next_WP_loc = new_loc; @@ -870,29 +873,8 @@ 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(); -} +#endif // AP_SCRIPTING_ENABLED // 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 @@ -906,7 +888,7 @@ void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const #endif pitch = ahrs.pitch; roll = ahrs.roll; - if (!(flight_option_enabled(FlightOptions::OSD_REMOVE_TRIM_PITCH_CD))) { // correct for TRIM_PITCH_CD + if (!(g2.flight_options & FlightOptions::OSD_REMOVE_TRIM_PITCH_CD)) { // correct for TRIM_PITCH_CD pitch -= g.pitch_trim_cd * 0.01 * DEG_TO_RAD; } } @@ -923,10 +905,4 @@ 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 39a9461ea63e7d..6d078e98475ea7 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -32,7 +32,7 @@ float Plane::calc_speed_scaler(void) speed_scaler = MIN(speed_scaler, new_scaler); // we also decay the integrator to prevent an integrator from before - // we were at low speed persistent at high speed + // we were at low speed persistint at high speed rollController.decay_I(); pitchController.decay_I(); yawController.decay_I(); @@ -50,8 +50,8 @@ float Plane::calc_speed_scaler(void) speed_scaler = 1; } if (!plane.ahrs.airspeed_sensor_enabled() && - (plane.flight_option_enabled(FlightOptions::SURPRESS_TKOFF_SCALING)) && - (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF)) { //scaling is suppressed during climb phase of automatic takeoffs with no airspeed sensor being used due to problems with inaccurate airspeed estimates + (plane.g2.flight_options & FlightOptions::SURPRESS_TKOFF_SCALING) && + (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF)) { //scaling is surpressed during climb phase of automatic takeoffs with no airspeed sensor being used due to problems with inaccurate airspeed estimates return MIN(speed_scaler, 1.0f) ; } return speed_scaler; @@ -131,16 +131,6 @@ float Plane::stabilize_roll_get_roll_out() if (!quadplane.use_fw_attitude_controllers()) { // use the VTOL rate for control, to ensure consistency const auto &pid_info = quadplane.attitude_control->get_rate_roll_pid().get_pid_info(); - - // scale FF to angle P - if (quadplane.option_is_set(QuadPlane::OPTION::SCALE_FF_ANGLE_P)) { - const float mc_angR = quadplane.attitude_control->get_angle_roll_p().kP() - * quadplane.attitude_control->get_last_angle_P_scale().x; - if (is_positive(mc_angR)) { - rollController.set_ff_scale(MIN(1.0, 1.0 / (mc_angR * rollController.tau()))); - } - } - const float roll_out = rollController.get_rate_out(degrees(pid_info.target), speed_scaler); /* when slaving fixed wing control to VTOL control we need to decay the integrator to prevent opposing integrators balancing between the two controllers @@ -155,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.flight_option_enabled(FlightOptions::DISABLE_GROUND_PID_SUPPRESSION))); + ground_mode && !(plane.g2.flight_options & FlightOptions::DISABLE_GROUND_PID_SUPPRESSION)); } /* @@ -184,16 +174,6 @@ float Plane::stabilize_pitch_get_pitch_out() if (!quadplane.use_fw_attitude_controllers()) { // use the VTOL rate for control, to ensure consistency const auto &pid_info = quadplane.attitude_control->get_rate_pitch_pid().get_pid_info(); - - // scale FF to angle P - if (quadplane.option_is_set(QuadPlane::OPTION::SCALE_FF_ANGLE_P)) { - const float mc_angP = quadplane.attitude_control->get_angle_pitch_p().kP() - * quadplane.attitude_control->get_last_angle_P_scale().y; - if (is_positive(mc_angP)) { - pitchController.set_ff_scale(MIN(1.0, 1.0 / (mc_angP * pitchController.tau()))); - } - } - const int32_t pitch_out = pitchController.get_rate_out(degrees(pid_info.target), speed_scaler); /* when slaving fixed wing control to VTOL control we need to decay the integrator to prevent opposing integrators balancing between the two controllers @@ -228,29 +208,23 @@ float Plane::stabilize_pitch_get_pitch_out() } return pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, speed_scaler, disable_integrator, - ground_mode && !(plane.flight_option_enabled(FlightOptions::DISABLE_GROUND_PID_SUPPRESSION))); + ground_mode && !(plane.g2.flight_options & 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 ModeStabilize::stabilize_stick_mixing_direct() +void Plane::stabilize_stick_mixing_direct() { - if (!plane.stick_mixing_enabled()) { + if (!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 = plane.channel_roll->stick_mixing(aileron); + aileron = 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 = plane.channel_pitch->stick_mixing(elevator); + elevator = channel_pitch->stick_mixing(elevator); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator); } @@ -275,7 +249,6 @@ 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; @@ -295,7 +268,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.flight_option_enabled(FlightOptions::ENABLE_LOITER_ALT_CONTROL))) { + if ((control_mode == &mode_loiter) && (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL)) { // loiter is using altitude control based on the pitch stick, don't use it again here return; } @@ -327,57 +300,38 @@ void Plane::stabilize_stick_mixing_fbw() */ void Plane::stabilize_yaw() { - bool ground_steering = false; if (landing.is_flaring()) { // in flaring then enable ground steering - ground_steering = true; + steering_control.ground_steering = true; } else { // otherwise use ground steering when no input control and we // are below the GROUND_STEER_ALT - ground_steering = (channel_roll->get_control_in() == 0 && + steering_control.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 - ground_steering = false; + steering_control.ground_steering = false; } } /* - first calculate steering for a nose or tail + first calculate steering_control.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 && ground_steering)) { - steering_output = calc_nav_yaw_course(); - } else if (ground_steering) { - steering_output = calc_nav_yaw_ground(); + (steer_state.hold_course_cd != -1 && steering_control.ground_steering)) { + calc_nav_yaw_course(); + } else if (steering_control.ground_steering) { + calc_nav_yaw_ground(); } /* - now calculate rudder for the rudder + now calculate steering_control.rudder for the rudder */ - 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); - } - + calc_nav_yaw_coordinated(); } /* @@ -385,21 +339,35 @@ 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); + quadplane.transition->set_FW_roll_pitch(nav_pitch_cd, nav_roll_cd, allow_stick_mixing); } #endif if (now - last_stabilize_ms > 2000) { - // if we haven't run the rate controllers for 2 seconds then reset - control_mode->reset_controllers(); + // 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; } last_stabilize_ms = now; - if (control_mode == &mode_training || - control_mode == &mode_manual) { + if (control_mode == &mode_training) { plane.control_mode->run(); #if AP_SCRIPTING_ENABLED } else if (nav_scripting_active()) { @@ -409,20 +377,47 @@ 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()) { - rudder = nav_scripting.rudder_offset_pct * 45; + float 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; } - SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder); - SRV_Channels::set_output_scaled(SRV_Channel::k_steering, rudder); #endif - } else { + } 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(); + } +#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(); } /* @@ -458,6 +453,14 @@ 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); } @@ -468,7 +471,7 @@ void Plane::calc_throttle() /* calculate yaw control for coordinated flight */ -int16_t Plane::calc_nav_yaw_coordinated() +void Plane::calc_nav_yaw_coordinated() { const float speed_scaler = get_speed_scaler(); bool disable_integrator = false; @@ -486,7 +489,7 @@ int16_t Plane::calc_nav_yaw_coordinated() // user is doing an AUTOTUNE with yaw rate control const float rudd_expo = rudder_in_expo(true); const float yaw_rate = (rudd_expo/SERVO_MAX) * g.acro_yaw_rate; - // add in the coordinated turn yaw rate to make it easier to fly while tuning the yaw rate controller + // add in the corrdinated turn yaw rate to make it easier to fly while tuning the yaw rate controller const float coordination_yaw_rate = degrees(GRAVITY_MSS * tanf(radians(nav_roll_cd*0.01f))/MAX(aparm.airspeed_min,smoothed_airspeed)); commanded_rudder = yawController.get_rate_out(yaw_rate+coordination_yaw_rate, speed_scaler, false); using_rate_controller = true; @@ -502,35 +505,35 @@ int16_t 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 */ -int16_t Plane::calc_nav_yaw_course(void) +void 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(); - int16_t steering = steerController.get_steering_out_angle_error(bearing_error_cd); + steering_control.steering = steerController.get_steering_out_angle_error(bearing_error_cd); if (stick_mixing_enabled()) { - steering = channel_rudder->stick_mixing(steering); + steering_control.steering = channel_rudder->stick_mixing(steering_control.steering); } - return constrain_int16(steering, -4500, 4500); + steering_control.steering = constrain_int16(steering_control.steering, -4500, 4500); } /* calculate yaw control for ground steering */ -int16_t Plane::calc_nav_yaw_ground(void) +void Plane::calc_nav_yaw_ground(void) { if (gps.ground_speed() < 1 && is_zero(get_throttle_input()) && @@ -539,7 +542,8 @@ int16_t Plane::calc_nav_yaw_ground(void) // manual rudder control while still steer_state.locked_course = false; steer_state.locked_course_err = 0; - return rudder_input(); + steering_control.steering = rudder_input(); + return; } // if we haven't been steering for 1s then clear locked course @@ -566,16 +570,15 @@ int16_t Plane::calc_nav_yaw_ground(void) } } - int16_t steering; if (!steer_state.locked_course) { // use a rate controller at the pilot specified rate - steering = steerController.get_steering_out_rate(steer_rate); + steering_control.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 = steerController.get_steering_out_angle_error(yaw_error_cd); + steering_control.steering = steerController.get_steering_out_angle_error(yaw_error_cd); } - return constrain_int16(steering, -4500, 4500); + steering_control.steering = constrain_int16(steering_control.steering, -4500, 4500); } @@ -584,7 +587,17 @@ int16_t 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()); } @@ -595,6 +608,43 @@ 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(); } @@ -658,11 +708,11 @@ void Plane::update_load_factor(void) nav_roll_cd = constrain_int32(nav_roll_cd, -2500, 2500); roll_limit_cd = MIN(roll_limit_cd, 2500); } else if (max_load_factor < aerodynamic_load_factor) { - // the demanded nav_roll would take us past the aerodynamic + // the demanded nav_roll would take us past the aerodymamic // load limit. Limit our roll to a bank angle that will keep // the load within what the airframe can handle. We always // allow at least 25 degrees of roll however, to ensure the - // aircraft can be manoeuvred with a bad airspeed estimate. At + // aircraft can be maneuvered with a bad airspeed estimate. At // 25 degrees the load factor is 1.1 (10%) int32_t roll_limit = degrees(acosf(sq(1.0f / max_load_factor)))*100; if (roll_limit < 2500) { diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index efb77d48129e54..db021d09858791 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.flight_option_enabled(FlightOptions::GCS_REMOVE_TRIM_PITCH_CD))) { + if (!(plane.g2.flight_options & FlightOptions::GCS_REMOVE_TRIM_PITCH_CD)) { p -= radians(plane.g.pitch_trim_cd * 0.01f); } @@ -157,6 +157,39 @@ void GCS_MAVLINK_Plane::send_attitude() const omega.z); } +void GCS_MAVLINK_Plane::send_attitude_target() +{ +#if HAL_QUADPLANE_ENABLED + // Check if the attitude target is valid for reporting + const uint32_t now = AP_HAL::millis(); + if (now - plane.quadplane.last_att_control_ms > 100) { + return; + } + + const Quaternion quat = plane.quadplane.attitude_control->get_attitude_target_quat(); + const Vector3f ang_vel = plane.quadplane.attitude_control->get_attitude_target_ang_vel(); + const float throttle = plane.quadplane.attitude_control->get_throttle_in(); + + const float quat_out[4] {quat.q1, quat.q2, quat.q3, quat.q4}; + + const uint16_t typemask = 0; + + mavlink_msg_attitude_target_send( + chan, + now, // time since boot (ms) + typemask, // Bitmask that tells the system what control dimensions should be ignored by the vehicle + quat_out, // Target attitude quaternion [w, x, y, z] order, zero-rotation is [1, 0, 0, 0], unit-length + ang_vel.x, // bodyframe target roll rate (rad/s) + ang_vel.y, // bodyframe target pitch rate (rad/s) + ang_vel.z, // bodyframe yaw rate (rad/s) + throttle); // Collective thrust, normalized to 0 .. 1 + +#endif // HAL_QUADPLANE_ENABLED + +} + + + void GCS_MAVLINK_Plane::send_aoa_ssa() { AP_AHRS &ahrs = AP::ahrs(); @@ -596,9 +629,7 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { MSG_GPS2_RAW, MSG_GPS2_RTK, MSG_NAV_CONTROLLER_OUTPUT, -#if AP_FENCE_ENABLED MSG_FENCE_STATUS, -#endif MSG_POSITION_TARGET_GLOBAL_INT, }; static const ap_message STREAM_POSITION_msgs[] = { @@ -646,10 +677,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = { MSG_BATTERY_STATUS, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_OPTICAL_FLOW, -#if COMPASS_CAL_ENABLED MSG_MAG_CAL_REPORT, MSG_MAG_CAL_PROGRESS, -#endif MSG_EKF_STATUS_REPORT, MSG_VIBRATION, }; @@ -700,7 +729,7 @@ void GCS_MAVLINK_Plane::handle_change_alt_request(AP_Mission::Mission_Command &c } -MAV_RESULT GCS_MAVLINK_Plane::handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) +MAV_RESULT GCS_MAVLINK_Plane::handle_command_preflight_calibration(const mavlink_command_long_t &packet, const mavlink_message_t &msg) { plane.in_calibration = true; MAV_RESULT ret = GCS_MAVLINK::handle_command_preflight_calibration(packet, msg); @@ -715,7 +744,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 && AP_FOLLOW_ENABLED +#if AP_SCRIPTING_ENABLED // pass message to follow library plane.g2.follow.handle_msg(msg); #endif @@ -921,6 +950,11 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl float new_target_heading = radians(wrap_180(packet.param2)); + // if packet is requesting us to go to the heading we are already going to, we-re already on it. + if ( (is_equal(new_target_heading,plane.guided_state.target_heading))) { // compare two floats as near-enough + return MAV_RESULT_ACCEPTED; + } + // course over ground if ( int(packet.param1) == HEADING_TYPE_COURSE_OVER_GROUND) { // compare as nearest int plane.guided_state.target_heading_type = GUIDED_HEADING_COG; @@ -949,13 +983,10 @@ 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, const mavlink_message_t &msg) +MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_int_t &packet) { switch(packet.command) { - case MAV_CMD_DO_AUTOTUNE_ENABLE: - return handle_MAV_CMD_DO_AUTOTUNE_ENABLE(packet); - case MAV_CMD_DO_REPOSITION: return handle_command_int_do_reposition(packet); @@ -965,70 +996,26 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in case MAV_CMD_GUIDED_CHANGE_HEADING: return handle_command_int_guided_slew_commands(packet); -#if AP_SCRIPTING_ENABLED && AP_FOLLOW_ENABLED case MAV_CMD_DO_FOLLOW: +#if AP_SCRIPTING_ENABLED // param1: sysid of target to follow if ((packet.param1 > 0) && (packet.param1 <= 255)) { plane.g2.follow.set_target_sysid((uint8_t)packet.param1); return MAV_RESULT_ACCEPTED; } - return MAV_RESULT_DENIED; #endif - -#if AP_ICENGINE_ENABLED - case MAV_CMD_DO_ENGINE_CONTROL: - if (!plane.g2.ice_control.engine_control(packet.param1, packet.param2, packet.param3)) { - return MAV_RESULT_FAILED; - } - return MAV_RESULT_ACCEPTED; -#endif - - case MAV_CMD_DO_CHANGE_SPEED: - return handle_command_DO_CHANGE_SPEED(packet); - -#if PARACHUTE == ENABLED - case MAV_CMD_DO_PARACHUTE: - return handle_MAV_CMD_DO_PARACHUTE(packet); -#endif - -#if HAL_QUADPLANE_ENABLED - case MAV_CMD_DO_MOTOR_TEST: - return handle_MAV_CMD_DO_MOTOR_TEST(packet); - - case MAV_CMD_DO_VTOL_TRANSITION: - return handle_command_DO_VTOL_TRANSITION(packet); -#endif - - case MAV_CMD_DO_GO_AROUND: - return plane.trigger_land_abort(packet.param1) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED; - - case MAV_CMD_DO_LAND_START: - // attempt to switch to next DO_LAND_START command in the mission - if (plane.mission.jump_to_landing_sequence()) { - plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND); - return MAV_RESULT_ACCEPTED; - } return MAV_RESULT_FAILED; - - case MAV_CMD_MISSION_START: - plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND); - return MAV_RESULT_ACCEPTED; - - case MAV_CMD_NAV_LOITER_UNLIM: - plane.set_mode(plane.mode_loiter, ModeReason::GCS_COMMAND); - return MAV_RESULT_ACCEPTED; - - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - plane.set_mode(plane.mode_rtl, ModeReason::GCS_COMMAND); - return MAV_RESULT_ACCEPTED; - + default: - return GCS_MAVLINK::handle_command_int_packet(packet, msg); + return GCS_MAVLINK::handle_command_int_packet(packet); } } -MAV_RESULT GCS_MAVLINK_Plane::handle_command_DO_CHANGE_SPEED(const mavlink_command_int_t &packet) +MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_long_t &packet) { + switch(packet.command) { + + case MAV_CMD_DO_CHANGE_SPEED: { // if we're in failsafe modes (e.g., RTL, LOITER) or in pilot // controlled modes (e.g., MANUAL, TRAINING) // this command should be ignored since it comes in from GCS @@ -1039,15 +1026,23 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_DO_CHANGE_SPEED(const mavlink_comma return MAV_RESULT_FAILED; } - if (plane.do_change_speed(packet.param1, packet.param2, packet.param3)) { + AP_Mission::Mission_Command cmd; + if (AP_Mission::mavlink_cmd_long_to_mission_cmd(packet, cmd) != MAV_MISSION_ACCEPTED) { + return MAV_RESULT_DENIED; + } + if (plane.do_change_speed(cmd)) { return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; -} + } -MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg) -{ - switch(packet.command) { + case MAV_CMD_NAV_LOITER_UNLIM: + plane.set_mode(plane.mode_loiter, ModeReason::GCS_COMMAND); + return MAV_RESULT_ACCEPTED; + + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + plane.set_mode(plane.mode_rtl, ModeReason::GCS_COMMAND); + return MAV_RESULT_ACCEPTED; #if HAL_QUADPLANE_ENABLED case MAV_CMD_NAV_TAKEOFF: { @@ -1061,21 +1056,28 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l } #endif // HAL_QUADPLANE_ENABLED - default: - return GCS_MAVLINK::handle_command_long_packet(packet, msg); - } -} + case MAV_CMD_MISSION_START: + plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND); + return MAV_RESULT_ACCEPTED; -MAV_RESULT GCS_MAVLINK_Plane::handle_MAV_CMD_DO_AUTOTUNE_ENABLE(const mavlink_command_int_t &packet) -{ + case MAV_CMD_DO_LAND_START: + // attempt to switch to next DO_LAND_START command in the mission + if (plane.mission.jump_to_landing_sequence()) { + plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND); + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; + + case MAV_CMD_DO_GO_AROUND: + return plane.trigger_land_abort(packet.param1) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED; + + case MAV_CMD_DO_AUTOTUNE_ENABLE: // param1 : enable/disable plane.autotune_enable(!is_zero(packet.param1)); return MAV_RESULT_ACCEPTED; -} #if PARACHUTE == ENABLED -MAV_RESULT GCS_MAVLINK_Plane::handle_MAV_CMD_DO_PARACHUTE(const mavlink_command_int_t &packet) -{ + case MAV_CMD_DO_PARACHUTE: // configure or release parachute switch ((uint16_t)packet.param1) { case PARACHUTE_DISABLE: @@ -1102,13 +1104,10 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_MAV_CMD_DO_PARACHUTE(const mavlink_command_ break; } return MAV_RESULT_FAILED; -} #endif - #if HAL_QUADPLANE_ENABLED -MAV_RESULT GCS_MAVLINK_Plane::handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command_int_t &packet) -{ + case MAV_CMD_DO_MOTOR_TEST: // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle) // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum) // param3 : throttle (range depends upon param2) @@ -1119,18 +1118,38 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command (uint8_t)packet.param2, (uint16_t)packet.param3, packet.param4, - (uint8_t)packet.x); -} + (uint8_t)packet.param5); -MAV_RESULT GCS_MAVLINK_Plane::handle_command_DO_VTOL_TRANSITION(const mavlink_command_int_t &packet) -{ + case MAV_CMD_DO_VTOL_TRANSITION: if (!plane.quadplane.handle_do_vtol_transition((enum MAV_VTOL_STATE)packet.param1)) { return MAV_RESULT_FAILED; } return MAV_RESULT_ACCEPTED; -} #endif +#if AP_ICENGINE_ENABLED + case MAV_CMD_DO_ENGINE_CONTROL: + if (!plane.g2.ice_control.engine_control(packet.param1, packet.param2, packet.param3)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; +#endif + +#if AP_SCRIPTING_ENABLED + case MAV_CMD_DO_FOLLOW: + // param1: sysid of target to follow + if ((packet.param1 > 0) && (packet.param1 <= 255)) { + plane.g2.follow.set_target_sysid((uint8_t)packet.param1); + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; +#endif + + default: + return GCS_MAVLINK::handle_command_long_packet(packet); + } +} + // this is called on receipt of a MANUAL_CONTROL packet and is // expected to call manual_override to override RC input on desired // axes. @@ -1330,7 +1349,6 @@ 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 @@ -1340,7 +1358,6 @@ 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 c6643c39b9df52..cb55a8d39ee850 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -17,21 +17,20 @@ class GCS_MAVLINK_Plane : public GCS_MAVLINK 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 bool sysid_enforce() const override; - MAV_RESULT handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; - MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; - MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet, 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_int_packet(const mavlink_command_int_t &packet) override; + MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override; MAV_RESULT handle_command_do_set_mission_current(const mavlink_command_long_t &packet) override; void send_position_target_global_int() override; void send_aoa_ssa(); void send_attitude() const override; + void send_attitude_target() override; void send_wind() const; bool persist_streamrates() const override { return true; } @@ -54,11 +53,7 @@ class GCS_MAVLINK_Plane : public GCS_MAVLINK void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override; MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet); MAV_RESULT handle_command_int_guided_slew_commands(const mavlink_command_int_t &packet); - MAV_RESULT handle_MAV_CMD_DO_AUTOTUNE_ENABLE(const mavlink_command_int_t &packet); - MAV_RESULT handle_command_DO_CHANGE_SPEED(const mavlink_command_int_t &packet); - MAV_RESULT handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command_int_t &packet); - MAV_RESULT handle_MAV_CMD_DO_PARACHUTE(const mavlink_command_int_t &packet); - MAV_RESULT handle_command_DO_VTOL_TRANSITION(const mavlink_command_int_t &packet); + bool try_send_message(enum ap_message id) override; void packetReceived(const mavlink_status_t &status, const mavlink_message_t &msg) override; diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 8f46404a85669f..f8b1a3ac576491 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -62,9 +62,6 @@ 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(); - } } @@ -79,7 +76,6 @@ struct PACKED log_Control_Tuning { float rudder_out; float throttle_dem; float airspeed_estimate; - uint8_t airspeed_estimate_status; float synthetic_airspeed; float EAS2TAS; int32_t groundspeed_undershoot; @@ -89,8 +85,7 @@ struct PACKED log_Control_Tuning { void Plane::Log_Write_Control_Tuning() { float est_airspeed = 0; - AP_AHRS::AirspeedEstimateType airspeed_estimate_type = AP_AHRS::AirspeedEstimateType::NO_NEW_ESTIMATE; - ahrs.airspeed_estimate(est_airspeed, airspeed_estimate_type); + ahrs.airspeed_estimate(est_airspeed); float synthetic_airspeed; if (!ahrs.synthetic_airspeed(synthetic_airspeed)) { @@ -108,7 +103,6 @@ void Plane::Log_Write_Control_Tuning() rudder_out : SRV_Channels::get_output_scaled(SRV_Channel::k_rudder), throttle_dem : TECS_controller.get_throttle_demand(), airspeed_estimate : est_airspeed, - airspeed_estimate_status : (uint8_t)airspeed_estimate_type, synthetic_airspeed : synthetic_airspeed, EAS2TAS : ahrs.get_EAS2TAS(), groundspeed_undershoot : groundspeed_undershoot, @@ -116,7 +110,6 @@ 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; @@ -132,6 +125,7 @@ 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(), @@ -144,8 +138,8 @@ void Plane::Log_Write_OFG_Guided() target_heading_limit : guided_state.target_heading_accel_limit }; logger.WriteBlock(&pkt, sizeof(pkt)); -} #endif +} struct PACKED log_Nav_Tuning { LOG_PACKET_HEADER; @@ -225,7 +219,6 @@ struct PACKED log_AETR { float throttle; float rudder; float flap; - float steering; float speed_scaler; }; @@ -239,7 +232,6 @@ 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(), }; @@ -306,17 +298,15 @@ const struct LogStructure Plane::log_structure[] = { // @Field: NavPitch: desired pitch // @Field: Pitch: achieved pitch // @Field: ThO: scaled output throttle -// @Field: RdO: scaled output rudder +// @Field: RdrOut: scaled output rudder // @Field: ThD: demanded speed-height-controller throttle // @Field: As: airspeed estimate (or measurement if airspeed sensor healthy and ARSPD_USE>0) -// @Field: SAs: DCM's airspeed estimate, NaN if not available -// @Field: AsT: airspeed type ( old estimate or source of new estimate) -// @FieldValueEnum: AsT: AP_AHRS::AirspeedEstimateType +// @Field: SAs: synthetic airspeed measurement derived from non-airspeed sensors, NaN if not available // @Field: E2T: equivalent to true airspeed ratio // @Field: GU: groundspeed undershoot when flying with minimum groundspeed { LOG_CTUN_MSG, sizeof(log_Control_Tuning), - "CTUN", "QccccffffBffi", "TimeUS,NavRoll,Roll,NavPitch,Pitch,ThO,RdO,ThD,As,AsT,SAs,E2T,GU", "sdddd---n-n-n", "FBBBB---000-B" , true }, + "CTUN", "Qccccffffffi", "TimeUS,NavRoll,Roll,NavPitch,Pitch,ThO,RdrOut,ThD,As,SAs,E2T,GU", "sdddd---nn-n", "FBBBB---00-B" , true }, // @LoggerMessage: NTUN // @Description: Navigation Tuning information - e.g. vehicle destination @@ -384,8 +374,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: Transition state: 0-AirspeedWait,1-Timer,2-Done / TailSitter: 0-FW Wait,1-VTOL Wait,2-Done -// @Field: Ast: Q assist active +// @Field: Trn: Transistion state +// @Field: Ast: Q assist active state #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 }, @@ -403,8 +393,7 @@ const struct LogStructure Plane::log_structure[] = { // @Field: FF: controller feed-forward portion of response // @Field: Dmod: scaler applied to D gain to reduce limit cycling // @Field: SRate: slew rate -// @Field: Flags: bitmask of PID state flags -// @FieldBitmaskEnum: Flags: log_PID_Flags +// @Field: Limit: 1 if I term is limited due to output saturation { LOG_PIQR_MSG, sizeof(log_PID), "PIQR", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS , true }, { LOG_PIQP_MSG, sizeof(log_PID), @@ -426,25 +415,22 @@ const struct LogStructure Plane::log_structure[] = { // @Field: FF: controller feed-forward portion of response // @Field: Dmod: scaler applied to D gain to reduce limit cycling // @Field: SRate: slew rate -// @Field: Flags: bitmask of PID state flags -// @FieldBitmaskEnum: Flags: log_PID_Flags +// @Field: Limit: 1 if I term is limited due to output saturation { LOG_PIDG_MSG, sizeof(log_PID), "PIDG", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS , true }, // @LoggerMessage: AETR // @Description: Normalised pre-mixer control surface outputs // @Field: TimeUS: Time since system startup -// @Field: Ail: Pre-mixer value for aileron output (between -4500 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: 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: SS: Surface movement / airspeed scaling value { LOG_AETR_MSG, sizeof(log_AETR), - "AETR", "Qfffffff", "TimeUS,Ail,Elev,Thr,Rudd,Flap,Steer,SS", "s-------", "F-------" , true }, + "AETR", "Qffffff", "TimeUS,Ail,Elev,Thr,Rudd,Flap,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 @@ -457,7 +443,6 @@ 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 720eed4c273694..83188fe94552b6 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -38,14 +38,6 @@ 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 @@ -159,7 +151,7 @@ const AP_Param::Info Plane::var_info[] = { // @Param: TKOFF_TDRAG_SPD1 // @DisplayName: Takeoff tail dragger speed1 - // @Description: This parameter sets the airspeed at which to stop holding the tail down and transition to rudder control of steering on the ground. When TKOFF_TDRAG_SPD1 is reached the pitch of the aircraft will be held level until TKOFF_ROTATE_SPD is reached, at which point the takeoff pitch specified in the mission will be used to "rotate" the pitch for takeoff climb. Set TKOFF_TDRAG_SPD1 to zero to go straight to rotation. This should be set to zero for hand launch and catapult launch. It should also be set to zero for tricycle undercarriages unless you are using the method above to gently hold the nose wheel down. For tail dragger aircraft it should be set just below the stall speed. + // @Description: This parameter sets the airspeed at which to stop holding the tail down and transition to rudder control of steering on the ground. When TKOFF_TDRAG_SPD1 is reached the pitch of the aircraft will be held level until TKOFF_ROTATE_SPD is reached, at which point the takeoff pitch specified in the mission will be used to "rotate" the pitch for takeoff climb. Set TKOFF_TDRAG_SPD1 to zero to go straight to rotation. This should be set to zero for hand launch and catapult launch. It should also be set to zero for tricycle undercarriages unless you are using the method above to genetly hold the nose wheel down. For tail dragger aircraft it should be set just below the stall speed. // @Units: m/s // @Range: 0 30 // @Increment: 0.1 @@ -432,8 +424,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_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 + // @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 // @User: Standard GSCALAR(fs_action_long, "FS_LONG_ACTN", FS_ACTION_LONG_CONTINUE), @@ -613,10 +605,17 @@ 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,22:Fullrate Notch + // @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 // @User: Advanced GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), @@ -755,11 +754,9 @@ 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_ @@ -800,11 +797,9 @@ const AP_Param::Info Plane::var_info[] = { GOBJECT(quadplane, "Q_", QuadPlane), #endif -#if AP_TUNING_ENABLED // @Group: TUNE_ // @Path: tuning.cpp,../libraries/AP_Tuning/AP_Tuning.cpp GOBJECT(tuning, "TUNE_", AP_Tuning_Plane), -#endif #if HAL_QUADPLANE_ENABLED // @Group: Q_A_ @@ -948,11 +943,9 @@ const AP_Param::Info Plane::var_info[] = { // @Path: ../libraries/AP_Mission/AP_Mission.cpp GOBJECT(mission, "MIS_", AP_Mission), -#if HAL_RALLY_ENABLED // @Group: RALLY_ // @Path: ../libraries/AP_Rally/AP_Rally.cpp GOBJECT(rally, "RALLY_", AP_Rally), -#endif #if HAL_NAVEKF2_AVAILABLE // @Group: EK2_ @@ -1097,7 +1090,6 @@ 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), @@ -1245,7 +1237,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 && AP_FOLLOW_ENABLED +#if AP_SCRIPTING_ENABLED // @Group: FOLL // @Path: ../libraries/AP_Follow/AP_Follow.cpp AP_SUBGROUPINFO(follow, "FOLL", 33, ParametersG2, AP_Follow), @@ -1257,12 +1249,17 @@ 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 7210bfe7771267..db02c44565e8be 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, // unused + k_param_num_resets, k_param_NavEKF2, k_param_g2, k_param_avoidance_adsb, @@ -356,7 +356,6 @@ 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; @@ -421,7 +420,7 @@ class Parameters { AP_Int8 flight_mode6; AP_Int8 initial_mode; - // Navigational manoeuvring limits + // Navigational maneuvering limits // AP_Int16 alt_offset; AP_Int16 acro_roll_rate; @@ -435,6 +434,7 @@ class Parameters { AP_Float mixing_gain; AP_Int16 mixing_offset; AP_Int16 dspoiler_rud_rate; + AP_Int16 num_resets; AP_Int32 log_bitmask; AP_Int32 RTL_altitude_cm; AP_Int16 pitch_trim_cd; @@ -541,17 +541,17 @@ class ParametersG2 { AP_Int8 crow_flap_options; AP_Int8 crow_flap_aileron_matching; - // Forward throttle battery voltage compensation + // Forward throttle battery voltage compenstaion AP_Float fwd_thr_batt_voltage_max; AP_Float fwd_thr_batt_voltage_min; AP_Int8 fwd_thr_batt_idx; #if OFFBOARD_GUIDED == ENABLED // guided yaw heading PID - AC_PID guidedHeading{5000.0, 0.0, 0.0, 0 , 10.0, 5.0, 5.0 , 5.0 , 0.0}; + 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 && AP_FOLLOW_ENABLED +#if AP_SCRIPTING_ENABLED AP_Follow follow; #endif diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index b8d6e21db733c8..ab13b54e5e78d5 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -83,18 +83,11 @@ #include #include // Landing Gear library #include -#include -#if AP_EXTERNAL_CONTROL_ENABLED -#include -#endif #include "GCS_Mavlink.h" #include "GCS_Plane.h" #include "quadplane.h" -#include -#if AP_TUNING_ENABLED #include "tuning.h" -#endif // Configuration #include "config.h" @@ -197,7 +190,6 @@ 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; @@ -218,6 +210,16 @@ 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; @@ -225,9 +227,6 @@ 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; } @@ -245,14 +244,8 @@ class Plane : public AP_Vehicle { AP_OpticalFlow optflow; #endif -#if HAL_RALLY_ENABLED - // Rally Points + // Rally Ponints AP_Rally rally; -#endif - - // returns a Location for a rally point or home; if - // HAL_RALLY_ENABLED is false, just home. - Location calc_best_rally_or_home_location(const Location ¤t_loc, float rtl_home_alt_amsl_cm) const; #if OSD_ENABLED || OSD_PARAM_ENABLED AP_OSD osd; @@ -312,7 +305,7 @@ class Plane : public AP_Vehicle { // Failsafe struct { - // Used to track if the value on channel 3 (throttle) has fallen below the failsafe threshold + // Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold // RC receiver should be set up to output a low throttle value when signal is lost bool rc_failsafe; @@ -387,7 +380,6 @@ 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; @@ -403,13 +395,11 @@ 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 @@ -507,9 +497,6 @@ 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 @@ -556,7 +543,8 @@ class Plane : public AP_Vehicle { float target_heading_accel_limit; uint32_t target_heading_time_ms; guided_heading_type_t target_heading_type; - bool target_heading_limit; + bool target_heading_limit_low; + bool target_heading_limit_high; #endif // OFFBOARD_GUIDED == ENABLED } guided_state; @@ -618,7 +606,7 @@ class Plane : public AP_Vehicle { // The instantaneous desired pitch angle. Hundredths of a degree int32_t nav_pitch_cd; - // the aerodynamic load factor. This is calculated from the demanded + // the aerodymamic load factor. This is calculated from the demanded // roll before the roll is clipped, using 1/sqrt(cos(nav_roll)) float aerodynamic_load_factor = 1.0f; @@ -774,16 +762,11 @@ class Plane : public AP_Vehicle { AP_Mount camera_mount; #endif - // Arming/Disarming management class + // Arming/Disarming mangement class AP_Arming_Plane arming; AP_Param param_loader {var_info}; - // dummy implementation of external control -#if AP_EXTERNAL_CONTROL_ENABLED - AP_ExternalControl external_control; -#endif - static const AP_Scheduler::Task scheduler_tasks[]; static const AP_Param::Info var_info[]; @@ -798,10 +781,8 @@ class Plane : public AP_Vehicle { QuadPlane quadplane{ahrs}; #endif -#if AP_TUNING_ENABLED // support for transmitter tuning AP_Tuning_Plane tuning; -#endif static const struct LogStructure log_structure[]; @@ -878,11 +859,12 @@ 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(); - int16_t calc_nav_yaw_coordinated(); - int16_t calc_nav_yaw_course(void); - int16_t calc_nav_yaw_ground(void); + void calc_nav_yaw_coordinated(); + void calc_nav_yaw_course(void); + void calc_nav_yaw_ground(void); // Log.cpp uint32_t last_log_fast_ms; @@ -947,14 +929,22 @@ class Plane : public AP_Vehicle { bool verify_command_callback(const AP_Mission::Mission_Command& cmd); float get_wp_radius() const; + void do_nav_delay(const AP_Mission::Mission_Command& cmd); + bool verify_nav_delay(const AP_Mission::Mission_Command& cmd); + bool is_land_command(uint16_t cmd) const; - bool do_change_speed(uint8_t speedtype, float speed_target_ms, float rhtottle_pct); /* return true if in a specific AUTO mission command */ bool in_auto_mission_id(uint16_t command) const; - + + // Delay the next navigation command + struct { + uint32_t time_max_ms; + uint32_t time_start_ms; + } nav_delay; + #if AP_SCRIPTING_ENABLED // nav scripting support void do_nav_script_time(const AP_Mission::Mission_Command& cmd); @@ -976,6 +966,7 @@ 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); @@ -983,10 +974,6 @@ 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 @@ -1034,13 +1021,11 @@ 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(); @@ -1079,7 +1064,6 @@ 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); @@ -1095,6 +1079,7 @@ 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); @@ -1114,7 +1099,6 @@ 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); @@ -1242,8 +1226,6 @@ 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 cb426ab21f4a28..e0435c8253af5f 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -16,14 +16,9 @@ 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 (in_rc_failsafe()) { + if (plane.rc_failsafe_active() || plane.failsafe.rc_failsafe) { return false; } if (plane.failsafe.throttle_counter != 0) { @@ -50,7 +45,8 @@ 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) { - rc().reset_mode_switch(); +// TODO: rc().reset_mode_switch(); + plane.reset_control_switch(); } } } @@ -158,7 +154,6 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option, case AUX_FUNC::RTL: case AUX_FUNC::TAKEOFF: case AUX_FUNC::FBWA: - case AUX_FUNC::AIRBRAKE: #if HAL_QUADPLANE_ENABLED case AUX_FUNC::QRTL: case AUX_FUNC::QSTABILIZE: @@ -176,7 +171,6 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option, case AUX_FUNC::TRIM_TO_CURRENT_SERVO_RC: case AUX_FUNC::EMERGENCY_LANDING_EN: case AUX_FUNC::FW_AUTOTUNE: - case AUX_FUNC::VFWD_THR_OVERRIDE: break; case AUX_FUNC::SOARING: @@ -272,15 +266,6 @@ bool RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit case AUX_FUNC::QSTABILIZE: do_aux_function_change_mode(Mode::Number::QSTABILIZE, ch_flag); break; - - case AUX_FUNC::VFWD_THR_OVERRIDE: { - const bool enable = (ch_flag == AuxSwitchPos::HIGH); - if (enable != plane.quadplane.vfwd_enable_active) { - plane.quadplane.vfwd_enable_active = enable; - gcs().send_text(MAV_SEVERITY_INFO, "QFwdThr: %s", enable?"ON":"OFF"); - } - break; - } #endif case AUX_FUNC::SOARING: @@ -369,7 +354,7 @@ bool RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit break; case AUX_FUNC::MODE_SWITCH_RESET: - rc().reset_mode_switch(); + plane.reset_control_switch(); break; case AUX_FUNC::CRUISE: diff --git a/ArduPlane/RC_Channel.h b/ArduPlane/RC_Channel.h index 7ecc352c32aa8b..35663554a51e0f 100644 --- a/ArduPlane/RC_Channel.h +++ b/ArduPlane/RC_Channel.h @@ -13,9 +13,6 @@ 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, @@ -45,13 +42,10 @@ 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 1559e32c6bd29a..679bb529a38117 100644 --- a/ArduPlane/ReleaseNotes.txt +++ b/ArduPlane/ReleaseNotes.txt @@ -174,51 +174,6 @@ The full list of changes is: - 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 ------------------- @@ -312,7 +267,7 @@ of changes. Many thanks to all the people who have contributed! - Gimbal/Mount2 can be moved to retracted or neutral position - Gremsy ZIO support - IMAGE_START_CAPTURE, SET_CAMERA_ZOOM/FOCUS, VIDEO_START/STOP_CAPTURE command support - - Parameters renamed and rescaled + - 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 diff --git a/ArduPlane/afs_plane.cpp b/ArduPlane/afs_plane.cpp index 298d7f5fe8479b..2900d6e767a0f4 100644 --- a/ArduPlane/afs_plane.cpp +++ b/ArduPlane/afs_plane.cpp @@ -103,11 +103,4 @@ AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Plane::afs_mode(void) } return AP_AdvancedFailsafe::AFS_STABILIZED; } - -//to force entering auto mode when datalink loss - void AP_AdvancedFailsafe_Plane::set_mode_auto(void) - { - plane.set_mode(plane.mode_auto,ModeReason::GCS_FAILSAFE); - } - #endif // AP_ADVANCEDFAILSAFE_ENABLED diff --git a/ArduPlane/afs_plane.h b/ArduPlane/afs_plane.h index ffd341481c3ff9..142de7f3452538 100644 --- a/ArduPlane/afs_plane.h +++ b/ArduPlane/afs_plane.h @@ -39,9 +39,6 @@ class AP_AdvancedFailsafe_Plane : public AP_AdvancedFailsafe // return the AFS mapped control mode enum control_mode afs_mode(void) override; - - //to force entering auto mode when datalink loss - void set_mode_auto(void) override; }; #endif // AP_ADVANCEDFAILSAFE_ENABLED diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 4c3a35f4097cad..e65676979b1e27 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -28,27 +28,6 @@ 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 79887fabdeb3cc..5444a5c93d018b 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -210,7 +210,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) #endif case MAV_CMD_NAV_DELAY: - mode_auto.do_nav_delay(cmd); + do_nav_delay(cmd); break; default: @@ -315,7 +315,7 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret #endif case MAV_CMD_NAV_DELAY: - return mode_auto.verify_nav_delay(cmd); + return verify_nav_delay(cmd); // do commands (always return true) case MAV_CMD_DO_CHANGE_SPEED: @@ -348,7 +348,7 @@ void Plane::do_RTL(int32_t rtl_altitude_AMSL_cm) auto_state.next_wp_crosstrack = false; auto_state.crosstrack = false; prev_WP_loc = current_loc; - next_WP_loc = calc_best_rally_or_home_location(current_loc, rtl_altitude_AMSL_cm); + next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, rtl_altitude_AMSL_cm); setup_terrain_target_alt(next_WP_loc); set_target_altitude_location(next_WP_loc); @@ -364,17 +364,6 @@ void Plane::do_RTL(int32_t rtl_altitude_AMSL_cm) logger.Write_Mode(control_mode->mode_number(), control_mode_reason); } -Location Plane::calc_best_rally_or_home_location(const Location &_current_loc, float rtl_home_alt_amsl_cm) const -{ -#if HAL_RALLY_ENABLED - return plane.rally.calc_best_rally_or_home_location(_current_loc, rtl_home_alt_amsl_cm); -#else - Location destination = plane.home; - destination.set_alt_cm(rtl_home_alt_amsl_cm, Location::AltFrame::ABSOLUTE); - return destination; -#endif -} - /* start a NAV_TAKEOFF command */ @@ -537,7 +526,7 @@ void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) } // do_nav_delay - Delay the next navigation command -void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd) +void Plane::do_nav_delay(const AP_Mission::Mission_Command& cmd) { nav_delay.time_start_ms = millis(); @@ -546,11 +535,7 @@ void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd) nav_delay.time_max_ms = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds } else { // absolute delay to utc time -#if AP_RTC_ENABLED nav_delay.time_max_ms = AP::rtc().get_time_utc(cmd.content.nav_delay.hour_utc, cmd.content.nav_delay.min_utc, cmd.content.nav_delay.sec_utc, 0); -#else - nav_delay.time_max_ms = 0; -#endif } gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec", (unsigned)(nav_delay.time_max_ms/1000)); } @@ -560,11 +545,7 @@ void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd) /********************************************************************************/ bool Plane::verify_takeoff() { - bool trust_ahrs_yaw = AP::ahrs().initialised(); -#if AP_AHRS_DCM_ENABLED - trust_ahrs_yaw |= ahrs.dcm_yaw_initialised(); -#endif - if (trust_ahrs_yaw && steer_state.hold_course_cd == -1) { + if (ahrs.dcm_yaw_initialised() && steer_state.hold_course_cd == -1) { const float min_gps_speed = 5; if (auto_state.takeoff_speed_time_ms == 0 && gps.status() >= AP_GPS::GPS_OK_FIX_3D && @@ -892,9 +873,9 @@ bool Plane::verify_altitude_wait(const AP_Mission::Mission_Command &cmd) } // verify_nav_delay - check if we have waited long enough -bool ModeAuto::verify_nav_delay(const AP_Mission::Mission_Command& cmd) +bool Plane::verify_nav_delay(const AP_Mission::Mission_Command& cmd) { - if (AP::arming().is_armed_and_safety_off()) { + if (arming.is_armed_and_safety_off()) { // don't delay while armed, we need a nav controller running return true; } @@ -958,36 +939,28 @@ void Plane::do_loiter_at_location() bool Plane::do_change_speed(const AP_Mission::Mission_Command& cmd) { - return do_change_speed( - (uint8_t)cmd.content.speed.speed_type, - cmd.content.speed.target_ms, - cmd.content.speed.throttle_pct - ); -} - -bool Plane::do_change_speed(uint8_t speedtype, float speed_target_ms, float throttle_pct) -{ - switch (speedtype) { + switch (cmd.content.speed.speed_type) + { case 0: // Airspeed - if (is_equal(speed_target_ms, -2.0f)) { + if (is_equal(cmd.content.speed.target_ms, -2.0f)) { new_airspeed_cm = -1; // return to default airspeed return true; - } else if ((speed_target_ms >= aparm.airspeed_min.get()) && - (speed_target_ms <= aparm.airspeed_max.get())) { - new_airspeed_cm = speed_target_ms * 100; //new airspeed target for AUTO or GUIDED modes - gcs().send_text(MAV_SEVERITY_INFO, "Set airspeed %u m/s", (unsigned)speed_target_ms); + } else if ((cmd.content.speed.target_ms >= aparm.airspeed_min.get()) && + (cmd.content.speed.target_ms <= aparm.airspeed_max.get())) { + new_airspeed_cm = cmd.content.speed.target_ms * 100; //new airspeed target for AUTO or GUIDED modes + gcs().send_text(MAV_SEVERITY_INFO, "Set airspeed %u m/s", (unsigned)cmd.content.speed.target_ms); return true; } break; case 1: // Ground speed - gcs().send_text(MAV_SEVERITY_INFO, "Set groundspeed %u", (unsigned)speed_target_ms); - aparm.min_gndspeed_cm.set(speed_target_ms * 100); + gcs().send_text(MAV_SEVERITY_INFO, "Set groundspeed %u", (unsigned)cmd.content.speed.target_ms); + aparm.min_gndspeed_cm.set(cmd.content.speed.target_ms * 100); return true; } - if (throttle_pct > 0 && throttle_pct <= 100) { - gcs().send_text(MAV_SEVERITY_INFO, "Set throttle %u", (unsigned)throttle_pct); - aparm.throttle_cruise.set(throttle_pct); + if (cmd.content.speed.throttle_pct > 0 && cmd.content.speed.throttle_pct <= 100) { + gcs().send_text(MAV_SEVERITY_INFO, "Set throttle %u", (unsigned)cmd.content.speed.throttle_pct); + aparm.throttle_cruise.set(cmd.content.speed.throttle_pct); return true; } @@ -1082,7 +1055,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); @@ -1098,7 +1071,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 a8c6c890c96cb0..ba29c2062d8015 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -238,7 +238,7 @@ #endif #ifndef OFFBOARD_GUIDED - #define OFFBOARD_GUIDED 1 + #define OFFBOARD_GUIDED !HAL_MINIMIZE_FEATURES #endif ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp index 7f15e0e2663f90..8caeae82042799 100644 --- a/ArduPlane/control_modes.cpp +++ b/ArduPlane/control_modes.cpp @@ -98,23 +98,61 @@ Mode *Plane::mode_from_mode_num(const enum Mode::Number num) return ret; } -void RC_Channels_Plane::read_mode_switch() +void Plane::read_control_switch() { - if (millis() - plane.failsafe.last_valid_rc_ms > 100) { + 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) { // only use signals that are less than 0.1s old. return; } - RC_Channels::read_mode_switch(); + + 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; + } -void RC_Channel_Plane::mode_switch_changed(modeswitch_pos_t new_pos) +uint8_t Plane::readSwitch(void) const { - if (new_pos < 0 || (uint8_t)new_pos > plane.num_flight_modes) { - // should not have been called - return; - } + 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 +} - plane.set_mode_by_number((Mode::Number)plane.flight_modes[new_pos].get(), ModeReason::RC_COMMAND); +void Plane::reset_control_switch() +{ + oldSwitchPosition = 254; + read_control_switch(); } /* diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index c7389daf49bdca..e880eef494b3a0 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -8,8 +8,6 @@ #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 { @@ -44,7 +42,6 @@ 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 @@ -123,7 +120,6 @@ 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, @@ -165,7 +161,7 @@ enum FlightOptions { CENTER_THROTTLE_TRIM = (1<<10), DISABLE_GROUND_PID_SUPPRESSION = (1<<11), ENABLE_LOITER_ALT_CONTROL = (1<<12), - INDICATE_WAITING_FOR_RUDDER_NEUTRAL = (1<<13), + }; enum CrowFlapOptions { diff --git a/ArduPlane/ekf_check.cpp b/ArduPlane/ekf_check.cpp index e36e4ef1abcdf7..86b972225ce76a 100644 --- a/ArduPlane/ekf_check.cpp +++ b/ArduPlane/ekf_check.cpp @@ -112,7 +112,7 @@ bool Plane::ekf_over_threshold() return false; } - // Get EKF innovations normalised wrt the innovation test limits. + // Get EKF innovations normalised wrt the innovaton test limits. // A value above 1.0 means the EKF has rejected that sensor data float position_variance, vel_variance, height_variance, tas_variance; Vector3f mag_variance; @@ -157,7 +157,7 @@ void Plane::failsafe_ekf_event() ekf_check_state.failsafe_on = true; AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED); - // if not in a VTOL mode requiring position, then nothing needs to be done + // if not in a VTOL mode requring position, then nothing needs to be done #if HAL_QUADPLANE_ENABLED if (!quadplane.in_vtol_posvel_mode()) { return; diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index f49814aca9b903..9c2a215608ce78 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -107,7 +107,6 @@ 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(); @@ -125,14 +124,6 @@ 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; @@ -140,13 +131,9 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) { #if PARACHUTE == ENABLED parachute_release(); - //stop motors to avoid parachute tangling - plane.arming.disarm(AP_Arming::Method::PARACHUTE_RELEASE, false); #endif } else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) { set_mode(mode_fbwa, reason); - } else if (g.fs_action_long == FS_ACTION_LONG_AUTO) { - set_mode(mode_auto, reason); } else { set_mode(mode_rtl, reason); } @@ -179,32 +166,24 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason case Mode::Number::AVOID_ADSB: case Mode::Number::GUIDED: - if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) { #if PARACHUTE == ENABLED parachute_release(); - //stop motors to avoid parachute tangling - plane.arming.disarm(AP_Arming::Method::PARACHUTE_RELEASE, false); #endif } else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) { set_mode(mode_fbwa, reason); - } else if (g.fs_action_long == FS_ACTION_LONG_AUTO) { - set_mode(mode_auto, reason); } else if (g.fs_action_long == FS_ACTION_LONG_RTL) { set_mode(mode_rtl, reason); } break; case Mode::Number::RTL: - if (g.fs_action_long == FS_ACTION_LONG_AUTO) { - set_mode(mode_auto, reason); - } - break; #if HAL_QUADPLANE_ENABLED case Mode::Number::QLAND: case Mode::Number::QRTL: case Mode::Number::LOITER_ALT_QLAND: #endif + case Mode::Number::TAKEOFF: case Mode::Number::INITIALISING: break; } @@ -225,7 +204,6 @@ void Plane::failsafe_short_off_event(ModeReason reason) void Plane::failsafe_long_off_event(ModeReason reason) { - long_failsafe_pending = false; // We're back in radio contact with RC or GCS if (reason == ModeReason:: GCS_FAILSAFE) { gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe Off"); diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index 640ad65869926d..28f4abdca677e9 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -75,7 +75,7 @@ void Plane::fence_check() Location loc; if (fence.get_return_rally() != 0 || fence_act == AC_FENCE_ACTION_RTL_AND_LAND) { - loc = calc_best_rally_or_home_location(current_loc, get_RTL_altitude_cm()); + loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude_cm()); } else { //return to fence return point, not a rally point if (fence.get_return_altitude() > 0) { diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index ffe578736aff14..3b2c27396f59d0 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -261,7 +261,7 @@ void Plane::crash_detection_update(void) if (g.takeoff_throttle_min_accel > 0 && !throttle_suppressed) { // if you have an acceleration holding back throttle, but you met the - // accel threshold but still not flying, then you either shook/hit the + // accel threshold but still not fying, then you either shook/hit the // plane or it was a failed launch. crashed = true; crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS; diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index 068e5c5ec8849f..09564caed37798 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -32,9 +32,6 @@ 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; @@ -89,9 +86,6 @@ 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(); @@ -208,40 +202,3 @@ 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 a88dbec5b6e3c1..46e56c284c4a7d 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -8,7 +8,6 @@ #include #include "quadplane.h" #include -#include class AC_PosControl; class AC_AttitudeControl_Multi; @@ -66,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; @@ -80,9 +79,6 @@ 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 // @@ -129,13 +125,6 @@ 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 @@ -147,9 +136,6 @@ 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; @@ -220,25 +206,11 @@ class ModeAuto : public Mode bool mode_allows_autotuning() const override { return true; } - bool is_landing() const override; - - void do_nav_delay(const AP_Mission::Mission_Command& cmd); - bool verify_nav_delay(const AP_Mission::Mission_Command& cmd); - protected: bool _enter() override; void _exit() override; bool _pre_arm_checks(size_t buflen, char *buffer) const override; - -private: - - // Delay the next navigation command - struct { - uint32_t time_max_ms; - uint32_t time_start_ms; - } nav_delay; - }; @@ -383,8 +355,6 @@ class ModeManual : public Mode // methods that affect movement of the vehicle in this mode void update() override; - - void run() override; }; @@ -428,12 +398,6 @@ 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 c15970b9bdecec..5bfeea2f81b920 100644 --- a/ArduPlane/mode_acro.cpp +++ b/ArduPlane/mode_acro.cpp @@ -68,7 +68,7 @@ void ModeAcro::stabilize() int32_t roll_error_cd = -ToDeg(acro_state.locked_roll_err)*100; plane.nav_roll_cd = ahrs.roll_sensor + roll_error_cd; // try to reduce the integrated angular error to zero. We set - // 'stabilize' to true, which disables the roll integrator + // 'stabilze' to true, which disables the roll integrator SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.rollController.get_servo_out(roll_error_cd, speed_scaler, true, false)); @@ -104,24 +104,22 @@ void ModeAcro::stabilize() SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitchController.get_rate_out(pitch_rate, speed_scaler)); } - float rudder_output; + plane.steering_control.steering = plane.rudder_input(); + 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; - rudder_output = plane.yawController.get_rate_out(yaw_rate, speed_scaler, false); - } else if (plane.flight_option_enabled(FlightOptions::ACRO_YAW_DAMPER)) { + 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) { // use yaw controller - rudder_output = plane.calc_nav_yaw_coordinated(); + plane.calc_nav_yaw_coordinated(); } else { /* manual rudder */ - rudder_output = plane.rudder_input(); + plane.steering_control.rudder = plane.steering_control.steering; } - - output_rudder_and_steering(rudder_output); - } /* @@ -217,7 +215,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)); - output_rudder_and_steering(plane.yawController.get_rate_out(desired_rates.z, speed_scaler, false)); + plane.steering_control.steering = plane.steering_control.rudder = 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 6ebf3546ef1690..548dd0b2618b8a 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -159,8 +159,3 @@ 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 a523ddf87065ed..900b084f8ca596 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -35,62 +35,9 @@ void ModeGuided::update() return; } #endif - - // Received an external msg that guides roll in the last 3 seconds? - if (plane.guided_state.last_forced_rpy_ms.x > 0 && - millis() - plane.guided_state.last_forced_rpy_ms.x < 3000) { - plane.nav_roll_cd = constrain_int32(plane.guided_state.forced_rpy_cd.x, -plane.roll_limit_cd, plane.roll_limit_cd); - plane.update_load_factor(); - -#if OFFBOARD_GUIDED == ENABLED - // guided_state.target_heading is radians at this point between -pi and pi ( defaults to -4 ) - // This function is used in Guided and AvoidADSB, check for guided - } else if ((plane.control_mode == &plane.mode_guided) && (plane.guided_state.target_heading_type != GUIDED_HEADING_NONE) ) { - uint32_t tnow = AP_HAL::millis(); - float delta = (tnow - plane.guided_state.target_heading_time_ms) * 1e-3f; - plane.guided_state.target_heading_time_ms = tnow; - - float error = 0.0f; - if (plane.guided_state.target_heading_type == GUIDED_HEADING_HEADING) { - error = wrap_PI(plane.guided_state.target_heading - AP::ahrs().yaw); - } else { - Vector2f groundspeed = AP::ahrs().groundspeed_vector(); - error = wrap_PI(plane.guided_state.target_heading - atan2f(-groundspeed.y, -groundspeed.x) + M_PI); - } - - float bank_limit = degrees(atanf(plane.guided_state.target_heading_accel_limit/GRAVITY_MSS)) * 1e2f; - bank_limit = MIN(bank_limit, plane.roll_limit_cd); - - // push error into AC_PID - const float desired = plane.g2.guidedHeading.update_error(error, delta, plane.guided_state.target_heading_limit); - - // Check for output saturation - plane.guided_state.target_heading_limit = fabsf(desired) >= bank_limit; - - plane.nav_roll_cd = constrain_int32(desired, -bank_limit, bank_limit); - plane.update_load_factor(); - -#endif // OFFBOARD_GUIDED == ENABLED - } else { - plane.calc_nav_roll(); - } - - if (plane.guided_state.last_forced_rpy_ms.y > 0 && - millis() - plane.guided_state.last_forced_rpy_ms.y < 3000) { - plane.nav_pitch_cd = constrain_int32(plane.guided_state.forced_rpy_cd.y, plane.pitch_limit_min_cd, plane.aparm.pitch_limit_max_cd.get()); - } else { - plane.calc_nav_pitch(); - } - - // Received an external msg that guides throttle in the last 3 seconds? - if (plane.aparm.throttle_cruise > 1 && - plane.guided_state.last_forced_throttle_ms > 0 && - millis() - plane.guided_state.last_forced_throttle_ms < 3000) { - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.guided_state.forced_throttle); - } else { - plane.calc_throttle(); - } - + plane.calc_nav_roll(); + plane.calc_nav_pitch(); + plane.calc_throttle(); } void ModeGuided::navigate() diff --git a/ArduPlane/mode_loiter.cpp b/ArduPlane/mode_loiter.cpp index 05ce3a69568572..056d1e715562fd 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.flight_option_enabled(FlightOptions::ENABLE_LOITER_ALT_CONTROL))) { + if (plane.stick_mixing_enabled() && (plane.g2.flight_options & 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.flight_option_enabled(FlightOptions::ENABLE_LOITER_ALT_CONTROL)) { + if (plane.stick_mixing_enabled() && (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL)) { plane.update_fbwb_speed_height(); } else { plane.calc_nav_pitch(); @@ -93,7 +93,7 @@ bool ModeLoiter::isHeadingLinedUp_cd(const int32_t bearing_cd) void ModeLoiter::navigate() { - if (plane.flight_option_enabled(FlightOptions::ENABLE_LOITER_ALT_CONTROL)) { + if (plane.g2.flight_options & 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); } @@ -111,7 +111,7 @@ void ModeLoiter::navigate() void ModeLoiter::update_target_altitude() { - if (plane.stick_mixing_enabled() && (plane.flight_option_enabled(FlightOptions::ENABLE_LOITER_ALT_CONTROL))) { + if (plane.stick_mixing_enabled() && (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL)) { return; } Mode::update_target_altitude(); diff --git a/ArduPlane/mode_manual.cpp b/ArduPlane/mode_manual.cpp index 1f8fcd702cd14e..68535f96186ba7 100644 --- a/ArduPlane/mode_manual.cpp +++ b/ArduPlane/mode_manual.cpp @@ -5,30 +5,9 @@ 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)); - 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.steering_control.steering = plane.steering_control.rudder = plane.rudder_in_expo(false); 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 d49e1e0fb6152f..d1c69de4e3e0df 100644 --- a/ArduPlane/mode_qacro.cpp +++ b/ArduPlane/mode_qacro.cpp @@ -9,7 +9,7 @@ bool ModeQAcro::_enter() quadplane.transition->force_transition_complete(); attitude_control->relax_attitude_controllers(); - // disable yaw rate time constant to maintain old behaviour + // disable yaw rate time contant to mantain old behaviour quadplane.disable_yaw_rate_time_constant(); IGNORE_RETURN(ahrs.get_quaternion(plane.mode_acro.acro_state.q)); @@ -32,13 +32,6 @@ 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); @@ -71,9 +64,6 @@ 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 28bf977128faf2..d74cf792b5d05c 100644 --- a/ArduPlane/mode_qautotune.cpp +++ b/ArduPlane/mode_qautotune.cpp @@ -21,20 +21,9 @@ 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 fac2147f93c455..13c1a4c4e65edb 100644 --- a/ArduPlane/mode_qhover.cpp +++ b/ArduPlane/mode_qhover.cpp @@ -24,26 +24,14 @@ void ModeQHover::update() */ void ModeQHover::run() { - const uint32_t now = AP_HAL::millis(); - if (quadplane.tailsitter.in_vtol_transition(now)) { - // Tailsitters in FW pull up phase of VTOL transition run FW controllers - Mode::run(); - return; - } - if (quadplane.throttle_wait) { quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); attitude_control->set_throttle_out(0, true, 0); quadplane.relax_attitude_control(); pos_control->relax_z_controller(0); } else { - plane.quadplane.assign_tilt_to_fwd_thr(); quadplane.hold_hover(quadplane.get_pilot_desired_climb_rate_cms()); } - - // Stabilize with fixed wing surfaces - plane.stabilize_roll(); - plane.stabilize_pitch(); } #endif diff --git a/ArduPlane/mode_qloiter.cpp b/ArduPlane/mode_qloiter.cpp index 50dd8deea749bb..26ef2bf214f33c 100644 --- a/ArduPlane/mode_qloiter.cpp +++ b/ArduPlane/mode_qloiter.cpp @@ -28,13 +28,6 @@ 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); @@ -42,10 +35,6 @@ 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()) { @@ -56,6 +45,7 @@ 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(); @@ -83,8 +73,6 @@ void ModeQLoiter::run() plane.nav_roll_cd = loiter_nav->get_roll(); plane.nav_pitch_cd = loiter_nav->get_pitch(); - plane.quadplane.assign_tilt_to_fwd_thr(); - if (quadplane.transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) { pos_control->set_externally_limited_xy(); } @@ -124,10 +112,6 @@ 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 9be1c962bd6a4b..98bfe3a28342b0 100644 --- a/ArduPlane/mode_qrtl.cpp +++ b/ArduPlane/mode_qrtl.cpp @@ -17,7 +17,8 @@ bool ModeQRTL::_enter() int32_t RTL_alt_abs_cm = plane.home.alt + quadplane.qrtl_alt*100UL; if (quadplane.motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) { // VTOL motors are active, either in VTOL flight or assisted flight - Location destination = plane.calc_best_rally_or_home_location(plane.current_loc, RTL_alt_abs_cm); + Location destination = plane.rally.calc_best_rally_or_home_location(plane.current_loc, RTL_alt_abs_cm); + const float dist = plane.current_loc.get_distance(destination); const float radius = get_VTOL_return_radius(); @@ -86,13 +87,6 @@ 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 @@ -103,9 +97,6 @@ void ModeQRTL::run() // nav roll and pitch are controller by position controller plane.nav_roll_cd = pos_control->get_roll_cd(); plane.nav_pitch_cd = pos_control->get_pitch_cd(); - - plane.quadplane.assign_tilt_to_fwd_thr(); - if (quadplane.transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) { pos_control->set_externally_limited_xy(); } @@ -126,12 +117,12 @@ void ModeQRTL::run() ftype alt_diff; if (!stopping_loc.get_alt_distance(plane.next_WP_loc, alt_diff) || is_positive(alt_diff)) { - // climb finished or cant get alt diff, head home + // climb finshed or cant get alt diff, head home submode = SubMode::RTL; plane.prev_WP_loc = plane.current_loc; int32_t RTL_alt_abs_cm = plane.home.alt + quadplane.qrtl_alt*100UL; - Location destination = plane.calc_best_rally_or_home_location(plane.current_loc, RTL_alt_abs_cm); + Location destination = plane.rally.calc_best_rally_or_home_location(plane.current_loc, RTL_alt_abs_cm); const float dist = plane.current_loc.get_distance(destination); const float radius = get_VTOL_return_radius(); if (dist < radius) { @@ -175,10 +166,6 @@ void ModeQRTL::run() break; } } - - // Stabilize with fixed wing surfaces - plane.stabilize_roll(); - plane.stabilize_pitch(); } /* @@ -198,7 +185,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(float(plane.aparm.loiter_radius)), fabsf(float(plane.g.rtl_radius))); + const float radius = MAX(fabsf(plane.aparm.loiter_radius), fabsf(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; @@ -223,7 +210,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(float(plane.aparm.loiter_radius)), fabsf(float(plane.g.rtl_radius))) * 1.5; + return MAX(fabsf(plane.aparm.loiter_radius), fabsf(plane.g.rtl_radius)) * 1.5; } #endif diff --git a/ArduPlane/mode_qstabilize.cpp b/ArduPlane/mode_qstabilize.cpp index 2f78a210fb3c2d..2c8da2d96885aa 100644 --- a/ArduPlane/mode_qstabilize.cpp +++ b/ArduPlane/mode_qstabilize.cpp @@ -40,30 +40,15 @@ void ModeQStabilize::update() // quadplane stabilize mode void ModeQStabilize::run() { - const uint32_t now = AP_HAL::millis(); - if (quadplane.tailsitter.in_vtol_transition(now)) { - // Tailsitters in FW pull up phase of VTOL transition run FW controllers - Mode::run(); - return; - } - - plane.quadplane.assign_tilt_to_fwd_thr(); - // special check for ESC calibration in QSTABILIZE if (quadplane.esc_calibration != 0) { quadplane.run_esc_calibration(); - plane.stabilize_roll(); - plane.stabilize_pitch(); return; } // normal QSTABILIZE mode float pilot_throttle_scaled = quadplane.get_pilot_throttle(); quadplane.hold_stabilize(pilot_throttle_scaled); - - // Stabilize with fixed wing surfaces - plane.stabilize_roll(); - plane.stabilize_pitch(); } // set the desired roll and pitch for a tailsitter @@ -83,7 +68,7 @@ void ModeQStabilize::set_tailsitter_roll_pitch(const float roll_input, const flo plane.quadplane.transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd); } -// set the desired roll and pitch for normal quadplanes, also limited by forward flight limits +// set the desired roll and pitch for normal quadplanes, also limited by forward flight limtis void ModeQStabilize::set_limited_roll_pitch(const float roll_input, const float pitch_input) { plane.nav_roll_cd = roll_input * MIN(plane.roll_limit_cd, plane.quadplane.aparm.angle_max); diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp index 417eb6170f22ef..fbe368b2bc573b 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.flight_option_enabled(FlightOptions::CLIMB_BEFORE_TURN)) { + if (plane.g2.flight_options & 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 b73dfb6fc26506..7414d575065724 100644 --- a/ArduPlane/mode_stabilize.cpp +++ b/ArduPlane/mode_stabilize.cpp @@ -7,10 +7,3 @@ 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 db212361c398e8..834fa93de97a45 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -1,6 +1,5 @@ #include "mode.h" #include "Plane.h" -#include /* mode takeoff parameters @@ -35,7 +34,7 @@ const AP_Param::GroupInfo ModeTakeoff::var_info[] = { // @Param: DIST // @DisplayName: Takeoff mode distance - // @Description: This is the distance from the takeoff location where the plane will loiter. The loiter point will be in the direction of takeoff (the direction the plane is facing when the plane begins takeoff) + // @Description: This is the distance from the takeoff location where the plane will loiter. The loiter point will be in the direction of takeoff (the direction the plane is facing when the motor starts) // @Range: 0 500 // @Increment: 1 // @Units: m @@ -151,11 +150,6 @@ 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 23c81fbcc288f8..9215fe0aba34d5 100644 --- a/ArduPlane/mode_training.cpp +++ b/ArduPlane/mode_training.cpp @@ -63,7 +63,5 @@ void ModeTraining::run() } } - // Always manual rudder control - output_rudder_and_steering(plane.rudder_in_expo(false)); - + plane.stabilize_yaw(); } diff --git a/ArduPlane/motor_test.cpp b/ArduPlane/motor_test.cpp index 38229d2f351344..cfc640cab9859e 100644 --- a/ArduPlane/motor_test.cpp +++ b/ArduPlane/motor_test.cpp @@ -67,7 +67,7 @@ void QuadPlane::motor_test_output() // sanity check throttle values if (pwm >= RC_Channel::RC_MIN_LIMIT_PWM && pwm <= RC_Channel::RC_MAX_LIMIT_PWM) { - // turn on motor to specified pwm value + // turn on motor to specified pwm vlaue motors->output_test_seq(motor_test.seq, pwm); } else { motor_test_stop(); @@ -87,14 +87,6 @@ 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 76846b0fe6ed64..edb65fc2c541d7 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -58,7 +58,7 @@ void Plane::loiter_angle_update(void) } } if (terrain_status_ok && - fabsF(altitude_agl - target_altitude.terrain_alt_cm*0.01) < 5) { + fabsf(altitude_agl - target_altitude.terrain_alt_cm*0.01) < 5) { reached_target_alt = true; } else #endif @@ -73,7 +73,7 @@ void Plane::loiter_angle_update(void) loiter.next_sum_lap_cd = loiter.sum_cd + lap_check_interval_cd; } else if (!loiter.reached_target_alt && labs(loiter.sum_cd) >= loiter.next_sum_lap_cd) { - // check every few laps for scenario where up/downward inhibit you from loitering up/down for too long + // check every few laps for scenario where up/downdrafts inhibit you from loitering up/down for too long loiter.unable_to_acheive_target_alt = labs(current_loc.alt - loiter.start_lap_alt_cm) < 500; loiter.start_lap_alt_cm = current_loc.alt; loiter.next_sum_lap_cd += lap_check_interval_cd; @@ -95,8 +95,6 @@ void Plane::navigate() return; } - check_home_alt_change(); - // waypoint distance from plane // ---------------------------- auto_state.wp_distance = current_loc.get_distance(next_WP_loc); @@ -159,9 +157,9 @@ void Plane::calc_airspeed_errors() // FBW_B/cruise airspeed target if (!failsafe.rc_failsafe && (control_mode == &mode_fbwb || control_mode == &mode_cruise)) { - if (flight_option_enabled(FlightOptions::CRUISE_TRIM_AIRSPEED)) { + if (g2.flight_options & FlightOptions::CRUISE_TRIM_AIRSPEED) { target_airspeed_cm = aparm.airspeed_cruise_cm; - } else if (flight_option_enabled(FlightOptions::CRUISE_TRIM_THROTTLE)) { + } else if (g2.flight_options & FlightOptions::CRUISE_TRIM_THROTTLE) { float control_min = 0.0f; float control_mid = 0.0f; const float control_max = channel_throttle->get_range(); @@ -188,7 +186,7 @@ void Plane::calc_airspeed_errors() get_throttle_input()) + ((int32_t)aparm.airspeed_min * 100); } #if OFFBOARD_GUIDED == ENABLED - } else if (control_mode == &mode_guided && guided_state.target_airspeed_cm > 0.0) { // if offboard guided speed change cmd not set, then this section is skipped + } else if (control_mode == &mode_guided && guided_state.target_airspeed_cm > 0.0) { // if offbd guided speed change cmd not set, then this section is skipped // offboard airspeed demanded uint32_t now = AP_HAL::millis(); float delta = 1e-3f * (now - guided_state.target_airspeed_time_ms); @@ -246,10 +244,9 @@ void Plane::calc_airspeed_errors() // but only when this is faster than the target airspeed commanded // above. if (control_mode->does_auto_throttle() && - 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; + aparm.min_gndspeed_cm > 0 && + control_mode != &mode_circle) { + int32_t min_gnd_target_airspeed = airspeed_measured*100 + groundspeed_undershoot; if (min_gnd_target_airspeed > target_airspeed_cm) { target_airspeed_cm = min_gnd_target_airspeed; } @@ -279,18 +276,16 @@ void Plane::calc_gndspeed_undershoot() { // Use the component of ground speed in the forward direction // This prevents flyaway if wind takes plane backwards - Vector3f velNED; - if (ahrs.have_inertial_nav() && ahrs.get_velocity_NED(velNED)) { + if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) { + Vector2f gndVel = ahrs.groundspeed_vector(); 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 * velNED.xy(); - groundspeed_undershoot_is_valid = aparm.min_gndspeed_cm > 0; - groundspeed_undershoot = groundspeed_undershoot_is_valid ? (aparm.min_gndspeed_cm - gndSpdFwd*100) : 0; + float gndSpdFwd = yawVect * gndVel; + groundspeed_undershoot = (aparm.min_gndspeed_cm > 0) ? (aparm.min_gndspeed_cm - gndSpdFwd*100) : 0; } } else { - groundspeed_undershoot_is_valid = false; groundspeed_undershoot = 0; } } diff --git a/ArduPlane/qautotune.h b/ArduPlane/qautotune.h index 3ec890ac3dc3f9..1a1bf6e873fdc2 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 + #define QAUTOTUNE_ENABLED HAL_QUADPLANE_ENABLED && !HAL_MINIMIZE_FEATURES #endif #if QAUTOTUNE_ENABLED diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index cf51a688386818..b52688af910df3 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -33,7 +33,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @DisplayName: Transition time // @Description: Transition time in milliseconds after minimum airspeed is reached // @Units: ms - // @Range: 2000 30000 + // @Range: 1 30000 // @User: Advanced AP_GROUPINFO("TRANSITION_MS", 11, QuadPlane, transition_time_ms, 5000), @@ -167,7 +167,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Param: VFWD_GAIN // @DisplayName: Forward velocity hold gain - // @Description: The use of this parameter is no longer recommended and has been superseded by a method that works in all VTOL modes with the exception of autotune which is controlled by the Q_FWD_THR_USE parameter. This Q_VFD_GAIN parameter controls use of the forward motor in VTOL modes that use the velocity controller. Set to 0 to disable this function. A value of 0.05 is a good place to start if you want to use the forward motor for position control. No forward motor will be used in QSTABILIZE or QHOVER modes. Use QLOITER for position hold with the forward motor. + // @Description: Controls use of forward motor in vtol modes. If this is zero then the forward motor will not be used for position control in VTOL modes. A value of 0.05 is a good place to start if you want to use the forward motor for position control. No forward motor will be used in QSTABILIZE or QHOVER modes. Use QLOITER for position hold with the forward motor. // @Range: 0 0.5 // @Increment: 0.01 // @User: Standard @@ -269,7 +269,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Bitmask: 8: Mtrs_Only_Qassist-in tailsitters only uses VTOL motors and not flying surfaces for QASSIST // @Bitmask: 10: Disarmed Yaw Tilt-enable motor tilt for yaw when disarmed // @Bitmask: 11: Delay Spoolup-delay VTOL spoolup for 2 seconds after arming - // @Bitmask: 12: Disable speed based Qassist when using synthethic airspeed estimates + // @Bitmask: 12: Disable Qassist-based on synthetic airspeed even if airspeed sensor is used // @Bitmask: 13: Disable Ground Effect Compensation-on baro altitude reports // @Bitmask: 14: Ignore forward flight angle limits-in Qmodes and use Q_ANGLE_MAX exclusively // @Bitmask: 15: ThrLandControl-enable throttle stick control of landing rate @@ -279,7 +279,6 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Bitmask: 19: CompleteTransition-to fixed wing if Q_TRANS_FAIL timer times out instead of QLAND // @Bitmask: 20: Force RTL mode-forces RTL mode on rc failsafe in VTOL modes overriding bit 5(USE_QRTL) // @Bitmask: 21: Tilt rotor-tilt motors up when disarmed in FW modes (except manual) to prevent ground strikes. - // @Bitmask: 22: Scale FF by the ratio of VTOL/plane angle P gains in VTOL modes rather than reducing VTOL angle P based on airspeed. AP_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0), AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2), @@ -400,7 +399,7 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @Param: ASSIST_ALT // @DisplayName: Quadplane assistance altitude - // @Description: This is the altitude below which quadplane assistance will be triggered. This acts the same way as Q_ASSIST_ANGLE and Q_ASSIST_SPEED, but triggers if the aircraft drops below the given altitude while the VTOL motors are not running. A value of zero disables this feature. The altitude is calculated as being above ground level. The height above ground is given from a Lidar used if available and RNGFND_LANDING=1. Otherwise it comes from terrain data if TERRAIN_FOLLOW=1 and comes from height above home otherwise. + // @Description: This is the altitude below which quadplane assistance will be triggered. This acts the same way as Q_ASSIST_ANGLE and Q_ASSIST_SPEED, but triggers if the aircraft drops below the given altitude while the VTOL motors are not running. A value of zero disables this feature. The altutude is calculated as being above ground level. The height above ground is given from a Lidar used if available and RNGFND_LANDING=1. Otherwise it comes from terrain data if TERRAIN_FOLLOW=1 and comes from height above home otherwise. // @Units: m // @Range: 0 120 // @Increment: 1 @@ -505,30 +504,6 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @User: Standard AP_GROUPINFO("RTL_ALT_MIN", 34, QuadPlane, qrtl_alt_min, 10), - // @Param: FWD_THR_GAIN - // @DisplayName: Q mode fwd throttle gain - // @Description: This parameter sets the gain from forward accel/tilt to forward throttle in Q modes. The Q modes this feature operates in is controlled by the Q_FWD_THR_USE parameter. Vehicles using separate forward thrust motors, eg quadplanes, should set this parameter to (all up weight) / (maximum combined thrust of forward motors) with a value of 2 being typical. Vehicles that tilt lifting rotors to provide forward thrust should set this parameter to (all up weight) / (weight lifted by tilting rotors) which for most aircraft can be approximated as (total number of lifting rotors) / (number of lifting rotors that tilt). When using this method of forward throttle control, the forward tilt angle limit is controlled by the Q_FWD_PIT_LIM parameter. - // @Range: 0.0 5.0 - // @Increment: 0.1 - // @User: Standard - AP_GROUPINFO("FWD_THR_GAIN", 35, QuadPlane, q_fwd_thr_gain, 2.0f), - - // @Param: FWD_PIT_LIM - // @DisplayName: Q mode forward pitch limit - // @Description: When forward throttle is being controlled by the Q_FWD_THR_GAIN parameter in Q modes, the vehicle forward (nose down) pitch rotation will be limited to the value specified by this parameter and the any additional forward acceleration required will be produced by use of the forward thrust motor(s) or tilting of moveable rotors. Larger values allow the vehicle to pitch more nose down. Set initially to the amount of nose down pitch required to remove wing lift. - // @Units: deg - // @Range: 0.0 5.0 - // @Increment: 0.1 - // @User: Standard - AP_GROUPINFO("FWD_PIT_LIM", 36, QuadPlane, q_fwd_pitch_lim, 3.0f), - - // @Param: FWD_THR_USE - // @DisplayName: Q mode forward throttle use - // @Description: This parameter determines when the feature that uses forward throttle instead of forward tilt is used. The amount of forward throttle is controlled by the Q_FWD_THR_GAIN parameter. The maximum amount of forward pitch allowed is controlled by the Q_FWD_PIT_LIM parameter. Q_FWD_THR_USE = 0 disables the feature. Q_FWD_THR_USE = 1 enables the feature in all position controlled modes such as QLOITER, QLAND, QRTL and VTOL TAKEOFF. Q_FWD_THR_USE = 2 enables the feature in all Q modes except QAUTOTUNE and QACRO. When enabling the feature, the legacy method of controlling forward throttle use via velocity controller error should be disabled by setting Q_VFWD_GAIN to 0. Do not use this feature with tailsitters. - // @Values: 0:Off,1:On in all position controlled Q modes,2:On in all Q modes except QAUTOTUNE and QACRO - // @User: Standard - AP_GROUPINFO("FWD_THR_USE", 37, QuadPlane, q_fwd_thr_use, uint8_t(FwdThrUse::OFF)), - AP_GROUPEND }; @@ -1699,15 +1674,14 @@ void SLT_Transition::update() // after airspeed is reached we degrade throttle over the // transition time, but continue to stabilize const uint32_t transition_timer_ms = now - transition_low_airspeed_ms; - const float trans_time_ms = constrain_float(quadplane.transition_time_ms,2000,30000); - if (transition_timer_ms > unsigned(trans_time_ms)) { + if (transition_timer_ms > (unsigned)quadplane.transition_time_ms) { transition_state = TRANSITION_DONE; in_forced_transition = false; transition_start_ms = 0; transition_low_airspeed_ms = 0; gcs().send_text(MAV_SEVERITY_INFO, "Transition done"); } - + float trans_time_ms = MAX((float)quadplane.transition_time_ms.get(),1); float transition_scale = (trans_time_ms - transition_timer_ms) / trans_time_ms; float throttle_scaled = last_throttle * transition_scale; @@ -2094,9 +2068,6 @@ bool QuadPlane::handle_do_vtol_transition(enum MAV_VTOL_STATE state) const gcs().send_text(MAV_SEVERITY_NOTICE, "Entered VTOL mode"); } plane.auto_state.vtol_mode = true; - // This is a precaution. It should be looked after by the call to QuadPlane::mode_enter(void) on mode entry. - plane.quadplane.q_fwd_throttle = 0.0f; - plane.quadplane.q_fwd_pitch_lim_cd = 100.0f * plane.quadplane.q_fwd_pitch_lim; return true; case MAV_VTOL_STATE_FW: @@ -2252,10 +2223,6 @@ void QuadPlane::run_xy_controller(float accel_limit) pos_control->init_xy_controller(); } pos_control->set_lean_angle_max_cd(MIN(4500, MAX(accel_to_angle(accel_limit)*100, aparm.angle_max))); - if (q_fwd_throttle > 0.95f) { - // prevent wind up of the velocity controller I term due to a saturated forward throttle - pos_control->set_externally_limited_xy(); - } pos_control->update_xy_controller(); } @@ -2400,7 +2367,7 @@ void QuadPlane::vtol_position_controller(void) case QPOS_APPROACH: if (in_vtol_mode()) { - // this means we're not running transition update code and + // this means we're not running update_transition() 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 @@ -2571,6 +2538,7 @@ void QuadPlane::vtol_position_controller(void) } } + if (poscontrol.get_state() == QPOS_APPROACH) { poscontrol_init_approach(); } @@ -2707,8 +2675,6 @@ void QuadPlane::vtol_position_controller(void) plane.nav_roll_cd = pos_control->get_roll_cd(); plane.nav_pitch_cd = pos_control->get_pitch_cd(); - assign_tilt_to_fwd_thr(); - if (transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) { pos_control->set_externally_limited_xy(); } @@ -2762,8 +2728,6 @@ void QuadPlane::vtol_position_controller(void) plane.nav_roll_cd = pos_control->get_roll_cd(); plane.nav_pitch_cd = pos_control->get_pitch_cd(); - assign_tilt_to_fwd_thr(); - if (transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) { pos_control->set_externally_limited_xy(); } @@ -2808,8 +2772,6 @@ void QuadPlane::vtol_position_controller(void) plane.nav_roll_cd = pos_control->get_roll_cd(); plane.nav_pitch_cd = pos_control->get_pitch_cd(); - assign_tilt_to_fwd_thr(); - // call attitude controller set_pilot_yaw_rate_time_constant(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, @@ -2933,111 +2895,6 @@ void QuadPlane::vtol_position_controller(void) } } -/* - determine which fwd throttle handling method is active - */ -QuadPlane::ActiveFwdThr QuadPlane::get_vfwd_method(void) const -{ - const bool have_fwd_thr_gain = is_positive(q_fwd_thr_gain); - const bool have_vfwd_gain = is_positive(vel_forward.gain); - -#if AP_ICENGINE_ENABLED - const auto ice_state = plane.g2.ice_control.get_state(); - if (ice_state != AP_ICEngine::ICE_DISABLED && ice_state != AP_ICEngine::ICE_RUNNING) { - // we need the engine running for fwd throttle - return ActiveFwdThr::NONE; - } -#endif - -#if QAUTOTUNE_ENABLED - if (plane.control_mode == &plane.mode_qautotune) { - return ActiveFwdThr::NONE; - } -#endif - - if (have_fwd_thr_gain) { - if (vfwd_enable_active) { - // user has used AUX function to activate new method - return ActiveFwdThr::NEW; - } - if (q_fwd_thr_use == FwdThrUse::ALL) { - return ActiveFwdThr::NEW; - } - if (q_fwd_thr_use == FwdThrUse::POSCTRL && pos_control->is_active_xy()) { - return ActiveFwdThr::NEW; - } - } - if (have_vfwd_gain && pos_control->is_active_xy()) { - return ActiveFwdThr::OLD; - } - return ActiveFwdThr::NONE; -} - -/* - map from pitch tilt to fwd throttle when enabled - */ -void QuadPlane::assign_tilt_to_fwd_thr(void) { - - const auto fwd_thr_active = get_vfwd_method(); - if (fwd_thr_active != ActiveFwdThr::NEW) { - q_fwd_throttle = 0.0f; - q_fwd_pitch_lim_cd = 100.0f * q_fwd_pitch_lim; - return; - } - // Handle the case where we are limiting the forward pitch angle to prevent negative wing lift - // and are using the forward thrust motor or tilting rotors to provide the forward acceleration - float fwd_tilt_rad = radians(constrain_float(-0.01f * (float)plane.nav_pitch_cd, 0.0f, 45.0f)); - q_fwd_throttle = MIN(q_fwd_thr_gain * tanf(fwd_tilt_rad), 1.0f); - - // Relax forward tilt limit if the position controller is saturating in the forward direction because - // the forward thrust motor could be failed - const float fwd_tilt_range_cd = (float)aparm.angle_max - 100.0f * q_fwd_pitch_lim; - if (is_positive(fwd_tilt_range_cd)) { - // rate limit the forward tilt change to slew between the motor good and motor failed - // value over 10 seconds - const bool fwd_limited = plane.quadplane.pos_control->is_active_xy() and plane.quadplane.pos_control->get_fwd_pitch_is_limited(); - const float fwd_pitch_lim_cd_tgt = fwd_limited ? (float)aparm.angle_max : 100.0f * q_fwd_pitch_lim; - const float delta_max = 0.1f * fwd_tilt_range_cd * plane.G_Dt; - q_fwd_pitch_lim_cd += constrain_float((fwd_pitch_lim_cd_tgt - q_fwd_pitch_lim_cd), -delta_max, delta_max); - // Don't let the forward pitch limit be more than the forward pitch demand before limiting to - // avoid opening up the limit more than necessary - q_fwd_pitch_lim_cd = MIN(q_fwd_pitch_lim_cd, MAX(-(float)plane.nav_pitch_cd, 100.0f * q_fwd_pitch_lim)); - } else { - // take the lesser of the two limits - q_fwd_pitch_lim_cd = (float)aparm.angle_max; - } - - float fwd_thr_scaler; - if (!in_vtol_land_approach()) { - // To prevent forward motor prop strike, reduce throttle to zero when close to ground. - float alt_cutoff = MAX(0,vel_forward_alt_cutoff); - float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing); - fwd_thr_scaler = linear_interpolate(0.0f, 1.0f, height_above_ground, alt_cutoff, alt_cutoff+2); - } else { - // When we are doing horizontal positioning in a VTOL land we always allow the fwd motor - // to run. Otherwise a bad height above landing point estimate could cause the aircraft - // not to be able to approach the landing point - fwd_thr_scaler = 1.0f; - } - q_fwd_throttle *= fwd_thr_scaler; - - // When reducing forward throttle use, relax lower pitch limit to maintain forward - // acceleration capability. - const float nav_pitch_lower_limit_cd = - (int32_t)((float)aparm.angle_max * (1.0f - fwd_thr_scaler) + q_fwd_pitch_lim_cd * fwd_thr_scaler); - - // Diagnostics logging - remove when feature is fully flight tested. - AP::logger().WriteStreaming("FWDT", - "TimeUS,fts,qfplcd,npllcd,npcd,qft", // labels - "Qfffff", // fmt - AP_HAL::micros64(), - (double)fwd_thr_scaler, - (double)q_fwd_pitch_lim_cd, - (double)nav_pitch_lower_limit_cd, - (double)plane.nav_pitch_cd, - (double)q_fwd_throttle); - - plane.nav_pitch_cd = MAX(plane.nav_pitch_cd, (int32_t)nav_pitch_lower_limit_cd); -} /* we want to limit WP speed to a lower speed when more than 20 degrees @@ -3116,9 +2973,9 @@ void QuadPlane::takeoff_controller(void) // 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 - 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; + if (now - rudder_takeoff_warn_ms > 3000) { + gcs().send_text(MAV_SEVERITY_WARNING, "takeoff wait rudder release"); + rudder_takeoff_warn_ms = now; } return; } @@ -3163,8 +3020,6 @@ void QuadPlane::takeoff_controller(void) // nav roll and pitch are controller by position controller plane.nav_roll_cd = pos_control->get_roll_cd(); plane.nav_pitch_cd = pos_control->get_pitch_cd(); - - assign_tilt_to_fwd_thr(); } run_xy_controller(); @@ -3205,7 +3060,8 @@ void QuadPlane::waypoint_controller(void) const Location &loc = plane.next_WP_loc; const uint32_t now = AP_HAL::millis(); - if (!loc.same_loc_as(last_auto_target) || + if (!loc.same_latlon_as(last_auto_target) || + plane.next_WP_loc.alt != last_auto_target.alt || now - last_loiter_ms > 500) { wp_nav->set_wp_destination(poscontrol.target_cm.tofloat()); last_auto_target = loc; @@ -3222,8 +3078,6 @@ void QuadPlane::waypoint_controller(void) plane.nav_roll_cd = wp_nav->get_roll(); plane.nav_pitch_cd = wp_nav->get_pitch(); - assign_tilt_to_fwd_thr(); - if (transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) { pos_control->set_externally_limited_xy(); } @@ -3682,11 +3536,6 @@ void QuadPlane::Log_Write_QControl_Tuning() */ float QuadPlane::forward_throttle_pct() { - // handle special case where forward thrust motor is used instead of forward pitch. - if (get_vfwd_method() == ActiveFwdThr::NEW) { - return 100.0f * q_fwd_throttle; - } - /* Unless an RC channel is assigned for manual forward throttle control, we don't use forward throttle in QHOVER or QSTABILIZE as they are the primary @@ -3710,9 +3559,15 @@ float QuadPlane::forward_throttle_pct() } /* - see if the controller should be active + in qautotune mode or modes without a velocity controller */ - if (get_vfwd_method() != ActiveFwdThr::OLD) { + bool use_forward_gain = (vel_forward.gain > 0); +#if QAUTOTUNE_ENABLED + if (plane.control_mode == &plane.mode_qautotune) { + use_forward_gain = false; + } +#endif + if (!use_forward_gain) { return 0; } @@ -4520,7 +4375,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) +void SLT_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd, bool& allow_stick_mixing) { if (quadplane.in_vtol_mode() || quadplane.in_vtol_airbrake()) { // not in FW flight @@ -4582,9 +4437,6 @@ void QuadPlane::mode_enter(void) // state for special behaviour guided_wait_takeoff_on_mode_enter = guided_wait_takeoff; guided_wait_takeoff = false; - - q_fwd_throttle = 0.0f; - q_fwd_pitch_lim_cd = 100.0f * q_fwd_pitch_lim; } // Set attitude control yaw rate time constant to pilot input command model value @@ -4702,17 +4554,6 @@ 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 diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index b919be2c445869..481d1b80d35e70 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -47,7 +47,6 @@ class QuadPlane friend class Tailsitter_Transition; friend class Mode; - friend class ModeManual; friend class ModeAuto; friend class ModeRTL; friend class ModeAvoidADSB; @@ -181,9 +180,6 @@ 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 @@ -233,6 +229,9 @@ 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); @@ -395,32 +394,6 @@ class QuadPlane AP_Float acro_pitch_rate; AP_Float acro_yaw_rate; - // gain from forward acceleration to forward throttle - AP_Float q_fwd_thr_gain; - - // limit applied to forward pitch to prevent wing producing negative lift - AP_Float q_fwd_pitch_lim; - - // which fwd throttle handling method is active - enum class ActiveFwdThr : uint8_t { - NONE = 0, - OLD = 1, - NEW = 2, - }; - // override with AUX function - bool vfwd_enable_active; - - // specifies when the feature controlled by q_fwd_thr_gain and q_fwd_pitch_lim is used - enum class FwdThrUse : uint8_t { - OFF = 0, - POSCTRL = 1, - ALL = 2, - }; - AP_Enum q_fwd_thr_use; - - // return which vfwd method to use - ActiveFwdThr get_vfwd_method(void) const; - // time we last got an EKF yaw reset uint32_t ekfYawReset_ms; @@ -437,9 +410,6 @@ class QuadPlane Location last_auto_target; - float q_fwd_throttle; // forward throttle used in q modes - float q_fwd_pitch_lim_cd; // forward pitch limit applied when using q_fwd_throttle - // when did we last run the attitude controller? uint32_t last_att_control_ms; @@ -610,7 +580,6 @@ class QuadPlane TRANS_FAIL_TO_FW=(1<<19), FS_RTL=(1<<20), DISARMED_TILT_UP=(1<<21), - SCALE_FF_ANGLE_P=(1<<22), }; bool option_is_set(OPTION option) const { return (options.get() & int32_t(option)) != 0; @@ -620,6 +589,7 @@ 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; @@ -712,11 +682,6 @@ class QuadPlane */ void set_desired_spool_state(AP_Motors::DesiredSpoolState state); - /* - limit forward pitch demand if using rotor tilt or forward flight motor to provide forward acceleration. - */ - void assign_tilt_to_fwd_thr(void); - /* get a scaled Q_WP_SPEED based on direction of movement */ diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index cb7e68ff226f32..02fa2f79e8ba4e 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -139,7 +139,6 @@ 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 @@ -210,10 +209,8 @@ void Plane::read_radio() quadplane.tailsitter.check_input(); #endif -#if AP_TUNING_ENABLED // check for transmitter tuning changes tuning.check_input(control_mode->mode_number()); -#endif } int16_t Plane::rudder_input(void) @@ -224,7 +221,7 @@ int16_t Plane::rudder_input(void) return 0; } - if ((flight_option_enabled(FlightOptions::DIRECT_RUDDER_ONLY)) && + if ((g2.flight_options & 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; @@ -280,10 +277,7 @@ void Plane::control_failsafe() } } - 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 + if (ThrFailsafe(g.throttle_fs_enabled.get()) != ThrFailsafe::Enabled) { return; } @@ -441,8 +435,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 (((!(flight_option_enabled(FlightOptions::CENTER_THROTTLE_TRIM) && channel_throttle->in_trim_dz())) || - (flight_option_enabled(FlightOptions::CENTER_THROTTLE_TRIM)&& channel_throttle->in_min_dz()))) { + 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()))) { return true; } return false; diff --git a/ArduPlane/reverse_thrust.cpp b/ArduPlane/reverse_thrust.cpp index f64f37b276d6b3..04033d00f665ac 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) || - (flight_option_enabled(FlightOptions::CENTER_THROTTLE_TRIM)) == 0) { + (g2.flight_options & 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 ced42525788e83..11f50fabd8494c 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -376,6 +376,32 @@ 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 @@ -546,7 +572,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, g.throttle_passthru_stabilize ? 0.0 : MAX(min_throttle,0)); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); } else if (g.throttle_passthru_stabilize) { // manual pass through of throttle while in FBWA or // STABILIZE mode with THR_PASS_STAB set @@ -819,7 +845,34 @@ void Plane::set_servos(void) return; } - if (control_mode != &mode_manual) { + /* + 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 { set_servos_controlled(); } @@ -915,23 +968,6 @@ void Plane::landing_neutral_control_surface_servos(void) } -/* - sets rudder/vtail , and elevon to indicator positions that we are in a rudder arming waiting for neutral stick state -*/ -void Plane::indicate_waiting_for_rud_neutral_to_takeoff(void) -{ - if (takeoff_state.waiting_for_rudder_neutral) { - SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0); - channel_function_mixer(SRV_Channel::k_rudder, SRV_Channel::k_elevator, SRV_Channel::k_vtail_right, SRV_Channel::k_vtail_left); - if (!SRV_Channels::function_assigned(SRV_Channel::k_rudder) && !SRV_Channels::function_assigned(SRV_Channel::k_vtail_left)) { - // if no rudder indication possible, neutral elevons during wait because on takeoff stance they are usually both full up - SRV_Channels::set_output_scaled(SRV_Channel::k_elevon_right, 0); - SRV_Channels::set_output_scaled(SRV_Channel::k_elevon_left, 0); - } - } -} - - /* run configured output mixer. This takes calculated servo_out values for each channel and calculates PWM values, then pushes them to @@ -962,11 +998,6 @@ 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 7c7066bef06b60..149aa273e36587 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -42,15 +42,17 @@ 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(); @@ -140,7 +142,7 @@ void Plane::init_ardupilot() // set the correct flight mode // --------------------------- - rc().reset_mode_switch(); + reset_control_switch(); // initialise sensor #if AP_OPTICALFLOW_ENABLED @@ -195,6 +197,11 @@ 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); } @@ -218,46 +225,12 @@ static bool mode_reason_is_landing_sequence(const ModeReason reason) } #endif // AP_FENCE_ENABLED -// Check if this mode can be entered from the GCS -bool Plane::gcs_mode_enabled(const Mode::Number mode_num) const -{ - // List of modes that can be blocked, index is bit number in parameter bitmask - static const uint8_t mode_list [] { - (uint8_t)Mode::Number::MANUAL, - (uint8_t)Mode::Number::CIRCLE, - (uint8_t)Mode::Number::STABILIZE, - (uint8_t)Mode::Number::TRAINING, - (uint8_t)Mode::Number::ACRO, - (uint8_t)Mode::Number::FLY_BY_WIRE_A, - (uint8_t)Mode::Number::FLY_BY_WIRE_B, - (uint8_t)Mode::Number::CRUISE, - (uint8_t)Mode::Number::AUTOTUNE, - (uint8_t)Mode::Number::AUTO, - (uint8_t)Mode::Number::LOITER, - (uint8_t)Mode::Number::TAKEOFF, - (uint8_t)Mode::Number::AVOID_ADSB, - (uint8_t)Mode::Number::GUIDED, - (uint8_t)Mode::Number::THERMAL, -#if HAL_QUADPLANE_ENABLED - (uint8_t)Mode::Number::QSTABILIZE, - (uint8_t)Mode::Number::QHOVER, - (uint8_t)Mode::Number::QLOITER, - (uint8_t)Mode::Number::QACRO, -#if QAUTOTUNE_ENABLED - (uint8_t)Mode::Number::QAUTOTUNE -#endif -#endif - }; - - return !block_GCS_mode_change((uint8_t)mode_num, mode_list, ARRAY_SIZE(mode_list)); -} - bool Plane::set_mode(Mode &new_mode, const ModeReason reason) { if (control_mode == &new_mode) { // don't switch modes if we are already in the correct mode. - // only make happy noise if using a different method to switch, this stops beeping for repeated change mode requests from GCS + // only make happy noise if using a difent method to switch, this stops beeping for repeated change mode requests from GCS if ((reason != control_mode_reason) && (reason != ModeReason::INITIALISED)) { AP_Notify::events.user_mode_change = 1; } @@ -301,12 +274,6 @@ 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 f40fa33d1f26eb..815aac3c65ed01 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -27,7 +27,7 @@ const AP_Param::GroupInfo Tailsitter::var_info[] = { // @Param: ENABLE // @DisplayName: Enable Tailsitter // @Values: 0:Disable, 1:Enable, 2:Enable Always - // @Description: This enables Tailsitter functionality. A value of 2 forces Qassist active and always stabilize in forward flight with airmode for stabilisation at 0 throttle, for use on vehicles with no control surfaces, vehicle will not arm in forward flight modes, see also Q_OPTIONS "Mtrs_Only_Qassist" + // @Description: This enables Tailsitter functionality. A value of 2 forces Qassist active and always stabilize in forward flight with airmode for stabalisation at 0 throttle, for use on vehicles with no control surfaces, vehicle will not arm in forward flight modes, see also Q_OPTIONS "Mtrs_Only_Qassist" // @User: Standard // @RebootRequired: True AP_GROUPINFO_FLAGS("ENABLE", 1, Tailsitter, enable, 0, AP_PARAM_FLAG_ENABLE), @@ -161,7 +161,7 @@ const AP_Param::GroupInfo Tailsitter::var_info[] = { // @Param: MIN_VO // @DisplayName: Tailsitter Disk loading minimum outflow speed - // @Description: Use in conjunction with disk theory gain scaling and Q_TAILSIT_DSKLD to specify minumum airspeed over control surfaces, this will be used to boost throttle, when descending for example, 0 disables + // @Description: Use in conjunction with disk therory gain scaling and Q_TAILSIT_DSKLD to specify minumum airspeed over control surfaces, this will be used to boost throttle, when decending for example, 0 disables // @Range: 0 15 AP_GROUPINFO("MIN_VO", 22, Tailsitter, disk_loading_min_outflow, 0), @@ -302,11 +302,11 @@ void Tailsitter::output(void) */ if (!is_negative(transition_throttle_vtol)) { // Q_TAILSIT_THR_VT is positive use it until transition is complete - throttle = motors->thr_lin.actuator_to_thrust(MIN(transition_throttle_vtol*0.01,1.0)); + throttle = motors->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->thr_lin.actuator_to_thrust(plane.aparm.throttle_cruise.get() * 0.01)); + throttle = MAX(throttle,motors->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->thr_lin.thrust_to_actuator(throttle); + throttle = motors->thrust_to_actuator(throttle); // override AP_MotorsTailsitter throttles during back transition @@ -330,7 +330,7 @@ void Tailsitter::output(void) SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, throttle_pwm); SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, throttle_pwm); - // throttle output is not used by AP_Motors so might have different PWM range, set scaled + // throttle output is not used by AP_Motors so might have diffrent PWM range, set scaled SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle * 100.0); } } @@ -366,32 +366,20 @@ void Tailsitter::output(void) if (quadplane.option_is_set(QuadPlane::OPTION::TAILSIT_Q_ASSIST_MOTORS_ONLY)) { // only use motors for Q assist, control surfaces remain under plane control. Zero copter I terms and use plane. // Smoothly relax to zero so there is no step change in output, must also set limit flags so integrator cannot build faster than the relax. - // Assume there is always roll control surfaces, otherwise motors only assist should not be set. + // Assume there is always roll and pitch control surfaces, otherwise motors only assist should not be set. const float dt = quadplane.attitude_control->get_dt(); - - // VTOL yaw / FW roll + quadplane.attitude_control->get_rate_pitch_pid().relax_integrator(0.0, dt, AC_ATTITUDE_RATE_RELAX_TC); + motors->limit.pitch = true; quadplane.attitude_control->get_rate_yaw_pid().relax_integrator(0.0, dt, AC_ATTITUDE_RATE_RELAX_TC); motors->limit.yaw = true; - // VTOL and FW pitch - if (_have_elevator || _have_elevon || _have_v_tail) { - // have pitch control surfaces, use them - quadplane.attitude_control->get_rate_pitch_pid().relax_integrator(0.0, dt, AC_ATTITUDE_RATE_RELAX_TC); - motors->limit.pitch = true; - } else { - // no pitch control surfaces, zero plane I terms and use motors - // We skip the outputting to surfaces for this axis from the copter controller but there are none setup - plane.pitchController.reset_I(); - } - - // VTOL roll / FW yaw if (_have_rudder || _have_v_tail) { // there are yaw control surfaces, zero motor I term quadplane.attitude_control->get_rate_roll_pid().relax_integrator(0.0, dt, AC_ATTITUDE_RATE_RELAX_TC); motors->limit.roll = true; } else { // no yaw control surfaces, zero plane I terms and use motors - // We skip the outputting to surfaces for this axis from the copter controller but there are none setup + // We skip the outputing to surfaces for this axis from the copter controller but there are none setup plane.yawController.reset_I(); } @@ -513,7 +501,7 @@ void Tailsitter::output(void) bool Tailsitter::transition_fw_complete(void) { if (!plane.arming.is_armed_and_safety_off()) { - // instant transition when disarmed, no message + // instant trainsition when disarmed, no message return true; } if (labs(quadplane.ahrs_view->pitch_sensor) > transition_angle_fw*100) { @@ -539,7 +527,7 @@ bool Tailsitter::transition_fw_complete(void) bool Tailsitter::transition_vtol_complete(void) const { if (!plane.arming.is_armed_and_safety_off()) { - // instant transition when disarmed, no message + // instant trainsition when disarmed, no message return true; } // for vectored tailsitters at zero pilot throttle @@ -634,7 +622,7 @@ void Tailsitter::speed_scaling(void) float spd_scaler = 1.0f; float disk_loading_min_throttle = 0.0; - // Scaling with throttle + // Scaleing with throttle float throttle_scaler = throttle_scale_max; if (is_positive(throttle)) { throttle_scaler = constrain_float(hover_throttle / throttle, gain_scaling_min, throttle_scale_max); @@ -892,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) +void Tailsitter_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd, bool& allow_stick_mixing) { uint32_t now = AP_HAL::millis(); if (tailsitter.in_vtol_transition(now)) { @@ -904,6 +892,7 @@ 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 @@ -919,24 +908,12 @@ void Tailsitter_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& na } else { nav_pitch_cd = pitch_limit_cd; nav_roll_cd = 0; + allow_stick_mixing = false; } } } } -bool Tailsitter_Transition::allow_stick_mixing() const -{ - // Transitioning into VTOL flight, inital pitch up - if (tailsitter.in_vtol_transition()) { - return false; - } - // Transitioning into fixed wing flight, levelling off - if ((transition_state == TRANSITION_DONE) && (fw_limit_start_ms != 0)) { - return false; - } - return true; -} - bool Tailsitter_Transition::set_VTOL_roll_pitch_limit(int32_t& nav_roll_cd, int32_t& nav_pitch_cd) { if (vtol_limit_start_ms == 0) { diff --git a/ArduPlane/tailsitter.h b/ArduPlane/tailsitter.h index 4f2ff33c3ba370..1d91fa49e589b7 100644 --- a/ArduPlane/tailsitter.h +++ b/ArduPlane/tailsitter.h @@ -159,9 +159,7 @@ friend class Tailsitter; bool show_vtol_view() const override; - void set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd) override; - - bool allow_stick_mixing() const override; + void set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd, bool& allow_stick_mixing) override; MAV_VTOL_STATE get_mav_vtol_state() const override; diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index 5574ad2c9b0c10..31aeab02295f07 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -29,24 +29,6 @@ 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) { @@ -54,7 +36,7 @@ bool Plane::auto_takeoff_check(void) return false; } - bool do_takeoff_attitude_check = !(flight_option_enabled(FlightOptions::DISABLE_TOFF_ATTITUDE_CHK)); + bool do_takeoff_attitude_check = !(g2.flight_options & FlightOptions::DISABLE_TOFF_ATTITUDE_CHK); #if HAL_QUADPLANE_ENABLED // disable attitude check on tailsitters do_takeoff_attitude_check = !quadplane.tailsitter.enabled(); @@ -238,7 +220,7 @@ int16_t Plane::get_takeoff_pitch_min_cd(void) return auto_state.takeoff_pitch_cd * scalar; } - // are we entering the region where we want to start levelling off before we reach takeoff alt? + // are we entering the region where we want to start leveling off before we reach takeoff alt? if (auto_state.sink_rate < -0.1f) { float sec_to_target = (remaining_height_to_target_cm * 0.01f) / (-auto_state.sink_rate); if (sec_to_target > 0 && @@ -262,7 +244,7 @@ int16_t Plane::get_takeoff_pitch_min_cd(void) */ int8_t Plane::takeoff_tail_hold(void) { - bool in_takeoff = ((plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF) || + bool in_takeoff = ((control_mode == &mode_auto && !auto_state.takeoff_complete) || (control_mode == &mode_fbwa && auto_state.fbwa_tdrag_takeoff_mode)); if (!in_takeoff) { // not in takeoff diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index e8babe5e86d93b..5591fb3b6d1733 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -39,7 +39,7 @@ const AP_Param::GroupInfo Tiltrotor::var_info[] = { // @Param: TYPE // @DisplayName: Tiltrotor type - // @Description: This is the type of tiltrotor when TILT_MASK is non-zero. A continuous tiltrotor can tilt the rotors to any angle on demand. A binary tiltrotor assumes a retract style servo where the servo is either fully forward or fully up. In both cases the servo can't move faster than Q_TILT_RATE. A vectored yaw tiltrotor will use the tilt of the motors to control yaw in hover, Bicopter tiltrotor must use the tailsitter frame class (10) + // @Description: This is the type of tiltrotor when TILT_MASK is non-zero. A continuous tiltrotor can tilt the rotors to any angle on demand. A binary tiltrotor assumes a retract style servo where the servo is either fully forward or fully up. In both cases the servo can't move faster than Q_TILT_RATE. A vectored yaw tiltrotor will use the tilt of the motors to control yaw in hover, Bicopter tiltrottor must use the tailsitter frame class (10) // @Values: 0:Continuous,1:Binary,2:VectoredYaw,3:Bicopter AP_GROUPINFO("TYPE", 5, Tiltrotor, type, TILT_TYPE_CONTINUOUS), @@ -54,7 +54,7 @@ const AP_Param::GroupInfo Tiltrotor::var_info[] = { // @Param: YAW_ANGLE // @DisplayName: Tilt minimum angle for vectored yaw - // @Description: This is the angle of the tilt servos when in VTOL mode and at minimum output (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. + // @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 // @Range: 0 30 AP_GROUPINFO("YAW_ANGLE", 7, Tiltrotor, tilt_yaw_angle, 0), @@ -115,7 +115,7 @@ void Tiltrotor::setup() && (type != TILT_TYPE_BICOPTER)); - // check if there are any permanent VTOL motors + // check if there are any perminant VTOL motors for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; ++i) { if (motors->is_motor_enabled(i) && ((tilt_mask & (1U<<1)) == 0)) { // enabled motor not set in tilt mask @@ -193,7 +193,7 @@ void Tiltrotor::slew(float newtilt) SRV_Channels::set_output_scaled(SRV_Channel::k_motor_tilt, 1000 * current_tilt); } -// return the current tilt value that represents forward flight +// return the current tilt value that represens forward flight // tilt wings can sustain forward flight with some amount of wing tilt float Tiltrotor::get_fully_forward_tilt() const { @@ -258,25 +258,21 @@ void Tiltrotor::continuous_update(void) /* we are in a VTOL mode. We need to work out how much tilt is - needed. There are 5 strategies we will use: + needed. There are 4 strategies we will use: - 1) With use of a forward throttle controlled by Q_FWD_THR_GAIN in all - VTOL modes except Q_AUTOTUNE, we set the angle based on a calculated - forward throttle. + 1) without manual forward throttle control, the angle will be set to zero + in QAUTOTUNE QACRO, QSTABILIZE and QHOVER. This + enables these modes to be used as a safe recovery mode. - 2) With manual forward throttle control we set the angle based on the - RC input demanded forward throttle for QACRO, QSTABILIZE and QHOVER. + 2) with manual forward throttle control we will set the angle based on + the demanded forward throttle via RC input. - 3) Without a RC input or calculated forward throttle value, the angle - will be set to zero in QAUTOTUNE, QACRO, QSTABILIZE and QHOVER. - This enables these modes to be used as a safe recovery mode. + 3) in fixed wing assisted flight or velocity controlled modes we + will set the angle based on the demanded forward throttle, + with a maximum tilt given by Q_TILT_MAX. This relies on + Q_VFWD_GAIN being set. - 4) In fixed wing assisted flight or velocity controlled modes we will - set the angle based on the demanded forward throttle, with a maximum - tilt given by Q_TILT_MAX. This relies on Q_FWD_THR_GAIN or Q_VFWD_GAIN - being set. - - 5) if we are in TRANSITION_TIMER mode then we are transitioning + 4) if we are in TRANSITION_TIMER mode then we are transitioning to forward flight and should put the rotors all the way forward */ @@ -287,23 +283,11 @@ void Tiltrotor::continuous_update(void) } #endif + // if not in assisted flight and in QACRO, QSTABILIZE or QHOVER mode if (!quadplane.assisted_flight && - quadplane.get_vfwd_method() == QuadPlane::ActiveFwdThr::NEW && - quadplane.is_flying_vtol()) - { - // We are using the rotor tilt functionality controlled by Q_FWD_THR_GAIN which can - // operate in all VTOL modes except Q_AUTOTUNE. Forward rotor tilt is used to produce - // forward thrust equivalent to what would have been produced by a forward thrust motor - // set to quadplane.forward_throttle_pct() - const float fwd_g_demand = 0.01f * quadplane.forward_throttle_pct() / plane.quadplane.q_fwd_thr_gain; - const float fwd_tilt_deg = MIN(degrees(atanf(fwd_g_demand)), (float)max_angle_deg); - slew(MIN(fwd_tilt_deg * (1/90.0), get_forward_flight_tilt())); - return; - } else if (!quadplane.assisted_flight && - (plane.control_mode == &plane.mode_qacro || - plane.control_mode == &plane.mode_qstabilize || - plane.control_mode == &plane.mode_qhover)) - { + (plane.control_mode == &plane.mode_qacro || + plane.control_mode == &plane.mode_qstabilize || + plane.control_mode == &plane.mode_qhover)) { if (quadplane.rc_fwd_thr_ch == nullptr) { // no manual throttle control, set angle to zero slew(0); diff --git a/ArduPlane/transition.h b/ArduPlane/transition.h index b31e45ff040ece..9ddc1d5b5f5790 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) {}; + virtual void set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd, bool& allow_stick_mixing) {}; virtual bool set_FW_roll_limit(int32_t& roll_limit_cd) { return false; } @@ -56,8 +56,6 @@ class Transition virtual void set_last_fw_pitch(void) {} - virtual bool allow_stick_mixing() const { return true; } - protected: // refences for convenience @@ -89,7 +87,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) override; + void set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd, bool& allow_stick_mixing) override; bool set_FW_roll_limit(int32_t& roll_limit_cd) override; diff --git a/ArduPlane/tuning.cpp b/ArduPlane/tuning.cpp index be860264fd339a..2218a11ab0a67d 100644 --- a/ArduPlane/tuning.cpp +++ b/ArduPlane/tuning.cpp @@ -1,7 +1,3 @@ -#include - -#if AP_TUNING_ENABLED - #include "Plane.h" /* @@ -94,7 +90,6 @@ const AP_Tuning_Plane::tuning_name AP_Tuning_Plane::tuning_names[] = { { TUNING_PIT_I, "PitchI" }, { TUNING_PIT_D, "PitchD" }, { TUNING_PIT_FF, "PitchFF" }, - { TUNING_Q_FWD_THR, "QModeFwdThr" }, { TUNING_NONE, nullptr } }; @@ -191,9 +186,6 @@ AP_Float *AP_Tuning_Plane::get_param_pointer(uint8_t parm) case TUNING_RATE_YAW_FF: return &plane.quadplane.attitude_control->get_rate_yaw_pid().ff(); - - case TUNING_Q_FWD_THR: - return &plane.quadplane.q_fwd_thr_gain; #endif // HAL_QUADPLANE_ENABLED // fixed wing tuning parameters @@ -313,4 +305,3 @@ void AP_Tuning_Plane::reload_value(uint8_t parm) } } -#endif // AP_TUNING_ENABLED diff --git a/ArduPlane/tuning.h b/ArduPlane/tuning.h index 4334455a09657f..0a005f7dafdbfb 100644 --- a/ArduPlane/tuning.h +++ b/ArduPlane/tuning.h @@ -1,9 +1,3 @@ -#pragma once - -#include - -#if AP_TUNING_ENABLED - #include /* @@ -76,8 +70,6 @@ class AP_Tuning_Plane : public AP_Tuning TUNING_PIT_I = 55, TUNING_PIT_D = 56, TUNING_PIT_FF = 57, - - TUNING_Q_FWD_THR = 58, }; /* @@ -116,5 +108,3 @@ class AP_Tuning_Plane : public AP_Tuning // mask of what params have been set uint64_t have_set; }; - -#endif // AP_TUNING_ENABLED diff --git a/ArduPlane/version.h b/ArduPlane/version.h index cdf466f7155fb6..c78870616122d1 100644 --- a/ArduPlane/version.h +++ b/ArduPlane/version.h @@ -6,14 +6,14 @@ #include "ap_version.h" -#define THISFIRMWARE "ArduPlane V4.5.0-dev" +#define THISFIRMWARE "ArduPlane V4.4.2-beta1" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,5,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 4,4,2,FIRMWARE_VERSION_TYPE_BETA #define FW_MAJOR 4 -#define FW_MINOR 5 -#define FW_PATCH 0 -#define FW_TYPE FIRMWARE_VERSION_TYPE_DEV +#define FW_MINOR 4 +#define FW_PATCH 2 +#define FW_TYPE FIRMWARE_VERSION_TYPE_BETA #include diff --git a/ArduPlane/wscript b/ArduPlane/wscript index 4dd670ccc34a52..1386bd7b7c1c9d 100644 --- a/ArduPlane/wscript +++ b/ArduPlane/wscript @@ -28,6 +28,7 @@ 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 84789a10f664cc..7eb1589a8178d4 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((uint8_t)sub.control_mode, sub.control_mode_reason); + AP::logger().Write_Mode(sub.control_mode, sub.control_mode_reason); // reenable failsafe sub.mainloop_failsafe_enable(); diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index b8ded91fddc343..918a189eb34130 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -80,6 +80,7 @@ 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), @@ -101,9 +102,6 @@ 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 @@ -140,7 +138,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 != Mode::Number::MANUAL && control_mode != Mode::Number::MOTOR_DETECT) { + if (control_mode != MANUAL && control_mode != MOTOR_DETECT) { // run low level rate controllers that only require IMU data and set loop time attitude_control.rate_controller_run(); } @@ -201,7 +199,7 @@ void Sub::ten_hz_logging_loop() if (should_log(MASK_LOG_RCOUT)) { logger.Write_RCOUT(); } - if (should_log(MASK_LOG_NTUN) && (sub.flightmode->requires_GPS() || !sub.flightmode->has_manual_throttle())) { + if (should_log(MASK_LOG_NTUN) && (mode_requires_GPS(control_mode) || !mode_has_manual_throttle(control_mode))) { pos_control.write_log(); } if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) { @@ -210,11 +208,6 @@ 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 @@ -260,9 +253,7 @@ 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 @@ -355,15 +346,4 @@ 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 4bc904184e86d5..f078629041daf0 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 Mode::Number::AUTO: - case Mode::Number::GUIDED: - case Mode::Number::CIRCLE: - case Mode::Number::POSHOLD: + case AUTO: + case GUIDED: + case CIRCLE: + case 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 (uint32_t)sub.control_mode; + return sub.control_mode; } MAV_STATE GCS_MAVLINK_Sub::vehicle_system_status() const @@ -354,9 +354,7 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { MSG_GPS2_RAW, MSG_GPS2_RTK, MSG_NAV_CONTROLLER_OUTPUT, -#if AP_FENCE_ENABLED MSG_FENCE_STATUS, -#endif MSG_NAMED_FLOAT }; static const ap_message STREAM_POSITION_msgs[] = { @@ -388,10 +386,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = { MSG_BATTERY_STATUS, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_OPTICAL_FLOW, -#if COMPASS_CAL_ENABLED MSG_MAG_CAL_REPORT, MSG_MAG_CAL_PROGRESS, -#endif MSG_EKF_STATUS_REPORT, MSG_VIBRATION, #if AP_RPM_ENABLED @@ -435,9 +431,9 @@ MAV_RESULT GCS_MAVLINK_Sub::_handle_command_preflight_calibration_baro(const mav return MAV_RESULT_ACCEPTED; } -MAV_RESULT GCS_MAVLINK_Sub::_handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) +MAV_RESULT GCS_MAVLINK_Sub::_handle_command_preflight_calibration(const mavlink_command_long_t &packet, const mavlink_message_t &msg) { - if (packet.y == 1) { + if (is_equal(packet.param6,1.0f)) { // compassmot calibration //result = sub.mavlink_compassmot(chan); gcs().send_text(MAV_SEVERITY_INFO, "#CompassMot calibration not supported"); @@ -452,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.mode_auto.set_auto_yaw_roi(roi_loc); + sub.set_auto_yaw_roi(roi_loc); return MAV_RESULT_ACCEPTED; } @@ -463,51 +459,23 @@ bool GCS_MAVLINK_Sub::set_home(const Location& loc, bool _lock) { return sub.set_home(loc, _lock); } -MAV_RESULT GCS_MAVLINK_Sub::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) -{ - switch(packet.command) { - - case MAV_CMD_CONDITION_YAW: - return handle_MAV_CMD_CONDITION_YAW(packet); - - case MAV_CMD_DO_CHANGE_SPEED: - return handle_MAV_CMD_DO_CHANGE_SPEED(packet); - - case MAV_CMD_DO_MOTOR_TEST: - return handle_MAV_CMD_DO_MOTOR_TEST(packet); - - case MAV_CMD_MISSION_START: - return handle_MAV_CMD_MISSION_START(packet); - - case MAV_CMD_NAV_LOITER_UNLIM: - return handle_MAV_CMD_NAV_LOITER_UNLIM(packet); - - case MAV_CMD_NAV_LAND: - return handle_MAV_CMD_NAV_LAND(packet); - - } - return GCS_MAVLINK::handle_command_int_packet(packet, msg); -} - -MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_NAV_LOITER_UNLIM(const mavlink_command_int_t &packet) +MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_long_t &packet) { - if (!sub.set_mode(Mode::Number::POSHOLD, ModeReason::GCS_COMMAND)) { + switch (packet.command) { + case MAV_CMD_NAV_LOITER_UNLIM: + if (!sub.set_mode(POSHOLD, ModeReason::GCS_COMMAND)) { return MAV_RESULT_FAILED; } return MAV_RESULT_ACCEPTED; -} -MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_NAV_LAND(const mavlink_command_int_t &packet) -{ - if (!sub.set_mode(Mode::Number::SURFACE, ModeReason::GCS_COMMAND)) { + case MAV_CMD_NAV_LAND: + if (!sub.set_mode(SURFACE, ModeReason::GCS_COMMAND)) { return MAV_RESULT_FAILED; } return MAV_RESULT_ACCEPTED; -} -MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_CONDITION_YAW(const mavlink_command_int_t &packet) -{ + case MAV_CMD_CONDITION_YAW: // param1 : target angle [0-360] // param2 : speed during change [deg per second] // param3 : direction (-1:ccw, +1:cw) @@ -515,14 +483,12 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_CONDITION_YAW(const mavlink_command_i if ((packet.param1 >= 0.0f) && (packet.param1 <= 360.0f) && (is_zero(packet.param4) || is_equal(packet.param4,1.0f))) { - sub.mode_auto.set_auto_yaw_look_at_heading(packet.param1, packet.param2, (int8_t)packet.param3, (uint8_t)packet.param4); + sub.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_DENIED; -} + return MAV_RESULT_FAILED; -MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_DO_CHANGE_SPEED(const mavlink_command_int_t &packet) -{ + case MAV_CMD_DO_CHANGE_SPEED: // param1 : unused // param2 : new speed in m/s // param3 : unused @@ -532,18 +498,14 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_DO_CHANGE_SPEED(const mavlink_command return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; -} -MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_MISSION_START(const mavlink_command_int_t &packet) -{ - if (sub.motors.armed() && sub.set_mode(Mode::Number::AUTO, ModeReason::GCS_COMMAND)) { + case MAV_CMD_MISSION_START: + if (sub.motors.armed() && sub.set_mode(AUTO, ModeReason::GCS_COMMAND)) { return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; -} -MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command_int_t &packet) -{ + case MAV_CMD_DO_MOTOR_TEST: // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle) // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum) // param3 : throttle (range depends upon param2) @@ -552,8 +514,14 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command_i return MAV_RESULT_FAILED; } return MAV_RESULT_ACCEPTED; + + default: + return GCS_MAVLINK::handle_command_long_packet(packet); + } } + + void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) { switch (msg.msgid) { @@ -620,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.mode_guided.guided_set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), climb_rate_cms); + sub.guided_set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), climb_rate_cms); break; } @@ -630,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 != Mode::Number::GUIDED) && !(sub.control_mode == Mode::Number::AUTO && sub.auto_mode == Auto_NavGuided)) { + if ((sub.control_mode != GUIDED) && !(sub.control_mode == AUTO && sub.auto_mode == Auto_NavGuided)) { break; } @@ -638,32 +606,34 @@ 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_FRD) { + packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) { 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; - 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; + + /* + * 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; + */ // 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 from body-frame if necessary + // rotate to 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(); } @@ -674,31 +644,19 @@ 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 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) { + // rotate to body-frame if necessary + if (packet.coordinate_frame == MAV_FRAME_BODY_NED || 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.mode_guided.guided_set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); + sub.guided_set_destination_posvel(pos_vector, vel_vector); } else if (pos_ignore && !vel_ignore && acc_ignore) { - sub.mode_guided.guided_set_velocity(vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); + sub.guided_set_velocity(vel_vector); } else if (!pos_ignore && vel_ignore && acc_ignore) { - sub.mode_guided.guided_set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); + sub.guided_set_destination(pos_vector); } break; @@ -710,9 +668,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 != Mode::Number::GUIDED) - && !(sub.control_mode == Mode::Number::AUTO && sub.auto_mode == Auto_NavGuided) - && !(sub.control_mode == Mode::Number::ALT_HOLD)) { + if ((sub.control_mode != GUIDED) + && !(sub.control_mode == AUTO && sub.auto_mode == Auto_NavGuided) + && !(sub.control_mode == ALT_HOLD)) { break; } @@ -728,7 +686,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 == Mode::Number::ALT_HOLD) { // Control only target depth when in ALT_HOLD + if (!z_ignore && sub.control_mode == ALT_HOLD) { // Control only target depth when in ALT_HOLD sub.pos_control.set_pos_target_z_cm(packet.alt*100); break; } @@ -757,11 +715,11 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) } if (!pos_ignore && !vel_ignore && acc_ignore) { - sub.mode_guided.guided_set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f)); + sub.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.mode_guided.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f)); + sub.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.mode_guided.guided_set_destination(pos_neu_cm); + sub.guided_set_destination(pos_neu_cm); } break; @@ -807,7 +765,7 @@ uint64_t GCS_MAVLINK_Sub::capabilities() const ); } -MAV_RESULT GCS_MAVLINK_Sub::handle_flight_termination(const mavlink_command_int_t &packet) { +MAV_RESULT GCS_MAVLINK_Sub::handle_flight_termination(const mavlink_command_long_t &packet) { if (packet.param1 > 0.5f) { sub.arming.disarm(AP_Arming::Method::TERMINATION); return MAV_RESULT_ACCEPTED; @@ -836,7 +794,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 == Mode::Number::AUTO || sub.control_mode == Mode::Number::GUIDED) { + if (sub.control_mode == AUTO || sub.control_mode == GUIDED) { return 0.01 * (global_position_current.alt + sub.pos_control.get_pos_error_z_cm()); } return 0; @@ -846,7 +804,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 == Mode::Number::AUTO || sub.control_mode == Mode::Number::GUIDED) { + if (sub.control_mode == AUTO || sub.control_mode == GUIDED) { // need to convert -18000->18000 to 0->360/2 return wrap_360_cd(sub.wp_nav.get_wp_bearing_to_destination()) / 200; } @@ -856,7 +814,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 == Mode::Number::AUTO || sub.control_mode == Mode::Number::GUIDED) { + if (sub.control_mode == AUTO || sub.control_mode == GUIDED) { return MIN(sub.wp_nav.get_wp_distance_to_destination() * 0.001, UINT16_MAX); } return 0; @@ -865,7 +823,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 == Mode::Number::AUTO || sub.control_mode == Mode::Number::GUIDED) { + if (sub.control_mode == AUTO || sub.control_mode == 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 91c62ddff5278a..334586941c2a0d 100644 --- a/ArduSub/GCS_Mavlink.h +++ b/ArduSub/GCS_Mavlink.h @@ -15,13 +15,12 @@ class GCS_MAVLINK_Sub : public GCS_MAVLINK { return 0; }; - MAV_RESULT handle_flight_termination(const mavlink_command_int_t &packet) override; + MAV_RESULT handle_flight_termination(const mavlink_command_long_t &packet) override; MAV_RESULT handle_command_do_set_roi(const Location &roi_loc) override; MAV_RESULT _handle_command_preflight_calibration_baro(const mavlink_message_t &msg) override; - MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; - - MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; + MAV_RESULT _handle_command_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; // override sending of scaled_pressure3 to send on-board temperature: void send_scaled_pressure3() override; @@ -52,13 +51,6 @@ class GCS_MAVLINK_Sub : public GCS_MAVLINK { int16_t vfr_hud_throttle() const override; - MAV_RESULT handle_MAV_CMD_CONDITION_YAW(const mavlink_command_int_t &packet); - MAV_RESULT handle_MAV_CMD_MISSION_START(const mavlink_command_int_t &packet); - MAV_RESULT handle_MAV_CMD_DO_CHANGE_SPEED(const mavlink_command_int_t &packet); - MAV_RESULT handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command_int_t &packet); - MAV_RESULT handle_MAV_CMD_NAV_LOITER_UNLIM(const mavlink_command_int_t &packet); - MAV_RESULT handle_MAV_CMD_NAV_LAND(const mavlink_command_int_t &packet); - #if HAL_HIGH_LATENCY2_ENABLED int16_t high_latency_target_altitude() const override; uint8_t high_latency_tgt_heading() const override; diff --git a/ArduSub/GCS_Sub.cpp b/ArduSub/GCS_Sub.cpp index b57839c9bc842e..d535a823e6b6c0 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 Mode::Number::ALT_HOLD: - case Mode::Number::AUTO: - case Mode::Number::GUIDED: - case Mode::Number::CIRCLE: - case Mode::Number::SURFACE: - case Mode::Number::POSHOLD: + case ALT_HOLD: + case AUTO: + case GUIDED: + case CIRCLE: + case SURFACE: + case 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 f2649bc8417a92..1dbf759d6ab938 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((uint8_t)control_mode, control_mode_reason); + logger.Write_Mode(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 e7162c5ebcf1b7..75afeb22ca1c65 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -409,11 +409,9 @@ 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 @@ -622,11 +620,7 @@ 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 @@ -701,7 +695,22 @@ void Sub::load_parameters() AP_Param::set_frame_type_flags(AP_PARAM_FRAME_SUB); convert_old_parameters(); - AP_Param::set_defaults_from_table(defaults_table, ARRAY_SIZE(defaults_table)); + + 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 // 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 f170667859e8f5..eeff0a26935c14 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -5,8 +5,6 @@ #include #include -#include -#include #if AP_SCRIPTING_ENABLED #include @@ -49,7 +47,7 @@ class Parameters { // Layout version number, always key zero. // k_param_format_version = 0, - k_param_software_type, // unused + k_param_software_type, // unusued k_param_g2, // 2nd block of parameters @@ -320,10 +318,6 @@ 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[]; @@ -350,22 +344,3 @@ 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 e7ee787b509356..7b7139a1e946bd 100644 --- a/ArduSub/RC_Channel.h +++ b/ArduSub/RC_Channel.h @@ -25,9 +25,6 @@ class RC_Channels_Sub : public RC_Channels return &obj_channels[chan]; } - // tell the gimbal code all is good with RC input: - bool in_rc_failsafe() const override { return false; }; - protected: // note that these callbacks are not presently used on Plane: diff --git a/ArduSub/ReleaseNotes.txt b/ArduSub/ReleaseNotes.txt index 6d68f5216e7f51..937999ce180a49 100644 --- a/ArduSub/ReleaseNotes.txt +++ b/ArduSub/ReleaseNotes.txt @@ -704,7 +704,7 @@ Changes from 3.1.4 11) CLI removed from APM1/2 to save flash space, critical functions moved to MAVLink: a) Individual motor tests (see MP's Initial Setup > Optional Hardware > Motor Test) b) compassmot (see MP's Initial Setup > Optional Hardware > Compass/Motor Calib) - c) parameter reset to factory defaults (see MP's Config/Tuning > Full Parameter List > Reset to Default) + c) parameter reset to factory defautls (see MP's Config/Tuning > Full Parameter List > Reset to Default) ------------------------------------------------------------------ ArduCopter 3.1.5 27-May-2014 / 3.1.5-rc2 20-May-2014 Changes from 3.1.5-rc1 diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp index 2a90c7f3a685e3..bd73ce41a6bf15 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(Mode::Number::MANUAL), + control_mode(MANUAL), motors(MAIN_LOOP_RATE), auto_mode(Auto_WP), guided_mode(Guided_WP), @@ -37,19 +37,12 @@ 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), - flightmode(&mode_manual) + param_loader(var_info) { #if CONFIG_HAL_BOARD != HAL_BOARD_SITL failsafe.pilot_input = true; #endif - if (_singleton != nullptr) { - AP_HAL::panic("Can only be one Sub"); - } - _singleton = this; } -Sub *Sub::_singleton = nullptr; - Sub sub; AP_Vehicle& vehicle = sub; diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 2e4c3a41360444..95205f84b260fc 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -71,8 +71,6 @@ #include "Parameters.h" #include "AP_Arming_Sub.h" #include "GCS_Sub.h" -#include "mode.h" -#include "script_button.h" #include // Optical Flow library @@ -110,17 +108,6 @@ 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); @@ -203,9 +190,9 @@ class Sub : public AP_Vehicle { // This is the state of the flight control system // There are multiple states defined such as STABILIZE, ACRO, - Mode::Number control_mode; + control_mode_t control_mode; - Mode::Number prev_control_mode; + control_mode_t prev_control_mode; #if RCMAP_ENABLED == ENABLED RCMapper rcmap; @@ -247,6 +234,12 @@ 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 @@ -296,9 +289,6 @@ 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; @@ -426,7 +416,61 @@ 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); @@ -440,12 +484,15 @@ class Sub : public AP_Vehicle { void mainloop_failsafe_enable(); void mainloop_failsafe_disable(); void fence_check(); - bool set_mode(Mode::Number mode, ModeReason reason); - bool set_mode(const uint8_t new_mode, const ModeReason reason) override; + bool set_mode(control_mode_t mode, ModeReason reason); + bool set_mode(const uint8_t mode, const ModeReason reason) override; uint8_t get_mode() const override { return (uint8_t)control_mode; } void update_flight_mode(); - void exit_mode(Mode::Number old_control_mode, Mode::Number new_control_mode); - void notify_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 read_inertia(); void update_surface_and_bottom_detector(); void set_surfaced(bool at_surface); @@ -516,7 +563,8 @@ 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); @@ -525,12 +573,10 @@ class Sub : public AP_Vehicle { bool surface_init(void); void surface_run(); - void stats_update(); - uint16_t get_pilot_speed_dn() const; void convert_old_parameters(void); - bool handle_do_motor_test(mavlink_command_int_t command); + bool handle_do_motor_test(mavlink_command_long_t command); bool init_motor_test(); bool verify_motor_test(); @@ -562,45 +608,9 @@ class Sub : public AP_Vehicle { static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1, "_failsafe_priorities is missing the sentinel"); - Mode *mode_from_mode_num(const Mode::Number num); - void exit_mode(Mode *&old_flightmode, Mode *&new_flightmode); - - Mode *flightmode; - ModeManual mode_manual; - ModeStabilize mode_stabilize; - ModeAcro mode_acro; - ModeAlthold mode_althold; - ModeAuto mode_auto; - ModeGuided mode_guided; - ModePoshold mode_poshold; - ModeCircle mode_circle; - ModeSurface mode_surface; - ModeMotordetect mode_motordetect; - - // Auto - AutoSubMode auto_mode; // controls which auto controller is run - GuidedSubMode guided_mode; - -#if AP_SCRIPTING_ENABLED - ScriptButton script_buttons[4]; -#endif // AP_SCRIPTING_ENABLED public: void mainloop_failsafe_check(); - - static Sub *_singleton; - - static Sub *get_singleton() { - return _singleton; - } - -#if AP_SCRIPTING_ENABLED - // For Lua scripting, so index is 1..4, not 0..3 - bool is_button_pressed(uint8_t index); - - // For Lua scripting, so index is 1..4, not 0..3 - uint8_t get_and_clear_button_count(uint8_t index); -#endif // AP_SCRIPTING_ENABLED }; extern const AP_HAL::HAL& hal; diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index 681022a3dd93bf..855f8b0adf6862 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 == Mode::Number::AUTO) { + if (control_mode == 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 (!mode_auto.auto_loiter_start()) { - set_mode(Mode::Number::ALT_HOLD, ModeReason::MISSION_END); + if (!auto_loiter_start()) { + set_mode(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 - mode_auto.auto_wp_start(target_loc); + 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 - mode_auto.auto_wp_start(target_location); + auto_wp_start(target_location); } void Sub::do_RTL() { - mode_auto.auto_wp_start(ahrs.get_home()); + 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 - mode_auto.auto_wp_start(target_loc); + 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 - mode_auto.auto_circle_movetoedge_start(circle_center, circle_radius_m, circle_direction_ccw); + 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 - mode_auto.guided_limit_init_time_and_pos(); + guided_limit_init_time_and_pos(); // set navigation target - mode_auto.auto_nav_guided_start(); + auto_nav_guided_start(); } } #endif // NAV_GUIDED @@ -401,11 +401,7 @@ void Sub::do_nav_delay(const AP_Mission::Mission_Command& cmd) nav_delay_time_max_ms = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds } else { // absolute delay to utc time -#if AP_RTC_ENABLED nav_delay_time_max_ms = AP::rtc().get_time_utc(cmd.content.nav_delay.hour_utc, cmd.content.nav_delay.min_utc, cmd.content.nav_delay.sec_utc, 0); -#else - nav_delay_time_max_ms = 0; -#endif } gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec", (unsigned)(nav_delay_time_max_ms/1000)); } @@ -414,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) { - mode_guided.guided_limit_set(cmd.p1 * 1000, // convert seconds to ms + 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 @@ -463,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); - mode_auto.auto_wp_start(target_location); + auto_wp_start(target_location); // advance to next state auto_surface_state = AUTO_SURFACE_STATE_ASCEND; @@ -533,13 +529,13 @@ bool Sub::verify_circle(const AP_Mission::Mission_Command& cmd) } // start circling - mode_auto.auto_circle_start(); + auto_circle_start(); } return false; } // check if we have completed circling - return fabsf(sub.circle_nav.get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1); + return fabsf(circle_nav.get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1); } #if NAV_GUIDED == ENABLED @@ -552,7 +548,7 @@ bool Sub::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) } // check time and position limits - return mode_auto.guided_limit_check(); + return guided_limit_check(); } #endif // NAV_GUIDED @@ -583,7 +579,7 @@ void Sub::do_within_distance(const AP_Mission::Mission_Command& cmd) void Sub::do_yaw(const AP_Mission::Mission_Command& cmd) { - sub.mode_auto.set_auto_yaw_look_at_heading( + set_auto_yaw_look_at_heading( cmd.content.yaw.angle_deg, cmd.content.yaw.turn_rate_dps, cmd.content.yaw.direction, @@ -618,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) { - sub.mode_auto.set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING); + set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING); } // check if we are within 2 degrees of the target heading @@ -633,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 != Mode::Number::GUIDED && !(control_mode == Mode::Number::AUTO && auto_mode == Auto_NavGuided)) { + if (control_mode != GUIDED && !(control_mode == AUTO && auto_mode == Auto_NavGuided)) { return false; } @@ -642,7 +638,7 @@ bool Sub::do_guided(const AP_Mission::Mission_Command& cmd) case MAV_CMD_NAV_WAYPOINT: { // set wp_nav's destination - return sub.mode_guided.guided_set_destination(cmd.content.location); + return guided_set_destination(cmd.content.location); } case MAV_CMD_CONDITION_YAW: @@ -685,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) { - sub.mode_auto.set_auto_yaw_roi(cmd.content.location); + 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 4a517afe13adbb..d09d7e351c129b 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -190,12 +190,6 @@ # 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 new file mode 100644 index 00000000000000..959ad69f8e8117 --- /dev/null +++ b/ArduSub/control_acro.cpp @@ -0,0 +1,162 @@ +#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 new file mode 100644 index 00000000000000..a420cfe3149ea7 --- /dev/null +++ b/ArduSub/control_althold.cpp @@ -0,0 +1,120 @@ +#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/mode_auto.cpp b/ArduSub/control_auto.cpp similarity index 54% rename from ArduSub/mode_auto.cpp rename to ArduSub/control_auto.cpp index 87ad912976b5cf..4631e8dcef1809 100644 --- a/ArduSub/mode_auto.cpp +++ b/ArduSub/control_auto.cpp @@ -7,38 +7,42 @@ * While in the auto flight mode, navigation or do/now commands can be run. * Code in this file implements the navigation commands */ -bool ModeAuto::init(bool ignore_checks) { - if (!sub.position_ok() || sub.mission.num_commands() < 2) { + +// auto_init - initialise auto controller +bool Sub::auto_init() +{ + if (!position_ok() || mission.num_commands() < 2) { return false; } - sub.auto_mode = Auto_Loiter; + 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 (sub.auto_yaw_mode == AUTO_YAW_ROI) { + if (auto_yaw_mode == AUTO_YAW_ROI) { set_auto_yaw_mode(AUTO_YAW_HOLD); } // initialise waypoint controller - sub.wp_nav.wp_and_spline_init(); + wp_nav.wp_and_spline_init(); // clear guided limits guided_limit_clear(); // start/resume the mission (based on MIS_RESTART parameter) - sub.mission.start_or_resume(); + mission.start_or_resume(); return true; } // auto_run - runs the appropriate auto controller // according to the current auto_mode -void ModeAuto::run() +// should be called at 100hz or more +void Sub::auto_run() { - sub.mission.update(); + mission.update(); // call the correct auto controller - switch (sub.auto_mode) { + switch (auto_mode) { case Auto_WP: case Auto_CircleMoveToEdge: @@ -66,42 +70,42 @@ void ModeAuto::run() } // auto_wp_start - initialises waypoint controller to implement flying to a particular destination -void ModeAuto::auto_wp_start(const Vector3f& destination) +void Sub::auto_wp_start(const Vector3f& destination) { - sub.auto_mode = Auto_WP; + auto_mode = Auto_WP; // initialise wpnav (no need to check return status because terrain data is not used) - sub.wp_nav.set_wp_destination(destination, false); + 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 (sub.auto_yaw_mode != AUTO_YAW_ROI) { + if (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 ModeAuto::auto_wp_start(const Location& dest_loc) +void Sub::auto_wp_start(const Location& dest_loc) { - sub.auto_mode = Auto_WP; + auto_mode = Auto_WP; // send target to waypoint controller - if (!sub.wp_nav.set_wp_destination_loc(dest_loc)) { + if (!wp_nav.set_wp_destination_loc(dest_loc)) { // failure to set destination can only be because of missing terrain data - sub.failsafe_terrain_on_event(); + 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 (sub.auto_yaw_mode != AUTO_YAW_ROI) { + if (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 ModeAuto::auto_wp_run() +void Sub::auto_wp_run() { // if not armed set throttle to zero and exit immediately if (!motors.armed()) { @@ -110,17 +114,17 @@ void ModeAuto::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(); - sub.wp_nav.wp_and_spline_init(); // Reset xy target + attitude_control.set_throttle_out(0,true,g.throttle_filt); + attitude_control.relax_attitude_controllers(); + wp_nav.wp_and_spline_init(); // Reset xy target return; } // process pilot's yaw input float target_yaw_rate = 0; - if (!sub.failsafe.pilot_input) { + if (!failsafe.pilot_input) { // get pilot's desired yaw rate - target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + 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); } @@ -133,13 +137,13 @@ void ModeAuto::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 - sub.failsafe_terrain_set_status(sub.wp_nav.update_wpnav()); + failsafe_terrain_set_status(wp_nav.update_wpnav()); /////////////////////// // update xy outputs // float lateral_out, forward_out; - sub.translate_wpnav_rp(lateral_out, forward_out); + translate_wpnav_rp(lateral_out, forward_out); // Send to forward/lateral outputs motors.set_lateral(lateral_out); @@ -147,52 +151,52 @@ void ModeAuto::auto_wp_run() // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle - position_control->update_z_controller(); + pos_control.update_z_controller(); //////////////////////////// // update attitude output // // get pilot desired lean angles float target_roll, target_pitch; - sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, sub.aparm.angle_max); + get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); // call attitude controller - if (sub.auto_yaw_mode == AUTO_YAW_HOLD) { + if (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 sub.circle_nav.set_center() +// we assume the caller has set the circle's circle with circle_nav.set_center() // we assume the caller has performed all required GPS_ok checks -void ModeAuto::auto_circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn) +void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn) { // set circle center - sub.circle_nav.set_center(circle_center); + circle_nav.set_center(circle_center); // set circle radius if (!is_zero(radius_m)) { - sub.circle_nav.set_radius_cm(radius_m * 100.0f); + circle_nav.set_radius_cm(radius_m * 100.0f); } // set circle direction by using rate - float current_rate = sub.circle_nav.get_rate(); + float current_rate = circle_nav.get_rate(); current_rate = ccw_turn ? -fabsf(current_rate) : fabsf(current_rate); - sub.circle_nav.set_rate(current_rate); + circle_nav.set_rate(current_rate); // check our distance from edge of circle Vector3f circle_edge_neu; - sub.circle_nav.get_closest_point_on_circle(circle_edge_neu); + 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 - sub.auto_mode = Auto_CircleMoveToEdge; + auto_mode = Auto_CircleMoveToEdge; // convert circle_edge_neu to Location Location circle_edge(circle_edge_neu, Location::AltFrame::ABOVE_ORIGIN); @@ -201,14 +205,14 @@ void ModeAuto::auto_circle_movetoedge_start(const Location &circle_center, float circle_edge.set_alt_cm(circle_center.alt, circle_center.get_alt_frame()); // initialise wpnav to move to edge of circle - if (!sub.wp_nav.set_wp_destination_loc(circle_edge)) { + if (!wp_nav.set_wp_destination_loc(circle_edge)) { // failure to set destination can only be because of missing terrain data - sub.failsafe_terrain_on_event(); + 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(), sub.circle_nav.get_center().xy()); - if (dist_to_center > sub.circle_nav.get_radius() && dist_to_center > 500) { + 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) { 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 @@ -221,23 +225,23 @@ void ModeAuto::auto_circle_movetoedge_start(const Location &circle_center, float // 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 ModeAuto::auto_circle_start() +void Sub::auto_circle_start() { - sub.auto_mode = Auto_Circle; + auto_mode = Auto_Circle; // initialise circle controller - sub.circle_nav.init(sub.circle_nav.get_center(), sub.circle_nav.center_is_terrain_alt(), sub.circle_nav.get_rate()); + circle_nav.init(circle_nav.get_center(), circle_nav.center_is_terrain_alt(), circle_nav.get_rate()); } // auto_circle_run - circle in AUTO flight mode // called by auto_run at 100hz or more -void ModeAuto::auto_circle_run() +void Sub::auto_circle_run() { // call circle controller - sub.failsafe_terrain_set_status(sub.circle_nav.update()); + failsafe_terrain_set_status(circle_nav.update()); float lateral_out, forward_out; - sub.translate_circle_nav_rp(lateral_out, forward_out); + translate_circle_nav_rp(lateral_out, forward_out); // Send to forward/lateral outputs motors.set_lateral(lateral_out); @@ -245,47 +249,50 @@ void ModeAuto::auto_circle_run() // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle - position_control->update_z_controller(); + pos_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(), sub.circle_nav.get_yaw(), true); + attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), circle_nav.get_yaw(), true); } #if NAV_GUIDED == ENABLED // auto_nav_guided_start - hand over control to external navigation controller in AUTO mode -void ModeAuto::auto_nav_guided_start() +void Sub::auto_nav_guided_start() { - sub.mode_guided.init(true); - sub.auto_mode = Auto_NavGuided; + auto_mode = Auto_NavGuided; + + // call regular guided flight mode initialisation + guided_init(true); + // initialise guided start time and position as reference for limit checking - sub.mode_auto.guided_limit_init_time_and_pos(); + 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 ModeAuto::auto_nav_guided_run() +void Sub::auto_nav_guided_run() { // call regular guided flight mode run function - sub.mode_guided.run(); + 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 ModeAuto::auto_loiter_start() +bool Sub::auto_loiter_start() { // return failure if GPS is bad - if (!sub.position_ok()) { + if (!position_ok()) { return false; } - sub.auto_mode = Auto_Loiter; + auto_mode = Auto_Loiter; // calculate stopping point Vector3f stopping_point; - sub.wp_nav.get_wp_stopping_point(stopping_point); + wp_nav.get_wp_stopping_point(stopping_point); // initialise waypoint controller target to stopping point - sub.wp_nav.set_wp_destination(stopping_point); + wp_nav.set_wp_destination(stopping_point); // hold yaw at current heading set_auto_yaw_mode(AUTO_YAW_HOLD); @@ -295,35 +302,35 @@ bool ModeAuto::auto_loiter_start() // auto_loiter_run - loiter in AUTO flight mode // called by auto_run at 100hz or more -void ModeAuto::auto_loiter_run() +void Sub::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(); - sub.wp_nav.wp_and_spline_init(); // Reset xy target + wp_nav.wp_and_spline_init(); // Reset xy target return; } // accept pilot input of yaw float target_yaw_rate = 0; - if (!sub.failsafe.pilot_input) { - target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + if (!failsafe.pilot_input) { + target_yaw_rate = 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 - sub.failsafe_terrain_set_status(sub.wp_nav.update_wpnav()); + failsafe_terrain_set_status(wp_nav.update_wpnav()); /////////////////////// // update xy outputs // float lateral_out, forward_out; - sub.translate_wpnav_rp(lateral_out, forward_out); + translate_wpnav_rp(lateral_out, forward_out); // Send to forward/lateral outputs motors.set_lateral(lateral_out); @@ -331,41 +338,113 @@ void ModeAuto::auto_loiter_run() // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle - position_control->update_z_controller(); + pos_control.update_z_controller(); // get pilot desired lean angles float target_roll, target_pitch; - sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, sub.aparm.angle_max); + get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); // roll & pitch & yaw rate from pilot - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); +} + +// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter +// set rtl parameter to true if this is during an RTL +uint8_t Sub::get_default_auto_yaw_mode(bool rtl) const +{ + switch (g.wp_yaw_behavior) { + + case WP_YAW_BEHAVIOR_NONE: + return AUTO_YAW_HOLD; + break; + + case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL: + if (rtl) { + return AUTO_YAW_HOLD; + } else { + return AUTO_YAW_LOOK_AT_NEXT_WP; + } + break; + + case WP_YAW_BEHAVIOR_LOOK_AHEAD: + return AUTO_YAW_LOOK_AHEAD; + break; + + case WP_YAW_BEHAVIOR_CORRECT_XTRACK: + return AUTO_YAW_CORRECT_XTRACK; + break; + + case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP: + default: + return AUTO_YAW_LOOK_AT_NEXT_WP; + break; + } } +// set_auto_yaw_mode - sets the yaw mode for auto +void Sub::set_auto_yaw_mode(uint8_t yaw_mode) +{ + // return immediately if no change + if (auto_yaw_mode == yaw_mode) { + return; + } + auto_yaw_mode = yaw_mode; + + // perform initialisation + switch (auto_yaw_mode) { + + case AUTO_YAW_LOOK_AT_NEXT_WP: + // wpnav will initialise heading when wpnav's set_destination method is called + break; + + case AUTO_YAW_ROI: + // point towards a location held in yaw_look_at_WP + yaw_look_at_WP_bearing = ahrs.yaw_sensor; + break; + + case AUTO_YAW_LOOK_AT_HEADING: + // keep heading pointing in the direction held in yaw_look_at_heading + // caller should set the yaw_look_at_heading + break; + + case AUTO_YAW_LOOK_AHEAD: + // Commanded Yaw to automatically look ahead. + yaw_look_ahead_bearing = ahrs.yaw_sensor; + break; + + case AUTO_YAW_RESETTOARMEDYAW: + // initial_armed_bearing will be set during arming so no init required + break; + } +} // set_auto_yaw_look_at_heading - sets the yaw look at heading for auto mode -void ModeAuto::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle) +void Sub::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle) { - // get current yaw - int32_t curr_yaw_target = attitude_control->get_att_target_euler_cd().z; + // get current yaw target + 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 - sub.yaw_look_at_heading = wrap_360_cd(angle_deg * 100); + yaw_look_at_heading = wrap_360_cd(angle_deg * 100); } else { // relative angle if (direction < 0) { angle_deg = -angle_deg; } - sub.yaw_look_at_heading = wrap_360_cd((angle_deg * 100 + curr_yaw_target)); + 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 - sub.yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE; + yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE; } else { - sub.yaw_look_at_heading_slew = MIN(turn_rate_dps, AUTO_YAW_SLEW_RATE); // deg / sec + 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 } // set yaw mode @@ -374,19 +453,8 @@ void ModeAuto::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps // 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 ModeAuto::set_auto_yaw_roi(const Location &roi_location) +void Sub::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) { @@ -394,20 +462,20 @@ void ModeAuto::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 (sub.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { - sub.camera_mount.set_mode_to_default(); + if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { + 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 (!sub.camera_mount.has_pan_control()) { - if (roi_location.get_vector_from_origin_NEU(sub.roi_WP)) { + if (!camera_mount.has_pan_control()) { + if (roi_location.get_vector_from_origin_NEU(roi_WP)) { set_auto_yaw_mode(AUTO_YAW_ROI); } } // send the command to the camera mount - sub.camera_mount.set_roi_target(roi_location); + 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 @@ -417,18 +485,70 @@ void ModeAuto::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(sub.roi_WP)) { + if (roi_location.get_vector_from_origin_NEU(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 ModeAuto::auto_terrain_recover_start() +bool Sub::auto_terrain_recover_start() { // Check rangefinder status to see if recovery is possible - switch (sub.rangefinder.status_orient(ROTATION_PITCH_270)) { + switch (rangefinder.status_orient(ROTATION_PITCH_270)) { case RangeFinder::Status::OutOfRangeLow: case RangeFinder::Status::OutOfRangeHigh: @@ -436,7 +556,7 @@ bool ModeAuto::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: - sub.auto_mode = Auto_TerrainRecover; + auto_mode = Auto_TerrainRecover; break; // Not connected or no data @@ -445,21 +565,21 @@ bool ModeAuto::auto_terrain_recover_start() } // Initialize recovery timeout time - sub.fs_terrain_recover_start_ms = AP_HAL::millis(); + fs_terrain_recover_start_ms = AP_HAL::millis(); // Stop mission - sub.mission.stop(); + mission.stop(); // Reset xy target - sub.loiter_nav.clear_pilot_desired_acceleration(); - sub.loiter_nav.init_target(); + loiter_nav.clear_pilot_desired_acceleration(); + loiter_nav.init_target(); // Reset z axis controller - position_control->relax_z_controller(motors.get_throttle_hover()); + pos_control.relax_z_controller(motors.get_throttle_hover()); // initialize vertical maximum speeds 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()); + 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()); gcs().send_text(MAV_SEVERITY_WARNING, "Attempting auto failsafe recovery"); return true; @@ -468,7 +588,7 @@ bool ModeAuto::auto_terrain_recover_start() // Attempt recovery from terrain failsafe // If recovery is successful resume mission // If recovery fails revert to failsafe action -void ModeAuto::auto_terrain_recover_run() +void Sub::auto_terrain_recover_run() { float target_climb_rate = 0; static uint32_t rangefinder_recovery_ms = 0; @@ -476,23 +596,23 @@ void ModeAuto::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(); - sub.loiter_nav.init_target(); // Reset xy target - position_control->relax_z_controller(motors.get_throttle_hover()); // Reset z axis controller + loiter_nav.init_target(); // Reset xy target + pos_control.relax_z_controller(motors.get_throttle_hover()); // Reset z axis controller return; } - switch (sub.rangefinder.status_orient(ROTATION_PITCH_270)) { + switch (rangefinder.status_orient(ROTATION_PITCH_270)) { case RangeFinder::Status::OutOfRangeLow: - target_climb_rate = sub.wp_nav.get_default_speed_up(); + target_climb_rate = wp_nav.get_default_speed_up(); rangefinder_recovery_ms = 0; break; case RangeFinder::Status::OutOfRangeHigh: - target_climb_rate = sub.wp_nav.get_default_speed_down(); + target_climb_rate = wp_nav.get_default_speed_down(); rangefinder_recovery_ms = 0; break; @@ -500,21 +620,21 @@ void ModeAuto::auto_terrain_recover_run() target_climb_rate = 0; // Attempt to hold current depth - if (sub.rangefinder_state.alt_healthy) { + if (rangefinder_state.alt_healthy) { // Start timer as soon as rangefinder is healthy if (rangefinder_recovery_ms == 0) { rangefinder_recovery_ms = AP_HAL::millis(); - position_control->relax_z_controller(motors.get_throttle_hover()); // Reset alt hold targets + pos_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!"); - 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 + 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 rangefinder_recovery_ms = 0; // Reset for subsequent recoveries } @@ -526,25 +646,25 @@ void ModeAuto::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!"); - sub.failsafe_terrain_act(); + failsafe_terrain_act(); rangefinder_recovery_ms = 0; return; } // exit on failure (timeout) - if (AP_HAL::millis() > sub.fs_terrain_recover_start_ms + FS_TERRAIN_RECOVER_TIMEOUT_MS) { + if (AP_HAL::millis() > 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!"); - sub.failsafe_terrain_act(); + failsafe_terrain_act(); } // run loiter controller - sub.loiter_nav.update(); + loiter_nav.update(); /////////////////////// // update xy targets // float lateral_out, forward_out; - sub.translate_wpnav_rp(lateral_out, forward_out); + translate_wpnav_rp(lateral_out, forward_out); // Send to forward/lateral outputs motors.set_lateral(lateral_out); @@ -552,8 +672,8 @@ void ModeAuto::auto_terrain_recover_run() ///////////////////// // update z target // - position_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate); - position_control->update_z_controller(); + pos_control.set_pos_target_z_from_climb_rate_cm(target_climb_rate); + pos_control.update_z_controller(); //////////////////////////// // update angular targets // @@ -561,11 +681,11 @@ void ModeAuto::auto_terrain_recover_run() float target_pitch = 0; // 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); + // 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); 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/control_circle.cpp b/ArduSub/control_circle.cpp new file mode 100644 index 00000000000000..fca637e9bf8f3f --- /dev/null +++ b/ArduSub/control_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 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 new file mode 100644 index 00000000000000..3c13e04b231182 --- /dev/null +++ b/ArduSub/control_guided.cpp @@ -0,0 +1,563 @@ +#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 new file mode 100644 index 00000000000000..93e821740a12b4 --- /dev/null +++ b/ArduSub/control_manual.cpp @@ -0,0 +1,36 @@ +#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/mode_motordetect.cpp b/ArduSub/control_motordetect.cpp similarity index 96% rename from ArduSub/mode_motordetect.cpp rename to ArduSub/control_motordetect.cpp index 370e1838ce1829..1718de53b421d5 100644 --- a/ArduSub/mode_motordetect.cpp +++ b/ArduSub/control_motordetect.cpp @@ -45,7 +45,7 @@ namespace { static int16_t current_direction; } -bool ModeMotordetect::init(bool ignore_checks) +bool Sub::motordetect_init() { current_motor = 0; md_state = STANDBY; @@ -53,7 +53,7 @@ bool ModeMotordetect::init(bool ignore_checks) return true; } -void ModeMotordetect::run() +void Sub::motordetect_run() { // if not armed set throttle to zero and exit immediately if (!motors.armed()) { @@ -167,8 +167,8 @@ void ModeMotordetect::run() break; } case DONE: - set_mode(sub.prev_control_mode, ModeReason::MISSION_END); - sub.arming.disarm(AP_Arming::Method::MOTORDETECTDONE); + control_mode = prev_control_mode; + arming.disarm(AP_Arming::Method::MOTORDETECTDONE); break; } } diff --git a/ArduSub/mode_poshold.cpp b/ArduSub/control_poshold.cpp similarity index 50% rename from ArduSub/mode_poshold.cpp rename to ArduSub/control_poshold.cpp index b85e359ede568c..46121952ae054a 100644 --- a/ArduSub/mode_poshold.cpp +++ b/ArduSub/control_poshold.cpp @@ -7,47 +7,47 @@ #if POSHOLD_ENABLED == ENABLED // poshold_init - initialise PosHold controller -bool ModePoshold::init(bool ignore_checks) +bool Sub::poshold_init() { // fail to initialise PosHold mode if no GPS lock - if (!sub.position_ok()) { + if (!position_ok()) { return false; } // initialize vertical speeds and acceleration - position_control->set_max_speed_accel_xy(sub.wp_nav.get_default_speed_xy(), sub.wp_nav.get_wp_acceleration()); - position_control->set_correction_speed_accel_xy(sub.wp_nav.get_default_speed_xy(), sub.wp_nav.get_wp_acceleration()); - position_control->set_max_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - position_control->set_correction_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + pos_control.set_max_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration()); + pos_control.set_correction_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration()); + pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise position and desired velocity - position_control->init_xy_controller_stopping_point(); - position_control->init_z_controller(); + pos_control.init_xy_controller_stopping_point(); + pos_control.init_z_controller(); // Stop all thrusters - attitude_control->set_throttle_out(0.5f ,true, g.throttle_filt); - attitude_control->relax_attitude_controllers(); - position_control->relax_z_controller(0.5f); + attitude_control.set_throttle_out(0.5f ,true, g.throttle_filt); + attitude_control.relax_attitude_controllers(); + pos_control.relax_z_controller(0.5f); - sub.last_pilot_heading = ahrs.yaw_sensor; + last_pilot_heading = ahrs.yaw_sensor; return true; } // poshold_run - runs the PosHold controller // should be called at 100hz or more -void ModePoshold::run() +void Sub::poshold_run() { uint32_t tnow = AP_HAL::millis(); // When unarmed, disable motors and stabilization if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle) - attitude_control->set_throttle_out(0.5f ,true, g.throttle_filt); - attitude_control->relax_attitude_controllers(); - position_control->init_xy_controller_stopping_point(); - position_control->relax_z_controller(0.5f); - sub.last_pilot_heading = ahrs.yaw_sensor; + attitude_control.set_throttle_out(0.5f ,true, g.throttle_filt); + attitude_control.relax_attitude_controllers(); + pos_control.init_xy_controller_stopping_point(); + pos_control.relax_z_controller(0.5f); + last_pilot_heading = ahrs.yaw_sensor; return; } @@ -62,15 +62,15 @@ void ModePoshold::run() float lateral_out = 0; float forward_out = 0; - if (sub.position_ok()) { + if (position_ok()) { // Allow pilot to reposition the sub if (fabsf(pilot_lateral) > 0.1 || fabsf(pilot_forward) > 0.1) { - position_control->init_xy_controller_stopping_point(); + pos_control.init_xy_controller_stopping_point(); } - sub.translate_pos_control_rp(lateral_out, forward_out); - position_control->update_xy_controller(); + translate_pos_control_rp(lateral_out, forward_out); + pos_control.update_xy_controller(); } else { - position_control->init_xy_controller_stopping_point(); + pos_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 ModePoshold::run() // Update attitude // // get pilot's desired yaw rate - float target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // convert pilot input to lean angles // To-Do: convert get_pilot_desired_lean_angles to return angles as floats float target_roll, target_pitch; - 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_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); // update attitude controller targets if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); - sub.last_pilot_heading = ahrs.yaw_sensor; - sub.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); + 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 manoeuvres + // 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 + 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); - sub.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); + 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); + attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true); } } diff --git a/ArduSub/control_stabilize.cpp b/ArduSub/control_stabilize.cpp new file mode 100644 index 00000000000000..356ce2479be1d6 --- /dev/null +++ b/ArduSub/control_stabilize.cpp @@ -0,0 +1,69 @@ +#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 new file mode 100644 index 00000000000000..407b04ab5a88ec --- /dev/null +++ b/ArduSub/control_surface.cpp @@ -0,0 +1,63 @@ +#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 87e45820bc9828..b3c7109f10fda4 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -26,8 +26,21 @@ 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_YAW_RATE = 7 // steer the sub with the desired yaw rate + 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 }; // Acro Trainer types @@ -39,10 +52,35 @@ enum autopilot_yaw_mode { #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) +#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 5c0d08d32499cf..4b2fa212b7eeb6 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 == Mode::Number::ALT_HOLD || control_mode == Mode::Number::SURFACE || sub.flightmode->requires_GPS()) { + if (control_mode == ALT_HOLD || control_mode == SURFACE || mode_requires_GPS(control_mode)) { // This should always succeed - if (!set_mode(Mode::Number::MANUAL, ModeReason::BAD_DEPTH)) { + if (!set_mode(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(Mode::Number::SURFACE, ModeReason::BATTERY_FAILSAFE); + set_mode(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(Mode::Number::SURFACE, ModeReason::LEAK_FAILSAFE); + set_mode(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(Mode::Number::ALT_HOLD, ModeReason::GCS_FAILSAFE)) { + if (!set_mode(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(Mode::Number::SURFACE, ModeReason::GCS_FAILSAFE)) { + if (!set_mode(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 == Mode::Number::ACRO || control_mode == Mode::Number::MANUAL) { + if (control_mode == ACRO || control_mode == 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 == Mode::Number::AUTO || control_mode == Mode::Number::GUIDED); + bool valid_mode = (control_mode == AUTO || control_mode == 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 || !sub.mode_auto.auto_terrain_recover_start()) { + if (!rangefinder_state.enabled || !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(Mode::Number::POSHOLD, ModeReason::TERRAIN_FAILSAFE)) { - set_mode(Mode::Number::ALT_HOLD, ModeReason::TERRAIN_FAILSAFE); + if (!set_mode(POSHOLD, ModeReason::TERRAIN_FAILSAFE)) { + set_mode(ALT_HOLD, ModeReason::TERRAIN_FAILSAFE); } AP_Notify::events.failsafe_mode_change = 1; break; case FS_TERRAIN_SURFACE: - set_mode(Mode::Number::SURFACE, ModeReason::TERRAIN_FAILSAFE); + set_mode(SURFACE, ModeReason::TERRAIN_FAILSAFE); AP_Notify::events.failsafe_mode_change = 1; break; diff --git a/ArduSub/fence.cpp b/ArduSub/fence.cpp index c39e84aebf5319..7d89c89cae1ee4 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 = sub.fence.check(); + const uint8_t new_breaches = 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 new file mode 100644 index 00000000000000..521586923fd8fc --- /dev/null +++ b/ArduSub/flight_mode.cpp @@ -0,0 +1,230 @@ +#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 f10d7d3a451089..bbbd19c3464cc9 100644 --- a/ArduSub/joystick.cpp +++ b/ArduSub/joystick.cpp @@ -1,5 +1,4 @@ #include "Sub.h" -#include "mode.h" // Functions that will handle joystick/gamepad input // ---------------------------------------------------------------------------- @@ -35,7 +34,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(Mode::Number::MANUAL, ModeReason::RC_COMMAND); // Initialize flight mode + set_mode(MANUAL, ModeReason::RC_COMMAND); // Initialize flight mode if (g.numGainSettings < 1) { g.numGainSettings.set_and_save(1); @@ -158,28 +157,28 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held) break; case JSButton::button_function_t::k_mode_manual: - set_mode(Mode::Number::MANUAL, ModeReason::RC_COMMAND); + set_mode(MANUAL, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_stabilize: - set_mode(Mode::Number::STABILIZE, ModeReason::RC_COMMAND); + set_mode(STABILIZE, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_depth_hold: - set_mode(Mode::Number::ALT_HOLD, ModeReason::RC_COMMAND); + set_mode(ALT_HOLD, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_auto: - set_mode(Mode::Number::AUTO, ModeReason::RC_COMMAND); + set_mode(AUTO, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_guided: - set_mode(Mode::Number::GUIDED, ModeReason::RC_COMMAND); + set_mode(GUIDED, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_circle: - set_mode(Mode::Number::CIRCLE, ModeReason::RC_COMMAND); + set_mode(CIRCLE, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_acro: - set_mode(Mode::Number::ACRO, ModeReason::RC_COMMAND); + set_mode(ACRO, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_poshold: - set_mode(Mode::Number::POSHOLD, ModeReason::RC_COMMAND); + set_mode(POSHOLD, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mount_center: @@ -360,7 +359,6 @@ 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; @@ -425,12 +423,10 @@ 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 @@ -561,7 +557,6 @@ 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) { @@ -593,21 +588,6 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held) case JSButton::button_function_t::k_custom_6: // Not implemented break; - -#if AP_SCRIPTING_ENABLED - case JSButton::button_function_t::k_script_1: - sub.script_buttons[0].press(); - break; - case JSButton::button_function_t::k_script_2: - sub.script_buttons[1].press(); - break; - case JSButton::button_function_t::k_script_3: - sub.script_buttons[2].press(); - break; - case JSButton::button_function_t::k_script_4: - sub.script_buttons[3].press(); - break; -#endif // AP_SCRIPTING_ENABLED } } @@ -615,7 +595,6 @@ 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; @@ -628,8 +607,6 @@ 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: { @@ -651,22 +628,6 @@ void Sub::handle_jsbutton_release(uint8_t _button, bool shift) { ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_trim()); // 1-indexed } break; -#endif - -#if AP_SCRIPTING_ENABLED - case JSButton::button_function_t::k_script_1: - sub.script_buttons[0].release(); - break; - case JSButton::button_function_t::k_script_2: - sub.script_buttons[1].release(); - break; - case JSButton::button_function_t::k_script_3: - sub.script_buttons[2].release(); - break; - case JSButton::button_function_t::k_script_4: - sub.script_buttons[3].release(); - break; -#endif // AP_SCRIPTING_ENABLED } } @@ -760,15 +721,3 @@ void Sub::clear_input_hold() zTrim = 0; input_hold_engaged = false; } - -#if AP_SCRIPTING_ENABLED -bool Sub::is_button_pressed(uint8_t index) -{ - return script_buttons[index - 1].is_pressed(); -} - -uint8_t Sub::get_and_clear_button_count(uint8_t index) -{ - return script_buttons[index - 1].get_and_clear_count(); -} -#endif // AP_SCRIPTING_ENABLED diff --git a/ArduSub/mode.cpp b/ArduSub/mode.cpp deleted file mode 100644 index 323836c2f94abb..00000000000000 --- a/ArduSub/mode.cpp +++ /dev/null @@ -1,292 +0,0 @@ -#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 deleted file mode 100644 index 584f5b26d315db..00000000000000 --- a/ArduSub/mode.h +++ /dev/null @@ -1,449 +0,0 @@ -#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 deleted file mode 100644 index 29b1e61891667c..00000000000000 --- a/ArduSub/mode_acro.cpp +++ /dev/null @@ -1,46 +0,0 @@ -#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 deleted file mode 100644 index 3282054d7329ad..00000000000000 --- a/ArduSub/mode_althold.cpp +++ /dev/null @@ -1,114 +0,0 @@ -#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/mode_circle.cpp b/ArduSub/mode_circle.cpp deleted file mode 100644 index 28be79dbad54cd..00000000000000 --- a/ArduSub/mode_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 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 deleted file mode 100644 index 25857bf5c94e64..00000000000000 --- a/ArduSub/mode_guided.cpp +++ /dev/null @@ -1,866 +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 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 deleted file mode 100644 index 9d6e29892ea91a..00000000000000 --- a/ArduSub/mode_manual.cpp +++ /dev/null @@ -1,35 +0,0 @@ -#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/mode_stabilize.cpp b/ArduSub/mode_stabilize.cpp deleted file mode 100644 index cd75be241b6c2c..00000000000000 --- a/ArduSub/mode_stabilize.cpp +++ /dev/null @@ -1,68 +0,0 @@ -#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 deleted file mode 100644 index 6bab07a067b860..00000000000000 --- a/ArduSub/mode_surface.cpp +++ /dev/null @@ -1,63 +0,0 @@ -#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 895ade8c8c53b5..5d33b5052b40e4 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 == Mode::Number::MOTOR_DETECT){ + if (control_mode == MOTOR_DETECT){ return; } // check if we are performing the motor test @@ -78,7 +78,7 @@ bool Sub::verify_motor_test() return true; } -bool Sub::handle_do_motor_test(mavlink_command_int_t command) { +bool Sub::handle_do_motor_test(mavlink_command_long_t command) { last_do_motor_test_ms = AP_HAL::millis(); // If we are not already testing motors, initialize test @@ -103,9 +103,9 @@ bool Sub::handle_do_motor_test(mavlink_command_int_t command) { float throttle = command.param3; // float timeout_s = command.param4; // not used // float motor_count = command.param5; // not used - const uint32_t test_type = command.y; + float test_type = command.param6; - if (test_type != MOTOR_TEST_ORDER_BOARD) { + if (!is_equal(test_type, (float)MOTOR_TEST_ORDER_BOARD)) { gcs().send_text(MAV_SEVERITY_WARNING, "bad test type %0.2f", (double)test_type); return false; // test type not supported here } diff --git a/ArduSub/script_button.cpp b/ArduSub/script_button.cpp deleted file mode 100644 index fa8f85079636d0..00000000000000 --- a/ArduSub/script_button.cpp +++ /dev/null @@ -1,47 +0,0 @@ -#include "AP_Scripting/AP_Scripting_config.h" - -#if AP_SCRIPTING_ENABLED - -#include -#include "script_button.h" - -void ScriptButton::press() -{ - if (!pressed) { - pressed = true; - - // The count will max out at 255, but it won't roll over to 0. - if (count < std::numeric_limits::max()) { - count++; - } - } -} - -void ScriptButton::release() -{ - pressed = false; -} - -bool ScriptButton::is_pressed() const -{ - return pressed; -} - -uint8_t ScriptButton::get_count() const -{ - return count; -} - -void ScriptButton::clear_count() -{ - count = 0; -} - -uint8_t ScriptButton::get_and_clear_count() -{ - auto result = get_count(); - clear_count(); - return result; -} - -#endif // AP_SCRIPTING_ENABLED diff --git a/ArduSub/script_button.h b/ArduSub/script_button.h deleted file mode 100644 index 31a08a24004096..00000000000000 --- a/ArduSub/script_button.h +++ /dev/null @@ -1,34 +0,0 @@ -#pragma once - -#if AP_SCRIPTING_ENABLED - -#include - -// Joystick button object for use in Lua scripts. -// -// Provide 2 ways to use a joystick button: -// is_pressed() returns true if the button is currently (as of the most recent MANUAL_CONTROL msg) pressed -// get_and_clear_count() returns the number of times the button was pressed since the last call -// -class ScriptButton { -public: - ScriptButton(): pressed(false), count(0) {} - - void press(); - - void release(); - - bool is_pressed() const WARN_IF_UNUSED; - - uint8_t get_count() const WARN_IF_UNUSED; - - void clear_count(); - - uint8_t get_and_clear_count(); - -private: - bool pressed; - uint8_t count; -}; - -#endif // AP_SCRIPTING_ENABLED diff --git a/ArduSub/surface_bottom_detector.cpp b/ArduSub/surface_bottom_detector.cpp index 0338e77c99d4c3..4a1b4cd215edd2 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 hit bottom or surface and updates the ap.at_bottom and ap.at_surface flags +// checks if we have 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 487abe625e063a..7ac209a87f17b1 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -19,11 +19,6 @@ 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(); @@ -75,9 +70,7 @@ 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 @@ -104,7 +97,7 @@ void Sub::init_ardupilot() #if HAL_MOUNT_ENABLED // initialise camera mount camera_mount.init(); - // This step is necessary so that the servo is properly initialized + // This step ncessary so 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); @@ -171,6 +164,11 @@ 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 89da451ee21e7f..de8dc48a7b984e 100644 --- a/ArduSub/wscript +++ b/ArduSub/wscript @@ -18,6 +18,7 @@ def build(bld): 'AP_Beacon', 'AP_TemperatureSensor', 'AP_Arming', + 'AP_KDECAN', 'AP_OSD', ], ) diff --git a/BUILD.md b/BUILD.md index 9124d7acb7d3d1..0143d70da6258a 100644 --- a/BUILD.md +++ b/BUILD.md @@ -8,17 +8,18 @@ git clone --recursive https://github.com/ArduPilot/ardupilot.git cd ardupilot ``` -You can also read more about the build system in the +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 [Waf Book](https://waf.io/book/). -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. +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`). ## Basic usage ## -There are several commands in the build system for advanced usage, but here we +There are several commands in the build system for advanced usages, but here we list some basic and more used commands as example. * **Build ArduCopter** @@ -163,18 +164,6 @@ 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 ea36c2c1f0d873..c72d250cc2dbc8 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_up->get_radio_min() <= blimp.g.failsafe_throttle_value+10 || blimp.g.failsafe_throttle_value < 910) { + if (blimp.channel_down->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 ff5327006738d9..9c9f457b137162 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,9 +72,7 @@ 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), @@ -217,27 +215,12 @@ void Blimp::read_AHRS(void) ahrs.update(true); IGNORE_RETURN(ahrs.get_velocity_NED(vel_ned)); - IGNORE_RETURN(ahrs.get_relative_position_NED_origin(pos_ned)); + IGNORE_RETURN(ahrs.get_relative_position_NED_home(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 32ac39b5bddf4f..0c6e7a2627e37e 100644 --- a/Blimp/Blimp.h +++ b/Blimp/Blimp.h @@ -59,7 +59,6 @@ #include "config.h" #include "Fins.h" -#include "Loiter.h" #include "RC_Channel.h" // RC Channel Library @@ -92,10 +91,8 @@ 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); @@ -111,7 +108,7 @@ class Blimp : public AP_Vehicle // primary input control channels RC_Channel *channel_right; RC_Channel *channel_front; - RC_Channel *channel_up; + RC_Channel *channel_down; RC_Channel *channel_yaw; AP_Logger logger; @@ -175,7 +172,7 @@ class Blimp : public AP_Vehicle RCMapper rcmap; - // inertial nav alt when we armed + // intertial nav alt when we armed float arming_altitude_m; // Failsafe @@ -194,7 +191,6 @@ class Blimp : public AP_Vehicle // Motor Output Fins *motors; - Loiter *loiter; int32_t _home_bearing; uint32_t _home_distance; @@ -364,8 +360,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); @@ -382,7 +378,7 @@ class Blimp : public AP_Vehicle void notify_flight_mode(); // mode_land.cpp - void set_mode_land_failsafe(ModeReason reason); + void set_mode_land_with_pause(ModeReason reason); bool landing_with_GPS(); // // motors.cpp @@ -434,7 +430,6 @@ 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 89541eea7cd97d..e51a465b3690b9 100644 --- a/Blimp/Fins.h +++ b/Blimp/Fins.h @@ -10,7 +10,6 @@ class Fins { public: friend class Blimp; - friend class Loiter; enum motor_frame_class { MOTOR_FRAME_UNDEFINED = 0, @@ -50,7 +49,7 @@ class Fins uint16_t _speed_hz; // speed in hz to send updates to motors float _throttle_avg_max; // last throttle input from set_throttle_avg_max - float _time; // current timestamp + float _time; //current timestep bool _armed; // 0 if disarmed, 1 if armed @@ -71,7 +70,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 @@ -96,7 +95,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_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index 78442c9aaabb77..515a2870b6b9c4 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, @@ -323,9 +323,7 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { MSG_GPS2_RAW, MSG_GPS2_RTK, MSG_NAV_CONTROLLER_OUTPUT, -#if AP_FENCE_ENABLED MSG_FENCE_STATUS, -#endif MSG_POSITION_TARGET_GLOBAL_INT, }; static const ap_message STREAM_POSITION_msgs[] = { @@ -355,10 +353,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = { MSG_BATTERY_STATUS, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_OPTICAL_FLOW, -#if COMPASS_CAL_ENABLED MSG_MAG_CAL_REPORT, MSG_MAG_CAL_PROGRESS, -#endif MSG_EKF_STATUS_REPORT, MSG_VIBRATION, #if AP_RPM_ENABLED @@ -421,7 +417,7 @@ void GCS_MAVLINK_Blimp::send_banner() send_text(MAV_SEVERITY_INFO, "Frame: %s", blimp.get_frame_string()); } -MAV_RESULT GCS_MAVLINK_Blimp::_handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) +MAV_RESULT GCS_MAVLINK_Blimp::_handle_command_preflight_calibration(const mavlink_command_long_t &packet, const mavlink_message_t &msg) { return GCS_MAVLINK::_handle_command_preflight_calibration(packet, msg); } @@ -470,17 +466,30 @@ 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, const mavlink_message_t &msg) +MAV_RESULT GCS_MAVLINK_Blimp::handle_command_int_packet(const mavlink_command_int_t &packet) { 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, msg); + 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_mount(packet); } -MAV_RESULT GCS_MAVLINK_Blimp::handle_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg) +MAV_RESULT GCS_MAVLINK_Blimp::handle_command_long_packet(const mavlink_command_long_t &packet) { switch (packet.command) { @@ -506,7 +515,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, msg); + return GCS_MAVLINK::handle_command_long_packet(packet); } } @@ -531,7 +540,7 @@ void GCS_MAVLINK_Blimp::handleMessage(const mavlink_message_t &msg) } // end handle mavlink -MAV_RESULT GCS_MAVLINK_Blimp::handle_flight_termination(const mavlink_command_int_t &packet) +MAV_RESULT GCS_MAVLINK_Blimp::handle_flight_termination(const mavlink_command_long_t &packet) { MAV_RESULT result = MAV_RESULT_FAILED; if (packet.param1 > 0.5f) { diff --git a/Blimp/GCS_Mavlink.h b/Blimp/GCS_Mavlink.h index ed406ac0f93661..3e2d67d7d4015c 100644 --- a/Blimp/GCS_Mavlink.h +++ b/Blimp/GCS_Mavlink.h @@ -13,7 +13,7 @@ class GCS_MAVLINK_Blimp : public GCS_MAVLINK uint32_t telem_delay() const override; - MAV_RESULT handle_flight_termination(const mavlink_command_int_t &packet) override; + MAV_RESULT handle_flight_termination(const mavlink_command_long_t &packet) override; uint8_t sysid_my_gcs() const override; bool sysid_enforce() const override; @@ -21,13 +21,14 @@ class GCS_MAVLINK_Blimp : public GCS_MAVLINK bool params_ready() const override; void send_banner() override; - MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; + MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet, const mavlink_message_t &msg) override; void send_position_target_global_int() override; MAV_RESULT handle_command_do_set_roi(const Location &roi_loc) override; - MAV_RESULT handle_command_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_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_do_reposition(const mavlink_command_int_t &packet); @@ -73,7 +74,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 01a3e0f3e7b293..dab16518e3913e 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,9 +92,7 @@ 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 @@ -234,6 +232,74 @@ tuning_max : tune_max logger.WriteBlock(&pkt_tune, sizeof(pkt_tune)); } +struct PACKED log_SysIdD { + LOG_PACKET_HEADER; + uint64_t time_us; + float waveform_time; + float waveform_sample; + float waveform_freq; + float angle_x; + float angle_y; + float angle_z; + float accel_x; + float accel_y; + float accel_z; +}; + +// Write an rate packet +void Blimp::Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z) +{ +#if MODE_SYSTEMID_ENABLED == ENABLED + struct log_SysIdD pkt_sidd = { + LOG_PACKET_HEADER_INIT(LOG_SYSIDD_MSG), +time_us : AP_HAL::micros64(), +waveform_time : waveform_time, +waveform_sample : waveform_sample, +waveform_freq : waveform_freq, +angle_x : angle_x, +angle_y : angle_y, +angle_z : angle_z, +accel_x : accel_x, +accel_y : accel_y, +accel_z : accel_z + }; + logger.WriteBlock(&pkt_sidd, sizeof(pkt_sidd)); +#endif +} + +struct PACKED log_SysIdS { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t systemID_axis; + float waveform_magnitude; + float frequency_start; + float frequency_stop; + float time_fade_in; + float time_const_freq; + float time_record; + float time_fade_out; +}; + +// Write an rate packet +void Blimp::Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out) +{ +#if MODE_SYSTEMID_ENABLED == ENABLED + struct log_SysIdS pkt_sids = { + LOG_PACKET_HEADER_INIT(LOG_SYSIDS_MSG), +time_us : AP_HAL::micros64(), +systemID_axis : systemID_axis, +waveform_magnitude : waveform_magnitude, +frequency_start : frequency_start, +frequency_stop : frequency_stop, +time_fade_in : time_fade_in, +time_const_freq : time_const_freq, +time_record : time_record, +time_fade_out : time_fade_out + }; + logger.WriteBlock(&pkt_sids, sizeof(pkt_sids)); +#endif +} + // type and unit information can be found in // libraries/AP_Logger/Logstructure.h; search for "log_Units" for // units and "Format characters" for field type information @@ -248,10 +314,8 @@ 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 @@ -265,10 +329,8 @@ 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 @@ -282,28 +344,17 @@ const struct LogStructure Blimp::log_structure[] = { // @Field: FF: controller feed-forward portion of response // @Field: Dmod: scaler applied to D gain to reduce limit cycling // @Field: SRate: slew rate - // @Field: Flags: bitmask of PID state flags - // @FieldBitmaskEnum: Flags: log_PID_Flags - { - LOG_PIDD_MSG, sizeof(log_PID), - "PIDD", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS - }, - { - LOG_PIVN_MSG, sizeof(log_PID), - "PIVN", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS - }, - { - LOG_PIVE_MSG, sizeof(log_PID), - "PIVE", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS - }, - { - LOG_PIVD_MSG, sizeof(log_PID), - "PIVD", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS - }, - { - LOG_PIVY_MSG, sizeof(log_PID), - "PIVY", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS - }, + // @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 }, // @LoggerMessage: PTUN // @Description: Parameter Tuning information @@ -390,6 +441,41 @@ const struct LogStructure Blimp::log_structure[] = { LOG_DATA_FLOAT_MSG, sizeof(log_Data_Float), "DFLT", "QBf", "TimeUS,Id,Value", "s--", "F--" }, + + // @LoggerMessage: SIDD + // @Description: System ID data + // @Field: TimeUS: Time since system startup + // @Field: Time: Time reference for waveform + // @Field: Targ: Current waveform sample + // @Field: F: Instantaneous waveform frequency + // @Field: Gx: Delta angle, X-Axis + // @Field: Gy: Delta angle, Y-Axis + // @Field: Gz: Delta angle, Z-Axis + // @Field: Ax: Delta velocity, X-Axis + // @Field: Ay: Delta velocity, Y-Axis + // @Field: Az: Delta velocity, Z-Axis + + { + LOG_SYSIDD_MSG, sizeof(log_SysIdD), + "SIDD", "Qfffffffff", "TimeUS,Time,Targ,F,Gx,Gy,Gz,Ax,Ay,Az", "ss-zkkkooo", "F---------" + }, + + // @LoggerMessage: SIDS + // @Description: System ID settings + // @Field: TimeUS: Time since system startup + // @Field: Ax: The axis which is being excited + // @Field: Mag: Magnitude of the chirp waveform + // @Field: FSt: Frequency at the start of chirp + // @Field: FSp: Frequency at the end of chirp + // @Field: TFin: Time to reach maximum amplitude of chirp + // @Field: TC: Time at constant frequency before chirp starts + // @Field: TR: Time taken to complete chirp waveform + // @Field: TFout: Time to reach zero amplitude after chirp finishes + + { + LOG_SYSIDS_MSG, sizeof(log_SysIdS), + "SIDS", "QBfffffff", "TimeUS,Ax,Mag,FSt,FSp,TFin,TC,TR,TFout", "s--ssssss", "F--------" + }, }; void Blimp::Log_Write_Vehicle_Startup_Messages() @@ -419,6 +505,8 @@ 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 deleted file mode 100644 index 8c6b5d5e7aea7c..00000000000000 --- a/Blimp/Loiter.cpp +++ /dev/null @@ -1,202 +0,0 @@ -#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 deleted file mode 100644 index c21fdd57c6f251..00000000000000 --- a/Blimp/Loiter.h +++ /dev/null @@ -1,59 +0,0 @@ -#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 bb0ac2bb40d6af..487d0981146b01 100644 --- a/Blimp/Parameters.cpp +++ b/Blimp/Parameters.cpp @@ -53,6 +53,15 @@ 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. @@ -175,7 +184,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,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,11:Optical Flow,12:PID,13:Compass // @User: Standard GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), @@ -263,20 +272,12 @@ const AP_Param::Info Blimp::var_info[] = { // @Param: DIS_MASK // @DisplayName: Disable output mask - // @Description: Mask for disabling (setting to zero) one or more of the 4 output axis in mode Velocity or Loiter + // @Description: Mask for disabling 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 @@ -721,14 +722,6 @@ const AP_Param::Info Blimp::var_info[] = { // @Range: 0 200 // @Increment: 0.5 // @User: Advanced - - // @Param: POSYAW_PDMX - // @DisplayName: Position (yaw) axis controller PD sum maximum - // @Description: Position (yaw) axis controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output - // @Range: 0 4000 - // @Increment: 10 - // @Units: d% - // @User: Advanced GOBJECT(pid_pos_yaw, "POSYAW_", AC_PID), // @Group: diff --git a/Blimp/Parameters.h b/Blimp/Parameters.h index 8ec466d62c0c6d..c819ad0ab46543 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, //unused + k_param_pilot_takeoff_alt, // AP_ADSB Library k_param_adsb, @@ -110,7 +110,6 @@ class Parameters k_param_max_pos_yaw, k_param_simple_mode, k_param_dis_mask, - k_param_pid_dz, // // 90: misc2 @@ -136,7 +135,7 @@ class Parameters k_param_gcs2, k_param_serial_manager, k_param_gcs3, - k_param_gcs_pid_mask, + k_param_gcs_pid_mask, // 126 k_param_gcs4, k_param_gcs5, k_param_gcs6, @@ -214,6 +213,7 @@ 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,7 +254,6 @@ 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 7709867cf25c4a..39682160ae5dc1 100644 --- a/Blimp/RC_Channel.cpp +++ b/Blimp/RC_Channel.cpp @@ -36,11 +36,6 @@ 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) { @@ -106,7 +101,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_up->get_control_in() == 0)) { + (blimp.channel_down->get_control_in() == 0)) { blimp.save_trim(); } break; diff --git a/Blimp/RC_Channel.h b/Blimp/RC_Channel.h index ec37259794cb82..80e53ceda23028 100644 --- a/Blimp/RC_Channel.h +++ b/Blimp/RC_Channel.h @@ -30,7 +30,6 @@ 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 9c5908f65fed98..7eba1cceba3243 100644 --- a/Blimp/config.h +++ b/Blimp/config.h @@ -22,6 +22,13 @@ # 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 @@ -29,6 +36,53 @@ # 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 @@ -71,9 +125,136 @@ # define EKF_ORIGIN_MAX_DIST_M 50000 // EKF origin and waypoints (including home) must be within 50km #endif +////////////////////////////////////////////////////////////////////////////// +// OPTICAL_FLOW +#ifndef OPTFLOW +# define OPTFLOW ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Auto Tuning +#ifndef AUTOTUNE_ENABLED +# define AUTOTUNE_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Precision Landing with companion computer or IRLock sensor +#ifndef PRECISION_LANDING +# define PRECISION_LANDING ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Parachute release +#ifndef PARACHUTE +# define PARACHUTE HAL_PARACHUTE_ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Nav-Guided - allows external nav computer to control vehicle +#ifndef NAV_GUIDED +# define NAV_GUIDED !HAL_MINIMIZE_FEATURES +#endif + +////////////////////////////////////////////////////////////////////////////// +// Acro - fly vehicle in acrobatic mode +#ifndef MODE_ACRO_ENABLED +# define MODE_ACRO_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Auto mode - allows vehicle to trace waypoints and perform automated actions +#ifndef MODE_AUTO_ENABLED +# define MODE_AUTO_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Brake mode - bring vehicle to stop +#ifndef MODE_BRAKE_ENABLED +# define MODE_BRAKE_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Circle - fly vehicle around a central point +#ifndef MODE_CIRCLE_ENABLED +# define MODE_CIRCLE_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Drift - fly vehicle in altitude-held, coordinated-turn mode +#ifndef MODE_DRIFT_ENABLED +# define MODE_DRIFT_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// flip - fly vehicle in flip in pitch and roll direction mode +#ifndef MODE_FLIP_ENABLED +# define MODE_FLIP_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Follow - follow another vehicle or GCS +#ifndef MODE_FOLLOW_ENABLED +# define MODE_FOLLOW_ENABLED !HAL_MINIMIZE_FEATURES +#endif + +////////////////////////////////////////////////////////////////////////////// +// Guided mode - control vehicle's position or angles from GCS +#ifndef MODE_GUIDED_ENABLED +# define MODE_GUIDED_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// GuidedNoGPS mode - control vehicle's angles from GCS +#ifndef MODE_GUIDED_NOGPS_ENABLED +# define MODE_GUIDED_NOGPS_ENABLED !HAL_MINIMIZE_FEATURES +#endif + +////////////////////////////////////////////////////////////////////////////// +// Loiter mode - allows vehicle to hold global position +#ifndef MODE_LOITER_ENABLED +# define MODE_LOITER_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Position Hold - enable holding of global position +#ifndef MODE_POSHOLD_ENABLED +# define MODE_POSHOLD_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// RTL - Return To Launch +#ifndef MODE_RTL_ENABLED +# define MODE_RTL_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// SmartRTL - allows vehicle to retrace a (loop-eliminated) breadcrumb home +#ifndef MODE_SMARTRTL_ENABLED +# define MODE_SMARTRTL_ENABLED ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Sport - fly vehicle in rate-controlled (earth-frame) mode +#ifndef MODE_SPORT_ENABLED +# define MODE_SPORT_ENABLED !HAL_MINIMIZE_FEATURES +#endif + +////////////////////////////////////////////////////////////////////////////// +// System ID - conduct system identification tests on vehicle +#ifndef MODE_SYSTEMID_ENABLED +# define MODE_SYSTEMID_ENABLED !HAL_MINIMIZE_FEATURES +#endif + +////////////////////////////////////////////////////////////////////////////// +// RADIO CONFIGURATION +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// + + ////////////////////////////////////////////////////////////////////////////// // FLIGHT_MODE // + #ifndef FLIGHT_MODE_1 # define FLIGHT_MODE_1 Mode::Number::MANUAL #endif @@ -93,6 +274,7 @@ # define FLIGHT_MODE_6 Mode::Number::MANUAL #endif + ////////////////////////////////////////////////////////////////////////////// // Throttle Failsafe // @@ -100,17 +282,188 @@ # 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 0a73072a1aea5f..f356e77600f551 100644 --- a/Blimp/defines.h +++ b/Blimp/defines.h @@ -10,6 +10,111 @@ #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, @@ -79,7 +184,8 @@ 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_LAND_EVEN_MANUAL 3 // switch to Land mode on EKF failsafe even if in Manual mode +#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 // 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 fd524b99ab91a5..8bc678922f0b5b 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_MANUAL)) { + if (!blimp.flightmode->requires_GPS() && (g.fs_ekf_action != FS_EKF_ACTION_LAND_EVEN_STABILIZE)) { 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_MANUAL: + case FS_EKF_ACTION_LAND_EVEN_STABILIZE: default: - set_mode_land_failsafe(ModeReason::EKF_FAILSAFE); + set_mode_land_with_pause(ModeReason::EKF_FAILSAFE); break; } } diff --git a/Blimp/events.cpp b/Blimp/events.cpp index 452f624c5b879f..cbd778cd6ca69f 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_failsafe(reason); + set_mode_land_with_pause(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 b7d80bd906100f..511ac6be5bbc84 100644 --- a/Blimp/mode.cpp +++ b/Blimp/mode.cpp @@ -16,10 +16,9 @@ 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_up(blimp.channel_up), + channel_down(blimp.channel_down), channel_yaw(blimp.channel_yaw), G_Dt(blimp.G_Dt) { }; @@ -42,9 +41,6 @@ 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; } @@ -140,7 +136,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() @@ -156,23 +152,18 @@ void Mode::update_navigation() run_autopilot(); } -// returns desired thrust/acceleration -void Mode::get_pilot_input(Vector3f &pilot, float &yaw) +// returns desired angle in centi-degrees +void Mode::get_pilot_desired_accelerations(float &right_out, float &front_out) const { // throttle failsafe check if (blimp.failsafe.radio || !blimp.ap.rc_receiver_present) { - pilot.y = 0; - pilot.x = 0; - pilot.z = 0; - yaw = 0; + right_out = 0; + front_out = 0; return; } - // 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); + // fetch roll and pitch inputs + right_out = channel_right->get_control_in(); + front_out = channel_front->get_control_in(); } bool Mode::is_disarmed_or_landed() const diff --git a/Blimp/mode.h b/Blimp/mode.h index 8a990fbb31b30c..81ef932059a89f 100644 --- a/Blimp/mode.h +++ b/Blimp/mode.h @@ -17,7 +17,6 @@ class Mode MANUAL = 1, // manual control VELOCITY = 2, // velocity mode LOITER = 3, // loiter mode (position hold) - RTL = 4, // rtl }; // constructor @@ -84,7 +83,7 @@ class Mode void update_navigation(); // pilot input processing - void get_pilot_input(Vector3f &pilot, float &yaw); + void get_pilot_desired_accelerations(float &right_out, float &front_out) const; protected: @@ -105,14 +104,75 @@ class Mode AP_InertialNav &inertial_nav; AP_AHRS &ahrs; Fins *&motors; - Loiter *&loiter; RC_Channel *&channel_right; RC_Channel *&channel_front; - RC_Channel *&channel_up; + RC_Channel *&channel_down; RC_Channel *&channel_yaw; float &G_Dt; public: + // Navigation Yaw control + class AutoYaw + { + + public: + + // yaw(): main product of AutoYaw; the heading: + float yaw(); + + // mode(): current method of determining desired yaw: + autopilot_yaw_mode mode() const + { + return (autopilot_yaw_mode)_mode; + } + void set_mode_to_default(bool rtl); + void set_mode(autopilot_yaw_mode new_mode); + autopilot_yaw_mode default_mode(bool rtl) const; + + // rate_cds(): desired yaw rate in centidegrees/second: + float rate_cds() const; + void set_rate(float new_rate_cds); + + // set_roi(...): set a "look at" location: + void set_roi(const Location &roi_location); + + void set_fixed_yaw(float angle_deg, + float turn_rate_dps, + int8_t direction, + bool relative_angle); + + private: + + float look_ahead_yaw(); + float roi_yaw(); + + // auto flight mode's yaw mode + uint8_t _mode = AUTO_YAW_LOOK_AT_NEXT_WP; + + // Yaw will point at this location if mode is set to AUTO_YAW_ROI + Vector3f roi; + + // bearing from current location to the ROI + float _roi_yaw; + + // yaw used for YAW_FIXED yaw_mode + int32_t _fixed_yaw; + + // Deg/s we should turn + int16_t _fixed_yaw_slewrate; + + // heading when in yaw_look_ahead_yaw + float _look_ahead_yaw; + + // turn rate (in cds) when auto_yaw_mode is set to AUTO_YAW_RATE + float _rate_cds; + + // used to reduce update rate to 100hz: + uint8_t roi_yaw_counter; + + }; + static AutoYaw auto_yaw; + // pass-through functions to reduce code churn on conversion; // these are candidates for moving into the Mode base // class. @@ -247,6 +307,7 @@ class ModeLoiter : public Mode private: Vector3f target_pos; float target_yaw; + float loop_period; }; class ModeLand : public Mode @@ -289,43 +350,3 @@ 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 640e69ef528df8..f6e334839e1d04 100644 --- a/Blimp/mode_land.cpp +++ b/Blimp/mode_land.cpp @@ -13,11 +13,12 @@ void ModeLand::run() motors->down_out = 0; } -// 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_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(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 725a702480815a..71e2794c2bbae3 100644 --- a/Blimp/mode_loiter.cpp +++ b/Blimp/mode_loiter.cpp @@ -3,47 +3,66 @@ * 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; - 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) { + 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){ //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 (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<<(2-1)))){ + motors->front_out = actuator.x; } - 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<<(1-1)))){ + motors->right_out = actuator.y; } - 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<<(3-1)))){ + motors->down_out = act_down; } - if (fabsf(wrap_PI(target_yaw-ahrs.get_yaw())) < (g.max_pos_yaw*POS_LAG)) { - target_yaw = wrap_PI(target_yaw + pilot_yaw); + if(!(blimp.g.dis_mask & (1<<(4-1)))){ + motors->yaw_out = act_yaw; } - blimp.loiter->run(target_pos, target_yaw, Vector4b{false,false,false,false}); + 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); } diff --git a/Blimp/mode_manual.cpp b/Blimp/mode_manual.cpp index 30a9eb7bbaffff..1183b4bd7a1ad1 100644 --- a/Blimp/mode_manual.cpp +++ b/Blimp/mode_manual.cpp @@ -6,11 +6,8 @@ // Runs the main manual controller void ModeManual::run() { - 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; + 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); } diff --git a/Blimp/mode_rtl.cpp b/Blimp/mode_rtl.cpp deleted file mode 100644 index c1b593cd9c9331..00000000000000 --- a/Blimp/mode_rtl.cpp +++ /dev/null @@ -1,20 +0,0 @@ -#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 ae6011c619fb48..a333d6cbc29440 100644 --- a/Blimp/mode_velocity.cpp +++ b/Blimp/mode_velocity.cpp @@ -8,17 +8,34 @@ // Runs the main velocity controller void ModeVelocity::run() { + const float dt = blimp.scheduler.get_last_loop_time_s(); + Vector3f target_vel; - 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.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; } - target_vel.z *= g.max_vel_z; - target_vel_yaw *= g.max_vel_yaw; - blimp.loiter->run_vel(target_vel, target_vel_yaw, Vector4b{false,false,false,false}); + 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); } diff --git a/Blimp/motors.cpp b/Blimp/motors.cpp index 35ba08b90cce93..c2034774880f41 100644 --- a/Blimp/motors.cpp +++ b/Blimp/motors.cpp @@ -18,7 +18,7 @@ void Blimp::arm_motors_check() } // ensure throttle is down - if (channel_up->get_control_in() > 0) { //MIR what dow we do with this? + if (channel_down->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 861c057820a5fc..e7640c44c2aa96 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_up->set_default_dead_zone(30); + channel_down->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_up = 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_down = 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_up->set_angle(RC_SCALE); + channel_down->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_up->get_radio_in()); - set_throttle_zero_flag(channel_up->get_control_in()); + set_throttle_and_failsafe(channel_down->get_radio_in()); + set_throttle_zero_flag(channel_down->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_up->get_control_in(), dt); + rc_throttle_control_in_filter.apply(channel_down->get_control_in(), dt); last_radio_update_ms = tnow_ms; return; } diff --git a/Blimp/system.cpp b/Blimp/system.cpp index f3d8bdc08e69f1..cec5103bb53aac 100644 --- a/Blimp/system.cpp +++ b/Blimp/system.cpp @@ -46,7 +46,6 @@ 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); @@ -59,9 +58,7 @@ 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 @@ -90,6 +87,11 @@ 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 e5bfa27c87be87..a922ff5c4c1e02 100644 --- a/Blimp/version.h +++ b/Blimp/version.h @@ -6,14 +6,14 @@ #include "ap_version.h" -#define THISFIRMWARE "Blimp V4.5.0-dev" +#define THISFIRMWARE "Blimp V4.3.0-dev" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,5,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 4,3,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 4 -#define FW_MINOR 5 +#define FW_MINOR 3 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV -#include +#include \ No newline at end of file diff --git a/Blimp/wscript b/Blimp/wscript index 34cb8c170490e0..a45b13a3137502 100644 --- a/Blimp/wscript +++ b/Blimp/wscript @@ -7,7 +7,9 @@ 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', @@ -25,4 +27,5 @@ 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 495c843488054b..f0be5d2aaa6090 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,6 +1,4 @@ -ARG BASE_IMAGE="ubuntu" -ARG TAG="22.04" -FROM ${BASE_IMAGE}:${TAG} +FROM ubuntu:20.04 WORKDIR /ardupilot ARG DEBIAN_FRONTEND=noninteractive diff --git a/README.md b/README.md index 7b603fc1632425..36994430d4fcbc 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://ardupilot.org/dev/docs/license-gplv3.html) +- [Overview of license](https://dev.ardupilot.com/wiki/license-gplv3) - [Full Text](https://github.com/ArduPilot/ardupilot/blob/master/COPYING.txt) diff --git a/Rover/AP_Arming.cpp b/Rover/AP_Arming.cpp index dc0f717d173e59..805f23d720a497 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 estimate is ok + // ensure position esetimate is ok if (!rover.ekf_position_ok()) { // vehicle level position estimate checks check_failed(display_failure, "Need Position Estimate"); @@ -78,7 +78,11 @@ bool AP_Arming_Rover::pre_arm_checks(bool report) //are arming checks disabled? if (checks_to_perform == 0) { - return mandatory_checks(report); + return true; + } + if (SRV_Channels::get_emergency_stop()) { + check_failed(report, "Motors Emergency Stopped"); + return false; } if (rover.g2.sailboat.sail_enabled() && !rover.g2.windvane.enabled()) { diff --git a/Rover/AP_Rally.cpp b/Rover/AP_Rally.cpp index b0daf506f10856..1a836159645eff 100644 --- a/Rover/AP_Rally.cpp +++ b/Rover/AP_Rally.cpp @@ -13,13 +13,11 @@ along with this program. If not, see . */ -#include "AP_Rally.h" - -#if HAL_RALLY_ENABLED +#include #include "Rover.h" -#include +#include "AP_Rally.h" bool AP_Rally_Rover::is_valid(const Location &rally_point) const { @@ -30,5 +28,3 @@ 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 197d6c92797fff..0055dcb1e5e93e 100644 --- a/Rover/AP_Rally.h +++ b/Rover/AP_Rally.h @@ -15,8 +15,7 @@ #pragma once #include - -#if HAL_RALLY_ENABLED +#include class AP_Rally_Rover : public AP_Rally { @@ -29,5 +28,3 @@ 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 24a41058cf756a..acfa8699b98365 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -53,7 +53,7 @@ MAV_MODE GCS_MAVLINK_Rover::base_mode() const uint32_t GCS_Rover::custom_mode() const { - return (uint32_t)rover.control_mode->mode_number(); + return rover.control_mode->mode_number(); } MAV_STATE GCS_MAVLINK_Rover::vehicle_system_status() const @@ -398,10 +398,8 @@ 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); } @@ -531,9 +529,7 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { MSG_GPS2_RAW, MSG_GPS2_RTK, MSG_NAV_CONTROLLER_OUTPUT, -#if AP_FENCE_ENABLED MSG_FENCE_STATUS, -#endif MSG_POSITION_TARGET_GLOBAL_INT, }; static const ap_message STREAM_POSITION_msgs[] = { @@ -566,10 +562,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = { MSG_BATTERY_STATUS, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_OPTICAL_FLOW, -#if COMPASS_CAL_ENABLED MSG_MAG_CAL_REPORT, MSG_MAG_CAL_PROGRESS, -#endif MSG_EKF_STATUS_REPORT, MSG_VIBRATION, #if AP_RPM_ENABLED @@ -614,15 +608,15 @@ bool GCS_MAVLINK_Rover::handle_guided_request(AP_Mission::Mission_Command &cmd) return rover.mode_guided.set_desired_location(cmd.content.location); } -MAV_RESULT GCS_MAVLINK_Rover::_handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) +MAV_RESULT GCS_MAVLINK_Rover::_handle_command_preflight_calibration(const mavlink_command_long_t &packet, const mavlink_message_t &msg) { - if (packet.y == 1) { + if (is_equal(packet.param6, 1.0f)) { if (rover.g2.windvane.start_direction_calibration()) { return MAV_RESULT_ACCEPTED; } else { return MAV_RESULT_FAILED; } - } else if (packet.y == 2) { + } else if (is_equal(packet.param6, 2.0f)) { if (rover.g2.windvane.start_speed_calibration()) { return MAV_RESULT_ACCEPTED; } else { @@ -641,7 +635,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, const mavlink_message_t &msg) +MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_packet(const mavlink_command_int_t &packet) { switch (packet.command) { @@ -661,46 +655,45 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_packet(const mavlink_command_in rover.control_mode->set_reversed(is_equal(packet.param1,1.0f)); return MAV_RESULT_ACCEPTED; + default: + return GCS_MAVLINK::handle_command_int_packet(packet); + } +} + +MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_long_t &packet) +{ + switch (packet.command) { + case MAV_CMD_NAV_RETURN_TO_LAUNCH: if (rover.set_mode(rover.mode_rtl, ModeReason::GCS_COMMAND)) { return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; - case MAV_CMD_DO_MOTOR_TEST: - // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle) - // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum) - // param3 : throttle (range depends upon param2) - // param4 : timeout (in seconds) - return rover.mavlink_motor_test_start(*this, - (AP_MotorsUGV::motor_test_order)packet.param1, - static_cast(packet.param2), - static_cast(packet.param3), - packet.param4); - case MAV_CMD_MISSION_START: if (rover.set_mode(rover.mode_auto, ModeReason::GCS_COMMAND)) { return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; -#if AP_MAVLINK_MAV_CMD_NAV_SET_YAW_SPEED_ENABLED - case MAV_CMD_NAV_SET_YAW_SPEED: - send_received_message_deprecation_warning("MAV_CMD_NAV_SET_YAW_SPEED"); - return handle_command_nav_set_yaw_speed(packet, msg); -#endif + case MAV_CMD_DO_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; - default: - return GCS_MAVLINK::handle_command_int_packet(packet, msg); - } -} + 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; -#if AP_MAVLINK_MAV_CMD_NAV_SET_YAW_SPEED_ENABLED -MAV_RESULT GCS_MAVLINK_Rover::handle_command_nav_set_yaw_speed(const mavlink_command_int_t &packet, const mavlink_message_t &msg) -{ - // param1 : yaw angle (may be absolute or relative) - // param2 : Speed - in metres/second - // param3 : 0 = param1 is absolute, 1 = param1 is relative + case MAV_CMD_NAV_SET_YAW_SPEED: + { + // param1 : yaw angle to adjust direction by in centidegress + // param2 : Speed - normalized to 0 .. 1 + // param3 : 0 = absolute, 1 = relative // exit if vehicle is not in Guided mode if (!rover.control_mode->in_guided_mode()) { @@ -716,8 +709,23 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_nav_set_yaw_speed(const mavlink_com rover.mode_guided.set_desired_heading_and_speed(packet.param1 * 100.0f, packet.param2); } return MAV_RESULT_ACCEPTED; + } + + case MAV_CMD_DO_MOTOR_TEST: + // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle) + // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum) + // param3 : throttle (range depends upon param2) + // param4 : timeout (in seconds) + return rover.mavlink_motor_test_start(*this, + (AP_MotorsUGV::motor_test_order)packet.param1, + static_cast(packet.param2), + static_cast(packet.param3), + packet.param4); + + default: + return GCS_MAVLINK::handle_command_long_packet(packet); + } } -#endif MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_do_reposition(const mavlink_command_int_t &packet) { @@ -1055,7 +1063,7 @@ void GCS_MAVLINK_Rover::handle_radio(const mavlink_message_t &msg) */ void GCS_MAVLINK_Rover::handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) { -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED rover.precland.handle_msg(packet, timestamp_ms); #endif } diff --git a/Rover/GCS_Mavlink.h b/Rover/GCS_Mavlink.h index f628bfa530bc4e..028c92f497a6b4 100644 --- a/Rover/GCS_Mavlink.h +++ b/Rover/GCS_Mavlink.h @@ -2,11 +2,6 @@ #include - // set 0 in 4.6, remove feature in 4.7: -#ifndef AP_MAVLINK_MAV_CMD_NAV_SET_YAW_SPEED_ENABLED -#define AP_MAVLINK_MAV_CMD_NAV_SET_YAW_SPEED_ENABLED 1 -#endif - class GCS_MAVLINK_Rover : public GCS_MAVLINK { public: @@ -20,10 +15,10 @@ class GCS_MAVLINK_Rover : public GCS_MAVLINK uint8_t sysid_my_gcs() const override; bool sysid_enforce() const override; - MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; - MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; + MAV_RESULT _handle_command_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_do_reposition(const mavlink_command_int_t &packet); - MAV_RESULT handle_command_nav_set_yaw_speed(const mavlink_command_int_t &packet, const mavlink_message_t &msg); void send_position_target_global_int() override; diff --git a/Rover/Log.cpp b/Rover/Log.cpp index c8c18d8ee9ec1e..6f40ceae0bf599 100644 --- a/Rover/Log.cpp +++ b/Rover/Log.cpp @@ -241,7 +241,7 @@ void Rover::Log_Write_RC(void) void Rover::Log_Write_Vehicle_Startup_Messages() { // only 200(?) bytes are guaranteed by AP_Logger - logger.Write_Mode((uint8_t)control_mode->mode_number(), control_mode_reason); + logger.Write_Mode(control_mode->mode_number(), control_mode_reason); ahrs.Log_Write_Home_And_Origin(); gps.Write_AP_Logger_Log_Startup_messages(); } diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index 313cec0c21dc62..5a155755c7b79e 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -29,7 +29,7 @@ const AP_Param::Info Rover::var_info[] = { // @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. // @CopyValuesFrom: MODE1 // @User: Advanced - GSCALAR(initial_mode, "INITIAL_MODE", (int8_t)Mode::Number::MANUAL), + GSCALAR(initial_mode, "INITIAL_MODE", Mode::Number::MANUAL), // @Param: SYSID_THISMAV // @DisplayName: MAVLink system ID of this vehicle @@ -109,7 +109,7 @@ const AP_Param::Info Rover::var_info[] = { // @Description: What to do on a failsafe event // @Values: 0:Nothing,1:RTL,2:Hold,3:SmartRTL or RTL,4:SmartRTL or Hold // @User: Standard - GSCALAR(fs_action, "FS_ACTION", (int8_t)FailsafeAction::Hold), + GSCALAR(fs_action, "FS_ACTION", Failsafe_Action_Hold), // @Param: FS_TIMEOUT // @DisplayName: Failsafe timeout @@ -174,38 +174,38 @@ const AP_Param::Info Rover::var_info[] = { // @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", (int8_t)Mode::Number::MANUAL), + GSCALAR(mode1, "MODE1", Mode::Number::MANUAL), // @Param: MODE2 // @DisplayName: Mode2 // @Description: Driving mode for switch position 2 (1231 to 1360) // @CopyValuesFrom: MODE1 // @User: Standard - GSCALAR(mode2, "MODE2", (int8_t)Mode::Number::MANUAL), + GSCALAR(mode2, "MODE2", Mode::Number::MANUAL), // @Param: MODE3 // @CopyFieldsFrom: MODE1 // @DisplayName: Mode3 // @Description: Driving mode for switch position 3 (1361 to 1490) - GSCALAR(mode3, "MODE3", (int8_t)Mode::Number::MANUAL), + GSCALAR(mode3, "MODE3", Mode::Number::MANUAL), // @Param: MODE4 // @CopyFieldsFrom: MODE1 // @DisplayName: Mode4 // @Description: Driving mode for switch position 4 (1491 to 1620) - GSCALAR(mode4, "MODE4", (int8_t)Mode::Number::MANUAL), + GSCALAR(mode4, "MODE4", Mode::Number::MANUAL), // @Param: MODE5 // @CopyFieldsFrom: MODE1 // @DisplayName: Mode5 // @Description: Driving mode for switch position 5 (1621 to 1749) - GSCALAR(mode5, "MODE5", (int8_t)Mode::Number::MANUAL), + GSCALAR(mode5, "MODE5", Mode::Number::MANUAL), // @Param: MODE6 // @CopyFieldsFrom: MODE1 // @DisplayName: Mode6 // @Description: Driving mode for switch position 6 (1750 to 2049) - GSCALAR(mode6, "MODE6", (int8_t)Mode::Number::MANUAL), + GSCALAR(mode6, "MODE6", Mode::Number::MANUAL), // variables not in the g class which contain EEPROM saved variables @@ -221,11 +221,9 @@ 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 @@ -299,7 +297,7 @@ const AP_Param::Info Rover::var_info[] = { GOBJECT(camera, "CAM", AP_Camera), #endif -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED // @Group: PLND_ // @Path: ../libraries/AC_PrecLand/AC_PrecLand.cpp GOBJECT(precland, "PLND_", AC_PrecLand), @@ -427,11 +425,9 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_SUBGROUPINFO(afs, "AFS_", 5, ParametersG2, AP_AdvancedFailsafe), #endif -#if AP_BEACON_ENABLED // @Group: BCN // @Path: ../libraries/AP_Beacon/AP_Beacon.cpp AP_SUBGROUPINFO(beacon, "BCN", 6, ParametersG2, AP_Beacon), -#endif // 7 was used by AP_VisualOdometry @@ -517,11 +513,9 @@ 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 @@ -548,7 +542,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 HAL_RALLY_ENABLED +#if AP_RALLY == ENABLED // @Group: RALLY_ // @Path: AP_Rally.cpp,../libraries/AP_Rally/AP_Rally.cpp AP_SUBGROUPINFO(rally, "RALLY_", 28, ParametersG2, AP_Rally_Rover), @@ -740,10 +734,8 @@ ParametersG2::ParametersG2(void) #if ADVANCED_FAILSAFE == ENABLED afs(), #endif -#if AP_BEACON_ENABLED beacon(), -#endif - motors(wheel_rate_control), + motors(rover.ServoRelayEvents, wheel_rate_control), wheel_rate_control(wheel_encoder), attitude_control(), smart_rtl(), @@ -754,9 +746,7 @@ 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 035d16f4d0ffb4..25a89e0c46c3b5 100644 --- a/Rover/Parameters.h +++ b/Rover/Parameters.h @@ -312,17 +312,15 @@ class ParametersG2 { AP_AdvancedFailsafe_Rover afs; #endif -#if AP_BEACON_ENABLED 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,10 +358,8 @@ 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; @@ -381,10 +377,8 @@ 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; diff --git a/Rover/RC_Channel.cpp b/Rover/RC_Channel.cpp index e799f38a70fd24..e83a9fd0d011a2 100644 --- a/Rover/RC_Channel.cpp +++ b/Rover/RC_Channel.cpp @@ -62,14 +62,9 @@ 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 (in_rc_failsafe()) { + if (rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) { return false; } return true; @@ -214,12 +209,10 @@ 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 d25d714aa261cd..844b4c1b9069d4 100644 --- a/Rover/RC_Channel.h +++ b/Rover/RC_Channel.h @@ -32,7 +32,6 @@ 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 4b6497382b48dd..ec8f77801d0c9e 100644 --- a/Rover/Rover.cpp +++ b/Rover/Rover.cpp @@ -80,9 +80,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = { SCHED_TASK(set_servos, 400, 200, 15), SCHED_TASK_CLASS(AP_GPS, &rover.gps, update, 50, 300, 18), SCHED_TASK_CLASS(AP_Baro, &rover.barometer, update, 10, 200, 21), -#if AP_BEACON_ENABLED SCHED_TASK_CLASS(AP_Beacon, &rover.g2.beacon, update, 50, 200, 24), -#endif #if HAL_PROXIMITY_ENABLED SCHED_TASK_CLASS(AP_Proximity, &rover.g2.proximity, update, 50, 200, 27), #endif @@ -96,15 +94,13 @@ 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 -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED SCHED_TASK(update_precland, 400, 50, 70), #endif +#endif #if AP_RPM_ENABLED SCHED_TASK_CLASS(AP_RPM, &rover.rpm_sensor, update, 10, 100, 72), #endif @@ -118,6 +114,7 @@ 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), @@ -388,9 +385,7 @@ void Rover::update_logging1(void) if (should_log(MASK_LOG_THR)) { Log_Write_Throttle(); -#if AP_BEACON_ENABLED g2.beacon.log(); -#endif } if (should_log(MASK_LOG_NTUN)) { @@ -429,11 +424,6 @@ 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 a15d086b4d1609..4c127dc351cefa 100644 --- a/Rover/Rover.h +++ b/Rover/Rover.h @@ -42,12 +42,6 @@ #include #include #include -#include -#include -#include -#if AP_EXTERNAL_CONTROL_ENABLED -#include -#endif // Configuration #include "defines.h" @@ -67,10 +61,10 @@ #include "GCS_Mavlink.h" #include "GCS_Rover.h" #include "AP_Rally.h" -#if AC_PRECLAND_ENABLED +#include "RC_Channel.h" // RC Channel Library +#if PRECISION_LANDING == ENABLED #include #endif -#include "RC_Channel.h" // RC Channel Library #include "mode.h" @@ -96,9 +90,7 @@ 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; @@ -147,11 +139,6 @@ 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 @@ -159,7 +146,7 @@ class Rover : public AP_Vehicle { #if OSD_ENABLED || OSD_PARAM_ENABLED AP_OSD osd; #endif -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED AC_PrecLand precland; #endif // GCS handling @@ -248,9 +235,7 @@ 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; @@ -375,15 +360,9 @@ class Rover : public AP_Vehicle { void init_ardupilot() override; void startup_ground(void); void update_ahrs_flyforward(); - bool gcs_mode_enabled(const Mode::Number mode_num) const; bool set_mode(Mode &new_mode, ModeReason reason); bool set_mode(const uint8_t new_mode, ModeReason reason) override; - bool set_mode(Mode::Number new_mode, ModeReason reason); uint8_t get_mode() const override { return (uint8_t)control_mode->mode_number(); } - bool current_mode_requires_mission() const override { - return control_mode == &mode_auto; - } - void startup_INS_ground(void); void notify_mode(const Mode *new_mode); uint8_t check_digital_pin(uint8_t pin); @@ -395,13 +374,13 @@ class Rover : public AP_Vehicle { bool get_wp_bearing_deg(float &bearing) const override; bool get_wp_crosstrack_error_m(float &xtrack_error) const override; - enum class FailsafeAction: int8_t { - None = 0, - RTL = 1, - Hold = 2, - SmartRTL = 3, - SmartRTL_Hold = 4, - Terminate = 5 + enum Failsafe_Action { + Failsafe_Action_None = 0, + Failsafe_Action_RTL = 1, + Failsafe_Action_Hold = 2, + Failsafe_Action_SmartRTL = 3, + Failsafe_Action_SmartRTL_Hold = 4, + Failsafe_Action_Terminate = 5 }; enum class Failsafe_Options : uint32_t { @@ -409,12 +388,12 @@ class Rover : public AP_Vehicle { }; static constexpr int8_t _failsafe_priorities[] = { - (int8_t)FailsafeAction::Terminate, - (int8_t)FailsafeAction::Hold, - (int8_t)FailsafeAction::RTL, - (int8_t)FailsafeAction::SmartRTL_Hold, - (int8_t)FailsafeAction::SmartRTL, - (int8_t)FailsafeAction::None, + Failsafe_Action_Terminate, + Failsafe_Action_Hold, + Failsafe_Action_RTL, + Failsafe_Action_SmartRTL_Hold, + Failsafe_Action_SmartRTL, + Failsafe_Action_None, -1 // the priority list must end with a sentinel of -1 }; static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1, diff --git a/Rover/afs_rover.cpp b/Rover/afs_rover.cpp index 4265a20887ebb2..e0349eefc0ea87 100644 --- a/Rover/afs_rover.cpp +++ b/Rover/afs_rover.cpp @@ -29,9 +29,4 @@ AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Rover::afs_mode(void) return AP_AdvancedFailsafe::AFS_STABILIZED; } -//to force entering auto mode when datalink loss - void AP_AdvancedFailsafe_Rover::set_mode_auto(void) - { - over.set_mode(rover.mode_auto,ModeReason::GCS_FAILSAFE); - } #endif // ADVANCED_FAILSAFE diff --git a/Rover/afs_rover.h b/Rover/afs_rover.h index aa3473ddc39d54..36dd0a0aa9f2f1 100644 --- a/Rover/afs_rover.h +++ b/Rover/afs_rover.h @@ -38,9 +38,6 @@ class AP_AdvancedFailsafe_Rover : public AP_AdvancedFailsafe // return the AFS mapped control mode enum control_mode afs_mode(void) override; - - //to force entering auto mode when datalink loss - void set_mode_auto(void) override; }; #endif // ADVANCED_FAILSAFE diff --git a/Rover/config.h b/Rover/config.h index 7a280e88f3497a..6983f9d122eed5 100644 --- a/Rover/config.h +++ b/Rover/config.h @@ -31,6 +31,19 @@ #error XXX #endif +////////////////////////////////////////////////////////////////////////////// +// RALLY POINTS +// +#ifndef AP_RALLY + #define AP_RALLY ENABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// Precision Landing with companion computer or IRLock sensor +#ifndef PRECISION_LANDING + # define PRECISION_LANDING ENABLED +#endif + ////////////////////////////////////////////////////////////////////////////// // NAVL1 // @@ -57,13 +70,7 @@ ////////////////////////////////////////////////////////////////////////////// // Dock mode - allows vehicle to dock to a docking target #ifndef MODE_DOCK_ENABLED -# 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 +# define MODE_DOCK_ENABLED PRECISION_LANDING #endif diff --git a/Rover/failsafe.cpp b/Rover/failsafe.cpp index da877c2bb7482a..2520c73f663345 100644 --- a/Rover/failsafe.cpp +++ b/Rover/failsafe.cpp @@ -80,30 +80,29 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, const char* type_str, bool o // continue with mission in auto mode gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe - Continuing Auto Mode"); } else { - switch ((FailsafeAction)g.fs_action.get()) { - case FailsafeAction::None: + switch (g.fs_action) { + case Failsafe_Action_None: break; - case FailsafeAction::SmartRTL: - if (set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) { - break; + case Failsafe_Action_RTL: + if (!set_mode(mode_rtl, ModeReason::FAILSAFE)) { + set_mode(mode_hold, ModeReason::FAILSAFE); } - FALLTHROUGH; - case FailsafeAction::RTL: - if (set_mode(mode_rtl, ModeReason::BATTERY_FAILSAFE)) { - break; + break; + case Failsafe_Action_Hold: + set_mode(mode_hold, ModeReason::FAILSAFE); + break; + case Failsafe_Action_SmartRTL: + if (!set_mode(mode_smartrtl, ModeReason::FAILSAFE)) { + if (!set_mode(mode_rtl, ModeReason::FAILSAFE)) { + set_mode(mode_hold, ModeReason::FAILSAFE); + } } - FALLTHROUGH; - case FailsafeAction::Hold: - set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE); break; - case FailsafeAction::SmartRTL_Hold: + case Failsafe_Action_SmartRTL_Hold: if (!set_mode(mode_smartrtl, ModeReason::FAILSAFE)) { set_mode(mode_hold, ModeReason::FAILSAFE); } break; - case FailsafeAction::Terminate: - arming.disarm(AP_Arming::Method::FAILSAFE_ACTION_TERMINATE); - break; } } } @@ -111,28 +110,28 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, const char* type_str, bool o void Rover::handle_battery_failsafe(const char* type_str, const int8_t action) { - switch ((FailsafeAction)action) { - case FailsafeAction::None: + switch ((Failsafe_Action)action) { + case Failsafe_Action_None: break; - case FailsafeAction::SmartRTL: + case Failsafe_Action_SmartRTL: if (set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) { break; } FALLTHROUGH; - case FailsafeAction::RTL: + case Failsafe_Action_RTL: if (set_mode(mode_rtl, ModeReason::BATTERY_FAILSAFE)) { break; } FALLTHROUGH; - case FailsafeAction::Hold: + case Failsafe_Action_Hold: set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE); break; - case FailsafeAction::SmartRTL_Hold: + case Failsafe_Action_SmartRTL_Hold: if (!set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) { set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE); } break; - case FailsafeAction::Terminate: + case Failsafe_Action_Terminate: #if ADVANCED_FAILSAFE == ENABLED char battery_type_str[17]; snprintf(battery_type_str, 17, "%s battery", type_str); diff --git a/Rover/fence.cpp b/Rover/fence.cpp index 7150c88c467ccc..fba0706bdefd6a 100644 --- a/Rover/fence.cpp +++ b/Rover/fence.cpp @@ -18,33 +18,32 @@ void Rover::fence_check() // if there is a new breach take action if (new_breaches) { // if the user wants some kind of response and motors are armed - if ((FailsafeAction)fence.get_action() != FailsafeAction::None) { + if (fence.get_action() != Failsafe_Action_None) { // if within 100m of the fence, it will take the action specified by the FENCE_ACTION parameter if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) { - switch ((FailsafeAction)fence.get_action()) { - case FailsafeAction::None: + switch (fence.get_action()) { + case Failsafe_Action_None: break; - case FailsafeAction::SmartRTL: - if (set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) { - break; + case Failsafe_Action_RTL: + if (!set_mode(mode_rtl, ModeReason::FENCE_BREACHED)) { + set_mode(mode_hold, ModeReason::FENCE_BREACHED); } - FALLTHROUGH; - case FailsafeAction::RTL: - if (set_mode(mode_rtl, ModeReason::BATTERY_FAILSAFE)) { - break; + break; + case Failsafe_Action_Hold: + set_mode(mode_hold, ModeReason::FENCE_BREACHED); + break; + case Failsafe_Action_SmartRTL: + if (!set_mode(mode_smartrtl, ModeReason::FENCE_BREACHED)) { + if (!set_mode(mode_rtl, ModeReason::FENCE_BREACHED)) { + set_mode(mode_hold, ModeReason::FENCE_BREACHED); + } } - FALLTHROUGH; - case FailsafeAction::Hold: - set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE); break; - case FailsafeAction::SmartRTL_Hold: + case Failsafe_Action_SmartRTL_Hold: if (!set_mode(mode_smartrtl, ModeReason::FENCE_BREACHED)) { set_mode(mode_hold, ModeReason::FENCE_BREACHED); } break; - case FailsafeAction::Terminate: - arming.disarm(AP_Arming::Method::FENCEBREACH); - break; } } else { // if more than 100m outside the fence just force to HOLD diff --git a/Rover/mode.cpp b/Rover/mode.cpp index 842789c1006698..5f70481c191eb0 100644 --- a/Rover/mode.cpp +++ b/Rover/mode.cpp @@ -531,11 +531,9 @@ 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; diff --git a/Rover/mode.h b/Rover/mode.h index fb9a57514dbe4c..0ac0c4c4097e77 100644 --- a/Rover/mode.h +++ b/Rover/mode.h @@ -11,7 +11,7 @@ class Mode // Auto Pilot modes // ---------------- - enum class Number : uint8_t { + enum Number : uint8_t { MANUAL = 0, ACRO = 1, STEERING = 3, @@ -43,7 +43,7 @@ class Mode void exit(); // returns a unique number specific to this mode - virtual Number mode_number() const = 0; + virtual uint32_t mode_number() const = 0; // returns short text name (up to 4 bytes) virtual const char *name4() const = 0; @@ -218,7 +218,7 @@ class ModeAcro : public Mode { public: - Number mode_number() const override { return Number::ACRO; } + uint32_t mode_number() const override { return ACRO; } const char *name4() const override { return "ACRO"; } // methods that affect movement of the vehicle in this mode @@ -240,7 +240,7 @@ class ModeAuto : public Mode { public: - Number mode_number() const override { return Number::AUTO; } + uint32_t mode_number() const override { return AUTO; } const char *name4() const override { return "AUTO"; } // methods that affect movement of the vehicle in this mode @@ -251,7 +251,7 @@ class ModeAuto : public Mode bool is_autopilot_mode() const override { return true; } // return if external control is allowed in this mode (Guided or Guided-within-Auto) - bool in_guided_mode() const override { return _submode == SubMode::Guided || _submode == SubMode::NavScriptTime; } + 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; @@ -294,15 +294,15 @@ class ModeAuto : public Mode bool _enter() override; void _exit() override; - enum SubMode: uint8_t { - WP, // drive to a given location - HeadingAndSpeed, // turn to a given heading - RTL, // perform RTL within auto mode - Loiter, // perform Loiter within auto mode - Guided, // handover control to external navigation system from within auto mode - Stop, // stop the vehicle as quickly as possible - NavScriptTime, // accept targets from lua scripts while NAV_SCRIPT_TIME commands are executing - Circle, // circle a given location + enum AutoSubMode { + Auto_WP, // drive to a given location + Auto_HeadingAndSpeed, // turn to a given heading + Auto_RTL, // perform RTL within auto mode + Auto_Loiter, // perform Loiter within auto mode + Auto_Guided, // handover control to external navigation system from within auto mode + Auto_Stop, // stop the vehicle as quickly as possible + Auto_NavScriptTime, // accept targets from lua scripts while NAV_SCRIPT_TIME commands are executing + Auto_Circle, // circle a given location } _submode; private: @@ -405,7 +405,7 @@ class ModeCircle : public Mode // Does not allow copies CLASS_NO_COPY(ModeCircle); - Number mode_number() const override { return Number::CIRCLE; } + 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 @@ -489,7 +489,7 @@ class ModeGuided : public Mode { public: - Number mode_number() const override { return Number::GUIDED; } + uint32_t mode_number() const override { return GUIDED; } const char *name4() const override { return "GUID"; } // methods that affect movement of the vehicle in this mode @@ -544,13 +544,13 @@ class ModeGuided : public Mode protected: - enum class SubMode: uint8_t { - WP, - HeadingAndSpeed, - TurnRateAndSpeed, - Loiter, - SteeringAndThrottle, - Stop + enum GuidedMode { + Guided_WP, + Guided_HeadingAndSpeed, + Guided_TurnRateAndSpeed, + Guided_Loiter, + Guided_SteeringAndThrottle, + Guided_Stop }; // enum for GUID_OPTIONS parameter @@ -564,7 +564,7 @@ class ModeGuided : public Mode // scurves provide path planning and object avoidance but cannot handle fast updates to the destination (for fast updates use position controller input shaping) bool use_scurves_for_navigation() const; - SubMode _guided_mode; // stores which GUIDED mode the vehicle is in + GuidedMode _guided_mode; // stores which GUIDED mode the vehicle is in // attitude control bool have_attitude_target; // true if we have a valid attitude target @@ -593,7 +593,7 @@ class ModeHold : public Mode { public: - Number mode_number() const override { return Number::HOLD; } + uint32_t mode_number() const override { return HOLD; } const char *name4() const override { return "HOLD"; } // methods that affect movement of the vehicle in this mode @@ -611,7 +611,7 @@ class ModeLoiter : public Mode { public: - Number mode_number() const override { return Number::LOITER; } + uint32_t mode_number() const override { return LOITER; } const char *name4() const override { return "LOIT"; } // methods that affect movement of the vehicle in this mode @@ -643,7 +643,7 @@ class ModeManual : public Mode { public: - Number mode_number() const override { return Number::MANUAL; } + uint32_t mode_number() const override { return MANUAL; } const char *name4() const override { return "MANU"; } // methods that affect movement of the vehicle in this mode @@ -667,7 +667,7 @@ class ModeRTL : public Mode { public: - Number mode_number() const override { return Number::RTL; } + uint32_t mode_number() const override { return RTL; } const char *name4() const override { return "RTL"; } // methods that affect movement of the vehicle in this mode @@ -702,7 +702,7 @@ class ModeSmartRTL : public Mode { public: - Number mode_number() const override { return Number::SMART_RTL; } + uint32_t mode_number() const override { return SMART_RTL; } const char *name4() const override { return "SRTL"; } // methods that affect movement of the vehicle in this mode @@ -719,7 +719,7 @@ class ModeSmartRTL : public Mode // return distance (in meters) to destination float get_distance_to_destination() const override { return _distance_to_destination; } - bool reached_destination() const override { return smart_rtl_state == SmartRTLState::StopAtHome; } + bool reached_destination() const override { return smart_rtl_state == SmartRTL_StopAtHome; } // set desired speed in m/s bool set_desired_speed(float speed) override; @@ -730,11 +730,11 @@ class ModeSmartRTL : public Mode protected: // Safe RTL states - enum class SmartRTLState: uint8_t { - WaitForPathCleanup, - PathFollow, - StopAtHome, - Failure + enum SmartRTLState { + SmartRTL_WaitForPathCleanup, + SmartRTL_PathFollow, + SmartRTL_StopAtHome, + SmartRTL_Failure } smart_rtl_state; bool _enter() override; @@ -748,7 +748,7 @@ class ModeSteering : public Mode { public: - Number mode_number() const override { return Number::STEERING; } + uint32_t mode_number() const override { return STEERING; } const char *name4() const override { return "STER"; } // methods that affect movement of the vehicle in this mode @@ -773,7 +773,7 @@ class ModeInitializing : public Mode { public: - Number mode_number() const override { return Number::INITIALISING; } + uint32_t mode_number() const override { return INITIALISING; } const char *name4() const override { return "INIT"; } // methods that affect movement of the vehicle in this mode @@ -789,12 +789,11 @@ class ModeInitializing : public Mode bool _enter() override { return false; }; }; -#if MODE_FOLLOW_ENABLED == ENABLED class ModeFollow : public Mode { public: - Number mode_number() const override { return Number::FOLLOW; } + uint32_t mode_number() const override { return FOLLOW; } const char *name4() const override { return "FOLL"; } // methods that affect movement of the vehicle in this mode @@ -824,13 +823,12 @@ class ModeFollow : public Mode float _desired_speed; // desired speed in m/s }; -#endif class ModeSimple : public Mode { public: - Number mode_number() const override { return Number::SIMPLE; } + uint32_t mode_number() const override { return SIMPLE; } const char *name4() const override { return "SMPL"; } // methods that affect movement of the vehicle in this mode @@ -860,7 +858,7 @@ class ModeDock : public Mode // Does not allow copies CLASS_NO_COPY(ModeDock); - Number mode_number() const override { return Number::DOCK; } + uint32_t mode_number() const override { return DOCK; } const char *name4() const override { return "DOCK"; } // methods that affect movement of the vehicle in this mode diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp index 439f4a014b034e..c8bdfe82f50aa3 100644 --- a/Rover/mode_auto.cpp +++ b/Rover/mode_auto.cpp @@ -44,12 +44,6 @@ void ModeAuto::_exit() void ModeAuto::update() { - // check if mission exists (due to being cleared while disarmed in AUTO, - // if no mission, then stop...needs mode change out of AUTO, mission load, - // and change back to AUTO to run a mission at this point - if (!hal.util->get_soft_armed() && mission.num_commands() <= 1) { - start_stop(); - } // start or update mission if (waiting_to_start) { // don't start the mission until we have an origin @@ -66,7 +60,7 @@ void ModeAuto::update() // check for mission changes if (mis_change_detector.check_for_mission_change()) { // if mission is running restart the current command if it is a waypoint command - if ((mission.state() == AP_Mission::MISSION_RUNNING) && (_submode == SubMode::WP)) { + if ((mission.state() == AP_Mission::MISSION_RUNNING) && (_submode == AutoSubMode::Auto_WP)) { if (mission.restart_current_nav_cmd()) { gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto mission changed, restarted command"); } else { @@ -80,7 +74,7 @@ void ModeAuto::update() } switch (_submode) { - case SubMode::WP: + case Auto_WP: { // check if we've reached the destination if (!g2.wp_nav.reached_destination() || g2.wp_nav.is_fast_waypoint()) { @@ -101,7 +95,7 @@ void ModeAuto::update() break; } - case SubMode::HeadingAndSpeed: + case Auto_HeadingAndSpeed: { if (!_reached_heading) { // run steering and throttle controllers @@ -122,15 +116,15 @@ void ModeAuto::update() break; } - case SubMode::RTL: + case Auto_RTL: rover.mode_rtl.update(); break; - case SubMode::Loiter: + case Auto_Loiter: rover.mode_loiter.update(); break; - case SubMode::Guided: + case Auto_Guided: { // send location target to offboard navigation system send_guided_position_target(); @@ -138,15 +132,15 @@ void ModeAuto::update() break; } - case SubMode::Stop: + case Auto_Stop: stop_vehicle(); break; - case SubMode::NavScriptTime: + case Auto_NavScriptTime: rover.mode_guided.update(); break; - case SubMode::Circle: + case Auto_Circle: rover.g2.mode_circle.update(); break; } @@ -166,19 +160,19 @@ void ModeAuto::calc_throttle(float target_speed, bool avoidance_enabled) float ModeAuto::wp_bearing() const { switch (_submode) { - case SubMode::WP: + case Auto_WP: return g2.wp_nav.wp_bearing_cd() * 0.01f; - case SubMode::HeadingAndSpeed: - case SubMode::Stop: + case Auto_HeadingAndSpeed: + case Auto_Stop: return 0.0f; - case SubMode::RTL: + case Auto_RTL: return rover.mode_rtl.wp_bearing(); - case SubMode::Loiter: + case Auto_Loiter: return rover.mode_loiter.wp_bearing(); - case SubMode::Guided: - case SubMode::NavScriptTime: + case Auto_Guided: + case Auto_NavScriptTime: return rover.mode_guided.wp_bearing(); - case SubMode::Circle: + case Auto_Circle: return rover.g2.mode_circle.wp_bearing(); } @@ -190,19 +184,19 @@ float ModeAuto::wp_bearing() const float ModeAuto::nav_bearing() const { switch (_submode) { - case SubMode::WP: + case Auto_WP: return g2.wp_nav.nav_bearing_cd() * 0.01f; - case SubMode::HeadingAndSpeed: - case SubMode::Stop: + case Auto_HeadingAndSpeed: + case Auto_Stop: return 0.0f; - case SubMode::RTL: + case Auto_RTL: return rover.mode_rtl.nav_bearing(); - case SubMode::Loiter: + case Auto_Loiter: return rover.mode_loiter.nav_bearing(); - case SubMode::Guided: - case SubMode::NavScriptTime: + case Auto_Guided: + case Auto_NavScriptTime: return rover.mode_guided.nav_bearing(); - case SubMode::Circle: + case Auto_Circle: return rover.g2.mode_circle.nav_bearing(); } @@ -214,19 +208,19 @@ float ModeAuto::nav_bearing() const float ModeAuto::crosstrack_error() const { switch (_submode) { - case SubMode::WP: + case Auto_WP: return g2.wp_nav.crosstrack_error(); - case SubMode::HeadingAndSpeed: - case SubMode::Stop: + case Auto_HeadingAndSpeed: + case Auto_Stop: return 0.0f; - case SubMode::RTL: + case Auto_RTL: return rover.mode_rtl.crosstrack_error(); - case SubMode::Loiter: + case Auto_Loiter: return rover.mode_loiter.crosstrack_error(); - case SubMode::Guided: - case SubMode::NavScriptTime: + case Auto_Guided: + case Auto_NavScriptTime: return rover.mode_guided.crosstrack_error(); - case SubMode::Circle: + case Auto_Circle: return rover.g2.mode_circle.crosstrack_error(); } @@ -238,19 +232,19 @@ float ModeAuto::crosstrack_error() const float ModeAuto::get_desired_lat_accel() const { switch (_submode) { - case SubMode::WP: + case Auto_WP: return g2.wp_nav.get_lat_accel(); - case SubMode::HeadingAndSpeed: - case SubMode::Stop: + case Auto_HeadingAndSpeed: + case Auto_Stop: return 0.0f; - case SubMode::RTL: + case Auto_RTL: return rover.mode_rtl.get_desired_lat_accel(); - case SubMode::Loiter: + case Auto_Loiter: return rover.mode_loiter.get_desired_lat_accel(); - case SubMode::Guided: - case SubMode::NavScriptTime: + case Auto_Guided: + case Auto_NavScriptTime: return rover.mode_guided.get_desired_lat_accel(); - case SubMode::Circle: + case Auto_Circle: return rover.g2.mode_circle.get_desired_lat_accel(); } @@ -262,20 +256,20 @@ float ModeAuto::get_desired_lat_accel() const float ModeAuto::get_distance_to_destination() const { switch (_submode) { - case SubMode::WP: + case Auto_WP: return _distance_to_destination; - case SubMode::HeadingAndSpeed: - case SubMode::Stop: + case Auto_HeadingAndSpeed: + case Auto_Stop: // no valid distance so return zero return 0.0f; - case SubMode::RTL: + case Auto_RTL: return rover.mode_rtl.get_distance_to_destination(); - case SubMode::Loiter: + case Auto_Loiter: return rover.mode_loiter.get_distance_to_destination(); - case SubMode::Guided: - case SubMode::NavScriptTime: + case Auto_Guided: + case Auto_NavScriptTime: return rover.mode_guided.get_distance_to_destination(); - case SubMode::Circle: + case Auto_Circle: return rover.g2.mode_circle.get_distance_to_destination(); } @@ -287,24 +281,24 @@ float ModeAuto::get_distance_to_destination() const bool ModeAuto::get_desired_location(Location& destination) const { switch (_submode) { - case SubMode::WP: + case Auto_WP: if (g2.wp_nav.is_destination_valid()) { destination = g2.wp_nav.get_oa_destination(); return true; } return false; - case SubMode::HeadingAndSpeed: - case SubMode::Stop: + case Auto_HeadingAndSpeed: + case Auto_Stop: // no desired location for this submode return false; - case SubMode::RTL: + case Auto_RTL: return rover.mode_rtl.get_desired_location(destination); - case SubMode::Loiter: + case Auto_Loiter: return rover.mode_loiter.get_desired_location(destination); - case SubMode::Guided: - case SubMode::NavScriptTime: + case Auto_Guided: + case Auto_NavScriptTime: return rover.mode_guided.get_desired_location(destination); - case SubMode::Circle: + case Auto_Circle: return rover.g2.mode_circle.get_desired_location(destination); } @@ -320,7 +314,7 @@ bool ModeAuto::set_desired_location(const Location &destination, Location next_d return false; } - _submode = SubMode::WP; + _submode = Auto_WP; return true; } @@ -329,24 +323,24 @@ bool ModeAuto::set_desired_location(const Location &destination, Location next_d bool ModeAuto::reached_destination() const { switch (_submode) { - case SubMode::WP: + case Auto_WP: return g2.wp_nav.reached_destination(); break; - case SubMode::HeadingAndSpeed: - case SubMode::Stop: + case Auto_HeadingAndSpeed: + case Auto_Stop: // always return true because this is the safer option to allow missions to continue return true; break; - case SubMode::RTL: + case Auto_RTL: return rover.mode_rtl.reached_destination(); break; - case SubMode::Loiter: + case Auto_Loiter: return rover.mode_loiter.reached_destination(); break; - case SubMode::Guided: - case SubMode::NavScriptTime: + case Auto_Guided: + case Auto_NavScriptTime: return rover.mode_guided.reached_destination(); - case SubMode::Circle: + case Auto_Circle: return rover.g2.mode_circle.reached_destination(); } @@ -358,20 +352,20 @@ bool ModeAuto::reached_destination() const bool ModeAuto::set_desired_speed(float speed) { switch (_submode) { - case SubMode::WP: - case SubMode::Stop: + case Auto_WP: + case Auto_Stop: return g2.wp_nav.set_speed_max(speed); - case SubMode::HeadingAndSpeed: + case Auto_HeadingAndSpeed: _desired_speed = speed; return true; - case SubMode::RTL: + case Auto_RTL: return rover.mode_rtl.set_desired_speed(speed); - case SubMode::Loiter: + case Auto_Loiter: return rover.mode_loiter.set_desired_speed(speed); - case SubMode::Guided: - case SubMode::NavScriptTime: + case Auto_Guided: + case Auto_NavScriptTime: return rover.mode_guided.set_desired_speed(speed); - case SubMode::Circle: + case Auto_Circle: return rover.g2.mode_circle.set_desired_speed(speed); } return false; @@ -381,7 +375,7 @@ bool ModeAuto::set_desired_speed(float speed) void ModeAuto::start_RTL() { if (rover.mode_rtl.enter()) { - _submode = SubMode::RTL; + _submode = Auto_RTL; } } @@ -389,7 +383,7 @@ void ModeAuto::start_RTL() bool ModeAuto::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4) { #if AP_SCRIPTING_ENABLED - if (_submode == SubMode::NavScriptTime) { + if (_submode == AutoSubMode::Auto_NavScriptTime) { id = nav_scripting.id; cmd = nav_scripting.command; arg1 = nav_scripting.arg1; @@ -406,7 +400,7 @@ bool ModeAuto::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &a void ModeAuto::nav_script_time_done(uint16_t id) { #if AP_SCRIPTING_ENABLED - if ((_submode == SubMode::NavScriptTime) && (id == nav_scripting.id)) { + if ((_submode == AutoSubMode::Auto_NavScriptTime) && (id == nav_scripting.id)) { nav_scripting.done = true; } #endif @@ -458,7 +452,7 @@ bool ModeAuto::check_trigger(void) bool ModeAuto::start_loiter() { if (rover.mode_loiter.enter()) { - _submode = SubMode::Loiter; + _submode = Auto_Loiter; return true; } return false; @@ -468,7 +462,7 @@ bool ModeAuto::start_loiter() void ModeAuto::start_guided(const Location& loc) { if (rover.mode_guided.enter()) { - _submode = SubMode::Guided; + _submode = Auto_Guided; // initialise guided start time and position as reference for limit checking rover.mode_guided.limit_init_time_and_location(); @@ -487,7 +481,7 @@ void ModeAuto::start_guided(const Location& loc) // start stopping vehicle as quickly as possible void ModeAuto::start_stop() { - _submode = SubMode::Stop; + _submode = Auto_Stop; } // send latest position target to offboard navigation system @@ -788,11 +782,7 @@ void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd) nav_delay_time_max_ms = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds } else { // absolute delay to utc time -#if AP_RTC_ENABLED nav_delay_time_max_ms = AP::rtc().get_time_utc(cmd.content.nav_delay.hour_utc, cmd.content.nav_delay.min_utc, cmd.content.nav_delay.sec_utc, 0); -#else - nav_delay_time_max_ms = 0; -#endif } gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec", (unsigned)(nav_delay_time_max_ms/1000)); } @@ -824,7 +814,7 @@ void ModeAuto::do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd) _desired_speed = constrain_float(cmd.content.set_yaw_speed.speed, -speed_max, speed_max); _desired_yaw_cd = desired_heading_cd; _reached_heading = false; - _submode = SubMode::HeadingAndSpeed; + _submode = Auto_HeadingAndSpeed; } /********************************************************************************/ @@ -900,7 +890,7 @@ bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) { // if we failed to enter guided or this command disables guided // return true so we move to next command - if (_submode != SubMode::Guided || cmd.p1 == 0) { + if (_submode != Auto_Guided || cmd.p1 == 0) { return true; } @@ -918,7 +908,7 @@ bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) // verify_yaw - return true if we have reached the desired heading bool ModeAuto::verify_nav_set_yaw_speed() { - if (_submode == SubMode::HeadingAndSpeed) { + if (_submode == Auto_HeadingAndSpeed) { return _reached_heading; } // we should never reach here but just in case, return true to allow missions to continue @@ -941,7 +931,7 @@ bool ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd) // initialise circle mode if (g2.mode_circle.set_center(circle_center, circle_radius_m, cmd.content.location.loiter_ccw)) { - _submode = SubMode::Circle; + _submode = Auto_Circle; return true; } return false; @@ -1035,7 +1025,7 @@ void ModeAuto::do_nav_script_time(const AP_Mission::Mission_Command& cmd) { // call regular guided flight mode initialisation if (rover.mode_guided.enter()) { - _submode = SubMode::NavScriptTime; + _submode = AutoSubMode::Auto_NavScriptTime; nav_scripting.done = false; nav_scripting.id++; nav_scripting.start_ms = millis(); diff --git a/Rover/mode_follow.cpp b/Rover/mode_follow.cpp index 964a81a102a6ba..437cb53deb8445 100644 --- a/Rover/mode_follow.cpp +++ b/Rover/mode_follow.cpp @@ -1,6 +1,5 @@ #include "Rover.h" -#if MODE_FOLLOW_ENABLED // initialize follow mode bool ModeFollow::_enter() { @@ -95,5 +94,3 @@ bool ModeFollow::set_desired_speed(float speed) _desired_speed = speed; return true; } - -#endif // MODE_FOLLOW_ENABLED diff --git a/Rover/mode_guided.cpp b/Rover/mode_guided.cpp index a1d18cf92d006d..76f34367607bb6 100644 --- a/Rover/mode_guided.cpp +++ b/Rover/mode_guided.cpp @@ -22,7 +22,7 @@ bool ModeGuided::_enter() void ModeGuided::update() { switch (_guided_mode) { - case SubMode::WP: + case Guided_WP: { // check if we've reached the destination if (!g2.wp_nav.reached_destination()) { @@ -49,7 +49,7 @@ void ModeGuided::update() break; } - case SubMode::HeadingAndSpeed: + case Guided_HeadingAndSpeed: { // stop vehicle if target not updated within 3 seconds if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) { @@ -73,7 +73,7 @@ void ModeGuided::update() break; } - case SubMode::TurnRateAndSpeed: + case Guided_TurnRateAndSpeed: { // stop vehicle if target not updated within 3 seconds if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) { @@ -101,13 +101,13 @@ void ModeGuided::update() break; } - case SubMode::Loiter: + case Guided_Loiter: { rover.mode_loiter.update(); break; } - case SubMode::SteeringAndThrottle: + case Guided_SteeringAndThrottle: { // handle timeout if (_have_strthr && (AP_HAL::millis() - _strthr_time_ms) > 3000) { @@ -131,7 +131,7 @@ void ModeGuided::update() break; } - case SubMode::Stop: + case Guided_Stop: stop_vehicle(); break; @@ -145,15 +145,15 @@ void ModeGuided::update() float ModeGuided::wp_bearing() const { switch (_guided_mode) { - case SubMode::WP: + case Guided_WP: return g2.wp_nav.wp_bearing_cd() * 0.01f; - case SubMode::HeadingAndSpeed: - case SubMode::TurnRateAndSpeed: + case Guided_HeadingAndSpeed: + case Guided_TurnRateAndSpeed: return 0.0f; - case SubMode::Loiter: + case Guided_Loiter: return rover.mode_loiter.wp_bearing(); - case SubMode::SteeringAndThrottle: - case SubMode::Stop: + case Guided_SteeringAndThrottle: + case Guided_Stop: return 0.0f; } @@ -164,15 +164,15 @@ float ModeGuided::wp_bearing() const float ModeGuided::nav_bearing() const { switch (_guided_mode) { - case SubMode::WP: + case Guided_WP: return g2.wp_nav.nav_bearing_cd() * 0.01f; - case SubMode::HeadingAndSpeed: - case SubMode::TurnRateAndSpeed: + case Guided_HeadingAndSpeed: + case Guided_TurnRateAndSpeed: return 0.0f; - case SubMode::Loiter: + case Guided_Loiter: return rover.mode_loiter.nav_bearing(); - case SubMode::SteeringAndThrottle: - case SubMode::Stop: + case Guided_SteeringAndThrottle: + case Guided_Stop: return 0.0f; } @@ -183,15 +183,15 @@ float ModeGuided::nav_bearing() const float ModeGuided::crosstrack_error() const { switch (_guided_mode) { - case SubMode::WP: + case Guided_WP: return g2.wp_nav.crosstrack_error(); - case SubMode::HeadingAndSpeed: - case SubMode::TurnRateAndSpeed: + case Guided_HeadingAndSpeed: + case Guided_TurnRateAndSpeed: return 0.0f; - case SubMode::Loiter: + case Guided_Loiter: return rover.mode_loiter.crosstrack_error(); - case SubMode::SteeringAndThrottle: - case SubMode::Stop: + case Guided_SteeringAndThrottle: + case Guided_Stop: return 0.0f; } @@ -202,15 +202,15 @@ float ModeGuided::crosstrack_error() const float ModeGuided::get_desired_lat_accel() const { switch (_guided_mode) { - case SubMode::WP: + case Guided_WP: return g2.wp_nav.get_lat_accel(); - case SubMode::HeadingAndSpeed: - case SubMode::TurnRateAndSpeed: + case Guided_HeadingAndSpeed: + case Guided_TurnRateAndSpeed: return 0.0f; - case SubMode::Loiter: + case Guided_Loiter: return rover.mode_loiter.get_desired_lat_accel(); - case SubMode::SteeringAndThrottle: - case SubMode::Stop: + case Guided_SteeringAndThrottle: + case Guided_Stop: return 0.0f; } @@ -222,15 +222,15 @@ float ModeGuided::get_desired_lat_accel() const float ModeGuided::get_distance_to_destination() const { switch (_guided_mode) { - case SubMode::WP: + case Guided_WP: return _distance_to_destination; - case SubMode::HeadingAndSpeed: - case SubMode::TurnRateAndSpeed: + case Guided_HeadingAndSpeed: + case Guided_TurnRateAndSpeed: return 0.0f; - case SubMode::Loiter: + case Guided_Loiter: return rover.mode_loiter.get_distance_to_destination(); - case SubMode::SteeringAndThrottle: - case SubMode::Stop: + case Guided_SteeringAndThrottle: + case Guided_Stop: return 0.0f; } @@ -242,13 +242,13 @@ float ModeGuided::get_distance_to_destination() const bool ModeGuided::reached_destination() const { switch (_guided_mode) { - case SubMode::WP: + case Guided_WP: return g2.wp_nav.reached_destination(); - case SubMode::HeadingAndSpeed: - case SubMode::TurnRateAndSpeed: - case SubMode::Loiter: - case SubMode::SteeringAndThrottle: - case SubMode::Stop: + case Guided_HeadingAndSpeed: + case Guided_TurnRateAndSpeed: + case Guided_Loiter: + case Guided_SteeringAndThrottle: + case Guided_Stop: return true; } @@ -260,16 +260,16 @@ bool ModeGuided::reached_destination() const bool ModeGuided::set_desired_speed(float speed) { switch (_guided_mode) { - case SubMode::WP: + case Guided_WP: return g2.wp_nav.set_speed_max(speed); - case SubMode::HeadingAndSpeed: - case SubMode::TurnRateAndSpeed: + case Guided_HeadingAndSpeed: + case Guided_TurnRateAndSpeed: // speed is set from mavlink message return false; - case SubMode::Loiter: + case Guided_Loiter: return rover.mode_loiter.set_desired_speed(speed); - case SubMode::SteeringAndThrottle: - case SubMode::Stop: + case Guided_SteeringAndThrottle: + case Guided_Stop: // no speed control return false; } @@ -280,21 +280,21 @@ bool ModeGuided::set_desired_speed(float speed) bool ModeGuided::get_desired_location(Location& destination) const { switch (_guided_mode) { - case SubMode::WP: + case Guided_WP: if (g2.wp_nav.is_destination_valid()) { destination = g2.wp_nav.get_oa_destination(); return true; } return false; - case SubMode::HeadingAndSpeed: - case SubMode::TurnRateAndSpeed: + case Guided_HeadingAndSpeed: + case Guided_TurnRateAndSpeed: // not supported in these submodes return false; - case SubMode::Loiter: + case Guided_Loiter: // get destination from loiter return rover.mode_loiter.get_desired_location(destination); - case SubMode::SteeringAndThrottle: - case SubMode::Stop: + case Guided_SteeringAndThrottle: + case Guided_Stop: // no desired location in this submode break; } @@ -320,9 +320,9 @@ bool ModeGuided::set_desired_location(const Location &destination, Location next } // handle guided specific initialisation and logging - _guided_mode = SubMode::WP; + _guided_mode = ModeGuided::Guided_WP; send_notification = true; - rover.Log_Write_GuidedTarget((uint8_t)_guided_mode, Vector3f(destination.lat, destination.lng, 0), Vector3f(g2.wp_nav.get_speed_max(), 0.0f, 0.0f)); + rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(destination.lat, destination.lng, 0), Vector3f(g2.wp_nav.get_speed_max(), 0.0f, 0.0f)); return true; } @@ -330,7 +330,7 @@ bool ModeGuided::set_desired_location(const Location &destination, Location next void ModeGuided::set_desired_heading_and_speed(float yaw_angle_cd, float target_speed) { // initialisation and logging - _guided_mode = SubMode::HeadingAndSpeed; + _guided_mode = ModeGuided::Guided_HeadingAndSpeed; _des_att_time_ms = AP_HAL::millis(); // record targets @@ -339,14 +339,14 @@ void ModeGuided::set_desired_heading_and_speed(float yaw_angle_cd, float target_ have_attitude_target = true; // log new target - rover.Log_Write_GuidedTarget((uint8_t)_guided_mode, Vector3f(_desired_yaw_cd, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f)); + rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(_desired_yaw_cd, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f)); } void ModeGuided::set_desired_heading_delta_and_speed(float yaw_delta_cd, float target_speed) { // handle initialisation - if (_guided_mode != SubMode::HeadingAndSpeed) { - _guided_mode = SubMode::HeadingAndSpeed; + if (_guided_mode != ModeGuided::Guided_HeadingAndSpeed) { + _guided_mode = ModeGuided::Guided_HeadingAndSpeed; _desired_yaw_cd = ahrs.yaw_sensor; } set_desired_heading_and_speed(wrap_180_cd(_desired_yaw_cd + yaw_delta_cd), target_speed); @@ -356,7 +356,7 @@ void ModeGuided::set_desired_heading_delta_and_speed(float yaw_delta_cd, float t void ModeGuided::set_desired_turn_rate_and_speed(float turn_rate_cds, float target_speed) { // handle initialisation - _guided_mode = SubMode::TurnRateAndSpeed; + _guided_mode = ModeGuided::Guided_TurnRateAndSpeed; _des_att_time_ms = AP_HAL::millis(); // record targets @@ -365,13 +365,13 @@ void ModeGuided::set_desired_turn_rate_and_speed(float turn_rate_cds, float targ have_attitude_target = true; // log new target - rover.Log_Write_GuidedTarget((uint8_t)_guided_mode, Vector3f(_desired_yaw_rate_cds, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f)); + rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(_desired_yaw_rate_cds, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f)); } // set steering and throttle (both in the range -1 to +1) void ModeGuided::set_steering_and_throttle(float steering, float throttle) { - _guided_mode = SubMode::SteeringAndThrottle; + _guided_mode = ModeGuided::Guided_SteeringAndThrottle; _strthr_time_ms = AP_HAL::millis(); _strthr_steering = constrain_float(steering, -1.0f, 1.0f); _strthr_throttle = constrain_float(throttle, -1.0f, 1.0f); @@ -381,7 +381,7 @@ void ModeGuided::set_steering_and_throttle(float steering, float throttle) bool ModeGuided::start_loiter() { if (rover.mode_loiter.enter()) { - _guided_mode = SubMode::Loiter; + _guided_mode = Guided_Loiter; return true; } return false; @@ -391,7 +391,7 @@ bool ModeGuided::start_loiter() // start stopping vehicle as quickly as possible void ModeGuided::start_stop() { - _guided_mode = SubMode::Stop; + _guided_mode = Guided_Stop; } // set guided timeout and movement limits diff --git a/Rover/mode_rtl.cpp b/Rover/mode_rtl.cpp index e896c5151fc878..b9a8ee49649e17 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 HAL_RALLY_ENABLED +#if AP_RALLY == ENABLED if (!g2.wp_nav.set_desired_location(g2.rally.calc_best_rally_or_home_location(rover.current_loc, ahrs.get_home().alt))) { return false; } diff --git a/Rover/mode_smart_rtl.cpp b/Rover/mode_smart_rtl.cpp index 0080d3569fd8cd..0d083965e4b32d 100644 --- a/Rover/mode_smart_rtl.cpp +++ b/Rover/mode_smart_rtl.cpp @@ -22,7 +22,7 @@ bool ModeSmartRTL::_enter() } // init state - smart_rtl_state = SmartRTLState::WaitForPathCleanup; + smart_rtl_state = SmartRTL_WaitForPathCleanup; _loitering = false; return true; @@ -31,24 +31,24 @@ bool ModeSmartRTL::_enter() void ModeSmartRTL::update() { switch (smart_rtl_state) { - case SmartRTLState::WaitForPathCleanup: + case SmartRTL_WaitForPathCleanup: // check if return path is computed and if yes, begin journey home if (g2.smart_rtl.request_thorough_cleanup()) { - smart_rtl_state = SmartRTLState::PathFollow; + smart_rtl_state = SmartRTL_PathFollow; _load_point = true; } // Note: this may lead to an unnecessary 20ms slow down of the vehicle (but it is unlikely) stop_vehicle(); break; - case SmartRTLState::PathFollow: + case SmartRTL_PathFollow: // load point if required if (_load_point) { Vector3f dest_NED; if (!g2.smart_rtl.pop_point(dest_NED)) { // if not more points, we have reached home gcs().send_text(MAV_SEVERITY_INFO, "Reached destination"); - smart_rtl_state = SmartRTLState::StopAtHome; + smart_rtl_state = SmartRTL_StopAtHome; break; } else { // peek at the next point. this can fail if the IO task currently has the path semaphore @@ -57,7 +57,7 @@ void ModeSmartRTL::update() if (!g2.wp_nav.set_desired_location_NED(dest_NED, next_dest_NED)) { // this should never happen because the EKF origin should already be set gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination"); - smart_rtl_state = SmartRTLState::Failure; + smart_rtl_state = SmartRTL_Failure; INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } } else { @@ -65,7 +65,7 @@ void ModeSmartRTL::update() if (!g2.wp_nav.set_desired_location_NED(dest_NED)) { // this should never happen because the EKF origin should already be set gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination"); - smart_rtl_state = SmartRTLState::Failure; + smart_rtl_state = SmartRTL_Failure; INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } } @@ -81,8 +81,8 @@ void ModeSmartRTL::update() } break; - case SmartRTLState::StopAtHome: - case SmartRTLState::Failure: + case SmartRTL_StopAtHome: + case SmartRTL_Failure: _reached_destination = true; // we have reached the destination // boats loiters, rovers stop @@ -107,16 +107,16 @@ void ModeSmartRTL::update() bool ModeSmartRTL::get_desired_location(Location& destination) const { switch (smart_rtl_state) { - case SmartRTLState::WaitForPathCleanup: + case SmartRTL_WaitForPathCleanup: return false; - case SmartRTLState::PathFollow: + case SmartRTL_PathFollow: if (g2.wp_nav.is_destination_valid()) { destination = g2.wp_nav.get_destination(); return true; } return false; - case SmartRTLState::StopAtHome: - case SmartRTLState::Failure: + case SmartRTL_StopAtHome: + case SmartRTL_Failure: return false; } // should never reach here but just in case diff --git a/Rover/precision_landing.cpp b/Rover/precision_landing.cpp index c62b499b6a3eca..12b33174ebbe5f 100644 --- a/Rover/precision_landing.cpp +++ b/Rover/precision_landing.cpp @@ -4,7 +4,7 @@ #include "Rover.h" -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED void Rover::init_precland() { diff --git a/Rover/release-notes.txt b/Rover/release-notes.txt index e6957078113187..60c4ec6230c884 100644 --- a/Rover/release-notes.txt +++ b/Rover/release-notes.txt @@ -10,12 +10,8 @@ Changes from 4.4.0-beta7 - SpeedyBee-F405-Mini support - T-Motor H743 Mini support 2) EKF3 supports baroless boards -3) GPS-for-yaw allows base GPS to update at only 3Hz -4) INA battery monitor supports config of shunt resistor used (see BATTx_SHUNT) -5) Log VER message includes vehicle type -6) OpenDroneId option to auto-store IDs in persistent flash -7) RC SBUS protection against invalid data in first 4 channels -8) Bug fixes +3) INA battery monitor supports config of shunt resistor used (see BATTx_SHUNT) +4) Bug fixes - BMI088 IMU error value handling fixed to avoid occasional negative spike - Dev environment CI autotest stability improvements - OSD correct DisplayPort BF MSP symbols @@ -306,24 +302,6 @@ Changes from 4.3.0-beta12 - 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 492463e8a2345a..c7a17b10d5ac99 100644 --- a/Rover/system.cpp +++ b/Rover/system.cpp @@ -75,10 +75,8 @@ void Rover::init_ardupilot() g2.proximity.init(); #endif -#if AP_BEACON_ENABLED // init beacons used for non-gps position estimation g2.beacon.init(); -#endif // and baro for EKF barometer.set_log_baro_bit(MASK_LOG_IMU); @@ -107,9 +105,7 @@ 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 @@ -121,7 +117,7 @@ void Rover::init_ardupilot() camera.init(); #endif -#if AC_PRECLAND_ENABLED +#if PRECISION_LANDING == ENABLED // initialise precision landing init_precland(); #endif @@ -188,6 +184,10 @@ void Rover::startup_ground(void) #if AP_SCRIPTING_ENABLED g2.scripting.init(); #endif // AP_SCRIPTING_ENABLED + + // we don't want writes to the serial port to cause us to pause + // so set serial ports non-blocking once we are ready to drive + serial_manager.set_blocking_writes_all(false); } // update the ahrs flyforward setting which can allow @@ -219,30 +219,6 @@ 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) { @@ -250,12 +226,6 @@ 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 @@ -281,7 +251,7 @@ bool Rover::set_mode(Mode &new_mode, ModeReason reason) old_mode.exit(); control_mode_reason = reason; - logger.Write_Mode((uint8_t)control_mode->mode_number(), control_mode_reason); + logger.Write_Mode(control_mode->mode_number(), control_mode_reason); gcs().send_message(MSG_HEARTBEAT); notify_mode(control_mode); @@ -291,14 +261,9 @@ bool Rover::set_mode(Mode &new_mode, ModeReason reason) bool Rover::set_mode(const uint8_t new_mode, ModeReason reason) { static_assert(sizeof(Mode::Number) == sizeof(new_mode), "The new mode can't be mapped to the vehicles mode number"); - return rover.set_mode(static_cast(new_mode), reason); -} - -bool Rover::set_mode(Mode::Number new_mode, ModeReason reason) -{ - Mode *mode = rover.mode_from_mode_num(new_mode); + Mode *mode = rover.mode_from_mode_num((enum Mode::Number)new_mode); if (mode == nullptr) { - notify_no_such_mode((uint8_t)new_mode); + notify_no_such_mode(new_mode); return false; } return rover.set_mode(*mode, reason); @@ -322,7 +287,7 @@ void Rover::startup_INS_ground(void) void Rover::notify_mode(const Mode *mode) { AP_Notify::flags.autopilot_mode = mode->is_autopilot_mode(); - notify.flags.flight_mode = (uint8_t)mode->mode_number(); + notify.flags.flight_mode = mode->mode_number(); notify.set_flight_mode_str(mode->name4()); } diff --git a/Rover/version.h b/Rover/version.h index 11b034f62f7d68..672ec4e0479c7b 100644 --- a/Rover/version.h +++ b/Rover/version.h @@ -6,14 +6,14 @@ #include "ap_version.h" -#define THISFIRMWARE "ArduRover V4.5.0-dev" +#define THISFIRMWARE "ArduRover V4.4.0-beta8" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,5,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 4,4,0,FIRMWARE_VERSION_TYPE_BETA+7 #define FW_MAJOR 4 -#define FW_MINOR 5 +#define FW_MINOR 4 #define FW_PATCH 0 -#define FW_TYPE FIRMWARE_VERSION_TYPE_DEV +#define FW_TYPE FIRMWARE_VERSION_TYPE_BETA #include diff --git a/Tools/AP_Bootloader/AP_Bootloader.cpp b/Tools/AP_Bootloader/AP_Bootloader.cpp index f42271208073ba..d383905f3a7a79 100644 --- a/Tools/AP_Bootloader/AP_Bootloader.cpp +++ b/Tools/AP_Bootloader/AP_Bootloader.cpp @@ -63,8 +63,6 @@ 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; @@ -163,14 +161,6 @@ 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(); } @@ -184,6 +174,14 @@ 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 5bc0df7b2877f7..c0b7171a2043a9 100644 --- a/Tools/AP_Bootloader/bl_protocol.cpp +++ b/Tools/AP_Bootloader/bl_protocol.cpp @@ -163,7 +163,7 @@ extern AP_FlashIface_JEDEC ext_flash; /* 1ms timer tick callback */ -static void sys_tick_handler(virtual_timer_t* vt, void *ctx) +static void sys_tick_handler(void *ctx) { chSysLockFromISR(); chVTSetI(&systick_vt, chTimeMS2I(1), sys_tick_handler, nullptr); @@ -240,10 +240,6 @@ do_jump(uint32_t stacktop, uint32_t entrypoint) #define APP_START_ADDRESS (FLASH_LOAD_ADDRESS + (FLASH_BOOTLOADER_LOAD_KB + APP_START_OFFSET_KB)*1024U) #endif -#if !defined(STM32_OTG2_IS_OTG1) -#define STM32_OTG2_IS_OTG1 0 -#endif - void jump_to_app() { @@ -320,17 +316,12 @@ 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 !STM32_OTG2_IS_OTG1 +#if HAL_USE_SERIAL_USB == TRUE rccResetOTG_FS(); -#endif #if defined(rccResetOTG_HS) rccResetOTG_HS(); #endif @@ -634,7 +625,7 @@ bootloader(unsigned timeout) led_set(LED_OFF); // erase all sectors - for (uint16_t i = 0; flash_func_sector_size(i) != 0; i++) { + for (uint8_t i = 0; flash_func_sector_size(i) != 0; i++) { #if defined(STM32F7) || defined(STM32H7) if (!flash_func_erase_sector(i, c == PROTO_CHIP_FULL_ERASE)) { #else diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index ab5b3fa8bde839..16112dbabc043c 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_PRIMARY 1069 -AP_HW_CUBERED_SECONDARY 1070 +AP_HW_CUBERED 1069 +AP_HW_CUBERED_IO 1070 AP_HW_GreenSight_UltraBlue 1071 AP_HW_GreenSight_microBlue 1072 AP_HW_MAMBAH743_V4 1073 @@ -228,29 +228,7 @@ AP_HW_YJUAV_A6Nano 1114 AP_HW_ACNS_CM4PILOT 1115 AP_HW_ACNS_F405AIO 1116 AP_HW_BLITZF7 1117 -AP_HW_RADIX2HD 1118 -AP_HW_HEEWING_F405 1119 -AP_HW_PodmanH7 1120 -AP_HW_mRo-M10053 1121 -AP_HW_mRo-M10044 1122 -AP_HW_SIYI_N7 1123 -AP_HW_mRoCZOEM_revG 1124 -AP_HW_BETAFPV_F405 1125 -AP_HW_QioTekAdeptH743 1126 AP_HW_YJUAV_A6SE 1127 -AP_HW_QioTekAdept_6C 1128 - -AP_HW_PixFlamingoL4R5_V2 1129 -AP_HW_PixFlamingoF427_V1 1130 -AP_HW_PixFlamingoF767_V1 1131 -AP_HW_PixFlamingoH743I 1132 -AP_HW_PixFlamingoH743V 1133 - -AP_HW_AR-F407SmartBat 1134 -AP_HW_SPEEDYBEEF4MINI 1135 -AP_HW_SPEEDYBEEF4V4 1136 -AP_HW_FlywooF405Pro 1137 -AP_HW_TMOTORH7 1138 AP_HW_ESP32_PERIPH 1205 AP_HW_ESP32S3_PERIPH 1206 @@ -260,15 +238,6 @@ AP_HW_CUBEBLACK_PERIPH 1401 AP_HW_PIXRACER_PERIPH 1402 AP_HW_SWBOOMBOARD_PERIPH 1403 -AP_HW_VIMDRONES_FLOW 1404 -AP_HW_VIMDRONES_MOSAIC_X5 1405 -AP_HW_VIMDRONES_MOSAIC_H 1406 -AP_HW_VIMDRONES_PERIPH 1407 - -# IDs 5000-5099 reserved for Carbonix -# IDs 5100-5199 reserved for SYPAQ Systems -# IDs 6000-6099 reserved for SpektreWorks - # OpenDroneID enabled boards. Use 10000 + the base board ID AP_HW_CubeOrange_ODID 10140 AP_HW_Pixhawk6_ODID 10053 diff --git a/Tools/AP_Bootloader/can.cpp b/Tools/AP_Bootloader/can.cpp index 24ed583284eba6..715070edc69729 100644 --- a/Tools/AP_Bootloader/can.cpp +++ b/Tools/AP_Bootloader/can.cpp @@ -47,20 +47,18 @@ static CANConfig cancfg = { CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP, 0 // filled in below }; -// pipelining is not faster when using ChibiOS CAN driver -#define FW_UPDATE_PIPELINE_LEN 1 #else static ChibiOS::CANIface can_iface[HAL_NUM_CAN_IFACES]; #endif #ifndef CAN_APP_VERSION_MAJOR -#define CAN_APP_VERSION_MAJOR 2 +#define CAN_APP_VERSION_MAJOR 1 #endif #ifndef CAN_APP_VERSION_MINOR #define CAN_APP_VERSION_MINOR 0 #endif #ifndef CAN_APP_NODE_NAME -#define CAN_APP_NODE_NAME "org.ardupilot." CHIBIOS_BOARD_NAME +#define CAN_APP_NODE_NAME "org.ardupilot.ap_periph" #endif static uint8_t node_id_allocation_transfer_id; @@ -68,30 +66,14 @@ static uavcan_protocol_NodeStatus node_status; static uint32_t send_next_node_id_allocation_request_at_ms; static uint8_t node_id_allocation_unique_id_offset; -static void processTx(void); - -// keep up to 4 transfers in progress -#ifndef FW_UPDATE_PIPELINE_LEN -#define FW_UPDATE_PIPELINE_LEN 4 -#endif - static struct { - uint32_t rtt_ms; - uint32_t ofs; + uint64_t ofs; + uint32_t last_ms; uint8_t node_id; + uint8_t transfer_id; uint8_t path[sizeof(uavcan_protocol_file_Path::path.data)+1]; uint8_t sector; uint32_t sector_ofs; - uint8_t transfer_id; - uint8_t idx; - struct { - uint8_t tx_id; - uint32_t sent_ms; - uint32_t offset; - bool have_reply; - uavcan_protocol_file_ReadResponse pkt; - } reads[FW_UPDATE_PIPELINE_LEN]; - uint16_t erased_to; } fw_update; /* @@ -168,184 +150,87 @@ static void handle_get_node_info(CanardInstance* ins, /* send a read for a fw update */ -static bool send_fw_read(uint8_t idx) +static void send_fw_read(void) { - auto &r = fw_update.reads[idx]; - r.tx_id = fw_update.transfer_id; - r.have_reply = false; - - uavcan_protocol_file_ReadRequest pkt {}; - pkt.path.path.len = strlen((const char *)fw_update.path); - pkt.offset = r.offset; - memcpy(pkt.path.path.data, fw_update.path, pkt.path.path.len); - - uint8_t buffer[UAVCAN_PROTOCOL_FILE_READ_REQUEST_MAX_SIZE]; - uint16_t total_size = uavcan_protocol_file_ReadRequest_encode(&pkt, buffer, true); - - if (canardRequestOrRespond(&canard, - fw_update.node_id, - UAVCAN_PROTOCOL_FILE_READ_SIGNATURE, - UAVCAN_PROTOCOL_FILE_READ_ID, - &fw_update.transfer_id, - CANARD_TRANSFER_PRIORITY_HIGH, - CanardRequest, - &buffer[0], - total_size) > 0) { - // mark it as having been sent - r.sent_ms = AP_HAL::millis(); - return true; + uint32_t now = AP_HAL::millis(); + if (now - fw_update.last_ms < 750) { + // the server may still be responding + return; } - return false; -} + fw_update.last_ms = now; -/* - send a read for a fw update - */ -static void send_fw_reads(void) -{ - const uint32_t now = AP_HAL::millis(); - - for (uint8_t i=0; itransfer_id+1)%256 != fw_update.transfer_id || + transfer->source_node_id != fw_update.node_id) { return; } - flash_func_erase_sector(sector); - fw_update.erased_to = sector+1; + int16_t error = 0; + canardDecodeScalar(transfer, 0, 16, true, (void*)&error); + uint16_t len = transfer->payload_len - 2; - /* - pre-erase any non-erased pages up to end of flash. This puts all - the load of erasing at the start of flashing which is much - faster than flashing as we go on boards with small flash - sectors. We stop at the first already erased page so we don't - end up wasting time erasing already erased pages when the - firmware is much smaller than the total flash size - */ - while (flash_func_sector_size(fw_update.erased_to) != 0 && - !flash_func_is_erased(fw_update.erased_to)) { - flash_func_erase_sector(fw_update.erased_to); - fw_update.erased_to++; + uint32_t offset = 16; + uint32_t buf32[(len+3)/4]; + uint8_t *buf = (uint8_t *)&buf32[0]; + for (uint16_t i=0; isource_node_id != fw_update.node_id) { - return; + const uint32_t sector_size = flash_func_sector_size(fw_update.sector); + + if (fw_update.sector_ofs == 0) { + flash_func_erase_sector(fw_update.sector); } - /* - match the response to a sent request - */ - uint8_t idx = 0; - bool found = false; - for (idx=0; idxtransfer_id) { - found = true; - break; - } + if (fw_update.sector_ofs+len > sector_size) { + flash_func_erase_sector(fw_update.sector+1); } - if (!found) { - // not a current transfer - return; + for (uint16_t i=0; i= flash_func_sector_size(fw_update.sector)) { + fw_update.sector++; + fw_update.sector_ofs -= sector_size; } - fw_update.reads[idx].have_reply = true; - uint32_t rtt = MIN(3000,MAX(AP_HAL::millis() - fw_update.reads[idx].sent_ms, 25)); - fw_update.rtt_ms = uint32_t(0.9 * fw_update.rtt_ms + 0.1 * rtt); - - while (fw_update.reads[fw_update.idx].have_reply) { - auto &r = fw_update.reads[fw_update.idx]; - if (r.offset != fw_update.ofs) { - // bad sequence - r.have_reply = false; - r.sent_ms = 0; - break; - } - const auto &pkt = r.pkt; - const uint16_t len = pkt.data.len; - const uint16_t len_words = (len+3U)/4U; - const uint8_t *buf = (uint8_t *)pkt.data.data; - uint32_t buf32[len_words] {}; - memcpy((uint8_t*)buf32, buf, len); - - if (fw_update.ofs == 0) { - flash_set_keep_unlocked(true); - } - - const uint32_t sector_size = flash_func_sector_size(fw_update.sector); - if (sector_size == 0) { - // firmware is too big - fw_update.node_id = 0; - flash_write_flush(); - flash_set_keep_unlocked(false); - node_status.vendor_specific_status_code = uint8_t(check_fw_result_t::FAIL_REASON_BAD_LENGTH_APP); - break; - } - if (fw_update.sector_ofs == 0) { - erase_to(fw_update.sector); - } - if (fw_update.sector_ofs+len > sector_size) { - erase_to(fw_update.sector+1); - } - if (!flash_write_buffer(fw_update.ofs, buf32, len_words)) { - continue; - } - - fw_update.ofs += len; - fw_update.sector_ofs += len; - if (fw_update.sector_ofs >= flash_func_sector_size(fw_update.sector)) { - fw_update.sector++; - fw_update.sector_ofs -= sector_size; - } - - if (len < sizeof(uavcan_protocol_file_ReadResponse::data.data)) { - fw_update.node_id = 0; - flash_write_flush(); - flash_set_keep_unlocked(false); - const auto ok = check_good_firmware(); - node_status.vendor_specific_status_code = uint8_t(ok); - if (ok == check_fw_result_t::CHECK_FW_OK) { - jump_to_app(); - } - return; + if (len < sizeof(uavcan_protocol_file_ReadResponse::data.data)) { + fw_update.node_id = 0; + flash_write_flush(); + const auto ok = check_good_firmware(); + node_status.vendor_specific_status_code = uint8_t(ok); + if (ok == check_fw_result_t::CHECK_FW_OK) { + jump_to_app(); } - - r.have_reply = false; - r.sent_ms = 0; - r.offset += FW_UPDATE_PIPELINE_LEN*sizeof(uavcan_protocol_file_ReadResponse::data.data); - send_fw_read(fw_update.idx); - processTx(); - - fw_update.idx = (fw_update.idx + 1) % FW_UPDATE_PIPELINE_LEN; } // show offset number we are flashing in kbyte as crude progress indicator node_status.vendor_specific_status_code = 1 + (fw_update.ofs / 1024U); + + fw_update.last_ms = 0; } /* @@ -353,21 +238,23 @@ static void handle_file_read_response(CanardInstance* ins, CanardRxTransfer* tra */ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* transfer) { + // manual decoding due to TAO bug in libcanard generated code + if (transfer->payload_len < 1 || transfer->payload_len > sizeof(fw_update.path)+1) { + return; + } + if (fw_update.node_id == 0) { - uavcan_protocol_file_BeginFirmwareUpdateRequest pkt; - if (uavcan_protocol_file_BeginFirmwareUpdateRequest_decode(transfer, &pkt)) { - return; + uint32_t offset = 0; + canardDecodeScalar(transfer, 0, 8, false, (void*)&fw_update.node_id); + offset += 8; + for (uint8_t i=0; ipayload_len-1; i++) { + canardDecodeScalar(transfer, offset, 8, false, (void*)&fw_update.path[i]); + offset += 8; } - if (pkt.image_file_remote_path.path.len > sizeof(fw_update.path)-1) { - return; - } - memset(&fw_update, 0, sizeof(fw_update)); - for (uint8_t i=0; isource_node_id; } @@ -387,6 +274,8 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* CanardResponse, &buffer[0], total_size); + + send_fw_read(); } static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* transfer) @@ -402,28 +291,35 @@ static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* tr return; } - struct uavcan_protocol_dynamic_node_id_Allocation msg; - if (uavcan_protocol_dynamic_node_id_Allocation_decode(transfer, &msg)) { - return; + // Copying the unique ID from the message + static const uint8_t UniqueIDBitOffset = 8; + uint8_t received_unique_id[sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data)]; + uint8_t received_unique_id_len = 0; + for (; received_unique_id_len < (transfer->payload_len - (UniqueIDBitOffset / 8U)); received_unique_id_len++) { + const uint8_t bit_offset = (uint8_t)(UniqueIDBitOffset + received_unique_id_len * 8U); + (void) canardDecodeScalar(transfer, bit_offset, 8, false, &received_unique_id[received_unique_id_len]); } - + // Obtaining the local unique ID uint8_t my_unique_id[sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data)]; readUniqueID(my_unique_id); // Matching the received UID against the local one - if (memcmp(msg.unique_id.data, my_unique_id, msg.unique_id.len) != 0) { + if (memcmp(received_unique_id, my_unique_id, received_unique_id_len) != 0) { node_id_allocation_unique_id_offset = 0; return; // No match, return } - if (msg.unique_id.len < sizeof(msg.unique_id.data)) { + if (received_unique_id_len < sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data)) { // The allocator has confirmed part of unique ID, switching to the next stage and updating the timeout. - node_id_allocation_unique_id_offset = msg.unique_id.len; + node_id_allocation_unique_id_offset = received_unique_id_len; send_next_node_id_allocation_request_at_ms -= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS; } else { // Allocation complete - copying the allocated node ID from the message - canardSetLocalNodeID(ins, msg.node_id); + uint8_t allocated_node_id = 0; + (void) canardDecodeScalar(transfer, 0, 7, false, &allocated_node_id); + + canardSetLocalNodeID(ins, allocated_node_id); } } @@ -721,9 +617,6 @@ bool can_check_update(void) if (comms->magic == APP_BOOTLOADER_COMMS_MAGIC) { can_set_node_id(comms->my_node_id); fw_update.node_id = comms->server_node_id; - for (uint8_t i=0; ipath, sizeof(uavcan_protocol_file_Path::path.data)+1); ret = true; // clear comms region @@ -811,8 +704,8 @@ void can_start() void can_update() { - // do one loop of CAN support. If we are doing a firmware update - // then loop until it is finished + // do one loop of CAN support. If we are doing a few update then + // loop until it is finished do { processTx(); processRx(); @@ -824,12 +717,8 @@ void can_update() process1HzTasks(AP_HAL::micros64()); } if (fw_update.node_id != 0) { - send_fw_reads(); + send_fw_read(); } -#if CH_CFG_ST_FREQUENCY >= 1000000 - // give a bit of time for background processing - chThdSleepMicroseconds(200); -#endif } while (fw_update.node_id != 0); } diff --git a/Tools/AP_Bootloader/custom.cpp b/Tools/AP_Bootloader/custom.cpp deleted file mode 100644 index ef5f7ef434d38b..00000000000000 --- a/Tools/AP_Bootloader/custom.cpp +++ /dev/null @@ -1,52 +0,0 @@ -/* - 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); - uint8_t *endp = &revstr[max - 1]; - uint8_t *strp = revstr; + mcu_des_t des = mcu_descriptions[STM32_UNKNOWN]; for (const auto &desc : mcu_descriptions) { if (mcuid == desc.mcuid) { - // copy the string in: - const char *tmp = desc.desc; - while (strp < endp && *tmp) { - *strp++ = *tmp++; - } + des = desc; break; } } - // comma-separated: + 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++; + } + if (strp < endp) { *strp++ = ','; } - for (const auto &rev : silicon_revs) { - if (rev.revid == revid) { - if (strp < endp) { - *strp++ = rev.rev; - } - } + if (strp < endp) { + *strp++ = des.rev; } return strp - revstr; @@ -353,17 +343,6 @@ void uprintf(const char *fmt, ...) #endif } -static void thread_sleep_ms(uint32_t ms) -{ - while (ms > 0) { - // don't sleep more than 65 at a time, to cope with 16 bit - // timer - const uint32_t dt = ms > 65? 65: ms; - chThdSleepMilliseconds(dt); - ms -= dt; - } -} - // generate a pulse sequence forever, for debugging void led_pulses(uint8_t npulses) { @@ -371,11 +350,11 @@ void led_pulses(uint8_t npulses) while (true) { for (uint8_t i=0; i= 0) { auto *uart = hal.serial(g.proximity_port); if (uart != nullptr) { @@ -250,15 +238,6 @@ 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)); @@ -273,10 +252,6 @@ 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 @@ -284,7 +259,7 @@ void AP_Periph_FW::init() #if AP_SCRIPTING_ENABLED scripting.init(); #endif - start_ms = AP_HAL::millis(); + start_ms = AP_HAL::native_millis(); } #if (defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY) && HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY == 8) || defined(HAL_PERIPH_ENABLE_NOTIFY) @@ -302,7 +277,7 @@ void AP_Periph_FW::update_rainbow() if (rainbow_done) { return; } - uint32_t now = AP_HAL::millis(); + uint32_t now = AP_HAL::native_millis(); if (now - start_ms > 1500) { rainbow_done = true; #if defined (HAL_PERIPH_ENABLE_NOTIFY) @@ -386,7 +361,7 @@ void AP_Periph_FW::update() #endif static uint32_t last_led_ms; - uint32_t now = AP_HAL::millis(); + uint32_t now = AP_HAL::native_millis(); if (now - last_led_ms > 1000) { last_led_ms = now; #ifdef HAL_GPIO_PIN_LED @@ -422,8 +397,10 @@ void AP_Periph_FW::update() rcout_init_1Hz(); #endif - GCS_SEND_MESSAGE(MSG_HEARTBEAT); - GCS_SEND_MESSAGE(MSG_SYS_STATUS); +#if HAL_GCS_ENABLED + gcs().send_message(MSG_HEARTBEAT); + gcs().send_message(MSG_SYS_STATUS); +#endif } static uint32_t last_error_ms; @@ -436,13 +413,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 (debug_option_is_set(DebugOptions::SHOW_STACK) && now - last_debug_ms > 5000) { + if ((g.debug&(1< 5000) { last_debug_ms = now; show_stack_free(); } #endif - if (debug_option_is_set(DebugOptions::AUTOREBOOT) && AP_HAL::millis() > 15000) { + if ((g.debug&(1< 15000) { // attempt reboot with HOLD after 15s periph.prepare_reboot(); #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS @@ -455,18 +432,10 @@ void AP_Periph_FW::update() if (now - battery.last_read_ms >= 100) { // update battery at 10Hz battery.last_read_ms = now; - battery_lib.read(); + battery.lib.read(); } #endif -#ifdef HAL_PERIPH_ENABLE_RCIN - rcin_update(); -#endif - -#ifdef HAL_PERIPH_ENABLE_BATTERY_BALANCE - batt_balance_update(); -#endif - static uint32_t fiftyhz_last_update_ms; if (now - fiftyhz_last_update_ms >= 20) { // update at 50Hz @@ -488,23 +457,12 @@ 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 @@ -545,8 +503,8 @@ void AP_Periph_FW::check_for_serial_reboot_cmd(const int8_t serial_index) const char reboot_string_len = sizeof(reboot_string)-1; // -1 is to remove the null termination static uint16_t index[hal.num_serial]; - uint8_t data; - if (!uart->read(data)) { + const int16_t data = uart->read(); + if (data < 0 || data > 0xff) { // read error continue; } diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 8ed77e88fc1fbe..d9fa52efb06d97 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -1,7 +1,6 @@ #pragma once #include -#include #include #include #include @@ -14,28 +13,17 @@ #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 "batt_balance.h" + #include #if HAL_NMEA_OUTPUT_ENABLED && !(HAL_GCS_ENABLED && defined(HAL_PERIPH_ENABLE_GPS)) @@ -47,8 +35,6 @@ #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 @@ -68,14 +54,6 @@ #endif #endif -#ifndef AP_PERIPH_SAFETY_SWITCH_ENABLED -#define AP_PERIPH_SAFETY_SWITCH_ENABLED defined(HAL_PERIPH_ENABLE_RC_OUT) -#endif - -#ifndef HAL_PERIPH_CAN_MIRROR -#define HAL_PERIPH_CAN_MIRROR 0 -#endif - #include "Parameters.h" #if CONFIG_HAL_BOARD == HAL_BOARD_SITL @@ -91,23 +69,6 @@ extern "C" { void can_printf(const char *fmt, ...) FMT_PRINTF(1,2); } -struct CanardInstance; -struct CanardRxTransfer; - -#define MAKE_TRANSFER_DESCRIPTOR(data_type_id, transfer_type, src_node_id, dst_node_id) \ - (((uint32_t)(data_type_id)) | (((uint32_t)(transfer_type)) << 16U) | \ - (((uint32_t)(src_node_id)) << 18U) | (((uint32_t)(dst_node_id)) << 25U)) - -#ifndef HAL_CAN_POOL_SIZE -#if HAL_CANFD_SUPPORTED - #define HAL_CAN_POOL_SIZE 16000 -#elif GPS_MOVING_BASELINE - #define HAL_CAN_POOL_SIZE 8000 -#else - #define HAL_CAN_POOL_SIZE 4000 -#endif -#endif - class AP_Periph_FW { public: AP_Periph_FW(); @@ -137,11 +98,7 @@ class AP_Periph_FW { void can_airspeed_update(); void can_rangefinder_update(); void can_battery_update(); - void can_battery_send_cells(uint8_t instance); void can_proximity_update(); - void can_buzzer_update(void); - void can_safety_button_update(void); - void can_safety_LED_update(void); void load_parameters(); void prepare_reboot(); @@ -190,15 +147,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 - void handle_battery_failsafe(const char* type_str, const int8_t action) { } - AP_BattMonitor battery_lib{0, FUNCTOR_BIND_MEMBER(&AP_Periph_FW::handle_battery_failsafe, void, const char*, const int8_t), nullptr}; - struct { + struct AP_Periph_Battery { + void handle_battery_failsafe(const char* type_str, const int8_t action) { } + AP_BattMonitor lib{0, FUNCTOR_BIND_MEMBER(&AP_Periph_FW::AP_Periph_Battery::handle_battery_failsafe, void, const char*, const int8_t), nullptr}; + uint32_t last_read_ms; uint32_t last_can_send_ms; } battery; @@ -208,7 +161,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_CAN::Protocol can_protocol_cached[HAL_NUM_CAN_IFACES]; + AP_CANManager::Driver_Type can_protocol_cached[HAL_NUM_CAN_IFACES]; #endif #ifdef HAL_PERIPH_ENABLE_MSP @@ -236,7 +189,6 @@ class AP_Periph_FW { struct { mavlink_message_t msg; mavlink_status_t status; - uint32_t last_heartbeat_ms; } adsb; #endif @@ -249,7 +201,7 @@ class AP_Periph_FW { uint32_t last_sample_ms; #endif -#ifdef HAL_PERIPH_ENABLE_PROXIMITY +#ifdef HAL_PERIPH_ENABLE_PRX AP_Proximity proximity; #endif @@ -275,16 +227,7 @@ 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; @@ -308,21 +251,6 @@ class AP_Periph_FW { void rcout_handle_safety_state(uint8_t safety_state); #endif -#ifdef HAL_PERIPH_ENABLE_RCIN - void rcin_init(); - void rcin_update(); - void can_send_RCInput(uint8_t quality, uint16_t *values, uint8_t nvalues, bool in_failsafe, bool quality_valid); - bool rcin_initialised; - uint32_t rcin_last_sent_RCInput_ms; - const char *rcin_rc_protocol; // protocol currently being decoded - Parameters_RCIN g_rcin; -#endif - -#ifdef HAL_PERIPH_ENABLE_BATTERY_BALANCE - void batt_balance_update(); - BattBalance battery_balance; -#endif - #if AP_TEMPERATURE_SENSOR_ENABLED AP_TemperatureSensor temperature_sensor; #endif @@ -355,14 +283,6 @@ class AP_Periph_FW { AP_Logger logger; #endif -#ifdef HAL_PERIPH_ENABLE_NETWORKING - AP_Networking networking; -#endif - -#ifdef HAL_PERIPH_ENABLE_RTC - AP_RTC rtc; -#endif - #if HAL_GCS_ENABLED GCS_Periph _gcs; #endif @@ -371,140 +291,33 @@ class AP_Periph_FW { static const AP_Param::Info var_info[]; -#ifdef HAL_PERIPH_ENABLE_EFI - uint32_t last_efi_update_ms; -#endif -#ifdef HAL_PERIPH_ENABLE_MAG uint32_t last_mag_update_ms; -#endif -#ifdef HAL_PERIPH_ENABLE_GPS uint32_t last_gps_update_ms; uint32_t last_gps_yaw_ms; -#endif uint32_t last_relposheading_ms; -#ifdef HAL_PERIPH_ENABLE_BARO uint32_t last_baro_update_ms; -#endif -#ifdef HAL_PERIPH_ENABLE_AIRSPEED uint32_t last_airspeed_update_ms; -#endif -#ifdef HAL_PERIPH_ENABLE_GPS bool saw_gps_lock_once; -#endif static AP_Periph_FW *_singleton; - enum class DebugOptions { - SHOW_STACK = 0, - AUTOREBOOT = 1, - ENABLE_STATS = 2, + enum { + DEBUG_SHOW_STACK, + DEBUG_AUTOREBOOT }; - // check if an option is set - bool debug_option_is_set(const DebugOptions option) const { - return (uint8_t(g.debug.get()) & (1U<delay(10); periph.prepare_reboot(); diff --git a/Tools/AP_Periph/GCS_MAVLink.h b/Tools/AP_Periph/GCS_MAVLink.h index ce1f17b1ba2ed7..98f39b885256ae 100644 --- a/Tools/AP_Periph/GCS_MAVLink.h +++ b/Tools/AP_Periph/GCS_MAVLink.h @@ -31,7 +31,7 @@ class GCS_MAVLINK_Periph : public GCS_MAVLINK uint32_t telem_delay() const override { return 0; } void handleMessage(const mavlink_message_t &msg) override { handle_common_message(msg); } bool handle_guided_request(AP_Mission::Mission_Command &cmd) override { return true; } - MAV_RESULT handle_preflight_reboot(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; + MAV_RESULT handle_preflight_reboot(const mavlink_command_long_t &packet, const mavlink_message_t &msg) override; uint8_t sysid_my_gcs() const override; protected: diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 80b3ae7d44a219..ea6a4dfe1c18de 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -43,10 +43,6 @@ extern const AP_HAL::HAL &hal; #define AP_PERIPH_BARO_ENABLE_DEFAULT 1 #endif -#ifndef HAL_PERIPH_BATT_HIDE_MASK_DEFAULT -#define HAL_PERIPH_BATT_HIDE_MASK_DEFAULT 0 -#endif - #ifndef AP_PERIPH_EFI_PORT_DEFAULT #define AP_PERIPH_EFI_PORT_DEFAULT 3 #endif @@ -61,14 +57,6 @@ extern const AP_HAL::HAL &hal; #define MAV_SYSTEM_ID HAL_DEFAULT_MAV_SYSTEM_ID #endif -#ifndef APD_ESC_SERIAL_0 - #define APD_ESC_SERIAL_0 -1 -#endif - -#ifndef APD_ESC_SERIAL_1 - #define APD_ESC_SERIAL_1 -1 -#endif - /* * AP_Periph parameter definitions * @@ -111,30 +99,44 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { // @Param: CAN_PROTOCOL // @DisplayName: Enable use of specific protocol to be used on this port // @Description: Enabling this option starts selected protocol that will use this virtual driver. At least one CAN port must be UAVCAN or else CAN1 gets set to UAVCAN - // @Values: 0:Disabled,1:UAVCAN,4:PiccoloCAN,6:EFI_NWPMU,7:USD1,8:KDECAN + // @Values: 0:Disabled,1:UAVCAN,4:PiccoloCAN,5:CANTester,6:EFI_NWPMU,7:USD1,8:KDECAN // @User: Advanced // @RebootRequired: True - GARRAY(can_protocol, 0, "CAN_PROTOCOL", float(AP_CAN::Protocol::DroneCAN)), + GARRAY(can_protocol, 0, "CAN_PROTOCOL", AP_CANManager::Driver_Type_UAVCAN), // @Param: CAN2_BAUDRATE - // @CopyFieldsFrom: CAN_BAUDRATE // @DisplayName: Bitrate of CAN2 interface + // @Description: Bit rate can be set up to from 10000 to 1000000 + // @Range: 10000 1000000 + // @User: Advanced + // @RebootRequired: True GARRAY(can_baudrate, 1, "CAN2_BAUDRATE", 1000000), // @Param: CAN2_PROTOCOL - // @CopyFieldsFrom: CAN_PROTOCOL - GARRAY(can_protocol, 1, "CAN2_PROTOCOL", float(AP_CAN::Protocol::DroneCAN)), + // @DisplayName: Enable use of specific protocol to be used on this port + // @Description: Enabling this option starts selected protocol that will use this virtual driver. At least one CAN port must be UAVCAN or else CAN1 gets set to UAVCAN + // @Values: 0:Disabled,1:UAVCAN,4:PiccoloCAN,5:CANTester,6:EFI_NWPMU,7:USD1,8:KDECAN + // @User: Advanced + // @RebootRequired: True + GARRAY(can_protocol, 1, "CAN2_PROTOCOL", AP_CANManager::Driver_Type_UAVCAN), #endif #if HAL_NUM_CAN_IFACES >= 3 // @Param: CAN3_BAUDRATE // @DisplayName: Bitrate of CAN3 interface - // @CopyFieldsFrom: CAN_BAUDRATE + // @Description: Bit rate can be set up to from 10000 to 1000000 + // @Range: 10000 1000000 + // @User: Advanced + // @RebootRequired: True GARRAY(can_baudrate, 2, "CAN3_BAUDRATE", 1000000), // @Param: CAN3_PROTOCOL - // @CopyFieldsFrom: CAN_PROTOCOL - GARRAY(can_protocol, 2, "CAN3_PROTOCOL", float(AP_CAN::Protocol::DroneCAN)), + // @DisplayName: Enable use of specific protocol to be used on this port + // @Description: Enabling this option starts selected protocol that will use this virtual driver. At least one CAN port must be UAVCAN or else CAN1 gets set to UAVCAN + // @Values: 0:Disabled,1:UAVCAN,4:PiccoloCAN,5:CANTester,6:EFI_NWPMU,7:USD1,8:KDECAN + // @User: Advanced + // @RebootRequired: True + GARRAY(can_protocol, 2, "CAN3_PROTOCOL", AP_CANManager::Driver_Type_UAVCAN), #endif #if HAL_CANFD_SUPPORTED @@ -156,9 +158,11 @@ 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 @@ -175,7 +179,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { // @Param: DEBUG // @DisplayName: Debug // @Description: Debug - // @Bitmask: 0:Show free stack space, 1:Auto Reboot after 15sec, 2:Enable sending stats + // @Bitmask: 0:Disabled, 1:Show free stack space, 2:Auto Reboot after 15sec // @User: Advanced GSCALAR(debug, "DEBUG", 0), @@ -227,14 +231,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { #ifdef HAL_PERIPH_ENABLE_BATTERY // @Group: BATT // @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp - GOBJECT(battery_lib, "BATT", AP_BattMonitor), - - // @Param: BATT_HIDE_MASK - // @DisplayName: Battery hide mask - // @Description: Instance mask of local battery index(es) to prevent transmitting their status over CAN. This is useful for hiding a "battery" instance that is used locally in the peripheral but don't want them to be treated as a battery source(s) to the autopilot. For example, an AP_Periph battery monitor with multiple batteries that monitors each locally for diagnostic or other purposes, but only reports as a single SUM battery monitor to the autopilot. - // @Bitmask: 0:BATT, 1:BATT2, 2:BATT3, 3:BATT4, 4:BATT5, 5:BATT6, 6:BATT7, 7:BATT8, 8:BATT9, 9:BATTA, 10:BATTB, 11:BATTC, 12:BATTD, 13:BATTE, 14:BATTF, 15:BATTG - // @User: Advanced - GSCALAR(battery_hide_mask, "BATT_HIDE_MASK", HAL_PERIPH_BATT_HIDE_MASK_DEFAULT), + GOBJECT(battery, "BATT", AP_BattMonitor), #endif #ifdef HAL_PERIPH_ENABLE_MAG @@ -346,13 +343,13 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GSCALAR(hardpoint_rate, "HARDPOINT_RATE", 100), #endif -#if defined(HAL_PERIPH_ENABLE_HWESC) || defined(HAL_PERIPH_ENABLE_ESC_APD) +#ifdef HAL_PERIPH_ENABLE_HWESC // @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 - GARRAY(esc_number, 0, "ESC_NUMBER", 0), + GSCALAR(esc_number, "ESC_NUMBER", 0), #endif #ifdef HAL_PERIPH_ENABLE_RC_OUT @@ -486,7 +483,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GOBJECT(efi, "EFI", AP_EFI), #endif -#ifdef HAL_PERIPH_ENABLE_PROXIMITY +#ifdef HAL_PERIPH_ENABLE_PRX // @Param: PRX_BAUDRATE // @DisplayName: Proximity Sensor serial baudrate // @Description: Proximity Sensor serial baudrate. @@ -516,9 +513,9 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { // Proximity driver // @Group: PRX - // @Path: ../libraries/AP_Proximity/AP_Proximity.cpp + // @Path: ../libraries/AP_RangeFinder/AP_Proximity.cpp GOBJECT(proximity, "PRX", AP_Proximity), -#endif // HAL_PERIPH_ENABLE_PROXIMITY +#endif #if HAL_NMEA_OUTPUT_ENABLED // @Group: NMEA_ @@ -526,94 +523,6 @@ 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 - -#ifdef HAL_PERIPH_ENABLE_BATTERY_BALANCE - // @Group: BAL - // @Path: batt_balance.cpp - GOBJECT(battery_balance, "BAL", BattBalance), -#endif - - // NOTE: sim parameters should go last -#if AP_SIM_ENABLED - // @Group: SIM_ - // @Path: ../libraries/SITL/SITL.cpp - GOBJECT(sitl, "SIM_", SITL::SIM), - -#if AP_AHRS_ENABLED - // @Group: AHRS_ - // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp - GOBJECT(ahrs, "AHRS_", AP_AHRS), -#endif -#endif // AP_SIM_ENABLED - -#if HAL_PERIPH_CAN_MIRROR - // @Param: CAN_MIRROR_PORTS - // @DisplayName: CAN ports to mirror traffic between - // @Description: Any set ports will participate in blindly mirroring traffic from one port to the other. It is the users responsibility to ensure that no loops exist that cause traffic to be infinitly repeated, and both ports must be running the same baud rates. - // @Bitmask: 0:CAN1, 1:CAN2, 2:CAN3 - // @User: Advanced - GSCALAR(can_mirror_ports, "CAN_MIRROR_PORTS", 0), -#endif // HAL_PERIPH_CAN_MIRROR - -#ifdef HAL_PERIPH_ENABLE_RTC - // @Group: RTC - // @Path: ../libraries/AP_RTC/AP_RTC.cpp - GOBJECT(rtc, "RTC", AP_RTC), -#endif - AP_VAREND }; diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index fb4a628b021099..63322b10d467c6 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -30,8 +30,8 @@ class Parameters { k_param_hardpoint_id, k_param_hardpoint_rate, k_param_baro_enable, - k_param_esc_number0, - k_param_battery_lib, + k_param_esc_number, + k_param_battery, k_param_debug, k_param_serial_number, k_param_adsb_port, @@ -70,21 +70,6 @@ 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, - k_param_battery_balance, - k_param_battery_hide_mask, - k_param_can_mirror_ports, - k_param_rtc, }; AP_Int16 format_version; @@ -92,7 +77,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 @@ -118,7 +103,7 @@ class Parameters { AP_Int16 rangefinder_max_rate; #endif -#ifdef HAL_PERIPH_ENABLE_PROXIMITY +#ifdef HAL_PERIPH_ENABLE_PRX AP_Int32 proximity_baud; AP_Int8 proximity_port; AP_Int16 proximity_max_rate; @@ -135,21 +120,8 @@ class Parameters { AP_Int8 hardpoint_rate; #endif -#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]; +#ifdef HAL_PERIPH_ENABLE_HWESC + AP_Int8 esc_number; #endif #ifdef HAL_PERIPH_ENABLE_GPS @@ -186,18 +158,10 @@ class Parameters { AP_Int16 sysid_this_mav; #endif -#ifdef HAL_PERIPH_ENABLE_BATTERY - AP_Int32 battery_hide_mask; -#endif - #ifdef HAL_PERIPH_ENABLE_EFI AP_Int32 efi_baudrate; AP_Int8 efi_port; #endif - -#if HAL_PERIPH_CAN_MIRROR - AP_Int8 can_mirror_ports; -#endif // HAL_PERIPH_CAN_MIRROR #if HAL_CANFD_SUPPORTED AP_Int8 can_fdmode; @@ -205,7 +169,6 @@ class Parameters { #else static constexpr uint8_t can_fdmode = 0; #endif - AP_Int8 node_stats; Parameters() {} }; diff --git a/Tools/AP_Periph/README.md b/Tools/AP_Periph/README.md index 67aba113d923d5..90e5a62cd05b04 100644 --- a/Tools/AP_Periph/README.md +++ b/Tools/AP_Periph/README.md @@ -1,50 +1,42 @@ -# AP_Periph DroneCAN Peripheral Firmware +# AP_Periph UAVCAN Peripheral Firmware -This is an ArduPilot based DroneCAN peripheral firmware. This firmware +This is an ArduPilot based UAVCAN peripheral firmware. This firmware takes advantage of the wide range of sensor drivers in ArduPilot to -make building a DroneCAN peripheral firmware easy. +make building a UAVCAN peripheral firmware easy. The AP_Periph firmware is based on the same ChibiOS hwdef.dat system that is used to define pinouts for STM32 based flight controllers supported by ArduPilot. That means you can add support for a new -DroneCAN peripheral based on the STM32 by just writing a simple +UAVCAN peripheral based on the STM32 by just writing a simple hwdef.dat that defines the pinout of your device. -We have over 60 build targets building for AP_Periph firmwares. All -ArduPilot supported MCUs can be used, including: +Currently we have four targets building for AP_Periph firmwares: - - STM32F1xx - - STM32F3xx - - STM32F4xx - - STM32F7xx - - STM32H7xx - - STM32L4xx - - STM32G4xx + - A STM32F103 128k flash part made by mRobotics (target f103-GPS) + - A STM32F412 512k flash part made by CUAV (target CUAV_GPS) + - A STM32F105 256k flash part (used in ZubaxGNSSv2) + - A STM32F303 256k flash part made by mRobotics (target f303-GPS) More can be added using the hwdef.dat system # Features The AP_Periph firmware can be configured to enable a wide range of -DroneCAN sensor types. Support is included for: +UAVCAN sensor types. Support is included for: - GPS modules (including RTK GPS) - Magnetometers (SPI or I2C) - Barometers (SPI or I2C) - Airspeed sensors (I2C) - Rangefinders (UART or I2C) - - ADSB (uAvionix compatible Ping ADSB receiver on UART) + - ADSB (Ping ADSB receiver on UART) - Battery Monitor (Analog, I2C/SMBus, UART) - LEDs (GPIO, I2C or WS2812 serial) - Safety LED and Safety Switch - Buzzer (tonealarm or simple GPIO) - RC Output (All standard RCOutput protocols) - - RC input - - battery balance monitor - - EFI engines - - Proximity sensors -An AP_Periph DroneCAN firmware supports these DroneCAN features: +An AP_Periph UAVCAN firmware supports these UAVCAN features: - dynamic or static CAN node allocation - firmware upload @@ -64,8 +56,8 @@ Using f103-GPS as an example, build the main firmware like this: - ./waf AP_Periph that will build a file build/f103-GPS/bin/AP_Periph.bin. You can -now load that using the CAN bootloader and either dronecan_gui_tool or -MissionPlanner DroneCAN support. +now load that using the CAN bootloader and either uavcan_gui_tool or +MissionPlanner SLCAN support. # Flashing @@ -95,32 +87,95 @@ the resulting bootloader will be in Tools/bootloaders Firmware targets are automatically built and distributed on the ArduPilot firmware server on firmware.ardupilot.org. These firmwares -can be loaded using Mission Planner or the DroneCAN GUI Tool. Parameters -for peripherals can be changed using the Mission Planner DroneCAN support -or using DroneCAN GUI Tools. +can be loaded using Mission Planner or the UAVCAN GUI Tool. Parameters +for peripherals can be changed using the Mission Planner SLCAN support +or using UAVCAN GUI Tools. # User Bootloader Update The bootloader is automatically stored in ROMFS in the main -firmware. End users can update the bootloader by setting the DroneCAN +firmware. End users can update the bootloader by setting the UAVCAN parameter "FLASH_BOOTLOADER" to 1. After setting it to 1 the node will -respond with a debug text message which can be seen in the DroneCAN GUI +respond with a debug text message which can be seen in the UAVCAN GUI tool to show the result of the flash. # SITL Testing -A wide range of DroneCAN peripherals are supported in the SITL -simulation system. The simplest way of starting a DroneCAN enabled -simulated vehicle is to use sim_vehicle.py. - -For a quadplane use: sim_vehicle.py with the option -f quadplane-can - -For a quadcopter use: sim_vehicle.py with the option -f quad-can +Currently GPS peripheral build is supported under linux environment, +we simulate a UAVCAN GPS Peripheral on SocketCAN. + +Setup can be done as follows, this is on top of usual setup required +to build ardupilot: + +``` +sudo dpkg --add-architecture i386 +sudo apt-get update +sudo apt-get install -y gcc-multilib g++-multilib +sudo apt-get update +sudo apt-get -y install can-utils iproute2 linux-modules-extra-$(uname -r) +sudo modprobe vcan +sudo ip link add dev vcan0 type vcan +sudo ip link set up vcan0 +``` +Build Commands: +``` +./waf configure --board sitl_periph_gps +./waf AP_Periph +``` +Autotest Command: +``` +Tools/autotest/autotest.py -v Copter build.SITLPeriphGPS test.CAN +``` + + +--- +**Note** + +To run valgrind on AP_Periph binary you might need to get 32 bit version of libc6-dbg which can be simply get using following command for Ubuntu machines: `sudo apt-get install libc6-dbg:i386` + +--- + + +https://github.com/linux-can/can-utils contains a nice set of utility to do CAN related testings on Linux system. I used Ubuntu for this development, for Ubuntu systems you can simply download this tool using `sudo apt-get install can-utils` + +Following are the common commands that can be used while testing or developing: +* Create Virtual CAN Interface: +``` +sudo modprobe vcan +sudo ip link add dev vcan0 type vcan +sudo ip link set up vcan0 +sudo ip link add dev vcan1 type vcan +sudo ip link set up vcan1 +``` +* Route one CANSocket to another +``` +sudo modprobe can-gw +sudo cangw -A -s vcan0 -d vcan1 -e +sudo cangw -A -s vcan1 -d vcan0 -e +``` +* Delete routes +``` +sudo cangw -D -s vcan0 -d vcan1 -e +sudo cangw -D -s vcan1 -d vcan0 -e +``` +* Route SLCAN to VCAN, this allows connecting CAN devices to SITL run via CAN Adapter like the one running in Ardupilot itself. +``` +sudo modprobe slcan +sudo modprobe can-gw +sudo slcan_attach -f -s8 -o /dev/ttyACM0 +sudo slcand ttyACM0 slcan0 +sudo ifconfig slcan0 up +sudo cangw -A -s vcan0 -d slcan0 -e +sudo cangw -A -s slcan0 -d vcan0 -e +``` +* Dump can messages: +``` +sudo candump vcan0 +``` # Discussion and Feedback Please join the discussions at these locations: - - https://discuss.ardupilot.org/ - - https://ardupilot.org/discord - + - https://discuss.ardupilot.org/t/ap-periph-1-0-0-stable-released/49049 + - https://gitter.im/ArduPilot/CANBUS diff --git a/Tools/AP_Periph/ReleaseNotes.txt b/Tools/AP_Periph/ReleaseNotes.txt index 252044e1efceb7..d265eff2d41ca4 100644 --- a/Tools/AP_Periph/ReleaseNotes.txt +++ b/Tools/AP_Periph/ReleaseNotes.txt @@ -1,45 +1,3 @@ -Release 1.6.0 8th September 2023 --------------------------------- - -This is a major release with the following changes: - - - much faster CAN bootloader for faster firmware update - - improved handling of peripherals with 2 or more CAN interfaces - - support most AP_Periph features in SITL testing - - added RC input support - - added battery balance plug support - - support sending RPM over DroneCAN - - support for pitot temperature reporting - -Release 1.5.1 23rd July 2023 ---------------------------- - -This is a major release with the following changes: - -- support serial tunnelling over DroneCAN -- raised CAN priority of MovingBaseline data -- support APD ESC telemetry -- support DroneCAN and CAN statistics reporting -- support KDECAN to DroneCAN translation - -The serial tunnelling support allows for uCenter to be used over -DroneCAN with the serial tunnelling panel in the DroneCAN GUI -tool. This allows for monitoring of uBlox GPS over a telemetry link, -and update of F9P firmware over DroneCAN - -Release 1.5.0 27th Mar 2023 ---------------------------- - -This is a major release with the following changes: - -- fixed airspeed bus default -- limit mag to 25Hz by default -- fixed send rate of GPS yaw -- fixed HW ESC telem temp units -- allow set of port for HW telem -- send GNSS heading message if available -- stop sending old GNSS Fix message - Release 1.4.1 27th Sep 2022 --------------------------- diff --git a/Tools/AP_Periph/adsb.cpp b/Tools/AP_Periph/adsb.cpp index 1a1229826bee6e..0e44461eb7ff36 100644 --- a/Tools/AP_Periph/adsb.cpp +++ b/Tools/AP_Periph/adsb.cpp @@ -23,10 +23,21 @@ #ifdef HAL_PERIPH_ENABLE_ADSB #include -#include extern const AP_HAL::HAL &hal; +# if !HAL_GCS_ENABLED + +#include "include/mavlink/v2.0/protocol.h" +#include "include/mavlink/v2.0/mavlink_types.h" +#include "include/mavlink/v2.0/all/mavlink.h" +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wmissing-declarations" +#include "include/mavlink/v2.0/mavlink_helpers.h" +#pragma GCC diagnostic pop + +#endif + /* init ADSB support */ @@ -41,6 +52,7 @@ void AP_Periph_FW::adsb_init(void) } } + /* update ADSB subsystem */ @@ -61,7 +73,7 @@ void AP_Periph_FW::adsb_update(void) const uint8_t c = (uint8_t)uart->read(); // Try to get a new message - if (mavlink_frame_char_buffer(&adsb.msg, &adsb.status, c, &adsb.msg, &adsb.status) == MAVLINK_FRAMING_OK) { + if (mavlink_parse_char(MAVLINK_COMM_0, c, &adsb.msg, &adsb.status)) { if (adsb.msg.msgid == MAVLINK_MSG_ID_ADSB_VEHICLE) { // decode and send as UAVCAN TrafficReport static mavlink_adsb_vehicle_t msg; @@ -70,82 +82,6 @@ void AP_Periph_FW::adsb_update(void) } } } - - /* - some ADSB devices need a heartbeat to get the system ID - */ - const uint32_t now_ms = AP_HAL::millis(); - if (now_ms - adsb.last_heartbeat_ms >= 1000) { - adsb.last_heartbeat_ms = now_ms; - mavlink_heartbeat_t heartbeat {}; - mavlink_message_t msg; - heartbeat.type = MAV_TYPE_GENERIC; - heartbeat.autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA; - auto len = mavlink_msg_heartbeat_encode_status(1, - MAV_COMP_ID_PERIPHERAL, - &adsb.status, - &msg, &heartbeat); - - uart->write((uint8_t*)&msg.magic, len); - } -} - -/* - map an ADSB_VEHICLE MAVLink message to a UAVCAN TrafficReport message - */ -void AP_Periph_FW::can_send_ADSB(struct __mavlink_adsb_vehicle_t &msg) -{ - ardupilot_equipment_trafficmonitor_TrafficReport pkt {}; - pkt.timestamp.usec = 0; - pkt.icao_address = msg.ICAO_address; - pkt.tslc = msg.tslc; - pkt.latitude_deg_1e7 = msg.lat; - pkt.longitude_deg_1e7 = msg.lon; - pkt.alt_m = msg.altitude * 1e-3; - - pkt.heading = radians(msg.heading * 1e-2); - - pkt.velocity[0] = cosf(pkt.heading) * msg.hor_velocity * 1e-2; - pkt.velocity[1] = sinf(pkt.heading) * msg.hor_velocity * 1e-2; - pkt.velocity[2] = -msg.ver_velocity * 1e-2; - - pkt.squawk = msg.squawk; - memcpy(pkt.callsign, msg.callsign, MIN(sizeof(msg.callsign),sizeof(pkt.callsign))); - if (msg.flags & 0x8000) { - pkt.source = ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_SOURCE_ADSB_UAT; - } else { - pkt.source = ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_SOURCE_ADSB; - } - - pkt.traffic_type = msg.emitter_type; - - if ((msg.flags & ADSB_FLAGS_VALID_ALTITUDE) != 0 && msg.altitude_type == 0) { - pkt.alt_type = ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ALT_TYPE_PRESSURE_AMSL; - } else if ((msg.flags & ADSB_FLAGS_VALID_ALTITUDE) != 0 && msg.altitude_type == 1) { - pkt.alt_type = ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ALT_TYPE_WGS84; - } else { - pkt.alt_type = ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ALT_TYPE_ALT_UNKNOWN; - } - - pkt.lat_lon_valid = (msg.flags & ADSB_FLAGS_VALID_COORDS) != 0; - pkt.heading_valid = (msg.flags & ADSB_FLAGS_VALID_HEADING) != 0; - pkt.velocity_valid = (msg.flags & ADSB_FLAGS_VALID_VELOCITY) != 0; - pkt.callsign_valid = (msg.flags & ADSB_FLAGS_VALID_CALLSIGN) != 0; - pkt.ident_valid = (msg.flags & ADSB_FLAGS_VALID_SQUAWK) != 0; - pkt.simulated_report = (msg.flags & ADSB_FLAGS_SIMULATED) != 0; - - // these flags are not in common.xml - pkt.vertical_velocity_valid = (msg.flags & 0x0080) != 0; - pkt.baro_valid = (msg.flags & 0x0100) != 0; - - uint8_t buffer[ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_MAX_SIZE] {}; - uint16_t total_size = ardupilot_equipment_trafficmonitor_TrafficReport_encode(&pkt, buffer, !periph.canfdout()); - - canard_broadcast(ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_SIGNATURE, - ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ID, - CANARD_TRANSFER_PRIORITY_LOWEST, - &buffer[0], - total_size); } #endif // HAL_PERIPH_ENABLE_ADSB diff --git a/Tools/AP_Periph/airspeed.cpp b/Tools/AP_Periph/airspeed.cpp deleted file mode 100644 index 17a4da160731c5..00000000000000 --- a/Tools/AP_Periph/airspeed.cpp +++ /dev/null @@ -1,85 +0,0 @@ -#include "AP_Periph.h" - -#ifdef HAL_PERIPH_ENABLE_AIRSPEED - -/* - airspeed support - */ - -#include - -#ifndef AP_PERIPH_PROBE_CONTINUOUS -#define AP_PERIPH_PROBE_CONTINUOUS 0 -#endif - -/* - update CAN airspeed - */ -void AP_Periph_FW::can_airspeed_update(void) -{ - if (!airspeed.enabled()) { - return; - } -#if AP_PERIPH_PROBE_CONTINUOUS - if (!airspeed.healthy()) { - uint32_t now = AP_HAL::millis(); - static uint32_t last_probe_ms; - if (now - last_probe_ms >= 1000) { - last_probe_ms = now; - airspeed.allocate(); - } - } -#endif - uint32_t now = AP_HAL::millis(); - if (now - last_airspeed_update_ms < 50) { - // max 20Hz data - return; - } - last_airspeed_update_ms = now; - airspeed.update(); - if (!airspeed.healthy()) { - // don't send any data - return; - } - const float press = airspeed.get_corrected_pressure(); - float temp; - if (!airspeed.get_temperature(temp)) { - temp = nanf(""); - } else { - temp = C_TO_KELVIN(temp); - } - - uavcan_equipment_air_data_RawAirData pkt {}; - - // unfilled elements are NaN - pkt.static_pressure = nanf(""); - pkt.static_pressure_sensor_temperature = nanf(""); - pkt.differential_pressure_sensor_temperature = nanf(""); - pkt.pitot_temperature = nanf(""); - - // populate the elements we have - pkt.differential_pressure = press; - pkt.static_air_temperature = temp; - - // if a Pitot tube temperature sensor is available, use it -#if AP_TEMPERATURE_SENSOR_ENABLED - for (uint8_t i=0; i - -/* - update CAN baro - */ -void AP_Periph_FW::can_baro_update(void) -{ - if (!periph.g.baro_enable) { - return; - } - baro.update(); - if (last_baro_update_ms == baro.get_last_update()) { - return; - } - - last_baro_update_ms = baro.get_last_update(); - if (!baro.healthy()) { - // don't send any data - return; - } - const float press = baro.get_pressure(); - const float temp = baro.get_temperature(); - - { - uavcan_equipment_air_data_StaticPressure pkt {}; - pkt.static_pressure = press; - pkt.static_pressure_variance = 0; // should we make this a parameter? - - uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_air_data_StaticPressure_encode(&pkt, buffer, !periph.canfdout()); - - canard_broadcast(UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_SIGNATURE, - UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); - } - - { - uavcan_equipment_air_data_StaticTemperature pkt {}; - pkt.static_temperature = C_TO_KELVIN(temp); - pkt.static_temperature_variance = 0; // should we make this a parameter? - - uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_air_data_StaticTemperature_encode(&pkt, buffer, !periph.canfdout()); - - canard_broadcast(UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_SIGNATURE, - UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); - } -} - -#endif // HAL_PERIPH_ENABLE_BARO diff --git a/Tools/AP_Periph/batt_balance.cpp b/Tools/AP_Periph/batt_balance.cpp deleted file mode 100644 index 86c7c7a11cd7c4..00000000000000 --- a/Tools/AP_Periph/batt_balance.cpp +++ /dev/null @@ -1,138 +0,0 @@ -/* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ - -#include "AP_Periph.h" - -#ifdef HAL_PERIPH_ENABLE_BATTERY_BALANCE - -#include - -extern const AP_HAL::HAL &hal; - -#ifndef AP_PERIPH_BATTERY_BALANCE_NUMCELLS_DEFAULT -#define AP_PERIPH_BATTERY_BALANCE_NUMCELLS_DEFAULT 0 -#endif - -#ifndef AP_PERIPH_BATTERY_BALANCE_RATE_DEFAULT -#define AP_PERIPH_BATTERY_BALANCE_RATE_DEFAULT 1 -#endif - -#ifndef AP_PERIPH_BATTERY_BALANCE_CELL1_PIN_DEFAULT -#define AP_PERIPH_BATTERY_BALANCE_CELL1_PIN_DEFAULT 1 -#endif - -#ifndef AP_PERIPH_BATTERY_BALANCE_ID_DEFAULT -#define AP_PERIPH_BATTERY_BALANCE_ID_DEFAULT 0 -#endif - - -const AP_Param::GroupInfo BattBalance::var_info[] { - // @Param: _NUM_CELLS - // @DisplayName: Number of battery cells - // @Description: Number of battery cells to monitor - // @Range: 0 64 - AP_GROUPINFO("_NUM_CELLS", 1, BattBalance, num_cells, AP_PERIPH_BATTERY_BALANCE_NUMCELLS_DEFAULT), - - // @Param: _ID - // @DisplayName: Battery ID - // @Description: Battery ID to match against other batteries - // @Range: 0 127 - AP_GROUPINFO("_ID", 2, BattBalance, id, AP_PERIPH_BATTERY_BALANCE_ID_DEFAULT), - - // @Param: _RATE - // @DisplayName: Send Rate - // @Description: Rate to send cell information - // @Range: 0 20 - AP_GROUPINFO("_RATE", 3, BattBalance, rate, AP_PERIPH_BATTERY_BALANCE_RATE_DEFAULT), - - // @Param: _CELL1_PIN - // @DisplayName: First analog pin - // @Description: Analog pin of the first cell. Later cells must be sequential - // @Range: 0 127 - AP_GROUPINFO("_CELL1_PIN", 4, BattBalance, cell1_pin, AP_PERIPH_BATTERY_BALANCE_CELL1_PIN_DEFAULT), - - AP_GROUPEND -}; - -BattBalance::BattBalance(void) -{ - AP_Param::setup_object_defaults(this, var_info); -} - -void AP_Periph_FW::batt_balance_update() -{ - const int8_t ncell = battery_balance.num_cells; - if (ncell <= 0) { - return; - } - - // allocate cell sources if needed - if (battery_balance.cells == nullptr) { - battery_balance.cells = new AP_HAL::AnalogSource*[ncell]; - if (battery_balance.cells == nullptr) { - return; - } - battery_balance.cells_allocated = ncell; - for (uint8_t i=0; ichannel(battery_balance.cell1_pin + i); - } - } - - const uint32_t now = AP_HAL::millis(); - if (now - battery_balance.last_send_ms < 1000.0/battery_balance.rate.get()) { - return; - } - battery_balance.last_send_ms = now; - - // allocate space for the packet. This is a large - // packet that won't fit on the stack, so dynamically allocate - auto *pkt = new ardupilot_equipment_power_BatteryInfoAux; - uint8_t *buffer = new uint8_t[ARDUPILOT_EQUIPMENT_POWER_BATTERYINFOAUX_MAX_SIZE]; - if (pkt == nullptr || buffer == nullptr) { - delete pkt; - delete [] buffer; - return; - } - - pkt->voltage_cell.len = battery_balance.cells_allocated; - float last_cell = 0; - for (uint8_t i=0; ivoltage_average(); - pkt->voltage_cell.data[i] = v - last_cell; - last_cell = v; - } - pkt->max_current = nanf(""); - pkt->nominal_voltage = nanf(""); - pkt->battery_id = uint8_t(battery_balance.id); - - // encode and send message: - const uint16_t total_size = ardupilot_equipment_power_BatteryInfoAux_encode(pkt, buffer, !periph.canfdout()); - - canard_broadcast(ARDUPILOT_EQUIPMENT_POWER_BATTERYINFOAUX_SIGNATURE, - ARDUPILOT_EQUIPMENT_POWER_BATTERYINFOAUX_ID, - CANARD_TRANSFER_PRIORITY_LOW, - buffer, - total_size); - - delete pkt; - delete [] buffer; -} - -#endif // HAL_PERIPH_ENABLE_BATTERY_BALANCE - diff --git a/Tools/AP_Periph/batt_balance.h b/Tools/AP_Periph/batt_balance.h deleted file mode 100644 index 961875c660ab71..00000000000000 --- a/Tools/AP_Periph/batt_balance.h +++ /dev/null @@ -1,24 +0,0 @@ -#pragma once - -#ifdef HAL_PERIPH_ENABLE_BATTERY_BALANCE - -class BattBalance { -public: - friend class AP_Periph_FW; - BattBalance(void); - - static const struct AP_Param::GroupInfo var_info[]; - -private: - AP_Int8 num_cells; - AP_Int8 id; - AP_Int8 cell1_pin; - AP_Float rate; - uint32_t last_send_ms; - - AP_HAL::AnalogSource **cells; - uint8_t cells_allocated; -}; - -#endif // HAL_PERIPH_ENABLE_BATTERY_BALANCE - diff --git a/Tools/AP_Periph/battery.cpp b/Tools/AP_Periph/battery.cpp deleted file mode 100644 index 8f07834d22e66c..00000000000000 --- a/Tools/AP_Periph/battery.cpp +++ /dev/null @@ -1,128 +0,0 @@ -#include "AP_Periph.h" - -#ifdef HAL_PERIPH_ENABLE_BATTERY - -/* - battery support - */ - -#include - -extern const AP_HAL::HAL &hal; - -#ifndef AP_PERIPH_BATTERY_MODEL_NAME -#define AP_PERIPH_BATTERY_MODEL_NAME CAN_APP_NODE_NAME -#endif - -/* - update CAN battery monitor - */ -void AP_Periph_FW::can_battery_update(void) -{ - const uint32_t now_ms = AP_HAL::millis(); - if (now_ms - battery.last_can_send_ms < 100) { - return; - } - battery.last_can_send_ms = now_ms; - - const uint8_t battery_instances = battery_lib.num_instances(); - for (uint8_t i=0; i= 0) ? serial_number : i+1; - - pkt.voltage = battery_lib.voltage(i); - - float current; - if (battery_lib.current_amps(current, i)) { - pkt.current = current; - } - float temperature; - if (battery_lib.get_temperature(temperature, i)) { - // Battery lib reports temperature in Celsius. - // Convert Celsius to Kelvin for transmission on CAN. - pkt.temperature = C_TO_KELVIN(temperature); - } - - pkt.state_of_health_pct = UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATE_OF_HEALTH_UNKNOWN; - uint8_t percentage = 0; - if (battery_lib.capacity_remaining_pct(percentage, i)) { - pkt.state_of_charge_pct = percentage; - } - pkt.model_instance_id = i+1; - -#if !defined(HAL_PERIPH_BATTERY_SKIP_NAME) - // example model_name: "org.ardupilot.ap_periph SN 123" - hal.util->snprintf((char*)pkt.model_name.data, sizeof(pkt.model_name.data), "%s %ld", AP_PERIPH_BATTERY_MODEL_NAME, (long int)serial_number); - pkt.model_name.len = strnlen((char*)pkt.model_name.data, sizeof(pkt.model_name.data)); -#endif //defined(HAL_PERIPH_BATTERY_SKIP_NAME) - - uint8_t buffer[UAVCAN_EQUIPMENT_POWER_BATTERYINFO_MAX_SIZE] {}; - const uint16_t total_size = uavcan_equipment_power_BatteryInfo_encode(&pkt, buffer, !periph.canfdout()); - - canard_broadcast(UAVCAN_EQUIPMENT_POWER_BATTERYINFO_SIGNATURE, - UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); - - // Send individual cell information if available - if (battery_lib.has_cell_voltages(i)) { - can_battery_send_cells(i); - } - } -} - -/* - send individual cell voltages if available - */ -void AP_Periph_FW::can_battery_send_cells(uint8_t instance) -{ - // allocate space for the packet. This is a large - // packet that won't fit on the stack, so dynamically allocate - auto* pkt = new ardupilot_equipment_power_BatteryInfoAux; - uint8_t* buffer = new uint8_t[ARDUPILOT_EQUIPMENT_POWER_BATTERYINFOAUX_MAX_SIZE]; - if (pkt == nullptr || buffer == nullptr) { - delete pkt; - delete [] buffer; - return; - } - const auto &cell_voltages = battery_lib.get_cell_voltages(instance); - - for (uint8_t i = 0; i < ARRAY_SIZE(cell_voltages.cells); i++) { - if (cell_voltages.cells[i] == 0xFFFFU) { - break; - } - pkt->voltage_cell.data[i] = cell_voltages.cells[i]*0.001; - pkt->voltage_cell.len = i+1; - } - - pkt->max_current = nanf(""); - pkt->nominal_voltage = nanf(""); - - // encode and send message: - const uint16_t total_size = ardupilot_equipment_power_BatteryInfoAux_encode(pkt, buffer, !periph.canfdout()); - - canard_broadcast(ARDUPILOT_EQUIPMENT_POWER_BATTERYINFOAUX_SIGNATURE, - ARDUPILOT_EQUIPMENT_POWER_BATTERYINFOAUX_ID, - CANARD_TRANSFER_PRIORITY_LOW, - buffer, - total_size); - - // Delete temporary buffers - delete pkt; - delete [] buffer; -} - -#endif // HAL_PERIPH_ENABLE_BATTERY - diff --git a/Tools/AP_Periph/buzzer.cpp b/Tools/AP_Periph/buzzer.cpp deleted file mode 100644 index c5285e3612d375..00000000000000 --- a/Tools/AP_Periph/buzzer.cpp +++ /dev/null @@ -1,55 +0,0 @@ -#include "AP_Periph.h" - -#if defined(HAL_PERIPH_ENABLE_NOTIFY) || defined(HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY) - -/* - buzzer support - */ - -#include - -extern const AP_HAL::HAL &hal; - -static uint32_t buzzer_start_ms; -static uint32_t buzzer_len_ms; - -/* - handle BeepCommand - */ -void AP_Periph_FW::handle_beep_command(CanardInstance* canard_instance, CanardRxTransfer* transfer) -{ - uavcan_equipment_indication_BeepCommand req; - if (uavcan_equipment_indication_BeepCommand_decode(transfer, &req)) { - return; - } - static bool initialised; - if (!initialised) { - initialised = true; - hal.rcout->init(); - hal.util->toneAlarm_init(AP_Notify::Notify_Buzz_Builtin); - } - buzzer_start_ms = AP_HAL::millis(); - buzzer_len_ms = req.duration*1000; -#ifdef HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY - float volume = constrain_float(periph.g.buzz_volume/100.0f, 0, 1); -#elif defined(HAL_PERIPH_ENABLE_NOTIFY) - float volume = constrain_float(periph.notify.get_buzz_volume()/100.0f, 0, 1); -#endif - hal.util->toneAlarm_set_buzzer_tone(req.frequency, volume, uint32_t(req.duration*1000)); -} - -/* - update buzzer - */ -void AP_Periph_FW::can_buzzer_update(void) -{ - if (buzzer_start_ms != 0) { - uint32_t now = AP_HAL::millis(); - if (now - buzzer_start_ms > buzzer_len_ms) { - hal.util->toneAlarm_set_buzzer_tone(0, 0, 0); - buzzer_start_ms = 0; - } - } -} - -#endif // (HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY) || (HAL_PERIPH_ENABLE_NOTIFY) diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index deadcd03f36f71..85a9519a8797eb 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -20,6 +20,8 @@ #include #include #include "AP_Periph.h" +#include +#include #include #include #include @@ -36,7 +38,7 @@ #include #endif -#define IFACE_ALL ((1U<<(HAL_NUM_CAN_IFACES))-1U) +#define IFACE_ALL ((1U<<(HAL_NUM_CAN_IFACES+1U))-1U) #include "i2c.h" #include @@ -45,6 +47,7 @@ #include #endif + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL extern const HAL_SITL &hal; #else @@ -53,6 +56,14 @@ extern const AP_HAL::HAL &hal; extern AP_Periph_FW periph; +#ifndef HAL_CAN_POOL_SIZE +#if HAL_CANFD_SUPPORTED + #define HAL_CAN_POOL_SIZE 16000 +#else + #define HAL_CAN_POOL_SIZE 4000 +#endif +#endif + #ifndef HAL_PERIPH_LOOP_DELAY_US // delay between can loop updates. This needs to be longer on F4 #if defined(STM32H7) @@ -62,13 +73,17 @@ extern AP_Periph_FW periph; #endif #endif -#define DEBUG_PKTS 0 +#ifndef AP_PERIPH_MAG_MAX_RATE +#define AP_PERIPH_MAG_MAX_RATE 25U +#endif -#if HAL_PERIPH_CAN_MIRROR - #ifndef HAL_PERIPH_CAN_MIRROR_QUEUE_SIZE - #define HAL_PERIPH_CAN_MIRROR_QUEUE_SIZE 64 - #endif -#endif //HAL_PERIPH_CAN_MIRROR +#define DEBUG_PRINTS 0 +#define DEBUG_PKTS 0 +#if DEBUG_PRINTS + # define Debug(fmt, args ...) do {can_printf(fmt "\n", ## args);} while(0) +#else + # define Debug(fmt, args ...) +#endif #ifndef HAL_PERIPH_SUPPORT_LONG_CAN_PRINTF // When enabled, can_printf() strings longer than the droneCAN max text length (90 chars) @@ -87,18 +102,27 @@ static struct instance_t { #elif CONFIG_HAL_BOARD == HAL_BOARD_SITL HALSITL::CANIface* iface; #endif - -#if HAL_PERIPH_CAN_MIRROR - #if HAL_NUM_CAN_IFACES < 2 - #error "Can't use HAL_PERIPH_CAN_MIRROR if there are not at least 2 HAL_NUM_CAN_IFACES" - #endif - ObjectBuffer *mirror_queue; - uint8_t mirror_fail_count; -#endif // HAL_PERIPH_CAN_MIRROR } instances[HAL_NUM_CAN_IFACES]; - -#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && defined(HAL_GPIO_PIN_TERMCAN1) && (HAL_NUM_CAN_IFACES >= 2) +static struct dronecan_protocol_t { + CanardInstance canard; + uint32_t canard_memory_pool[HAL_CAN_POOL_SIZE/sizeof(uint32_t)]; + struct tid_map { + uint32_t transfer_desc; + uint8_t tid; + tid_map *next; + } *tid_map_head; + /* + * Variables used for dynamic node ID allocation. + * RTFM at http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation + */ + uint32_t send_next_node_id_allocation_request_at_ms; ///< When the next node ID allocation request should be sent + uint8_t node_id_allocation_unique_id_offset; ///< Depends on the stage of the next request + uint8_t tx_fail_count; + uint8_t dna_interface = 1; +} dronecan; + +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && defined(HAL_GPIO_PIN_TERMCAN1) static ioline_t can_term_lines[] = { HAL_GPIO_PIN_TERMCAN1 @@ -121,11 +145,19 @@ HAL_GPIO_PIN_TERMCAN1 }; #endif // CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && defined(HAL_GPIO_PIN_TERMCAN1) +#ifndef CAN_APP_NODE_NAME +#define CAN_APP_NODE_NAME "org.ardupilot.ap_periph" +#endif + #ifndef HAL_CAN_DEFAULT_NODE_ID #define HAL_CAN_DEFAULT_NODE_ID CANARD_BROADCAST_NODE_ID #endif uint8_t PreferredNodeID = HAL_CAN_DEFAULT_NODE_ID; +#ifndef AP_PERIPH_BATTERY_MODEL_NAME +#define AP_PERIPH_BATTERY_MODEL_NAME CAN_APP_NODE_NAME +#endif + #ifndef AP_PERIPH_PROBE_CONTINUOUS #define AP_PERIPH_PROBE_CONTINUOUS 0 #endif @@ -148,9 +180,7 @@ 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 */ @@ -174,13 +204,13 @@ static void readUniqueID(uint8_t* out_uid) /* handle a GET_NODE_INFO request */ -void AP_Periph_FW::handle_get_node_info(CanardInstance* canard_instance, - CanardRxTransfer* transfer) +static void handle_get_node_info(CanardInstance* ins, + CanardRxTransfer* transfer) { uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE] {}; uavcan_protocol_GetNodeInfoResponse pkt {}; - node_status.uptime_sec = AP_HAL::millis() / 1000U; + node_status.uptime_sec = AP_HAL::native_millis() / 1000U; pkt.status = node_status; pkt.software_version.major = AP::fwversion().major; @@ -198,16 +228,16 @@ void AP_Periph_FW::handle_get_node_info(CanardInstance* canard_instance, pkt.hardware_version.major = APJ_BOARD_ID >> 8; pkt.hardware_version.minor = APJ_BOARD_ID & 0xFF; - if (g.serial_number > 0) { - hal.util->snprintf((char*)pkt.name.data, sizeof(pkt.name.data), "%s(%u)", CAN_APP_NODE_NAME, (unsigned)g.serial_number); + if (periph.g.serial_number > 0) { + hal.util->snprintf((char*)pkt.name.data, sizeof(pkt.name.data), "%s(%u)", CAN_APP_NODE_NAME, (unsigned)periph.g.serial_number); } else { hal.util->snprintf((char*)pkt.name.data, sizeof(pkt.name.data), "%s", CAN_APP_NODE_NAME); } pkt.name.len = strnlen((char*)pkt.name.data, sizeof(pkt.name.data)); - uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer, !canfdout()); + uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer, !periph.canfdout()); - const int16_t resp_res = canardRequestOrRespond(canard_instance, + const int16_t resp_res = canardRequestOrRespond(ins, transfer->source_node_id, UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, UAVCAN_PROTOCOL_GETNODEINFO_ID, @@ -220,7 +250,7 @@ void AP_Periph_FW::handle_get_node_info(CanardInstance* canard_instance, , IFACE_ALL #endif #if HAL_CANFD_SUPPORTED - , canfdout() + , periph.canfdout() #endif ); if (resp_res <= 0) { @@ -231,7 +261,7 @@ void AP_Periph_FW::handle_get_node_info(CanardInstance* canard_instance, /* handle parameter GetSet request */ -void AP_Periph_FW::handle_param_getset(CanardInstance* canard_instance, CanardRxTransfer* transfer) +static void handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer) { // param fetch all can take a long time, so pat watchdog stm32_watchdog_pat(); @@ -314,9 +344,9 @@ void AP_Periph_FW::handle_param_getset(CanardInstance* canard_instance, CanardRx } uint8_t buffer[UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_MAX_SIZE] {}; - uint16_t total_size = uavcan_protocol_param_GetSetResponse_encode(&pkt, buffer, !canfdout()); + uint16_t total_size = uavcan_protocol_param_GetSetResponse_encode(&pkt, buffer, !periph.canfdout()); - canardRequestOrRespond(canard_instance, + canardRequestOrRespond(ins, transfer->source_node_id, UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE, UAVCAN_PROTOCOL_PARAM_GETSET_ID, @@ -329,7 +359,7 @@ void AP_Periph_FW::handle_param_getset(CanardInstance* canard_instance, CanardRx , IFACE_ALL #endif #if HAL_CANFD_SUPPORTED - ,canfdout() + ,periph.canfdout() #endif ); @@ -338,7 +368,7 @@ void AP_Periph_FW::handle_param_getset(CanardInstance* canard_instance, CanardRx /* handle parameter executeopcode request */ -void AP_Periph_FW::handle_param_executeopcode(CanardInstance* canard_instance, CanardRxTransfer* transfer) +static void handle_param_executeopcode(CanardInstance* ins, CanardRxTransfer* transfer) { uavcan_protocol_param_ExecuteOpcodeRequest req; if (uavcan_protocol_param_ExecuteOpcodeRequest_decode(transfer, &req)) { @@ -350,22 +380,22 @@ void AP_Periph_FW::handle_param_executeopcode(CanardInstance* canard_instance, C AP_Param::load_all(); AP_Param::setup_sketch_defaults(); #ifdef HAL_PERIPH_ENABLE_GPS - AP_Param::setup_object_defaults(&gps, gps.var_info); + AP_Param::setup_object_defaults(&periph.gps, periph.gps.var_info); #endif #ifdef HAL_PERIPH_ENABLE_BATTERY - AP_Param::setup_object_defaults(&battery, battery_lib.var_info); + AP_Param::setup_object_defaults(&periph.battery, periph.battery.lib.var_info); #endif #ifdef HAL_PERIPH_ENABLE_MAG - AP_Param::setup_object_defaults(&compass, compass.var_info); + AP_Param::setup_object_defaults(&periph.compass, periph.compass.var_info); #endif #ifdef HAL_PERIPH_ENABLE_BARO - AP_Param::setup_object_defaults(&baro, baro.var_info); + AP_Param::setup_object_defaults(&periph.baro, periph.baro.var_info); #endif #ifdef HAL_PERIPH_ENABLE_AIRSPEED - AP_Param::setup_object_defaults(&airspeed, airspeed.var_info); + AP_Param::setup_object_defaults(&periph.airspeed, periph.airspeed.var_info); #endif #ifdef HAL_PERIPH_ENABLE_RANGEFINDER - AP_Param::setup_object_defaults(&rangefinder, rangefinder.var_info); + AP_Param::setup_object_defaults(&periph.rangefinder, periph.rangefinder.var_info); #endif } @@ -374,9 +404,9 @@ void AP_Periph_FW::handle_param_executeopcode(CanardInstance* canard_instance, C pkt.ok = true; uint8_t buffer[UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_MAX_SIZE] {}; - uint16_t total_size = uavcan_protocol_param_ExecuteOpcodeResponse_encode(&pkt, buffer, !canfdout()); + uint16_t total_size = uavcan_protocol_param_ExecuteOpcodeResponse_encode(&pkt, buffer, !periph.canfdout()); - canardRequestOrRespond(canard_instance, + canardRequestOrRespond(ins, transfer->source_node_id, UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE, UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID, @@ -389,12 +419,20 @@ void AP_Periph_FW::handle_param_executeopcode(CanardInstance* canard_instance, C , IFACE_ALL #endif #if HAL_CANFD_SUPPORTED - ,canfdout() + ,periph.canfdout() #endif ); } -void AP_Periph_FW::handle_begin_firmware_update(CanardInstance* canard_instance, CanardRxTransfer* transfer) +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) { #if HAL_RAM_RESERVE_START >= 256 // setup information on firmware request at start of ram @@ -402,24 +440,28 @@ void AP_Periph_FW::handle_begin_firmware_update(CanardInstance* canard_instance, memset(comms, 0, sizeof(struct app_bootloader_comms)); comms->magic = APP_BOOTLOADER_COMMS_MAGIC; - uavcan_protocol_file_BeginFirmwareUpdateRequest req; - if (uavcan_protocol_file_BeginFirmwareUpdateRequest_decode(transfer, &req)) { + // manual decoding due to TAO bug in libcanard generated code + if (transfer->payload_len < 1 || transfer->payload_len > sizeof(comms->path)+1) { return; } - - comms->server_node_id = req.source_node_id; + uint32_t offset = 0; + canardDecodeScalar(transfer, 0, 8, false, (void*)&comms->server_node_id); + offset += 8; + for (uint8_t i=0; ipayload_len-1; i++) { + canardDecodeScalar(transfer, offset, 8, false, (void*)&comms->path[i]); + offset += 8; + } if (comms->server_node_id == 0) { 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(canard_instance); + comms->my_node_id = canardGetLocalNodeID(ins); 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, !canfdout()); - canardRequestOrRespond(canard_instance, + uint32_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, buffer, !periph.canfdout()); + canardRequestOrRespond(ins, transfer->source_node_id, UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE, UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID, @@ -432,7 +474,7 @@ void AP_Periph_FW::handle_begin_firmware_update(CanardInstance* canard_instance, ,IFACE_ALL #endif #if HAL_CANFD_SUPPORTED - ,canfdout() + ,periph.canfdout() #endif ); uint8_t count = 50; @@ -444,18 +486,18 @@ void AP_Periph_FW::handle_begin_firmware_update(CanardInstance* canard_instance, // instant reboot, with backup register used to give bootloader // the node_id - prepare_reboot(); + periph.prepare_reboot(); #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS - set_fast_reboot((rtc_boot_magic)(RTC_BOOT_CANBL | canardGetLocalNodeID(canard_instance))); + set_fast_reboot((rtc_boot_magic)(RTC_BOOT_CANBL | canardGetLocalNodeID(ins))); NVIC_SystemReset(); #endif } -void AP_Periph_FW::handle_allocation_response(CanardInstance* canard_instance, CanardRxTransfer* transfer) +static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* transfer) { // Rule C - updating the randomized time interval dronecan.send_next_node_id_allocation_request_at_ms = - AP_HAL::millis() + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS + + AP_HAL::native_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) @@ -489,19 +531,19 @@ void AP_Periph_FW::handle_allocation_response(CanardInstance* canard_instance, C printf("Matching allocation response: %d\n", msg.unique_id.len); } else { // Allocation complete - copying the allocated node ID from the message - canardSetLocalNodeID(canard_instance, msg.node_id); + canardSetLocalNodeID(ins, msg.node_id); printf("IF%d Node ID allocated: %d\n", dronecan.dna_interface, msg.node_id); #if defined(HAL_PERIPH_ENABLE_GPS) && (HAL_NUM_CAN_IFACES >= 2) && GPS_MOVING_BASELINE - if (g.gps_mb_only_can_port) { + if (periph.g.gps_mb_only_can_port) { // we need to assign the unallocated port to be used for Moving Baseline only - gps_mb_can_port = (dronecan.dna_interface+1)%HAL_NUM_CAN_IFACES; + periph.gps_mb_can_port = (dronecan.dna_interface+1)%HAL_NUM_CAN_IFACES; if (canardGetLocalNodeID(&dronecan.canard) == CANARD_BROADCAST_NODE_ID) { // copy node id from the primary iface canardSetLocalNodeID(&dronecan.canard, msg.node_id); #ifdef HAL_GPIO_PIN_TERMCAN1 // also terminate the line as we don't have any other device on this port - palWriteLine(can_term_lines[gps_mb_can_port], 1); + palWriteLine(can_term_lines[periph.gps_mb_can_port], 1); #endif } } @@ -509,6 +551,48 @@ void AP_Periph_FW::handle_allocation_response(CanardInstance* canard_instance, C } } +#if defined(HAL_PERIPH_ENABLE_NOTIFY) || defined(HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY) +static uint32_t buzzer_start_ms; +static uint32_t buzzer_len_ms; +/* + handle BeepCommand + */ +static void handle_beep_command(CanardInstance* ins, CanardRxTransfer* transfer) +{ + uavcan_equipment_indication_BeepCommand req; + if (uavcan_equipment_indication_BeepCommand_decode(transfer, &req)) { + return; + } + static bool initialised; + if (!initialised) { + initialised = true; + hal.rcout->init(); + hal.util->toneAlarm_init(AP_Notify::Notify_Buzz_Builtin); + } + buzzer_start_ms = AP_HAL::native_millis(); + buzzer_len_ms = req.duration*1000; +#ifdef HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY + float volume = constrain_float(periph.g.buzz_volume/100.0f, 0, 1); +#elif defined(HAL_PERIPH_ENABLE_NOTIFY) + float volume = constrain_float(periph.notify.get_buzz_volume()/100.0f, 0, 1); +#endif + hal.util->toneAlarm_set_buzzer_tone(req.frequency, volume, uint32_t(req.duration*1000)); +} + +/* + update buzzer + */ +static void can_buzzer_update(void) +{ + if (buzzer_start_ms != 0) { + uint32_t now = AP_HAL::native_millis(); + if (now - buzzer_start_ms > buzzer_len_ms) { + hal.util->toneAlarm_set_buzzer_tone(0, 0, 0); + buzzer_start_ms = 0; + } + } +} +#endif // (HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY) || (HAL_PERIPH_ENABLE_NOTIFY) #if defined(HAL_GPIO_PIN_SAFE_LED) || defined(HAL_PERIPH_ENABLE_RC_OUT) static uint8_t safety_state; @@ -516,15 +600,15 @@ static uint8_t safety_state; /* handle SafetyState */ -void AP_Periph_FW::handle_safety_state(CanardInstance* canard_instance, CanardRxTransfer* transfer) +static void handle_safety_state(CanardInstance* ins, CanardRxTransfer* transfer) { ardupilot_indication_SafetyState req; if (ardupilot_indication_SafetyState_decode(transfer, &req)) { return; } safety_state = req.status; -#if AP_PERIPH_SAFETY_SWITCH_ENABLED - rcout_handle_safety_state(safety_state); +#ifdef HAL_PERIPH_ENABLE_RC_OUT + periph.rcout_handle_safety_state(safety_state); #endif } #endif // HAL_GPIO_PIN_SAFE_LED @@ -532,7 +616,7 @@ void AP_Periph_FW::handle_safety_state(CanardInstance* canard_instance, CanardRx /* handle ArmingStatus */ -void AP_Periph_FW::handle_arming_status(CanardInstance* canard_instance, CanardRxTransfer* transfer) +static void handle_arming_status(CanardInstance* ins, CanardRxTransfer* transfer) { uavcan_equipment_safety_ArmingStatus req; if (uavcan_equipment_safety_ArmingStatus_decode(transfer, &req)) { @@ -541,15 +625,44 @@ void AP_Periph_FW::handle_arming_status(CanardInstance* canard_instance, CanardR hal.util->set_soft_armed(req.status == UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_STATUS_FULLY_ARMED); } +#ifdef HAL_PERIPH_ENABLE_GPS +/* + handle gnss::RTCMStream + */ +static void handle_RTCMStream(CanardInstance* ins, CanardRxTransfer* transfer) +{ + uavcan_equipment_gnss_RTCMStream req; + if (uavcan_equipment_gnss_RTCMStream_decode(transfer, &req)) { + return; + } + periph.gps.handle_gps_rtcm_fragment(0, req.data.data, req.data.len); +} + +/* + handle gnss::MovingBaselineData +*/ +#if GPS_MOVING_BASELINE +static void handle_MovingBaselineData(CanardInstance* ins, CanardRxTransfer* transfer) +{ + ardupilot_gnss_MovingBaselineData msg; + if (ardupilot_gnss_MovingBaselineData_decode(transfer, &msg)) { + return; + } + periph.gps.inject_MBL_data(msg.data.data, msg.data.len); + Debug("MovingBaselineData: len=%u\n", msg.data.len); +} +#endif // GPS_MOVING_BASELINE + +#endif // HAL_PERIPH_ENABLE_GPS #if defined(AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NOTIFY) -void AP_Periph_FW::set_rgb_led(uint8_t red, uint8_t green, uint8_t blue) +static void set_rgb_led(uint8_t red, uint8_t green, uint8_t blue) { #ifdef HAL_PERIPH_ENABLE_NOTIFY - notify.handle_rgb(red, green, blue); + periph.notify.handle_rgb(red, green, blue); #ifdef HAL_PERIPH_ENABLE_RC_OUT - rcout_has_new_data_to_update = true; + periph.rcout_has_new_data_to_update = true; #endif // HAL_PERIPH_ENABLE_RC_OUT #endif // HAL_PERIPH_ENABLE_NOTIFY @@ -628,7 +741,7 @@ void AP_Periph_FW::set_rgb_led(uint8_t red, uint8_t green, uint8_t blue) /* handle lightscommand */ -void AP_Periph_FW::handle_lightscommand(CanardInstance* canard_instance, CanardRxTransfer* transfer) +static void handle_lightscommand(CanardInstance* ins, CanardRxTransfer* transfer) { uavcan_equipment_indication_LightsCommand req; if (uavcan_equipment_indication_LightsCommand_decode(transfer, &req)) { @@ -642,9 +755,9 @@ void AP_Periph_FW::handle_lightscommand(CanardInstance* canard_instance, CanardR uint8_t green = (cmd.color.green>>1U)<<3U; uint8_t blue = cmd.color.blue<<3U; #ifdef HAL_PERIPH_ENABLE_NOTIFY - const int8_t brightness = notify.get_rgb_led_brightness_percent(); + const int8_t brightness = periph.notify.get_rgb_led_brightness_percent(); #elif defined(AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY) - const int8_t brightness = g.led_brightness; + const int8_t brightness = periph.g.led_brightness; #endif if (brightness != 100 && brightness >= 0) { const float scale = brightness * 0.01; @@ -658,34 +771,57 @@ void AP_Periph_FW::handle_lightscommand(CanardInstance* canard_instance, CanardR #endif // AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY #ifdef HAL_PERIPH_ENABLE_RC_OUT -void AP_Periph_FW::handle_esc_rawcommand(CanardInstance* canard_instance, CanardRxTransfer* transfer) +static void handle_esc_rawcommand(CanardInstance* ins, CanardRxTransfer* transfer) { uavcan_equipment_esc_RawCommand cmd; if (uavcan_equipment_esc_RawCommand_decode(transfer, &cmd)) { return; } - rcout_esc(cmd.cmd.data, cmd.cmd.len); + periph.rcout_esc(cmd.cmd.data, cmd.cmd.len); // Update internal copy for disabling output to ESC when CAN packets are lost - last_esc_num_channels = cmd.cmd.len; - last_esc_raw_command_ms = AP_HAL::millis(); + periph.last_esc_num_channels = cmd.cmd.len; + periph.last_esc_raw_command_ms = AP_HAL::millis(); } -void AP_Periph_FW::handle_act_command(CanardInstance* canard_instance, CanardRxTransfer* transfer) +static void handle_act_command(CanardInstance* ins, CanardRxTransfer* transfer) { - uavcan_equipment_actuator_ArrayCommand cmd; - if (uavcan_equipment_actuator_ArrayCommand_decode(transfer, &cmd)) { + // manual decoding due to TAO bug in libcanard generated code + if (transfer->payload_len < 1 || transfer->payload_len > UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_MAX_SIZE+1) { return; } - for (uint8_t i=0; i < cmd.commands.len; i++) { - const auto &c = cmd.commands.data[i]; - switch (c.command_type) { + const uint8_t data_count = (transfer->payload_len / UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_MAX_SIZE); + uavcan_equipment_actuator_Command data[data_count] {}; + + uint32_t offset = 0; + for (uint8_t i=0; i 100) { last_update_ms = now; static uint8_t led_counter; @@ -746,11 +882,11 @@ void AP_Periph_FW::can_safety_LED_update(void) /* update safety button */ -void AP_Periph_FW::can_safety_button_update(void) +static void can_safety_button_update(void) { static uint32_t last_update_ms; static uint8_t counter; - uint32_t now = AP_HAL::millis(); + uint32_t now = AP_HAL::native_millis(); // send at 10Hz when pressed if (palReadLine(HAL_GPIO_PIN_SAFE_BUTTON) != HAL_SAFE_BUTTON_ON) { counter = 0; @@ -769,56 +905,51 @@ void AP_Periph_FW::can_safety_button_update(void) pkt.press_time = counter; uint8_t buffer[ARDUPILOT_INDICATION_BUTTON_MAX_SIZE] {}; - uint16_t total_size = ardupilot_indication_Button_encode(&pkt, buffer, !canfdout()); + 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); + 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. */ -void AP_Periph_FW::onTransferReceived(CanardInstance* canard_instance, - CanardRxTransfer* transfer) +static void onTransferReceived(CanardInstance* ins, + CanardRxTransfer* transfer) { #ifdef HAL_GPIO_PIN_LED_CAN1 palToggleLine(HAL_GPIO_PIN_LED_CAN1); #endif -#if HAL_CANFD_SUPPORTED - // enable tao for decoding when not on CANFD - transfer->tao = !transfer->canfd; -#endif - /* * Dynamic node ID allocation protocol. * Taking this branch only if we don't have a node ID, ignoring otherwise. */ - if (canardGetLocalNodeID(canard_instance) == CANARD_BROADCAST_NODE_ID) { + if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID) { if (transfer->transfer_type == CanardTransferTypeBroadcast && transfer->data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID) { - handle_allocation_response(canard_instance, transfer); + handle_allocation_response(ins, transfer); } return; } switch (transfer->data_type_id) { case UAVCAN_PROTOCOL_GETNODEINFO_ID: - handle_get_node_info(canard_instance, transfer); + handle_get_node_info(ins, transfer); break; case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID: - handle_begin_firmware_update(canard_instance, transfer); + handle_begin_firmware_update(ins, transfer); break; case UAVCAN_PROTOCOL_RESTARTNODE_ID: printf("RestartNode\n"); hal.scheduler->delay(10); - prepare_reboot(); + periph.prepare_reboot(); #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS NVIC_SystemReset(); #elif CONFIG_HAL_BOARD == HAL_BOARD_SITL @@ -827,81 +958,65 @@ void AP_Periph_FW::onTransferReceived(CanardInstance* canard_instance, break; case UAVCAN_PROTOCOL_PARAM_GETSET_ID: - handle_param_getset(canard_instance, transfer); + handle_param_getset(ins, transfer); break; case UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID: - handle_param_executeopcode(canard_instance, transfer); + handle_param_executeopcode(ins, transfer); break; #if defined(HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY) || defined (HAL_PERIPH_ENABLE_NOTIFY) case UAVCAN_EQUIPMENT_INDICATION_BEEPCOMMAND_ID: - handle_beep_command(canard_instance, transfer); + handle_beep_command(ins, transfer); break; #endif #if defined(HAL_GPIO_PIN_SAFE_LED) || defined(HAL_PERIPH_ENABLE_RC_OUT) case ARDUPILOT_INDICATION_SAFETYSTATE_ID: - handle_safety_state(canard_instance, transfer); + handle_safety_state(ins, transfer); break; #endif case UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_ID: - handle_arming_status(canard_instance, transfer); + handle_arming_status(ins, transfer); break; #ifdef HAL_PERIPH_ENABLE_GPS case UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_ID: - handle_RTCMStream(canard_instance, transfer); + handle_RTCMStream(ins, transfer); break; #if GPS_MOVING_BASELINE case ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID: - handle_MovingBaselineData(canard_instance, transfer); + handle_MovingBaselineData(ins, transfer); break; #endif -#endif // HAL_PERIPH_ENABLE_GPS - -#if AP_UART_MONITOR_ENABLED - case UAVCAN_TUNNEL_TARGETTED_ID: - handle_tunnel_Targetted(canard_instance, transfer); - break; #endif - + #if defined(AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NOTIFY) case UAVCAN_EQUIPMENT_INDICATION_LIGHTSCOMMAND_ID: - handle_lightscommand(canard_instance, transfer); + handle_lightscommand(ins, transfer); break; #endif #ifdef HAL_PERIPH_ENABLE_RC_OUT case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID: - handle_esc_rawcommand(canard_instance, transfer); + handle_esc_rawcommand(ins, transfer); break; case UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_ID: - handle_act_command(canard_instance, transfer); + handle_act_command(ins, transfer); break; #endif #ifdef HAL_PERIPH_ENABLE_NOTIFY case ARDUPILOT_INDICATION_NOTIFYSTATE_ID: - handle_notify_state(canard_instance, transfer); + handle_notify_state(ins, transfer); break; #endif } } -/** - * This callback is invoked by the library when a new message or request or response is received. - */ -static void onTransferReceived_trampoline(CanardInstance* canard_instance, - CanardRxTransfer* transfer) -{ - AP_Periph_FW *fw = (AP_Periph_FW *)canard_instance->user_reference; - fw->onTransferReceived(canard_instance, transfer); -} - /** * This callback is invoked by the library when it detects beginning of a new transfer on the bus that can be received @@ -910,15 +1025,15 @@ static void onTransferReceived_trampoline(CanardInstance* canard_instance, * If the callback returns false, the library will ignore the transfer. * All transfers that are addressed to other nodes are always ignored. */ -bool AP_Periph_FW::shouldAcceptTransfer(const CanardInstance* canard_instance, - uint64_t* out_data_type_signature, - uint16_t data_type_id, - CanardTransferType transfer_type, - uint8_t source_node_id) +static bool shouldAcceptTransfer(const CanardInstance* ins, + uint64_t* out_data_type_signature, + uint16_t data_type_id, + CanardTransferType transfer_type, + uint8_t source_node_id) { (void)source_node_id; - if (canardGetLocalNodeID(canard_instance) == CANARD_BROADCAST_NODE_ID) + if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID) { /* * If we're in the process of allocation of dynamic node ID, accept only relevant transfers. @@ -976,14 +1091,7 @@ bool AP_Periph_FW::shouldAcceptTransfer(const CanardInstance* canard_instance, *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; @@ -1005,22 +1113,16 @@ bool AP_Periph_FW::shouldAcceptTransfer(const CanardInstance* canard_instance, return false; } -static bool shouldAcceptTransfer_trampoline(const CanardInstance* canard_instance, - uint64_t* out_data_type_signature, - uint16_t data_type_id, - CanardTransferType transfer_type, - uint8_t source_node_id) -{ - AP_Periph_FW *fw = (AP_Periph_FW *)canard_instance->user_reference; - return fw->shouldAcceptTransfer(canard_instance, out_data_type_signature, data_type_id, transfer_type, source_node_id); -} - -void AP_Periph_FW::cleanup_stale_transactions(uint64_t ×tamp_usec) +static void cleanup_stale_transactions(uint64_t ×tamp_usec) { canardCleanupStaleTransfers(&dronecan.canard, timestamp_usec); } -uint8_t *AP_Periph_FW::get_tid_ptr(uint32_t transfer_desc) +#define MAKE_TRANSFER_DESCRIPTOR(data_type_id, transfer_type, src_node_id, dst_node_id) \ + (((uint32_t)(data_type_id)) | (((uint32_t)(transfer_type)) << 16U) | \ + (((uint32_t)(src_node_id)) << 18U) | (((uint32_t)(dst_node_id)) << 25U)) + +static uint8_t* get_tid_ptr(uint32_t transfer_desc) { // check head if (!dronecan.tid_map_head) { @@ -1054,22 +1156,24 @@ uint8_t *AP_Periph_FW::get_tid_ptr(uint32_t transfer_desc) return &tid_map_ptr->next->tid; } -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) +static void 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 false; + return; } 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 false; + return; } - - const int16_t res = canardBroadcast(&dronecan.canard, +#if DEBUG_PKTS + const int16_t res = +#endif + canardBroadcast(&dronecan.canard, data_type_signature, data_type_id, tid_ptr, @@ -1080,7 +1184,7 @@ bool AP_Periph_FW::canard_broadcast(uint64_t data_type_signature, , IFACE_ALL // send over all ifaces #endif #if HAL_CANFD_SUPPORTED - , canfdout() + , periph.canfdout() #endif ); @@ -1089,19 +1193,11 @@ bool AP_Periph_FW::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; } -void AP_Periph_FW::processTx(void) +static void processTx(void) { - for (CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&dronecan.canard)) != NULL;) { + for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&dronecan.canard)) != NULL;) { AP_HAL::CANFrame txmsg {}; txmsg.dlc = AP_HAL::CANFrame::dataLengthToDlc(txf->data_len); memcpy(txmsg.data, txf->data, txf->data_len); @@ -1111,39 +1207,24 @@ void AP_Periph_FW::processTx(void) #endif // push message with 1s timeout bool sent = true; - const uint64_t now_us = AP_HAL::micros64(); - const uint64_t deadline = now_us + 1000000U; + const uint64_t deadline = AP_HAL::native_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<<_ins.index))) { + if (!(txf->iface_mask & (1U<= 2 - if (can_protocol_cached[_ins.index] != AP_CAN::Protocol::DroneCAN) { + if (periph.can_protocol_cached[ins.index] != AP_CANManager::Driver_Type_UAVCAN) { continue; } #endif - if (_ins.iface->send(txmsg, deadline, 0) <= 0) { - /* - We were not able to queue the frame for - sending. Only mark the send as failing if the - interface is active. We consider an interface as - active if it has had a successful transmit in the - last 2 seconds - */ - const auto *stats = _ins.iface->get_statistics(); - if (stats == nullptr || now_us - stats->last_transmit_us < 2000000UL) { - sent = false; - } - } else { -#if CANARD_MULTI_IFACE - txf->iface_mask &= ~(1U<<_ins.index); -#endif + if (ins.iface->send(txmsg, deadline, 0) <= 0) { + sent = false; } } if (sent) { @@ -1155,9 +1236,6 @@ void AP_Periph_FW::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; @@ -1165,52 +1243,7 @@ void AP_Periph_FW::processTx(void) } } -#if HAL_ENABLE_SENDING_STATS -void AP_Periph_FW::update_rx_protocol_stats(int16_t res) -{ - switch (res) { - case CANARD_OK: - protocol_stats.rx_frames++; - break; - case -CANARD_ERROR_OUT_OF_MEMORY: - protocol_stats.rx_error_oom++; - break; - case -CANARD_ERROR_INTERNAL: - protocol_stats.rx_error_internal++; - break; - case -CANARD_ERROR_RX_INCOMPATIBLE_PACKET: - protocol_stats.rx_ignored_not_wanted++; - break; - case -CANARD_ERROR_RX_WRONG_ADDRESS: - protocol_stats.rx_ignored_wrong_address++; - break; - case -CANARD_ERROR_RX_NOT_WANTED: - protocol_stats.rx_ignored_not_wanted++; - break; - case -CANARD_ERROR_RX_MISSED_START: - protocol_stats.rx_error_missed_start++; - break; - case -CANARD_ERROR_RX_WRONG_TOGGLE: - protocol_stats.rx_error_wrong_toggle++; - break; - case -CANARD_ERROR_RX_UNEXPECTED_TID: - protocol_stats.rx_ignored_unexpected_tid++; - break; - case -CANARD_ERROR_RX_SHORT_FRAME: - protocol_stats.rx_error_short_frame++; - break; - case -CANARD_ERROR_RX_BAD_CRC: - protocol_stats.rx_error_bad_crc++; - break; - default: - // mark all other errors as internal - protocol_stats.rx_error_internal++; - break; - } -} -#endif - -void AP_Periph_FW::processRx(void) +static void processRx(void) { AP_HAL::CANFrame rxmsg; for (auto &ins : instances) { @@ -1218,7 +1251,7 @@ void AP_Periph_FW::processRx(void) continue; } #if HAL_NUM_CAN_IFACES >= 2 - if (can_protocol_cached[ins.index] != AP_CAN::Protocol::DroneCAN) { + if (periph.can_protocol_cached[ins.index] != AP_CANManager::Driver_Type_UAVCAN) { continue; } #endif @@ -1237,17 +1270,6 @@ void AP_Periph_FW::processRx(void) if (ins.iface->receive(rxmsg, timestamp, flags) <= 0) { break; } -#if HAL_PERIPH_CAN_MIRROR - for (auto &other_instance : instances) { - if (other_instance.mirror_queue == nullptr) { // we aren't mirroring here, or failed on memory - continue; - } - if (other_instance.index == ins.index) { // don't self add - continue; - } - other_instance.mirror_queue->push(rxmsg); - } -#endif // HAL_PERIPH_CAN_MIRROR rx_frame.data_len = AP_HAL::CANFrame::dlcToDataLength(rxmsg.dlc); memcpy(rx_frame.data, rxmsg.data, rx_frame.data_len); #if HAL_CANFD_SUPPORTED @@ -1257,133 +1279,54 @@ void AP_Periph_FW::processRx(void) #if CANARD_MULTI_IFACE rx_frame.iface_id = ins.index; #endif - - 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++; - } - } else { - update_rx_protocol_stats(res); - } -#else - (void)res; +#if DEBUG_PKTS + const int16_t res = #endif - } - } -} - -#if HAL_PERIPH_CAN_MIRROR -void AP_Periph_FW::processMirror(void) -{ - const uint64_t deadline = AP_HAL::micros64() + 1000000; - - for (auto &ins : instances) { - if (ins.iface == nullptr || ins.mirror_queue == nullptr) { // can't send on a null interface - continue; - } - - const uint32_t pending = ins.mirror_queue->available(); - for (uint32_t i = 0; i < pending; i++) { // limit how long we can loop - AP_HAL::CANFrame txmsg {}; - - if (!ins.mirror_queue->peek(txmsg)) { - break; - } - - if (ins.iface->send(txmsg, deadline, 0) <= 0) { - if (ins.mirror_fail_count < 8) { - ins.mirror_fail_count++; - } else { - ins.mirror_queue->pop(); + 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]); } - break; - } else { - ins.mirror_fail_count = 0; - ins.mirror_queue->pop(); + printf("\n"); } +#endif } } } -#endif // HAL_PERIPH_CAN_MIRROR -uint16_t AP_Periph_FW::pool_peak_percent() +static uint16_t pool_peak_percent() { const CanardPoolAllocatorStatistics stats = canardGetPoolAllocatorStatistics(&dronecan.canard); const uint16_t peak_percent = (uint16_t)(100U * stats.peak_usage_blocks / stats.capacity_blocks); return peak_percent; } -void AP_Periph_FW::node_status_send(void) +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, !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); - } -#if HAL_ENABLE_SENDING_STATS - if (debug_option_is_set(AP_Periph_FW::DebugOptions::ENABLE_STATS)) { - { - uint8_t buffer[DRONECAN_PROTOCOL_STATS_MAX_SIZE]; - uint32_t len = dronecan_protocol_Stats_encode(&protocol_stats, buffer, !canfdout()); - canard_broadcast(DRONECAN_PROTOCOL_STATS_SIGNATURE, - DRONECAN_PROTOCOL_STATS_ID, - CANARD_TRANSFER_PRIORITY_LOWEST, - buffer, - len); - } - for (auto &ins : instances) { - uint8_t buffer[DRONECAN_PROTOCOL_CANSTATS_MAX_SIZE]; - dronecan_protocol_CanStats can_stats; - const AP_HAL::CANIface::bus_stats_t *bus_stats = ins.iface->get_statistics(); - if (bus_stats == nullptr) { - return; - } - can_stats.interface = ins.index; - can_stats.tx_requests = bus_stats->tx_requests; - can_stats.tx_rejected = bus_stats->tx_rejected; - can_stats.tx_overflow = bus_stats->tx_overflow; - can_stats.tx_success = bus_stats->tx_success; - can_stats.tx_timedout = bus_stats->tx_timedout; - can_stats.tx_abort = bus_stats->tx_abort; - can_stats.rx_received = bus_stats->rx_received; - can_stats.rx_overflow = bus_stats->rx_overflow; - can_stats.rx_errors = bus_stats->rx_errors; - can_stats.busoff_errors = bus_stats->num_busoff_err; - uint32_t len = dronecan_protocol_CanStats_encode(&can_stats, buffer, !canfdout()); - canard_broadcast(DRONECAN_PROTOCOL_CANSTATS_SIGNATURE, - DRONECAN_PROTOCOL_CANSTATS_ID, - CANARD_TRANSFER_PRIORITY_LOWEST, - buffer, - len); - } - } -#endif + canard_broadcast(UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, + UAVCAN_PROTOCOL_NODESTATUS_ID, + CANARD_TRANSFER_PRIORITY_LOW, + buffer, + len); } /** * This function is called at 1 Hz rate from the main loop. */ -void AP_Periph_FW::process1HzTasks(uint64_t timestamp_usec) +static void process1HzTasks(uint64_t timestamp_usec) { /* * Purging transfers that are no longer transmitted. This will occasionally free up some memory. @@ -1410,9 +1353,9 @@ void AP_Periph_FW::process1HzTasks(uint64_t timestamp_usec) node_status_send(); #if !defined(HAL_NO_FLASH_SUPPORT) && !defined(HAL_NO_ROMFS_SUPPORT) - if (g.flash_bootloader.get()) { - const uint8_t flash_bl = g.flash_bootloader.get(); - g.flash_bootloader.set_and_save_ifchanged(0); + if (periph.g.flash_bootloader.get()) { + const uint8_t flash_bl = periph.g.flash_bootloader.get(); + periph.g.flash_bootloader.set_and_save_ifchanged(0); if (flash_bl == 42) { // magic developer value to test watchdog support with main loop lockup while (true) { @@ -1461,12 +1404,12 @@ void AP_Periph_FW::process1HzTasks(uint64_t timestamp_usec) #if 0 // test code for watchdog reset - if (AP_HAL::millis() > 15000) { + if (AP_HAL::native_millis() > 15000) { while (true) ; } #endif #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS - if (AP_HAL::millis() > 30000) { + if (AP_HAL::native_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 @@ -1479,15 +1422,14 @@ void AP_Periph_FW::process1HzTasks(uint64_t timestamp_usec) wait for dynamic allocation of node ID */ bool AP_Periph_FW::no_iface_finished_dna = true; - -bool AP_Periph_FW::can_do_dna() +static bool can_do_dna() { if (canardGetLocalNodeID(&dronecan.canard) != CANARD_BROADCAST_NODE_ID) { AP_Periph_FW::no_iface_finished_dna = false; return true; } - const uint32_t now = AP_HAL::millis(); + const uint32_t now = AP_HAL::native_millis(); static uint8_t node_id_allocation_transfer_id = 0; @@ -1535,7 +1477,6 @@ bool AP_Periph_FW::can_do_dna() ,(1U << dronecan.dna_interface) #endif #if HAL_CANFD_SUPPORTED - // always send allocation request as non-FD ,false #endif ); @@ -1552,25 +1493,25 @@ 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::millis() / 1000U; + node_status.uptime_sec = AP_HAL::native_millis() / 1000U; if (g.can_node >= 0 && g.can_node < 128) { PreferredNodeID = g.can_node; } #if !defined(HAL_NO_FLASH_SUPPORT) && !defined(HAL_NO_ROMFS_SUPPORT) - g.flash_bootloader.set_and_save_ifchanged(0); + periph.g.flash_bootloader.set_and_save_ifchanged(0); #endif #if AP_PERIPH_ENFORCE_AT_LEAST_ONE_PORT_IS_UAVCAN_1MHz && HAL_NUM_CAN_IFACES >= 2 bool has_uavcan_at_1MHz = false; for (uint8_t i=0; i (HAL_PERIPH_CAN_MIRROR_QUEUE_SIZE); - } -#endif //HAL_PERIPH_CAN_MIRROR #if HAL_NUM_CAN_IFACES >= 2 can_protocol_cached[i] = g.can_protocol[i]; CANSensor::set_periph(i, can_protocol_cached[i], can_iface_periph[i]); @@ -1608,20 +1544,100 @@ void AP_Periph_FW::can_start() instances[slcan_selected_index].iface = (AP_HAL::CANIface*)&slcan_interface; // ensure there's a serial port mapped to SLCAN - if (!serial_manager.have_serial(AP_SerialManager::SerialProtocol_SLCAN, 0)) { - serial_manager.set_protocol_and_baud(SERIALMANAGER_NUM_PORTS-1, AP_SerialManager::SerialProtocol_SLCAN, 1500000); + if (!periph.serial_manager.have_serial(AP_SerialManager::SerialProtocol_SLCAN, 0)) { + periph.serial_manager.set_protocol_and_baud(SERIALMANAGER_NUM_PORTS-1, AP_SerialManager::SerialProtocol_SLCAN, 1500000); } } #endif canardInit(&dronecan.canard, (uint8_t *)dronecan.canard_memory_pool, sizeof(dronecan.canard_memory_pool), - onTransferReceived_trampoline, shouldAcceptTransfer_trampoline, this); + onTransferReceived, shouldAcceptTransfer, NULL); if (PreferredNodeID != CANARD_BROADCAST_NODE_ID) { canardSetLocalNodeID(&dronecan.canard, PreferredNodeID); } } +#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT +void AP_Periph_FW::pwm_hardpoint_init() +{ + hal.gpio->attach_interrupt( + PWM_HARDPOINT_PIN, + FUNCTOR_BIND_MEMBER(&AP_Periph_FW::pwm_irq_handler, void, uint8_t, bool, uint32_t), AP_HAL::GPIO::INTERRUPT_BOTH); + +} + +/* + called on PWM pin transition + */ +void AP_Periph_FW::pwm_irq_handler(uint8_t pin, bool pin_state, uint32_t timestamp) +{ + if (pin_state == 0 && pwm_hardpoint.last_state == 1 && pwm_hardpoint.last_ts_us != 0) { + uint32_t width = timestamp - pwm_hardpoint.last_ts_us; + if (width > 500 && width < 2500) { + pwm_hardpoint.pwm_value = width; + if (width > pwm_hardpoint.highest_pwm) { + pwm_hardpoint.highest_pwm = width; + } + } + } + pwm_hardpoint.last_state = pin_state; + pwm_hardpoint.last_ts_us = timestamp; +} + +void AP_Periph_FW::pwm_hardpoint_update() +{ + uint32_t now = AP_HAL::native_millis(); + // send at 10Hz + void *save = hal.scheduler->disable_interrupts_save(); + uint16_t value = pwm_hardpoint.highest_pwm; + pwm_hardpoint.highest_pwm = 0; + hal.scheduler->restore_interrupts(save); + float rate = g.hardpoint_rate; + rate = constrain_float(rate, 10, 100); + if (value > 0 && now - pwm_hardpoint.last_send_ms >= 1000U/rate) { + pwm_hardpoint.last_send_ms = now; + uavcan_equipment_hardpoint_Command cmd {}; + cmd.hardpoint_id = g.hardpoint_id; + cmd.command = value; + + uint8_t buffer[UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_MAX_SIZE] {}; + uint16_t total_size = uavcan_equipment_hardpoint_Command_encode(&cmd, buffer, !periph.canfdout()); + canard_broadcast(UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_SIGNATURE, + UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); + } +} +#endif // HAL_PERIPH_ENABLE_PWM_HARDPOINT + +#ifdef HAL_PERIPH_ENABLE_HWESC +void AP_Periph_FW::hwesc_telem_update() +{ + if (!hwesc_telem.update()) { + return; + } + const HWESC_Telem::HWESC &t = hwesc_telem.get_telem(); + + uavcan_equipment_esc_Status pkt {}; + pkt.esc_index = g.esc_number; + pkt.voltage = t.voltage; + pkt.current = t.current; + pkt.temperature = C_TO_KELVIN(MAX(t.mos_temperature, t.cap_temperature)); + pkt.rpm = t.rpm; + pkt.power_rating_pct = t.phase_current; + pkt.error_count = t.error_count; + + uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {}; + uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer, !periph.canfdout()); + canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, + UAVCAN_EQUIPMENT_ESC_STATUS_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); +} +#endif // HAL_PERIPH_ENABLE_HWESC #ifdef HAL_PERIPH_ENABLE_RC_OUT #if HAL_WITH_ESC_TELEM @@ -1667,7 +1683,7 @@ void AP_Periph_FW::esc_telem_update() pkt.error_count = 0; uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer, !canfdout()); + 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, @@ -1676,46 +1692,12 @@ void AP_Periph_FW::esc_telem_update() } } #endif // HAL_WITH_ESC_TELEM - -#ifdef HAL_PERIPH_ENABLE_ESC_APD -void AP_Periph_FW::apd_esc_telem_update() -{ - for(uint8_t i = 0; i < ARRAY_SIZE(apd_esc_telem); i++) { - if (apd_esc_telem[i] == nullptr) { - continue; - } - ESC_APD_Telem &esc = *apd_esc_telem[i]; - - if (esc.update()) { - const ESC_APD_Telem::telem &t = esc.get_telem(); - - uavcan_equipment_esc_Status pkt {}; - static_assert(APD_ESC_INSTANCES <= ARRAY_SIZE(g.esc_number), "There must be an ESC instance number for each APD ESC"); - pkt.esc_index = g.esc_number[i]; - pkt.voltage = t.voltage; - pkt.current = t.current; - pkt.temperature = t.temperature; - pkt.rpm = t.rpm; - pkt.power_rating_pct = t.power_rating_pct; - pkt.error_count = t.error_count; - - uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer, !canfdout()); - canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, - UAVCAN_EQUIPMENT_ESC_STATUS_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); - } - } - -} -#endif // HAL_PERIPH_ENABLE_ESC_APD #endif // HAL_PERIPH_ENABLE_RC_OUT + void AP_Periph_FW::can_update() { - const uint32_t now = AP_HAL::millis(); + const uint32_t now = AP_HAL::native_millis(); const uint32_t led_pattern = 0xAAAA; const uint32_t led_change_period = 50; static uint8_t led_idx = 0; @@ -1743,37 +1725,20 @@ 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::micros64()); + process1HzTasks(AP_HAL::native_micros64()); } #if CONFIG_HAL_BOARD == HAL_BOARD_SITL if (!hal.run_in_maintenance_mode()) #endif { -#ifdef HAL_PERIPH_ENABLE_MAG can_mag_update(); -#endif -#ifdef HAL_PERIPH_ENABLE_GPS can_gps_update(); -#endif -#if AP_UART_MONITOR_ENABLED - send_serial_monitor_data(); -#endif -#ifdef HAL_PERIPH_ENABLE_BATTERY can_battery_update(); -#endif -#ifdef HAL_PERIPH_ENABLE_BARO can_baro_update(); -#endif -#ifdef HAL_PERIPH_ENABLE_AIRSPEED can_airspeed_update(); -#endif -#ifdef HAL_PERIPH_ENABLE_RANGEFINDER can_rangefinder_update(); -#endif -#ifdef HAL_PERIPH_ENABLE_PROXIMITY can_proximity_update(); -#endif #if defined(HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY) || defined (HAL_PERIPH_ENABLE_NOTIFY) can_buzzer_update(); #endif @@ -1789,9 +1754,6 @@ 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 @@ -1805,20 +1767,920 @@ void AP_Periph_FW::can_update() const uint32_t now_us = AP_HAL::micros(); while ((AP_HAL::micros() - now_us) < 1000) { hal.scheduler->delay_microseconds(HAL_PERIPH_LOOP_DELAY_US); + processTx(); + processRx(); + } +} -#if HAL_CANFD_SUPPORTED - // allow for user enabling/disabling CANFD at runtime - dronecan.canard.tao_disabled = g.can_fdmode == 1; +/* + update CAN magnetometer + */ +void AP_Periph_FW::can_mag_update(void) +{ +#ifdef HAL_PERIPH_ENABLE_MAG + if (!compass.available()) { + return; + } + +#if AP_PERIPH_MAG_MAX_RATE > 0 + // don't flood the bus with very high rate magnetometers + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - last_mag_update_ms < (1000U / AP_PERIPH_MAG_MAX_RATE)) { + return; + } #endif - processTx(); - processRx(); -#if HAL_PERIPH_CAN_MIRROR - processMirror(); -#endif // HAL_PERIPH_CAN_MIRROR + compass.read(); +#if AP_PERIPH_PROBE_CONTINUOUS + if (compass.get_count() == 0) { + static uint32_t last_probe_ms; + uint32_t now = AP_HAL::native_millis(); + if (now - last_probe_ms >= 1000) { + last_probe_ms = now; + compass.init(); + } + } +#endif + + if (last_mag_update_ms == compass.last_update_ms()) { + return; + } + if (!compass.healthy()) { + return; + } + + last_mag_update_ms = compass.last_update_ms(); + const Vector3f &field = compass.get_field(); + uavcan_equipment_ahrs_MagneticFieldStrength pkt {}; + + // the canard dsdl compiler doesn't understand float16 + for (uint8_t i=0; i<3; i++) { + pkt.magnetic_field_ga[i] = field[i] * 0.001; } + + uint8_t buffer[UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_MAX_SIZE] {}; + uint16_t total_size = uavcan_equipment_ahrs_MagneticFieldStrength_encode(&pkt, buffer, !periph.canfdout()); + + canard_broadcast(UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_SIGNATURE, + UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); +#endif // HAL_PERIPH_ENABLE_MAG } +/* + update CAN battery monitor + */ +void AP_Periph_FW::can_battery_update(void) +{ +#ifdef HAL_PERIPH_ENABLE_BATTERY + const uint32_t now_ms = AP_HAL::native_millis(); + if (now_ms - battery.last_can_send_ms < 100) { + return; + } + battery.last_can_send_ms = now_ms; + + const uint8_t battery_instances = battery.lib.num_instances(); + for (uint8_t i=0; i= 0) ? serial_number : i+1; + + pkt.voltage = battery.lib.voltage(i); + + float current; + if (battery.lib.current_amps(current, i)) { + pkt.current = current; + } + float temperature; + if (battery.lib.get_temperature(temperature, i)) { + // Battery lib reports temperature in Celsius. + // Convert Celsius to Kelvin for transmission on CAN. + pkt.temperature = C_TO_KELVIN(temperature); + } + + pkt.state_of_health_pct = UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATE_OF_HEALTH_UNKNOWN; + uint8_t percentage = 0; + if (battery.lib.capacity_remaining_pct(percentage, i)) { + pkt.state_of_charge_pct = percentage; + } + pkt.model_instance_id = i+1; + +#if !defined(HAL_PERIPH_BATTERY_SKIP_NAME) + // example model_name: "org.ardupilot.ap_periph SN 123" + hal.util->snprintf((char*)pkt.model_name.data, sizeof(pkt.model_name.data), "%s %ld", AP_PERIPH_BATTERY_MODEL_NAME, (long int)serial_number); + pkt.model_name.len = strnlen((char*)pkt.model_name.data, sizeof(pkt.model_name.data)); +#endif //defined(HAL_PERIPH_BATTERY_SKIP_NAME) + + uint8_t buffer[UAVCAN_EQUIPMENT_POWER_BATTERYINFO_MAX_SIZE] {}; + const uint16_t total_size = uavcan_equipment_power_BatteryInfo_encode(&pkt, buffer, !periph.canfdout()); + + canard_broadcast(UAVCAN_EQUIPMENT_POWER_BATTERYINFO_SIGNATURE, + UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); + } +#endif +} + +#ifdef HAL_PERIPH_ENABLE_GPS +/* + convert large values to NaN for float16 + */ +static void check_float16_range(float *v, uint8_t len) +{ + for (uint8_t i=0; i= f16max) { + v[i] = nanf(""); + } + } +} +#endif + +/* + update CAN GPS + */ +void AP_Periph_FW::can_gps_update(void) +{ +#ifdef HAL_PERIPH_ENABLE_GPS + if (gps.get_type(0) == AP_GPS::GPS_Type::GPS_TYPE_NONE) { + return; + } + gps.update(); + send_moving_baseline_msg(); + send_relposheading_msg(); + if (last_gps_update_ms == gps.last_message_time_ms()) { + return; + } + last_gps_update_ms = gps.last_message_time_ms(); + + { + /* + send Fix2 packet + */ + uavcan_equipment_gnss_Fix2 pkt {}; + const Location &loc = gps.location(); + const Vector3f &vel = gps.velocity(); + if (gps.status() < AP_GPS::GPS_OK_FIX_2D && !saw_gps_lock_once) { + pkt.timestamp.usec = AP_HAL::micros64(); + pkt.gnss_timestamp.usec = 0; + } else { + saw_gps_lock_once = true; + pkt.timestamp.usec = gps.time_epoch_usec(); + pkt.gnss_timestamp.usec = gps.last_message_epoch_usec(); + } + if (pkt.gnss_timestamp.usec == 0) { + pkt.gnss_time_standard = UAVCAN_EQUIPMENT_GNSS_FIX_GNSS_TIME_STANDARD_NONE; + } else { + pkt.gnss_time_standard = UAVCAN_EQUIPMENT_GNSS_FIX_GNSS_TIME_STANDARD_UTC; + } + pkt.longitude_deg_1e8 = uint64_t(loc.lng) * 10ULL; + pkt.latitude_deg_1e8 = uint64_t(loc.lat) * 10ULL; + pkt.height_ellipsoid_mm = loc.alt * 10; + pkt.height_msl_mm = loc.alt * 10; + float undulation; + if (gps.get_undulation(undulation)) { + pkt.height_ellipsoid_mm -= undulation*1000; + } + for (uint8_t i=0; i<3; i++) { + pkt.ned_velocity[i] = vel[i]; + } + pkt.sats_used = gps.num_sats(); + switch (gps.status()) { + case AP_GPS::GPS_Status::NO_GPS: + case AP_GPS::GPS_Status::NO_FIX: + pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_NO_FIX; + pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE; + pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_2D: + pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_2D_FIX; + pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE; + pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_3D: + pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX; + pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE; + pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS: + pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX; + pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_DGPS; + pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_SBAS; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT: + pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX; + pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_RTK; + pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FLOAT; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED: + pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX; + pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_RTK; + pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FIXED; + break; + } + + pkt.covariance.len = 6; + + float hacc; + if (gps.horizontal_accuracy(hacc)) { + pkt.covariance.data[0] = pkt.covariance.data[1] = sq(hacc); + } + + float vacc; + if (gps.vertical_accuracy(vacc)) { + pkt.covariance.data[2] = sq(vacc); + } + + float sacc; + if (gps.speed_accuracy(sacc)) { + float vc3 = sq(sacc); + pkt.covariance.data[3] = pkt.covariance.data[4] = pkt.covariance.data[5] = vc3; + } + + check_float16_range(pkt.covariance.data, pkt.covariance.len); + + uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_FIX2_MAX_SIZE] {}; + uint16_t total_size = uavcan_equipment_gnss_Fix2_encode(&pkt, buffer, !periph.canfdout()); + + canard_broadcast(UAVCAN_EQUIPMENT_GNSS_FIX2_SIGNATURE, + UAVCAN_EQUIPMENT_GNSS_FIX2_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); + } + + /* + send aux packet + */ + { + uavcan_equipment_gnss_Auxiliary aux {}; + aux.hdop = gps.get_hdop() * 0.01; + aux.vdop = gps.get_vdop() * 0.01; + + uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_AUXILIARY_MAX_SIZE] {}; + uint16_t total_size = uavcan_equipment_gnss_Auxiliary_encode(&aux, buffer, !periph.canfdout()); + canard_broadcast(UAVCAN_EQUIPMENT_GNSS_AUXILIARY_SIGNATURE, + UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); + } + + // send the gnss status packet + { + ardupilot_gnss_Status status {}; + + status.healthy = gps.is_healthy(); + if (gps.logging_present() && gps.logging_enabled() && !gps.logging_failed()) { + status.status |= ARDUPILOT_GNSS_STATUS_STATUS_LOGGING; + } + uint8_t idx; // unused + if (status.healthy && !gps.first_unconfigured_gps(idx)) { + status.status |= ARDUPILOT_GNSS_STATUS_STATUS_ARMABLE; + } + + uint32_t error_codes; + if (gps.get_error_codes(error_codes)) { + status.error_codes = error_codes; + } + + uint8_t buffer[ARDUPILOT_GNSS_STATUS_MAX_SIZE] {}; + const uint16_t total_size = ardupilot_gnss_Status_encode(&status, buffer, !periph.canfdout()); + canard_broadcast(ARDUPILOT_GNSS_STATUS_SIGNATURE, + ARDUPILOT_GNSS_STATUS_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); + + } + + // send Heading message if we are not sending RelPosHeading messages and have yaw + if (gps.have_gps_yaw() && last_relposheading_ms == 0) { + float yaw_deg, yaw_acc_deg; + uint32_t yaw_time_ms; + if (gps.gps_yaw_deg(yaw_deg, yaw_acc_deg, yaw_time_ms) && yaw_time_ms != last_gps_yaw_ms) { + last_gps_yaw_ms = yaw_time_ms; + + ardupilot_gnss_Heading heading {}; + heading.heading_valid = true; + heading.heading_accuracy_valid = is_positive(yaw_acc_deg); + heading.heading_rad = radians(yaw_deg); + heading.heading_accuracy_rad = radians(yaw_acc_deg); + uint8_t buffer[ARDUPILOT_GNSS_HEADING_MAX_SIZE] {}; + const uint16_t total_size = ardupilot_gnss_Heading_encode(&heading, buffer, !periph.canfdout()); + canard_broadcast(ARDUPILOT_GNSS_HEADING_SIGNATURE, + ARDUPILOT_GNSS_HEADING_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); + } + } +#endif // HAL_PERIPH_ENABLE_GPS +} + + +void AP_Periph_FW::send_moving_baseline_msg() +{ +#if defined(HAL_PERIPH_ENABLE_GPS) && GPS_MOVING_BASELINE + const uint8_t *data = nullptr; + uint16_t len = 0; + if (!gps.get_RTCMV3(data, len)) { + return; + } + if (len == 0 || data == nullptr) { + return; + } + // send the packet from Moving Base to be used RelPosHeading calc by GPS module + ardupilot_gnss_MovingBaselineData mbldata {}; + // get the data from the moving base + static_assert(sizeof(ardupilot_gnss_MovingBaselineData::data.data) == RTCM3_MAX_PACKET_LEN, "Size of Moving Base data is wrong"); + mbldata.data.len = len; + memcpy(mbldata.data.data, data, len); + uint8_t buffer[ARDUPILOT_GNSS_MOVINGBASELINEDATA_MAX_SIZE] {}; + const uint16_t total_size = ardupilot_gnss_MovingBaselineData_encode(&mbldata, buffer, !periph.canfdout()); + +#if HAL_NUM_CAN_IFACES >= 2 + if (gps_mb_can_port != -1 && (gps_mb_can_port < HAL_NUM_CAN_IFACES)) { + uint8_t *tid_ptr = get_tid_ptr(MAKE_TRANSFER_DESCRIPTOR(ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE, ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID, 0, CANARD_BROADCAST_NODE_ID)); + canardBroadcast(&dronecan.canard, + ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE, + ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID, + tid_ptr, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size +#if CANARD_MULTI_IFACE + ,(1U<= 1000) { + last_probe_ms = now; + airspeed.allocate(); + } + } +#endif + uint32_t now = AP_HAL::native_millis(); + if (now - last_airspeed_update_ms < 50) { + // max 20Hz data + return; + } + last_airspeed_update_ms = now; + airspeed.update(); + if (!airspeed.healthy()) { + // don't send any data + return; + } + const float press = airspeed.get_corrected_pressure(); + float temp; + if (!airspeed.get_temperature(temp)) { + temp = nanf(""); + } else { + temp = C_TO_KELVIN(temp); + } + + uavcan_equipment_air_data_RawAirData pkt {}; + pkt.differential_pressure = press; + pkt.static_air_temperature = temp; + + // unfilled elements are NaN + pkt.static_pressure = nanf(""); + pkt.static_pressure_sensor_temperature = nanf(""); + pkt.differential_pressure_sensor_temperature = nanf(""); + pkt.pitot_temperature = nanf(""); + + uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_MAX_SIZE] {}; + uint16_t total_size = uavcan_equipment_air_data_RawAirData_encode(&pkt, buffer, !periph.canfdout()); + + canard_broadcast(UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_SIGNATURE, + UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); +#endif // HAL_PERIPH_ENABLE_AIRSPEED +} + + +/* + update CAN rangefinder + */ +void AP_Periph_FW::can_rangefinder_update(void) +{ +#ifdef HAL_PERIPH_ENABLE_RANGEFINDER + if (rangefinder.get_type(0) == RangeFinder::Type::NONE) { + return; + } +#if AP_PERIPH_PROBE_CONTINUOUS + if (rangefinder.num_sensors() == 0) { + uint32_t now = AP_HAL::native_millis(); + static uint32_t last_probe_ms; + if (now - last_probe_ms >= 1000) { + last_probe_ms = now; + rangefinder.init(ROTATION_NONE); + } + } +#endif + uint32_t now = AP_HAL::native_millis(); + static uint32_t last_update_ms; + if (g.rangefinder_max_rate > 0 && + now - last_update_ms < 1000/g.rangefinder_max_rate) { + // limit to max rate + return; + } + last_update_ms = now; + rangefinder.update(); + RangeFinder::Status status = rangefinder.status_orient(ROTATION_NONE); + if (status <= RangeFinder::Status::NoData) { + // don't send any data + return; + } + const uint32_t sample_ms = rangefinder.last_reading_ms(ROTATION_NONE); + if (last_sample_ms == sample_ms) { + return; + } + last_sample_ms = sample_ms; + + uint16_t dist_cm = rangefinder.distance_cm_orient(ROTATION_NONE); + uavcan_equipment_range_sensor_Measurement pkt {}; + pkt.sensor_id = rangefinder.get_address(0); + switch (status) { + case RangeFinder::Status::OutOfRangeLow: + pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_CLOSE; + break; + case RangeFinder::Status::OutOfRangeHigh: + pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_FAR; + break; + case RangeFinder::Status::Good: + pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_VALID_RANGE; + break; + default: + pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_UNDEFINED; + break; + } + switch (rangefinder.get_mav_distance_sensor_type_orient(ROTATION_NONE)) { + case MAV_DISTANCE_SENSOR_LASER: + pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_LIDAR; + break; + case MAV_DISTANCE_SENSOR_ULTRASOUND: + pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_SONAR; + break; + case MAV_DISTANCE_SENSOR_RADAR: + pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_RADAR; + break; + default: + pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_UNDEFINED; + break; + } + + pkt.range = dist_cm * 0.01; + + uint8_t buffer[UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE] {}; + uint16_t total_size = uavcan_equipment_range_sensor_Measurement_encode(&pkt, buffer, !periph.canfdout()); + + canard_broadcast(UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE, + UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); +#endif // HAL_PERIPH_ENABLE_RANGEFINDER +} + + +void AP_Periph_FW::can_proximity_update() +{ +#ifdef HAL_PERIPH_ENABLE_PRX + if (proximity.get_type(0) == AP_Proximity::Type::None) { + return; + } + + uint32_t now = AP_HAL::native_millis(); + static uint32_t last_update_ms; + if (g.proximity_max_rate > 0 && + now - last_update_ms < 1000/g.proximity_max_rate) { + // limit to max rate + return; + } + last_update_ms = now; + proximity.update(); + AP_Proximity::Status status = proximity.get_status(); + if (status <= AP_Proximity::Status::NoData) { + // don't send any data + return; + } + + ardupilot_equipment_proximity_sensor_Proximity pkt {}; + + const uint8_t obstacle_count = proximity.get_obstacle_count(); + + // if no objects return + if (obstacle_count == 0) { + return; + } + + // calculate maximum roll, pitch values from objects + for (uint8_t i=0; i - -#ifndef AP_PERIPH_MAG_MAX_RATE -#define AP_PERIPH_MAG_MAX_RATE 25U -#endif - -#ifndef AP_PERIPH_PROBE_CONTINUOUS -#define AP_PERIPH_PROBE_CONTINUOUS 0 -#endif - -/* - update CAN magnetometer - */ -void AP_Periph_FW::can_mag_update(void) -{ - if (!compass.available()) { - return; - } - -#if AP_PERIPH_MAG_MAX_RATE > 0 - // don't flood the bus with very high rate magnetometers - const uint32_t now_ms = AP_HAL::millis(); - if (now_ms - last_mag_update_ms < (1000U / AP_PERIPH_MAG_MAX_RATE)) { - return; - } -#endif - - compass.read(); - -#if AP_PERIPH_PROBE_CONTINUOUS - if (compass.get_count() == 0) { - static uint32_t last_probe_ms; - uint32_t now = AP_HAL::millis(); - if (now - last_probe_ms >= 1000) { - last_probe_ms = now; - compass.init(); - } - } -#endif - - if (last_mag_update_ms == compass.last_update_ms()) { - return; - } - if (!compass.healthy()) { - return; - } - - last_mag_update_ms = compass.last_update_ms(); - const Vector3f &field = compass.get_field(); - uavcan_equipment_ahrs_MagneticFieldStrength pkt {}; - - // the canard dsdl compiler doesn't understand float16 - for (uint8_t i=0; i<3; i++) { - pkt.magnetic_field_ga[i] = field[i] * 0.001; - } - - uint8_t buffer[UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_ahrs_MagneticFieldStrength_encode(&pkt, buffer, !periph.canfdout()); - - canard_broadcast(UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_SIGNATURE, - UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); -} - -#endif // HAL_PERIPH_ENABLE_MAG diff --git a/Tools/AP_Periph/efi.cpp b/Tools/AP_Periph/efi.cpp deleted file mode 100644 index d1bb114cf633d7..00000000000000 --- a/Tools/AP_Periph/efi.cpp +++ /dev/null @@ -1,207 +0,0 @@ -#include "AP_Periph.h" - -#ifdef HAL_PERIPH_ENABLE_EFI - -/* - EFI support - */ - -#include - -#ifndef AP_PERIPH_EFI_MAX_RATE -// default to 2x the AP_Vehicle rate -#define AP_PERIPH_EFI_MAX_RATE 100U -#endif - -/* - update CAN EFI - */ -void AP_Periph_FW::can_efi_update(void) -{ - if (!efi.enabled()) { - return; - } - -#if AP_PERIPH_EFI_MAX_RATE > 0 - const uint32_t now_ms = AP_HAL::millis(); - if (now_ms - last_efi_update_ms < (1000U / AP_PERIPH_EFI_MAX_RATE)) { - return; - } - last_efi_update_ms = now_ms; -#endif - - efi.update(); - const uint32_t update_ms = efi.get_last_update_ms(); - if (!efi.is_healthy() || efi_update_ms == update_ms) { - return; - } - efi_update_ms = update_ms; - EFI_State state; - efi.get_state(state); - - { - /* - send status packet - */ - uavcan_equipment_ice_reciprocating_Status pkt {}; - - // state maps 1:1 from Engine_State - pkt.state = uint8_t(state.engine_state); - - switch (state.crankshaft_sensor_status) { - case Crankshaft_Sensor_Status::NOT_SUPPORTED: - break; - case Crankshaft_Sensor_Status::OK: - pkt.flags |= UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_CRANKSHAFT_SENSOR_ERROR_SUPPORTED; - break; - case Crankshaft_Sensor_Status::ERROR: - pkt.flags |= - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_CRANKSHAFT_SENSOR_ERROR_SUPPORTED | - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_CRANKSHAFT_SENSOR_ERROR; - break; - } - - switch (state.temperature_status) { - case Temperature_Status::NOT_SUPPORTED: - break; - case Temperature_Status::OK: - pkt.flags |= UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_SUPPORTED; - break; - case Temperature_Status::BELOW_NOMINAL: - pkt.flags |= - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_SUPPORTED | - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_BELOW_NOMINAL; - break; - case Temperature_Status::ABOVE_NOMINAL: - pkt.flags |= - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_SUPPORTED | - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_ABOVE_NOMINAL; - break; - case Temperature_Status::OVERHEATING: - pkt.flags |= - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_SUPPORTED | - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_OVERHEATING; - break; - case Temperature_Status::EGT_ABOVE_NOMINAL: - pkt.flags |= - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_SUPPORTED | - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_EGT_ABOVE_NOMINAL; - break; - } - - switch (state.fuel_pressure_status) { - case Fuel_Pressure_Status::NOT_SUPPORTED: - break; - case Fuel_Pressure_Status::OK: - pkt.flags |= UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_SUPPORTED; - break; - case Fuel_Pressure_Status::BELOW_NOMINAL: - pkt.flags |= - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_SUPPORTED | - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_BELOW_NOMINAL; - break; - case Fuel_Pressure_Status::ABOVE_NOMINAL: - pkt.flags |= - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_SUPPORTED | - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_ABOVE_NOMINAL; - break; - } - - switch (state.oil_pressure_status) { - case Oil_Pressure_Status::NOT_SUPPORTED: - break; - case Oil_Pressure_Status::OK: - pkt.flags |= UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_SUPPORTED; - break; - case Oil_Pressure_Status::BELOW_NOMINAL: - pkt.flags |= - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_SUPPORTED | - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_BELOW_NOMINAL; - break; - case Oil_Pressure_Status::ABOVE_NOMINAL: - pkt.flags |= - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_SUPPORTED | - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_ABOVE_NOMINAL; - break; - } - - switch (state.detonation_status) { - case Detonation_Status::NOT_SUPPORTED: - break; - case Detonation_Status::NOT_OBSERVED: - pkt.flags |= - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DETONATION_SUPPORTED; - break; - case Detonation_Status::OBSERVED: - pkt.flags |= - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DETONATION_SUPPORTED | - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DETONATION_OBSERVED; - break; - } - - switch (state.misfire_status) { - case Misfire_Status::NOT_SUPPORTED: - break; - case Misfire_Status::NOT_OBSERVED: - pkt.flags |= - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_MISFIRE_SUPPORTED; - break; - case Misfire_Status::OBSERVED: - pkt.flags |= - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_MISFIRE_SUPPORTED | - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_MISFIRE_OBSERVED; - break; - } - - switch (state.debris_status) { - case Debris_Status::NOT_SUPPORTED: - break; - case Debris_Status::NOT_DETECTED: - pkt.flags |= - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DEBRIS_SUPPORTED; - break; - case Debris_Status::DETECTED: - pkt.flags |= - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DEBRIS_SUPPORTED | - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DEBRIS_DETECTED; - break; - } - - pkt.engine_load_percent = state.engine_load_percent; - pkt.engine_speed_rpm = state.engine_speed_rpm; - pkt.spark_dwell_time_ms = state.spark_dwell_time_ms; - pkt.atmospheric_pressure_kpa = state.atmospheric_pressure_kpa; - pkt.intake_manifold_pressure_kpa = state.intake_manifold_pressure_kpa; - pkt.intake_manifold_temperature = state.intake_manifold_temperature; - pkt.coolant_temperature = state.coolant_temperature; - pkt.oil_pressure = state.oil_pressure; - pkt.oil_temperature = state.oil_temperature; - pkt.fuel_pressure = state.fuel_pressure; - pkt.fuel_consumption_rate_cm3pm = state.fuel_consumption_rate_cm3pm; - pkt.estimated_consumed_fuel_volume_cm3 = state.estimated_consumed_fuel_volume_cm3; - pkt.throttle_position_percent = state.throttle_position_percent; - pkt.ecu_index = state.ecu_index; - pkt.spark_plug_usage = uint8_t(state.spark_plug_usage); - - // assume single set of cylinder status - pkt.cylinder_status.len = 1; - auto &c = pkt.cylinder_status.data[0]; - const auto &state_c = state.cylinder_status; - c.ignition_timing_deg = state_c.ignition_timing_deg; - c.injection_time_ms = state_c.injection_time_ms; - c.cylinder_head_temperature = state_c.cylinder_head_temperature; - c.exhaust_gas_temperature = state_c.exhaust_gas_temperature; - c.lambda_coefficient = state_c.lambda_coefficient; - - uint8_t buffer[UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_MAX_SIZE] {}; - const uint16_t total_size = uavcan_equipment_ice_reciprocating_Status_encode(&pkt, buffer, !canfdout()); - - canard_broadcast(UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_SIGNATURE, - UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); - } -} - -#endif // HAL_PERIPH_ENABLE_EFI diff --git a/Tools/AP_Periph/esc_apd_telem.cpp b/Tools/AP_Periph/esc_apd_telem.cpp deleted file mode 100644 index ed438d60f36c5d..00000000000000 --- a/Tools/AP_Periph/esc_apd_telem.cpp +++ /dev/null @@ -1,97 +0,0 @@ -/* - ESC Telemetry for the APD HV Pro ESC - - Protocol is here: https://docs.powerdrives.net/products/hv_pro/uart-telemetry-output - */ -#include "esc_apd_telem.h" -#include -#include -#include -#include - -#ifdef HAL_PERIPH_ENABLE_ESC_APD - -extern const AP_HAL::HAL& hal; - -#define TELEM_HEADER 0x9B -#define TELEM_LEN 0x16 - -ESC_APD_Telem::ESC_APD_Telem (AP_HAL::UARTDriver *_uart, float num_poles) : - pole_count(num_poles), - uart(_uart) { - uart->begin(115200); -} - -bool ESC_APD_Telem::update() { - uint32_t n = uart->available(); - if (n == 0) { - return false; - } - - // don't read too much in one loop to prevent too high CPU load - if (n > 50) { - n = 50; - } - - bool ret = false; - - while (n--) { - uint8_t b = uart->read(); - received.bytes[len++] = b; - - // check the packet size first - if ((size_t)len >= sizeof(received.packet)) { - // we have a full packet, check the stop byte - if (received.packet.stop == 65535) { - // valid stop byte, check the CRC - if (crc_fletcher16(received.bytes, 18) == received.packet.checksum) { - // valid packet, copy the data we need and reset length - decoded.voltage = le16toh(received.packet.voltage) * 1e-2f; - decoded.temperature = convert_temperature(le16toh(received.packet.temperature)); - decoded.current = ((int16_t)le16toh(received.packet.bus_current)) * (1 / 12.5f); - decoded.rpm = le32toh(received.packet.erpm) / pole_count; - decoded.power_rating_pct = le16toh(received.packet.motor_duty) * 1e-2f; - ret = true; - len = 0; - } else { - // we have an invalid packet, shift it back a byte - shift_buffer(); - } - } else { - // invalid stop byte, we've lost sync, shift the packet by 1 byte - shift_buffer(); - } - - } - } - return ret; -} - -// shift the decode buffer left by 1 byte, and rewind the progress -void ESC_APD_Telem::shift_buffer(void) { - memmove(received.bytes, received.bytes + 1, sizeof(received.bytes) - 1); - len--; -} - -// convert the raw ESC temperature to a useful value (in Kelvin) -// based on the 1.1 example C code found here https://docs.powerdrives.net/products/hv_pro/uart-telemetry-output -float ESC_APD_Telem::convert_temperature(uint16_t raw) const { - const float series_resistor = 10000; - const float nominal_resistance = 10000; - const float nominal_temperature = 25; - const float b_coefficent = 3455; - - - const float Rntc = series_resistor / ((4096 / float(raw)) - 1); - - float temperature = Rntc / nominal_resistance; // (R/Ro) - temperature = logf(temperature); // ln(R/Ro) - temperature /= b_coefficent; // 1/B * ln(R/Ro) - temperature += 1 / C_TO_KELVIN(nominal_temperature); // + (1/To) - temperature = 1 / temperature; // invert - - // the example code rejected anything below 0C, or above 200C, the 200C clamp makes some sense, the below 0C is harder to accept - return temperature; -} - -#endif // HAL_PERIPH_ENABLE_ESC_APD diff --git a/Tools/AP_Periph/esc_apd_telem.h b/Tools/AP_Periph/esc_apd_telem.h deleted file mode 100644 index 42f2f416ad6112..00000000000000 --- a/Tools/AP_Periph/esc_apd_telem.h +++ /dev/null @@ -1,61 +0,0 @@ -/* - ESC Telemetry for APD ESC. - */ - -#pragma once - -#include - -#ifdef HAL_PERIPH_ENABLE_ESC_APD - -class ESC_APD_Telem { -public: - ESC_APD_Telem (AP_HAL::UARTDriver *_uart, float num_poles); - bool update(); - - CLASS_NO_COPY(ESC_APD_Telem); - - struct telem { - uint32_t error_count; - float voltage; - float current; - float temperature; // kelvin - int32_t rpm; - uint8_t power_rating_pct; - }; - - const telem &get_telem(void) { - return decoded; - } - -private: - AP_HAL::UARTDriver *uart; - - union { - struct PACKED { - uint16_t voltage; - uint16_t temperature; - int16_t bus_current; - uint16_t reserved0; - uint32_t erpm; - uint16_t input_duty; - uint16_t motor_duty; - uint16_t reserved1; - uint16_t checksum; // 16 bit fletcher checksum - uint16_t stop; // should always be 65535 on a valid packet - } packet; - uint8_t bytes[22]; - } received; - static_assert(sizeof(received.packet) == sizeof(received.bytes), "The packet must be the same size as the raw buffer"); - - uint8_t len; - - struct telem decoded; - - float pole_count; - - float convert_temperature(uint16_t raw) const; - void shift_buffer(void); -}; - -#endif // HAL_PERIPH_ENABLE_ESC_APD diff --git a/Tools/AP_Periph/gps.cpp b/Tools/AP_Periph/gps.cpp deleted file mode 100644 index e8262e2efb4345..00000000000000 --- a/Tools/AP_Periph/gps.cpp +++ /dev/null @@ -1,322 +0,0 @@ -#include "AP_Periph.h" - -#ifdef HAL_PERIPH_ENABLE_GPS - -/* - GPS support - */ - -#include -#include - -#define DEBUG_PRINTS 0 - -#if DEBUG_PRINTS - # define Debug(fmt, args ...) do {can_printf(fmt "\n", ## args);} while(0) -#else - # define Debug(fmt, args ...) -#endif - -/* - handle gnss::RTCMStream - */ -void AP_Periph_FW::handle_RTCMStream(CanardInstance* canard_instance, CanardRxTransfer* transfer) -{ - uavcan_equipment_gnss_RTCMStream req; - if (uavcan_equipment_gnss_RTCMStream_decode(transfer, &req)) { - return; - } - gps.handle_gps_rtcm_fragment(0, req.data.data, req.data.len); -} - -/* - handle gnss::MovingBaselineData -*/ -#if GPS_MOVING_BASELINE -void AP_Periph_FW::handle_MovingBaselineData(CanardInstance* canard_instance, CanardRxTransfer* transfer) -{ - ardupilot_gnss_MovingBaselineData msg; - if (ardupilot_gnss_MovingBaselineData_decode(transfer, &msg)) { - return; - } - gps.inject_MBL_data(msg.data.data, msg.data.len); - Debug("MovingBaselineData: len=%u\n", msg.data.len); -} -#endif // GPS_MOVING_BASELINE - -/* - convert large values to NaN for float16 - */ -static void check_float16_range(float *v, uint8_t len) -{ - for (uint8_t i=0; i= f16max) { - v[i] = nanf(""); - } - } -} - -/* - update CAN GPS - */ -void AP_Periph_FW::can_gps_update(void) -{ - if (gps.get_type(0) == AP_GPS::GPS_Type::GPS_TYPE_NONE) { - return; - } - gps.update(); - send_moving_baseline_msg(); - send_relposheading_msg(); - if (last_gps_update_ms == gps.last_message_time_ms()) { - return; - } - last_gps_update_ms = gps.last_message_time_ms(); - - { - /* - send Fix2 packet - */ - uavcan_equipment_gnss_Fix2 pkt {}; - const Location &loc = gps.location(); - const Vector3f &vel = gps.velocity(); - if (gps.status() < AP_GPS::GPS_OK_FIX_2D && !saw_gps_lock_once) { - pkt.timestamp.usec = AP_HAL::micros64(); - pkt.gnss_timestamp.usec = 0; - } else { - saw_gps_lock_once = true; - pkt.timestamp.usec = gps.time_epoch_usec(); - pkt.gnss_timestamp.usec = gps.last_message_epoch_usec(); - } - if (pkt.gnss_timestamp.usec == 0) { - pkt.gnss_time_standard = UAVCAN_EQUIPMENT_GNSS_FIX_GNSS_TIME_STANDARD_NONE; - } else { - pkt.gnss_time_standard = UAVCAN_EQUIPMENT_GNSS_FIX_GNSS_TIME_STANDARD_UTC; - } - pkt.longitude_deg_1e8 = uint64_t(loc.lng) * 10ULL; - pkt.latitude_deg_1e8 = uint64_t(loc.lat) * 10ULL; - pkt.height_ellipsoid_mm = loc.alt * 10; - pkt.height_msl_mm = loc.alt * 10; - float undulation; - if (gps.get_undulation(undulation)) { - pkt.height_ellipsoid_mm -= undulation*1000; - } - for (uint8_t i=0; i<3; i++) { - pkt.ned_velocity[i] = vel[i]; - } - pkt.sats_used = gps.num_sats(); - switch (gps.status()) { - case AP_GPS::GPS_Status::NO_GPS: - case AP_GPS::GPS_Status::NO_FIX: - pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_NO_FIX; - pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE; - pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER; - break; - case AP_GPS::GPS_Status::GPS_OK_FIX_2D: - pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_2D_FIX; - pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE; - pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER; - break; - case AP_GPS::GPS_Status::GPS_OK_FIX_3D: - pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX; - pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE; - pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER; - break; - case AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS: - pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX; - pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_DGPS; - pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_SBAS; - break; - case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT: - pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX; - pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_RTK; - pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FLOAT; - break; - case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED: - pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX; - pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_RTK; - pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FIXED; - break; - } - - pkt.covariance.len = 6; - - float hacc; - if (gps.horizontal_accuracy(hacc)) { - pkt.covariance.data[0] = pkt.covariance.data[1] = sq(hacc); - } - - float vacc; - if (gps.vertical_accuracy(vacc)) { - pkt.covariance.data[2] = sq(vacc); - } - - float sacc; - if (gps.speed_accuracy(sacc)) { - float vc3 = sq(sacc); - pkt.covariance.data[3] = pkt.covariance.data[4] = pkt.covariance.data[5] = vc3; - } - - check_float16_range(pkt.covariance.data, pkt.covariance.len); - - uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_FIX2_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_gnss_Fix2_encode(&pkt, buffer, !canfdout()); - - canard_broadcast(UAVCAN_EQUIPMENT_GNSS_FIX2_SIGNATURE, - UAVCAN_EQUIPMENT_GNSS_FIX2_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); - } - - /* - send aux packet - */ - { - uavcan_equipment_gnss_Auxiliary aux {}; - aux.hdop = gps.get_hdop() * 0.01; - aux.vdop = gps.get_vdop() * 0.01; - - uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_AUXILIARY_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_gnss_Auxiliary_encode(&aux, buffer, !canfdout()); - canard_broadcast(UAVCAN_EQUIPMENT_GNSS_AUXILIARY_SIGNATURE, - UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); - } - - // send the gnss status packet - { - ardupilot_gnss_Status status {}; - - status.healthy = gps.is_healthy(); - if (gps.logging_present() && gps.logging_enabled() && !gps.logging_failed()) { - status.status |= ARDUPILOT_GNSS_STATUS_STATUS_LOGGING; - } - uint8_t idx; // unused - if (status.healthy && !gps.first_unconfigured_gps(idx)) { - status.status |= ARDUPILOT_GNSS_STATUS_STATUS_ARMABLE; - } - - uint32_t error_codes; - if (gps.get_error_codes(error_codes)) { - status.error_codes = error_codes; - } - - uint8_t buffer[ARDUPILOT_GNSS_STATUS_MAX_SIZE] {}; - const uint16_t total_size = ardupilot_gnss_Status_encode(&status, buffer, !canfdout()); - canard_broadcast(ARDUPILOT_GNSS_STATUS_SIGNATURE, - ARDUPILOT_GNSS_STATUS_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); - - } - - // send Heading message if we are not sending RelPosHeading messages and have yaw - if (gps.have_gps_yaw() && last_relposheading_ms == 0) { - float yaw_deg, yaw_acc_deg; - uint32_t yaw_time_ms; - if (gps.gps_yaw_deg(yaw_deg, yaw_acc_deg, yaw_time_ms) && yaw_time_ms != last_gps_yaw_ms) { - last_gps_yaw_ms = yaw_time_ms; - - ardupilot_gnss_Heading heading {}; - heading.heading_valid = true; - heading.heading_accuracy_valid = is_positive(yaw_acc_deg); - heading.heading_rad = radians(yaw_deg); - heading.heading_accuracy_rad = radians(yaw_acc_deg); - uint8_t buffer[ARDUPILOT_GNSS_HEADING_MAX_SIZE] {}; - const uint16_t total_size = ardupilot_gnss_Heading_encode(&heading, buffer, !canfdout()); - canard_broadcast(ARDUPILOT_GNSS_HEADING_SIGNATURE, - ARDUPILOT_GNSS_HEADING_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); - } - } -} - - -void AP_Periph_FW::send_moving_baseline_msg() -{ -#if GPS_MOVING_BASELINE - const uint8_t *data = nullptr; - uint16_t len = 0; - if (!gps.get_RTCMV3(data, len)) { - return; - } - if (len == 0 || data == nullptr) { - return; - } - // send the packet from Moving Base to be used RelPosHeading calc by GPS module - ardupilot_gnss_MovingBaselineData mbldata {}; - // get the data from the moving base - static_assert(sizeof(ardupilot_gnss_MovingBaselineData::data.data) == RTCM3_MAX_PACKET_LEN, "Size of Moving Base data is wrong"); - mbldata.data.len = len; - memcpy(mbldata.data.data, data, len); - uint8_t buffer[ARDUPILOT_GNSS_MOVINGBASELINEDATA_MAX_SIZE] {}; - const uint16_t total_size = ardupilot_gnss_MovingBaselineData_encode(&mbldata, buffer, !canfdout()); - -#if HAL_NUM_CAN_IFACES >= 2 - if (gps_mb_can_port != -1 && (gps_mb_can_port < HAL_NUM_CAN_IFACES)) { - uint8_t *tid_ptr = get_tid_ptr(MAKE_TRANSFER_DESCRIPTOR(ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE, ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID, 0, CANARD_BROADCAST_NODE_ID)); - canardBroadcast(&dronecan.canard, - ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE, - ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID, - tid_ptr, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size -#if CANARD_MULTI_IFACE - ,(1U< - -extern const AP_HAL::HAL &hal; - -/* - hardpoint support - */ - -#include - -void AP_Periph_FW::pwm_hardpoint_init() -{ - hal.gpio->attach_interrupt( - PWM_HARDPOINT_PIN, - FUNCTOR_BIND_MEMBER(&AP_Periph_FW::pwm_irq_handler, void, uint8_t, bool, uint32_t), AP_HAL::GPIO::INTERRUPT_BOTH); -} - -/* - called on PWM pin transition - */ -void AP_Periph_FW::pwm_irq_handler(uint8_t pin, bool pin_state, uint32_t timestamp) -{ - if (pin_state == 0 && pwm_hardpoint.last_state == 1 && pwm_hardpoint.last_ts_us != 0) { - uint32_t width = timestamp - pwm_hardpoint.last_ts_us; - if (width > 500 && width < 2500) { - pwm_hardpoint.pwm_value = width; - if (width > pwm_hardpoint.highest_pwm) { - pwm_hardpoint.highest_pwm = width; - } - } - } - pwm_hardpoint.last_state = pin_state; - pwm_hardpoint.last_ts_us = timestamp; -} - -void AP_Periph_FW::pwm_hardpoint_update() -{ - uint32_t now = AP_HAL::millis(); - // send at 10Hz - void *save = hal.scheduler->disable_interrupts_save(); - uint16_t value = pwm_hardpoint.highest_pwm; - pwm_hardpoint.highest_pwm = 0; - hal.scheduler->restore_interrupts(save); - float rate = g.hardpoint_rate; - rate = constrain_float(rate, 10, 100); - if (value > 0 && now - pwm_hardpoint.last_send_ms >= 1000U/rate) { - pwm_hardpoint.last_send_ms = now; - uavcan_equipment_hardpoint_Command cmd {}; - cmd.hardpoint_id = g.hardpoint_id; - cmd.command = value; - - uint8_t buffer[UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_hardpoint_Command_encode(&cmd, buffer, !canfdout()); - canard_broadcast(UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_SIGNATURE, - UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); - } -} - -#endif // HAL_PERIPH_ENABLE_PWM_HARDPOINT diff --git a/Tools/AP_Periph/hwing_esc.cpp b/Tools/AP_Periph/hwing_esc.cpp index 871c2e0517c45f..a79862f3cf6d0e 100644 --- a/Tools/AP_Periph/hwing_esc.cpp +++ b/Tools/AP_Periph/hwing_esc.cpp @@ -6,11 +6,8 @@ This protocol only allows for one ESC per UART RX line, so using a CAN node per ESC works well. */ - -#include "AP_Periph.h" #include "hwing_esc.h" #include -#include #ifdef HAL_PERIPH_ENABLE_HWESC @@ -42,7 +39,7 @@ bool HWESC_Telem::update() } // we expect at least 50ms idle between frames - uint32_t now = AP_HAL::millis(); + uint32_t now = AP_HAL::native_millis(); bool frame_gap = (now - last_read_ms) > 10; last_read_ms = now; @@ -146,30 +143,5 @@ bool HWESC_Telem::parse_packet(void) return true; } -void AP_Periph_FW::hwesc_telem_update() -{ - if (!hwesc_telem.update()) { - return; - } - const HWESC_Telem::HWESC &t = hwesc_telem.get_telem(); - - uavcan_equipment_esc_Status pkt {}; - pkt.esc_index = g.esc_number[0]; // only supports a single ESC - pkt.voltage = t.voltage; - pkt.current = t.current; - pkt.temperature = C_TO_KELVIN(MAX(t.mos_temperature, t.cap_temperature)); - pkt.rpm = t.rpm; - pkt.power_rating_pct = t.phase_current; - pkt.error_count = t.error_count; - - uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer, !canfdout()); - canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, - UAVCAN_EQUIPMENT_ESC_STATUS_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); -} - #endif // HAL_PERIPH_ENABLE_HWESC diff --git a/Tools/AP_Periph/proximity.cpp b/Tools/AP_Periph/proximity.cpp deleted file mode 100644 index 2c88d3fc0f98af..00000000000000 --- a/Tools/AP_Periph/proximity.cpp +++ /dev/null @@ -1,75 +0,0 @@ -#include "AP_Periph.h" - -#ifdef HAL_PERIPH_ENABLE_PROXIMITY - -/* - proximity support - */ - -#include - -void AP_Periph_FW::can_proximity_update() -{ - if (proximity.get_type(0) == AP_Proximity::Type::None) { - return; - } - - uint32_t now = AP_HAL::millis(); - static uint32_t last_update_ms; - if (g.proximity_max_rate > 0 && - now - last_update_ms < 1000/g.proximity_max_rate) { - // limit to max rate - return; - } - last_update_ms = now; - proximity.update(); - AP_Proximity::Status status = proximity.get_status(); - if (status <= AP_Proximity::Status::NoData) { - // don't send any data - return; - } - - ardupilot_equipment_proximity_sensor_Proximity pkt {}; - - const uint8_t obstacle_count = proximity.get_obstacle_count(); - - // if no objects return - if (obstacle_count == 0) { - return; - } - - // calculate maximum roll, pitch values from objects - for (uint8_t i=0; i - -#ifndef AP_PERIPH_PROBE_CONTINUOUS -#define AP_PERIPH_PROBE_CONTINUOUS 0 -#endif - -/* - update CAN rangefinder - */ -void AP_Periph_FW::can_rangefinder_update(void) -{ - if (rangefinder.get_type(0) == RangeFinder::Type::NONE) { - return; - } -#if AP_PERIPH_PROBE_CONTINUOUS - if (rangefinder.num_sensors() == 0) { - uint32_t now = AP_HAL::millis(); - static uint32_t last_probe_ms; - if (now - last_probe_ms >= 1000) { - last_probe_ms = now; - rangefinder.init(ROTATION_NONE); - } - } -#endif - uint32_t now = AP_HAL::millis(); - static uint32_t last_update_ms; - if (g.rangefinder_max_rate > 0 && - now - last_update_ms < uint32_t(1000/g.rangefinder_max_rate)) { - // limit to max rate - return; - } - last_update_ms = now; - rangefinder.update(); - RangeFinder::Status status = rangefinder.status_orient(ROTATION_NONE); - if (status <= RangeFinder::Status::NoData) { - // don't send any data - return; - } - const uint32_t sample_ms = rangefinder.last_reading_ms(ROTATION_NONE); - if (last_sample_ms == sample_ms) { - return; - } - last_sample_ms = sample_ms; - - uint16_t dist_cm = rangefinder.distance_cm_orient(ROTATION_NONE); - uavcan_equipment_range_sensor_Measurement pkt {}; - pkt.sensor_id = rangefinder.get_address(0); - switch (status) { - case RangeFinder::Status::OutOfRangeLow: - pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_CLOSE; - break; - case RangeFinder::Status::OutOfRangeHigh: - pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_FAR; - break; - case RangeFinder::Status::Good: - pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_VALID_RANGE; - break; - default: - pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_UNDEFINED; - break; - } - switch (rangefinder.get_mav_distance_sensor_type_orient(ROTATION_NONE)) { - case MAV_DISTANCE_SENSOR_LASER: - pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_LIDAR; - break; - case MAV_DISTANCE_SENSOR_ULTRASOUND: - pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_SONAR; - break; - case MAV_DISTANCE_SENSOR_RADAR: - pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_RADAR; - break; - default: - pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_UNDEFINED; - break; - } - - pkt.range = dist_cm * 0.01; - - uint8_t buffer[UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_range_sensor_Measurement_encode(&pkt, buffer, !periph.canfdout()); - - canard_broadcast(UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE, - UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); -} - -#endif // HAL_PERIPH_ENABLE_RANGEFINDER diff --git a/Tools/AP_Periph/rc_in.cpp b/Tools/AP_Periph/rc_in.cpp deleted file mode 100644 index 83f691bda2816b..00000000000000 --- a/Tools/AP_Periph/rc_in.cpp +++ /dev/null @@ -1,180 +0,0 @@ -/* - 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; iforce_safety_on(); -#else - hal.rcout->force_safety_off(); -#endif #if HAL_WITH_ESC_TELEM && !HAL_GCS_ENABLED if (g.esc_telem_port >= 0) { @@ -136,8 +132,7 @@ void AP_Periph_FW::rcout_update() const bool has_esc_rawcommand_timed_out = esc_timeout_ms != 0 && ((now_ms - last_esc_raw_command_ms) >= esc_timeout_ms); if (last_esc_num_channels > 0 && has_esc_rawcommand_timed_out) { // If we've seen ESCs previously, and a timeout has occurred, then zero the outputs - int16_t esc_output[last_esc_num_channels]; - memset(esc_output, 0, sizeof(esc_output)); + int16_t esc_output[last_esc_num_channels] {}; rcout_esc(esc_output, last_esc_num_channels); // register that the output has been changed diff --git a/Tools/AP_Periph/serial_tunnel.cpp b/Tools/AP_Periph/serial_tunnel.cpp deleted file mode 100644 index a1becc8aff65a3..00000000000000 --- a/Tools/AP_Periph/serial_tunnel.cpp +++ /dev/null @@ -1,213 +0,0 @@ -/* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ -/* - handle tunnelling of serial data over DroneCAN - */ - -#include -#include "AP_Periph.h" - -#if AP_UART_MONITOR_ENABLED - -#include - -extern const AP_HAL::HAL &hal; - -#define TUNNEL_LOCK_KEY 0xf2e460e4U - -#ifndef TUNNEL_DEBUG -#define TUNNEL_DEBUG 0 -#endif - -#if TUNNEL_DEBUG -# define debug(fmt, args...) can_printf(fmt "\n", ##args) -#else -# define debug(fmt, args...) -#endif - -/* - get the default port to tunnel if the client requests port -1 - */ -int8_t AP_Periph_FW::get_default_tunnel_serial_port(void) const -{ - int8_t uart_num = -1; -#ifdef HAL_PERIPH_ENABLE_GPS - if (uart_num == -1) { - uart_num = g.gps_port; - } -#endif -#ifdef HAL_PERIPH_ENABLE_RANGEFINDER - if (uart_num == -1) { - uart_num = g.rangefinder_port; - } -#endif -#ifdef HAL_PERIPH_ENABLE_ADSB - if (uart_num == -1) { - uart_num = g.adsb_port; - } -#endif -#ifdef HAL_PERIPH_ENABLE_PROXIMITY - if (uart_num == -1) { - uart_num = g.proximity_port; - } -#endif - return uart_num; -} - -/* - handle tunnel data - */ -void AP_Periph_FW::handle_tunnel_Targetted(CanardInstance* canard_ins, CanardRxTransfer* transfer) -{ - uavcan_tunnel_Targetted pkt; - if (uavcan_tunnel_Targetted_decode(transfer, &pkt)) { - return; - } - if (pkt.target_node != canardGetLocalNodeID(canard_ins)) { - return; - } - if (uart_monitor.buffer == nullptr) { - uart_monitor.buffer = new ByteBuffer(1024); - if (uart_monitor.buffer == nullptr) { - return; - } - } - int8_t uart_num = pkt.serial_id; - if (uart_num == -1) { - uart_num = get_default_tunnel_serial_port(); - } - if (uart_num < 0) { - return; - } - auto *uart = hal.serial(uart_num); - if (uart == nullptr) { - return; - } - if (uart_monitor.uart_num != uart_num && uart_monitor.uart != nullptr) { - // remove monitor from previous uart - hal.serial(uart_monitor.uart_num)->set_monitor_read_buffer(nullptr); - } - uart_monitor.uart_num = uart_num; - if (uart != uart_monitor.uart) { - // change of uart or expired, clear old data - uart_monitor.buffer->clear(); - uart_monitor.uart = uart; - uart_monitor.baudrate = 0; - } - if (uart_monitor.uart == nullptr) { - return; - } - /* - allow for locked state to change at any time, so users can - switch between locked and unlocked while connected - */ - const bool was_locked = uart_monitor.locked; - uart_monitor.locked = (pkt.options & UAVCAN_TUNNEL_TARGETTED_OPTION_LOCK_PORT) != 0; - if (uart_monitor.locked) { - uart_monitor.uart->lock_port(TUNNEL_LOCK_KEY, TUNNEL_LOCK_KEY); - } else { - uart_monitor.uart->lock_port(0,0); - } - uart_monitor.node_id = transfer->source_node_id; - uart_monitor.protocol = pkt.protocol.protocol; - if (pkt.baudrate != uart_monitor.baudrate || !was_locked) { - if (uart_monitor.locked && pkt.baudrate != 0) { - // ensure we have enough buffer space for a uBlox fw update and fast uCenter data - uart_monitor.uart->begin_locked(pkt.baudrate, 2048, 2048, TUNNEL_LOCK_KEY); - debug("begin_locked %u", unsigned(pkt.baudrate)); - } - uart_monitor.baudrate = pkt.baudrate; - } - uart_monitor.uart->set_monitor_read_buffer(uart_monitor.buffer); - uart_monitor.last_request_ms = AP_HAL::millis(); - - // write to device - if (pkt.buffer.len > 0) { - if (uart_monitor.locked) { - debug("write_locked %u", unsigned(pkt.buffer.len)); - uart_monitor.uart->write_locked(pkt.buffer.data, pkt.buffer.len, TUNNEL_LOCK_KEY); - } else { - uart_monitor.uart->write(pkt.buffer.data, pkt.buffer.len); - } - } else { - debug("locked keepalive"); - } -} - -/* - send tunnelled serial data - */ -void AP_Periph_FW::send_serial_monitor_data() -{ - if (uart_monitor.uart == nullptr || - uart_monitor.node_id == 0 || - uart_monitor.buffer == nullptr) { - return; - } - const uint32_t last_req_ms = uart_monitor.last_request_ms; - const uint32_t now_ms = AP_HAL::millis(); - if (now_ms - last_req_ms >= 3000) { - // stop sending and unlock, but don't release the buffer - if (uart_monitor.locked) { - debug("unlock"); - uart_monitor.uart->lock_port(0, 0); - } - uart_monitor.uart = nullptr; - return; - } - if (uart_monitor.locked) { - /* - when the port is locked nobody is reading the uart so the - monitor doesn't fill. We read here to ensure it fills - */ - uint8_t buf[120]; - for (uint8_t i=0; i<8; i++) { - if (uart_monitor.uart->read_locked(buf, sizeof(buf), TUNNEL_LOCK_KEY) <= 0) { - break; - } - } - } - uint8_t sends = 8; - while (uart_monitor.buffer->available() > 0 && sends-- > 0) { - uint32_t n; - const uint8_t *buf = uart_monitor.buffer->readptr(n); - if (n == 0) { - return; - } - // broadcast data as tunnel packets, can be used for uCenter debug and device fw update - uavcan_tunnel_Targetted pkt {}; - n = MIN(n, sizeof(pkt.buffer.data)); - pkt.target_node = uart_monitor.node_id; - pkt.protocol.protocol = uart_monitor.protocol; - pkt.buffer.len = n; - pkt.baudrate = uart_monitor.baudrate; - memcpy(pkt.buffer.data, buf, n); - - uint8_t buffer[UAVCAN_TUNNEL_TARGETTED_MAX_SIZE] {}; - const uint16_t total_size = uavcan_tunnel_Targetted_encode(&pkt, buffer, !canfdout()); - - debug("read %u", unsigned(n)); - - if (!canard_broadcast(UAVCAN_TUNNEL_TARGETTED_SIGNATURE, - UAVCAN_TUNNEL_TARGETTED_ID, - CANARD_TRANSFER_PRIORITY_MEDIUM, - &buffer[0], - total_size)) { - break; - } - uart_monitor.buffer->advance(n); - } -} -#endif // AP_UART_MONITOR_ENABLED diff --git a/Tools/AP_Periph/version.h b/Tools/AP_Periph/version.h index 090841e1d57fdc..84aad587c28b2a 100644 --- a/Tools/AP_Periph/version.h +++ b/Tools/AP_Periph/version.h @@ -2,17 +2,15 @@ #include -#define THISFIRMWARE "AP_Periph V1.7.0-dev" +#define THISFIRMWARE "AP_Periph V1.5.0-dev" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 1,7,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 1,5,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 1 -#define FW_MINOR 7 +#define FW_MINOR 5 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV - - diff --git a/Tools/AP_Periph/wscript b/Tools/AP_Periph/wscript index 8f238ce14b0d50..d6a733b5ffafd0 100644 --- a/Tools/AP_Periph/wscript +++ b/Tools/AP_Periph/wscript @@ -38,13 +38,11 @@ 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', @@ -70,19 +68,8 @@ def build(bld): 'AP_Stats', 'AP_EFI', 'AP_CheckFirmware', - 'AP_RPM', 'AP_Proximity', - 'AP_RCProtocol', - 'AP_AHRS', - 'AP_Terrain', - 'AP_Torqeedo', - 'AP_Volz_Protocol', - 'AP_SBusOut', - 'AP_RobotisServo', - 'AP_FETtecOneWire', - 'GCS_MAVLink', ] - bld.ap_stlib( name= 'AP_Periph_libs', dynamic_source='modules/DroneCAN/libcanard/dsdlc_generated/src/**.c', diff --git a/Tools/CPUInfo/CPUInfo.cpp b/Tools/CPUInfo/CPUInfo.cpp index a20db4924d95d6..73d1c6a45a9cfc 100644 --- a/Tools/CPUInfo/CPUInfo.cpp +++ b/Tools/CPUInfo/CPUInfo.cpp @@ -25,8 +25,7 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); #if CONFIG_HAL_BOARD != HAL_BOARD_LINUX // On H750 we want to measure external flash to ram performance -#if defined(EXT_FLASH_SIZE_MB) && EXT_FLASH_SIZE_MB>0 && defined(STM32H7) -#include "ch.h" +#if defined(EXT_FLASH_SIZE_MB) && defined(STM32H7) #define DISABLE_CACHES #endif @@ -45,9 +44,7 @@ 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(); @@ -100,7 +97,6 @@ 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 deleted file mode 100644 index b33cc18a18bd2b..00000000000000 --- a/Tools/CPUInfo/output-PixFlamingo.txt +++ /dev/null @@ -1,71 +0,0 @@ -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 deleted file mode 100644 index 54d9c2e446169d..00000000000000 --- a/Tools/CPUInfo/output-esp32-classic.txt +++ /dev/null @@ -1,94 +0,0 @@ -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 54dfab7eb55bac..cce4dddf532196 100644 --- a/Tools/GIT_Test/GIT_Success.txt +++ b/Tools/GIT_Test/GIT_Success.txt @@ -173,7 +173,4 @@ Ava Falkner Emre Can Suiçmez Yacine Thabet - 57 Greg Poulos -Torre Orazio -seba czapnik -Ramy Gad -Matthew R. Cunningham \ No newline at end of file +Torre Orazio \ No newline at end of file diff --git a/Tools/IO_Firmware/iofirmware_dshot_highpolh.bin b/Tools/IO_Firmware/iofirmware_dshot_highpolh.bin deleted file mode 100755 index 83a7e2b6299695..00000000000000 Binary files a/Tools/IO_Firmware/iofirmware_dshot_highpolh.bin and /dev/null differ diff --git a/Tools/IO_Firmware/iofirmware_dshot_lowpolh.bin b/Tools/IO_Firmware/iofirmware_dshot_lowpolh.bin deleted file mode 100755 index dbc520edd07a61..00000000000000 Binary files a/Tools/IO_Firmware/iofirmware_dshot_lowpolh.bin and /dev/null differ diff --git a/Tools/IO_Firmware/iofirmware_f103_8MHz_highpolh.bin b/Tools/IO_Firmware/iofirmware_f103_8MHz_highpolh.bin index a722bc5bd2ee92..5cc1c4f7de66a1 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 cdb40f2fab870f..f036a1f0f8e557 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 deleted file mode 100755 index 3d993264ce8699..00000000000000 Binary files a/Tools/IO_Firmware/iofirmware_f103_dshot_highpolh.bin and /dev/null differ diff --git a/Tools/IO_Firmware/iofirmware_f103_dshot_lowpolh.bin b/Tools/IO_Firmware/iofirmware_f103_dshot_lowpolh.bin deleted file mode 100755 index ef5755a62abef3..00000000000000 Binary files a/Tools/IO_Firmware/iofirmware_f103_dshot_lowpolh.bin and /dev/null differ diff --git a/Tools/IO_Firmware/iofirmware_f103_highpolh.bin b/Tools/IO_Firmware/iofirmware_f103_highpolh.bin deleted file mode 100755 index 89d0449fb12566..00000000000000 Binary files a/Tools/IO_Firmware/iofirmware_f103_highpolh.bin and /dev/null differ diff --git a/Tools/IO_Firmware/iofirmware_f103_lowpolh.bin b/Tools/IO_Firmware/iofirmware_f103_lowpolh.bin deleted file mode 100755 index 2179b5cbc5efb9..00000000000000 Binary files a/Tools/IO_Firmware/iofirmware_f103_lowpolh.bin and /dev/null differ diff --git a/Tools/IO_Firmware/iofirmware_highpolh.bin b/Tools/IO_Firmware/iofirmware_highpolh.bin index 4e2b62dec6a670..e946710b3e2809 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 3ba40363bcd4a8..9e8396c28e2a02 100755 Binary files a/Tools/IO_Firmware/iofirmware_lowpolh.bin and b/Tools/IO_Firmware/iofirmware_lowpolh.bin differ diff --git a/Tools/LogAnalyzer/DataflashLog.py b/Tools/LogAnalyzer/DataflashLog.py index a96cca589cecd5..38fc82b21943db 100644 --- a/Tools/LogAnalyzer/DataflashLog.py +++ b/Tools/LogAnalyzer/DataflashLog.py @@ -169,7 +169,7 @@ def to_class(self): _pack_=True, ) - if isinstance(self.types[0], str): + if type(self.types[0]) == str: fieldtypes = [i for i in self.types] else: fieldtypes = [chr(i) for i in self.types] @@ -756,7 +756,7 @@ def read_text(self, f, ignoreBadlines): else: raise Exception("") else: - if tokens[0] not in self.formats: + if not tokens[0] in self.formats: raise ValueError("Unknown Format {}".format(tokens[0])) e = self.formats[tokens[0]](*tokens[1:]) self.process(lineNumber, e) diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp index cbdca233afaf68..8b9d2bd4315111 100644 --- a/Tools/Replay/LR_MsgHandler.cpp +++ b/Tools/Replay/LR_MsgHandler.cpp @@ -289,12 +289,6 @@ 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 4e30a6571d35ce..b258f496e728a0 100644 --- a/Tools/Replay/LR_MsgHandler.h +++ b/Tools/Replay/LR_MsgHandler.h @@ -57,12 +57,6 @@ 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 9a84053ab81ada..d36c3da3fe9e19 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -118,8 +118,6 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f) msgparser[f.type] = new LR_MsgHandler_ROFH(formats[f.type], ekf2, ekf3); } else if (streq(name, "REPH")) { msgparser[f.type] = new LR_MsgHandler_REPH(formats[f.type], ekf2, ekf3); - } else if (streq(name, "RSLL")) { - msgparser[f.type] = new LR_MsgHandler_RSLL(formats[f.type], ekf2, ekf3); } else if (streq(name, "REVH")) { msgparser[f.type] = new LR_MsgHandler_REVH(formats[f.type], ekf2, ekf3); } else if (streq(name, "RWOH")) { diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp index a55a33a799d1b3..63c0cfba19106a 100644 --- a/Tools/Replay/Replay.cpp +++ b/Tools/Replay/Replay.cpp @@ -100,10 +100,8 @@ const struct AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { }; GCS_Dummy _gcs; -#if AP_ADVANCEDFAILSAFE_ENABLED AP_AdvancedFailsafe *AP::advancedfailsafe() { return nullptr; } bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate, const char *reason) { return false; } -#endif // dummy method to avoid linking AP_Avoidance // AP_Avoidance *AP::ap_avoidance() { return nullptr; } diff --git a/Tools/ardupilotwaf/ap_library.py b/Tools/ardupilotwaf/ap_library.py index b49010ae06001c..d8fdd4c05ca10a 100644 --- a/Tools/ardupilotwaf/ap_library.py +++ b/Tools/ardupilotwaf/ap_library.py @@ -239,25 +239,6 @@ def __str__(self): def keyword(self): return 'Checking included headers' -def custom_flags_check(tgen): - ''' - check for tasks marked as having custom cpp or c flags - a library can do this by setting AP_LIB_EXTRA_CXXFLAGS and AP_LIB_EXTRA_CFLAGS - - For example add this is the configure section of the library, using AP_DDS as an example: - - cfg.env.AP_LIB_EXTRA_CXXFLAGS['AP_DDS'] = ['-DSOME_CXX_FLAG'] - cfg.env.AP_LIB_EXTRA_CFLAGS['AP_DDS'] = ['-DSOME_C_FLAG'] - ''' - if not tgen.name.startswith("objs/"): - return - libname = tgen.name[5:] - if libname in tgen.env.AP_LIB_EXTRA_CXXFLAGS: - tgen.env.CXXFLAGS.extend(tgen.env.AP_LIB_EXTRA_CXXFLAGS[libname]) - if libname in tgen.env.AP_LIB_EXTRA_CFLAGS: - tgen.env.CFLAGS.extend(tgen.env.AP_LIB_EXTRA_CFLAGS[libname]) - - def double_precision_check(tasks): '''check for tasks marked as double precision''' @@ -270,15 +251,11 @@ def double_precision_check(tasks): double_tasks.append([library, s]) src = str(t.inputs[0]).split('/')[-2:] - double_library = t.env.DOUBLE_PRECISION_LIBRARIES.get(src[0],False) - - if double_library or src in double_tasks: + if src in double_tasks: + single_precision_option='-fsingle-precision-constant' t.env.CXXFLAGS = t.env.CXXFLAGS[:] - for opt in ['-fsingle-precision-constant', '-cl-single-precision-constant']: - try: - t.env.CXXFLAGS.remove(opt) - except ValueError: - pass + if single_precision_option in t.env.CXXFLAGS: + t.env.CXXFLAGS.remove(single_precision_option) t.env.CXXFLAGS.append("-DALLOW_DOUBLE_MATH_FUNCTIONS") @@ -307,7 +284,6 @@ def ap_library_register_for_check(self): if not hasattr(self, 'compiled_tasks'): return - custom_flags_check(self) double_precision_check(self.compiled_tasks) if self.env.ENABLE_ONVIF: gsoap_library_check(self.bld, self.compiled_tasks) @@ -322,7 +298,4 @@ def ap_library_register_for_check(self): def configure(cfg): cfg.env.AP_LIBRARIES_OBJECTS_KW = dict() cfg.env.AP_LIB_EXTRA_SOURCES = dict() - cfg.env.AP_LIB_EXTRA_CXXFLAGS = dict() - cfg.env.AP_LIB_EXTRA_CFLAGS = dict() cfg.env.DOUBLE_PRECISION_SOURCES = dict() - cfg.env.DOUBLE_PRECISION_LIBRARIES = dict() diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py index 75c9975b826bd7..0f9fa3d46e99d2 100644 --- a/Tools/ardupilotwaf/ardupilotwaf.py +++ b/Tools/ardupilotwaf/ardupilotwaf.py @@ -34,7 +34,6 @@ 'AP_HAL', 'AP_HAL_Empty', 'AP_InertialSensor', - 'AP_KDECAN', 'AP_Math', 'AP_Mission', 'AP_DAL', @@ -64,7 +63,6 @@ 'AP_Module', 'AP_Button', 'AP_ICEngine', - 'AP_Networking', 'AP_Frsky_Telem', 'AP_FlashStorage', 'AP_Relay', @@ -115,7 +113,6 @@ 'AP_AIS', 'AP_OpenDroneID', 'AP_CheckFirmware', - 'AP_ExternalControl', ] def get_legacy_defines(sketch_name, bld): @@ -137,7 +134,6 @@ def get_legacy_defines(sketch_name, bld): IGNORED_AP_LIBRARIES = [ 'doc', 'AP_Scripting', # this gets explicitly included when it is needed and should otherwise never be globbed in - 'AP_DDS', ] @@ -305,8 +301,6 @@ 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): @@ -330,9 +324,6 @@ def ap_stlib(bld, **kw): for l in kw['ap_libraries']: bld.ap_library(l, kw['ap_vehicle']) - if 'dynamic_source' not in kw: - kw['dynamic_source'] = 'modules/DroneCAN/libcanard/dsdlc_generated/src/**.c' - kw['features'] = kw.get('features', []) + ['cxx', 'cxxstlib'] kw['target'] = kw['name'] kw['source'] = [] @@ -358,7 +349,7 @@ def ap_stlib_target(self): self.target = '#%s' % os.path.join('lib', self.target) @conf -def ap_find_tests(bld, use=[], DOUBLE_PRECISION_SOURCES=[]): +def ap_find_tests(bld, use=[]): if not bld.env.HAS_GTEST: return @@ -372,7 +363,7 @@ def ap_find_tests(bld, use=[], DOUBLE_PRECISION_SOURCES=[]): includes = [bld.srcnode.abspath() + '/tests/'] for f in bld.path.ant_glob(incl='*.cpp'): - t = ap_program( + ap_program( bld, features=features, includes=includes, @@ -383,16 +374,6 @@ def ap_find_tests(bld, use=[], DOUBLE_PRECISION_SOURCES=[]): 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 = [] @@ -579,11 +560,6 @@ 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 faf500cc24ee30..426f01baae5e61 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -47,43 +47,34 @@ def srcpath(path): env.SRCROOT = srcpath('') self.configure_env(cfg, env) - # Setup scripting: env.DEFINES.update( - LUA_32BITS = 1, + AP_SCRIPTING_ENABLED = 0, ) - env.AP_LIBRARIES += [ - 'AP_Scripting', - 'AP_Scripting/lua/src', - ] + # 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))): - if cfg.options.enable_scripting: env.DEFINES.update( AP_SCRIPTING_ENABLED = 1, - ) - elif cfg.options.disable_scripting: - env.DEFINES.update( - AP_SCRIPTING_ENABLED = 0, - ) + LUA_32BITS = 1, + ) + + env.AP_LIBRARIES += [ + 'AP_Scripting', + 'AP_Scripting/lua/src', + ] + + else: + cfg.options.disable_scripting = True # allow GCS disable for AP_DAL example if cfg.options.no_gcs: env.CXXFLAGS += ['-DHAL_GCS_ENABLED=0'] - # configurations for XRCE-DDS - if cfg.options.enable_dds: - cfg.recurse('libraries/AP_DDS') - env.ENABLE_DDS = True - env.AP_LIBRARIES += [ - 'AP_DDS' - ] - env.DEFINES.update(AP_DDS_ENABLED = 1) - # check for microxrceddsgen - cfg.find_program('microxrceddsgen',mandatory=True) - else: - env.ENABLE_DDS = False - env.DEFINES.update(AP_DDS_ENABLED = 0) - # setup for supporting onvif cam control if cfg.options.enable_onvif: cfg.recurse('libraries/AP_ONVIF') @@ -243,20 +234,16 @@ 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 += [ @@ -264,7 +251,6 @@ def configure_env(self, cfg, env): ] env.CXXFLAGS += [ '-fcheck-new', - '-fsingle-precision-constant', ] if cfg.env.DEBUG: @@ -275,7 +261,7 @@ def configure_env(self, cfg, env): env.DEFINES.update( HAL_DEBUG_BUILD = 1, ) - elif cfg.options.debug_symbols: + elif cfg.options.g: env.CFLAGS += [ '-g', ] @@ -434,19 +420,14 @@ def configure_env(self, cfg, env): if self.with_can and not cfg.env.AP_PERIPH: env.AP_LIBRARIES += [ - 'AP_DroneCAN', - 'modules/DroneCAN/libcanard/*.c', + 'AP_UAVCAN', + 'modules/uavcan/libuavcan/src/**/*.cpp' ] - if cfg.options.enable_dronecan_tests: - env.DEFINES.update( - AP_TEST_DRONECAN_DRIVERS = 1 - ) env.DEFINES.update( - DRONECAN_CXX_WRAPPERS = 1, - USE_USER_HELPERS = 1, - CANARD_ENABLE_DEADLINE = 1, - CANARD_ALLOCATE_SEM=1 + UAVCAN_CPP_VERSION = 'UAVCAN_CPP03', + UAVCAN_NO_ASSERTIONS = 1, + UAVCAN_NULLPTR = 'nullptr' ) @@ -551,11 +532,7 @@ def add_dynamic_boards_esp32(): continue hwdef = os.path.join(dirname, d, 'hwdef.dat') if os.path.exists(hwdef): - 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}) + newclass = type(d, (esp32,), {'name': d}) def get_boards_names(): add_dynamic_boards_chibios() @@ -614,7 +591,7 @@ def get_board(ctx): ''' % ctx.env.BOARD) boards = _board_classes.keys() - if ctx.env.BOARD not in boards: + if not ctx.env.BOARD in boards: ctx.fatal("Invalid board '%s': choices are %s" % (ctx.env.BOARD, ', '.join(sorted(boards, key=str.lower)))) _board = _board_classes[ctx.env.BOARD]() return _board @@ -626,7 +603,10 @@ def get_board(ctx): class sitl(Board): def __init__(self): - self.with_can = True + if Utils.unversioned_sys_platform().startswith("linux"): + self.with_can = True + else: + self.with_can = False def configure_env(self, cfg, env): super(sitl, self).configure_env(cfg, env) @@ -634,7 +614,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 - AP_BARO_PROBE_EXTERNAL_I2C_BUSES = 1, + HAL_PROBE_EXTERNAL_I2C_BAROS = 1, ) cfg.define('AP_SIM_ENABLED', 1) @@ -644,27 +624,13 @@ 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_ASSERTS = 1) - if not cfg.options.force_32bit: - # needed for cygwin - env.CXXFLAGS += [ '-DCANARD_64_BIT=1' ] - env.CFLAGS += [ '-DCANARD_64_BIT=1' ] - if Utils.unversioned_sys_platform().startswith("linux"): - cfg.define('HAL_CAN_WITH_SOCKETCAN', 1) - else: - cfg.define('HAL_CAN_WITH_SOCKETCAN', 0) + cfg.define('UAVCAN_EXCEPTIONS', 0) + cfg.define('UAVCAN_SUPPORT_CANFD', 1) env.CXXFLAGS += [ - '-Werror=float-equal', - '-Werror=missing-declarations', + '-Werror=float-equal' ] if cfg.options.ubsan or cfg.options.ubsan_abort: @@ -711,9 +677,10 @@ def configure_env(self, cfg, env): 'AP_CSVReader', ] - env.AP_LIBRARIES += [ - 'SITL', - ] + if not cfg.env.AP_PERIPH: + env.AP_LIBRARIES += [ + 'SITL', + ] if cfg.options.enable_sfml: if not cfg.check_SFML(env): @@ -768,7 +735,7 @@ def configure_env(self, cfg, env): '-fno-slp-vectorize' # compiler bug when trying to use SLP ] - if cfg.options.force_32bit: + if cfg.options.sitl_32bit: # 32bit platform flags env.CXXFLAGS += [ '-m32', @@ -780,34 +747,6 @@ 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__ @@ -815,57 +754,47 @@ 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"', + CAN_APP_NODE_NAME = '"org.ardupilot.ap_periph_gps"', + AP_AIRSPEED_ENABLED = 0, HAL_PERIPH_ENABLE_GPS = 1, - HAL_PERIPH_ENABLE_AIRSPEED = 1, - HAL_PERIPH_ENABLE_MAG = 1, - HAL_PERIPH_ENABLE_BARO = 1, - HAL_PERIPH_ENABLE_RANGEFINDER = 1, - HAL_PERIPH_ENABLE_BATTERY = 1, - HAL_PERIPH_ENABLE_EFI = 1, - HAL_PERIPH_ENABLE_RPM = 1, - HAL_PERIPH_ENABLE_RC_OUT = 1, - HAL_PERIPH_ENABLE_ADSB = 1, - AP_AIRSPEED_ENABLED = 1, - AP_AIRSPEED_AUTOCAL_ENABLE = 0, - AP_AHRS_ENABLED = 1, - AP_UART_MONITOR_ENABLED = 1, HAL_CAN_DEFAULT_NODE_ID = 0, HAL_RAM_RESERVE_START = 0, APJ_BOARD_ID = 100, HAL_GCS_ENABLED = 0, - HAL_MAVLINK_BINDINGS_ENABLED = 1, HAL_LOGGING_ENABLED = 0, HAL_LOGGING_MAVLINK_ENABLED = 0, AP_MISSION_ENABLED = 0, HAL_RALLY_ENABLED = 0, AP_SCHEDULER_ENABLED = 0, - CANARD_ENABLE_TAO_OPTION = 1, - AP_RCPROTOCOL_ENABLED = 0, CANARD_ENABLE_CANFD = 1, CANARD_MULTI_IFACE = 1, HAL_CANMANAGER_ENABLED = 0, COMPASS_CAL_ENABLED = 0, COMPASS_MOT_ENABLED = 0, COMPASS_LEARN_ENABLED = 0, - AP_BATTERY_ESC_ENABLED = 1, + AP_BATTERY_ESC_ENABLED = 0, 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, - AP_RTC_ENABLED = 0, ) + # libcanard is written for 32bit platforms + env.CXXFLAGS += [ + '-m32', + ] + env.CFLAGS += [ + '-m32', + ] + env.LDFLAGS += [ + '-m32', + ] + class esp32(Board): @@ -893,7 +822,7 @@ def expand_path(p): env.DEFINES.update( ENABLE_HEAP = 0, CONFIG_HAL_BOARD_SUBTYPE = 'HAL_BOARD_SUBTYPE_ESP32_%s' % tt.upper() , - HAL_HAVE_HARDWARE_DOUBLE = '1', + ALLOW_DOUBLE_MATH_FUNCTIONS = '1', ) env.AP_LIBRARIES += [ @@ -918,7 +847,6 @@ 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') @@ -948,9 +876,6 @@ 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 @@ -980,6 +905,7 @@ 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', @@ -1036,7 +962,6 @@ 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', @@ -1058,7 +983,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,%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) + '-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) ] if cfg.env.DEBUG: @@ -1071,11 +996,6 @@ 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' ] @@ -1131,22 +1051,24 @@ def configure_env(self, cfg, env): ] # whitelist of compilers which we should build with -Werror - gcc_whitelist = frozenset([ + gcc_whitelist = [ ('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) - else: - env.DEFINES.update(CANARD_ENABLE_TAO_OPTION=1) - if not cfg.options.bootloader and cfg.env.HAL_NUM_CAN_IFACES: - if int(cfg.env.HAL_NUM_CAN_IFACES) >= 1: - env.DEFINES.update(CANARD_IFACE_ALL=(1< 1: + env.DEFINES.update(CANARD_MULTI_IFACE=1) + else: + env.DEFINES.update(CANARD_MULTI_IFACE=0) if cfg.options.Werror or cfg.env.CC_VERSION in gcc_whitelist: cfg.msg("Enabling -Werror", "yes") if '-Werror' not in env.CXXFLAGS: @@ -1191,15 +1113,7 @@ def get_name(self): return self.name class linux(Board): - def __init__(self): - if self.toolchain == 'native': - self.with_can = True - else: - self.with_can = False - def configure_env(self, cfg, env): - if cfg.options.board == 'linux': - self.with_can = True super(linux, self).configure_env(cfg, env) env.DEFINES.update( @@ -1227,33 +1141,8 @@ def configure_env(self, cfg, env): 'AP_HAL_Linux', ] - if cfg.options.force_32bit: - env.DEFINES.update( - HAL_FORCE_32BIT = 1, - ) - # 32bit platform flags - cfg.env.CXXFLAGS += [ - '-m32', - ] - cfg.env.CFLAGS += [ - '-m32', - ] - cfg.env.LDFLAGS += [ - '-m32', - ] - else: - env.DEFINES.update( - HAL_FORCE_32BIT = 0, - ) - if self.with_can and cfg.options.board == 'linux': - cfg.env.HAL_NUM_CAN_IFACES = 2 - cfg.define('HAL_NUM_CAN_IFACES', 2) - cfg.define('HAL_CANFD_SUPPORTED', 1) - cfg.define('CANARD_ENABLE_CANFD', 1) - if self.with_can: - env.DEFINES.update(CANARD_MULTI_IFACE=1, - CANARD_IFACE_ALL = 0x3) + cfg.define('UAVCAN_EXCEPTIONS', 0) if cfg.options.apstatedir: cfg.define('AP_STATEDIR', cfg.options.apstatedir) @@ -1360,7 +1249,7 @@ def __init__(self): def configure_env(self, cfg, env): super(bbbmini, self).configure_env(cfg, env) - cfg.env.HAL_NUM_CAN_IFACES = 1 + env.DEFINES.update( CONFIG_HAL_BOARD_SUBTYPE = 'HAL_BOARD_SUBTYPE_LINUX_BBBMINI', ) @@ -1373,7 +1262,6 @@ def __init__(self): def configure_env(self, cfg, env): super(blue, self).configure_env(cfg, env) - cfg.env.HAL_NUM_CAN_IFACES = 1 env.DEFINES.update( CONFIG_HAL_BOARD_SUBTYPE = 'HAL_BOARD_SUBTYPE_LINUX_BLUE', @@ -1387,7 +1275,6 @@ def __init__(self): def configure_env(self, cfg, env): super(pocket, self).configure_env(cfg, env) - cfg.env.HAL_NUM_CAN_IFACES = 1 env.DEFINES.update( CONFIG_HAL_BOARD_SUBTYPE = 'HAL_BOARD_SUBTYPE_LINUX_POCKET', diff --git a/Tools/ardupilotwaf/build_summary.py b/Tools/ardupilotwaf/build_summary.py index 82f76367565185..d7002543965ce7 100644 --- a/Tools/ardupilotwaf/build_summary.py +++ b/Tools/ardupilotwaf/build_summary.py @@ -49,7 +49,6 @@ '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=''): @@ -171,26 +170,21 @@ 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]) - if ".extflash" in line: - row = line.strip().split() - if int(row[1]) > 0: - ext_flash_used = int(row[1]) + break import re pattern = re.compile("^.*TOTALS.*$") lines = s.splitlines()[1:] l = [] for line in lines: - if pattern.match(line) or totals is False: + if pattern.match(line) or totals==False: row = line.strip().split() # check if crash_log wasn't found @@ -209,9 +203,8 @@ 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]) - ext_flash_used, + size_total=int(row[0]) + int(row[1]), size_free_flash=size_free_flash, - ext_flash_used= ext_flash_used if ext_flash_used else None, )) return l @@ -292,5 +285,4 @@ 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 819b3474c540f1..46d2bb2ca00334 100644 --- a/Tools/ardupilotwaf/chibios.py +++ b/Tools/ardupilotwaf/chibios.py @@ -31,7 +31,7 @@ def _load_dynamic_env_data(bld): # relative paths from the make build are relative to BUILDROOT d = os.path.join(bld.env.BUILDROOT, d) d = os.path.normpath(d) - if d not in idirs2: + if not d in idirs2: idirs2.append(d) _dynamic_env_data['include_dirs'] = idirs2 @@ -64,13 +64,11 @@ 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 = "{} -u '{}/uploader.py' '{}'".format('python.exe', upload_tools, src.abspath()) + cmd = "{} '{}/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): @@ -98,7 +96,7 @@ def wsl2_prereq_checks(self): except subprocess.CalledProcessError: #if where.exe can't find the file it returns a non-zero result which throws this exception where_python = "" - if not where_python or "\Python\Python" not in where_python or "python.exe" not in where_python: + if not where_python or not "\Python\Python" in where_python or "python.exe" not in where_python: print(self.get_full_wsl2_error_msg("Windows python.exe not found")) return False return True @@ -350,7 +348,7 @@ def run(self): class build_abin(Task.Task): '''build an abin file for skyviper firmware upload via web UI''' color='CYAN' - run_str='${TOOLS_SCRIPTS}/make_abin.sh ${SRC} ${TGT}' + run_str='${TOOLS_SCRIPTS}/make_abin.sh ${SRC}.bin ${SRC}.abin' always_run = True def keyword(self): return "Generating" @@ -402,7 +400,7 @@ def chibios_firmware(self): if self.env.BUILD_ABIN: abin_target = self.bld.bldnode.find_or_declare('bin/' + link_output.change_ext('.abin').name) - abin_task = self.create_task('build_abin', src=bin_target, tgt=abin_target) + abin_task = self.create_task('build_abin', src=link_output, tgt=abin_target) abin_task.set_run_after(generate_apj_task) cleanup_task = self.create_task('build_normalized_bins', src=bin_target) @@ -444,22 +442,17 @@ def setup_canmgr_build(cfg): the build based on the presence of CAN pins in hwdef.dat except for AP_Periph builds''' env = cfg.env env.AP_LIBRARIES += [ - 'AP_DroneCAN', - 'modules/DroneCAN/libcanard/*.c', - ] - env.INCLUDES += [ - cfg.srcnode.find_dir('modules/DroneCAN/libcanard').abspath(), + 'AP_UAVCAN', + 'modules/uavcan/libuavcan/src/**/*.cpp', ] + env.CFLAGS += ['-DHAL_CAN_IFACES=2'] - if not env.AP_PERIPH: - env.DEFINES += [ - 'DRONECAN_CXX_WRAPPERS=1', - 'USE_USER_HELPERS=1', - 'CANARD_ENABLE_DEADLINE=1', - 'CANARD_MULTI_IFACE=1', - 'CANARD_ALLOCATE_SEM=1' - ] + env.DEFINES += [ + 'UAVCAN_CPP_VERSION=UAVCAN_CPP03', + 'UAVCAN_NO_ASSERTIONS=1', + 'UAVCAN_NULLPTR=nullptr' + ] if cfg.env.HAL_CANFD_SUPPORTED: env.DEFINES += ['UAVCAN_SUPPORT_CANFD=1'] @@ -495,8 +488,6 @@ 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: @@ -578,8 +569,6 @@ def bldpath(path): load_env_vars(cfg.env) if env.HAL_NUM_CAN_IFACES and not env.AP_PERIPH: setup_canmgr_build(cfg) - if env.HAL_NUM_CAN_IFACES and env.AP_PERIPH and int(env.HAL_NUM_CAN_IFACES)>1 and not env.BOOTLOADER: - env.DEFINES += [ 'CANARD_MULTI_IFACE=1' ] setup_optimization(cfg.env) def generate_hwdef_h(env): @@ -654,22 +643,13 @@ def build(bld): bld( # create the file modules/ChibiOS/include_dirs - 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}'", + 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}'", 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') @@ -681,7 +661,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}' 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, + 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, 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')] @@ -689,7 +669,7 @@ def build(bld): else: ch_task = bld( # build libch.a from ChibiOS sources and hwdef.h - 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, + 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, group='dynamic_sources', source=common_src, target=bld.bldnode.find_or_declare('modules/ChibiOS/libch.a') @@ -720,6 +700,6 @@ def build(bld): 'clearerr', 'fseek', 'ferror', 'fclose', 'tmpfile', 'getc', 'ungetc', 'feof', 'ftell', 'freopen', 'remove', 'vfprintf', 'fscanf', '_gettimeofday', '_times', '_times_r', '_gettimeofday_r', 'time', 'clock', - '_sbrk', '_sbrk_r', '_malloc_r', '_calloc_r', '_free_r'] + '_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 0616de6128ed6f..9ec9691c150ae1 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.env.CMAKE_MIN_VERSION = '3.5.2' + cfg.CMAKE_MIN_VERSION = '3.5.2' cfg.load('cmake') Usage example:: diff --git a/Tools/ardupilotwaf/dronecangen.py b/Tools/ardupilotwaf/dronecangen.py index f6e067da3ea237..dd1d4fcd65c380 100644 --- a/Tools/ardupilotwaf/dronecangen.py +++ b/Tools/ardupilotwaf/dronecangen.py @@ -19,7 +19,7 @@ def run(self): python = self.env.get_flat('PYTHON') out = self.env.get_flat('OUTPUT_DIR') src = self.env.get_flat('SRC') - dsdlc = self.env.get_flat("DC_DSDL_COMPILER") + dsdlc = self.env.get_flat("DSDL_COMPILER") ret = self.exec_command(['{}'.format(python), '{}'.format(dsdlc), @@ -66,7 +66,10 @@ 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' - cfg.msg('DC_DSDL compiler', env.DC_DSDL_COMPILER) + env.DSDL_COMPILER_DIR = cfg.srcnode.make_node('modules/DroneCAN/dronecan_dsdlc/').abspath() + env.DSDL_COMPILER = env.DSDL_COMPILER_DIR + '/dronecan_dsdlc.py' + cfg.msg('DSDL compiler', env.DSDL_COMPILER) diff --git a/Tools/ardupilotwaf/embed.py b/Tools/ardupilotwaf/embed.py index ccc9673985ea10..d833b40c977459 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, '__EXTFLASHFUNC__ static const uint8_t ap_romfs_%u[] = {' % idx) + write_encode(out, '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 20a9528860de18..8631abff3c2d3a 100644 --- a/Tools/ardupilotwaf/esp32.py +++ b/Tools/ardupilotwaf/esp32.py @@ -18,8 +18,7 @@ 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() @@ -31,13 +30,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/'+target+'/esp-idf') + env.AP_HAL_ESP32 = srcpath('libraries/AP_HAL_ESP32/targets/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') @@ -64,11 +63,10 @@ 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/'+target+'/esp-idf', + cmake_src='libraries/AP_HAL_ESP32/targets/esp-idf', cmake_bld='esp-idf_build', ) diff --git a/Tools/ardupilotwaf/mavgen.py b/Tools/ardupilotwaf/mavgen.py index f246d768d5a3fa..0dec86207594d4 100644 --- a/Tools/ardupilotwaf/mavgen.py +++ b/Tools/ardupilotwaf/mavgen.py @@ -44,7 +44,7 @@ def scan(self): node.parent.path_from(entry_point.parent), path ) - if path not in names: + if not path in names: names.append(path) return nodes, names @@ -94,5 +94,8 @@ def configure(cfg): """ setup environment for mavlink header generator """ + cfg.load('python') + cfg.check_python_version(minver=(2,7,0)) + env = cfg.env env.MAVLINK_DIR = cfg.srcnode.make_node('modules/mavlink/').abspath() diff --git a/Tools/ardupilotwaf/toolchain.py b/Tools/ardupilotwaf/toolchain.py index eb86084307cb29..5d242a4fc6c385 100644 --- a/Tools/ardupilotwaf/toolchain.py +++ b/Tools/ardupilotwaf/toolchain.py @@ -119,7 +119,7 @@ def _set_pkgconfig_crosscompilation_wrapper(cfg): @conf def new_validate_cfg(kw): - if 'path' not in kw: + if not 'path' in kw: if not cfg.env.PKGCONFIG: cfg.find_program('%s-pkg-config' % cfg.env.TOOLCHAIN, var='PKGCONFIG') kw['path'] = cfg.env.PKGCONFIG diff --git a/Tools/ardupilotwaf/uavcangen.py b/Tools/ardupilotwaf/uavcangen.py new file mode 100644 index 00000000000000..0ca74c947b6e54 --- /dev/null +++ b/Tools/ardupilotwaf/uavcangen.py @@ -0,0 +1,76 @@ +# encoding: utf-8 + +""" +generate DSDLC headers for uavcan +""" + +from waflib import Logs, Task, Utils, Node +from waflib.TaskGen import feature, before_method, extension +import os +import os.path +from xml.etree import ElementTree as et + +class uavcangen(Task.Task): + """generate uavcan header files""" + color = 'BLUE' + before = 'cxx c' + + def run(self): + python = self.env.get_flat('PYTHON') + out = self.env.get_flat('OUTPUT_DIR') + src = self.env.get_flat('SRC') + dsdlc = self.env.get_flat("DSDL_COMPILER") + + ret = self.exec_command(['{}'.format(python), + '{}'.format(dsdlc), + '-O{}'.format(out)] + [x.abspath() for x in self.inputs]) + + if ret != 0: + # ignore if there was a signal to the interpreter rather + # than a real error in the script. Some environments use a + # signed and some an unsigned return for this + if ret > 128 or ret < 0: + Logs.warn('uavcangen crashed with code: {}'.format(ret)) + ret = 0 + else: + Logs.error('uavcangen returned {} error code'.format(ret)) + return ret + + def post_run(self): + super(uavcangen, self).post_run() + for header in self.generator.output_dir.ant_glob("*.hpp **/*.hpp", remove=False): + header.sig = header.cache_sig = self.cache_sig + +def options(opt): + opt.load('python') + +@feature('uavcangen') +@before_method('process_source') +def process_uavcangen(self): + if not hasattr(self, 'output_dir'): + self.bld.fatal('uavcangen: missing option output_dir') + + inputs = self.to_nodes(self.source) + outputs = [] + + self.source = [] + + if not isinstance(self.output_dir, Node.Node): + self.output_dir = self.bld.bldnode.find_or_declare(self.output_dir) + + task = self.create_task('uavcangen', inputs, outputs) + task.env['OUTPUT_DIR'] = self.output_dir.abspath() + + task.env.env = dict(os.environ) + +def configure(cfg): + """ + setup environment for uavcan header generator + """ + cfg.load('python') + cfg.check_python_version(minver=(2,7,0)) + + env = cfg.env + env.DSDL_COMPILER_DIR = cfg.srcnode.make_node('modules/uavcan/libuavcan/dsdl_compiler').abspath() + env.DSDL_COMPILER = env.DSDL_COMPILER_DIR + '/libuavcan_dsdlc' + cfg.msg('DSDL compiler', env.DSDL_COMPILER) diff --git a/Tools/autotest/ArduCopter_Tests/PayloadPlaceMission/copter_payload_place.txt b/Tools/autotest/ArduCopter_Tests/PayLoadPlaceMission/copter_payload_place.txt similarity index 100% rename from Tools/autotest/ArduCopter_Tests/PayloadPlaceMission/copter_payload_place.txt rename to Tools/autotest/ArduCopter_Tests/PayLoadPlaceMission/copter_payload_place.txt diff --git a/Tools/autotest/ArduPlane_Tests/GUIDEDToAUTO/mission.txt b/Tools/autotest/ArduPlane_Tests/GUIDEDToAUTO/mission.txt deleted file mode 100644 index 12bb5ed5f65e6b..00000000000000 --- a/Tools/autotest/ArduPlane_Tests/GUIDEDToAUTO/mission.txt +++ /dev/null @@ -1,36 +0,0 @@ -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/MicroStrainEAHRS5/ap1.txt b/Tools/autotest/ArduPlane_Tests/LordEAHRS/ap1.txt similarity index 100% rename from Tools/autotest/ArduPlane_Tests/MicroStrainEAHRS5/ap1.txt rename to Tools/autotest/ArduPlane_Tests/LordEAHRS/ap1.txt diff --git a/Tools/autotest/ArduPlane_Tests/MAV_CMD_DO_GO_AROUND/mission.txt b/Tools/autotest/ArduPlane_Tests/MAV_CMD_DO_GO_AROUND/mission.txt deleted file mode 100644 index 41e2938f9c9d60..00000000000000 --- a/Tools/autotest/ArduPlane_Tests/MAV_CMD_DO_GO_AROUND/mission.txt +++ /dev/null @@ -1,8 +0,0 @@ -QGC WPL 110 -0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165238 584.089966 1 -1 0 3 22 0.000000 0.000000 0.000000 0.000000 -35.361438 149.165031 50.000000 1 -2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361505 149.163723 50.000000 1 -3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365845 149.164175 52.219997 1 -4 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.368656 149.165976 54.410000 1 -5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.366148 149.165741 50.849998 1 -6 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.362920 149.165127 50.000000 1 diff --git a/Tools/autotest/Generic_Missions/QuadPlaneDalbyRTL.txt b/Tools/autotest/Generic_Missions/QuadPlaneDalbyRTL.txt deleted file mode 100644 index e88bb0c7fd6c0a..00000000000000 --- a/Tools/autotest/Generic_Missions/QuadPlaneDalbyRTL.txt +++ /dev/null @@ -1,6 +0,0 @@ -QGC WPL 110 -0 0 0 16 0.000000 0.000000 0.000000 0.000000 -27.274439 151.290064 343.209991 1 -1 0 3 189 0.000000 0.000000 0.000000 0.000000 -27.270617 151.283268 28.590000 1 -2 0 3 16 0.000000 0.000000 0.000000 0.000000 -27.272859 151.286018 28.789999 1 -3 0 3 16 0.000000 0.000000 0.000000 0.000000 -27.273316 151.288023 30.000000 1 -4 0 3 85 0.000000 0.000000 0.000000 0.000000 -27.273771 151.289905 0.000000 1 diff --git a/Tools/autotest/antennatracker.py b/Tools/autotest/antennatracker.py index a4cdf01a460d28..ba452956aafd92 100644 --- a/Tools/autotest/antennatracker.py +++ b/Tools/autotest/antennatracker.py @@ -116,36 +116,34 @@ def MANUAL(self): self.set_rc(chan, pwm) self.wait_servo_channel_value(chan, pwm) - def MAV_CMD_DO_SET_SERVO(self): + def SERVOTEST(self): '''Test SERVOTEST mode''' self.change_mode(0) # "MANUAL" # magically changes to SERVOTEST (3) - for method in self.run_cmd, self.run_cmd_int: - for value in 1900, 1200: - channel = 1 - method( - mavutil.mavlink.MAV_CMD_DO_SET_SERVO, - p1=channel, - p2=value, - timeout=1, - ) - self.wait_servo_channel_value(channel, value) - for value in 1300, 1670: - channel = 2 - method( - mavutil.mavlink.MAV_CMD_DO_SET_SERVO, - p1=channel, - p2=value, - timeout=1, - ) - self.wait_servo_channel_value(channel, value) - - def MAV_CMD_MISSION_START(self): - '''test MAV_CMD_MISSION_START mavlink command''' - for method in self.run_cmd, self.run_cmd_int: - self.change_mode(0) # "MANUAL" - method(mavutil.mavlink.MAV_CMD_MISSION_START) - self.wait_mode("AUTO") + for value in 1900, 1200: + channel = 1 + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, + channel, + value, + 0, + 0, + 0, + 0, + 0, + timeout=1) + self.wait_servo_channel_value(channel, value) + for value in 1300, 1670: + channel = 2 + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, + channel, + value, + 0, + 0, + 0, + 0, + 0, + timeout=1) + self.wait_servo_channel_value(channel, value) def SCAN(self): '''Test SCAN mode''' @@ -174,8 +172,7 @@ def tests(self): ret.extend([ self.GUIDED, self.MANUAL, - self.MAV_CMD_DO_SET_SERVO, - self.MAV_CMD_MISSION_START, + self.SERVOTEST, self.NMEAOutput, self.SCAN, ]) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 043c791540b57a..32727063f88377 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -123,6 +123,19 @@ 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_altitude(alt_min-1, alt_min+max_err, relative=True, timeout=timeout) + def takeoff(self, alt_min=30, takeoff_throttle=1700, @@ -391,13 +404,13 @@ def RecordThenPlayMission(self, side=50, timeout=300): self.land_and_disarm() # enter RTL mode and wait for the vehicle to disarm - def do_RTL(self, distance_min=None, check_alt=True, distance_max=10, timeout=250, quiet=False): + def do_RTL(self, distance_min=None, check_alt=True, distance_max=10, timeout=250): """Enter RTL mode and wait for the vehicle to disarm at Home.""" self.change_mode("RTL") self.hover() - self.wait_rtl_complete(check_alt=check_alt, distance_max=distance_max, timeout=timeout, quiet=True) + self.wait_rtl_complete(check_alt=check_alt, distance_max=distance_max, timeout=timeout) - def wait_rtl_complete(self, check_alt=True, distance_max=10, timeout=250, quiet=False): + def wait_rtl_complete(self, check_alt=True, distance_max=10, timeout=250): """Wait for RTL to reach home and disarm""" self.progress("Waiting RTL to reach Home and disarm") tstart = self.get_sim_time() @@ -414,9 +427,8 @@ def wait_rtl_complete(self, check_alt=True, distance_max=10, timeout=250, quiet= else: if distance_valid: home = "HOME" - if not quiet: - self.progress("Alt: %.02f HomeDist: %.02f %s" % - (alt, home_distance, home)) + self.progress("Alt: %.02f HomeDist: %.02f %s" % + (alt, home_distance, home)) # our post-condition is that we are disarmed: if not self.armed(): @@ -688,47 +700,6 @@ 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''' @@ -1159,14 +1130,11 @@ 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, - p7=10, - ) + self.run_cmd(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 10) else: self.set_rc(3, 1700) # we may never see ourselves as armed in a heartbeat - self.wait_statustext("Takeoff blocked: ESC RPM out of range", check_context=True) + self.wait_statustext("Takeoff blocked: ESC RPM too low", check_context=True) self.context_pop() self.zero_throttle() self.disarm_vehicle() @@ -1191,21 +1159,6 @@ 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, @@ -2620,23 +2573,7 @@ def CANGPSCopterMission(self): "CAN_P1_DRIVER": 1, "GPS_TYPE": 9, "GPS_TYPE2": 9, - # 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, + "SIM_GPS2_DISABLE": 0, }) self.context_push() @@ -2645,8 +2582,8 @@ def CANGPSCopterMission(self): self.reboot_sitl() # Test UAVCAN GPS ordering working - gps1_det_text = self.wait_text("GPS 1: specified as DroneCAN.*", regex=True, check_context=True) - gps2_det_text = self.wait_text("GPS 2: specified as DroneCAN.*", regex=True, check_context=True) + gps1_det_text = self.wait_text("GPS 1: specified as UAVCAN.*", regex=True, check_context=True) + gps2_det_text = self.wait_text("GPS 2: specified as UAVCAN.*", regex=True, check_context=True) gps1_nodeid = int(gps1_det_text.split('-')[1]) gps2_nodeid = int(gps2_det_text.split('-')[1]) if gps1_nodeid is None or gps2_nodeid is None: @@ -2676,11 +2613,11 @@ def CANGPSCopterMission(self): gps1_det_text = None gps2_det_text = None try: - gps1_det_text = self.wait_text("GPS 1: specified as DroneCAN.*", regex=True, check_context=True) + gps1_det_text = self.wait_text("GPS 1: specified as UAVCAN.*", regex=True, check_context=True) except AutoTestTimeoutException: pass try: - gps2_det_text = self.wait_text("GPS 2: specified as DroneCAN.*", regex=True, check_context=True) + gps2_det_text = self.wait_text("GPS 2: specified as UAVCAN.*", regex=True, check_context=True) except AutoTestTimeoutException: pass @@ -2704,12 +2641,16 @@ 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, - p1=1, # ARM - timeout=10, - want_result=mavutil.mavlink.MAV_RESULT_FAILED, - ) + 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(case[4], check_context=True) self.context_stop_collecting('STATUSTEXT') self.progress("############################### All GPS Order Cases Tests Passed") @@ -2717,34 +2658,23 @@ 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, - p1=1, # ARM - timeout=10, - want_result=mavutil.mavlink.MAV_RESULT_FAILED, - ) + 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", 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): @@ -3666,8 +3596,8 @@ def test_rangefinder_switchover(self): if ex is not None: raise ex - def _Parachute(self, command): - '''Test Parachute Functionality using specific mavlink command''' + def Parachute(self): + '''Test Parachute Functionality''' self.set_rc(9, 1000) self.set_parameters({ "CHUTE_ENABLED": 1, @@ -3690,10 +3620,14 @@ def _Parachute(self, command): self.progress("Test triggering with mavlink message") self.takeoff(20) - command( - mavutil.mavlink.MAV_CMD_DO_PARACHUTE, - p1=2, # release - ) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_PARACHUTE, + 2, # release + 0, + 0, + 0, + 0, + 0, + 0) self.wait_statustext('BANG', timeout=60) self.disarm_vehicle(force=True) self.reboot_sitl() @@ -3711,10 +3645,15 @@ def _Parachute(self, command): self.progress("Test mavlink triggering") self.takeoff(20) - command( - mavutil.mavlink.MAV_CMD_DO_PARACHUTE, - p1=mavutil.mavlink.PARACHUTE_DISABLE, - ) + 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 + ) ok = False try: self.wait_statustext('BANG', timeout=2) @@ -3722,10 +3661,15 @@ def _Parachute(self, command): ok = True if not ok: raise NotAchievedException("Disabled parachute fired") - command( - mavutil.mavlink.MAV_CMD_DO_PARACHUTE, - p1=mavutil.mavlink.PARACHUTE_ENABLE, - ) + 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 + ) ok = False try: self.wait_statustext('BANG', timeout=2) @@ -3740,10 +3684,15 @@ def _Parachute(self, command): # parachute should not fire if you go from disabled to release: self.takeoff(20) - command( - mavutil.mavlink.MAV_CMD_DO_PARACHUTE, - p1=mavutil.mavlink.PARACHUTE_RELEASE, - ) + 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 + ) ok = False try: self.wait_statustext('BANG', timeout=2) @@ -3753,14 +3702,24 @@ def _Parachute(self, command): raise NotAchievedException("Parachute fired when going straight from disabled to release") # now enable then release parachute: - command( - mavutil.mavlink.MAV_CMD_DO_PARACHUTE, - p1=mavutil.mavlink.PARACHUTE_ENABLE, - ) - command( - mavutil.mavlink.MAV_CMD_DO_PARACHUTE, - p1=mavutil.mavlink.PARACHUTE_RELEASE, - ) + self.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.wait_statustext('BANG! Parachute deployed', timeout=2) self.disarm_vehicle(force=True) self.reboot_sitl() @@ -3800,10 +3759,47 @@ def _Parachute(self, command): self.disarm_vehicle(force=True) self.reboot_sitl() - def Parachute(self): - '''Test Parachute Functionality''' - self._Parachute(self.run_cmd) - self._Parachute(self.run_cmd_int) + def MotorTest(self, timeout=60): + '''Run Motor Tests''' + self.start_subtest("Testing PWM output") + pwm_in = 1300 + # default frame is "+" - start motor of 2 is "B", which is + # motor 1... see + # https://ardupilot.org/copter/docs/connect-escs-and-motors.html + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, + 2, # start motor + mavutil.mavlink.MOTOR_TEST_THROTTLE_PWM, + pwm_in, # pwm-to-output + 2, # timeout in seconds + 2, # number of motors to output + 0, # compass learning + 0, + timeout=timeout) + # long timeouts here because there's a pause before we start motors + self.wait_servo_channel_value(1, pwm_in, timeout=10) + self.wait_servo_channel_value(4, pwm_in, timeout=10) + self.wait_statustext("finished motor test") + self.end_subtest("Testing PWM output") + + self.start_subtest("Testing percentage output") + percentage = 90.1 + # since MOT_SPIN_MIN and MOT_SPIN_MAX are not set, the RC3 + # min/max are used. + expected_pwm = 1000 + (self.get_parameter("RC3_MAX") - self.get_parameter("RC3_MIN")) * percentage/100.0 + self.progress("expected pwm=%f" % expected_pwm) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, + 2, # start motor + mavutil.mavlink.MOTOR_TEST_THROTTLE_PERCENT, + percentage, # pwm-to-output + 2, # timeout in seconds + 2, # number of motors to output + 0, # compass learning + 0, + timeout=timeout) + self.wait_servo_channel_value(1, expected_pwm, timeout=10) + self.wait_servo_channel_value(4, expected_pwm, timeout=10) + self.wait_statustext("finished motor test") + self.end_subtest("Testing percentage output") def PrecisionLanding(self): """Use PrecLand backends precision messages to land aircraft.""" @@ -4366,7 +4362,7 @@ def fly_guided_move_local(self, x, y, z_up, timeout=100): MAV_POS_TARGET_TYPE_MASK.POS_ONLY | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE, # mask specifying use-only-x-y-z x, # x y, # y - -z_up, # z + -z_up,# z 0, # vx 0, # vy 0, # vz @@ -4599,7 +4595,7 @@ def precision_loiter_to_pos(self, x, y, z, timeout=40): 0.01 # size of target in radians, Y-axis ) - def PayloadPlaceMission(self): + def PayLoadPlaceMission(self): """Test payload placing in auto.""" self.context_push() @@ -4696,74 +4692,6 @@ def Weathervane(self): self.wait_heading(100, accuracy=8, timeout=100) self.do_RTL() - def _DO_WINCH(self, command): - self.context_push() - self.load_default_params_file("copter-winch.parm") - self.reboot_sitl() - self.wait_ready_to_arm() - - self.start_subtest("starts relaxed") - self.wait_servo_channel_value(9, 0) - - self.start_subtest("rate control") - command( - mavutil.mavlink.MAV_CMD_DO_WINCH, - p1=1, # instance number - p2=mavutil.mavlink.WINCH_RATE_CONTROL, # command - p3=0, # length to release - p4=1, # rate in m/s - ) - self.wait_servo_channel_value(9, 1900) - - self.start_subtest("relax") - command( - mavutil.mavlink.MAV_CMD_DO_WINCH, - p1=1, # instance number - p2=mavutil.mavlink.WINCH_RELAXED, # command - p3=0, # length to release - p4=1, # rate in m/s - ) - self.wait_servo_channel_value(9, 0) - - self.start_subtest("hold but zero output") - command( - mavutil.mavlink.MAV_CMD_DO_WINCH, - p1=1, # instance number - p2=mavutil.mavlink.WINCH_RATE_CONTROL, # command - p3=0, # length to release - p4=0, # rate in m/s - ) - self.wait_servo_channel_value(9, 1500) - - self.start_subtest("relax") - command( - mavutil.mavlink.MAV_CMD_DO_WINCH, - p1=1, # instance number - p2=mavutil.mavlink.WINCH_RELAXED, # command - p3=0, # length to release - p4=1, # rate in m/s - ) - self.wait_servo_channel_value(9, 0) - - self.start_subtest("position") - command( - mavutil.mavlink.MAV_CMD_DO_WINCH, - p1=1, # instance number - p2=mavutil.mavlink.WINCH_RELATIVE_LENGTH_CONTROL, # command - p3=2, # length to release - p4=1, # rate in m/s - ) - self.wait_servo_channel_value(9, 1900) - self.wait_servo_channel_value(9, 1500, timeout=60) - - self.context_pop() - self.reboot_sitl() - - def DO_WINCH(self): - '''test mavlink DO_WINCH command''' - self._DO_WINCH(self.run_cmd_int) - self._DO_WINCH(self.run_cmd) - def GuidedSubModeChange(self): """"Ensure we can move around in guided after a takeoff command.""" @@ -4878,10 +4806,15 @@ def ManualThrottleModeChange(self): self.progress("Check setting an invalid mode") self.run_cmd( mavutil.mavlink.MAV_CMD_DO_SET_MODE, - p1=mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, - p2=126, + mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, + 126, + 0, + 0, + 0, + 0, + 0, want_result=mavutil.mavlink.MAV_RESULT_FAILED, - timeout=1, + timeout=1 ) self.set_rc(3, 1000) self.run_cmd_do_set_mode("ACRO") @@ -4908,7 +4841,8 @@ def test_mount_pitch(self, despitch, despitch_tolerance, mount_mode, timeout=10, (mount_pitch, despitch)) if success_start == 0: success_start = now - if now - success_start >= hold: + continue + if now - success_start > hold: self.progress("Mount pitch achieved") return @@ -4925,17 +4859,6 @@ def do_pitch(self, pitch): 0, # yaw rate (rad/s) 0.5) # thrust, 0 to 1, translated to a climb/descent rate - def do_yaw_rate(self, yaw_rate): - '''yaw aircraft in guided/rate mode''' - self.run_cmd( - mavutil.mavlink.MAV_CMD_CONDITION_YAW, - p1=60, # target angle - p2=0, # degrees/second - p3=1, # -1 is counter-clockwise, 1 clockwise - p4=1, # 1 for relative, 0 for absolute - quiet=True, - ) - def setup_servo_mount(self, roll_servo=5, pitch_servo=6, yaw_servo=7): '''configure a rpy servo mount; caller responsible for required rebooting''' self.progress("Setting up servo mount") @@ -4961,107 +4884,11 @@ def get_mount_roll_pitch_yaw_deg(self): def set_mount_mode(self, mount_mode): '''set mount mode''' - self.run_cmd_int( - mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE, - p1=mount_mode, - p2=0, # stabilize roll (unsupported) - p3=0, # stabilize pitch (unsupported) - ) - self.run_cmd( - mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE, - p1=mount_mode, - p2=0, # stabilize roll (unsupported) - p3=0, # stabilize pitch (unsupported) - ) - - def test_mount_rc_targetting(self): - '''called in multipleplaces to make sure that mount RC targetting works''' - try: - self.context_push() - self.set_parameters({ - 'RC6_OPTION': 0, - 'RC11_OPTION': 212, # MOUNT1_ROLL - 'RC12_OPTION': 213, # MOUNT1_PITCH - 'RC13_OPTION': 214, # MOUNT1_YAW - 'RC12_MIN': 1100, - 'RC12_MAX': 1900, - 'RC12_TRIM': 1500, - 'MNT1_PITCH_MIN': -45, - 'MNT1_PITCH_MAX': 45, - }) - self.progress("Testing RC angular control") - # default RC min=1100 max=1900 - self.set_rc_from_map({ - 11: 1500, - 12: 1500, - 13: 1500, - }) - self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.progress("Testing RC input down 1/4 of its range in the output, should be down 1/4 range in output") - rc12_in = 1400 - rc12_min = 1100 # default - rc12_max = 1900 # default - mpitch_min = -45.0 - mpitch_max = 45.0 - expected_pitch = (float(rc12_in-rc12_min)/float(rc12_max-rc12_min) * (mpitch_max-mpitch_min)) + mpitch_min - self.progress("expected mount pitch: %f" % expected_pitch) - if expected_pitch != -11.25: - raise NotAchievedException("Calculation wrong - defaults changed?!") - self.set_rc(12, rc12_in) - self.test_mount_pitch(-11.25, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.set_rc(12, 1800) - self.test_mount_pitch(33.75, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.set_rc_from_map({ - 11: 1500, - 12: 1500, - 13: 1500, - }) - - try: - self.context_push() - self.set_parameters({ - "RC12_MIN": 1000, - "RC12_MAX": 2000, - "MNT1_PITCH_MIN": -90, - "MNT1_PITCH_MAX": 10, - }) - self.set_rc(12, 1000) - self.test_mount_pitch(-90.00, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.set_rc(12, 2000) - self.test_mount_pitch(10.00, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.set_rc(12, 1500) - self.test_mount_pitch(-40.00, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - finally: - self.context_pop() - - self.set_rc(12, 1500) - - self.progress("Testing RC rate control") - self.set_parameter('MNT1_RC_RATE', 10) - self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.set_rc(12, 1300) - self.test_mount_pitch(-5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_pitch(-10, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_pitch(-15, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_pitch(-20, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.set_rc(12, 1700) - self.test_mount_pitch(-15, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_pitch(-10, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_pitch(-5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_pitch(5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - - self.progress("Reverting to angle mode") - self.set_parameter('MNT1_RC_RATE', 0) - self.set_rc(12, 1500) - self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - - self.context_pop() - - except Exception as e: - self.print_exception_caught(e) - self.context_pop() - raise e + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE, + mount_mode, + 0, # stabilize roll (unsupported) + 0, # stabilize pitch (unsupported) + 0, 0, 0, 0) def Mount(self): '''Test Camera/Antenna Mount''' @@ -5127,18 +4954,15 @@ 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) - for (method, angle) in (self.run_cmd, -20), (self.run_cmd_int, -30): - method( - mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW, - p1=angle, # pitch angle in degrees - p2=0, # yaw angle in degrees - p3=0, # pitch rate in degrees (NaN to ignore) - p4=0, # yaw rate in degrees (NaN to ignore) - p5=0, # flags (0=Body-frame, 16/GIMBAL_MANAGER_FLAGS_YAW_LOCK=Earth Frame) - p6=0, # unused - p7=0, # gimbal id - ) - self.test_mount_pitch(angle, 1, mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW, + -20, # pitch angle in degrees + 0, # yaw angle in degrees + 0, # pitch rate in degrees (NaN to ignore) + 0, # yaw rate in degrees (NaN to ignore) + 0, # flags (0=Body-frame, 16/GIMBAL_MANAGER_FLAGS_YAW_LOCK=Earth Frame) + 0, # unused + 0) # gimbal id + self.test_mount_pitch(-20, 1, mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING) # point gimbal at specified location self.progress("Point gimbal at Location using MOUNT_CONTROL (GPS)") @@ -5166,6 +4990,9 @@ def Mount(self): ) self.test_mount_pitch(-52, 5, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT) + # now test RC targetting + self.progress("Testing mount RC targetting") + # this is a one-off; ArduCopter *will* time out this directive! self.progress("Levelling aircraft") self.mav.mav.set_attitude_target_send( @@ -5179,13 +5006,94 @@ def Mount(self): 0, # yaw rate (rad/s) 0.5) # thrust, 0 to 1, translated to a climb/descent rate - self.wait_groundspeed(0, 1) + self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - # now test RC targetting - self.progress("Testing mount RC targetting") + try: + self.context_push() + self.set_parameters({ + 'RC6_OPTION': 0, + 'RC11_OPTION': 212, # MOUNT1_ROLL + 'RC12_OPTION': 213, # MOUNT1_PITCH + 'RC13_OPTION': 214, # MOUNT1_YAW + 'RC12_MIN': 1100, + 'RC12_MAX': 1900, + 'RC12_TRIM': 1500, + 'MNT1_PITCH_MIN': -45, + 'MNT1_PITCH_MAX': 45, + }) + self.progress("Testing RC angular control") + # default RC min=1100 max=1900 + self.set_rc_from_map({ + 11: 1500, + 12: 1500, + 13: 1500, + }) + self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.progress("Testing RC input down 1/4 of its range in the output, should be down 1/4 range in output") + rc12_in = 1400 + rc12_min = 1100 # default + rc12_max = 1900 # default + mpitch_min = -45.0 + mpitch_max = 45.0 + expected_pitch = (float(rc12_in-rc12_min)/float(rc12_max-rc12_min) * (mpitch_max-mpitch_min)) + mpitch_min + self.progress("expected mount pitch: %f" % expected_pitch) + if expected_pitch != -11.25: + raise NotAchievedException("Calculation wrong - defaults changed?!") + self.set_rc(12, rc12_in) + self.test_mount_pitch(-11.25, 0.01, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.set_rc(12, 1800) + self.test_mount_pitch(33.75, 0.01, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.set_rc_from_map({ + 11: 1500, + 12: 1500, + 13: 1500, + }) - self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_rc_targetting() + try: + self.context_push() + self.set_parameters({ + "RC12_MIN": 1000, + "RC12_MAX": 2000, + "MNT1_PITCH_MIN": -90, + "MNT1_PITCH_MAX": 10, + }) + self.set_rc(12, 1000) + self.test_mount_pitch(-90.00, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.set_rc(12, 2000) + self.test_mount_pitch(10.00, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.set_rc(12, 1500) + self.test_mount_pitch(-40.00, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + finally: + self.context_pop() + + self.set_rc(12, 1500) + + self.progress("Testing RC rate control") + self.set_parameter('MNT1_RC_RATE', 10) + self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.set_rc(12, 1300) + self.test_mount_pitch(-5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.test_mount_pitch(-10, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.test_mount_pitch(-15, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.test_mount_pitch(-20, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.set_rc(12, 1700) + self.test_mount_pitch(-15, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.test_mount_pitch(-10, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.test_mount_pitch(-5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + self.test_mount_pitch(5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + + self.progress("Reverting to angle mode") + self.set_parameter('MNT1_RC_RATE', 0) + self.set_rc(12, 1500) + self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + + self.context_pop() + + except Exception as e: + self.print_exception_caught(e) + self.context_pop() + raise e self.progress("Testing mount ROI behaviour") self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) @@ -5197,35 +5105,27 @@ 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, - 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.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION, + 0, + 0, + 0, + 0, + roi_lat, + roi_lon, + roi_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) - self.run_cmd_int(mavutil.mavlink.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.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) start = self.mav.location() @@ -5235,12 +5135,15 @@ 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, - p5=roi_lat, - p6=roi_lon, - p7=roi_alt, - ) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI, + 0, + 0, + 0, + 0, + roi_lat, + roi_lon, + roi_alt, + ) self.test_mount_pitch(-7.5, 1, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT) start = self.mav.location() @@ -5290,10 +5193,15 @@ 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, - p1=self.mav.source_system, - ) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI_SYSID, + self.mav.source_system, + 0, + 0, + 0, + 0, + 0, + 0, + ) self.mav.mav.global_position_int_send( 0, # time boot ms int(roi_lat * 1e7), @@ -5307,11 +5215,6 @@ 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), @@ -5341,193 +5244,6 @@ def Mount(self): if ex is not None: raise ex - def assert_mount_rpy(self, r, p, y, tolerance=1): - '''assert mount atttiude in degrees''' - got_r, got_p, got_y = self.get_mount_roll_pitch_yaw_deg() - for (want, got, name) in (r, got_r, "roll"), (p, got_p, "pitch"), (y, got_y, "yaw"): - if abs(want - got) > tolerance: - raise NotAchievedException("%s incorrect; want=%f got=%f" % - (name, want, got)) - - def neutralise_gimbal(self): - '''put mount into neutralise mode, assert it is at zero angles''' - self.run_cmd( - mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, - p7=mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL, - ) - self.test_mount_pitch(0, 0, mavutil.mavlink.MAV_MOUNT_MODE_RETRACT) - - def MAV_CMD_DO_MOUNT_CONTROL(self): - '''test MAV_CMD_DO_MOUNT_CONTROL mavlink command''' - - # setup mount parameters - self.context_push() - self.setup_servo_mount() - self.reboot_sitl() # to handle MNT_TYPE changing - - takeoff_loc = self.mav.location() - - self.takeoff(20, mode='GUIDED') - self.guided_achieve_heading(315) - - self.run_cmd( - mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, - p7=mavutil.mavlink.MAV_MOUNT_MODE_RETRACT, - ) - self.run_cmd_int( - mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, - p7=mavutil.mavlink.MAV_MOUNT_MODE_RETRACT, - ) - - for method in self.run_cmd, self.run_cmd_int: - self.start_subtest("MAV_MOUNT_MODE_GPS_POINT") - - self.progress("start=%s" % str(takeoff_loc)) - t = self.offset_location_ne(takeoff_loc, 20, 0) - self.progress("targetting=%s" % str(t)) - - # this command is *weird* as the lat/lng is *always* 1e7, - # even when transported via COMMAND_LONG! - x = int(t.lat * 1e7) - y = int(t.lng * 1e7) - method( - mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, - p4=0, # this is a relative altitude! - p5=x, - p6=y, - p7=mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT, - ) - self.test_mount_pitch(-45, 5, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT) - self.neutralise_gimbal() - - self.start_subtest("MAV_MOUNT_MODE_HOME_LOCATION") - method( - mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, - p7=mavutil.mavlink.MAV_MOUNT_MODE_HOME_LOCATION, - ) - self.test_mount_pitch(-90, 5, mavutil.mavlink.MAV_MOUNT_MODE_HOME_LOCATION) - self.neutralise_gimbal() - - # try an invalid mount mode. Note that this is asserting we - # are receiving a result code which is actually incorrect; - # this should be MAV_RESULT_DENIED - self.start_subtest("Invalid mode") - method( - mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, - p7=87, - want_result=mavutil.mavlink.MAV_RESULT_FAILED, - ) - - self.start_subtest("MAV_MOUNT_MODE_MAVLINK_TARGETING") - r = 15 - p = 20 - y = 30 - method( - mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, - p1=p, - p2=r, - p3=y, - p7=mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, - ) - self.delay_sim_time(2) - self.assert_mount_rpy(r, p, y) - self.neutralise_gimbal() - - self.start_subtest("MAV_MOUNT_MODE_RC_TARGETING") - method( - mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, - p7=mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, - ) - self.test_mount_rc_targetting() - - self.start_subtest("MAV_MOUNT_MODE_RETRACT") - self.context_push() - retract_r = 13 - retract_p = 23 - retract_y = 33 - self.set_parameters({ - "MNT1_RETRACT_X": retract_r, - "MNT1_RETRACT_Y": retract_p, - "MNT1_RETRACT_Z": retract_y, - }) - method( - mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, - p7=mavutil.mavlink.MAV_MOUNT_MODE_RETRACT, - ) - self.delay_sim_time(3) - self.assert_mount_rpy(retract_r, retract_p, retract_y) - self.context_pop() - - self.do_RTL() - - self.context_pop() - self.reboot_sitl() - - def MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE(self): - '''test MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE mavlink command''' - # setup mount parameters - self.context_push() - self.setup_servo_mount() - self.reboot_sitl() # to handle MNT_TYPE changing - - self.context_set_message_rate_hz('GIMBAL_MANAGER_STATUS', 10) - self.assert_received_message_field_values('GIMBAL_MANAGER_STATUS', { - "gimbal_device_id": 1, - "primary_control_sysid": 0, - "primary_control_compid": 0, - }) - - for method in self.run_cmd, self.run_cmd_int: - self.start_subtest("set_sysid-compid") - method( - mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, - p1=37, - p2=38, - ) - self.assert_received_message_field_values('GIMBAL_MANAGER_STATUS', { - "gimbal_device_id": 1, - "primary_control_sysid": 37, - "primary_control_compid": 38, - }) - - self.start_subtest("leave unchanged") - method(mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, p1=-1) - self.assert_received_message_field_values('GIMBAL_MANAGER_STATUS', { - "gimbal_device_id": 1, - "primary_control_sysid": 37, - "primary_control_compid": 38, - }) - - # ardupilot currently handles this incorrectly: - # self.start_subtest("self-controlled") - # method(mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, p1=-2) - # self.assert_received_message_field_values('GIMBAL_MANAGER_STATUS', { - # "gimbal_device_id": 1, - # "primary_control_sysid": 1, - # "primary_control_compid": 1, - # }) - - self.start_subtest("release control") - method( - mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, - p1=self.mav.source_system, - p2=self.mav.source_component, - ) - self.assert_received_message_field_values('GIMBAL_MANAGER_STATUS', { - "gimbal_device_id": 1, - "primary_control_sysid": self.mav.source_system, - "primary_control_compid": self.mav.source_component, - }) - method(mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, p1=-3) - self.assert_received_message_field_values('GIMBAL_MANAGER_STATUS', { - "gimbal_device_id": 1, - "primary_control_sysid": 0, - "primary_control_compid": 0, - }) - - self.context_pop() - self.reboot_sitl() - def MountYawVehicleForMountROI(self): '''Test Camera/Antenna Mount vehicle yawing for ROI''' self.context_push() @@ -5557,12 +5273,15 @@ 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, - p5=roi_lat, - p6=roi_lon, - p7=roi_alt, - ) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI, + 0, + 0, + 0, + 0, + roi_lat, + roi_lon, + roi_alt, + ) self.progress("Waiting for vehicle to point towards ROI") self.wait_heading(225, timeout=600, minimum_duration=2) @@ -5596,12 +5315,15 @@ 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, - p5=roi_lat, - p6=roi_lon, - p7=roi_alt, - ) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI, + 0, + 0, + 0, + 0, + roi_lat, + roi_lon, + roi_alt, + ) self.progress("Wait for vehicle to point sssse due to moving") self.wait_heading(170, timeout=600, minimum_duration=1) @@ -7010,8 +6732,8 @@ def OBSTACLE_DISTANCE_3D(self): self.context_pop() self.reboot_sitl() - def AC_Avoidance_Proximity(self): - '''Test proximity avoidance slide behaviour''' + def fly_proximity_avoidance_test_corners(self): + self.start_subtest("Corners") self.context_push() ex = None try: @@ -7050,14 +6772,14 @@ def ProximitySensors(self): }) sensors = [ # tuples of name, prx_type ('sf45b', 8, { - 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, + 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, }), ('rplidara2', 5, { mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 277, @@ -7118,8 +6840,8 @@ def ProximitySensors(self): "Distance too great (%s) (want=%s != got=%s)" % (name, wants, gots)) - def AC_Avoidance_Proximity_AVOID_ALT_MIN(self): - '''Test proximity avoidance with AVOID_ALT_MIN''' + def fly_proximity_avoidance_test_alt_no_avoid(self): + self.start_subtest("Alt-no-avoid") self.context_push() ex = None try: @@ -7149,8 +6871,14 @@ def AC_Avoidance_Proximity_AVOID_ALT_MIN(self): mavutil.mavlink.MAV_SENSOR_ROTATION_NONE, # orientation 255 # covariance ) - self.send_mavlink_arm_command() - + self.send_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + 1, # ARM + 0, + 0, + 0, + 0, + 0, + 0) self.takeoff(15, mode='LOITER') self.progress("Poking vehicle; should avoid") @@ -7193,11 +6921,27 @@ 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.load_fence("copter-avoidance-fence.txt") - self.set_parameter("FENCE_ENABLE", 1) - self.check_avoidance_corners() + 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 def global_position_int_for_location(self, loc, time_boot, heading=0): return self.mav.mav.global_position_int_encode( @@ -7675,38 +7419,6 @@ def AuxFunctionsInMission(self): self.change_mode('AUTO') self.wait_rtl_complete() - def MAV_CMD_AIRFRAME_CONFIGURATION(self): - '''deploy/retract landing gear using mavlink command''' - self.context_push() - self.set_parameters({ - "LGR_ENABLE": 1, - "SERVO10_FUNCTION": 29, - "SERVO10_MIN": 1001, - "SERVO10_MAX": 1999, - }) - self.reboot_sitl() - - # starts loose: - self.wait_servo_channel_value(10, 0) - - # 0 is down: - self.start_subtest("Put gear down") - self.run_cmd(mavutil.mavlink.MAV_CMD_AIRFRAME_CONFIGURATION, p2=0) - self.wait_servo_channel_value(10, 1999) - - # 1 is up: - self.start_subtest("Put gear up") - self.run_cmd_int(mavutil.mavlink.MAV_CMD_AIRFRAME_CONFIGURATION, p2=1) - self.wait_servo_channel_value(10, 1001) - - # 0 is down: - self.start_subtest("Put gear down") - self.run_cmd(mavutil.mavlink.MAV_CMD_AIRFRAME_CONFIGURATION, p2=0) - self.wait_servo_channel_value(10, 1999) - - self.context_pop() - self.reboot_sitl() - def WatchAlts(self): '''Ensure we can monitor different altitudes''' self.takeoff(30, mode='GUIDED') @@ -7943,7 +7655,13 @@ def fly_rangefinder_mavlink_distance_sensor(self): self.run_cmd( mavutil.mavlink.MAV_CMD_AIRFRAME_CONFIGURATION, - p2=0, # deploy + 0, + 0, # deploy + 0, + 0, + 0, + 0, + 0 ) self.context_collect("STATUSTEXT") @@ -8001,19 +7719,29 @@ def fly_rangefinder_mavlink_distance_sensor(self): def GSF(self): '''test the Gaussian Sum filter''' + ex = None self.context_push() - 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() + 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.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 @@ -8023,46 +7751,6 @@ 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() @@ -8211,7 +7899,6 @@ def RangeFinderDrivers(self): ("benewake_tf03", 27), ("gyus42v2", 31), ("teraranger_serial", 35), - ("nooploop_tofsense", 37), ] while len(drivers): do_drivers = drivers[0:3] @@ -8616,9 +8303,7 @@ 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)) @@ -8637,7 +8322,7 @@ def FlyEachFrame(self): # to carry the path to the JSON. actual_model = model.split(":")[0] defaults = self.model_defaults_filepath(actual_model) - if not isinstance(defaults, list): + if type(defaults) != list: defaults = [defaults] self.customise_SITL_commandline( ["--defaults", ','.join(defaults), ], @@ -8653,10 +8338,9 @@ def verify_yaw(mav, m): if m.yawspeed > yawspeed_thresh_rads: raise NotAchievedException("Excessive yaw on takeoff: %f deg/s > %f deg/s (frame=%s)" % (math.degrees(m.yawspeed), math.degrees(yawspeed_thresh_rads), frame)) - self.context_push() - self.install_message_hook_context(verify_yaw) + self.install_message_hook(verify_yaw) self.takeoff(10) - self.context_pop() + self.remove_message_hook(verify_yaw) self.hover() self.change_mode('ALT_HOLD') self.delay_sim_time(1) @@ -8672,14 +8356,13 @@ def verify_rollpitch(mav, m): if m.roll > roll_thresh_rad: raise NotAchievedException("Excessive roll %f deg > %f deg" % (math.degrees(m.roll), math.degrees(roll_thresh_rad))) - self.context_push() - self.install_message_hook_context(verify_rollpitch) + self.install_message_hook(verify_rollpitch) for i in range(5): self.set_rc(4, 2000) self.delay_sim_time(0.5) self.set_rc(4, 1500) self.delay_sim_time(5) - self.context_pop() + self.remove_message_hook(verify_rollpitch) self.do_RTL() @@ -9011,7 +8694,7 @@ def GroundEffectCompensation_takeOffExpected(self): raise NotAchievedException("Was expecting takeoff for longer than expected; got=%f want<=%f" % (duration, want_lt)) - def _MAV_CMD_CONDITION_YAW(self, command): + def MAV_CMD_CONDITION_YAW_absolute(self): self.start_subtest("absolute") self.takeoff(20, mode='GUIDED') @@ -9020,15 +8703,17 @@ def _MAV_CMD_CONDITION_YAW(self, command): self.progress("Ensuring initial heading is steady") target = initial_heading - command( + self.run_cmd( mavutil.mavlink.MAV_CMD_CONDITION_YAW, - p1=target, # target angle - p2=10, # degrees/second - p3=1, # -1 is counter-clockwise, 1 clockwise - p4=0, # 1 for relative, 0 for absolute + 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 ) self.wait_heading(target, minimum_duration=2, timeout=50) - self.wait_yaw_speed(0) degsecond = 2 @@ -9042,12 +8727,15 @@ def rate_watcher(mav, m): self.progress("Yaw CW 60 degrees") target = initial_heading + 60 part_way_target = initial_heading + 10 - command( + self.run_cmd( mavutil.mavlink.MAV_CMD_CONDITION_YAW, - p1=target, # target angle - p2=degsecond, # degrees/second - p3=1, # -1 is counter-clockwise, 1 clockwise - p4=0, # 1 for relative, 0 for absolute + 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 ) self.wait_heading(part_way_target) self.wait_heading(target, minimum_duration=2) @@ -9055,27 +8743,28 @@ def rate_watcher(mav, m): self.progress("Yaw CCW 60 degrees") target = initial_heading part_way_target = initial_heading + 30 - command( + self.run_cmd( mavutil.mavlink.MAV_CMD_CONDITION_YAW, - p1=target, # target angle - p2=degsecond, # degrees/second - p3=-1, # -1 is counter-clockwise, 1 clockwise - p4=0, # 1 for relative, 0 for absolute + 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 ) self.wait_heading(part_way_target) self.wait_heading(target, minimum_duration=2) - self.disarm_vehicle(force=True) - self.reboot_sitl() + self.do_RTL() + + def MAV_CMD_CONDITION_YAW_relative(self): + pass def MAV_CMD_CONDITION_YAW(self): - '''Test response to MAV_CMD_CONDITION_YAW via mavlink''' - self.context_push() - self._MAV_CMD_CONDITION_YAW(self.run_cmd_int) - self.context_pop() - self.context_push() - self._MAV_CMD_CONDITION_YAW(self.run_cmd) - self.context_pop() + '''Test response to MAV_CMD_CONDITION_YAW''' + self.MAV_CMD_CONDITION_YAW_absolute() + self.MAV_CMD_CONDITION_YAW_relative() def GroundEffectCompensation_touchDownExpected(self): '''Test EKF's handling of touchdown-expected''' @@ -9420,6 +9109,9 @@ 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, @@ -9457,9 +9149,8 @@ def AUTO_LAND_TO_BRAKE(self): self.set_analog_rangefinder_parameters() - self.load_mission('mission.txt') self.customise_SITL_commandline([ - "--home", self.sitl_home_string_from_mission("mission.txt"), + "--home", "%s,%s,%s,%s" % home_loc ]) self.set_parameter('AUTO_OPTIONS', 3) @@ -9850,7 +9541,14 @@ def Sprayer(self): self.start_subtest("Checking mavlink commands") self.progress("Starting Sprayer") - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SPRAYER, p1=1) + 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( @@ -9861,8 +9559,14 @@ def Sprayer(self): ) self.start_subtest("Stopping Sprayer") - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SPRAYER, p1=0) - + 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.context_pop() @@ -9882,7 +9586,7 @@ def tests1a(self): self.GuidedSubModeChange, self.MAV_CMD_CONDITION_YAW, self.LoiterToAlt, - self.PayloadPlaceMission, + self.PayLoadPlaceMission, self.PrecisionLoiterCompanion, self.Landing, self.PrecisionLanding, @@ -9903,7 +9607,6 @@ def tests1b(self): self.BrakeMode, self.RecordThenPlayMission, self.ThrottleFailsafe, - self.ThrottleFailsafePassthrough, self.GCSFailsafe, self.CustomController, ]) @@ -9918,14 +9621,12 @@ 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, ]) @@ -9975,8 +9676,6 @@ def tests1e(self): self.RTLSpeed, self.Mount, self.MountYawVehicleForMountROI, - self.MAV_CMD_DO_MOUNT_CONTROL, - self.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, self.Button, self.ShipTakeoff, self.RangeFinder, @@ -10001,10 +9700,6 @@ def tests1e(self): self.IE24, self.MAVLandedStateTakeoff, self.Weathervane, - self.MAV_CMD_AIRFRAME_CONFIGURATION, - self.MAV_CMD_NAV_LOITER_UNLIM, - self.MAV_CMD_NAV_RETURN_TO_LAUNCH, - self.MAV_CMD_NAV_VTOL_LAND, ]) return ret @@ -10036,7 +9731,7 @@ def ScriptMountPOI(self): self.setup_servo_mount() self.reboot_sitl() self.set_rc(6, 1300) - self.install_applet_script_context('mount-poi.lua') + self.install_example_script_context('mount-poi.lua') self.reboot_sitl() self.wait_ready_to_arm() self.context_collect('STATUSTEXT') @@ -10061,315 +9756,6 @@ def AHRSTrimLand(self): self.context_pop() self.reboot_sitl() - def turn_safety_x(self, value): - self.mav.mav.set_mode_send( - self.mav.target_system, - mavutil.mavlink.MAV_MODE_FLAG_DECODE_POSITION_SAFETY, - value) - - def turn_safety_off(self): - self.turn_safety_x(0) - - def turn_safety_on(self): - self.turn_safety_x(1) - - def SafetySwitch(self): - '''test safety switch behaviour''' - self.wait_ready_to_arm() - - self.turn_safety_on() - self.assert_prearm_failure("safety switch") - - self.turn_safety_off() - self.wait_ready_to_arm() - - self.takeoff(2, mode='LOITER') - self.turn_safety_on() - - self.wait_servo_channel_value(1, 0) - self.turn_safety_off() - - self.change_mode('LAND') - self.wait_disarmed() - - def GuidedYawRate(self): - '''ensuer guided yaw rate is not affected by rate of sewt-attitude messages''' - self.takeoff(30, mode='GUIDED') - rates = {} - for rate in 1, 10: - # command huge yaw rate for a while - tstart = self.get_sim_time() - interval = 1/rate - yawspeed_rads_sum = 0 - yawspeed_rads_count = 0 - last_sent = 0 - while True: - self.drain_mav() - tnow = self.get_sim_time_cached() - if tnow - last_sent > interval: - self.do_yaw_rate(60) # this is... unlikely - last_sent = tnow - if tnow - tstart < 5: # let it spin up to speed first - continue - yawspeed_rads_sum += self.mav.messages['ATTITUDE'].yawspeed - yawspeed_rads_count += 1 - if tnow - tstart > 15: # 10 seconds of measurements - break - yawspeed_degs = math.degrees(yawspeed_rads_sum / yawspeed_rads_count) - rates[rate] = yawspeed_degs - self.progress("Input rate %u hz: average yaw rate %f deg/s" % (rate, yawspeed_degs)) - - if rates[10] < rates[1] * 0.95: - raise NotAchievedException("Guided yaw rate slower for higher rate updates") - - self.do_RTL() - - def test_rplidar(self, sim_device_name, expected_distances): - '''plonks a Copter with a RPLidarA2 in the middle of a simulated field - of posts and checks that the measurements are what we expect.''' - self.set_parameters({ - "SERIAL5_PROTOCOL": 11, - "PRX1_TYPE": 5, - }) - self.customise_SITL_commandline([ - "--uartF=sim:%s:" % sim_device_name, - "--home", "51.8752066,14.6487840,0,0", # SITL has "posts" here - ]) - - self.wait_ready_to_arm() - - wanting_distances = copy.copy(expected_distances) - tstart = self.get_sim_time() - timeout = 60 - while True: - now = self.get_sim_time_cached() - if now - tstart > timeout: - raise NotAchievedException("Did not get all distances") - m = self.mav.recv_match(type="DISTANCE_SENSOR", - blocking=True, - timeout=1) - if m is None: - continue - self.progress("Got (%s)" % str(m)) - if m.orientation not in wanting_distances: - continue - if abs(m.current_distance - wanting_distances[m.orientation]) > 5: - self.progress("Wrong distance orient=%u want=%u got=%u" % - (m.orientation, - wanting_distances[m.orientation], - m.current_distance)) - continue - self.progress("Correct distance for orient %u (want=%u got=%u)" % - (m.orientation, - wanting_distances[m.orientation], - m.current_distance)) - del wanting_distances[m.orientation] - if len(wanting_distances.items()) == 0: - break - - def RPLidarA2(self): - '''test Raspberry Pi Lidar A2''' - expected_distances = { - mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 276, - mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 256, - mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 1130, - mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 1286, - mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 626, - mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 971, - mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 762, - mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 792, - } - - self.test_rplidar("rplidara2", expected_distances) - - def RPLidarA1(self): - '''test Raspberry Pi Lidar A1''' - return # we don't send distances when too long? - expected_distances = { - mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 276, - mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 256, - mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 800, - mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 800, - mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 626, - mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 800, - mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 762, - mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 792, - } - - self.test_rplidar("rplidara1", expected_distances) - - def BrakeZ(self): - '''check jerk limit correct in Brake mode''' - self.set_parameter('PSC_JERK_Z', 3) - self.takeoff(50, mode='GUIDED') - vx, vy, vz_up = (0, 0, -1) - self.test_guided_local_velocity_target(vx=vx, vy=vy, vz_up=vz_up, timeout=10) - - self.wait_for_local_velocity(vx=vx, vy=vy, vz_up=vz_up, timeout=10) - self.change_mode('BRAKE') - self.wait_for_local_velocity(vx=0, vy=0, vz_up=0, timeout=10) - self.land_and_disarm() - - def MISSION_START(self): - '''test mavlink command MAV_CMD_MISSION_START''' - self.upload_simple_relhome_mission([ - (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 200), - (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0), - ]) - for command in self.run_cmd, self.run_cmd_int: - self.change_mode('LOITER') - self.set_current_waypoint(1) - self.wait_ready_to_arm() - self.arm_vehicle() - self.change_mode('AUTO') - command(mavutil.mavlink.MAV_CMD_MISSION_START) - self.wait_altitude(20, 1000, relative=True) - self.change_mode('RTL') - self.wait_disarmed() - - def DO_CHANGE_SPEED_in_guided(self): - '''test Copter DO_CHANGE_SPEED handling in guided mode''' - self.takeoff(20, mode='GUIDED') - - new_loc = self.mav.location() - new_loc_offset_n = 2000 - new_loc_offset_e = 0 - self.location_offset_ne(new_loc, new_loc_offset_n, new_loc_offset_e) - - second_loc_offset_n = -1000 - second_loc_offset_e = 0 - second_loc = self.mav.location() - self.location_offset_ne(second_loc, second_loc_offset_n, second_loc_offset_e) - - # for run_cmd we fly away from home - for (tloc, command) in (new_loc, self.run_cmd), (second_loc, self.run_cmd_int): - self.run_cmd_int( - mavutil.mavlink.MAV_CMD_DO_REPOSITION, - p1=-1, # "default" - p2=0, # flags; none supplied here - p3=0, # loiter radius for planes, zero ignored - p4=float("nan"), # nan means do whatever you want to do - p5=int(tloc.lat * 1e7), - p6=int(tloc.lng * 1e7), - p7=tloc.alt, - frame=mavutil.mavlink.MAV_FRAME_GLOBAL, - ) - for speed in [2, 10, 4]: - command( - mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, - p1=1, # groundspeed, - p2=speed, - p3=-1, # throttle, -1 is no-change - p4=0, # absolute value, not relative - ) - self.wait_groundspeed(speed-0.2, speed+0.2, minimum_duration=10, timeout=20) - - # we've made random changes to vehicle guided speeds above; - # reboot vehicle to reset those: - self.disarm_vehicle(force=True) - self.reboot_sitl() - - def _MAV_CMD_DO_FLIGHTTERMINATION(self, command): - self.set_parameters({ - "SYSID_MYGCS": self.mav.source_system, - "DISARM_DELAY": 0, - }) - self.wait_ready_to_arm() - self.arm_vehicle() - self.context_collect('STATUSTEXT') - command(mavutil.mavlink.MAV_CMD_DO_FLIGHTTERMINATION, p1=1) - self.wait_disarmed() - self.reboot_sitl() - - def MAV_CMD_DO_FLIGHTTERMINATION(self): - '''test MAV_CMD_DO_FLIGHTTERMINATION works on Copter''' - self._MAV_CMD_DO_FLIGHTTERMINATION(self.run_cmd) - self._MAV_CMD_DO_FLIGHTTERMINATION(self.run_cmd_int) - - def MAV_CMD_NAV_LOITER_UNLIM(self): - '''ensure MAV_CMD_NAV_LOITER_UNLIM via mavlink works''' - for command in self.run_cmd, self.run_cmd_int: - self.change_mode('STABILIZE') - command(mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM) - self.wait_mode('LOITER') - - def MAV_CMD_NAV_RETURN_TO_LAUNCH(self): - '''ensure MAV_CMD_NAV_RETURN_TO_LAUNCH via mavlink works''' - for command in self.run_cmd, self.run_cmd_int: - self.change_mode('STABILIZE') - command(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH) - self.wait_mode('RTL') - - def MAV_CMD_NAV_VTOL_LAND(self): - '''ensure MAV_CMD_NAV_LAND via mavlink works''' - for command in self.run_cmd, self.run_cmd_int: - self.change_mode('STABILIZE') - command(mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND) - self.wait_mode('LAND') - self.change_mode('STABILIZE') - command(mavutil.mavlink.MAV_CMD_NAV_LAND) - self.wait_mode('LAND') - - def start_flying_simple_rehome_mission(self, items): - '''uploads items, changes mode to auto, waits ready to arm and arms - vehicle. If the first item it a takeoff you can expect the - vehicle to fly after this method returns - ''' - - self.upload_simple_relhome_mission(items) - - self.set_parameter("AUTO_OPTIONS", 3) - self.change_mode('AUTO') - self.wait_ready_to_arm() - - self.arm_vehicle() - - def _MAV_CMD_DO_LAND_START(self, run_cmd): - alt = 5 - self.start_flying_simple_rehome_mission([ - (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, alt), - (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 200, 0, alt), - (mavutil.mavlink.MAV_CMD_NAV_LAND, 0, 0, 0), - (mavutil.mavlink.MAV_CMD_DO_LAND_START, 0, 0, alt), - (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 200, 2000, alt), - (mavutil.mavlink.MAV_CMD_NAV_LAND, 0, 0, 0), - ]) - - self.wait_current_waypoint(2) - run_cmd(mavutil.mavlink.MAV_CMD_DO_LAND_START) - self.wait_current_waypoint(5) - # we pretend to be in RTL mode while doing this: - self.wait_mode("AUTO_RTL") - self.do_RTL() - - def MAV_CMD_DO_LAND_START(self): - '''test handling of mavlink-received MAV_CMD_DO_LAND_START command''' - self._MAV_CMD_DO_LAND_START(self.run_cmd) - self.zero_throttle() - self._MAV_CMD_DO_LAND_START(self.run_cmd_int) - - def _MAV_CMD_SET_EKF_SOURCE_SET(self, run_cmd): - run_cmd( - mavutil.mavlink.MAV_CMD_SET_EKF_SOURCE_SET, - 17, - want_result=mavutil.mavlink.MAV_RESULT_DENIED, - ) - - self.change_mode('LOITER') - self.wait_ready_to_arm() - - run_cmd(mavutil.mavlink.MAV_CMD_SET_EKF_SOURCE_SET, 2) - - self.assert_prearm_failure('Need Position Estimate') - run_cmd(mavutil.mavlink.MAV_CMD_SET_EKF_SOURCE_SET, 1) - - self.wait_ready_to_arm() - - def MAV_CMD_SET_EKF_SOURCE_SET(self): - '''test setting of source sets using mavlink command''' - self._MAV_CMD_SET_EKF_SOURCE_SET(self.run_cmd) - self._MAV_CMD_SET_EKF_SOURCE_SET(self.run_cmd_int) - def tests2b(self): # this block currently around 9.5mins here '''return list of all tests''' ret = ([ @@ -10390,7 +9776,6 @@ 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, @@ -10406,12 +9791,10 @@ def tests2b(self): # this block currently around 9.5mins here self.GroundEffectCompensation_touchDownExpected, self.GroundEffectCompensation_takeOffExpected, self.DO_CHANGE_SPEED, - self.MISSION_START, self.AUTO_LAND_TO_BRAKE, self.WPNAV_SPEED, self.WPNAV_SPEED_UP, self.WPNAV_SPEED_DN, - self.DO_WINCH, self.SensorErrorFlags, self.GPSForYaw, self.DefaultIntervalsFromFiles, @@ -10427,16 +9810,6 @@ def tests2b(self): # this block currently around 9.5mins here self.FlyMissionTwice, self.IMUConsistency, self.AHRSTrimLand, - self.GuidedYawRate, - self.NoArmWithoutMissionItems, - self.DO_CHANGE_SPEED_in_guided, - self.RPLidarA1, - self.RPLidarA2, - self.SafetySwitch, - self.BrakeZ, - self.MAV_CMD_DO_FLIGHTTERMINATION, - self.MAV_CMD_DO_LAND_START, - self.MAV_CMD_SET_EKF_SOURCE_SET, ]) return ret diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index e6060a0bde2e79..1864e33a8d3b3f 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -20,12 +20,10 @@ 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 @@ -94,26 +92,9 @@ def get_disarm_delay(self): def set_autodisarm_delay(self, delay): self.set_parameter("LAND_DISARMDELAY", delay) - def takeoff(self, alt=150, alt_max=None, relative=True, mode=None, timeout=None): + def takeoff(self, alt=150, alt_max=None, relative=True): """Takeoff to altitude.""" - if mode == "TAKEOFF": - return self.takeoff_in_TAKEOFF(alt=alt, relative=relative, timeout=timeout) - - return self.takeoff_in_FBWA(alt=alt, alt_max=alt_max, relative=relative, timeout=timeout) - - def takeoff_in_TAKEOFF(self, alt=150, relative=True, mode=None, alt_epsilon=2, timeout=None): - if relative is not True: - raise ValueError("Only relative alt supported ATM") - self.change_mode("TAKEOFF") - self.context_push() - self.set_parameter('TKOFF_ALT', alt) - self.wait_ready_to_arm() - self.arm_vehicle() - self.wait_altitude(alt-alt_epsilon, alt+alt_epsilon, relative=True, timeout=timeout) - self.context_pop() - - def takeoff_in_FBWA(self, alt=150, alt_max=None, relative=True, mode=None, timeout=30): if alt_max is None: alt_max = alt + 30 @@ -146,7 +127,7 @@ def takeoff_in_FBWA(self, alt=150, alt_max=None, relative=True, mode=None, timeo }) # gain a bit of altitude - self.wait_altitude(alt, alt_max, timeout=timeout, relative=relative) + self.wait_altitude(alt, alt_max, timeout=30, relative=relative) # level off self.set_rc(2, 1500) @@ -339,83 +320,78 @@ 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) - 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_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() + try: - 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"]) + 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) except Exception as e: self.change_mode('FBWA') self.set_rc(3, 1700) @@ -585,156 +561,19 @@ 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), - 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 + int(loc.lat * 1e7), + int(loc.lng * 1e7), + new_alt, # alt + frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, ) + 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): @@ -837,36 +676,18 @@ def SmartBattery(self): if not self.current_onboard_log_contains_message("BCL2"): raise NotAchievedException("Expected BCL2 message") - def context_push_do_change_speed(self): + def DO_CHANGE_SPEED(self): + '''Test DO_CHANGE_SPEED command/item''' # the following lines ensure we revert these parameter values # - DO_CHANGE_AIRSPEED is a permanent vehicle change! - self.context_push() self.set_parameters({ "TRIM_ARSPD_CM": self.get_parameter("TRIM_ARSPD_CM"), "MIN_GNDSPD_CM": self.get_parameter("MIN_GNDSPD_CM"), - "TRIM_THROTTLE": self.get_parameter("TRIM_THROTTLE"), - }) - - def DO_CHANGE_SPEED(self): - '''Test DO_CHANGE_SPEED command/item''' - self.set_parameters({ "RTL_AUTOLAND": 1, }) - self.context_push_do_change_speed() - self.DO_CHANGE_SPEED_mavlink_long() - self.context_pop() - - self.set_current_waypoint(1) - self.zero_throttle() - - self.context_push_do_change_speed() - self.DO_CHANGE_SPEED_mavlink_int() - self.context_pop() - - self.context_push_do_change_speed() + self.DO_CHANGE_SPEED_mavlink() self.DO_CHANGE_SPEED_mission() - self.context_pop() def DO_CHANGE_SPEED_mission(self): '''test DO_CHANGE_SPEED as a mission item''' @@ -896,25 +717,23 @@ def DO_CHANGE_SPEED_mission(self): self.fly_home_land_and_disarm() - def DO_CHANGE_SPEED_mavlink_int(self): - self.DO_CHANGE_SPEED_mavlink(self.run_cmd_int) - - def DO_CHANGE_SPEED_mavlink_long(self): - self.DO_CHANGE_SPEED_mavlink(self.run_cmd) - - def DO_CHANGE_SPEED_mavlink(self, run_cmd_method): + def DO_CHANGE_SPEED_mavlink(self): '''test DO_CHANGE_SPEED as a mavlink command''' self.progress("Takeoff") - self.takeoff(alt=100, mode="TAKEOFF", timeout=120) + self.takeoff(alt=100) self.set_rc(3, 1500) # ensure we know what the airspeed is: self.progress("Entering guided and flying somewhere constant") self.change_mode("GUIDED") self.run_cmd_int( mavutil.mavlink.MAV_CMD_DO_REPOSITION, - p5=12345, # lat* 1e7 - p6=12345, # lon* 1e7 - p7=100 # alt + 0, + 0, + 0, + 0, + 12345, # lat* 1e7 + 12345, # lon* 1e7 + 100 # alt ) self.delay_sim_time(10) self.progress("Ensuring initial speed is known and relatively constant") @@ -922,42 +741,47 @@ def DO_CHANGE_SPEED_mavlink(self, run_cmd_method): timeout = 15 self.wait_airspeed(initial_speed-1, initial_speed+1, minimum_duration=5, timeout=timeout) - self.start_subtest("Setting groundspeed") - for new_target_groundspeed in initial_speed + 5, initial_speed + 2: - run_cmd_method( - mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, - p1=1, # groundspeed - p2=new_target_groundspeed, - p3=-1, # throttle / no change - p4=0, # absolute values - ) - self.wait_groundspeed(new_target_groundspeed-2, new_target_groundspeed+2, timeout=80, minimum_duration=5) - self.progress("Adding some wind, ensuring groundspeed holds") - self.set_parameter("SIM_WIND_SPD", 5) - self.delay_sim_time(5) - self.wait_groundspeed(new_target_groundspeed-2, new_target_groundspeed+2, timeout=40, minimum_duration=5) - self.set_parameter("SIM_WIND_SPD", 0) + self.progress("Setting groundspeed") + new_target_groundspeed = initial_speed + 5 + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, + 1, # groundspeed + new_target_groundspeed, + -1, # throttle / no change + 0, # absolute values + 0, + 0, + 0) + self.wait_groundspeed(new_target_groundspeed-0.5, new_target_groundspeed+0.5, timeout=40) + self.progress("Adding some wind, ensuring groundspeed holds") + self.set_parameter("SIM_WIND_SPD", 5) + self.delay_sim_time(5) + self.wait_groundspeed(new_target_groundspeed-0.5, new_target_groundspeed+0.5, timeout=40) + self.set_parameter("SIM_WIND_SPD", 0) # clear target groundspeed - run_cmd_method( + self.run_cmd( mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, - p1=1, # groundspeed - p2=0, - p3=-1, # throttle / no change - p4=0, # absolute values - ) - - self.start_subtest("Setting airspeed") - for new_target_airspeed in initial_speed - 5, initial_speed + 5: - run_cmd_method( - mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, - p1=0, # airspeed - p2=new_target_airspeed, - p3=-1, # throttle / no change - p4=0, # absolute values - ) - self.wait_airspeed(new_target_airspeed-2, new_target_airspeed+2, minimum_duration=5) + 1, # groundspeed + 0, + -1, # throttle / no change + 0, # absolute values + 0, + 0, + 0) + self.progress("Setting airspeed") + new_target_airspeed = initial_speed + 5 + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, + 0, # airspeed + new_target_airspeed, + -1, # throttle / no change + 0, # absolute values + 0, + 0, + 0) + self.wait_airspeed(new_target_airspeed-0.5, new_target_airspeed+0.5) self.context_push() self.progress("Adding some wind, hoping groundspeed increases/decreases") self.set_parameters({ @@ -977,38 +801,6 @@ def DO_CHANGE_SPEED_mavlink(self, run_cmd_method): if delta > want_delta: break self.context_pop() - - # cancel minimum groundspeed: - run_cmd_method( - mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, - p1=0, # groundspeed - p2=-2, # return to default - p3=0, # throttle / no change - p4=0, # absolute values - ) - # cancel airspeed: - run_cmd_method( - mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, - p1=1, # airspeed - p2=-2, # return to default - p3=0, # throttle / no change - p4=0, # absolute values - ) - - self.start_subtest("Setting throttle") - self.set_parameter('ARSPD_USE', 0) # setting throttle only effective without airspeed - for (set_throttle, expected_throttle) in (97, 79), (60, 51), (95, 77): - run_cmd_method( - mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, - p1=3, # throttle - p2=0, - p3=set_throttle, # throttle / no change - p4=0, # absolute values - ) - self.wait_message_field_values('VFR_HUD', { - "throttle": expected_throttle, - }, minimum_duration=5, epsilon=2) - self.fly_home_land_and_disarm(timeout=240) def fly_home_land_and_disarm(self, timeout=120): @@ -1507,31 +1299,6 @@ 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 @@ -1836,82 +1603,6 @@ def FenceRetRally(self): self.do_fence_disable() # Disable fence so we can land self.fly_home_land_and_disarm() # Pack it up, we're going home. - def TerrainRally(self): - """ Tests terrain follow with a rally point """ - self.context_push() - self.install_terrain_handlers_context() - - def terrain_following_above_80m(mav, m): - if m.get_type() == 'TERRAIN_REPORT': - if m.current_height < 50: - raise NotAchievedException( - "TERRAIN_REPORT.current_height below 50m %fm" % m.current_height) - if m.get_type() == 'VFR_HUD': - if m.groundspeed < 2: - raise NotAchievedException("hit ground") - - def terrain_wait_path(loc1, loc2, steps): - '''wait till we have terrain for N steps from loc1 to loc2''' - tstart = self.get_sim_time_cached() - self.progress("Waiting for terrain data") - while True: - now = self.get_sim_time_cached() - if now - tstart > 60: - raise NotAchievedException("Did not get correct required terrain") - for i in range(steps): - lat = loc1.lat + i * (loc2.lat-loc1.lat)/steps - lon = loc1.lng + i * (loc2.lng-loc1.lng)/steps - self.mav.mav.terrain_check_send(int(lat*1.0e7), int(lon*1.0e7)) - - report = self.assert_receive_message('TERRAIN_REPORT', timeout=60) - self.progress("Terrain pending=%u" % report.pending) - if report.pending == 0: - break - self.progress("Got required terrain") - - self.wait_ready_to_arm() - self.homeloc = self.mav.location() - - guided_loc = mavutil.location(-35.39723762, 149.07284612, 99.0, 0) - rally_loc = mavutil.location(-35.3654952000, 149.1558698000, 100, 0) - - terrain_wait_path(self.homeloc, rally_loc, 10) - - # set a rally point to the west of home - self.upload_rally_points_from_locations([rally_loc]) - - self.set_parameter("TKOFF_ALT", 100) - self.change_mode("TAKEOFF") - self.wait_ready_to_arm() - self.arm_vehicle() - self.set_parameter("TERRAIN_FOLLOW", 1) - self.wait_altitude(90, 120, timeout=30, relative=True) - self.progress("Done takeoff") - - self.install_message_hook_context(terrain_following_above_80m) - - self.change_mode("GUIDED") - self.do_reposition(guided_loc, frame=mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT) - self.progress("Flying to guided location") - self.wait_location(guided_loc, - accuracy=200, - target_altitude=None, - timeout=600) - - self.progress("Reached guided location") - self.set_parameter("RALLY_LIMIT_KM", 50) - self.change_mode("RTL") - self.progress("Flying to rally point") - self.wait_location(rally_loc, - accuracy=200, - target_altitude=None, - timeout=600) - self.progress("Reached rally point") - - self.context_pop() - self.disarm_vehicle(force=True) - self.reboot_sitl() - def Parachute(self): '''Test Parachute''' self.set_rc(9, 1000) @@ -2200,11 +1891,13 @@ 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, - p1=0, - p2=mavutil.mavlink.MAV_DO_REPOSITION_FLAGS_CHANGE_MODE, - p5=int(loc.lat * 1e7), - p6=int(loc.lng * 1e7), - p7=100, # alt + 0, + mavutil.mavlink.MAV_DO_REPOSITION_FLAGS_CHANGE_MODE, + 0, + 0, + int(loc.lat * 1e7), + int(loc.lng * 1e7), + 100, # alt frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, ) self.wait_location(loc, accuracy=100) @@ -3138,11 +2831,9 @@ 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, - p5=4, - timeout=5, - ) + self.run_cmd(mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, + 0, 0, 0, 0, 4, 0, 0, + timeout=5) self.wait_ready_to_arm() self.arm_vehicle() @@ -3236,9 +2927,9 @@ def VectorNavEAHRS(self): '''Test VectorNav EAHRS support''' self.fly_external_AHRS("VectorNav", 1, "ap1.txt") - def MicroStrainEAHRS5(self): - '''Test MicroStrain EAHRS series 5 support''' - self.fly_external_AHRS("MicroStrain5", 2, "ap1.txt") + def LordEAHRS(self): + '''Test LORD Microstrain EAHRS support''' + self.fly_external_AHRS("LORD", 2, "ap1.txt") def get_accelvec(self, m): return Vector3(m.xacc, m.yacc, m.zacc) * 0.001 * 9.81 @@ -3301,17 +2992,13 @@ def IMUTempCal(self): self.set_parameter("SIM_IMUT_FIXED", 12) self.progress("Running accel cal") - self.run_cmd( - mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, - p5=4, - timeout=5, - ) + self.run_cmd(mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, + 0, 0, 0, 0, 4, 0, 0, + timeout=5) self.progress("Running gyro cal") - self.run_cmd( - mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, - p5=1, - timeout=5, - ) + self.run_cmd(mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, + 0, 0, 0, 0, 1, 0, 0, + timeout=5) self.set_parameters({ "SIM_IMUT_FIXED": 0, "INS_TCAL1_ENABLE": 2, @@ -3551,9 +3238,13 @@ def fail_speed(): loc = self.mav.location() self.run_cmd_int( mavutil.mavlink.MAV_CMD_DO_REPOSITION, - p5=int(loc.lat * 1e7), - p6=int(loc.lng * 1e7), - p7=50, # alt + 0, + 0, + 0, + 0, + int(loc.lat * 1e7), + int(loc.lng * 1e7), + 50 # alt ) self.delay_sim_time(5) # create an airspeed sensor error by freezing to the @@ -3564,10 +3255,13 @@ def fail_speed(): self.set_parameter("SIM_ARSPD_FAIL", m.airspeed) self.run_cmd( mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, - p1=0, # airspeed - p2=30, - p3=-1, # throttle / no change - p4=0, # absolute values + 0, # airspeed + 30, + -1, # throttle / no change + 0, # absolute values + 0, + 0, + 0 ) 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]: @@ -3869,36 +3563,29 @@ def attempt_fence_breached_disable(start_mode, end_mode, expected_mode, action): attempt_fence_breached_disable(start_mode="FBWA", end_mode="FBWA", expected_mode="GUIDED", action=6) attempt_fence_breached_disable(start_mode="FBWA", end_mode="FBWA", expected_mode="GUIDED", action=7) - def _MAV_CMD_DO_AUX_FUNCTION(self, run_cmd): + def MAV_DO_AUX_FUNCTION(self): '''Test triggering Auxiliary Functions via mavlink''' self.context_collect('STATUSTEXT') - self.run_auxfunc(64, 2, run_cmd=run_cmd) # 64 == reverse throttle + self.run_auxfunc(64, 2) # 64 == reverse throttle self.wait_statustext("RevThrottle: ENABLE", check_context=True) - self.run_auxfunc(64, 0, run_cmd=run_cmd) + self.run_auxfunc(64, 0) self.wait_statustext("RevThrottle: DISABLE", check_context=True) - self.run_auxfunc(65, 2, run_cmd=run_cmd) # 65 == GPS_DISABLE + self.run_auxfunc(65, 2) # 65 == GPS_DISABLE self.start_subtest("Bad auxfunc") self.run_auxfunc( 65231, 2, - want_result=mavutil.mavlink.MAV_RESULT_FAILED, - run_cmd=run_cmd, + want_result=mavutil.mavlink.MAV_RESULT_FAILED ) self.start_subtest("Bad switchpos") self.run_auxfunc( 62, 17, - want_result=mavutil.mavlink.MAV_RESULT_DENIED, - run_cmd=run_cmd, + want_result=mavutil.mavlink.MAV_RESULT_DENIED ) - def MAV_CMD_DO_AUX_FUNCTION(self): - '''Test triggering Auxiliary Functions via mavlink''' - self._MAV_CMD_DO_AUX_FUNCTION(run_cmd=self.run_cmd) - self._MAV_CMD_DO_AUX_FUNCTION(run_cmd=self.run_cmd_int) - def FlyEachFrame(self): '''Fly each supported internal frame''' vinfo = vehicleinfo.VehicleInfo() @@ -3911,7 +3598,6 @@ 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)) @@ -3930,7 +3616,7 @@ def FlyEachFrame(self): # to carry the path to the JSON. actual_model = model.split(":")[0] defaults = self.model_defaults_filepath(actual_model) - if not isinstance(defaults, list): + if type(defaults) != list: defaults = [defaults] self.customise_SITL_commandline( ["--defaults", ','.join(defaults), ], @@ -3996,9 +3682,13 @@ def WatchdogHome(self): new_home.altitude = new_home.altitude + 300000 # 300 metres self.run_cmd_int( mavutil.mavlink.MAV_CMD_DO_SET_HOME, - p5=new_home.latitude, - p6=new_home.longitude, - p7=new_home.altitude/1000.0, # mm => m + 0, # p1, + 0, # p2, + 0, # p3, + 0, # p4, + new_home.latitude, + new_home.longitude, + new_home.altitude/1000.0, # mm => m ) old_bootcount = self.get_parameter('STAT_BOOTCNT') self.progress("Forcing watchdog reset") @@ -4075,18 +3765,6 @@ 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) @@ -4670,13 +4348,25 @@ 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, - p1=jump_target + 1, + jump_target + 1, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + 0, # p5 + 0, # p6 + 0, # p7 want_result=mavutil.mavlink.MAV_RESULT_FAILED ) self.progress("Checking correct tag behaviour") self.run_cmd( mavutil.mavlink.MAV_CMD_DO_JUMP_TAG, - p1=jump_target, + jump_target, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + 0, # p5 + 0, # p6 + 0, # p7 ) self.assert_current_waypoint(4) @@ -4714,7 +4404,13 @@ 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, - p1=17, + 17, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + 0, # p5 + 0, # p6 + 0, # p7 want_result=mavutil.mavlink.MAV_RESULT_FAILED ) self.disarm_vehicle() @@ -4758,10 +4454,8 @@ def AltResetBadGPS(self): # reboot to clear potentially bad state def trigger_airspeed_cal(self): - self.run_cmd( - mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, - p3=1, - ) + self.run_cmd(mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, + 0, 0, 1, 0, 0, 0, 0) def AirspeedCal(self): '''test Airspeed calibration''' @@ -4778,7 +4472,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', check_context=True) + self.wait_statustext('No airspeed sensor present or enabled', check_context=True) self.trigger_airspeed_cal() self.delay_sim_time(5) if self.statustext_in_collections('Airspeed 1 calibrated'): @@ -4798,338 +4492,6 @@ def AirspeedCal(self): self.reboot_sitl() - def RunMissionScript(self): - '''Test run_mission.py script''' - script = os.path.join('Tools', 'autotest', 'run_mission.py') - self.stop_SITL() - util.run_cmd([ - util.reltopdir(script), - self.binary, - 'plane', - self.generic_mission_filepath_for_filename("flaps.txt"), - ]) - self.start_SITL() - - def MAV_CMD_GUIDED_CHANGE_ALTITUDE(self): - '''test handling of MAV_CMD_GUIDED_CHANGE_ALTITUDE''' - self.takeoff(30, relative=True) - self.change_mode('GUIDED') - for alt in 50, 70: - self.run_cmd_int( - mavutil.mavlink.MAV_CMD_GUIDED_CHANGE_ALTITUDE, - p7=alt, - frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, - ) - self.wait_altitude(alt-1, alt+1, timeout=30, relative=True) - - # test for #24535 - self.change_mode('LOITER') - self.delay_sim_time(5) - self.change_mode('GUIDED') - self.wait_altitude( - alt-3, # NOTE: reuse of alt from above loop! - alt+3, - minimum_duration=10, - timeout=30, - relative=True, - ) - self.fly_home_land_and_disarm() - - def _MAV_CMD_PREFLIGHT_CALIBRATION(self, command): - self.context_push() - self.start_subtest("Denied when armed") - self.wait_ready_to_arm() - self.arm_vehicle() - command( - mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, - p1=1, - want_result=mavutil.mavlink.MAV_RESULT_FAILED, - ) - self.disarm_vehicle() - - self.context_collect('STATUSTEXT') - - self.start_subtest("gyro cal") - command( - mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, - p1=1, - ) - - self.start_subtest("baro cal") - command( - mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, - p3=1, - ) - self.wait_statustext('Barometer calibration complete', check_context=True) - - # accelcal skipped here, it is checked elsewhere - - self.start_subtest("ins trim") - command( - mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, - p5=2, - ) - - # enforced delay between cals: - self.delay_sim_time(5) - - self.start_subtest("simple accel cal") - command( - mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, - p5=4, - ) - # simple gyro cal makes the GPS units go unhealthy as they are - # not maintaining their update rate (gyro cal is synchronous - # in the main loop). Usually ~30 seconds to recover... - self.wait_gps_sys_status_not_present_or_enabled_and_healthy(timeout=60) - - self.start_subtest("force save accels") - command( - mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, - p5=76, - ) - - self.start_subtest("force save compasses") - command( - mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, - p2=76, - ) - - self.context_pop() - - def MAV_CMD_PREFLIGHT_CALIBRATION(self): - '''test MAV_CMD_PREFLIGHT_CALIBRATION mavlink handling''' - self._MAV_CMD_PREFLIGHT_CALIBRATION(self.run_cmd) - self._MAV_CMD_PREFLIGHT_CALIBRATION(self.run_cmd_int) - - def MAV_CMD_DO_INVERTED_FLIGHT(self): - '''fly upside-down mission item''' - alt = 30 - wps = self.create_simple_relhome_mission([ - (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, alt), - (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 400, 0, alt), - self.create_MISSION_ITEM_INT( - mavutil.mavlink.MAV_CMD_DO_INVERTED_FLIGHT, - p1=1, - ), - (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 800, 0, alt), - self.create_MISSION_ITEM_INT( - mavutil.mavlink.MAV_CMD_DO_INVERTED_FLIGHT, - p1=0, - ), - (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1200, 0, alt), - (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0), - ]) - self.check_mission_upload_download(wps) - - self.change_mode('AUTO') - self.wait_ready_to_arm() - - self.arm_vehicle() - - self.wait_current_waypoint(2) # upright flight - self.wait_message_field_values("NAV_CONTROLLER_OUTPUT", { - "nav_roll": 0, - "nav_pitch": 0, - }, epsilon=10) - - def check_altitude(mav, m): - global initial_airspeed_threshold_reached - m_type = m.get_type() - if m_type != 'GLOBAL_POSITION_INT': - return - if abs(30 - m.relative_alt * 0.001) > 15: - raise NotAchievedException("Bad altitude while flying inverted") - - self.context_push() - self.install_message_hook_context(check_altitude) - - self.wait_current_waypoint(4) # inverted flight - self.wait_message_field_values("NAV_CONTROLLER_OUTPUT", { - "nav_roll": 180, - "nav_pitch": 9, - }, epsilon=10,) - - self.wait_current_waypoint(6) # upright flight - self.wait_message_field_values("NAV_CONTROLLER_OUTPUT", { - "nav_roll": 0, - "nav_pitch": 0, - }, epsilon=10) - - self.context_pop() # remove the check_altitude call - - self.wait_current_waypoint(7) - - self.fly_home_land_and_disarm() - - def MAV_CMD_DO_AUTOTUNE_ENABLE(self): - '''test enabling autotune via mavlink''' - self.context_collect('STATUSTEXT') - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_AUTOTUNE_ENABLE, p1=1) - self.wait_statustext('Started autotune', check_context=True) - self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_AUTOTUNE_ENABLE, p1=0) - self.wait_statustext('Stopped autotune', check_context=True) - - def DO_PARACHUTE(self): - '''test triggering parachute via mavlink''' - self.set_parameters({ - "CHUTE_ENABLED": 1, - "CHUTE_TYPE": 10, - "SERVO9_FUNCTION": 27, - "SIM_PARA_ENABLE": 1, - "SIM_PARA_PIN": 9, - "FS_LONG_ACTN": 3, - }) - for command in self.run_cmd, self.run_cmd_int: - self.wait_servo_channel_value(9, 1100) - self.wait_ready_to_arm() - self.arm_vehicle() - command( - mavutil.mavlink.MAV_CMD_DO_PARACHUTE, - p1=mavutil.mavlink.PARACHUTE_RELEASE, - ) - self.wait_servo_channel_value(9, 1300) - self.disarm_vehicle() - self.reboot_sitl() - - def _MAV_CMD_DO_GO_AROUND(self, command): - self.load_mission("mission.txt") - self.set_parameter("RTL_AUTOLAND", 3) - self.change_mode('AUTO') - self.wait_ready_to_arm() - self.arm_vehicle() - self.wait_current_waypoint(6) - command(mavutil.mavlink.MAV_CMD_DO_GO_AROUND, p1=150) - self.wait_current_waypoint(5) - self.wait_altitude(135, 165, relative=True) - self.wait_disarmed(timeout=300) - - def MAV_CMD_DO_GO_AROUND(self): - '''test MAV_CMD_DO_GO_AROUND as a mavlink command''' - self._MAV_CMD_DO_GO_AROUND(self.run_cmd) - self._MAV_CMD_DO_GO_AROUND(self.run_cmd_int) - - def _MAV_CMD_DO_FLIGHTTERMINATION(self, command): - self.set_parameters({ - "AFS_ENABLE": 1, - "SYSID_MYGCS": self.mav.source_system, - "AFS_TERM_ACTION": 42, - }) - self.wait_ready_to_arm() - self.arm_vehicle() - self.context_collect('STATUSTEXT') - command(mavutil.mavlink.MAV_CMD_DO_FLIGHTTERMINATION, p1=1) - self.wait_disarmed() - self.wait_text('Terminating due to GCS request', check_context=True) - self.reboot_sitl() - - def MAV_CMD_DO_FLIGHTTERMINATION(self): - '''test MAV_CMD_DO_FLIGHTTERMINATION works on Plane''' - self._MAV_CMD_DO_FLIGHTTERMINATION(self.run_cmd) - self._MAV_CMD_DO_FLIGHTTERMINATION(self.run_cmd_int) - - def MAV_CMD_DO_LAND_START(self): - '''test MAV_CMD_DO_LAND_START as mavlink command''' - self.set_parameters({ - "RTL_AUTOLAND": 3, - }) - self.upload_simple_relhome_mission([ - (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30), - (mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM, 0, 0, 30), - self.create_MISSION_ITEM_INT( - mavutil.mavlink.MAV_CMD_DO_LAND_START, - ), - (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 800, 0, 0), - ]) - - self.change_mode('AUTO') - self.wait_ready_to_arm() - - self.arm_vehicle() - - self.start_subtest("DO_LAND_START as COMMAND_LONG") - self.wait_current_waypoint(2) - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_LAND_START) - self.wait_current_waypoint(4) - - self.start_subtest("DO_LAND_START as COMMAND_INT") - self.set_current_waypoint(2) - self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_LAND_START) - self.wait_current_waypoint(4) - - self.fly_home_land_and_disarm() - - def start_flying_simple_rehome_mission(self, items): - '''uploads items, changes mode to auto, waits ready to arm and arms - vehicle. If the first item it a takeoff you can expect the - vehicle to fly after this method returns - ''' - - self.upload_simple_relhome_mission(items) - - self.change_mode('AUTO') - self.wait_ready_to_arm() - - self.arm_vehicle() - - def InteractTest(self): - '''just takeoff''' - - if self.mavproxy is None: - raise NotAchievedException("Must be started with --map") - - self.start_flying_simple_rehome_mission([ - (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30), - (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 800, 0, 0), - (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 800, 800, 0), - (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 400, 0), - ]) - - self.wait_current_waypoint(4) - - self.set_parameter('SIM_SPEEDUP', 1) - - self.mavproxy.interact() - - def MAV_CMD_MISSION_START(self): - '''test MAV_CMD_MISSION_START starts AUTO''' - self.upload_simple_relhome_mission([ - (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 800, 0, 0), - ]) - for run_cmd in self.run_cmd, self.run_cmd_int: - self.change_mode('LOITER') - run_cmd(mavutil.mavlink.MAV_CMD_MISSION_START) - self.wait_mode('AUTO') - - def MAV_CMD_NAV_LOITER_UNLIM(self): - '''test receiving MAV_CMD_NAV_LOITER_UNLIM from GCS''' - self.takeoff(10) - self.run_cmd(mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM) - self.wait_mode('LOITER') - self.change_mode('GUIDED') - self.run_cmd_int(mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM) - self.wait_mode('LOITER') - self.fly_home_land_and_disarm() - - def MAV_CMD_NAV_RETURN_TO_LAUNCH(self): - '''test receiving MAV_CMD_NAV_RETURN_TO_LAUNCH from GCS''' - self.set_parameter('RTL_AUTOLAND', 1) - self.start_flying_simple_rehome_mission([ - (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30), - (mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM, 0, 0, 30), - self.create_MISSION_ITEM_INT( - mavutil.mavlink.MAV_CMD_DO_LAND_START, - ), - (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 800, 0, 0), - ]) - - for i in self.run_cmd, self.run_cmd_int: - self.wait_current_waypoint(2) - self.run_cmd(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH) - self.wait_current_waypoint(4) - self.set_current_waypoint(2) - self.fly_home_land_and_disarm() - def tests(self): '''return list of all tests''' ret = super(AutoTestPlane, self).tests() @@ -5148,7 +4510,6 @@ def tests(self): self.TestGripperMission, self.Parachute, self.ParachuteSinkRate, - self.DO_PARACHUTE, self.PitotBlockage, self.AIRSPEED_AUTOCAL, self.RangeFinder, @@ -5182,7 +4543,7 @@ def tests(self): self.TerrainMission, self.TerrainLoiter, self.VectorNavEAHRS, - self.MicroStrainEAHRS5, + self.LordEAHRS, self.Deadreckoning, self.DeadreckoningNoAirSpeed, self.EKFlaneswitch, @@ -5190,7 +4551,7 @@ def tests(self): self.RTL_CLIMB_MIN, self.ClimbBeforeTurn, self.IMUTempCal, - self.MAV_CMD_DO_AUX_FUNCTION, + self.MAV_DO_AUX_FUNCTION, self.SmartBattery, self.FlyEachFrame, self.RCDisableAirspeedUse, @@ -5201,7 +4562,6 @@ def tests(self): self.DCMFallback, self.MAVFTP, self.AUTOTUNE, - self.AutotuneFiltering, self.MegaSquirt, self.MSP_DJI, self.SpeedToFly, @@ -5211,33 +4571,16 @@ def tests(self): self.EmbeddedParamParser, self.AerobaticsScripting, self.MANUAL_CONTROL, - self.RunMissionScript, self.WindEstimates, self.AltResetBadGPS, self.AirspeedCal, self.MissionJumpTags, Test(self.GCSFailsafe, speedup=8), self.SDCardWPTest, - self.NoArmWithoutMissionItems, - self.MODE_SWITCH_RESET, - self.ExternalPositionEstimate, - self.MAV_CMD_GUIDED_CHANGE_ALTITUDE, - self.MAV_CMD_PREFLIGHT_CALIBRATION, - self.MAV_CMD_DO_INVERTED_FLIGHT, - self.MAV_CMD_DO_AUTOTUNE_ENABLE, - self.MAV_CMD_DO_GO_AROUND, - self.MAV_CMD_DO_FLIGHTTERMINATION, - self.MAV_CMD_DO_LAND_START, - self.InteractTest, - self.MAV_CMD_MISSION_START, - self.TerrainRally, - self.MAV_CMD_NAV_LOITER_UNLIM, - self.MAV_CMD_NAV_RETURN_TO_LAUNCH, ]) return ret def disabled_tests(self): return { "LandingDrift": "Flapping test. See https://github.com/ArduPilot/ardupilot/issues/20054", - "InteractTest": "requires user interaction", } diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 230bfbd1838f39..58a5f6805c91a0 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -373,13 +373,17 @@ def SET_POSITION_TARGET_GLOBAL_INT(self): def reboot_sitl(self): """Reboot SITL instance and wait it to reconnect.""" - # our battery is reset to full on reboot. So reduce it to 10% + # 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, - p1=65535, # battery mask - p2=10, # percentage - ) + self.run_cmd(mavutil.mavlink.MAV_CMD_BATTERY_RESET, + 255, # battery mask + 10, # percentage + 0, + 0, + 0, + 0, + 0, + 0) self.run_cmd_reboot() tstart = time.time() while True: @@ -388,10 +392,14 @@ def reboot_sitl(self): # ask for the message: batt = None try: - self.send_cmd( - mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE, - p1=mavutil.mavlink.MAVLINK_MSG_ID_BATTERY_STATUS, - ) + self.send_cmd(mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE, + mavutil.mavlink.MAVLINK_MSG_ID_BATTERY_STATUS, + 0, + 0, + 0, + 0, + 0, + 0) batt = self.mav.recv_match(type='BATTERY_STATUS', blocking=True, timeout=1) @@ -422,100 +430,9 @@ def disabled_tests(self): ret = super(AutoTestSub, self).disabled_tests() ret.update({ "ConfigErrorLoop": "Sub does not instantiate AP_Stats. Also see https://github.com/ArduPilot/ardupilot/issues/10247", # noqa - "MAV_CMD_DO_CHANGE_SPEED": "Doesn't work", }) return ret - def MAV_CMD_NAV_LOITER_UNLIM(self): - '''test handling of MAV_CMD_NAV_LOITER_UNLIM received via mavlink''' - for cmd in self.run_cmd, self.run_cmd_int: - self.change_mode('CIRCLE') - cmd(mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM) - self.assert_mode('POSHOLD') - - def MAV_CMD_NAV_LAND(self): - '''test handling of MAV_CMD_NAV_LAND received via mavlink''' - for cmd in self.run_cmd, self.run_cmd_int: - self.change_mode('CIRCLE') - cmd(mavutil.mavlink.MAV_CMD_NAV_LAND) - self.assert_mode('SURFACE') - - def MAV_CMD_MISSION_START(self): - '''test handling of MAV_CMD_NAV_LAND received via mavlink''' - self.upload_simple_relhome_mission([ - (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2000, 0, 0), - (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0), - ]) - - self.wait_ready_to_arm() - self.arm_vehicle() - for cmd in self.run_cmd, self.run_cmd_int: - self.change_mode('CIRCLE') - cmd(mavutil.mavlink.MAV_CMD_MISSION_START) - self.assert_mode('AUTO') - self.disarm_vehicle() - - def MAV_CMD_DO_CHANGE_SPEED(self): - '''ensure vehicle changes speeds when DO_CHANGE_SPEED received''' - items = [ - (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2000, 0, 0), - (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0), - ] - items = [] - for i in range(0, 2000, 10): - items.append((mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, i, 0, 0)) - self.upload_simple_relhome_mission(items) - - self.wait_ready_to_arm() - self.arm_vehicle() - self.run_cmd(mavutil.mavlink.MAV_CMD_MISSION_START) - for run_cmd in self.run_cmd, self.run_cmd_int: - for speed in [1, 2, 3, 1]: - run_cmd(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, p2=speed) - self.wait_groundspeed(speed-0.02, speed+0.02, minimum_duration=2) - self.disarm_vehicle() - - def _MAV_CMD_CONDITION_YAW(self, run_cmd): - self.arm_vehicle() - self.change_mode('GUIDED') - for angle in 5, 30, 60, 10: - angular_rate = 10 - direction = 1 - relative_or_absolute = 0 - run_cmd( - mavutil.mavlink.MAV_CMD_CONDITION_YAW, - p1=angle, - p2=angular_rate, - p3=direction, - p4=relative_or_absolute, # 1 for relative, 0 for absolute - ) - self.wait_heading(angle, minimum_duration=2) - - self.start_subtest('Relative angle') - run_cmd( - mavutil.mavlink.MAV_CMD_CONDITION_YAW, - p1=0, - p2=10, - p3=1, - p4=0, # 1 for relative, 0 for absolute - ) - self.wait_heading(0, minimum_duration=2) - run_cmd( - mavutil.mavlink.MAV_CMD_CONDITION_YAW, - p1=20, - p2=10, - p3=1, - p4=1, # 1 for relative, 0 for absolute - ) - self.wait_heading(20, minimum_duration=2) - - self.disarm_vehicle() - - def MAV_CMD_CONDITION_YAW(self): - '''ensure vehicle yaws according to GCS command''' - self._MAV_CMD_CONDITION_YAW(self.run_cmd) - self._MAV_CMD_CONDITION_YAW(self.run_cmd_int) - def tests(self): '''return list of all tests''' ret = super(AutoTestSub, self).tests() @@ -531,11 +448,6 @@ def tests(self): self.MotorThrustHoverParameterIgnore, self.SET_POSITION_TARGET_GLOBAL_INT, self.TestLogDownloadMAVProxy, - self.MAV_CMD_NAV_LOITER_UNLIM, - self.MAV_CMD_NAV_LAND, - self.MAV_CMD_MISSION_START, - self.MAV_CMD_DO_CHANGE_SPEED, - self.MAV_CMD_CONDITION_YAW, ]) return ret diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index 84c5884e070db7..2659e68ceac3b8 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python3 +#!/usr/bin/env python """ ArduPilot automatic test suite. @@ -21,7 +21,6 @@ import time import traceback -import blimp import rover import arducopter import arduplane @@ -98,7 +97,7 @@ def build_binaries(): def build_examples(**kwargs): """Build examples.""" - for target in 'Pixhawk1', 'navio', 'linux': + for target in 'fmuv2', 'Pixhawk1', 'navio', 'linux': print("Running build.examples for %s" % target) try: util.build_examples(target, **kwargs) @@ -339,7 +338,6 @@ 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 @@ -359,9 +357,8 @@ def find_specific_test_to_run(step): "test.CAN": arducopter.AutoTestCAN, } -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"], +suplementary_test_binary_map = { + "test.CAN": ["sitl_periph_gps.AP_Periph", "sitl_periph_gps.AP_Periph.1"], } @@ -378,7 +375,7 @@ def run_specific_test(step, *args, **kwargs): # print("Got %s" % str(tester)) for a in tester.tests(): - if not isinstance(a, Test): + if type(a) != Test: a = Test(a) print("Got %s" % (a.name)) if a.name == test: @@ -405,11 +402,10 @@ def run_step(step): "postype_single": opts.postype_single, "extra_configure_args": opts.waf_configure_args, "coverage": opts.coverage, - "force_32bit" : opts.force_32bit, + "sitl_32bit" : opts.sitl_32bit, "ubsan" : opts.ubsan, "ubsan_abort" : opts.ubsan_abort, "num_aux_imus" : opts.num_aux_imus, - "dronecan_tests" : opts.dronecan_tests, } if opts.Werror: @@ -462,30 +458,24 @@ def run_step(step): binary = binary_path(step, debug=opts.debug) - # see if we need any supplementary binaries 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 - + 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 = [] fly_opts = { "viewerip": opts.viewerip, "use_map": opts.map, @@ -684,7 +674,6 @@ def write_fullresults(): results.addglob('APM:Copter documentation', 'docs/ArduCopter/index.html') results.addglob('APM:Rover documentation', 'docs/Rover/index.html') results.addglob('APM:Sub documentation', 'docs/ArduSub/index.html') - results.addglob('APM:Blimp documentation', 'docs/Blimp/index.html') results.addglobimage("Flight Track", '*.png') write_webresults(results) @@ -720,7 +709,7 @@ def run_tests(steps): try: success = run_step(step) testinstance = None - if isinstance(success, tuple): + if type(success) == tuple: (success, testinstance) = success if success: results.add(step, 'PASSED', @@ -773,7 +762,7 @@ def run_tests(steps): return passed -vehicle_list = ['Sub', 'Copter', 'Plane', 'Tracker', 'Rover', 'QuadPlane', 'BalanceBot', 'Helicopter', 'Sailboat', 'Blimp'] +vehicle_list = ['Sub', 'Copter', 'Plane', 'Tracker', 'Rover', 'QuadPlane', 'BalanceBot', 'Helicopter', 'Sailboat'] def list_subtests(): @@ -806,7 +795,7 @@ def list_subtests_for_vehicle(vehicle_type): subtests = tester.tests() sorted_list = [] for subtest in subtests: - if not isinstance(subtest, Test): + if type(subtest) != Test: subtest = Test(subtest) sorted_list.append([subtest.name, subtest.description]) sorted_list.sort() @@ -930,10 +919,10 @@ def format_epilog(self, formatter): action="store_true", dest="ekf_single", help="force single precision EKF") - group_build.add_option("--force-32bit", + group_build.add_option("--sitl-32bit", default=False, action='store_true', - dest="force_32bit", + dest="sitl_32bit", help="compile sitl using 32-bit") group_build.add_option("", "--ubsan", default=False, @@ -950,11 +939,6 @@ def format_epilog(self, formatter): default=0, type='int', help='number of auxiliary IMUs to simulate') - group_build.add_option("--enable-dronecan-tests", - default=False, - action='store_true', - dest="dronecan_tests", - help="enable dronecan tests") parser.add_option_group(group_build) group_sim = optparse.OptionGroup(parser, "Simulation options") diff --git a/Tools/autotest/bisect-helper.py b/Tools/autotest/bisect-helper.py index 3858a53fb6b3a8..e0da93182ef33d 100755 --- a/Tools/autotest/bisect-helper.py +++ b/Tools/autotest/bisect-helper.py @@ -2,10 +2,6 @@ '''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: @@ -119,6 +115,7 @@ 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, @@ -133,7 +130,7 @@ def run_program(self, prefix, cmd_list): # select not available on Windows... probably... time.sleep(0.1) continue - if isinstance(x, bytes): + if type(x) == bytes: x = x.decode('utf-8') self.program_output += x x = x.rstrip() @@ -187,10 +184,6 @@ 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() @@ -280,7 +273,7 @@ def run(self): if code == self.exit_fail_code(): with open("/tmp/fail-counts", "a") as f: - f.write("Failed on run %u\n" % (i+1,)) + print("Failed on run %u" % (i+1,), file=f) if ignore: self.progress("Ignoring this run") continue @@ -289,8 +282,6 @@ def run(self): self.git_reset() - self.progress("Exit code is %u" % code) - sys.exit(code) @@ -303,7 +294,8 @@ def run(self): help="Help bisect a build failure") parser.add_option("--build-failure-string", type='string', - help="Must be present in" + default=None, + help="If supplied, 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 deleted file mode 100644 index affeec2cecd878..00000000000000 --- a/Tools/autotest/blimp.py +++ /dev/null @@ -1,252 +0,0 @@ -''' -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, 0) - -# Flight mode switch positions are set-up in blimp.parm to be -# switch 1 = Land -# switch 2 = Manual -# switch 3 = Velocity -# switch 4 = Loiter -# switch 5 = Manual -# switch 6 = Manual - - -class AutoTestBlimp(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 FlyManual(self): - '''test manual mode''' - self.change_mode('MANUAL') - self.wait_ready_to_arm() - self.arm_vehicle() - - acc = 0.5 - - # make sure we don't drift: - bl = self.mav.location() - tl = self.offset_location_ne(location=bl, metres_north=2, metres_east=0) - ttl = self.offset_location_ne(location=bl, metres_north=4, metres_east=0) - tr = self.offset_location_ne(location=bl, metres_north=4, metres_east=2) - ttr = self.offset_location_ne(location=bl, metres_north=4, metres_east=4) - - if self.mavproxy is not None: - self.mavproxy.send(f"map icon {bl.lat} {bl.lng} flag\n") - self.mavproxy.send(f"map icon {tl.lat} {tl.lng} flag\n") - self.mavproxy.send(f"map icon {ttl.lat} {ttl.lng} flag\n") - self.mavproxy.send(f"map icon {tr.lat} {tr.lng} flag\n") - self.mavproxy.send(f"map icon {ttr.lat} {ttr.lng} flag\n") - - self.set_rc(2, 2000) - self.wait_distance_to_location(tl, 0, acc, timeout=10) - self.set_rc(2, 1500) - self.wait_distance_to_location(ttl, 0, acc, timeout=15) - self.set_rc(1, 2000) - self.wait_distance_to_location(tr, 0, acc, timeout=10) - self.set_rc(1, 1500) - self.wait_distance_to_location(ttr, 0, acc, timeout=15) - self.change_mode('RTL') - self.wait_distance_to_location(bl, 0, 0.5, timeout=30, minimum_duration=5) # make sure it can hold position - self.change_mode('MANUAL') - - self.wait_distance_to_location(bl, 0, acc, timeout=5) # make sure we haven't moved from the spot - - self.set_rc(3, 2000) - self.wait_altitude(5, 5.5, relative=True, timeout=15) - self.set_rc(3, 1500) - - self.wait_distance_to_location(bl, 0, acc, timeout=5) # make sure we haven't moved from the spot - - self.set_rc(4, 1000) - self.wait_heading(340, accuracy=5, timeout=5) # short timeout to check yawrate - self.set_rc(4, 1500) - - self.wait_distance_to_location(bl, 0, acc, timeout=5) # make sure we haven't moved from the spot - - self.set_rc(3, 1000) - self.wait_altitude(0, 0.5, relative=True, timeout=20) - self.set_rc(3, 1500) - - self.wait_distance_to_location(bl, 0, acc, timeout=5) # make sure we haven't moved from the spot - - self.set_rc(4, 2000) - self.wait_heading(135, accuracy=5, timeout=10) # short timeout to check yawrate - self.set_rc(4, 1500) - - self.wait_distance_to_location(bl, 0, acc, timeout=5) # make sure we haven't moved from the spot - - self.disarm_vehicle() - - def FlyLoiter(self): - '''test loiter mode''' - - self.change_mode('LOITER') - self.wait_ready_to_arm() - self.arm_vehicle() - - siz = 5 - tim = 60 - - # make sure we don't drift: - bl = self.mav.location() - tl = self.offset_location_ne(location=bl, metres_north=siz, metres_east=0) - tr = self.offset_location_ne(location=bl, metres_north=siz, metres_east=siz) - br = self.offset_location_ne(location=bl, metres_north=0, metres_east=siz) - - print("Locations are:") - print("bottom left ", bl.lat, bl.lng) - print("top left ", tl.lat, tl.lng) - print("top right ", tr.lat, tr.lng) - print("bottom right ", br.lat, br.lng) - - if self.mavproxy is not None: - self.mavproxy.send(f"map icon {bl.lat} {bl.lng} flag\n") - self.mavproxy.send(f"map icon {tl.lat} {tl.lng} flag\n") - self.mavproxy.send(f"map icon {tr.lat} {tr.lng} flag\n") - self.mavproxy.send(f"map icon {br.lat} {br.lng} flag\n") - - self.set_parameter("SIMPLE_MODE", 1) - - self.set_rc(2, 2000) - self.wait_distance_to_location(tl, 0, 0.2, timeout=tim) - self.set_rc(2, 1500) - - self.set_rc(1, 2000) - self.wait_distance_to_location(tr, 0, 0.5, timeout=tim) - self.set_rc(1, 1500) - - self.set_rc(2, 1000) - self.wait_distance_to_location(br, 0, 0.5, timeout=tim) - self.set_rc(2, 1500) - - self.set_rc(1, 1000) - self.wait_distance_to_location(bl, 0, 0.5, timeout=tim) - self.set_rc(1, 1500) - - fin = self.mav.location() - - self.set_rc(4, 1700) - self.wait_heading(135, accuracy=2, timeout=tim) - self.set_rc(4, 1500) - - self.wait_distance_to_location(fin, 0, 0.15, timeout=5) # make sure we haven't moved from the spot - - self.set_rc(3, 2000) - self.wait_altitude(5, 5.5, relative=True, timeout=60) - self.set_rc(3, 1000) - self.wait_altitude(0, 0.5, relative=True, timeout=60) - self.set_rc(3, 1500) - - self.wait_distance_to_location(fin, 0, 0.15, timeout=5) # make sure we haven't moved from the spot - - self.set_rc(4, 1300) - self.wait_heading(0, accuracy=2, timeout=tim) - self.set_rc(4, 1500) - - self.wait_distance_to_location(fin, 0, 0.15, timeout=5) # make sure we haven't moved from the spot - - self.disarm_vehicle() - - def tests(self): - '''return list of all tests''' - # ret = super(AutoTestBlimp, self).tests() - ret = [] - ret.extend([ - self.FlyManual, - self.FlyLoiter, - ]) - return ret - - def disabled_tests(self): - return { - } diff --git a/Tools/autotest/check_autotest_speedup.py b/Tools/autotest/check_autotest_speedup.py index 621726c8677c79..a601dcc5e99201 100755 --- a/Tools/autotest/check_autotest_speedup.py +++ b/Tools/autotest/check_autotest_speedup.py @@ -38,6 +38,7 @@ 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, @@ -52,7 +53,7 @@ def run_program(self, prefix, cmd_list): # select not available on Windows... probably... time.sleep(0.1) continue - if isinstance(x, bytes): + if type(x) == bytes: x = x.decode('utf-8') self.program_output += x x = x.rstrip() diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index b8baef0d7c7586..d47d061746334d 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -37,7 +37,6 @@ from pymavlink import mavextra from pymavlink.rotmat import Vector3 from pymavlink import quaternion -from pymavlink.generator import mavgen from pysim import util, vehicleinfo @@ -168,11 +167,6 @@ 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 @@ -202,14 +196,11 @@ 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, suppress_stdout=False): - self.suppress_stdout = suppress_stdout + def __init__(self, name, mode, mavproxy_logfile): self.file = open(name, mode) self.stdout = sys.stdout self.stderr = sys.stderr @@ -227,11 +218,8 @@ def close(self): self.file = None def write(self, data): - if isinstance(data, bytes): - data = data.decode('ascii') self.file.write(data) - if not self.suppress_stdout: - self.stdout.write(data) + self.stdout.write(data) def flush(self): self.file.flush() @@ -1448,13 +1436,12 @@ def __init__(self, lat, lon, alt, yaw): class Test(object): '''a test definition - information about a test''' - def __init__(self, function, kwargs={}, attempts=1, speedup=None): + def __init__(self, function, attempts=1, speedup=None): self.name = function.__name__ self.description = function.__doc__ if self.description is None: raise ValueError("%s is missing a docstring" % self.name) self.function = function - self.kwargs = kwargs self.attempts = attempts self.speedup = speedup @@ -1507,11 +1494,10 @@ def __init__(self, replay=False, sup_binaries=[], reset_after_every_test=False, - force_32bit=False, + sitl_32bit=False, ubsan=False, ubsan_abort=False, num_aux_imus=0, - dronecan_tests=False, build_opts={}): self.start_time = time.time() @@ -1535,7 +1521,7 @@ def __init__(self, self.speedup = self.default_speedup() self.sup_binaries = sup_binaries self.reset_after_every_test = reset_after_every_test - self.force_32bit = force_32bit + self.sitl_32bit = sitl_32bit self.ubsan = ubsan self.ubsan_abort = ubsan_abort self.build_opts = build_opts @@ -1592,8 +1578,6 @@ def __init__(self, offline=self.terrain_in_offline_mode ) self.terrain_data_messages_sent = 0 # count of messages back - self.dronecan_tests = dronecan_tests - self.statustext_id = 1 def __del__(self): if self.rc_thread is not None: @@ -1664,27 +1648,13 @@ 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:%u" % self.adjust_ardupilot_port(5760) - - def sitl_rcin_port(self, offset=0): - if offset > 2: - raise ValueError("offset too large") - return 5501 + offset + return "tcp:127.0.0.1:5760" 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', @@ -1893,13 +1863,26 @@ def reboot_check_valgrind_log(self): shutil.move(valgrind_log, backup_valgrind_log) def run_cmd_reboot(self): - self.run_cmd_int( - mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, - p1=1, # reboot autopilot - ) + self.run_cmd(mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, + 1, # confirmation + 1, # reboot autopilot + 0, + 0, + 0, + 0, + 0, + 0) def run_cmd_run_prearms(self): - self.run_cmd(mavutil.mavlink.MAV_CMD_RUN_PREARM_CHECKS) + self.run_cmd(mavutil.mavlink.MAV_CMD_RUN_PREARM_CHECKS, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0) def run_cmd_enable_high_latency(self, new_state): p1 = 0 @@ -1908,7 +1891,13 @@ def run_cmd_enable_high_latency(self, new_state): self.run_cmd( mavutil.mavlink.MAV_CMD_CONTROL_HIGH_LATENCY, - p1=p1, # enable/disable + p1, # p1 - enable/disable + 0, # p2 + 0, # p3 + 0, # p4 + 0, # p5 + 0, # p6 + 0, # p7 ) def reboot_sitl_mav(self, required_bootcount=None, force=False): @@ -1948,10 +1937,13 @@ 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, - p1=1, - p2=1, - p6=p6, - ) + 1, + 1, + 0, + 0, + 0, + p6, + 0) do_context = True if do_context: self.context_push() @@ -2041,9 +2033,9 @@ def scripting_restart(self): self.progress("Restarting Scripting") self.run_cmd_int( mavutil.mavlink.MAV_CMD_SCRIPTING, - p1=mavutil.mavlink.SCRIPTING_CMD_STOP_AND_RESTART, - timeout=5, - ) + mavutil.mavlink.SCRIPTING_CMD_STOP_AND_RESTART, + 0, 0, 0, 0, 0, 0, + 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''' @@ -2140,11 +2132,41 @@ def test_adsb_send_threatening_adsb_message(self, here, offset_ne=None): def get_sim_parameter_documentation_get_whitelist(self): # common parameters ret = set([ + "SIM_ACC1_BIAS_X", + "SIM_ACC1_BIAS_Y", + "SIM_ACC1_BIAS_Z", "SIM_ACC1_RND", + "SIM_ACC1_SCAL_X", + "SIM_ACC1_SCAL_Y", + "SIM_ACC1_SCAL_Z", + "SIM_ACC2_BIAS_X", + "SIM_ACC2_BIAS_Y", + "SIM_ACC2_BIAS_Z", "SIM_ACC2_RND", + "SIM_ACC2_SCAL_X", + "SIM_ACC2_SCAL_Y", + "SIM_ACC2_SCAL_Z", + "SIM_ACC3_BIAS_X", + "SIM_ACC3_BIAS_Y", + "SIM_ACC3_BIAS_Z", "SIM_ACC3_RND", + "SIM_ACC3_SCAL_X", + "SIM_ACC3_SCAL_Y", + "SIM_ACC3_SCAL_Z", "SIM_ACC4_RND", + "SIM_ACC4_SCAL_X", + "SIM_ACC4_SCAL_Y", + "SIM_ACC4_SCAL_Z", + "SIM_ACC4_BIAS_X", + "SIM_ACC4_BIAS_Y", + "SIM_ACC4_BIAS_Z", "SIM_ACC5_RND", + "SIM_ACC5_SCAL_X", + "SIM_ACC5_SCAL_Y", + "SIM_ACC5_SCAL_Z", + "SIM_ACC5_BIAS_X", + "SIM_ACC5_BIAS_Y", + "SIM_ACC5_BIAS_Z", "SIM_ACC_FILE_RW", "SIM_ACC_TRIM_X", "SIM_ACC_TRIM_Y", @@ -2153,10 +2175,19 @@ def get_sim_parameter_documentation_get_whitelist(self): "SIM_ADSB_COUNT", "SIM_ADSB_RADIUS", "SIM_ADSB_TX", + "SIM_ARSPD2_FAIL", + "SIM_ARSPD2_FAILP", "SIM_ARSPD2_OFS", + "SIM_ARSPD2_PITOT", + "SIM_ARSPD2_RATIO", "SIM_ARSPD2_RND", + "SIM_ARSPD2_SIGN", + "SIM_ARSPD_FAILP", "SIM_ARSPD_OFS", + "SIM_ARSPD_PITOT", + "SIM_ARSPD_RATIO", "SIM_ARSPD_RND", + "SIM_ARSPD_SIGN", "SIM_BAR2_DELAY", "SIM_BAR2_DISABLE", "SIM_BAR2_DRIFT", @@ -2189,6 +2220,7 @@ 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", @@ -2196,19 +2228,86 @@ def get_sim_parameter_documentation_get_whitelist(self): "SIM_ENGINE_FAIL", "SIM_ENGINE_MUL", "SIM_ESC_ARM_RPM", + "SIM_FLOW_DELAY", + "SIM_FLOW_ENABLE", + "SIM_FLOW_POS_X", + "SIM_FLOW_POS_Y", + "SIM_FLOW_POS_Z", + "SIM_FLOW_RATE", + "SIM_FLOW_RND", "SIM_FTOWESC_ENA", "SIM_FTOWESC_POW", "SIM_GND_BEHAV", + "SIM_GPS2_ACC", + "SIM_GPS2_ALT_OFS", + "SIM_GPS2_BYTELOS", + "SIM_GPS2_DRFTALT", + "SIM_GPS2_GLTCH_X", + "SIM_GPS2_GLTCH_Y", + "SIM_GPS2_GLTCH_Z", + "SIM_GPS2_HDG", + "SIM_GPS2_HZ", + "SIM_GPS2_LAG_MS", + "SIM_GPS2_LCKTIME", + "SIM_GPS2_NOISE", + "SIM_GPS2_NUMSATS", + "SIM_GPS2_POS_X", + "SIM_GPS2_POS_Y", + "SIM_GPS2_POS_Z", + "SIM_GPS2_TYPE", + "SIM_GPS2_VERR_X", + "SIM_GPS2_VERR_Y", + "SIM_GPS2_VERR_Z", + "SIM_GPS_ACC", + "SIM_GPS_ALT_OFS", + "SIM_GPS_BYTELOSS", + "SIM_GPS_DRIFTALT", + "SIM_GPS_GLITCH_X", + "SIM_GPS_GLITCH_Y", + "SIM_GPS_GLITCH_Z", + "SIM_GPS_HDG", + "SIM_GPS_HZ", + "SIM_GPS_LAG_MS", + "SIM_GPS_LOCKTIME", + "SIM_GPS_LOG_NUM", + "SIM_GPS_NOISE", + "SIM_GPS_NUMSATS", + "SIM_GPS_POS_X", + "SIM_GPS_POS_Y", + "SIM_GPS_POS_Z", + "SIM_GPS_TYPE", + "SIM_GPS_VERR_X", + "SIM_GPS_VERR_Y", + "SIM_GPS_VERR_Z", "SIM_GYR1_RND", + "SIM_GYR1_SCALE_X", + "SIM_GYR1_SCALE_Y", + "SIM_GYR1_SCALE_Z", "SIM_GYR2_RND", + "SIM_GYR2_SCALE_X", + "SIM_GYR2_SCALE_Y", + "SIM_GYR2_SCALE_Z", "SIM_GYR3_RND", + "SIM_GYR3_SCALE_X", + "SIM_GYR3_SCALE_Y", + "SIM_GYR3_SCALE_Z", "SIM_GYR4_RND", + "SIM_GYR4_SCALE_X", + "SIM_GYR4_SCALE_Y", + "SIM_GYR4_SCALE_Z", "SIM_GYR5_RND", + "SIM_GYR5_SCALE_X", + "SIM_GYR5_SCALE_Y", + "SIM_GYR5_SCALE_Z", "SIM_GYR_FAIL_MSK", "SIM_GYR_FILE_RW", "SIM_IE24_ENABLE", "SIM_IE24_ERROR", "SIM_IE24_STATE", + "SIM_IMU_COUNT", + "SIM_IMU_POS_X", + "SIM_IMU_POS_Y", + "SIM_IMU_POS_Z", "SIM_IMUT1_ACC1_X", "SIM_IMUT1_ACC1_Y", "SIM_IMUT1_ACC1_Z", @@ -2318,9 +2417,13 @@ def get_sim_parameter_documentation_get_whitelist(self): "SIM_IMUT_FIXED", "SIM_IMUT_START", "SIM_IMUT_TCONST", + "SIM_INIT_ALT_OFS", + "SIM_INIT_LAT_OFS", + "SIM_INIT_LON_OFS", "SIM_INS_THR_MIN", "SIM_LED_LAYOUT", "SIM_LOOP_DELAY", + "SIM_MAG1_DEVID", "SIM_MAG1_SCALING", "SIM_MAG2_DEVID", "SIM_MAG2_DIA_X", @@ -2380,6 +2483,7 @@ 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", @@ -2522,7 +2626,7 @@ def test_parameter_documentation_get_all_parameters(self): def find_format_defines(self, lines): ret = {} for line in lines: - if isinstance(line, bytes): + if type(line) == bytes: line = line.decode("utf-8") m = re.match(r'#define (\w+_(?:LABELS|FMT|UNITS|MULTS))\s+(".*")', line) if m is None: @@ -2579,7 +2683,7 @@ def all_log_format_ids(self): for line in open(f).readlines(): if debug: print("line: %s" % line) - if isinstance(line, bytes): + if type(line) == bytes: line = line.decode("utf-8") line = re.sub("//.*", "", line) # trim comments if re.match(r"\s*$", line): @@ -2658,7 +2762,7 @@ def all_log_format_ids(self): linestate_within = 90 linestate = linestate_none for line in open(filepath, 'rb').readlines(): - if isinstance(line, bytes): + if type(line) == bytes: line = line.decode("utf-8") line = re.sub("//.*", "", line) # trim comments if re.match(r"\s*$", line): @@ -2678,9 +2782,7 @@ def all_log_format_ids(self): continue if "#if FRAME_CONFIG == HELI_FRAME" in line: continue - if "#if AC_PRECLAND_ENABLED" in line: - continue - if "#if OFFBOARD_GUIDED == ENABLED" in line: + if "#if PRECISION_LANDING == ENABLED" in line: continue if "#end" in line: continue @@ -2758,7 +2860,7 @@ def all_log_format_ids(self): continue count = 0 for line in open(filepath, 'rb').readlines(): - if isinstance(line, bytes): + if type(line) == bytes: line = line.decode("utf-8") if state == state_outside: if (re.match(r"\s*AP::logger\(\)[.]Write(?:Streaming)?\(", line) or @@ -2858,7 +2960,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', 'RSLL'] + 'ROFH', 'REPH', 'REVH', 'RWOH', 'RBOH'] docco_ids = {} for thing in tree.logformat: @@ -3128,7 +3230,7 @@ def message_hook(self, mav, msg): """Called as each mavlink msg is received.""" # print("msg: %s" % str(msg)) if msg.get_type() == 'STATUSTEXT': - self.progress("AP: %s" % msg.text, send_statustext=False) + self.progress("AP: %s" % msg.text) self.write_msg_to_tlog(msg) @@ -3348,17 +3450,6 @@ def HIGH_LATENCY2(self): raise NotAchievedException("Air Temperature not received from HIGH_LATENCY2") self.HIGH_LATENCY2_links() - def context_set_message_rate_hz(self, id, rate_hz, run_cmd=None): - if run_cmd is None: - run_cmd = self.run_cmd - - overridden_message_rates = self.context_get().overridden_message_rates - - if id not in overridden_message_rates: - overridden_message_rates[id] = self.measure_message_rate(id) - - self.set_message_rate_hz(id, rate_hz, run_cmd=run_cmd) - def HIGH_LATENCY2_links(self): self.start_subtest("SerialProtocol_MAVLinkHL links") @@ -3373,7 +3464,7 @@ def HIGH_LATENCY2_links(self): self.reboot_sitl() mav2 = mavutil.mavlink_connection( - "tcp:localhost:%u" % self.adjust_ardupilot_port(5763), + "tcp:localhost:5763", robust_parsing=True, source_system=7, source_component=7, @@ -3725,13 +3816,6 @@ def save_wp(self, ch=7): self.set_rc(ch, 1000) self.delay_sim_time(1) - def correct_wp_seq_numbers(self, wps): - # renumber the items: - count = 0 - for item in wps: - item.seq = count - count += 1 - def create_simple_relhome_mission(self, items_in, target_system=1, target_component=1): '''takes a list of (type, n, e, alt) items. Creates a mission in absolute frame using alt as relative-to-home and n and e as @@ -3742,24 +3826,34 @@ def create_simple_relhome_mission(self, items_in, target_system=1, target_compon items.extend(items_in) seq = 0 ret = [] - for item in items: - if not isinstance(item, tuple): - # hope this is a mission item... - item.seq = seq - seq += 1 - ret.append(item) - continue - (t, n, e, alt) = item + for (t, n, e, alt) in items: lat = 0 lng = 0 if n != 0 or e != 0: loc = self.home_relative_loc_ne(n, e) lat = loc.lat lng = loc.lng + p1 = 0 frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT if not self.ardupilot_stores_frame_for_cmd(t): frame = mavutil.mavlink.MAV_FRAME_GLOBAL - ret.append(self.create_MISSION_ITEM_INT(t, seq=seq, frame=frame, x=int(lat*1e7), y=int(lng*1e7), z=alt)) + ret.append(self.mav.mav.mission_item_int_encode( + target_system, + target_component, + seq, # seq + frame, + t, + 0, # current + 0, # autocontinue + p1, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(lat*1e7), # latitude + int(lng*1e7), # longitude + alt, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_MISSION), + ) seq += 1 return ret @@ -3777,15 +3871,17 @@ def get_mission_count(self): def run_auxfunc(self, function, level, - run_cmd=None, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): - if run_cmd is None: - run_cmd = self.run_cmd - run_cmd( + self.run_cmd( mavutil.mavlink.MAV_CMD_DO_AUX_FUNCTION, - p1=function, - p2=level, - want_result=want_result, + function, # p1 + level, # p2 + 0, # p3 + 0, # p4 + 0, # p5 + 0, # p6 + 0, # p7 + want_result=want_result ) def assert_mission_count(self, expected): @@ -3855,27 +3951,13 @@ 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_string, got_string)) + (m.get_type(), fieldname, value, got)) return False if verbose: self.progress("%s.%s has expected value %s" % - (m.get_type(), fieldname, value_string)) + (m.get_type(), fieldname, value)) return True def assert_message_field_values(self, m, fieldvalues, verbose=True, epsilon=None): @@ -3883,39 +3965,19 @@ def assert_message_field_values(self, m, fieldvalues, verbose=True, epsilon=None return raise NotAchievedException("Did not get expected field values") - 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) + def assert_received_message_field_values(self, message, fieldvalues, verbose=True, very_verbose=False, epsilon=None): m = self.assert_receive_message(message, verbose=verbose, very_verbose=very_verbose) self.assert_message_field_values(m, fieldvalues, verbose=verbose, epsilon=epsilon) return m - # FIXME: try to use wait_and_maintain here? - def wait_message_field_values(self, message, fieldvalues, timeout=10, epsilon=None, instance=None, minimum_duration=None): - + def wait_message_field_values(self, message, fieldvalues, timeout=10, epsilon=None, instance=None): tstart = self.get_sim_time_cached() - pass_start = None while True: - now = self.get_sim_time_cached() - if now - tstart > timeout: + if self.get_sim_time_cached() - tstart > timeout: raise NotAchievedException("Field never reached values") m = self.assert_receive_message(message, instance=instance) if self.message_has_field_values(m, fieldvalues, epsilon=epsilon): - if minimum_duration is not None: - if pass_start is None: - pass_start = now - continue - if now - pass_start < minimum_duration: - continue return m - pass_start = None def onboard_logging_not_log_disarmed(self): self.start_subtest("Test LOG_DISARMED-is-false behaviour") @@ -4154,18 +4216,6 @@ 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''' @@ -4440,17 +4490,16 @@ def load_rally_using_mavproxy(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( - self.generic_mission_filepath_for_filename(filename), + os.path.join(testdir, "Generic_Missions"), + filename, strict=strict) def load_mission(self, filename, strict=True): return self.load_mission_from_filepath( - os.path.join(testdir, self.current_test_name_directory, filename), + self.current_test_name_directory, + filename, strict=strict) def wp_to_mission_item_int(self, wp): @@ -4477,54 +4526,40 @@ def wp_to_mission_item_int(self, wp): wp.z) return wp_int - def mission_from_filepath(self, filepath, target_system=1, target_component=1): + def mission_from_filepath(self, filepath, filename, target_system=1, target_component=1): '''returns a list of mission-item-ints from filepath''' - print("filepath: %s" % filepath) - self.progress("Loading mission (%s)" % os.path.basename(filepath)) + self.progress("Loading mission (%s)" % filename) + path = os.path.join(testdir, filepath, filename) wploader = mavwp.MAVWPLoader( target_system=target_system, target_component=target_component ) - wploader.load(filepath) + wploader.load(path) 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 ''' - return self.get_home_tuple_from_mission_filepath( - os.path.join(testdir, self.current_test_name_directory, filename) + items = self.mission_from_filepath( + 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 ) @@ -4701,6 +4736,7 @@ def check_mission_item_upload_download(self, items, itype, mission_type, strict= self.upload_using_mission_protocol(mission_type, items) self.progress("check %s upload/download: download items" % itype) downloaded_items = self.download_using_mission_protocol(mission_type) + self.progress("Downloaded items: (%s)" % str(downloaded_items)) if len(items) != len(downloaded_items): raise NotAchievedException("Did not download same number of items as uploaded want=%u got=%u" % (len(items), len(downloaded_items))) @@ -4708,8 +4744,6 @@ def check_mission_item_upload_download(self, items, itype, mission_type, strict= self.check_fence_items_same(items, downloaded_items, strict=strict) elif mission_type == mavutil.mavlink.MAV_MISSION_TYPE_MISSION: self.check_mission_waypoint_items_same(items, downloaded_items, strict=strict) - elif mission_type == mavutil.mavlink.MAV_MISSION_TYPE_RALLY: - self.check_mission_waypoint_items_same(items, downloaded_items, strict=strict) else: raise NotAchievedException("Unhandled") @@ -4726,13 +4760,6 @@ def check_mission_upload_download(self, items, strict=True): mavutil.mavlink.MAV_MISSION_TYPE_MISSION, strict=strict) - def check_rally_upload_download(self, items): - self.check_mission_item_upload_download( - items, - "rally", - mavutil.mavlink.MAV_MISSION_TYPE_RALLY - ) - def check_dflog_message_rates(self, log_filepath, message_rates): reader = self.dfreader_for_path(log_filepath) @@ -4803,7 +4830,7 @@ def rc_defaults(self): def set_rc_from_map(self, _map, timeout=20): map_copy = _map.copy() for v in map_copy.values(): - if not isinstance(v, int): + if type(v) != int: raise NotAchievedException("RC values must be integers") self.rc_queue.put(map_copy) @@ -4837,7 +4864,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:%u" % self.sitl_rcin_port(), input=False) + sitl_output = mavutil.mavudp("127.0.0.1:5501", input=False) buf = None while True: @@ -4956,23 +4983,17 @@ def arming_test_mission(self): else: return None - def set_safetyswitch_on(self, **kwargs): - self.set_safetyswitch(1, **kwargs) + def set_safetyswitch_on(self): + self.set_safetyswitch(1) - def set_safetyswitch_off(self, **kwargs): - self.set_safetyswitch(0, **kwargs) + def set_safetyswitch_off(self): + self.set_safetyswitch(0) 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""" @@ -4980,19 +5001,36 @@ def armed(self): return self.mav.motors_armed() def send_mavlink_arm_command(self): - self.send_cmd( - mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - p1=1, # ARM - ) - - def send_mavlink_disarm_command(self): - self.send_cmd( + target_sysid = 1 + target_compid = 1 + self.mav.mav.command_long_send( + target_sysid, + target_compid, mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - p1=0, # DISARM - ) + 1, # confirmation + 1, # ARM + 0, + 0, + 0, + 0, + 0, + 0) def send_mavlink_run_prearms_command(self): - self.send_cmd(mavutil.mavlink.MAV_CMD_RUN_PREARM_CHECKS) + 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) def analog_rangefinder_parameters(self): return { @@ -5008,29 +5046,43 @@ 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, - p1=32451, # magic number to trap - timeout=timeout, - ) + self.run_cmd(mavutil.mavlink.MAV_CMD_DEBUG_TRAP, + 32451, # magic number to trap + 0, + 0, + 0, + 0, + 0, + 0, + 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, - p1=1, # ARM - 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, + 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) 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, - p1=1, # ARM + 1, # ARM + 0, + 0, + 0, + 0, + 0, + 0, target_sysid=None, target_compid=None, )) @@ -5042,12 +5094,15 @@ def arm_vehicle(self, timeout=20, force=False): if force: p2 = 2989 try: - self.run_cmd( - mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - p1=1, # ARM - p2=p2, - timeout=timeout, - ) + self.run_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + 1, # ARM + p2, + 0, + 0, + 0, + 0, + 0, + timeout=timeout) except ValueError as e: # statustexts are queued; give it a second to arrive: self.delay_sim_time(5) @@ -5073,23 +5128,30 @@ 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, - p1=0, # DISARM - p2=p2, - timeout=timeout, - ) + self.run_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + 0, # DISARM + p2, + 0, + 0, + 0, + 0, + 0, + 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, - p1=0, # DISARM - timeout=10, - want_result=mavutil.mavlink.MAV_RESULT_FAILED, - ) + 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.progress("Disarm - forced") self.disarm_vehicle(force=True) @@ -5464,7 +5526,7 @@ def get_parameter(self, *args, **kwargs): def send_get_parameter_direct(self, name): encname = name - if sys.version_info.major >= 3 and not isinstance(encname, bytes): + if sys.version_info.major >= 3 and type(encname) != bytes: encname = bytes(encname, 'ascii') self.mav.mav.param_request_read_send(self.sysid_thismav(), 1, @@ -5579,7 +5641,7 @@ def context_stop_collecting(self, msg_type): del context.collections[msg_type] return ret - def context_pop(self, process_interaction_allowed=True): + def context_pop(self): """Set parameters to origin values in reverse order.""" dead = self.contexts.pop() # remove hooks first; these hooks can raise exceptions which @@ -5588,19 +5650,13 @@ def context_pop(self, process_interaction_allowed=True): 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] - if process_interaction_allowed: - self.set_parameters(dead_parameters_dict, add_to_context=False) + self.set_parameters(dead_parameters_dict, add_to_context=False) if getattr(self, "old_binary", None) is not None: self.stop_SITL() @@ -5643,72 +5699,20 @@ def __exit__(self, type, value, traceback): def sysid_thismav(self): return 1 - def create_MISSION_ITEM_INT( - self, - t, - p1=0, - p2=0, - p3=0, - p4=0, - x=0, - y=0, - z=0, - frame=mavutil.mavlink.MAV_FRAME_GLOBAL, - autocontinue=1, - current=0, - target_system=1, - target_component=1, - seq=0, - mission_type=mavutil.mavlink.MAV_MISSION_TYPE_MISSION, - ): - return self.mav.mav.mission_item_int_encode( - target_system, - target_component, - seq, # seq - frame, - t, - 0, # current - 0, # autocontinue - p1, # p1 - 0, # p2 - 0, # p3 - 0, # p4 - x, # latitude - y, # longitude - z, # altitude - mission_type - ) - def run_cmd_int(self, command, - p1=0, - p2=0, - p3=0, - p4=0, - x=0, - y=0, - z=0, + p1, + p2, + p3, + p4, + x, + y, + z, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED, timeout=10, target_sysid=None, target_compid=None, - frame=mavutil.mavlink.MAV_FRAME_GLOBAL_INT, - p5=None, - p6=None, - p7=None, - quiet=False, - mav=None, - ): - - if mav is None: - mav = self.mav - - if p5 is not None: - x = p5 - if p6 is not None: - y = p6 - if p7 is not None: - z = p7 + frame=mavutil.mavlink.MAV_FRAME_GLOBAL_INT): if target_sysid is None: target_sysid = self.sysid_thismav() @@ -5718,48 +5722,30 @@ def run_cmd_int(self, self.get_sim_time() # required for timeout in run_cmd_get_ack to work """Send a MAVLink command int.""" - if not quiet: - try: - command_name = mavutil.mavlink.enums["MAV_CMD"][command].name - except KeyError: - command_name = "UNKNOWNu" - self.progress("Sending COMMAND_INT to (%u,%u) (%s=%u) (p1=%f p2=%f p3=%f p4=%f p5=%u p6=%u p7=%f)" % - ( - target_sysid, - target_compid, - command_name, - command, - p1, - p2, - p3, - p4, - x, - y, - z)) - mav.mav.command_int_send(target_sysid, - target_compid, - frame, - command, - 0, # current - 0, # autocontinue - p1, - p2, - p3, - p4, - x, - y, - z) - self.run_cmd_get_ack(command, want_result, timeout, mav=mav) + self.mav.mav.command_int_send(target_sysid, + target_compid, + frame, + command, + 0, # current + 0, # autocontinue + p1, + p2, + p3, + p4, + x, + y, + z) + self.run_cmd_get_ack(command, want_result, timeout) def send_cmd(self, command, - p1=0, - p2=0, - p3=0, - p4=0, - p5=0, - p6=0, - p7=0, + p1, + p2, + p3, + p4, + p5, + p6, + p7, target_sysid=None, target_compid=None, mav=None, @@ -5772,17 +5758,16 @@ def send_cmd(self, target_sysid = self.sysid_thismav() if target_compid is None: target_compid = 1 + try: + command_name = mavutil.mavlink.enums["MAV_CMD"][command].name + except KeyError: + command_name = "UNKNOWN=%u" % command if not quiet: - try: - command_name = mavutil.mavlink.enums["MAV_CMD"][command].name - except KeyError: - command_name = "UNKNOWN" - self.progress("Sending COMMAND_LONG to (%u,%u) (%s=%u) (p1=%f p2=%f p3=%f p4=%f p5=%f p6=%f p7=%f)" % + self.progress("Sending COMMAND_LONG to (%u,%u) (%s) (p1=%f p2=%f p3=%f p4=%f p5=%f p6=%f p7=%f)" % ( target_sysid, target_compid, command_name, - command, p1, p2, p3, @@ -5804,13 +5789,13 @@ def send_cmd(self, def run_cmd(self, command, - p1=0, - p2=0, - p3=0, - p4=0, - p5=0, - p6=0, - p7=0, + p1, + p2, + p3, + p4, + p5, + p6, + p7, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED, target_sysid=None, target_compid=None, @@ -5831,7 +5816,6 @@ def run_cmd(self, target_sysid=target_sysid, target_compid=target_compid, mav=mav, - quiet=quiet, ) self.run_cmd_get_ack(command, want_result, timeout, quiet=quiet, mav=mav) @@ -5871,7 +5855,13 @@ 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, - p1=seq, + seq, + 0, + 0, + 0, + 0, + 0, + 0, timeout=1, target_sysid=target_sysid, target_compid=target_compid) @@ -5901,7 +5891,7 @@ def verify_parameter_values(self, parameter_stuff, max_delta=0.0): for param in parameter_stuff: fetched_value = self.get_parameter(param) wanted_value = parameter_stuff[param] - if isinstance(wanted_value, tuple): + if type(wanted_value) == tuple: max_delta = wanted_value[1] wanted_value = wanted_value[0] if abs(fetched_value - wanted_value) > max_delta: @@ -6004,8 +5994,13 @@ def get_bearing(loc1, loc2): def send_cmd_do_set_mode(self, mode): self.send_cmd( mavutil.mavlink.MAV_CMD_DO_SET_MODE, - p1=mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, - p2=self.get_mode_from_mode_mapping(mode), + mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, + self.get_mode_from_mode_mapping(mode), + 0, + 0, + 0, + 0, + 0 ) def assert_mode(self, mode): @@ -6081,21 +6076,23 @@ def get_mode_from_mode_mapping(self, mode): def run_cmd_do_set_mode(self, mode, timeout=30, - run_cmd=None, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): - if run_cmd is None: - run_cmd = self.run_cmd base_mode = mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED custom_mode = self.get_mode_from_mode_mapping(mode) - run_cmd( + self.run_cmd( mavutil.mavlink.MAV_CMD_DO_SET_MODE, - p1=base_mode, - p2=custom_mode, + base_mode, + custom_mode, + 0, + 0, + 0, + 0, + 0, want_result=want_result, - timeout=timeout, + timeout=timeout ) - def do_set_mode_via_command_XYZZY(self, mode, run_cmd, timeout=30): + def do_set_mode_via_command_long(self, mode, timeout=30): """Set mode with a command long message.""" tstart = self.get_sim_time() want_custom_mode = self.get_mode_from_mode_mapping(mode) @@ -6103,18 +6100,12 @@ def do_set_mode_via_command_XYZZY(self, mode, run_cmd, timeout=30): remaining = timeout - (self.get_sim_time_cached() - tstart) if remaining <= 0: raise AutoTestTimeoutException("Failed to change mode") - self.run_cmd_do_set_mode(mode, run_cmd=run_cmd, timeout=10) + self.run_cmd_do_set_mode(mode, timeout=10) m = self.wait_heartbeat() self.progress("Got mode=%u want=%u" % (m.custom_mode, want_custom_mode)) if m.custom_mode == want_custom_mode: return - def do_set_mode_via_command_long(self, mode, timeout=30): - self.do_set_mode_via_command_XYZZY(mode, self.run_cmd, timeout=timeout) - - def do_set_mode_via_command_int(self, mode, timeout=30): - self.do_set_mode_via_command_XYZZY(mode, self.run_cmd_int, timeout=timeout) - def mavproxy_do_set_mode_via_command_long(self, mavproxy, mode, timeout=30): """Set mode with a command long message with Mavproxy.""" base_mode = mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED @@ -6190,10 +6181,13 @@ def guided_achieve_heading(self, heading, accuracy=None): raise NotAchievedException("Did not achieve heading") self.run_cmd( mavutil.mavlink.MAV_CMD_CONDITION_YAW, - p1=heading, # target angle - p2=10, # degrees/second - p3=1, # -1 is counter-clockwise, 1 clockwise - p4=0, # 1 for relative, 0 for absolute + 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 ) m = self.mav.recv_match(type='VFR_HUD', blocking=True) self.progress("heading=%d want=%d" % (m.heading, int(heading))) @@ -6214,12 +6208,15 @@ 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, - p1=relay_num, - p2=on_off, - timeout=timeout, - ) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, + relay_num, + on_off, + 0, + 0, + 0, + 0, + 0, + timeout=timeout) def do_set_relay_mavproxy(self, relay_num, on_off): """Set relay with mavproxy.""" @@ -6233,11 +6230,15 @@ 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=p1, # param1 - want_result=want_result, - ) + 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) def do_fence_enable(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): self.do_fence_en_or_dis_able(True, want_result=want_result) @@ -6447,10 +6448,6 @@ def validator(value2, target2=None): **kwargs ) - def groundspeed(self): - m = self.assert_receive_message('VFR_HUD') - return m.groundspeed - def wait_groundspeed(self, speed_min, speed_max, timeout=30, **kwargs): self.wait_vfr_hud_speed("groundspeed", speed_min, speed_max, timeout=timeout, **kwargs) @@ -6520,7 +6517,7 @@ def validator(value2, target2): ) def wait_and_maintain(self, value_name, target, current_value_getter, validator=None, accuracy=2.0, timeout=30, **kwargs): - if isinstance(target, Vector3): + if type(target) is Vector3: return self.wait_and_maintain_vector( value_name, target, @@ -6551,7 +6548,6 @@ def wait_and_maintain_vector(self, achieving_duration_start = None sum_of_achieved_values = Vector3() last_value = Vector3() - last_fail_print = 0 count_of_achieved_values = 0 called_function = kwargs.get("called_function", None) minimum_duration = kwargs.get("minimum_duration", 0) @@ -6561,15 +6557,7 @@ def wait_and_maintain_vector(self, self.progress("Waiting for %s=(%s)" % (value_name, str(target))) last_print_time = 0 - while True: # if we failed to received message with the getter the sim time isn't updated # noqa - now = self.get_sim_time_cached() - if now - tstart > timeout: - raise AutoTestTimeoutException( - "Failed to attain %s want %s, reached %s" % - (value_name, - str(target), - str(sum_of_achieved_values / count_of_achieved_values) if count_of_achieved_values != 0 else str(last_value))) # noqa - + while self.get_sim_time_cached() < tstart + timeout: # if we failed to received message with the getter the sim time isn't updated # noqa last_value = current_value_getter() if called_function is not None: called_function(last_value, target) @@ -6606,10 +6594,11 @@ def wait_and_maintain_vector(self, achieving_duration_start = None sum_of_achieved_values.zero() count_of_achieved_values = 0 - if now - last_fail_print > 1: - self.progress("Waiting for (%s), got %s" % - (target, last_value)) - last_fail_print = now + raise AutoTestTimeoutException( + "Failed to attain %s want %s, reached %s" % + (value_name, + str(target), + str(sum_of_achieved_values / count_of_achieved_values) if count_of_achieved_values != 0 else str(last_value))) def validate_kwargs(self, kwargs, valid={}): for key in kwargs: @@ -6652,10 +6641,7 @@ def wait_and_maintain_range(self, while self.get_sim_time_cached() < tstart + timeout: # if we failed to received message with the getter the sim time isn't updated # noqa last_value = current_value_getter() if called_function is not None: - if print_diagnostics_as_target_not_range: - called_function(last_value, target) - else: - called_function(last_value, minimum, maximum) + called_function(last_value, target) if validator is not None: if print_diagnostics_as_target_not_range: is_value_valid = validator(last_value, target) @@ -6684,7 +6670,7 @@ def wait_and_maintain_range(self, achieved_duration_bit) ) else: - if isinstance(last_value, float): + if type(last_value) is float: self.progress( "%s=%0.2f (%s between %s and %s)%s" % (value_name, @@ -7094,35 +7080,6 @@ def assert_rc_channel_value(self, channel, value): raise NotAchievedException("Expected %s to be %u got %u" % (channel, value, m_value)) - def do_reposition(self, - loc, - frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT): - '''send a DO_REPOSITION command for a location''' - self.run_cmd_int( - mavutil.mavlink.MAV_CMD_DO_REPOSITION, - 0, - 0, - 0, - 0, - int(loc.lat*1e7), # lat* 1e7 - int(loc.lng*1e7), # lon* 1e7 - loc.alt, - frame=frame - ) - - def add_rally_point(self, loc, seq, total): - '''add a rally point at the given location''' - self.mav.mav.rally_point_send(1, # target system - 0, # target component - seq, # sequence number - total, # total count - int(loc.lat * 1e7), - int(loc.lng * 1e7), - loc.alt, # relative alt - 0, # "break" alt?! - 0, # "land dir" - 0) # flags - def wait_location(self, loc, accuracy=5.0, @@ -7232,10 +7189,6 @@ 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) @@ -7259,10 +7212,6 @@ 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() @@ -7285,8 +7234,7 @@ def wait_gps_sys_status_not_present_or_enabled_and_healthy(self, timeout=30): if (not (m.onboard_control_sensors_health & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_GPS)): self.progress("GPS not healthy") continue - self.progress("GPS healthy after %f/%f seconds" % - ((now - tstart), timeout)) + self.progress("GPS healthy") return def assert_sensor_state(self, sensor, present=True, enabled=True, healthy=True, verbose=False): @@ -7366,19 +7314,6 @@ 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, @@ -7523,7 +7458,6 @@ 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: @@ -7539,11 +7473,6 @@ 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) @@ -7682,19 +7611,6 @@ 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) @@ -7716,15 +7632,6 @@ 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: @@ -7782,14 +7689,7 @@ def send_statustext(self, text): text = bytes(text, "ascii") elif 'unicode' in str(type(text)): text = text.encode('ascii') - seq = 0 - while len(text): - self.mav.mav.statustext_send(mavutil.mavlink.MAV_SEVERITY_WARNING, text[:50], id=self.statustext_id, chunk_seq=seq) - text = text[50:] - seq += 1 - self.statustext_id += 1 - if self.statustext_id > 255: - self.statustext_id = 1 + self.mav.mav.statustext_send(mavutil.mavlink.MAV_SEVERITY_WARNING, text) def get_stacktrace(self): return ''.join(traceback.format_stack()) @@ -7846,14 +7746,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, suppress_stdout=False): + def run_one_test(self, test, interact=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, suppress_stdout=suppress_stdout) + result = self.run_one_test_attempt(test, interact=interact, attempt=i+2) if result.passed: return result self.progress("Run attempt failed. Retrying") - return self.run_one_test_attempt(test, interact=interact, attempt=1, suppress_stdout=suppress_stdout) + return self.run_one_test_attempt(test, interact=interact, attempt=1) def print_exception_caught(self, e, send_statustext=True): self.progress("Exception caught: %s" % @@ -7870,36 +7770,11 @@ def progress_file_content(self, filepath): for line in f: self.progress(line.rstrip()) - 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): + def run_one_test_attempt(self, test, interact=False, attempt=1): '''called by run_one_test to actually run the test in a retry loop''' name = test.name desc = test.description test_function = test.function - test_kwargs = test.kwargs if attempt != 1: self.progress("RETRYING %s" % name) test_output_filename = self.buildlogs_path("%s-%s-retry-%u.txt" % @@ -7908,7 +7783,7 @@ def run_one_test_attempt(self, test, interact=False, attempt=1, suppress_stdout= test_output_filename = self.buildlogs_path("%s-%s.txt" % (self.log_name(), name)) - tee = TeeBoth(test_output_filename, 'w', self.mavproxy_logfile, suppress_stdout=suppress_stdout) + tee = TeeBoth(test_output_filename, 'w', self.mavproxy_logfile) start_message_hooks = self.mav.message_hooks @@ -7936,7 +7811,7 @@ def run_one_test_attempt(self, test, interact=False, attempt=1, suppress_stdout= orig_speedup = self.get_parameter("SIM_SPEEDUP") self.set_parameter("SIM_SPEEDUP", test.speedup) - test_function(**test_kwargs) + test_function() except Exception as e: self.print_exception_caught(e) ex = e @@ -7955,6 +7830,12 @@ def run_one_test_attempt(self, test, interact=False, attempt=1, suppress_stdout= 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 @@ -7964,16 +7845,28 @@ def run_one_test_attempt(self, test, interact=False, attempt=1, suppress_stdout= except Exception: # process is dead self.progress("No heartbeat after test", send_statustext=False) - self.dump_process_status(result) + 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") - passed = False - reset_needed = True + # 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") - try: - self.context_pop(process_interaction_allowed=ardupilot_alive) - except Exception as e: - self.print_exception_caught(e, send_statustext=False) passed = False + reset_needed = True # if we haven't already reset ArduPilot because it's dead, # then ensure the vehicle was disarmed at the end of the test. @@ -7994,7 +7887,7 @@ def run_one_test_attempt(self, test, interact=False, attempt=1, suppress_stdout= self.progress("Force-rebooting SITL") self.reboot_sitl() # that'll learn it passed = False - elif ardupilot_alive and not passed: # implicit reboot after a failed test: + elif not passed: # implicit reboot after a failed test: self.progress("Test failed but ArduPilot process alive; rebooting") self.reboot_sitl() # that'll learn it @@ -8022,7 +7915,7 @@ def run_one_test_attempt(self, test, interact=False, attempt=1, suppress_stdout= # pop off old contexts to clean up message hooks etc while len(self.contexts) > old_contexts_length: try: - self.context_pop(process_interaction_allowed=ardupilot_alive) + self.context_pop() except Exception as e: self.print_exception_caught(e, send_statustext=False) self.progress("Done popping extra contexts") @@ -8070,7 +7963,7 @@ def run_one_test_attempt(self, test, interact=False, attempt=1, suppress_stdout= def defaults_filepath(self): return None - def start_mavproxy(self, sitl_rcin_port=None): + def start_mavproxy(self): self.start_mavproxy_count += 1 if self.mavproxy is not None: return self.mavproxy @@ -8082,17 +7975,11 @@ def start_mavproxy(self, sitl_rcin_port=None): 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, - sitl_rcin_port=sitl_rcin_port, - ) + pexpect_timeout=pexpect_timeout) mavproxy.expect(r'Telemetry log: (\S+)\r\n') self.logfile = mavproxy.match.group(1) self.progress("LOGFILE %s" % self.logfile) @@ -8111,9 +7998,7 @@ def stop_mavproxy(self, mavproxy): util.pexpect_close(mavproxy) self._mavproxy = None - def start_SITL(self, binary=None, sitl_home=None, **sitl_args): - if sitl_home is None: - sitl_home = self.sitl_home() + def start_SITL(self, binary=None, **sitl_args): start_sitl_args = { "breakpoints": self.breakpoints, "disable_breakpoints": self.disable_breakpoints, @@ -8121,7 +8006,7 @@ def start_SITL(self, binary=None, sitl_home=None, **sitl_args): "gdb_no_tui": self.gdb_no_tui, "gdbserver": self.gdbserver, "lldb": self.lldb, - "home": sitl_home, + "home": self.sitl_home(), "speedup": self.speedup, "valgrind": self.valgrind, "callgrind": self.callgrind, @@ -8140,24 +8025,20 @@ def start_SITL(self, binary=None, sitl_home=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['customisation']] + start_sitl_args["customisations"] = [sup_binary[1]] start_sitl_args["supplementary"] = True - 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) + sup_prog_link = util.start_SITL(sup_binary[0], **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_supplementary_programs(self): + def get_suplementary_programs(self): return self.sup_prog def stop_sup_program(self, instance=None): @@ -8190,18 +8071,23 @@ def start_sup_program(self, instance=None, args=None): "callgrind": self.callgrind, "wipe": True, } - 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 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]] if args is not None: - start_sitl_args["customisations"] = [sup_binary['customisation'], args] + start_sitl_args["customisations"] = [self.sup_binaries[instance][1], args] start_sitl_args["supplementary"] = True - 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 + sup_prog_link = util.start_SITL(self.sup_binaries[instance][0], **start_sitl_args) + self.sup_prog[instance] = sup_prog_link # add to list self.expect_list_add(sup_prog_link) # add to expect list def sitl_is_running(self): @@ -8421,8 +8307,14 @@ def poll_home_position(self, quiet=True, timeout=30): try: self.run_cmd( mavutil.mavlink.MAV_CMD_GET_HOME_POSITION, - quiet=quiet, - ) + 0, + 0, + 0, + 0, + 0, + 0, + 0, + quiet=quiet) except ValueError: continue m = self.mav.messages.get("HOME_POSITION", None) @@ -8524,12 +8416,15 @@ 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, - p5=new_x, - p6=new_y, - p7=new_z/1000.0, # mm => m - ) + 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 + ) home = self.poll_home_position() self.progress("home: %s" % str(home)) @@ -8559,35 +8454,43 @@ 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, - p5=0, # lat - p6=0, # lon - p7=new_z/1000.0, # mm => m - ) + 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 + ) 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, - p5=new_x, - p6=new_y, - p7=new_z/1000.0, # mm => m - ) + 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 + ) 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, - p1=1, # use current location - p5=37, # lat - p6=21, # lon - p7=new_z/1000.0, # mm => m - ) + 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 + ) home = self.poll_home_position() self.progress("home: %s" % str(home)) if self.distance_to_home() > 1: @@ -8601,18 +8504,33 @@ 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, - 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.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.progress("zeroed mag parameters") params = [ [("SIM_MAG1_OFS1_X", "COMPASS_OFS_X", 0), @@ -8696,15 +8614,17 @@ 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, - p1=tmask, # p1: mag_mask - p2=0, # retry - p3=0, # autosave - p4=0, # delay - want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED, - timeout=20, - ) + 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, + ) mavproxy.send("sitl_magcal\n") def do_prep_mag_cal_test(mavproxy, params): @@ -8812,19 +8732,29 @@ 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, - p1=target_mask, - want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED, - timeout=20, - ) + 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, + ) if reached_pct[0] > 30: - self.run_cmd( - mavutil.mavlink.MAV_CMD_DO_CANCEL_MAG_CAL, - p1=target_mask, - want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED, - ) + 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, + ) if tstop is None: tstop = self.get_sim_time_cached() if tstop is not None: @@ -8920,12 +8850,17 @@ 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, - p1=target_mask, # p1: mag_mask - want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED, - timeout=20, - ) + 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.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): @@ -8947,15 +8882,17 @@ 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, - 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.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.disarm_vehicle() self.mavproxy_unload_module(mavproxy, "relay") self.mavproxy_unload_module(mavproxy, "sitl_calibration") @@ -9225,18 +9162,16 @@ def FixedYawCalibration(self): ss = self.assert_receive_message('SIMSTATE', timeout=1, verbose=True) - self.run_cmd( - mavutil.mavlink.MAV_CMD_FIXED_MAG_CAL_YAW, - p1=math.degrees(ss.yaw), - ) - self.verify_parameter_values(wanted) + self.run_cmd(mavutil.mavlink.MAV_CMD_FIXED_MAG_CAL_YAW, + math.degrees(ss.yaw), # param1 + 0, # param2 + 0, # param3 + 0, # param4 - # 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), - ) + 0, # param5 + 0, # param6 + 0 # param7 + ) self.verify_parameter_values(wanted) self.progress("Rebooting and making sure we could arm with these values") @@ -9523,16 +9458,6 @@ 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): @@ -9696,11 +9621,16 @@ 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, - p1=1, # ARM - want_result=mavutil.mavlink.MAV_RESULT_FAILED, - ) + 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.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) @@ -9713,11 +9643,16 @@ 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, - p1=1, # ARM - want_result=mavutil.mavlink.MAV_RESULT_FAILED, - ) + 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.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) @@ -9775,7 +9710,7 @@ def ArmFeatures(self): self.progress("ALL PASS") # TODO : Test arming magic; - def measure_message_rate(self, victim_message, timeout=10, mav=None): + def get_message_rate(self, victim_message, timeout=10, mav=None): if mav is None: mav = self.mav tstart = self.get_sim_time() @@ -9799,40 +9734,28 @@ def measure_message_rate(self, victim_message, timeout=10, mav=None): def rate_to_interval_us(self, rate): return 1/float(rate)*1000000.0 - def interval_us_to_rate(self, interval): - if interval == 0: - raise ValueError("Zero interval is infinite rate") - return 1000000.0/float(interval) - - def set_message_rate_hz(self, id, rate_hz, mav=None, run_cmd=None): + def set_message_rate_hz(self, id, rate_hz, mav=None): '''set a message rate in Hz; 0 for original, -1 to disable''' - if run_cmd is None: - run_cmd = self.run_cmd - if isinstance(id, str): + if type(id) == str: id = eval("mavutil.mavlink.MAVLINK_MSG_ID_%s" % id) if rate_hz == 0 or rate_hz == -1: set_interval = rate_hz else: set_interval = self.rate_to_interval_us(rate_hz) - run_cmd( - mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, - p1=id, - p2=set_interval, - mav=mav, - ) - - def get_message_rate_hz(self, id, mav=None, run_cmd=None): - '''return rate message is being sent, in Hz''' - if run_cmd is None: - run_cmd = self.run_cmd - - interval = self.get_message_interval(id, mav=mav, run_cmd=run_cmd) - return self.interval_us_to_rate(interval) + self.run_cmd(mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, + id, + set_interval, + 0, + 0, + 0, + 0, + 0, + mav=mav) def send_get_message_interval(self, victim_message, mav=None): if mav is None: mav = self.mav - if isinstance(victim_message, str): + if type(victim_message) == str: victim_message = eval("mavutil.mavlink.MAVLINK_MSG_ID_%s" % victim_message) mav.mav.command_long_send( 1, @@ -9847,32 +9770,6 @@ def send_get_message_interval(self, victim_message, mav=None): 0, 0) - def get_message_interval(self, victim_message, mav=None, run_cmd=None): - '''returns message interval in microseconds''' - if run_cmd is None: - run_cmd = self.run_cmd - - self.send_get_message_interval(victim_message, mav=mav) - m = self.assert_receive_message('MESSAGE_INTERVAL', timeout=1, mav=mav) - - if isinstance(victim_message, str): - victim_message = eval("mavutil.mavlink.MAVLINK_MSG_ID_%s" % victim_message) - if m.message_id != victim_message: - raise NotAchievedException(f"Unexpected ID in MESSAGE_INTERVAL (want={victim_message}, got={m.message_id}") - - return m.interval_us - - def set_message_interval(self, victim_message, interval_us, mav=None): - '''sets message interval in microseconds''' - if isinstance(victim_message, str): - victim_message = eval("mavutil.mavlink.MAVLINK_MSG_ID_%s" % victim_message) - self.run_cmd( - mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, - p1=victim_message, - p2=interval_us, - mav=mav, - ) - def test_rate(self, desc, in_rate, @@ -9889,7 +9786,7 @@ def test_rate(self, self.set_message_rate_hz(victim_message, in_rate, mav=mav) - new_measured_rate = self.measure_message_rate(victim_message, timeout=message_rate_sample_period, mav=mav) + new_measured_rate = self.get_message_rate(victim_message, timeout=message_rate_sample_period, mav=mav) self.progress( "Measured rate: %f (want %f)" % (round(new_measured_rate, ndigits=ndigits), @@ -9933,22 +9830,6 @@ def SET_MESSAGE_INTERVAL(self): self.start_subtest('Many-message tests') self.test_set_message_interval_many() - def MESSAGE_INTERVAL_COMMAND_INT(self): - '''Test MAV_CMD_SET_MESSAGE_INTERVAL works as COMMAND_INT''' - original_rate = round(self.measure_message_rate("VFR_HUD", 20)) - self.context_set_message_rate_hz('VFR_HUD', original_rate*2, run_cmd=self.run_cmd_int) - if abs(original_rate*2 - round(self.get_message_rate_hz("VFR_HUD", run_cmd=self.run_cmd_int))) > 1: - raise NotAchievedException("Did not set rate") - - self.start_subtest("Use REQUEST_MESSAGE via COMMAND_INT") - # 148 is AUTOPILOT_VERSION: - self.context_collect('AUTOPILOT_VERSION') - self.run_cmd_int(mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE, 148) - self.delay_sim_time(2) - count = len(self.context_collection('AUTOPILOT_VERSION')) - if count != 1: - raise NotAchievedException(f"Did not get single AUTOPILOT_VERSION message (count={count}") - def test_set_message_interval_many(self): messages = [ 'CAMERA_FEEDBACK', @@ -9977,7 +9858,7 @@ def assert_message_rate_hz(self, message, want_rate, sample_period=20, ndigits=0 if mav is None: mav = self.mav self.drain_mav(mav) - rate = round(self.measure_message_rate(message, sample_period, mav=mav), ndigits=ndigits) + rate = round(self.get_message_rate(message, sample_period, mav=mav), ndigits=ndigits) self.progress("%s: Want=%f got=%f" % (message, round(want_rate, ndigits=ndigits), round(rate, ndigits=ndigits))) if rate != want_rate: raise NotAchievedException("Did not get expected rate (want=%f got=%f)" % (want_rate, rate)) @@ -9985,7 +9866,7 @@ def assert_message_rate_hz(self, message, want_rate, sample_period=20, ndigits=0 def test_set_message_interval_basic(self): ex = None try: - rate = round(self.measure_message_rate("VFR_HUD", 20)) + rate = round(self.get_message_rate("VFR_HUD", 20)) self.progress("Initial rate: %u" % rate) self.test_rate("Test set to %u" % (rate/2,), rate/2, rate/2, victim_message="VFR_HUD") @@ -9995,7 +9876,7 @@ def test_set_message_interval_basic(self): self.test_rate("Resetting original rate", 0, rate) self.progress("try getting a message which is not ordinarily streamed out") - rate = round(self.measure_message_rate("CAMERA_FEEDBACK", 20)) + rate = round(self.get_message_rate("CAMERA_FEEDBACK", 20)) if rate != 0: raise PreconditionFailedException("Already getting CAMERA_FEEDBACK") self.progress("try various message rates") @@ -10012,7 +9893,7 @@ def test_set_message_interval_basic(self): want_rate = self.get_parameter("SCHED_LOOP_RATE") * 0.8 self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_CAMERA_FEEDBACK, want_rate) - rate = round(self.measure_message_rate("CAMERA_FEEDBACK", 20)) + rate = round(self.get_message_rate("CAMERA_FEEDBACK", 20)) self.set_parameter("SIM_SPEEDUP", old_speedup) self.progress("Want=%f got=%f" % (want_rate, rate)) if abs(rate - want_rate) > 2: @@ -10043,21 +9924,25 @@ def test_set_message_interval_basic(self): def send_poll_message(self, message_id, target_sysid=None, target_compid=None, quiet=False, mav=None): if mav is None: mav = self.mav - if isinstance(message_id, str): + 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, - p1=message_id, - target_sysid=target_sysid, - target_compid=target_compid, - quiet=quiet, - mav=mav, - ) + 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) def poll_message(self, message_id, timeout=10, quiet=False, mav=None): if mav is None: mav = self.mav - if isinstance(message_id, str): + if type(message_id) == str: message_id = eval("mavutil.mavlink.MAVLINK_MSG_ID_%s" % message_id) tstart = self.get_sim_time() # required for timeout in run_cmd_get_ack to work self.send_poll_message(message_id, quiet=quiet, mav=mav) @@ -10066,7 +9951,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: @@ -10107,7 +9992,7 @@ def REQUEST_MESSAGE(self, timeout=60): '''Test MAV_CMD_REQUEST_MESSAGE''' self.set_parameter("CAM1_TYPE", 1) # Camera with servo trigger self.reboot_sitl() # needed for CAM1_TYPE to take effect - rate = round(self.measure_message_rate("CAMERA_FEEDBACK", 10)) + rate = round(self.get_message_rate("CAMERA_FEEDBACK", 10)) if rate != 0: raise PreconditionFailedException("Receiving camera feedback") self.poll_message("CAMERA_FEEDBACK") @@ -10118,11 +10003,10 @@ 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_blimp(): + if not self.is_tracker() and not self.is_plane(): self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE) - 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_MISSION) + if not self.is_sub() and not self.is_tracker(): self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_RALLY) self.last_wp_load = time.time() return @@ -10131,11 +10015,16 @@ def clear_mission(self, mission_type, target_system=1, target_component=1): target_component, 0, mission_type) - 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, - }) + 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,)) if mission_type == mavutil.mavlink.MAV_MISSION_TYPE_MISSION: self.last_wp_load = time.time() @@ -10638,78 +10527,6 @@ 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 @@ -11072,9 +10889,6 @@ 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 @@ -11112,7 +10926,7 @@ def upload_fences_from_locations(self, seq = 0 items = [] for locs in list_of_list_of_locs: - if isinstance(locs, dict): + if type(locs) == dict: # circular fence if vertex_type == mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION: v = mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION @@ -11160,22 +10974,6 @@ def upload_fences_from_locations(self, self.check_fence_upload_download(items) - def rally_MISSION_ITEM_INT_from_loc(self, loc): - return self.create_MISSION_ITEM_INT( - mavutil.mavlink.MAV_CMD_NAV_RALLY_POINT, - x=int(loc.lat*1e7), - y=int(loc.lng*1e7), - z=loc.alt, - frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, - mission_type=mavutil.mavlink.MAV_MISSION_TYPE_RALLY - ) - - def upload_rally_points_from_locations(self, rally_point_locs): - '''takes a sequence of locations, sets vehicle rally points to those locations''' - items = [self.rally_MISSION_ITEM_INT_from_loc(x) for x in rally_point_locs] - self.correct_wp_seq_numbers(items) - self.check_rally_upload_download(items) - def wait_for_initial_mode(self): '''wait until we get a heartbeat with an expected initial mode (the one specified in the vehicle constructor)''' @@ -11458,19 +11256,27 @@ 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, - p1=1, # ARM - timeout=4, - want_result=mavutil.mavlink.MAV_RESULT_FAILED, - ) + 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.context_pop() - self.run_cmd( - mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - p1=1, # ARM - timeout=4, - want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED, - ) + 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.disarm_vehicle() def assert_not_receiving_message(self, message, timeout=1, mav=None): @@ -11554,7 +11360,13 @@ def AdvancedFailsafe(self): self.context_collect("STATUSTEXT") self.run_cmd( mavutil.mavlink.MAV_CMD_DO_FLIGHTTERMINATION, - p1=1, # terminate + 1, # terminate + 0, + 0, + 0, + 0, + 0, + 0, ) self.wait_statustext("Terminating due to GCS request", check_context=True) self.context_pop() @@ -11594,10 +11406,9 @@ 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:%u" % port, # GPS2 is NMEA.... - "--uartF=tcpclient:127.0.0.1:%u" % port, # serial5 spews to localhost port + "--uartE=tcp:6735", # GPS2 is NMEA.... + "--uartF=tcpclient:127.0.0.1:6735", # serial5 spews to localhost:6735 ]) self.do_timesync_roundtrip() self.wait_gps_fix_type_gte(3) @@ -11757,17 +11568,6 @@ 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() @@ -11879,15 +11679,19 @@ def Button(self): # try to arm the vehicle: self.run_cmd( mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - p1=1, # ARM - want_result=mavutil.mavlink.MAV_RESULT_FAILED, + 1, # ARM + 0, + 0, + 0, + 0, + 0, + 0, + want_result=mavutil.mavlink.MAV_RESULT_FAILED ) - self.assert_prearm_failure("Motors Emergency Stopped", - other_prearm_failures_fatal=False) + self.wait_statustext("PreArm: Motors Emergency Stopped", check_context=True) self.reboot_sitl() - self.assert_prearm_failure( - "Motors Emergency Stopped", - other_prearm_failures_fatal=False) + self.delay_sim_time(10) + self.assert_prearm_failure("Motors Emergency Stopped") self.context_pop() self.reboot_sitl() @@ -12211,11 +12015,10 @@ def FRSkyPassThroughStatustext(self): "RPM1_TYPE": 10, # enable RPM output "TERRAIN_ENABLE": 0, }) - port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:%u" % port # serial5 spews to localhost port + "--uartF=tcp:6735" # serial5 spews to localhost:6735 ]) - frsky = FRSkyPassThrough(("127.0.0.1", port), + frsky = FRSkyPassThrough(("127.0.0.1", 6735), get_time=self.get_sim_time_cached) # waiting until we are ready to arm should ensure our wanted @@ -12232,10 +12035,14 @@ def FRSkyPassThroughStatustext(self): self.context_collect('STATUSTEXT') command = mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION - self.send_cmd( - command, - p3=1, # p3, baro - ) + self.send_cmd(command, + 0, # p1 + 0, # p2 + 1, # p3, baro + 0, # p4 + 0, # p5 + 0, # p6 + 0) # p7 # 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) @@ -12302,11 +12109,10 @@ 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:%u" % port # serial5 spews to localhost port + "--uartF=tcp:6735" # serial5 spews to localhost:6735 ]) - frsky = FRSkyPassThrough(("127.0.0.1", port), + frsky = FRSkyPassThrough(("127.0.0.1", 6735), get_time=self.get_sim_time_cached) self.wait_ready_to_arm() @@ -12457,11 +12263,10 @@ 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:%u" % port # serial5 spews to localhost port + "--uartF=tcp:6735" # serial5 spews to localhost:6735 ]) - frsky = FRSkyPassThrough(("127.0.0.1", port)) + frsky = FRSkyPassThrough(("127.0.0.1", 6735)) frsky.connect() sport_to_mavlite = SPortToMAVlite() @@ -12732,11 +12537,10 @@ 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:%u" % port # serial5 spews to localhost port + "--uartF=tcp:6735" # serial5 spews to localhost:6735 ]) - frsky = FRSkySPort(("127.0.0.1", port), verbose=True) + frsky = FRSkySPort(("127.0.0.1", 6735), verbose=True) self.wait_ready_to_arm() # we need to start the engine to get some RPM readings, we do it for plane only @@ -12805,21 +12609,24 @@ 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:%u" % port # serial5 spews to localhost port + "--uartF=tcp:6735" # serial5 spews to localhost:6735 ]) - frsky = FRSkyD(("127.0.0.1", port)) + frsky = FRSkyD(("127.0.0.1", 6735)) 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, - p1=65535, # battery mask - p2=96, # percentage - ) + self.run_cmd(mavutil.mavlink.MAV_CMD_BATTERY_RESET, + 255, # battery mask + 96, # percentage + 0, + 0, + 0, + 0, + 0, + 0) m = self.assert_receive_message('BATTERY_STATUS', timeout=1) want_battery_remaining_pct = m.battery_remaining @@ -12924,11 +12731,10 @@ 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:%u" % port # serial5 spews to localhost port + "--uartF=tcp:6735" # serial5 spews to localhost:6735 ]) - ltm = LTM(("127.0.0.1", port)) + ltm = LTM(("127.0.0.1", 6735)) self.wait_ready_to_arm() wants = { @@ -12967,11 +12773,10 @@ 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:%u" % port # serial5 spews to localhost port + "--uartF=tcp:6735" # serial5 spews to localhost:6735 ]) - devo = DEVO(("127.0.0.1", port)) + devo = DEVO(("127.0.0.1", 6735)) self.wait_ready_to_arm() m = self.assert_receive_message('GLOBAL_POSITION_INT', timeout=1) @@ -13040,11 +12845,10 @@ 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:%u" % port # serial5 spews to localhost port + "--uartF=tcp:6735" # serial5 spews to localhost:6735 ]) - msp = MSP_DJI(("127.0.0.1", port)) + msp = MSP_DJI(("127.0.0.1", 6735)) self.wait_ready_to_arm() tstart = self.get_sim_time() @@ -13068,11 +12872,10 @@ 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:%u" % port # serial5 reads from to localhost port + "--uartF=tcp:6735" # serial5 reads from to localhost:6735 ]) - crsf = CRSF(("127.0.0.1", port)) + crsf = CRSF(("127.0.0.1", 6735)) crsf.connect() self.progress("Writing vtx_frame") @@ -13093,33 +12896,6 @@ def CRSF(self): if ex is not None: raise ex - def CompassPrearms(self): - '''test compass prearm checks''' - self.wait_ready_to_arm() - # XY are checked specially: - for axis in 'X', 'Y': # ArduPilot only checks these two axes - self.context_push() - self.set_parameter(f"COMPASS_OFS2_{axis}", 1000) - self.assert_prearm_failure("Compasses inconsistent") - self.context_pop() - self.wait_ready_to_arm() - - # now test the total anglular difference: - self.context_push() - self.set_parameters({ - "COMPASS_OFS2_X": 1000, - "COMPASS_OFS2_Y": -1000, - "COMPASS_OFS2_Z": -10000, - }) - self.assert_prearm_failure("Compasses inconsistent") - self.context_pop() - self.wait_ready_to_arm() - # the following line papers over a probably problem with the - # EKF recovering from bad compass offsets. Without it, the - # EKF will maintain a 10-degree offset from the true compass - # heading seemingly indefinitely. - self.reboot_sitl() - def AHRS_ORIENTATION(self): '''test AHRS_ORIENTATION parameter works''' self.context_push() @@ -13148,26 +12924,23 @@ def GPSTypes(self): # if gps_type is None we auto-detect sim_gps = [ # (0, "NONE"), - (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 + (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 # (9, "FILE"), ] self.context_collect("STATUSTEXT") - for (sim_gps_type, name, gps_type, detect_name, serial_protocol, detect_prefix) in sim_gps: + for (sim_gps_type, name, gps_type, detect_name) 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("%s as %s" % (detect_prefix, detect_name), check_context=True) + self.wait_statustext("detected as %s" % detect_name, check_context=True) n = self.poll_home_position(timeout=120) distance = self.get_distance_int(orig, n) if distance > 1: @@ -13321,7 +13094,7 @@ def write_content_to_filepath(self, content, filepath): '''write biunary content to filepath''' with open(filepath, "wb") as f: if sys.version_info.major >= 3: - if not isinstance(content, bytes): + if type(content) != bytes: raise NotAchievedException("Want bytes to write_content_to_filepath") f.write(content) f.close() @@ -13385,77 +13158,6 @@ def EmbeddedParamParser(self): self.customise_SITL_commandline([], binary=binary_with_defaults) self.assert_parameter_values(param_values) - def _MotorTest(self, - command, - timeout=60, - mot1_servo_chan=1, - mot4_servo_chan=4, - wait_finish_text=True, - quadplane=False): - '''Run Motor Tests (with specific mavlink message)''' - self.start_subtest("Testing PWM output") - pwm_in = 1300 - # default frame is "+" - start motor of 2 is "B", which is - # motor 1... see - # https://ardupilot.org/copter/docs/connect-escs-and-motors.html - command( - mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, - p1=2, # start motor - p2=mavutil.mavlink.MOTOR_TEST_THROTTLE_PWM, - p3=pwm_in, # pwm-to-output - p4=2, # timeout in seconds - p5=2, # number of motors to output - p6=0, # compass learning - timeout=timeout, - ) - # long timeouts here because there's a pause before we start motors - self.wait_servo_channel_value(mot1_servo_chan, pwm_in, timeout=10) - self.wait_servo_channel_value(mot4_servo_chan, pwm_in, timeout=10) - if wait_finish_text: - self.wait_statustext("finished motor test") - self.wait_disarmed() - # wait_disarmed is not sufficient here; it's actually the - # *motors* being armed which causes the problem, not the - # vehicle's arm state! Could we use SYS_STATUS here instead? - self.delay_sim_time(10) - self.end_subtest("Testing PWM output") - - self.start_subtest("Testing percentage output") - percentage = 90.1 - # since MOT_SPIN_MIN and MOT_SPIN_MAX are not set, the RC3 - # min/max are used. - expected_pwm = 1000 + (self.get_parameter("RC3_MAX") - self.get_parameter("RC3_MIN")) * percentage/100.0 - # quadplane doesn't use the expect value - it wants 1900 - # rather than the calculated 1901... - if quadplane: - expected_pwm = 1900 - self.progress("expected pwm=%f" % expected_pwm) - command( - mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, - p1=2, # start motor - p2=mavutil.mavlink.MOTOR_TEST_THROTTLE_PERCENT, - p3=percentage, # pwm-to-output - p4=2, # timeout in seconds - p5=2, # number of motors to output - p6=0, # compass learning - timeout=timeout, - ) - self.wait_servo_channel_value(mot1_servo_chan, expected_pwm, timeout=10) - self.wait_servo_channel_value(mot4_servo_chan, expected_pwm, timeout=10) - if wait_finish_text: - self.wait_statustext("finished motor test") - self.wait_disarmed() - # wait_disarmed is not sufficient here; it's actually the - # *motors* being armed which causes the problem, not the - # vehicle's arm state! Could we use SYS_STATUS here instead? - self.delay_sim_time(10) - self.end_subtest("Testing percentage output") - - def MotorTest(self, timeout=60, **kwargs): - '''Run Motor Tests''' # common to Copter and QuadPlane - self._MotorTest(self.run_cmd, **kwargs) - self._MotorTest(self.run_cmd_int, **kwargs) - def tests(self): return [ self.PIDTuning, @@ -13491,7 +13193,7 @@ def autotest(self, tests=None, allow_skips=True): tests = self.tests() all_tests = [] for test in tests: - if not isinstance(test, Test): + if type(test) != Test: test = Test(test) all_tests.append(test) @@ -13500,11 +13202,7 @@ 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])) @@ -13647,17 +13345,25 @@ 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, - p1=0, # 0: pause, 1: continue - ) + 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 def send_resume_command(self): '''resume AUTO/GUIDED modes''' - self.run_cmd( - mavutil.mavlink.MAV_CMD_DO_PAUSE_CONTINUE, - p1=1, # 0: pause, 1: continue - ) + 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 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 a790eec33454f5..192455b115c7b9 100644 --- a/Tools/autotest/default_params/blimp.parm +++ b/Tools/autotest/default_params/blimp.parm @@ -38,18 +38,6 @@ 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 @@ -88,18 +76,16 @@ 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.15 +MAX_POS_XY 0.3 MAX_POS_YAW 0.3 MAX_POS_Z 0.15 -MAX_VEL_XY 0.2 -MAX_VEL_YAW 0.4 +MAX_VEL_XY 0.4 +MAX_VEL_YAW 0.5 MAX_VEL_Z 0.2 -VELXY_D 1.0 +VELXY_D 0.0 VELXY_FF 0.0 VELXY_FLTD 3.0 VELXY_FLTE 3.0 @@ -112,7 +98,7 @@ VELYAW_FLTD 3.0 VELYAW_FLTE 3.0 VELYAW_I 0.8 VELYAW_IMAX 5.0 -VELYAW_P 10.0 +VELYAW_P 15.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 5b5e58adda58a6..d12d6725f63c4a 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.0005 -ATC_RAT_PIT_P 0.02 -ATC_RAT_PIT_FF 0.0 +ATC_RAT_PIT_D 0.0012 +ATC_RAT_PIT_P 0.001 +ATC_RAT_PIT_FF 0.17 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 deleted file mode 100644 index 38269b2d2aa68e..00000000000000 --- a/Tools/autotest/default_params/copter-heli-gas.parm +++ /dev/null @@ -1,9 +0,0 @@ -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 994bf370801ecf..08a836dd01f97f 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 12 +H_COL_ANG_MAX 10 H_COL_ANG_MIN -2 H_RSC_MODE 2 H_RSC_SETPOINT 66 diff --git a/Tools/autotest/default_params/copter-tri.parm b/Tools/autotest/default_params/copter-tri.parm index 16927c554186f5..0f861b77064703 100644 --- a/Tools/autotest/default_params/copter-tri.parm +++ b/Tools/autotest/default_params/copter-tri.parm @@ -1,6 +1,6 @@ SERVO7_MIN 1000 SERVO7_MAX 2000 -SERVO7_TRIM 1500 +SERVO7_TRIM 1360 FRAME_CLASS 7 ATC_RAT_YAW_FLTE 5.0 MOT_YAW_SV_ANGLE 60 diff --git a/Tools/autotest/default_params/copter-winch.parm b/Tools/autotest/default_params/copter-winch.parm deleted file mode 100644 index 4f2279c00d9927..00000000000000 --- a/Tools/autotest/default_params/copter-winch.parm +++ /dev/null @@ -1,6 +0,0 @@ -# servo winch type -# winch connected to servo output channel 9 -# rc input chnanel 9 used by pilot to control winch -WINCH_TYPE 1 -SERVO9_FUNCTION 88 -RC9_OPTION 45 diff --git a/Tools/autotest/default_params/copter.parm b/Tools/autotest/default_params/copter.parm index 17e5c25b26d972..7f41498a4ece87 100644 --- a/Tools/autotest/default_params/copter.parm +++ b/Tools/autotest/default_params/copter.parm @@ -2,8 +2,8 @@ FRAME_TYPE 0 FS_THR_ENABLE 1 ARSPD_PIN 1 ARSPD_BUS 2 -ATC_RAT_YAW_P 0.3 -ATC_RAT_YAW_I 0.02 +ATC_RAT_YAW_P 0.09 +ATC_RAT_YAW_I 0.009 BATT_MONITOR 4 COMPASS_OFS_X 5 COMPASS_OFS_Y 13 diff --git a/Tools/autotest/default_params/periph.parm b/Tools/autotest/default_params/periph.parm deleted file mode 100644 index 506d302c6676cc..00000000000000 --- a/Tools/autotest/default_params/periph.parm +++ /dev/null @@ -1,24 +0,0 @@ -# parameters for SITL peripheral - -GPS_TYPE 1 -COMPASS_ENABLE 1 -BARO_ENABLE 1 -ARSPD_TYPE 100 -RNGFND1_TYPE 100 -RNGFND1_MAX_CM 12000 -BATT_MONITOR 16 -BATT_I2C_BUS 2 - -# by default disable motors/servos, overridden in vehicle specific parameters -OUT1_FUNCTION -1 -OUT2_FUNCTION -1 -OUT3_FUNCTION -1 -OUT4_FUNCTION -1 -OUT5_FUNCTION -1 -OUT6_FUNCTION -1 -OUT7_FUNCTION -1 -OUT8_FUNCTION -1 - -# enable some simulated ADSB vehicles -SIM_ADSB_COUNT 4 -ADSB_PORT 2 diff --git a/Tools/autotest/default_params/plane-jsbsim.parm b/Tools/autotest/default_params/plane-jsbsim.parm index a77ec1084ef7d1..e53e15acd64f26 100644 --- a/Tools/autotest/default_params/plane-jsbsim.parm +++ b/Tools/autotest/default_params/plane-jsbsim.parm @@ -7,7 +7,6 @@ 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 deleted file mode 100644 index 0fe872319e3659..00000000000000 --- a/Tools/autotest/default_params/quad-can.parm +++ /dev/null @@ -1,8 +0,0 @@ -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 deleted file mode 100644 index 2e29723e09d2eb..00000000000000 --- a/Tools/autotest/default_params/quad-periph.parm +++ /dev/null @@ -1,17 +0,0 @@ -# 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 deleted file mode 100644 index 3971389f992640..00000000000000 --- a/Tools/autotest/default_params/quadplane-can.parm +++ /dev/null @@ -1,13 +0,0 @@ -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 deleted file mode 100644 index dc7a19687b6122..00000000000000 --- a/Tools/autotest/default_params/quadplane-periph.parm +++ /dev/null @@ -1,23 +0,0 @@ -# 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 0dddcea057b831..8c2008abcd779a 100644 --- a/Tools/autotest/helicopter.py +++ b/Tools/autotest/helicopter.py @@ -152,13 +152,7 @@ def takeoff(self, self.progress("Raising rotor speed") self.set_rc(8, 2000) self.progress("wait for rotor runup to complete") - 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) + self.wait_servo_channel_value(8, 1659, timeout=10) # wait for motor runup self.delay_sim_time(20) @@ -194,7 +188,7 @@ def FlyEachFrame(self): # to carry the path to the JSON. actual_model = model.split(":")[0] defaults = self.model_defaults_filepath(actual_model) - if not isinstance(defaults, list): + if type(defaults) != list: defaults = [defaults] self.customise_SITL_commandline( ["--defaults", ','.join(defaults), ], @@ -205,18 +199,6 @@ 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) @@ -303,7 +285,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, 1650) + self.set_rc(3, 1600) self.progress("Monitoring takeoff") self.wait_altitude(6.9, 8, relative=True) @@ -401,27 +383,20 @@ 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, 1000) + self.set_rc(3, 1300) self.set_rc(8, 1000) - self.wait_servo_channel_value(8, 1199, timeout=3) + self.wait_servo_channel_value(8, 1200, 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, 1500) + self.set_rc(3, 1700) self.delay_sim_time(5) # initiate autorotation again - self.set_rc(3, 1000) + self.set_rc(3, 1200) self.set_rc(8, 1000) self.wait_statustext(r"SIM Hit ground at ([0-9.]+) m/s", @@ -556,7 +531,11 @@ def scurve_nasty_mission(self, target_system=1, target_component=1): copy.copy(wp5_by_three), self.mission_item_rtl(target_system=target_system, target_component=target_component), ]) - self.correct_wp_seq_numbers(ret) + # renumber the items: + count = 0 + for item in ret: + item.seq = count + count += 1 return ret def scurve_nasty_up_mission(self, target_system=1, target_component=1): @@ -629,7 +608,11 @@ def scurve_nasty_up_mission(self, target_system=1, target_component=1): wp7, self.mission_item_rtl(target_system=target_system, target_component=target_component), ]) - self.correct_wp_seq_numbers(ret) + # renumber the items: + count = 0 + for item in ret: + item.seq = count + count += 1 return ret def fly_mission_points(self, points): @@ -810,7 +793,6 @@ def tests(self): self.SplineWaypoint, self.AutoRotation, self.ManAutoRotation, - self.governortest, self.FlyEachFrame, self.AirspeedDrivers, self.TurbineStart, diff --git a/Tools/ros2/ardupilot_dds_tests/resource/ardupilot_dds_tests b/Tools/autotest/jsb_sim/__init__.py similarity index 100% rename from Tools/ros2/ardupilot_dds_tests/resource/ardupilot_dds_tests rename to Tools/autotest/jsb_sim/__init__.py diff --git a/Tools/autotest/jsb_sim/runsim.py b/Tools/autotest/jsb_sim/runsim.py new file mode 100755 index 00000000000000..416a4e97697d06 --- /dev/null +++ b/Tools/autotest/jsb_sim/runsim.py @@ -0,0 +1,385 @@ +#!/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 3717b3613f9899..ce3cf76fed012e 100644 --- a/Tools/autotest/locations.txt +++ b/Tools/autotest/locations.txt @@ -56,7 +56,6 @@ 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 deleted file mode 100644 index d6d1cb78778ba4..00000000000000 --- a/Tools/autotest/logger_metadata/emit_md.py +++ /dev/null @@ -1,82 +0,0 @@ -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 50c026a21651f3..24ff2106653a02 100755 --- a/Tools/autotest/logger_metadata/enum_parse.py +++ b/Tools/autotest/logger_metadata/enum_parse.py @@ -72,11 +72,6 @@ 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 ea58198ea2bcc8..f7d4bd9e715690 100755 --- a/Tools/autotest/logger_metadata/parse.py +++ b/Tools/autotest/logger_metadata/parse.py @@ -11,7 +11,6 @@ import emit_html import emit_rst import emit_xml -import emit_md import enum_parse from enum_parse import EnumDocco @@ -51,7 +50,6 @@ def __init__(self, vehicle): emit_html.HTMLEmitter(), emit_rst.RSTEmitter(), emit_xml.XMLEmitter(), - emit_md.MDEmitter(), ] class Docco(object): @@ -186,7 +184,7 @@ def emit_output(self): # expand things like PIDR,PIDQ,PIDA into multiple doccos new_doccos = [] for docco in self.doccos: - if isinstance(docco.name, list): + if type(docco.name) == list: for name in docco.name: tmpdocco = copy.copy(docco) tmpdocco.name = name diff --git a/Tools/autotest/param_metadata/param.py b/Tools/autotest/param_metadata/param.py index 3cc5266a47ecc4..8ba842c3ea1389 100644 --- a/Tools/autotest/param_metadata/param.py +++ b/Tools/autotest/param_metadata/param.py @@ -4,9 +4,6 @@ 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): @@ -50,7 +47,6 @@ def has_param(self, pname): 'Volatile', 'ReadOnly', 'Calibration', - 'Vector3Parameter', ] # Follow SI units conventions from: diff --git a/Tools/autotest/param_metadata/param_parse.py b/Tools/autotest/param_metadata/param_parse.py index 7f5f9c21202dee..057eaf00077d21 100755 --- a/Tools/autotest/param_metadata/param_parse.py +++ b/Tools/autotest/param_metadata/param_parse.py @@ -12,6 +12,7 @@ import os import re import sys +import glob from argparse import ArgumentParser from param import (Library, Parameter, Vehicle, known_group_fields, @@ -90,19 +91,16 @@ def debug(str_to_print): def lua_applets(): '''return list of Library objects for lua applets and drivers''' lua_lib = Library("", reference="Lua Script", not_rst=True, check_duplicates=True) - dirs = ["libraries/AP_Scripting/applets", "libraries/AP_Scripting/drivers"] + patterns = ["libraries/AP_Scripting/applets/*.lua", "libraries/AP_Scripting/drivers/*.lua"] paths = [] - for d in dirs: - for root, dirs, files in os.walk(os.path.join(apm_path, d)): - for file in files: - if not file.endswith(".lua"): - continue - f = os.path.join(root, file) - debug("Adding lua path %s" % f) - # the library is expected to have the path as a relative path from within - # a vehicle directory - f = f.replace(apm_path, "../") - paths.append(f) + for p in patterns: + debug("Adding lua paths %s" % p) + luafiles = glob.glob(os.path.join(apm_path, p)) + for f in luafiles: + # the library is expected to have the path as a relative path from within + # a vehicle directory + f = f.replace(apm_path, "../") + paths.append(f) setattr(lua_lib, "Path", ','.join(paths)) return lua_lib @@ -111,7 +109,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("", reference="VEHICLE") # the "" is tacked onto the front of param name +ap_vehicle_lib = Library("") # the "" is tacked onto the front of param name setattr(ap_vehicle_lib, "Path", os.path.join('..', 'libraries', 'AP_Vehicle', 'AP_Vehicle.cpp')) libraries.append(ap_vehicle_lib) @@ -302,7 +300,6 @@ def process_library(vehicle, library, pathprefix=None): # a parameter is considered to be vehicle-specific if # there does not exist a Values: or Values{VehicleName} # for that vehicle but @Values{OtherVehicle} exists. - seen_values_or_bitmask_for_this_vehicle = False seen_values_or_bitmask_for_other_vehicle = False for field in fields: only_for_vehicles = field[1].split(",") @@ -316,26 +313,11 @@ def process_library(vehicle, library, pathprefix=None): error("tagged param: unknown parameter metadata field '%s'" % field[0]) continue if vehicle.name not in only_for_vehicles: - if len(only_for_vehicles) and field[0] in documentation_tags_which_are_comma_separated_nv_pairs: + if len(only_for_vehicles) and field[0] in ['Values', 'Bitmask']: seen_values_or_bitmask_for_other_vehicle = True continue - - append_value = False - if field[0] in documentation_tags_which_are_comma_separated_nv_pairs: - if vehicle.name in only_for_vehicles: - if seen_values_or_bitmask_for_this_vehicle: - append_value = hasattr(p, field[0]) - seen_values_or_bitmask_for_this_vehicle = True - else: - if seen_values_or_bitmask_for_this_vehicle: - continue - append_value = hasattr(p, field[0]) - value = re.sub('@PREFIX@', library.name, field[2]) - if append_value: - setattr(p, field[0], getattr(p, field[0]) + ',' + value) - else: - setattr(p, field[0], value) + setattr(p, field[0], value) if (getattr(p, 'Values', None) is not None and getattr(p, 'Bitmask', None) is not None): @@ -352,25 +334,11 @@ def process_library(vehicle, library, pathprefix=None): # applicable for this vehicle. continue - 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) + 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)) @@ -448,9 +416,6 @@ 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"): @@ -458,10 +423,6 @@ 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 @@ -491,11 +452,6 @@ 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(":") @@ -560,7 +516,7 @@ def validate(param, is_library=False): i = i.replace(" ", "") values.append(i.partition(":")[0]) if (len(values) != len(set(values))): - error("Duplicate values found" + str({x for x in values if values.count(x) > 1})) + error("Duplicate values found") # Validate units if (hasattr(param, "Units")): if (param.__dict__["Units"] != "") and (param.__dict__["Units"] not in known_units): diff --git a/Tools/autotest/pysim/aircraft.py b/Tools/autotest/pysim/aircraft.py new file mode 100644 index 00000000000000..f3981648733131 --- /dev/null +++ b/Tools/autotest/pysim/aircraft.py @@ -0,0 +1,112 @@ +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 new file mode 100755 index 00000000000000..c76f68f3fc949d --- /dev/null +++ b/Tools/autotest/pysim/testwind.py @@ -0,0 +1,20 @@ +#!/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 6b5729bf9894f2..d4f6e8b576353c 100644 --- a/Tools/autotest/pysim/util.py +++ b/Tools/autotest/pysim/util.py @@ -7,6 +7,7 @@ import atexit import math import os +import random import re import shlex import signal @@ -14,10 +15,12 @@ import sys import tempfile import time - +from math import acos, atan2, cos, pi, sqrt import pexpect +from pymavlink.rotmat import Vector3, Matrix3 + if sys.version_info[0] >= 3: ENCODING = 'ascii' else: @@ -109,13 +112,12 @@ def waf_configure(board, coverage=False, ekf_single=False, postype_single=False, - force_32bit=False, + sitl_32bit=False, extra_args=[], extra_hwdef=None, ubsan=False, ubsan_abort=False, num_aux_imus=0, - dronecan_tests=False, extra_defines={}): cmd_configure = [relwaf(), "configure", "--board", board] if debug: @@ -128,16 +130,14 @@ def waf_configure(board, cmd_configure.append('--ekf-single') if postype_single: cmd_configure.append('--postype-single') - if force_32bit: - cmd_configure.append('--force-32bit') + if sitl_32bit: + cmd_configure.append('--sitl-32bit') if ubsan: cmd_configure.append('--ubsan') if ubsan_abort: cmd_configure.append('--ubsan-abort') if num_aux_imus > 0: cmd_configure.append('--num-aux-imus=%u' % num_aux_imus) - if dronecan_tests: - cmd_configure.append('--enable-dronecan-tests') if extra_hwdef is not None: cmd_configure.extend(['--extra-hwdef', extra_hwdef]) for nv in extra_defines.items(): @@ -174,11 +174,10 @@ def build_SITL( j=None, math_check_indexes=False, postype_single=False, - force_32bit=False, + sitl_32bit=False, ubsan=False, ubsan_abort=False, num_aux_imus=0, - dronecan_tests=False, ): # first configure @@ -190,12 +189,11 @@ def build_SITL( ekf_single=ekf_single, postype_single=postype_single, coverage=coverage, - force_32bit=force_32bit, + sitl_32bit=sitl_32bit, ubsan=ubsan, ubsan_abort=ubsan_abort, extra_defines=extra_defines, num_aux_imus=num_aux_imus, - dronecan_tests=dronecan_tests, extra_args=extra_configure_args,) # then clean @@ -211,8 +209,7 @@ def build_SITL( def build_examples(board, j=None, debug=False, clean=False, configure=True, math_check_indexes=False, coverage=False, - ekf_single=False, postype_single=False, force_32bit=False, ubsan=False, ubsan_abort=False, - num_aux_imus=0, dronecan_tests=False, + ekf_single=False, postype_single=False, sitl_32bit=False, ubsan=False, ubsan_abort=False, num_aux_imus=0, extra_configure_args=[]): # first configure if configure: @@ -223,11 +220,10 @@ def build_examples(board, j=None, debug=False, clean=False, configure=True, math ekf_single=ekf_single, postype_single=postype_single, coverage=coverage, - force_32bit=force_32bit, + sitl_32bit=sitl_32bit, ubsan=ubsan, ubsan_abort=ubsan_abort, - extra_args=extra_configure_args, - dronecan_tests=dronecan_tests) + extra_args=extra_configure_args) # then clean if clean: @@ -262,11 +258,10 @@ def build_tests(board, coverage=False, ekf_single=False, postype_single=False, - force_32bit=False, + sitl_32bit=False, ubsan=False, ubsan_abort=False, num_aux_imus=0, - dronecan_tests=False, extra_configure_args=[]): # first configure @@ -278,12 +273,10 @@ def build_tests(board, ekf_single=ekf_single, postype_single=postype_single, coverage=coverage, - force_32bit=force_32bit, + sitl_32bit=sitl_32bit, ubsan=ubsan, ubsan_abort=ubsan_abort, - num_aux_imus=num_aux_imus, - dronecan_tests=dronecan_tests, - extra_args=extra_configure_args,) + extra_args=extra_configure_args) # then clean if clean: @@ -415,10 +408,7 @@ def close(self): def write(self, data): self.buffer += data - lines = self.buffer.split("\n") - self.buffer = lines[-1] - lines.pop() - for line in lines: + for line in self.buffer.split("\n"): self.print_prefixed_line(line) def print_prefixed_line(self, line): @@ -438,7 +428,6 @@ def start_SITL(binary, home=None, model=None, speedup=1, - sim_rate_hz=None, defaults_filepath=None, unhide_parameters=False, gdbserver=False, @@ -506,7 +495,7 @@ def start_SITL(binary, '-d', '-m', '-S', 'ardupilot-gdb', - 'gdb', '--cd', os.getcwd(), '-x', '/tmp/x.gdb', binary, '--args']) + 'gdb', '-x', '/tmp/x.gdb', binary, '--args']) elif lldb: f = open("/tmp/x.lldb", "w") for breakingpoint in breakpoints: @@ -532,10 +521,15 @@ def start_SITL(binary, if home is not None: cmd.extend(['--home', home]) cmd.extend(['--model', model]) - if speedup is not None and speedup != 1: + if speedup != 1: cmd.extend(['--speedup', str(speedup)]) - if sim_rate_hz is not None: - cmd.extend(['--rate', str(sim_rate_hz)]) + 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 unhide_parameters: cmd.extend(['--unhide-groups']) # somewhere for MAVProxy to connect to: @@ -543,14 +537,6 @@ def start_SITL(binary, if not enable_fgview_output: cmd.append("--disable-fgview") - if defaults_filepath is not None: - if isinstance(defaults_filepath, list): - defaults = [reltopdir(path) for path in defaults_filepath] - if len(defaults): - cmd.extend(['--defaults', ",".join(defaults)]) - else: - cmd.extend(['--defaults', reltopdir(defaults_filepath)]) - cmd.extend(customisations) pexpect_logfile_prefix = stdout_prefix @@ -605,6 +591,8 @@ def start_SITL(binary, rest = cmd[1:] 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. @@ -636,15 +624,11 @@ def MAVProxy_version(): def start_MAVProxy_SITL(atype, aircraft=None, setup=False, - master=None, + master='tcp:127.0.0.1:5762', 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) @@ -653,11 +637,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: @@ -678,6 +662,8 @@ 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: @@ -742,6 +728,51 @@ def check_parent(parent_pid=None): sys.exit(1) +def EarthRatesToBodyRates(dcm, earth_rates): + """Convert the angular velocities from earth frame to + body frame. Thanks to James Goppert for the formula + + all inputs and outputs are in radians + + returns a gyro vector in body frame, in rad/s . + """ + from math import sin, cos + + (phi, theta, psi) = dcm.to_euler() + phiDot = earth_rates.x + thetaDot = earth_rates.y + psiDot = earth_rates.z + + p = phiDot - psiDot * sin(theta) + q = cos(phi) * thetaDot + sin(phi) * psiDot * cos(theta) + r = cos(phi) * psiDot * cos(theta) - sin(phi) * thetaDot + return Vector3(p, q, r) + + +def BodyRatesToEarthRates(dcm, gyro): + """Convert the angular velocities from body frame to + earth frame. + + all inputs and outputs are in radians/s + + returns a earth rate vector. + """ + from math import sin, cos, tan, fabs + + p = gyro.x + q = gyro.y + r = gyro.z + + (phi, theta, psi) = dcm.to_euler() + + phiDot = p + tan(theta) * (q * sin(phi) + r * cos(phi)) + thetaDot = q * cos(phi) - r * sin(phi) + if fabs(cos(theta)) < 1.0e-20: + theta += 1.0e-10 + psiDot = (q * sin(phi) + r * cos(phi)) / cos(theta) + return Vector3(phiDot, thetaDot, psiDot) + + def gps_newpos(lat, lon, bearing, distance): """Extrapolate latitude/longitude given a heading and distance thanks to http://www.movable-type.co.uk/scripts/latlong.html . @@ -792,6 +823,132 @@ 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 9709dcd917e0d2..fbcb2b54df0d60 100644 --- a/Tools/autotest/pysim/vehicleinfo.py +++ b/Tools/autotest/pysim/vehicleinfo.py @@ -149,11 +149,6 @@ 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", @@ -188,11 +183,6 @@ 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": { @@ -202,11 +192,6 @@ 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", @@ -265,11 +250,6 @@ 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", @@ -429,7 +409,6 @@ 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 9074baaceacade..bccc3e04c5235b 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -11,10 +11,8 @@ import math from pymavlink import mavutil -from pymavlink.rotmat import Vector3 from common import AutoTest -from common import Test from common import AutoTestTimeoutException, NotAchievedException, PreconditionFailedException import operator @@ -171,10 +169,6 @@ def AirMode(self): self.wait_ready_to_arm() self.start_subtest("Verify that arming with switch does not spin motors in other modes") - # disable compass magnetic field arming check that is triggered by the simulated lean of vehicle - # this is required because adjusting the AHRS_TRIM values only affects the IMU and not external compasses - arming_magthresh = self.get_parameter("ARMING_MAGTHRESH") - self.set_parameter("ARMING_MAGTHRESH", 0) # introduce a large attitude error to verify that stabilization is not active ahrs_trim_x = self.get_parameter("AHRS_TRIM_X") self.set_parameter("AHRS_TRIM_X", math.radians(-60)) @@ -213,9 +207,8 @@ def AirMode(self): self.progress("Waiting for Motor1 to stop") self.wait_servo_channel_value(5, min_pwm, comparator=operator.le) self.wait_ready_to_arm() - # remove attitude error and reinstance compass arming check + # remove attitude error self.set_parameter("AHRS_TRIM_X", ahrs_trim_x) - self.set_parameter("ARMING_MAGTHRESH", arming_magthresh) self.start_subtest("verify that AIRMODE auxswitch turns airmode on/off while armed") """set RC7_OPTION to AIRMODE""" @@ -359,99 +352,53 @@ def EXTENDED_SYS_STATE(self): '''Check extended sys state works''' self.EXTENDED_SYS_STATE_SLT() - def QAUTOTUNE(self): - '''test Plane QAutoTune mode''' - - # this is a list of all parameters modified by QAUTOTUNE. Set - # them so that when the context is popped we get the original - # values back: - parameter_values = self.get_parameters([ - "Q_A_RAT_RLL_P", - "Q_A_RAT_RLL_I", - "Q_A_RAT_RLL_D", - "Q_A_ANG_RLL_P", - "Q_A_ACCEL_R_MAX", - "Q_A_RAT_PIT_P", - "Q_A_RAT_PIT_I", - "Q_A_RAT_PIT_D", - "Q_A_ANG_PIT_P", - "Q_A_ACCEL_P_MAX", - "Q_A_RAT_YAW_P", - "Q_A_RAT_YAW_I", - "Q_A_RAT_YAW_FLTE", - "Q_A_ANG_YAW_P", - "Q_A_ACCEL_Y_MAX", - ]) - self.set_parameters(parameter_values) - - self.takeoff(15, mode='GUIDED') + def fly_qautotune(self): + self.change_mode("QHOVER") + self.wait_ready_to_arm() + self.arm_vehicle() + self.set_rc(3, 1800) + self.wait_altitude(30, + 40, + relative=True, + timeout=30) self.set_rc(3, 1500) - self.change_mode("QLOITER") self.change_mode("QAUTOTUNE") tstart = self.get_sim_time() - self.context_collect('STATUSTEXT') - while True: + sim_time_expected = 5000 + deadline = tstart + sim_time_expected + while self.get_sim_time_cached() < deadline: now = self.get_sim_time_cached() - if now - tstart > 5000: - raise NotAchievedException("Did not get success message") - try: - self.wait_text("AutoTune: Success", timeout=1, check_context=True) - except AutoTestTimeoutException: + m = self.mav.recv_match(type='STATUSTEXT', + blocking=True, + timeout=1) + if m is None: continue - # got success message - break + self.progress("STATUSTEXT (%u<%u): %s" % (now, deadline, m.text)) + if "AutoTune: Success" in m.text: + break self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart)) - self.context_clear_collection('STATUSTEXT') - - self.progress("Landing to save gains") self.set_rc(3, 1200) - self.wait_speed_vector( - Vector3(float('nan'), float('nan'), 1.4), - timeout=5, - ) - self.wait_speed_vector( - Vector3(0.0, 0.0, 0.0), - timeout=20, - ) - distance = self.distance_to_home() - if distance > 20: - raise NotAchievedException("wandered from home (distance=%f)" % - (distance,)) - self.set_rc(3, 1000) - tstart = self.get_sim_time() - while True: - now = self.get_sim_time_cached() - if now - tstart > 500: - raise NotAchievedException("Did not get success message") - self.send_mavlink_disarm_command() + self.wait_altitude(-5, 1, relative=True, timeout=30) + while self.get_sim_time_cached() < deadline: + self.mavproxy.send('disarm\n') try: - self.wait_text( - "AutoTune: Saved gains for Roll Pitch Yaw.*", - timeout=0.5, - check_context=True, - regex=True, - ) + self.wait_text("AutoTune: Saved gains for Roll Pitch Yaw", timeout=0.5) except AutoTestTimeoutException: continue break - self.wait_disarmed() - self.reboot_sitl() # far from home - def takeoff(self, height, mode, timeout=30): + def takeoff(self, height, mode): """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=timeout) + timeout=30) self.set_rc(3, 1500) def do_RTL(self): @@ -461,24 +408,14 @@ def do_RTL(self): self.zero_throttle() def fly_home_land_and_disarm(self, timeout=30): - self.context_push() - self.change_mode('LOITER') - self.set_parameter('RTL_AUTOLAND', 2) - filename = "QuadPlaneDalbyRTL.txt" + self.set_parameter("LAND_TYPE", 0) + filename = "flaps.txt" self.progress("Using %s to fly home" % filename) - self.load_generic_mission(filename) - self.send_cmd_do_set_mode("RTL") - self.wait_mode('AUTO') - self.wait_current_waypoint(4) - self.wait_statustext('Land descend started') - self.wait_statustext('Land final started', timeout=60) + self.load_mission(filename) + self.change_mode("AUTO") + self.set_current_waypoint(7) self.wait_disarmed(timeout=timeout) - self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_MISSION) - # the following command is accepted, but doesn't actually - # work! Should be able to remove check_afterwards! self.set_current_waypoint(0, check_afterwards=False) - self.change_mode('MANUAL') - self.context_pop() def wait_level_flight(self, accuracy=5, timeout=30): """Wait for level flight.""" @@ -745,40 +682,6 @@ def PilotYaw(self): self.set_rc(4, 1500) self.do_RTL() - def FwdThrInVTOL(self): - '''test use of fwd motor throttle into wind''' - self.set_parameters({"SIM_WIND_SPD": 25, # need very strong wind for this test - "SIM_WIND_DIR": 360, - "Q_WVANE_ENABLE": 1, - "Q_WVANE_GAIN": 1, - "STICK_MIXING": 0, - "Q_FWD_THR_USE": 2, - "SIM_ENGINE_FAIL": 2}) # we want to fail the forward thrust motor only - - self.takeoff(10, mode="QLOITER") - self.set_rc(2, 1000) - self.delay_sim_time(10) - # Check that it is using some forward throttle - fwd_thr_pwm = self.get_servo_channel_value(3) - if fwd_thr_pwm < 1150 : - raise NotAchievedException("fwd motor pwm command low, want >= 1150 got %f" % (fwd_thr_pwm)) - # check that pitch is on limit - m = self.mav.recv_match(type='ATTITUDE', blocking=True) - pitch = math.degrees(m.pitch) - if abs(pitch + 3.0) > 0.5 : - raise NotAchievedException("pitch should be -3.0 +- 0.5 deg, got %f" % (pitch)) - self.set_rc(2, 1500) - self.delay_sim_time(5) - loc1 = self.mav.location() - self.set_parameter("SIM_ENGINE_MUL", 0) # simulate a complete loss of forward motor thrust - self.delay_sim_time(20) - self.change_mode('QLAND') - self.wait_disarmed(timeout=60) - loc2 = self.mav.location() - position_drift = self.get_distance(loc1, loc2) - if position_drift > 5.0 : - raise NotAchievedException("position drift high, want < 5.0 m got %f m" % (position_drift)) - def Weathervane(self): '''test nose-into-wind functionality''' # We test nose into wind code paths and yaw direction in copter autotest, @@ -991,19 +894,6 @@ 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) @@ -1026,24 +916,19 @@ def Tailsitter(self): raise NotAchievedException("Changed throttle output on mode change to QHOVER") self.disarm_vehicle() - def setup_ICEngine_vehicle(self, start_chan): - '''restarts SITL with an IC Engine setup''' + def ICEngine(self): + '''Test ICE Engine support''' + rc_engine_start_chan = 11 self.set_parameters({ - 'ICE_START_CHAN': start_chan, + 'ICE_START_CHAN': rc_engine_start_chan, }) - model = "quadplane-ice" + self.customise_SITL_commandline( [], model=model, defaults_filepath=self.model_defaults_filepath(model), - wipe=False, - ) - - def ICEngine(self): - '''Test ICE Engine support''' - rc_engine_start_chan = 11 - self.setup_ICEngine_vehicle(start_chan=rc_engine_start_chan) + wipe=False) self.wait_ready_to_arm() self.wait_rpm(1, 0, 0, minimum_duration=1) @@ -1082,8 +967,18 @@ def ICEngine(self): def ICEngineMission(self): '''Test ICE Engine Mission support''' rc_engine_start_chan = 11 - self.setup_ICEngine_vehicle(start_chan=rc_engine_start_chan) + self.set_parameters({ + 'ICE_START_CHAN': rc_engine_start_chan, + }) + model = "quadplane-ice" + self.customise_SITL_commandline( + [], + model=model, + defaults_filepath=self.model_defaults_filepath(model), + wipe=False) + + self.reboot_sitl() self.load_mission("mission.txt") self.wait_ready_to_arm() self.set_rc(rc_engine_start_chan, 2000) @@ -1091,81 +986,6 @@ def ICEngineMission(self): self.change_mode('AUTO') self.wait_disarmed(timeout=300) - def MAV_CMD_DO_ENGINE_CONTROL(self): - '''test MAV_CMD_DO_ENGINE_CONTROL mavlink command''' - - expected_idle_rpm_min = 65 - expected_idle_rpm_max = 75 - expected_starter_rpm_min = 345 - expected_starter_rpm_max = 355 - - rc_engine_start_chan = 11 - self.setup_ICEngine_vehicle(start_chan=rc_engine_start_chan) - - self.wait_ready_to_arm() - - for method in self.run_cmd, self.run_cmd_int: - self.change_mode('MANUAL') - self.set_rc(rc_engine_start_chan, 1500) # allow motor to run - self.wait_rpm(1, 0, 0, minimum_duration=1) - self.arm_vehicle() - self.wait_rpm(1, 0, 0, minimum_duration=1) - self.start_subtest("Start motor") - method(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=1) - self.wait_rpm(1, expected_starter_rpm_min, expected_starter_rpm_max) - self.wait_rpm(1, expected_idle_rpm_min, expected_idle_rpm_max, minimum_duration=10) - - # starting the motor while it is running is failure - # (probably wrong, but that's how this works): - self.start_subtest("try start motor again") - self.context_collect('STATUSTEXT') - method(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=1, want_result=mavutil.mavlink.MAV_RESULT_FAILED) - self.wait_statustext("already running", check_context=True) - self.context_stop_collecting('STATUSTEXT') - # shouldn't affect run state: - self.wait_rpm(1, expected_idle_rpm_min, expected_idle_rpm_max, minimum_duration=1) - - self.start_subtest("Stop motor") - method(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=0) - self.wait_rpm(1, 0, 0, minimum_duration=1) - - self.start_subtest("Stop motor (again)") - method(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=0) - self.wait_rpm(1, 0, 0, minimum_duration=1) - - self.start_subtest("Check start chan control disable") - old_start_channel_value = self.get_rc_channel_value(rc_engine_start_chan) - self.set_rc(rc_engine_start_chan, 1000) - self.context_collect('STATUSTEXT') - method(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=1, want_result=mavutil.mavlink.MAV_RESULT_FAILED) - self.wait_statustext("start control disabled", check_context=True) - self.context_stop_collecting('STATUSTEXT') - self.set_rc(rc_engine_start_chan, old_start_channel_value) - self.wait_rpm(1, 0, 0, minimum_duration=1) - - self.start_subtest("test start-at-height") - self.wait_rpm(1, 0, 0, minimum_duration=1) - self.context_collect('STATUSTEXT') - method( - mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, - p1=1, # start - p3=15.5, # ... at 15.5 metres - ) - self.wait_statustext("height set to 15.5m", check_context=True) - self.wait_rpm(1, 0, 0, minimum_duration=2) - - self.takeoff(20, mode='GUIDED') - self.wait_rpm(1, expected_starter_rpm_min, expected_starter_rpm_max, minimum_duration=1) - self.wait_statustext("Engine running", check_context=True) - self.context_stop_collecting('STATUSTEXT') - - # stop the motor again: - method(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=0) - self.wait_rpm(1, 0, 0, minimum_duration=1) - - self.change_mode('QLAND') - self.wait_disarmed() - def Ship(self): '''Ensure we can take off from simulated ship''' self.context_push() @@ -1388,71 +1208,15 @@ def RCDisableAirspeedUse(self): self.context_pop() self.reboot_sitl() - def mission_MAV_CMD_DO_VTOL_TRANSITION(self): - '''mission item forces transition''' - wps = self.create_simple_relhome_mission([ - (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30), - (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 200, 0, 30), - self.create_MISSION_ITEM_INT( - mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION, - p1=mavutil.mavlink.MAV_VTOL_STATE_MC - ), - (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 300, 200, 30), - self.create_MISSION_ITEM_INT( - mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION, - p1=mavutil.mavlink.MAV_VTOL_STATE_FW - ), - (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 100, 200, 30), - (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0), - ]) - self.check_mission_upload_download(wps) - - self.change_mode('AUTO') - self.wait_ready_to_arm() - - self.arm_vehicle() - self.wait_current_waypoint(4) - self.wait_servo_channel_value(5, 1200, comparator=operator.gt) - self.wait_current_waypoint(6) - self.wait_servo_channel_value(5, 1000, comparator=operator.eq, timeout=90) - - self.fly_home_land_and_disarm() - - def mavlink_MAV_CMD_DO_VTOL_TRANSITION(self): - '''mavlink command forces transition during mission''' - wps = self.create_simple_relhome_mission([ - (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30), - (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2000, 0, 30), - (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0), - ]) - self.check_mission_upload_download(wps) - - self.change_mode('AUTO') - self.wait_ready_to_arm() - - self.arm_vehicle() - self.wait_current_waypoint(2) - self.wait_servo_channel_value(5, 1000, comparator=operator.eq, timeout=90) - - for command in self.run_cmd, self.run_cmd_int: - command(mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION, p1=mavutil.mavlink.MAV_VTOL_STATE_MC) - self.wait_servo_channel_value(5, 1200, comparator=operator.gt, timeout=300) - command(mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION, p1=mavutil.mavlink.MAV_VTOL_STATE_FW) - self.wait_servo_channel_value(5, 1000, comparator=operator.eq, timeout=90) - - self.fly_home_land_and_disarm() - def tests(self): '''return list of all tests''' ret = super(AutoTestQuadPlane, self).tests() ret.extend([ - self.FwdThrInVTOL, self.AirMode, self.TestMotorMask, self.PilotYaw, self.ParameterChecks, - self.QAUTOTUNE, self.LogDownload, self.EXTENDED_SYS_STATE, self.Mission, @@ -1462,23 +1226,13 @@ def tests(self): self.Tailsitter, self.ICEngine, self.ICEngineMission, - self.MAV_CMD_DO_ENGINE_CONTROL, self.MidAirDisarmDisallowed, - self.GUIDEDToAUTO, self.BootInAUTO, self.Ship, self.MAV_CMD_NAV_LOITER_TO_ALT, self.LoiterAltQLand, self.VTOLLandSpiral, self.VTOLQuicktune, - Test(self.MotorTest, kwargs={ # tests motors 4 and 2 - "mot1_servo_chan": 8, # quad-x second motor cw from f-r - "mot4_servo_chan": 6, # quad-x third motor cw from f-r - "wait_finish_text": False, - "quadplane": True, - }), self.RCDisableAirspeedUse, - self.mission_MAV_CMD_DO_VTOL_TRANSITION, - self.mavlink_MAV_CMD_DO_VTOL_TRANSITION, ]) return ret diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 3825e0dbf7a8e9..c2f882dd3fa662 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -320,13 +320,27 @@ 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, p1=1) + 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.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, p1=0) + 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.set_rc(3, 1000) # start driving forward @@ -342,21 +356,38 @@ def Sprayer(self): def DriveMaxRCIN(self, timeout=30): """Drive rover at max RC inputs""" - self.progress("Testing max RC inputs") - self.change_mode("MANUAL") + self.context_push() + ex = None - self.wait_ready_to_arm() - self.arm_vehicle() + try: + self.progress("Testing max RC inputs") + self.change_mode("MANUAL") - self.set_rc(3, 2000) - self.set_rc(1, 1000) + self.wait_ready_to_arm() + self.arm_vehicle() - 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.set_rc(3, 2000) + self.set_rc(1, 1000) + + 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) + + # reduce throttle + self.set_rc(3, 1500) + self.set_rc(1, 1500) + + except Exception as e: + self.print_exception_caught(e) + ex = e self.disarm_vehicle() + self.context_pop() + + if ex: + raise ex ################################################# # AUTOTEST ALL @@ -582,113 +613,19 @@ def AC_Avoidance(self): def ServoRelayEvents(self): '''Test ServoRelayEvents''' - for method in self.run_cmd, self.run_cmd_int: - self.context_push() - method(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, p1=0, p2=0) - off = self.get_parameter("SIM_PIN_MASK") - method(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, p1=0, p2=1) - on = self.get_parameter("SIM_PIN_MASK") - if on == off: - raise NotAchievedException( - "Pin mask unchanged after relay cmd") - self.progress("Pin mask changed after relay command") - method(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, p1=0, p2=0) - - self.set_message_rate_hz("RELAY_STATUS", 10) - - # default configuration for relays in sim have one relay: - self.assert_received_message_field_values('RELAY_STATUS', { - "present": 3, - "on": 0, - }) - method(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, p1=0, p2=1) - self.assert_received_message_field_values('RELAY_STATUS', { - "present": 3, - "on": 1, - }) - method(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, p1=1, p2=1) - self.assert_received_message_field_values('RELAY_STATUS', { - "present": 3, - "on": 3, - }) - method(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, p1=0, p2=0) - method(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, p1=1, p2=0) - self.assert_received_message_field_values('RELAY_STATUS', { - "present": 3, - "on": 0, - }) - - # add another servo: - self.set_parameter("RELAY_PIN6", 14) - self.assert_received_message_field_values('RELAY_STATUS', { - "present": 35, - "on": 0, - }) - method(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, p1=5, p2=1) - self.assert_received_message_field_values('RELAY_STATUS', { - "present": 35, - "on": 32, - }) - method(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, p1=0, p2=1) - self.assert_received_message_field_values('RELAY_STATUS', { - "present": 35, - "on": 33, - }) - method(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, p1=5, p2=0) - self.assert_received_message_field_values('RELAY_STATUS', { - "present": 35, - "on": 1, - }) - - self.start_subtest("test MAV_CMD_DO_REPEAT_RELAY") - self.context_push() - self.set_parameter("SIM_SPEEDUP", 1) - method( - mavutil.mavlink.MAV_CMD_DO_REPEAT_RELAY, - p1=0, # servo 1 - p2=5, # 5 times - p3=0.5, # 1 second between being on - ) - for value in 0, 1, 0, 1, 0, 1, 0, 1: - self.wait_message_field_values('RELAY_STATUS', { - "on": value, - }) - self.context_pop() - self.delay_sim_time(3) - self.assert_received_message_field_values('RELAY_STATUS', { - "on": 1, # back to initial state - }) - self.context_pop() - - self.start_subtest("test MAV_CMD_DO_SET_SERVO") - for value in 1678, 2300, 0: - method(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=13, p2=value) - self.wait_servo_channel_value(13, value) - - self.start_subtest("test MAV_CMD_DO_REPEAT_SERVO") - - self.context_push() - self.set_parameter("SIM_SPEEDUP", 1) - trim = self.get_parameter("SERVO13_TRIM") - value = 2000 - method( - mavutil.mavlink.MAV_CMD_DO_REPEAT_SERVO, - p1=12, # servo12 - p2=value, # pwm - p3=5, # count - p4=0.5, # cycle time (1 second between high and high) - ) - for value in trim, value, trim, value, trim, value, trim, value: - self.wait_servo_channel_value(12, value) - self.context_pop() - - self.set_message_rate_hz("RELAY_STATUS", 0) + self.do_set_relay(0, 0) + off = self.get_parameter("SIM_PIN_MASK") + self.do_set_relay(0, 1) + on = self.get_parameter("SIM_PIN_MASK") + if on == off: + raise NotAchievedException( + "Pin mask unchanged after relay cmd") + self.progress("Pin mask changed after relay command") def MAVProxy_SetModeUsingSwitch(self): """Set modes via mavproxy switch""" - port = self.sitl_rcin_port(offset=1) self.customise_SITL_commandline([ - "--rc-in-port", str(port), + "--rc-in-port", "5502", ]) ex = None try: @@ -700,7 +637,7 @@ def MAVProxy_SetModeUsingSwitch(self): (4, 'AUTO'), (5, 'AUTO'), # non-existant mode, should stay in RTL (6, 'MANUAL')] - mavproxy = self.start_mavproxy(sitl_rcin_port=port) + mavproxy = self.start_mavproxy() for (num, expected) in fnoo: mavproxy.send('switch %u\n' % num) self.wait_mode(expected) @@ -1256,10 +1193,8 @@ def DO_SET_MODE(self): '''Set mode via MAV_COMMAND_DO_SET_MODE''' self.do_set_mode_via_command_long("HOLD") self.do_set_mode_via_command_long("MANUAL") - self.do_set_mode_via_command_int("HOLD") - self.do_set_mode_via_command_int("MANUAL") - def RoverInitialMode(self): + def InitialMode(self): '''test INITIAL_MODE parameter works''' # from mavproxy_rc.py self.wait_ready_to_arm() @@ -4838,12 +4773,13 @@ def MotorTest(self): self.wait_ready_to_arm() self.run_cmd( mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, - 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) + 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 ) self.wait_armed() self.progress("Waiting for magic throttle value") @@ -5285,19 +5221,15 @@ def test_scripting_internal_test(self): self.context_push() - 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"] + test_scripts = ["scripting_test.lua", "math.lua", "strings.lua"] + success_text = ["Internal tests passed", "Math tests passed", "String tests passed"] messages = [] - named_value_float = [] def my_message_hook(mav, 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) + if message.get_type() != 'STATUSTEXT': + return + messages.append(message) self.install_message_hook_context(my_message_hook) self.set_parameters({ @@ -5305,8 +5237,6 @@ 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() @@ -5324,21 +5254,9 @@ 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("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") + raise NotAchievedException("Scripting internal test failed") def test_scripting_hello_world(self): self.start_subtest("Scripting hello world") @@ -5993,14 +5911,18 @@ 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, - p1=17, - 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, + 17, + 0, + 0, + 0, + 0, + 0, + 0, + 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)''' @@ -6154,9 +6076,8 @@ def EStopAtBoot(self): }) self.set_rc(9, 2000) self.reboot_sitl() - self.assert_prearm_failure( - "Motors Emergency Stopped", - other_prearm_failures_fatal=False) + self.delay_sim_time(10) + self.assert_prearm_failure("Motors Emergency Stopped") self.context_pop() self.reboot_sitl() @@ -6259,8 +6180,7 @@ def AutoDock(self): def PrivateChannel(self): '''test the serial option bit specifying a mavlink channel as private''' global mav2 - port = self.adjust_ardupilot_port(5763) - mav2 = mavutil.mavlink_connection("tcp:localhost:%u" % port, + mav2 = mavutil.mavlink_connection("tcp:localhost:5763", robust_parsing=True, source_system=7, source_component=7) @@ -6361,161 +6281,6 @@ def printmessage(mav, m): # both the vehicle and this tests's special heartbeat raise NotAchievedException("Got heartbeat on private channel from non-vehicle") - def MAV_CMD_DO_SET_REVERSE(self): - '''test MAV_CMD_DO_SET_REVERSE command''' - self.change_mode('GUIDED') - self.wait_ready_to_arm() - self.arm_vehicle() - - here = self.mav.location() - target_loc = self.offset_location_ne(here, 2000, 0) - self.send_guided_mission_item(target_loc) - - self.wait_groundspeed(3, 100, minimum_duration=5) - - for method in self.run_cmd, self.run_cmd_int: - self.progress("Forwards!") - method(mavutil.mavlink.MAV_CMD_DO_SET_REVERSE, p1=0) - self.wait_heading(0) - - self.progress("Backwards!") - method(mavutil.mavlink.MAV_CMD_DO_SET_REVERSE, p1=1) - self.wait_heading(180) - - self.progress("Forwards!") - method(mavutil.mavlink.MAV_CMD_DO_SET_REVERSE, p1=0) - self.wait_heading(0) - - self.disarm_vehicle() - - def MAV_CMD_NAV_RETURN_TO_LAUNCH(self): - '''test MAV_CMD_NAV_RETURN_TO_LAUNCH mavlink command''' - self.change_mode('GUIDED') - self.wait_ready_to_arm() - self.arm_vehicle() - - here = self.mav.location() - target_loc = self.offset_location_ne(here, 2000, 0) - self.send_guided_mission_item(target_loc) - self.wait_distance_to_home(20, 100) - - self.run_cmd(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH) - self.wait_mode('RTL') - - self.change_mode('GUIDED') - - self.run_cmd_int(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH) - self.wait_mode('RTL') - - self.wait_distance_to_home(0, 5, timeout=30) - self.disarm_vehicle() - - def MAV_CMD_DO_CHANGE_SPEED(self): - '''test MAV_CMD_NAV_RETURN_TO_LAUNCH mavlink command''' - self.change_mode('GUIDED') - self.wait_ready_to_arm() - self.arm_vehicle() - - original_loc = self.mav.location() - here = original_loc - target_loc = self.offset_location_ne(here, 2000, 0) - self.send_guided_mission_item(target_loc) - self.wait_distance_to_home(20, 100) - - speeds = 3, 7, 12, 4 - - for speed in speeds: - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, p2=speed) - self.wait_groundspeed(speed-0.5, speed+0.5, minimum_duration=5) - - self.send_guided_mission_item(original_loc) - - for speed in speeds: - self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, p2=speed) - self.wait_groundspeed(speed-0.5, speed+0.5, minimum_duration=5) - - self.change_mode('RTL') - - self.wait_distance_to_home(0, 5, timeout=30) - self.disarm_vehicle() - - def MAV_CMD_MISSION_START(self): - '''simple test for starting missing using this command''' - # home and 1 waypoint a long way away: - self.upload_simple_relhome_mission([ - (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0), - (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2000, 0, 0), - ]) - self.change_mode('AUTO') - self.wait_ready_to_arm() - self.arm_vehicle() - for method in self.run_cmd, self.run_cmd_int: - self.change_mode('MANUAL') - self.wait_groundspeed(0, 1) - method(mavutil.mavlink.MAV_CMD_MISSION_START) - self.wait_mode('AUTO') - self.wait_groundspeed(3, 100) - self.disarm_vehicle() - - def MAV_CMD_NAV_SET_YAW_SPEED(self): - '''tests for MAV_CMD_NAV_SET_YAW_SPEED guided-mode command''' - self.change_mode('GUIDED') - self.wait_ready_to_arm() - self.arm_vehicle() - - for method in self.run_cmd, self.run_cmd_int: - self.change_mode('MANUAL') - self.wait_groundspeed(0, 1) - self.change_mode('GUIDED') - self.start_subtest("Absolute angles") - for (heading, speed) in (10, 5), (190, 10), (0, 2), (135, 6): - def cf(*args, **kwargs): - method( - mavutil.mavlink.MAV_CMD_NAV_SET_YAW_SPEED, - p1=heading, - p2=speed, - p3=0, # zero is absolute-angles - ) - self.wait_groundspeed(speed-0.5, speed+0.5, called_function=cf, minimum_duration=2) - self.wait_heading(heading-0.5, heading+0.5, called_function=cf, minimum_duration=2) - - self.start_subtest("relative angles") - original_angle = 90 - method( - mavutil.mavlink.MAV_CMD_NAV_SET_YAW_SPEED, - p1=original_angle, - p2=5, - p3=0, # zero is absolute-angles - ) - self.wait_groundspeed(4, 6) - self.wait_heading(original_angle-0.5, original_angle+0.5) - - expected_angle = original_angle - for (angle_delta, speed) in (5, 6), (-30, 2), (180, 7): - method( - mavutil.mavlink.MAV_CMD_NAV_SET_YAW_SPEED, - p1=angle_delta, - p2=speed, - p3=1, # one is relative-angles - ) - - def cf(*args, **kwargs): - method( - mavutil.mavlink.MAV_CMD_NAV_SET_YAW_SPEED, - p1=0, - p2=speed, - p3=1, # one is absolute-angles - ) - expected_angle += angle_delta - if expected_angle < 0: - expected_angle += 360 - if expected_angle > 360: - expected_angle -= 360 - self.wait_groundspeed(speed-0.5, speed+0.5, called_function=cf, minimum_duration=2) - self.wait_heading(expected_angle, called_function=cf, minimum_duration=2) - self.do_RTL() - self.disarm_vehicle() - def tests(self): '''return list of all tests''' ret = super(AutoTestRover, self).tests() @@ -6544,15 +6309,11 @@ def tests(self): self.Gripper, self.GripperMission, self.SET_MESSAGE_INTERVAL, - self.MESSAGE_INTERVAL_COMMAND_INT, self.REQUEST_MESSAGE, self.SYSID_ENFORCE, self.SET_ATTITUDE_TARGET, self.SET_POSITION_TARGET_LOCAL_NED, self.MAV_CMD_DO_SET_MISSION_CURRENT, - self.MAV_CMD_DO_CHANGE_SPEED, - self.MAV_CMD_MISSION_START, - self.MAV_CMD_NAV_SET_YAW_SPEED, self.Button, self.Rally, self.Offboard, @@ -6586,16 +6347,12 @@ 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.RoverInitialMode, + self.InitialMode, 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 7a83f1c565dacd..86bd86a2d16200 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" bash -c "cd $PWD; $*" + screen -X screen -t "$name" $* 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 deleted file mode 100755 index 99a0bc519868f5..00000000000000 --- a/Tools/autotest/run_mission.py +++ /dev/null @@ -1,107 +0,0 @@ -#!/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 3e0fb79eb4c566..3f80bbd1819ed5 100755 --- a/Tools/autotest/sim_vehicle.py +++ b/Tools/autotest/sim_vehicle.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python3 +#!/usr/bin/env python """ Framework to start a simulated vehicle and connect it to MAVProxy. @@ -393,8 +393,8 @@ def do_build(opts, frame_options): if opts.ekf_single: cmd_configure.append("--ekf-single") - if opts.force_32bit: - cmd_configure.append("--force-32bit") + if opts.sitl_32bit: + cmd_configure.append("--sitl-32bit") if opts.ubsan: cmd_configure.append("--ubsan") @@ -408,15 +408,11 @@ def do_build(opts, frame_options): for nv in opts.define: cmd_configure.append("--define=%s" % nv) - if opts.enable_dds: - cmd_configure.append("--enable-dds") - pieces = [shlex.split(x) for x in opts.waf_configure_args] for piece in pieces: cmd_configure.extend(piece) - if not cmd_opts.no_configure: - run_cmd_blocking("Configure waf", cmd_configure, check=True) + run_cmd_blocking("Configure waf", cmd_configure, check=True) if opts.clean: run_cmd_blocking("Building clean", [waf_light, "clean"]) @@ -647,10 +643,13 @@ def start_antenna_tracker(opts): oldpwd = os.getcwd() os.chdir(vehicledir) tracker_uarta = "tcp:127.0.0.1:" + str(5760 + 10 * tracker_instance) - binary_basedir = "build/sitl" - exe = os.path.join(root_dir, - binary_basedir, - "bin/antennatracker") + 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") run_in_terminal_window("AntennaTracker", ["nice", exe, @@ -660,50 +659,16 @@ def start_antenna_tracker(opts): os.chdir(oldpwd) -def start_CAN_Periph(opts, frame_info): - """Compile and run the sitl_periph""" +def start_CAN_GPS(opts): + """Compile and run the sitl_periph_gps""" global can_uarta progress("Preparing sitl_periph_gps") options = vinfo.options["sitl_periph_gps"]['frames']['gps'] - 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) + do_build(opts, options) exe = os.path.join(root_dir, 'build/sitl_periph_gps', 'bin/AP_Periph') - cmd = ["nice"] - cmd_name = "sitl_periph_gps" - if opts.valgrind: - cmd_name += " (valgrind)" - cmd.append("valgrind") - # adding this option allows valgrind to cope with the overload - # of operator new - cmd.append("--soname-synonyms=somalloc=nouserintercepts") - cmd.append("--track-origins=yes") - if opts.gdb or opts.gdb_stopped: - cmd_name += " (gdb)" - cmd.append("gdb") - gdb_commands_file = tempfile.NamedTemporaryFile(mode='w', delete=False) - atexit.register(os.unlink, gdb_commands_file.name) - gdb_commands_file.write("set pagination off\n") - if not opts.gdb_stopped: - gdb_commands_file.write("r\n") - gdb_commands_file.close() - cmd.extend(["-x", gdb_commands_file.name]) - cmd.append("--args") - cmd.append(exe) - if defaults_path is not None: - cmd.append("--defaults") - cmd.append(defaults_path) - run_in_terminal_window(cmd_name, cmd) + run_in_terminal_window("sitl_periph_gps", + ["nice", exe]) def start_vehicle(binary, opts, stuff, spawns=None): @@ -783,7 +748,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 or opts.auto_sysid: + if cmd_opts.count > 1: # we are in a subdirectory when using -n path = os.path.join("..", path) progress("Using defaults from (%s)" % (path,)) @@ -872,7 +837,7 @@ def start_mavproxy(opts, stuff): for i in instances: if not opts.no_extra_ports: - ports = [14550 + 10 * i] + ports = [p + 10 * i for p in [14550, 14551]] for port in ports: if under_vagrant(): # We're running inside of a vagrant guest; forward our @@ -989,20 +954,6 @@ def generate_frame_help(): return ret -# map from some vehicle aliases back to directory names. APMrover2 -# was the old name / directory name for Rover. -vehicle_map = { - "APMrover2": "Rover", - "Copter": "ArduCopter", - "Plane": "ArduPlane", - "Sub": "ArduSub", - "Blimp" : "Blimp", - "Rover": "Rover", -} -# add lower-case equivalents too -for k in list(vehicle_map.keys()): - vehicle_map[k.lower()] = vehicle_map[k] - # define and run parser parser = CompatOptionParser( "sim_vehicle.py", @@ -1014,10 +965,15 @@ def generate_frame_help(): "simulate ArduPlane") vehicle_choices = list(vinfo.options.keys()) - -# add vehicle aliases to argument parser options: -for c in vehicle_map.keys(): - vehicle_choices.append(c) +# add an alias for people with too much m +vehicle_choices.append("APMrover2") +vehicle_choices.append("Copter") # should change to ArduCopter at some stage +vehicle_choices.append("Plane") # should change to ArduPlane at some stage +vehicle_choices.append("Sub") # should change to Sub at some stage +vehicle_choices.append("copter") # should change to ArduCopter at some stage +vehicle_choices.append("plane") # should change to ArduPlane at some stage +vehicle_choices.append("sub") # should change to Sub at some stage +vehicle_choices.append("blimp") # should change to Blimp at some stage parser.add_option("-v", "--vehicle", type='choice', @@ -1043,10 +999,6 @@ 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, @@ -1064,15 +1016,20 @@ def generate_frame_help(): default=None, type='string', help="override SITL build target") +group_build.add_option("-s", "--build-system", + default="waf", + type='choice', + choices=["make", "waf"], + help="build system to use") group_build.add_option("--enable-math-check-indexes", default=False, action="store_true", dest="math_check_indexes", help="enable checking of math indexes") -group_build.add_option("", "--force-32bit", +group_build.add_option("", "--sitl-32bit", default=False, action='store_true', - dest="force_32bit", + dest="sitl_32bit", help="compile sitl using 32-bit") group_build.add_option("", "--configure-define", default=[], @@ -1145,10 +1102,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-peripherals", +group_sim.add_option("", "--can-gps", action='store_true', default=False, - help="start a DroneCAN peripheral instance") + help="start a DroneCAN GPS instance (use Tools/scripts/CAN/can_sitl_nodev.sh first)") group_sim.add_option("-A", "--sitl-instance-args", type='string', default=None, @@ -1324,9 +1281,6 @@ def generate_frame_help(): type=str, default="127.0.0.1", help="IP address of the simulator. Defaults to localhost") -group_sim.add_option("--enable-dds", action='store_true', - help="Enable the dds client to connect with ROS2/DDS") - parser.add_option_group(group_sim) @@ -1441,10 +1395,22 @@ def generate_frame_help(): break cwd = os.path.dirname(cwd) +# map from some vehicle aliases back to canonical names. APMrover2 +# was the old name / directory name for Rover. +vehicle_map = { + "APMrover2": "Rover", + "Copter": "ArduCopter", # will switch eventually + "Plane": "ArduPlane", # will switch eventually + "Sub": "ArduSub", # will switch eventually + "copter": "ArduCopter", # will switch eventually + "plane": "ArduPlane", # will switch eventually + "sub": "ArduSub", # will switch eventually + "blimp" : "Blimp", # will switch eventually +} if cmd_opts.vehicle in vehicle_map: + progress("%s is now known as %s" % + (cmd_opts.vehicle, vehicle_map[cmd_opts.vehicle])) cmd_opts.vehicle = vehicle_map[cmd_opts.vehicle] -elif cmd_opts.vehicle.lower() in vehicle_map: - cmd_opts.vehicle = vehicle_map[cmd_opts.vehicle.lower()] # try to validate vehicle if cmd_opts.vehicle not in vinfo.options: @@ -1486,8 +1452,8 @@ def generate_frame_help(): if cmd_opts.tracker: start_antenna_tracker(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.can_gps: + start_CAN_GPS(cmd_opts) if cmd_opts.custom_location: location = [(float)(x) for x in cmd_opts.custom_location.split(",")] @@ -1551,11 +1517,13 @@ def generate_frame_help(): if cmd_opts.vehicle_binary is not None: vehicle_binary = cmd_opts.vehicle_binary - else: + elif cmd_opts.build_system == "waf": 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,)) @@ -1617,7 +1585,7 @@ def generate_frame_help(): entities[i][k] = v config['entities'] = list(entities.values()) env = Environment(loader=FileSystemLoader(os.path.join(autotest_dir, 'template'))) - mission = env.get_template('scrimmage.xml.j2').render(**config) + mission = env.get_template('scrimmage.xml').render(**config) tmp = mkstemp() atexit.register(os.remove, tmp[1]) diff --git a/Tools/autotest/template/scrimmage.xml.j2 b/Tools/autotest/template/scrimmage.xml similarity index 100% rename from Tools/autotest/template/scrimmage.xml.j2 rename to Tools/autotest/template/scrimmage.xml diff --git a/Tools/autotest/test_build_options.py b/Tools/autotest/test_build_options.py index 839269415554c9..50e9626d0e664f 100755 --- a/Tools/autotest/test_build_options.py +++ b/Tools/autotest/test_build_options.py @@ -23,13 +23,9 @@ import fnmatch import optparse import os -import sys from pysim import util -sys.path.insert(1, os.path.join(os.path.dirname(__file__), '..', 'scripts')) -import extract_features # noqa - class TestBuildOptionsResult(object): '''object to return results from a comparison''' @@ -74,14 +70,7 @@ 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([])) @@ -171,29 +160,6 @@ def test_disable_feature(self, feature, options): self.test_compile_with_defines(defines) - # if the feature is truly disabled then extract_features.py - # should say so: - for target in self.build_targets: - path = self.target_to_elf_path(target) - extracter = extract_features.ExtractFeatures(path) - (compiled_in_feature_defines, not_compiled_in_feature_defines) = extracter.extract() - for define in defines: - # the following defines are known not to work on some - # or all vehicles: - feature_define_whitelist = set([ - 'AP_RANGEFINDER_ENABLED', # only at vehicle level ATM - 'AC_AVOID_ENABLED', # Rover doesn't obey this - 'AC_OAPATHPLANNER_ENABLED', # Rover doesn't obey this - 'BEACON_ENABLED', # Rover doesn't obey this (should also be AP_BEACON_ENABLED) - 'WINCH_ENABLED', # Copter doesn't use this; should use AP_WINCH_ENABLED - ]) - if define in compiled_in_feature_defines: - error = f"feature gated by {define} still compiled into ({target}); extract_features.py bug?" - if define in feature_define_whitelist: - print("warn: " + error) - else: - raise ValueError(error) - def test_enable_feature(self, feature, options): defines = self.get_enable_defines(feature, options) @@ -229,9 +195,9 @@ def test_compile_with_defines(self, defines): (t,)) raise - def target_to_path(self, target, extension=None): - '''given a build target (e.g. copter), return expected path to .bin - file for that target''' + def find_build_sizes(self): + '''returns a hash with size of all build targets''' + ret = {} target_to_binpath = { "copter": "arducopter", "plane": "arduplane", @@ -240,26 +206,8 @@ def target_to_path(self, target, extension=None): "sub": "ardusub", "blimp": "blimp", } - filename = target_to_binpath[target] - if extension is not None: - filename += "." + extension - return os.path.join("build", self.board(), "bin", filename) - - def target_to_bin_path(self, target): - '''given a build target (e.g. copter), return expected path to .bin - file for that target''' - return self.target_to_path(target, 'bin') - - def target_to_elf_path(self, target): - '''given a build target (e.g. copter), return expected path to .elf - file for that target''' - return self.target_to_path(target) - - def find_build_sizes(self): - '''returns a hash with size of all build targets''' - ret = {} for target in self.build_targets: - path = self.target_to_bin_path(target) + path = os.path.join("build", self.board(), "bin", "%s.bin" % target_to_binpath[target]) ret[target] = os.path.getsize(path) return ret @@ -301,20 +249,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: - f.write(f"{count}/{len(options)} {feature.define}\n") - # if feature.define < "WINCH_ENABLED": - # count += 1 - # continue if feature.define in self.must_have_defines_for_board(self._board): self.progress("Feature %s(%s) (%u/%u) is a MUST-HAVE" % (feature.label, feature.define, count, len(options))) - count += 1 continue self.progress("Disabling feature %s(%s) (%u/%u)" % (feature.label, feature.define, count, len(options))) @@ -324,15 +265,12 @@ 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 new file mode 100644 index 00000000000000..5d204aee15a8d7 --- /dev/null +++ b/Tools/autotest/web-firmware/Tools/FilterTool/FileSaver.js @@ -0,0 +1,171 @@ +/* +* 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 new file mode 100644 index 00000000000000..76ded1b765fe86 --- /dev/null +++ b/Tools/autotest/web-firmware/Tools/FilterTool/app.py @@ -0,0 +1,16 @@ +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 new file mode 100644 index 00000000000000..b68bc3b67371b3 --- /dev/null +++ b/Tools/autotest/web-firmware/Tools/FilterTool/filters.js @@ -0,0 +1,1196 @@ +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