diff --git a/.appveyor.yml b/.appveyor.yml index 22beb1402e63091d60aad79ff8d188c4d11c9ea0..6447bda72ec135f6b69ec850fdf9c62eeec07926 100644 --- a/.appveyor.yml +++ b/.appveyor.yml @@ -15,7 +15,7 @@ environment: install: - git submodule update --init --recursive - call "%ProgramFiles(x86)%\Microsoft Visual Studio 14.0\VC\vcvarsall.bat" x86 - - set PATH=C:\Qt\Tools\QtCreator\bin;C:\Qt\5.9.3\msvc2015\bin;%PATH% + - set PATH=C:\Qt\Tools\QtCreator\bin;C:\Qt\5.11.0\msvc2015\bin;%PATH% - mkdir %LOCALAPPDATA%\QtProject && copy test\qtlogging.ini %LOCALAPPDATA%\QtProject\ - ps: | Write-Host "Installing GStreamer..." -ForegroundColor Cyan @@ -35,7 +35,7 @@ install: Write-Host "Installed" -ForegroundColor Green build_script: - - mkdir %SHADOW_BUILD_DIR% && cd %SHADOW_BUILD_DIR% && C:\Qt\5.9.3\msvc2015\bin\qmake -r CONFIG-=debug_and_release CONFIG+=%CONFIG% CONFIG+=WarningsAsErrorsOn %APPVEYOR_BUILD_FOLDER%\qgroundcontrol.pro + - mkdir %SHADOW_BUILD_DIR% && cd %SHADOW_BUILD_DIR% && C:\Qt\5.11.0\msvc2015\bin\qmake -r CONFIG-=debug_and_release CONFIG+=%CONFIG% CONFIG+=WarningsAsErrorsOn %APPVEYOR_BUILD_FOLDER%\qgroundcontrol.pro - cd %SHADOW_BUILD_DIR% && jom - if "%CONFIG%" EQU "installer" ( copy %SHADOW_BUILD_DIR%\release\QGroundControl-installer.exe %APPVEYOR_BUILD_FOLDER%\QGroundControl-installer.exe ) # Generate the source server information to embed in the PDB diff --git a/.travis.yml b/.travis.yml index c634ee4a3d724a1b0cce8d926e19ff5003701574..d87adcd88e0f03a256ddce4a1a060cad59e04550 100644 --- a/.travis.yml +++ b/.travis.yml @@ -73,9 +73,9 @@ before_install: install: # linux dependencies: qt - if [ "${SPEC}" = "linux-g++-64" ]; then - wget --quiet https://s3-us-west-2.amazonaws.com/qgroundcontrol/dependencies/Qt5.9.3-gcc_64-min.tar.bz2 && - tar jxf Qt5.9.3-gcc_64-min.tar.bz2 -C /tmp && - export PATH=/tmp/Qt5.9-gcc_64/5.9.3/gcc_64/bin:$PATH && + wget --quiet https://s3-us-west-2.amazonaws.com/qgroundcontrol/dependencies/Qt5.11.0-gcc_64-min.tar.bz2 && + tar jxf Qt5.11.0-gcc_64-min.tar.bz2 -C /tmp && + export PATH=/tmp/Qt5.11-gcc_64/5.11.0/gcc_64/bin:$PATH && export DISPLAY=:99.0 && sh -e /etc/init.d/xvfb start ; @@ -83,8 +83,8 @@ install: # android dependencies: qt, gstreamer, android-ndk - if [ "${SPEC}" = "android-g++" ]; then - wget --quiet https://s3-us-west-2.amazonaws.com/qgroundcontrol/dependencies/Qt5.9.3-android_armv7-min.tar.bz2 && - tar jxf Qt5.9.3-android_armv7-min.tar.bz2 -C /tmp && + wget --quiet https://s3-us-west-2.amazonaws.com/qgroundcontrol/dependencies/Qt5.11.0-android_armv7-min.tar.bz2 && + tar jxf Qt5.11.0-android_armv7-min.tar.bz2 -C /tmp && wget --quiet https://s3-us-west-2.amazonaws.com/qgroundcontrol/dependencies/gstreamer-1.0-android-armv7-1.5.2.tar.bz2 && mkdir -p ${TRAVIS_BUILD_DIR}/gstreamer-1.0-android-armv7-1.5.2 && tar jxf gstreamer-1.0-android-armv7-1.5.2.tar.bz2 -C ${TRAVIS_BUILD_DIR}/gstreamer-1.0-android-armv7-1.5.2 && @@ -93,14 +93,14 @@ install: ./android-ndk-r10e-linux-x86_64.bin > /dev/null && export ANDROID_NDK_ROOT=`pwd`/android-ndk-r10e && export ANDROID_SDK_ROOT=/usr/local/android-sdk && - export PATH=/tmp/Qt5.9-android_armv7/5.9.3/android_armv7/bin:`pwd`/android-ndk-r10e:$PATH && echo $PATH + export PATH=/tmp/Qt5.11-android_armv7/5.11.0/android_armv7/bin:`pwd`/android-ndk-r10e:$PATH && echo $PATH ; fi # osx dependencies: qt (master builds only: gstreamer, gstreamer-devel) - if [ "${SPEC}" = "macx-clang" ]; then - wget --quiet https://s3-us-west-2.amazonaws.com/qgroundcontrol/dependencies/Qt5.9.3-clang_64-min.tar.bz2 && - tar jxf Qt5.9.3-clang_64-min.tar.bz2 -C /tmp + wget --quiet https://s3-us-west-2.amazonaws.com/qgroundcontrol/dependencies/Qt5.11.0-clang_64-min.tar.bz2 && + tar jxf Qt5.11.0-clang_64-min.tar.bz2 -C /tmp ; fi @@ -123,7 +123,7 @@ install: fi - if [ "${SPEC}" = "macx-clang" ]; then - export QT_DIR=Qt5.9-clang_64/5.9.3/clang_64 && + export QT_DIR=Qt5.11-clang_64/5.11.0/clang_64 && export QT_QPA_PLATFORM_PLUGIN_PATH=/tmp/$QT_DIR/plugins && export QML2_IMPORT_PATH=/tmp/$QT_DIR/qml && export PATH=/tmp/$QT_DIR/bin:$PATH @@ -161,7 +161,12 @@ before_script: script: # run qmake - mkdir ${SHADOW_BUILD_DIR} && cd ${SHADOW_BUILD_DIR} - - qmake -r ${TRAVIS_BUILD_DIR}/qgroundcontrol.pro CONFIG+=${CONFIG} CONFIG+=WarningsAsErrorsOn -spec ${SPEC} + # Due to possible bug in Qt 5.11 WarningsAsErrorsOn is off for Linux builds. Hopefully back on once that is resolved. + - if [ "${SPEC}" = "macx-clang" ]; then + qmake -r ${TRAVIS_BUILD_DIR}/qgroundcontrol.pro CONFIG+=${CONFIG} CONFIG+=WarningsAsErrorsOn -spec ${SPEC}; + else + qmake -r ${TRAVIS_BUILD_DIR}/qgroundcontrol.pro CONFIG+=${CONFIG} -spec ${SPEC}; + fi # compile - if [ "${SPEC}" != "macx-ios-clang" ]; then diff --git a/.vagrantconfig.yml b/.vagrantconfig.yml index bd54ba4c39b89ccec0ef21960814576a943b08cf..685571d83f374cc7de88c425d97a8f12bf47eedb 100644 --- a/.vagrantconfig.yml +++ b/.vagrantconfig.yml @@ -1,13 +1,13 @@ configs: dev: - 'qt_deps_tarball': 'Qt5.9.1-linux-vagrant.tar.bz2' + 'qt_deps_tarball': 'Qt5.11.0-gcc_64-min.tar.bz2' 'qt_deps_unpack_parent_dir': '/tmp' - 'qt_deps_unpack_dir': '/tmp/Qt5.9-vagrant/5.9.1' - 'qt_deps_bin_unpack_dir': '/tmp/Qt5.9-vagrant/5.9.1/gcc_64/bin' - 'qt_deps_lib_unpack_dir': '/tmp/Qt5.9-vagrant/5.9.1/gcc_64/lib' - 'qt_deps_plugins_unpack_dir': '/tmp/Qt5.9-vagrant/5.9.1/gcc_64/plugins' - 'qt_deps_qml_unpack_dir': '/tmp/Qt5.9-vagrant/5.9.1/gcc_64/qml' + 'qt_deps_unpack_dir': '/tmp/Qt5.11-gcc_64/5.11.0' + 'qt_deps_bin_unpack_dir': '/tmp/Qt5.11-gcc_64/5.11.0/gcc_64/bin' + 'qt_deps_lib_unpack_dir': '/tmp/Qt5.11-gcc_64/5.11.0/gcc_64/lib' + 'qt_deps_plugins_unpack_dir': '/tmp/Qt5.11-gcc_64/5.11.0/gcc_64/plugins' + 'qt_deps_qml_unpack_dir': '/tmp/Qt5.11-gcc_64/5.11.0/gcc_64/qml' 'project_root_dir': '/vagrant' @@ -20,4 +20,4 @@ configs: 'spec': 'linux-g++-64' 'shadow_build_dir': '/vagrant/shadow-build' 'pro': '/vagrant/qgroundcontrol.pro' - 'deps_url': 'https://s3-us-west-2.amazonaws.com/qgroundcontrol/dependencies/Qt5.9.1-linux-vagrant.tar.bz2' + 'deps_url': 'https://s3-us-west-2.amazonaws.com/qgroundcontrol/dependencies/Qt5.11.0-gcc_64-min.tar.bz2' diff --git a/ISSUE_TEMPLATE.md b/ISSUE_TEMPLATE.md index 58ac2ca2be4596cdc38610eae40c2e8d9a88f342..0e8b07a49519e80671cbdbd96fcfe740e3680bfd 100644 --- a/ISSUE_TEMPLATE.md +++ b/ISSUE_TEMPLATE.md @@ -4,6 +4,7 @@ This system is for posting bugs or feature requests ONLY. For questions about ho When posting bug reports, include the following informaiton: - Ground station operating system. - QGroundControl version and build (daily, stable, self-built from source, etc.) +- **If you are using a Stable build which is not the latest, first step is to install latest Stable build and try again prior to entering a bug report.** - Autopilot board: Pixhawk I, Pixhawk Mini, Pixhawk 2, etc. - Autopilot firmware (e.g. PX4, ArduPilot, custom) and version. - Exact steps to reproduce the problem. Starting from booting QGroundControl to when the problem occurs. diff --git a/Jenkinsfile b/Jenkinsfile index 649233dbb9f40894aed1fc4c1bb6e1dfc7d927e0..cd26b58b8cc4f4613745b3d656c2249efbb26377 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -1,8 +1,39 @@ pipeline { - agent any + agent none stages { + stage('build') { parallel { + + stage('Android Release') { + environment { + CCACHE_BASEDIR = "${env.WORKSPACE}" + QGC_CONFIG = 'release' + QMAKE_VER = "5.9.2/android_armv7/bin/qmake" + } + agent { + docker { + image 'mavlink/qgc-build-android:2018-04-14' + args '-v ${CCACHE_DIR}:${CCACHE_DIR}:rw' + } + } + steps { + sh 'export' + sh 'ccache -z' + sh 'git submodule deinit -f .' + sh 'git clean -ff -x -d .' + sh 'git submodule update --init --recursive --force' + sh 'mkdir build; cd build; ${QT_PATH}/${QMAKE_VER} -r ${WORKSPACE}/qgroundcontrol.pro CONFIG+=${QGC_CONFIG} CONFIG+=WarningsAsErrorsOn' + sh 'cd build; make -j`nproc --all`' + sh 'ccache -s' + } + post { + cleanup { + sh 'git clean -ff -x -d .' + } + } + } + stage('Linux Debug') { environment { CCACHE_BASEDIR = "${env.WORKSPACE}" @@ -11,7 +42,7 @@ pipeline { } agent { docker { - image 'mavlink/qgc-build-linux' + image 'mavlink/qgc-build-linux:2018-04-14' args '-v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -25,7 +56,13 @@ pipeline { sh 'cd build; make -j`nproc --all`' sh 'ccache -s' } + post { + cleanup { + sh 'git clean -ff -x -d .' + } + } } + stage('Linux Release') { environment { CCACHE_BASEDIR = "${env.WORKSPACE}" @@ -34,7 +71,7 @@ pipeline { } agent { docker { - image 'mavlink/qgc-build-linux' + image 'mavlink/qgc-build-linux:2018-04-14' args '-v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -48,7 +85,13 @@ pipeline { sh 'cd build; make -j`nproc --all`' sh 'ccache -s' } + post { + cleanup { + sh 'git clean -ff -x -d .' + } + } } + stage('OSX Debug') { agent { node { @@ -70,7 +113,13 @@ pipeline { sh 'cd build; make -j`sysctl -n hw.ncpu`' sh 'ccache -s' } + post { + cleanup { + sh 'git clean -ff -x -d .' + } + } } + stage('OSX Release') { agent { node { @@ -79,7 +128,7 @@ pipeline { } environment { CCACHE_BASEDIR = "${env.WORKSPACE}" - QGC_CONFIG = 'release' + QGC_CONFIG = 'installer' QMAKE_VER = "5.9.3/clang_64/bin/qmake" } steps { @@ -92,13 +141,27 @@ pipeline { sh 'cd build; make -j`sysctl -n hw.ncpu`' sh 'ccache -s' } + post { + success { + archiveArtifacts(artifacts: 'build/**/*.dmg', fingerprint: true) + } + cleanup { + sh 'git clean -ff -x -d .' + } + } } - } - } - } + } // parallel + } // stage('build') + } // stages + environment { CCACHE_CPP2 = '1' CCACHE_DIR = '/tmp/ccache' QT_FATAL_WARNINGS = '1' } + + options { + buildDiscarder(logRotator(numToKeepStr: '10', artifactDaysToKeepStr: '30')) + timeout(time: 60, unit: 'MINUTES') + } } diff --git a/QGCInstaller.pri b/QGCInstaller.pri index f3ba9bc22fc2e44bdafc4602f9448b595ce52b11..443ca699e8203120edc641b2a367886f73e2772e 100644 --- a/QGCInstaller.pri +++ b/QGCInstaller.pri @@ -26,12 +26,12 @@ installer { # We cd to release directory so we can run macdeployqt without a path to the # qgroundcontrol.app file. If you specify a path to the .app file the symbolic # links to plugins will not be created correctly. - QMAKE_POST_LINK += && mkdir -p $${DESTDIR}/package QMAKE_POST_LINK += && cd $${DESTDIR} && $$dirname(QMAKE_QMAKE)/macdeployqt $${TARGET}.app -appstore-compliant -verbose=2 -qmldir=$${BASEDIR}/src # macdeploy is missing some relocations once in a while. "Fix" it: QMAKE_POST_LINK += && python $$BASEDIR/tools/osxrelocator.py $${TARGET}.app/Contents @rpath @executable_path/../Frameworks -r > /dev/null 2>&1 # Create package QMAKE_POST_LINK += && hdiutil create /tmp/tmp.dmg -ov -volname "$${TARGET}-$${MAC_VERSION}" -fs HFS+ -srcfolder "$${DESTDIR}/" + QMAKE_POST_LINK += && mkdir -p $${DESTDIR}/package QMAKE_POST_LINK += && hdiutil convert /tmp/tmp.dmg -format UDBZ -o $${DESTDIR}/package/$${TARGET}.dmg QMAKE_POST_LINK += && rm /tmp/tmp.dmg } diff --git a/QGCSetup.pri b/QGCSetup.pri index 32a4803b903793cdd68ecb9f54f766180f0f37c7..49e55081daa0f1041e13815cdaa1f91b3f4c0113 100644 --- a/QGCSetup.pri +++ b/QGCSetup.pri @@ -63,7 +63,9 @@ WindowsBuild { ReleaseBuild: DLL_QT_DEBUGCHAR = "" COPY_FILE_LIST = \ $$BASEDIR\\libs\\lib\\sdl2\\msvc\\lib\\x86\\SDL2.dll \ - $$BASEDIR\\deploy\\libeay32.dll + $$BASEDIR\\deploy\\libeay32.dll \ + $$BASEDIR_WIN\\deploy\\libssl32.dll \ + $$BASEDIR_WIN\\deploy\\ssleay32.dll for(COPY_FILE, COPY_FILE_LIST) { QMAKE_POST_LINK += $$escape_expand(\\n) $$QMAKE_COPY \"$$COPY_FILE\" \"$$DESTDIR_WIN\" @@ -102,15 +104,17 @@ LinuxBuild { # QT_INSTALL_LIBS QT_LIB_LIST = \ + libQt5Charts.so.5 \ libQt5Core.so.5 \ libQt5DBus.so.5 \ libQt5Gui.so.5 \ libQt5Location.so.5 \ libQt5Multimedia.so.5 \ - libQt5MultimediaQuick_p.so.5 \ + libQt5MultimediaQuick.so.5 \ libQt5Network.so.5 \ libQt5OpenGL.so.5 \ libQt5Positioning.so.5 \ + libQt5PositioningQuick.so.5 \ libQt5PrintSupport.so.5 \ libQt5Qml.so.5 \ libQt5Quick.so.5 \ diff --git a/README.md b/README.md index b74050dd6632a86a24abee5c2109c46c9cbc94cc..904ef9e4b4d3450c694c65b14f491e5440566b1a 100644 --- a/README.md +++ b/README.md @@ -6,83 +6,20 @@ [![Gitter](https://badges.gitter.im/Join%20Chat.svg)](https://gitter.im/mavlink/qgroundcontrol?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge&utm_content=badge) -Website: -## Obtaining source code +*QGroundControl* (QGC) is an intuitive and powerful ground control station (GCS) for UAVs. -Source code for QGroundControl is kept on GitHub: https://github.com/mavlink/qgroundcontrol. -``` -git clone --recursive https://github.com/mavlink/qgroundcontrol.git -``` -Each time you pull new source to your repository you should run `git submodule update` to get the latest submodules as well. Since QGroundControl uses submodules, using the zip file for source download will not work. You must use git. +The primary goal of QGC is ease of use for both first time and professional users. +It provides full flight control and mission planning for any MAVLink enabled drone, and vehicle setup for both PX4 and ArduPilot powered UAVs. Instructions for *using QGroundControl* are provided in the [User Manual](https://docs.qgroundcontrol.com/en/) (you may not need them because the UI is very intuitive!) -The source code is [dual-licensed under Apache 2.0 and GPLv3](https://github.com/mavlink/qgroundcontrol/blob/master/COPYING.md). +All the code is open-source, so you can contribute and evolve it as you want. +The [Developer Guide](https://dev.qgroundcontrol.com/en/) explains how to [build](https://dev.qgroundcontrol.com/en/getting_started/) and extend QGC. -### User Manual -https://docs.qgroundcontrol.com/en/ -### Supported Builds - -#### Native Builds -QGroundControl builds are supported for OSX, Linux, Windows, iOS and Android. QGroundControl uses [Qt](http://www.qt.io) as its cross-platform support library and uses [QtCreator](http://doc.qt.io/qtcreator/index.html) as its default build environment. - -* OSX: OSX 10.7 or higher, 64 bit, clang compiler (IMPORTANT: XCode 8 requires a workaround described below) -* Ubuntu: 64 bit, gcc compiler -* Windows: Vista or higher, 32 bit, [Visual Studio 2015 compiler](http://www.visualstudio.com/downloads/download-visual-studio-vs#d-express-windows-desktop) -* iOS: 8.0 and higher -* Android: Jelly Bean (4.1) and higher. Standard QGC is built against ndk version 19. -* Qt version: **5.9.3 only** - -###### Install QT -You **need to install Qt as described below** instead of using pre-built packages from say, a Linux distribution, because QGroundControl needs access to private Qt headers. -* Download the [Qt installer](http://www.qt.io/download-open-source) - * Make sure to install Qt version **5.9.3**. You will also need to install the Qt Speech package. - * Ubuntu: Set the downloaded file to executable using:`chmod +x`. Install to default location for use with ./qgroundcontrol-start.sh. If you install Qt to a non-default location you will need to modify qgroundcontrol-start.sh in order to run downloaded builds. - * Windows: Make sure to install VS 2015 32 bit package. - -###### Install additional packages: -* Ubuntu: sudo apt-get install speech-dispatcher libudev-dev libsdl2-dev libgstreamer1.0-0 gstreamer1.0-plugins-base libgstreamer-plugins-base1.0-dev gstreamer1.0* -* Fedora: sudo dnf install speech-dispatcher SDL2-devel SDL2 systemd-devel -* Arch Linux: pacman -Sy speech-dispatcher -* Windows: [USB Driver](http://www.pixhawk.org/firmware/downloads) to connect to Pixhawk/PX4Flow/3DR Radio -* Android: [Qt Android Setup](http://doc.qt.io/qt-5/androidgs.html) - -###### Building using Qt Creator - -* Launch Qt Creator and open the `qgroundcontrol.pro` project. -* Select the appropriate kit for your needs: - * OSX: Desktop Qt 5.9.3 clang 64 bit - * Ubuntu: Desktop Qt 5.9.3 GCC bit - * Windows: Desktop Qt 5.9.3 MSVC2015 32bit - * Android: Android for armeabi-v7a (GCC 4.9, Qt 5.9.3) -* Note: iOS builds must be built using xCode: http://doc.qt.io/qt-5/ios-support.html. Use Qt Creator to generate the XCode project (*Run Qmake* from the context menu). - -#### Vagrant - -A Vagrantfile is provided to build QGroundControl using the [Vagrant](https://www.vagrantup.com/) system. This will produce a native Linux build which can be run in the Vagrant Virtual Machine or on the host machine if it is compatible. - -* [Download](https://www.vagrantup.com/downloads.html) Vagrant -* [Install](https://www.vagrantup.com/docs/getting-started/) Vagrant -* From the root directory of the QGroundControl repository run "vagrant up" -* To use the graphical environment run "vagrant reload" - -#### Additional build notes for all supported OS - -* Warnings as Errors: Specifying `CONFIG+=WarningsAsErrorsOn` will turn all warnings into errors which breaks the build. If you are working on a pull request you plan to submit to github for consideration, you should always run with this setting turned on, since it is required for all pull requests. NOTE: Putting this line into a file called "user_config.pri" in the top-level directory (same directory as `qgroundcontrol.pro`) will set this flag on all builds without interfering with the GIT history. -* Parallel builds: For non Windows builds, you can use the '-j#' option to run parellel builds. -* Location of built files: Individual build file results can be found in the `build_debug` or `build_release` directories. The built executable can be found in the `debug` or `release` directory. -* If you get this error when running qgroundcontrol: /usr/lib/x86_64-linux-gnu/libstdc++.so.6: version 'GLIBCXX_3.4.20' not found. You need to either update to the latest gcc, or install the latest libstdc++.6 using: sudo apt-get install libstdc++6. - -## Additional functionality -QGroundControl has functionality that is dependent on the operating system and libraries installed by the user. The following sections describe these features, their dependencies, and how to disable/alter them during the build process. These features can be forcibly enabled/disabled by specifying additional values to qmake. - -### Opal-RT's RT-LAB simulator -Integration with Opal-RT's RT-LAB simulator can be enabled on Windows by installing RT-LAB 7.2.4. This allows vehicles to be simulated in RT-LAB and communicate directly with QGC on the same computer as if the UAS was actually deployed. This support is enabled by default once the requisite RT-LAB software is installed. Disabling this can be done by adding `DEFINES+=DISABLE_RTLAB` to qmake. - -### XBee support -QGroundControl can talk to XBee wireless devices using their proprietary protocol directly on Windows and Linux platforms. This support is not necessary if you're not using XBee devices or aren't using their proprietary protocol. On Windows, the necessary dependencies are included in this repository and no additional steps are required. For Linux, change to the `libs/thirdParty/libxbee` folder and run `make;sudo make install` to install libxbee on your system (uninstalling can be done with a `sudo make uninstall`). `qmake` will automatically detect the library on Linux, so no other work is necessary. - -To disable XBee support you may add `DEFINES+=DISABLE_XBEE` to qmake. - -### Video Streaming -Check the [Video Streaming](https://github.com/mavlink/qgroundcontrol/tree/master/src/VideoStreaming) directory for further instructions. +Key Links: +* [Website](http://qgroundcontrol.com) (qgroundcontrol.com) +* [User Manual](https://docs.qgroundcontrol.com/en/) +* [Developer Guide](https://dev.qgroundcontrol.com/en/) +* [Discussion/Support](https://docs.qgroundcontrol.com/en/Support/Support.html) +* [Contributing](https://dev.qgroundcontrol.com/en/contribute/) +* [License](https://github.com/mavlink/qgroundcontrol/blob/master/COPYING.md) diff --git a/Vagrantfile b/Vagrantfile index a3df179e62ca9ac7c92c5ce68233df413e7794b9..c2c9c326ec006a5a7e819c92ed2a9dcfef53f840 100644 --- a/Vagrantfile +++ b/Vagrantfile @@ -45,6 +45,7 @@ Vagrant.configure(2) do |config| $config_shell = <<-'SHELL' set -e + set -x export %{build_env} export JOBS=$((`cat /proc/cpuinfo | grep -c ^processor`+1)) diff --git a/android/AndroidManifest.xml b/android/AndroidManifest.xml index d78675443011f3abe13e9cd6e17302842f04620b..9f3f7b589f9ecd6d1e74790634eedb4dbc98f77d 100644 --- a/android/AndroidManifest.xml +++ b/android/AndroidManifest.xml @@ -59,7 +59,15 @@ - + + + + + + + + + diff --git a/android/build.gradle b/android/build.gradle index ef416b0b88c1420d7bed0975240c678198aa73c2..dbe261805e2779d80f1481aa19148ba67a7e4817 100644 --- a/android/build.gradle +++ b/android/build.gradle @@ -1,6 +1,9 @@ buildscript { + repositories { - jcenter() + maven { + url "http://repo1.maven.org/maven2" + } } dependencies { @@ -51,6 +54,10 @@ android { } } + aaptOptions { + cruncherEnabled = false + } + lintOptions { abortOnError false } diff --git a/deploy/MakeQtTravisTarball.sh b/deploy/MakeQtTravisTarball.sh index 7d9d9e27cd6cbeff6b4803e7e627573d8af6290a..8ce1aae9a8a526dde25f6506834cac56a706632e 100755 --- a/deploy/MakeQtTravisTarball.sh +++ b/deploy/MakeQtTravisTarball.sh @@ -1,28 +1,23 @@ #!/bin/bash -x -if [[ $# -eq 0 ]]; then - echo 'MakeQtTravisTarball.sh QtDirectory QtFullVersion QtBaseVersion BuildType' - exit 1 +if [ $# -ne 2 ]; then + echo 'MakeQtTravisTarball.sh QtDirectory BuildType' + exit 1 fi QT_DIRECTORY=$1 if [ ! -d ${QT_DIRECTORY} ]; then - echo 'Specify directory for Qt Directory.' - exit 1 + echo 'Specify directory for Qt Directory to copy from.' + exit 1 fi -QT_FULL_VERSION=$2 -if [ ! -d ${QT_DIRECTORY}/${QT_FULL_VERSION} ]; then - echo 'Qt version directory not found' - exit 1 -fi - -QT_BASE_VERSION=$3 +QT_FULL_VERSION=5.11.0 +QT_BASE_VERSION=5.11 -QT_BUILD_TYPE=$4 +QT_BUILD_TYPE=$2 if [ ! -d ${QT_DIRECTORY}/${QT_FULL_VERSION}/${QT_BUILD_TYPE} ]; then - echo 'Qt build type directory not found' - exit 1 + echo 'Qt build type directory not found. Specify example: clang_64' + exit 1 fi mkdir -p Qt${QT_BASE_VERSION}-${QT_BUILD_TYPE}/${QT_FULL_VERSION}/${QT_BUILD_TYPE} diff --git a/deploy/libeay32.dll b/deploy/libeay32.dll index fadf8b2d519836e7e03bfdbdadbba9d391829b62..ffd92b884dc051c9bf4cd398e1c8db6abe845ce0 100644 Binary files a/deploy/libeay32.dll and b/deploy/libeay32.dll differ diff --git a/deploy/libssl32.dll b/deploy/libssl32.dll new file mode 100644 index 0000000000000000000000000000000000000000..0592244daa24a2c67e8ad4fb8077c2c0b56b1eb5 Binary files /dev/null and b/deploy/libssl32.dll differ diff --git a/deploy/qgroundcontrol_installer.nsi b/deploy/qgroundcontrol_installer.nsi index 67a17f29dd89737bdbb8d123fdf30ffda7e8b9ff..e3064b681af7269891ed8f49f819aa58d568ac84 100644 --- a/deploy/qgroundcontrol_installer.nsi +++ b/deploy/qgroundcontrol_installer.nsi @@ -73,8 +73,12 @@ doinstall: File /r /x ${EXENAME}.pdb /x ${EXENAME}.lib /x ${EXENAME}.exp ${DESTDIR}\*.* File deploy\px4driver.msi WriteUninstaller $INSTDIR\${EXENAME}-Uninstall.exe - WriteRegStr HKLM "Software\Microsoft\Windows\CurrentVersion\Uninstall\${APPNAME}" "DisplayName" "${APPNAME}" - WriteRegStr HKLM "Software\Microsoft\Windows\CurrentVersion\Uninstall\${APPNAME}" "UninstallString" "$\"$INSTDIR\${EXENAME}-Uninstall.exe$\"" + WriteRegStr HKLM "SOFTWARE\Microsoft\Windows\CurrentVersion\Uninstall\${APPNAME}" "DisplayName" "${APPNAME}" + WriteRegStr HKLM "SOFTWARE\Microsoft\Windows\CurrentVersion\Uninstall\${APPNAME}" "UninstallString" "$\"$INSTDIR\${EXENAME}-Uninstall.exe$\"" + SetRegView 64 + WriteRegDWORD HKLM "SOFTWARE\Microsoft\Windows\Windows Error Reporting\LocalDumps\${EXENAME}.exe" "DumpCount" 5 + WriteRegDWORD HKLM "SOFTWARE\Microsoft\Windows\Windows Error Reporting\LocalDumps\${EXENAME}.exe" "DumpType" 2 + WriteRegExpandStr HKLM "SOFTWARE\Microsoft\Windows\Windows Error Reporting\LocalDumps\${EXENAME}.exe" "DumpFolder" "%LOCALAPPDATA%\QGCCrashDumps" ; Only attempt to install the PX4 driver if the version isn't present !define ROOTKEY "SOFTWARE\Microsoft\Windows\CurrentVersion\Uninstall\434608CF2B6E31F0DDBA5C511053F957B55F098E" @@ -106,7 +110,8 @@ Section "Uninstall" RMDir /r /REBOOTOK "$SMPROGRAMS\$StartMenuFolder\" SetShellVarContext current RMDir /r /REBOOTOK "$APPDATA\${ORGNAME}\" - DeleteRegKey HKLM "Software\Microsoft\Windows\CurrentVersion\Uninstall\${APPNAME}" + DeleteRegKey HKLM "SOFTWARE\Microsoft\Windows\CurrentVersion\Uninstall\${APPNAME}" + DeleteRegKey HKLM "SOFTWARE\Microsoft\Windows\Windows Error Reporting\LocalDumps\${EXENAME}.exe" SectionEnd Section "create Start Menu Shortcuts" diff --git a/deploy/ssleay32.dll b/deploy/ssleay32.dll new file mode 100644 index 0000000000000000000000000000000000000000..0592244daa24a2c67e8ad4fb8077c2c0b56b1eb5 Binary files /dev/null and b/deploy/ssleay32.dll differ diff --git a/libs/mavlink/include/mavlink/v2.0 b/libs/mavlink/include/mavlink/v2.0 index 3ddcbdfcd360c846246d8905d877add3488c8363..653ac745a57794a38c831f1ff296066a2e09c09b 160000 --- a/libs/mavlink/include/mavlink/v2.0 +++ b/libs/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit 3ddcbdfcd360c846246d8905d877add3488c8363 +Subproject commit 653ac745a57794a38c831f1ff296066a2e09c09b diff --git a/qgcresources.qrc b/qgcresources.qrc index 5b6fa54b53d55297f9774605cfbb30482e0a7eb5..ff5c03e4dbebbbdfabfd3a0879844fd8c20c3774 100644 --- a/qgcresources.qrc +++ b/qgcresources.qrc @@ -171,6 +171,7 @@ src/FirmwarePlugin/APM/APMBrandImageSub.png src/FirmwarePlugin/PX4/PX4BrandImage.png src/FlightMap/Images/sub.png + resources/check.svg resources/action.svg diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index cb0226f85f21580fe3315b39daa45f178c4f3204..f8cb1d6881192b0d75a19e319d98bb27072c113e 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -240,6 +240,7 @@ AndroidBuild || iOSBuild { QT += \ printsupport \ serialport \ + charts \ } contains(DEFINES, QGC_ENABLE_BLUETOOTH) { @@ -346,6 +347,7 @@ INCLUDEPATH += \ src/QtLocationPlugin \ src/QtLocationPlugin/QMLControl \ src/Settings \ + src/Terrain \ src/VehicleSetup \ src/ViewWidgets \ src/Audio \ @@ -391,12 +393,14 @@ HEADERS += \ src/api/QGCOptions.h \ src/api/QGCSettings.h \ src/api/QmlComponentInfo.h \ + src/comm/HeartbeatTimer.h SOURCES += \ src/api/QGCCorePlugin.cc \ src/api/QGCOptions.cc \ src/api/QGCSettings.cc \ src/api/QmlComponentInfo.cc \ + src/comm/HeartbeatTimer.cc # # Unit Test specific configuration goes here (requires full debug build with all plugins) @@ -431,7 +435,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin { src/MissionManager/SimpleMissionItemTest.h \ src/MissionManager/SpeedSectionTest.h \ src/MissionManager/StructureScanComplexItemTest.h \ - src/MissionManager/SurveyMissionItemTest.h \ + src/MissionManager/SurveyComplexItemTest.h \ src/MissionManager/TransectStyleComplexItemTest.h \ src/MissionManager/VisualMissionItemTest.h \ src/qgcunittest/FileDialogTest.h \ @@ -472,7 +476,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin { src/MissionManager/SimpleMissionItemTest.cc \ src/MissionManager/SpeedSectionTest.cc \ src/MissionManager/StructureScanComplexItemTest.cc \ - src/MissionManager/SurveyMissionItemTest.cc \ + src/MissionManager/SurveyComplexItemTest.cc \ src/MissionManager/TransectStyleComplexItemTest.cc \ src/MissionManager/VisualMissionItemTest.cc \ src/qgcunittest/FileDialogTest.cc \ @@ -511,6 +515,7 @@ HEADERS += \ src/Joystick/Joystick.h \ src/Joystick/JoystickManager.h \ src/JsonHelper.h \ + src/KMLFileHelper.h \ src/LogCompressor.h \ src/MG.h \ src/MissionManager/CameraCalc.h \ @@ -544,7 +549,7 @@ HEADERS += \ src/MissionManager/Section.h \ src/MissionManager/SpeedSection.h \ src/MissionManager/StructureScanComplexItem.h \ - src/MissionManager/SurveyMissionItem.h \ + src/MissionManager/SurveyComplexItem.h \ src/MissionManager/TransectStyleComplexItem.h \ src/MissionManager/VisualMissionItem.h \ src/PositionManager/PositionManager.h \ @@ -567,7 +572,6 @@ HEADERS += \ src/QmlControls/AppMessages.h \ src/QmlControls/CoordinateVector.h \ src/QmlControls/EditPositionDialogController.h \ - src/QmlControls/MavlinkQmlSingleton.h \ src/QmlControls/ParameterEditorController.h \ src/QmlControls/QGCFileDialogController.h \ src/QmlControls/QGCImageProvider.h \ @@ -586,7 +590,8 @@ HEADERS += \ src/Settings/SettingsManager.h \ src/Settings/UnitsSettings.h \ src/Settings/VideoSettings.h \ - src/Terrain.h \ + src/Terrain/TerrainQuery.h \ + src/TerrainTile.h \ src/Vehicle/MAVLinkLogManager.h \ src/VehicleSetup/JoystickConfigController.h \ src/comm/LinkConfiguration.h \ @@ -707,6 +712,7 @@ SOURCES += \ src/Joystick/Joystick.cc \ src/Joystick/JoystickManager.cc \ src/JsonHelper.cc \ + src/KMLFileHelper.cc \ src/LogCompressor.cc \ src/MissionManager/CameraCalc.cc \ src/MissionManager/CameraSection.cc \ @@ -738,7 +744,7 @@ SOURCES += \ src/MissionManager/SimpleMissionItem.cc \ src/MissionManager/SpeedSection.cc \ src/MissionManager/StructureScanComplexItem.cc \ - src/MissionManager/SurveyMissionItem.cc \ + src/MissionManager/SurveyComplexItem.cc \ src/MissionManager/TransectStyleComplexItem.cc \ src/MissionManager/VisualMissionItem.cc \ src/PositionManager/PositionManager.cpp \ @@ -778,7 +784,8 @@ SOURCES += \ src/Settings/SettingsManager.cc \ src/Settings/UnitsSettings.cc \ src/Settings/VideoSettings.cc \ - src/Terrain.cc \ + src/Terrain/TerrainQuery.cc \ + src/TerrainTile.cc\ src/Vehicle/MAVLinkLogManager.cc \ src/VehicleSetup/JoystickConfigController.cc \ src/comm/LinkConfiguration.cc \ @@ -941,6 +948,7 @@ APMFirmwarePlugin { src/AutoPilotPlugins/APM/APMCompassCal.h \ src/AutoPilotPlugins/APM/APMFlightModesComponent.h \ src/AutoPilotPlugins/APM/APMFlightModesComponentController.h \ + src/AutoPilotPlugins/APM/APMHeliComponent.h \ src/AutoPilotPlugins/APM/APMLightsComponent.h \ src/AutoPilotPlugins/APM/APMSubFrameComponent.h \ src/AutoPilotPlugins/APM/APMPowerComponent.h \ @@ -966,6 +974,7 @@ APMFirmwarePlugin { src/AutoPilotPlugins/APM/APMCompassCal.cc \ src/AutoPilotPlugins/APM/APMFlightModesComponent.cc \ src/AutoPilotPlugins/APM/APMFlightModesComponentController.cc \ + src/AutoPilotPlugins/APM/APMHeliComponent.cc \ src/AutoPilotPlugins/APM/APMLightsComponent.cc \ src/AutoPilotPlugins/APM/APMSubFrameComponent.cc \ src/AutoPilotPlugins/APM/APMPowerComponent.cc \ @@ -1054,7 +1063,7 @@ HEADERS += \ src/FactSystem/FactGroup.h \ src/FactSystem/FactMetaData.h \ src/FactSystem/FactSystem.h \ - src/FactSystem/FactValidator.h \ + src/FactSystem/FactValueSliderListModel.h \ src/FactSystem/ParameterManager.h \ src/FactSystem/SettingsFact.h \ @@ -1064,7 +1073,7 @@ SOURCES += \ src/FactSystem/FactGroup.cc \ src/FactSystem/FactMetaData.cc \ src/FactSystem/FactSystem.cc \ - src/FactSystem/FactValidator.cc \ + src/FactSystem/FactValueSliderListModel.cc \ src/FactSystem/ParameterManager.cc \ src/FactSystem/SettingsFact.cc \ diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index ce7371c7502275d9f819ae8f4b60ad40c684843e..1d4ec4ff3c15e24720c1acd9eb06a2a2528b4212 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -9,10 +9,11 @@ src/ui/toolbar/GPSRTKIndicator.qml src/ui/toolbar/MessageIndicator.qml src/ui/toolbar/ModeIndicator.qml - src/ui/toolbar/VTOLModeIndicator.qml + src/ui/toolbar/VTOLModeIndicator.qml src/ui/toolbar/RCRSSIIndicator.qml src/ui/toolbar/TelemetryRSSIIndicator.qml src/ui/toolbar/JoystickIndicator.qml + src/ui/toolbar/LinkIndicator.qml src/PlanView/CorridorScanEditor.qml @@ -85,7 +86,12 @@ src/QmlControls/PageView.qml src/QmlControls/ParameterEditor.qml src/QmlControls/ParameterEditorDialog.qml + src/QmlControls/PIDTuning.qml src/PlanView/PlanToolBar.qml + src/QmlControls/PreFlightCheckButton.qml + src/QmlControls/PreFlightCheckGroup.qml + src/QmlControls/PreFlightCheckList.qml + src/QmlControls/PreFlightCheckModel.qml src/QmlControls/QGCButton.qml src/QmlControls/QGCCheckBox.qml src/QmlControls/QGCColoredImage.qml @@ -118,13 +124,14 @@ src/PlanView/RallyPointMapVisuals.qml src/QmlControls/RCChannelMonitor.qml src/QmlControls/RoundButton.qml - src/PlanView/SectionHeader.qml + src/QmlControls/SectionHeader.qml src/AutoPilotPlugins/Common/SetupPage.qml src/ui/toolbar/SignalStrength.qml src/PlanView/SimpleItemMapVisual.qml src/QmlControls/SliderSwitch.qml src/QmlControls/SubMenuButton.qml src/PlanView/SurveyMapVisual.qml + src/PlanView/TransectStyleComplexItemStats.qml src/QmlControls/ToolStrip.qml src/QmlControls/VehicleRotationCal.qml src/QmlControls/VehicleSummaryRow.qml @@ -137,6 +144,7 @@ src/FactSystem/FactControls/FactTextField.qml src/FactSystem/FactControls/FactTextFieldGrid.qml src/FactSystem/FactControls/FactTextFieldRow.qml + src/FactSystem/FactControls/FactValueSlider.qml src/FactSystem/FactControls/qmldir src/FlightDisplay/FlightDisplayView.qml src/FlightDisplay/FlightDisplayViewMap.qml @@ -147,6 +155,12 @@ src/FlightDisplay/GuidedActionsController.qml src/FlightDisplay/GuidedAltitudeSlider.qml src/FlightDisplay/MultiVehicleList.qml + src/FlightDisplay/PreFlightBatteryCheck.qml + src/FlightDisplay/BuiltInPreFlightCheckModel.qml + src/FlightDisplay/PreFlightGPSCheck.qml + src/FlightDisplay/PreFlightRCCheck.qml + src/FlightDisplay/PreFlightSensorsHealthCheck.qml + src/FlightDisplay/PreFlightSoundCheck.qml src/FlightDisplay/qmldir src/FlightMap/MapItems/CameraTriggerIndicator.qml src/FlightMap/Widgets/CenterMapDropButton.qml @@ -221,8 +235,10 @@ src/comm/USBBoardInfo.json src/Vehicle/BatteryFact.json src/Vehicle/ClockFact.json + src/Vehicle/DistanceSensorFact.json src/Vehicle/GPSFact.json src/Vehicle/GPSRTKFact.json + src/Vehicle/SetpointFact.json src/Vehicle/SubmarineFact.json src/Vehicle/TemperatureFact.json src/Vehicle/VehicleFact.json diff --git a/resources/calibration/mode1/radioRollLeft.png b/resources/calibration/mode1/radioRollLeft.png index 7fed7e5061d350cbe1555876faa672d45fa7ec13..8f6a931b8f30d3c41b8968f6b399185a16257ef9 100644 Binary files a/resources/calibration/mode1/radioRollLeft.png and b/resources/calibration/mode1/radioRollLeft.png differ diff --git a/resources/calibration/mode1/radioRollRight.png b/resources/calibration/mode1/radioRollRight.png index b532961f3b4baaef14b9352a66f75d5d8823415e..6d805022c4b67156fbda4d69bbb3b5d650ff340f 100644 Binary files a/resources/calibration/mode1/radioRollRight.png and b/resources/calibration/mode1/radioRollRight.png differ diff --git a/resources/check.svg b/resources/check.svg new file mode 100644 index 0000000000000000000000000000000000000000..f5c4901ebe56008b699828a31f3c22c2cfa42e9e --- /dev/null +++ b/resources/check.svg @@ -0,0 +1,80 @@ + + + +image/svg+xml \ No newline at end of file diff --git a/src/AnalyzeView/GeoTagController.cc b/src/AnalyzeView/GeoTagController.cc index 33ee123fc3f81901b8c32c4a7afe0f583828bb49..3f0627deb204b1ce6f5e775e6b62cf435131e488 100644 --- a/src/AnalyzeView/GeoTagController.cc +++ b/src/AnalyzeView/GeoTagController.cc @@ -70,8 +70,7 @@ void GeoTagController::startTagging(void) QDir imageDirectory = QDir(_worker.imageDirectory()); if(!imageDirectory.exists()) { - _errorMessage = tr("Cannot find the image directory"); - emit errorMessageChanged(_errorMessage); + _setErrorMessage(tr("Cannot find the image directory")); return; } if(_worker.saveDirectory() == "") { @@ -83,23 +82,20 @@ void GeoTagController::startTagging(void) msgBox.setWindowModality(Qt::ApplicationModal); msgBox.addButton(tr("Replace"), QMessageBox::ActionRole); if (msgBox.exec() == QMessageBox::Cancel) { - _errorMessage = tr("Images have already been tagged"); - emit errorMessageChanged(_errorMessage); + _setErrorMessage(tr("Images have already been tagged")); return; } QDir oldTaggedFolder = QDir(_worker.imageDirectory() + "/TAGGED"); oldTaggedFolder.removeRecursively(); if(!imageDirectory.mkdir(_worker.imageDirectory() + "/TAGGED")) { - _errorMessage = tr("Couldn't replace the previously tagged images"); - emit errorMessageChanged(_errorMessage); + _setErrorMessage(tr("Couldn't replace the previously tagged images")); return; } } } else { QDir saveDirectory = QDir(_worker.saveDirectory()); if(!saveDirectory.exists()) { - _errorMessage = tr("Cannot find the save directory"); - emit errorMessageChanged(_errorMessage); + _setErrorMessage(tr("Cannot find the save directory")); return; } saveDirectory.setFilter(QDir::Files | QDir::Readable | QDir::NoSymLinks | QDir::Writable); @@ -115,15 +111,13 @@ void GeoTagController::startTagging(void) msgBox.setWindowModality(Qt::ApplicationModal); msgBox.addButton(tr("Replace"), QMessageBox::ActionRole); if (msgBox.exec() == QMessageBox::Cancel) { - _errorMessage = tr("Save folder not empty"); - emit errorMessageChanged(_errorMessage); + _setErrorMessage(tr("Save folder not empty")); return; } foreach(QString dirFile, imageList) { if(!saveDirectory.remove(dirFile)) { - _errorMessage = tr("Couldn't replace the existing images"); - emit errorMessageChanged(_errorMessage); + _setErrorMessage(tr("Couldn't replace the existing images")); return; } } @@ -144,6 +138,13 @@ void GeoTagController::_workerError(QString errorMessage) emit errorMessageChanged(errorMessage); } + +void GeoTagController::_setErrorMessage(const QString& error) +{ + _errorMessage = error; + emit errorMessageChanged(error); +} + GeoTagWorker::GeoTagWorker(void) : _cancel(false) , _logFile("") @@ -210,9 +211,10 @@ void GeoTagWorker::run(void) // Instantiate appropriate parser _triggerList.clear(); bool parseComplete = false; - if(isULog) { + QString errorString; + if (isULog) { ULogParser parser; - parseComplete = parser.getTagsFromLog(log, _triggerList); + parseComplete = parser.getTagsFromLog(log, _triggerList, errorString); } else { PX4LogParser parser; @@ -227,7 +229,8 @@ void GeoTagWorker::run(void) return; } else { qCDebug(GeotaggingLog) << "Log parsing failed"; - emit error(tr("Log parsing failed - tagging cancelled")); + errorString = tr("%1 - tagging cancelled").arg(errorString.isEmpty() ? tr("Log parsing failed") : errorString); + emit error(errorString); return; } } @@ -259,6 +262,11 @@ void GeoTagWorker::run(void) int maxIndex = std::min(_imageIndices.count(), _triggerIndices.count()); maxIndex = std::min(maxIndex, _imageList.count()); for(int i = 0; i < maxIndex; i++) { + int imageIndex = _imageIndices[i]; + if (imageIndex >= _imageList.count()) { + emit error(tr("Geotagging failed. Image requested not present.")); + return; + } QFile fileRead(_imageList.at(_imageIndices[i]).absoluteFilePath()); if (!fileRead.open(QIODevice::ReadOnly)) { emit error(tr("Geotagging failed. Couldn't open an image.")); diff --git a/src/AnalyzeView/GeoTagController.h b/src/AnalyzeView/GeoTagController.h index aee58f6dd68063514c8744bd6aa82f9ab2b4b2ad..f3955d2712351a8501e72116b5a3bd2728030438 100644 --- a/src/AnalyzeView/GeoTagController.h +++ b/src/AnalyzeView/GeoTagController.h @@ -117,8 +117,9 @@ signals: void errorMessageChanged (QString errorMessage); private slots: - void _workerProgressChanged(double progress); - void _workerError(QString errorMsg); + void _workerProgressChanged (double progress); + void _workerError (QString errorMsg); + void _setErrorMessage (const QString& error); private: QString _errorMessage; diff --git a/src/AnalyzeView/GeoTagPage.qml b/src/AnalyzeView/GeoTagPage.qml index d4f4386f074ddfd71c5a2526bf3c3349e73b7bff..c64147e401dc9178f2ab4024ee27cda29f3365d9 100644 --- a/src/AnalyzeView/GeoTagPage.qml +++ b/src/AnalyzeView/GeoTagPage.qml @@ -124,8 +124,9 @@ AnalyzePage { } QGCButton { - text: geoController.inProgress ? qsTr("Cancel Tagging") : qsTr("Start Tagging") - width: ScreenTools.defaultFontPixelWidth * 30 + text: geoController.inProgress ? qsTr("Cancel Tagging") : qsTr("Start Tagging") + width: ScreenTools.defaultFontPixelWidth * 30 + onClicked: { if (geoController.inProgress) { geoController.cancelTagging() diff --git a/src/AnalyzeView/LogDownloadController.cc b/src/AnalyzeView/LogDownloadController.cc index 133a55671663451192a5d4ad5481485970413585..3eeb02ada316fda8682e1480f84485757fdff64d 100644 --- a/src/AnalyzeView/LogDownloadController.cc +++ b/src/AnalyzeView/LogDownloadController.cc @@ -321,7 +321,7 @@ LogDownloadController::_logData(UASInterface* uas, uint32_t ofs, uint16_t id, ui if(ofs <= _downloadData->entry->size()) { const uint32_t chunk = ofs / kChunkSize; if (chunk != _downloadData->current_chunk) { - qWarning() << "Ignored packet for out of order chunk" << chunk; + qWarning() << "Ignored packet for out of order chunk actual:expected" << chunk << _downloadData->current_chunk; return; } const uint16_t bin = (ofs - chunk*kChunkSize) / MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN; diff --git a/src/AnalyzeView/LogDownloadPage.qml b/src/AnalyzeView/LogDownloadPage.qml index bbe5b37979d213eba2881cf3597fb5b3c220d93b..298d19e797cfaedb9f4ee580958b9ac87ca8a839 100644 --- a/src/AnalyzeView/LogDownloadPage.qml +++ b/src/AnalyzeView/LogDownloadPage.qml @@ -51,8 +51,7 @@ AnalyzePage { TableView { id: tableView - anchors.top: parent.top - anchors.bottom: parent.bottom + Layout.fillHeight: true model: logController.model selectionMode: SelectionMode.MultiSelection Layout.fillWidth: true @@ -155,7 +154,7 @@ AnalyzePage { fileDialog.qgcView = logDownloadPage fileDialog.title = qsTr("Select save directory") fileDialog.selectExisting = true - fileDialog.folder = QGroundControl.settingsManager.appSettings.telemetrySavePath + fileDialog.folder = QGroundControl.settingsManager.appSettings.logSavePath fileDialog.selectFolder = true fileDialog.openForLoad() } @@ -186,7 +185,7 @@ AnalyzePage { message: qsTr("All log files will be erased permanently. Is this really what you want?") function accept() { - logDownloadPage.hideDialog() + hideDialog() logController.eraseAll() } } diff --git a/src/AnalyzeView/MavlinkConsoleController.cc b/src/AnalyzeView/MavlinkConsoleController.cc index 6bc3004378b70ee194e6b83696195845a779614c..12d8c282227c679af2461d25dd87cae2c5d2654c 100644 --- a/src/AnalyzeView/MavlinkConsoleController.cc +++ b/src/AnalyzeView/MavlinkConsoleController.cc @@ -33,12 +33,25 @@ MavlinkConsoleController::~MavlinkConsoleController() void MavlinkConsoleController::sendCommand(QString command) { + _history.append(command); command.append("\n"); _sendSerialData(qPrintable(command)); _cursor_home_pos = -1; _cursor = rowCount(); } +QString +MavlinkConsoleController::historyUp(const QString& current) +{ + return _history.up(current); +} + +QString +MavlinkConsoleController::historyDown(const QString& current) +{ + return _history.down(current); +} + void MavlinkConsoleController::_setActiveVehicle(Vehicle* vehicle) { @@ -192,3 +205,43 @@ MavlinkConsoleController::writeLine(int line, const QByteArray &text) auto idx = index(line); setData(idx, data(idx, Qt::DisplayRole).toString() + text); } + +void MavlinkConsoleController::CommandHistory::append(const QString& command) +{ + if (command.length() > 0) { + + // do not append duplicates + if (_history.length() == 0 || _history.last() != command) { + + if (_history.length() >= maxHistoryLength) { + _history.removeFirst(); + } + _history.append(command); + } + } + _index = _history.length(); +} + +QString MavlinkConsoleController::CommandHistory::up(const QString& current) +{ + if (_index <= 0) + return current; + + --_index; + if (_index < _history.length()) { + return _history[_index]; + } + return ""; +} + +QString MavlinkConsoleController::CommandHistory::down(const QString& current) +{ + if (_index >= _history.length()) + return current; + + ++_index; + if (_index < _history.length()) { + return _history[_index]; + } + return ""; +} diff --git a/src/AnalyzeView/MavlinkConsoleController.h b/src/AnalyzeView/MavlinkConsoleController.h index 950b3786bb723ae2a34428a95cfb9bd5e41f50cc..46cc5d42a80dbed8eb7cd50c2ad30d1fbdef33ea 100644 --- a/src/AnalyzeView/MavlinkConsoleController.h +++ b/src/AnalyzeView/MavlinkConsoleController.h @@ -27,13 +27,12 @@ class MavlinkConsoleController : public QStringListModel public: MavlinkConsoleController(); - ~MavlinkConsoleController(); + virtual ~MavlinkConsoleController(); -public slots: - void sendCommand(QString command); + Q_INVOKABLE void sendCommand(QString command); -signals: - void cursorChanged(int); + Q_INVOKABLE QString historyUp(const QString& current); + Q_INVOKABLE QString historyDown(const QString& current); private slots: void _setActiveVehicle (Vehicle* vehicle); @@ -44,10 +43,23 @@ private: void _sendSerialData(QByteArray, bool close = false); void writeLine(int line, const QByteArray &text); + class CommandHistory + { + public: + void append(const QString& command); + QString up(const QString& current); + QString down(const QString& current); + private: + static constexpr int maxHistoryLength = 100; + QList _history; + int _index = 0; + }; + int _cursor_home_pos; int _cursor; QByteArray _incoming_buffer; Vehicle* _vehicle; QList _uas_connections; + CommandHistory _history; }; diff --git a/src/AnalyzeView/MavlinkConsolePage.qml b/src/AnalyzeView/MavlinkConsolePage.qml index 75def438b092eb8b429ccdefe697f01e8445c350..3f313ee1c5c7b87c4e912411d5aa48e1ecfbc869 100644 --- a/src/AnalyzeView/MavlinkConsolePage.qml +++ b/src/AnalyzeView/MavlinkConsolePage.qml @@ -26,7 +26,7 @@ AnalyzePage { pageName: qsTr("Mavlink Console") pageDescription: qsTr("Mavlink Console provides a connection to the vehicle's system shell.") - property bool loaded: false + property bool isLoaded: false Component { id: pageComponent @@ -41,7 +41,7 @@ AnalyzePage { onDataChanged: { // Keep the view in sync if the button is checked - if (loaded) { + if (isLoaded) { if (followTail.checked) { listview.positionViewAtEnd(); } @@ -53,7 +53,7 @@ AnalyzePage { id: delegateItem Rectangle { color: qgcPal.windowShade - height: Math.round(ScreenTools.defaultFontPixelHeight * 0.5 + field.height) + height: Math.round(ScreenTools.defaultFontPixelHeight * 0.1 + field.height) width: listview.width QGCLabel { @@ -69,11 +69,10 @@ AnalyzePage { QGCListView { Component.onCompleted: { - loaded = true + isLoaded = true } Layout.fillHeight: true - anchors.left: parent.left - anchors.right: parent.right + Layout.fillWidth: true clip: true id: listview model: conController @@ -86,8 +85,7 @@ AnalyzePage { } RowLayout { - anchors.left: parent.left - anchors.right: parent.right + Layout.fillWidth: true QGCTextField { id: command Layout.fillWidth: true @@ -96,6 +94,15 @@ AnalyzePage { conController.sendCommand(text) text = "" } + Keys.onPressed: { + if (event.key == Qt.Key_Up) { + text = conController.historyUp(text); + event.accepted = true; + } else if (event.key == Qt.Key_Down) { + text = conController.historyDown(text); + event.accepted = true; + } + } } QGCButton { @@ -105,7 +112,7 @@ AnalyzePage { checked: true onCheckedChanged: { - if (checked && loaded) { + if (checked && isLoaded) { listview.positionViewAtEnd(); } } diff --git a/src/AnalyzeView/ULogParser.cc b/src/AnalyzeView/ULogParser.cc index 8a7e172bcd85d6d54fba277948cb9cd72e47a3ae..d6b7f365eefee50535fe47c5b8938ee920c77cc4 100644 --- a/src/AnalyzeView/ULogParser.cc +++ b/src/AnalyzeView/ULogParser.cc @@ -86,11 +86,13 @@ bool ULogParser::parseFieldFormat(QString& fields) return false; } -bool ULogParser::getTagsFromLog(QByteArray& log, QList& cameraFeedback) +bool ULogParser::getTagsFromLog(QByteArray& log, QList& cameraFeedback, QString& errorMessage) { + errorMessage.clear(); + //verify it's an ULog file if(!log.contains(_ULogMagic)) { - qWarning() << "Could not detect ULog file header magic"; + errorMessage = tr("Could not detect ULog file header magic"); return false; } @@ -139,15 +141,10 @@ bool ULogParser::getTagsFromLog(QByteArray& log, QList #include +#include #include "GeoTagController.h" @@ -10,10 +11,14 @@ class ULogParser { + Q_DECLARE_TR_FUNCTIONS(ULogParser) + public: ULogParser(); ~ULogParser(); - bool getTagsFromLog(QByteArray& log, QList& cameraFeedback); + + /// @return true: failed, errorMessage set + bool getTagsFromLog(QByteArray& log, QList& cameraFeedback, QString& errorMessage); private: diff --git a/src/Audio/AudioOutput.cc b/src/Audio/AudioOutput.cc index 6a9e9411ead75b36809ff536b0cd2231dc269f8c..88a3d4e79afe77419af56cbd104b143cafd84fe0 100644 --- a/src/Audio/AudioOutput.cc +++ b/src/Audio/AudioOutput.cc @@ -18,8 +18,8 @@ AudioOutput::AudioOutput(QGCApplication* app, QGCToolbox* toolbox) : QGCTool(app, toolbox) + , _tts(new QTextToSpeech(this)) { - _tts = new QTextToSpeech(this); connect(_tts, &QTextToSpeech::stateChanged, this, &AudioOutput::_stateChanged); } @@ -73,6 +73,7 @@ QString AudioOutput::fixTextMessageForAudio(const QString& string) { QString match; QString newNumber; QString result = string; + //-- Look for codified terms if(result.contains("ERR ", Qt::CaseInsensitive)) { result.replace("ERR ", "error ", Qt::CaseInsensitive); @@ -118,6 +119,9 @@ QString AudioOutput::fixTextMessageForAudio(const QString& string) { if(result.contains(" ADSB ", Qt::CaseInsensitive)) { result.replace(" ADSB ", " Hey Dee Ess Bee ", Qt::CaseInsensitive); } + if(result.contains(" EKF ", Qt::CaseInsensitive)) { + result.replace(" EKF ", " Eee Kay Eff ", Qt::CaseInsensitive); + } // Convert negative numbers QRegularExpression re(QStringLiteral("(-)[0-9]*\\.?[0-9]")); @@ -125,9 +129,18 @@ QString AudioOutput::fixTextMessageForAudio(const QString& string) { while (reMatch.hasMatch()) { if (!reMatch.captured(1).isNull()) { // There is a negative prefix - qDebug() << "negative" << reMatch.captured(1) << reMatch.capturedStart(1) << reMatch.capturedEnd(1); result.replace(reMatch.capturedStart(1), reMatch.capturedEnd(1) - reMatch.capturedStart(1), tr(" negative ")); - qDebug() << result; + } + reMatch = re.match(result); + } + + // Convert real number with decimal point + re.setPattern(QStringLiteral("([0-9]+)(\\.)([0-9]+)")); + reMatch = re.match(result); + while (reMatch.hasMatch()) { + if (!reMatch.captured(2).isNull()) { + // There is a decimal point + result.replace(reMatch.capturedStart(2), reMatch.capturedEnd(2) - reMatch.capturedStart(2), tr(" point ")); } reMatch = re.match(result); } @@ -138,9 +151,7 @@ QString AudioOutput::fixTextMessageForAudio(const QString& string) { while (reMatch.hasMatch()) { if (!reMatch.captured(1).isNull()) { // There is a meter postfix - qDebug() << "meters" << reMatch.captured(1) << reMatch.capturedStart(1) << reMatch.capturedEnd(1); result.replace(reMatch.capturedStart(1), reMatch.capturedEnd(1) - reMatch.capturedStart(1), tr(" meters")); - qDebug() << result; } reMatch = re.match(result); } diff --git a/src/Audio/AudioOutputTest.cc b/src/Audio/AudioOutputTest.cc index 3395b8934d28f804a8f1a87df8863e1fb92b9b5a..6cdb6e4b29918ea96250ab94b272b84ea6473d5e 100644 --- a/src/Audio/AudioOutputTest.cc +++ b/src/Audio/AudioOutputTest.cc @@ -18,7 +18,7 @@ AudioOutputTest::AudioOutputTest(void) void AudioOutputTest::_testSpokenReplacements(void) { QString result = AudioOutput::fixTextMessageForAudio(QStringLiteral("-10.5m, -10.5m. -10.5 m")); - QCOMPARE(result, QStringLiteral(" negative 10.5 meters, negative 10.5 meters. negative 10.5 meters")); + QCOMPARE(result, QStringLiteral(" negative 10 point 5 meters, negative 10 point 5 meters. negative 10 point 5 meters")); result = AudioOutput::fixTextMessageForAudio(QStringLiteral("-10m -10 m")); QCOMPARE(result, QStringLiteral(" negative 10 meters negative 10 meters")); result = AudioOutput::fixTextMessageForAudio(QStringLiteral("foo -10m -10 m bar")); diff --git a/src/AutoPilotPlugins/APM/APMAirframeComponent.qml b/src/AutoPilotPlugins/APM/APMAirframeComponent.qml index 54ac4519a3e2a4bd181b29f7804ab953276454db..e29e7fdc734245c463cf8ebbbf3523505984bd1d 100644 --- a/src/AutoPilotPlugins/APM/APMAirframeComponent.qml +++ b/src/AutoPilotPlugins/APM/APMAirframeComponent.qml @@ -83,10 +83,9 @@ SetupPage { id: newFramePageComponent Grid { - anchors.left: parent.left - anchors.right: parent.right - spacing: _margins - columns: 2 + width: availableWidth + spacing: _margins + columns: 2 QGCLabel { text: qsTr("Frame Class:") diff --git a/src/AutoPilotPlugins/APM/APMAirframeComponentSummary.qml b/src/AutoPilotPlugins/APM/APMAirframeComponentSummary.qml index 4f9f447ed8b02dd9b25e128887a9b9103bc7db61..d5f6e3261cc59e7354130aa4e6a3ac625ebc0438 100644 --- a/src/AutoPilotPlugins/APM/APMAirframeComponentSummary.qml +++ b/src/AutoPilotPlugins/APM/APMAirframeComponentSummary.qml @@ -18,36 +18,36 @@ FactPanel { factPanel: panel } - property bool _useOldFrameParam: controller.parameterExists(-1, "FRAME") - property Fact _oldFrameParam: controller.getParameterFact(-1, "FRAME", false) - property Fact _newFrameParam: controller.getParameterFact(-1, "FRAME_CLASS", false) - property Fact _frameTypeParam: controller.getParameterFact(-1, "FRAME_TYPE", false) + property bool _frameAvailable: controller.parameterExists(-1, "FRAME") + + property Fact _frame: controller.getParameterFact(-1, "FRAME", false) + property Fact _frameClass: controller.getParameterFact(-1, "FRAME_CLASS", false) + property Fact _frameType: controller.getParameterFact(-1, "FRAME_TYPE", false) Column { anchors.fill: parent VehicleSummaryRow { - labelText: qsTr("Frame Type:") - valueText: controller.currentAirframeTypeName() + " " + _oldFrameParam.enumStringValue - visible: _useOldFrameParam + labelText: qsTr("Frame Type") + valueText: visible ? controller.currentAirframeTypeName() + " " + _frame.enumStringValue : "" + visible: _frameAvailable } VehicleSummaryRow { - labelText: qsTr("Frame Class:") - valueText: _newFrameParam.enumStringValue - visible: !_useOldFrameParam + labelText: qsTr("Frame Class") + valueText: visible ? _frameClass.enumStringValue : "" + visible: !_frameAvailable } VehicleSummaryRow { - labelText: qsTr("Frame Type:") - valueText: _frameTypeParam.enumStringValue - visible: !_useOldFrameParam - + labelText: qsTr("Frame Type") + valueText: visible ? _frameType.enumStringValue : "" + visible: !_frameAvailable } VehicleSummaryRow { - labelText: qsTr("Firmware Version:") + labelText: qsTr("Firmware Version") valueText: activeVehicle.firmwareMajorVersion == -1 ? qsTr("Unknown") : activeVehicle.firmwareMajorVersion + "." + activeVehicle.firmwareMinorVersion + "." + activeVehicle.firmwarePatchVersion + activeVehicle.firmwareVersionTypeString } } diff --git a/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc b/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc index 1df7e73923bba6e97fde80c1e60b80a13e777e64..58b3e13b85cb14e686e1298586d15384aee6ef4b 100644 --- a/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc @@ -28,27 +28,29 @@ #include "APMLightsComponent.h" #include "APMSubFrameComponent.h" #include "ESP8266Component.h" +#include "APMHeliComponent.h" /// This is the AutoPilotPlugin implementatin for the MAV_AUTOPILOT_ARDUPILOT type. APMAutoPilotPlugin::APMAutoPilotPlugin(Vehicle* vehicle, QObject* parent) - : AutoPilotPlugin(vehicle, parent) + : AutoPilotPlugin (vehicle, parent) , _incorrectParameterVersion(false) - , _airframeComponent(NULL) - , _cameraComponent(NULL) - , _lightsComponent(NULL) - , _subFrameComponent(NULL) - , _flightModesComponent(NULL) - , _powerComponent(NULL) + , _airframeComponent (NULL) + , _cameraComponent (NULL) + , _lightsComponent (NULL) + , _subFrameComponent (NULL) + , _flightModesComponent (NULL) + , _powerComponent (NULL) #if 0 // Temporarily removed, waiting for new command implementation - , _motorComponent(NULL) + , _motorComponent (NULL) #endif - , _radioComponent(NULL) - , _safetyComponent(NULL) - , _sensorsComponent(NULL) - , _tuningComponent(NULL) - , _airframeFacts(new APMAirframeLoader(this, vehicle->uas(), this)) - , _esp8266Component(NULL) + , _radioComponent (NULL) + , _safetyComponent (NULL) + , _sensorsComponent (NULL) + , _tuningComponent (NULL) + , _airframeFacts (new APMAirframeLoader(this, vehicle->uas(), this)) + , _esp8266Component (NULL) + , _heliComponent (NULL) { APMAirframeLoader::loadAirframeFactMetaData(); } @@ -101,6 +103,12 @@ const QVariantList& APMAutoPilotPlugin::vehicleComponents(void) _safetyComponent->setupTriggerSignals(); _components.append(QVariant::fromValue((VehicleComponent*)_safetyComponent)); + if (_vehicle->vehicleType() == MAV_TYPE_HELICOPTER) { + _heliComponent = new APMHeliComponent(_vehicle, this); + _heliComponent->setupTriggerSignals(); + _components.append(QVariant::fromValue((VehicleComponent*)_heliComponent)); + } + _tuningComponent = new APMTuningComponent(_vehicle, this); _tuningComponent->setupTriggerSignals(); _components.append(QVariant::fromValue((VehicleComponent*)_tuningComponent)); diff --git a/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.h b/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.h index fe6246f27e2bde05bbb53af224645b7077093594..018eb11502e8bdbd2dc013412448dc942ce0065d 100644 --- a/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.h +++ b/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.h @@ -27,6 +27,7 @@ class APMCameraComponent; class APMLightsComponent; class APMSubFrameComponent; class ESP8266Component; +class APMHeliComponent; /// This is the APM specific implementation of the AutoPilot class. class APMAutoPilotPlugin : public AutoPilotPlugin @@ -59,6 +60,7 @@ protected: APMTuningComponent* _tuningComponent; APMAirframeLoader* _airframeFacts; ESP8266Component* _esp8266Component; + APMHeliComponent* _heliComponent; private: QVariantList _components; diff --git a/src/AutoPilotPlugins/APM/APMCameraComponent.qml b/src/AutoPilotPlugins/APM/APMCameraComponent.qml index 121cfdf728021863264103e7af73ea0621ad2aa2..df52605d179c8b001108d6a0c50af24995e9afc4 100644 --- a/src/AutoPilotPlugins/APM/APMCameraComponent.qml +++ b/src/AutoPilotPlugins/APM/APMCameraComponent.qml @@ -170,17 +170,29 @@ SetupPage { ListModel { id: gimbalOutModel + // It appears that QGCComboBox can't handle models that don't have a initial item + // after onModelChanged ListElement { text: qsTr("Disabled"); value: 0 } - ListElement { text: qsTr("Channel 5"); value: 5 } - ListElement { text: qsTr("Channel 6"); value: 6 } - ListElement { text: qsTr("Channel 7"); value: 7 } - ListElement { text: qsTr("Channel 8"); value: 8 } - ListElement { text: qsTr("Channel 9"); value: 9 } - ListElement { text: qsTr("Channel 10"); value: 10 } - ListElement { text: qsTr("Channel 11"); value: 11 } - ListElement { text: qsTr("Channel 12"); value: 12 } - ListElement { text: qsTr("Channel 13"); value: 13 } - ListElement { text: qsTr("Channel 14"); value: 14 } + + function update(number) { + // Not enough channels + if(number < 6) { + return + } + for(var i = 5; i <= number; i++) { + var text = qsTr("Channel ") + i + append({"text": text, "value": i}) + } + } + + Component.onCompleted: { + // Number of main outputs + var baseValue = 8 + // Extra outputs + // http://ardupilot.org/copter/docs/parameters.html#brd-pwm-count-auxiliary-pin-config + var brd_pwm_count_value = controller.getParameterFact(-1, "BRD_PWM_COUNT").value + update(8 + (brd_pwm_count_value == 7 ? 3 : brd_pwm_count_value)) + } } Component { diff --git a/src/AutoPilotPlugins/APM/APMCameraComponentSummary.qml b/src/AutoPilotPlugins/APM/APMCameraComponentSummary.qml index c97de7af0ccb45f0187459dbde2508da4d5c7e8c..d2cf18393e68c76fa79681932604d107cc06b4d0 100644 --- a/src/AutoPilotPlugins/APM/APMCameraComponentSummary.qml +++ b/src/AutoPilotPlugins/APM/APMCameraComponentSummary.qml @@ -27,22 +27,22 @@ FactPanel { VehicleSummaryRow { visible: _mountTypeExists - labelText: qsTr("Gimbal type:") + labelText: qsTr("Gimbal type") valueText: _mountTypeValue } VehicleSummaryRow { - labelText: qsTr("Tilt input channel:") + labelText: qsTr("Tilt input channel") valueText: _mountRCInTilt.enumStringValue } VehicleSummaryRow { - labelText: qsTr("Pan input channel:") + labelText: qsTr("Pan input channel") valueText: _mountRCInPan.enumStringValue } VehicleSummaryRow { - labelText: qsTr("Roll input channel:") + labelText: qsTr("Roll input channel") valueText: _mountRCInRoll.enumStringValue } } diff --git a/src/AutoPilotPlugins/APM/APMFlightModesComponentController.cc b/src/AutoPilotPlugins/APM/APMFlightModesComponentController.cc index 0411846bf09613cac704441d04cffaceda57b6cd..f13e78858f844a92ddc59d95fa1cdd684a417a2e 100644 --- a/src/AutoPilotPlugins/APM/APMFlightModesComponentController.cc +++ b/src/AutoPilotPlugins/APM/APMFlightModesComponentController.cc @@ -68,7 +68,7 @@ void APMFlightModesComponentController::_rcChannelsChanged(int channelCount, int for (int i=0; i<6; i++) { _rgChannelOptionEnabled[i] = QVariant(false); channelValue = pwmValues[i+6]; - if (channelValue != -1 && channelValue > 1800) { + if (channelValue > 1800) { _rgChannelOptionEnabled[i] = QVariant(true); } } diff --git a/src/AutoPilotPlugins/APM/APMFlightModesComponentSummary.qml b/src/AutoPilotPlugins/APM/APMFlightModesComponentSummary.qml index fe6cb2bcd736db96f13d60447d2d79c9d9e0639b..1a6037697956da1d2663de10b73093977a577177 100644 --- a/src/AutoPilotPlugins/APM/APMFlightModesComponentSummary.qml +++ b/src/AutoPilotPlugins/APM/APMFlightModesComponentSummary.qml @@ -27,32 +27,32 @@ FactPanel { anchors.fill: parent VehicleSummaryRow { - labelText: qsTr("Flight Mode 1:") + labelText: qsTr("Flight Mode 1") valueText: flightMode1.enumStringValue } VehicleSummaryRow { - labelText: qsTr("Flight Mode 2:") + labelText: qsTr("Flight Mode 2") valueText: flightMode2.enumStringValue } VehicleSummaryRow { - labelText: qsTr("Flight Mode 3:") + labelText: qsTr("Flight Mode 3") valueText: flightMode3.enumStringValue } VehicleSummaryRow { - labelText: qsTr("Flight Mode 4:") + labelText: qsTr("Flight Mode 4") valueText: flightMode4.enumStringValue } VehicleSummaryRow { - labelText: qsTr("Flight Mode 5:") + labelText: qsTr("Flight Mode 5") valueText: flightMode5.enumStringValue } VehicleSummaryRow { - labelText: qsTr("Flight Mode 6:") + labelText: qsTr("Flight Mode 6") valueText: flightMode6.enumStringValue } } diff --git a/src/AutoPilotPlugins/APM/APMHeliComponent.cc b/src/AutoPilotPlugins/APM/APMHeliComponent.cc new file mode 100644 index 0000000000000000000000000000000000000000..9251d299c94c00df3ed9e23444c6e1c44481c531 --- /dev/null +++ b/src/AutoPilotPlugins/APM/APMHeliComponent.cc @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "APMHeliComponent.h" +#include "APMAutoPilotPlugin.h" + +APMHeliComponent::APMHeliComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent) + : VehicleComponent(vehicle, autopilot, parent) + , _name(tr("Heli")) +{ +} + +QString APMHeliComponent::name(void) const +{ + return _name; +} + +QString APMHeliComponent::description(void) const +{ + return tr("Heli Setup is used to setup parameters which are specific to a helicopter."); +} + +QString APMHeliComponent::iconResource(void) const +{ + return QString(); +} + +bool APMHeliComponent::requiresSetup(void) const +{ + return false; +} + +bool APMHeliComponent::setupComplete(void) const +{ + return true; +} + +QStringList APMHeliComponent::setupCompleteChangedTriggerList(void) const +{ + return QStringList(); +} + +QUrl APMHeliComponent::setupSource(void) const +{ + return QStringLiteral("qrc:/qml/APMHeliComponent.qml"); +} + +QUrl APMHeliComponent::summaryQmlSource(void) const +{ + return QUrl(); +} diff --git a/src/AutoPilotPlugins/APM/APMHeliComponent.h b/src/AutoPilotPlugins/APM/APMHeliComponent.h new file mode 100644 index 0000000000000000000000000000000000000000..cac462523bb80156bbf6e369c4a8e650fb79a4c5 --- /dev/null +++ b/src/AutoPilotPlugins/APM/APMHeliComponent.h @@ -0,0 +1,37 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "VehicleComponent.h" + +class APMHeliComponent : public VehicleComponent +{ + Q_OBJECT + +public: + APMHeliComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent = NULL); + + // Virtuals from VehicleComponent + QStringList setupCompleteChangedTriggerList(void) const override; + + // Virtuals from VehicleComponent + QString name(void) const override; + QString description(void) const override; + QString iconResource(void) const override; + bool requiresSetup(void) const override; + bool setupComplete(void) const override; + QUrl setupSource(void) const override; + QUrl summaryQmlSource(void) const override; + bool allowSetupWhileArmed(void) const override { return true; } + +private: + const QString _name; + QVariantList _summaryItems; +}; diff --git a/src/AutoPilotPlugins/APM/APMHeliComponent.qml b/src/AutoPilotPlugins/APM/APMHeliComponent.qml new file mode 100644 index 0000000000000000000000000000000000000000..cfe1564c522b294ac11c2e536a3ee877202a51dc --- /dev/null +++ b/src/AutoPilotPlugins/APM/APMHeliComponent.qml @@ -0,0 +1,199 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + + +import QtQuick 2.3 +import QtQuick.Controls 1.2 +import QtGraphicalEffects 1.0 +import QtQuick.Layouts 1.2 + +import QGroundControl.FactSystem 1.0 +import QGroundControl.FactControls 1.0 +import QGroundControl.Palette 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.ScreenTools 1.0 + +SetupPage { + id: safetyPage + pageComponent: safetyPageComponent + + Component { + id: safetyPageComponent + + Flow { + id: flowLayout + width: availableWidth + spacing: _margins + + FactPanelController { id: controller; factPanel: safetyPage.viewPanel } + + QGCPalette { id: ggcPal; colorGroupEnabled: true } + + property Fact _failsafeGCSEnable: controller.getParameterFact(-1, "FS_GCS_ENABLE") + property Fact _failsafeBattLowAct: controller.getParameterFact(-1, "r.BATT_FS_LOW_ACT") + property Fact _failsafeBattMah: controller.getParameterFact(-1, "r.BATT_LOW_MAH") + property Fact _failsafeBattVoltage: controller.getParameterFact(-1, "r.BATT_LOW_VOLT") + property Fact _failsafeThrEnable: controller.getParameterFact(-1, "FS_THR_ENABLE") + property Fact _failsafeThrValue: controller.getParameterFact(-1, "FS_THR_VALUE") + + property bool _failsafeBattCritActAvailable: controller.parameterExists(-1, "BATT_FS_CRT_ACT") + property bool _failsafeBatt2LowActAvailable: controller.parameterExists(-1, "BATT2_FS_LOW_ACT") + property bool _failsafeBatt2CritActAvailable: controller.parameterExists(-1, "BATT2_FS_CRT_ACT") + property bool _batt2MonitorAvailable: controller.parameterExists(-1, "BATT2_MONITOR") + property bool _batt2MonitorEnabled: _batt2MonitorAvailable ? _batt2Monitor.rawValue !== 0 : false + + property Fact _failsafeBattCritAct: controller.getParameterFact(-1, "BATT_FS_CRT_ACT", false /* reportMissing */) + property Fact _batt2Monitor: controller.getParameterFact(-1, "BATT2_MONITOR", false /* reportMissing */) + property Fact _failsafeBatt2LowAct: controller.getParameterFact(-1, "BATT2_FS_LOW_ACT", false /* reportMissing */) + property Fact _failsafeBatt2CritAct: controller.getParameterFact(-1, "BATT2_FS_CRT_ACT", false /* reportMissing */) + property Fact _failsafeBatt2Mah: controller.getParameterFact(-1, "BATT2_LOW_MAH", false /* reportMissing */) + property Fact _failsafeBatt2Voltage: controller.getParameterFact(-1, "BATT2_LOW_VOLT", false /* reportMissing */) + + property Fact _fenceAction: controller.getParameterFact(-1, "FENCE_ACTION") + property Fact _fenceAltMax: controller.getParameterFact(-1, "FENCE_ALT_MAX") + property Fact _fenceEnable: controller.getParameterFact(-1, "FENCE_ENABLE") + property Fact _fenceMargin: controller.getParameterFact(-1, "FENCE_MARGIN") + property Fact _fenceRadius: controller.getParameterFact(-1, "FENCE_RADIUS") + property Fact _fenceType: controller.getParameterFact(-1, "FENCE_TYPE") + + property Fact _landSpeedFact: controller.getParameterFact(-1, "LAND_SPEED") + property Fact _rtlAltFact: controller.getParameterFact(-1, "RTL_ALT") + property Fact _rtlLoitTimeFact: controller.getParameterFact(-1, "RTL_LOIT_TIME") + property Fact _rtlAltFinalFact: controller.getParameterFact(-1, "RTL_ALT_FINAL") + + property Fact _armingCheck: controller.getParameterFact(-1, "ARMING_CHECK") + + property real _margins: ScreenTools.defaultFontPixelHeight + property bool _showIcon: !ScreenTools.isTinyScreen + + ExclusiveGroup { id: fenceActionRadioGroup } + ExclusiveGroup { id: landLoiterRadioGroup } + ExclusiveGroup { id: returnAltRadioGroup } + + Column { + spacing: _margins / 2 + + QGCLabel { + text: qsTr("Servo Setup") + font.family: ScreenTools.demiboldFontFamily + } + + Rectangle { + width: servoGrid.x + servoGrid.width + _margins + height: servoGrid.y + servoGrid.height + _margins + color: ggcPal.windowShade + + GridLayout { + id: servoGrid + columns: 6 + + QGCLabel { text: qsTr("Servo") } + QGCLabel { text: qsTr("Function") } + QGCLabel { text: qsTr("Min") } + QGCLabel { text: qsTr("Max") } + QGCLabel { text: qsTr("Trim") } + QGCLabel { text: qsTr("Reversed") } + + QGCLabel { text: qsTr("1") } + FactComboBox { + fact: controller.getParameterFact(-1, "SERVO1_FUNCTION") + indexModel: false + Layout.fillWidth: true + } + FactTextField { + fact: controller.getParameterFact(-1, "SERVO1_MIN") + Layout.fillWidth: true + } + FactTextField { + fact: controller.getParameterFact(-1, "SERVO1_MAX") + Layout.fillWidth: true + } + FactTextField { + fact: controller.getParameterFact(-1, "SERVO1_TRIM") + Layout.fillWidth: true + } + FactCheckBox { + fact: controller.getParameterFact(-1, "SERVO1_REVERSED") + Layout.fillWidth: true + } + + QGCLabel { text: qsTr("2") } + FactComboBox { + fact: controller.getParameterFact(-1, "SERVO2_FUNCTION") + indexModel: false + Layout.fillWidth: true + } + FactTextField { + fact: controller.getParameterFact(-1, "SERVO2_MIN") + Layout.fillWidth: true + } + FactTextField { + fact: controller.getParameterFact(-1, "SERVO2_MAX") + Layout.fillWidth: true + } + FactTextField { + fact: controller.getParameterFact(-1, "SERVO2_TRIM") + Layout.fillWidth: true + } + FactCheckBox { + fact: controller.getParameterFact(-1, "SERVO2_REVERSED") + Layout.fillWidth: true + } + + QGCLabel { text: qsTr("3") } + FactComboBox { + fact: controller.getParameterFact(-1, "SERVO3_FUNCTION") + indexModel: false + Layout.fillWidth: true + } + FactTextField { + fact: controller.getParameterFact(-1, "SERVO3_MIN") + Layout.fillWidth: true + } + FactTextField { + fact: controller.getParameterFact(-1, "SERVO3_MAX") + Layout.fillWidth: true + } + FactTextField { + fact: controller.getParameterFact(-1, "SERVO3_TRIM") + Layout.fillWidth: true + } + FactCheckBox { + fact: controller.getParameterFact(-1, "SERVO3_REVERSED") + Layout.fillWidth: true + } + + QGCLabel { text: qsTr("4") } + FactComboBox { + fact: controller.getParameterFact(-1, "SERVO4_FUNCTION") + indexModel: false + Layout.fillWidth: true + } + FactTextField { + fact: controller.getParameterFact(-1, "SERVO4_MIN") + Layout.fillWidth: true + } + FactTextField { + fact: controller.getParameterFact(-1, "SERVO4_MAX") + Layout.fillWidth: true + } + FactTextField { + fact: controller.getParameterFact(-1, "SERVO4_TRIM") + Layout.fillWidth: true + } + FactCheckBox { + fact: controller.getParameterFact(-1, "SERVO4_REVERSED") + Layout.fillWidth: true + } + } // GridLayout + } // Rectangle + } // Column + } // Flow + } // Component +} // SetupView diff --git a/src/AutoPilotPlugins/APM/APMLightsComponent.qml b/src/AutoPilotPlugins/APM/APMLightsComponent.qml index d12a51fd416a8e4170efb30af114c1c0a949a449..4e42ba06678ac9bda4236f3ee27edf8f3930d710 100644 --- a/src/AutoPilotPlugins/APM/APMLightsComponent.qml +++ b/src/AutoPilotPlugins/APM/APMLightsComponent.qml @@ -132,17 +132,29 @@ SetupPage { ListModel { id: lightsOutModel + // It appears that QGCComboBox can't handle models that don't have a initial item + // after onModelChanged ListElement { text: qsTr("Disabled"); value: 0 } - ListElement { text: qsTr("Channel 5"); value: 5 } - ListElement { text: qsTr("Channel 6"); value: 6 } - ListElement { text: qsTr("Channel 7"); value: 7 } - ListElement { text: qsTr("Channel 8"); value: 8 } - ListElement { text: qsTr("Channel 9"); value: 9 } - ListElement { text: qsTr("Channel 10"); value: 10 } - ListElement { text: qsTr("Channel 11"); value: 11 } - ListElement { text: qsTr("Channel 12"); value: 12 } - ListElement { text: qsTr("Channel 13"); value: 13 } - ListElement { text: qsTr("Channel 14"); value: 14 } + + function update(number) { + // Not enough channels + if(number < 6) { + return + } + for(var i = 5; i <= number; i++) { + var text = qsTr("Channel ") + i + append({"text": text, "value": i}) + } + } + + Component.onCompleted: { + // Number of main outputs + var baseValue = 8 + // Extra outputs + // http://ardupilot.org/copter/docs/parameters.html#brd-pwm-count-auxiliary-pin-config + var brd_pwm_count_value = controller.getParameterFact(-1, "BRD_PWM_COUNT").value + update(8 + (brd_pwm_count_value == 7 ? 3 : brd_pwm_count_value)) + } } Component { diff --git a/src/AutoPilotPlugins/APM/APMLightsComponentSummary.qml b/src/AutoPilotPlugins/APM/APMLightsComponentSummary.qml index 2f06d3b276a0f59c8d8e0dac09b17f797ecc7901..ab5ceebe2af948304029a1969fe26a4957c43d1e 100644 --- a/src/AutoPilotPlugins/APM/APMLightsComponentSummary.qml +++ b/src/AutoPilotPlugins/APM/APMLightsComponentSummary.qml @@ -89,12 +89,12 @@ FactPanel { anchors.fill: parent VehicleSummaryRow { - labelText: qsTr("Lights Output 1:") + labelText: qsTr("Lights Output 1") valueText: lightsOutModel.get(lightsLoader.lights1OutIndex).text } VehicleSummaryRow { - labelText: qsTr("Lights Output 2:") + labelText: qsTr("Lights Output 2") valueText: lightsOutModel.get(lightsLoader.lights2OutIndex).text } } diff --git a/src/AutoPilotPlugins/APM/APMPowerComponent.h b/src/AutoPilotPlugins/APM/APMPowerComponent.h index bf9267723386b7df2d6c6e72bf7355531434fcbd..e033041fba18f84c2c1040b34305e90a61e030dd 100644 --- a/src/AutoPilotPlugins/APM/APMPowerComponent.h +++ b/src/AutoPilotPlugins/APM/APMPowerComponent.h @@ -20,18 +20,19 @@ class APMPowerComponent : public VehicleComponent public: APMPowerComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent = NULL); - // Virtuals from VehicleComponent - QStringList setupCompleteChangedTriggerList(void) const final; + // Overrides from VehicleComponent + QStringList setupCompleteChangedTriggerList(void) const override; // Virtuals from VehicleComponent - QString name (void) const final; - QString description (void) const final; - QString iconResource (void) const final; - bool requiresSetup (void) const final; - bool setupComplete (void) const final; - QUrl setupSource (void) const final; - QUrl summaryQmlSource (void) const final; - + QString name (void) const override; + QString description (void) const override; + QString iconResource (void) const override; + bool requiresSetup (void) const override; + bool setupComplete (void) const override; + QUrl setupSource (void) const override; + QUrl summaryQmlSource (void) const override; + bool allowSetupWhileArmed (void) const override { return true; } + private: const QString _name; QVariantList _summaryItems; diff --git a/src/AutoPilotPlugins/APM/APMPowerComponent.qml b/src/AutoPilotPlugins/APM/APMPowerComponent.qml index 16e036fff74015f2320427fcdc9ea27a7c648913..3bb5028e5696c5185995071c4f7ab57f9c726277 100644 --- a/src/AutoPilotPlugins/APM/APMPowerComponent.qml +++ b/src/AutoPilotPlugins/APM/APMPowerComponent.qml @@ -23,22 +23,137 @@ SetupPage { id: powerPage pageComponent: powerPageComponent + FactPanelController { + id: controller + factPanel: powerPage.viewPanel + } + Component { id: powerPageComponent + Flow { + id: flowLayout + width: availableWidth + spacing: _margins + + property bool _batt2ParamsAvailable: controller.parameterExists(-1, "BATT2_FS_LOW_ACT") + property bool _batt2MonitorAvailable: controller.parameterExists(-1, "BATT2_MONITOR") + property Fact _batt2Monitor: controller.getParameterFact(-1, "BATT2_MONITOR", false /* reportMissing */) + + QGCPalette { id: ggcPal; colorGroupEnabled: true } + + // Battery 1 settings + Column { + spacing: _margins / 2 + + QGCLabel { + text: qsTr("Battery 1") + font.family: ScreenTools.demiboldFontFamily + } + + Rectangle { + width: battery1Loader.x + battery1Loader.width + _margins + height: battery1Loader.y + battery1Loader.height + _margins + color: ggcPal.windowShade + + Loader { + id: battery1Loader + anchors.margins: _margins + anchors.top: parent.top + anchors.left: parent.left + sourceComponent: powerSetupComponent + + property Fact armVoltMin: controller.getParameterFact(-1, "r.ARMING_VOLT_MIN") + property Fact battAmpPerVolt: controller.getParameterFact(-1, "r.BATT_AMP_PERVLT") + property Fact battCapacity: controller.getParameterFact(-1, "BATT_CAPACITY") + property Fact battCurrPin: controller.getParameterFact(-1, "BATT_CURR_PIN") + property Fact battMonitor: controller.getParameterFact(-1, "BATT_MONITOR") + property Fact battVoltMult: controller.getParameterFact(-1, "BATT_VOLT_MULT") + property Fact battVoltPin: controller.getParameterFact(-1, "BATT_VOLT_PIN") + property Fact vehicleVoltage: controller.vehicle.battery.voltage + property Fact vehicleCurrent: controller.vehicle.battery.current + } + } + } + + // Batter2 Monitor settings only - used when only monitor param is available + Column { + spacing: _margins / 2 + visible: _batt2MonitorAvailable && !_batt2ParamsAvailable + + QGCLabel { + text: qsTr("Battery 2") + font.family: ScreenTools.demiboldFontFamily + } + + Rectangle { + width: batt2MonitorRow.x + batt2MonitorRow.width + _margins + height: batt2MonitorRow.y + batt2MonitorRow.height + _margins + color: ggcPal.windowShade + + RowLayout { + id: batt2MonitorRow + anchors.margins: _margins + anchors.top: parent.top + anchors.left: parent.left + spacing: ScreenTools.defaultFontPixelWidth + visible: _batt2MonitorAvailable && !_batt2ParamsAvailable + + QGCLabel { text: qsTr("Battery2 monitor:") } + FactComboBox { + id: monitorCombo + fact: _batt2Monitor + indexModel: false + } + } + } + } + + // Battery 2 settings - Used when full params are available + Column { + spacing: _margins / 2 + visible: _batt2ParamsAvailable + + QGCLabel { + text: qsTr("Battery 2") + font.family: ScreenTools.demiboldFontFamily + } + + Rectangle { + width: battery2Loader.x + battery2Loader.width + _margins + height: battery2Loader.y + battery2Loader.height + _margins + color: ggcPal.windowShade + + Loader { + id: battery2Loader + anchors.margins: _margins + anchors.top: parent.top + anchors.left: parent.left + sourceComponent: _batt2ParamsAvailable ? powerSetupComponent : undefined + + property Fact armVoltMin: controller.getParameterFact(-1, "r.ARMING_VOLT2_MIN", false /* reportMissing */) + property Fact battAmpPerVolt: controller.getParameterFact(-1, "r.BATT2_AMP_PERVLT", false /* reportMissing */) + property Fact battCapacity: controller.getParameterFact(-1, "BATT2_CAPACITY", false /* reportMissing */) + property Fact battCurrPin: controller.getParameterFact(-1, "BATT2_CURR_PIN", false /* reportMissing */) + property Fact battMonitor: controller.getParameterFact(-1, "BATT2_MONITOR", false /* reportMissing */) + property Fact battVoltMult: controller.getParameterFact(-1, "BATT2_VOLT_MULT", false /* reportMissing */) + property Fact battVoltPin: controller.getParameterFact(-1, "BATT2_VOLT_PIN", false /* reportMissing */) + property Fact vehicleVoltage: controller.vehicle.battery2.voltage + property Fact vehicleCurrent: controller.vehicle.battery2.current + } + } + } + } // Flow + } // Component - powerPageComponent + + Component { + id: powerSetupComponent + Column { spacing: _margins - property Fact armVoltMin: controller.getParameterFact(-1, "r.ARMING_VOLT_MIN") - property Fact battAmpPerVolt: controller.getParameterFact(-1, "BATT_AMP_PERVOLT") - property Fact battCapacity: controller.getParameterFact(-1, "BATT_CAPACITY") - property Fact battCurrPin: controller.getParameterFact(-1, "BATT_CURR_PIN") - property Fact battMonitor: controller.getParameterFact(-1, "BATT_MONITOR") - property Fact battVoltMult: controller.getParameterFact(-1, "BATT_VOLT_MULT") - property Fact battVoltPin: controller.getParameterFact(-1, "BATT_VOLT_PIN") - property real _margins: ScreenTools.defaultFontPixelHeight / 2 - property bool _showAdvanced: sensorCombo.currentIndex == sensorModel.count - 1 + property bool _showAdvanced: sensorCombo.currentIndex === sensorModel.count - 1 property real _fieldWidth: ScreenTools.defaultFontPixelWidth * 25 Component.onCompleted: calcSensor() @@ -58,11 +173,6 @@ SetupPage { QGCPalette { id: palette; colorGroupEnabled: true } - FactPanelController { - id: controller - factPanel: powerPage.viewPanel - } - ListModel { id: sensorModel @@ -95,121 +205,6 @@ SetupPage { } } - Component { - id: calcVoltageMultiplierDlgComponent - - QGCViewDialog { - id: calcVoltageMultiplierDlg - - QGCFlickable { - anchors.fill: parent - contentHeight: column.height - contentWidth: column.width - - Column { - id: column - width: calcVoltageMultiplierDlg.width - spacing: ScreenTools.defaultFontPixelHeight - - QGCLabel { - width: parent.width - wrapMode: Text.WordWrap - text: qsTr("Measure battery voltage using an external voltmeter and enter the value below. Click Calculate to set the new voltage multiplier.") - } - - Grid { - columns: 2 - spacing: ScreenTools.defaultFontPixelHeight / 2 - verticalItemAlignment: Grid.AlignVCenter - - QGCLabel { - text: qsTr("Measured voltage:") - } - QGCTextField { id: measuredVoltage } - - QGCLabel { text: qsTr("Vehicle voltage:") } - QGCLabel { text: controller.vehicle.battery.voltage.valueString } - - QGCLabel { text: qsTr("Voltage multiplier:") } - FactLabel { fact: battVoltMult } - } - - QGCButton { - text: "Calculate" - - onClicked: { - var measuredVoltageValue = parseFloat(measuredVoltage.text) - if (measuredVoltageValue == 0 || isNaN(measuredVoltageValue)) { - return - } - var newVoltageMultiplier = (measuredVoltageValue * battVoltMult.value) / controller.vehicle.battery.voltage.value - if (newVoltageMultiplier > 0) { - battVoltMult.value = newVoltageMultiplier - } - } - } - } // Column - } // QGCFlickable - } // QGCViewDialog - } // Component - calcVoltageMultiplierDlgComponent - - Component { - id: calcAmpsPerVoltDlgComponent - - QGCViewDialog { - id: calcAmpsPerVoltDlg - - QGCFlickable { - anchors.fill: parent - contentHeight: column.height - contentWidth: column.width - - Column { - id: column - width: calcAmpsPerVoltDlg.width - spacing: ScreenTools.defaultFontPixelHeight - - QGCLabel { - width: parent.width - wrapMode: Text.WordWrap - text: qsTr("Measure current draw using an external current meter and enter the value below. Click Calculate to set the new amps per volt value.") - } - - Grid { - columns: 2 - spacing: ScreenTools.defaultFontPixelHeight / 2 - verticalItemAlignment: Grid.AlignVCenter - - QGCLabel { - text: qsTr("Measured current:") - } - QGCTextField { id: measuredCurrent } - - QGCLabel { text: qsTr("Vehicle current:") } - QGCLabel { text: controller.vehicle.battery.current.valueString } - - QGCLabel { text: qsTr("Amps per volt:") } - FactLabel { fact: battAmpPerVolt } - } - - QGCButton { - text: "Calculate" - - onClicked: { - var measuredCurrentValue = parseFloat(measuredCurrent.text) - if (measuredCurrentValue == 0) { - return - } - var newAmpsPerVolt = (measuredCurrentValue * battAmpPerVolt.value) / controller.vehicle.battery.current.value - if (newAmpsPerVolt != 0) { - battAmpPerVolt.value = newAmpsPerVolt - } - } - } - } // Column - } // QGCFlickable - } // QGCViewDialog - } // Component - calcAmpsPerVoltDlgComponent GridLayout { columns: 3 @@ -351,9 +346,125 @@ SetupPage { font.pointSize: ScreenTools.smallFontPointSize wrapMode: Text.WordWrap text: qsTr("If the current draw reported by the vehicle is largely different than the current read externally using a current meter you can adjust the amps per volt value to correct this. Click the Calculate button for help with calculating a new value.") - visible: _showAdvanced + visible: _showAdvanced } } // GridLayout } // Column - } // Component + } // Component - powerSetupComponent + + Component { + id: calcVoltageMultiplierDlgComponent + + QGCViewDialog { + id: calcVoltageMultiplierDlg + + QGCFlickable { + anchors.fill: parent + contentHeight: column.height + contentWidth: column.width + + Column { + id: column + width: calcVoltageMultiplierDlg.width + spacing: ScreenTools.defaultFontPixelHeight + + QGCLabel { + width: parent.width + wrapMode: Text.WordWrap + text: qsTr("Measure battery voltage using an external voltmeter and enter the value below. Click Calculate to set the new voltage multiplier.") + } + + Grid { + columns: 2 + spacing: ScreenTools.defaultFontPixelHeight / 2 + verticalItemAlignment: Grid.AlignVCenter + + QGCLabel { + text: qsTr("Measured voltage:") + } + QGCTextField { id: measuredVoltage } + + QGCLabel { text: qsTr("Vehicle voltage:") } + QGCLabel { text: vehicleVoltage.valueString } + + QGCLabel { text: qsTr("Voltage multiplier:") } + FactLabel { fact: battVoltMult } + } + + QGCButton { + text: "Calculate" + + onClicked: { + var measuredVoltageValue = parseFloat(measuredVoltage.text) + if (measuredVoltageValue == 0 || isNaN(measuredVoltageValue)) { + return + } + var newVoltageMultiplier = (measuredVoltageValue * battVoltMult.value) / vehicleVoltage.value + if (newVoltageMultiplier > 0) { + battVoltMult.value = newVoltageMultiplier + } + } + } + } // Column + } // QGCFlickable + } // QGCViewDialog + } // Component - calcVoltageMultiplierDlgComponent + + Component { + id: calcAmpsPerVoltDlgComponent + + QGCViewDialog { + id: calcAmpsPerVoltDlg + + QGCFlickable { + anchors.fill: parent + contentHeight: column.height + contentWidth: column.width + + Column { + id: column + width: calcAmpsPerVoltDlg.width + spacing: ScreenTools.defaultFontPixelHeight + + QGCLabel { + width: parent.width + wrapMode: Text.WordWrap + text: qsTr("Measure current draw using an external current meter and enter the value below. Click Calculate to set the new amps per volt value.") + } + + Grid { + columns: 2 + spacing: ScreenTools.defaultFontPixelHeight / 2 + verticalItemAlignment: Grid.AlignVCenter + + QGCLabel { + text: qsTr("Measured current:") + } + QGCTextField { id: measuredCurrent } + + QGCLabel { text: qsTr("Vehicle current:") } + QGCLabel { text: vehicleCurrent.valueString } + + QGCLabel { text: qsTr("Amps per volt:") } + FactLabel { fact: battAmpPerVolt } + } + + QGCButton { + text: "Calculate" + + onClicked: { + var measuredCurrentValue = parseFloat(measuredCurrent.text) + if (measuredCurrentValue == 0) { + return + } + var newAmpsPerVolt = (measuredCurrentValue * battAmpPerVolt.value) / vehicleCurrent.value + if (newAmpsPerVolt != 0) { + battAmpPerVolt.value = newAmpsPerVolt + } + } + } + } // Column + } // QGCFlickable + } // QGCViewDialog + } // Component - calcAmpsPerVoltDlgComponent } // SetupPage diff --git a/src/AutoPilotPlugins/APM/APMPowerComponentSummary.qml b/src/AutoPilotPlugins/APM/APMPowerComponentSummary.qml index 373d6ffa7d6bc352fd063fcfe81ab22d6a38f6df..ab23561769b0914cc8f6c917628989b70d3f35c5 100644 --- a/src/AutoPilotPlugins/APM/APMPowerComponentSummary.qml +++ b/src/AutoPilotPlugins/APM/APMPowerComponentSummary.qml @@ -24,20 +24,37 @@ FactPanel { QGCPalette { id: qgcPal; colorGroupEnabled: enabled } FactPanelController { id: controller; factPanel: panel } - property Fact battCapacity: controller.getParameterFact(-1, "BATT_CAPACITY") - property Fact battMonitor: controller.getParameterFact(-1, "BATT_MONITOR") + property bool _batt2MonitorAvailable: controller.parameterExists(-1, "BATT2_MONITOR") + property bool _batt2CapacityAvailable: controller.parameterExists(-1, "BATT2_CAPACITY") + + property Fact _battCapacity: controller.getParameterFact(-1, "BATT_CAPACITY") + property Fact _batt2Capacity: controller.getParameterFact(-1, "BATT2_CAPACITY", false /* reportMissing */) + property Fact _battMonitor: controller.getParameterFact(-1, "BATT_MONITOR") + property Fact _batt2Monitor: controller.getParameterFact(-1, "BATT2_MONITOR", false /* reportMissing */) Column { anchors.fill: parent VehicleSummaryRow { - labelText: qsTr("Battery monitor:") - valueText: battMonitor.enumStringValue + labelText: qsTr("Battery monitor") + valueText: _battMonitor.enumStringValue + } + + VehicleSummaryRow { + labelText: qsTr("Battery capacity") + valueText: _battCapacity.valueString + " " + _battCapacity.units + } + + VehicleSummaryRow { + labelText: qsTr("Battery2 monitor") + valueText: _batt2MonitorAvailable ? _batt2Monitor.enumStringValue : "" + visible: _batt2MonitorAvailable } VehicleSummaryRow { - labelText: qsTr("Battery capacity:") - valueText: battCapacity.valueString + " " + battCapacity.units + labelText: qsTr("Battery2 capacity") + valueText: _batt2CapacityAvailable ? _batt2Capacity.valueString + " " + _battCapacity.units : "" + visible: _batt2CapacityAvailable } } } diff --git a/src/AutoPilotPlugins/APM/APMRadioComponentSummary.qml b/src/AutoPilotPlugins/APM/APMRadioComponentSummary.qml index 5433dc9971fd14e827f1d817e4199b43a14c5b86..47937ec2d32237baabdd4db3fe3c4e32f213b4d8 100644 --- a/src/AutoPilotPlugins/APM/APMRadioComponentSummary.qml +++ b/src/AutoPilotPlugins/APM/APMRadioComponentSummary.qml @@ -23,22 +23,22 @@ FactPanel { anchors.fill: parent VehicleSummaryRow { - labelText: qsTr("Roll:") + labelText: qsTr("Roll") valueText: mapRollFact.value == 0 ? qsTr("Setup required") : qsTr("Channel %1").arg(mapRollFact.valueString) } VehicleSummaryRow { - labelText: qsTr("Pitch:") + labelText: qsTr("Pitch") valueText: mapPitchFact.value == 0 ? qsTr("Setup required") : qsTr("Channel %1").arg(mapPitchFact.valueString) } VehicleSummaryRow { - labelText: qsTr("Yaw:") + labelText: qsTr("Yaw") valueText: mapYawFact.value == 0 ? qsTr("Setup required") : qsTr("Channel %1").arg(mapYawFact.valueString) } VehicleSummaryRow { - labelText: qsTr("Throttle:") + labelText: qsTr("Throttle") valueText: mapThrottleFact.value == 0 ? qsTr("Setup required") : qsTr("Channel %1").arg(mapThrottleFact.valueString) } } diff --git a/src/AutoPilotPlugins/APM/APMSafetyComponentCopter.qml b/src/AutoPilotPlugins/APM/APMSafetyComponentCopter.qml index 24f5c0752aa87a94d3078ee3bd8f99fcc385edc7..fdcf50ce3542c8c1eb811d6645a01dd0f62ff491 100644 --- a/src/AutoPilotPlugins/APM/APMSafetyComponentCopter.qml +++ b/src/AutoPilotPlugins/APM/APMSafetyComponentCopter.qml @@ -11,6 +11,7 @@ import QtQuick 2.3 import QtQuick.Controls 1.2 import QtGraphicalEffects 1.0 +import QtQuick.Layouts 1.2 import QGroundControl.FactSystem 1.0 import QGroundControl.FactControls 1.0 @@ -34,12 +35,25 @@ SetupPage { QGCPalette { id: ggcPal; colorGroupEnabled: true } - property Fact _failsafeGCSEnable: controller.getParameterFact(-1, "FS_GCS_ENABLE") - property Fact _failsafeBattEnable: controller.getParameterFact(-1, "FS_BATT_ENABLE") - property Fact _failsafeBattMah: controller.getParameterFact(-1, "FS_BATT_MAH") - property Fact _failsafeBattVoltage: controller.getParameterFact(-1, "FS_BATT_VOLTAGE") - property Fact _failsafeThrEnable: controller.getParameterFact(-1, "FS_THR_ENABLE") - property Fact _failsafeThrValue: controller.getParameterFact(-1, "FS_THR_VALUE") + property Fact _failsafeGCSEnable: controller.getParameterFact(-1, "FS_GCS_ENABLE") + property Fact _failsafeBattLowAct: controller.getParameterFact(-1, "r.BATT_FS_LOW_ACT") + property Fact _failsafeBattMah: controller.getParameterFact(-1, "r.BATT_LOW_MAH") + property Fact _failsafeBattVoltage: controller.getParameterFact(-1, "r.BATT_LOW_VOLT") + property Fact _failsafeThrEnable: controller.getParameterFact(-1, "FS_THR_ENABLE") + property Fact _failsafeThrValue: controller.getParameterFact(-1, "FS_THR_VALUE") + + property bool _failsafeBattCritActAvailable: controller.parameterExists(-1, "BATT_FS_CRT_ACT") + property bool _failsafeBatt2LowActAvailable: controller.parameterExists(-1, "BATT2_FS_LOW_ACT") + property bool _failsafeBatt2CritActAvailable: controller.parameterExists(-1, "BATT2_FS_CRT_ACT") + property bool _batt2MonitorAvailable: controller.parameterExists(-1, "BATT2_MONITOR") + property bool _batt2MonitorEnabled: _batt2MonitorAvailable ? _batt2Monitor.rawValue !== 0 : false + + property Fact _failsafeBattCritAct: controller.getParameterFact(-1, "BATT_FS_CRT_ACT", false /* reportMissing */) + property Fact _batt2Monitor: controller.getParameterFact(-1, "BATT2_MONITOR", false /* reportMissing */) + property Fact _failsafeBatt2LowAct: controller.getParameterFact(-1, "BATT2_FS_LOW_ACT", false /* reportMissing */) + property Fact _failsafeBatt2CritAct: controller.getParameterFact(-1, "BATT2_FS_CRT_ACT", false /* reportMissing */) + property Fact _failsafeBatt2Mah: controller.getParameterFact(-1, "BATT2_LOW_MAH", false /* reportMissing */) + property Fact _failsafeBatt2Voltage: controller.getParameterFact(-1, "BATT2_LOW_VOLT", false /* reportMissing */) property Fact _fenceAction: controller.getParameterFact(-1, "FENCE_ACTION") property Fact _fenceAltMax: controller.getParameterFact(-1, "FENCE_ALT_MAX") @@ -66,135 +80,193 @@ SetupPage { spacing: _margins / 2 QGCLabel { - id: failsafeLabel - text: qsTr("Failsafe Triggers") + text: qsTr("Battery Failsafe Triggers") font.family: ScreenTools.demiboldFontFamily } Rectangle { - id: failsafeSettings - width: throttleEnableCombo.x + throttleEnableCombo.width + _margins - height: mahField.y + mahField.height + _margins + width: batteryFailsafeColumn.x + batteryFailsafeColumn.width + _margins + height: batteryFailsafeColumn.y + batteryFailsafeColumn.height + _margins color: ggcPal.windowShade - QGCLabel { - id: gcsEnableLabel + Column { + id: batteryFailsafeColumn anchors.margins: _margins + anchors.top: parent.top anchors.left: parent.left - anchors.baseline: gcsEnableCombo.baseline - text: qsTr("Ground Station failsafe:") - } + spacing: _margins + + GridLayout { + id: gridLayout + columnSpacing: _margins + rowSpacing: _margins + columns: 2 + QGCLabel { text: qsTr("Battery low action:") } + FactComboBox { + fact: _failsafeBattLowAct + indexModel: false + Layout.fillWidth: true + } - FactComboBox { - id: gcsEnableCombo - anchors.topMargin: _margins - anchors.leftMargin: _margins - anchors.left: gcsEnableLabel.right - anchors.top: parent.top - width: voltageField.width - fact: _failsafeGCSEnable - indexModel: false - } + QGCLabel { + text: qsTr("Battery critical action:") + visible: _failsafeBattCritActAvailable + } + FactComboBox { + fact: _failsafeBattCritAct + visible: _failsafeBattCritActAvailable + indexModel: false + Layout.fillWidth: true + } - QGCLabel { - id: throttleEnableLabel - anchors.margins: _margins - anchors.left: parent.left - anchors.baseline: throttleEnableCombo.baseline - text: qsTr("Throttle failsafe:") - } + QGCCheckBox { + text: qsTr("Voltage threshold:") + checked: _failsafeBattVoltage.value != 0 + onClicked: _failsafeBattVoltage.value = checked ? 10.5 : 0 + } + FactTextField { + fact: _failsafeBattVoltage + showUnits: true + Layout.fillWidth: true + } - QGCComboBox { - id: throttleEnableCombo - anchors.topMargin: _margins - anchors.left: gcsEnableCombo.left - anchors.top: gcsEnableCombo.bottom - width: voltageField.width - model: [qsTr("Disabled"), qsTr("Always RTL"), - qsTr("Continue with Mission in Auto Mode"), qsTr("Always Land")] - currentIndex: _failsafeThrEnable.value - - onActivated: _failsafeThrEnable.value = index - } + QGCCheckBox { + text: qsTr("MAH threshold:") + checked: _failsafeBattMah.value != 0 + onClicked: _failsafeBattMah.value = checked ? 600 : 0 + } + FactTextField { + fact: _failsafeBattMah + showUnits: true + Layout.fillWidth: true + } + } // GridLayout + } // Column + } // Rectangle + } // Column - Battery Failsafe Settings - QGCLabel { - id: throttlePWMLabel - anchors.margins: _margins - anchors.left: parent.left - anchors.baseline: throttlePWMField.baseline - text: qsTr("PWM threshold:") - } + Column { + spacing: _margins / 2 + visible: _batt2MonitorEnabled && _failsafeBatt2LowActAvailable - FactTextField { - id: throttlePWMField - anchors.topMargin: _margins / 2 - anchors.left: gcsEnableCombo.left - anchors.top: throttleEnableCombo.bottom - fact: _failsafeThrValue - showUnits: true - } + QGCLabel { + text: qsTr("Battery2 Failsafe Triggers") + font.family: ScreenTools.demiboldFontFamily + } - QGCLabel { - id: batteryEnableLabel + Rectangle { + id: failsafeSettings + width: battery2FailsafeColumn.x + battery2FailsafeColumn.width + _margins + height: battery2FailsafeColumn.y + battery2FailsafeColumn.height + _margins + color: ggcPal.windowShade + + Column { + id: battery2FailsafeColumn anchors.margins: _margins + anchors.top: parent.top anchors.left: parent.left - anchors.baseline: batteryEnableCombo.baseline - text: qsTr("Battery failsafe:") - } + spacing: _margins + + GridLayout { + columnSpacing: _margins + rowSpacing: _margins + columns: 2 + visible: _batt2MonitorEnabled && _failsafeBatt2LowActAvailable + + QGCLabel { text: qsTr("Battery low action:") } + FactComboBox { + fact: _failsafeBatt2LowAct + indexModel: false + Layout.fillWidth: true + } - QGCComboBox { - id: batteryEnableCombo - anchors.topMargin: _margins - anchors.left: gcsEnableCombo.left - anchors.top: throttlePWMField.bottom - width: voltageField.width - model: [qsTr("Disabled"), qsTr("Land"), qsTr("Return to Launch")] - currentIndex: _failsafeBattEnable.value + QGCLabel { + text: qsTr("Battery critical action:") + } + FactComboBox { + fact: _failsafeBatt2CritAct + indexModel: false + Layout.fillWidth: true + } - onActivated: _failsafeBattEnable.value = index - } + QGCCheckBox { + text: qsTr("Voltage threshold:") + checked: _failsafeBatt2Voltage.value != 0 + onClicked: _failsafeBatt2Voltage.value = checked ? 10.5 : 0 + } + FactTextField { + fact: _failsafeBatt2Voltage + showUnits: true + Layout.fillWidth: true + } - QGCCheckBox { - id: voltageLabel - anchors.margins: _margins - anchors.left: parent.left - anchors.baseline: voltageField.baseline - text: qsTr("Voltage threshold:") - checked: _failsafeBattVoltage.value != 0 + QGCCheckBox { + text: qsTr("MAH threshold:") + checked: _failsafeBatt2Mah.value != 0 + onClicked: _failsafeBatt2Mah.value = checked ? 600 : 0 + } + FactTextField { + fact: _failsafeBatt2Mah + showUnits: true + Layout.fillWidth: true + } + } // GridLayout + } // Column + } // Rectangle + } // Column - Battery2 Failsafe Settings - onClicked: _failsafeBattVoltage.value = checked ? 10.5 : 0 - } + Column { + spacing: _margins / 2 - FactTextField { - id: voltageField - anchors.topMargin: _margins / 2 - anchors.left: gcsEnableCombo.left - anchors.top: batteryEnableCombo.bottom - fact: _failsafeBattVoltage - showUnits: true - } + QGCLabel { + text: qsTr("General Failsafe Triggers") + font.family: ScreenTools.demiboldFontFamily + } - QGCCheckBox { - id: mahLabel + Rectangle { + width: generalFailsafeColumn.x + generalFailsafeColumn.width + _margins + height: generalFailsafeColumn.y + generalFailsafeColumn.height + _margins + color: ggcPal.windowShade + + Column { + id: generalFailsafeColumn anchors.margins: _margins + anchors.top: parent.top anchors.left: parent.left - anchors.baseline: mahField.baseline - text: qsTr("MAH threshold:") - checked: _failsafeBattMah.value != 0 + spacing: _margins + + GridLayout { + columnSpacing: _margins + rowSpacing: _margins + columns: 2 + + QGCLabel { text: qsTr("Ground Station failsafe:") } + FactComboBox { + fact: _failsafeGCSEnable + indexModel: false + Layout.fillWidth: true + } - onClicked: _failsafeBattMah.value = checked ? 600 : 0 - } + QGCLabel { text: qsTr("Throttle failsafe:") } + QGCComboBox { + model: [qsTr("Disabled"), qsTr("Always RTL"), + qsTr("Continue with Mission in Auto Mode"), qsTr("Always Land")] + currentIndex: _failsafeThrEnable.value + Layout.fillWidth: true - FactTextField { - id: mahField - anchors.topMargin: _margins / 2 - anchors.left: gcsEnableCombo.left - anchors.top: voltageField.bottom - fact: _failsafeBattMah - showUnits: true - } + onActivated: _failsafeThrEnable.value = index + } + + QGCLabel { text: qsTr("PWM threshold:") } + FactTextField { + fact: _failsafeThrValue + showUnits: true + Layout.fillWidth: true + } + } // GridLayout + } // Column } // Rectangle - Failsafe Settings - } // Column - Failsafe Settings + } // Column - General Failsafe Settings Column { spacing: _margins / 2 diff --git a/src/AutoPilotPlugins/APM/APMSafetyComponentPlane.qml b/src/AutoPilotPlugins/APM/APMSafetyComponentPlane.qml index 700603914711e3490ba84eaba9df72801d23fbd1..36f9027970d4d4baa0afd122eeb79a6a4ac5070e 100644 --- a/src/AutoPilotPlugins/APM/APMSafetyComponentPlane.qml +++ b/src/AutoPilotPlugins/APM/APMSafetyComponentPlane.qml @@ -34,8 +34,8 @@ SetupPage { QGCPalette { id: palette; colorGroupEnabled: true } - property Fact _failsafeBattMah: controller.getParameterFact(-1, "FS_BATT_MAH") - property Fact _failsafeBattVoltage: controller.getParameterFact(-1, "FS_BATT_VOLTAGE") + property Fact _failsafeBattMah: controller.getParameterFact(-1, "r.BATT_LOW_MAH") + property Fact _failsafeBattVoltage: controller.getParameterFact(-1, "r.BATT_LOW_VOLT") property Fact _failsafeThrEnable: controller.getParameterFact(-1, "THR_FAILSAFE") property Fact _failsafeThrValue: controller.getParameterFact(-1, "THR_FS_VALUE") property Fact _failsafeGCSEnable: controller.getParameterFact(-1, "FS_GCS_ENABL") diff --git a/src/AutoPilotPlugins/APM/APMSafetyComponentSummaryCopter.qml b/src/AutoPilotPlugins/APM/APMSafetyComponentSummaryCopter.qml index 785147b391bfd18a08283baa67daaf8f12b10c35..a08b9c082dd33c1f1befea2414d3c91b3e6578a7 100644 --- a/src/AutoPilotPlugins/APM/APMSafetyComponentSummaryCopter.qml +++ b/src/AutoPilotPlugins/APM/APMSafetyComponentSummaryCopter.qml @@ -14,7 +14,7 @@ FactPanel { QGCPalette { id: qgcPal; colorGroupEnabled: enabled } FactPanelController { id: controller; factPanel: panel } - property Fact _failsafeBattEnable: controller.getParameterFact(-1, "FS_BATT_ENABLE") + property Fact _failsafeBattLowAct: controller.getParameterFact(-1, "r.BATT_FS_LOW_ACT") property Fact _failsafeThrEnable: controller.getParameterFact(-1, "FS_THR_ENABLE") property Fact _fenceAction: controller.getParameterFact(-1, "FENCE_ACTION") @@ -28,60 +28,14 @@ FactPanel { property Fact _armingCheck: controller.getParameterFact(-1, "ARMING_CHECK") - property string _failsafeBattEnableText - property string _failsafeThrEnableText + property bool _failsafeBattCritActAvailable: controller.parameterExists(-1, "BATT_FS_CRT_ACT") + property bool _failsafeBatt2LowActAvailable: controller.parameterExists(-1, "BATT2_FS_LOW_ACT") + property bool _failsafeBatt2CritActAvailable: controller.parameterExists(-1, "BATT2_FS_CRT_ACT") - Component.onCompleted: { - setFailsafeBattEnableText() - setFailsafeThrEnableText() - } - - Connections { - target: _failsafeBattEnable - - onValueChanged: setFailsafeBattEnableText() - } - - Connections { - target: _failsafeThrEnable - - onValueChanged: setFailsafeThrEnableText() - } - - function setFailsafeThrEnableText() { - switch (_failsafeThrEnable.value) { - case 0: - _failsafeThrEnableText = qsTr("Disabled") - break - case 1: - _failsafeThrEnableText = qsTr("Always RTL") - break - case 2: - _failsafeThrEnableText = qsTr("Continue with Mission in Auto Mode") - break - case 3: - _failsafeThrEnableText = qsTr("Always Land") - break - default: - _failsafeThrEnableText = qsTr("Unknown") - } - } - - function setFailsafeBattEnableText() { - switch (_failsafeBattEnable.value) { - case 0: - _failsafeBattEnableText = qsTr("Disabled") - break - case 1: - _failsafeBattEnableText = qsTr("Land") - break - case 2: - _failsafeBattEnableText = qsTr("Return to Launch") - break - default: - _failsafeThrEnableText = qsTr("Unknown") - } - } + property Fact _failsafeBattCritAct: controller.getParameterFact(-1, "BATT_FS_CRT_ACT", false /* reportMissing */) + property Fact _batt2Monitor: controller.getParameterFact(-1, "BATT2_MONITOR", false /* reportMissing */) + property Fact _failsafeBatt2LowAct: controller.getParameterFact(-1, "BATT2_FS_LOW_ACT", false /* reportMissing */) + property Fact _failsafeBatt2CritAct: controller.getParameterFact(-1, "BATT2_FS_CRT_ACT", false /* reportMissing */) Column { anchors.fill: parent @@ -93,12 +47,30 @@ FactPanel { VehicleSummaryRow { labelText: qsTr("Throttle failsafe:") - valueText: _failsafeThrEnableText + valueText: _failsafeBattLowAct.enumStringValue + } + + VehicleSummaryRow { + labelText: qsTr("Batt low failsafe:") + valueText: _failsafeBattLowAct.enumStringValue + } + + VehicleSummaryRow { + labelText: qsTr("Batt critical failsafe:") + valueText: _failsafeBattCritActAvailable ? _failsafeBattCritAct.enumStringValue : "" + visible: _failsafeBattCritActAvailable + } + + VehicleSummaryRow { + labelText: qsTr("Batt2 low failsafe:") + valueText: _failsafeBatt2LowActAvailable ? _failsafeBatt2LowAct.enumStringValue : "" + visible: _failsafeBatt2LowActAvailable } VehicleSummaryRow { - labelText: qsTr("Battery failsafe:") - valueText: _failsafeBattEnableText + labelText: qsTr("Batt2 critical failsafe:") + valueText: _failsafeBatt2CritActAvailable ? _failsafeBatt2CritAct.enumStringValue : "" + visible: _failsafeBatt2CritActAvailable } VehicleSummaryRow { diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentSummary.qml b/src/AutoPilotPlugins/APM/APMSensorsComponentSummary.qml index bd209e9522ac1d2749f7a48c17a7544d55a2ec92..10fc56d14c839335f4af7fb8df0ee495b0df6b5c 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponentSummary.qml +++ b/src/AutoPilotPlugins/APM/APMSensorsComponentSummary.qml @@ -34,7 +34,7 @@ FactPanel { model: 3 VehicleSummaryRow { - labelText: qsTr("Compass ") + (index + 1) + ":" + labelText: qsTr("Compass ") + (index + 1) + "" valueText: sensorParams.rgCompassAvailable[index] ? (sensorParams.rgCompassCalibrated[index] ? (sensorParams.rgCompassPrimary[index] ? "Primary" : "Secondary") + @@ -47,7 +47,7 @@ FactPanel { } VehicleSummaryRow { - labelText: qsTr("Accelerometer(s):") + labelText: qsTr("Accelerometer(s)") valueText: controller.accelSetupNeeded ? qsTr("Setup required") : qsTr("Ready") } } diff --git a/src/AutoPilotPlugins/APM/APMSubFrameComponentSummary.qml b/src/AutoPilotPlugins/APM/APMSubFrameComponentSummary.qml index cdcd7f85a919dae0273ff27d6c16c15a4c9c6d3b..9c2d3a364c5cea5e38e131747ab7c7baab9e5239 100644 --- a/src/AutoPilotPlugins/APM/APMSubFrameComponentSummary.qml +++ b/src/AutoPilotPlugins/APM/APMSubFrameComponentSummary.qml @@ -45,17 +45,17 @@ FactPanel { anchors.fill: parent VehicleSummaryRow { id: nameRow; - labelText: qsTr("Frame Type:") + labelText: qsTr("Frame Type") valueText: frameName() } VehicleSummaryRow { - labelText: qsTr("Firmware Version:") + labelText: qsTr("Firmware Version") valueText: activeVehicle.firmwareMajorVersion == -1 ? qsTr("Unknown") : activeVehicle.firmwareMajorVersion + "." + activeVehicle.firmwareMinorVersion + "." + activeVehicle.firmwarePatchVersion + " " + activeVehicle.firmwareVersionTypeString } VehicleSummaryRow { - labelText: qsTr("Git Revision:") + labelText: qsTr("Git Revision") valueText: activeVehicle.gitHash == -1 ? qsTr("Unknown") : activeVehicle.gitHash } } diff --git a/src/AutoPilotPlugins/APM/APMTuningComponentCopter.qml b/src/AutoPilotPlugins/APM/APMTuningComponentCopter.qml index 64faea8acc0fd1d4a2d6c99b2ee25b7e2e6141eb..bfe22008f76eae6c413e0a8a83466a8471f3bfd7 100644 --- a/src/AutoPilotPlugins/APM/APMTuningComponentCopter.qml +++ b/src/AutoPilotPlugins/APM/APMTuningComponentCopter.qml @@ -32,20 +32,16 @@ SetupPage { QGCPalette { id: palette; colorGroupEnabled: true } - // Older firmwares use THR_MODE, newer use MOT_THST_HOVER - property bool _throttleMidExists: controller.parameterExists(-1, "THR_MID") - property Fact _hoverTuneParam: controller.getParameterFact(-1, _throttleMidExists ? "THR_MID" : "MOT_THST_HOVER") - property real _hoverTuneMin: _throttleMidExists ? 200 : 0 - property real _hoverTuneMax: _throttleMidExists ? 800 : 1 - property real _hoverTuneStep: _throttleMidExists ? 10 : 0.01 - - property Fact _rcFeel: controller.getParameterFact(-1, "RC_FEEL_RP") - property Fact _rateRollP: controller.getParameterFact(-1, "r.ATC_RAT_RLL_P") - property Fact _rateRollI: controller.getParameterFact(-1, "r.ATC_RAT_RLL_I") - property Fact _ratePitchP: controller.getParameterFact(-1, "r.ATC_RAT_PIT_P") - property Fact _ratePitchI: controller.getParameterFact(-1, "r.ATC_RAT_PIT_I") - property Fact _rateClimbP: controller.getParameterFact(-1, "ACCEL_Z_P") - property Fact _rateClimbI: controller.getParameterFact(-1, "ACCEL_Z_I") + property bool _rcFeelAvailable: controller.parameterExists(-1, "RC_FEEL") + property bool _atcInputTCAvailable: controller.parameterExists(-1, "ATC_INPUT_TC") + property Fact _rcFeel: controller.getParameterFact(-1, "RC_FEEL", false) + property Fact _atcInputTC: controller.getParameterFact(-1, "ATC_INPUT_TC", false) + property Fact _rateRollP: controller.getParameterFact(-1, "r.ATC_RAT_RLL_P") + property Fact _rateRollI: controller.getParameterFact(-1, "r.ATC_RAT_RLL_I") + property Fact _ratePitchP: controller.getParameterFact(-1, "r.ATC_RAT_PIT_P") + property Fact _ratePitchI: controller.getParameterFact(-1, "r.ATC_RAT_PIT_I") + property Fact _rateClimbP: controller.getParameterFact(-1, "r.PSC_ACCZ_P") + property Fact _rateClimbI: controller.getParameterFact(-1, "r.PSC_ACCZ_I") property Fact _ch7Opt: controller.getParameterFact(-1, "CH7_OPT") property Fact _ch8Opt: controller.getParameterFact(-1, "CH8_OPT") @@ -75,10 +71,14 @@ SetupPage { // handler which updates your property with the new value, this first value change will trash // your bound values. In order to work around this we don't set the values into the Sliders until // after Qml load is done. We also don't track value changes until Qml load completes. - throttleHover.value = _hoverTuneParam.value rollPitch.value = _rateRollP.value climb.value = _rateClimbP.value - rcFeel.value = _rcFeel.value + if (_rcFeelAvailable) { + rcFeel.value = _rcFeel.value + } + if (_atcInputTCAvailable) { + atcInputTC.value = _atcInputTC.value + } _loadComplete = true calcAutoTuneChannel() @@ -142,36 +142,6 @@ SetupPage { anchors.top: parent.top spacing: _margins - Column { - anchors.left: parent.left - anchors.right: parent.right - - QGCLabel { - text: qsTr("Throttle Hover") - font.family: ScreenTools.demiboldFontFamily - } - - QGCLabel { - text: qsTr("How much throttle is needed to maintain a steady hover") - } - - Slider { - id: throttleHover - anchors.left: parent.left - anchors.right: parent.right - minimumValue: _hoverTuneMin - maximumValue: _hoverTuneMax - stepSize: _hoverTuneStep - tickmarksEnabled: true - - onValueChanged: { - if (_loadComplete) { - _hoverTuneParam.value = value - } - } - } - } - Column { anchors.left: parent.left anchors.right: parent.right @@ -240,6 +210,7 @@ SetupPage { Column { anchors.left: parent.left anchors.right: parent.right + visible: _rcFeelAvailable QGCLabel { text: qsTr("RC Roll/Pitch Feel") @@ -266,6 +237,37 @@ SetupPage { } } } + + Column { + anchors.left: parent.left + anchors.right: parent.right + visible: _atcInputTCAvailable + + QGCLabel { + text: qsTr("RC Roll/Pitch Feel") + font.family: ScreenTools.demiboldFontFamily + } + + QGCLabel { + text: qsTr("Slide to the left for soft control, slide to the right for crisp control") + } + + Slider { + id: atcInputTC + anchors.left: parent.left + anchors.right: parent.right + minimumValue: _atcInputTC.min + maximumValue: _atcInputTC.max + stepSize: _atcInputTC.increment + tickmarksEnabled: true + + onValueChanged: { + if (_loadComplete) { + _atcInputTC.value = value + } + } + } + } } } // Rectangle - Basic tuning diff --git a/src/AutoPilotPlugins/AutoPilotPlugin.cc b/src/AutoPilotPlugins/AutoPilotPlugin.cc index 6107fd4551bded99871d1979c2ce16527213e38c..0feb2f33fb877410dfde150a5e4c36727dbd3ee2 100644 --- a/src/AutoPilotPlugins/AutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/AutoPilotPlugin.cc @@ -13,8 +13,6 @@ #include "AutoPilotPlugin.h" #include "QGCApplication.h" -#include "ParameterManager.h" -#include "UAS.h" #include "FirmwarePlugin.h" AutoPilotPlugin::AutoPilotPlugin(Vehicle* vehicle, QObject* parent) @@ -28,7 +26,7 @@ AutoPilotPlugin::AutoPilotPlugin(Vehicle* vehicle, QObject* parent) AutoPilotPlugin::~AutoPilotPlugin() { - + } void AutoPilotPlugin::_recalcSetupComplete(void) diff --git a/src/AutoPilotPlugins/AutoPilotPlugin.h b/src/AutoPilotPlugins/AutoPilotPlugin.h index 66728f885bfd6b0e0030ef9277828149bf8c11d1..39fd1f78686c330d9a5c260fdc3a8bc881ce52b8 100644 --- a/src/AutoPilotPlugins/AutoPilotPlugin.h +++ b/src/AutoPilotPlugins/AutoPilotPlugin.h @@ -19,7 +19,6 @@ #include "FactSystem.h" #include "Vehicle.h" -class ParameterManager; class Vehicle; class FirmwarePlugin; diff --git a/src/AutoPilotPlugins/Common/ESP8266ComponentSummary.qml b/src/AutoPilotPlugins/Common/ESP8266ComponentSummary.qml index 57f60c4dcdf6a5e7b7d969d77316117d97443493..25f3506ce6cfb62432f83f5eb949dbee23ab316f 100644 --- a/src/AutoPilotPlugins/Common/ESP8266ComponentSummary.qml +++ b/src/AutoPilotPlugins/Common/ESP8266ComponentSummary.qml @@ -30,38 +30,38 @@ FactPanel { Column { anchors.fill: parent VehicleSummaryRow { - labelText: qsTr("Firmware Version:") + labelText: qsTr("Firmware Version") valueText: esp8266.version } VehicleSummaryRow { - labelText: qsTr("WiFi Mode:") + labelText: qsTr("WiFi Mode") valueText: wifiMode ? (wifiMode.value === 0 ? "AP Mode" : "Station Mode") : "AP Mode" } VehicleSummaryRow { - labelText: qsTr("WiFi Channel:") + labelText: qsTr("WiFi Channel") valueText: wifiChannel ? wifiChannel.valueString : "" visible: wifiMode ? wifiMode.value === 0 : true } VehicleSummaryRow { - labelText: qsTr("WiFi AP SSID:") + labelText: qsTr("WiFi AP SSID") valueText: esp8266.wifiSSID } VehicleSummaryRow { - labelText: qsTr("WiFi AP Password:") + labelText: qsTr("WiFi AP Password") valueText: esp8266.wifiPassword } /* Too much info makes it all crammed VehicleSummaryRow { - labelText: qsTr("WiFi STA SSID:") + labelText: qsTr("WiFi STA SSID") valueText: esp8266.wifiSSIDSta } VehicleSummaryRow { - labelText: qsTr("WiFi STA Password:") + labelText: qsTr("WiFi STA Password") valueText: esp8266.wifiPasswordSta } */ VehicleSummaryRow { - labelText: qsTr("UART Baud Rate:") + labelText: qsTr("UART Baud Rate") valueText: uartBaud ? uartBaud.valueString : "" } } diff --git a/src/AutoPilotPlugins/Common/SetupPage.qml b/src/AutoPilotPlugins/Common/SetupPage.qml index a2912e368a4de630873568b4028d8779386e2427..891b6f1fabfd87ada3fb85548a41f150ca9fce8b 100644 --- a/src/AutoPilotPlugins/Common/SetupPage.qml +++ b/src/AutoPilotPlugins/Common/SetupPage.qml @@ -10,6 +10,7 @@ import QtQuick 2.3 import QtQuick.Controls 1.2 import QtQuick.Dialogs 1.2 +import QtQuick.Layouts 1.2 import QGroundControl 1.0 import QGroundControl.FactSystem 1.0 @@ -23,21 +24,26 @@ import QGroundControl.Controllers 1.0 QGCView { id: setupView viewPanel: setupPanel - enabled: !_shouldDisableWhenArmed + enabled: !_disableDueToArmed && !_disableDueToFlying property alias pageComponent: pageLoader.sourceComponent property string pageName: vehicleComponent ? vehicleComponent.name : "" property string pageDescription: vehicleComponent ? vehicleComponent.description : "" property real availableWidth: width - pageLoader.x property real availableHeight: height - pageLoader.y + property bool showAdvanced: false + property alias advanced: advancedCheckBox.checked - property bool _vehicleArmed: QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle.armed : false - property bool _shouldDisableWhenArmed: _vehicleArmed ? (vehicleComponent ? !vehicleComponent.allowSetupWhileArmed : false) : false + property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle + property bool _vehicleArmed: _activeVehicle ? _activeVehicle.armed : false + property bool _vehicleFlying: _activeVehicle ? _activeVehicle.flying : false + property bool _disableDueToArmed: vehicleComponent ? (!vehicleComponent.allowSetupWhileArmed && _vehicleArmed) : false + property bool _disableDueToFlying: vehicleComponent ? (!vehicleComponent.allowSetupWhileFlying && _vehicleFlying) : false + property string _disableReason: _disableDueToArmed ? qsTr("armed") : qsTr("flying") property real _margins: ScreenTools.defaultFontPixelHeight * 0.5 property string _pageTitle: qsTr("%1 Setup").arg(pageName) - QGCPalette { id: qgcPal; colorGroupEnabled: setupPanel.enabled } QGCViewPanel { @@ -50,35 +56,49 @@ QGCView { contentHeight: pageLoader.y + pageLoader.item.height clip: true - Column { - id: headingColumn - width: setupPanel.width + RowLayout { + id: headingRow + anchors.left: parent.left + anchors.right: parent.right spacing: _margins + layoutDirection: Qt.RightToLeft - QGCLabel { - font.pointSize: ScreenTools.largeFontPointSize - text: _shouldDisableWhenArmed ? _pageTitle + "" + qsTr(" (Disabled while the vehicle is armed)") + "" : _pageTitle - visible: !ScreenTools.isShortScreen + QGCCheckBox { + id: advancedCheckBox + text: qsTr("Advanced") + visible: showAdvanced } - QGCLabel { - anchors.left: parent.left - anchors.right: parent.right - wrapMode: Text.WordWrap - text: pageDescription - visible: !ScreenTools.isShortScreen + Column { + spacing: _margins + Layout.fillWidth: true + + QGCLabel { + font.pointSize: ScreenTools.largeFontPointSize + text: !setupView.enabled ? _pageTitle + "" + qsTr(" (Disabled while the vehicle is %1)").arg(_disableReason) + "" : _pageTitle + visible: !ScreenTools.isShortScreen + } + + QGCLabel { + anchors.left: parent.left + anchors.right: parent.right + wrapMode: Text.WordWrap + text: pageDescription + visible: !ScreenTools.isShortScreen + } } } Loader { id: pageLoader anchors.topMargin: _margins - anchors.top: headingColumn.bottom + anchors.top: headingRow.bottom } + // Overlay to display when vehicle is armed and this setup page needs // to be disabled Rectangle { - visible: _shouldDisableWhenArmed + visible: !setupView.enabled anchors.fill: pageLoader color: "black" opacity: 0.5 diff --git a/src/AutoPilotPlugins/PX4/AirframeComponentSummary.qml b/src/AutoPilotPlugins/PX4/AirframeComponentSummary.qml index 1adf6162fa099f12ecaf13f17d0ab02c23fa70e8..dfba597fc2510b380728f4f6f316e81ac5999569 100644 --- a/src/AutoPilotPlugins/PX4/AirframeComponentSummary.qml +++ b/src/AutoPilotPlugins/PX4/AirframeComponentSummary.qml @@ -23,20 +23,20 @@ FactPanel { Column { anchors.fill: parent VehicleSummaryRow { - labelText: qsTr("System ID:") + labelText: qsTr("System ID") valueText: sysIdFact ? sysIdFact.valueString : "" } VehicleSummaryRow { - labelText: qsTr("Airframe type:") + labelText: qsTr("Airframe type") valueText: autoStartSet ? controller.currentAirframeType : qsTr("Setup required") } VehicleSummaryRow { - labelText: qsTr("Vehicle:") + labelText: qsTr("Vehicle") valueText: autoStartSet ? controller.currentVehicleName : qsTr("Setup required") } VehicleSummaryRow { - labelText: qsTr("Firmware Version:") + labelText: qsTr("Firmware Version") valueText: activeVehicle.firmwareMajorVersion === -1 ? qsTr("Unknown") : activeVehicle.firmwareMajorVersion + "." + activeVehicle.firmwareMinorVersion + "." + activeVehicle.firmwarePatchVersion + activeVehicle.firmwareVersionTypeString } } diff --git a/src/AutoPilotPlugins/PX4/AirframeFactMetaData.xml b/src/AutoPilotPlugins/PX4/AirframeFactMetaData.xml index adcbccdc4720a5e3b2f10291e73bdd4302ee6c57..c762c5deeae25f33b76cb5cb9177639e364939cd 100644 --- a/src/AutoPilotPlugins/PX4/AirframeFactMetaData.xml +++ b/src/AutoPilotPlugins/PX4/AirframeFactMetaData.xml @@ -329,6 +329,11 @@ Michael Schaeuble Quadrotor x + + Copter + Lorenz Meier <lorenz@px4.io> + Quadrotor x + Copter Thomas Gubler <thomas@px4.io> diff --git a/src/AutoPilotPlugins/PX4/CameraComponent.qml b/src/AutoPilotPlugins/PX4/CameraComponent.qml index a848bb9277c78e258e813313884404b3ae5330c3..2c291d22824e6fc51ff89318d19ddfc6b75eada9 100644 --- a/src/AutoPilotPlugins/PX4/CameraComponent.qml +++ b/src/AutoPilotPlugins/PX4/CameraComponent.qml @@ -121,14 +121,14 @@ SetupPage { } QGCLabel { - anchors.baseline: camTrigCombo.baseline + Layout.alignment: Qt.AlignVCenter text: qsTr("Trigger mode") } FactComboBox { - id: camTrigCombo fact: _camTriggerMode indexModel: false enabled: !_rebooting + Layout.alignment: Qt.AlignVCenter Layout.minimumWidth: _editFieldWidth onActivated: { applyAndRestart.visible = true @@ -136,14 +136,14 @@ SetupPage { } QGCLabel { - anchors.baseline: camInterfaceCombo.baseline + Layout.alignment: Qt.AlignVCenter text: qsTr("Trigger interface") } FactComboBox { - id: camInterfaceCombo fact: _camTriggerInterface indexModel: false enabled: !_rebooting && (_camTriggerInterface ? true : false) + Layout.alignment: Qt.AlignVCenter Layout.minimumWidth: _editFieldWidth onActivated: { applyAndRestart.visible = true @@ -152,7 +152,7 @@ SetupPage { QGCLabel { text: qsTr("Time Interval") - anchors.baseline: timeIntervalField.baseline + Layout.alignment: Qt.AlignVCenter color: qgcPal.text visible: timeIntervalField.visible } @@ -161,12 +161,13 @@ SetupPage { fact: controller.getParameterFact(-1, "TRIG_INTERVAL", false) showUnits: true Layout.minimumWidth: _editFieldWidth + Layout.alignment: Qt.AlignVCenter visible: _camTriggerMode.value === 2 } QGCLabel { text: qsTr("Distance Interval") - anchors.baseline: trigDistField.baseline + Layout.alignment: Qt.AlignVCenter color: qgcPal.text visible: trigDistField.visible } @@ -174,6 +175,7 @@ SetupPage { id: trigDistField fact: controller.getParameterFact(-1, "TRIG_DISTANCE", false) showUnits: true + Layout.alignment: Qt.AlignVCenter Layout.minimumWidth: _editFieldWidth visible: _camTriggerMode.value === 3 } @@ -199,8 +201,8 @@ SetupPage { } Row { - spacing: _margins - anchors.horizontalCenter: parent.horizontalCenter + spacing: _margins + Layout.alignment: Qt.AlignHCenter GridLayout { rows: 2 diff --git a/src/AutoPilotPlugins/PX4/CameraComponentSummary.qml b/src/AutoPilotPlugins/PX4/CameraComponentSummary.qml index d475a6600bfc7be1d99b6e5bfe66ee892479a055..dd0e451a44c76b1188a704df131eacd796354070 100644 --- a/src/AutoPilotPlugins/PX4/CameraComponentSummary.qml +++ b/src/AutoPilotPlugins/PX4/CameraComponentSummary.qml @@ -25,36 +25,36 @@ FactPanel { anchors.fill: parent VehicleSummaryRow { - labelText: qsTr("Trigger interface:") + labelText: qsTr("Trigger interface") valueText: _camTriggerInterface ? _camTriggerInterface.enumStringValue : "" } VehicleSummaryRow { - labelText: qsTr("Trigger mode:") + labelText: qsTr("Trigger mode") valueText: _camTriggerMode ? _camTriggerMode.enumStringValue : "" } VehicleSummaryRow { visible: _timeInterval && _camTriggerMode.value === 2 - labelText: qsTr("Time interval:") + labelText: qsTr("Time interval") valueText: _timeInterval ? _timeInterval.value : "" } VehicleSummaryRow { visible: _distanceInterval && _camTriggerMode.value === 3 - labelText: qsTr("Distance interval:") + labelText: qsTr("Distance interval") valueText: _distanceInterval ? _distanceInterval.value : "" } VehicleSummaryRow { visible: _auxPins - labelText: qsTr("AUX pins:") + labelText: qsTr("AUX pins") valueText: _auxPins ? _auxPins.value : "" } VehicleSummaryRow { visible: _camTriggerPol - labelText: qsTr("AUX pin polarity:") + labelText: qsTr("AUX pin polarity") valueText: _camTriggerPol ? (_camTriggerPol.value ? "High (3.3V)" : "Low (0V)") : "" } diff --git a/src/AutoPilotPlugins/PX4/FlightModesComponentSummary.qml b/src/AutoPilotPlugins/PX4/FlightModesComponentSummary.qml index 3755d61a2197ddc7e2840117d019a514527fd01b..600c05ca9110b0dbe91c2be5e17ae8e901f147f7 100644 --- a/src/AutoPilotPlugins/PX4/FlightModesComponentSummary.qml +++ b/src/AutoPilotPlugins/PX4/FlightModesComponentSummary.qml @@ -29,13 +29,13 @@ FactPanel { id: simple Column { VehicleSummaryRow { - labelText: qsTr("Mode switch:") + labelText: qsTr("Mode switch") valueText: _rcMapFltmode.value === 0 ? qsTr("Setup required") : _rcMapFltmode.enumStringValue } Repeater { model: 6 VehicleSummaryRow { - labelText: qsTr("Flight Mode %1 :").arg(index + 1) + labelText: qsTr("Flight Mode %1 ").arg(index + 1) valueText: controller.getParameterFact(-1, "COM_FLTMODE" + (index + 1)).enumStringValue } } @@ -49,19 +49,19 @@ FactPanel { property Fact loiterSwFact: controller.getParameterFact(-1, "RC_MAP_LOITER_SW") property Fact returnSwFact: controller.getParameterFact(-1, "RC_MAP_RETURN_SW") VehicleSummaryRow { - labelText: qsTr("Mode switch:") + labelText: qsTr("Mode switch") valueText: _rcMapModeSw.value === 0 ? qsTr("Setup required") : _rcMapModeSw.valueString } VehicleSummaryRow { - labelText: qsTr("Position Ctl switch:") + labelText: qsTr("Position Ctl switch") valueText: posCtlSwFact.value === 0 ? qsTr("Disabled") : posCtlSwFact.valueString } VehicleSummaryRow { - labelText: qsTr("Loiter switch:") + labelText: qsTr("Loiter switch") valueText: loiterSwFact.value === 0 ? qsTr("Disabled") : loiterSwFact.valueString } VehicleSummaryRow { - labelText: qsTr("Return switch:") + labelText: qsTr("Return switch") valueText: returnSwFact.value === 0 ? qsTr("Disabled") : returnSwFact.valueString } } diff --git a/src/AutoPilotPlugins/PX4/PX4RadioComponentSummary.qml b/src/AutoPilotPlugins/PX4/PX4RadioComponentSummary.qml index 1c36cc09223e1a47c274a775fcf2a41d9ea9eee9..13a7e849afc3755e7f55a3aea6e23db487fdf1a0 100644 --- a/src/AutoPilotPlugins/PX4/PX4RadioComponentSummary.qml +++ b/src/AutoPilotPlugins/PX4/PX4RadioComponentSummary.qml @@ -26,38 +26,38 @@ FactPanel { anchors.fill: parent VehicleSummaryRow { - labelText: qsTr("Roll:") + labelText: qsTr("Roll") valueText: mapRollFact ? (mapRollFact.value === 0 ? qsTr("Setup required") : mapRollFact.valueString) : "" } VehicleSummaryRow { - labelText: qsTr("Pitch:") + labelText: qsTr("Pitch") valueText: mapPitchFact ? (mapPitchFact.value === 0 ? qsTr("Setup required") : mapPitchFact.valueString) : "" } VehicleSummaryRow { - labelText: qsTr("Yaw:") + labelText: qsTr("Yaw") valueText: mapYawFact ? (mapYawFact.value === 0 ? qsTr("Setup required") : mapYawFact.valueString) : "" } VehicleSummaryRow { - labelText: qsTr("Throttle:") + labelText: qsTr("Throttle") valueText: mapThrottleFact ? (mapThrottleFact.value === 0 ? qsTr("Setup required") : mapThrottleFact.valueString) : "" } VehicleSummaryRow { - labelText: qsTr("Flaps:") + labelText: qsTr("Flaps") valueText: mapFlapsFact ? (mapFlapsFact.value === 0 ? qsTr("Disabled") : mapFlapsFact.valueString) : "" visible: !controller.vehicle.multiRotor } VehicleSummaryRow { - labelText: qsTr("Aux1:") + labelText: qsTr("Aux1") valueText: mapAux1Fact ? (mapAux1Fact.value === 0 ? qsTr("Disabled") : mapAux1Fact.valueString) : "" } VehicleSummaryRow { - labelText: qsTr("Aux2:") + labelText: qsTr("Aux2") valueText: mapAux2Fact ? (mapAux2Fact.value === 0 ? qsTr("Disabled") : mapAux2Fact.valueString) : "" } } diff --git a/src/AutoPilotPlugins/PX4/PX4TuningComponent.h b/src/AutoPilotPlugins/PX4/PX4TuningComponent.h index 3080b965c83f3ac77cc2f9961822e20fa413c805..5fe989dde16416afabbf069094485b301fb7f1f0 100644 --- a/src/AutoPilotPlugins/PX4/PX4TuningComponent.h +++ b/src/AutoPilotPlugins/PX4/PX4TuningComponent.h @@ -32,6 +32,7 @@ public: QUrl setupSource(void) const final; QUrl summaryQmlSource(void) const final; bool allowSetupWhileArmed(void) const final { return true; } + bool allowSetupWhileFlying(void) const final { return true; } private: const QString _name; diff --git a/src/AutoPilotPlugins/PX4/PX4TuningComponentCopter.qml b/src/AutoPilotPlugins/PX4/PX4TuningComponentCopter.qml index 56980007f6e5eae06a24e833232feb33d96f9687..4c57ab20399f02f8fff24b6de755f77cd72db68c 100644 --- a/src/AutoPilotPlugins/PX4/PX4TuningComponentCopter.qml +++ b/src/AutoPilotPlugins/PX4/PX4TuningComponentCopter.qml @@ -7,11 +7,16 @@ * ****************************************************************************/ +import QtQuick 2.3 +import QtQuick.Controls 1.2 +import QtCharts 2.2 +import QtQuick.Layouts 1.2 -import QtQuick 2.3 -import QtQuick.Controls 1.2 - -import QGroundControl.Controls 1.0 +import QGroundControl 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.FactSystem 1.0 +import QGroundControl.FactControls 1.0 +import QGroundControl.ScreenTools 1.0 SetupPage { id: tuningPage @@ -20,47 +25,97 @@ SetupPage { Component { id: pageComponent - FactSliderPanel { - width: availableWidth - qgcViewPanel: tuningPage.viewPanel + Column { + width: availableWidth - sliderModel: ListModel { - ListElement { - title: qsTr("Hover Throttle") - description: qsTr("Adjust throttle so hover is at mid-throttle. Slide to the left if hover is lower than throttle center. Slide to the right if hover is higher than throttle center.") - param: "MPC_THR_HOVER" - min: 20 - max: 80 - step: 1 - } + Component.onCompleted: { + showAdvanced = !ScreenTools.isMobile + } - ListElement { - title: qsTr("Manual minimum throttle") - description: qsTr("Slide to the left to start the motors with less idle power. Slide to the right if descending in manual flight becomes unstable.") - param: "MPC_MANTHR_MIN" - min: 0 - max: 15 - step: 1 - } + FactPanelController { + id: controller + factPanel: tuningPage.viewPanel + } - ListElement { - title: qsTr("Roll sensitivity") - description: qsTr("Slide to the left to make roll control faster and more accurate. Slide to the right if roll oscillates or is too twitchy.") - param: "MC_ROLL_TC" - min: 0.15 - max: 0.25 - step: 0.01 - } + // Standard tuning page + FactSliderPanel { + width: availableWidth + qgcViewPanel: tuningPage.viewPanel + visible: !advanced + + sliderModel: ListModel { + ListElement { + title: qsTr("Hover Throttle") + description: qsTr("Adjust throttle so hover is at mid-throttle. Slide to the left if hover is lower than throttle center. Slide to the right if hover is higher than throttle center.") + param: "MPC_THR_HOVER" + min: 20 + max: 80 + step: 1 + } - ListElement { - title: qsTr("Pitch sensitivity") - description: qsTr("Slide to the left to make pitch control faster and more accurate. Slide to the right if pitch oscillates or is too twitchy.") - param: "MC_PITCH_TC" - min: 0.15 - max: 0.25 - step: 0.01 + ListElement { + title: qsTr("Manual minimum throttle") + description: qsTr("Slide to the left to start the motors with less idle power. Slide to the right if descending in manual flight becomes unstable.") + param: "MPC_MANTHR_MIN" + min: 0 + max: 15 + step: 1 + } + /* + These seem to have disappeared from PX4 firmware! + ListElement { + title: qsTr("Roll sensitivity") + description: qsTr("Slide to the left to make roll control faster and more accurate. Slide to the right if roll oscillates or is too twitchy.") + param: "MC_ROLL_TC" + min: 0.15 + max: 0.25 + step: 0.01 + } + + ListElement { + title: qsTr("Pitch sensitivity") + description: qsTr("Slide to the left to make pitch control faster and more accurate. Slide to the right if pitch oscillates or is too twitchy.") + param: "MC_PITCH_TC" + min: 0.15 + max: 0.25 + step: 0.01 + } +*/ } } - } - } // Component + + Loader { + anchors.left: parent.left + anchors.right: parent.right + sourceComponent: advanced ? advancePageComponent : undefined + } + + Component { + id: advancePageComponent + + PIDTuning { + anchors.left: parent.left + anchors.right: parent.right + tuneList: [ qsTr("Roll"), qsTr("Pitch"), qsTr("Yaw") ] + params: [ + [ controller.getParameterFact(-1, "MC_ROLL_P"), + controller.getParameterFact(-1, "MC_ROLLRATE_P"), + controller.getParameterFact(-1, "MC_ROLLRATE_I"), + controller.getParameterFact(-1, "MC_ROLLRATE_D"), + controller.getParameterFact(-1, "MC_ROLLRATE_FF") ], + [ controller.getParameterFact(-1, "MC_PITCH_P"), + controller.getParameterFact(-1, "MC_PITCHRATE_P"), + controller.getParameterFact(-1, "MC_PITCHRATE_I"), + controller.getParameterFact(-1, "MC_PITCHRATE_D"), + controller.getParameterFact(-1, "MC_PITCHRATE_FF") ], + [ controller.getParameterFact(-1, "MC_YAW_P"), + controller.getParameterFact(-1, "MC_YAWRATE_P"), + controller.getParameterFact(-1, "MC_YAWRATE_I"), + controller.getParameterFact(-1, "MC_YAWRATE_D"), + controller.getParameterFact(-1, "MC_YAW_FF"), + controller.getParameterFact(-1, "MC_YAWRATE_FF") ] ] + } + } // Component - Advanced Page + } // Column + } // Component - pageComponent } // SetupPage diff --git a/src/AutoPilotPlugins/PX4/PX4TuningComponentPlane.qml b/src/AutoPilotPlugins/PX4/PX4TuningComponentPlane.qml index 40fa8ba467a70e50e9a4b0055b440494e68fdf78..bc3a4916966d681d6afe8bce68b7c272ab7f9ef1 100644 --- a/src/AutoPilotPlugins/PX4/PX4TuningComponentPlane.qml +++ b/src/AutoPilotPlugins/PX4/PX4TuningComponentPlane.qml @@ -7,11 +7,16 @@ * ****************************************************************************/ +import QtQuick 2.3 +import QtQuick.Controls 1.2 +import QtCharts 2.2 +import QtQuick.Layouts 1.2 -import QtQuick 2.3 -import QtQuick.Controls 1.2 - -import QGroundControl.Controls 1.0 +import QGroundControl 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.FactSystem 1.0 +import QGroundControl.FactControls 1.0 +import QGroundControl.ScreenTools 1.0 SetupPage { id: tuningPage @@ -20,47 +25,64 @@ SetupPage { Component { id: pageComponent - FactSliderPanel { - width: availableWidth - qgcViewPanel: tuningPage.viewPanel + Column { + width: availableWidth - sliderModel: ListModel { - ListElement { - title: qsTr("Roll sensitivity") - description: qsTr("Slide to the left to make roll control faster and more accurate. Slide to the right if roll oscillates or is too twitchy.") - param: "FW_R_TC" - min: 0.2 - max: 0.8 - step: 0.01 - } + Component.onCompleted: { + showAdvanced = !ScreenTools.isMobile + } - ListElement { - title: qsTr("Pitch sensitivity") - description: qsTr("Slide to the left to make pitch control faster and more accurate. Slide to the right if pitch oscillates or is too twitchy.") - param: "FW_P_TC" - min: 0.2 - max: 0.8 - step: 0.01 - } + FactPanelController { + id: controller + factPanel: tuningPage.viewPanel + } - ListElement { - title: qsTr("Cruise throttle") - description: qsTr("This is the throttle setting required to achieve the desired cruise speed. Most planes need 50-60%.") - param: "FW_THR_CRUISE" - min: 20 - max: 80 - step: 1 - } + // Standard tuning page + FactSliderPanel { + width: availableWidth + qgcViewPanel: tuningPage.viewPanel + visible: !advanced - ListElement { - title: qsTr("Mission mode sensitivity") - description: qsTr("Slide to the left to make position control more accurate and more aggressive. Slide to the right to make flight in mission mode smoother and less twitchy.") - param: "FW_L1_PERIOD" - min: 12 - max: 50 - step: 0.5 + sliderModel: ListModel { + ListElement { + title: qsTr("Cruise throttle") + description: qsTr("This is the throttle setting required to achieve the desired cruise speed. Most planes need 50-60%.") + param: "FW_THR_CRUISE" + min: 20 + max: 80 + step: 1 + } } } - } - } -} + + + Loader { + anchors.left: parent.left + anchors.right: parent.right + sourceComponent: advanced ? advancePageComponent : undefined + } + + Component { + id: advancePageComponent + + PIDTuning { + anchors.left: parent.left + anchors.right: parent.right + tuneList: [ qsTr("Roll"), qsTr("Pitch"), qsTr("Yaw") ] + params: [ + [ controller.getParameterFact(-1, "FW_RR_P"), + controller.getParameterFact(-1, "FW_RR_I"), + controller.getParameterFact(-1, "FW_RR_FF"), + controller.getParameterFact(-1, "FW_R_TC"),], + [ controller.getParameterFact(-1, "FW_PR_P"), + controller.getParameterFact(-1, "FW_PR_I"), + controller.getParameterFact(-1, "FW_PR_FF"), + controller.getParameterFact(-1, "FW_P_TC") ], + [ controller.getParameterFact(-1, "FW_YR_P"), + controller.getParameterFact(-1, "FW_YR_I"), + controller.getParameterFact(-1, "FW_YR_FF") ] ] + } + } // Component - Advanced Page + } // Column + } // Component - pageComponent +} // SetupPage diff --git a/src/AutoPilotPlugins/PX4/PX4TuningComponentVTOL.qml b/src/AutoPilotPlugins/PX4/PX4TuningComponentVTOL.qml index 3feb7ca67e7c0c25474fb51ccc0a3244b15d1ea4..dd572435258a1e784b7bb527e2c302cfe7125d0e 100644 --- a/src/AutoPilotPlugins/PX4/PX4TuningComponentVTOL.qml +++ b/src/AutoPilotPlugins/PX4/PX4TuningComponentVTOL.qml @@ -26,24 +26,6 @@ SetupPage { sliderModel: ListModel { - ListElement { - title: qsTr("Hover Roll sensitivity") - description: qsTr("Slide to the left to make roll control during hover faster and more accurate. Slide to the right if roll oscillates or is too twitchy.") - param: "MC_ROLL_TC" - min: 0.15 - max: 0.25 - step: 0.01 - } - - ListElement { - title: qsTr("Hover Pitch sensitivity") - description: qsTr("Slide to the left to make pitch control during hover faster and more accurate. Slide to the right if pitch oscillates or is too twitchy.") - param: "MC_PITCH_TC" - min: 0.15 - max: 0.25 - step: 0.01 - } - ListElement { title: qsTr("Plane Roll sensitivity") description: qsTr("Slide to the left to make roll control faster and more accurate. Slide to the right if roll oscillates or is too twitchy.") @@ -81,7 +63,7 @@ SetupPage { } ListElement { - title: qsTr("Hoever manual minimum throttle") + title: qsTr("Hover manual minimum throttle") description: qsTr("Slide to the left to start the motors with less idle power. Slide to the right if descending in manual flight becomes unstable.") param: "MPC_MANTHR_MIN" min: 0 diff --git a/src/AutoPilotPlugins/PX4/PowerComponent.h b/src/AutoPilotPlugins/PX4/PowerComponent.h index fb4835d3eef38b226dd0f9026cbfe42156c08836..6ea78976da9a023ef4b83f282e6b3d269c608b6c 100644 --- a/src/AutoPilotPlugins/PX4/PowerComponent.h +++ b/src/AutoPilotPlugins/PX4/PowerComponent.h @@ -24,18 +24,19 @@ class PowerComponent : public VehicleComponent public: PowerComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent = NULL); - // Virtuals from VehicleComponent - virtual QStringList setupCompleteChangedTriggerList(void) const; - - // Virtuals from VehicleComponent - virtual QString name (void) const; - virtual QString description (void) const; - virtual QString iconResource (void) const; - virtual bool requiresSetup (void) const; - virtual bool setupComplete (void) const; - virtual QUrl setupSource (void) const; - virtual QUrl summaryQmlSource (void) const; + // Overrides from VehicleComponent + QStringList setupCompleteChangedTriggerList(void) const override; + // Overrides from VehicleComponent + QString name (void) const override; + QString description (void) const override; + QString iconResource (void) const override; + bool requiresSetup (void) const override; + bool setupComplete (void) const override; + QUrl setupSource (void) const override; + QUrl summaryQmlSource (void) const override; + bool allowSetupWhileArmed (void) const override { return true; } + private: const QString _name; QVariantList _summaryItems; diff --git a/src/AutoPilotPlugins/PX4/PowerComponent.qml b/src/AutoPilotPlugins/PX4/PowerComponent.qml index ef0f665be9cfbd1b18fb417120bf14495ec207db..3eeec49131fd0703cf030c9746a5c57f0047b32c 100644 --- a/src/AutoPilotPlugins/PX4/PowerComponent.qml +++ b/src/AutoPilotPlugins/PX4/PowerComponent.qml @@ -348,9 +348,9 @@ SetupPage { } // QGCGroupBox - Battery settings QGCGroupBox { - anchors.left: batteryGroup.left - anchors.right: batteryGroup.right - title: qsTr("ESC PWM Minimum and Maximum Calibration") + Layout.maximumWidth: batteryGroup.width + Layout.fillWidth: true + title: qsTr("ESC PWM Minimum and Maximum Calibration") ColumnLayout { anchors.left: parent.left @@ -383,10 +383,10 @@ SetupPage { } QGCGroupBox { - anchors.left: batteryGroup.left - anchors.right: batteryGroup.right - title: qsTr("UAVCAN Bus Configuration") - visible: showUAVCAN.checked + Layout.maximumWidth: batteryGroup.width + Layout.fillWidth: true + title: qsTr("UAVCAN Bus Configuration") + visible: showUAVCAN.checked Row { id: uavCanConfigRow @@ -407,10 +407,10 @@ SetupPage { } QGCGroupBox { - anchors.left: batteryGroup.left - anchors.right: batteryGroup.right - title: qsTr("UAVCAN Motor Index and Direction Assignment") - visible: showUAVCAN.checked + Layout.maximumWidth: batteryGroup.width + Layout.fillWidth: true + title: qsTr("UAVCAN Motor Index and Direction Assignment") + visible: showUAVCAN.checked ColumnLayout { anchors.left: parent.left @@ -456,10 +456,10 @@ SetupPage { } QGCGroupBox { - anchors.left: batteryGroup.left - anchors.right: batteryGroup.right - title: qsTr("Advanced Power Settings") - visible: showAdvanced.checked + Layout.maximumWidth: batteryGroup.width + Layout.fillWidth: true + title: qsTr("Advanced Power Settings") + visible: showAdvanced.checked ColumnLayout { anchors.left: parent.left @@ -487,7 +487,7 @@ SetupPage { text: qsTr("Batteries show less voltage at high throttle. Enter the difference in Volts between idle throttle and full ") + qsTr("throttle, divided by the number of battery cells. Leave at the default if unsure. ") + highlightPrefix + qsTr("If this value is set too high, the battery might be deep discharged and damaged.") + highlightSuffix - Layout.fillWidth: true + Layout.maximumWidth: ScreenTools.defaultFontPixelWidth * 60 } Row { diff --git a/src/AutoPilotPlugins/PX4/PowerComponentSummary.qml b/src/AutoPilotPlugins/PX4/PowerComponentSummary.qml index 7991e829f4a2b9abc5795a277786b3301d328f20..3e81f1d194d2554ce8b36c88dea30d480db53cb6 100644 --- a/src/AutoPilotPlugins/PX4/PowerComponentSummary.qml +++ b/src/AutoPilotPlugins/PX4/PowerComponentSummary.qml @@ -36,17 +36,17 @@ FactPanel { anchors.fill: parent VehicleSummaryRow { - labelText: qsTr("Battery Full:") + labelText: qsTr("Battery Full") valueText: batVChargedFact ? batVChargedFact.valueString + " " + batVChargedFact.units : "" } VehicleSummaryRow { - labelText: qsTr("Battery Empty:") + labelText: qsTr("Battery Empty") valueText: batVEmptyFact ? batVEmptyFact.valueString + " " + batVEmptyFact.units : "" } VehicleSummaryRow { - labelText: qsTr("Number of Cells:") + labelText: qsTr("Number of Cells") valueText: batCellsFact ? batCellsFact.valueString : "" } } diff --git a/src/AutoPilotPlugins/PX4/SafetyComponentSummary.qml b/src/AutoPilotPlugins/PX4/SafetyComponentSummary.qml index cb51780cc684c49fe077f0a337ce9266c026e256..2cf19b336d0b4ee8b64bd3d25c2e7b59631fcb04 100644 --- a/src/AutoPilotPlugins/PX4/SafetyComponentSummary.qml +++ b/src/AutoPilotPlugins/PX4/SafetyComponentSummary.qml @@ -14,46 +14,64 @@ FactPanel { QGCPalette { id: qgcPal; colorGroupEnabled: enabled } FactPanelController { id: controller; factPanel: panel } - property Fact returnAltFact: controller.getParameterFact(-1, "RTL_RETURN_ALT") - property Fact descendAltFact: controller.getParameterFact(-1, "RTL_DESCEND_ALT") - property Fact landDelayFact: controller.getParameterFact(-1, "RTL_LAND_DELAY") - property Fact commRCLossFact: controller.getParameterFact(-1, "COM_RC_LOSS_T") - property Fact lowBattAction: controller.getParameterFact(-1, "COM_LOW_BAT_ACT") - property Fact rcLossAction: controller.getParameterFact(-1, "NAV_RCL_ACT") - property Fact dataLossAction: controller.getParameterFact(-1, "NAV_DLL_ACT") + property Fact returnAltFact: controller.getParameterFact(-1, "RTL_RETURN_ALT") + property Fact _descendAltFact: controller.getParameterFact(-1, "RTL_DESCEND_ALT") + property Fact landDelayFact: controller.getParameterFact(-1, "RTL_LAND_DELAY") + property Fact commRCLossFact: controller.getParameterFact(-1, "COM_RC_LOSS_T") + property Fact lowBattAction: controller.getParameterFact(-1, "COM_LOW_BAT_ACT") + property Fact rcLossAction: controller.getParameterFact(-1, "NAV_RCL_ACT") + property Fact dataLossAction: controller.getParameterFact(-1, "NAV_DLL_ACT") + property Fact _rtlLandDelayFact: controller.getParameterFact(-1, "RTL_LAND_DELAY") + property int _rtlLandDelayValue: _rtlLandDelayFact.value Column { anchors.fill: parent VehicleSummaryRow { - labelText: qsTr("RTL min alt:") - valueText: returnAltFact ? returnAltFact.valueString + " " + returnAltFact.units : "" + labelText: qsTr("Low Battery Failsafe") + valueText: lowBattAction ? lowBattAction.enumStringValue : "" } VehicleSummaryRow { - labelText: qsTr("RTL home alt:") - valueText: descendAltFact ? descendAltFact.valueString + " " + descendAltFact.units : "" + labelText: qsTr("RC Loss Failsafe") + valueText: rcLossAction ? rcLossAction.enumStringValue : "" } VehicleSummaryRow { - labelText: qsTr("RC loss RTL:") + labelText: qsTr("RC Loss Timeout") valueText: commRCLossFact ? commRCLossFact.valueString + " " + commRCLossFact.units : "" } VehicleSummaryRow { - labelText: qsTr("RC loss action:") - valueText: rcLossAction ? rcLossAction.enumStringValue : "" + labelText: qsTr("Data Link Loss Failsafe") + valueText: dataLossAction ? dataLossAction.enumStringValue : "" } VehicleSummaryRow { - labelText: qsTr("Link loss action:") - valueText: dataLossAction ? dataLossAction.enumStringValue : "" + labelText: qsTr("RTL Climb To") + valueText: returnAltFact ? returnAltFact.valueString + " " + returnAltFact.units : "" } VehicleSummaryRow { - labelText: qsTr("Low battery action:") - valueText: lowBattAction ? lowBattAction.enumStringValue : "" + labelText: qsTr("RTL, Then") + valueText: _rtlLandDelayValue === 0 ? + qsTr("Land immediately") : + (_rtlLandDelayValue < 0 ? + qsTr("Loiter and do not land") : + qsTr("Loiter and land after specified time")) + } + VehicleSummaryRow { + labelText: qsTr("Loiter Alt") + valueText: _descendAltFact.valueString + " " + _descendAltFact.units + visible: _rtlLandDelayValue !== 0 + } + + VehicleSummaryRow { + labelText: qsTr("Land Delay") + valueText: _rtlLandDelayValue + " " + _rtlLandDelayFact.units + visible: _rtlLandDelayValue > 0 + } } } diff --git a/src/AutoPilotPlugins/PX4/SensorsComponent.cc b/src/AutoPilotPlugins/PX4/SensorsComponent.cc index 7fbc76bd013d936ff7bd053019a5843d5c77a6c0..0a2d96752e57c075228fa76f75581bb7fa03b4e0 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponent.cc +++ b/src/AutoPilotPlugins/PX4/SensorsComponent.cc @@ -20,11 +20,14 @@ const char* SensorsComponent::_airspeedBreakerParam = "CBRK_AIRSPD_CHK"; const char* SensorsComponent::_airspeedDisabledParam = "FW_ARSP_MODE"; const char* SensorsComponent::_airspeedCalParam = "SENS_DPRES_OFF"; +const char* SensorsComponent::_magEnabledParam = "SYS_HAS_MAG"; +const char* SensorsComponent::_magCalParam = "CAL_MAG0_ID"; + SensorsComponent::SensorsComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent) : VehicleComponent(vehicle, autopilot, parent), _name(tr("Sensors")) { - _deviceIds << QStringLiteral("CAL_MAG0_ID") << QStringLiteral("CAL_GYRO0_ID") << QStringLiteral("CAL_ACC0_ID"); + _deviceIds << QStringLiteral("CAL_GYRO0_ID") << QStringLiteral("CAL_ACC0_ID"); } QString SensorsComponent::name(void) const @@ -54,6 +57,14 @@ bool SensorsComponent::setupComplete(void) const return false; } } + bool magEnabled = true; + if (_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _magEnabledParam)) { + magEnabled = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _magEnabledParam)->rawValue().toBool(); + } + + if (magEnabled && _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _magCalParam)->rawValue().toFloat() == 0.0f) { + return false; + } if (_vehicle->fixedWing() || _vehicle->vtol()) { if (!_vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _airspeedDisabledParam)->rawValue().toBool() && @@ -70,7 +81,7 @@ QStringList SensorsComponent::setupCompleteChangedTriggerList(void) const { QStringList triggers; - triggers << _deviceIds; + triggers << _deviceIds << _magCalParam << _magEnabledParam; if (_vehicle->fixedWing() || _vehicle->vtol()) { triggers << _airspeedCalParam << _airspeedBreakerParam; } diff --git a/src/AutoPilotPlugins/PX4/SensorsComponent.h b/src/AutoPilotPlugins/PX4/SensorsComponent.h index 9201c5e8e8dd0995284428bef57876ae607a160f..13baae45d977dcf60b804fd06aa46a0349809c88 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponent.h +++ b/src/AutoPilotPlugins/PX4/SensorsComponent.h @@ -44,6 +44,9 @@ private: static const char* _airspeedDisabledParam; static const char* _airspeedBreakerParam; static const char* _airspeedCalParam; + + static const char* _magEnabledParam; + static const char* _magCalParam; }; #endif diff --git a/src/AutoPilotPlugins/PX4/SensorsComponentController.cc b/src/AutoPilotPlugins/PX4/SensorsComponentController.cc index 5ab1327fe8d354215aae1fac7eb9f1e8712202eb..032f54037a5f8b1341f736be377a8aec4bbce674 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponentController.cc +++ b/src/AutoPilotPlugins/PX4/SensorsComponentController.cc @@ -7,10 +7,6 @@ * ****************************************************************************/ - -/// @file -/// @author Don Gagne - #include "SensorsComponentController.h" #include "QGCMAVLink.h" #include "UAS.h" @@ -22,46 +18,48 @@ QGC_LOGGING_CATEGORY(SensorsComponentControllerLog, "SensorsComponentControllerLog") -SensorsComponentController::SensorsComponentController(void) : - _statusLog(NULL), - _progressBar(NULL), - _compassButton(NULL), - _gyroButton(NULL), - _accelButton(NULL), - _airspeedButton(NULL), - _levelButton(NULL), - _cancelButton(NULL), - _setOrientationsButton(NULL), - _showOrientationCalArea(false), - _gyroCalInProgress(false), - _magCalInProgress(false), - _accelCalInProgress(false), - _orientationCalDownSideDone(false), - _orientationCalUpsideDownSideDone(false), - _orientationCalLeftSideDone(false), - _orientationCalRightSideDone(false), - _orientationCalNoseDownSideDone(false), - _orientationCalTailDownSideDone(false), - _orientationCalDownSideVisible(false), - _orientationCalUpsideDownSideVisible(false), - _orientationCalLeftSideVisible(false), - _orientationCalRightSideVisible(false), - _orientationCalNoseDownSideVisible(false), - _orientationCalTailDownSideVisible(false), - _orientationCalDownSideInProgress(false), - _orientationCalUpsideDownSideInProgress(false), - _orientationCalLeftSideInProgress(false), - _orientationCalRightSideInProgress(false), - _orientationCalNoseDownSideInProgress(false), - _orientationCalTailDownSideInProgress(false), - _orientationCalDownSideRotate(false), - _orientationCalUpsideDownSideRotate(false), - _orientationCalLeftSideRotate(false), - _orientationCalRightSideRotate(false), - _orientationCalNoseDownSideRotate(false), - _orientationCalTailDownSideRotate(false), - _unknownFirmwareVersion(false), - _waitingForCancel(false) +SensorsComponentController::SensorsComponentController(void) + : _statusLog (NULL) + , _progressBar (NULL) + , _compassButton (NULL) + , _gyroButton (NULL) + , _accelButton (NULL) + , _airspeedButton (NULL) + , _levelButton (NULL) + , _cancelButton (NULL) + , _setOrientationsButton (NULL) + , _showOrientationCalArea (false) + , _gyroCalInProgress (false) + , _magCalInProgress (false) + , _accelCalInProgress (false) + , _airspeedCalInProgress (false) + , _levelCalInProgress (false) + , _orientationCalDownSideDone (false) + , _orientationCalUpsideDownSideDone (false) + , _orientationCalLeftSideDone (false) + , _orientationCalRightSideDone (false) + , _orientationCalNoseDownSideDone (false) + , _orientationCalTailDownSideDone (false) + , _orientationCalDownSideVisible (false) + , _orientationCalUpsideDownSideVisible (false) + , _orientationCalLeftSideVisible (false) + , _orientationCalRightSideVisible (false) + , _orientationCalNoseDownSideVisible (false) + , _orientationCalTailDownSideVisible (false) + , _orientationCalDownSideInProgress (false) + , _orientationCalUpsideDownSideInProgress (false) + , _orientationCalLeftSideInProgress (false) + , _orientationCalRightSideInProgress (false) + , _orientationCalNoseDownSideInProgress (false) + , _orientationCalTailDownSideInProgress (false) + , _orientationCalDownSideRotate (false) + , _orientationCalUpsideDownSideRotate (false) + , _orientationCalLeftSideRotate (false) + , _orientationCalRightSideRotate (false) + , _orientationCalNoseDownSideRotate (false) + , _orientationCalTailDownSideRotate (false) + , _unknownFirmwareVersion (false) + , _waitingForCancel (false) { } @@ -165,7 +163,9 @@ void SensorsComponentController::_stopCalibration(SensorsComponentController::St switch (code) { case StopCalibrationSuccess: _orientationCalAreaHelpText->setProperty("text", tr("Calibration complete")); - emit resetStatusTextArea(); + if (!_airspeedCalInProgress && !_levelCalInProgress) { + emit resetStatusTextArea(); + } if (_magCalInProgress) { emit setCompassRotations(); } @@ -186,6 +186,7 @@ void SensorsComponentController::_stopCalibration(SensorsComponentController::St _magCalInProgress = false; _accelCalInProgress = false; _gyroCalInProgress = false; + _airspeedCalInProgress = false; } void SensorsComponentController::calibrateGyro(void) @@ -336,6 +337,10 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in emit orientationCalSidesVisibleChanged(); emit orientationCalSidesInProgressChanged(); _updateAndEmitShowOrientationCalArea(true); + } else if (text == "airspeed") { + _airspeedCalInProgress = true; + } else if (text == "level") { + _levelCalInProgress = true; } return; } diff --git a/src/AutoPilotPlugins/PX4/SensorsComponentController.h b/src/AutoPilotPlugins/PX4/SensorsComponentController.h index b2369a4bc80f3c9303547352346efa3bd8070c50..d9cb441c21e84f301b11624c712b33ade3a6d3ea 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponentController.h +++ b/src/AutoPilotPlugins/PX4/SensorsComponentController.h @@ -131,7 +131,9 @@ private: bool _gyroCalInProgress; bool _magCalInProgress; bool _accelCalInProgress; - + bool _airspeedCalInProgress; + bool _levelCalInProgress; + bool _orientationCalDownSideDone; bool _orientationCalUpsideDownSideDone; bool _orientationCalLeftSideDone; diff --git a/src/AutoPilotPlugins/PX4/SensorsComponentSummary.qml b/src/AutoPilotPlugins/PX4/SensorsComponentSummary.qml index 675ea9426bc329acae534a810541c944659ccfc9..121bd9dff37730ab389de772bddc23cf93829a01 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponentSummary.qml +++ b/src/AutoPilotPlugins/PX4/SensorsComponentSummary.qml @@ -29,29 +29,29 @@ FactPanel { anchors.fill: parent VehicleSummaryRow { - labelText: qsTr("Compass 0:") + labelText: qsTr("Compass 0") valueText: mag0IdFact ? (mag0IdFact.value === 0 ? qsTr("Setup required") : qsTr("Ready")) : "" } VehicleSummaryRow { - labelText: qsTr("Compass 1:") + labelText: qsTr("Compass 1") visible: mag1IdFact.value !== 0 valueText: qsTr("Ready") } VehicleSummaryRow { - labelText: qsTr("Compass 2:") + labelText: qsTr("Compass 2") visible: mag2IdFact.value !== 0 valueText: qsTr("Ready") } VehicleSummaryRow { - labelText: qsTr("Gyro:") + labelText: qsTr("Gyro") valueText: gyro0IdFact ? (gyro0IdFact.value === 0 ? qsTr("Setup required") : qsTr("Ready")) : "" } VehicleSummaryRow { - labelText: qsTr("Accelerometer:") + labelText: qsTr("Accelerometer") valueText: accel0IdFact ? (accel0IdFact.value === 0 ? qsTr("Setup required") : qsTr("Ready")) : "" } } diff --git a/src/AutoPilotPlugins/PX4/SensorsComponentSummaryFixedWing.qml b/src/AutoPilotPlugins/PX4/SensorsComponentSummaryFixedWing.qml index 4670bbe3d4da60666096b9fceb9b3c2438194d35..3dcb546be5d361f385c8bb76211e3fa198d94fae 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponentSummaryFixedWing.qml +++ b/src/AutoPilotPlugins/PX4/SensorsComponentSummaryFixedWing.qml @@ -25,7 +25,7 @@ FactPanel { property Fact airspeedDisabledFact: controller.getParameterFact(-1, "FW_ARSP_MODE") property Fact airspeedBreakerFact: controller.getParameterFact(-1, "CBRK_AIRSPD_CHK") - property bool _airspeedVisible: airspeedDisabledFact.value === false && airspeedBreakerFact.value !== 162128 + property bool _airspeedVisible: airspeedDisabledFact.value == 0 && airspeedBreakerFact.value !== 162128 property bool _airspeedCalRequired: _airspeedVisible && dpressOffFact.value === 0 Column { diff --git a/src/AutoPilotPlugins/PX4/SensorsSetup.qml b/src/AutoPilotPlugins/PX4/SensorsSetup.qml index fdbca17573a97decdf398d44dd6875cddf8adcac..028973fbfea8ee11644716c10752c840f4497400 100644 --- a/src/AutoPilotPlugins/PX4/SensorsSetup.qml +++ b/src/AutoPilotPlugins/PX4/SensorsSetup.qml @@ -140,9 +140,7 @@ Item { onWaitingForCancelChanged: { if (controller.waitingForCancel) { - showMessage(qsTr("Calibration Cancel"), qsTr("Waiting for Vehicle to response to Cancel. This may take a few seconds."), 0) - } else { - hideDialog() + showDialog(waitForCancelDialogComponent, qsTr("Calibration Cancel"), qgcView.showDialogDefaultWidth, 0) } } } @@ -154,6 +152,24 @@ Item { } } + Component { + id: waitForCancelDialogComponent + + QGCViewMessage { + message: qsTr("Waiting for Vehicle to response to Cancel. This may take a few seconds.") + + Connections { + target: controller + + onWaitingForCancelChanged: { + if (!controller.waitingForCancel) { + hideDialog() + } + } + } + } + } + Component { id: preCalibrationDialogComponent @@ -349,11 +365,12 @@ Item { spacing: ScreenTools.defaultFontPixelHeight / 2 IndicatorButton { + property bool _hasMag: controller.parameterExists(-1, "SYS_HAS_MAG") ? controller.getParameterFact(-1, "SYS_HAS_MAG").value !== 0 : true id: compassButton width: _buttonWidth text: qsTr("Compass") indicatorGreen: cal_mag0_id.value !== 0 - visible: QGroundControl.corePlugin.options.showSensorCalibrationCompass && showSensorCalibrationCompass + visible: _hasMag && QGroundControl.corePlugin.options.showSensorCalibrationCompass && showSensorCalibrationCompass onClicked: { preCalibrationDialogType = "compass" @@ -410,7 +427,7 @@ Item { width: _buttonWidth text: qsTr("Airspeed") visible: (controller.vehicle.fixedWing || controller.vehicle.vtol) && - controller.getParameterFact(-1, "FW_ARSP_MODE").value === false && + controller.getParameterFact(-1, "FW_ARSP_MODE").value == 0 && controller.getParameterFact(-1, "CBRK_AIRSPD_CHK").value !== 162128 && QGroundControl.corePlugin.options.showSensorCalibrationAirspeed && showSensorCalibrationAirspeed diff --git a/src/Camera/QGCCameraControl.cc b/src/Camera/QGCCameraControl.cc index d2cfe911cff25310d33e873a5c2c5f8a57eec9a7..ff11f7f6328f1377b94350fb078a91b29ebf216d 100644 --- a/src/Camera/QGCCameraControl.cc +++ b/src/Camera/QGCCameraControl.cc @@ -790,7 +790,7 @@ QGCCameraControl::_loadSettings(const QDomNodeList nodeList) QVariant typedValue; QString errorString; if (metaData->convertAndValidateRaw(attr, true /* convertOnly */, typedValue, errorString)) { - metaData->setIncrement(typedValue.toDouble()); + metaData->setRawIncrement(typedValue.toDouble()); } else { qWarning() << "Invalid step value for" << factName << " type:" << metaData->type() diff --git a/src/FactSystem/Fact.cc b/src/FactSystem/Fact.cc index 3afce178fd287c5a40d106ffde28be1d9a1b8c1d..b4ecbb2f3fa1a0d161cf4a4af83a49993b697d35 100644 --- a/src/FactSystem/Fact.cc +++ b/src/FactSystem/Fact.cc @@ -8,6 +8,7 @@ ****************************************************************************/ #include "Fact.h" +#include "FactValueSliderListModel.h" #include "QGCMAVLink.h" #include "QGCApplication.h" #include "QGCCorePlugin.h" @@ -18,13 +19,14 @@ static const char* kMissingMetadata = "Meta data pointer missing"; Fact::Fact(QObject* parent) - : QObject(parent) - , _componentId(-1) - , _rawValue(0) - , _type(FactMetaData::valueTypeInt32) - , _metaData(NULL) - , _sendValueChangedSignals(true) + : QObject (parent) + , _componentId (-1) + , _rawValue (0) + , _type (FactMetaData::valueTypeInt32) + , _metaData (NULL) + , _sendValueChangedSignals (true) , _deferredValueChangeSignal(false) + , _valueSliderModel (NULL) { FactMetaData* metaData = new FactMetaData(_type, this); setMetaData(metaData); @@ -34,14 +36,15 @@ Fact::Fact(QObject* parent) } Fact::Fact(int componentId, QString name, FactMetaData::ValueType_t type, QObject* parent) - : QObject(parent) - , _name(name) - , _componentId(componentId) - , _rawValue(0) - , _type(type) - , _metaData(NULL) - , _sendValueChangedSignals(true) + : QObject (parent) + , _name (name) + , _componentId (componentId) + , _rawValue (0) + , _type (type) + , _metaData (NULL) + , _sendValueChangedSignals (true) , _deferredValueChangeSignal(false) + , _valueSliderModel (NULL) { FactMetaData* metaData = new FactMetaData(_type, this); setMetaData(metaData); @@ -57,9 +60,8 @@ Fact::Fact(FactMetaData* metaData, QObject* parent) , _metaData (NULL) , _sendValueChangedSignals (true) , _deferredValueChangeSignal(false) + , _valueSliderModel (NULL) { - // Allow core plugin a chance to override the default value - qgcApp()->toolbox()->corePlugin()->adjustSettingMetaData(*metaData); setMetaData(metaData, true /* setDefaultFromMetaData */); } @@ -78,7 +80,7 @@ const Fact& Fact::operator=(const Fact& other) _type = other._type; _sendValueChangedSignals = other._sendValueChangedSignals; _deferredValueChangeSignal = other._deferredValueChangeSignal; - + _valueSliderModel = NULL; if (_metaData && other._metaData) { *_metaData = *other._metaData; } else { @@ -633,10 +635,20 @@ QString Fact::enumOrValueString(void) return QString(); } -double Fact::increment(void) const +double Fact::rawIncrement(void) const { if (_metaData) { - return _metaData->increment(); + return _metaData->rawIncrement(); + } else { + qWarning() << kMissingMetadata << name(); + } + return std::numeric_limits::quiet_NaN(); +} + +double Fact::cookedIncrement(void) const +{ + if (_metaData) { + return _metaData->cookedIncrement(); } else { qWarning() << kMissingMetadata << name(); } @@ -682,3 +694,11 @@ bool Fact::volatileValue(void) const return false; } } + +FactValueSliderListModel* Fact::valueSliderModel(void) +{ + if (!_valueSliderModel) { + _valueSliderModel = new FactValueSliderListModel(*this); + } + return _valueSliderModel; +} diff --git a/src/FactSystem/Fact.h b/src/FactSystem/Fact.h index cc0336ee80de34e289b58ae89fc77d5a51bc86c5..74fe6b518a7c2b1fdba0d1146e19231c11828c29 100644 --- a/src/FactSystem/Fact.h +++ b/src/FactSystem/Fact.h @@ -20,6 +20,9 @@ #include #include #include +#include + +class FactValueSliderListModel; /// @brief A Fact is used to hold a single value within the system. class Fact : public QObject @@ -66,7 +69,7 @@ public: Q_PROPERTY(bool valueEqualsDefault READ valueEqualsDefault NOTIFY valueChanged) Q_PROPERTY(QString valueString READ cookedValueString NOTIFY valueChanged) Q_PROPERTY(QString enumOrValueString READ enumOrValueString NOTIFY valueChanged) - Q_PROPERTY(double increment READ increment CONSTANT) + Q_PROPERTY(double increment READ cookedIncrement CONSTANT) Q_PROPERTY(bool typeIsString READ typeIsString CONSTANT) Q_PROPERTY(bool typeIsBool READ typeIsBool CONSTANT) Q_PROPERTY(bool hasControl READ hasControl CONSTANT) @@ -115,7 +118,8 @@ public: bool valueEqualsDefault (void) const; bool rebootRequired (void) const; QString enumOrValueString (void); // This is not const, since an unknown value can modify the enum lists - double increment (void) const; + double rawIncrement (void) const; + double cookedIncrement (void) const; bool typeIsString (void) const { return type() == FactMetaData::valueTypeString; } bool typeIsBool (void) const { return type() == FactMetaData::valueTypeBool; } bool hasControl (void) const; @@ -123,6 +127,8 @@ public: bool writeOnly (void) const; bool volatileValue (void) const; + Q_INVOKABLE FactValueSliderListModel* valueSliderModel(void); + /// Returns the values as a string with full 18 digit precision if float/double. QString rawValueStringFullPrecision(void) const; @@ -192,6 +198,7 @@ protected: FactMetaData* _metaData; bool _sendValueChangedSignals; bool _deferredValueChangeSignal; + FactValueSliderListModel* _valueSliderModel; }; #endif diff --git a/src/FactSystem/FactControls/FactCheckBox.qml b/src/FactSystem/FactControls/FactCheckBox.qml index 4fca3e72b9d60624d0062d1ca10933f8161bda62..7ed1757ff41f3b91ae45ceb556f4dfead05365b9 100644 --- a/src/FactSystem/FactControls/FactCheckBox.qml +++ b/src/FactSystem/FactControls/FactCheckBox.qml @@ -8,14 +8,13 @@ import QGroundControl.Controls 1.0 QGCCheckBox { property Fact fact: Fact { } - + property variant checkedValue: 1 + property variant uncheckedValue: 0 checkedState: fact ? (fact.typeIsBool ? (fact.value === false ? Qt.Unchecked : Qt.Checked) : (fact.value === 0 ? Qt.Unchecked : Qt.Checked)) : Qt.Unchecked - text: qsTr("Label") - - onClicked: fact.value = checked ? 1 : 0 + onClicked: fact.value = (checked ? checkedValue : uncheckedValue) } diff --git a/src/FactSystem/FactControls/FactPanelController.cc b/src/FactSystem/FactControls/FactPanelController.cc index dc19cb91f0a6aa1e696dd61bf89293fbdf438c28..ea3e6aded19985f502c5c54b88c940a84d7be01a 100644 --- a/src/FactSystem/FactControls/FactPanelController.cc +++ b/src/FactSystem/FactControls/FactPanelController.cc @@ -88,6 +88,8 @@ void FactPanelController::_reportMissingParameter(int componentId, const QString QString missingParam = QString("%1:%2").arg(componentId).arg(name); + qCWarning(FactPanelControllerLog) << "Missing parameter:" << missingParam; + // If missing parameters a reported from the constructor of a derived class we // will not have access to _factPanel yet. Just record list of missing facts // in that case instead of notify. Once _factPanel is available they will be diff --git a/src/FactSystem/FactControls/FactTextField.qml b/src/FactSystem/FactControls/FactTextField.qml index f17ea565cb063c8c2c8c3766ec7a697355c1e7d0..ab66d9f2d5deb57da887ebaca77fa1f1078558a8 100644 --- a/src/FactSystem/FactControls/FactTextField.qml +++ b/src/FactSystem/FactControls/FactTextField.qml @@ -20,16 +20,11 @@ QGCTextField { property string _validateString - // At this point all Facts are numeric inputMethodHints: ((fact && fact.typeIsString) || ScreenTools.isiOS) ? Qt.ImhNone : // iOS numeric keyboard has no done button, we can't use it Qt.ImhFormattedNumbersOnly // Forces use of virtual numeric keyboard onEditingFinished: { - if (ScreenTools.isMobile) { - // Toss focus on mobile after Done on virtual keyboard. Prevent strange interactions. - focus = false - } if (typeof qgcView !== 'undefined' && qgcView) { var errorString = fact.validate(text, false /* convertOnly */) if (errorString === "") { diff --git a/src/FactSystem/FactControls/FactValueSlider.qml b/src/FactSystem/FactControls/FactValueSlider.qml new file mode 100644 index 0000000000000000000000000000000000000000..3483ec8b0fbb208f3d46fd9f141965263b8902eb --- /dev/null +++ b/src/FactSystem/FactControls/FactValueSlider.qml @@ -0,0 +1,144 @@ +import QtQuick 2.3 +import QtQuick.Controls 1.2 +import QtQuick.Dialogs 1.2 + +import QGroundControl.Palette 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.FactSystem 1.0 + +Rectangle { + height: _itemHeight + width: _totalSlots * _itemWidth + color: qgcPal.textField + + property Fact fact: undefined + property int digitCount: 4 ///< The number of digits to show for each value + property int incrementSlots: 1 ///< The number of visible slots to left/right of center value + + property int _totalDigitCount: digitCount + 1 + fact.units.length + property real _margins: (ScreenTools.implicitTextFieldHeight - ScreenTools.defaultFontPixelHeight) / 2 + property real _increment: fact.increment + property real _value: fact.value + property int _decimalPlaces: fact.decimalPlaces + property string _units: fact.units + property real _prevValue: _value - _increment + property real _nextValue: _value + _increment + property real _itemWidth: (_totalDigitCount * ScreenTools.defaultFontPixelWidth) + (_margins * 2) + property real _itemHeight: ScreenTools.implicitTextFieldHeight + property var _valueModel + property int _totalSlots: (incrementSlots * 2) + 1 + property int _currentIndex: _totalSlots / 2 + property int _currentRelativeIndex: _currentIndex + property int _prevIncrementSlots: incrementSlots + property int _nextIncrementSlots: incrementSlots + property int _selectionWidth: 3 + property var _model: fact.valueSliderModel() + property var _fact: fact + + QGCPalette { id: qgcPal; colorGroupEnabled: parent.enabled } + QGCPalette { id: qgcPalDisabled; colorGroupEnabled: false } + + function firstVisibleIndex() { + return valueListView.contentX / _itemWidth + } + + function recalcRelativeIndex() { + _currentRelativeIndex = _currentIndex - firstVisibleIndex() + _prevIncrementSlots = _currentRelativeIndex + _nextIncrementSlots = _totalSlots - _currentRelativeIndex - 1 + } + + function reset() { + valueListView.positionViewAtIndex(0, ListView.Beginning) + _currentIndex = _model.resetInitialValue() + valueListView.positionViewAtIndex(_currentIndex, ListView.Center) + recalcRelativeIndex() + } + + Component.onCompleted: { + valueListView.maximumFlickVelocity = valueListView.maximumFlickVelocity / 2 + reset() + } + + Connections { + target: _fact + onValueChanged: reset() + } + + Component { + id: editDialogComponent + + ParameterEditorDialog { + fact: _fact + } + } + + QGCListView { + id: valueListView + anchors.fill: parent + orientation: ListView.Horizontal + snapMode: ListView.SnapToItem + clip: true + model: _model + + delegate: QGCLabel { + width: _itemWidth + height: _itemHeight + verticalAlignment: Text.AlignVCenter + horizontalAlignment: Text.AlignHCenter + text: value + " " + _units + color: qgcPal.textFieldText + + MouseArea { + anchors.fill: parent + onClicked: { + valueListView.focus = true + if (_currentIndex === index) { + qgcView.showDialog(editDialogComponent, qsTr("Value Details"), qgcView.showDialogDefaultWidth, StandardButton.Save | StandardButton.Cancel) + } else { + _currentIndex = index + valueListView.positionViewAtIndex(_currentIndex, ListView.Center) + recalcRelativeIndex() + fact.value = value + } + } + } + } + + onMovementStarted: valueListView.focus = true + + onMovementEnded: { + _currentIndex = firstVisibleIndex() + _currentRelativeIndex + fact.value = _model.valueAtModelIndex(_currentIndex) + } + } + + Rectangle { + id: leftOverlay + width: _itemWidth * _prevIncrementSlots + height: _itemHeight + color: qgcPal.textField + opacity: 0.5 + } + + Rectangle { + width: _itemWidth * _nextIncrementSlots + height: _itemHeight + anchors.right: parent.right + color: qgcPal.textField + opacity: 0.5 + } + + Rectangle { + x: _currentRelativeIndex * _itemWidth - _borderWidth + y: -_borderWidth + width: _itemWidth + (_borderWidth * 2) + height: _itemHeight + (_borderWidth * 2) + border.width: _borderWidth + border.color: qgcPal.brandingBlue + color: "transparent" + + readonly property int _borderWidth: 3 + } +} diff --git a/src/FactSystem/FactControls/qmldir b/src/FactSystem/FactControls/qmldir index 6bf7d2359c1bfc007565d9bea2571d2754b128a5..02efebba4da263b96e4b77562d843e954197717e 100644 --- a/src/FactSystem/FactControls/qmldir +++ b/src/FactSystem/FactControls/qmldir @@ -8,3 +8,4 @@ FactPanel 1.0 FactPanel.qml FactTextField 1.0 FactTextField.qml FactTextFieldGrid 1.0 FactTextFieldGrid.qml FactTextFieldRow 1.0 FactTextFieldRow.qml +FactValueSlider 1.0 FactValueSlider.qml diff --git a/src/FactSystem/FactGroup.cc b/src/FactSystem/FactGroup.cc index ee41fdc34c592a495dec30d3797b16fe7e6bf768..ad11c2400d9d47fccd35b9c084955e613b35e62b 100644 --- a/src/FactSystem/FactGroup.cc +++ b/src/FactSystem/FactGroup.cc @@ -105,6 +105,7 @@ void FactGroup::_addFact(Fact* fact, const QString& name) fact->setMetaData(_nameToFactMetaDataMap[name]); } _nameToFactMap[name] = fact; + _factNames.append(name); } void FactGroup::_addFactGroup(FactGroup* factGroup, const QString& name) diff --git a/src/FactSystem/FactGroup.h b/src/FactSystem/FactGroup.h index 605844abcd6a02666c793d10c176159824ac02c8..78a97fcb60c79ddc140b68a7476d50ae2e8930f2 100644 --- a/src/FactSystem/FactGroup.h +++ b/src/FactSystem/FactGroup.h @@ -38,7 +38,7 @@ public: /// @return FactGroup for specified name, NULL if not found Q_INVOKABLE FactGroup* getFactGroup(const QString& name); - QStringList factNames(void) const { return _nameToFactMap.keys(); } + QStringList factNames(void) const { return _factNames; } QStringList factGroupNames(void) const { return _nameToFactGroupMap.keys(); } protected: @@ -59,7 +59,7 @@ protected: QMap _nameToFactMap; QMap _nameToFactGroupMap; QMap _nameToFactMetaDataMap; - + QStringList _factNames; }; #endif diff --git a/src/FactSystem/FactMetaData.cc b/src/FactSystem/FactMetaData.cc index 167c68101f4ef653021a07b11cdd00eacbbf03b3..79514d03f1cd53af843ed4852148f8ff897ee3ab 100644 --- a/src/FactSystem/FactMetaData.cc +++ b/src/FactSystem/FactMetaData.cc @@ -78,6 +78,7 @@ const char* FactMetaData::_defaultValueJsonKey = "defaultValue"; const char* FactMetaData::_mobileDefaultValueJsonKey = "mobileDefaultValue"; const char* FactMetaData::_minJsonKey = "min"; const char* FactMetaData::_maxJsonKey = "max"; +const char* FactMetaData::_incrementJsonKey = "increment"; const char* FactMetaData::_hasControlJsonKey = "control"; FactMetaData::FactMetaData(QObject* parent) @@ -93,7 +94,7 @@ FactMetaData::FactMetaData(QObject* parent) , _rawTranslator (_defaultTranslator) , _cookedTranslator (_defaultTranslator) , _rebootRequired (false) - , _increment (std::numeric_limits::quiet_NaN()) + , _rawIncrement (std::numeric_limits::quiet_NaN()) , _hasControl (true) , _readOnly (false) , _writeOnly (false) @@ -116,7 +117,7 @@ FactMetaData::FactMetaData(ValueType_t type, QObject* parent) , _rawTranslator (_defaultTranslator) , _cookedTranslator (_defaultTranslator) , _rebootRequired (false) - , _increment (std::numeric_limits::quiet_NaN()) + , _rawIncrement (std::numeric_limits::quiet_NaN()) , _hasControl (true) , _readOnly (false) , _writeOnly (false) @@ -146,7 +147,7 @@ FactMetaData::FactMetaData(ValueType_t type, const QString name, QObject* parent , _rawTranslator (_defaultTranslator) , _cookedTranslator (_defaultTranslator) , _rebootRequired (false) - , _increment (std::numeric_limits::quiet_NaN()) + , _rawIncrement (std::numeric_limits::quiet_NaN()) , _hasControl (true) , _readOnly (false) , _writeOnly (false) @@ -180,7 +181,7 @@ const FactMetaData& FactMetaData::operator=(const FactMetaData& other) _rawTranslator = other._rawTranslator; _cookedTranslator = other._cookedTranslator; _rebootRequired = other._rebootRequired; - _increment = other._increment; + _rawIncrement = other._rawIncrement; _hasControl = other._hasControl; _readOnly = other._readOnly; _writeOnly = other._writeOnly; @@ -615,7 +616,7 @@ void FactMetaData::setBuiltInTranslator(void) for (size_t i=0; irawUnits == _rawUnits.toLower()) { + if (pBuiltInTranslation->rawUnits.toLower() == _rawUnits.toLower()) { _cookedUnits = pBuiltInTranslation->cookedUnits; setTranslators(pBuiltInTranslation->rawTranslator, pBuiltInTranslation->cookedTranslator); return; @@ -874,13 +875,33 @@ void FactMetaData::_setAppSettingsTranslators(void) if (!_enumStrings.count() && (type() == valueTypeDouble || type() == valueTypeFloat)) { for (size_t i=0; irawUnits == _rawUnits.toLower() && ( - (pAppSettingsTranslation->unitType == UnitTemperature && pAppSettingsTranslation->unitOption == qgcApp()->toolbox()->settingsManager()->unitsSettings()->temperatureUnits()->rawValue().toUInt()) || - (pAppSettingsTranslation->unitType == UnitSpeed && pAppSettingsTranslation->unitOption == qgcApp()->toolbox()->settingsManager()->unitsSettings()->speedUnits()->rawValue().toUInt()) || - (pAppSettingsTranslation->unitType == UnitDistance && pAppSettingsTranslation->unitOption == qgcApp()->toolbox()->settingsManager()->unitsSettings()->distanceUnits()->rawValue().toUInt()) || - (pAppSettingsTranslation->unitType == UnitArea && pAppSettingsTranslation->unitOption == qgcApp()->toolbox()->settingsManager()->unitsSettings()->areaUnits()->rawValue().toUInt()))) - { - _cookedUnits = pAppSettingsTranslation->cookedUnits; + + if (_rawUnits.toLower() != pAppSettingsTranslation->rawUnits.toLower()) { + continue; + } + + UnitsSettings* settings = qgcApp()->toolbox()->settingsManager()->unitsSettings(); + uint settingsUnits = 0; + + switch (pAppSettingsTranslation->unitType) { + case UnitDistance: + settingsUnits = settings->distanceUnits()->rawValue().toUInt(); + break; + case UnitSpeed: + settingsUnits = settings->speedUnits()->rawValue().toUInt(); + break; + case UnitArea: + settingsUnits = settings->areaUnits()->rawValue().toUInt(); + break; + case UnitTemperature: + settingsUnits = settings->temperatureUnits()->rawValue().toUInt(); + break; + default: + break; + } + + if (settingsUnits == pAppSettingsTranslation->unitOption) { + _cookedUnits = pAppSettingsTranslation->cookedUnits; setTranslators(pAppSettingsTranslation->rawTranslator, pAppSettingsTranslation->cookedTranslator); return; } @@ -892,8 +913,15 @@ const FactMetaData::AppSettingsTranslation_s* FactMetaData::_findAppSettingsDist { for (size_t i=0; irawUnits == rawUnits.toLower() && - (pAppSettingsTranslation->unitType == UnitDistance && pAppSettingsTranslation->unitOption == qgcApp()->toolbox()->settingsManager()->unitsSettings()->distanceUnits()->rawValue().toUInt())) { + + if (rawUnits.toLower() != pAppSettingsTranslation->rawUnits.toLower()) { + continue; + } + + uint settingsUnits = qgcApp()->toolbox()->settingsManager()->unitsSettings()->areaUnits()->rawValue().toUInt(); + + if (pAppSettingsTranslation->unitType == UnitDistance + && pAppSettingsTranslation->unitOption == settingsUnits) { return pAppSettingsTranslation; } } @@ -904,8 +932,15 @@ const FactMetaData::AppSettingsTranslation_s* FactMetaData::_findAppSettingsArea { for (size_t i=0; irawUnits == rawUnits.toLower() && - (pAppSettingsTranslation->unitType == UnitArea && pAppSettingsTranslation->unitOption == qgcApp()->toolbox()->settingsManager()->unitsSettings()->areaUnits()->rawValue().toUInt())) { + + if (rawUnits.toLower() != pAppSettingsTranslation->rawUnits.toLower()) { + continue; + } + + uint settingsUnits = qgcApp()->toolbox()->settingsManager()->unitsSettings()->areaUnits()->rawValue().toUInt(); + + if (pAppSettingsTranslation->unitType == UnitArea + && pAppSettingsTranslation->unitOption == settingsUnits) { return pAppSettingsTranslation; } } @@ -973,13 +1008,18 @@ QString FactMetaData::appSettingsAreaUnitsString(void) } } +double FactMetaData::cookedIncrement(void) const +{ + return _rawTranslator(this->rawIncrement()).toDouble(); +} + int FactMetaData::decimalPlaces(void) const { int actualDecimalPlaces = defaultDecimalPlaces; int incrementDecimalPlaces = unknownDecimalPlaces; // First determine decimal places from increment - double increment = _rawTranslator(this->increment()).toDouble(); + double increment = _rawTranslator(this->rawIncrement()).toDouble(); if (!qIsNaN(increment)) { double integralPart; @@ -1024,12 +1064,18 @@ FactMetaData* FactMetaData::createFromJsonObject(const QJsonObject& json, QObjec return new FactMetaData(valueTypeUint32, metaDataParent); } - // Validate key types - QStringList keys; - QList types; - keys << _nameJsonKey << _decimalPlacesJsonKey << _typeJsonKey << _shortDescriptionJsonKey << _longDescriptionJsonKey << _unitsJsonKey << _minJsonKey << _maxJsonKey << _hasControlJsonKey; - types << QJsonValue::String << QJsonValue::Double << QJsonValue::String << QJsonValue::String << QJsonValue::String << QJsonValue::String << QJsonValue::Double << QJsonValue::Double << QJsonValue::Bool; - if (!JsonHelper::validateKeyTypes(json, keys, types, errorString)) { + QList keyInfoList = { + { _nameJsonKey, QJsonValue::String, true }, + { _typeJsonKey, QJsonValue::String, true }, + { _shortDescriptionJsonKey, QJsonValue::String, false }, + { _longDescriptionJsonKey, QJsonValue::String, false }, + { _unitsJsonKey, QJsonValue::String, false }, + { _decimalPlacesJsonKey, QJsonValue::Double, false }, + { _minJsonKey, QJsonValue::Double, false }, + { _maxJsonKey, QJsonValue::Double, false }, + { _hasControlJsonKey, QJsonValue::Bool, false }, + }; + if (!JsonHelper::validateKeys(json, keyInfoList, errorString)) { qWarning() << errorString; return new FactMetaData(valueTypeUint32, metaDataParent); } @@ -1095,6 +1141,20 @@ FactMetaData* FactMetaData::createFromJsonObject(const QJsonObject& json, QObjec } } + if (json.contains(_incrementJsonKey)) { + QVariant typedValue; + QString errorString; + QVariant initialValue = json[_incrementJsonKey].toVariant(); + if (metaData->convertAndValidateRaw(initialValue, true /* convertOnly */, typedValue, errorString)) { + metaData->setRawIncrement(typedValue.toDouble()); + } else { + qWarning() << "Invalid increment value, name:" << metaData->name() + << " type:" << metaData->type() + << " value:" << initialValue + << " error:" << errorString; + } + } + if (json.contains(_minJsonKey)) { QVariant typedValue; QString errorString; diff --git a/src/FactSystem/FactMetaData.h b/src/FactSystem/FactMetaData.h index 4b298ca2422b2407b6b9c3b0fc5e814fc48756f8..da0ee47e155944d61122b65945ac9ee6a4b48ca5 100644 --- a/src/FactSystem/FactMetaData.h +++ b/src/FactSystem/FactMetaData.h @@ -111,7 +111,8 @@ public: /// Amount to increment value when used in controls such as spin button or slider with detents. /// NaN for no increment available. - double increment (void) const { return _increment; } + double rawIncrement (void) const { return _rawIncrement; } + double cookedIncrement (void) const; Translator rawTranslator (void) const { return _rawTranslator; } Translator cookedTranslator (void) const { return _cookedTranslator; } @@ -135,7 +136,7 @@ public: void setShortDescription(const QString& shortDescription) { _shortDescription = shortDescription; } void setRawUnits (const QString& rawUnits); void setRebootRequired (bool rebootRequired) { _rebootRequired = rebootRequired; } - void setIncrement (double increment) { _increment = increment; } + void setRawIncrement (double increment) { _rawIncrement = increment; } void setHasControl (bool bValue) { _hasControl = bValue; } void setReadOnly (bool bValue) { _readOnly = bValue; } void setWriteOnly (bool bValue) { _writeOnly = bValue; } @@ -215,7 +216,7 @@ private: }; struct AppSettingsTranslation_s { - const char* rawUnits; + QString rawUnits; const char* cookedUnits; UnitTypes unitType; uint32_t unitOption; @@ -248,7 +249,7 @@ private: Translator _rawTranslator; Translator _cookedTranslator; bool _rebootRequired; - double _increment; + double _rawIncrement; bool _hasControl; bool _readOnly; bool _writeOnly; @@ -264,7 +265,7 @@ private: } constants; struct BuiltInTranslation_s { - const char* rawUnits; + QString rawUnits; const char* cookedUnits; Translator rawTranslator; Translator cookedTranslator; @@ -285,6 +286,7 @@ private: static const char* _mobileDefaultValueJsonKey; static const char* _minJsonKey; static const char* _maxJsonKey; + static const char* _incrementJsonKey; static const char* _hasControlJsonKey; }; diff --git a/src/FactSystem/FactSystem.h b/src/FactSystem/FactSystem.h index e4321cddf09de7b8fe2779bf87e52a69b55e937e..29089ceab3e25f62be9531ac1b39af08d2719113 100644 --- a/src/FactSystem/FactSystem.h +++ b/src/FactSystem/FactSystem.h @@ -20,10 +20,6 @@ /// The components of the FactSystem are a Fact which holds an individual value. FactMetaData holds /// additional meta data associated with a Fact such as description, min/max ranges and so forth. -/// The FactValidator object is a QML validator which validates input according to the FactMetaData -/// settings. Client code can then use this system to expose sets of Facts to QML code. An example -/// of this is the PX4ParameterMetaData onbject which is part of the PX4 AutoPilot plugin. It exposes -/// the firmware parameters to QML such that you can bind QML ui elements directly to parameters. class FactSystem : public QGCTool { diff --git a/src/FactSystem/FactValidator.cc b/src/FactSystem/FactValidator.cc deleted file mode 100644 index c7f7e9696a99056b1cffca0ec3eb91406e0b0c63..0000000000000000000000000000000000000000 --- a/src/FactSystem/FactValidator.cc +++ /dev/null @@ -1,33 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2016 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - - -/// @file -/// @author Don Gagne - -#include "FactValidator.h" - -FactValidator::FactValidator(QObject* parent) : - QValidator(parent) -{ - -} - -void FactValidator::fixup(QString& input) const -{ - Q_UNUSED(input); -} - -FactValidator::State FactValidator::validate(QString& input, int& pos) const -{ - Q_UNUSED(input); - Q_UNUSED(pos); - - return Acceptable; -} diff --git a/src/FactSystem/FactValidator.h b/src/FactSystem/FactValidator.h deleted file mode 100644 index d81a117bc8731cfdbfe7f2bb0f3279b86b01d585..0000000000000000000000000000000000000000 --- a/src/FactSystem/FactValidator.h +++ /dev/null @@ -1,56 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2016 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - - -/// @file -/// @author Don Gagne - -#ifndef FactValidator_H -#define FactValidator_H - -#include - -class Fact; - -/// QML Validator for Facts (Work In Progress) -/// -/// The validator uses the FactMetaData to impose restrictions on the input. It is used as follows: -/// @code{.unparsed} -/// TextInput { -/// validator: FactValidator { fact: parameters["RC_MAP_THROTTLE"]; } -/// } -/// @endcode -class FactValidator : public QValidator -{ - Q_OBJECT - - Q_PROPERTY(Fact* fact READ fact WRITE setFact) - -public: - FactValidator(QObject* parent = NULL); - - // Property system methods - - /// Read accessor for fact property - Fact* fact(void) { return _fact; } - - /// Write accessor for fact property - void setFact(Fact* fact) { _fact = fact; } - - /// Override from QValidator - virtual void fixup(QString& input) const; - - /// Override from QValidator - virtual State validate(QString& input, int& pos) const; - -private: - Fact* _fact; ///< Fact that the validator is working on -}; - -#endif \ No newline at end of file diff --git a/src/FactSystem/FactValueSliderListModel.cc b/src/FactSystem/FactValueSliderListModel.cc new file mode 100644 index 0000000000000000000000000000000000000000..7bce4adb88a80e2c082d2b55ac61678335e68e1c --- /dev/null +++ b/src/FactSystem/FactValueSliderListModel.cc @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "FactValueSliderListModel.h" + +#include +#include +#include + +#include + +const int FactValueSliderListModel::_valueRole = Qt::UserRole; +const int FactValueSliderListModel::_valueIndexRole = Qt::UserRole + 1; + +FactValueSliderListModel::FactValueSliderListModel(Fact& fact, QObject* parent) + : QAbstractListModel (parent) + , _fact (fact) + , _cValues (0) + , _firstValueIndexInWindow (0) + , _initialValueIndex (0) + , _cPrevValues (0) + , _cNextValues (0) + , _initialValue (0) + , _initialValueRounded (0) + , _increment (0) +{ + QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); +} + +FactValueSliderListModel::~FactValueSliderListModel() +{ +} + +int FactValueSliderListModel::resetInitialValue(void) +{ + if (_cValues > 0) { + // Remove any old rows + beginRemoveRows(QModelIndex(), 0, _cValues - 1); + _cValues = 0; + endRemoveRows(); + } + + _initialValue = _fact.cookedValue().toDouble(); + _initialValueRounded = qRound(_initialValue); + if (qRound(_fact.rawIncrement()) == _fact.rawIncrement()) { + _increment = qRound(_fact.cookedIncrement()); + } else { + _increment = _fact.cookedIncrement(); + } + _cPrevValues = qMin((_initialValue - _fact.cookedMin().toDouble()), 1000.0) / _increment; + _cNextValues = qMin((_fact.cookedMax().toDouble() - _initialValue), 1000.0) / _increment; + _initialValueIndex = _cPrevValues; + + int totalValueCount = _cPrevValues + 1 + _cNextValues; + beginInsertRows(QModelIndex(), 0, totalValueCount - 1); + _cValues = totalValueCount; + endInsertRows(); + + return _initialValueIndex; +} + +int FactValueSliderListModel::rowCount(const QModelIndex& parent) const +{ + Q_UNUSED(parent); + + return _cValues; +} + +QVariant FactValueSliderListModel::data(const QModelIndex &index, int role) const +{ + Q_UNUSED(role); + + if (!index.isValid()) { + return QVariant(); + } + + int valueIndex = index.row(); + if (valueIndex >= _cValues) { + return QVariant(); + } + + if (role == _valueRole) { + double value; + int cIncrementCount = valueIndex - _initialValueIndex; + if (cIncrementCount == 0) { + value = _initialValue; + } else { + value = _initialValueRounded + (cIncrementCount * _increment); + } + double precision = qPow(10, _fact.decimalPlaces()); + double atPrecision = qRound(value * precision) / precision; + //qDebug() << value << precision << atPrecision << _fact.decimalPlaces() << _fact.name(); + return QVariant(atPrecision); + } else if (role == _valueIndexRole) { + return QVariant::fromValue(valueIndex); + } else { + return QVariant(); + } + + +} + +QHash FactValueSliderListModel::roleNames(void) const +{ + QHash hash; + + hash[_valueRole] = "value"; + hash[_valueIndexRole] = "valueIndex"; + + return hash; +} + +double FactValueSliderListModel::valueAtModelIndex(int index) +{ + return data(createIndex(index, 0), _valueRole).toDouble(); + +} + +int FactValueSliderListModel::valueIndexAtModelIndex(int index) +{ + return data(createIndex(index, 0), _valueIndexRole).toInt(); +} diff --git a/src/FactSystem/FactValueSliderListModel.h b/src/FactSystem/FactValueSliderListModel.h new file mode 100644 index 0000000000000000000000000000000000000000..3f35b7156979beae1620ada3bd0d4fa1f72b4891 --- /dev/null +++ b/src/FactSystem/FactValueSliderListModel.h @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include + +#include "Fact.h" + +/// Provides a list model of values for incrementing/decrementing the value of a Fact +class FactValueSliderListModel : public QAbstractListModel +{ + Q_OBJECT + +public: + FactValueSliderListModel(Fact& fact, QObject* parent = NULL); + ~FactValueSliderListModel(); + + Q_INVOKABLE int resetInitialValue(void); + Q_INVOKABLE double valueAtModelIndex(int index); + Q_INVOKABLE int valueIndexAtModelIndex(int index); + +private: + // Overrides from QAbstractListModel + int rowCount(const QModelIndex & parent = QModelIndex()) const override; + QVariant data(const QModelIndex & index, int role = Qt::DisplayRole) const override; + QHash roleNames(void) const override; + + Fact& _fact; + int _cValues; + int _firstValueIndexInWindow; + int _initialValueIndex; + int _cPrevValues; + int _cNextValues; + int _windowSize; + double _initialValue; + double _initialValueRounded; + double _increment; + + static const int _valueRole; + static const int _valueIndexRole; +}; diff --git a/src/FactSystem/ParameterManager.cc b/src/FactSystem/ParameterManager.cc index bccb88ec47a45f2df71336303beaf849c5c70df3..78fe43f0c55e5b250c0388f381946a6be8d49b50 100644 --- a/src/FactSystem/ParameterManager.cc +++ b/src/FactSystem/ParameterManager.cc @@ -22,12 +22,9 @@ #include #include -/* types for local parameter cache */ -typedef QPair ParamTypeVal; -typedef QMap CacheMapName2ParamTypeVal; - -QGC_LOGGING_CATEGORY(ParameterManagerVerbose1Log, "ParameterManagerVerbose1Log") -QGC_LOGGING_CATEGORY(ParameterManagerVerbose2Log, "ParameterManagerVerbose2Log") +QGC_LOGGING_CATEGORY(ParameterManagerVerbose1Log, "ParameterManagerVerbose1Log") +QGC_LOGGING_CATEGORY(ParameterManagerVerbose2Log, "ParameterManagerVerbose2Log") +QGC_LOGGING_CATEGORY(ParameterManagerDebugCacheFailureLog, "ParameterManagerDebugCacheFailureLog") // Turn on to debug parameter cache crc misses Fact ParameterManager::_defaultFact; @@ -89,7 +86,7 @@ ParameterManager::ParameterManager(Vehicle* vehicle) _waitingForDefaultComponent = false; emit parametersReadyChanged(_parametersReady); emit missingParametersChanged(_missingParameters); - } else { + } else if (!_logReplay){ refreshAllParameters(); } } @@ -151,6 +148,24 @@ void ParameterManager::_parameterUpdate(int vehicleId, int componentId, QString return; } + // Used to debug cache crc misses (turn on ParameterManagerDebugCacheFailureLog) + if (!_initialLoadComplete && !_logReplay && _debugCacheCRC.contains(componentId) && _debugCacheCRC[componentId]) { + if (_debugCacheMap[componentId].contains(parameterName)) { + const ParamTypeVal& cacheParamTypeVal = _debugCacheMap[componentId][parameterName]; + size_t dataSize = FactMetaData::typeToSize(static_cast(cacheParamTypeVal.first)); + const void *cacheData = cacheParamTypeVal.second.constData(); + + const void *vehicleData = value.constData(); + + if (memcmp(cacheData, vehicleData, dataSize)) { + qDebug() << "Cache/Vehicle values differ for name:cache:actual" << parameterName << value << cacheParamTypeVal.second; + } + _debugCacheParamSeen[componentId][parameterName] = true; + } else { + qDebug() << "Parameter missing from cache" << parameterName; + } + } + _initialRequestTimeoutTimer.stop(); _waitingParamTimeoutTimer.stop(); @@ -340,15 +355,10 @@ void ParameterManager::_parameterUpdate(int vehicleId, int componentId, QString _setupCategoryMap(); } - if (_prevWaitingWriteParamNameCount != 0 && waitingWriteParamNameCount == 0) { - // If all the writes just finished the vehicle is up to date, so persist. - _saveToEEPROM(); - } - // Update param cache. The param cache is only used on PX4 Firmware since ArduPilot and Solo have volatile params // which invalidate the cache. The Solo also streams param updates in flight for things like gimbal values // which in turn causes a perf problem with all the param cache updates. - if (_vehicle->px4Firmware()) { + if (!_logReplay && _vehicle->px4Firmware()) { if (_prevWaitingReadParamIndexCount + _prevWaitingReadParamNameCount != 0 && readWaitingParamCount == 0) { // All reads just finished, update the cache _writeLocalParamCache(vehicleId, componentId); @@ -594,6 +604,10 @@ bool ParameterManager::_fillIndexBatchQueue(bool waitingParamTimeout) void ParameterManager::_waitingParamTimeout(void) { + if (_logReplay) { + return; + } + bool paramsRequested = false; const int maxBatchSize = 10; int batchCount = 0; @@ -875,22 +889,13 @@ void ParameterManager::_tryCacheHashLoad(int vehicleId, int componentId, QVarian _parameterSetMajorVersion = -1; _clearMetaData(); qCInfo(ParameterManagerLog) << "Parameters cache match failed" << qPrintable(QFileInfo(cacheFile).absoluteFilePath()); - } -} - -void ParameterManager::_saveToEEPROM(void) -{ - if (_saveRequired) { - _saveRequired = false; - if (_vehicle->firmwarePlugin()->isCapable(_vehicle, FirmwarePlugin::MavCmdPreflightStorageCapability)) { - _vehicle->sendMavCommand(MAV_COMP_ID_ALL, - MAV_CMD_PREFLIGHT_STORAGE, - true, // showError - 1, // Write parameters to EEPROM - -1); // Don't do anything with mission storage - qCDebug(ParameterManagerLog) << _logVehiclePrefix() << "_saveToEEPROM"; - } else { - qCDebug(ParameterManagerLog) << _logVehiclePrefix() << "_saveToEEPROM skipped due to FirmwarePlugin::isCapable"; + if (ParameterManagerDebugCacheFailureLog().isDebugEnabled()) { + _debugCacheCRC[componentId] = true; + _debugCacheMap[componentId] = cacheMap; + foreach (const QString& name, cacheMap.keys()) { + _debugCacheParamSeen[componentId][name] = false; + } + qgcApp()->showMessage(tr("Parameter cache CRC match failed")); } } } @@ -1110,6 +1115,18 @@ void ParameterManager::_checkInitialLoadComplete(void) // We aren't waiting for any more initial parameter updates, initial parameter loading is complete _initialLoadComplete = true; + // Parameter cache crc failure debugging + foreach (int componentId, _debugCacheParamSeen.keys()) { + if (!_logReplay && _debugCacheCRC.contains(componentId) && _debugCacheCRC[componentId]) { + foreach (const QString& paramName, _debugCacheParamSeen[componentId].keys()) { + if (!_debugCacheParamSeen[componentId][paramName]) { + qDebug() << "Parameter in cache but not on vehicle componentId:Name" << componentId << paramName; + } + } + } + } + _debugCacheCRC.clear(); + qCDebug(ParameterManagerLog) << _logVehiclePrefix() << "Initial load complete"; // Check for index based load failures @@ -1423,6 +1440,7 @@ void ParameterManager::_loadOfflineEditingParams(void) _setupCategoryMap(); _parametersReady = true; _initialLoadComplete = true; + _debugCacheCRC.clear(); } void ParameterManager::saveToJson(int componentId, const QStringList& paramsToSave, QJsonObject& saveObject) diff --git a/src/FactSystem/ParameterManager.h b/src/FactSystem/ParameterManager.h index d0f45b16fb4259b97ca136c3e9c09d4382f27281..30dd672b92af317a7043affea7d4b6e5bd88f331 100644 --- a/src/FactSystem/ParameterManager.h +++ b/src/FactSystem/ParameterManager.h @@ -7,9 +7,7 @@ * ****************************************************************************/ - -#ifndef ParameterManager_H -#define ParameterManager_H +#pragma once #include #include @@ -25,32 +23,26 @@ #include "QGCMAVLink.h" #include "Vehicle.h" -/// @file -/// @author Don Gagne - Q_DECLARE_LOGGING_CATEGORY(ParameterManagerVerbose1Log) Q_DECLARE_LOGGING_CATEGORY(ParameterManagerVerbose2Log) +Q_DECLARE_LOGGING_CATEGORY(ParameterManagerDebugCacheFailureLog) -/// Connects to Parameter Manager to load/update Facts class ParameterManager : public QObject { Q_OBJECT public: /// @param uas Uas which this set of facts is associated with - ParameterManager(Vehicle* vehicle); - ~ParameterManager(); + ParameterManager (Vehicle* vehicle); + ~ParameterManager (); - /// true: Parameters are ready for use - Q_PROPERTY(bool parametersReady READ parametersReady NOTIFY parametersReadyChanged) - bool parametersReady(void) { return _parametersReady; } + Q_PROPERTY(bool parametersReady READ parametersReady NOTIFY parametersReadyChanged) ///< true: Parameters are ready for use + Q_PROPERTY(bool missingParameters READ missingParameters NOTIFY missingParametersChanged) ///< true: Parameters are missing from firmware response, false: all parameters received from firmware + Q_PROPERTY(double loadProgress READ loadProgress NOTIFY loadProgressChanged) - /// true: Parameters are missing from firmware response, false: all parameters received from firmware - Q_PROPERTY(bool missingParameters READ missingParameters NOTIFY missingParametersChanged) - bool missingParameters(void) { return _missingParameters; } - - Q_PROPERTY(double loadProgress READ loadProgress NOTIFY loadProgressChanged) - double loadProgress(void) const { return _loadProgress; } + bool parametersReady (void) const { return _parametersReady; } + bool missingParameters (void) const { return _missingParameters; } + double loadProgress (void) const { return _loadProgress; } /// @return Directory of parameter caches static QDir parameterCacheDir(); @@ -58,7 +50,6 @@ public: /// @return Location of parameter cache file static QString parameterCacheFile(int vehicleId, int componentId); - /// Re-request the full set of parameters from the autopilot void refreshAllParameters(uint8_t componentID = MAV_COMP_ID_ALL); @@ -153,7 +144,6 @@ private: MAV_PARAM_TYPE _factTypeToMavType(FactMetaData::ValueType_t factType); FactMetaData::ValueType_t _mavTypeToFactType(MAV_PARAM_TYPE mavType); - void _saveToEEPROM(void); void _checkInitialLoadComplete(void); /// First mapping is by component id @@ -175,6 +165,13 @@ private: int _parameterSetMajorVersion; ///< Version for parameter set, -1 if not known QObject* _parameterMetaData; ///< Opaque data from FirmwarePlugin::loadParameterMetaDataCall + typedef QPair ParamTypeVal; + typedef QMap CacheMapName2ParamTypeVal; + + QMap _debugCacheCRC; ///< true: debug cache crc failure + QMap _debugCacheMap; + QMap> _debugCacheParamSeen; + // Wait counts from previous parameter update cycle int _prevWaitingReadParamIndexCount; int _prevWaitingReadParamNameCount; @@ -210,5 +207,3 @@ private: static const char* _jsonParamNameKey; static const char* _jsonParamValueKey; }; - -#endif diff --git a/src/FactSystem/SettingsFact.cc b/src/FactSystem/SettingsFact.cc index 81a5db8728ffcd76420705767f78c6c30652efef..1d8f4f58f7db4a1fcb44dd8e025d44b6acecfa23 100644 --- a/src/FactSystem/SettingsFact.cc +++ b/src/FactSystem/SettingsFact.cc @@ -20,19 +20,19 @@ SettingsFact::SettingsFact(QObject* parent) QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); } -SettingsFact::SettingsFact(QString settingGroup, FactMetaData* metaData, QObject* parent) - : Fact(0, metaData->name(), metaData->type(), parent) - , _settingGroup(settingGroup) - , _visible(true) +SettingsFact::SettingsFact(QString settingsGroup, FactMetaData* metaData, QObject* parent) + : Fact (0, metaData->name(), metaData->type(), parent) + , _settingsGroup(settingsGroup) + , _visible (true) { QSettings settings; - if (!_settingGroup.isEmpty()) { - settings.beginGroup(_settingGroup); + if (!_settingsGroup.isEmpty()) { + settings.beginGroup(_settingsGroup); } // Allow core plugin a chance to override the default value - _visible = qgcApp()->toolbox()->corePlugin()->adjustSettingMetaData(*metaData); + _visible = qgcApp()->toolbox()->corePlugin()->adjustSettingMetaData(settingsGroup, *metaData); setMetaData(metaData); QVariant rawDefaultValue = metaData->rawDefaultValue(); @@ -60,7 +60,7 @@ const SettingsFact& SettingsFact::operator=(const SettingsFact& other) { Fact::operator=(other); - _settingGroup = other._settingGroup; + _settingsGroup = other._settingsGroup; return *this; } @@ -69,8 +69,8 @@ void SettingsFact::_rawValueChanged(QVariant value) { QSettings settings; - if (!_settingGroup.isEmpty()) { - settings.beginGroup(_settingGroup); + if (!_settingsGroup.isEmpty()) { + settings.beginGroup(_settingsGroup); } settings.setValue(_name, value); diff --git a/src/FactSystem/SettingsFact.h b/src/FactSystem/SettingsFact.h index eba4a241797746145755193d28814d88e68d3f15..a59c8e5e98d1f27472a673e164ce903c718569e5 100644 --- a/src/FactSystem/SettingsFact.h +++ b/src/FactSystem/SettingsFact.h @@ -7,12 +7,7 @@ * ****************************************************************************/ - -/// @file -/// @author Don Gagne - -#ifndef SettingsFact_H -#define SettingsFact_H +#pragma once #include "Fact.h" @@ -23,7 +18,7 @@ class SettingsFact : public Fact public: SettingsFact(QObject* parent = NULL); - SettingsFact(QString settingGroup, FactMetaData* metaData, QObject* parent = NULL); + SettingsFact(QString settingsGroup, FactMetaData* metaData, QObject* parent = NULL); SettingsFact(const SettingsFact& other, QObject* parent = NULL); const SettingsFact& operator=(const SettingsFact& other); @@ -37,8 +32,6 @@ private slots: void _rawValueChanged(QVariant value); private: - QString _settingGroup; + QString _settingsGroup; bool _visible; }; - -#endif diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 744114b391ca39ead17d32cbb16b8e2db0aec8fe..667c17728e9f7002866dfd521030525c53981b89 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -366,6 +366,8 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_mess supportedMinorNumber = 4; break; case MAV_TYPE_QUADROTOR: + // Start TCP video handshake with ARTOO in case it's a Solo running ArduPilot firmware + _soloVideoHandshake(vehicle, false /* originalSoloFirmware */); case MAV_TYPE_COAXIAL: case MAV_TYPE_HELICOPTER: case MAV_TYPE_SUBMARINE: @@ -420,7 +422,7 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_mess _setInfoSeverity(message); // Start TCP video handshake with ARTOO - _soloVideoHandshake(vehicle); + _soloVideoHandshake(vehicle, true /* originalSoloFirmware */); } else if (messageText.contains(APM_FRAME_REXP)) { // We need to parse the Frame: message in order to determine whether the motors are coaxial or not QRegExp frameTypeRegex("^Frame: (\\S*)"); @@ -595,6 +597,18 @@ void APMFirmwarePlugin::_adjustCalibrationMessageSeverity(mavlink_message_t* mes mavlink_msg_statustext_encode_chan(message->sysid, message->compid, 0, message, &statusText); } +void APMFirmwarePlugin::initializeStreamRates(Vehicle* vehicle) +{ + vehicle->requestDataStream(MAV_DATA_STREAM_RAW_SENSORS, 2); + vehicle->requestDataStream(MAV_DATA_STREAM_EXTENDED_STATUS, 2); + vehicle->requestDataStream(MAV_DATA_STREAM_RC_CHANNELS, 2); + vehicle->requestDataStream(MAV_DATA_STREAM_POSITION, 3); + vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA1, 10); + vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA2, 10); + vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA3, 3); +} + + void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle) { vehicle->setFirmwarePluginInstanceData(new APMFirmwarePluginInstanceData); @@ -632,13 +646,7 @@ void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle) } } else { // Streams are not started automatically on APM stack - vehicle->requestDataStream(MAV_DATA_STREAM_RAW_SENSORS, 2); - vehicle->requestDataStream(MAV_DATA_STREAM_EXTENDED_STATUS, 2); - vehicle->requestDataStream(MAV_DATA_STREAM_RC_CHANNELS, 2); - vehicle->requestDataStream(MAV_DATA_STREAM_POSITION, 3); - vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA1, 10); - vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA2, 10); - vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA3, 3); + initializeStreamRates(vehicle); } } @@ -744,14 +752,16 @@ bool APMFirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const return vehicle->flightMode() == "Guided"; } -void APMFirmwarePlugin::_soloVideoHandshake(Vehicle* vehicle) +void APMFirmwarePlugin::_soloVideoHandshake(Vehicle* vehicle, bool originalSoloFirmware) { Q_UNUSED(vehicle); QTcpSocket* socket = new QTcpSocket(); socket->connectToHost(_artooIP, _artooVideoHandshakePort); - QObject::connect(socket, static_cast(&QTcpSocket::error), this, &APMFirmwarePlugin::_artooSocketError); + if (originalSoloFirmware) { + QObject::connect(socket, static_cast(&QTcpSocket::error), this, &APMFirmwarePlugin::_artooSocketError); + } } void APMFirmwarePlugin::_artooSocketError(QAbstractSocket::SocketError socketError) @@ -781,6 +791,8 @@ QString APMFirmwarePlugin::internalParameterMetaDataFile(Vehicle* vehicle) return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.4.xml"); case 5: return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.5.xml"); + case 6: + return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.6.xml"); default: if (minorVersion < 3) { return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.3.xml"); @@ -951,7 +963,6 @@ bool APMFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) return false; } - // FIXME: Is this needed? if (!_armVehicleAndValidate(vehicle)) { qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to arm.")); return false; @@ -966,36 +977,35 @@ bool APMFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) return true; } -// FIXME: Review for a better way to do this void APMFirmwarePlugin::startMission(Vehicle* vehicle) { - double currentAlt = vehicle->altitudeRelative()->rawValue().toDouble(); - - if (!vehicle->flying()) { - if (_guidedModeTakeoff(vehicle, qQNaN())) { + if (vehicle->flying()) { + // Vehicle already in the air, we just need to switch to auto + if (!_setFlightModeAndValidate(vehicle, "Auto")) { + qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to change to Auto mode.")); + } + return; + } - // Wait for vehicle to get off ground before switching to auto (10 seconds) - bool didTakeoff = false; - for (int i=0; i<100; i++) { - if (vehicle->altitudeRelative()->rawValue().toDouble() >= currentAlt + 1.0) { - didTakeoff = true; - break; - } - QGC::SLEEP::msleep(100); - qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents); - } + if (!vehicle->armed()) { + // First switch to flight mode we can arm from + if (!_setFlightModeAndValidate(vehicle, "Guided")) { + qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to change to Guided mode.")); + return; + } - if (!didTakeoff) { - qgcApp()->showMessage(tr("Unable to start mission. Vehicle takeoff failed.")); - return; - } - } else { + if (!_armVehicleAndValidate(vehicle)) { + qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to arm.")); return; } } - if (!_setFlightModeAndValidate(vehicle, missionFlightMode())) { - qgcApp()->showMessage(tr("Unable to start mission. Vehicle failed to change to auto.")); - return; + if (vehicle->fixedWing()) { + if (!_setFlightModeAndValidate(vehicle, "Auto")) { + qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to change to Auto mode.")); + return; + } + } else { + vehicle->sendMavCommand(vehicle->defaultComponentId(), MAV_CMD_MISSION_START, true /*show error */); } } diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 1353e0ca087947550cbf0b5a33bb0920bc312bd7..54ae7245472581f3db9c85acb59dd93e68d472be 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -92,6 +92,7 @@ public: int manualControlReservedButtonCount(void) override; bool adjustIncomingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message) override; void adjustOutgoingMavlinkMessage (Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message) override; + virtual void initializeStreamRates (Vehicle* vehicle); void initializeVehicle (Vehicle* vehicle) override; bool sendHomePositionToVehicle (void) override; void addMetaDataToFact (QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType) override; @@ -102,6 +103,7 @@ public: QObject* loadParameterMetaData (const QString& metaDataFile) override; QString brandImageIndoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); } QString brandImageOutdoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); } + bool supportsTerrainFrame (void) const override { return true; } protected: /// All access to singleton is through stack specific implementation @@ -123,7 +125,7 @@ private: bool _handleIncomingStatusText(Vehicle* vehicle, mavlink_message_t* message); void _handleIncomingHeartbeat(Vehicle* vehicle, mavlink_message_t* message); void _handleOutgoingParamSet(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message); - void _soloVideoHandshake(Vehicle* vehicle); + void _soloVideoHandshake(Vehicle* vehicle, bool originalSoloFirmware); bool _guidedModeTakeoff(Vehicle* vehicle, double altitudeRel); // Any instance data here must be global to all vehicles diff --git a/src/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.6.xml b/src/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.6.xml new file mode 100644 index 0000000000000000000000000000000000000000..c1a96770e7a6fd75643bf93e2a84f82770d91ad5 --- /dev/null +++ b/src/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.6.xml @@ -0,0 +1,9398 @@ + + + + + + +True + + +True + +ArduPlane +AntennaTracker +Copter +Rover +ArduSub + + + +1 255 + + + +Mission Planner and DroidPlanner + AP Planner 2 + + + +0 10 +.5 +Hz +hertz + + +0.0 1000.0 +10 +cm +centimeters + + +0 500 +10 + + +0:Feedback from mid stick,1:High throttle cancels landing,2:Disarm on land detection + +None +Feedback from mid stick +High throttle cancels landing +Disarm on land detection + + + +0 30 +1 +s +seconds + + +0:Roll,1:Pitch,2:Yaw + +None +Roll +Pitch +Yaw + + + +0 8000 +1 +cm +centimeters + + +0.5 10.0 + +Disabled +Shallow +Steep + +.1 + + +0 2000 +50 +cm/s +centimeters per second + + +-1 1000 +1 +cm +centimeters + + +0 3000 +10 +cm +centimeters + + +0 60000 +1000 +ms +milliseconds + + +0.01 2.0 +0.01 + + + +Disabled +Enabled always RTL +Enabled Continue with Mission in Auto Mode +Enabled always SmartRTL or RTL +Enabled always SmartRTL or Land + + + +100 900 + + + +Disabled +Enabled + + + + +Disabled +Mode1 +Mode2 +Mode1+2 +Mode3 +Mode1+3 +Mode2+3 +Mode1+2+3 +Mode4 +Mode1+4 +Mode2+4 +Mode1+2+4 +Mode3+4 +Mode1+3+4 +Mode2+3+4 +Mode1+2+3+4 +Mode5 +Mode1+5 +Mode2+5 +Mode1+2+5 +Mode3+5 +Mode1+3+5 +Mode2+3+5 +Mode1+2+3+5 +Mode4+5 +Mode1+4+5 +Mode2+4+5 +Mode1+2+4+5 +Mode3+4+5 +Mode1+3+4+5 +Mode2+3+4+5 +Mode1+2+3+4+5 +Mode6 +Mode1+6 +Mode2+6 +Mode1+2+6 +Mode3+6 +Mode1+3+6 +Mode2+3+6 +Mode1+2+3+6 +Mode4+6 +Mode1+4+6 +Mode2+4+6 +Mode1+2+4+6 +Mode3+4+6 +Mode1+3+4+6 +Mode2+3+4+6 +Mode1+2+3+4+6 +Mode5+6 +Mode1+5+6 +Mode2+5+6 +Mode1+2+5+6 +Mode3+5+6 +Mode1+3+5+6 +Mode2+3+5+6 +Mode1+2+3+5+6 +Mode4+5+6 +Mode1+4+5+6 +Mode2+4+5+6 +Mode1+2+4+5+6 +Mode3+4+5+6 +Mode1+3+4+5+6 +Mode2+3+4+5+6 +Mode1+2+3+4+5+6 + + + + +Never change yaw +Face next waypoint +Face next waypoint except RTL +Face along GPS course + + + +30 200 +10 +cm/s +centimeters per second + + +0 500 +10 +cm/s +centimeters per second + + +50 500 +10 +cm/s +centimeters per second + + +50 500 +10 +cm/s/s +centimeters per square second + + + +Disabled +Enabled always RTL +Enabled Continue with Mission in Auto Mode +Enabled always Land +Enabled always SmartRTL or RTL +Enabled always SmartRTL or Land + + + +925 1100 +1 +PWM +PWM in microseconds + + +0 300 +1 +PWM +PWM in microseconds + + + +Stabilize +Acro +AltHold +Auto +Guided +Loiter +RTL +Circle +Land +Drift +Sport +Flip +AutoTune +PosHold +Brake +Throw +Avoid_ADSB +Guided_NoGPS +Smart_RTL +FlowHold +Follow + + + + +Stabilize +Acro +AltHold +Auto +Guided +Loiter +RTL +Circle +Land +Drift +Sport +Flip +AutoTune +PosHold +Brake +Throw +Avoid_ADSB +Guided_NoGPS +Smart_RTL +FlowHold +Follow + + + + +Stabilize +Acro +AltHold +Auto +Guided +Loiter +RTL +Circle +Land +Drift +Sport +Flip +AutoTune +PosHold +Brake +Throw +Avoid_ADSB +Guided_NoGPS +Smart_RTL +FlowHold +Follow + + + + +Stabilize +Acro +AltHold +Auto +Guided +Loiter +RTL +Circle +Land +Drift +Sport +Flip +AutoTune +PosHold +Brake +Throw +Avoid_ADSB +Guided_NoGPS +Smart_RTL +FlowHold +Follow + + + + +Stabilize +Acro +AltHold +Auto +Guided +Loiter +RTL +Circle +Land +Drift +Sport +Flip +AutoTune +PosHold +Brake +Throw +Avoid_ADSB +Guided_NoGPS +Smart_RTL +FlowHold +Follow + + + + +Stabilize +Acro +AltHold +Auto +Guided +Loiter +RTL +Circle +Land +Drift +Sport +Flip +AutoTune +PosHold +Brake +Throw +Avoid_ADSB +Guided_NoGPS +Smart_RTL +FlowHold +Follow + + + + +Disabled +Channel5 +Channel6 +Channel7 +Channel8 + + + + + +0:ATTITUDE_FAST,1:ATTITUDE_MED,2:GPS,3:PM,4:CTUN,5:NTUN,6:RCIN,7:IMU,8:CMD,9:CURRENT,10:RCOUT,11:OPTFLOW,12:PID,13:COMPASS,14:INAV,15:CAMERA,17:MOTBATT,18:IMU_FAST,19:IMU_RAW + +Default +Default+RCIN +Default+IMU +Default+Motors +NearlyAll-AC315 +NearlyAll +All+FastATT +All+MotBatt +All+FastIMU +All+FastIMU+PID +All+FullIMU +Disabled + + + + +Normal Start-up +Start-up in ESC Calibration mode if throttle high +Start-up in ESC Calibration mode regardless of throttle +Start-up and automatically calibrate ESCs +Disabled + + + + +None +Stab Roll/Pitch kP +Rate Roll/Pitch kP +Rate Roll/Pitch kI +Rate Roll/Pitch kD +Stab Yaw kP +Rate Yaw kP +Rate Yaw kD +Rate Yaw Filter +Motor Yaw Headroom +AltHold kP +Throttle Rate kP +Throttle Accel kP +Throttle Accel kI +Throttle Accel kD +Loiter Pos kP +Velocity XY kP +Velocity XY kI +WP Speed +Acro RollPitch kP +Acro Yaw kP +RC Feel +Heli Ext Gyro +Declination +Circle Rate +RangeFinder Gain +Rate Pitch kP +Rate Pitch kI +Rate Pitch kD +Rate Roll kP +Rate Roll kI +Rate Roll kD +Rate Pitch FF +Rate Roll FF +Rate Yaw FF +Winch + + + +0 32767 + + +0 32767 + + + +Plus +X +V +H +V-Tail +A-Tail +Y6B + +True + + + +Do Nothing +Flip +Simple Mode +RTL +Save Trim +Save WP +Camera Trigger +RangeFinder +Fence +Super Simple Mode +Acro Trainer +Sprayer +Auto +AutoTune +Land +Gripper +Parachute Enable +Parachute Release +Parachute 3pos +Auto Mission Reset +AttCon Feed Forward +AttCon Accel Limits +Retract Mount +Relay On/Off +Relay2 On/Off +Relay3 On/Off +Relay4 On/Off +Landing Gear +Lost Copter Sound +Motor Emergency Stop +Motor Interlock +Brake +Throw +ADSB-Avoidance +PrecLoiter +Object Avoidance +ArmDisarm +SmartRTL +InvertedFlight +Winch Enable +WinchControl + + + + +Do Nothing +Flip +Simple Mode +RTL +Save Trim +Save WP +Camera Trigger +RangeFinder +Fence +Super Simple Mode +Acro Trainer +Sprayer +Auto +AutoTune +Land +Gripper +Parachute Enable +Parachute Release +Parachute 3pos +Auto Mission Reset +AttCon Feed Forward +AttCon Accel Limits +Retract Mount +Relay On/Off +Relay2 On/Off +Relay3 On/Off +Relay4 On/Off +Landing Gear +Lost Copter Sound +Motor Emergency Stop +Motor Interlock +Brake +Throw +ADSB-Avoidance +PrecLoiter +Object Avoidance +ArmDisarm +SmartRTL +InvertedFlight +Winch Enable +WinchControl + + + + +Do Nothing +Flip +Simple Mode +RTL +Save Trim +Save WP +Camera Trigger +RangeFinder +Fence +Super Simple Mode +Acro Trainer +Sprayer +Auto +AutoTune +Land +Gripper +Parachute Enable +Parachute Release +Parachute 3pos +Auto Mission Reset +AttCon Feed Forward +AttCon Accel Limits +Retract Mount +Relay On/Off +Relay2 On/Off +Relay3 On/Off +Relay4 On/Off +Landing Gear +Lost Copter Sound +Motor Emergency Stop +Motor Interlock +Brake +Throw +ADSB-Avoidance +PrecLoiter +Object Avoidance +ArmDisarm +SmartRTL +InvertedFlight +Winch Enable +WinchControl + + + + +Do Nothing +Flip +Simple Mode +RTL +Save Trim +Save WP +Camera Trigger +RangeFinder +Fence +Super Simple Mode +Acro Trainer +Sprayer +Auto +AutoTune +Land +Gripper +Parachute Enable +Parachute Release +Parachute 3pos +Auto Mission Reset +AttCon Feed Forward +AttCon Accel Limits +Retract Mount +Relay On/Off +Relay2 On/Off +Relay3 On/Off +Relay4 On/Off +Landing Gear +Lost Copter Sound +Motor Emergency Stop +Motor Interlock +Brake +Throw +ADSB-Avoidance +PrecLoiter +Object Avoidance +ArmDisarm +SmartRTL +InvertedFlight +Winch Enable +WinchControl + + + + +Do Nothing +Flip +Simple Mode +RTL +Save Trim +Save WP +Camera Trigger +RangeFinder +Fence +Super Simple Mode +Acro Trainer +Sprayer +Auto +AutoTune +Land +Gripper +Parachute Enable +Parachute Release +Parachute 3pos +Auto Mission Reset +AttCon Feed Forward +AttCon Accel Limits +Retract Mount +Relay On/Off +Relay2 On/Off +Relay3 On/Off +Relay4 On/Off +Landing Gear +Lost Copter Sound +Motor Emergency Stop +Motor Interlock +Brake +Throw +ADSB-Avoidance +PrecLoiter +Object Avoidance +ArmDisarm +SmartRTL +InvertedFlight +Winch Enable +WinchControl + + + + +Do Nothing +Flip +Simple Mode +RTL +Save Trim +Save WP +Camera Trigger +RangeFinder +Fence +Super Simple Mode +Acro Trainer +Sprayer +Auto +AutoTune +Land +Gripper +Parachute Enable +Parachute Release +Parachute 3pos +Auto Mission Reset +AttCon Feed Forward +AttCon Accel Limits +Retract Mount +Relay On/Off +Relay2 On/Off +Relay3 On/Off +Relay4 On/Off +Landing Gear +Lost Copter Sound +Motor Emergency Stop +Motor Interlock +Brake +Throw +ADSB-Avoidance +PrecLoiter +Object Avoidance +ArmDisarm +SmartRTL +InvertedFlight +Winch Enable +WinchControl + + + +0 127 +s +seconds + + +1000 8000 +cdeg +centidegrees + + +4 12 +deg/s +degrees per second + + +2000 4500 +cdeg +centidegrees + + + +No repositioning +Repositioning + + + + +Land +AltHold +Land even in Stabilize + + + +0.6:Strict, 0.8:Default, 1.0:Relaxed + + + +Disabled +Enabled + + + +50 490 +1 +Hz +hertz + + +1 10 + + +1 10 + + +0 3 +0.1 + + +0 3 +0.1 + + + +Disabled +Leveling +Leveling and Limited + + + +-0.5 1.0 + +Disabled +Very Low +Low +Medium +High +Very High + + + +0:Roll,1:Pitch,2:Yaw + +All +Roll Only +Pitch Only +Yaw Only +Roll and Pitch +Roll and Yaw +Pitch and Yaw + + + +0.05 0.10 + + +0.001 0.006 + + + +Stopped +Running + + + + +Do Not Use in RTL and Land +Use in RTL and Land + + + +0 5 + + + +Auto +Guided +LOITER +RTL +Land +Brake +Throw + + + + +Upward Throw +Drop + + + + +Disabled +Enabled + + + +0:ADSBMavlinkProcessing + + +-0.5 1.0 + +Disabled +Very Low +Low +Medium +High +Very High + + + +0 1 + + + +NotEnforced +Enforced + + + + +Undefined +Quad +Hexa +Octa +OctaQuad +Y6 +Heli +Tri +SingleCopter +CoaxCopter +Heli_Dual +DodecaHexa +HeliQuad + +True + + +50 500 +10 +cm/s +centimeters per second + + +100 10000 +10 +cm +centimeters + + + + + +Disabled +Enabled + + + +1 100 + + +1 100000 + + +-1 16777215 + + + +NoInfo +Light +Small +Large +HighVortexlarge +Heavy +HighlyManuv +Rotocraft +RESERVED +Glider +LightAir +Parachute +UltraLight +RESERVED +UAV +Space +RESERVED +EmergencySurface +ServiceSurface +PointObstacle + + + + +NO_DATA +L15W23 +L25W28P5 +L25W34 +L35W33 +L35W38 +L45W39P5 +L45W45 +L55W45 +L55W52 +L65W59P5 +L65W67 +L75W72P5 +L75W80 +L85W80 +L85W90 + + + + +NoData +Left2m +Left4m +Left6m +Center +Right2m +Right4m +Right6m + + + + +NO_DATA +AppliedBySensor + + + + +Disabled +Rx-Only +Tx-Only +Rx and Tx Enabled + + + +octal +octal + + +0:UAT_in,1:1090ES_in,2:UAT_out,3:1090ES_out + +Unknown +Rx UAT only +Rx UAT and 1090ES +Rx&Tx UAT and 1090ES + + + + + + + + + + + + + + + + + + + + + +m +meters + + +m +meters + + +mbar +millibar + + + + + + + + + + + + + + +s +seconds + + + + +0.0 1.0 +.01 + + + +Disabled +Enabled + + + +0.1 0.4 +.01 + + +0.1 0.4 +.01 + + +0 127 +1 +m/s +meters per second + + +-0.1745 +0.1745 +0.01 +rad +radians + + +-0.1745 +0.1745 +0.01 +rad +radians + + +-0.1745 +0.1745 +0.01 +rad +radians + + + +None +Yaw45 +Yaw90 +Yaw135 +Yaw180 +Yaw225 +Yaw270 +Yaw315 +Roll180 +Roll180Yaw45 +Roll180Yaw90 +Roll180Yaw135 +Pitch180 +Roll180Yaw225 +Roll180Yaw270 +Roll180Yaw315 +Roll90 +Roll90Yaw45 +Roll90Yaw90 +Roll90Yaw135 +Roll270 +Roll270Yaw45 +Roll270Yaw90 +Roll270Yaw135 +Pitch90 +Pitch270 +Pitch180Yaw90 +Pitch180Yaw270 +Roll90Pitch90 +Roll180Pitch90 +Roll270Pitch90 +Roll90Pitch180 +Roll270Pitch180 +Roll90Pitch270 +Roll180Pitch270 +Roll270Pitch270 +Roll90Pitch180Yaw90 +Roll90Yaw270 +Yaw293Pitch68Roll180 +Pitch315 +Roll90Pitch315 +Custom + + + +0.001 0.5 +.01 + + +0 10 +1 + + + +Disabled +Enable EKF2 +Enable EKF3 + + + +-180 180 +1 +deg +degrees + + +-180 180 +1 +deg +degrees + + +-180 180 +1 +deg +degrees + + + + + +Disabled +THR_MIN PWM when disarmed +0 PWM when disarmed + + + +0:All,1:Barometer,2:Compass,3:GPS lock,4:INS,5:Parameters,6:RC Channels,7:Board voltage,8:Battery Level,10:Logging Available,11:Hardware safety switch,12:GPS Configuration + +None +All +Barometer +Compass +GPS Lock +INS(INertial Sensors - accels & gyros) +Parameters(unused) +RC Channels +Board voltage +Battery Level +LoggingAvailable +Hardware safety switch +GPS configuration + + + +0.25 3.0 +m/s/s +meters per square second + + +0.1 +V +volt + + +0.1 +V +volt + + + + +500 18000 +100 +cdeg/s +centidegrees per second + + +0 72000 + +Disabled +VerySlow +Slow +Medium +Fast + +1000 +cdeg/s/s +centidegrees per square second + + + +Disabled +Enabled + + + +0 180000 + +Disabled +VerySlow +Slow +Medium +Fast + +1000 +cdeg/s/s +centidegrees per square second + + +0 180000 + +Disabled +VerySlow +Slow +Medium +Fast + +1000 +cdeg/s/s +centidegrees per square second + + + +Disabled +Enabled + + + +3.000 12.000 + + +3.000 12.000 + + +3.000 6.000 + + +0.5 10.0 + + +0 1080 + +Disabled +Slow +Medium +Fast + +1 +deg/s +degrees per second + + +0 1080 + +Disabled +Slow +Medium +Fast + +1 +deg/s +degrees per second + + +0 1080 + +Disabled +Slow +Medium +Fast + +1 +deg/s +degrees per second + + +0 1 +0.5:Very Soft, 0.2:Soft, 0.15:Medium, 0.1:Crisp, 0.05:Very Crisp +0.01 +s +seconds + + +0.05 0.5 +0.005 + + +0.01 2.0 +0.01 + + +0 1 +0.01 +% +percent + + +0.0 0.02 +0.001 + + +0 0.5 +0.001 + + +1 100 +1 +Hz +hertz + + +0.05 0.50 +0.005 + + +0.01 2.0 +0.01 + + +0 1 +0.01 +% +percent + + +0.0 0.02 +0.001 + + +0 0.5 +0.001 + + +1 100 +1 +Hz +hertz + + +0.10 2.50 +0.005 + + +0.010 1.0 +0.01 + + +0 1 +0.01 +% +percent + + +0.000 0.02 +0.001 + + +0 0.5 +0.001 + + +1 10 +1 +Hz +hertz + + +0.1 0.25 + + +0.5 0.9 + + +0.1 0.9 + + +0 1000 +cdeg +centidegrees + + +0.08 0.35 +0.005 + + +0.01 0.6 +0.01 + + +0 1 +0.01 + + +0.001 0.03 +0.001 + + +0 0.5 +0.001 + + +1 20 +1 +Hz +hertz + + +0.08 0.35 +0.005 + + +0.01 0.6 +0.01 + + +0 1 +0.01 + + +0.001 0.03 +0.001 + + +0 0.5 +0.001 + + +1 20 +1 +Hz +hertz + + +0.180 0.60 +0.005 + + +0.01 0.06 +0.01 + + +0 1 +0.01 + + +0.000 0.02 +0.001 + + +0 0.5 +0.001 + + +1 20 +1 +Hz +hertz + + + +Disabled +Enabled + + + + + + +Disabled +Enabled + + + + +Remain in AVOID_ADSB +Resume previous flight mode +RTL +Resume if AUTO else Loiter + + + + + +s +seconds + + +s +seconds + + +m +meters + + +m +meters + + +m +meters + + +m +meters + + +m +meters + + + + +0:StopAtFence,1:UseProximitySensor,2:StopAtBeaconFence + +None +StopAtFence +UseProximitySensor +StopAtFence and UseProximitySensor +StopAtBeaconFence +All + + + +0 4500 +cdeg +centidegrees + + +1 30 +m +meters + + +1 10 +m +meters + + + +Slide +Stop + + + + + + +Disabled +Analog Voltage Only +Analog Voltage and Current +Solo +Bebop +SMBus-Maxell +UAVCAN-BatteryInfo + + + + +Disabled +A0 +A1 +Pixhawk/Pixracer/Navio2/Pixhawk2_PM1 +Pixhawk2_PM2 +PX4-v1 + + + + +Disabled +A1 +A2 +Pixhawk/Pixracer/Navio2/Pixhawk2_PM1 +Pixhawk2_PM2 +PX4-v1 + + + + + +A/V +ampere per volt + + +V +volt + + +50 +mAh +milliampere hour + + +1 +W +watt + + + + +0 120 +1 +s +seconds + + + +Raw Voltage +Sag Compensated Voltage + + + +0.1 +V +volt + + +50 +mAh +milliampere hour + + +0.1 +V +volt + + +50 +mAh +milliampere hour + + + +None +Land +RTL +SmartRTL +SmartRTL or Land +Terminate + + + + +None +Land +RTL +SmartRTL +SmartRTL or Land +Terminate + + + + + + +Disabled +Analog Voltage Only +Analog Voltage and Current +Solo +Bebop +SMBus-Maxell +UAVCAN-BatteryInfo + + + + +Disabled +A0 +A1 +Pixhawk/Pixracer/Navio2/Pixhawk2_PM1 +Pixhawk2_PM2 +PX4-v1 + + + + +Disabled +A1 +A2 +Pixhawk/Pixracer/Navio2/Pixhawk2_PM1 +Pixhawk2_PM2 +PX4-v1 + + + + + +A/V +ampere per volt + + +V +volt + + +50 +mAh +milliampere hour + + +1 +W +watt + + + + +0 120 +1 +s +seconds + + + +Raw Voltage +Sag Compensated Voltage + + + +0.1 +V +volt + + +50 +mAh +milliampere hour + + +0.1 +V +volt + + +50 +mAh +milliampere hour + + + +None +Land +RTL +SmartRTL +SmartRTL or Land +Terminate + + + + +None +Land +RTL +SmartRTL +SmartRTL or Land +Terminate + + + + + + +None +Pozyx +Marvelmind + + + +-90 90 +0.000001 +deg +degrees + + +-180 180 +0.000001 +deg +degrees + + +0 10000 +1 +m +meters + + +-180 +180 +1 +deg +degrees + + + + + +No PWMs +Two PWMs +Four PWMs +Six PWMs +Three PWMs and One Capture + +True + + + +Disabled +Enabled +Auto + +True + + + +Disabled +Enabled +Auto + +True + + + +Disabled +Enabled + +True + + + +Disabled +50Hz +75Hz +100Hz +150Hz +200Hz +250Hz +300Hz + +True + + +-32768 32767 + + +0:Ch1,1:Ch2,2:Ch3,3:Ch4,4:Ch5,5:Ch6,6:Ch7,7:Ch8,8:Ch9,9:Ch10,10:Ch11,11:Ch12,12:Ch13,13:Ch14 + +Disabled +Enabled + +True + + +-1 80 +degC +degrees Celsius + + + +AUTO +PX4V1 +Pixhawk +Cube/Pixhawk2 +Pixracer +PixhawkMini +Pixhawk2Slim +VRBrain 5.1 +VRBrain 5.2 +VR Micro Brain 5.1 +VR Micro Brain 5.2 +VRBrain Core 1.0 +VRBrain 5.4 +Intel Aero FC +AUAV2.1 + +True + + + +Disabled +Enabled + +True + + +0:ActiveForSafetyEnable,1:ActiveForSafetyDisable,2:ActiveWhenArmed + + + + + +None +CYRF6936 + + + + +Auto +DSM2 +DSMX + + + +0 4 + + + +NotDisabled +Disabled + + + +0 16 + + +0 16 + + + +Disabled +Enabled + + + +1 8 + + + +Disabled +MinChannel +MidChannel +MaxChannel +MinChannelCW +MidChannelCW +MaxChannelCW + + + + +Mode1 +Mode2 + + + + +Disabled +TestChan1 +TestChan2 +TestChan3 +TestChan4 +TestChan5 +TestChan6 +TestChan7 +TestChan8 + + + +0 16 + + +0 16 + + +1 8 + + +0 40 + + +0 120 + + +0 31 + + + + + +Disabled +Enabled + + + + +Disabled +Pixhawk AUXOUT1 +Pixhawk AUXOUT2 +Pixhawk AUXOUT3 +Pixhawk AUXOUT4 +Pixhawk AUXOUT5 +Pixhawk AUXOUT6 +PX4 FMU Relay1 +PX4 FMU Relay2 +PX4IO Relay1 +PX4IO Relay2 +PX4IO ACC1 +PX4IO ACC2 + + + + +Disabled +Pixhawk AUXOUT1 +Pixhawk AUXOUT2 +Pixhawk AUXOUT3 +Pixhawk AUXOUT4 +Pixhawk AUXOUT5 +Pixhawk AUXOUT6 +PX4 FMU Relay1 +PX4 FMU Relay2 +PX4IO Relay1 +PX4IO Relay2 +PX4IO ACC1 +PX4IO ACC2 + + + + +Disabled +Pixhawk AUXOUT1 +Pixhawk AUXOUT2 +Pixhawk AUXOUT3 +Pixhawk AUXOUT4 +Pixhawk AUXOUT5 +Pixhawk AUXOUT6 +PX4 FMU Relay1 +PX4 FMU Relay2 +PX4IO Relay1 +PX4IO Relay2 +PX4IO ACC1 +PX4IO ACC2 + + + + +Disabled +Pixhawk AUXOUT1 +Pixhawk AUXOUT2 +Pixhawk AUXOUT3 +Pixhawk AUXOUT4 +Pixhawk AUXOUT5 +Pixhawk AUXOUT6 +PX4 FMU Relay1 +PX4 FMU Relay2 +PX4IO Relay1 +PX4IO Relay2 +PX4IO ACC1 +PX4IO ACC2 + + + +0 3600 + + + + + +Servo +Relay + + + +0 50 +ds +deciseconds + + +1000 2000 +PWM +PWM in microseconds + + +1000 2000 +PWM +PWM in microseconds + + +0 1000 +m +meters + + + +Low +High + + + +0 10000 +ms +milliseconds + + +0 180 +deg +degrees + + + +Disabled +PX4 AUX1 +PX4 AUX2 +PX4 AUX3 +PX4 AUX4(fast capture) +PX4 AUX5 +PX4 AUX6 + + + + +TriggerLow +TriggerHigh + + + + +Always +Only when in AUTO + + + + + + +Disabled +UAVCAN + +True + + + + +1 250 + + +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 + + +0: ESC 1, 1: ESC 2, 2: ESC 3, 3: ESC 4, 4: ESC 5, 5: ESC 6, 6: ESC 7, 7: ESC 8, 8: ESC 9, 9: ESC 10, 10: ESC 11, 11: ESC 12, 12: ESC 13, 13: ESC 14, 14: ESC 15, 15: ESC 16 + + +1 200 +Hz +hertz + + + + + +Disabled +UAVCAN + +True + + + + +1 250 + + +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 + + +0: ESC 1, 1: ESC 2, 2: ESC 3, 3: ESC 4, 4: ESC 5, 5: ESC 6, 6: ESC 7, 7: ESC 8, 8: ESC 9, 9: ESC 10, 10: ESC 11, 11: ESC 12, 12: ESC 13, 13: ESC 14, 14: ESC 15, 15: ESC 16 + + +1 200 +Hz +hertz + + + + + +Disabled +UAVCAN + +True + + + + +1 250 + + +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 + + +0: ESC 1, 1: ESC 2, 2: ESC 3, 3: ESC 4, 4: ESC 5, 5: ESC 6, 6: ESC 7, 7: ESC 8, 8: ESC 9, 9: ESC 10, 10: ESC 11, 11: ESC 12, 12: ESC 13, 13: ESC 14, 14: ESC 15, 15: ESC 16 + + +1 200 +Hz +hertz + + + + + +Disabled +First driver +Second driver + +True + + +10000 1000000 + + + +Disabled +Major messages +All messages + + + + + + +Disabled +First driver +Second driver + +True + + +10000 1000000 + + + +Disabled +Major messages +All messages + + + + + + +Disabled +First driver +Second driver + +True + + +10000 1000000 + + + +Disabled +Major messages +All messages + + + + + + +Disabled +Enabled + + + + +First Relay +Second Relay +Third Relay +Fourth Relay +Servo + + + +1000 2000 +1 +PWM +PWM in microseconds + + +1000 2000 +1 +PWM +PWM in microseconds + + +0 32000 +1 +m +meters + + +0 5000 +1 +ms +milliseconds + + + + +0 10000 +100 +cm +centimeters + + +-90 90 +1 +deg/s +degrees per second + + + + +-400 400 +1 +mGauss +milligauss + + +-400 400 +1 +mGauss +milligauss + + +-400 400 +1 +mGauss +milligauss + + +-3.142 3.142 +0.01 +rad +radians + + + +Disabled +Internal-Learning +EKF-Learning + + + + +Disabled +Enabled + + + + +Disabled +Enabled + + + + +Disabled +Use Throttle +Use Current + + + +-1000 1000 +1 +mGauss/A +milligauss per ampere + + +-1000 1000 +1 +mGauss/A +milligauss per ampere + + +-1000 1000 +1 +mGauss/A +milligauss per ampere + + + +None +Yaw45 +Yaw90 +Yaw135 +Yaw180 +Yaw225 +Yaw270 +Yaw315 +Roll180 +Roll180Yaw45 +Roll180Yaw90 +Roll180Yaw135 +Pitch180 +Roll180Yaw225 +Roll180Yaw270 +Roll180Yaw315 +Roll90 +Roll90Yaw45 +Roll90Yaw90 +Roll90Yaw135 +Roll270 +Roll270Yaw45 +Roll270Yaw90 +Roll270Yaw135 +Pitch90 +Pitch270 +Pitch180Yaw90 +Pitch180Yaw270 +Roll90Pitch90 +Roll180Pitch90 +Roll270Pitch90 +Roll90Pitch180 +Roll270Pitch180 +Roll90Pitch270 +Roll180Pitch270 +Roll270Pitch270 +Roll90Pitch180Yaw90 +Roll90Yaw270 +Yaw293Pitch68Roll180 +Pitch315 +Roll90Pitch315 + + + + +Internal +External +ForcedExternal + + + +-400 400 +1 +mGauss +milligauss + + +-400 400 +1 +mGauss +milligauss + + +-400 400 +1 +mGauss +milligauss + + +-1000 1000 +1 +mGauss/A +milligauss per ampere + + +-1000 1000 +1 +mGauss/A +milligauss per ampere + + +-1000 1000 +1 +mGauss/A +milligauss per ampere + + + +FirstCompass +SecondCompass +ThirdCompass + + + +-400 400 +1 +mGauss +milligauss + + +-400 400 +1 +mGauss +milligauss + + +-400 400 +1 +mGauss +milligauss + + +-1000 1000 +1 +mGauss/A +milligauss per ampere + + +-1000 1000 +1 +mGauss/A +milligauss per ampere + + +-1000 1000 +1 +mGauss/A +milligauss per ampere + + + + + + + + + +Disabled +Enabled + + + + +None +Yaw45 +Yaw90 +Yaw135 +Yaw180 +Yaw225 +Yaw270 +Yaw315 +Roll180 +Roll180Yaw45 +Roll180Yaw90 +Roll180Yaw135 +Pitch180 +Roll180Yaw225 +Roll180Yaw270 +Roll180Yaw315 +Roll90 +Roll90Yaw45 +Roll90Yaw90 +Roll90Yaw135 +Roll270 +Roll270Yaw45 +Roll270Yaw90 +Roll270Yaw135 +Pitch90 +Pitch270 +Pitch180Yaw90 +Pitch180Yaw270 +Roll90Pitch90 +Roll180Pitch90 +Roll270Pitch90 +Roll90Pitch180 +Roll270Pitch180 +Roll90Pitch270 +Roll180Pitch270 +Roll270Pitch270 +Roll90Pitch180Yaw90 +Roll90Yaw270 +Yaw293Pitch68Roll180 +Pitch315 +Roll90Pitch315 + + + + +Internal +External +ForcedExternal + + + + +Disabled +Enabled + + + + +None +Yaw45 +Yaw90 +Yaw135 +Yaw180 +Yaw225 +Yaw270 +Yaw315 +Roll180 +Roll180Yaw45 +Roll180Yaw90 +Roll180Yaw135 +Pitch180 +Roll180Yaw225 +Roll180Yaw270 +Roll180Yaw315 +Roll90 +Roll90Yaw45 +Roll90Yaw90 +Roll90Yaw135 +Roll270 +Roll270Yaw45 +Roll270Yaw90 +Roll270Yaw135 +Pitch90 +Pitch270 +Pitch180Yaw90 +Pitch180Yaw270 +Roll90Pitch90 +Roll180Pitch90 +Roll270Pitch90 +Roll90Pitch180 +Roll270Pitch180 +Roll90Pitch270 +Roll180Pitch270 +Roll270Pitch270 +Roll90Pitch180Yaw90 +Roll90Yaw270 +Yaw293Pitch68Roll180 +Pitch315 +Roll90Pitch315 + + + + +Internal +External +ForcedExternal + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +4 32 + +Very Strict +Strict +Default +Relaxed + +0.1 + + +500 3000 +1 + + +0:HMC5883,1:LSM303D,2:AK8963,3:BMM150,4:LSM9DS1,5:LIS3MDL,6:AK09916,7:IST8310,8:ICM20948,9:MMC3416,11:UAVCAN,12:QMC5883 + + +0 100 +1 +% +percent + + + + + +Disabled +Enabled + + + +0 2 +0.01 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +Disabled +Enabled + +True + + + +GPS 3D Vel and 2D Pos +GPS 2D vel and 2D pos +GPS 2D pos +No GPS + + + +0.05 5.0 +0.05 +m/s +meters per second + + +0.05 5.0 +0.05 +m/s +meters per second + + +100 1000 +25 + + +0.1 10.0 +0.1 +m +meters + + +100 1000 +25 + + +10 100 +5 +m +meters + + +0 250 +10 +ms +milliseconds +True + + + +Use Baro +Use Range Finder +Use GPS +Use Range Beacon + +True + + +0.1 10.0 +0.1 +m +meters + + +100 1000 +25 + + +0 250 +10 +ms +milliseconds +True + + +0.01 0.5 +0.01 +Gauss +gauss + + + +When flying +When manoeuvring +Never +After first climb yaw reset +Always + + + +100 1000 +25 + + +0.5 5.0 +0.1 +m/s +meters per second + + +100 1000 +25 + + +0.1 10.0 +0.1 +m +meters + + +100 1000 +25 + + +1.0 4.0 +0.1 +rad/s +radians per second + + +0.05 1.0 +0.05 +rad/s +radians per second + + +100 1000 +25 + + +0 127 +10 +ms +milliseconds +True + + +0.0001 0.1 +0.0001 +rad/s +radians per second + + +0.01 1.0 +0.01 +m/s/s +meters per square second + + +0.00001 0.001 +rad/s/s +radians per square second + + +0.000001 0.001 +Hz +hertz + + +0.00001 0.001 +m/s/s/s +meters per cubic second + + +0.01 1.0 +0.1 +m/s/s +meters per square second + + +0.0 1.0 +0.1 + + +0:NSats,1:HDoP,2:speed error,3:position error,4:yaw error,5:pos drift,6:vert speed,7:horiz speed + + +0:FirstIMU,1:SecondIMU,2:ThirdIMU,3:FourthIMU,4:FifthIMU,5:SixthIMU +True + + +50 200 +% +percent + + +0.5 50.0 +m +meters + + +0:FirstIMU,1:SecondIMU,2:ThirdIMU,3:FourthIMU,4:FifthIMU,5:SixthIMU +True + + +0.05 1.0 +0.05 +rad +radians + + +100 1000 +25 + + +10 50 +5 +cs +centiseconds + + +0.00001 0.01 +Gauss/s +gauss per second + + +0.00001 0.01 +Gauss/s +gauss per second + + +-1 70 +1 +% +percent + + +0 0.2 +0.01 + + +0.1 10.0 +0.1 +m +meters + + +100 1000 +25 + + +0 127 +10 +ms +milliseconds +True + + +2.0 6.0 +0.5 +m/s +meters per second + + +0:FirstEKF,1:SecondEKF,2:ThirdEKF,3:FourthEKF,4:FifthEKF,5:SixthEKF +True + + +0:Correct when using Baro height,1:Correct when using range finder height,2:Apply corrections to local position +True + + + + + +Disabled +Enabled + +True + + + +GPS 3D Vel and 2D Pos +GPS 2D vel and 2D pos +GPS 2D pos +No GPS + + + +0.05 5.0 +0.05 +m/s +meters per second + + +0.05 5.0 +0.05 +m/s +meters per second + + +100 1000 +25 + + +0.1 10.0 +0.1 +m +meters + + +100 1000 +25 + + +10 100 +5 +m +meters + + + +Use Baro +Use Range Finder +Use GPS +Use Range Beacon + +True + + +0.1 10.0 +0.1 +m +meters + + +100 1000 +25 + + +0 250 +10 +ms +milliseconds +True + + +0.01 0.5 +0.01 +Gauss +gauss + + + +When flying +When manoeuvring +Never +After first climb yaw reset +Always + +True + + +100 1000 +25 + + +0.5 5.0 +0.1 +m/s +meters per second + + +100 1000 +25 + + +0.1 10.0 +0.1 +m +meters + + +100 1000 +25 + + +1.0 4.0 +0.1 +rad/s +radians per second + + +0.05 1.0 +0.05 +rad/s +radians per second + + +100 1000 +25 + + +0 250 +10 +ms +milliseconds +True + + +0.0001 0.1 +0.0001 +rad/s +radians per second + + +0.01 1.0 +0.01 +m/s/s +meters per square second + + +0.00001 0.001 +rad/s/s +radians per square second + + +0.00001 0.001 +m/s/s/s +meters per cubic second + + +0.01 1.0 +0.1 +m/s/s +meters per square second + + +0.0 1.0 +0.1 + + +0:NSats,1:HDoP,2:speed error,3:position error,4:yaw error,5:pos drift,6:vert speed,7:horiz speed + + +0:FirstIMU,1:SecondIMU,2:ThirdIMU,3:FourthIMU,4:FifthIMU,5:SixthIMU +True + + +50 200 +% +percent + + +0.5 50.0 +m +meters + + +0:FirstIMU,1:SecondIMU,2:ThirdIMU,3:FourthIMU,4:FifthIMU,5:SixthIMU +True + + +0.05 1.0 +0.05 +rad +radians + + +100 1000 +25 + + +10 50 +5 +cs +centiseconds + + +0.00001 0.01 +Gauss/s +gauss per second + + +0.00001 0.01 +Gauss/s +gauss per second + + +-1 70 +1 +% +percent + + +0 0.2 +0.01 + + +0.1 10.0 +0.1 +m +meters + + +100 1000 +25 + + +0 250 +10 +ms +milliseconds +True + + +2.0 6.0 +0.5 +m/s +meters per second + + +0.5 2.5 +0.1 +m/s/s +meters per square second + + +0:FirstEKF,1:SecondEKF,2:ThirdEKF,3:FourthEKF,4:FifthEKF,5:SixthEKF +True + + +0:Correct when using Baro height,1:Correct when using range finder height,2:Apply corrections to local position +True + + +0.05 0.5 +0.05 +m/s +meters per second + + +0.5 5.0 +0.1 +m/s +meters per second + + +0.01 1.0 +0.1 +m/s +meters per second + + + + + +Disabled +Enabled + + + +0:Altitude,1:Circle,2:Polygon + +None +Altitude +Circle +Altitude and Circle +Polygon +Altitude and Polygon +Circle and Polygon +All + + + + +Report Only +RTL or Land + + + +10 1000 +1 +m +meters + + +30 10000 +m +meters + + +1 10 +m +meters + + +1 20 + + +-100 100 +1 +m +meters + + + + +0.1 6.0 +0.1 + + +0.02 1.00 +0.01 + + +0 4500 +10 +cdeg +centidegrees + + +0.1 2.5 + + +1 100 +Hz +hertz + + +0 255 + + +1 30 +deg/s +degrees per second + + + + + +Disabled +Enabled + + + +-200 +200 +1 + + +-200 +200 +1 + + +-18000 +18000 +1 + + +m +meters + + +m +meters + + +m +meters + + +0 127 + + + + + +Disabled +Enabled + + + +0 255 + + +1 1000 +m +meters + + + +North-East-Down +Relative to lead vehicle heading + + + +-100 100 +1 +m +meters + + +-100 100 +1 +m +meters + + +-100 100 +1 +m +meters + + + +None +Face Lead Vehicle +Same as Lead vehicle +Direction of Flight + + + +0.01 1.00 +0.01 + + + + +True +True +1 +Pa +pascal + + +True +1 +degC +degrees Celsius + + +0.1 +m +meters + + + +FirstBaro +2ndBaro +3rdBaro + + + + +Disabled +Bus0 +Bus1 + + + +1.0:Freshwater,1.024:Saltwater + + +True +True +1 +Pa +pascal + + +True +True +1 +Pa +pascal + + +0 100 +1 +% +percent + + + + + +None +AUTO +uBlox +MTK +MTK19 +NMEA +SiRF +HIL +SwiftNav +UAVCAN +SBF +GSOF +ERB +MAV +NOVA + +True + + + +None +AUTO +uBlox +MTK +MTK19 +NMEA +SiRF +HIL +SwiftNav +UAVCAN +SBF +GSOF +ERB +MAV +NOVA + +True + + + +Portable +Stationary +Pedestrian +Automotive +Sea +Airborne1G +Airborne2G +Airborne4G + + + + +Disabled +UseBest +Blend + + + + +Any +FloatRTK +IntegerRTK + +True + + + +Disabled +Enabled +NoChange + + + +-100 90 +deg +degrees + + + +send to first GPS +send to 2nd GPS +send to all + + + + +None (0x0000) +All (0xFFFF) +External only (0xFF00) + + + + +Ignore +Always log +Stop logging when disarmed (SBF only) +Only log every five samples (uBlox only) + +True + + +0:GPS,1:SBAS,2:Galileo,3:Beidou,4:IMES,5:QZSS,6:GLOSNASS + +Leave as currently configured +GPS-NoSBAS +GPS+SBAS +Galileo-NoSBAS +Galileo+SBAS +Beidou +GPS+IMES+QZSS+SBAS (Japan Only) +GLONASS +GLONASS+SBAS +GPS+GLONASS+SBAS + + + + +Do not save config +Save config +Save only when needed + + + +0:GPS,1:SBAS,2:Galileo,3:Beidou,4:IMES,5:QZSS,6:GLOSNASS + +Leave as currently configured +GPS-NoSBAS +GPS+SBAS +Galileo-NoSBAS +Galileo+SBAS +Beidou +GPS+IMES+QZSS+SBAS (Japan Only) +GLONASS +GLONASS+SBAS +GPS+GLONASS+SBAS + + + + +Disables automatic configuration +Enable automatic configuration + + + +50 200 + +10Hz +8Hz +5Hz + +ms +milliseconds + + +50 200 + +10Hz +8Hz +5Hz + +ms +milliseconds + + +m +meters + + +m +meters + + +m +meters + + +m +meters + + +m +meters + + +m +meters + + +0 250 +ms +milliseconds +True + + +0 250 +ms +milliseconds +True + + +0:Horiz Pos,1:Vert Pos,2:Speed + + +5.0 30.0 +s +seconds + + + + + +Disabled +Enabled + + + + +None +Servo +EPM + + + +1000 2000 +PWM +PWM in microseconds + + +1000 2000 +PWM +PWM in microseconds + + +1000 2000 +PWM +PWM in microseconds + + +0 255 +s +seconds + + +0 255 + + + + +-180 180 +1 +deg +degrees + + +-180 180 +1 +deg +degrees + + +-180 180 +1 +deg +degrees + + + +Servo only +Servo with ExtGyro +DirectDrive VarPitch +DirectDrive FixedPitch + + + + +H3 CCPM Adjustable +H1 Straight Swash +H3_140 CCPM + + + +0 1000 +1 +PWM +PWM in microseconds + + +-30 30 +1 +deg +degrees + + +-10 10 +0.1 + + + +NoFlybar +Flybar + + + +0 1000 +1 +PWM +PWM in microseconds + + +0 1000 +1 +PWM +PWM in microseconds + + +0: Normal, 1: Reversed + + +-180 180 +1 +deg +degrees + + +-180 180 +1 +deg +degrees + + +-180 180 +1 +deg +degrees + + +-180 180 +1 +deg +degrees + + +-180 180 +1 +deg +degrees + + +-180 180 +1 +deg +degrees + + +-90 90 +1 +deg +degrees + + +-90 90 +1 +deg +degrees + + + +Longitudinal +Transverse + + + +0 1 + + +-10 10 +0.1 + + +-10 10 +0.1 + + +1000 2000 +1 +PWM +PWM in microseconds + + +1000 2000 +1 +PWM +PWM in microseconds + + +1000 2000 +1 +PWM +PWM in microseconds + + +0: Normal, 1: Reversed + + +1000 2000 +1 +PWM +PWM in microseconds + + +1000 2000 +1 +PWM +PWM in microseconds + + +1000 2000 +1 +PWM +PWM in microseconds + + + +Disabled +Passthrough +Max collective +Mid collective +Min collective + + + +0 1000 +10 +PWM +PWM in microseconds + + + +Ch8 Input +SetPoint +Throttle Curve + + + +0 500 +1 +PWM +PWM in microseconds + + +0 60 +s +seconds + + +0 60 +s +seconds + + +0 1000 +10 + + +0 500 +10 + + +0 18000 +100 +cdeg +centidegrees + + +0 10 +1 + + +0 500 +10 + + +0 1000 +10 + + +0 1000 +10 + + +0 1000 +10 + + +0 1000 +10 + + +0 1000 +10 + + + + +0 500 +1 +d% +decipercent + + +0 500 +1 +d% +decipercent + + +500 1000 +1 +d% +decipercent + + +500 1000 +1 +d% +decipercent + + + +Disabled +Very Low +Low +Medium +High +Very High + + + + + + + +rad/s +radians per second + + +rad/s +radians per second + + +rad/s +radians per second + + +rad/s +radians per second + + +rad/s +radians per second + + +rad/s +radians per second + + +rad/s +radians per second + + +rad/s +radians per second + + +rad/s +radians per second + + +0.8 1.2 + + +0.8 1.2 + + +0.8 1.2 + + +-3.5 3.5 +m/s/s +meters per square second + + +-3.5 3.5 +m/s/s +meters per square second + + +-3.5 3.5 +m/s/s +meters per square second + + +0.8 1.2 + + +0.8 1.2 + + +0.8 1.2 + + +-3.5 3.5 +m/s/s +meters per square second + + +-3.5 3.5 +m/s/s +meters per square second + + +-3.5 3.5 +m/s/s +meters per square second + + +0.8 1.2 + + +0.8 1.2 + + +0.8 1.2 + + +-3.5 3.5 +m/s/s +meters per square second + + +-3.5 3.5 +m/s/s +meters per square second + + +-3.5 3.5 +m/s/s +meters per square second + + +0 127 +Hz +hertz + + +0 127 +Hz +hertz + + + +Disabled +Enabled + + + + +Disabled +Enabled + + + + +Disabled +Enabled + + + +0.05 50 + + + +Never +Start-up only + + + + +Don't adjust the trims +Assume first orientation was level +Assume ACC_BODYFIX is perfectly aligned to the vehicle + + + + +IMU 1 +IMU 2 +IMU 3 + + + +m +meters + + +m +meters + + +m +meters + + +m +meters + + +m +meters + + +m +meters + + +m +meters + + +m +meters + + +m +meters + + +True + + +True + + +True + + +True + + +True + + +True + + +0:FirstIMU,1:SecondIMU,2:ThirdIMU + +FirstIMUOnly +FirstAndSecondIMU + + + + + +32 + + +0:IMU1,1:IMU2,2:IMU3 + +None +First IMU +All + + + +0:Sensor-Rate Logging (sample at full sensor rate seen by AP) + + +ms +milliseconds +10 + + +1 + + + + + +Disabled +Enabled + + + +10 200 +Hz +hertz + + +5 50 +Hz +hertz + + +5 30 +dB +decibel + + + + +1000 2000 +1 +PWM +PWM in microseconds + + +1000 2000 +1 +PWM +PWM in microseconds + + + +WaitForPilotInput +Retract +Deploy + + + + + + +None +File +MAVLink +BothFileAndMAVLink + + + + + + +Disabled +Enabled + + + + +Disabled +Enabled + + + + +Disabled +Enabled + + + +kB +kilobytes + + + + +0 45 +1 +deg +degrees + + +20 2000 +50 +cm/s +centimeters per second + + +100 981 +1 +cm/s/s +centimeters per square second + + +25 250 +1 +cm/s/s +centimeters per square second + + +500 5000 +1 +cm/s/s/s +centimeters per cubic second + + +0 2 +0.1 +s +seconds + + + + +0 32766 +1 + + + +Resume Mission +Restart Mission + + + +0:Clear Mission on reboot + + + + + +Retracted +Neutral +MavLink Targeting +RC Targeting +GPS Point + + + +-180.00 179.99 +1 +deg +degrees + + +-180.00 179.99 +1 +deg +degrees + + +-180.00 179.99 +1 +deg +degrees + + +-180.00 179.99 +1 +deg +degrees + + +-180.00 179.99 +1 +deg +degrees + + +-180.00 179.99 +1 +deg +degrees + + + +Disabled +Enabled + + + + +Disabled +Enabled + + + + +Disabled +Enabled + + + + +Disabled +RC5 +RC6 +RC7 +RC8 +RC9 +RC10 +RC11 +RC12 + + + +-18000 17999 +1 +cdeg +centidegrees + + +-18000 17999 +1 +cdeg +centidegrees + + + +Disabled +RC5 +RC6 +RC7 +RC8 +RC9 +RC10 +RC11 +RC12 + + + +-18000 17999 +1 +cdeg +centidegrees + + +-18000 17999 +1 +cdeg +centidegrees + + + +Disabled +RC5 +RC6 +RC7 +RC8 +RC9 +RC10 +RC11 +RC12 + + + +-18000 17999 +1 +cdeg +centidegrees + + +-18000 17999 +1 +cdeg +centidegrees + + +0 100 +1 + + +0.0 0.2 +.005 +s +seconds + + +0.0 0.2 +.005 +s +seconds + + + +None +Servo +3DR Solo +Alexmos Serial +SToRM32 MAVLink +SToRM32 Serial + +True + + + +Retracted +Neutral +MavLink Targeting +RC Targeting +GPS Point + + + +-180.00 179.99 +1 +deg +degrees + + +-180.00 179.99 +1 +deg +degrees + + +-180.00 179.99 +1 +deg +degrees + + +-180.00 179.99 +1 +deg +degrees + + +-180.00 179.99 +1 +deg +degrees + + +-180.00 179.99 +1 +deg +degrees + + + +Disabled +Enabled + + + + +Disabled +Enabled + + + + +Disabled +Enabled + + + + +Disabled +RC5 +RC6 +RC7 +RC8 +RC9 +RC10 +RC11 +RC12 + + + +-18000 17999 +1 +cdeg +centidegrees + + +-18000 17999 +1 +cdeg +centidegrees + + + +Disabled +RC5 +RC6 +RC7 +RC8 +RC9 +RC10 +RC11 +RC12 + + + +-18000 17999 +1 +cdeg +centidegrees + + +-18000 17999 +1 +cdeg +centidegrees + + + +Disabled +RC5 +RC6 +RC7 +RC8 +RC9 +RC10 +RC11 +RC12 + + + +-18000 17999 +1 +cdeg +centidegrees + + +-18000 17999 +1 +cdeg +centidegrees + + +0.0 0.2 +.005 +s +seconds + + +0.0 0.2 +.005 +s +seconds + + + +None +Servo +3DR Solo +Alexmos Serial +SToRM32 MAVLink +SToRM32 Serial + + + + + +0 500 +PWM +PWM in microseconds + + +0.25 0.8 + + +0.9:Low, 0.95:Default, 1.0:High + + +6 35 +V +volt + + +6 35 +V +volt + + +0 200 +A +ampere + + + +Normal +OneShot +OneShot125 +Brushed +DShot150 +DShot300 +DShot600 +DShot1200 + +True + + +0 2000 +PWM +PWM in microseconds + + +0 2000 +PWM +PWM in microseconds + + +0.0:Low, 0.15:Default, 0.3:High + + +0.0:Low, 0.1:Default, 0.2:High + + +0 10 +s +seconds + + +0.2 0.8 + + + +Disabled +Learn +LearnAndSave + + + + +PWM enabled while disarmed +PWM disabled while disarmed + + + +5 80 +1 +deg +degrees + + +0 2 +0.1 +s +seconds + + +0 5 +0.1 + + + +First battery +Second battery + + + + + + +Off +Low +Medium +High + + + + +Disable +Enable + + + + +Disable +Enable + + + + +Disable +ssd1306 +sh1106 + + + + +Disabled +Aircraft +Rover + + + + +Disabled + + + + + + +Disabled +Enabled Always Land +Enabled Strict + + + + +None +CompanionComputer +IRLock +SITL_Gazebo +SITL + + + +0 360 +1 +cdeg +centidegrees + + +-20 20 +1 +cm +centimeters + + +-20 20 +1 +cm +centimeters + + + +RawSensor +KalmanFilter + + + +0.5 5 + + +m +meters + + +m +meters + + +m +meters + + + +DefaultBus +InternalI2C +ExternalI2C + + + + + + +None +LightWareSF40C +MAVLink +TeraRangerTower +RangeFinder +RPLidarA2 + +True + + + +Default +Upside Down + + + +-180 180 +deg +degrees + + +0 360 +deg +degrees + + +0 45 +deg +degrees + + +0 360 +deg +degrees + + +0 45 +deg +degrees + + +0 360 +deg +degrees + + +0 45 +deg +degrees + + +0 360 +deg +degrees + + +0 45 +deg +degrees + + +0 360 +deg +degrees + + +0 45 +deg +degrees + + +0 360 +deg +degrees + + +0 45 +deg +degrees + + + +None +LightWareSF40C +MAVLink +TeraRangerTower +RangeFinder +RPLidarA2 + +True + + + +Default +Upside Down + + + +-180 180 +deg +degrees + + + + +0.5 5 +0.1 +Hz +hertz + + +1.000 3.000 + + +1.000 8.000 + + +0.500 1.500 +0.05 + + +0.000 3.000 + + +0 1000 +d% +decipercent + + +0.000 0.400 + + +1.000 100.000 +Hz +hertz + + +0.500 2.000 + + +0.1 6.0 +0.1 + + +0.02 1.00 +0.01 + + +0.00 1.00 +0.001 + + +0 4500 +10 +cm/s/s +centimeters per square second + + +0 100 +Hz +hertz + + +0 100 +Hz +hertz + + +0 45 +1 +deg +degrees + + + + + + +0.1 +km +kilometers + + + +DoNotIncludeHome +IncludeHome + + + + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + +0 200 +PWM +PWM in microseconds + + + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + +0 200 +PWM +PWM in microseconds + + + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + +0 200 +PWM +PWM in microseconds + + + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + +0 200 +PWM +PWM in microseconds + + + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + +0 200 +PWM +PWM in microseconds + + + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + +0 200 +PWM +PWM in microseconds + + + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + +0 200 +PWM +PWM in microseconds + + + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + +0 200 +PWM +PWM in microseconds + + + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + +0 200 +PWM +PWM in microseconds + + + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + +0 200 +PWM +PWM in microseconds + + + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + +0 200 +PWM +PWM in microseconds + + + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + +0 200 +PWM +PWM in microseconds + + + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + +0 200 +PWM +PWM in microseconds + + + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + +0 200 +PWM +PWM in microseconds + + + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + +0 200 +PWM +PWM in microseconds + + + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + +0 200 +PWM +PWM in microseconds + + + + +1 8 +1 +True + + +1 8 +1 +True + + +1 8 +1 +True + + +1 8 +1 +True + + +1 8 +1 +True + + +1 8 +1 +True + + + + + +Disabled +APM2 A9 pin +APM1 relay +BB Blue GP0 pin 4 +Pixhawk AUXOUT1 +Pixhawk AUXOUT2 +Pixhawk AUXOUT3 +Pixhawk AUXOUT4 +Pixhawk AUXOUT5 +Pixhawk AUXOUT6 +BB Blue GP0 pin 3 +PX4 FMU Relay1 +PX4 FMU Relay2 +PX4IO Relay1/BB Blue GP0 pin 6 +PX4IO Relay2 +PX4IO ACC1 +PX4IO ACC2/BB Blue GP0 pin 5 + + + + +Disabled +APM2 A9 pin +APM1 relay +BB Blue GP0 pin 4 +Pixhawk AUXOUT1 +Pixhawk AUXOUT2 +Pixhawk AUXOUT3 +Pixhawk AUXOUT4 +Pixhawk AUXOUT5 +Pixhawk AUXOUT6 +BB Blue GP0 pin 3 +PX4 FMU Relay1 +PX4 FMU Relay2 +PX4IO Relay1/BB Blue GP0 pin 6 +PX4IO Relay2 +PX4IO ACC1 +PX4IO ACC2/BB Blue GP0 pin 5 + + + + +Disabled +APM2 A9 pin +APM1 relay +BB Blue GP0 pin 4 +Pixhawk AUXOUT1 +Pixhawk AUXOUT2 +Pixhawk AUXOUT3 +Pixhawk AUXOUT4 +Pixhawk AUXOUT5 +Pixhawk AUXOUT6 +BB Blue GP0 pin 3 +PX4 FMU Relay1 +PX4 FMU Relay2 +PX4IO Relay1/BB Blue GP0 pin 6 +PX4IO Relay2 +PX4IO ACC1 +PX4IO ACC2/BB Blue GP0 pin 5 + + + + +Disabled +APM2 A9 pin +APM1 relay +BB Blue GP0 pin 4 +Pixhawk AUXOUT1 +Pixhawk AUXOUT2 +Pixhawk AUXOUT3 +Pixhawk AUXOUT4 +Pixhawk AUXOUT5 +Pixhawk AUXOUT6 +BB Blue GP0 pin 3 +PX4 FMU Relay1 +PX4 FMU Relay2 +PX4IO Relay1/BB Blue GP0 pin 6 +PX4IO Relay2 +PX4IO ACC1 +PX4IO ACC2/BB Blue GP0 pin 5 + + + + +Off +On +NoChange + + + + + + +None +Analog +MaxbotixI2C +LidarLiteV2-I2C +PX4-PWM +BBB-PRU +LightWareI2C +LightWareSerial +Bebop +MAVLink +uLanding +LeddarOne +MaxbotixSerial +TeraRangerI2C +LidarLiteV3-I2C +VL53L0X +NMEA + + + + +Not Used +APM2-A0 +APM2-A1 +APM2-A2 +APM2-A3 +APM2-A4 +APM2-A5 +APM2-A6 +APM2-A7 +APM2-A8 +APM2-A9 +PX4-airspeed port +Pixhawk-airspeed port +APM1-airspeed port + + + +0.001 +m/V +meters per volt + + +0.001 +V +volt + + + +Linear +Inverted +Hyperbolic + + + +1 +cm +centimeters + + +1 +cm +centimeters + + + +Not Used +Pixhawk AUXOUT1 +Pixhawk AUXOUT2 +Pixhawk AUXOUT3 +Pixhawk AUXOUT4 +Pixhawk AUXOUT5 +Pixhawk AUXOUT6 +PX4 FMU Relay1 +PX4 FMU Relay2 +PX4IO Relay1 +PX4IO Relay2 +PX4IO ACC1 +PX4IO ACC2 + + + +1 +ms +milliseconds + + + +No +Yes + + + +0 32767 +m +meters + + +5 127 +1 +cm +centimeters + + +0 127 +1 + + +m +meters + + +m +meters + + +m +meters + + + +Forward +Forward-Right +Right +Back-Right +Back +Back-Left +Left +Forward-Left +Up +Down + + + + +None +Analog +MaxbotixI2C +LidarLiteV2-I2C +PX4-PWM +BBB-PRU +LightWareI2C +LightWareSerial +Bebop +MAVLink +uLanding +LeddarOne +MaxbotixSerial +TeraRangerI2C +LidarLiteV3-I2C +VL53L0X +NMEA + + + + +Not Used +APM2-A0 +APM2-A1 +APM2-A2 +APM2-A3 +APM2-A4 +APM2-A5 +APM2-A6 +APM2-A7 +APM2-A8 +APM2-A9 +PX4-airspeed port +Pixhawk-airspeed port +APM1-airspeed port + + + +0.001 +m/V +meters per volt + + +0.001 +V +volt + + + +Linear +Inverted +Hyperbolic + + + +1 +cm +centimeters + + +1 +cm +centimeters + + + +Not Used +Pixhawk AUXOUT1 +Pixhawk AUXOUT2 +Pixhawk AUXOUT3 +Pixhawk AUXOUT4 +Pixhawk AUXOUT5 +Pixhawk AUXOUT6 +PX4 FMU Relay1 +PX4 FMU Relay2 +PX4IO Relay1 +PX4IO Relay2 +PX4IO ACC1 +PX4IO ACC2 + + + +1 +ms +milliseconds + + + +No +Yes + + + +0 127 +1 +cm +centimeters + + +0 127 +1 + + +m +meters + + +m +meters + + +m +meters + + + +Forward +Forward-Right +Right +Back-Right +Back +Back-Left +Left +Forward-Left +Up +Down + + + + +None +Analog +MaxbotixI2C +LidarLiteV2-I2C +PX4-PWM +BBB-PRU +LightWareI2C +LightWareSerial +Bebop +MAVLink +uLanding +LeddarOne +MaxbotixSerial +TeraRangerI2C +LidarLiteV3-I2C +VL53L0X +NMEA + + + + +Not Used +APM2-A0 +APM2-A1 +APM2-A2 +APM2-A3 +APM2-A4 +APM2-A5 +APM2-A6 +APM2-A7 +APM2-A8 +APM2-A9 +PX4-airspeed port +Pixhawk-airspeed port +APM1-airspeed port + + + +0.001 +m/V +meters per volt + + +0.001 +V +volt + + + +Linear +Inverted +Hyperbolic + + + +1 +cm +centimeters + + +1 +cm +centimeters + + + +Not Used +Pixhawk AUXOUT1 +Pixhawk AUXOUT2 +Pixhawk AUXOUT3 +Pixhawk AUXOUT4 +Pixhawk AUXOUT5 +Pixhawk AUXOUT6 +PX4 FMU Relay1 +PX4 FMU Relay2 +PX4IO Relay1 +PX4IO Relay2 +PX4IO ACC1 +PX4IO ACC2 + + + +1 +ms +milliseconds + + + +No +Yes + + + +0 127 +1 +cm +centimeters + + +0 127 +1 + + +m +meters + + +m +meters + + +m +meters + + + +Forward +Forward-Right +Right +Back-Right +Back +Back-Left +Left +Forward-Left +Up +Down + + + + +None +Analog +MaxbotixI2C +LidarLiteV2-I2C +PX4-PWM +BBB-PRU +LightWareI2C +LightWareSerial +Bebop +MAVLink +uLanding +LeddarOne +MaxbotixSerial +TeraRangerI2C +LidarLiteV3-I2C +VL53L0X +NMEA + + + + +Not Used +APM2-A0 +APM2-A1 +APM2-A2 +APM2-A3 +APM2-A4 +APM2-A5 +APM2-A6 +APM2-A7 +APM2-A8 +APM2-A9 +PX4-airspeed port +Pixhawk-airspeed port +APM1-airspeed port + + + +0.001 +m/V +meters per volt + + +0.001 +V +volt + + + +Linear +Inverted +Hyperbolic + + + +1 +cm +centimeters + + +1 +cm +centimeters + + + +Not Used +Pixhawk AUXOUT1 +Pixhawk AUXOUT2 +Pixhawk AUXOUT3 +Pixhawk AUXOUT4 +Pixhawk AUXOUT5 +Pixhawk AUXOUT6 +PX4 FMU Relay1 +PX4 FMU Relay2 +PX4IO Relay1 +PX4IO Relay2 +PX4IO ACC1 +PX4IO ACC2 + + + +1 +ms +milliseconds + + + +No +Yes + + + +0 127 +1 +cm +centimeters + + +0 127 +1 + + +m +meters + + +m +meters + + +m +meters + + + +Forward +Forward-Right +Right +Back-Right +Back +Back-Left +Left +Forward-Left +Up +Down + + + + + + +None +PX4-PWM +AUXPIN + + + +0.001 + + +1 + + +1 + + +0.1 + + + +Disabled +PixhawkAUX1 +PixhawkAUX2 +PixhawkAUX3 +PixhawkAUX4 +PixhawkAUX5 +PixhawkAUX6 + + + + +None +PX4-PWM +AUXPIN + + + +0.001 + + + +Disabled +PixhawkAUX1 +PixhawkAUX2 +PixhawkAUX3 +PixhawkAUX4 +PixhawkAUX5 +PixhawkAUX6 + + + + + + +Disabled +AnalogPin +RCChannelPwmValue +ReceiverProtocol + + + + +APM2 A0 +APM2 A1 +APM2 A13 +Pixracer +Pixhawk ADC4 +Pixhawk ADC3 +Pixhawk ADC6 +Pixhawk SBUS +Pixhawk2 ADC + + + +0 5.0 +0.01 +V +volt + + +0 5.0 +0.01 +V +volt + + + + +0 2000 +PWM +PWM in microseconds + + +0 2000 +PWM +PWM in microseconds + + + + + +Disabled +ShowSlips +ShowOverruns + + + + +50Hz +100Hz +200Hz +250Hz +300Hz +400Hz + +True + + + + + +1200 +2400 +4800 +9600 +19200 +38400 +57600 +111100 +115200 +460800 +500000 +921600 +1500000 + + + + +MAVlink1 +MAVLink2 + +True + + + +None +MAVLink1 +MAVLink2 +Frsky D +Frsky SPort +GPS +Alexmos Gimbal Serial +SToRM32 Gimbal Serial +Rangefinder +FrSky SPort Passthrough (OpenTX) +Lidar360 +Beacon +Volz servo out +SBus servo out +ESC Telemetry +Devo Telemetry + +True + + + +1200 +2400 +4800 +9600 +19200 +38400 +57600 +111100 +115200 +500000 +921600 +1500000 + + + + +None +MAVLink1 +MAVLink2 +Frsky D +Frsky SPort +GPS +Alexmos Gimbal Serial +SToRM32 Gimbal Serial +Rangefinder +FrSky SPort Passthrough (OpenTX) +Lidar360 +Beacon +Volz servo out +SBus servo out +ESC Telemetry +Devo Telemetry + +True + + + +1200 +2400 +4800 +9600 +19200 +38400 +57600 +111100 +115200 +500000 +921600 +1500000 + + + + +None +MAVLink1 +MAVLink2 +Frsky D +Frsky SPort +GPS +Alexmos Gimbal Serial +SToRM32 Gimbal Serial +Rangefinder +FrSky SPort Passthrough (OpenTX) +Lidar360 +Beacon +Volz servo out +SBus servo out +ESC Telemetry +Devo Telemetry + +True + + + +1200 +2400 +4800 +9600 +19200 +38400 +57600 +111100 +115200 +500000 +921600 +1500000 + + + + +None +MAVLink1 +MAVLink2 +Frsky D +Frsky SPort +GPS +Alexmos Gimbal Serial +SToRM32 Gimbal Serial +Rangefinder +FrSky SPort Passthrough (OpenTX) +Lidar360 +Beacon +Volz servo out +SBus servo out +ESC Telemetry +Devo Telemetry + +True + + + +1200 +2400 +4800 +9600 +19200 +38400 +57600 +111100 +115200 +500000 +921600 +1500000 + + + + +None +MAVLink1 +MAVLink2 +Frsky D +Frsky SPort +GPS +Alexmos Gimbal Serial +SToRM32 Gimbal Serial +Rangefinder +FrSky SPort Passthrough (OpenTX) +Lidar360 +Beacon +Volz servo out +SBus servo out +ESC Telemetry +Devo Telemetry + +True + + + +1200 +2400 +4800 +9600 +19200 +38400 +57600 +111100 +115200 +500000 +921600 +1500000 + + + + + + +Disable +Enable + + + +25 400 +Hz +hertz + + + + +500 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + + +Disabled +RCPassThru +Flap +Flap_auto +Aileron +mount_pan +mount_tilt +mount_roll +mount_open +camera_trigger +release +mount2_pan +mount2_tilt +mount2_roll +mount2_open +DifferentialSpoilerLeft1 +DifferentialSpoilerRight1 +DifferentialSpoilerLeft2 +DifferentialSpoilerRight2 +Elevator +Rudder +FlaperonLeft +FlaperonRight +GroundSteering +Parachute +EPM +LandingGear +EngineRunEnable +HeliRSC +HeliTailRSC +Motor1 +Motor2 +Motor3 +Motor4 +Motor5 +Motor6 +Motor7 +Motor8 +MotorTilt +RCIN1 +RCIN2 +RCIN3 +RCIN4 +RCIN5 +RCIN6 +RCIN7 +RCIN8 +RCIN9 +RCIN10 +RCIN11 +RCIN12 +RCIN13 +RCIN14 +RCIN15 +RCIN16 +Ignition +Choke +Starter +Throttle +TrackerYaw +TrackerPitch +ThrottleLeft +ThrottleRight +tiltMotorLeft +tiltMotorRight +ElevonLeft +ElevonRight +VTailLeft +VTailRight +BoostThrottle +Motor9 +Motor10 +Motor11 +Motor12 +Winch + + + + + +500 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + + +Disabled +RCPassThru +Flap +Flap_auto +Aileron +mount_pan +mount_tilt +mount_roll +mount_open +camera_trigger +release +mount2_pan +mount2_tilt +mount2_roll +mount2_open +DifferentialSpoilerLeft1 +DifferentialSpoilerRight1 +DifferentialSpoilerLeft2 +DifferentialSpoilerRight2 +Elevator +Rudder +FlaperonLeft +FlaperonRight +GroundSteering +Parachute +EPM +LandingGear +EngineRunEnable +HeliRSC +HeliTailRSC +Motor1 +Motor2 +Motor3 +Motor4 +Motor5 +Motor6 +Motor7 +Motor8 +MotorTilt +RCIN1 +RCIN2 +RCIN3 +RCIN4 +RCIN5 +RCIN6 +RCIN7 +RCIN8 +RCIN9 +RCIN10 +RCIN11 +RCIN12 +RCIN13 +RCIN14 +RCIN15 +RCIN16 +Ignition +Choke +Starter +Throttle +TrackerYaw +TrackerPitch +ThrottleLeft +ThrottleRight +tiltMotorLeft +tiltMotorRight +ElevonLeft +ElevonRight +VTailLeft +VTailRight +BoostThrottle +Motor9 +Motor10 +Motor11 +Motor12 +Winch + + + + + +500 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + + +Disabled +RCPassThru +Flap +Flap_auto +Aileron +mount_pan +mount_tilt +mount_roll +mount_open +camera_trigger +release +mount2_pan +mount2_tilt +mount2_roll +mount2_open +DifferentialSpoilerLeft1 +DifferentialSpoilerRight1 +DifferentialSpoilerLeft2 +DifferentialSpoilerRight2 +Elevator +Rudder +FlaperonLeft +FlaperonRight +GroundSteering +Parachute +EPM +LandingGear +EngineRunEnable +HeliRSC +HeliTailRSC +Motor1 +Motor2 +Motor3 +Motor4 +Motor5 +Motor6 +Motor7 +Motor8 +MotorTilt +RCIN1 +RCIN2 +RCIN3 +RCIN4 +RCIN5 +RCIN6 +RCIN7 +RCIN8 +RCIN9 +RCIN10 +RCIN11 +RCIN12 +RCIN13 +RCIN14 +RCIN15 +RCIN16 +Ignition +Choke +Starter +Throttle +TrackerYaw +TrackerPitch +ThrottleLeft +ThrottleRight +tiltMotorLeft +tiltMotorRight +ElevonLeft +ElevonRight +VTailLeft +VTailRight +BoostThrottle +Motor9 +Motor10 +Motor11 +Motor12 +Winch + + + + + +500 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + + +Disabled +RCPassThru +Flap +Flap_auto +Aileron +mount_pan +mount_tilt +mount_roll +mount_open +camera_trigger +release +mount2_pan +mount2_tilt +mount2_roll +mount2_open +DifferentialSpoilerLeft1 +DifferentialSpoilerRight1 +DifferentialSpoilerLeft2 +DifferentialSpoilerRight2 +Elevator +Rudder +FlaperonLeft +FlaperonRight +GroundSteering +Parachute +EPM +LandingGear +EngineRunEnable +HeliRSC +HeliTailRSC +Motor1 +Motor2 +Motor3 +Motor4 +Motor5 +Motor6 +Motor7 +Motor8 +MotorTilt +RCIN1 +RCIN2 +RCIN3 +RCIN4 +RCIN5 +RCIN6 +RCIN7 +RCIN8 +RCIN9 +RCIN10 +RCIN11 +RCIN12 +RCIN13 +RCIN14 +RCIN15 +RCIN16 +Ignition +Choke +Starter +Throttle +TrackerYaw +TrackerPitch +ThrottleLeft +ThrottleRight +tiltMotorLeft +tiltMotorRight +ElevonLeft +ElevonRight +VTailLeft +VTailRight +BoostThrottle +Motor9 +Motor10 +Motor11 +Motor12 +Winch + + + + + +500 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + + +Disabled +RCPassThru +Flap +Flap_auto +Aileron +mount_pan +mount_tilt +mount_roll +mount_open +camera_trigger +release +mount2_pan +mount2_tilt +mount2_roll +mount2_open +DifferentialSpoilerLeft1 +DifferentialSpoilerRight1 +DifferentialSpoilerLeft2 +DifferentialSpoilerRight2 +Elevator +Rudder +FlaperonLeft +FlaperonRight +GroundSteering +Parachute +EPM +LandingGear +EngineRunEnable +HeliRSC +HeliTailRSC +Motor1 +Motor2 +Motor3 +Motor4 +Motor5 +Motor6 +Motor7 +Motor8 +MotorTilt +RCIN1 +RCIN2 +RCIN3 +RCIN4 +RCIN5 +RCIN6 +RCIN7 +RCIN8 +RCIN9 +RCIN10 +RCIN11 +RCIN12 +RCIN13 +RCIN14 +RCIN15 +RCIN16 +Ignition +Choke +Starter +Throttle +TrackerYaw +TrackerPitch +ThrottleLeft +ThrottleRight +tiltMotorLeft +tiltMotorRight +ElevonLeft +ElevonRight +VTailLeft +VTailRight +BoostThrottle +Motor9 +Motor10 +Motor11 +Motor12 +Winch + + + + + +500 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + + +Disabled +RCPassThru +Flap +Flap_auto +Aileron +mount_pan +mount_tilt +mount_roll +mount_open +camera_trigger +release +mount2_pan +mount2_tilt +mount2_roll +mount2_open +DifferentialSpoilerLeft1 +DifferentialSpoilerRight1 +DifferentialSpoilerLeft2 +DifferentialSpoilerRight2 +Elevator +Rudder +FlaperonLeft +FlaperonRight +GroundSteering +Parachute +EPM +LandingGear +EngineRunEnable +HeliRSC +HeliTailRSC +Motor1 +Motor2 +Motor3 +Motor4 +Motor5 +Motor6 +Motor7 +Motor8 +MotorTilt +RCIN1 +RCIN2 +RCIN3 +RCIN4 +RCIN5 +RCIN6 +RCIN7 +RCIN8 +RCIN9 +RCIN10 +RCIN11 +RCIN12 +RCIN13 +RCIN14 +RCIN15 +RCIN16 +Ignition +Choke +Starter +Throttle +TrackerYaw +TrackerPitch +ThrottleLeft +ThrottleRight +tiltMotorLeft +tiltMotorRight +ElevonLeft +ElevonRight +VTailLeft +VTailRight +BoostThrottle +Motor9 +Motor10 +Motor11 +Motor12 +Winch + + + + + +500 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + + +Disabled +RCPassThru +Flap +Flap_auto +Aileron +mount_pan +mount_tilt +mount_roll +mount_open +camera_trigger +release +mount2_pan +mount2_tilt +mount2_roll +mount2_open +DifferentialSpoilerLeft1 +DifferentialSpoilerRight1 +DifferentialSpoilerLeft2 +DifferentialSpoilerRight2 +Elevator +Rudder +FlaperonLeft +FlaperonRight +GroundSteering +Parachute +EPM +LandingGear +EngineRunEnable +HeliRSC +HeliTailRSC +Motor1 +Motor2 +Motor3 +Motor4 +Motor5 +Motor6 +Motor7 +Motor8 +MotorTilt +RCIN1 +RCIN2 +RCIN3 +RCIN4 +RCIN5 +RCIN6 +RCIN7 +RCIN8 +RCIN9 +RCIN10 +RCIN11 +RCIN12 +RCIN13 +RCIN14 +RCIN15 +RCIN16 +Ignition +Choke +Starter +Throttle +TrackerYaw +TrackerPitch +ThrottleLeft +ThrottleRight +tiltMotorLeft +tiltMotorRight +ElevonLeft +ElevonRight +VTailLeft +VTailRight +BoostThrottle +Motor9 +Motor10 +Motor11 +Motor12 +Winch + + + + + +500 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + + +Disabled +RCPassThru +Flap +Flap_auto +Aileron +mount_pan +mount_tilt +mount_roll +mount_open +camera_trigger +release +mount2_pan +mount2_tilt +mount2_roll +mount2_open +DifferentialSpoilerLeft1 +DifferentialSpoilerRight1 +DifferentialSpoilerLeft2 +DifferentialSpoilerRight2 +Elevator +Rudder +FlaperonLeft +FlaperonRight +GroundSteering +Parachute +EPM +LandingGear +EngineRunEnable +HeliRSC +HeliTailRSC +Motor1 +Motor2 +Motor3 +Motor4 +Motor5 +Motor6 +Motor7 +Motor8 +MotorTilt +RCIN1 +RCIN2 +RCIN3 +RCIN4 +RCIN5 +RCIN6 +RCIN7 +RCIN8 +RCIN9 +RCIN10 +RCIN11 +RCIN12 +RCIN13 +RCIN14 +RCIN15 +RCIN16 +Ignition +Choke +Starter +Throttle +TrackerYaw +TrackerPitch +ThrottleLeft +ThrottleRight +tiltMotorLeft +tiltMotorRight +ElevonLeft +ElevonRight +VTailLeft +VTailRight +BoostThrottle +Motor9 +Motor10 +Motor11 +Motor12 +Winch + + + + + +500 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + + +Disabled +RCPassThru +Flap +Flap_auto +Aileron +mount_pan +mount_tilt +mount_roll +mount_open +camera_trigger +release +mount2_pan +mount2_tilt +mount2_roll +mount2_open +DifferentialSpoilerLeft1 +DifferentialSpoilerRight1 +DifferentialSpoilerLeft2 +DifferentialSpoilerRight2 +Elevator +Rudder +FlaperonLeft +FlaperonRight +GroundSteering +Parachute +EPM +LandingGear +EngineRunEnable +HeliRSC +HeliTailRSC +Motor1 +Motor2 +Motor3 +Motor4 +Motor5 +Motor6 +Motor7 +Motor8 +MotorTilt +RCIN1 +RCIN2 +RCIN3 +RCIN4 +RCIN5 +RCIN6 +RCIN7 +RCIN8 +RCIN9 +RCIN10 +RCIN11 +RCIN12 +RCIN13 +RCIN14 +RCIN15 +RCIN16 +Ignition +Choke +Starter +Throttle +TrackerYaw +TrackerPitch +ThrottleLeft +ThrottleRight +tiltMotorLeft +tiltMotorRight +ElevonLeft +ElevonRight +VTailLeft +VTailRight +BoostThrottle +Motor9 +Motor10 +Motor11 +Motor12 +Winch + + + + + +500 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + + +Disabled +RCPassThru +Flap +Flap_auto +Aileron +mount_pan +mount_tilt +mount_roll +mount_open +camera_trigger +release +mount2_pan +mount2_tilt +mount2_roll +mount2_open +DifferentialSpoilerLeft1 +DifferentialSpoilerRight1 +DifferentialSpoilerLeft2 +DifferentialSpoilerRight2 +Elevator +Rudder +FlaperonLeft +FlaperonRight +GroundSteering +Parachute +EPM +LandingGear +EngineRunEnable +HeliRSC +HeliTailRSC +Motor1 +Motor2 +Motor3 +Motor4 +Motor5 +Motor6 +Motor7 +Motor8 +MotorTilt +RCIN1 +RCIN2 +RCIN3 +RCIN4 +RCIN5 +RCIN6 +RCIN7 +RCIN8 +RCIN9 +RCIN10 +RCIN11 +RCIN12 +RCIN13 +RCIN14 +RCIN15 +RCIN16 +Ignition +Choke +Starter +Throttle +TrackerYaw +TrackerPitch +ThrottleLeft +ThrottleRight +tiltMotorLeft +tiltMotorRight +ElevonLeft +ElevonRight +VTailLeft +VTailRight +BoostThrottle +Motor9 +Motor10 +Motor11 +Motor12 +Winch + + + + + +500 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + + +Disabled +RCPassThru +Flap +Flap_auto +Aileron +mount_pan +mount_tilt +mount_roll +mount_open +camera_trigger +release +mount2_pan +mount2_tilt +mount2_roll +mount2_open +DifferentialSpoilerLeft1 +DifferentialSpoilerRight1 +DifferentialSpoilerLeft2 +DifferentialSpoilerRight2 +Elevator +Rudder +FlaperonLeft +FlaperonRight +GroundSteering +Parachute +EPM +LandingGear +EngineRunEnable +HeliRSC +HeliTailRSC +Motor1 +Motor2 +Motor3 +Motor4 +Motor5 +Motor6 +Motor7 +Motor8 +MotorTilt +RCIN1 +RCIN2 +RCIN3 +RCIN4 +RCIN5 +RCIN6 +RCIN7 +RCIN8 +RCIN9 +RCIN10 +RCIN11 +RCIN12 +RCIN13 +RCIN14 +RCIN15 +RCIN16 +Ignition +Choke +Starter +Throttle +TrackerYaw +TrackerPitch +ThrottleLeft +ThrottleRight +tiltMotorLeft +tiltMotorRight +ElevonLeft +ElevonRight +VTailLeft +VTailRight +BoostThrottle +Motor9 +Motor10 +Motor11 +Motor12 +Winch + + + + + +500 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + + +Disabled +RCPassThru +Flap +Flap_auto +Aileron +mount_pan +mount_tilt +mount_roll +mount_open +camera_trigger +release +mount2_pan +mount2_tilt +mount2_roll +mount2_open +DifferentialSpoilerLeft1 +DifferentialSpoilerRight1 +DifferentialSpoilerLeft2 +DifferentialSpoilerRight2 +Elevator +Rudder +FlaperonLeft +FlaperonRight +GroundSteering +Parachute +EPM +LandingGear +EngineRunEnable +HeliRSC +HeliTailRSC +Motor1 +Motor2 +Motor3 +Motor4 +Motor5 +Motor6 +Motor7 +Motor8 +MotorTilt +RCIN1 +RCIN2 +RCIN3 +RCIN4 +RCIN5 +RCIN6 +RCIN7 +RCIN8 +RCIN9 +RCIN10 +RCIN11 +RCIN12 +RCIN13 +RCIN14 +RCIN15 +RCIN16 +Ignition +Choke +Starter +Throttle +TrackerYaw +TrackerPitch +ThrottleLeft +ThrottleRight +tiltMotorLeft +tiltMotorRight +ElevonLeft +ElevonRight +VTailLeft +VTailRight +BoostThrottle +Motor9 +Motor10 +Motor11 +Motor12 +Winch + + + + + +500 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + + +Disabled +RCPassThru +Flap +Flap_auto +Aileron +mount_pan +mount_tilt +mount_roll +mount_open +camera_trigger +release +mount2_pan +mount2_tilt +mount2_roll +mount2_open +DifferentialSpoilerLeft1 +DifferentialSpoilerRight1 +DifferentialSpoilerLeft2 +DifferentialSpoilerRight2 +Elevator +Rudder +FlaperonLeft +FlaperonRight +GroundSteering +Parachute +EPM +LandingGear +EngineRunEnable +HeliRSC +HeliTailRSC +Motor1 +Motor2 +Motor3 +Motor4 +Motor5 +Motor6 +Motor7 +Motor8 +MotorTilt +RCIN1 +RCIN2 +RCIN3 +RCIN4 +RCIN5 +RCIN6 +RCIN7 +RCIN8 +RCIN9 +RCIN10 +RCIN11 +RCIN12 +RCIN13 +RCIN14 +RCIN15 +RCIN16 +Ignition +Choke +Starter +Throttle +TrackerYaw +TrackerPitch +ThrottleLeft +ThrottleRight +tiltMotorLeft +tiltMotorRight +ElevonLeft +ElevonRight +VTailLeft +VTailRight +BoostThrottle +Motor9 +Motor10 +Motor11 +Motor12 +Winch + + + + + +500 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + + +Disabled +RCPassThru +Flap +Flap_auto +Aileron +mount_pan +mount_tilt +mount_roll +mount_open +camera_trigger +release +mount2_pan +mount2_tilt +mount2_roll +mount2_open +DifferentialSpoilerLeft1 +DifferentialSpoilerRight1 +DifferentialSpoilerLeft2 +DifferentialSpoilerRight2 +Elevator +Rudder +FlaperonLeft +FlaperonRight +GroundSteering +Parachute +EPM +LandingGear +EngineRunEnable +HeliRSC +HeliTailRSC +Motor1 +Motor2 +Motor3 +Motor4 +Motor5 +Motor6 +Motor7 +Motor8 +MotorTilt +RCIN1 +RCIN2 +RCIN3 +RCIN4 +RCIN5 +RCIN6 +RCIN7 +RCIN8 +RCIN9 +RCIN10 +RCIN11 +RCIN12 +RCIN13 +RCIN14 +RCIN15 +RCIN16 +Ignition +Choke +Starter +Throttle +TrackerYaw +TrackerPitch +ThrottleLeft +ThrottleRight +tiltMotorLeft +tiltMotorRight +ElevonLeft +ElevonRight +VTailLeft +VTailRight +BoostThrottle +Motor9 +Motor10 +Motor11 +Motor12 +Winch + + + + + +500 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + + +Disabled +RCPassThru +Flap +Flap_auto +Aileron +mount_pan +mount_tilt +mount_roll +mount_open +camera_trigger +release +mount2_pan +mount2_tilt +mount2_roll +mount2_open +DifferentialSpoilerLeft1 +DifferentialSpoilerRight1 +DifferentialSpoilerLeft2 +DifferentialSpoilerRight2 +Elevator +Rudder +FlaperonLeft +FlaperonRight +GroundSteering +Parachute +EPM +LandingGear +EngineRunEnable +HeliRSC +HeliTailRSC +Motor1 +Motor2 +Motor3 +Motor4 +Motor5 +Motor6 +Motor7 +Motor8 +MotorTilt +RCIN1 +RCIN2 +RCIN3 +RCIN4 +RCIN5 +RCIN6 +RCIN7 +RCIN8 +RCIN9 +RCIN10 +RCIN11 +RCIN12 +RCIN13 +RCIN14 +RCIN15 +RCIN16 +Ignition +Choke +Starter +Throttle +TrackerYaw +TrackerPitch +ThrottleLeft +ThrottleRight +tiltMotorLeft +tiltMotorRight +ElevonLeft +ElevonRight +VTailLeft +VTailRight +BoostThrottle +Motor9 +Motor10 +Motor11 +Motor12 +Winch + + + + + +500 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + +800 2200 +1 +PWM +PWM in microseconds + + + +Normal +Reversed + + + + +Disabled +RCPassThru +Flap +Flap_auto +Aileron +mount_pan +mount_tilt +mount_roll +mount_open +camera_trigger +release +mount2_pan +mount2_tilt +mount2_roll +mount2_open +DifferentialSpoilerLeft1 +DifferentialSpoilerRight1 +DifferentialSpoilerLeft2 +DifferentialSpoilerRight2 +Elevator +Rudder +FlaperonLeft +FlaperonRight +GroundSteering +Parachute +EPM +LandingGear +EngineRunEnable +HeliRSC +HeliTailRSC +Motor1 +Motor2 +Motor3 +Motor4 +Motor5 +Motor6 +Motor7 +Motor8 +MotorTilt +RCIN1 +RCIN2 +RCIN3 +RCIN4 +RCIN5 +RCIN6 +RCIN7 +RCIN8 +RCIN9 +RCIN10 +RCIN11 +RCIN12 +RCIN13 +RCIN14 +RCIN15 +RCIN16 +Ignition +Choke +Starter +Throttle +TrackerYaw +TrackerPitch +ThrottleLeft +ThrottleRight +tiltMotorLeft +tiltMotorRight +ElevonLeft +ElevonRight +VTailLeft +VTailRight +BoostThrottle +Motor9 +Motor10 +Motor11 +Motor12 +Winch + + + + + +0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16 + + + +Disabled +Enabled + + + + +Disabled +TestMotor1 +TestMotor2 +TestMotor3 +TestMotor4 +TestMotor5 +TestMotor6 +TestMotor7 +TestMotor8 + + + +0 300 +s +seconds + + +0 500 +Hz +hertz + + + +Disabled +Enabled + + + + +None +OneShot +OneShot125 +Brushed +DShot150 +DShot300 +DShot600 +DShot1200 + + + + + +25 250 +Hz +hertz + + + + +0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16 + + + + + +Disabled +Enabled + + + +0 100 +% +percent + + +1000 2000 +ms +milliseconds + + +0 1000 +cm/s +centimeters per second + + +0 100 +% +percent + + + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 50 +1 +Hz +hertz + + + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 50 +1 +Hz +hertz + + + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 50 +1 +Hz +hertz + + + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 10 +1 +Hz +hertz + + +0 50 +1 +Hz +hertz + + + + +0 10 +m +meters + + +0 500 +True + + + + +True + + +True +s +seconds + + +True +s +seconds + + +True +s +seconds + + + + + +Disabled +Enabled +EnableAndLearn + + + +True +True +degC +degrees Celsius + + +True +True +degC +degrees Celsius + + +True +True +degC +degrees Celsius + + +True +True + + + + + +Disable +Enable + + + +1 +m +meters + + + + + +Disabled +EnableVersion1 +EnableVersion2 + + + + +Stabilize +Acro +AltHold +Auto +Guided +Loiter +RTL +Circle +Land +Drift +Sport +Flip +AutoTune +PosHold +Brake +Throw +Avoid_ADSB +Guided_NoGPS +FlowHold + + + + +Stabilize +Acro +AltHold +Auto +Guided +Loiter +RTL +Circle +Land +Drift +Sport +Flip +AutoTune +PosHold +Brake +Throw +Avoid_ADSB +Guided_NoGPS +FlowHold + + + + +None +TakePhoto +ToggleVideo +ModeAcro +ModeAltHold +ModeAuto +ModeLoiter +ModeRTL +ModeCircle +ModeLand +ModeDrift +ModeSport +ModeAutoTune +ModePosHold +ModeBrake +ModeThrow +Flip +ModeStabilize +Disarm +ToggleMode +Arm-Land-RTL +ToggleSimpleMode +ToggleSuperSimpleMode +MotorLoadTest +ModeFlowHold + + + + +None +TakePhoto +ToggleVideo +ModeAcro +ModeAltHold +ModeAuto +ModeLoiter +ModeRTL +ModeCircle +ModeLand +ModeDrift +ModeSport +ModeAutoTune +ModePosHold +ModeBrake +ModeThrow +Flip +ModeStabilize +Disarm +ToggleMode +Arm-Land-RTL +ToggleSimpleMode +ToggleSuperSimpleMode +MotorLoadTest +ModeFlowHold + + + + +None +TakePhoto +ToggleVideo +ModeAcro +ModeAltHold +ModeAuto +ModeLoiter +ModeRTL +ModeCircle +ModeLand +ModeDrift +ModeSport +ModeAutoTune +ModePosHold +ModeBrake +ModeThrow +Flip +ModeStabilize +Disarm +ToggleMode +Arm-Land-RTL +ToggleSimpleMode +ToggleSuperSimpleMode +MotorLoadTest +ModeFlowHold + + + + +None +TakePhoto +ToggleVideo +ModeAcro +ModeAltHold +ModeAuto +ModeLoiter +ModeRTL +ModeCircle +ModeLand +ModeDrift +ModeSport +ModeAutoTune +ModePosHold +ModeBrake +ModeThrow +Flip +ModeStabilize +Disarm +ToggleMode +Arm-Land-RTL +ToggleSimpleMode +ToggleSuperSimpleMode +MotorLoadTest +ModeFlowHold + + + + +None +TakePhoto +ToggleVideo +ModeAcro +ModeAltHold +ModeAuto +ModeLoiter +ModeRTL +ModeCircle +ModeLand +ModeDrift +ModeSport +ModeAutoTune +ModePosHold +ModeBrake +ModeThrow +Flip +ModeStabilize +Disarm +ToggleMode +Arm-Land-RTL +ToggleSimpleMode +ToggleSuperSimpleMode +MotorLoadTest +ModeFlowHold + + + + +None +TakePhoto +ToggleVideo +ModeAcro +ModeAltHold +ModeAuto +ModeLoiter +ModeRTL +ModeCircle +ModeLand +ModeDrift +ModeSport +ModeAutoTune +ModePosHold +ModeBrake +ModeThrow +Flip +ModeStabilize +Disarm +ToggleMode +Arm-Land-RTL +ToggleSimpleMode +ToggleSuperSimpleMode +MotorLoadTest +ModeFlowHold + + + + +None +TakePhoto +ToggleVideo +ModeAcro +ModeAltHold +ModeAuto +ModeLoiter +ModeRTL +ModeCircle +ModeLand +ModeDrift +ModeSport +ModeAutoTune +ModePosHold +ModeBrake +ModeThrow +Flip +ModeStabilize +Disarm +ToggleMode +Arm-Land-RTL +ToggleSimpleMode +ToggleSuperSimpleMode +MotorLoadTest +ModeFlowHold + + + + +None +TakePhoto +ToggleVideo +ModeAcro +ModeAltHold +ModeAuto +ModeLoiter +ModeRTL +ModeCircle +ModeLand +ModeDrift +ModeSport +ModeAutoTune +ModePosHold +ModeBrake +ModeThrow +Flip +ModeStabilize +Disarm +ToggleMode +Arm-Land-RTL +ToggleSimpleMode +ToggleSuperSimpleMode +MotorLoadTest +ModeFlowHold + + + +0 100 + + + +None +TakePhoto +ToggleVideo +ModeAcro +ModeAltHold +ModeAuto +ModeLoiter +ModeRTL +ModeCircle +ModeLand +ModeDrift +ModeSport +ModeAutoTune +ModePosHold +ModeBrake +ModeThrow +Flip +ModeStabilize +Disarm +ToggleMode +Arm-Land-RTL +ToggleSimpleMode +ToggleSuperSimpleMode +MotorLoadTest + + + +0:DisarmOnLowThrottle,1:ArmOnHighThrottle,2:UpgradeToLoiter,3:RTLStickCancel + + +0 5 +0.01 + + +0 5 +0.01 + + +0 1 +0.01 + + +0 1 +0.01 + + +0 1 +0.01 + + +0 100 + + + +ConstantThrust +LogReplay1 +LogReplay2 + + + + + + +None +MAV + + + +m +meters + + +m +meters + + +m +meters + + + +Forward +Right +Back +Left +Up +Down + + + + + + +None +Quadrature + + + +1 + + +0.001 + + +0.01 +m +meters + + +0.01 +m +meters + + +0.01 +m +meters + + + +Disabled +PixhawkAUX1 +PixhawkAUX2 +PixhawkAUX3 +PixhawkAUX4 +PixhawkAUX5 +PixhawkAUX6 + + + + +Disabled +PixhawkAUX1 +PixhawkAUX2 +PixhawkAUX3 +PixhawkAUX4 +PixhawkAUX5 +PixhawkAUX6 + + + + +None +Quadrature + + + +1 + + +0.001 + + +0.01 +m +meters + + +0.01 +m +meters + + +0.01 +m +meters + + + +Disabled +PixhawkAUX1 +PixhawkAUX2 +PixhawkAUX3 +PixhawkAUX4 +PixhawkAUX5 +PixhawkAUX6 + + + + +Disabled +PixhawkAUX1 +PixhawkAUX2 +PixhawkAUX3 +PixhawkAUX4 +PixhawkAUX5 +PixhawkAUX6 + + + + + + +Disabled +Enabled + + + + +Servo with encoder + + + +0 10 +m/s +meters per second + + +0.01 10.0 + + +0.100 2.000 + + +0.000 2.000 + + +0.000 1.000 + + +0.000 0.400 + + +1.000 100.000 +Hz +hertz + + + + +20 2000 +50 +cm/s +centimeters per second + + +10 1000 +1 +cm +centimeters + + +10 1000 +50 +cm/s +centimeters per second + + +10 500 +10 +cm/s +centimeters per second + + +50 500 +10 +cm/s/s +centimeters per square second + + +50 500 +10 +cm/s/s +centimeters per square second + + + +Disable +Enable + + + + diff --git a/src/FirmwarePlugin/APM/APMParameterFactMetaData.Sub.3.5.xml b/src/FirmwarePlugin/APM/APMParameterFactMetaData.Sub.3.5.xml index 81d8663b3b9d94882a0499cda92795bc4207c1c4..8756cfa495a9cd220717195f588c80f3b0ab4115 100644 --- a/src/FirmwarePlugin/APM/APMParameterFactMetaData.Sub.3.5.xml +++ b/src/FirmwarePlugin/APM/APMParameterFactMetaData.Sub.3.5.xml @@ -205,18 +205,13 @@ 1 10 - -30 400 - - -30 400 + +1 10 +PWM 0.5 4.0 - -1000 2000 - BlueROV1 @@ -235,6 +230,11 @@ 1 Hz + +0 500 +10 +%/s + 1 10 @@ -416,15 +416,15 @@ - + Disabled THR_MIN PWM when disarmed 0 PWM when disarmed - -0:All,1:Barometer,2:Compass,3:GPS lock,4:INS,5:Parameters,6:RC,7:Board voltage,8:Battery Level,9:Airspeed,10:Logging Available,11:Hardware safety switch,12:GPS Configuration + +0:All,1:Barometer,2:Compass,3:GPS lock,4:INS,5:Parameters,6:RC Channels,7:Board voltage,8:Battery Level,10:Logging Available,11:Hardware safety switch,12:GPS Configuration None All @@ -433,10 +433,9 @@ GPS Lock INS(INertial Sensors - accels & gyros) Parameters(unused) -RC Failsafe +RC Channels Board voltage Battery Level -Airspeed LoggingAvailable Hardware safety switch GPS configuration @@ -446,11 +445,11 @@ 0.25 3.0 m/s/s - + 0.1 Volts - + 0.1 Volts @@ -507,23 +506,23 @@ -3.000 12.000 +0.0 12.000 -3.000 12.000 +0.0 12.000 -3.000 6.000 +0.0 6.000 0.5 10.0 -0.08 0.30 +0.0 0.30 0.005 -0.01 0.5 +0.0 0.5 0.01 @@ -545,11 +544,11 @@ Hz -0.08 0.30 +0.0 0.30 0.005 -0.01 0.5 +0.0 0.5 0.01 @@ -571,11 +570,11 @@ Hz -0.10 0.50 +0.0 0.50 0.005 -0.010 0.05 +0.0 0.05 0.01 @@ -707,7 +706,7 @@ PX4-v1 - + Amps/Volt @@ -888,6 +887,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -903,12 +905,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -954,6 +975,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -969,12 +993,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -1022,6 +1065,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -1037,12 +1083,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -1088,6 +1153,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -1103,12 +1171,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -1156,6 +1243,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -1171,12 +1261,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -1222,6 +1331,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -1237,12 +1349,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -1290,6 +1421,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -1305,12 +1439,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -1356,6 +1509,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -1371,12 +1527,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -1424,6 +1599,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -1439,12 +1617,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -1490,6 +1687,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -1505,12 +1705,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -1558,6 +1777,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -1573,12 +1795,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -1624,6 +1865,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -1639,12 +1883,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -1692,6 +1955,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -1707,12 +1973,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -1758,6 +2043,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -1773,12 +2061,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -1826,6 +2133,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -1841,12 +2151,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -1892,6 +2221,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -1907,12 +2239,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -1960,6 +2311,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -1975,12 +2329,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -2026,6 +2399,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -2041,12 +2417,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -2094,6 +2489,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -2109,12 +2507,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -2160,6 +2577,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -2175,12 +2595,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -2228,6 +2667,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -2243,12 +2685,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -2294,6 +2755,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -2309,12 +2773,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -2362,6 +2845,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -2377,12 +2863,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -2428,6 +2933,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -2443,12 +2951,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -2496,6 +3023,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -2511,12 +3041,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -2562,6 +3111,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -2577,12 +3129,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -2630,6 +3201,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -2645,12 +3219,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -2696,6 +3289,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -2711,12 +3307,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -2764,6 +3379,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -2779,12 +3397,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -2830,6 +3467,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -2845,12 +3485,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -2898,6 +3557,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -2913,12 +3575,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary @@ -2964,6 +3645,9 @@ relay_2_on relay_2_off relay_2_toggle +relay_3_on +relay_3_off +relay_3_toggle servo_1_inc servo_1_dec servo_1_min @@ -2979,12 +3663,31 @@ servo_3_min servo_3_max servo_3_center +servo_1_min_momentary +servo_1_max_momentary +servo_1_min_toggle +servo_1_max_toggle +servo_2_min_momentary +servo_2_max_momentary +servo_2_min_toggle +servo_2_max_toggle +servo_3_min_momentary +servo_3_max_momentary +servo_3_min_toggle +servo_3_max_toggle custom_1 custom_2 custom_3 custom_4 custom_5 custom_6 +relay_4_on +relay_4_off +relay_4_toggle +relay_1_momentary +relay_2_momentary +relay_3_momentary +relay_4_momentary diff --git a/src/FirmwarePlugin/APM/APMParameterMetaData.cc b/src/FirmwarePlugin/APM/APMParameterMetaData.cc index 2760464324a7ea528ca20a9e2e31818b98dc8868..d297313e7c4cec9d3d23eba6ff337361597f4458 100644 --- a/src/FirmwarePlugin/APM/APMParameterMetaData.cc +++ b/src/FirmwarePlugin/APM/APMParameterMetaData.cc @@ -584,7 +584,7 @@ void APMParameterMetaData::addMetaDataToFact(Fact* fact, MAV_TYPE vehicleType) bool ok; increment = rawMetaData->incrementSize.toDouble(&ok); if (ok) { - metaData->setIncrement(increment); + metaData->setRawIncrement(increment); } else { qCDebug(APMParameterMetaDataLog) << "Invalid value for increment, name:" << metaData->name() << " increment:" << rawMetaData->incrementSize; } diff --git a/src/FirmwarePlugin/APM/APMResources.qrc b/src/FirmwarePlugin/APM/APMResources.qrc index c99292bd406cb2058a5b31bdd2a8b65a9659c0a2..df6ffa966370b0359dd61078c07874fe78a69a16 100644 --- a/src/FirmwarePlugin/APM/APMResources.qrc +++ b/src/FirmwarePlugin/APM/APMResources.qrc @@ -6,6 +6,7 @@ ../../AutoPilotPlugins/APM/APMCameraComponentSummary.qml ../../AutoPilotPlugins/APM/APMFlightModesComponent.qml ../../AutoPilotPlugins/APM/APMFlightModesComponentSummary.qml + ../../AutoPilotPlugins/APM/APMHeliComponent.qml ../../AutoPilotPlugins/APM/APMLightsComponent.qml ../../AutoPilotPlugins/APM/APMLightsComponentSummary.qml ../../AutoPilotPlugins/APM/APMSubFrameComponent.qml @@ -47,6 +48,7 @@ APMParameterFactMetaData.Copter.3.3.xml APMParameterFactMetaData.Copter.3.4.xml APMParameterFactMetaData.Copter.3.5.xml + APMParameterFactMetaData.Copter.3.6.xml APMParameterFactMetaData.Rover.3.0.xml APMParameterFactMetaData.Rover.3.2.xml APMParameterFactMetaData.Sub.3.4.xml diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index 3df92033b4f289b59f34ecd6b74108ba5513c845..9a20e4400612e0fbaf4fda939a15e376760da8f3 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -148,6 +148,16 @@ ArduCopterFirmwarePlugin::ArduCopterFirmwarePlugin(void) remapV3_5["ARMING_VOLT_MIN"] = QStringLiteral("ARMING_MIN_VOLT"); remapV3_5["ARMING_VOLT2_MIN"] = QStringLiteral("ARMING_MIN_VOLT2"); + FirmwarePlugin::remapParamNameMap_t& remapV3_6 = _remapParamName[3][6]; + + remapV3_6["BATT_AMP_PERVLT"] = QStringLiteral("BATT_AMP_PERVOL"); + remapV3_6["BATT2_AMP_PERVLT"] = QStringLiteral("BATT2_AMP_PERVOL"); + remapV3_6["BATT_LOW_MAH"] = QStringLiteral("FS_BATT_MAH"); + remapV3_6["BATT_LOW_VOLT"] = QStringLiteral("FS_BATT_VOLTAGE"); + remapV3_6["BATT_FS_LOW_ACT"] = QStringLiteral("FS_BATT_ENABLE"); + remapV3_6["PSC_ACCZ_P"] = QStringLiteral("ACCEL_Z_P"); + remapV3_6["PSC_ACCZ_I"] = QStringLiteral("ACCEL_Z_I"); + _remapParamNameIntialized = true; } } diff --git a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc index 834ce5977d89c7084849ee37744891e15332489f..60b5e9d87b3bbd6748cf979a31e9fbd1f0a9562b 100644 --- a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc @@ -164,6 +164,17 @@ int ArduSubFirmwarePlugin::manualControlReservedButtonCount(void) return 0; } +void ArduSubFirmwarePlugin::initializeStreamRates(Vehicle* vehicle) { + vehicle->requestDataStream(MAV_DATA_STREAM_RAW_SENSORS, 2); + vehicle->requestDataStream(MAV_DATA_STREAM_EXTENDED_STATUS, 2); + vehicle->requestDataStream(MAV_DATA_STREAM_RC_CHANNELS, 2); + vehicle->requestDataStream(MAV_DATA_STREAM_POSITION, 3); + vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA1, 20); + vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA2, 10); + vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA3, 3); +} + + bool ArduSubFirmwarePlugin::supportsThrottleModeCenterZero(void) { return false; diff --git a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h index c03062b99aea085c6d232df29b779b5cf3552b67..577bd5284183d538fca77a6961f58605fa805448 100644 --- a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h @@ -108,13 +108,15 @@ class ArduSubFirmwarePlugin : public APMFirmwarePlugin public: ArduSubFirmwarePlugin(void); - QList supportedMissionCommands(void); + QList supportedMissionCommands(void) final; // Overrides from FirmwarePlugin int manualControlReservedButtonCount(void) final; int defaultJoystickTXMode(void) final { return 3; } + void initializeStreamRates(Vehicle* vehicle) override final; + bool supportsThrottleModeCenterZero(void) final; bool supportsRadio(void) final; diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 96efc3a1015fcf9d127f69aaca137f60e84daef9..344748ac96bf8aac2659006ad708564f3f43d3ed 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -150,6 +150,12 @@ bool FirmwarePlugin::supportsJSButton(void) return false; } +bool FirmwarePlugin::supportsTerrainFrame(void) const +{ + // Generic firmware supports this since we don't know + return true; +} + bool FirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message) { Q_UNUSED(vehicle); @@ -338,6 +344,7 @@ const QVariantList &FirmwarePlugin::toolBarIndicators(const Vehicle* vehicle) _toolBarIndicatorList.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/VTOLModeIndicator.qml"))); _toolBarIndicatorList.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/ArmedIndicator.qml"))); _toolBarIndicatorList.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/GPSRTKIndicator.qml"))); + _toolBarIndicatorList.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/LinkIndicator.qml"))); } return _toolBarIndicatorList; } diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 7d95f4053584ab325563590104caeb1a32668702..c161e096c5b3d685143d7a23b3a04f9ff2a4d646 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -43,11 +43,10 @@ public: /// Set of optional capabilites which firmware may support typedef enum { SetFlightModeCapability = 1 << 0, ///< FirmwarePlugin::setFlightMode method is supported - MavCmdPreflightStorageCapability = 1 << 1, ///< MAV_CMD_PREFLIGHT_STORAGE is supported - PauseVehicleCapability = 1 << 2, ///< Vehicle supports pausing at current location - GuidedModeCapability = 1 << 3, ///< Vehicle supports guided mode commands - OrbitModeCapability = 1 << 4, ///< Vehicle supports orbit mode - TakeoffVehicleCapability = 1 << 5, ///< Vehicle supports guided takeoff + PauseVehicleCapability = 1 << 1, ///< Vehicle supports pausing at current location + GuidedModeCapability = 1 << 2, ///< Vehicle supports guided mode commands + OrbitModeCapability = 1 << 3, ///< Vehicle supports orbit mode + TakeoffVehicleCapability = 1 << 4, ///< Vehicle supports guided takeoff } FirmwareCapabilities; /// Maps from on parameter name to another @@ -80,7 +79,8 @@ public: /// free when no longer needed. virtual QList componentsForVehicle(AutoPilotPlugin* vehicle); - /// Returns the list of available flight modes + /// Returns the list of available flight modes. Flight modes can be different in normal/advanced ui mode. + /// Call will be made again if advanced mode changes. virtual QStringList flightModes(Vehicle* vehicle) { Q_UNUSED(vehicle); return QStringList(); @@ -180,6 +180,9 @@ public: /// (CompassMot). Default is true. virtual bool supportsMotorInterference(void); + /// Returns true if the firmware supports MAV_FRAME_GLOBAL_TERRAIN_ALT + virtual bool supportsTerrainFrame(void) const; + /// Called before any mavlink message is processed by Vehicle such that the firmwre plugin /// can adjust any message characteristics. This is handy to adjust or differences in mavlink /// spec implementations such that the base code can remain mavlink generic. diff --git a/src/FirmwarePlugin/PX4/MavCmdInfoCommon.json b/src/FirmwarePlugin/PX4/MavCmdInfoCommon.json index f50fe22108663f9d810aa67947d470531eb34589..8657261895e728cd9fdf5aa479ab99e3d4d1e8c7 100644 --- a/src/FirmwarePlugin/PX4/MavCmdInfoCommon.json +++ b/src/FirmwarePlugin/PX4/MavCmdInfoCommon.json @@ -18,6 +18,11 @@ "id": 201, "comment": "MAV_CMD_DO_SET_ROI", "paramRemove": "1,2,3" + }, + { + "id": 2500, + "comment": "MAV_CMD_VIDEO_START_CAPTURE", + "paramRemove": "2" } ] } diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 8ea457b678810bf189621c5f3364dde3ddef3281..100b3f26dd9ef764a007073354a5e1489320c6f7 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -48,6 +48,7 @@ PX4FirmwarePlugin::PX4FirmwarePlugin(void) , _missionFlightMode(tr("Mission")) , _rtlFlightMode(tr("Return")) , _landingFlightMode(tr("Land")) + , _preclandFlightMode(tr("Precision Land")) , _rtgsFlightMode(tr("Return to Groundstation")) , _followMeFlightMode(tr("Follow Me")) , _simpleFlightMode(tr("Simple")) @@ -84,6 +85,7 @@ PX4FirmwarePlugin::PX4FirmwarePlugin(void) { PX4_CUSTOM_MAIN_MODE_OFFBOARD, 0, true, false, true }, // modes that can't be directly set by the user { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_LAND, false, true, true }, + { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND, false, false, true }, { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_READY, false, true, true }, { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_RTGS, false, true, true }, { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF, false, true, true }, @@ -104,6 +106,7 @@ PX4FirmwarePlugin::PX4FirmwarePlugin(void) &_followMeFlightMode, &_offboardFlightMode, &_landingFlightMode, + &_preclandFlightMode, &_readyFlightMode, &_rtgsFlightMode, &_takeoffFlightMode, @@ -227,7 +230,7 @@ int PX4FirmwarePlugin::manualControlReservedButtonCount(void) bool PX4FirmwarePlugin::isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities) { - int available = MavCmdPreflightStorageCapability | SetFlightModeCapability | PauseVehicleCapability | GuidedModeCapability; + int available = SetFlightModeCapability | PauseVehicleCapability | GuidedModeCapability; if (vehicle->multiRotor() || vehicle->vtol()) { available |= TakeoffVehicleCapability; } diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index 6497793e9295da58596cfaca371180d69f99c23c..ddc69af0df0ef9292f603fcf143cfd51c895acfd 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -71,6 +71,7 @@ public: QGCCameraManager* createCameraManager (Vehicle* vehicle) override; QGCCameraControl* createCameraControl (const mavlink_camera_information_t* info, Vehicle* vehicle, int compID, QObject* parent = NULL) override; uint32_t highLatencyCustomModeTo32Bits (uint16_t hlCustomMode) override; + bool supportsTerrainFrame (void) const override { return false; } protected: typedef struct { @@ -101,6 +102,7 @@ protected: QString _missionFlightMode; QString _rtlFlightMode; QString _landingFlightMode; + QString _preclandFlightMode; QString _rtgsFlightMode; QString _followMeFlightMode; QString _simpleFlightMode; diff --git a/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml b/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml index 397f1e36216865dc37fe7356654d082c5e93626f..cfe7ec3508e5a8bebd37dc3e758ffd38b13d7f05 100644 --- a/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml +++ b/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml @@ -199,6 +199,7 @@ Set to 2 to use heading from motion capture Complimentary filter magnetometer weight + Set to 0 to avoid using the magnetometer. 0 1 2 @@ -221,7 +222,7 @@ Set to 2 to use heading from motion capture 0 50 true - modules/systemlib + lib/battery Scaling from ADC counts to volt on the ADC input (battery current) @@ -244,7 +245,7 @@ Set to 2 to use heading from motion capture 2 0.01 true - modules/systemlib + lib/battery Emergency threshold @@ -255,7 +256,7 @@ Set to 2 to use heading from motion capture 2 0.01 true - modules/systemlib + lib/battery Low threshold @@ -266,14 +267,14 @@ Set to 2 to use heading from motion capture 2 0.01 true - modules/systemlib + lib/battery Number of cells Defines the number of cells the attached battery consists of. S true - modules/systemlib + lib/battery Unconfigured 2S Battery @@ -300,7 +301,7 @@ Set to 2 to use heading from motion capture 0.2 Ohms true - modules/systemlib + lib/battery Battery monitoring source @@ -320,7 +321,7 @@ Set to 2 to use heading from motion capture 2 0.01 true - modules/systemlib + lib/battery Battery voltage divider (V divider) @@ -335,7 +336,7 @@ Set to 2 to use heading from motion capture 2 0.01 true - modules/systemlib + lib/battery Voltage drop per cell on full throttle @@ -346,7 +347,7 @@ Set to 2 to use heading from motion capture 2 0.01 true - modules/systemlib + lib/battery Offset in volt as seen by the ADC input of the current sensor @@ -527,7 +528,7 @@ Set to 2 to use heading from motion capture - Arm authorization parameters, this uint32_t will be splitted between starting from the LSB: + Arm authorization parameters, this uint32_t will be split between starting from the LSB: - 8bits to authorizer system id - 16bits to authentication method parameter, this will be used to store a timeout for the first 2 methods but can be used to another parameter for other new authentication methods. - 7bits to authentication method @@ -834,6 +835,22 @@ Note: ekf2 will limit the delta velocity bias estimate magnitude to be less than Follow Me + + High Latency Datalink loss time threshold + After this amount of seconds without datalink the data link lost mode triggers + 60 + 3600 + s + modules/commander + + + High Latency Datalink regain time threshold + After a data link loss: after this this amount of seconds with a healthy datalink the 'datalink loss' flag is set back to false + 0 + 60 + s + modules/commander + Home set horizontal threshold The home position will be set if the estimated positioning accuracy is below the threshold. @@ -862,9 +879,9 @@ Note: ekf2 will limit the delta velocity bias estimate magnitude to be less than modules/commander Warning - Return to land - Land at current position - Return to land at critically low level, land at current position if reaching dangerously low levels + Return mode + Land mode + Return mode at critically low level, Land mode at current position if reaching dangerously low levels @@ -879,10 +896,24 @@ See COM_OBL_ACT and COM_OBL_RC_ACT to configure action Loss of position failsafe activation delay This sets number of seconds that the position checks need to be failed before the failsafe will activate. The default value has been optimised for rotary wing applications. For fixed wing applications, a larger value between 5 and 10 should be used. + 1 + 100 sec true modules/commander + + Horizontal position error threshold + This is the horizontal position error (EPV) threshold that will trigger a failsafe. The default is appropriate for a multicopter. Can be increased for a fixed-wing. + m + modules/commander + + + Vertical position error threshold + This is the vertical position error (EPV) threshold that will trigger a failsafe. The default is appropriate for a multicopter. Can be increased for a fixed-wing. + m + modules/commander + Loss of position probation gain factor This sets the rate that the loss of position probation time grows when position checks are failing. The default value has been optimised for rotary wing applications. For fixed wing applications a value of 0 should be used. @@ -892,6 +923,8 @@ See COM_OBL_ACT and COM_OBL_RC_ACT to configure action Loss of position probation delay at takeoff The probation delay is the number of seconds that the EKF innovation checks need to pass for the position to be declared good after it has been declared bad. The probation delay will be reset to this parameter value when takeoff is detected. After takeoff, if position checks are passing, the probation delay will reduce by one second for every lapsed second of valid position down to a minimum of 1 second. If position checks are failing, the probation delay will increase by COM_POS_FS_GAIN seconds for every lapsed second up to a maximum of 100 seconds. The default value has been optimised for rotary wing applications. For fixed wing applications, a value of 1 should be used. + 1 + 100 sec true modules/commander @@ -940,6 +973,12 @@ See COM_OBL_ACT and COM_OBL_RC_ACT to configure action 0.05 modules/commander + + Horizontal velocity error threshold + This is the horizontal velocity error (EVH) threshold that will trigger a failsafe. The default is appropriate for a multicopter. Can be increased for a fixed-wing. + m + modules/commander + @@ -1597,6 +1636,13 @@ Baro and Magnetometer data will be averaged before downsampling, other data will 1 modules/ekf2 + + Maximum lapsed time from last fusion of measurements that constrain velocity drift before the EKF will report the horizontal nav solution as invalid + 500000 + 10000000 + uSec + modules/ekf2 + Optical flow measurement delay relative to IMU measurements Assumes measurement is timestamped at trailing edge of integration period @@ -1925,6 +1971,78 @@ This is the ratio of static pressure error to dynamic pressure generated by a wi modules/fw_att_control + + Pitch trim increment for flaps configuration + This increment is added to the pitch trim whenever flaps are fully deployed. + -0.25 + 0.25 + 2 + 0.01 + modules/fw_att_control + + + Pitch trim increment at maximum airspeed + This increment is added to TRIM_PITCH when airspeed is FW_AIRSP_MAX. + -0.25 + 0.25 + 2 + 0.01 + modules/fw_att_control + + + Pitch trim increment at minimum airspeed + This increment is added to TRIM_PITCH when airspeed is FW_AIRSPD_MIN. + -0.25 + 0.25 + 2 + 0.01 + modules/fw_att_control + + + Roll trim increment for flaps configuration + This increment is added to TRIM_ROLL whenever flaps are fully deployed. + -0.25 + 0.25 + 2 + 0.01 + modules/fw_att_control + + + Roll trim increment at maximum airspeed + This increment is added to TRIM_ROLL when airspeed is FW_AIRSP_MAX. + -0.25 + 0.25 + 2 + 0.01 + modules/fw_att_control + + + Roll trim increment at minimum airspeed + This increment is added to TRIM_ROLL when airspeed is FW_AIRSPD_MIN. + -0.25 + 0.25 + 2 + 0.01 + modules/fw_att_control + + + Yaw trim increment at maximum airspeed + This increment is added to TRIM_YAW when airspeed is FW_AIRSP_MAX. + -0.25 + 0.25 + 2 + 0.01 + modules/fw_att_control + + + Yaw trim increment at minimum airspeed + This increment is added to TRIM_YAW when airspeed is FW_AIRSPD_MIN. + -0.25 + 0.25 + 2 + 0.01 + modules/fw_att_control + Scale factor for flaperons 0.0 @@ -2042,7 +2160,7 @@ This is the ratio of static pressure error to dynamic pressure generated by a wi Maximum negative / down pitch rate - This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. + This limits the maximum pitch down up angular rate the controller will output (in degrees per second). 0.0 90.0 deg/s @@ -2052,7 +2170,7 @@ This is the ratio of static pressure error to dynamic pressure generated by a wi Maximum positive / up pitch rate - This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. + This limits the maximum pitch up angular rate the controller will output (in degrees per second). 0.0 90.0 deg/s @@ -2138,7 +2256,7 @@ This is the ratio of static pressure error to dynamic pressure generated by a wi Maximum roll rate - This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit. + This limits the maximum roll rate the controller will output (in degrees per second). 0.0 90.0 deg/s @@ -2200,9 +2318,9 @@ This is the ratio of static pressure error to dynamic pressure generated by a wi modules/fw_att_control - + Maximum wheel steering rate - This limits the maximum wheel steering rate the controller will output (in degrees per second). Setting a value of zero disables the limit. + This limits the maximum wheel steering rate the controller will output (in degrees per second). 0.0 90.0 deg/s @@ -2220,7 +2338,7 @@ This is the ratio of static pressure error to dynamic pressure generated by a wi 0.05 modules/fw_att_control - + Yaw rate integrator gain This gain defines how much control response will result out of a steady state error. It trims any constant error. 0.0 @@ -2249,9 +2367,9 @@ This is the ratio of static pressure error to dynamic pressure generated by a wi 0.005 modules/fw_att_control - + Maximum yaw rate - This limits the maximum yaw rate the controller will output (in degrees per second). Setting a value of zero disables the limit. + This limits the maximum yaw rate the controller will output (in degrees per second). 0.0 90.0 deg/s @@ -2406,7 +2524,7 @@ Set to 0 to disable heading hold Scale throttle by pressure change Automatically adjust throttle to account for decreased air density at higher altitudes. Start with a scale factor of 1.0 and adjust for different propulsion systems. When flying without airspeed sensor this will help to keep a constant performance over large altitude ranges. The default value of 0 will disable scaling. 0.0 - 2.0 + 10.0 1 0.1 modules/fw_pos_control_l1 @@ -3054,9 +3172,9 @@ but also ignore less noise None Warning - Loiter - Return to Land - Flight terminate + Hold mode + Return mode + Terminate @@ -3109,13 +3227,28 @@ but also ignore less noise - - Satellite radio read interval + + Satellite radio read interval. Only required to be nonzero if data is not sent using a ring call + 0 + 5000 + s + drivers/telemetry/iridiumsbd + + + Iridium SBD session timeout 0 300 s drivers/telemetry/iridiumsbd + + Time [ms] the Iridium driver will wait for additional mavlink messages to combine them into one SBD message +Value 0 turns the functionality off + 0 + 500 + ms + drivers/telemetry/iridiumsbd + @@ -3751,9 +3884,9 @@ Used to calculate increased terrain random walk nosie due to movementThe offboard loss failsafe will only be entered after a timeout, set by COM_OF_LOSS_T in seconds. modules/commander - Land at current position - Loiter - Return to Land + Land mode + Hold mode + Return mode @@ -3761,21 +3894,21 @@ Used to calculate increased terrain random walk nosie due to movementThe offboard loss failsafe will only be entered after a timeout, set by COM_OF_LOSS_T in seconds. modules/commander - Position control - Altitude control + Position mode + Altitude mode Manual - Return to Land - Land at current position - Loiter + Return mode + Land mode + Hold mode Position control navigation loss response - This sets the flight mode that will be used if navigation accuracy is no longer adequte for position control. Navigation accuracy checks can be disabled using the CBRK_VELPOSERR parameter, but doing so will remove protection for all flight modes. + This sets the flight mode that will be used if navigation accuracy is no longer adequate for position control. Navigation accuracy checks can be disabled using the CBRK_VELPOSERR parameter, but doing so will remove protection for all flight modes. modules/commander - Assume use of remote control after fallback. Switch to ALTCTL if a height estimate is available, else switch to MANUAL. - Assume no use of remote control after fallback. Switch to DESCEND if a height estimate is available, else switch to TERMINATION. + Assume use of remote control after fallback. Switch to Altitude mode if a height estimate is available, else switch to MANUAL. + Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION. @@ -3897,9 +4030,9 @@ Used to calculate increased terrain random walk nosie due to movementmodules/navigator Disabled - Loiter - Return to Land - Land at current position + Hold mode + Return mode + Land mode Data Link Auto Recovery (CASA Outback Challenge rules) Terminate Lockdown @@ -3922,7 +4055,7 @@ Used to calculate increased terrain random walk nosie due to movement Loiter radius (FW only) - Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only). + Default value of loiter radius for missions, Hold mode, Return mode, etc. (fixedwing only). 25 1000 m @@ -3946,9 +4079,9 @@ Used to calculate increased terrain random walk nosie due to movementmodules/navigator Disabled - Loiter - Return to Land - Land at current position + Hold mode + Return mode + Land mode RC Auto Recovery (CASA Outback Challenge rules) Terminate Lockdown @@ -3970,8 +4103,8 @@ Used to calculate increased terrain random walk nosie due to movement Disabled Warn only - Return to Land - Land immediately + Return mode + Land mode @@ -4136,9 +4269,16 @@ if required by the gimbal (only in AUX output mode) - Acro Expo factor -applied to input of all axis: roll, pitch, yaw - 0 Purely linear input curve 1 Purely cubic input curve + Acro mode Expo factor for Roll and Pitch + Exponential factor for tuning the input curve shape. 0 Purely linear input curve 1 Purely cubic input curve + 0 + 1 + 2 + modules/mc_att_control + + + Acro mode Expo factor for Yaw + Exponential factor for tuning the input curve shape. 0 Purely linear input curve 1 Purely cubic input curve 0 1 2 @@ -4148,7 +4288,7 @@ applied to input of all axis: roll, pitch, yaw Max acro pitch rate default: 2 turns per second 0.0 - 1000.0 + 1800.0 deg/s 1 5 @@ -4158,16 +4298,23 @@ default: 2 turns per second Max acro roll rate default: 2 turns per second 0.0 - 1000.0 + 1800.0 deg/s 1 5 modules/mc_att_control - Acro SuperExpo factor -applied to input of all axis: roll, pitch, yaw - 0 Pure Expo function 0.7 resonable shape enhancement for intuitive stick feel 0.95 very strong bent input curve only near maxima have effect + Acro mode SuperExpo factor for Roll and Pitch + SuperExpo factor for refining the input curve shape tuned using MC_ACRO_EXPO. 0 Pure Expo function 0.7 resonable shape enhancement for intuitive stick feel 0.95 very strong bent input curve only near maxima have effect + 0 + 0.95 + 2 + modules/mc_att_control + + + Acro mode SuperExpo factor for Yaw + SuperExpo factor for refining the input curve shape tuned using MC_ACRO_EXPO_Y. 0 Pure Expo function 0.7 resonable shape enhancement for intuitive stick feel 0.95 very strong bent input curve only near maxima have effect 0 0.95 2 @@ -4177,19 +4324,25 @@ applied to input of all axis: roll, pitch, yaw Max acro yaw rate default 1.5 turns per second 0.0 - 1000.0 + 1800.0 deg/s 1 5 modules/mc_att_control + + Multicopter air-mode + The air-mode enables the mixer to increase the total thrust of the multirotor in order to keep attitude and rate control even at low and high throttle. This function should be disabled during tuning as it will help the controller to diverge if the closed-loop is unstable. + + modules/mc_att_control + Battery power level scaler This compensates for voltage drop of the battery over time by attempting to normalize performance across the operating range of the battery. The copter should constantly behave as if it was fully charged with reduced max acceleration at lower battery percentages. i.e. if hover is at 0.5 throttle at 100% battery, it will still be 0.5 at 60% battery. modules/mc_att_control - + Cutoff frequency for the low pass filter on the D-term in the rate controller The D-term uses the derivative of the rate and thus is the most susceptible to noise. Therefore, using a D-term filter allows to decrease the driver-level filtering, which leads to reduced control latency and permits to increase the P gains. A value of 0 disables the filter. 0 @@ -4251,16 +4404,6 @@ default 1.5 turns per second 0.1 modules/mc_att_control - - Pitch time constant - Reduce if the system is too twitchy, increase if the response is too slow and sluggish. - 0.15 - 0.25 - s - 2 - 0.01 - modules/mc_att_control - Pitch rate integrator limit Pitch rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large pitch moment trim changes. @@ -4331,16 +4474,6 @@ default 1.5 turns per second 0.1 modules/mc_att_control - - Roll time constant - Reduce if the system is too twitchy, increase if the response is too slow and sluggish. - 0.15 - 0.25 - s - 2 - 0.01 - modules/mc_att_control - Roll rate integrator limit Roll rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large roll moment trim changes. @@ -4561,6 +4694,17 @@ is 90 degrees. It should be lower than MPC_XY_CRUISE 1 modules/mc_pos_control + + Flag to test flight tasks instead of legacy functionality +Temporary Parameter during the transition to flight tasks + 0 + 1 + modules/mc_pos_control + + Legacy Functionality + Test flight tasks + + Deadzone of sticks where position hold is enabled 0.0 @@ -4632,9 +4776,9 @@ towards MPC_ACC_HOR_MAX/MPC_ACC_UP_MAX with jerk limit 1 modules/mc_pos_control - + Maximum manual thrust - Limit max allowed thrust. Setting a value of one can put the system into actuator saturation as no spread between the motors is possible any more. A value of 0.8 - 0.9 is recommended. + Limit max allowed thrust for Manual mode. 0.0 1.0 norm @@ -4644,7 +4788,7 @@ towards MPC_ACC_HOR_MAX/MPC_ACC_UP_MAX with jerk limit Minimum manual thrust - Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust. + Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust. With MC_AIRMODE set to 1, this can safely be set to 0. 0.0 1.0 norm @@ -4678,11 +4822,11 @@ towards MPC_ACC_HOR_MAX/MPC_ACC_UP_MAX with jerk limit 0.01 modules/mc_pos_control - + Maximum thrust in auto thrust control - Limit max allowed thrust. Setting a value of one can put the system into actuator saturation as no spread between the motors is possible any more. A value of 0.8 - 0.9 is recommended. + Limit max allowed thrust 0.0 - 0.95 + 1.0 norm 2 0.01 @@ -4863,6 +5007,17 @@ the setpoint will be capped to MPC_XY_VEL_MAX + + Motor Ordering + Determines the motor ordering. This can be used for example in combination with a 4-in-1 ESC that assumes a motor ordering which is different from PX4. ONLY supported for Quads. ONLY supported for fmu output (Pixracer or Omnibus F4). When changing this, make sure to test the motor response without props first. + 0 + 1 + drivers/px4fmu + + PX4 + Betaflight / Cleanflight + + Minimum motor rise time (slew rate limit) Minimum time allowed for the motor input signal to pass through a range of 1000 PWM units. A value x means that the motor signal can only go from 1000 to 2000 PWM in maximum x seconds. Zero means that slew rate limiting is disabled. @@ -7311,9 +7466,9 @@ the setpoint will be capped to MPC_XY_VEL_MAX modules/sensors - + - RTL loiter altitude + Return mode loiter altitude Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed. 2 100 @@ -7323,8 +7478,8 @@ the setpoint will be capped to MPC_XY_VEL_MAX modules/navigator - RTL delay - Delay after descend before landing in RTL mode. If set to -1 the system will not land but loiter at RTL_DESCEND_ALT. + Return mode delay + Delay after descend before landing in Return mode. If set to -1 the system will not land but loiter at RTL_DESCEND_ALT. -1 300 s @@ -7332,15 +7487,6 @@ the setpoint will be capped to MPC_XY_VEL_MAX 0.5 modules/navigator - - RTL land location - Land at the home location or planned mission landing - modules/navigator - - Home Position - Planned Landing (Mission) - - Minimum distance to trigger rising to a safe altitude If the system is horizontally closer than this distance to home it will land straight on home instead of raising to the return altitude first. @@ -7362,6 +7508,18 @@ the setpoint will be capped to MPC_XY_VEL_MAX modules/navigator + + + Return type + Fly straight to the home location or planned mission landing and land there or use the planned mission to get to those points. + modules/navigator + + Return home via direct path + Return to a planned mission landing, if available, via direct path, else return to home via direct path + Return to a planned mission landing, if available, using the mission path, else return to home via the reverse mission path + + + Min. airspeed scaling factor for takeoff. @@ -8113,7 +8271,7 @@ This is used for gathering replay logs for the ekf2 module true modules/sensors - + Driver level cutoff frequency for gyro The cutoff frequency for the 2nd order butterworth filter on the gyro driver. This features is currently supported by the mpu6000 and mpu9250. This only affects the signal sent to the controllers, not the estimators. 0 disables the filter. 0 @@ -8209,7 +8367,7 @@ This is used for gathering replay logs for the ekf2 module - Maxbotix Soanr (mb12xx) + Maxbotix Sonar (mb12xx) true drivers/distance_sensor/mb12xx @@ -8268,7 +8426,8 @@ This is used for gathering replay logs for the ekf2 module Disabled Autodetect TROne - TREvo + TREvo60m + TREvo600Hz @@ -8446,6 +8605,7 @@ This is used for gathering replay logs for the ekf2 module ESP8266 (921600 baud, 8N1) Normal Telemetry (115200 baud, 8N1) Minimal Telemetry (115200 baud, 8N1) + RTPS Client (460800 baud) @@ -8455,6 +8615,20 @@ This is used for gathering replay logs for the ekf2 module true drivers/px4fmu + + Control if the vehicle has a barometer + Disable this if the board has no barometer, such as some of the the Omnibus F4 SD variants. If disabled, the preflight checks will not check for the presence of a barometer. + + true + modules/systemlib + + + Control if the vehicle has a magnetometer + Disable this if the board has no magnetometer, such as the Omnibus F4 SD. If disabled, the preflight checks will not check for the presence of a magnetometer. + + true + modules/systemlib + Enable HITL mode on next boot While enabled the system will boot in HITL mode and not enable all sensors and checks. When disabled the same vehicle can be normally flown outdoors. @@ -9240,15 +9414,16 @@ This is used for gathering replay logs for the ekf2 module UAVCAN mode - 0 - UAVCAN disabled. 1 - Basic support for UAVCAN actuators and sensors. 2 - Full support for dynamic node ID allocation and firmware update. 3 - Sets the motor control outputs to UAVCAN and enables support for dynamic node ID allocation and firmware update. + 0 - UAVCAN disabled. 1 - Enables support for UAVCAN sensors without dynamic node ID allocation and firmware update. 2 - Enables support for UAVCAN sensors with dynamic node ID allocation and firmware update. 3 - Enables support for UAVCAN sensors and actuators with dynamic node ID allocation and firmware update. Also sets the motor control outputs to UAVCAN. 0 3 true modules/uavcan Disabled - Only Sensors - Sensors and Motors + Sensors Manual Config + Sensors Automatic Config + Sensors and Actuators (ESCs) Automatic Config @@ -9336,9 +9511,10 @@ Airbrakes need to be enables for your selected model/mixer modules/vtol_att_control - Thottle output during back transition -For ESCs and mixers that support reverse thrust on low PWM values set this to a negative value to apply active breaking -For ESCs that support thrust reversal with a control channel please set VT_B_REV_OUT and set this to a positive value to apply active breaking + Target throttle value for the transition to hover flight. +standard vtol: pusher +tailsitter, tiltrotor: main throttle + Note for standard vtol: For ESCs and mixers that support reverse thrust on low PWM values set this to a negative value to apply active breaking For ESCs that support thrust reversal with a control channel please set VT_B_REV_OUT and set this to a positive value to apply active breaking -1 1 2 @@ -9368,7 +9544,7 @@ to accelerate forward if necessary Adaptive QuadChute - Maximum negative altitude error, when in fixed wing the altitude drops below this copared to the altitude setpoint the vehicle will transition back to MC mode and enter failsafe RTL + Maximum negative altitude error for fixed wing flight. If the altitude drops below this value below the altitude setpoint the vehicle will transition back to MC mode and enter failsafe RTL. 0.0 200.0 modules/vtol_att_control @@ -9411,15 +9587,6 @@ to accelerate forward if necessary modules/vtol_att_control - - Fixed wing pitch trim - This parameter allows to adjust the neutral elevon position in fixed wing mode. - -1.0 - 1.0 - 2 - 0.01 - modules/vtol_att_control - QuadChute Max Pitch Maximum pitch angle before QuadChute engages Above this the vehicle will transition back to MC mode and enter failsafe RTL @@ -9444,6 +9611,16 @@ to accelerate forward if necessary 1 modules/vtol_att_control + + Target throttle value for the transition to fixed wing flight. +standard vtol: pusher +tailsitter, tiltrotor: main throttle + 0.0 + 1.0 + 3 + 0.01 + modules/vtol_att_control + Airspeed less front transition time (open loop) The duration of the front transition when there is no airspeed feedback available. @@ -9469,9 +9646,12 @@ to accelerate forward if necessary 1 modules/vtol_att_control - - Optimal recovery strategy for pitch-weak tailsitters - + + Defines the time window during which the pusher throttle will be ramped up linearly to VT_F_TRANS_THR during a transition +to fixed wing mode. Zero or negative values will produce an instant throttle rise to VT_F_TRANS_THR + 20 + 2 + 0.01 modules/vtol_att_control @@ -9516,14 +9696,6 @@ to accelerate forward if necessary 0.01 modules/vtol_att_control - - Target throttle value for pusher/puller motor during the transition to fw mode - 0.0 - 1.0 - 3 - 0.01 - modules/vtol_att_control - Front transition timeout Time in seconds after which transition will be cancelled. Disabled if set to 0. @@ -9556,6 +9728,51 @@ to accelerate forward if necessary modules/vtol_att_control + + + Gate size for true sideslip fusion + Sets the number of standard deviations used by the innovation consistency test. + 1 + 5 + SD + modules/wind_estimator + + + Wind estimator sideslip measurement noise + 0 + 1 + rad + modules/wind_estimator + + + Wind estimator true airspeed scale process noise + 0 + 0.1 + modules/wind_estimator + + + Gate size for true airspeed fusion + Sets the number of standard deviations used by the innovation consistency test. + 1 + 5 + SD + modules/wind_estimator + + + Wind estimator true airspeed measurement noise + 0 + 4 + m/s + modules/wind_estimator + + + Wind estimator wind process noise + 0 + 1 + m/s/s + modules/wind_estimator + + EXFW_HDNG_P diff --git a/src/FirmwarePlugin/PX4/PX4ParameterMetaData.cc b/src/FirmwarePlugin/PX4/PX4ParameterMetaData.cc index a4c3b6fc1889a75c1799f72518468986ad706611..5654a2a9c8cb81695e5b7800fe43fd7f2e7ac0cb 100644 --- a/src/FirmwarePlugin/PX4/PX4ParameterMetaData.cc +++ b/src/FirmwarePlugin/PX4/PX4ParameterMetaData.cc @@ -332,7 +332,7 @@ void PX4ParameterMetaData::loadParameterFactMetaDataFile(const QString& metaData QString text = xml.readElementText(); increment = text.toDouble(&ok); if (ok) { - metaData->setIncrement(increment); + metaData->setRawIncrement(increment); } else { qCWarning(PX4ParameterMetaDataLog) << "Invalid value for increment, name:" << metaData->name() << " increment:" << text; } diff --git a/src/FirmwarePlugin/PX4/px4_custom_mode.h b/src/FirmwarePlugin/PX4/px4_custom_mode.h index 48a29f2cda456b06abc950b0b99feb6eef6641c7..f6b3c905502748b30e1d73ae4d337981308f1d1f 100644 --- a/src/FirmwarePlugin/PX4/px4_custom_mode.h +++ b/src/FirmwarePlugin/PX4/px4_custom_mode.h @@ -34,7 +34,7 @@ /** * @file px4_custom_mode.h * PX4 custom flight modes - * Copied from PX4 2017-07-08 - https://github.com/PX4/Firmware/blob/master/src/modules/commander/px4_custom_mode.h#L45 + * Copied from PX4 2018-04-07 - https://github.com/PX4/Firmware/blob/master/src/modules/commander/px4_custom_mode.h#L45 */ #ifndef PX4_CUSTOM_MODE_H_ @@ -62,7 +62,8 @@ enum PX4_CUSTOM_SUB_MODE_AUTO { PX4_CUSTOM_SUB_MODE_AUTO_RTL, PX4_CUSTOM_SUB_MODE_AUTO_LAND, PX4_CUSTOM_SUB_MODE_AUTO_RTGS, - PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET + PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET, + PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND }; union px4_custom_mode { diff --git a/src/FlightDisplay/BuiltInPreFlightCheckModel.qml b/src/FlightDisplay/BuiltInPreFlightCheckModel.qml new file mode 100644 index 0000000000000000000000000000000000000000..c39a6396602f43188732f546b349bed38af3d128 --- /dev/null +++ b/src/FlightDisplay/BuiltInPreFlightCheckModel.qml @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.3 +import QtQml.Models 2.1 + +import QGroundControl 1.0 +import QGroundControl.FlightDisplay 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.Palette 1.0 +import QGroundControl.Vehicle 1.0 + +PreFlightCheckModel { + PreFlightCheckGroup { + name: qsTr("Initial checks") + + // Standard check list items (group 0) - Available from the start + PreFlightCheckButton { + name: qsTr("Hardware") + manualText: qsTr("Props mounted? Wings secured? Tail secured?") + } + + PreFlightBatteryCheck { + failurePercent: 40 + allowFailurePercentOverride: false + } + + PreFlightSensorsHealthCheck { + } + + PreFlightGPSCheck { + failureSatCount: 9 + allowOverrideSatCount: true + } + + PreFlightRCCheck { + } + } + + PreFlightCheckGroup { + name: qsTr("Please arm the vehicle here") + + PreFlightCheckButton { + name: qsTr("Actuators") + manualText: qsTr("Move all control surfaces. Did they work properly?") + } + + PreFlightCheckButton { + name: qsTr("Motors") + manualText: qsTr("Propellers free? Then throttle up gently. Working properly?") + } + + PreFlightCheckButton { + name: qsTr("Mission") + manualText: qsTr("Please confirm mission is valid (waypoints valid, no terrain collision).") + } + + PreFlightSoundCheck { + } + } + + PreFlightCheckGroup { + name: qsTr("Last preparations before launch") + + // Check list item group 2 - Final checks before launch + PreFlightCheckButton { + name: qsTr("Payload") + manualText: qsTr("Configured and started? Payload lid closed?") + } + + PreFlightCheckButton { + name: "Wind & weather" + manualText: qsTr("OK for your platform? Lauching into the wind?") + } + + PreFlightCheckButton { + name: qsTr("Flight area") + manualText: qsTr("Launch area and path free of obstacles/people?") + } + } +} // Object Model diff --git a/src/FlightDisplay/FlightDisplayView.qml b/src/FlightDisplay/FlightDisplayView.qml index 58425335678e7958c138836fb842744aa6f8fdd2..7126411db1bedb7b2eb2e09996d0c3e6e5d93d9b 100644 --- a/src/FlightDisplay/FlightDisplayView.qml +++ b/src/FlightDisplay/FlightDisplayView.qml @@ -14,9 +14,9 @@ import QtQuick.Controls.Styles 1.4 import QtQuick.Dialogs 1.2 import QtLocation 5.3 import QtPositioning 5.3 -import QtMultimedia 5.5 import QtQuick.Layouts 1.2 import QtQuick.Window 2.2 +import QtQml.Models 2.1 import QGroundControl 1.0 import QGroundControl.FlightDisplay 1.0 @@ -46,13 +46,13 @@ QGCView { property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle property bool _mainIsMap: QGroundControl.videoManager.hasVideo ? QGroundControl.loadBoolGlobalSetting(_mainIsMapKey, true) : true property bool _isPipVisible: QGroundControl.videoManager.hasVideo ? QGroundControl.loadBoolGlobalSetting(_PIPVisibleKey, true) : false + property bool _useChecklist: QGroundControl.settingsManager.appSettings.useChecklist.rawValue property real _savedZoomLevel: 0 property real _margins: ScreenTools.defaultFontPixelWidth / 2 property real _pipSize: flightView.width * 0.2 property alias _guidedController: guidedActionsController property alias _altitudeSlider: altitudeSlider - readonly property var _dynamicCameras: _activeVehicle ? _activeVehicle.dynamicCameras : null readonly property bool _isCamera: _dynamicCameras ? _dynamicCameras.cameras.count > 0 : false readonly property bool isBackgroundDark: _mainIsMap ? (_flightMap ? _flightMap.isSatelliteMap : true) : true @@ -109,7 +109,11 @@ QGCView { PlanMasterController { id: masterController - Component.onCompleted: start(false /* editMode */) + Component.onCompleted: start(true /* flyView */) + } + + BuiltInPreFlightCheckModel { + id: preFlightCheckModel } Connections { @@ -188,9 +192,9 @@ QGCView { QGCButton { Layout.fillWidth: true + Layout.alignment: Qt.AlignHCenter text: qsTr("Leave plan on vehicle") - anchors.horizontalCenter: parent.horizontalCenter - onClicked: hideDialog() + onClicked: hideDialog() } } } @@ -212,7 +216,23 @@ QGCView { _flightVideo.state = "unpopup" videoWindow.visible = false } + } + /* This timer will startVideo again after the popup window appears and is loaded. + * Such approach was the only one to avoid a crash for windows users + */ + Timer { + id: videoPopUpTimer + interval: 2000; + running: false; + repeat: false + onTriggered: { + // If state is popup, the next one will be popup-finished + if (_flightVideo.state == "popup") { + _flightVideo.state = "popup-finished" + } + QGroundControl.videoManager.startVideo() + } } QGCMapPalette { id: mapPal; lightColors: _mainIsMap ? _flightMap.isSatelliteMap : true } @@ -270,6 +290,20 @@ QGCView { anchors.left: _panel.left anchors.bottom: _panel.bottom visible: QGroundControl.videoManager.hasVideo && (!_mainIsMap || _isPipVisible) + + onParentChanged: { + /* If video comes back from popup + * correct anchors. + * Such thing is not possible with ParentChange. + */ + if(parent == _panel) { + // Do anchors again after popup + anchors.left = _panel.left + anchors.bottom = _panel.bottom + anchors.margins = ScreenTools.defaultFontPixelHeight + } + } + states: [ State { name: "pipMode" @@ -296,36 +330,41 @@ QGCView { State { name: "popup" StateChangeScript { - script: QGroundControl.videoManager.stopVideo() + script: { + // Stop video, restart it again with Timer + // Avoiding crashs if ParentChange is not yet done + QGroundControl.videoManager.stopVideo() + videoPopUpTimer.running = true + } } + PropertyChanges { + target: _flightVideoPipControl + inPopup: true + } + }, + State { + name: "popup-finished" ParentChange { target: _flightVideo parent: videoItem x: 0 y: 0 - width: videoWindow.width - height: videoWindow.height - } - PropertyChanges { - target: _flightVideoPipControl - inPopup: true + width: videoItem.width + height: videoItem.height } }, State { name: "unpopup" StateChangeScript { - script: QGroundControl.videoManager.stopVideo() + script: { + QGroundControl.videoManager.stopVideo() + videoPopUpTimer.running = true + } } ParentChange { target: _flightVideo parent: _panel } - PropertyChanges { - target: _flightVideo - anchors.left: _panel.left - anchors.bottom: _panel.bottom - anchors.margins: ScreenTools.defaultFontPixelHeight - } PropertyChanges { target: _flightVideoPipControl inPopup: false @@ -469,8 +508,8 @@ QGCView { z: _panel.z + 4 title: qsTr("Fly") maxHeight: (_flightVideo.visible ? _flightVideo.y : parent.height) - toolStrip.y - buttonVisible: [ _guidedController.showTakeoff || !_guidedController.showLand, _guidedController.showLand && !_guidedController.showTakeoff, true, true, true, _guidedController.smartShotsAvailable ] - buttonEnabled: [ _guidedController.showTakeoff, _guidedController.showLand, _guidedController.showRTL, _guidedController.showPause, _anyActionAvailable, _anySmartShotAvailable ] + buttonVisible: [ _useChecklist, _guidedController.showTakeoff || !_guidedController.showLand, _guidedController.showLand && !_guidedController.showTakeoff, true, true, true, _guidedController.smartShotsAvailable ] + buttonEnabled: [ _useChecklist && _activeVehicle, _guidedController.showTakeoff, _guidedController.showLand, _guidedController.showRTL, _guidedController.showPause, _anyActionAvailable, _anySmartShotAvailable ] property bool _anyActionAvailable: _guidedController.showStartMission || _guidedController.showResumeMission || _guidedController.showChangeAlt || _guidedController.showLandAbort property bool _anySmartShotAvailable: _guidedController.showOrbit @@ -516,6 +555,11 @@ QGCView { ] model: [ + { + name: "Checklist", + iconSource: "/qmlimages/check.svg", + dropPanelComponent: checklistDropPanel + }, { name: _guidedController.takeoffTitle, iconSource: "/res/takeoff.svg", @@ -555,10 +599,10 @@ QGCView { guidedActionsController.closeAll() var action = model[index].action if (action === -1) { - if (index == 4) { + if (index == 5) { guidedActionList.model = _actionModel guidedActionList.visible = true - } else if (index == 5) { + } else if (index == 6) { guidedActionList.model = _smartShotModel guidedActionList.visible = true } @@ -642,4 +686,13 @@ QGCView { visible: false } } -} + + //-- Checklist GUI + Component { + id: checklistDropPanel + + PreFlightCheckList { + model: preFlightCheckModel + } + } //Component +} //QGC View diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index 7ae130ab8383362b83c2e7230a2147310b22c06f..bbc199045103f40b1b2dcf797804e61d7f673b0a 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -100,7 +100,13 @@ FlightMap { function recenterNeeded() { var vehiclePoint = flightMap.fromCoordinate(_activeVehicleCoordinate, false /* clipToViewport */) - var centerViewport = Qt.rect(0, 0, width, height) + var toolStripRightEdge = mapFromItem(toolStrip, toolStrip.x, 0).x + toolStrip.width + var instrumentsWidth = 0 + if (QGroundControl.corePlugin.options.instrumentWidget.widgetPosition === CustomInstrumentWidget.POS_TOP_RIGHT) { + // Assume standard instruments + instrumentsWidth = flightDisplayViewWidgets.getPreferredInstrumentWidth() + } + var centerViewport = Qt.rect(toolStripRightEdge, 0, width - toolStripRightEdge - instrumentsWidth, height) return !pointInRect(vehiclePoint, centerViewport) } @@ -155,7 +161,7 @@ FlightMap { } MapFitFunctions { - id: mapFitFunctions + id: mapFitFunctions // The name for this id cannot be changed without breaking references outside of this code. Beware! map: _flightMap usePlannedHomePosition: false planMasterController: _planMasterController @@ -237,7 +243,7 @@ FlightMap { myGeoFenceController: _geoFenceController interactive: false planView: false - homePosition: _activeVehicle && _activeVehicle.homePosition.isValid ? _activeVehicle.homePosition : undefined + homePosition: _activeVehicle && _activeVehicle.homePosition.isValid ? _activeVehicle.homePosition : QtPositioning.coordinate() } // Rally points on map diff --git a/src/FlightDisplay/FlightDisplayViewWidgets.qml b/src/FlightDisplay/FlightDisplayViewWidgets.qml index 8f4b08f9690e8f7f6f024cca8ea25c360795f81c..fc0c9d82b2a124a6157fdfa6ac6b8eeecda28487 100644 --- a/src/FlightDisplay/FlightDisplayViewWidgets.qml +++ b/src/FlightDisplay/FlightDisplayViewWidgets.qml @@ -73,15 +73,7 @@ Item { break; } } else { - // Note: We currently show alternate instruments all the time. This is a trial change for daily builds. - // Leaving non-alternate code in for now in case the trial fails. - var useAlternateInstruments = true//QGroundControl.settingsManager.appSettings.virtualJoystick.value || ScreenTools.isTinyScreen - if(useAlternateInstruments) { - instrumentsLoader.source = "qrc:/qml/QGCInstrumentWidgetAlternate.qml" - } else { - instrumentsLoader.source = "qrc:/qml/QGCInstrumentWidget.qml" - instrumentsLoader.state = QGroundControl.settingsManager.appSettings.showLargeCompass.value === 1 ? "centerRightMode" : "topRightMode" - } + instrumentsLoader.source = "qrc:/qml/QGCInstrumentWidgetAlternate.qml" } } else { instrumentsLoader.source = "" @@ -135,7 +127,7 @@ Item { z: QGroundControl.zOrderTopMost color: mapPal.text font.pointSize: ScreenTools.largeFontPointSize - text: "The vehicle has failed a pre-arm check. In order to arm the vehicle, resolve the failure or disable the arming check via the Safety tab on the Vehicle Setup page." + text: "The vehicle has failed a pre-arm check. In order to arm the vehicle, resolve the failure." } } diff --git a/src/FlightDisplay/GuidedActionConfirm.qml b/src/FlightDisplay/GuidedActionConfirm.qml index 36aedb24b15d75e584ac176c09b9cd8ba84d5545..971fa460d9b375989568df84e79bf0623b29539e 100644 --- a/src/FlightDisplay/GuidedActionConfirm.qml +++ b/src/FlightDisplay/GuidedActionConfirm.qml @@ -18,13 +18,12 @@ import QGroundControl.Palette 1.0 /// Guided actions confirmation dialog Rectangle { id: _root - border.color: qgcPal.alertBorder - border.width: 1 width: confirmColumn.width + (_margins * 4) height: confirmColumn.height + (_margins * 4) radius: ScreenTools.defaultFontPixelHeight / 2 - color: qgcPal.alertBackground - opacity: 0.9 + color: qgcPal.window + border.color: _emergencyAction ? "red" : qgcPal.windowShade + border.width: _emergencyAction ? 4 : 1 z: guidedController.z visible: false @@ -36,16 +35,35 @@ Rectangle { property var actionData property bool hideTrigger: false - property real _margins: ScreenTools.defaultFontPixelWidth + property real _margins: ScreenTools.defaultFontPixelWidth + property bool _emergencyAction: action === guidedController.actionEmergencyStop onHideTriggerChanged: { if (hideTrigger) { hideTrigger = false altitudeSlider.visible = false + visibleTimer.stop() visible = false } } + function show(immediate) { + if (immediate) { + visible = true + } else { + // We delay showing the confirmation for a small amount in order to any other state + // changes to propogate through the system. This way only the final state shows up. + visibleTimer.restart() + } + } + + Timer { + id: visibleTimer + interval: 1000 + repeat: false + onTriggered: visible = true + } + QGCPalette { id: qgcPal } DeadMouseArea { @@ -60,7 +78,6 @@ Rectangle { QGCLabel { id: titleText - color: qgcPal.alertText anchors.left: slider.left anchors.right: slider.right horizontalAlignment: Text.AlignHCenter @@ -69,7 +86,6 @@ Rectangle { QGCLabel { id: messageText - color: qgcPal.alertText anchors.left: slider.left anchors.right: slider.right horizontalAlignment: Text.AlignHCenter @@ -109,7 +125,7 @@ Rectangle { sourceSize.height: width source: "/res/XDelete.svg" fillMode: Image.PreserveAspectFit - color: qgcPal.alertText + color: qgcPal.text QGCMouseArea { fillItem: parent onClicked: { diff --git a/src/FlightDisplay/GuidedActionList.qml b/src/FlightDisplay/GuidedActionList.qml index 768c7d814f5b79a3ff4b4f01163bb81e54957729..b4bfbcd5e4c0a0483156b6f66a7854ed125d8433 100644 --- a/src/FlightDisplay/GuidedActionList.qml +++ b/src/FlightDisplay/GuidedActionList.qml @@ -85,9 +85,9 @@ Rectangle { } QGCButton { - id: actionButton - anchors.horizontalCenter: parent.horizontalCenter - text: modelData.title + id: actionButton + text: modelData.title + Layout.alignment: Qt.AlignCenter onClicked: { _root.visible = false diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml index 941846659cac41b3afdad78be9986ec16dc992b2..b8fa3ba19c8a73a298bd36c3296ad87b11b5c7d4 100644 --- a/src/FlightDisplay/GuidedActionsController.qml +++ b/src/FlightDisplay/GuidedActionsController.qml @@ -32,7 +32,7 @@ Item { property var actionList property var altitudeSlider - readonly property string emergencyStopTitle: qsTr("Emergency Stop") + readonly property string emergencyStopTitle: qsTr("EMERGENCY STOP") readonly property string armTitle: qsTr("Arm") readonly property string disarmTitle: qsTr("Disarm") readonly property string rtlTitle: qsTr("RTL") @@ -52,7 +52,7 @@ Item { readonly property string armMessage: qsTr("Arm the vehicle.") readonly property string disarmMessage: qsTr("Disarm the vehicle") - readonly property string emergencyStopMessage: qsTr("WARNING: This will stop all motors. If vehicle is currently in air it will crash.") + readonly property string emergencyStopMessage: qsTr("WARNING: THIS WILL STOP ALL MOTORS. IF VEHICLE IS CURRENTLY IN THE AIR IT WILL CRASH.") readonly property string takeoffMessage: qsTr("Takeoff from ground and hold position.") readonly property string startMissionMessage: qsTr("Takeoff from ground and start the current mission.") readonly property string continueMissionMessage: qsTr("Continue the mission from the current waypoint.") @@ -66,7 +66,7 @@ Item { property string setWaypointMessage: qsTr("Adjust current waypoint to %1.").arg(_actionData) readonly property string orbitMessage: qsTr("Orbit the vehicle around the current location.") readonly property string landAbortMessage: qsTr("Abort the landing sequence.") - readonly property string pauseMessage: qsTr("Pause the vehicle at it's current position.") + readonly property string pauseMessage: qsTr("Pause the vehicle at it's current position, adjusting altitude up or down as needed.") readonly property string mvPauseMessage: qsTr("Pause all vehicles at their current position.") readonly property string vtolTransitionFwdMessage: qsTr("Transition VTOL to fixed wing flight.") readonly property string vtolTransitionMRMessage: qsTr("Transition VTOL to multi-rotor flight.") @@ -93,25 +93,26 @@ Item { readonly property int actionVtolTransitionToFwdFlight: 20 readonly property int actionVtolTransitionToMRFlight: 21 - property bool showEmergenyStop: !_hideEmergenyStop && _activeVehicle && _vehicleArmed && _vehicleFlying - property bool showArm: _activeVehicle && !_vehicleArmed - property bool showDisarm: _activeVehicle && _vehicleArmed && !_vehicleFlying - property bool showRTL: _activeVehicle && _vehicleArmed && _activeVehicle.guidedModeSupported && _vehicleFlying && !_vehicleInRTLMode - property bool showTakeoff: _activeVehicle && _activeVehicle.takeoffVehicleSupported && !_vehicleFlying - property bool showLand: _activeVehicle && _activeVehicle.guidedModeSupported && _vehicleArmed && !_activeVehicle.fixedWing && !_vehicleInLandMode - property bool showStartMission: _activeVehicle && _missionAvailable && !_missionActive && !_vehicleFlying - property bool showContinueMission: _activeVehicle && _missionAvailable && !_missionActive && _vehicleFlying && (_currentMissionIndex < missionController.visualItems.count - 1) - property bool showPause: _activeVehicle && _vehicleArmed && _activeVehicle.pauseVehicleSupported && _vehicleFlying && !_vehiclePaused - property bool showChangeAlt: (_activeVehicle && _vehicleFlying) && _activeVehicle.guidedModeSupported && _vehicleArmed && !_missionActive - property bool showOrbit: !_hideOrbit && _activeVehicle && _vehicleFlying && _activeVehicle.orbitModeSupported && _vehicleArmed && !_missionActive - property bool showLandAbort: _activeVehicle && _vehicleFlying && _activeVehicle.fixedWing && _vehicleLanding - property bool showGotoLocation: _activeVehicle && _vehicleFlying + property bool showEmergenyStop: _guidedActionsEnabled && !_hideEmergenyStop && _vehicleArmed && _vehicleFlying + property bool showArm: _guidedActionsEnabled && !_vehicleArmed + property bool showDisarm: _guidedActionsEnabled && _vehicleArmed && !_vehicleFlying + property bool showRTL: _guidedActionsEnabled && _vehicleArmed && _activeVehicle.guidedModeSupported && _vehicleFlying && !_vehicleInRTLMode + property bool showTakeoff: _guidedActionsEnabled && _activeVehicle.takeoffVehicleSupported && !_vehicleFlying + property bool showLand: _guidedActionsEnabled && _activeVehicle.guidedModeSupported && _vehicleArmed && !_activeVehicle.fixedWing && !_vehicleInLandMode + property bool showStartMission: _guidedActionsEnabled && _missionAvailable && !_missionActive && !_vehicleFlying + property bool showContinueMission: _guidedActionsEnabled && _missionAvailable && !_missionActive && _vehicleFlying && (_currentMissionIndex < missionController.visualItems.count - 1) + property bool showPause: _guidedActionsEnabled && _vehicleArmed && _activeVehicle.pauseVehicleSupported && _vehicleFlying && !_vehiclePaused + property bool showChangeAlt: _guidedActionsEnabled && _vehicleFlying && _activeVehicle.guidedModeSupported && _vehicleArmed && !_missionActive + property bool showOrbit: _guidedActionsEnabled && !_hideOrbit && _vehicleFlying && _activeVehicle.orbitModeSupported && _vehicleArmed && !_missionActive + property bool showLandAbort: _guidedActionsEnabled && _vehicleFlying && _activeVehicle.fixedWing && _vehicleLanding + property bool showGotoLocation: _guidedActionsEnabled && _vehicleFlying // Note: The 'missionController.visualItems.count - 3' is a hack to not trigger resume mission when a mission ends with an RTL item property bool showResumeMission: _activeVehicle && !_vehicleArmed && _vehicleWasFlying && _missionAvailable && _resumeMissionIndex > 0 && (_resumeMissionIndex < missionController.visualItems.count - 3) property bool guidedUIVisible: guidedActionConfirm.visible || guidedActionList.visible + property bool _guidedActionsEnabled: (!ScreenTools.isDebug && QGroundControl.corePlugin.options.guidedActionsRequireRCRSSI && _activeVehicle) ? _rcRSSIAvailable : _activeVehicle property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle property string _flightMode: _activeVehicle ? _activeVehicle.flightMode : "" property bool _missionAvailable: missionController.containsItems @@ -128,46 +129,53 @@ Item { property bool _hideEmergenyStop: !QGroundControl.corePlugin.options.guidedBarShowEmergencyStop property bool _hideOrbit: !QGroundControl.corePlugin.options.guidedBarShowOrbit property bool _vehicleWasFlying: false + property bool _rcRSSIAvailable: _activeVehicle ? _activeVehicle.rcRSSI > 0 && _activeVehicle.rcRSSI < 255 : false - /* //Handy code for debugging state problems - property bool __guidedModeSupported: _activeVehicle ? _activeVehicle.guidedModeSupported : false + property bool __debugGuidedStates: false + property bool __guidedModeSupported: _activeVehicle ? _activeVehicle.guidedModeSupported : false property bool __pauseVehicleSupported: _activeVehicle ? _activeVehicle.pauseVehicleSupported : false - property bool __flightMode: _flightMode + property bool __flightMode: _flightMode function _outputState() { - //console.log(qsTr("_activeVehicle(%1) _vehicleArmed(%2) guidedModeSupported(%3) _vehicleFlying(%4) _vehicleInRTLMode(%5) pauseVehicleSupported(%6) _vehiclePaused(%7) _flightMode(%8)").arg(_activeVehicle ? 1 : 0).arg(_vehicleArmed ? 1 : 0).arg(__guidedModeSupported ? 1 : 0).arg(_vehicleFlying ? 1 : 0).arg(_vehicleInRTLMode ? 1 : 0).arg(__pauseVehicleSupported ? 1 : 0).arg(_vehiclePaused ? 1 : 0).arg(_flightMode)) + if (__debugGuidedStates) { + console.log(qsTr("_activeVehicle(%1) _vehicleArmed(%2) guidedModeSupported(%3) _vehicleFlying(%4) _vehicleInRTLMode(%5) pauseVehicleSupported(%6) _vehiclePaused(%7) _flightMode(%8)").arg(_activeVehicle ? 1 : 0).arg(_vehicleArmed ? 1 : 0).arg(__guidedModeSupported ? 1 : 0).arg(_vehicleFlying ? 1 : 0).arg(_vehicleInRTLMode ? 1 : 0).arg(__pauseVehicleSupported ? 1 : 0).arg(_vehiclePaused ? 1 : 0).arg(_flightMode)) + } } - Component.onCompleted: _outputState() - on_ActiveVehicleChanged: _outputState() - on_VehicleArmedChanged: _outputState() - on_VehicleInRTLModeChanged: _outputState() - on_VehiclePausedChanged: _outputState() - on__FlightModeChanged: _outputState() - on__GuidedModeSupportedChanged: _outputState() - on__PauseVehicleSupportedChanged: _outputState() + Component.onCompleted: _outputState() + on_ActiveVehicleChanged: _outputState() + on_VehicleArmedChanged: _outputState() + on_VehicleInRTLModeChanged: _outputState() + on_VehiclePausedChanged: _outputState() + on__FlightModeChanged: _outputState() + on__GuidedModeSupportedChanged: _outputState() + on__PauseVehicleSupportedChanged: _outputState() - on_CurrentMissionIndexChanged: console.log("_currentMissionIndex", _currentMissionIndex) - on_ResumeMissionIndexChanged: console.log("_resumeMissionIndex", _resumeMissionIndex) + on_CurrentMissionIndexChanged: console.log("_currentMissionIndex", _currentMissionIndex) + on_ResumeMissionIndexChanged: console.log("_resumeMissionIndex", _resumeMissionIndex) onShowResumeMissionChanged: { - console.log("showResumeMission", showResumeMission) + if (__debugGuidedStates) { + console.log("showResumeMission", showResumeMission) + } _outputState() } onShowStartMissionChanged: { - console.log("showStartMission", showStartMission) + if (__debugGuidedStates) { + console.log("showStartMission", showStartMission) + } _outputState() } onShowContinueMissionChanged: { - console.log("showContinueMission", showContinueMission) + if (__debugGuidedStates) { + console.log("showContinueMission", showContinueMission) + } _outputState() } - // End of hack - */ on_VehicleFlyingChanged: { - //_outputState() + _outputState() if (!_vehicleFlying) { // We use _vehicleWasFLying to help trigger Resume Mission only if the vehicle actually flew and came back down. // Otherwise it may trigger during the Start Mission sequence due to signal ordering or armed and resume mission index. @@ -185,13 +193,15 @@ Item { // Called when an action is about to be executed in order to confirm function confirmAction(actionCode, actionData) { + var showImmediate = true closeAll() confirmDialog.action = actionCode confirmDialog.actionData = actionData + confirmDialog.hideTrigger = true _actionData = actionData switch (actionCode) { case actionArm: - if (_vehicleFlying) { + if (_vehicleFlying || !_guidedActionsEnabled) { return } confirmDialog.title = armTitle @@ -219,6 +229,7 @@ Item { altitudeSlider.visible = true break; case actionStartMission: + showImmediate = false confirmDialog.title = startMissionTitle confirmDialog.message = startMissionMessage confirmDialog.hideTrigger = Qt.binding(function() { return !showStartMission }) @@ -229,11 +240,13 @@ Item { confirmDialog.hideTrigger = true break; case actionContinueMission: + showImmediate = false confirmDialog.title = continueMissionTitle confirmDialog.message = continueMissionMessage confirmDialog.hideTrigger = Qt.binding(function() { return !showContinueMission }) break; case actionResumeMission: + showImmediate = false confirmDialog.title = resumeMissionTitle confirmDialog.message = resumeMissionMessage confirmDialog.hideTrigger = Qt.binding(function() { return !showResumeMission }) @@ -288,6 +301,8 @@ Item { confirmDialog.title = pauseTitle confirmDialog.message = pauseMessage confirmDialog.hideTrigger = Qt.binding(function() { return !showPause }) + altitudeSlider.reset() + altitudeSlider.visible = true break; case actionMVPause: confirmDialog.title = pauseTitle @@ -308,7 +323,7 @@ Item { console.warn("Unknown actionCode", actionCode) return } - confirmDialog.visible = true + confirmDialog.show(showImmediate) } // Executes the specified action @@ -369,6 +384,7 @@ Item { break case actionPause: _activeVehicle.pauseVehicle() + _activeVehicle.guidedModeChangeAltitude(actionData) break case actionMVPause: rgVehicle = QGroundControl.multiVehicleManager.vehicles diff --git a/src/FlightDisplay/MultiVehicleList.qml b/src/FlightDisplay/MultiVehicleList.qml index 4c62a4f1cd7aa4a421faf7b541703c75e95967d0..82f799661566095626dc1bc40474e4553898f62b 100644 --- a/src/FlightDisplay/MultiVehicleList.qml +++ b/src/FlightDisplay/MultiVehicleList.qml @@ -107,8 +107,7 @@ Item { spacing: _margin RowLayout { - anchors.left: parent.left - anchors.right: parent.left + Layout.fillWidth: true QGCLabel { Layout.alignment: Qt.AlignTop @@ -121,14 +120,14 @@ Item { spacing: _margin FlightModeMenu { - anchors.horizontalCenter: parent.horizontalCenter + Layout.alignment: Qt.AlignHCenter font.pointSize: ScreenTools.largeFontPointSize color: _textColor activeVehicle: _vehicle } QGCLabel { - anchors.horizontalCenter: parent.horizontalCenter + Layout.alignment: Qt.AlignHCenter text: _vehicle.armed ? qsTr("Armed") : qsTr("Disarmed") color: _textColor } diff --git a/src/FlightDisplay/PreFlightBatteryCheck.qml b/src/FlightDisplay/PreFlightBatteryCheck.qml new file mode 100644 index 0000000000000000000000000000000000000000..00449582446a64f97156bc8139fe5833cb8e532c --- /dev/null +++ b/src/FlightDisplay/PreFlightBatteryCheck.qml @@ -0,0 +1,32 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.3 + +import QGroundControl 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.Vehicle 1.0 + +// This class stores the data and functions of the check list but NOT the GUI (which is handled somewhere else). +PreFlightCheckButton { + name: qsTr("Battery") + manualText: qsTr("Battery connector firmly plugged?") + telemetryFailure: _batLow + telemetryTextFailure: allowTelemetryFailureOverride ? + qsTr("Warning - Battery charge below %1%.").arg(failurePercent) : + qsTr("Battery charge below %1%. Please recharge.").arg(failurePercent) + allowTelemetryFailureOverride: allowFailurePercentOverride + + property int failurePercent: 40 + property bool allowFailurePercentOverride: false + + property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle + property var _batPercentRemaining: _activeVehicle ? _activeVehicle.battery.percentRemaining.value : 0 + property bool _batLow: _batPercentRemaining < failurePercent +} diff --git a/src/FlightDisplay/PreFlightGPSCheck.qml b/src/FlightDisplay/PreFlightGPSCheck.qml new file mode 100644 index 0000000000000000000000000000000000000000..4005d80e283d13fb3c7f8c986b2e458d3f79a2f6 --- /dev/null +++ b/src/FlightDisplay/PreFlightGPSCheck.qml @@ -0,0 +1,33 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.3 + +import QGroundControl 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.Vehicle 1.0 + +PreFlightCheckButton { + name: qsTr("GPS") + telemetryFailure: _3dLockFailure || _satCountFailure + telemetryTextFailure: _3dLockFailure ? + qsTr("Waiting for 3D lock.") : + (_satCountFailure ? _satCountFailureText : "") + allowTelemetryFailureOverride: !_3dLockFailure && _satCountFailure && allowOverrideSatCount + + property bool allowOverrideSatCount: false ///< true: sat count above failureSatCount reguired to pass, false: user can click past satCount <= failureSetCount + property int failureSatCount: -1 ///< -1 indicates no sat count check + + property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle + property bool _3dLock: _activeVehicle ? _activeVehicle.gps.lock.rawValue >= 3 : false + property int _satCount: _activeVehicle ? _activeVehicle.gps.count.rawValue : 0 + property bool _3dLockFailure: !_3dLock + property bool _satCountFailure: failureSatCount !== -1 && _satCount <= failureSatCount + property string _satCountFailureText: allowOverrideSatCount ? qsTr("Warning - Sat count below %1.").arg(failureSatCount + 1) : qsTr("Waiting for sat count above %1.").arg(failureSatCount) +} diff --git a/src/FlightDisplay/PreFlightRCCheck.qml b/src/FlightDisplay/PreFlightRCCheck.qml new file mode 100644 index 0000000000000000000000000000000000000000..9b010824a67039789e73187f2eef85e70f8e0faa --- /dev/null +++ b/src/FlightDisplay/PreFlightRCCheck.qml @@ -0,0 +1,24 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.3 + +import QGroundControl 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.Vehicle 1.0 + +PreFlightCheckButton { + name: qsTr("Radio Control") + manualText: qsTr("Receiving signal. Perform range test & confirm.") + telemetryTextFailure: qsTr("No signal or invalid autopilot-RC config. Check RC and console.") + telemetryFailure: _unhealthySensors & Vehicle.SysStatusSensorRCReceiver + + property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle + property int _unhealthySensors: _activeVehicle ? _activeVehicle.sensorsUnhealthyBits : 0 +} diff --git a/src/FlightDisplay/PreFlightSensorsHealthCheck.qml b/src/FlightDisplay/PreFlightSensorsHealthCheck.qml new file mode 100644 index 0000000000000000000000000000000000000000..d242c8380f5d5d8aa404c41f6a8f80a48de08562 --- /dev/null +++ b/src/FlightDisplay/PreFlightSensorsHealthCheck.qml @@ -0,0 +1,45 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.3 + +import QGroundControl 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.Vehicle 1.0 + +PreFlightCheckButton { + name: qsTr("Sensors") + telemetryFailure: _unhealthySensors & _allCheckedSensors + + property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle + property int _unhealthySensors: _activeVehicle ? _activeVehicle.sensorsUnhealthyBits : 0 + property int _allCheckedSensors: Vehicle.SysStatusSensor3dMag | + Vehicle.SysStatusSensor3dAccel | + Vehicle.SysStatusSensor3dGyro | + Vehicle.SysStatusSensorAbsolutePressure | + Vehicle.SysStatusSensorDifferentialPressure | + Vehicle.SysStatusSensorGPS | + Vehicle.SysStatusSensorAHRS + + on_UnhealthySensorsChanged: updateTelemetryTextFailure() + + Component.onCompleted: updateTelemetryTextFailure() + + function updateTelemetryTextFailure() { + if(_unhealthySensors & _allCheckedSensors) { + if (_unhealthySensors & Vehicle.SysStatusSensor3dMag) telemetryTextFailure = qsTr("Failure. Magnetometer issues. Check console.") + else if(_unhealthySensors & Vehicle.SysStatusSensor3dAccel) telemetryTextFailure = qsTr("Failure. Accelerometer issues. Check console.") + else if(_unhealthySensors & Vehicle.SysStatusSensor3dGyro) telemetryTextFailure = qsTr("Failure. Gyroscope issues. Check console.") + else if(_unhealthySensors & Vehicle.SysStatusSensorAbsolutePressure) telemetryTextFailure = qsTr("Failure. Barometer issues. Check console.") + else if(_unhealthySensors & Vehicle.SysStatusSensorDifferentialPressure) telemetryTextFailure = qsTr("Failure. Airspeed sensor issues. Check console.") + else if(_unhealthySensors & Vehicle.SysStatusSensorAHRS) telemetryTextFailure = qsTr("Failure. AHRS issues. Check console.") + else if(_unhealthySensors & Vehicle.SysStatusSensorGPS) telemetryTextFailure = qsTr("Failure. GPS issues. Check console.") + } + } +} diff --git a/src/FlightDisplay/PreFlightSoundCheck.qml b/src/FlightDisplay/PreFlightSoundCheck.qml new file mode 100644 index 0000000000000000000000000000000000000000..2942191df5256300bf25915276a14c2c899b492f --- /dev/null +++ b/src/FlightDisplay/PreFlightSoundCheck.qml @@ -0,0 +1,20 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.3 + +import QGroundControl 1.0 +import QGroundControl.Controls 1.0 + +PreFlightCheckButton { + name: qsTr("Sound output") + manualText: qsTr("QGC audio output enabled. System audio output enabled, too?") + telemetryTextFailure: qsTr("QGC audio output is disabled. Please enable it under application settings->general to hear audio warnings!") + telemetryFailure: QGroundControl.settingsManager.appSettings.audioMuted.rawValue +} diff --git a/src/FlightDisplay/VideoManager.cc b/src/FlightDisplay/VideoManager.cc index a2b839a47889c1b0392272654251e7034b77d508..702e202502c560823ec8898012e74c70e4319184 100644 --- a/src/FlightDisplay/VideoManager.cc +++ b/src/FlightDisplay/VideoManager.cc @@ -78,7 +78,7 @@ VideoManager::setToolbox(QGCToolbox *toolbox) emit isGStreamerChanged(); qCDebug(VideoManagerLog) << "New Video Source:" << videoSource; - _videoReceiver = new VideoReceiver(this); + _videoReceiver = toolbox->corePlugin()->createVideoReceiver(this); _updateSettings(); if(isGStreamer()) { _videoReceiver->start(); diff --git a/src/FlightDisplay/VideoManager.h b/src/FlightDisplay/VideoManager.h index f419af90a281a1889feba1642979e35ca930dde4..a63d6837abedb3490012dd1d02b4503632ecdf05 100644 --- a/src/FlightDisplay/VideoManager.h +++ b/src/FlightDisplay/VideoManager.h @@ -56,7 +56,7 @@ public: // Override from QGCTool void setToolbox (QGCToolbox *toolbox); - Q_INVOKABLE void startVideo() {_videoReceiver->stop();}; + Q_INVOKABLE void startVideo() {_videoReceiver->start();}; Q_INVOKABLE void stopVideo() {_videoReceiver->stop();}; signals: diff --git a/src/FlightDisplay/qmldir b/src/FlightDisplay/qmldir index f29069e4677e831ff7a5c8015e8827c712ee2831..f37894a8b7de3e97cf3a70eb14bd974b818c5000 100644 --- a/src/FlightDisplay/qmldir +++ b/src/FlightDisplay/qmldir @@ -9,4 +9,10 @@ GuidedActionsController 1.0 GuidedActionsController.qml GuidedActionList 1.0 GuidedActionList.qml GuidedAltitudeSlider 1.0 GuidedAltitudeSlider.qml MultiVehicleList 1.0 MultiVehicleList.qml +PreFlightBatteryCheck 1.0 PreFlightBatteryCheck.qml +BuiltInPreFlightCheckModel 1.0 BuiltInPreFlightCheckModel.qml +PreFlightGPSCheck 1.0 PreFlightGPSCheck.qml +PreFlightRCCheck 1.0 PreFlightRCCheck.qml +PreFlightSensorsHealthCheck 1.0 PreFlightSensorsHealthCheck.qml +PreFlightSoundCheck 1.0 PreFlightSoundCheck.qml diff --git a/src/FlightMap/FlightMap.qml b/src/FlightMap/FlightMap.qml index f478186ae1d2b8e80edf8f7b3d41f25468251825..6e969d9f923cfd1c061d6d639d2bb4ca873c4ae9 100644 --- a/src/FlightMap/FlightMap.qml +++ b/src/FlightMap/FlightMap.qml @@ -11,6 +11,7 @@ import QtQuick 2.3 import QtQuick.Controls 1.2 import QtLocation 5.3 import QtPositioning 5.3 +import QtQuick.Dialogs 1.2 import QGroundControl 1.0 import QGroundControl.FactSystem 1.0 @@ -19,7 +20,6 @@ import QGroundControl.FlightMap 1.0 import QGroundControl.ScreenTools 1.0 import QGroundControl.MultiVehicleManager 1.0 import QGroundControl.Vehicle 1.0 -import QGroundControl.Mavlink 1.0 import QGroundControl.QGCPositionManager 1.0 Map { @@ -41,6 +41,7 @@ Map { property bool firstGCSPositionReceived: false ///< true: first gcs position update was responded to property bool firstVehiclePositionReceived: false ///< true: first vehicle position update was responded to property bool planView: false ///< true: map being using for Plan view, items should be draggable + property var qgcView readonly property real maxZoomLevel: 20 @@ -63,6 +64,20 @@ Map { } } + function centerToSpecifiedLocation() { + qgcView.showDialog(specifyMapPositionDialog, qsTr("Specify Position"), qgcView.showDialogDefaultWidth, StandardButton.Cancel) + + } + + Component { + id: specifyMapPositionDialog + + EditPositionDialog { + coordinate: center + onCoordinateChanged: center = coordinate + } + } + ExclusiveGroup { id: mapTypeGroup } // Update ground station position diff --git a/src/FlightMap/Images/attitudeDial.svg b/src/FlightMap/Images/attitudeDial.svg old mode 100644 new mode 100755 index 1e74c332107021d7d1e2319c22e44d2a03132dc9..470a24a2c66fd95dcd8dcce7449aa1db59f6024c --- a/src/FlightMap/Images/attitudeDial.svg +++ b/src/FlightMap/Images/attitudeDial.svg @@ -1,15 +1,23 @@ - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + diff --git a/src/FlightMap/Widgets/CenterMapDropButton.qml b/src/FlightMap/Widgets/CenterMapDropButton.qml index ca48c00e398a164fdcb5b336b4a847267a1534e9..5d9cca2551f5713c904232d7c93ddda89ec22a02 100644 --- a/src/FlightMap/Widgets/CenterMapDropButton.qml +++ b/src/FlightMap/Widgets/CenterMapDropButton.qml @@ -209,6 +209,17 @@ DropButton { } } + + QGCButton { + text: qsTr("Specified Location") + Layout.fillWidth: true + + onClicked: { + dropButton.hideDropDown() + map.centerToSpecifiedLocation() + } + } + QGCButton { text: qsTr("Vehicle") Layout.fillWidth: true diff --git a/src/FlightMap/Widgets/CenterMapDropPanel.qml b/src/FlightMap/Widgets/CenterMapDropPanel.qml index 90dd02b3bc0c7af12b0dd4d234ba869915b395ff..db60b88547082b01231ef5add3e8603d65272298 100644 --- a/src/FlightMap/Widgets/CenterMapDropPanel.qml +++ b/src/FlightMap/Widgets/CenterMapDropPanel.qml @@ -62,6 +62,17 @@ ColumnLayout { } } + QGCButton { + text: qsTr("Vehicle") + Layout.fillWidth: true + enabled: _activeVehicle && _activeVehicle.coordinate.isValid + + onClicked: { + dropPanel.hide() + map.center = activeVehicle.coordinate + } + } + QGCButton { text: qsTr("Current Location") Layout.fillWidth: true @@ -74,13 +85,12 @@ ColumnLayout { } QGCButton { - text: qsTr("Vehicle") + text: qsTr("Specified Location") Layout.fillWidth: true - enabled: _activeVehicle && _activeVehicle.coordinate.isValid onClicked: { dropPanel.hide() - map.center = activeVehicle.coordinate + map.centerToSpecifiedLocation() } } } // Column diff --git a/src/FlightMap/Widgets/MapFitFunctions.qml b/src/FlightMap/Widgets/MapFitFunctions.qml index c35caa38c12d4cfeddab997f6a65323aeffa426a..c6336514fddfa7efbdcd849986f442466652dc7d 100644 --- a/src/FlightMap/Widgets/MapFitFunctions.qml +++ b/src/FlightMap/Widgets/MapFitFunctions.qml @@ -90,10 +90,6 @@ Item { } function addMissionItemCoordsForFit(coordList) { - var homePosition = fitHomePosition() - if (homePosition.isValid) { - coordList.push(homePosition) - } for (var i=1; i<_missionController.visualItems.count; i++) { var missionItem = _missionController.visualItems.get(i) if (missionItem.specifiesCoordinate && !missionItem.isStandaloneCoordinate) { diff --git a/src/FlightMap/Widgets/ValuePageWidget.qml b/src/FlightMap/Widgets/ValuePageWidget.qml index b68146cddc25713a12f2b5ac78d3dc5a79987aa1..c334a96167b8a2839f59e6e1c0d2d0542927d51b 100644 --- a/src/FlightMap/Widgets/ValuePageWidget.qml +++ b/src/FlightMap/Widgets/ValuePageWidget.qml @@ -134,7 +134,7 @@ Column { QGCFlickable { anchors.fill: parent - contentHeight: _loader.y + _loader.height + contentHeight: column.height flickableDirection: Flickable.VerticalFlick clip: true @@ -169,13 +169,26 @@ Column { } Loader { - id: _loader anchors.left: parent.left anchors.right: parent.right sourceComponent: factGroupList property var factGroup: _activeVehicle - property string factGroupName: "Vehicle" + property string factGroupName: qsTr("Vehicle") + } + + Repeater { + model: _activeVehicle.factGroupNames + + Loader { + anchors.left: parent.left + anchors.right: parent.right + sourceComponent: factGroupList + + property var factGroup: _activeVehicle.getFactGroup(modelData) + property string factGroupName: modelData + } + } } } @@ -190,91 +203,92 @@ Column { // property string factGroupName Column { - id: _root spacing: _margins - QGCLabel { - width: parent.width - wrapMode: Text.WordWrap - text: factGroup ? factGroupName : qsTr("Vehicle must be connected to assign values.") + SectionHeader { + id: header + anchors.left: parent.left + anchors.right: parent.right + text: factGroupName.charAt(0).toUpperCase() + factGroupName.slice(1) + checked: false } - Repeater { - model: factGroup ? factGroup.factNames : 0 + Column { + spacing: _margins + visible: header.checked + + Repeater { + model: factGroup ? factGroup.factNames : 0 - RowLayout { - spacing: _margins + RowLayout { + spacing: _margins - property string propertyName: factGroupName + "." + modelData + property string propertyName: factGroupName + "." + modelData - function removeFromList(list, value) { - var newList = [] - for (var i=0; isettingsManager()->appSettings()->followTarget(), &Fact::rawValueChanged, this, &FollowMe::_settingsChanged); + _currentMode = _toolbox->settingsManager()->appSettings()->followTarget()->rawValue().toUInt(); + if(_currentMode == MODE_ALWAYS) { + _enable(); + } } void FollowMe::followMeHandleManager(const QString&) { - QmlObjectListModel & vehicles = *_toolbox->multiVehicleManager()->vehicles(); - - for (int i=0; i< vehicles.count(); i++) { - Vehicle* vehicle = qobject_cast(vehicles[i]); - if (vehicle->px4Firmware() && vehicle->flightMode().compare(FirmwarePlugin::px4FollowMeFlightMode, Qt::CaseInsensitive) == 0) { - _enable(); - return; + //-- If we are to change based on current flight mode + if(_currentMode == MODE_FOLLOWME) { + QmlObjectListModel & vehicles = *_toolbox->multiVehicleManager()->vehicles(); + for (int i=0; i< vehicles.count(); i++) { + Vehicle* vehicle = qobject_cast(vehicles[i]); + if (vehicle->px4Firmware() && vehicle->flightMode().compare(FirmwarePlugin::px4FollowMeFlightMode, Qt::CaseInsensitive) == 0) { + _enable(); + return; + } } + _disable(); } +} - _disable(); +void FollowMe::_settingsChanged() +{ + uint32_t mode = _toolbox->settingsManager()->appSettings()->followTarget()->rawValue().toUInt(); + if(_currentMode != mode) { + _currentMode = mode; + switch (mode) { + case MODE_NEVER: + if(_gcsMotionReportTimer.isActive()) { + _disable(); + } + break; + case MODE_ALWAYS: + if(!_gcsMotionReportTimer.isActive()) { + _enable(); + } + break; + case MODE_FOLLOWME: + if(!_gcsMotionReportTimer.isActive()) { + followMeHandleManager(QString()); + } + break; + default: + break; + } + } } void FollowMe::_enable() @@ -48,7 +87,6 @@ void FollowMe::_enable() SIGNAL(positionInfoUpdated(QGeoPositionInfo)), this, SLOT(_setGPSLocation(QGeoPositionInfo))); - _gcsMotionReportTimer.setInterval(_toolbox->qgcPositionManager()->updateInterval()); _gcsMotionReportTimer.start(); } @@ -59,21 +97,22 @@ void FollowMe::_disable() SIGNAL(positionInfoUpdated(QGeoPositionInfo)), this, SLOT(_setGPSLocation(QGeoPositionInfo))); - _gcsMotionReportTimer.stop(); } void FollowMe::_setGPSLocation(QGeoPositionInfo geoPositionInfo) { - if (geoPositionInfo.isValid()) - { + if (!geoPositionInfo.isValid()) { + //-- Invalidate cached coordinates + memset(&_motionReport, 0, sizeof(motionReport_s)); + } else { // get the current location coordinates QGeoCoordinate geoCoordinate = geoPositionInfo.coordinate(); - _motionReport.lat_int = geoCoordinate.latitude()*1e7; - _motionReport.lon_int = geoCoordinate.longitude()*1e7; - _motionReport.alt = geoCoordinate.altitude(); + _motionReport.lat_int = geoCoordinate.latitude() * 1e7; + _motionReport.lon_int = geoCoordinate.longitude() * 1e7; + _motionReport.alt = geoCoordinate.altitude(); estimatation_capabilities |= (1 << POS); @@ -101,7 +140,7 @@ void FollowMe::_setGPSLocation(QGeoPositionInfo geoPositionInfo) estimatation_capabilities |= (1 << VEL); qreal direction = _degreesToRadian(geoPositionInfo.attribute(QGeoPositionInfo::Direction)); - qreal velocity = geoPositionInfo.attribute(QGeoPositionInfo::GroundSpeed); + qreal velocity = geoPositionInfo.attribute(QGeoPositionInfo::GroundSpeed); _motionReport.vx = cos(direction)*velocity; _motionReport.vy = sin(direction)*velocity; @@ -113,15 +152,20 @@ void FollowMe::_setGPSLocation(QGeoPositionInfo geoPositionInfo) } } -void FollowMe::_sendGCSMotionReport(void) +void FollowMe::_sendGCSMotionReport() { - QmlObjectListModel & vehicles = *_toolbox->multiVehicleManager()->vehicles(); + + //-- Do we have any real data? + if(_motionReport.lat_int == 0 && _motionReport.lon_int == 0 && _motionReport.alt == 0) { + return; + } + + QmlObjectListModel & vehicles = *_toolbox->multiVehicleManager()->vehicles(); MAVLinkProtocol* mavlinkProtocol = _toolbox->mavlinkProtocol(); mavlink_follow_target_t follow_target; - memset(&follow_target, 0, sizeof(follow_target)); - follow_target.timestamp = runTime.nsecsElapsed()*1e-6; + follow_target.timestamp = runTime.nsecsElapsed() * 1e-6; follow_target.est_capabilities = estimatation_capabilities; follow_target.position_cov[0] = _motionReport.pos_std_dev[0]; follow_target.position_cov[2] = _motionReport.pos_std_dev[2]; @@ -133,7 +177,7 @@ void FollowMe::_sendGCSMotionReport(void) for (int i=0; i< vehicles.count(); i++) { Vehicle* vehicle = qobject_cast(vehicles[i]); - if(vehicle->flightMode().compare(FirmwarePlugin::px4FollowMeFlightMode, Qt::CaseInsensitive) == 0) { + if(_currentMode || vehicle->flightMode().compare(FirmwarePlugin::px4FollowMeFlightMode, Qt::CaseInsensitive) == 0) { mavlink_message_t message; mavlink_msg_follow_target_encode_chan(mavlinkProtocol->getSystemId(), mavlinkProtocol->getComponentId(), diff --git a/src/FollowMe/FollowMe.h b/src/FollowMe/FollowMe.h index 541df1112c4ed67f7d2621cef9ea95382f4fad6d..4b744a538343fb32e04e623beb6b8f7f1afa7d2d 100644 --- a/src/FollowMe/FollowMe.h +++ b/src/FollowMe/FollowMe.h @@ -29,12 +29,15 @@ class FollowMe : public QGCTool public: FollowMe(QGCApplication* app, QGCToolbox* toolbox); + void setToolbox(QGCToolbox* toolbox) override; + public slots: - void followMeHandleManager(const QString&); + void followMeHandleManager (const QString&); private slots: - void _setGPSLocation(QGeoPositionInfo geoPositionInfo); - void _sendGCSMotionReport(void); + void _setGPSLocation (QGeoPositionInfo geoPositionInfo); + void _sendGCSMotionReport (); + void _settingsChanged (); private: QElapsedTimer runTime; @@ -65,8 +68,16 @@ private: uint8_t estimatation_capabilities; - void _disable(); - void _enable(); + void _disable (); + void _enable (); + + double _degreesToRadian(double deg); + + enum { + MODE_NEVER, + MODE_ALWAYS, + MODE_FOLLOWME + }; - double _degreesToRadian(double deg); + uint32_t _currentMode; }; diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index 04d51ccf348baad2f900c5659e8f3e72abbe1df7..b14f810ea68b3885177c9519f1bc85959d570f49 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -25,6 +25,8 @@ const char* Joystick::_throttleModeSettingsKey = "ThrottleMode"; const char* Joystick::_exponentialSettingsKey = "Exponential"; const char* Joystick::_accumulatorSettingsKey = "Accumulator"; const char* Joystick::_deadbandSettingsKey = "Deadband"; +const char* Joystick::_circleCorrectionSettingsKey = "Circle_Correction"; +const char* Joystick::_frequencySettingsKey = "Frequency"; const char* Joystick::_txModeSettingsKey = NULL; const char* Joystick::_fixedWingTXModeSettingsKey = "TXMode_FixedWing"; const char* Joystick::_multiRotorTXModeSettingsKey = "TXMode_MultiRotor"; @@ -32,6 +34,11 @@ const char* Joystick::_roverTXModeSettingsKey = "TXMode_Rover"; const char* Joystick::_vtolTXModeSettingsKey = "TXMode_VTOL"; const char* Joystick::_submarineTXModeSettingsKey = "TXMode_Submarine"; +const char* Joystick::_buttonActionArm = QT_TR_NOOP("Arm"); +const char* Joystick::_buttonActionDisarm = QT_TR_NOOP("Disarm"); +const char* Joystick::_buttonActionVTOLFixedWing = QT_TR_NOOP("VTOL: Fixed Wing"); +const char* Joystick::_buttonActionVTOLMultiRotor = QT_TR_NOOP("VTOL: Multi-Rotor"); + const char* Joystick::_rgFunctionSettingsKey[Joystick::maxFunction] = { "RollAxis", "PitchAxis", @@ -59,6 +66,8 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC , _exponential(0) , _accumulator(false) , _deadband(false) + , _circleCorrection(true) + , _frequency(25.0f) , _activeVehicle(NULL) , _pollingStartedForCalibration(false) , _multiVehicleManager(multiVehicleManager) @@ -84,6 +93,10 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC Joystick::~Joystick() { + // Crash out of the thread if it is still running + terminate(); + wait(); + delete[] _rgAxisValues; delete[] _rgCalibration; delete[] _rgButtonValues; @@ -115,6 +128,8 @@ void Joystick::_setDefaultCalibration(void) { _exponential = 0; _accumulator = false; _deadband = false; + _circleCorrection = false; + _frequency = 25.0f; _throttleMode = ThrottleModeCenterZero; _calibrated = true; @@ -179,6 +194,8 @@ void Joystick::_loadSettings(void) _exponential = settings.value(_exponentialSettingsKey, 0).toFloat(); _accumulator = settings.value(_accumulatorSettingsKey, false).toBool(); _deadband = settings.value(_deadbandSettingsKey, false).toBool(); + _circleCorrection = settings.value(_circleCorrectionSettingsKey, false).toBool(); + _frequency = settings.value(_frequencySettingsKey, 25.0f).toFloat(); _throttleMode = (ThrottleMode_t)settings.value(_throttleModeSettingsKey, ThrottleModeCenterZero).toInt(&convertOk); badSettings |= !convertOk; @@ -255,9 +272,11 @@ void Joystick::_saveSettings(void) settings.setValue(_exponentialSettingsKey, _exponential); settings.setValue(_accumulatorSettingsKey, _accumulator); settings.setValue(_deadbandSettingsKey, _deadband); + settings.setValue(_circleCorrectionSettingsKey, _circleCorrection); + settings.setValue(_frequencySettingsKey, _frequency); settings.setValue(_throttleModeSettingsKey, _throttleMode); - qCDebug(JoystickLog) << "_saveSettings calibrated:throttlemode:deadband:txmode" << _calibrated << _throttleMode << _deadband << _transmitterMode; + qCDebug(JoystickLog) << "_saveSettings calibrated:throttlemode:deadband:txmode" << _calibrated << _throttleMode << _deadband << _circleCorrection << _transmitterMode; QString minTpl ("Axis%1Min"); QString maxTpl ("Axis%1Max"); @@ -431,7 +450,7 @@ void Joystick::run(void) } } - if (_outputEnabled && _calibrated) { + if (_activeVehicle->joystickEnabled() && !_calibrationMode && _calibrated) { int axis = _rgFunctionAxis[rollFunction]; float roll = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); @@ -453,17 +472,19 @@ void Joystick::run(void) throttle = throttle_accu; } - float roll_limited = std::max(static_cast(-M_PI_4), std::min(roll, static_cast(M_PI_4))); - float pitch_limited = std::max(static_cast(-M_PI_4), std::min(pitch, static_cast(M_PI_4))); - float yaw_limited = std::max(static_cast(-M_PI_4), std::min(yaw, static_cast(M_PI_4))); - float throttle_limited = std::max(static_cast(-M_PI_4), std::min(throttle, static_cast(M_PI_4))); - - // Map from unit circle to linear range and limit - roll = std::max(-1.0f, std::min(tanf(asinf(roll_limited)), 1.0f)); - pitch = std::max(-1.0f, std::min(tanf(asinf(pitch_limited)), 1.0f)); - yaw = std::max(-1.0f, std::min(tanf(asinf(yaw_limited)), 1.0f)); - throttle = std::max(-1.0f, std::min(tanf(asinf(throttle_limited)), 1.0f)); - + if ( _circleCorrection ) { + float roll_limited = std::max(static_cast(-M_PI_4), std::min(roll, static_cast(M_PI_4))); + float pitch_limited = std::max(static_cast(-M_PI_4), std::min(pitch, static_cast(M_PI_4))); + float yaw_limited = std::max(static_cast(-M_PI_4), std::min(yaw, static_cast(M_PI_4))); + float throttle_limited = std::max(static_cast(-M_PI_4), std::min(throttle, static_cast(M_PI_4))); + + // Map from unit circle to linear range and limit + roll = std::max(-1.0f, std::min(tanf(asinf(roll_limited)), 1.0f)); + pitch = std::max(-1.0f, std::min(tanf(asinf(pitch_limited)), 1.0f)); + yaw = std::max(-1.0f, std::min(tanf(asinf(yaw_limited)), 1.0f)); + throttle = std::max(-1.0f, std::min(tanf(asinf(throttle_limited)), 1.0f)); + } + if ( _exponential != 0 ) { // Exponential (0% to -50% range like most RC radios) //_exponential is set by a slider in joystickConfig.qml @@ -526,8 +547,9 @@ void Joystick::run(void) emit manualControl(roll, -pitch, yaw, throttle, buttonPressedBits, _activeVehicle->joystickMode()); } - // Sleep, update rate of joystick is approx. 25 Hz (1000 ms / 25 = 40 ms) - QGC::SLEEP::msleep(40); + // Sleep. Update rate of joystick is by default 25 Hz + int mswait = (int)(1000.0f / _frequency); + QGC::SLEEP::msleep(mswait); } _close(); @@ -638,11 +660,12 @@ QStringList Joystick::actions(void) { QStringList list; - list << "Arm" << "Disarm"; + list << _buttonActionArm << _buttonActionDisarm; if (_activeVehicle) { list << _activeVehicle->flightModes(); } + list << _buttonActionVTOLFixedWing << _buttonActionVTOLMultiRotor; return list; } @@ -757,6 +780,34 @@ void Joystick::setDeadband(bool deadband) _saveSettings(); } +bool Joystick::circleCorrection(void) +{ + return _circleCorrection; +} + +void Joystick::setCircleCorrection(bool circleCorrection) +{ + _circleCorrection = circleCorrection; + + _saveSettings(); + emit circleCorrectionChanged(_circleCorrection); +} + +float Joystick::frequency() +{ + return _frequency; +} + +void Joystick::setFrequency(float val) +{ + //-- Arbitrary limits + if(val < 0.25f) val = 0.25f; + if(val > 100.0f) val = 100.0f; + _frequency = val; + _saveSettings(); + emit frequencyChanged(); +} + void Joystick::setCalibrationMode(bool calibrating) { _calibrationMode = calibrating; @@ -768,18 +819,8 @@ void Joystick::setCalibrationMode(bool calibrating) else if (_pollingStartedForCalibration) { stopPolling(); } - if (calibrating){ - setOutputEnabled(false); //Disable the joystick output before calibrating - } - else if (!calibrating && _calibrated){ - setOutputEnabled(true); //Enable joystick output after calibration - } } -void Joystick::setOutputEnabled(bool enabled){ - _outputEnabled = enabled; - emit outputEnabledChanged(_outputEnabled); -} void Joystick::_buttonAction(const QString& action) { @@ -787,10 +828,14 @@ void Joystick::_buttonAction(const QString& action) return; } - if (action == "Arm") { + if (action == _buttonActionArm) { _activeVehicle->setArmed(true); - } else if (action == "Disarm") { + } else if (action == _buttonActionDisarm) { _activeVehicle->setArmed(false); + } else if (action == _buttonActionVTOLFixedWing) { + _activeVehicle->setVtolInFwdFlight(true); + } else if (action == _buttonActionVTOLMultiRotor) { + _activeVehicle->setVtolInFwdFlight(false); } else if (_activeVehicle->flightModes().contains(action)) { _activeVehicle->setFlightMode(action); } else { diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index f90b8169b84440479e866a11ee7de3cf96f8ccf8..e6e7aa9eb8d22e14f067d6d59771ced599d4115a 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -61,7 +61,6 @@ public: Q_PROPERTY(QString name READ name CONSTANT) Q_PROPERTY(bool calibrated MEMBER _calibrated NOTIFY calibratedChanged) - Q_PROPERTY(bool outputEnabled MEMBER _outputEnabled WRITE setOutputEnabled NOTIFY outputEnabledChanged) Q_PROPERTY(int totalButtonCount READ totalButtonCount CONSTANT) Q_PROPERTY(int axisCount READ axisCount CONSTANT) @@ -76,8 +75,10 @@ public: Q_PROPERTY(bool negativeThrust READ negativeThrust WRITE setNegativeThrust NOTIFY negativeThrustChanged) Q_PROPERTY(float exponential READ exponential WRITE setExponential NOTIFY exponentialChanged) Q_PROPERTY(bool accumulator READ accumulator WRITE setAccumulator NOTIFY accumulatorChanged) - Q_PROPERTY(bool requiresCalibration READ requiresCalibration CONSTANT) - + Q_PROPERTY(bool requiresCalibration READ requiresCalibration CONSTANT) + Q_PROPERTY(bool circleCorrection READ circleCorrection WRITE setCircleCorrection NOTIFY circleCorrectionChanged) + Q_PROPERTY(float frequency READ frequency WRITE setFrequency NOTIFY frequencyChanged) + // Property accessors int axisCount(void) { return _axisCount; } @@ -120,16 +121,20 @@ public: bool deadband(void); void setDeadband(bool accu); + bool circleCorrection(void); + void setCircleCorrection(bool circleCorrection); + void setTXMode(int mode); int getTXMode(void) { return _transmitterMode; } /// Set the current calibration mode void setCalibrationMode(bool calibrating); - void setOutputEnabled(bool enabled); + + float frequency(); + void setFrequency(float val); signals: void calibratedChanged(bool calibrated); - void outputEnabledChanged(bool enabled); // The raw signals are only meant for use by calibration void rawAxisValueChanged(int index, int value); @@ -147,6 +152,8 @@ signals: void enabledChanged(bool enabled); + void circleCorrectionChanged(bool circleCorrection); + /// Signal containing new joystick information /// @param roll Range is -1:1, negative meaning roll left, positive meaning roll right /// @param pitch Range i -1:1, negative meaning pitch down, positive meaning pitch up @@ -157,6 +164,8 @@ signals: void buttonActionTriggered(int action); + void frequencyChanged(); + protected: void _setDefaultCalibration(void); void _saveSettings(void); @@ -196,7 +205,6 @@ protected: static int _transmitterMode; bool _calibrationMode; - bool _outputEnabled; int* _rgAxisValues; Calibration_t* _rgCalibration; @@ -213,6 +221,8 @@ protected: float _exponential; bool _accumulator; bool _deadband; + bool _circleCorrection; + float _frequency; Vehicle* _activeVehicle; bool _pollingStartedForCalibration; @@ -229,6 +239,8 @@ private: static const char* _exponentialSettingsKey; static const char* _accumulatorSettingsKey; static const char* _deadbandSettingsKey; + static const char* _circleCorrectionSettingsKey; + static const char* _frequencySettingsKey; static const char* _txModeSettingsKey; static const char* _fixedWingTXModeSettingsKey; static const char* _multiRotorTXModeSettingsKey; @@ -236,6 +248,11 @@ private: static const char* _vtolTXModeSettingsKey; static const char* _submarineTXModeSettingsKey; + static const char* _buttonActionArm; + static const char* _buttonActionDisarm; + static const char* _buttonActionVTOLFixedWing; + static const char* _buttonActionVTOLMultiRotor; + private slots: void _activeVehicleChanged(Vehicle* activeVehicle); }; diff --git a/src/JsonHelper.cc b/src/JsonHelper.cc index e7d47a6187a94fc4ab8288b64d3e7dcd8e8d449a..94858a42c6c68b1efc40875e43022e5bf6ceae46 100644 --- a/src/JsonHelper.cc +++ b/src/JsonHelper.cc @@ -97,7 +97,7 @@ bool JsonHelper::validateKeyTypes(const QJsonObject& jsonObject, const QStringLi QString valueKey = keys[i]; if (jsonObject.contains(valueKey)) { const QJsonValue& jsonValue = jsonObject[valueKey]; - if (types[i] == QJsonValue::Null && jsonValue.type() == QJsonValue::Double) { + if (jsonValue.type() == QJsonValue::Null && types[i] == QJsonValue::Double) { // Null type signals a NaN on a double value continue; } diff --git a/src/JsonHelper.h b/src/JsonHelper.h index 955392bd03eec2b868b45b13ce36fc34c1490a8a..c540cdb90edec0dfba26301930f7884f5b350825 100644 --- a/src/JsonHelper.h +++ b/src/JsonHelper.h @@ -106,7 +106,7 @@ public: static bool parseEnum(const QJsonObject& jsonObject, QStringList& enumStrings, QStringList& enumValues, QString& errorString, QString valueName = QString()); - /// Returns NaN if the value is null, or it not the double value + /// Returns NaN if the value is null, or if not, the double value static double possibleNaNJsonValue(const QJsonValue& value); static const char* jsonVersionKey; diff --git a/src/KMLFileHelper.cc b/src/KMLFileHelper.cc new file mode 100644 index 0000000000000000000000000000000000000000..7415c4d3ae6b4d2add6065fb81f66982ff04a2f2 --- /dev/null +++ b/src/KMLFileHelper.cc @@ -0,0 +1,176 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "KMLFileHelper.h" + +#include + +QDomDocument KMLFileHelper::loadFile(const QString& kmlFile, QString& errorString) +{ + QFile file(kmlFile); + + errorString.clear(); + + if (!file.exists()) { + errorString = tr("File not found: %1").arg(kmlFile); + return QDomDocument(); + } + + if (!file.open(QIODevice::ReadOnly)) { + errorString = tr("Unable to open file: %1 error: $%2").arg(kmlFile).arg(file.errorString()); + return QDomDocument(); + } + + QDomDocument doc; + QString errorMessage; + int errorLine; + if (!doc.setContent(&file, &errorMessage, &errorLine)) { + errorString = tr("Unable to parse KML file: %1 error: %2 line: %3").arg(kmlFile).arg(errorMessage).arg(errorLine); + return QDomDocument(); + } + + return doc; +} + +QVariantList KMLFileHelper::determineFileContents(const QString& kmlFile) +{ + QString errorString; + KMLFileContents fileContents = determineFileContents(kmlFile, errorString); + + QVariantList varList; + varList.append(QVariant::fromValue(fileContents)); + varList.append(QVariant::fromValue(errorString)); + + return varList; +} + +KMLFileHelper::KMLFileContents KMLFileHelper::determineFileContents(const QString& kmlFile, QString& errorString) +{ + QDomDocument domDocument = KMLFileHelper::loadFile(kmlFile, errorString); + if (!errorString.isEmpty()) { + return Error; + } + + QDomNodeList rgNodes = domDocument.elementsByTagName("Polygon"); + if (rgNodes.count()) { + return Polygon; + } + + rgNodes = domDocument.elementsByTagName("LineString"); + if (rgNodes.count()) { + return Polyline; + } + + errorString = tr("No known type found in KML file."); + return Error; +} + +bool KMLFileHelper::loadPolygonFromFile(const QString& kmlFile, QList& vertices, QString& errorString) +{ + errorString.clear(); + vertices.clear(); + + QDomDocument domDocument = KMLFileHelper::loadFile(kmlFile, errorString); + if (!errorString.isEmpty()) { + return false; + } + + QDomNodeList rgNodes = domDocument.elementsByTagName("Polygon"); + if (rgNodes.count() == 0) { + errorString = tr("Unable to find Polygon node in KML"); + return false; + } + + QDomNode coordinatesNode = rgNodes.item(0).namedItem("outerBoundaryIs").namedItem("LinearRing").namedItem("coordinates"); + if (coordinatesNode.isNull()) { + errorString = tr("Internal error: Unable to find coordinates node in KML"); + return false; + } + + QString coordinatesString = coordinatesNode.toElement().text().simplified(); + QStringList rgCoordinateStrings = coordinatesString.split(" "); + + QList rgCoords; + for (int i=0; i rgReversed; + + for (int i=0; i& coords, QString& errorString) +{ + errorString.clear(); + coords.clear(); + + QDomDocument domDocument = KMLFileHelper::loadFile(kmlFile, errorString); + if (!errorString.isEmpty()) { + return false; + } + + QDomNodeList rgNodes = domDocument.elementsByTagName("LineString"); + if (rgNodes.count() == 0) { + errorString = tr("Unable to find LineString node in KML"); + return false; + } + + QDomNode coordinatesNode = rgNodes.item(0).namedItem("coordinates"); + if (coordinatesNode.isNull()) { + errorString = tr("Internal error: Unable to find coordinates node in KML"); + return false; + } + + QString coordinatesString = coordinatesNode.toElement().text().simplified(); + QStringList rgCoordinateStrings = coordinatesString.split(" "); + + QList rgCoords; + for (int i=0; i + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include + +/// The QGCMapPolygon class provides a polygon which can be displayed on a map using a map visuals control. +/// It maintains a representation of the polygon on QVariantList and QmlObjectListModel format. +class KMLFileHelper : public QObject +{ + Q_OBJECT + +public: + enum KMLFileContents { + Polygon, + Polyline, + Error + }; + Q_ENUM(KMLFileContents) + + Q_INVOKABLE static QVariantList determineFileContents(const QString& kmlFile); + + static KMLFileContents determineFileContents(const QString& kmlFile, QString& errorString); + static QDomDocument loadFile(const QString& kmlFile, QString& errorString); + static bool loadPolygonFromFile(const QString& kmlFile, QList& vertices, QString& errorString); + static bool loadPolylineFromFile(const QString& kmlFile, QList& coords, QString& errorString); +}; diff --git a/src/MissionManager/CameraCalc.FactMetaData.json b/src/MissionManager/CameraCalc.FactMetaData.json index 7df18d0999f5d69b691a9047af7549c145e867ba..9860f5491bae0df69b62cdacc79424f00a83fcc6 100644 --- a/src/MissionManager/CameraCalc.FactMetaData.json +++ b/src/MissionManager/CameraCalc.FactMetaData.json @@ -1,4 +1,10 @@ [ +{ + "name": "CameraName", + "shortDescription": "Camera name.", + "type": "string", + "defaultValue": "Manual (no camera specs)" +}, { "name": "ValueSetIsDistance", "shortDescription": "Value specified is distance to surface.", diff --git a/src/MissionManager/CameraCalc.cc b/src/MissionManager/CameraCalc.cc index a1efa9c5e71c18a8cf119f9e5b9e960bfedb3a5f..9b50dd285c186d4cf9c788fb54db0b548747bcd7 100644 --- a/src/MissionManager/CameraCalc.cc +++ b/src/MissionManager/CameraCalc.cc @@ -14,46 +14,39 @@ #include -const char* CameraCalc::_valueSetIsDistanceName = "ValueSetIsDistance"; -const char* CameraCalc::_distanceToSurfaceName = "DistanceToSurface"; -const char* CameraCalc::_distanceToSurfaceRelativeName = "DistanceToSurfaceRelative"; -const char* CameraCalc::_imageDensityName = "ImageDensity"; -const char* CameraCalc::_frontalOverlapName = "FrontalOverlap"; -const char* CameraCalc::_sideOverlapName = "SideOverlap"; -const char* CameraCalc::_adjustedFootprintFrontalName = "AdjustedFootprintFrontal"; -const char* CameraCalc::_adjustedFootprintSideName = "AdjustedFootprintSide"; -const char* CameraCalc::_jsonCameraNameKey = "CameraName"; +const char* CameraCalc::cameraNameName = "CameraName"; +const char* CameraCalc::valueSetIsDistanceName = "ValueSetIsDistance"; +const char* CameraCalc::distanceToSurfaceName = "DistanceToSurface"; +const char* CameraCalc::distanceToSurfaceRelativeName = "DistanceToSurfaceRelative"; +const char* CameraCalc::imageDensityName = "ImageDensity"; +const char* CameraCalc::frontalOverlapName = "FrontalOverlap"; +const char* CameraCalc::sideOverlapName = "SideOverlap"; +const char* CameraCalc::adjustedFootprintFrontalName = "AdjustedFootprintFrontal"; +const char* CameraCalc::adjustedFootprintSideName = "AdjustedFootprintSide"; + const char* CameraCalc::_jsonCameraSpecTypeKey = "CameraSpecType"; -CameraCalc::CameraCalc(Vehicle* vehicle, QObject* parent) +CameraCalc::CameraCalc(Vehicle* vehicle, QString settingsGroup, QObject* parent) : CameraSpec (parent) , _vehicle (vehicle) , _dirty (false) - , _cameraName (manualCameraName()) , _disableRecalc (false) , _distanceToSurfaceRelative (true) , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/CameraCalc.FactMetaData.json"), this)) - , _valueSetIsDistanceFact (0, _valueSetIsDistanceName, FactMetaData::valueTypeBool) - , _distanceToSurfaceFact (0, _distanceToSurfaceName, FactMetaData::valueTypeDouble) - , _imageDensityFact (0, _imageDensityName, FactMetaData::valueTypeDouble) - , _frontalOverlapFact (0, _frontalOverlapName, FactMetaData::valueTypeDouble) - , _sideOverlapFact (0, _sideOverlapName, FactMetaData::valueTypeDouble) - , _adjustedFootprintSideFact (0, _adjustedFootprintSideName, FactMetaData::valueTypeDouble) - , _adjustedFootprintFrontalFact (0, _adjustedFootprintFrontalName, FactMetaData::valueTypeDouble) + , _cameraNameFact (settingsGroup, _metaDataMap[cameraNameName]) + , _valueSetIsDistanceFact (settingsGroup, _metaDataMap[valueSetIsDistanceName]) + , _distanceToSurfaceFact (settingsGroup, _metaDataMap[distanceToSurfaceName]) + , _imageDensityFact (settingsGroup, _metaDataMap[imageDensityName]) + , _frontalOverlapFact (settingsGroup, _metaDataMap[frontalOverlapName]) + , _sideOverlapFact (settingsGroup, _metaDataMap[sideOverlapName]) + , _adjustedFootprintSideFact (settingsGroup, _metaDataMap[adjustedFootprintSideName]) + , _adjustedFootprintFrontalFact (settingsGroup, _metaDataMap[adjustedFootprintFrontalName]) , _imageFootprintSide (0) , _imageFootprintFrontal (0) , _knownCameraList (_vehicle->staticCameraList()) { QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); - _valueSetIsDistanceFact.setMetaData (_metaDataMap[_valueSetIsDistanceName], true /* setDefaultFromMetaData */); - _distanceToSurfaceFact.setMetaData (_metaDataMap[_distanceToSurfaceName], true); - _imageDensityFact.setMetaData (_metaDataMap[_imageDensityName], true); - _frontalOverlapFact.setMetaData (_metaDataMap[_frontalOverlapName], true); - _sideOverlapFact.setMetaData (_metaDataMap[_sideOverlapName], true); - _adjustedFootprintSideFact.setMetaData (_metaDataMap[_adjustedFootprintSideName], true); - _adjustedFootprintFrontalFact.setMetaData (_metaDataMap[_adjustedFootprintFrontalName], true); - connect(&_valueSetIsDistanceFact, &Fact::valueChanged, this, &CameraCalc::_setDirty); connect(&_distanceToSurfaceFact, &Fact::valueChanged, this, &CameraCalc::_setDirty); connect(&_imageDensityFact, &Fact::valueChanged, this, &CameraCalc::_setDirty); @@ -61,19 +54,27 @@ CameraCalc::CameraCalc(Vehicle* vehicle, QObject* parent) connect(&_sideOverlapFact, &Fact::valueChanged, this, &CameraCalc::_setDirty); connect(&_adjustedFootprintSideFact, &Fact::valueChanged, this, &CameraCalc::_setDirty); connect(&_adjustedFootprintFrontalFact, &Fact::valueChanged, this, &CameraCalc::_setDirty); - connect(this, &CameraCalc::cameraNameChanged, this, &CameraCalc::_setDirty); + connect(&_cameraNameFact, &Fact::valueChanged, this, &CameraCalc::_setDirty); connect(this, &CameraCalc::distanceToSurfaceRelativeChanged, this, &CameraCalc::_setDirty); - connect(this, &CameraCalc::cameraNameChanged, this, &CameraCalc::_recalcTriggerDistance); - connect(this, &CameraCalc::cameraNameChanged, this, &CameraCalc::_adjustDistanceToSurfaceRelative); + connect(&_cameraNameFact, &Fact::valueChanged, this, &CameraCalc::_cameraNameChanged); + connect(&_cameraNameFact, &Fact::valueChanged, this, &CameraCalc::isManualCameraChanged); + connect(&_cameraNameFact, &Fact::valueChanged, this, &CameraCalc::isCustomCameraChanged); connect(&_distanceToSurfaceFact, &Fact::rawValueChanged, this, &CameraCalc::_recalcTriggerDistance); connect(&_imageDensityFact, &Fact::rawValueChanged, this, &CameraCalc::_recalcTriggerDistance); connect(&_frontalOverlapFact, &Fact::rawValueChanged, this, &CameraCalc::_recalcTriggerDistance); connect(&_sideOverlapFact, &Fact::rawValueChanged, this, &CameraCalc::_recalcTriggerDistance); + connect(sensorWidth(), &Fact::rawValueChanged, this, &CameraCalc::_recalcTriggerDistance); + connect(sensorHeight(), &Fact::rawValueChanged, this, &CameraCalc::_recalcTriggerDistance); + connect(imageWidth(), &Fact::rawValueChanged, this, &CameraCalc::_recalcTriggerDistance); + connect(imageHeight(), &Fact::rawValueChanged, this, &CameraCalc::_recalcTriggerDistance); + connect(focalLength(), &Fact::rawValueChanged, this, &CameraCalc::_recalcTriggerDistance); connect(landscape(), &Fact::rawValueChanged, this, &CameraCalc::_recalcTriggerDistance); - _recalcTriggerDistance(); + _cameraNameChanged(); + + setDirty(false); } void CameraCalc::setDirty(bool dirty) @@ -84,57 +85,64 @@ void CameraCalc::setDirty(bool dirty) } } -void CameraCalc::setCameraName(QString cameraName) +void CameraCalc::_cameraNameChanged(void) { - if (cameraName != _cameraName) { - _cameraName = cameraName; + if (_disableRecalc) { + return; + } + + QString cameraName = _cameraNameFact.rawValue().toString(); + + // Validate known camera name + bool foundKnownCamera = false; + CameraMetaData* cameraMetaData = NULL; + if (!isManualCamera() && !isCustomCamera()) { + for (int cameraIndex=0; cameraIndex<_knownCameraList.count(); cameraIndex++) { + cameraMetaData = _knownCameraList[cameraIndex].value(); + if (cameraName == cameraMetaData->name()) { + foundKnownCamera = true; + break; + } + } + + if (!foundKnownCamera) { + // This will cause another camera changed signal which will recurse back to this routine + _cameraNameFact.setRawValue(customCameraName()); + return; + } + } - if (_cameraName == manualCameraName() || _cameraName == customCameraName()) { + _disableRecalc = true; + if (foundKnownCamera) { + sensorWidth()->setRawValue (cameraMetaData->sensorWidth()); + sensorHeight()->setRawValue (cameraMetaData->sensorHeight()); + imageWidth()->setRawValue (cameraMetaData->imageWidth()); + imageHeight()->setRawValue (cameraMetaData->imageHeight()); + focalLength()->setRawValue (cameraMetaData->focalLength()); + landscape()->setRawValue (cameraMetaData->landscape()); + fixedOrientation()->setRawValue (cameraMetaData->fixedOrientation()); + minTriggerInterval()->setRawValue (cameraMetaData->minTriggerInterval()); + } else { + if (isManualCamera() || isCustomCamera()) { // These values are unknown for these types fixedOrientation()->setRawValue(false); minTriggerInterval()->setRawValue(0); - if (_cameraName == manualCameraName()) { + if (isManualCamera()) { valueSetIsDistance()->setRawValue(false); } } else { - // This should be a known camera name. Update camera specs to new camera - - bool foundKnownCamera = false; - CameraMetaData* cameraMetaData = NULL; - for (int cameraIndex=0; cameraIndex<_knownCameraList.count(); cameraIndex++) { - cameraMetaData = _knownCameraList[cameraIndex].value(); - if (cameraName == cameraMetaData->name()) { - foundKnownCamera = true; - break; - } - } - - _disableRecalc = true; - if (foundKnownCamera) { - sensorWidth()->setRawValue (cameraMetaData->sensorWidth()); - sensorHeight()->setRawValue (cameraMetaData->sensorHeight()); - imageWidth()->setRawValue (cameraMetaData->imageWidth()); - imageHeight()->setRawValue (cameraMetaData->imageHeight()); - focalLength()->setRawValue (cameraMetaData->focalLength()); - landscape()->setRawValue (cameraMetaData->landscape()); - fixedOrientation()->setRawValue (cameraMetaData->fixedOrientation()); - minTriggerInterval()->setRawValue (cameraMetaData->minTriggerInterval()); - } else { - // We don't know this camera, switch back to custom - _cameraName = customCameraName(); - fixedOrientation()->setRawValue(false); - minTriggerInterval()->setRawValue(0); - } - _disableRecalc = false; + qWarning() << "Internal Error: Not known camera, but now manual or custom either"; } - - emit cameraNameChanged(_cameraName); } + _disableRecalc = false; + + _recalcTriggerDistance(); + _adjustDistanceToSurfaceRelative(); } void CameraCalc::_recalcTriggerDistance(void) { - if (_disableRecalc || _cameraName == manualCameraName()) { + if (_disableRecalc || isManualCamera()) { return; } @@ -178,18 +186,18 @@ void CameraCalc::_recalcTriggerDistance(void) void CameraCalc::save(QJsonObject& json) const { json[JsonHelper::jsonVersionKey] = 1; - json[_adjustedFootprintSideName] = _adjustedFootprintSideFact.rawValue().toDouble(); - json[_adjustedFootprintFrontalName] = _adjustedFootprintFrontalFact.rawValue().toDouble(); - json[_distanceToSurfaceName] = _distanceToSurfaceFact.rawValue().toDouble(); - json[_distanceToSurfaceRelativeName] = _distanceToSurfaceRelative; - json[_jsonCameraNameKey] = _cameraName; + json[adjustedFootprintSideName] = _adjustedFootprintSideFact.rawValue().toDouble(); + json[adjustedFootprintFrontalName] = _adjustedFootprintFrontalFact.rawValue().toDouble(); + json[distanceToSurfaceName] = _distanceToSurfaceFact.rawValue().toDouble(); + json[distanceToSurfaceRelativeName] = _distanceToSurfaceRelative; + json[cameraNameName] = _cameraNameFact.rawValue().toString(); - if (_cameraName != manualCameraName()) { + if (!isManualCamera()) { CameraSpec::save(json); - json[_valueSetIsDistanceName] = _valueSetIsDistanceFact.rawValue().toBool(); - json[_imageDensityName] = _imageDensityFact.rawValue().toDouble(); - json[_frontalOverlapName] = _frontalOverlapFact.rawValue().toDouble(); - json[_sideOverlapName] = _sideOverlapFact.rawValue().toDouble(); + json[valueSetIsDistanceName] = _valueSetIsDistanceFact.rawValue().toBool(); + json[imageDensityName] = _imageDensityFact.rawValue().toDouble(); + json[frontalOverlapName] = _frontalOverlapFact.rawValue().toDouble(); + json[sideOverlapName] = _sideOverlapFact.rawValue().toDouble(); } } @@ -204,9 +212,9 @@ bool CameraCalc::load(const QJsonObject& json, QString& errorString) // _jsonCameraNameKey only set if CameraSpecKnown int cameraSpec = v1Json[_jsonCameraSpecTypeKey].toInt(CameraSpecNone); if (cameraSpec == CameraSpecCustom) { - v1Json[_jsonCameraNameKey] = customCameraName(); + v1Json[cameraNameName] = customCameraName(); } else if (cameraSpec == CameraSpecNone) { - v1Json[_jsonCameraNameKey] = manualCameraName(); + v1Json[cameraNameName] = manualCameraName(); } v1Json.remove(_jsonCameraSpecTypeKey); v1Json[JsonHelper::jsonVersionKey] = 1; @@ -219,11 +227,11 @@ bool CameraCalc::load(const QJsonObject& json, QString& errorString) } QList keyInfoList1 = { - { _jsonCameraNameKey, QJsonValue::String, true }, - { _adjustedFootprintSideName, QJsonValue::Double, true }, - { _adjustedFootprintFrontalName, QJsonValue::Double, true }, - { _distanceToSurfaceName, QJsonValue::Double, true }, - { _distanceToSurfaceRelativeName, QJsonValue::Bool, true }, + { cameraNameName, QJsonValue::String, true }, + { adjustedFootprintSideName, QJsonValue::Double, true }, + { adjustedFootprintFrontalName, QJsonValue::Double, true }, + { distanceToSurfaceName, QJsonValue::Double, true }, + { distanceToSurfaceRelativeName, QJsonValue::Bool, true }, }; if (!JsonHelper::validateKeys(v1Json, keyInfoList1, errorString)) { return false; @@ -231,29 +239,29 @@ bool CameraCalc::load(const QJsonObject& json, QString& errorString) _disableRecalc = true; - setCameraName(v1Json[_jsonCameraNameKey].toString()); - _adjustedFootprintSideFact.setRawValue (v1Json[_adjustedFootprintSideName].toDouble()); - _adjustedFootprintFrontalFact.setRawValue (v1Json[_adjustedFootprintFrontalName].toDouble()); - _distanceToSurfaceFact.setRawValue (v1Json[_distanceToSurfaceName].toDouble()); + _cameraNameFact.setRawValue (v1Json[cameraNameName].toString()); + _adjustedFootprintSideFact.setRawValue (v1Json[adjustedFootprintSideName].toDouble()); + _adjustedFootprintFrontalFact.setRawValue (v1Json[adjustedFootprintFrontalName].toDouble()); + _distanceToSurfaceFact.setRawValue (v1Json[distanceToSurfaceName].toDouble()); - _distanceToSurfaceRelative = v1Json[_distanceToSurfaceRelativeName].toBool(); + _distanceToSurfaceRelative = v1Json[distanceToSurfaceRelativeName].toBool(); - if (_cameraName != manualCameraName()) { + if (!isManualCamera()) { QList keyInfoList2 = { - { _valueSetIsDistanceName, QJsonValue::Bool, true }, - { _imageDensityName, QJsonValue::Double, true }, - { _frontalOverlapName, QJsonValue::Double, true }, - { _sideOverlapName, QJsonValue::Double, true }, + { valueSetIsDistanceName, QJsonValue::Bool, true }, + { imageDensityName, QJsonValue::Double, true }, + { frontalOverlapName, QJsonValue::Double, true }, + { sideOverlapName, QJsonValue::Double, true }, }; if (!JsonHelper::validateKeys(v1Json, keyInfoList2, errorString)) { return false; _disableRecalc = false; } - _valueSetIsDistanceFact.setRawValue (v1Json[_valueSetIsDistanceName].toBool()); - _imageDensityFact.setRawValue (v1Json[_imageDensityName].toDouble()); - _frontalOverlapFact.setRawValue (v1Json[_frontalOverlapName].toDouble()); - _sideOverlapFact.setRawValue (v1Json[_sideOverlapName].toDouble()); + _valueSetIsDistanceFact.setRawValue (v1Json[valueSetIsDistanceName].toBool()); + _imageDensityFact.setRawValue (v1Json[imageDensityName].toDouble()); + _frontalOverlapFact.setRawValue (v1Json[frontalOverlapName].toDouble()); + _sideOverlapFact.setRawValue (v1Json[sideOverlapName].toDouble()); if (!CameraSpec::load(v1Json, errorString)) { return false; diff --git a/src/MissionManager/CameraCalc.h b/src/MissionManager/CameraCalc.h index 953114ec5b6cb3950404350a16660311e8605873..1b2339e89ff60d421b6f8107d87d5181d84c9196 100644 --- a/src/MissionManager/CameraCalc.h +++ b/src/MissionManager/CameraCalc.h @@ -10,6 +10,7 @@ #pragma once #include "CameraSpec.h" +#include "SettingsFact.h" class Vehicle; @@ -18,12 +19,13 @@ class CameraCalc : public CameraSpec Q_OBJECT public: - CameraCalc(Vehicle* vehicle, QObject* parent = NULL); + CameraCalc(Vehicle* vehicle, QString settingsGroup, QObject* parent = NULL); - Q_PROPERTY(QString cameraName READ cameraName WRITE setCameraName NOTIFY cameraNameChanged) Q_PROPERTY(QString customCameraName READ customCameraName CONSTANT) ///< Camera name for custom camera setting Q_PROPERTY(QString manualCameraName READ manualCameraName CONSTANT) ///< Camera name for manual camera setting - Q_PROPERTY(bool isManualCamera READ isManualCamera NOTIFY cameraNameChanged) ///< true: using manual camera + Q_PROPERTY(bool isManualCamera READ isManualCamera NOTIFY isManualCameraChanged) + Q_PROPERTY(bool isCustomCamera READ isCustomCamera NOTIFY isCustomCameraChanged) + Q_PROPERTY(Fact* cameraName READ cameraName CONSTANT) Q_PROPERTY(Fact* valueSetIsDistance READ valueSetIsDistance CONSTANT) ///< true: distance specified, resolution calculated Q_PROPERTY(Fact* distanceToSurface READ distanceToSurface CONSTANT) ///< Distance to surface for image foot print calculation Q_PROPERTY(Fact* imageDensity READ imageDensity CONSTANT) ///< Image density on surface (cm/px) @@ -39,9 +41,8 @@ public: static QString customCameraName(void); static QString manualCameraName(void); - QString cameraName(void) const { return _cameraName; } - void setCameraName(QString cameraName); + Fact* cameraName (void) { return &_cameraNameFact; } Fact* valueSetIsDistance (void) { return &_valueSetIsDistanceFact; } Fact* distanceToSurface (void) { return &_distanceToSurfaceFact; } Fact* imageDensity (void) { return &_imageDensityFact; } @@ -59,7 +60,8 @@ public: const Fact* adjustedFootprintFrontal (void) const { return &_adjustedFootprintFrontalFact; } bool dirty (void) const { return _dirty; } - bool isManualCamera (void) { return cameraName() == manualCameraName(); } + bool isManualCamera (void) const { return _cameraNameFact.rawValue().toString() == manualCameraName(); } + bool isCustomCamera (void) const { return _cameraNameFact.rawValue().toString() == customCameraName(); } double imageFootprintSide (void) const { return _imageFootprintSide; } double imageFootprintFrontal (void) const { return _imageFootprintFrontal; } bool distanceToSurfaceRelative (void) const { return _distanceToSurfaceRelative; } @@ -70,50 +72,52 @@ public: void save(QJsonObject& json) const; bool load(const QJsonObject& json, QString& errorString); + static const char* cameraNameName; + static const char* valueSetIsDistanceName; + static const char* distanceToSurfaceName; + static const char* distanceToSurfaceRelativeName; + static const char* imageDensityName; + static const char* frontalOverlapName; + static const char* sideOverlapName; + static const char* adjustedFootprintSideName; + static const char* adjustedFootprintFrontalName; + signals: - void cameraNameChanged (QString cameraName); void dirtyChanged (bool dirty); void imageFootprintSideChanged (double imageFootprintSide); void imageFootprintFrontalChanged (double imageFootprintFrontal); void distanceToSurfaceRelativeChanged (bool distanceToSurfaceRelative); + void isManualCameraChanged (void); + void isCustomCameraChanged (void); private slots: void _recalcTriggerDistance (void); void _adjustDistanceToSurfaceRelative (void); void _setDirty (void); + void _cameraNameChanged (void); private: Vehicle* _vehicle; bool _dirty; - QString _cameraName; bool _disableRecalc; bool _distanceToSurfaceRelative; QMap _metaDataMap; - Fact _valueSetIsDistanceFact; - Fact _distanceToSurfaceFact; - Fact _imageDensityFact; - Fact _frontalOverlapFact; - Fact _sideOverlapFact; - Fact _adjustedFootprintSideFact; - Fact _adjustedFootprintFrontalFact; + SettingsFact _cameraNameFact; + SettingsFact _valueSetIsDistanceFact; + SettingsFact _distanceToSurfaceFact; + SettingsFact _imageDensityFact; + SettingsFact _frontalOverlapFact; + SettingsFact _sideOverlapFact; + SettingsFact _adjustedFootprintSideFact; + SettingsFact _adjustedFootprintFrontalFact; double _imageFootprintSide; double _imageFootprintFrontal; QVariantList _knownCameraList; - static const char* _valueSetIsDistanceName; - static const char* _distanceToSurfaceName; - static const char* _distanceToSurfaceRelativeName; - static const char* _imageDensityName; - static const char* _frontalOverlapName; - static const char* _sideOverlapName; - static const char* _adjustedFootprintSideName; - static const char* _adjustedFootprintFrontalName; - static const char* _jsonCameraNameKey; - // The following are deprecated usage and only included in order to convert older formats enum CameraSpecType { diff --git a/src/MissionManager/CameraCalcTest.cc b/src/MissionManager/CameraCalcTest.cc index cb44927c36a92ccb2994d10a0c50708865bad72c..d6fa3dcb072c512ef877cc84ed38955cacb72589 100644 --- a/src/MissionManager/CameraCalcTest.cc +++ b/src/MissionManager/CameraCalcTest.cc @@ -21,9 +21,10 @@ void CameraCalcTest::init(void) UnitTest::init(); _offlineVehicle = new Vehicle(MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, qgcApp()->toolbox()->firmwarePluginManager(), this); - _cameraCalc = new CameraCalc(_offlineVehicle, this); + _cameraCalc = new CameraCalc(_offlineVehicle, "CameraCalcUnitTest" /* settingsGroup */, this); + _cameraCalc->cameraName()->setRawValue(_cameraCalc->customCameraName()); + _cameraCalc->setDirty(false); - _rgSignals[cameraNameChangedIndex] = SIGNAL(cameraNameChanged(QString)); _rgSignals[dirtyChangedIndex] = SIGNAL(dirtyChanged(bool)); _rgSignals[imageFootprintSideChangedIndex] = SIGNAL(imageFootprintSideChanged(double)); _rgSignals[imageFootprintFrontalChangedIndex] = SIGNAL(imageFootprintFrontalChanged(double)); @@ -81,11 +82,52 @@ void CameraCalcTest::_testDirty(void) } rgFacts.clear(); - _cameraCalc->setCameraName(_cameraCalc->customCameraName()); + + _cameraCalc->setDistanceToSurfaceRelative(!_cameraCalc->distanceToSurfaceRelative()); QVERIFY(_cameraCalc->dirty()); _multiSpy->clearAllSignals(); - _cameraCalc->setDistanceToSurfaceRelative(!_cameraCalc->distanceToSurfaceRelative()); + _cameraCalc->cameraName()->setRawValue(_cameraCalc->manualCameraName()); QVERIFY(_cameraCalc->dirty()); _multiSpy->clearAllSignals(); } + +void CameraCalcTest::_testAdjustedFootprint(void) +{ + double adjustedFootprintFrontal = _cameraCalc->adjustedFootprintFrontal()->rawValue().toDouble(); + double adjustedFootprintSide = _cameraCalc->adjustedFootprintSide()->rawValue().toDouble(); + _cameraCalc->valueSetIsDistance()->setRawValue(true); + changeFactValue(_cameraCalc->distanceToSurface()); + QVERIFY(adjustedFootprintFrontal != _cameraCalc->adjustedFootprintFrontal()->rawValue().toDouble()); + QVERIFY(adjustedFootprintSide != _cameraCalc->adjustedFootprintSide()->rawValue().toDouble()); + + adjustedFootprintFrontal = _cameraCalc->adjustedFootprintFrontal()->rawValue().toDouble(); + adjustedFootprintSide = _cameraCalc->adjustedFootprintSide()->rawValue().toDouble(); + _cameraCalc->valueSetIsDistance()->setRawValue(false); + changeFactValue(_cameraCalc->imageDensity()); + QVERIFY(adjustedFootprintFrontal != _cameraCalc->adjustedFootprintFrontal()->rawValue().toDouble()); + QVERIFY(adjustedFootprintSide != _cameraCalc->adjustedFootprintSide()->rawValue().toDouble()); + + adjustedFootprintFrontal = _cameraCalc->adjustedFootprintFrontal()->rawValue().toDouble(); + _cameraCalc->valueSetIsDistance()->setRawValue(true); + changeFactValue(_cameraCalc->frontalOverlap()); + QVERIFY(adjustedFootprintFrontal != _cameraCalc->adjustedFootprintFrontal()->rawValue().toDouble()); + + adjustedFootprintSide = _cameraCalc->adjustedFootprintSide()->rawValue().toDouble(); + _cameraCalc->valueSetIsDistance()->setRawValue(false); + changeFactValue(_cameraCalc->sideOverlap()); + QVERIFY(adjustedFootprintSide != _cameraCalc->adjustedFootprintSide()->rawValue().toDouble()); +} + +void CameraCalcTest::_testAltDensityRecalc(void) +{ + _cameraCalc->valueSetIsDistance()->setRawValue(true); + double imageDensity = _cameraCalc->imageDensity()->rawValue().toDouble(); + _cameraCalc->distanceToSurface()->setRawValue(_cameraCalc->distanceToSurface()->rawValue().toDouble() + 1); + QVERIFY(imageDensity != _cameraCalc->imageDensity()->rawValue().toDouble()); + + _cameraCalc->valueSetIsDistance()->setRawValue(false); + double distanceToSurface = _cameraCalc->distanceToSurface()->rawValue().toDouble(); + _cameraCalc->imageDensity()->setRawValue(_cameraCalc->imageDensity()->rawValue().toDouble() + 1); + QVERIFY(distanceToSurface != _cameraCalc->distanceToSurface()->rawValue().toDouble()); +} diff --git a/src/MissionManager/CameraCalcTest.h b/src/MissionManager/CameraCalcTest.h index 02fa9b723a607a7d280da065ad7138836539baf1..3692a0cbbe8fc3e229c440c8310256c3929acf21 100644 --- a/src/MissionManager/CameraCalcTest.h +++ b/src/MissionManager/CameraCalcTest.h @@ -27,12 +27,13 @@ protected: void cleanup(void) final; private slots: - void _testDirty(void); + void _testDirty (void); + void _testAdjustedFootprint (void); + void _testAltDensityRecalc (void); private: enum { - cameraNameChangedIndex = 0, - dirtyChangedIndex, + dirtyChangedIndex = 0, imageFootprintSideChangedIndex, imageFootprintFrontalChangedIndex, distanceToSurfaceRelativeChangedIndex, @@ -40,7 +41,6 @@ private: }; enum { - cameraNameChangedMask = 1 << cameraNameChangedIndex, dirtyChangedMask = 1 << dirtyChangedIndex, imageFootprintSideChangedMask = 1 << imageFootprintSideChangedIndex, imageFootprintFrontalChangedMask = 1 << imageFootprintFrontalChangedIndex, diff --git a/src/MissionManager/CameraSection.FactMetaData.json b/src/MissionManager/CameraSection.FactMetaData.json index ab724a662d6cda5adfba7276d658fb7ff82f7591..818a18d42e9beb2e170fb13dd3853bbe4733ee02 100644 --- a/src/MissionManager/CameraSection.FactMetaData.json +++ b/src/MissionManager/CameraSection.FactMetaData.json @@ -32,6 +32,7 @@ "units": "gimbal-degrees", "min": -90, "max": 0, + "increment": 5, "decimalPlaces": 0, "defaultValue": 0 }, @@ -42,6 +43,7 @@ "units": "deg", "min": -180.0, "max": 180.0, + "increment": 5, "decimalPlaces": 0, "defaultValue": 0 }, diff --git a/src/MissionManager/CameraSection.cc b/src/MissionManager/CameraSection.cc index f16d54aaf1a111023475cb317f1b2bfe0732dd9c..9114c8f7ca76aefb510af6d7cae4b23399b58a9f 100644 --- a/src/MissionManager/CameraSection.cc +++ b/src/MissionManager/CameraSection.cc @@ -180,11 +180,11 @@ void CameraSection::appendSectionItems(QList& items, QObject* miss item = new MissionItem(nextSequenceNumber++, MAV_CMD_VIDEO_START_CAPTURE, MAV_FRAME_MISSION, - 0, // Reserved (Set to 0) - 0, // No CAMERA_CAPTURE_STATUS streaming - NAN, NAN, NAN, NAN, NAN, // param 3-7 reserved - true, // autoContinue - false, // isCurrentItem + 0, // Reserved (Set to 0) + VIDEO_CAPTURE_STATUS_INTERVAL, // CAMERA_CAPTURE_STATUS (default to every 5 seconds) + NAN, NAN, NAN, NAN, NAN, // param 3-7 reserved + true, // autoContinue + false, // isCurrentItem missionItemParent); break; @@ -360,7 +360,7 @@ bool CameraSection::_scanTakeVideo(QmlObjectListModel* visualItems, int scanInde if (item) { MissionItem& missionItem = item->missionItem(); if ((MAV_CMD)item->command() == MAV_CMD_VIDEO_START_CAPTURE) { - if (missionItem.param1() == 0 && missionItem.param2() == 0) { + if (missionItem.param1() == 0 && missionItem.param2() == VIDEO_CAPTURE_STATUS_INTERVAL) { cameraAction()->setRawValue(TakeVideo); visualItems->removeAt(scanIndex)->deleteLater(); return true; diff --git a/src/MissionManager/CameraSection.h b/src/MissionManager/CameraSection.h index 32d019e5efeb414cee187747aae6811571c09a57..9b105eaa9dc46095a2425211f51412c352204876 100644 --- a/src/MissionManager/CameraSection.h +++ b/src/MissionManager/CameraSection.h @@ -14,6 +14,8 @@ #include "MissionItem.h" #include "Fact.h" +#define VIDEO_CAPTURE_STATUS_INTERVAL 0.2 //-- Send capture status every 5 seconds + class CameraSection : public Section { Q_OBJECT diff --git a/src/MissionManager/CameraSectionTest.cc b/src/MissionManager/CameraSectionTest.cc index 69eb38db70fa25f201c1ea9547f27a4024dcfdb5..81225f4e5150b72643d3f819526022001e52a082 100644 --- a/src/MissionManager/CameraSectionTest.cc +++ b/src/MissionManager/CameraSectionTest.cc @@ -46,15 +46,15 @@ void CameraSectionTest::init(void) QVERIFY(_spySection); _validGimbalItem = new SimpleMissionItem(_offlineVehicle, - true, // editMode + false, // flyView MissionItem(0, MAV_CMD_DO_MOUNT_CONTROL, MAV_FRAME_MISSION, 10.1234, 0, 20.1234, 0, 0, 0, MAV_MOUNT_MODE_MAVLINK_TARGETING, true, false), this); _validTimeItem = new SimpleMissionItem(_offlineVehicle, - true, // editMode + false, // flyView MissionItem(0, MAV_CMD_IMAGE_START_CAPTURE, MAV_FRAME_MISSION, 0, 48, 0, NAN, NAN, NAN, NAN, true, false), this); _validDistanceItem = new SimpleMissionItem(_offlineVehicle, - true, // editMode + false, // flyView MissionItem(0, MAV_CMD_DO_SET_CAM_TRIGG_DIST, MAV_FRAME_MISSION, @@ -65,30 +65,30 @@ void CameraSectionTest::init(void) true, false), this); _validStartVideoItem = new SimpleMissionItem(_offlineVehicle, - true, // editMode + false, // flyView MissionItem(0, // sequence number MAV_CMD_VIDEO_START_CAPTURE, MAV_FRAME_MISSION, 0, // Reserved (Set to 0) - 0, // No CAMERA_CAPTURE_STATUS streaming + VIDEO_CAPTURE_STATUS_INTERVAL, // CAMERA_CAPTURE_STATUS (default to every 5 seconds) NAN, NAN, NAN, NAN, NAN, // param 3-7 reserved true, // autocontinue false), // isCurrentItem this); _validStopVideoItem = new SimpleMissionItem(_offlineVehicle, - true, // editMode + false, // flyView MissionItem(0, MAV_CMD_VIDEO_STOP_CAPTURE, MAV_FRAME_MISSION, 0, NAN, NAN, NAN, NAN, NAN, NAN, true, false), this); _validStopDistanceItem = new SimpleMissionItem(_offlineVehicle, - true, // editMode + false, // flyView MissionItem(0, MAV_CMD_DO_SET_CAM_TRIGG_DIST, MAV_FRAME_MISSION, 0, 0, 0, 0, 0, 0, 0, true, false), this); _validStopTimeItem = new SimpleMissionItem(_offlineVehicle, - true, // editMode + false, // flyView MissionItem(1, MAV_CMD_IMAGE_STOP_CAPTURE, MAV_FRAME_MISSION, 0, NAN, NAN, NAN, NAN, NAN, NAN, true, false), this); _validCameraPhotoModeItem = new SimpleMissionItem(_offlineVehicle, - true, // editMode + false, // flyView MissionItem(0, // sequence number MAV_CMD_SET_CAMERA_MODE, MAV_FRAME_MISSION, @@ -99,7 +99,7 @@ void CameraSectionTest::init(void) false), // isCurrentItem this); _validCameraVideoModeItem = new SimpleMissionItem(_offlineVehicle, - true, // editMode + false, // flyView MissionItem(0, // sequence number MAV_CMD_SET_CAMERA_MODE, MAV_FRAME_MISSION, @@ -110,7 +110,7 @@ void CameraSectionTest::init(void) false), // isCurrentItem this); _validCameraSurveyPhotoModeItem = new SimpleMissionItem(_offlineVehicle, - true, // editMode + false, // flyView MissionItem(0, // sequence number MAV_CMD_SET_CAMERA_MODE, MAV_FRAME_MISSION, @@ -121,7 +121,7 @@ void CameraSectionTest::init(void) false), // isCurrentItem this); _validTakePhotoItem = new SimpleMissionItem(_offlineVehicle, - true, // editMode + false, // flyView MissionItem(0, MAV_CMD_IMAGE_START_CAPTURE, MAV_FRAME_MISSION, @@ -362,7 +362,7 @@ void CameraSectionTest::_checkAvailable(void) 70.1234567, true, // autoContinue false); // isCurrentItem - SimpleMissionItem* item = new SimpleMissionItem(_offlineVehicle, true /* editMode */, missionItem); + SimpleMissionItem* item = new SimpleMissionItem(_offlineVehicle, false /* flyView */, missionItem, this); QVERIFY(item->cameraSection()); QCOMPARE(item->cameraSection()->available(), false); } @@ -606,7 +606,7 @@ void CameraSectionTest::_testScanForGimbalSection(void) // Check for a scan success - SimpleMissionItem* newValidGimbalItem = new SimpleMissionItem(_offlineVehicle, this); + SimpleMissionItem* newValidGimbalItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); newValidGimbalItem->missionItem() = _validGimbalItem->missionItem(); visualItems.append(newValidGimbalItem); scanIndex = 0; @@ -633,7 +633,7 @@ void CameraSectionTest::_testScanForGimbalSection(void) // Gimbal command but incorrect settings - SimpleMissionItem invalidSimpleItem(_offlineVehicle, true /* editMode */, _validGimbalItem->missionItem()); + SimpleMissionItem invalidSimpleItem(_offlineVehicle, false /* flyView */, _validGimbalItem->missionItem(), NULL); invalidSimpleItem.missionItem().setParam2(10); // roll is not supported visualItems.append(&invalidSimpleItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false); @@ -690,7 +690,7 @@ void CameraSectionTest::_testScanForCameraModeSection(void) // Check for a scan success - SimpleMissionItem* newValidCameraModeItem = new SimpleMissionItem(_offlineVehicle, this); + SimpleMissionItem* newValidCameraModeItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); newValidCameraModeItem->missionItem() = _validCameraPhotoModeItem->missionItem(); visualItems.append(newValidCameraModeItem); scanIndex = 0; @@ -723,7 +723,7 @@ void CameraSectionTest::_testScanForCameraModeSection(void) */ // Mode command but incorrect settings - SimpleMissionItem invalidSimpleItem(_offlineVehicle, true /* editMode */, _validCameraPhotoModeItem->missionItem()); + SimpleMissionItem invalidSimpleItem(_offlineVehicle, false /* flyView */, _validCameraPhotoModeItem->missionItem(), NULL); invalidSimpleItem.missionItem().setParam3(1); // Param3 should be NaN visualItems.append(&invalidSimpleItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false); @@ -749,7 +749,7 @@ void CameraSectionTest::_testScanForPhotoIntervalTimeSection(void) Mission Param #3 Number of images to capture total - 0 for unlimited capture */ - SimpleMissionItem* newValidTimeItem = new SimpleMissionItem(_offlineVehicle, this); + SimpleMissionItem* newValidTimeItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); newValidTimeItem->missionItem() = _validTimeItem->missionItem(); visualItems.append(newValidTimeItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), true); @@ -762,7 +762,7 @@ void CameraSectionTest::_testScanForPhotoIntervalTimeSection(void) // Image start command but incorrect settings - SimpleMissionItem invalidSimpleItem(_offlineVehicle, true /* editMode */, _validTimeItem->missionItem()); + SimpleMissionItem invalidSimpleItem(_offlineVehicle, false /* flyView */, _validTimeItem->missionItem(), NULL); invalidSimpleItem.missionItem().setParam3(10); // must be 0 for unlimited visualItems.append(&invalidSimpleItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false); @@ -790,7 +790,7 @@ void CameraSectionTest::_testScanForPhotoIntervalDistanceSection(void) Mission Param #7 Empty */ - SimpleMissionItem* newValidDistanceItem = new SimpleMissionItem(_offlineVehicle, this); + SimpleMissionItem* newValidDistanceItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); newValidDistanceItem->missionItem() = _validDistanceItem->missionItem(); visualItems.append(newValidDistanceItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), true); @@ -803,7 +803,7 @@ void CameraSectionTest::_testScanForPhotoIntervalDistanceSection(void) // Trigger distance command but incorrect settings - SimpleMissionItem invalidSimpleItem(_offlineVehicle, true /* editMode */, _validDistanceItem->missionItem()); + SimpleMissionItem invalidSimpleItem(_offlineVehicle, false /* flyView */, _validDistanceItem->missionItem(), NULL); invalidSimpleItem.missionItem().setParam1(-1); // must be >= 0 visualItems.append(&invalidSimpleItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false); @@ -876,7 +876,7 @@ void CameraSectionTest::_testScanForStartVideoSection(void) Mission Param #3 Reserved */ - SimpleMissionItem* newValidStartVideoItem = new SimpleMissionItem(_offlineVehicle, this); + SimpleMissionItem* newValidStartVideoItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); newValidStartVideoItem->missionItem() = _validStartVideoItem->missionItem(); visualItems.append(newValidStartVideoItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), true); @@ -888,7 +888,7 @@ void CameraSectionTest::_testScanForStartVideoSection(void) // Start Video command but incorrect settings - SimpleMissionItem invalidSimpleItem(_offlineVehicle, true /* editMode */, _validStartVideoItem->missionItem()); + SimpleMissionItem invalidSimpleItem(_offlineVehicle, false /* flyView */, _validStartVideoItem->missionItem(), NULL); invalidSimpleItem.missionItem().setParam1(10); // Reserved (must be 0) visualItems.append(&invalidSimpleItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false); @@ -896,13 +896,6 @@ void CameraSectionTest::_testScanForStartVideoSection(void) QCOMPARE(_cameraSection->settingsSpecified(), false); visualItems.clear(); - invalidSimpleItem.missionItem() = _validStartVideoItem->missionItem(); - invalidSimpleItem.missionItem().setParam2(10); // must be 0 - visualItems.append(&invalidSimpleItem); - QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false); - QCOMPARE(visualItems.count(), 1); - QCOMPARE(_cameraSection->settingsSpecified(), false); - visualItems.clear(); } void CameraSectionTest::_testScanForStopVideoSection(void) @@ -919,7 +912,7 @@ void CameraSectionTest::_testScanForStopVideoSection(void) Mission Param #1 Reserved (Set to 0) */ - SimpleMissionItem* newValidStopVideoItem = new SimpleMissionItem(_offlineVehicle, this); + SimpleMissionItem* newValidStopVideoItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); newValidStopVideoItem->missionItem() = _validStopVideoItem->missionItem(); visualItems.append(newValidStopVideoItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), true); @@ -931,13 +924,21 @@ void CameraSectionTest::_testScanForStopVideoSection(void) // Trigger distance command but incorrect settings - SimpleMissionItem invalidSimpleItem(_offlineVehicle, true /* editMode */, _validStopVideoItem->missionItem()); + SimpleMissionItem invalidSimpleItem(_offlineVehicle, false /* flyView */, _validStopVideoItem->missionItem(), NULL); invalidSimpleItem.missionItem().setParam1(10); // must be 0 visualItems.append(&invalidSimpleItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false); QCOMPARE(visualItems.count(), 1); QCOMPARE(_cameraSection->settingsSpecified(), false); visualItems.clear(); + + invalidSimpleItem.missionItem() = _validStartVideoItem->missionItem(); + invalidSimpleItem.missionItem().setParam2(VIDEO_CAPTURE_STATUS_INTERVAL + 1); // must be VIDEO_CAPTURE_STATUS_INTERVAL + visualItems.append(&invalidSimpleItem); + QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false); + QCOMPARE(visualItems.count(), 1); + QCOMPARE(_cameraSection->settingsSpecified(), false); + visualItems.clear(); } void CameraSectionTest::_testScanForStopImageSection(void) @@ -949,8 +950,8 @@ void CameraSectionTest::_testScanForStopImageSection(void) _commonScanTest(_cameraSection); - SimpleMissionItem* newValidStopDistanceItem = new SimpleMissionItem(_offlineVehicle, this); - SimpleMissionItem* newValidStopTimeItem = new SimpleMissionItem(_offlineVehicle, this); + SimpleMissionItem* newValidStopDistanceItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); + SimpleMissionItem* newValidStopTimeItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); newValidStopDistanceItem->missionItem() = _validStopDistanceItem->missionItem(); newValidStopTimeItem->missionItem() = _validStopTimeItem->missionItem(); visualItems.append(newValidStopDistanceItem); @@ -963,8 +964,8 @@ void CameraSectionTest::_testScanForStopImageSection(void) // Out of order commands - SimpleMissionItem validStopDistanceItem(_offlineVehicle); - SimpleMissionItem validStopTimeItem(_offlineVehicle); + SimpleMissionItem validStopDistanceItem(_offlineVehicle, false /* flyView */, NULL); + SimpleMissionItem validStopTimeItem(_offlineVehicle, false /* flyView */, NULL); validStopDistanceItem.missionItem() = _validStopDistanceItem->missionItem(); validStopTimeItem.missionItem() = _validStopTimeItem->missionItem(); visualItems.append(&validStopTimeItem); @@ -992,7 +993,7 @@ void CameraSectionTest::_testScanForTakePhotoSection(void) Mission Param #4 Reserved */ - SimpleMissionItem* newValidTakePhotoItem = new SimpleMissionItem(_offlineVehicle, this); + SimpleMissionItem* newValidTakePhotoItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); newValidTakePhotoItem->missionItem() = _validTakePhotoItem->missionItem(); visualItems.append(newValidTakePhotoItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), true); @@ -1004,7 +1005,7 @@ void CameraSectionTest::_testScanForTakePhotoSection(void) // Take Photo command but incorrect settings - SimpleMissionItem invalidSimpleItem(_offlineVehicle, true /* editMode */, _validTimeItem->missionItem()); + SimpleMissionItem invalidSimpleItem(_offlineVehicle, false /* flyView */, _validTimeItem->missionItem(), NULL); invalidSimpleItem.missionItem().setParam3(10); // must be 1 for single photo visualItems.append(&invalidSimpleItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false); @@ -1064,9 +1065,9 @@ void CameraSectionTest::_testScanForMultipleItems(void) // Camera action followed by gimbal/mode foreach (SimpleMissionItem* actionItem, rgActionItems) { foreach (SimpleMissionItem* cameraItem, rgCameraItems) { - SimpleMissionItem* item1 = new SimpleMissionItem(_offlineVehicle, this); + SimpleMissionItem* item1 = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); item1->missionItem() = actionItem->missionItem(); - SimpleMissionItem* item2 = new SimpleMissionItem(_offlineVehicle, this); + SimpleMissionItem* item2 = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); item2->missionItem() = cameraItem->missionItem(); visualItems.append(item1); visualItems.append(item2); @@ -1085,9 +1086,9 @@ void CameraSectionTest::_testScanForMultipleItems(void) // Gimbal/Mode followed by camera action foreach (SimpleMissionItem* actionItem, rgCameraItems) { foreach (SimpleMissionItem* cameraItem, rgActionItems) { - SimpleMissionItem* item1 = new SimpleMissionItem(_offlineVehicle, this); + SimpleMissionItem* item1 = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); item1->missionItem() = actionItem->missionItem(); - SimpleMissionItem* item2 = new SimpleMissionItem(_offlineVehicle, this); + SimpleMissionItem* item2 = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); item2->missionItem() = cameraItem->missionItem(); visualItems.append(item1); visualItems.append(item2); diff --git a/src/MissionManager/CameraSpec.cc b/src/MissionManager/CameraSpec.cc index d07cb293b3f24869b190e5c1e88d24a840d3a985..2bb34ef791eef2d934b70d67ddf9b85dbaba7518 100644 --- a/src/MissionManager/CameraSpec.cc +++ b/src/MissionManager/CameraSpec.cc @@ -121,8 +121,8 @@ bool CameraSpec::load(const QJsonObject& json, QString& errorString) _sensorWidthFact.setRawValue (json[_sensorWidthName].toDouble()); _sensorHeightFact.setRawValue (json[_sensorHeightName].toDouble()); - _imageWidthFact.setRawValue (json[_imageWidthName].toDouble()); - _imageHeightFact.setRawValue (json[_imageHeightName].toDouble()); + _imageWidthFact.setRawValue (json[_imageWidthName].toInt()); + _imageHeightFact.setRawValue (json[_imageHeightName].toInt()); _focalLengthFact.setRawValue (json[_focalLengthName].toDouble()); _landscapeFact.setRawValue (json[_landscapeName].toBool()); _fixedOrientationFact.setRawValue (json[_fixedOrientationName].toBool()); diff --git a/src/MissionManager/ComplexMissionItem.cc b/src/MissionManager/ComplexMissionItem.cc index 43075c1ee119556264c8c6f4189bdac7ce330e64..44af71895fa2eb43d483f72186a3baab7fb606ab 100644 --- a/src/MissionManager/ComplexMissionItem.cc +++ b/src/MissionManager/ComplexMissionItem.cc @@ -11,8 +11,8 @@ const char* ComplexMissionItem::jsonComplexItemTypeKey = "complexItemType"; -ComplexMissionItem::ComplexMissionItem(Vehicle* vehicle, QObject* parent) - : VisualMissionItem(vehicle, parent) +ComplexMissionItem::ComplexMissionItem(Vehicle* vehicle, bool flyView, QObject* parent) + : VisualMissionItem(vehicle, flyView, parent) { } diff --git a/src/MissionManager/ComplexMissionItem.h b/src/MissionManager/ComplexMissionItem.h index bc27fe7f80856a77f750925fb21b734bcc5fab0a..dc1b0d661e3780f049f9c7381f816f0d16ab6f9b 100644 --- a/src/MissionManager/ComplexMissionItem.h +++ b/src/MissionManager/ComplexMissionItem.h @@ -17,7 +17,7 @@ class ComplexMissionItem : public VisualMissionItem Q_OBJECT public: - ComplexMissionItem(Vehicle* vehicle, QObject* parent = NULL); + ComplexMissionItem(Vehicle* vehicle, bool flyView, QObject* parent); const ComplexMissionItem& operator=(const ComplexMissionItem& other); diff --git a/src/MissionManager/CorridorScanComplexItem.cc b/src/MissionManager/CorridorScanComplexItem.cc index 2a93b928bd2eb8bc704f2aa1cd7979e0281fce4c..827ba910ee5d69b253181fe16b62a795c713364e 100644 --- a/src/MissionManager/CorridorScanComplexItem.cc +++ b/src/MissionManager/CorridorScanComplexItem.cc @@ -23,13 +23,12 @@ QGC_LOGGING_CATEGORY(CorridorScanComplexItemLog, "CorridorScanComplexItemLog") const char* CorridorScanComplexItem::settingsGroup = "CorridorScan"; const char* CorridorScanComplexItem::corridorWidthName = "CorridorWidth"; -const char* CorridorScanComplexItem::_entryPointName = "EntryPoint"; +const char* CorridorScanComplexItem::_jsonEntryPointKey = "EntryPoint"; const char* CorridorScanComplexItem::jsonComplexItemTypeValue = "CorridorScan"; -CorridorScanComplexItem::CorridorScanComplexItem(Vehicle* vehicle, QObject* parent) - : TransectStyleComplexItem (vehicle, settingsGroup, parent) - , _ignoreRecalc (false) +CorridorScanComplexItem::CorridorScanComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlFile, QObject* parent) + : TransectStyleComplexItem (vehicle, flyView, settingsGroup, parent) , _entryPoint (0) , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/CorridorScan.SettingsGroup.json"), this)) , _corridorWidthFact (settingsGroup, _metaDataMap[corridorWidthName]) @@ -48,74 +47,54 @@ CorridorScanComplexItem::CorridorScanComplexItem(Vehicle* vehicle, QObject* pare connect(&_cameraCalc, &CameraCalc::distanceToSurfaceRelativeChanged, this, &CorridorScanComplexItem::exitCoordinateHasRelativeAltitudeChanged); connect(&_corridorPolyline, &QGCMapPolyline::dirtyChanged, this, &CorridorScanComplexItem::_polylineDirtyChanged); - connect(&_corridorPolyline, &QGCMapPolyline::countChanged, this, &CorridorScanComplexItem::_polylineCountChanged); - connect(&_corridorPolyline, &QGCMapPolyline::pathChanged, this, &CorridorScanComplexItem::_rebuildCorridor); - connect(&_corridorWidthFact, &Fact::valueChanged, this, &CorridorScanComplexItem::_rebuildCorridor); + connect(&_corridorPolyline, &QGCMapPolyline::pathChanged, this, &CorridorScanComplexItem::_rebuildCorridorPolygon); + connect(&_corridorWidthFact, &Fact::valueChanged, this, &CorridorScanComplexItem::_rebuildCorridorPolygon); - _rebuildCorridor(); -} - -void CorridorScanComplexItem::_polylineCountChanged(int count) -{ - Q_UNUSED(count); - emit lastSequenceNumberChanged(lastSequenceNumber()); -} - -int CorridorScanComplexItem::lastSequenceNumber(void) const -{ - int itemCount = _transectPoints.count(); // Each transpect point represents a waypoint item - - if (_cameraTriggerInTurnAroundFact.rawValue().toBool()) { - // Only one camera start and on camera stop - itemCount += 2; - } else { - // Each transect will have a camera start and stop in it - itemCount += _transectCount() * 2; + if (!kmlFile.isEmpty()) { + _corridorPolyline.loadKMLFile(kmlFile); + _corridorPolyline.setDirty(false); } - - return _sequenceNumber + itemCount - 1; + setDirty(false); } -void CorridorScanComplexItem::save(QJsonArray& missionItems) +void CorridorScanComplexItem::save(QJsonArray& planItems) { QJsonObject saveObject; - saveObject[JsonHelper::jsonVersionKey] = 1; + _save(saveObject); + + saveObject[JsonHelper::jsonVersionKey] = 2; saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; saveObject[corridorWidthName] = _corridorWidthFact.rawValue().toDouble(); - saveObject[turnAroundDistanceName] = _turnAroundDistanceFact.rawValue().toDouble(); - saveObject[_entryPointName] = _entryPoint; - - QJsonObject cameraCalcObject; - _cameraCalc.save(cameraCalcObject); - saveObject[_jsonCameraCalcKey] = cameraCalcObject; + saveObject[_jsonEntryPointKey] = _entryPoint; _corridorPolyline.saveToJson(saveObject); - _save(saveObject); - - missionItems.append(saveObject); + planItems.append(saveObject); } bool CorridorScanComplexItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString) { + // We don't recalc while loading since all the information we need is specified in the file + _ignoreRecalc = true; + QList keyInfoList = { { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, { VisualMissionItem::jsonTypeKey, QJsonValue::String, true }, { ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true }, { corridorWidthName, QJsonValue::Double, true }, - { turnAroundDistanceName, QJsonValue::Double, true }, - { _entryPointName, QJsonValue::Double, true }, + { _jsonEntryPointKey, QJsonValue::Double, true }, { QGCMapPolyline::jsonPolylineKey, QJsonValue::Array, true }, - { _jsonCameraCalcKey, QJsonValue::Object, true }, }; if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) { + _ignoreRecalc = false; return false; } if (!_corridorPolyline.loadFromJson(complexObject, true, errorString)) { + _ignoreRecalc = false; return false; } @@ -123,26 +102,31 @@ bool CorridorScanComplexItem::load(const QJsonObject& complexObject, int sequenc QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString(); if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonComplexItemTypeValue) { errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType); + _ignoreRecalc = false; return false; } int version = complexObject[JsonHelper::jsonVersionKey].toInt(); - if (version != 1) { + if (version != 2) { errorString = tr("%1 complex item version %2 not supported").arg(jsonComplexItemTypeValue).arg(version); + _ignoreRecalc = false; return false; } setSequenceNumber(sequenceNumber); if (!_load(complexObject, errorString)) { + _ignoreRecalc = false; return false; } _corridorWidthFact.setRawValue (complexObject[corridorWidthName].toDouble()); - _entryPoint = complexObject[_entryPointName].toInt(); + _entryPoint = complexObject[_jsonEntryPointKey].toInt(); + + _rebuildTransects(); - _rebuildCorridor(); + _ignoreRecalc = false; return true; } @@ -159,64 +143,59 @@ int CorridorScanComplexItem::_transectCount(void) const return fullWidth > 0.0 ? qCeil(fullWidth / transectSpacing) : 1; } -void CorridorScanComplexItem::appendMissionItems(QList& items, QObject* missionItemParent) +void CorridorScanComplexItem::_appendLoadedMissionItems(QList& items, QObject* missionItemParent) { - int seqNum = _sequenceNumber; - int pointIndex = 0; - bool imagesEverywhere = _cameraTriggerInTurnAroundFact.rawValue().toBool(); - - while (pointIndex < _transectPoints.count()) { - if (_hasTurnaround()) { - QGeoCoordinate vertexCoord = _transectPoints[pointIndex++].value(); - MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_NAV_WAYPOINT, - _cameraCalc.distanceToSurfaceRelative() ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, - 0, // No hold time - 0.0, // No acceptance radius specified - 0.0, // Pass through waypoint - std::numeric_limits::quiet_NaN(), // Yaw unchanged - vertexCoord.latitude(), - vertexCoord.longitude(), - _cameraCalc.distanceToSurface()->rawValue().toDouble(), // Altitude - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - if (imagesEverywhere && pointIndex == 1) { - item = new MissionItem(seqNum++, - MAV_CMD_DO_SET_CAM_TRIGG_DIST, - MAV_FRAME_MISSION, - _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble(), // trigger distance - 0, // shutter integration (ignore) - 1, // trigger immediately when starting - 0, 0, 0, 0, // param 4-7 unused - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - } - } + qCDebug(CorridorScanComplexItemLog) << "_appendLoadedMissionItems"; + + int seqNum = _sequenceNumber; + + foreach (const MissionItem* loadedMissionItem, _loadedMissionItems) { + MissionItem* item = new MissionItem(*loadedMissionItem, missionItemParent); + item->setSequenceNumber(seqNum++); + items.append(item); + } +} - bool addTrigger = imagesEverywhere ? false : true; - for (int i=0; i<_corridorPolyline.count(); i++) { - QGeoCoordinate vertexCoord = _transectPoints[pointIndex++].value(); - MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_NAV_WAYPOINT, - _cameraCalc.distanceToSurfaceRelative() ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, - 0, // No hold time - 0.0, // No acceptance radius specified - 0.0, // Pass through waypoint - std::numeric_limits::quiet_NaN(), // Yaw unchanged - vertexCoord.latitude(), - vertexCoord.longitude(), - _cameraCalc.distanceToSurface()->rawValue().toDouble(), // Altitude - true, // autoContinue - false, // isCurrentItem - missionItemParent); +void CorridorScanComplexItem::_buildAndAppendMissionItems(QList& items, QObject* missionItemParent) +{ + qCDebug(CorridorScanComplexItemLog) << "_buildAndAppendMissionItems"; + + // Now build the mission items from the transect points + + MissionItem* item; + int seqNum = _sequenceNumber; + bool imagesEverywhere = _cameraTriggerInTurnAroundFact.rawValue().toBool(); + bool addTriggerAtBeginning = imagesEverywhere; + bool firstOverallPoint = true; + + MAV_FRAME mavFrame = followTerrain() || !_cameraCalc.distanceToSurfaceRelative() ? MAV_FRAME_GLOBAL : MAV_FRAME_GLOBAL_RELATIVE_ALT; + + //qDebug() << "_buildAndAppendMissionItems"; + foreach (const QList& transect, _transects) { + bool entryPoint = true; + + //qDebug() << "start transect"; + foreach (const CoordInfo_t& transectCoordInfo, transect) { + //qDebug() << transectCoordInfo.coordType; + + item = new MissionItem(seqNum++, + MAV_CMD_NAV_WAYPOINT, + mavFrame, + 0, // No hold time + 0.0, // No acceptance radius specified + 0.0, // Pass through waypoint + std::numeric_limits::quiet_NaN(), // Yaw unchanged + transectCoordInfo.coord.latitude(), + transectCoordInfo.coord.longitude(), + transectCoordInfo.coord.altitude(), + true, // autoContinue + false, // isCurrentItem + missionItemParent); items.append(item); - if (addTrigger) { - addTrigger = false; + if (firstOverallPoint && addTriggerAtBeginning) { + // Start triggering + addTriggerAtBeginning = false; item = new MissionItem(seqNum++, MAV_CMD_DO_SET_CAM_TRIGG_DIST, MAV_FRAME_MISSION, @@ -229,42 +208,43 @@ void CorridorScanComplexItem::appendMissionItems(QList& items, QOb missionItemParent); items.append(item); } - } - - if (!imagesEverywhere) { - MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_DO_SET_CAM_TRIGG_DIST, - MAV_FRAME_MISSION, - 0, // stop triggering - 0, // shutter integration (ignore) - 0, // trigger immediately when starting - 0, 0, 0, 0, // param 4-7 unused - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - } - - if (_hasTurnaround()) { - QGeoCoordinate vertexCoord = _transectPoints[pointIndex++].value(); - MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_NAV_WAYPOINT, - _cameraCalc.distanceToSurfaceRelative() ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, - 0, // No hold time - 0.0, // No acceptance radius specified - 0.0, // Pass through waypoint - std::numeric_limits::quiet_NaN(), // Yaw unchanged - vertexCoord.latitude(), - vertexCoord.longitude(), - _cameraCalc.distanceToSurface()->rawValue().toDouble(), // Altitude - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); + firstOverallPoint = false; + + if (transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEdge && !imagesEverywhere) { + if (entryPoint) { + // Start of transect, start triggering + item = new MissionItem(seqNum++, + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + MAV_FRAME_MISSION, + _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble(), // trigger distance + 0, // shutter integration (ignore) + 1, // trigger immediately when starting + 0, 0, 0, 0, // param 4-7 unused + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); + } else { + // End of transect, stop triggering + item = new MissionItem(seqNum++, + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + MAV_FRAME_MISSION, + 0, // stop triggering + 0, // shutter integration (ignore) + 0, // trigger immediately when starting + 0, 0, 0, 0, // param 4-7 unused + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); + } + entryPoint = !entryPoint; + } } } if (imagesEverywhere) { + // Stop triggering MissionItem* item = new MissionItem(seqNum++, MAV_CMD_DO_SET_CAM_TRIGG_DIST, MAV_FRAME_MISSION, @@ -279,6 +259,17 @@ void CorridorScanComplexItem::appendMissionItems(QList& items, QOb } } +void CorridorScanComplexItem::appendMissionItems(QList& items, QObject* missionItemParent) +{ + if (_loadedMissionItems.count()) { + // We have mission items from the loaded plan, use those + _appendLoadedMissionItems(items, missionItemParent); + } else { + // Build the mission items on the fly + _buildAndAppendMissionItems(items, missionItemParent); + } +} + void CorridorScanComplexItem::applyNewAltitude(double newAltitude) { _cameraCalc.valueSetIsDistance()->setRawValue(true); @@ -300,7 +291,7 @@ void CorridorScanComplexItem::rotateEntryPoint(void) _entryPoint = 0; } - _rebuildCorridor(); + _rebuildTransects(); } void CorridorScanComplexItem::_rebuildCorridorPolygon(void) @@ -316,18 +307,32 @@ void CorridorScanComplexItem::_rebuildCorridorPolygon(void) QList secondSideVertices = _corridorPolyline.offsetPolyline(-halfWidth); _surveyAreaPolygon.clear(); + + QList rgCoord; foreach (const QGeoCoordinate& vertex, firstSideVertices) { - _surveyAreaPolygon.appendVertex(vertex); + rgCoord.append(vertex); } for (int i=secondSideVertices.count() - 1; i >= 0; i--) { - _surveyAreaPolygon.appendVertex(secondSideVertices[i]); + rgCoord.append(secondSideVertices[i]); } + _surveyAreaPolygon.appendVertices(rgCoord); } -void CorridorScanComplexItem::_rebuildTransects(void) +void CorridorScanComplexItem::_rebuildTransectsPhase1(void) { - _transectPoints.clear(); - _cameraShots = 0; + if (_ignoreRecalc) { + return; + } + + // If the transects are getting rebuilt then any previsouly loaded mission items are now invalid + if (_loadedMissionItemsParent) { + _loadedMissionItems.clear(); + _loadedMissionItemsParent->deleteLater(); + _loadedMissionItemsParent = NULL; + } + + _transects.clear(); + _transectsPathHeightInfo.clear(); double transectSpacing = _cameraCalc.adjustedFootprintSide()->rawValue().toDouble(); double fullWidth = _corridorWidthFact.rawValue().toDouble(); @@ -336,13 +341,10 @@ void CorridorScanComplexItem::_rebuildTransects(void) double normalizedTransectPosition = transectSpacing / 2.0; if (_corridorPolyline.count() >= 2) { - int singleTransectImageCount = qCeil(_corridorPolyline.length() / _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble()); - // First build up the transects all going the same direction - QList> transects; + //qDebug() << "_rebuildTransectsPhase1"; for (int i=0; i transect = _corridorPolyline.offsetPolyline(offsetDistance); + // Turn transect into CoordInfo transect + QList transect; + QList transectCoords = _corridorPolyline.offsetPolyline(offsetDistance); + for (int j=1; j> reversedTransects; - foreach (const QList& transect, transects) { + QList> reversedTransects; + foreach (const QList& transect, _transects) { reversedTransects.prepend(transect); } - transects = reversedTransects; + _transects = reversedTransects; } if (reverseVertices) { - for (int i=0; i reversedVertices; - foreach (const QGeoCoordinate& vertex, transects[i]) { + for (int i=0; i<_transects.count(); i++) { + QList reversedVertices; + foreach (const TransectStyleComplexItem::CoordInfo_t& vertex, _transects[i]) { reversedVertices.prepend(vertex); } - transects[i] = reversedVertices; + _transects[i] = reversedVertices; } } - // Convert the list of transects to grid points + // Adjust to lawnmower pattern reverseVertices = false; - for (int i=0; i transectVertices = transects[i]; + QList transectVertices = _transects[i]; if (reverseVertices) { reverseVertices = false; - QList reversedVertices; + QList reversedVertices; for (int j=transectVertices.count()-1; j>=0; j--) { reversedVertices.append(transectVertices[j]); } @@ -429,36 +453,41 @@ void CorridorScanComplexItem::_rebuildTransects(void) } else { reverseVertices = true; } - for (int i=0; i().distanceTo(_transectPoints[i+1].value()); + for (int i=0; i<_visualTransectPoints.count() - 1; i++) { + _complexDistance += _visualTransectPoints[i].value().distanceTo(_visualTransectPoints[i+1].value()); } - if (_cameraTriggerInTurnAroundFact.rawValue().toDouble()) { + if (_cameraTriggerInTurnAroundFact.rawValue().toBool()) { _cameraShots = qCeil(_complexDistance / _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble()); + } else { + int singleTransectImageCount = qCeil(_corridorPolyline.length() / _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble()); + _cameraShots = singleTransectImageCount * _transectCount(); } - _coordinate = _transectPoints.count() ? _transectPoints.first().value() : QGeoCoordinate(); - _exitCoordinate = _transectPoints.count() ? _transectPoints.last().value() : QGeoCoordinate(); + _coordinate = _visualTransectPoints.count() ? _visualTransectPoints.first().value() : QGeoCoordinate(); + _exitCoordinate = _visualTransectPoints.count() ? _visualTransectPoints.last().value() : QGeoCoordinate(); - emit transectPointsChanged(); emit cameraShotsChanged(); emit complexDistanceChanged(); emit coordinateChanged(_coordinate); emit exitCoordinateChanged(_exitCoordinate); } -void CorridorScanComplexItem::_rebuildCorridor(void) +bool CorridorScanComplexItem::readyForSave(void) const { - _rebuildCorridorPolygon(); - _rebuildTransects(); + return TransectStyleComplexItem::readyForSave(); +} + +double CorridorScanComplexItem::timeBetweenShots(void) +{ + return _cruiseSpeed == 0 ? 0 : _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / _cruiseSpeed; } diff --git a/src/MissionManager/CorridorScanComplexItem.h b/src/MissionManager/CorridorScanComplexItem.h index 01c1983ff05f465ee1dbb8e084a62570cafb03b9..a5fe2cab8acee6a353cd838c2862545caa04bc86 100644 --- a/src/MissionManager/CorridorScanComplexItem.h +++ b/src/MissionManager/CorridorScanComplexItem.h @@ -15,7 +15,6 @@ #include "QGCLoggingCategory.h" #include "QGCMapPolyline.h" #include "QGCMapPolygon.h" -#include "CameraCalc.h" Q_DECLARE_LOGGING_CATEGORY(CorridorScanComplexItemLog) @@ -24,9 +23,11 @@ class CorridorScanComplexItem : public TransectStyleComplexItem Q_OBJECT public: - CorridorScanComplexItem(Vehicle* vehicle, QObject* parent = NULL); + /// @param vehicle Vehicle which this is being contructed for + /// @param flyView true: Created for use in the Fly View, false: Created for use in the Plan View + /// @param kmlFile Polyline comes from this file, empty for default polyline + CorridorScanComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlFile, QObject* parent); - Q_PROPERTY(CameraCalc* cameraCalc READ cameraCalc CONSTANT) Q_PROPERTY(QGCMapPolyline* corridorPolyline READ corridorPolyline CONSTANT) Q_PROPERTY(Fact* corridorWidth READ corridorWidth CONSTANT) @@ -35,18 +36,22 @@ public: Q_INVOKABLE void rotateEntryPoint(void); - // Overrides from ComplexMissionItem - - int lastSequenceNumber (void) const final; - bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final; - QString mapVisualQML (void) const final { return QStringLiteral("CorridorScanMapVisual.qml"); } - // Overrides from TransectStyleComplexItem + void save (QJsonArray& planItems) final; + bool specifiesCoordinate (void) const final; + void appendMissionItems (QList& items, QObject* missionItemParent) final; + void applyNewAltitude (double newAltitude) final; + double timeBetweenShots (void) final; - void save (QJsonArray& missionItems) final; - bool specifiesCoordinate (void) const final; - void appendMissionItems (QList& items, QObject* missionItemParent) final; - void applyNewAltitude (double newAltitude) final; + // Overrides from ComplexMissionItem + bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final; + QString mapVisualQML (void) const final { return QStringLiteral("CorridorScanMapVisual.qml"); } + + // Overrides from VisualMissionionItem + QString commandDescription (void) const final { return tr("Corridor Scan"); } + QString commandName (void) const final { return tr("Corridor Scan"); } + QString abbreviation (void) const final { return tr("C"); } + bool readyForSave (void) const; static const char* jsonComplexItemTypeValue; @@ -54,26 +59,25 @@ public: static const char* corridorWidthName; private slots: - void _polylineDirtyChanged (bool dirty); - void _polylineCountChanged (int count); - void _rebuildCorridor (void); + void _polylineDirtyChanged (bool dirty); + void _rebuildCorridorPolygon (void); // Overrides from TransectStyleComplexItem - virtual void _rebuildTransects (void) final; + void _rebuildTransectsPhase1 (void) final; + void _rebuildTransectsPhase2 (void) final; private: - int _transectCount (void) const; - void _rebuildCorridorPolygon(void); - + int _transectCount (void) const; + void _buildAndAppendMissionItems(QList& items, QObject* missionItemParent); + void _appendLoadedMissionItems (QList& items, QObject* missionItemParent); QGCMapPolyline _corridorPolyline; QList> _transectSegments; ///< Internal transect segments including grid exit, turnaround and internal camera points - bool _ignoreRecalc; - int _entryPoint; + int _entryPoint; QMap _metaDataMap; SettingsFact _corridorWidthFact; - static const char* _entryPointName; + static const char* _jsonEntryPointKey; }; diff --git a/src/MissionManager/CorridorScanComplexItemTest.cc b/src/MissionManager/CorridorScanComplexItemTest.cc index bcbd9cf3fd56484e9bfa9f6517c98c333fe746b9..3086fb97972f1548b7356670a7cb7ce9c2db7afe 100644 --- a/src/MissionManager/CorridorScanComplexItemTest.cc +++ b/src/MissionManager/CorridorScanComplexItemTest.cc @@ -23,8 +23,13 @@ void CorridorScanComplexItemTest::init(void) UnitTest::init(); _offlineVehicle = new Vehicle(MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, qgcApp()->toolbox()->firmwarePluginManager(), this); - _corridorItem = new CorridorScanComplexItem(_offlineVehicle, this); -// _corridorItem->setTurnaroundDist(0); // Unit test written for no turnaround distance + _corridorItem = new CorridorScanComplexItem(_offlineVehicle, false /* flyView */, QString() /* kmlFile */, this /* parent */); + + // vehicleSpeed need for terrain calcs + MissionController::MissionFlightStatus_t missionFlightStatus; + missionFlightStatus.vehicleSpeed = 5; + _corridorItem->setMissionFlightStatus(missionFlightStatus); + _setPolyline(); _corridorItem->setDirty(false); @@ -112,10 +117,10 @@ void CorridorScanComplexItemTest::_testEntryLocation(void) QList rgSeenEntryCoords; QList rgEntryLocation; - rgEntryLocation << SurveyMissionItem::EntryLocationTopLeft - << SurveyMissionItem::EntryLocationTopRight - << SurveyMissionItem::EntryLocationBottomLeft - << SurveyMissionItem::EntryLocationBottomRight; + rgEntryLocation << SurveyComplexItem::EntryLocationTopLeft + << SurveyComplexItem::EntryLocationTopRight + << SurveyComplexItem::EntryLocationBottomLeft + << SurveyComplexItem::EntryLocationBottomRight; // Validate that each entry location is unique for (int i=0; ireadyForSave()) { + return; + } + QTest::qWait(500); + } + QVERIFY(false); +} + void CorridorScanComplexItemTest::_testItemCount(void) { QList items; + _corridorItem->turnAroundDistance()->setRawValue(0); + _corridorItem->cameraTriggerInTurnAround()->setRawValue(true); + _corridorItem->appendMissionItems(items, this); + QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber()); + items.clear(); + + _corridorItem->turnAroundDistance()->setRawValue(0); + _corridorItem->cameraTriggerInTurnAround()->setRawValue(false); + _corridorItem->appendMissionItems(items, this); + QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber()); + items.clear(); + + _corridorItem->turnAroundDistance()->setRawValue(20); + _corridorItem->cameraTriggerInTurnAround()->setRawValue(true); + _corridorItem->appendMissionItems(items, this); + QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber()); + items.clear(); + _corridorItem->turnAroundDistance()->setRawValue(20); + _corridorItem->cameraTriggerInTurnAround()->setRawValue(false); + _corridorItem->appendMissionItems(items, this); + QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber()); + items.clear(); +#if 0 + // Terrain queries seem to take random amount of time so these don't work 100% + _corridorItem->setFollowTerrain(true); + + _corridorItem->turnAroundDistance()->setRawValue(0); + _corridorItem->cameraTriggerInTurnAround()->setRawValue(true); + _waitForReadyForSave(); + _corridorItem->appendMissionItems(items, this); + QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber()); + items.clear(); + + _corridorItem->turnAroundDistance()->setRawValue(0); _corridorItem->cameraTriggerInTurnAround()->setRawValue(false); + _waitForReadyForSave(); _corridorItem->appendMissionItems(items, this); QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber()); items.clear(); + _corridorItem->turnAroundDistance()->setRawValue(20); _corridorItem->cameraTriggerInTurnAround()->setRawValue(true); + _waitForReadyForSave(); + _corridorItem->appendMissionItems(items, this); + QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber()); + items.clear(); + + _corridorItem->turnAroundDistance()->setRawValue(20); + _corridorItem->cameraTriggerInTurnAround()->setRawValue(false); + _waitForReadyForSave(); _corridorItem->appendMissionItems(items, this); QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber()); items.clear(); +#endif } void CorridorScanComplexItemTest::_testPathChanges(void) diff --git a/src/MissionManager/CorridorScanComplexItemTest.h b/src/MissionManager/CorridorScanComplexItemTest.h index 2f793843801054b69263de9965d0f647dc05d358..7f252de7acb479b20fee212c82a4dcef7fdc3369 100644 --- a/src/MissionManager/CorridorScanComplexItemTest.h +++ b/src/MissionManager/CorridorScanComplexItemTest.h @@ -36,6 +36,7 @@ private slots: private: void _setPolyline(void); + void _waitForReadyForSave(void); enum { corridorPolygonPathChangedIndex = 0, diff --git a/src/MissionManager/FWLandingPattern.FactMetaData.json b/src/MissionManager/FWLandingPattern.FactMetaData.json index ca77c066c2ef877f4e729492601a63872e09bd9b..6d1a9a16ba32fdc7ea274189b9c82f9c8806b569 100644 --- a/src/MissionManager/FWLandingPattern.FactMetaData.json +++ b/src/MissionManager/FWLandingPattern.FactMetaData.json @@ -44,12 +44,12 @@ "defaultValue": 0.0 }, { - "name": "DescentRate", - "shortDescription": "Descent rate between landing and loiter altitude.", + "name": "GlideSlope", + "shortDescription": "The glide slope between the loiter and landing point.", "type": "double", - "units": "%", + "units": "deg", "min": 0.1, - "max": 100, + "max": 90, "decimalPlaces": 1, "defaultValue": 12.0 } diff --git a/src/MissionManager/FixedWingLandingComplexItem.cc b/src/MissionManager/FixedWingLandingComplexItem.cc index e81cb512abc96865179a319bcc15cbdc32363326..18873b703796ac5f04645573e1e3f40e7787deea 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.cc +++ b/src/MissionManager/FixedWingLandingComplexItem.cc @@ -25,18 +25,22 @@ const char* FixedWingLandingComplexItem::landingHeadingName = "LandingHead const char* FixedWingLandingComplexItem::loiterAltitudeName = "LoiterAltitude"; const char* FixedWingLandingComplexItem::loiterRadiusName = "LoiterRadius"; const char* FixedWingLandingComplexItem::landingAltitudeName = "LandingAltitude"; -const char* FixedWingLandingComplexItem::fallRateName = "DescentRate"; +const char* FixedWingLandingComplexItem::glideSlopeName = "GlideSlope"; const char* FixedWingLandingComplexItem::_jsonLoiterCoordinateKey = "loiterCoordinate"; const char* FixedWingLandingComplexItem::_jsonLoiterRadiusKey = "loiterRadius"; const char* FixedWingLandingComplexItem::_jsonLoiterClockwiseKey = "loiterClockwise"; -const char* FixedWingLandingComplexItem::_jsonLoiterAltitudeRelativeKey = "loiterAltitudeRelative"; const char* FixedWingLandingComplexItem::_jsonLandingCoordinateKey = "landCoordinate"; -const char* FixedWingLandingComplexItem::_jsonLandingAltitudeRelativeKey = "landAltitudeRelative"; +const char* FixedWingLandingComplexItem::_jsonValueSetIsDistanceKey = "valueSetIsDistance"; +const char* FixedWingLandingComplexItem::_jsonAltitudesAreRelativeKey = "altitudesAreRelative"; + +// Usage deprecated const char* FixedWingLandingComplexItem::_jsonFallRateKey = "fallRate"; +const char* FixedWingLandingComplexItem::_jsonLandingAltitudeRelativeKey = "landAltitudeRelative"; +const char* FixedWingLandingComplexItem::_jsonLoiterAltitudeRelativeKey = "loiterAltitudeRelative"; -FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObject* parent) - : ComplexMissionItem (vehicle, parent) +FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, bool flyView, QObject* parent) + : ComplexMissionItem (vehicle, flyView, parent) , _sequenceNumber (0) , _dirty (false) , _landingCoordSet (false) @@ -47,10 +51,10 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObje , _loiterRadiusFact (_metaDataMap[loiterRadiusName]) , _landingHeadingFact (_metaDataMap[landingHeadingName]) , _landingAltitudeFact (_metaDataMap[landingAltitudeName]) - , _fallRateFact (_metaDataMap[fallRateName]) + , _glideSlopeFact (_metaDataMap[glideSlopeName]) , _loiterClockwise (true) - , _loiterAltitudeRelative (true) - , _landingAltitudeRelative (true) + , _altitudesAreRelative (true) + , _valueSetIsDistance (true) { _editorQml = "qrc:/qml/FWLandingPatternEditor.qml"; @@ -66,6 +70,8 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObje connect(this, &FixedWingLandingComplexItem::loiterCoordinateChanged, this, &FixedWingLandingComplexItem::_recalcFromCoordinateChange); connect(this, &FixedWingLandingComplexItem::landingCoordinateChanged, this, &FixedWingLandingComplexItem::_recalcFromCoordinateChange); + connect(&_glideSlopeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_glideSlopeChanged); + connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty); connect(&_landingAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty); connect(&_landingDistanceFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty); @@ -74,11 +80,10 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObje connect(this, &FixedWingLandingComplexItem::loiterCoordinateChanged, this, &FixedWingLandingComplexItem::_setDirty); connect(this, &FixedWingLandingComplexItem::landingCoordinateChanged, this, &FixedWingLandingComplexItem::_setDirty); connect(this, &FixedWingLandingComplexItem::loiterClockwiseChanged, this, &FixedWingLandingComplexItem::_setDirty); - connect(this, &FixedWingLandingComplexItem::loiterAltitudeRelativeChanged, this, &FixedWingLandingComplexItem::_setDirty); - connect(this, &FixedWingLandingComplexItem::landingAltitudeRelativeChanged, this, &FixedWingLandingComplexItem::_setDirty); + connect(this, &FixedWingLandingComplexItem::altitudesAreRelativeChanged, this, &FixedWingLandingComplexItem::_setDirty); - connect(this, &FixedWingLandingComplexItem::loiterAltitudeRelativeChanged, this, &FixedWingLandingComplexItem::coordinateHasRelativeAltitudeChanged); - connect(this, &FixedWingLandingComplexItem::landingAltitudeRelativeChanged, this, &FixedWingLandingComplexItem::exitCoordinateHasRelativeAltitudeChanged); + connect(this, &FixedWingLandingComplexItem::altitudesAreRelativeChanged, this, &FixedWingLandingComplexItem::coordinateHasRelativeAltitudeChanged); + connect(this, &FixedWingLandingComplexItem::altitudesAreRelativeChanged, this, &FixedWingLandingComplexItem::exitCoordinateHasRelativeAltitudeChanged); } int FixedWingLandingComplexItem::lastSequenceNumber(void) const @@ -99,7 +104,7 @@ void FixedWingLandingComplexItem::save(QJsonArray& missionItems) { QJsonObject saveObject; - saveObject[JsonHelper::jsonVersionKey] = 1; + saveObject[JsonHelper::jsonVersionKey] = 2; saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; @@ -116,10 +121,10 @@ void FixedWingLandingComplexItem::save(QJsonArray& missionItems) JsonHelper::saveGeoCoordinate(coordinate, true /* writeAltitude */, jsonCoordinate); saveObject[_jsonLandingCoordinateKey] = jsonCoordinate; - saveObject[_jsonLoiterRadiusKey] = _loiterRadiusFact.rawValue().toDouble(); - saveObject[_jsonLoiterClockwiseKey] = _loiterClockwise; - saveObject[_jsonLoiterAltitudeRelativeKey] = _loiterAltitudeRelative; - saveObject[_jsonLandingAltitudeRelativeKey] = _landingAltitudeRelative; + saveObject[_jsonLoiterRadiusKey] = _loiterRadiusFact.rawValue().toDouble(); + saveObject[_jsonLoiterClockwiseKey] = _loiterClockwise; + saveObject[_jsonAltitudesAreRelativeKey] = _altitudesAreRelative; + saveObject[_jsonValueSetIsDistanceKey] = _valueSetIsDistance; missionItems.append(saveObject); } @@ -142,9 +147,7 @@ bool FixedWingLandingComplexItem::load(const QJsonObject& complexObject, int seq { _jsonLoiterCoordinateKey, QJsonValue::Array, true }, { _jsonLoiterRadiusKey, QJsonValue::Double, true }, { _jsonLoiterClockwiseKey, QJsonValue::Bool, true }, - { _jsonLoiterAltitudeRelativeKey, QJsonValue::Bool, true }, { _jsonLandingCoordinateKey, QJsonValue::Array, true }, - { _jsonLandingAltitudeRelativeKey, QJsonValue::Bool, true }, }; if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) { return false; @@ -161,6 +164,45 @@ bool FixedWingLandingComplexItem::load(const QJsonObject& complexObject, int seq _ignoreRecalcSignals = true; + int version = complexObject[JsonHelper::jsonVersionKey].toInt(); + if (version == 1) { + QList v1KeyInfoList = { + { _jsonLoiterAltitudeRelativeKey, QJsonValue::Bool, true }, + { _jsonLandingAltitudeRelativeKey, QJsonValue::Bool, true }, + }; + if (!JsonHelper::validateKeys(complexObject, v1KeyInfoList, errorString)) { + return false; + } + + bool loiterAltitudeRelative = complexObject[_jsonLoiterAltitudeRelativeKey].toBool(); + bool landingAltitudeRelative = complexObject[_jsonLandingAltitudeRelativeKey].toBool(); + if (loiterAltitudeRelative != landingAltitudeRelative) { + qgcApp()->showMessage(tr("Fixed Wing Landing Pattern: " + "Setting the loiter and landing altitudes with different settings for altitude relative is no longer supported. " + "Both have been set to altitude relative. Be sure to adjust/check your plan prior to flight.")); + _altitudesAreRelative = true; + } else { + _altitudesAreRelative = loiterAltitudeRelative; + } + _valueSetIsDistance = true; + } else if (version == 2) { + QList v2KeyInfoList = { + { _jsonAltitudesAreRelativeKey, QJsonValue::Bool, true }, + { _jsonValueSetIsDistanceKey, QJsonValue::Bool, true }, + }; + if (!JsonHelper::validateKeys(complexObject, v2KeyInfoList, errorString)) { + _ignoreRecalcSignals = false; + return false; + } + + _altitudesAreRelative = complexObject[_jsonAltitudesAreRelativeKey].toBool(); + _valueSetIsDistance = complexObject[_jsonValueSetIsDistanceKey].toBool(); + } else { + errorString = tr("%1 complex item version %2 not supported").arg(jsonComplexItemTypeValue).arg(version); + _ignoreRecalcSignals = false; + return false; + } + QGeoCoordinate coordinate; if (!JsonHelper::loadGeoCoordinate(complexObject[_jsonLoiterCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) { return false; @@ -175,9 +217,9 @@ bool FixedWingLandingComplexItem::load(const QJsonObject& complexObject, int seq _landingAltitudeFact.setRawValue(coordinate.altitude()); _loiterRadiusFact.setRawValue(complexObject[_jsonLoiterRadiusKey].toDouble()); - _loiterClockwise = complexObject[_jsonLoiterClockwiseKey].toBool(); - _loiterAltitudeRelative = complexObject[_jsonLoiterAltitudeRelativeKey].toBool(); - _landingAltitudeRelative = complexObject[_jsonLandingAltitudeRelativeKey].toBool(); + _loiterClockwise = complexObject[_jsonLoiterClockwiseKey].toBool(); + + _calcGlideSlope(); _landingCoordSet = true; @@ -215,7 +257,7 @@ void FixedWingLandingComplexItem::appendMissionItems(QList& items, float loiterRadius = _loiterRadiusFact.rawValue().toDouble() * (_loiterClockwise ? 1.0 : -1.0); item = new MissionItem(seqNum++, MAV_CMD_NAV_LOITER_TO_ALT, - _loiterAltitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, + _altitudesAreRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, 1.0, // Heading required = true loiterRadius, // Loiter radius 0.0, // param 3 - unused @@ -230,7 +272,7 @@ void FixedWingLandingComplexItem::appendMissionItems(QList& items, item = new MissionItem(seqNum++, MAV_CMD_NAV_LAND, - _landingAltitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, + _altitudesAreRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, 0.0, 0.0, 0.0, 0.0, // param 1-4 _landingCoordinate.latitude(), _landingCoordinate.longitude(), @@ -241,7 +283,7 @@ void FixedWingLandingComplexItem::appendMissionItems(QList& items, items.append(item); } -bool FixedWingLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, Vehicle* vehicle) +bool FixedWingLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, bool flyView, Vehicle* vehicle) { qCDebug(FixedWingLandingComplexItemLog) << "FixedWingLandingComplexItem::scanForItem count" << visualItems->count(); @@ -261,6 +303,7 @@ bool FixedWingLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, V missionItemLand.param1() != 0 || missionItemLand.param2() != 0 || missionItemLand.param3() != 0 || missionItemLand.param4() == 1.0) { return false; } + MAV_FRAME landPointFrame = missionItemLand.frame(); item = visualItems->value(lastItem--); if (!item) { @@ -268,7 +311,7 @@ bool FixedWingLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, V } MissionItem& missionItemLoiter = item->missionItem(); if (missionItemLoiter.command() != MAV_CMD_NAV_LOITER_TO_ALT || - !(missionItemLoiter.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT || missionItemLoiter.frame() == MAV_FRAME_GLOBAL) || + missionItemLoiter.frame() != landPointFrame || missionItemLoiter.param1() != 1.0 || missionItemLoiter.param3() != 0 || missionItemLoiter.param4() != 1.0) { return false; } @@ -279,28 +322,29 @@ bool FixedWingLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, V } MissionItem& missionItemDoLandStart = item->missionItem(); if (missionItemDoLandStart.command() != MAV_CMD_DO_LAND_START || - missionItemDoLandStart.param1() != 0 || missionItemDoLandStart.param2() != 0 || missionItemDoLandStart.param3() != 0 || missionItemDoLandStart.param4() != 0|| missionItemDoLandStart.param5() != 0|| missionItemDoLandStart.param6() != 0|| missionItemDoLandStart.param6() != 0) { + missionItemDoLandStart.param1() != 0 || missionItemDoLandStart.param2() != 0 || missionItemDoLandStart.param3() != 0 || missionItemDoLandStart.param4() != 0|| missionItemDoLandStart.param5() != 0|| missionItemDoLandStart.param6() != 0) { return false; } // We made it this far so we do have a Fixed Wing Landing Pattern item at the end of the mission - FixedWingLandingComplexItem* complexItem = new FixedWingLandingComplexItem(vehicle, visualItems); + FixedWingLandingComplexItem* complexItem = new FixedWingLandingComplexItem(vehicle, flyView, visualItems); complexItem->_ignoreRecalcSignals = true; - complexItem->_loiterAltitudeRelative = missionItemLoiter.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT; + complexItem->_altitudesAreRelative = landPointFrame == MAV_FRAME_GLOBAL_RELATIVE_ALT; complexItem->_loiterRadiusFact.setRawValue(qAbs(missionItemLoiter.param2())); complexItem->_loiterClockwise = missionItemLoiter.param2() > 0; complexItem->_loiterCoordinate.setLatitude(missionItemLoiter.param5()); complexItem->_loiterCoordinate.setLongitude(missionItemLoiter.param6()); complexItem->_loiterAltitudeFact.setRawValue(missionItemLoiter.param7()); - complexItem->_landingAltitudeRelative = missionItemLand.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT; complexItem->_landingCoordinate.setLatitude(missionItemLand.param5()); complexItem->_landingCoordinate.setLongitude(missionItemLand.param6()); complexItem->_landingAltitudeFact.setRawValue(missionItemLand.param7()); + complexItem->_calcGlideSlope(); + complexItem->_landingCoordSet = true; complexItem->_ignoreRecalcSignals = false; @@ -418,6 +462,7 @@ void FixedWingLandingComplexItem::_recalcFromHeadingAndDistanceChange(void) // Adjusted: // loiter // loiter tangent + // glide slope if (!_ignoreRecalcSignals && _landingCoordSet) { // These are our known values @@ -442,6 +487,7 @@ void FixedWingLandingComplexItem::_recalcFromHeadingAndDistanceChange(void) emit loiterTangentCoordinateChanged(_loiterTangentCoordinate); emit loiterCoordinateChanged(_loiterCoordinate); emit coordinateChanged(_loiterCoordinate); + _calcGlideSlope(); _ignoreRecalcSignals = false; } } @@ -467,6 +513,7 @@ void FixedWingLandingComplexItem::_recalcFromCoordinateChange(void) // loiter tangent // heading // distance + // glide slope if (!_ignoreRecalcSignals && _landingCoordSet) { // These are our known values @@ -493,6 +540,7 @@ void FixedWingLandingComplexItem::_recalcFromCoordinateChange(void) _landingHeadingFact.setRawValue(heading); _landingDistanceFact.setRawValue(landToTangentDistance); emit loiterTangentCoordinateChanged(_loiterTangentCoordinate); + _calcGlideSlope(); _ignoreRecalcSignals = false; } } @@ -519,3 +567,20 @@ void FixedWingLandingComplexItem::applyNewAltitude(double newAltitude) { _loiterAltitudeFact.setRawValue(newAltitude); } + +void FixedWingLandingComplexItem::_glideSlopeChanged(void) +{ + if (!_ignoreRecalcSignals) { + double landingAltDifference = _loiterAltitudeFact.rawValue().toDouble() - _landingAltitudeFact.rawValue().toDouble(); + double glideSlope = _glideSlopeFact.rawValue().toDouble(); + _landingDistanceFact.setRawValue(landingAltDifference / qTan(qDegreesToRadians(glideSlope))); + } +} + +void FixedWingLandingComplexItem::_calcGlideSlope(void) +{ + double landingAltDifference = _loiterAltitudeFact.rawValue().toDouble() - _landingAltitudeFact.rawValue().toDouble(); + double landingDistance = _landingDistanceFact.rawValue().toDouble(); + + _glideSlopeFact.setRawValue(qRadiansToDegrees(qAtan(landingAltDifference / landingDistance))); +} diff --git a/src/MissionManager/FixedWingLandingComplexItem.h b/src/MissionManager/FixedWingLandingComplexItem.h index 9d04c053af843586181049b071bdd2317d2d71e8..cc08a0996787b192ba06cfc8c79de0f88f4b3679 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.h +++ b/src/MissionManager/FixedWingLandingComplexItem.h @@ -22,28 +22,28 @@ class FixedWingLandingComplexItem : public ComplexMissionItem Q_OBJECT public: - FixedWingLandingComplexItem(Vehicle* vehicle, QObject* parent = NULL); - - Q_PROPERTY(Fact* loiterAltitude READ loiterAltitude CONSTANT) - Q_PROPERTY(Fact* loiterRadius READ loiterRadius CONSTANT) - Q_PROPERTY(Fact* landingAltitude READ landingAltitude CONSTANT) - Q_PROPERTY(Fact* landingDistance READ landingDistance CONSTANT) - Q_PROPERTY(Fact* landingHeading READ landingHeading CONSTANT) - Q_PROPERTY(Fact* fallRate READ fallRate CONSTANT) - Q_PROPERTY(bool loiterClockwise MEMBER _loiterClockwise NOTIFY loiterClockwiseChanged) - Q_PROPERTY(bool loiterAltitudeRelative MEMBER _loiterAltitudeRelative NOTIFY loiterAltitudeRelativeChanged) - Q_PROPERTY(bool landingAltitudeRelative MEMBER _landingAltitudeRelative NOTIFY landingAltitudeRelativeChanged) - Q_PROPERTY(QGeoCoordinate loiterCoordinate READ loiterCoordinate WRITE setLoiterCoordinate NOTIFY loiterCoordinateChanged) - Q_PROPERTY(QGeoCoordinate loiterTangentCoordinate READ loiterTangentCoordinate NOTIFY loiterTangentCoordinateChanged) - Q_PROPERTY(QGeoCoordinate landingCoordinate READ landingCoordinate WRITE setLandingCoordinate NOTIFY landingCoordinateChanged) - Q_PROPERTY(bool landingCoordSet MEMBER _landingCoordSet NOTIFY landingCoordSetChanged) + FixedWingLandingComplexItem(Vehicle* vehicle, bool flyView, QObject* parent); + + Q_PROPERTY(Fact* loiterAltitude READ loiterAltitude CONSTANT) + Q_PROPERTY(Fact* loiterRadius READ loiterRadius CONSTANT) + Q_PROPERTY(Fact* landingAltitude READ landingAltitude CONSTANT) + Q_PROPERTY(Fact* landingHeading READ landingHeading CONSTANT) + Q_PROPERTY(bool valueSetIsDistance MEMBER _valueSetIsDistance NOTIFY valueSetIsDistanceChanged) + Q_PROPERTY(Fact* landingDistance READ landingDistance CONSTANT) + Q_PROPERTY(Fact* glideSlope READ glideSlope CONSTANT) + Q_PROPERTY(bool loiterClockwise MEMBER _loiterClockwise NOTIFY loiterClockwiseChanged) + Q_PROPERTY(bool altitudesAreRelative MEMBER _altitudesAreRelative NOTIFY altitudesAreRelativeChanged) + Q_PROPERTY(QGeoCoordinate loiterCoordinate READ loiterCoordinate WRITE setLoiterCoordinate NOTIFY loiterCoordinateChanged) + Q_PROPERTY(QGeoCoordinate loiterTangentCoordinate READ loiterTangentCoordinate NOTIFY loiterTangentCoordinateChanged) + Q_PROPERTY(QGeoCoordinate landingCoordinate READ landingCoordinate WRITE setLandingCoordinate NOTIFY landingCoordinateChanged) + Q_PROPERTY(bool landingCoordSet MEMBER _landingCoordSet NOTIFY landingCoordSetChanged) Fact* loiterAltitude (void) { return &_loiterAltitudeFact; } Fact* loiterRadius (void) { return &_loiterRadiusFact; } Fact* landingAltitude (void) { return &_landingAltitudeFact; } Fact* landingDistance (void) { return &_landingDistanceFact; } Fact* landingHeading (void) { return &_landingHeadingFact; } - Fact* fallRate (void) { return &_fallRateFact; } + Fact* glideSlope (void) { return &_glideSlopeFact; } QGeoCoordinate landingCoordinate (void) const { return _landingCoordinate; } QGeoCoordinate loiterCoordinate (void) const { return _loiterCoordinate; } QGeoCoordinate loiterTangentCoordinate (void) const { return _loiterTangentCoordinate; } @@ -52,7 +52,7 @@ public: void setLoiterCoordinate (const QGeoCoordinate& coordinate); /// Scans the loaded items for a landing pattern complex item - static bool scanForItem(QmlObjectListModel* visualItems, Vehicle* vehicle); + static bool scanForItem(QmlObjectListModel* visualItems, bool flyView, Vehicle* vehicle); // Overrides from ComplexMissionItem @@ -81,8 +81,8 @@ public: void appendMissionItems (QList& items, QObject* missionItemParent) final; void applyNewAltitude (double newAltitude) final; - bool coordinateHasRelativeAltitude (void) const final { return _loiterAltitudeRelative; } - bool exitCoordinateHasRelativeAltitude (void) const final { return _landingAltitudeRelative; } + bool coordinateHasRelativeAltitude (void) const final { return _altitudesAreRelative; } + bool exitCoordinateHasRelativeAltitude (void) const final { return _altitudesAreRelative; } bool exitCoordinateSameAsEntry (void) const final { return false; } void setDirty (bool dirty) final; @@ -97,7 +97,7 @@ public: static const char* loiterRadiusName; static const char* landingHeadingName; static const char* landingAltitudeName; - static const char* fallRateName; + static const char* glideSlopeName; signals: void loiterCoordinateChanged (QGeoCoordinate coordinate); @@ -105,21 +105,23 @@ signals: void landingCoordinateChanged (QGeoCoordinate coordinate); void landingCoordSetChanged (bool landingCoordSet); void loiterClockwiseChanged (bool loiterClockwise); - void loiterAltitudeRelativeChanged (bool loiterAltitudeRelative); - void landingAltitudeRelativeChanged (bool loiterAltitudeRelative); + void altitudesAreRelativeChanged (bool altitudesAreRelative); + void valueSetIsDistanceChanged (bool valueSetIsDistance); private slots: - void _recalcFromHeadingAndDistanceChange(void); - void _recalcFromCoordinateChange(void); - void _recalcFromRadiusChange(void); - void _updateLoiterCoodinateAltitudeFromFact(void); - void _updateLandingCoodinateAltitudeFromFact(void); - double _mathematicAngleToHeading(double angle); - double _headingToMathematicAngle(double heading); - void _setDirty(void); + void _recalcFromHeadingAndDistanceChange (void); + void _recalcFromCoordinateChange (void); + void _recalcFromRadiusChange (void); + void _updateLoiterCoodinateAltitudeFromFact (void); + void _updateLandingCoodinateAltitudeFromFact (void); + double _mathematicAngleToHeading (double angle); + double _headingToMathematicAngle (double heading); + void _setDirty (void); + void _glideSlopeChanged (void); private: - QPointF _rotatePoint(const QPointF& point, const QPointF& origin, double angle); + QPointF _rotatePoint (const QPointF& point, const QPointF& origin, double angle); + void _calcGlideSlope (void); int _sequenceNumber; bool _dirty; @@ -136,18 +138,22 @@ private: Fact _loiterRadiusFact; Fact _landingHeadingFact; Fact _landingAltitudeFact; - Fact _fallRateFact; + Fact _glideSlopeFact; bool _loiterClockwise; - bool _loiterAltitudeRelative; - bool _landingAltitudeRelative; + bool _altitudesAreRelative; + bool _valueSetIsDistance; static const char* _jsonLoiterCoordinateKey; static const char* _jsonLoiterRadiusKey; static const char* _jsonLoiterClockwiseKey; - static const char* _jsonLoiterAltitudeRelativeKey; static const char* _jsonLandingCoordinateKey; + static const char* _jsonValueSetIsDistanceKey; + static const char* _jsonAltitudesAreRelativeKey; + + // Only in older file formats static const char* _jsonLandingAltitudeRelativeKey; + static const char* _jsonLoiterAltitudeRelativeKey; static const char* _jsonFallRateKey; }; diff --git a/src/MissionManager/GeoFenceController.cc b/src/MissionManager/GeoFenceController.cc index 64980ba51b7d02ae0d6b3455edffd2c8c5d341dd..254790b3da6d49f3a1cb98306c5060ca41e4cc20 100644 --- a/src/MissionManager/GeoFenceController.cc +++ b/src/MissionManager/GeoFenceController.cc @@ -32,14 +32,19 @@ QGC_LOGGING_CATEGORY(GeoFenceControllerLog, "GeoFenceControllerLog") -const char* GeoFenceController::_jsonFileTypeValue = "GeoFence"; -const char* GeoFenceController::_jsonBreachReturnKey = "breachReturn"; +const char* GeoFenceController::_jsonFileTypeValue = "GeoFence"; +const char* GeoFenceController::_jsonBreachReturnKey = "breachReturn"; +const char* GeoFenceController::_jsonPolygonsKey = "polygons"; +const char* GeoFenceController::_jsonCirclesKey = "circles"; + +const char* GeoFenceController::_px4ParamCircularFence = "GF_MAX_HOR_DIST"; GeoFenceController::GeoFenceController(PlanMasterController* masterController, QObject* parent) - : PlanElementController(masterController, parent) - , _geoFenceManager(_managerVehicle->geoFenceManager()) - , _dirty(false) - , _itemsRequested(false) + : PlanElementController (masterController, parent) + , _geoFenceManager (_managerVehicle->geoFenceManager()) + , _dirty (false) + , _itemsRequested (false) + , _px4ParamCircularFenceFact(NULL) { connect(&_polygons, &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems); connect(&_circles, &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems); @@ -52,11 +57,11 @@ GeoFenceController::~GeoFenceController() } -void GeoFenceController::start(bool editMode) +void GeoFenceController::start(bool flyView) { - qCDebug(GeoFenceControllerLog) << "start editMode" << editMode; + qCDebug(GeoFenceControllerLog) << "start flyView" << flyView; - PlanElementController::start(editMode); + PlanElementController::start(flyView); _init(); } @@ -86,6 +91,7 @@ void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle) if (_managerVehicle) { _geoFenceManager->disconnect(this); _managerVehicle->disconnect(this); + _managerVehicle->parameterManager()->disconnect(this); _managerVehicle = NULL; _geoFenceManager = NULL; } @@ -102,7 +108,10 @@ void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle) connect(_geoFenceManager, &GeoFenceManager::removeAllComplete, this, &GeoFenceController::_managerRemoveAllComplete); connect(_geoFenceManager, &GeoFenceManager::inProgressChanged, this, &GeoFenceController::syncInProgressChanged); - connect(_managerVehicle, &Vehicle::capabilityBitsChanged, this, &RallyPointController::supportedChanged); + connect(_managerVehicle, &Vehicle::capabilityBitsChanged, this, &GeoFenceController::supportedChanged); + + connect(_managerVehicle->parameterManager(), &ParameterManager::parametersReadyChanged, this, &GeoFenceController::_parametersReady); + _parametersReady(); emit supportedChanged(supported()); _signalAll(); @@ -110,46 +119,84 @@ void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle) bool GeoFenceController::load(const QJsonObject& json, QString& errorString) { - Q_UNUSED(json); - Q_UNUSED(errorString); + removeAll(); -#if 0 - QString errorStr; - QString errorMessage = tr("GeoFence: %1"); + errorString.clear(); - if (json.contains(_jsonBreachReturnKey) && - !JsonHelper::loadGeoCoordinate(json[_jsonBreachReturnKey], false /* altitudeRequired */, _breachReturnPoint, errorStr)) { - errorString = errorMessage.arg(errorStr); + if (json.contains(JsonHelper::jsonVersionKey) && json[JsonHelper::jsonVersionKey].toInt() == 1) { + // We just ignore old version 1 data + return true; + } + + QList keyInfoList = { + { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, + { _jsonCirclesKey, QJsonValue::Array, true }, + { _jsonPolygonsKey, QJsonValue::Array, true }, + }; + if (!JsonHelper::validateKeys(json, keyInfoList, errorString)) { return false; } - if (!_mapPolygon.loadFromJson(json, true, errorStr)) { - errorString = errorMessage.arg(errorStr); + if (json[JsonHelper::jsonVersionKey].toInt() != _jsonCurrentVersion) { + errorString = tr("GeoFence supports version %1").arg(_jsonCurrentVersion); return false; } - _mapPolygon.setDirty(false); - setDirty(false); + QJsonArray jsonPolygonArray = json[_jsonPolygonsKey].toArray(); + foreach (const QJsonValue& jsonPolygonValue, jsonPolygonArray) { + if (jsonPolygonValue.type() != QJsonValue::Object) { + errorString = tr("GeoFence polygon not stored as object"); + return false; + } + + QGCFencePolygon* fencePolygon = new QGCFencePolygon(false /* inclusion */, this /* parent */); + if (!fencePolygon->loadFromJson(jsonPolygonValue.toObject(), true /* required */, errorString)) { + return false; + } + _polygons.append(fencePolygon); + } + + QJsonArray jsonCircleArray = json[_jsonCirclesKey].toArray(); + foreach (const QJsonValue& jsonCircleValue, jsonCircleArray) { + if (jsonCircleValue.type() != QJsonValue::Object) { + errorString = tr("GeoFence circle not stored as object"); + return false; + } + + QGCFenceCircle* fenceCircle = new QGCFenceCircle(this /* parent */); + if (!fenceCircle->loadFromJson(jsonCircleValue.toObject(), errorString)) { + return false; + } + _circles.append(fenceCircle); + } + + setDirty(false); _signalAll(); -#endif return true; } -void GeoFenceController::save(QJsonObject& json) +void GeoFenceController::save(QJsonObject& json) { - Q_UNUSED(json); -#if 0 - json[JsonHelper::jsonVersionKey] = 1; + json[JsonHelper::jsonVersionKey] = _jsonCurrentVersion; - if (_breachReturnPoint.isValid()) { - QJsonValue jsonBreachReturn; - JsonHelper::saveGeoCoordinate(_breachReturnPoint, false /* writeAltitude */, jsonBreachReturn); - json[_jsonBreachReturnKey] = jsonBreachReturn; + QJsonArray jsonPolygonArray; + for (int i=0; i<_polygons.count(); i++) { + QJsonObject jsonPolygon; + QGCFencePolygon* fencePolygon = _polygons.value(i); + fencePolygon->saveToJson(jsonPolygon); + jsonPolygonArray.append(jsonPolygon); } + json[_jsonPolygonsKey] = jsonPolygonArray; - _mapPolygon.saveToJson(json); -#endif + QJsonArray jsonCircleArray; + for (int i=0; i<_circles.count(); i++) { + QJsonObject jsonCircle; + QGCFenceCircle* fenceCircle = _circles.value(i); + fenceCircle->saveToJson(jsonCircle); + jsonCircleArray.append(jsonCircle); + } + json[_jsonCirclesKey] = jsonCircleArray; } void GeoFenceController::removeAll(void) @@ -263,7 +310,7 @@ void GeoFenceController::_managerLoadComplete(void) { // Fly view always reloads on _loadComplete // Plan view only reloads on _loadComplete if specifically requested - if (!_editMode || _itemsRequested) { + if (_flyView || _itemsRequested) { _setReturnPointFromManager(_geoFenceManager->breachReturnPoint()); _setFenceFromManager(_geoFenceManager->polygons(), _geoFenceManager->circles()); setDirty(false); @@ -276,7 +323,7 @@ void GeoFenceController::_managerLoadComplete(void) void GeoFenceController::_managerSendComplete(bool error) { // Fly view always reloads on manager sendComplete - if (!error && !_editMode) { + if (!error && _flyView) { showPlanFromManagerVehicle(); } } @@ -301,7 +348,7 @@ void GeoFenceController::_updateContainsItems(void) bool GeoFenceController::showPlanFromManagerVehicle(void) { - qCDebug(GeoFenceControllerLog) << "showPlanFromManagerVehicle" << _editMode; + qCDebug(GeoFenceControllerLog) << "showPlanFromManagerVehicle _flyView" << _flyView; if (_masterController->offline()) { qCWarning(GeoFenceControllerLog) << "GeoFenceController::showPlanFromManagerVehicle called while offline"; return true; // stops further propagation of showPlanFromManagerVehicle due to error @@ -413,3 +460,30 @@ bool GeoFenceController::supported(void) const { return (_managerVehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE) && (_managerVehicle->maxProtoVersion() >= 200); } + +// Hack for PX4 +double GeoFenceController::paramCircularFence(void) +{ + if (_managerVehicle->isOfflineEditingVehicle() || !_managerVehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _px4ParamCircularFence)) { + return 0; + } + + return _managerVehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _px4ParamCircularFence)->rawValue().toDouble(); +} + +void GeoFenceController::_parametersReady(void) +{ + if (_px4ParamCircularFenceFact) { + _px4ParamCircularFenceFact->disconnect(this); + _px4ParamCircularFenceFact = NULL; + } + + if (_managerVehicle->isOfflineEditingVehicle() || !_managerVehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _px4ParamCircularFence)) { + emit paramCircularFenceChanged(); + return; + } + + _px4ParamCircularFenceFact = _managerVehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _px4ParamCircularFence); + connect(_px4ParamCircularFenceFact, &Fact::rawValueChanged, this, &GeoFenceController::paramCircularFenceChanged); + emit paramCircularFenceChanged(); +} diff --git a/src/MissionManager/GeoFenceController.h b/src/MissionManager/GeoFenceController.h index dd13cec9256fc0493932468321570935d343fbf4..9412fc00530c0ca81c0408bf10af327ec5e59502 100644 --- a/src/MissionManager/GeoFenceController.h +++ b/src/MissionManager/GeoFenceController.h @@ -34,6 +34,9 @@ public: Q_PROPERTY(QmlObjectListModel* circles READ circles CONSTANT) Q_PROPERTY(QGeoCoordinate breachReturnPoint READ breachReturnPoint WRITE setBreachReturnPoint NOTIFY breachReturnPointChanged) + // Hack to expose PX4 circular fence controlled by GF_MAX_HOR_DIST + Q_PROPERTY(double paramCircularFence READ paramCircularFence NOTIFY paramCircularFenceChanged) + /// Add a new inclusion polygon to the fence /// @param topLeft - Top left coordinate or map viewport /// @param topLeft - Bottom right left coordinate or map viewport @@ -55,8 +58,11 @@ public: /// Clears the interactive bit from all fence items Q_INVOKABLE void clearAllInteractive(void); + double paramCircularFence(void); + + // Overrides from PlanElementController bool supported (void) const final; - void start (bool editMode) final; + void start (bool flyView) final; void save (QJsonObject& json) final; bool load (const QJsonObject& json, QString& errorString) final; void loadFromVehicle (void) final; @@ -80,17 +86,18 @@ signals: void breachReturnPointChanged (QGeoCoordinate breachReturnPoint); void editorQmlChanged (QString editorQml); void loadComplete (void); + void paramCircularFenceChanged (void); private slots: - void _polygonDirtyChanged(bool dirty); - void _setDirty(void); - void _setFenceFromManager(const QList& polygons, - const QList& circles); - void _setReturnPointFromManager(QGeoCoordinate breachReturnPoint); - void _managerLoadComplete(void); - void _updateContainsItems(void); - void _managerSendComplete(bool error); - void _managerRemoveAllComplete(bool error); + void _polygonDirtyChanged (bool dirty); + void _setDirty (void); + void _setFenceFromManager (const QList& polygons, const QList& circles); + void _setReturnPointFromManager (QGeoCoordinate breachReturnPoint); + void _managerLoadComplete (void); + void _updateContainsItems (void); + void _managerSendComplete (bool error); + void _managerRemoveAllComplete (bool error); + void _parametersReady (void); private: void _init(void); @@ -102,9 +109,16 @@ private: QmlObjectListModel _circles; QGeoCoordinate _breachReturnPoint; bool _itemsRequested; + Fact* _px4ParamCircularFenceFact; + + static const char* _px4ParamCircularFence; + + static const int _jsonCurrentVersion = 2; static const char* _jsonFileTypeValue; static const char* _jsonBreachReturnKey; + static const char* _jsonPolygonsKey; + static const char* _jsonCirclesKey; }; #endif diff --git a/src/MissionManager/MavCmdInfoCommon.json b/src/MissionManager/MavCmdInfoCommon.json index 9a7dd6cbcd962409dda9e1d84456787cdde3d03a..49c116bab9d0b7a15b7399f9d4f4822b4ef2f881 100644 --- a/src/MissionManager/MavCmdInfoCommon.json +++ b/src/MissionManager/MavCmdInfoCommon.json @@ -640,6 +640,7 @@ "friendlyName": "ROI to next waypoint" , "description": "Sets the region of interest to point towards the next waypoint with optional offsets.", "specifiesCoordinate": false, + "standaloneCoordinate": true, "friendlyEdit": true, "category": "Camera", "param5": { @@ -1005,11 +1006,6 @@ "friendlyName": "Start image capture" , "description": "Start taking one or more photos.", "category": "Camera", - "param1": { - "label": "Reserved", - "default": 0, - "decimalPlaces": 0 - }, "param2": { "label": "Interval", "default": 0, @@ -1027,12 +1023,7 @@ "rawName": "MAV_CMD_IMAGE_STOP_CAPTURE", "friendlyName": "Stop image capture", "description": "Stop taking photos.", - "category": "Camera", - "param1": { - "label": "Reserved", - "default": 0, - "decimalPlaces": 0 - } + "category": "Camera" }, { "id": 2003, "rawName": "MAV_CMD_DO_TRIGGER_CONTROL", "friendlyName": "Trigger control" }, { @@ -1041,10 +1032,11 @@ "friendlyName": "Start video capture", "description": "Start video capture.", "category": "Camera", - "param1": { - "label": "Reserved", - "default": 0, - "decimalPlaces": 0 + "param2": { + "label": "Status Frequency", + "default": 0.2, + "units": "Hz", + "decimalPlaces": 2 } }, { @@ -1052,12 +1044,7 @@ "rawName": "MAV_CMD_VIDEO_STOP_CAPTURE", "friendlyName": "Stop video capture", "description": "Stop video capture.", - "category": "Camera", - "param1": { - "label": "Reserved", - "default": 0, - "decimalPlaces": 0 - } + "category": "Camera" }, { "id": 2800, "rawName": "MAV_CMD_PANORAMA_CREATE", "friendlyName": "Create panorama" }, { diff --git a/src/MissionManager/MissionCommandList.h b/src/MissionManager/MissionCommandList.h index 79c6ef633dc1c29c9307269e8cfae6ef0e2e47b3..319020002311e3b50400e6eb2679326ff1cb6c96 100644 --- a/src/MissionManager/MissionCommandList.h +++ b/src/MissionManager/MissionCommandList.h @@ -14,7 +14,6 @@ #include "QGCMAVLink.h" #include "QGCLoggingCategory.h" #include "QmlObjectListModel.h" -#include "MavlinkQmlSingleton.h" #include #include diff --git a/src/MissionManager/MissionCommandTree.cc b/src/MissionManager/MissionCommandTree.cc index 9513a047929d29b28725a04ac3783457f26c6ead..c2db1a1fc6a5520039e0add1597dda6020b6f754 100644 --- a/src/MissionManager/MissionCommandTree.cc +++ b/src/MissionManager/MissionCommandTree.cc @@ -239,6 +239,11 @@ QVariantList MissionCommandTree::getCommandsForCategory(Vehicle* vehicle, const QVariantList list; QMap commandMap = _availableCommands[baseFirmwareType][baseVehicleType]; foreach (MAV_CMD command, commandMap.keys()) { + if (command == MAV_CMD_NAV_LAST) { + // MAV_CMD_NAV_LAST is used for Mission Settings item. Although we want to be able to get command info for it. + // The user should not be able to use it as a command. + continue; + } MissionCommandUIInfo* uiInfo = commandMap[command]; if (uiInfo->category() == category || category == _allCommandsCategory) { list.append(QVariant::fromValue(uiInfo)); diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index b093c3f58acc82e934cbb2104249a54ee7e7dcd9..572e0ca03840753e1da0fd3cb860530e9e3286d8 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -16,7 +16,7 @@ #include "FirmwarePlugin.h" #include "QGCApplication.h" #include "SimpleMissionItem.h" -#include "SurveyMissionItem.h" +#include "SurveyComplexItem.h" #include "FixedWingLandingComplexItem.h" #include "StructureScanComplexItem.h" #include "CorridorScanComplexItem.h" @@ -123,11 +123,11 @@ void MissionController::_resetMissionFlightStatus(void) } -void MissionController::start(bool editMode) +void MissionController::start(bool flyView) { - qCDebug(MissionControllerLog) << "start editMode" << editMode; + qCDebug(MissionControllerLog) << "start flyView" << flyView; - PlanElementController::start(editMode); + PlanElementController::start(flyView); _init(); } @@ -146,7 +146,7 @@ void MissionController::_newMissionItemsAvailableFromVehicle(bool removeAllReque // Fly view always reloads on _loadComplete // Plan view only reloads on _loadComplete if specifically requested - if (!_editMode || removeAllRequested || _itemsRequested) { + if (_flyView || removeAllRequested || _itemsRequested) { // Fly Mode (accept if): // - Always accepts new items from the vehicle so Fly view is kept up to date // Edit Mode (accept if): @@ -176,7 +176,7 @@ void MissionController::_newMissionItemsAvailableFromVehicle(bool removeAllReque for (; iappend(new SimpleMissionItem(_controllerVehicle, _editMode, *missionItem, this)); + newControllerMissionItems->append(new SimpleMissionItem(_controllerVehicle, _flyView, *missionItem, this)); } _deinitAllVisualItems(); @@ -187,7 +187,7 @@ void MissionController::_newMissionItemsAvailableFromVehicle(bool removeAllReque _visualItems = newControllerMissionItems; if (!_controllerVehicle->firmwarePlugin()->sendHomePositionToVehicle() || _visualItems->count() == 0) { - _addMissionSettings(_visualItems, _editMode && _visualItems->count() > 0 /* addToCenter */); + _addMissionSettings(_visualItems, !_flyView && _visualItems->count() > 0 /* addToCenter */); } MissionController::_scanForAdditionalSettings(_visualItems, _controllerVehicle); @@ -265,25 +265,21 @@ bool MissionController::_convertToMissionItems(QmlObjectListModel* visualMission void MissionController::convertToKMLDocument(QDomDocument& document) { - QJsonObject missionJson; - QmlObjectListModel* visualItems = new QmlObjectListModel(); - QList missionItems; - QString error; - save(missionJson); - _loadItemsFromJson(missionJson, visualItems, error); - _convertToMissionItems(visualItems, missionItems, this); + QObject* deleteParent = new QObject(); + QList rgMissionItems; - if (missionItems.count() == 0) { + _convertToMissionItems(_visualItems, rgMissionItems, deleteParent); + if (rgMissionItems.count() == 0) { return; } - float altitude = missionJson[_jsonPlannedHomePositionKey].toArray()[2].toDouble(); + const double homePositionAltitude = _settingsItem->coordinate().altitude(); QString coord; QStringList coords; // Drop home position bool dropPoint = true; - for(const auto& item : missionItems) { + for(const auto& item : rgMissionItems) { if(dropPoint) { dropPoint = false; continue; @@ -292,14 +288,18 @@ void MissionController::convertToKMLDocument(QDomDocument& document) qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_controllerVehicle, item->command()); if (uiInfo && uiInfo->specifiesCoordinate() && !uiInfo->isStandaloneCoordinate()) { + double amslAltitude = item->param7() + (item->frame() == MAV_FRAME_GLOBAL ? 0 : homePositionAltitude); coord = QString::number(item->param6(),'f',7) \ + "," \ + QString::number(item->param5(),'f',7) \ + "," \ - + QString::number(item->param7() + altitude,'f',2); + + QString::number(amslAltitude,'f',2); coords.append(coord); } } + + deleteParent->deleteLater(); + Kml kml; kml.points(coords); kml.save(document); @@ -331,25 +331,25 @@ int MissionController::_nextSequenceNumber(void) int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i) { int sequenceNumber = _nextSequenceNumber(); - SimpleMissionItem * newItem = new SimpleMissionItem(_controllerVehicle, this); + SimpleMissionItem * newItem = new SimpleMissionItem(_controllerVehicle, _flyView, this); newItem->setSequenceNumber(sequenceNumber); newItem->setCoordinate(coordinate); - newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT); + newItem->setCommand(MAV_CMD_NAV_WAYPOINT); _initVisualItem(newItem); if (_visualItems->count() == 1) { - MavlinkQmlSingleton::Qml_MAV_CMD takeoffCmd = _controllerVehicle->vtol() ? MavlinkQmlSingleton::MAV_CMD_NAV_VTOL_TAKEOFF : MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF; + MAV_CMD takeoffCmd = _controllerVehicle->vtol() ? MAV_CMD_NAV_VTOL_TAKEOFF : MAV_CMD_NAV_TAKEOFF; if (_controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains((MAV_CMD)takeoffCmd)) { newItem->setCommand(takeoffCmd); } } newItem->setDefaultsForCommand(); - if ((MAV_CMD)newItem->command() == MAV_CMD_NAV_WAYPOINT) { - double prevAltitude; - MAV_FRAME prevFrame; + if (newItem->specifiesAltitude()) { + double prevAltitude; + int prevAltitudeMode; - if (_findPreviousAltitude(i, &prevAltitude, &prevFrame)) { - newItem->missionItem().setFrame(prevFrame); - newItem->missionItem().setParam7(prevAltitude); + if (_findPreviousAltitude(i, &prevAltitude, &prevAltitudeMode)) { + newItem->altitude()->setRawValue(prevAltitude); + newItem->setAltitudeMode((SimpleMissionItem::AltitudeMode)prevAltitudeMode); } } newItem->setMissionFlightStatus(_missionFlightStatus); @@ -363,21 +363,21 @@ int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i) int MissionController::insertROIMissionItem(QGeoCoordinate coordinate, int i) { int sequenceNumber = _nextSequenceNumber(); - SimpleMissionItem * newItem = new SimpleMissionItem(_controllerVehicle, this); + SimpleMissionItem * newItem = new SimpleMissionItem(_controllerVehicle, _flyView, this); newItem->setSequenceNumber(sequenceNumber); - newItem->setCommand((MavlinkQmlSingleton::Qml_MAV_CMD)(_controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains((MAV_CMD)MAV_CMD_DO_SET_ROI_LOCATION) ? + newItem->setCommand((MAV_CMD)(_controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains((MAV_CMD)MAV_CMD_DO_SET_ROI_LOCATION) ? MAV_CMD_DO_SET_ROI_LOCATION : MAV_CMD_DO_SET_ROI)); _initVisualItem(newItem); newItem->setDefaultsForCommand(); newItem->setCoordinate(coordinate); - double prevAltitude; - MAV_FRAME prevFrame; + double prevAltitude; + int prevAltitudeMode; - if (_findPreviousAltitude(i, &prevAltitude, &prevFrame)) { - newItem->missionItem().setFrame(prevFrame); - newItem->missionItem().setParam7(prevAltitude); + if (_findPreviousAltitude(i, &prevAltitude, &prevAltitudeMode)) { + newItem->altitude()->setRawValue(prevAltitude); + newItem->setAltitudeMode((SimpleMissionItem::AltitudeMode)prevAltitudeMode); } _visualItems->insert(i, newItem); @@ -389,25 +389,48 @@ int MissionController::insertROIMissionItem(QGeoCoordinate coordinate, int i) int MissionController::insertComplexMissionItem(QString itemName, QGeoCoordinate mapCenterCoordinate, int i) { ComplexMissionItem* newItem; - bool surveyStyleItem = false; int sequenceNumber = _nextSequenceNumber(); if (itemName == _surveyMissionItemName) { - newItem = new SurveyMissionItem(_controllerVehicle, _visualItems); + newItem = new SurveyComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, _visualItems /* parent */); newItem->setCoordinate(mapCenterCoordinate); - surveyStyleItem = true; } else if (itemName == _fwLandingMissionItemName) { - newItem = new FixedWingLandingComplexItem(_controllerVehicle, _visualItems); + newItem = new FixedWingLandingComplexItem(_controllerVehicle, _flyView, _visualItems /* parent */); } else if (itemName == _structureScanMissionItemName) { - newItem = new StructureScanComplexItem(_controllerVehicle, _visualItems); + newItem = new StructureScanComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, _visualItems /* parent */); } else if (itemName == _corridorScanMissionItemName) { - newItem = new CorridorScanComplexItem(_controllerVehicle, _visualItems); - surveyStyleItem = true; + newItem = new CorridorScanComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, _visualItems /* parent */); } else { qWarning() << "Internal error: Unknown complex item:" << itemName; return sequenceNumber; } + return _insertComplexMissionItemWorker(newItem, i); +} + +int MissionController::insertComplexMissionItemFromKML(QString itemName, QString kmlFile, int i) +{ + ComplexMissionItem* newItem; + + if (itemName == _surveyMissionItemName) { + newItem = new SurveyComplexItem(_controllerVehicle, _flyView, kmlFile, _visualItems); + } else if (itemName == _structureScanMissionItemName) { + newItem = new StructureScanComplexItem(_controllerVehicle, _flyView, kmlFile, _visualItems); + } else if (itemName == _corridorScanMissionItemName) { + newItem = new CorridorScanComplexItem(_controllerVehicle, _flyView, kmlFile, _visualItems); + } else { + qWarning() << "Internal error: Unknown complex item:" << itemName; + return _nextSequenceNumber(); + } + + return _insertComplexMissionItemWorker(newItem, i); +} + +int MissionController::_insertComplexMissionItemWorker(ComplexMissionItem* complexItem, int i) +{ + int sequenceNumber = _nextSequenceNumber(); + bool surveyStyleItem = qobject_cast(complexItem) || qobject_cast(complexItem); + if (surveyStyleItem) { bool rollSupported = false; bool pitchSupported = false; @@ -434,14 +457,18 @@ int MissionController::insertComplexMissionItem(QString itemName, QGeoCoordinate } } - newItem->setSequenceNumber(sequenceNumber); - _initVisualItem(newItem); + complexItem->setSequenceNumber(sequenceNumber); + _initVisualItem(complexItem); - _visualItems->insert(i, newItem); + if (i == -1) { + _visualItems->append(complexItem); + } else { + _visualItems->insert(i, complexItem); + } _recalcAll(); - return newItem->sequenceNumber(); + return complexItem->sequenceNumber(); } void MissionController::removeMissionItem(int index) @@ -451,7 +478,7 @@ void MissionController::removeMissionItem(int index) return; } - bool removeSurveyStyle = _visualItems->value(index) || _visualItems->value(index); + bool removeSurveyStyle = _visualItems->value(index) || _visualItems->value(index); VisualMissionItem* item = qobject_cast(_visualItems->removeAt(index)); _deinitVisualItem(item); @@ -461,7 +488,7 @@ void MissionController::removeMissionItem(int index) // Determine if the mission still has another survey style item in it bool foundSurvey = false; for (int i=1; i<_visualItems->count(); i++) { - if (_visualItems->value(i) || _visualItems->value(index)) { + if (_visualItems->value(i) || _visualItems->value(i)) { foundSurvey = true; break; } @@ -518,7 +545,7 @@ bool MissionController::_loadJsonMissionFileV1(const QJsonObject& json, QmlObjec } // Read complex items - QList surveyItems; + QList surveyItems; QJsonArray complexArray(json[_jsonComplexItemsKey].toArray()); qCDebug(MissionControllerLog) << "Json load: complex item count" << complexArray.count(); for (int i=0; iload(itemObject, itemObject["id"].toInt(), errorString)) { surveyItems.append(item); @@ -551,7 +578,7 @@ bool MissionController::_loadJsonMissionFileV1(const QJsonObject& json, QmlObjec // If there is a complex item that should be next in sequence add it in if (nextComplexItemIndex < surveyItems.count()) { - SurveyMissionItem* complexItem = surveyItems[nextComplexItemIndex]; + SurveyComplexItem* complexItem = surveyItems[nextComplexItemIndex]; if (complexItem->sequenceNumber() == nextSequenceNumber) { qCDebug(MissionControllerLog) << "Json load: injecting complex item expectedSequence:actualSequence:" << nextSequenceNumber << complexItem->sequenceNumber(); @@ -572,7 +599,7 @@ bool MissionController::_loadJsonMissionFileV1(const QJsonObject& json, QmlObjec } const QJsonObject itemObject = itemValue.toObject(); - SimpleMissionItem* item = new SimpleMissionItem(_controllerVehicle, visualItems); + SimpleMissionItem* item = new SimpleMissionItem(_controllerVehicle, _flyView, visualItems); if (item->load(itemObject, itemObject["id"].toInt(), errorString)) { qCDebug(MissionControllerLog) << "Json load: adding simple item expectedSequence:actualSequence" << nextSequenceNumber << item->sequenceNumber(); nextSequenceNumber = item->lastSequenceNumber() + 1; @@ -584,10 +611,10 @@ bool MissionController::_loadJsonMissionFileV1(const QJsonObject& json, QmlObjec } while (nextSimpleItemIndex < itemArray.count() || nextComplexItemIndex < surveyItems.count()); if (json.contains(_jsonPlannedHomePositionKey)) { - SimpleMissionItem* item = new SimpleMissionItem(_controllerVehicle, visualItems); + SimpleMissionItem* item = new SimpleMissionItem(_controllerVehicle, _flyView, visualItems); if (item->load(json[_jsonPlannedHomePositionKey].toObject(), 0, errorString)) { - MissionSettingsItem* settingsItem = new MissionSettingsItem(_controllerVehicle, visualItems); + MissionSettingsItem* settingsItem = new MissionSettingsItem(_controllerVehicle, _flyView, visualItems); settingsItem->setCoordinate(item->coordinate()); visualItems->insert(0, settingsItem); item->deleteLater(); @@ -639,7 +666,7 @@ bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjec if (!JsonHelper::loadGeoCoordinate(json[_jsonPlannedHomePositionKey], true /* altitudeRequired */, homeCoordinate, errorString)) { return false; } - MissionSettingsItem* settingsItem = new MissionSettingsItem(_controllerVehicle, visualItems); + MissionSettingsItem* settingsItem = new MissionSettingsItem(_controllerVehicle, _flyView, visualItems); settingsItem->setCoordinate(homeCoordinate); visualItems->insert(0, settingsItem); qCDebug(MissionControllerLog) << "plannedHomePosition" << homeCoordinate; @@ -668,7 +695,7 @@ bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjec QString itemType = itemObject[VisualMissionItem::jsonTypeKey].toString(); if (itemType == VisualMissionItem::jsonTypeSimpleItemValue) { - SimpleMissionItem* simpleItem = new SimpleMissionItem(_controllerVehicle, visualItems); + SimpleMissionItem* simpleItem = new SimpleMissionItem(_controllerVehicle, _flyView, visualItems); if (simpleItem->load(itemObject, nextSequenceNumber, errorString)) { qCDebug(MissionControllerLog) << "Loading simple item: nextSequenceNumber:command" << nextSequenceNumber << simpleItem->command(); nextSequenceNumber = simpleItem->lastSequenceNumber() + 1; @@ -685,9 +712,9 @@ bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjec } QString complexItemType = itemObject[ComplexMissionItem::jsonComplexItemTypeKey].toString(); - if (complexItemType == SurveyMissionItem::jsonComplexItemTypeValue) { + if (complexItemType == SurveyComplexItem::jsonComplexItemTypeValue) { qCDebug(MissionControllerLog) << "Loading Survey: nextSequenceNumber" << nextSequenceNumber; - SurveyMissionItem* surveyItem = new SurveyMissionItem(_controllerVehicle, visualItems); + SurveyComplexItem* surveyItem = new SurveyComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, visualItems); if (!surveyItem->load(itemObject, nextSequenceNumber++, errorString)) { return false; } @@ -696,7 +723,7 @@ bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjec visualItems->append(surveyItem); } else if (complexItemType == FixedWingLandingComplexItem::jsonComplexItemTypeValue) { qCDebug(MissionControllerLog) << "Loading Fixed Wing Landing Pattern: nextSequenceNumber" << nextSequenceNumber; - FixedWingLandingComplexItem* landingItem = new FixedWingLandingComplexItem(_controllerVehicle, visualItems); + FixedWingLandingComplexItem* landingItem = new FixedWingLandingComplexItem(_controllerVehicle, _flyView, visualItems); if (!landingItem->load(itemObject, nextSequenceNumber++, errorString)) { return false; } @@ -705,7 +732,7 @@ bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjec visualItems->append(landingItem); } else if (complexItemType == StructureScanComplexItem::jsonComplexItemTypeValue) { qCDebug(MissionControllerLog) << "Loading Structure Scan: nextSequenceNumber" << nextSequenceNumber; - StructureScanComplexItem* structureItem = new StructureScanComplexItem(_controllerVehicle, visualItems); + StructureScanComplexItem* structureItem = new StructureScanComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, visualItems); if (!structureItem->load(itemObject, nextSequenceNumber++, errorString)) { return false; } @@ -714,7 +741,7 @@ bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjec visualItems->append(structureItem); } else if (complexItemType == CorridorScanComplexItem::jsonComplexItemTypeValue) { qCDebug(MissionControllerLog) << "Loading Corridor Scan: nextSequenceNumber" << nextSequenceNumber; - CorridorScanComplexItem* corridorItem = new CorridorScanComplexItem(_controllerVehicle, visualItems); + CorridorScanComplexItem* corridorItem = new CorridorScanComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, visualItems); if (!corridorItem->load(itemObject, nextSequenceNumber++, errorString)) { return false; } @@ -723,7 +750,7 @@ bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjec visualItems->append(corridorItem); } else if (complexItemType == MissionSettingsItem::jsonComplexItemTypeValue) { qCDebug(MissionControllerLog) << "Loading Mission Settings: nextSequenceNumber" << nextSequenceNumber; - MissionSettingsItem* settingsItem = new MissionSettingsItem(_controllerVehicle, visualItems); + MissionSettingsItem* settingsItem = new MissionSettingsItem(_controllerVehicle, _flyView, visualItems); if (!settingsItem->load(itemObject, nextSequenceNumber++, errorString)) { return false; } @@ -816,7 +843,7 @@ bool MissionController::_loadTextMissionFile(QTextStream& stream, QmlObjectListM MissionSettingsItem* settingsItem = visualItems->value(0); while (!stream.atEnd()) { - SimpleMissionItem* item = new SimpleMissionItem(_controllerVehicle, visualItems); + SimpleMissionItem* item = new SimpleMissionItem(_controllerVehicle, _flyView, visualItems); if (item->load(stream)) { if (firstItem && plannedHomePositionInFile) { @@ -839,7 +866,7 @@ bool MissionController::_loadTextMissionFile(QTextStream& stream, QmlObjectListM // Update sequence numbers in DO_JUMP commands to take into account added home position in index 0 for (int i=1; icount(); i++) { SimpleMissionItem* item = qobject_cast(visualItems->get(i)); - if (item && item->command() == MavlinkQmlSingleton::MAV_CMD_DO_JUMP) { + if (item && item->command() == MAV_CMD_DO_JUMP) { item->missionItem().setParam1((int)item->missionItem().param1() + 1); } } @@ -924,6 +951,18 @@ bool MissionController::loadTextFile(QFile& file, QString& errorString) return true; } +bool MissionController::readyForSaveSend(void) const +{ + for (int i=0; i<_visualItems->count(); i++) { + VisualMissionItem* visualItem = qobject_cast(_visualItems->get(i)); + if (!visualItem->readyForSave()) { + return false; + } + } + + return true; +} + void MissionController::save(QJsonObject& json) { json[JsonHelper::jsonVersionKey] = _missionFileVersion; @@ -1068,7 +1107,7 @@ void MissionController::_recalcWaypointLines(void) if (item->specifiesCoordinate() && !item->isStandaloneCoordinate()) { firstCoordinateItem = false; if (lastCoordinateItem != _settingsItem || (homePositionValid && linkStartToHome)) { - if (_editMode) { + if (!_flyView) { VisualItemPair pair(lastCoordinateItem, item); _addWaypointLineSegment(old_table, pair); } @@ -1083,7 +1122,7 @@ void MissionController::_recalcWaypointLines(void) } if (linkEndToHome && lastCoordinateItem != _settingsItem && homePositionValid) { - if (_editMode) { + if (!_flyView) { VisualItemPair pair(lastCoordinateItem, _settingsItem); _addWaypointLineSegment(old_table, pair); } else { @@ -1284,7 +1323,7 @@ void MissionController::_recalcMissionFlightStatus() } // Link back to home if first item is takeoff and we have home position - if (firstCoordinateItem && simpleItem && simpleItem->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) { + if (firstCoordinateItem && simpleItem && (simpleItem->command() == MAV_CMD_NAV_TAKEOFF || simpleItem->command() == MAV_CMD_NAV_VTOL_TAKEOFF)) { if (showHomePosition) { linkStartToHome = true; if (_controllerVehicle->multiRotor() || _controllerVehicle->vtol()) { @@ -1300,13 +1339,19 @@ void MissionController::_recalcMissionFlightStatus() // Update VTOL state if (simpleItem && _controllerVehicle->vtol()) { switch (simpleItem->command()) { - case MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF: + case MAV_CMD_NAV_TAKEOFF: vtolInHover = false; break; - case MavlinkQmlSingleton::MAV_CMD_NAV_LAND: + case MAV_CMD_NAV_VTOL_TAKEOFF: + vtolInHover = true; + break; + case MAV_CMD_NAV_LAND: vtolInHover = false; break; - case MavlinkQmlSingleton::MAV_CMD_DO_VTOL_TRANSITION: + case MAV_CMD_NAV_VTOL_LAND: + vtolInHover = true; + break; + case MAV_CMD_DO_VTOL_TRANSITION: { int transitionState = simpleItem->missionItem().param1(); if (transitionState == MAV_VTOL_STATE_TRANSITION_TO_MC) { @@ -1325,12 +1370,6 @@ void MissionController::_recalcMissionFlightStatus() _addCommandTimeDelay(simpleItem, vtolInHover); if (item->specifiesCoordinate()) { - // Update vehicle yaw assuming direction to next waypoint - if (item != lastCoordinateItem) { - _missionFlightStatus.vehicleYaw = lastCoordinateItem->exitCoordinate().azimuthTo(item->coordinate()); - lastCoordinateItem->setMissionVehicleYaw(_missionFlightStatus.vehicleYaw); - } - // Keep track of the min/max altitude for all waypoints so we can show altitudes as a percentage double absoluteAltitude = item->coordinate().altitude(); @@ -1348,6 +1387,13 @@ void MissionController::_recalcMissionFlightStatus() if (!item->isStandaloneCoordinate()) { firstCoordinateItem = false; + + // Update vehicle yaw assuming direction to next waypoint + if (item != lastCoordinateItem) { + _missionFlightStatus.vehicleYaw = lastCoordinateItem->exitCoordinate().azimuthTo(item->coordinate()); + lastCoordinateItem->setMissionVehicleYaw(_missionFlightStatus.vehicleYaw); + } + if (lastCoordinateItem != _settingsItem || linkStartToHome) { // This is a subsequent waypoint or we are forcing the first waypoint back to home double azimuth, distance, altDifference; @@ -1377,9 +1423,9 @@ void MissionController::_recalcMissionFlightStatus() } item->setMissionFlightStatus(_missionFlightStatus); - } - lastCoordinateItem = item; + lastCoordinateItem = item; + } } } lastCoordinateItem->setMissionVehicleYaw(_missionFlightStatus.vehicleYaw); @@ -1485,7 +1531,7 @@ void MissionController::_setPlannedHomePositionFromFirstCoordinate(void) void MissionController::_recalcAll(void) { - if (_editMode) { + if (!_flyView) { _setPlannedHomePositionFromFirstCoordinate(); } _recalcSequence(); @@ -1503,7 +1549,7 @@ void MissionController::_initAllVisualItems(void) qWarning() << "First item not MissionSettingsItem"; return; } - if (_editMode) { + if (!_flyView) { _settingsItem->setIsCurrentItem(true); } @@ -1648,11 +1694,11 @@ void MissionController::_inProgressChanged(bool inProgress) emit syncInProgressChanged(inProgress); } -bool MissionController::_findPreviousAltitude(int newIndex, double* prevAltitude, MAV_FRAME* prevFrame) +bool MissionController::_findPreviousAltitude(int newIndex, double* prevAltitude, int* prevAltitudeMode) { bool found = false; double foundAltitude; - MAV_FRAME foundFrame; + int foundAltitudeMode; if (newIndex > _visualItems->count()) { return false; @@ -1665,9 +1711,9 @@ bool MissionController::_findPreviousAltitude(int newIndex, double* prevAltitude if (visualItem->specifiesCoordinate() && !visualItem->isStandaloneCoordinate()) { if (visualItem->isSimpleItem()) { SimpleMissionItem* simpleItem = qobject_cast(visualItem); - if ((MAV_CMD)simpleItem->command() == MAV_CMD_NAV_WAYPOINT) { - foundAltitude = simpleItem->exitCoordinate().altitude(); - foundFrame = simpleItem->missionItem().frame(); + if (simpleItem->specifiesAltitude()) { + foundAltitude = simpleItem->altitude()->rawValue().toDouble(); + foundAltitudeMode = simpleItem->altitudeMode(); found = true; break; } @@ -1677,7 +1723,7 @@ bool MissionController::_findPreviousAltitude(int newIndex, double* prevAltitude if (found) { *prevAltitude = foundAltitude; - *prevFrame = foundFrame; + *prevAltitudeMode = foundAltitudeMode; } return found; @@ -1698,7 +1744,7 @@ double MissionController::_normalizeLon(double lon) /// Add the Mission Settings complex item to the front of the items void MissionController::_addMissionSettings(QmlObjectListModel* visualItems, bool addToCenter) { - MissionSettingsItem* settingsItem = new MissionSettingsItem(_controllerVehicle, visualItems); + MissionSettingsItem* settingsItem = new MissionSettingsItem(_controllerVehicle, _flyView, visualItems); qCDebug(MissionControllerLog) << "_addMissionSettings addToCenter" << addToCenter; @@ -1746,7 +1792,7 @@ int MissionController::resumeMissionIndex(void) const int resumeIndex = 0; - if (!_editMode) { + if (_flyView) { resumeIndex = _missionManager->lastCurrentIndex() + (_controllerVehicle->firmwarePlugin()->sendHomePositionToVehicle() ? 0 : 1); if (resumeIndex > 1 && resumeIndex != _visualItems->value(_visualItems->count() - 1)->sequenceNumber()) { // Resume at the item previous to the item we were heading towards @@ -1761,7 +1807,7 @@ int MissionController::resumeMissionIndex(void) const int MissionController::currentMissionIndex(void) const { - if (_editMode) { + if (!_flyView) { return -1; } else { int currentIndex = _missionManager->currentIndex(); @@ -1774,7 +1820,7 @@ int MissionController::currentMissionIndex(void) const void MissionController::_currentMissionIndexChanged(int sequenceNumber) { - if (!_editMode) { + if (_flyView) { if (!_controllerVehicle->firmwarePlugin()->sendHomePositionToVehicle()) { sequenceNumber++; } @@ -1807,10 +1853,8 @@ void MissionController::setDirty(bool dirty) void MissionController::_scanForAdditionalSettings(QmlObjectListModel* visualItems, Vehicle* vehicle) { - if (_editMode) { - // First we look for a Fixed Wing Landing Pattern which is at the end - FixedWingLandingComplexItem::scanForItem(visualItems, vehicle); - } + // First we look for a Fixed Wing Landing Pattern which is at the end + FixedWingLandingComplexItem::scanForItem(visualItems, _flyView, vehicle); int scanIndex = 0; while (scanIndex < visualItems->count()) { @@ -1818,7 +1862,7 @@ void MissionController::_scanForAdditionalSettings(QmlObjectListModel* visualIte qCDebug(MissionControllerLog) << "MissionController::_scanForAdditionalSettings count:scanIndex" << visualItems->count() << scanIndex; - if (_editMode) { + if (!_flyView) { MissionSettingsItem* settingsItem = qobject_cast(visualItem); if (settingsItem) { scanIndex++; @@ -1919,7 +1963,7 @@ void MissionController::_visualItemsDirtyChanged(bool dirty) bool MissionController::showPlanFromManagerVehicle (void) { - qCDebug(MissionControllerLog) << "showPlanFromManagerVehicle" << _editMode; + qCDebug(MissionControllerLog) << "showPlanFromManagerVehicle _flyView" << _flyView; if (_masterController->offline()) { qCWarning(MissionControllerLog) << "MissionController::showPlanFromManagerVehicle called while offline"; return true; // stops further propagation of showPlanFromManagerVehicle due to error @@ -1945,7 +1989,7 @@ bool MissionController::showPlanFromManagerVehicle (void) void MissionController::_managerSendComplete(bool error) { // Fly view always reloads on send complete - if (!error && !_editMode) { + if (!error && _flyView) { showPlanFromManagerVehicle(); } } diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index 390a4e8a62be8a3304959eea4d2168e90151068f..5015a95aea93afa5648fbeecb395fbd93458be05 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -15,7 +15,6 @@ #include "QmlObjectListModel.h" #include "Vehicle.h" #include "QGCLoggingCategory.h" -#include "MavlinkQmlSingleton.h" #include @@ -26,6 +25,7 @@ class MissionSettingsItem; class AppSettings; class MissionManager; class SimpleMissionItem; +class ComplexMissionItem; class QDomDocument; Q_DECLARE_LOGGING_CATEGORY(MissionControllerLog) @@ -41,7 +41,7 @@ public: MissionController(PlanMasterController* masterController, QObject* parent = NULL); ~MissionController(); - typedef struct { + typedef struct _MissionFlightStatus_t { double maxTelemetryDistance; double totalDistance; double totalTime; @@ -90,6 +90,10 @@ public: Q_PROPERTY(int batteryChangePoint READ batteryChangePoint NOTIFY batteryChangePointChanged) Q_PROPERTY(int batteriesRequired READ batteriesRequired NOTIFY batteriesRequiredChanged) + Q_PROPERTY(QString surveyComplexItemName READ surveyComplexItemName CONSTANT) + Q_PROPERTY(QString corridorScanComplexItemName READ corridorScanComplexItemName CONSTANT) + Q_PROPERTY(QString structureScanComplexItemName READ structureScanComplexItemName CONSTANT) + Q_INVOKABLE void removeMissionItem(int index); /// Add a new simple mission item to the list @@ -109,6 +113,12 @@ public: /// @return Sequence number for new item Q_INVOKABLE int insertComplexMissionItem(QString itemName, QGeoCoordinate mapCenterCoordinate, int i); + /// Add a new complex mission item to the list + /// @param itemName: Name of complex item to create (from complexMissionItemNames) + /// @param i: index to insert at, -1 for end + /// @return Sequence number for new item + Q_INVOKABLE int insertComplexMissionItemFromKML(QString itemName, QString kmlFile, int i); + Q_INVOKABLE void resumeMission(int resumeIndex); /// Updates the altitudes of the items in the current mission to the new default altitude @@ -118,6 +128,10 @@ public: /// @param sequenceNumber - index for new item, -1 to clear current item Q_INVOKABLE void setCurrentPlanViewIndex(int sequenceNumber, bool force); + /// Determines if the mission has all data needed to be saved or sent to the vehicle. Currently the only case where this + /// would return false is when it is still waiting on terrain data to determine correct altitudes. + bool readyForSaveSend(void) const; + /// Sends the mission items to the specified vehicle static void sendItemsToVehicle(Vehicle* vehicle, QmlObjectListModel* visualMissionItems); @@ -126,7 +140,7 @@ public: // Overrides from PlanElementController bool supported (void) const final { return true; } - void start (bool editMode) final; + void start (bool flyView) final; void save (QJsonObject& json) final; bool load (const QJsonObject& json, QString& errorString) final; void loadFromVehicle (void) final; @@ -145,13 +159,16 @@ public: // Property accessors - QmlObjectListModel* visualItems (void) { return _visualItems; } - QmlObjectListModel* waypointLines (void) { return &_waypointLines; } - QVariantList waypointPath (void) { return _waypointPath; } - QStringList complexMissionItemNames (void) const; - QGeoCoordinate plannedHomePosition (void) const; - VisualMissionItem* currentPlanViewItem (void) const; - double progressPct (void) const { return _progressPct; } + QmlObjectListModel* visualItems (void) { return _visualItems; } + QmlObjectListModel* waypointLines (void) { return &_waypointLines; } + QVariantList waypointPath (void) { return _waypointPath; } + QStringList complexMissionItemNames (void) const; + QGeoCoordinate plannedHomePosition (void) const; + VisualMissionItem* currentPlanViewItem (void) const; + double progressPct (void) const { return _progressPct; } + QString surveyComplexItemName (void) const { return _surveyMissionItemName; } + QString corridorScanComplexItemName (void) const { return _corridorScanMissionItemName; } + QString structureScanComplexItemName(void) const { return _structureScanMissionItemName; } int currentMissionIndex (void) const; int resumeMissionIndex (void) const; @@ -218,7 +235,7 @@ private: void _setupActiveVehicle(Vehicle* activeVehicle, bool forceLoadFromVehicle); void _calcPrevWaypointValues(double homeAlt, VisualMissionItem* currentItem, VisualMissionItem* prevItem, double* azimuth, double* distance, double* altDifference); static double _calcDistanceToHome(VisualMissionItem* currentItem, VisualMissionItem* homeItem); - bool _findPreviousAltitude(int newIndex, double* prevAltitude, MAV_FRAME* prevFrame); + bool _findPreviousAltitude(int newIndex, double* prevAltitude, int* prevAltitudeMode); static double _normalizeLat(double lat); static double _normalizeLon(double lon); void _addMissionSettings(QmlObjectListModel* visualItems, bool addToCenter); @@ -239,6 +256,7 @@ private: void _addWaypointLineSegment(CoordVectHashTable& prevItemPairHashTable, VisualItemPair& pair); void _addCommandTimeDelay(SimpleMissionItem* simpleItem, bool vtolInHover); void _addTimeDistance(bool vtolInHover, double hoverTime, double cruiseTime, double extraTime, double distance, int seqNum); + int _insertComplexMissionItemWorker(ComplexMissionItem* complexItem, int i); private: MissionManager* _missionManager; diff --git a/src/MissionManager/MissionControllerTest.cc b/src/MissionManager/MissionControllerTest.cc index c1bcdfc5c4b6c4bd7da7c8d5135ae31bd66e49c4..de06cccfff75035fcf8242e981923bc65f236883 100644 --- a/src/MissionManager/MissionControllerTest.cc +++ b/src/MissionManager/MissionControllerTest.cc @@ -59,7 +59,7 @@ void MissionControllerTest::_initForFirmwareType(MAV_AUTOPILOT firmwareType) Q_CHECK_PTR(_multiSpyMissionController); QCOMPARE(_multiSpyMissionController->init(_missionController, _rgMissionControllerSignals, _cMissionControllerSignals), true); - _masterController->start(true /* editMode */); + _masterController->start(false /* flyView */); // All signals should some through on start QCOMPARE(_multiSpyMissionController->checkOnlySignalsByMask(visualItemsChangedSignalMask | waypointLinesChangedSignalMask), true); @@ -132,7 +132,7 @@ void MissionControllerTest::_testAddWaypointWorker(MAV_AUTOPILOT firmwareType) QVERIFY(settingsItem); QVERIFY(simpleItem); - QCOMPARE(simpleItem->command(), MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF); + QCOMPARE((MAV_CMD)simpleItem->command(), MAV_CMD_NAV_TAKEOFF); QCOMPARE(simpleItem->childItems()->count(), 0); // If the first item added specifies a coordinate, then planned home position will be set @@ -166,7 +166,7 @@ void MissionControllerTest::_testOfflineToOnlineWorker(MAV_AUTOPILOT firmwareTyp // Start offline and add item _missionController = new MissionController(); Q_CHECK_PTR(_missionController); - _missionController->start(true /* editMode */); + _missionController->start(false /* flyView */); _missionController->insertSimpleMissionItem(QGeoCoordinate(37.803784, -122.462276), _missionController->visualItems()->count()); // Go online to empty vehicle diff --git a/src/MissionManager/MissionItem.h b/src/MissionManager/MissionItem.h index 8202a8480535ffdd41aaef21af3edb43fadfbc16..ccc2b53bd6623ce9051569e525fc46905bd499e4 100644 --- a/src/MissionManager/MissionItem.h +++ b/src/MissionManager/MissionItem.h @@ -20,13 +20,12 @@ #include "QGCMAVLink.h" #include "QGC.h" -#include "MavlinkQmlSingleton.h" #include "QmlObjectListModel.h" #include "Fact.h" #include "QGCLoggingCategory.h" #include "QmlObjectListModel.h" -class SurveyMissionItem; +class SurveyComplexItem; class SimpleMissionItem; class MissionController; #ifdef UNITTEST_BUILD @@ -152,7 +151,7 @@ private: static const char* _jsonParam3Key; static const char* _jsonParam4Key; - friend class SurveyMissionItem; + friend class SurveyComplexItem; friend class SimpleMissionItem; friend class MissionController; #ifdef UNITTEST_BUILD diff --git a/src/MissionManager/MissionItemTest.cc b/src/MissionManager/MissionItemTest.cc index 1480af7ade69dbcbaa502d373d12cd6fc60d7d9f..f4ed23e4ff9ff3e65d17d9b0385a9ab44548786e 100644 --- a/src/MissionManager/MissionItemTest.cc +++ b/src/MissionManager/MissionItemTest.cc @@ -282,7 +282,7 @@ void MissionItemTest::_testSimpleLoadFromStream(void) { // We specifically test SimpleMissionItem loading as well since it has additional // signalling which can affect values. - SimpleMissionItem simpleMissionItem(_offlineVehicle); + SimpleMissionItem simpleMissionItem(_offlineVehicle, false /* flyView */, NULL); QString testString("10\t0\t3\t80\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n"); QTextStream testStream(&testString, QIODevice::ReadOnly); @@ -452,7 +452,7 @@ void MissionItemTest::_testSimpleLoadFromJson(void) // We specifically test SimpleMissionItem loading as well since it has additional // signalling which can affect values. - SimpleMissionItem simpleMissionItem(_offlineVehicle); + SimpleMissionItem simpleMissionItem(_offlineVehicle, false /* flyView */, NULL); QString errorString; QJsonArray coordinateArray; QJsonObject jsonObject; diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h index e34d6e37dc69239ff7f09882fdc69bf10a35c6b0..a333678f700af0121eac22855ddfbd824c9229f6 100644 --- a/src/MissionManager/MissionManager.h +++ b/src/MissionManager/MissionManager.h @@ -38,10 +38,6 @@ public: /// from mission start to resumeIndex in the generate mission. void generateResumeMission(int resumeIndex); -signals: - void currentIndexChanged (int currentIndex); - void lastCurrentIndexChanged (int lastCurrentIndex); - private slots: void _mavlinkMessageReceived(const mavlink_message_t& message); diff --git a/src/MissionManager/MissionSettingsItem.cc b/src/MissionManager/MissionSettingsItem.cc index abe14a452a8f4e67363b8592349c89a54f3c2161..af784e7657642303936efb28747395eefde5e52e 100644 --- a/src/MissionManager/MissionSettingsItem.cc +++ b/src/MissionManager/MissionSettingsItem.cc @@ -27,8 +27,8 @@ const char* MissionSettingsItem::_plannedHomePositionAltitudeName = "PlannedHome QMap MissionSettingsItem::_metaDataMap; -MissionSettingsItem::MissionSettingsItem(Vehicle* vehicle, QObject* parent) - : ComplexMissionItem (vehicle, parent) +MissionSettingsItem::MissionSettingsItem(Vehicle* vehicle, bool flyView, QObject* parent) + : ComplexMissionItem (vehicle, flyView, parent) , _plannedHomePositionAltitudeFact (0, _plannedHomePositionAltitudeName, FactMetaData::valueTypeDouble) , _plannedHomePositionFromVehicle (false) , _missionEndRTL (false) @@ -57,7 +57,7 @@ MissionSettingsItem::MissionSettingsItem(Vehicle* vehicle, QObject* parent) connect(this, &MissionSettingsItem::terrainAltitudeChanged, this, &MissionSettingsItem::_setHomeAltFromTerrain); - connect(&_plannedHomePositionAltitudeFact, &Fact::valueChanged, this, &MissionSettingsItem::_updateAltitudeInCoordinate); + connect(&_plannedHomePositionAltitudeFact, &Fact::rawValueChanged, this, &MissionSettingsItem::_updateAltitudeInCoordinate); connect(&_cameraSection, &CameraSection::dirtyChanged, this, &MissionSettingsItem::_sectionDirtyChanged); connect(&_speedSection, &SpeedSection::dirtyChanged, this, &MissionSettingsItem::_sectionDirtyChanged); @@ -299,3 +299,8 @@ void MissionSettingsItem::_setHomeAltFromTerrain(double terrainAltitude) _plannedHomePositionAltitudeFact.setRawValue(terrainAltitude); } } + +QString MissionSettingsItem::abbreviation(void) const +{ + return _flyView ? tr("H") : tr("Planned Home"); +} diff --git a/src/MissionManager/MissionSettingsItem.h b/src/MissionManager/MissionSettingsItem.h index d20410917ddcd245265d2acd330b22d86a3ce5ce..2a031217578ecdc357eef70e1ee778e6ab1336bc 100644 --- a/src/MissionManager/MissionSettingsItem.h +++ b/src/MissionManager/MissionSettingsItem.h @@ -24,7 +24,7 @@ class MissionSettingsItem : public ComplexMissionItem Q_OBJECT public: - MissionSettingsItem(Vehicle* vehicle, QObject* parent = NULL); + MissionSettingsItem(Vehicle* vehicle, bool flyView, QObject* parent); Q_PROPERTY(Fact* plannedHomePositionAltitude READ plannedHomePositionAltitude CONSTANT) Q_PROPERTY(bool missionEndRTL READ missionEndRTL WRITE setMissionEndRTL NOTIFY missionEndRTLChanged) @@ -68,7 +68,7 @@ public: bool specifiesAltitudeOnly (void) const final { return false; } QString commandDescription (void) const final { return "Mission Start"; } QString commandName (void) const final { return "Mission Start"; } - QString abbreviation (void) const final { return "H"; } + QString abbreviation (void) const final; QGeoCoordinate coordinate (void) const final { return _plannedHomePositionCoordinate; } QGeoCoordinate exitCoordinate (void) const final { return _plannedHomePositionCoordinate; } int sequenceNumber (void) const final { return _sequenceNumber; } @@ -107,9 +107,8 @@ private: bool _missionEndRTL; CameraSection _cameraSection; SpeedSection _speedSection; - - int _sequenceNumber; - bool _dirty; + int _sequenceNumber; + bool _dirty; static QMap _metaDataMap; diff --git a/src/MissionManager/MissionSettingsTest.cc b/src/MissionManager/MissionSettingsTest.cc index 26b3a342e60a011209fb52b487ad4a0fea5c420f..4688d5afbd2daf6cc7f7450df960aacb815a0ec0 100644 --- a/src/MissionManager/MissionSettingsTest.cc +++ b/src/MissionManager/MissionSettingsTest.cc @@ -22,7 +22,7 @@ void MissionSettingsTest::init(void) { VisualMissionItemTest::init(); - _settingsItem = new MissionSettingsItem(_offlineVehicle, this); + _settingsItem = new MissionSettingsItem(_offlineVehicle, false /* flyView */, this); } void MissionSettingsTest::cleanup(void) diff --git a/src/MissionManager/PlanElementController.cc b/src/MissionManager/PlanElementController.cc index 0ec882bde185b7d7821056cf8b66d319ebf2c269..f83cfad032e2c6759d41e273b781700b0fa9bc64 100644 --- a/src/MissionManager/PlanElementController.cc +++ b/src/MissionManager/PlanElementController.cc @@ -15,11 +15,11 @@ #include "AppSettings.h" PlanElementController::PlanElementController(PlanMasterController* masterController, QObject* parent) - : QObject(parent) - , _masterController(masterController) + : QObject (parent) + , _masterController (masterController) , _controllerVehicle(masterController->controllerVehicle()) - , _managerVehicle(masterController->managerVehicle()) - , _editMode(false) + , _managerVehicle (masterController->managerVehicle()) + , _flyView (false) { } @@ -29,9 +29,9 @@ PlanElementController::~PlanElementController() } -void PlanElementController::start(bool editMode) +void PlanElementController::start(bool flyView) { - _editMode = editMode; + _flyView = flyView; } void PlanElementController::managerVehicleChanged(Vehicle* managerVehicle) diff --git a/src/MissionManager/PlanElementController.h b/src/MissionManager/PlanElementController.h index ffe29497831e3633b304b467f9985f9f271866b6..0690c84be5cf63a3b6f99d3fddb39ec30688b5f3 100644 --- a/src/MissionManager/PlanElementController.h +++ b/src/MissionManager/PlanElementController.h @@ -33,8 +33,7 @@ public: Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) ///< true: unsaved/sent changes are present, false: no changes since last save/send /// Should be called immediately upon Component.onCompleted. - /// @param editMode true: controller being used in Plan view, false: controller being used in Fly view - virtual void start(bool editMode); + virtual void start(bool flyView); virtual void save (QJsonObject& json) = 0; virtual bool load (const QJsonObject& json, QString& errorString) = 0; @@ -70,9 +69,9 @@ signals: protected: PlanMasterController* _masterController; - Vehicle* _controllerVehicle; - Vehicle* _managerVehicle; - bool _editMode; + Vehicle* _controllerVehicle; ///< Offline controller vehicle + Vehicle* _managerVehicle; ///< Either active vehicle or _controllerVehicle if none + bool _flyView; }; #endif diff --git a/src/MissionManager/PlanManager.cc b/src/MissionManager/PlanManager.cc index b6103ea68f9669f5c3bcc9f0f3f89b717c526b72..bd51e3702968787f70afd327e3fb088faa0ad6a2 100644 --- a/src/MissionManager/PlanManager.cc +++ b/src/MissionManager/PlanManager.cc @@ -712,7 +712,7 @@ QString PlanManager::_ackTypeToString(AckType_t ackType) QString PlanManager::_lastMissionReqestString(MAV_MISSION_RESULT result) { - if (_lastMissionRequest != -1 && _lastMissionRequest >= 0 && _lastMissionRequest < _writeMissionItems.count()) { + if (_lastMissionRequest >= 0 && _lastMissionRequest < _writeMissionItems.count()) { MissionItem* item = _writeMissionItems[_lastMissionRequest]; switch (result) { diff --git a/src/MissionManager/PlanMasterController.cc b/src/MissionManager/PlanMasterController.cc index 5f788998af9432b67c6aa795faac1d1167dbdae3..48b331a79e300727c75db37ca880d54a717cc717 100644 --- a/src/MissionManager/PlanMasterController.cc +++ b/src/MissionManager/PlanMasterController.cc @@ -29,20 +29,20 @@ const char* PlanMasterController::_jsonGeoFenceObjectKey = "geoFence"; const char* PlanMasterController::_jsonRallyPointsObjectKey = "rallyPoints"; PlanMasterController::PlanMasterController(QObject* parent) - : QObject(parent) - , _multiVehicleMgr(qgcApp()->toolbox()->multiVehicleManager()) - , _controllerVehicle(new Vehicle((MAV_AUTOPILOT)qgcApp()->toolbox()->settingsManager()->appSettings()->offlineEditingFirmwareType()->rawValue().toInt(), (MAV_TYPE)qgcApp()->toolbox()->settingsManager()->appSettings()->offlineEditingVehicleType()->rawValue().toInt(), qgcApp()->toolbox()->firmwarePluginManager())) - , _managerVehicle(_controllerVehicle) - , _editMode(false) - , _offline(true) - , _missionController(this) - , _geoFenceController(this) - , _rallyPointController(this) - , _loadGeoFence(false) - , _loadRallyPoints(false) - , _sendGeoFence(false) - , _sendRallyPoints(false) - , _syncInProgress(false) + : QObject (parent) + , _multiVehicleMgr (qgcApp()->toolbox()->multiVehicleManager()) + , _controllerVehicle (new Vehicle((MAV_AUTOPILOT)qgcApp()->toolbox()->settingsManager()->appSettings()->offlineEditingFirmwareType()->rawValue().toInt(), (MAV_TYPE)qgcApp()->toolbox()->settingsManager()->appSettings()->offlineEditingVehicleType()->rawValue().toInt(), qgcApp()->toolbox()->firmwarePluginManager())) + , _managerVehicle (_controllerVehicle) + , _flyView (true) + , _offline (true) + , _missionController (this) + , _geoFenceController (this) + , _rallyPointController (this) + , _loadGeoFence (false) + , _loadRallyPoints (false) + , _sendGeoFence (false) + , _sendRallyPoints (false) + , _syncInProgress (false) { connect(&_missionController, &MissionController::dirtyChanged, this, &PlanMasterController::dirtyChanged); connect(&_geoFenceController, &GeoFenceController::dirtyChanged, this, &PlanMasterController::dirtyChanged); @@ -62,12 +62,12 @@ PlanMasterController::~PlanMasterController() } -void PlanMasterController::start(bool editMode) +void PlanMasterController::start(bool flyView) { - _editMode = editMode; - _missionController.start(editMode); - _geoFenceController.start(editMode); - _rallyPointController.start(editMode); + _flyView = flyView; + _missionController.start(_flyView); + _geoFenceController.start(_flyView); + _rallyPointController.start(_flyView); connect(_multiVehicleMgr, &MultiVehicleManager::activeVehicleChanged, this, &PlanMasterController::_activeVehicleChanged); _activeVehicleChanged(_multiVehicleMgr->activeVehicle()); @@ -75,10 +75,10 @@ void PlanMasterController::start(bool editMode) void PlanMasterController::startStaticActiveVehicle(Vehicle* vehicle) { - _editMode = false; - _missionController.start(false); - _geoFenceController.start(false); - _rallyPointController.start(false); + _flyView = true; + _missionController.start(_flyView); + _geoFenceController.start(_flyView); + _rallyPointController.start(_flyView); _activeVehicleChanged(vehicle); } @@ -132,7 +132,7 @@ void PlanMasterController::_activeVehicleChanged(Vehicle* activeVehicle) _geoFenceController.managerVehicleChanged(_managerVehicle); _rallyPointController.managerVehicleChanged(_managerVehicle); - if (_editMode) { + if (!_flyView) { if (!offline()) { // We are in Plan view and we have a newly connected vehicle: // - If there is no plan available in Plan view show the one from the vehicle @@ -164,7 +164,7 @@ void PlanMasterController::loadFromVehicle(void) if (offline()) { qCWarning(PlanMasterControllerLog) << "PlanMasterController::loadFromVehicle called while offline"; - } else if (!_editMode) { + } else if (_flyView) { qCWarning(PlanMasterControllerLog) << "PlanMasterController::loadFromVehicle called from Fly view"; } else if (syncInProgress()) { qCWarning(PlanMasterControllerLog) << "PlanMasterController::loadFromVehicle called while syncInProgress"; @@ -181,7 +181,7 @@ void PlanMasterController::loadFromVehicle(void) void PlanMasterController::_loadMissionComplete(void) { - if (_editMode && _loadGeoFence) { + if (!_flyView && _loadGeoFence) { _loadGeoFence = false; _loadRallyPoints = true; if (_geoFenceController.supported()) { @@ -198,7 +198,7 @@ void PlanMasterController::_loadMissionComplete(void) void PlanMasterController::_loadGeoFenceComplete(void) { - if (_editMode && _loadRallyPoints) { + if (!_flyView && _loadRallyPoints) { _loadRallyPoints = false; if (_rallyPointController.supported()) { qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadGeoFenceComplete _rallyPointController.loadFromVehicle"; @@ -214,7 +214,7 @@ void PlanMasterController::_loadGeoFenceComplete(void) void PlanMasterController::_loadRallyPointsComplete(void) { - if (_editMode) { + if (!_flyView) { _syncInProgress = false; emit syncInProgressChanged(false); } @@ -416,6 +416,11 @@ void PlanMasterController::removeAll(void) _missionController.removeAll(); _geoFenceController.removeAll(); _rallyPointController.removeAll(); + if (_offline) { + _missionController.setDirty(false); + _geoFenceController.setDirty(false); + _rallyPointController.setDirty(false); + } } void PlanMasterController::removeAllFromVehicle(void) @@ -479,7 +484,7 @@ QStringList PlanMasterController::saveNameFilters(void) const return filters; } -QStringList PlanMasterController::saveKmlFilters(void) const +QStringList PlanMasterController::fileKmlFilters(void) const { QStringList filters; diff --git a/src/MissionManager/PlanMasterController.h b/src/MissionManager/PlanMasterController.h index 30dd38144326ac1cf87596bba410a92bd5f3858e..d6d454fa8509cb32c86d56e1bcdfb857597efdcc 100644 --- a/src/MissionManager/PlanMasterController.h +++ b/src/MissionManager/PlanMasterController.h @@ -43,14 +43,17 @@ public: ///< kml file extension for missions Q_PROPERTY(QStringList loadNameFilters READ loadNameFilters CONSTANT) ///< File filter list loading plan files Q_PROPERTY(QStringList saveNameFilters READ saveNameFilters CONSTANT) ///< File filter list saving plan files - Q_PROPERTY(QStringList saveKmlFilters READ saveKmlFilters CONSTANT) ///< File filter list saving KML files + Q_PROPERTY(QStringList fileKmlFilters READ fileKmlFilters CONSTANT) ///< File filter list for load/save KML files /// Should be called immediately upon Component.onCompleted. - /// @param editMode true: controller being used in Plan view, false: controller being used in Fly view - Q_INVOKABLE virtual void start(bool editMode); + Q_INVOKABLE void start(bool flyView); /// Starts the controller using a single static active vehicle. Will not track global active vehicle changes. - Q_INVOKABLE virtual void startStaticActiveVehicle(Vehicle* vehicle); + Q_INVOKABLE void startStaticActiveVehicle(Vehicle* vehicle); + + /// Determines if the plan has all data needed to be saved or sent to the vehicle. Currently the only case where this + /// would return false is when it is still waiting on terrain data to determine correct altitudes. + Q_INVOKABLE bool readyForSaveSend(void) const { return _missionController.readyForSaveSend(); } /// Sends a plan to the specified file /// @param[in] vehicle Vehicle we are sending a plan to @@ -78,7 +81,7 @@ public: QString kmlFileExtension(void) const; QStringList loadNameFilters (void) const; QStringList saveNameFilters (void) const; - QStringList saveKmlFilters (void) const; + QStringList fileKmlFilters (void) const; QJsonDocument saveToJson (); @@ -105,9 +108,9 @@ private: void _showPlanFromManagerVehicle(void); MultiVehicleManager* _multiVehicleMgr; - Vehicle* _controllerVehicle; - Vehicle* _managerVehicle; - bool _editMode; + Vehicle* _controllerVehicle; ///< Offline controller vehicle + Vehicle* _managerVehicle; ///< Either active vehicle or _controllerVehicle if none + bool _flyView; bool _offline; MissionController _missionController; GeoFenceController _geoFenceController; diff --git a/src/MissionManager/PlanMasterControllerTest.cc b/src/MissionManager/PlanMasterControllerTest.cc index c27162bc3443dcfdef59d70111c33a05bda21ec4..eae62cb08b992f1f1ec1ec5030fdae606f7384f6 100644 --- a/src/MissionManager/PlanMasterControllerTest.cc +++ b/src/MissionManager/PlanMasterControllerTest.cc @@ -27,7 +27,7 @@ void PlanMasterControllerTest::init(void) UnitTest::init(); _masterController = new PlanMasterController(this); - _masterController->start(true /* editMode */); + _masterController->start(false /* flyView */); } void PlanMasterControllerTest::cleanup(void) diff --git a/src/MissionManager/QGCFenceCircle.cc b/src/MissionManager/QGCFenceCircle.cc index 3c1e1b658fd66ae1218c95e15f1e35cc4532a8f6..b174ed49817ca1bce8ec61967890c9fdf436c166 100644 --- a/src/MissionManager/QGCFenceCircle.cc +++ b/src/MissionManager/QGCFenceCircle.cc @@ -54,26 +54,32 @@ void QGCFenceCircle::_setDirty(void) void QGCFenceCircle::saveToJson(QJsonObject& json) { - QGCMapCircle::saveToJson(json); - + json[JsonHelper::jsonVersionKey] = _jsonCurrentVersion; json[_jsonInclusionKey] = _inclusion; + QGCMapCircle::saveToJson(json); } bool QGCFenceCircle::loadFromJson(const QJsonObject& json, QString& errorString) { - if (!QGCMapCircle::loadFromJson(json, errorString)) { - return false; - } - errorString.clear(); QList keyInfoList = { - { _jsonInclusionKey, QJsonValue::Bool, true }, + { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, + { _jsonInclusionKey, QJsonValue::Bool, true }, }; if (!JsonHelper::validateKeys(json, keyInfoList, errorString)) { return false; } + if (json[JsonHelper::jsonVersionKey].toInt() != _jsonCurrentVersion) { + errorString = tr("GeoFence Circle only supports version %1").arg(_jsonCurrentVersion); + return false; + } + + if (!QGCMapCircle::loadFromJson(json, errorString)) { + return false; + } + setInclusion(json[_jsonInclusionKey].toBool()); return true; diff --git a/src/MissionManager/QGCFenceCircle.h b/src/MissionManager/QGCFenceCircle.h index 92d6d3ea1febcb81d538c409764f668d3f12aa89..d42a8f356556c730718dcb96d334d0f40db85e35 100644 --- a/src/MissionManager/QGCFenceCircle.h +++ b/src/MissionManager/QGCFenceCircle.h @@ -51,5 +51,7 @@ private: bool _inclusion; + static const int _jsonCurrentVersion = 1; + static const char* _jsonInclusionKey; }; diff --git a/src/MissionManager/QGCFencePolygon.cc b/src/MissionManager/QGCFencePolygon.cc index 1524b67f5c5060da61a0d76c697f008002204ce1..675e1f520777b6cb4444e188b88910e17ec61f43 100644 --- a/src/MissionManager/QGCFencePolygon.cc +++ b/src/MissionManager/QGCFencePolygon.cc @@ -47,26 +47,32 @@ void QGCFencePolygon::_setDirty(void) void QGCFencePolygon::saveToJson(QJsonObject& json) { - QGCMapPolygon::saveToJson(json); - + json[JsonHelper::jsonVersionKey] = _jsonCurrentVersion; json[_jsonInclusionKey] = _inclusion; + QGCMapPolygon::saveToJson(json); } bool QGCFencePolygon::loadFromJson(const QJsonObject& json, bool required, QString& errorString) { - if (!QGCMapPolygon::loadFromJson(json, required, errorString)) { - return false; - } - errorString.clear(); QList keyInfoList = { - { _jsonInclusionKey, QJsonValue::Bool, true }, + { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, + { _jsonInclusionKey, QJsonValue::Bool, true }, }; if (!JsonHelper::validateKeys(json, keyInfoList, errorString)) { return false; } + if (json[JsonHelper::jsonVersionKey].toInt() != _jsonCurrentVersion) { + errorString = tr("GeoFence Polygon only supports version %1").arg(_jsonCurrentVersion); + return false; + } + + if (!QGCMapPolygon::loadFromJson(json, required, errorString)) { + return false; + } + setInclusion(json[_jsonInclusionKey].toBool()); return true; diff --git a/src/MissionManager/QGCFencePolygon.h b/src/MissionManager/QGCFencePolygon.h index 5c8ed56398da2ffac11a519dcdb4cf3d7e4506d0..fb79ca4837a22b3cdb2fede6b5791afd0d930838 100644 --- a/src/MissionManager/QGCFencePolygon.h +++ b/src/MissionManager/QGCFencePolygon.h @@ -51,5 +51,7 @@ private: bool _inclusion; + static const int _jsonCurrentVersion = 1; + static const char* _jsonInclusionKey; }; diff --git a/src/MissionManager/QGCMapCircle.cc b/src/MissionManager/QGCMapCircle.cc index 143956c8e25cf9f8b6221d8ba88ed9771d845bb9..fd3ab5df13956fd8b4d2c801a31d13a4f0a884a8 100644 --- a/src/MissionManager/QGCMapCircle.cc +++ b/src/MissionManager/QGCMapCircle.cc @@ -111,11 +111,11 @@ bool QGCMapCircle::loadFromJson(const QJsonObject& json, QString& errorString) } QGeoCoordinate center; - if (!JsonHelper::loadGeoCoordinate(json[_jsonCenterKey], false /* altitudeRequired */, center, errorString)) { + if (!JsonHelper::loadGeoCoordinate(circleObject[_jsonCenterKey], false /* altitudeRequired */, center, errorString)) { return false; } setCenter(center); - _radius.setRawValue(json[_jsonRadiusKey].toDouble()); + _radius.setRawValue(circleObject[_jsonRadiusKey].toDouble()); return true; } diff --git a/src/MissionManager/QGCMapPolygon.cc b/src/MissionManager/QGCMapPolygon.cc index 3803e203ed08495ae8d2d3cb62dd9728999a143b..feec11d0ffb3dfd85d7aef124fc9380b9a8e9454 100644 --- a/src/MissionManager/QGCMapPolygon.cc +++ b/src/MissionManager/QGCMapPolygon.cc @@ -12,6 +12,7 @@ #include "JsonHelper.h" #include "QGCQGeoCoordinate.h" #include "QGCApplication.h" +#include "KMLFileHelper.h" #include #include @@ -56,9 +57,11 @@ const QGCMapPolygon& QGCMapPolygon::operator=(const QGCMapPolygon& other) clear(); QVariantList vertices = other.path(); - for (int i=0; i()); + QList rgCoord; + foreach (const QVariant& vertexVar, vertices) { + rgCoord.append(vertexVar.value()); } + appendVertices(rgCoord); setDirty(true); @@ -258,6 +261,18 @@ void QGCMapPolygon::appendVertex(const QGeoCoordinate& coordinate) emit pathChanged(); } +void QGCMapPolygon::appendVertices(const QList& coordinates) +{ + QList objects; + + foreach (const QGeoCoordinate& coordinate, coordinates) { + objects.append(new QGCQGeoCoordinate(coordinate, this)); + _polygonPath.append(QVariant::fromValue(coordinate)); + } + _polygonModel.append(objects); + emit pathChanged(); +} + void QGCMapPolygon::_polygonModelDirtyChanged(bool dirty) { if (dirty) { @@ -357,7 +372,7 @@ QGeoCoordinate QGCMapPolygon::vertexCoordinate(int vertex) const if (vertex >= 0 && vertex < _polygonPath.count()) { return _polygonPath[vertex].value(); } else { - qWarning() << "QGCMapPolygon::vertexCoordinate bad vertex requested"; + qWarning() << "QGCMapPolygon::vertexCoordinate bad vertex requested:count" << vertex << _polygonPath.count(); return QGeoCoordinate(); } } @@ -434,84 +449,20 @@ void QGCMapPolygon::offset(double distance) // Update internals clear(); - for (int i=0; ishowMessage(tr("File not found: %1").arg(kmlFile)); - return false; - } - - if (!file.open(QIODevice::ReadOnly)) { - qgcApp()->showMessage(tr("Unable to open file: %1 error: $%2").arg(kmlFile).arg(file.errorString())); - return false; - } - - QDomDocument doc; - QString errorMessage; - int errorLine; - if (!doc.setContent(&file, &errorMessage, &errorLine)) { - qgcApp()->showMessage(tr("Unable to parse KML file: %1 error: %2 line: %3").arg(kmlFile).arg(errorMessage).arg(errorLine)); - return false; - } - - QDomNodeList rgNodes = doc.elementsByTagName("Polygon"); - if (rgNodes.count() == 0) { - qgcApp()->showMessage(tr("Unable to find Polygon node in KML")); - return false; - } - - QDomNode coordinatesNode = rgNodes.item(0).namedItem("outerBoundaryIs").namedItem("LinearRing").namedItem("coordinates"); - if (coordinatesNode.isNull()) { - qgcApp()->showMessage(tr("Internal error: Unable to find coordinates node in KML")); - return false; - } - - QString coordinatesString = coordinatesNode.toElement().text().simplified(); - QStringList rgCoordinateStrings = coordinatesString.split(" "); - + QString errorString; QList rgCoords; - for (int i=0; i rgReversed; - - for (int i=0; ishowMessage(errorString); + return false; } clear(); - for (int i=0; i nedVertices = nedPolygon(); for (int i=0; i& coordinates); /// Adjust the value for the specified coordinate /// @param vertexIndex Polygon point index to modify (0-based) diff --git a/src/MissionManager/QGCMapPolygonTest.h b/src/MissionManager/QGCMapPolygonTest.h index 7f36e926ff372188dbcf5d7bca555fbefb19f880..ff3e783377267a99405e817fcf9d6759228a5829 100644 --- a/src/MissionManager/QGCMapPolygonTest.h +++ b/src/MissionManager/QGCMapPolygonTest.h @@ -14,7 +14,6 @@ #include "QGCMapPolygon.h" #include "QmlObjectListModel.h" -/// Unit test for SurveyMissionItem class QGCMapPolygonTest : public UnitTest { Q_OBJECT diff --git a/src/MissionManager/QGCMapPolygonVisuals.qml b/src/MissionManager/QGCMapPolygonVisuals.qml index c0bd2ee7ba343c155f1b878d930e00f14349ad26..d6512392e222314b6042149ef28446ee84039cf6 100644 --- a/src/MissionManager/QGCMapPolygonVisuals.qml +++ b/src/MissionManager/QGCMapPolygonVisuals.qml @@ -177,13 +177,68 @@ Item { title: qsTr("Select KML File") selectExisting: true nameFilters: [ qsTr("KML files (*.kml)") ] - fileExtension: "kml" + fileExtension: QGroundControl.settingsManager.appSettings.kmlFileExtension + onAcceptedForLoad: { mapPolygon.loadKMLFile(file) + mapFitFunctions.fitMapViewportToMissionItems() close() } } + Menu { + id: menu + + property int _removeVertexIndex + + function popUpWithIndex(curIndex) { + _removeVertexIndex = curIndex + removeVertexItem.visible = (mapPolygon.count > 3 && _removeVertexIndex >= 0) + menu.popup() + } + + MenuItem { + id: removeVertexItem + text: qsTr("Remove vertex") + onTriggered: { + if(menu._removeVertexIndex >= 0) { + mapPolygon.removeVertex(menu._removeVertexIndex) + } + } + } + + MenuSeparator { + visible: removeVertexItem.visible + } + + MenuItem { + text: qsTr("Circle" ) + onTriggered: resetCircle() + } + + MenuItem { + text: qsTr("Polygon") + onTriggered: resetPolygon() + } + + MenuItem { + text: qsTr("Set radius..." ) + visible: _circle + onTriggered: radiusDialog.visible = true + } + + MenuItem { + text: qsTr("Edit position..." ) + enabled: _circle + onTriggered: qgcView.showDialog(editPositionDialog, qsTr("Edit Position"), qgcView.showDialogDefaultWidth, StandardButton.Cancel) + } + + MenuItem { + text: qsTr("Load KML...") + onTriggered: kmlLoadDialog.openForLoad() + } + } + Component { id: polygonComponent @@ -289,32 +344,36 @@ Item { } } - onClicked: mapPolygon.removeVertex(polygonVertex) + onClicked: { + menu.popUpWithIndex(polygonVertex) + } } } Component { id: centerDragHandle - MapQuickItem { id: mapQuickItem - anchorPoint.x: dragHandle.width / 2 - anchorPoint.y: dragHandle.height / 2 + anchorPoint.x: dragHandle.width * 0.5 + anchorPoint.y: dragHandle.height * 0.5 z: _zorderDragHandle - sourceItem: Rectangle { - id: dragHandle - width: ScreenTools.defaultFontPixelHeight * 1.5 - height: width - radius: width / 2 - color: "white" - opacity: .90 - - QGCLabel { - anchors.horizontalCenter: parent.horizontalCenter - anchors.verticalCenter: parent.verticalCenter - text: "..." - color: "black" + id: dragHandle + width: ScreenTools.defaultFontPixelHeight * 1.5 + height: width + radius: width * 0.5 + color: Qt.rgba(1,1,1,0.8) + border.color: Qt.rgba(0,0,0,0.25) + border.width: 1 + QGCColoredImage { + width: parent.width + height: width + color: Qt.rgba(0,0,0,1) + mipmap: true + fillMode: Image.PreserveAspectFit + source: "/qmlimages/MapCenter.svg" + sourceSize.height: height + anchors.centerIn: parent } } } @@ -325,7 +384,7 @@ Item { MapQuickItem { id: mapQuickItem - anchorPoint.x: dragHandle.width / 2 + anchorPoint.x: dragHandle.width / 2 anchorPoint.y: dragHandle.height / 2 z: _zorderDragHandle visible: !_circle @@ -333,12 +392,13 @@ Item { property int polygonVertex sourceItem: Rectangle { - id: dragHandle - width: ScreenTools.defaultFontPixelHeight * 1.5 - height: width - radius: width / 2 - color: "white" - opacity: .90 + id: dragHandle + width: ScreenTools.defaultFontPixelHeight * 1.5 + height: width + radius: width * 0.5 + color: Qt.rgba(1,1,1,0.8) + border.color: Qt.rgba(0,0,0,0.25) + border.width: 1 } } } @@ -391,44 +451,16 @@ Item { onItemCoordinateChanged: mapPolygon.center = itemCoordinate onDragStart: mapPolygon.centerDrag = true onDragStop: mapPolygon.centerDrag = false - onClicked: menu.popup() + + onClicked: { + menu.popUpWithIndex(-1) //-- Don't offer a choice to delete vertex (cur index == -1) + } function setRadiusFromDialog() { setCircleRadius(mapPolygon.center, radiusField.text) radiusDialog.visible = false } - Menu { - id: menu - - MenuItem { - text: qsTr("Circle" ) - onTriggered: resetCircle() - } - - MenuItem { - text: qsTr("Polygon") - onTriggered: resetPolygon() - } - - MenuItem { - text: qsTr("Set radius..." ) - visible: _circle - onTriggered: radiusDialog.visible = true - } - - MenuItem { - text: qsTr("Edit position..." ) - enabled: _circle - onTriggered: qgcView.showDialog(editPositionDialog, qsTr("Edit Position"), qgcView.showDialogDefaultWidth, StandardButton.Cancel) - } - - MenuItem { - text: qsTr("Load KML...") - onTriggered: kmlLoadDialog.openForLoad() - } - } - Rectangle { id: radiusDialog anchors.margins: _margin diff --git a/src/MissionManager/QGCMapPolyline.cc b/src/MissionManager/QGCMapPolyline.cc index cb3a39ef8107a35e42d373b7a4d38d6a308ccf29..9d6eda2b93168b5b5500017106cf6065c8cb836d 100644 --- a/src/MissionManager/QGCMapPolyline.cc +++ b/src/MissionManager/QGCMapPolyline.cc @@ -12,6 +12,7 @@ #include "JsonHelper.h" #include "QGCQGeoCoordinate.h" #include "QGCApplication.h" +#include "KMLFileHelper.h" #include #include @@ -338,77 +339,15 @@ QList QGCMapPolyline::offsetPolyline(double distance) bool QGCMapPolyline::loadKMLFile(const QString& kmlFile) { - QFile file(kmlFile); - - if (!file.exists()) { - qgcApp()->showMessage(tr("File not found: %1").arg(kmlFile)); - return false; - } - - if (!file.open(QIODevice::ReadOnly)) { - qgcApp()->showMessage(tr("Unable to open file: %1 error: $%2").arg(kmlFile).arg(file.errorString())); - return false; - } - - QDomDocument doc; - QString errorMessage; - int errorLine; - if (!doc.setContent(&file, &errorMessage, &errorLine)) { - qgcApp()->showMessage(tr("Unable to parse KML file: %1 error: %2 line: %3").arg(kmlFile).arg(errorMessage).arg(errorLine)); - return false; - } - - QDomNodeList rgNodes = doc.elementsByTagName("Polygon"); - if (rgNodes.count() == 0) { - qgcApp()->showMessage(tr("Unable to find Polygon node in KML")); - return false; - } - - QDomNode coordinatesNode = rgNodes.item(0).namedItem("outerBoundaryIs").namedItem("LinearRing").namedItem("coordinates"); - if (coordinatesNode.isNull()) { - qgcApp()->showMessage(tr("Internal error: Unable to find coordinates node in KML")); - return false; - } - - QString coordinatesString = coordinatesNode.toElement().text().simplified(); - QStringList rgCoordinateStrings = coordinatesString.split(" "); - + QString errorString; QList rgCoords; - for (int i=0; i rgReversed; - - for (int i=0; ishowMessage(errorString); + return false; } clear(); - for (int i=0; i& coordinates) +{ + QList objects; + + foreach (const QGeoCoordinate& coordinate, coordinates) { + objects.append(new QGCQGeoCoordinate(coordinate, this)); + _polylinePath.append(QVariant::fromValue(coordinate)); + } + _polylineModel.append(objects); + emit pathChanged(); +} diff --git a/src/MissionManager/QGCMapPolyline.h b/src/MissionManager/QGCMapPolyline.h index f4ec936b2e2bebaab59f84c0d08d4f7b970d4ad3..08389a0416eda69e18996b0e2515e129206dc272 100644 --- a/src/MissionManager/QGCMapPolyline.h +++ b/src/MissionManager/QGCMapPolyline.h @@ -34,6 +34,7 @@ public: Q_INVOKABLE void clear(void); Q_INVOKABLE void appendVertex(const QGeoCoordinate& coordinate); Q_INVOKABLE void removeVertex(int vertexIndex); + Q_INVOKABLE void appendVertices(const QList& coordinates); /// Adjust the value for the specified coordinate /// @param vertexIndex Polygon point index to modify (0-based) diff --git a/src/MissionManager/QGCMapPolylineVisuals.qml b/src/MissionManager/QGCMapPolylineVisuals.qml index 27f9ed8a787b0fb76effea19e02e4ed687fc69b4..1d86a24ff7c9c36393f5a0d2a7ae823c78c397de 100644 --- a/src/MissionManager/QGCMapPolylineVisuals.qml +++ b/src/MissionManager/QGCMapPolylineVisuals.qml @@ -120,13 +120,38 @@ Item { selectExisting: true nameFilters: [ qsTr("KML files (*.kml)") ] - onAcceptedForLoad: { mapPolyline.loadKMLFile(file) close() } } + Menu { + id: menu + property int _removeVertexIndex + + function popUpWithIndex(curIndex) { + _removeVertexIndex = curIndex + removeVertexItem.visible = mapPolyline.count > 2 + menu.popup() + } + + MenuItem { + id: removeVertexItem + text: qsTr("Remove vertex" ) + onTriggered: mapPolyline.removeVertex(menu._removeVertexIndex) + } + + MenuSeparator { + visible: removeVertexItem.visible + } + + MenuItem { + text: qsTr("Load KML...") + onTriggered: kmlLoadDialog.openForLoad() + } + } + Component { id: polylineComponent @@ -152,7 +177,7 @@ Item { width: ScreenTools.defaultFontPixelHeight * 1.5 height: width radius: width / 2 - border.color: "white" + border.color: "white" color: "transparent" opacity: .50 z: _zorderSplitHandle @@ -227,7 +252,10 @@ Item { } } - onClicked: mapPolyline.removeVertex(polylineVertex) + onClicked: { + menu.popUpWithIndex(polylineVertex) + } + } } @@ -243,12 +271,13 @@ Item { property int polylineVertex sourceItem: Rectangle { - id: dragHandle - width: ScreenTools.defaultFontPixelHeight * 1.5 - height: width - radius: width / 2 - color: "white" - opacity: .90 + id: dragHandle + width: ScreenTools.defaultFontPixelHeight * 1.5 + height: width + radius: width * 0.5 + color: Qt.rgba(1,1,1,0.8) + border.color: Qt.rgba(0,0,0,0.25) + border.width: 1 } } } diff --git a/src/MissionManager/RallyPointController.cc b/src/MissionManager/RallyPointController.cc index bc88fdce93efa183f0aa5fa6cf744dc5c18f419e..58a369aa4ece83d646c9a3e0d97b2a7980de33c9 100644 --- a/src/MissionManager/RallyPointController.cc +++ b/src/MissionManager/RallyPointController.cc @@ -82,13 +82,28 @@ void RallyPointController::managerVehicleChanged(Vehicle* managerVehicle) bool RallyPointController::load(const QJsonObject& json, QString& errorString) { + removeAll(); + + errorString.clear(); + + if (json.contains(JsonHelper::jsonVersionKey) && json[JsonHelper::jsonVersionKey].toInt() == 1) { + // We just ignore old version 1 data + return true; + } + + QList keyInfoList = { + { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, + { _jsonPointsKey, QJsonValue::Array, true }, + }; + if (!JsonHelper::validateKeys(json, keyInfoList, errorString)) { + return false; + } + QString errorStr; QString errorMessage = tr("Rally: %1"); - // Check for required keys - QStringList requiredKeys = { _jsonPointsKey }; - if (!JsonHelper::validateRequiredKeys(json, requiredKeys, errorStr)) { - errorString = errorMessage.arg(errorStr); + if (json[JsonHelper::jsonVersionKey].toInt() != _jsonCurrentVersion) { + errorString = tr("Rally Points supports version %1").arg(_jsonCurrentVersion); return false; } @@ -97,7 +112,7 @@ bool RallyPointController::load(const QJsonObject& json, QString& errorString) errorString = errorMessage.arg(errorStr); return false; } - _points.clearAndDeleteContents(); + QObjectList pointList; for (int i=0; i rgPo { // Fly view always reloads on _loadComplete // Plan view only reloads on _loadComplete if specifically requested - if (!_editMode || _itemsRequested) { + if (_flyView || _itemsRequested) { _points.clearAndDeleteContents(); QObjectList pointList; for (int i=0; i rgPo void RallyPointController::_managerSendComplete(bool error) { // Fly view always reloads after send - if (!error && _editMode) { + if (!error && !_flyView) { showPlanFromManagerVehicle(); } } @@ -286,7 +301,7 @@ void RallyPointController::_updateContainsItems(void) bool RallyPointController::showPlanFromManagerVehicle (void) { - qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle _editMode" << _editMode; + qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle _flyView" << _flyView; if (_masterController->offline()) { qCWarning(RallyPointControllerLog) << "RallyPointController::showPlanFromManagerVehicle called while offline"; return true; // stops further propagation of showPlanFromManagerVehicle due to error @@ -300,10 +315,8 @@ bool RallyPointController::showPlanFromManagerVehicle (void) qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle: syncInProgress wait for signal"; return true; } else { - // Fake a _loadComplete with the current items - qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle: sync complete simulate signal"; + qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle: sync complete"; _itemsRequested = true; - _managerLoadComplete(_rallyPointManager->points()); return false; } } diff --git a/src/MissionManager/RallyPointController.h b/src/MissionManager/RallyPointController.h index 7df2dc79e8ad56cb1a5dc77ba67a10e153c2ed3a..eff4b242ee2176b3b96bc8cd7982c3d79a5814ab 100644 --- a/src/MissionManager/RallyPointController.h +++ b/src/MissionManager/RallyPointController.h @@ -74,6 +74,8 @@ private: QObject* _currentRallyPoint; bool _itemsRequested; + static const int _jsonCurrentVersion = 2; + static const char* _jsonFileTypeValue; static const char* _jsonPointsKey; }; diff --git a/src/MissionManager/RallyPointManager.cc b/src/MissionManager/RallyPointManager.cc index 072ac4ae0d8d96157b507197d815e224be5d843d..e0880cae67856372387198b9cbc8e0d68f89d75e 100644 --- a/src/MissionManager/RallyPointManager.cc +++ b/src/MissionManager/RallyPointManager.cc @@ -8,6 +8,7 @@ ****************************************************************************/ #include "RallyPointManager.h" +#include "ParameterManager.h" #include "Vehicle.h" QGC_LOGGING_CATEGORY(RallyPointManagerLog, "RallyPointManagerLog") @@ -15,10 +16,16 @@ QGC_LOGGING_CATEGORY(RallyPointManagerLog, "RallyPointManagerLog") RallyPointManager::RallyPointManager(Vehicle* vehicle) : QObject(vehicle) , _vehicle(vehicle) + , _planManager (vehicle, MAV_MISSION_TYPE_RALLY) { - + connect(&_planManager, &PlanManager::inProgressChanged, this, &RallyPointManager::inProgressChanged); + connect(&_planManager, &PlanManager::error, this, &RallyPointManager::error); + connect(&_planManager, &PlanManager::removeAllComplete, this, &RallyPointManager::removeAllComplete); + connect(&_planManager, &PlanManager::sendComplete, this, &RallyPointManager::sendComplete); + connect(&_planManager, &PlanManager::newMissionItemsAvailable, this, &RallyPointManager::_planManagerLoadComplete); } + RallyPointManager::~RallyPointManager() { @@ -33,24 +40,65 @@ void RallyPointManager::_sendError(ErrorCode_t errorCode, const QString& errorMs void RallyPointManager::loadFromVehicle(void) { - // No support in generic vehicle - emit loadComplete(QList()); + _planManager.loadFromVehicle(); } void RallyPointManager::sendToVehicle(const QList& rgPoints) { - // No support in generic vehicle - Q_UNUSED(rgPoints); - emit sendComplete(false /* error */); + QList rallyItems; + + for (int i=0; icapabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY) && (_vehicle->maxProtoVersion() >= 200); } + +void RallyPointManager::_planManagerLoadComplete(bool removeAllRequested) +{ + _rgPoints.clear(); + + Q_UNUSED(removeAllRequested); + + const QList& rallyItems = _planManager.missionItems(); + + for (int i=0; icommand(); + + if (command == MAV_CMD_NAV_RALLY_POINT) { + _rgPoints.append(QGeoCoordinate(item->param5(), item->param6(), item->param7())); + + + } else { + qCDebug(RallyPointManagerLog) << "RallyPointManager load: Unsupported command %1" << item->command(); + break; + } + } + + + emit loadComplete(_rgPoints); +} diff --git a/src/MissionManager/RallyPointManager.h b/src/MissionManager/RallyPointManager.h index cf9e1747d502c331912383aa726f74fd84743884..dc49edb82549b42049cc12b0c852d4eab46231a9 100644 --- a/src/MissionManager/RallyPointManager.h +++ b/src/MissionManager/RallyPointManager.h @@ -14,8 +14,10 @@ #include #include "QGCLoggingCategory.h" +#include "PlanManager.h" class Vehicle; +class PlanManager; Q_DECLARE_LOGGING_CATEGORY(RallyPointManagerLog) @@ -66,10 +68,14 @@ signals: void removeAllComplete (bool error); void sendComplete (bool error); +private slots: + void _planManagerLoadComplete (bool removeAllRequested); + protected: void _sendError(ErrorCode_t errorCode, const QString& errorMsg); Vehicle* _vehicle; + PlanManager _planManager; QList _rgPoints; }; diff --git a/src/MissionManager/SectionTest.cc b/src/MissionManager/SectionTest.cc index f3c72c1e9566d30876ee7b7e816ef0e082e3246e..4d8ffc3fb3fae70bb4b9b0077e625eb82dfb4343 100644 --- a/src/MissionManager/SectionTest.cc +++ b/src/MissionManager/SectionTest.cc @@ -8,7 +8,7 @@ ****************************************************************************/ #include "SectionTest.h" -#include "SurveyMissionItem.h" +#include "SurveyComplexItem.h" SectionTest::SectionTest(void) : _simpleItem(NULL) @@ -37,7 +37,7 @@ void SectionTest::init(void) 70.1234567, true, // autoContinue false); // isCurrentItem - _simpleItem = new SimpleMissionItem(_offlineVehicle, true /* editMode */, missionItem); + _simpleItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, missionItem, this); } void SectionTest::cleanup(void) @@ -77,13 +77,13 @@ void SectionTest::_commonScanTest(Section* section) QmlObjectListModel waypointVisualItems; MissionItem waypointItem(0, MAV_CMD_NAV_WAYPOINT, MAV_FRAME_GLOBAL_RELATIVE_ALT, 0, 0, 0, 0, 0, 0, 0, true, false); - SimpleMissionItem simpleItem(_offlineVehicle, true /* editMode */, waypointItem); + SimpleMissionItem simpleItem(_offlineVehicle, false /* flyView */, waypointItem, this); waypointVisualItems.append(&simpleItem); waypointVisualItems.append(&simpleItem); waypointVisualItems.append(&simpleItem); QmlObjectListModel complexVisualItems; - SurveyMissionItem surveyItem(_offlineVehicle); + SurveyComplexItem surveyItem(_offlineVehicle, false /* fly View */, QString() /* kmlFile */, this /* parent */); complexVisualItems.append(&surveyItem); // This tests the common cases which should not lead to scan succeess diff --git a/src/MissionManager/SimpleMissionItem.cc b/src/MissionManager/SimpleMissionItem.cc index cc3ee0783eb534c86811db514ce1031909d9056d..e24f2a0957757b62ea119b663a8afb01cb42efd9 100644 --- a/src/MissionManager/SimpleMissionItem.cc +++ b/src/MissionManager/SimpleMissionItem.cc @@ -27,6 +27,10 @@ FactMetaData* SimpleMissionItem::_frameMetaData = NULL; FactMetaData* SimpleMissionItem::_latitudeMetaData = NULL; FactMetaData* SimpleMissionItem::_longitudeMetaData = NULL; +const char* SimpleMissionItem::_jsonAltitudeModeKey = "AltitudeMode"; +const char* SimpleMissionItem::_jsonAltitudeKey = "Altitude"; +const char* SimpleMissionItem::_jsonAMSLAltAboveTerrainKey = "AMSLAltAboveTerrain"; + struct EnumInfo_s { const char * label; MAV_FRAME frame; @@ -47,30 +51,29 @@ static const struct EnumInfo_s _rgMavFrameInfo[] = { { "MAV_FRAME_GLOBAL_TERRAIN_ALT_INT", MAV_FRAME_GLOBAL_TERRAIN_ALT_INT }, }; -SimpleMissionItem::SimpleMissionItem(Vehicle* vehicle, QObject* parent) - : VisualMissionItem(vehicle, parent) - , _rawEdit(false) - , _dirty(false) - , _ignoreDirtyChangeSignals(false) - , _speedSection(NULL) - , _cameraSection(NULL) - , _commandTree(qgcApp()->toolbox()->missionCommandTree()) - , _altitudeRelativeToHomeFact (0, "Altitude is relative to home", FactMetaData::valueTypeUint32) - , _supportedCommandFact (0, "Command:", FactMetaData::valueTypeUint32) - , _param1MetaData(FactMetaData::valueTypeDouble) - , _param2MetaData(FactMetaData::valueTypeDouble) - , _param3MetaData(FactMetaData::valueTypeDouble) - , _param4MetaData(FactMetaData::valueTypeDouble) - , _param5MetaData(FactMetaData::valueTypeDouble) - , _param6MetaData(FactMetaData::valueTypeDouble) - , _param7MetaData(FactMetaData::valueTypeDouble) - , _syncingAltitudeRelativeToHomeAndFrame (false) - , _syncingHeadingDegreesAndParam4 (false) +SimpleMissionItem::SimpleMissionItem(Vehicle* vehicle, bool flyView, QObject* parent) + : VisualMissionItem (vehicle, flyView, parent) + , _rawEdit (false) + , _dirty (false) + , _ignoreDirtyChangeSignals (false) + , _speedSection (NULL) + , _cameraSection (NULL) + , _commandTree (qgcApp()->toolbox()->missionCommandTree()) + , _supportedCommandFact (0, "Command:", FactMetaData::valueTypeUint32) + , _altitudeMode (AltitudeRelative) + , _altitudeFact (0, "Altitude", FactMetaData::valueTypeDouble) + , _amslAltAboveTerrainFact (0, "Alt above terrain", FactMetaData::valueTypeDouble) + , _param1MetaData (FactMetaData::valueTypeDouble) + , _param2MetaData (FactMetaData::valueTypeDouble) + , _param3MetaData (FactMetaData::valueTypeDouble) + , _param4MetaData (FactMetaData::valueTypeDouble) + , _param5MetaData (FactMetaData::valueTypeDouble) + , _param6MetaData (FactMetaData::valueTypeDouble) + , _param7MetaData (FactMetaData::valueTypeDouble) + , _syncingHeadingDegreesAndParam4 (false) { _editorQml = QStringLiteral("qrc:/qml/SimpleItemEditor.qml"); - _altitudeRelativeToHomeFact.setRawValue(true); - _setupMetaData(); _connectSignals(); _updateOptionalSections(); @@ -78,115 +81,129 @@ SimpleMissionItem::SimpleMissionItem(Vehicle* vehicle, QObject* parent) setDefaultsForCommand(); _rebuildFacts(); - connect(&_missionItem, &MissionItem::specifiedFlightSpeedChanged, this, &SimpleMissionItem::specifiedFlightSpeedChanged); - - connect(this, &SimpleMissionItem::sequenceNumberChanged, this, &SimpleMissionItem::lastSequenceNumberChanged); - connect(this, &SimpleMissionItem::cameraSectionChanged, this, &SimpleMissionItem::_setDirtyFromSignal); - connect(this, &SimpleMissionItem::cameraSectionChanged, this, &SimpleMissionItem::_updateLastSequenceNumber); -} - -SimpleMissionItem::SimpleMissionItem(Vehicle* vehicle, bool editMode, const MissionItem& missionItem, QObject* parent) - : VisualMissionItem(vehicle, parent) - , _missionItem(missionItem) - , _rawEdit(false) - , _dirty(false) - , _ignoreDirtyChangeSignals(false) - , _speedSection(NULL) - , _cameraSection(NULL) - , _commandTree(qgcApp()->toolbox()->missionCommandTree()) - , _altitudeRelativeToHomeFact (0, "Altitude is relative to home", FactMetaData::valueTypeUint32) - , _supportedCommandFact (0, "Command:", FactMetaData::valueTypeUint32) - , _param1MetaData(FactMetaData::valueTypeDouble) - , _param2MetaData(FactMetaData::valueTypeDouble) - , _param3MetaData(FactMetaData::valueTypeDouble) - , _param4MetaData(FactMetaData::valueTypeDouble) - , _param5MetaData(FactMetaData::valueTypeDouble) - , _param6MetaData(FactMetaData::valueTypeDouble) - , _param7MetaData(FactMetaData::valueTypeDouble) - , _syncingAltitudeRelativeToHomeAndFrame (false) + setDirty(false); +} + +SimpleMissionItem::SimpleMissionItem(Vehicle* vehicle, bool flyView, const MissionItem& missionItem, QObject* parent) + : VisualMissionItem (vehicle, flyView, parent) + , _missionItem (missionItem) + , _rawEdit (false) + , _dirty (false) + , _ignoreDirtyChangeSignals (false) + , _speedSection (NULL) + , _cameraSection (NULL) + , _commandTree (qgcApp()->toolbox()->missionCommandTree()) + , _supportedCommandFact (0, "Command:", FactMetaData::valueTypeUint32) + , _altitudeFact (0, "Altitude", FactMetaData::valueTypeDouble) + , _amslAltAboveTerrainFact (0, "Alt above terrain", FactMetaData::valueTypeDouble) + , _param1MetaData (FactMetaData::valueTypeDouble) + , _param2MetaData (FactMetaData::valueTypeDouble) + , _param3MetaData (FactMetaData::valueTypeDouble) + , _param4MetaData (FactMetaData::valueTypeDouble) + , _param5MetaData (FactMetaData::valueTypeDouble) + , _param6MetaData (FactMetaData::valueTypeDouble) + , _param7MetaData (FactMetaData::valueTypeDouble) , _syncingHeadingDegreesAndParam4 (false) { _editorQml = QStringLiteral("qrc:/qml/SimpleItemEditor.qml"); - _altitudeRelativeToHomeFact.setRawValue(true); + struct MavFrame2AltMode_s { + MAV_FRAME mavFrame; + AltitudeMode altMode; + }; + + const struct MavFrame2AltMode_s rgMavFrame2AltMode[] = { + { MAV_FRAME_GLOBAL_TERRAIN_ALT, AltitudeTerrainFrame }, + { MAV_FRAME_GLOBAL, AltitudeAbsolute }, + { MAV_FRAME_GLOBAL_RELATIVE_ALT, AltitudeRelative }, + }; + _altitudeMode = AltitudeRelative; + for (size_t i=0; itoolbox()->missionCommandTree()) - , _altitudeRelativeToHomeFact (0, "Altitude is relative to home", FactMetaData::valueTypeUint32) - , _supportedCommandFact (0, "Command:", FactMetaData::valueTypeUint32) - , _param1MetaData(FactMetaData::valueTypeDouble) - , _param2MetaData(FactMetaData::valueTypeDouble) - , _param3MetaData(FactMetaData::valueTypeDouble) - , _param4MetaData(FactMetaData::valueTypeDouble) - , _syncingAltitudeRelativeToHomeAndFrame (false) + // Signal coordinate changed to kick off terrain query + emit coordinateChanged(coordinate()); + + setDirty(false); +} + +SimpleMissionItem::SimpleMissionItem(const SimpleMissionItem& other, bool flyView, QObject* parent) + : VisualMissionItem (other, flyView, parent) + , _missionItem (other._vehicle) + , _rawEdit (false) + , _dirty (false) + , _ignoreDirtyChangeSignals (false) + , _speedSection (NULL) + , _cameraSection (NULL) + , _commandTree (qgcApp()->toolbox()->missionCommandTree()) + , _supportedCommandFact (0, "Command:", FactMetaData::valueTypeUint32) + , _altitudeMode (other._altitudeMode) + , _altitudeFact (0, "Altitude", FactMetaData::valueTypeDouble) + , _amslAltAboveTerrainFact (qQNaN(), "Alt above terrain", FactMetaData::valueTypeDouble) + , _param1MetaData (FactMetaData::valueTypeDouble) + , _param2MetaData (FactMetaData::valueTypeDouble) + , _param3MetaData (FactMetaData::valueTypeDouble) + , _param4MetaData (FactMetaData::valueTypeDouble) , _syncingHeadingDegreesAndParam4 (false) { _editorQml = QStringLiteral("qrc:/qml/SimpleItemEditor.qml"); + _altitudeFact.setRawValue(other._altitudeFact.rawValue()); + _amslAltAboveTerrainFact.setRawValue(other._amslAltAboveTerrainFact.rawValue()); + _setupMetaData(); _connectSignals(); _updateOptionalSections(); - - *this = other; - - _rebuildFacts(); -} - -const SimpleMissionItem& SimpleMissionItem::operator=(const SimpleMissionItem& other) -{ - VisualMissionItem::operator=(other); - - setRawEdit(other._rawEdit); - setDirty(other._dirty); - setHomePositionSpecialCase(other._homePositionSpecialCase); - _syncFrameToAltitudeRelativeToHome(); _rebuildFacts(); - - return *this; + setDirty(false); } void SimpleMissionItem::_connectSignals(void) { // Connect to change signals to track dirty state - connect(&_missionItem._param1Fact, &Fact::valueChanged, this, &SimpleMissionItem::_setDirtyFromSignal); - connect(&_missionItem._param2Fact, &Fact::valueChanged, this, &SimpleMissionItem::_setDirtyFromSignal); - connect(&_missionItem._param3Fact, &Fact::valueChanged, this, &SimpleMissionItem::_setDirtyFromSignal); - connect(&_missionItem._param4Fact, &Fact::valueChanged, this, &SimpleMissionItem::_setDirtyFromSignal); - connect(&_missionItem._param5Fact, &Fact::valueChanged, this, &SimpleMissionItem::_setDirtyFromSignal); - connect(&_missionItem._param6Fact, &Fact::valueChanged, this, &SimpleMissionItem::_setDirtyFromSignal); - connect(&_missionItem._param7Fact, &Fact::valueChanged, this, &SimpleMissionItem::_setDirtyFromSignal); - connect(&_missionItem._frameFact, &Fact::valueChanged, this, &SimpleMissionItem::_setDirtyFromSignal); - connect(&_missionItem._commandFact, &Fact::valueChanged, this, &SimpleMissionItem::_setDirtyFromSignal); - connect(&_missionItem, &MissionItem::sequenceNumberChanged, this, &SimpleMissionItem::_setDirtyFromSignal); - - // Values from these facts must propagate back and forth between the real object storage - connect(&_altitudeRelativeToHomeFact, &Fact::valueChanged, this, &SimpleMissionItem::_syncAltitudeRelativeToHomeToFrame); - connect(&_missionItem._frameFact, &Fact::valueChanged, this, &SimpleMissionItem::_syncFrameToAltitudeRelativeToHome); + connect(&_missionItem._param1Fact, &Fact::valueChanged, this, &SimpleMissionItem::_setDirty); + connect(&_missionItem._param2Fact, &Fact::valueChanged, this, &SimpleMissionItem::_setDirty); + connect(&_missionItem._param3Fact, &Fact::valueChanged, this, &SimpleMissionItem::_setDirty); + connect(&_missionItem._param4Fact, &Fact::valueChanged, this, &SimpleMissionItem::_setDirty); + connect(&_missionItem._param5Fact, &Fact::valueChanged, this, &SimpleMissionItem::_setDirty); + connect(&_missionItem._param6Fact, &Fact::valueChanged, this, &SimpleMissionItem::_setDirty); + connect(&_missionItem._param7Fact, &Fact::valueChanged, this, &SimpleMissionItem::_setDirty); + connect(&_missionItem._frameFact, &Fact::valueChanged, this, &SimpleMissionItem::_setDirty); + connect(&_missionItem._commandFact, &Fact::valueChanged, this, &SimpleMissionItem::_setDirty); + connect(&_missionItem, &MissionItem::sequenceNumberChanged, this, &SimpleMissionItem::_setDirty); + connect(this, &SimpleMissionItem::altitudeModeChanged,this, &SimpleMissionItem::_setDirty); + + connect(&_altitudeFact, &Fact::valueChanged, this, &SimpleMissionItem::_altitudeChanged); + connect(this, &SimpleMissionItem::altitudeModeChanged,this, &SimpleMissionItem::_altitudeModeChanged); + connect(this, &SimpleMissionItem::terrainAltitudeChanged,this, &SimpleMissionItem::_terrainAltChanged); + + connect(this, &SimpleMissionItem::sequenceNumberChanged, this, &SimpleMissionItem::lastSequenceNumberChanged); + connect(this, &SimpleMissionItem::cameraSectionChanged, this, &SimpleMissionItem::_setDirty); + connect(this, &SimpleMissionItem::cameraSectionChanged, this, &SimpleMissionItem::_updateLastSequenceNumber); // These are coordinate parameters, they must emit coordinateChanged signal - connect(&_missionItem._param5Fact, &Fact::valueChanged, this, &SimpleMissionItem::_sendCoordinateChanged); - connect(&_missionItem._param6Fact, &Fact::valueChanged, this, &SimpleMissionItem::_sendCoordinateChanged); - connect(&_missionItem._param7Fact, &Fact::valueChanged, this, &SimpleMissionItem::_sendCoordinateChanged); + connect(&_missionItem._param5Fact, &Fact::valueChanged, this, &SimpleMissionItem::_sendCoordinateChanged); + connect(&_missionItem._param6Fact, &Fact::valueChanged, this, &SimpleMissionItem::_sendCoordinateChanged); + connect(&_missionItem._param7Fact, &Fact::valueChanged, this, &SimpleMissionItem::_sendCoordinateChanged); // The following changes may also change friendlyEditAllowed connect(&_missionItem._autoContinueFact, &Fact::valueChanged, this, &SimpleMissionItem::_sendFriendlyEditAllowedChanged); @@ -203,15 +220,18 @@ void SimpleMissionItem::_connectSignals(void) connect(&_missionItem._commandFact, &Fact::valueChanged, this, &SimpleMissionItem::isStandaloneCoordinateChanged); // Whenever these properties change the ui model changes as well - connect(this, &SimpleMissionItem::commandChanged, this, &SimpleMissionItem::_rebuildFacts); - connect(this, &SimpleMissionItem::rawEditChanged, this, &SimpleMissionItem::_rebuildFacts); + connect(this, &SimpleMissionItem::commandChanged, this, &SimpleMissionItem::_rebuildFacts); + connect(this, &SimpleMissionItem::rawEditChanged, this, &SimpleMissionItem::_rebuildFacts); // These fact signals must alway signal out through SimpleMissionItem signals connect(&_missionItem._commandFact, &Fact::valueChanged, this, &SimpleMissionItem::_sendCommandChanged); - connect(&_missionItem._frameFact, &Fact::valueChanged, this, &SimpleMissionItem::_sendFrameChanged); - // Sequence number is kept in mission iteem, so we need to propagate signal up as well - connect(&_missionItem, &MissionItem::sequenceNumberChanged, this, &SimpleMissionItem::sequenceNumberChanged); + // Propogate signals from MissionItem up to SimpleMissionItem + connect(&_missionItem, &MissionItem::sequenceNumberChanged, this, &SimpleMissionItem::sequenceNumberChanged); + connect(&_missionItem, &MissionItem::specifiedFlightSpeedChanged, this, &SimpleMissionItem::specifiedFlightSpeedChanged); + + // Firmware type change can affect supportsTerrainFrame + connect(_vehicle, &Vehicle::firmwareTypeChanged, this, &SimpleMissionItem::supportsTerrainFrameChanged); } void SimpleMissionItem::_setupMetaData(void) @@ -222,6 +242,7 @@ void SimpleMissionItem::_setupMetaData(void) if (!_altitudeMetaData) { _altitudeMetaData = new FactMetaData(FactMetaData::valueTypeDouble); _altitudeMetaData->setRawUnits("m"); + _altitudeMetaData->setRawIncrement(1); _altitudeMetaData->setDecimalPlaces(2); enumStrings.clear(); @@ -260,6 +281,8 @@ void SimpleMissionItem::_setupMetaData(void) _missionItem._commandFact.setMetaData(_commandMetaData); _missionItem._frameFact.setMetaData(_frameMetaData); + _altitudeFact.setMetaData(_altitudeMetaData); + _amslAltAboveTerrainFact.setMetaData(_altitudeMetaData); } SimpleMissionItem::~SimpleMissionItem() @@ -276,6 +299,14 @@ void SimpleMissionItem::save(QJsonArray& missionItems) MissionItem* item = items[i]; QJsonObject saveObject; item->save(saveObject); + if (i == 0) { + // This is the main simple item, save the alt/terrain data + if (specifiesCoordinate()) { + saveObject[_jsonAltitudeModeKey] = _altitudeMode; + saveObject[_jsonAltitudeKey] = _altitudeFact.rawValue().toDouble(); + saveObject[_jsonAMSLAltAboveTerrainKey] = _amslAltAboveTerrainFact.rawValue().toDouble(); + } + } missionItems.append(saveObject); item->deleteLater(); } @@ -285,6 +316,11 @@ bool SimpleMissionItem::load(QTextStream &loadStream) { bool success; if ((success = _missionItem.load(loadStream))) { + if (specifiesCoordinate()) { + _altitudeMode = _missionItem.relativeAltitude() ? AltitudeRelative : AltitudeAbsolute; + _altitudeFact.setRawValue(_missionItem._param7Fact.rawValue()); + _amslAltAboveTerrainFact.setRawValue(qQNaN()); + } _updateOptionalSections(); } return success; @@ -292,11 +328,34 @@ bool SimpleMissionItem::load(QTextStream &loadStream) bool SimpleMissionItem::load(const QJsonObject& json, int sequenceNumber, QString& errorString) { - bool success; - if ((success = _missionItem.load(json, sequenceNumber, errorString))) { - _updateOptionalSections(); + if (!_missionItem.load(json, sequenceNumber, errorString)) { + return false; } - return success; + + if (specifiesCoordinate()) { + if (json.contains(_jsonAltitudeModeKey) || json.contains(_jsonAltitudeKey) || json.contains(_jsonAMSLAltAboveTerrainKey)) { + QList keyInfoList = { + { _jsonAltitudeModeKey, QJsonValue::Double, true }, + { _jsonAltitudeKey, QJsonValue::Double, true }, + { _jsonAMSLAltAboveTerrainKey, QJsonValue::Double, true }, + }; + if (!JsonHelper::validateKeys(json, keyInfoList, errorString)) { + return false; + } + + _altitudeMode = (AltitudeMode)(int)json[_jsonAltitudeModeKey].toDouble(); + _altitudeFact.setRawValue(JsonHelper::possibleNaNJsonValue(json[_jsonAltitudeKey])); + _amslAltAboveTerrainFact.setRawValue(JsonHelper::possibleNaNJsonValue(json[_jsonAltitudeKey])); + } else { + _altitudeMode = _missionItem.relativeAltitude() ? AltitudeRelative : AltitudeAbsolute; + _altitudeFact.setRawValue(_missionItem._param7Fact.rawValue()); + _amslAltAboveTerrainFact.setRawValue(qQNaN()); + } + } + + _updateOptionalSections(); + + return true; } bool SimpleMissionItem::isStandaloneCoordinate(void) const @@ -357,15 +416,15 @@ QString SimpleMissionItem::abbreviation() const return tr("H"); switch(command()) { - case MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF: + case MAV_CMD_NAV_TAKEOFF: return tr("Takeoff"); - case MavlinkQmlSingleton::MAV_CMD_NAV_LAND: + case MAV_CMD_NAV_LAND: return tr("Land"); - case MavlinkQmlSingleton::MAV_CMD_NAV_VTOL_TAKEOFF: + case MAV_CMD_NAV_VTOL_TAKEOFF: return tr("VTOL Takeoff"); - case MavlinkQmlSingleton::MAV_CMD_NAV_VTOL_LAND: + case MAV_CMD_NAV_VTOL_LAND: return tr("VTOL Land"); - case MavlinkQmlSingleton::MAV_CMD_DO_SET_ROI: + case MAV_CMD_DO_SET_ROI: return tr("ROI"); default: return QString(); @@ -429,12 +488,6 @@ void SimpleMissionItem::_rebuildTextFieldFacts(void) } } - if (uiInfo->specifiesCoordinate() || uiInfo->specifiesAltitudeOnly()) { - _missionItem._param7Fact._setName("Altitude"); - _missionItem._param7Fact.setMetaData(_altitudeMetaData); - _textFieldFacts.append(&_missionItem._param7Fact); - } - _ignoreDirtyChangeSignals = false; } } @@ -489,15 +542,9 @@ void SimpleMissionItem::_rebuildNaNFacts(void) } } -void SimpleMissionItem::_rebuildCheckboxFacts(void) +bool SimpleMissionItem::specifiesAltitude(void) const { - _checkboxFacts.clear(); - - if (rawEdit()) { - _checkboxFacts.append(&_missionItem._autoContinueFact); - } else if ((specifiesCoordinate() || specifiesAltitudeOnly()) && !_homePositionSpecialCase) { - _checkboxFacts.append(&_altitudeRelativeToHomeFact); - } + return specifiesCoordinate() || specifiesAltitudeOnly(); } void SimpleMissionItem::_rebuildComboBoxFacts(void) @@ -541,7 +588,6 @@ void SimpleMissionItem::_rebuildFacts(void) { _rebuildTextFieldFacts(); _rebuildNaNFacts(); - _rebuildCheckboxFacts(); _rebuildComboBoxFacts(); } @@ -561,6 +607,8 @@ bool SimpleMissionItem::friendlyEditAllowed(void) const return true; break; + case MAV_FRAME_GLOBAL_TERRAIN_ALT: + return supportsTerrainFrame(); default: return false; } @@ -597,7 +645,7 @@ void SimpleMissionItem::setDirty(bool dirty) } } -void SimpleMissionItem::_setDirtyFromSignal(void) +void SimpleMissionItem::_setDirty(void) { if (!_ignoreDirtyChangeSignals) { setDirty(true); @@ -609,30 +657,84 @@ void SimpleMissionItem::_sendCoordinateChanged(void) emit coordinateChanged(coordinate()); } -void SimpleMissionItem::_syncAltitudeRelativeToHomeToFrame(const QVariant& value) +void SimpleMissionItem::_altitudeModeChanged(void) { - if (!_syncingAltitudeRelativeToHomeAndFrame) { - _syncingAltitudeRelativeToHomeAndFrame = true; - _missionItem.setFrame(value.toBool() ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL); - emit coordinateHasRelativeAltitudeChanged(value.toBool()); - _syncingAltitudeRelativeToHomeAndFrame = false; + switch (_altitudeMode) { + case AltitudeTerrainFrame: + _missionItem.setFrame(MAV_FRAME_GLOBAL_TERRAIN_ALT); + break; + case AltitudeAboveTerrain: + // Terrain altitudes are Absolute + _missionItem.setFrame(MAV_FRAME_GLOBAL); + // Clear any old calculated values + _missionItem._param7Fact.setRawValue(qQNaN()); + _amslAltAboveTerrainFact.setRawValue(qQNaN()); + break; + case AltitudeAbsolute: + _missionItem.setFrame(MAV_FRAME_GLOBAL); + break; + case AltitudeRelative: + _missionItem.setFrame(MAV_FRAME_GLOBAL_RELATIVE_ALT); + break; } + + // We always call _altitudeChanged to make sure that param7 is always setup correctly on mode change + _altitudeChanged(); + + emit coordinateHasRelativeAltitudeChanged(_altitudeMode == AltitudeRelative); } -void SimpleMissionItem::_syncFrameToAltitudeRelativeToHome(void) +void SimpleMissionItem::_altitudeChanged(void) { - if (!_syncingAltitudeRelativeToHomeAndFrame) { - _syncingAltitudeRelativeToHomeAndFrame = true; - _altitudeRelativeToHomeFact.setRawValue(relativeAltitude()); - emit coordinateHasRelativeAltitudeChanged(_altitudeRelativeToHomeFact.rawValue().toBool()); - _syncingAltitudeRelativeToHomeAndFrame = false; + if (!specifiesAltitude()) { + return; + } + + if (_altitudeMode == AltitudeAboveTerrain) { + _amslAltAboveTerrainFact.setRawValue(qQNaN()); + _terrainAltChanged(); + } else { + _missionItem._param7Fact.setRawValue(_altitudeFact.rawValue()); + } +} + +void SimpleMissionItem::_terrainAltChanged(void) +{ + if (!specifiesAltitude() || _altitudeMode != AltitudeAboveTerrain) { + // We don't need terrain data + return; + } + + if (qIsNaN(terrainAltitude())) { + qDebug() << "1"; + // Set NaNs to signal we are waiting on terrain data + _missionItem._param7Fact.setRawValue(qQNaN()); + _amslAltAboveTerrainFact.setRawValue(qQNaN()); + } else { + double newAboveTerrain = terrainAltitude() + _altitudeFact.rawValue().toDouble(); + double oldAboveTerrain = _amslAltAboveTerrainFact.rawValue().toDouble(); + qDebug() << "2" << newAboveTerrain << oldAboveTerrain; + if (qIsNaN(oldAboveTerrain) || !qFuzzyCompare(newAboveTerrain, oldAboveTerrain)) { + qDebug() << "3"; + _missionItem._param7Fact.setRawValue(newAboveTerrain); + _amslAltAboveTerrainFact.setRawValue(newAboveTerrain); + } } } +bool SimpleMissionItem::readyForSave(void) const +{ + return !specifiesAltitude() || !qIsNaN(_missionItem._param7Fact.rawValue().toDouble()); +} + void SimpleMissionItem::setDefaultsForCommand(void) { // We set these global defaults first, then if there are param defaults they will get reset - _missionItem.setParam7(qgcApp()->toolbox()->settingsManager()->appSettings()->defaultMissionItemAltitude()->rawValue().toDouble()); + _altitudeMode = AltitudeRelative; + double defaultAlt = qgcApp()->toolbox()->settingsManager()->appSettings()->defaultMissionItemAltitude()->rawValue().toDouble(); + _altitudeFact.setRawValue(defaultAlt); + _missionItem._param7Fact.setRawValue(defaultAlt); + _amslAltAboveTerrainFact.setRawValue(qQNaN()); MAV_CMD command = (MAV_CMD)this->command(); const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_vehicle, command); @@ -667,11 +769,6 @@ void SimpleMissionItem::setDefaultsForCommand(void) setRawEdit(false); } -void SimpleMissionItem::_sendFrameChanged(void) -{ - emit frameChanged(_missionItem.frame()); -} - void SimpleMissionItem::_sendCommandChanged(void) { emit commandChanged(command()); @@ -687,7 +784,7 @@ QString SimpleMissionItem::category(void) const return _commandTree->getUIInfo(_vehicle, (MAV_CMD)command())->category(); } -void SimpleMissionItem::setCommand(MavlinkQmlSingleton::Qml_MAV_CMD command) +void SimpleMissionItem::setCommand(int command) { if ((MAV_CMD)command != _missionItem.command()) { _missionItem.setCommand((MAV_CMD)command); @@ -824,7 +921,7 @@ void SimpleMissionItem::applyNewAltitude(double newAltitude) // Leave alone break; default: - _missionItem.setParam7(newAltitude); + _altitudeFact.setRawValue(newAltitude); break; } } @@ -846,3 +943,11 @@ void SimpleMissionItem::setMissionFlightStatus(MissionController::MissionFlightS } } } + +void SimpleMissionItem::setAltitudeMode(AltitudeMode altitudeMode) +{ + if (altitudeMode != _altitudeMode) { + _altitudeMode = altitudeMode; + emit altitudeModeChanged(); + } +} diff --git a/src/MissionManager/SimpleMissionItem.h b/src/MissionManager/SimpleMissionItem.h index c316804de0fa54cb67df9ba26f01bebc95ff434a..b172b14e5c0e2e8a8cd736b0f596238e2d9e2e22 100644 --- a/src/MissionManager/SimpleMissionItem.h +++ b/src/MissionManager/SimpleMissionItem.h @@ -23,26 +23,36 @@ class SimpleMissionItem : public VisualMissionItem Q_OBJECT public: - SimpleMissionItem(Vehicle* vehicle, QObject* parent = NULL); - SimpleMissionItem(Vehicle* vehicle, bool editMode, const MissionItem& missionItem, QObject* parent = NULL); - SimpleMissionItem(const SimpleMissionItem& other, QObject* parent = NULL); + SimpleMissionItem(Vehicle* vehicle, bool flyView, QObject* parent); + SimpleMissionItem(Vehicle* vehicle, bool flyView, const MissionItem& missionItem, QObject* parent); + SimpleMissionItem(const SimpleMissionItem& other, bool flyView, QObject* parent); ~SimpleMissionItem(); - const SimpleMissionItem& operator=(const SimpleMissionItem& other); + enum AltitudeMode { + AltitudeRelative, // MAV_FRAME_GLOBAL_RELATIVE_ALT + AltitudeAbsolute, // MAV_FRAME_GLOBAL + AltitudeAboveTerrain, // Absolute altitude above terrain calculated from terrain data + AltitudeTerrainFrame // MAV_FRAME_GLOBAL_TERRAIN_ALT + }; + + Q_ENUM(AltitudeMode) Q_PROPERTY(QString category READ category NOTIFY commandChanged) Q_PROPERTY(bool friendlyEditAllowed READ friendlyEditAllowed NOTIFY friendlyEditAllowedChanged) Q_PROPERTY(bool rawEdit READ rawEdit WRITE setRawEdit NOTIFY rawEditChanged) ///< true: raw item editing with all params - Q_PROPERTY(bool relativeAltitude READ relativeAltitude NOTIFY frameChanged) - Q_PROPERTY(MavlinkQmlSingleton::Qml_MAV_CMD command READ command WRITE setCommand NOTIFY commandChanged) + Q_PROPERTY(bool specifiesAltitude READ specifiesAltitude NOTIFY commandChanged) + Q_PROPERTY(Fact* altitude READ altitude CONSTANT) ///< Altitude as specified by altitudeMode. Not necessarily true mission item altitude + Q_PROPERTY(AltitudeMode altitudeMode READ altitudeMode WRITE setAltitudeMode NOTIFY altitudeModeChanged) + Q_PROPERTY(Fact* amslAltAboveTerrain READ amslAltAboveTerrain CONSTANT) ///< Actual AMSL altitude for item if altitudeMode == AltitudeAboveTerrain + Q_PROPERTY(int command READ command WRITE setCommand NOTIFY commandChanged) + Q_PROPERTY(bool supportsTerrainFrame READ supportsTerrainFrame NOTIFY supportsTerrainFrameChanged) /// Optional sections Q_PROPERTY(QObject* speedSection READ speedSection NOTIFY speedSectionChanged) Q_PROPERTY(QObject* cameraSection READ cameraSection NOTIFY cameraSectionChanged) // These properties are used to display the editing ui - Q_PROPERTY(QmlObjectListModel* checkboxFacts READ checkboxFacts CONSTANT) Q_PROPERTY(QmlObjectListModel* comboboxFacts READ comboboxFacts CONSTANT) Q_PROPERTY(QmlObjectListModel* textFieldFacts READ textFieldFacts CONSTANT) Q_PROPERTY(QmlObjectListModel* nanFacts READ nanFacts CONSTANT) @@ -57,22 +67,28 @@ public: // Property accesors QString category (void) const; - MavlinkQmlSingleton::Qml_MAV_CMD command(void) const { return (MavlinkQmlSingleton::Qml_MAV_CMD)_missionItem._commandFact.cookedValue().toInt(); } + int command(void) const { return _missionItem._commandFact.cookedValue().toInt(); } bool friendlyEditAllowed (void) const; bool rawEdit (void) const; + bool specifiesAltitude (void) const; + AltitudeMode altitudeMode (void) const { return _altitudeMode; } + Fact* altitude (void) { return &_altitudeFact; } + Fact* amslAltAboveTerrain (void) { return &_amslAltAboveTerrainFact; } + bool supportsTerrainFrame(void) const { return _vehicle->supportsTerrainFrame(); } + CameraSection* cameraSection (void) { return _cameraSection; } SpeedSection* speedSection (void) { return _speedSection; } QmlObjectListModel* textFieldFacts (void) { return &_textFieldFacts; } QmlObjectListModel* nanFacts (void) { return &_nanFacts; } - QmlObjectListModel* checkboxFacts (void) { return &_checkboxFacts; } QmlObjectListModel* comboboxFacts (void) { return &_comboboxFacts; } void setRawEdit(bool rawEdit); + void setAltitudeMode(AltitudeMode altitudeMode); void setCommandByIndex(int index); - void setCommand(MavlinkQmlSingleton::Qml_MAV_CMD command); + void setCommand(int command); void setAltDifference (double altDifference); void setAltPercent (double altPercent); @@ -82,8 +98,6 @@ public: bool load(QTextStream &loadStream); bool load(const QJsonObject& json, int sequenceNumber, QString& errorString); - bool relativeAltitude(void) { return _missionItem.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT; } - MissionItem& missionItem(void) { return _missionItem; } const MissionItem& missionItem(void) const { return _missionItem; } @@ -107,6 +121,7 @@ public: void appendMissionItems (QList& items, QObject* missionItemParent) final; void applyNewAltitude (double newAltitude) final; void setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final; + bool readyForSave (void) const final; bool coordinateHasRelativeAltitude (void) const final { return _missionItem.relativeAltitude(); } bool exitCoordinateHasRelativeAltitude (void) const final { return coordinateHasRelativeAltitude(); } @@ -123,51 +138,52 @@ public slots: signals: void commandChanged (int command); - void frameChanged (int frame); void friendlyEditAllowedChanged (bool friendlyEditAllowed); void headingDegreesChanged (double heading); void rawEditChanged (bool rawEdit); void cameraSectionChanged (QObject* cameraSection); void speedSectionChanged (QObject* cameraSection); + void altitudeModeChanged (void); + void supportsTerrainFrameChanged(void); private slots: - void _setDirtyFromSignal (void); - void _sectionDirtyChanged (bool dirty); - void _sendCommandChanged (void); - void _sendCoordinateChanged (void); - void _sendFrameChanged (void); - void _sendFriendlyEditAllowedChanged (void); - void _syncAltitudeRelativeToHomeToFrame (const QVariant& value); - void _syncFrameToAltitudeRelativeToHome (void); - void _updateLastSequenceNumber (void); - void _rebuildFacts (void); - + void _setDirty (void); + void _sectionDirtyChanged (bool dirty); + void _sendCommandChanged (void); + void _sendCoordinateChanged (void); + void _sendFriendlyEditAllowedChanged(void); + void _altitudeChanged (void); + void _altitudeModeChanged (void); + void _terrainAltChanged (void); + void _updateLastSequenceNumber (void); + void _rebuildFacts (void); + void _rebuildTextFieldFacts (void); private: void _connectSignals (void); void _setupMetaData (void); void _updateOptionalSections(void); - void _rebuildTextFieldFacts (void); void _rebuildNaNFacts (void); - void _rebuildCheckboxFacts (void); void _rebuildComboBoxFacts (void); - MissionItem _missionItem; - bool _rawEdit; - bool _dirty; - bool _ignoreDirtyChangeSignals; + MissionItem _missionItem; + bool _rawEdit; + bool _dirty; + bool _ignoreDirtyChangeSignals; SpeedSection* _speedSection; CameraSection* _cameraSection; MissionCommandTree* _commandTree; - Fact _altitudeRelativeToHomeFact; - Fact _supportedCommandFact; + Fact _supportedCommandFact; + + AltitudeMode _altitudeMode; + Fact _altitudeFact; + Fact _amslAltAboveTerrainFact; QmlObjectListModel _textFieldFacts; QmlObjectListModel _nanFacts; - QmlObjectListModel _checkboxFacts; QmlObjectListModel _comboboxFacts; static FactMetaData* _altitudeMetaData; @@ -185,8 +201,11 @@ private: FactMetaData _param6MetaData; FactMetaData _param7MetaData; - bool _syncingAltitudeRelativeToHomeAndFrame; ///< true: already in a sync signal, prevents signal loop - bool _syncingHeadingDegreesAndParam4; ///< true: already in a sync signal, prevents signal loop + bool _syncingHeadingDegreesAndParam4; ///< true: already in a sync signal, prevents signal loop + + static const char* _jsonAltitudeModeKey; + static const char* _jsonAltitudeKey; + static const char* _jsonAMSLAltAboveTerrainKey; }; #endif diff --git a/src/MissionManager/SimpleMissionItemTest.cc b/src/MissionManager/SimpleMissionItemTest.cc index 453987813328fd0057b5894ac65c316bef41ed88..3cef64fa51db0ee0df6f66ce02a60b5b73164282 100644 --- a/src/MissionManager/SimpleMissionItemTest.cc +++ b/src/MissionManager/SimpleMissionItemTest.cc @@ -15,43 +15,34 @@ const SimpleMissionItemTest::ItemInfo_t SimpleMissionItemTest::_rgItemInfo[] = { { MAV_CMD_NAV_WAYPOINT, MAV_FRAME_GLOBAL_RELATIVE_ALT }, - { MAV_CMD_NAV_LOITER_UNLIM, MAV_FRAME_GLOBAL_RELATIVE_ALT }, + { MAV_CMD_NAV_LOITER_UNLIM, MAV_FRAME_GLOBAL }, { MAV_CMD_NAV_LOITER_TURNS, MAV_FRAME_GLOBAL_RELATIVE_ALT }, - { MAV_CMD_NAV_LOITER_TIME, MAV_FRAME_GLOBAL_RELATIVE_ALT }, + { MAV_CMD_NAV_LOITER_TIME, MAV_FRAME_GLOBAL }, { MAV_CMD_NAV_LAND, MAV_FRAME_GLOBAL_RELATIVE_ALT }, - { MAV_CMD_NAV_TAKEOFF, MAV_FRAME_GLOBAL_RELATIVE_ALT }, + { MAV_CMD_NAV_TAKEOFF, MAV_FRAME_GLOBAL }, { MAV_CMD_DO_JUMP, MAV_FRAME_MISSION }, }; const SimpleMissionItemTest::FactValue_t SimpleMissionItemTest::_rgFactValuesWaypoint[] = { - { "Altitude", 70.1234567 }, { "Hold", 10.1234567 }, }; const SimpleMissionItemTest::FactValue_t SimpleMissionItemTest::_rgFactValuesLoiterUnlim[] = { - { "Altitude", 70.1234567 }, { "Radius", 30.1234567 }, }; const SimpleMissionItemTest::FactValue_t SimpleMissionItemTest::_rgFactValuesLoiterTurns[] = { - { "Altitude", 70.1234567 }, { "Radius", 30.1234567 }, { "Turns", 10.1234567 }, }; const SimpleMissionItemTest::FactValue_t SimpleMissionItemTest::_rgFactValuesLoiterTime[] = { - { "Altitude", 70.1234567 }, { "Radius", 30.1234567 }, { "Hold", 10.1234567 }, }; -const SimpleMissionItemTest::FactValue_t SimpleMissionItemTest::_rgFactValuesLand[] = { - { "Altitude", 70.1234567 }, -}; - const SimpleMissionItemTest::FactValue_t SimpleMissionItemTest::_rgFactValuesTakeoff[] = { { "Pitch", 10.1234567 }, - { "Altitude", 70.1234567 }, }; const SimpleMissionItemTest::FactValue_t SimpleMissionItemTest::_rgFactValuesDoJump[] = { @@ -60,13 +51,14 @@ const SimpleMissionItemTest::FactValue_t SimpleMissionItemTest::_rgFactValuesDoJ }; const SimpleMissionItemTest::ItemExpected_t SimpleMissionItemTest::_rgItemExpected[] = { - { sizeof(SimpleMissionItemTest::_rgFactValuesWaypoint)/sizeof(SimpleMissionItemTest::_rgFactValuesWaypoint[0]), SimpleMissionItemTest::_rgFactValuesWaypoint, true }, - { sizeof(SimpleMissionItemTest::_rgFactValuesLoiterUnlim)/sizeof(SimpleMissionItemTest::_rgFactValuesLoiterUnlim[0]), SimpleMissionItemTest::_rgFactValuesLoiterUnlim, true }, - { sizeof(SimpleMissionItemTest::_rgFactValuesLoiterTurns)/sizeof(SimpleMissionItemTest::_rgFactValuesLoiterTurns[0]), SimpleMissionItemTest::_rgFactValuesLoiterTurns, true }, - { sizeof(SimpleMissionItemTest::_rgFactValuesLoiterTime)/sizeof(SimpleMissionItemTest::_rgFactValuesLoiterTime[0]), SimpleMissionItemTest::_rgFactValuesLoiterTime, true }, - { sizeof(SimpleMissionItemTest::_rgFactValuesLand)/sizeof(SimpleMissionItemTest::_rgFactValuesLand[0]), SimpleMissionItemTest::_rgFactValuesLand, true }, - { sizeof(SimpleMissionItemTest::_rgFactValuesTakeoff)/sizeof(SimpleMissionItemTest::_rgFactValuesTakeoff[0]), SimpleMissionItemTest::_rgFactValuesTakeoff, true }, - { sizeof(SimpleMissionItemTest::_rgFactValuesDoJump)/sizeof(SimpleMissionItemTest::_rgFactValuesDoJump[0]), SimpleMissionItemTest::_rgFactValuesDoJump, false }, + // Text field facts count Fact Values Altitude Altitude Mode + { sizeof(SimpleMissionItemTest::_rgFactValuesWaypoint)/sizeof(SimpleMissionItemTest::_rgFactValuesWaypoint[0]), SimpleMissionItemTest::_rgFactValuesWaypoint, 70.1234567, SimpleMissionItem::AltitudeRelative }, + { sizeof(SimpleMissionItemTest::_rgFactValuesLoiterUnlim)/sizeof(SimpleMissionItemTest::_rgFactValuesLoiterUnlim[0]), SimpleMissionItemTest::_rgFactValuesLoiterUnlim, 70.1234567, SimpleMissionItem::AltitudeAbsolute }, + { sizeof(SimpleMissionItemTest::_rgFactValuesLoiterTurns)/sizeof(SimpleMissionItemTest::_rgFactValuesLoiterTurns[0]), SimpleMissionItemTest::_rgFactValuesLoiterTurns, 70.1234567, SimpleMissionItem::AltitudeRelative }, + { sizeof(SimpleMissionItemTest::_rgFactValuesLoiterTime)/sizeof(SimpleMissionItemTest::_rgFactValuesLoiterTime[0]), SimpleMissionItemTest::_rgFactValuesLoiterTime, 70.1234567, SimpleMissionItem::AltitudeAbsolute }, + { 0, NULL, 70.1234567, SimpleMissionItem::AltitudeRelative }, + { sizeof(SimpleMissionItemTest::_rgFactValuesTakeoff)/sizeof(SimpleMissionItemTest::_rgFactValuesTakeoff[0]), SimpleMissionItemTest::_rgFactValuesTakeoff, 70.1234567, SimpleMissionItem::AltitudeAbsolute }, + { sizeof(SimpleMissionItemTest::_rgFactValuesDoJump)/sizeof(SimpleMissionItemTest::_rgFactValuesDoJump[0]), SimpleMissionItemTest::_rgFactValuesDoJump, qQNaN(), SimpleMissionItem::AltitudeRelative }, }; SimpleMissionItemTest::SimpleMissionItemTest(void) @@ -80,12 +72,12 @@ void SimpleMissionItemTest::init(void) VisualMissionItemTest::init(); rgSimpleItemSignals[commandChangedIndex] = SIGNAL(commandChanged(int)); - rgSimpleItemSignals[frameChangedIndex] = SIGNAL(frameChanged(int)); + rgSimpleItemSignals[altitudeModeChangedIndex] = SIGNAL(altitudeModeChanged()); rgSimpleItemSignals[friendlyEditAllowedChangedIndex] = SIGNAL(friendlyEditAllowedChanged(bool)); rgSimpleItemSignals[headingDegreesChangedIndex] = SIGNAL(headingDegreesChanged(double)); rgSimpleItemSignals[rawEditChangedIndex] = SIGNAL(rawEditChanged(bool)); - rgSimpleItemSignals[cameraSectionChangedIndex] = SIGNAL(rawEditChanged(bool)); - rgSimpleItemSignals[speedSectionChangedIndex] = SIGNAL(rawEditChanged(bool)); + rgSimpleItemSignals[cameraSectionChangedIndex] = SIGNAL(cameraSectionChanged(QObject*)); + rgSimpleItemSignals[speedSectionChangedIndex] = SIGNAL(speedSectionChanged(QObject*)); rgSimpleItemSignals[coordinateHasRelativeAltitudeChangedIndex] = SIGNAL(coordinateHasRelativeAltitudeChanged(bool)); MissionItem missionItem(1, // sequence number @@ -100,7 +92,7 @@ void SimpleMissionItemTest::init(void) 70.1234567, true, // autoContinue false); // isCurrentItem - _simpleItem = new SimpleMissionItem(_offlineVehicle, true /* editMode */, missionItem); + _simpleItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, missionItem, this); // It's important top check that the right signals are emitted at the right time since that drives ui change. // It's also important to check that things are not being over-signalled when they should not be, since that can lead @@ -139,7 +131,7 @@ void SimpleMissionItemTest::_testEditorFacts(void) 70.1234567, true, // autoContinue false); // isCurrentItem - SimpleMissionItem simpleMissionItem(vehicle, true /* editMode */, missionItem); + SimpleMissionItem simpleMissionItem(vehicle, false /* flyView */, missionItem, NULL); // Validate that the fact values are correctly returned @@ -164,8 +156,10 @@ void SimpleMissionItemTest::_testEditorFacts(void) } QCOMPARE(factCount, expected->cFactValues); - int expectedCount = expected->relativeAltCheckbox ? 1 : 0; - QCOMPARE(simpleMissionItem.checkboxFacts()->count(), expectedCount); + if (!qIsNaN(expected->altValue)) { + QCOMPARE(simpleMissionItem.altitudeMode(), expected->altMode); + QCOMPARE(simpleMissionItem.altitude()->rawValue().toDouble(), expected->altValue); + } } delete vehicle; @@ -173,7 +167,7 @@ void SimpleMissionItemTest::_testEditorFacts(void) void SimpleMissionItemTest::_testDefaultValues(void) { - SimpleMissionItem item(_offlineVehicle); + SimpleMissionItem item(_offlineVehicle, false /* flyView */, NULL); item.missionItem().setCommand(MAV_CMD_NAV_WAYPOINT); item.missionItem().setFrame(MAV_FRAME_GLOBAL_RELATIVE_ALT); @@ -228,18 +222,8 @@ void SimpleMissionItemTest::_testSignals(void) QVERIFY(_spyVisualItem->checkOnlySignalByMask(dirtyChangedMask)); _spyVisualItem->clearAllSignals(); - // Check frameChanged signalling. Calling setFrame should signal: - // frameChanged - // dirtyChanged - // friendlyEditAllowedChanged - this signal is not very smart on when it gets sent - // coordinateHasRelativeAltitudeChanged - - missionItem.setFrame(MAV_FRAME_GLOBAL_RELATIVE_ALT); - QVERIFY(_spyVisualItem->checkNoSignals()); - QVERIFY(_spySimpleItem->checkNoSignals()); - - missionItem.setFrame(MAV_FRAME_GLOBAL); - QVERIFY(_spySimpleItem->checkOnlySignalByMask(frameChangedMask | dirtyChangedMask | friendlyEditAllowedChangedMask | coordinateHasRelativeAltitudeChangedMask)); + _simpleItem->setAltitudeMode(_simpleItem->altitudeMode() == SimpleMissionItem::AltitudeRelative ? SimpleMissionItem::AltitudeAbsolute : SimpleMissionItem::AltitudeRelative); + QVERIFY(_spySimpleItem->checkOnlySignalByMask(dirtyChangedMask | friendlyEditAllowedChangedMask | altitudeModeChangedMask | coordinateHasRelativeAltitudeChangedMask)); _spySimpleItem->clearAllSignals(); _spyVisualItem->clearAllSignals(); @@ -249,11 +233,11 @@ void SimpleMissionItemTest::_testSignals(void) // dirtyChanged // coordinateChanged - since altitude will be set back to default - _simpleItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT); + _simpleItem->setCommand(MAV_CMD_NAV_WAYPOINT); QVERIFY(_spyVisualItem->checkNoSignals()); QVERIFY(_spySimpleItem->checkNoSignals()); - _simpleItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_LOITER_TIME); + _simpleItem->setCommand(MAV_CMD_NAV_LOITER_TIME); QVERIFY(_spySimpleItem->checkSignalsByMask(commandChangedMask)); QVERIFY(_spyVisualItem->checkSignalsByMask(commandNameChangedMask | dirtyChangedMask | coordinateChangedMask)); } @@ -320,3 +304,18 @@ void SimpleMissionItemTest::_testSpeedSection(void) QCOMPARE(_spyVisualItem->checkSignalsByMask(specifiedFlightSpeedChangedMask), true); QCOMPARE(_simpleItem->dirty(), true); } + +void SimpleMissionItemTest::_testAltitudePropogation(void) +{ + // Make sure that changes to altitude propogate to param 7 of the mission item + + _simpleItem->setAltitudeMode(SimpleMissionItem::AltitudeRelative); + _simpleItem->altitude()->setRawValue(_simpleItem->altitude()->rawValue().toDouble() + 1); + QCOMPARE(_simpleItem->altitude()->rawValue().toDouble(), _simpleItem->missionItem().param7()); + QCOMPARE(_simpleItem->missionItem().frame(), MAV_FRAME_GLOBAL_RELATIVE_ALT); + + _simpleItem->setAltitudeMode(SimpleMissionItem::AltitudeAbsolute); + _simpleItem->altitude()->setRawValue(_simpleItem->altitude()->rawValue().toDouble() + 1); + QCOMPARE(_simpleItem->altitude()->rawValue().toDouble(), _simpleItem->missionItem().param7()); + QCOMPARE(_simpleItem->missionItem().frame(), MAV_FRAME_GLOBAL); +} diff --git a/src/MissionManager/SimpleMissionItemTest.h b/src/MissionManager/SimpleMissionItemTest.h index f96db89a335412993e38e084633888746c376192..db67cb4f66ade37a685febc08e8e5502649910d2 100644 --- a/src/MissionManager/SimpleMissionItemTest.h +++ b/src/MissionManager/SimpleMissionItemTest.h @@ -31,11 +31,12 @@ private slots: void _testSpeedSectionDirty(void); void _testCameraSection(void); void _testSpeedSection(void); + void _testAltitudePropogation(void); private: enum { commandChangedIndex = 0, - frameChangedIndex, + altitudeModeChangedIndex, friendlyEditAllowedChangedIndex, headingDegreesChangedIndex, rawEditChangedIndex, @@ -47,7 +48,7 @@ private: enum { commandChangedMask = 1 << commandChangedIndex, - frameChangedMask = 1 << frameChangedIndex, + altitudeModeChangedMask = 1 << altitudeModeChangedIndex, friendlyEditAllowedChangedMask = 1 << friendlyEditAllowedChangedIndex, headingDegreesChangedMask = 1 << headingDegreesChangedIndex, rawEditChangedMask = 1 << rawEditChangedIndex, @@ -70,9 +71,10 @@ private: } FactValue_t; typedef struct { - size_t cFactValues; - const FactValue_t* rgFactValues; - bool relativeAltCheckbox; + size_t cFactValues; + const FactValue_t* rgFactValues; + double altValue; + SimpleMissionItem::AltitudeMode altMode; } ItemExpected_t; SimpleMissionItem* _simpleItem; diff --git a/src/MissionManager/SpeedSectionTest.cc b/src/MissionManager/SpeedSectionTest.cc index b62db1c56fb6f7ddb48fc9b4b63550fffa1daff8..a52442c2761a6f2e069d3987d4439af2f34563f7 100644 --- a/src/MissionManager/SpeedSectionTest.cc +++ b/src/MissionManager/SpeedSectionTest.cc @@ -134,7 +134,7 @@ void SpeedSectionTest::_checkAvailable(void) 70.1234567, true, // autoContinue false); // isCurrentItem - SimpleMissionItem* item = new SimpleMissionItem(_offlineVehicle, true /* editMode */, missionItem); + SimpleMissionItem* item = new SimpleMissionItem(_offlineVehicle, false /* flyView */, missionItem, this); QVERIFY(item->speedSection()); QCOMPARE(item->speedSection()->available(), false); } @@ -193,7 +193,7 @@ void SpeedSectionTest::_testScanForSection(void) double flightSpeed = 10.123456; MissionItem validSpeedItem(0, MAV_CMD_DO_CHANGE_SPEED, MAV_FRAME_MISSION, _offlineVehicle->multiRotor() ? 1 : 0, flightSpeed, -1, 0, 0, 0, 0, true, false); - SimpleMissionItem simpleItem(_offlineVehicle, true /* editMode */, validSpeedItem); + SimpleMissionItem simpleItem(_offlineVehicle, false /* flyView */, validSpeedItem, NULL); MissionItem& simpleMissionItem = simpleItem.missionItem(); visualItems.append(&simpleItem); scanIndex = 0; @@ -263,9 +263,8 @@ void SpeedSectionTest::_testScanForSection(void) scanIndex = 0; // Valid item in wrong position - QmlObjectListModel waypointVisualItems; MissionItem waypointMissionItem(0, MAV_CMD_NAV_WAYPOINT, MAV_FRAME_GLOBAL_RELATIVE_ALT, 0, 0, 0, 0, 0, 0, 0, true, false); - SimpleMissionItem simpleWaypointItem(_offlineVehicle, true /* editMode */, waypointMissionItem); + SimpleMissionItem simpleWaypointItem(_offlineVehicle, false /* flyView */, waypointMissionItem, NULL); simpleMissionItem = validSpeedItem; visualItems.append(&simpleWaypointItem); visualItems.append(&simpleMissionItem); diff --git a/src/MissionManager/StructureScan.SettingsGroup.json b/src/MissionManager/StructureScan.SettingsGroup.json index 9c3beebabe537b31799f6365c8cee58cd2697ffc..69016744263a4d634d65245de7f5256eba754600 100644 --- a/src/MissionManager/StructureScan.SettingsGroup.json +++ b/src/MissionManager/StructureScan.SettingsGroup.json @@ -34,6 +34,14 @@ "min": 1, "defaultValue": 1 }, +{ + "name": "StructureHeight", + "shortDescription": "Height of structure being scanned.", + "type": "double", + "units": "m", + "min": 1, + "defaultValue": 100 +}, { "name": "Layer distance", "shortDescription": "Distance between each layer.", diff --git a/src/MissionManager/StructureScanComplexItem.cc b/src/MissionManager/StructureScanComplexItem.cc index 8c7aeeaf8b3d3aea23e6d6c0e7253c5b652d81c4..c9a2fc1a34dff31539659ebbb30506797021fefb 100644 --- a/src/MissionManager/StructureScanComplexItem.cc +++ b/src/MissionManager/StructureScanComplexItem.cc @@ -21,20 +21,18 @@ QGC_LOGGING_CATEGORY(StructureScanComplexItemLog, "StructureScanComplexItemLog") -const char* StructureScanComplexItem::_altitudeFactName = "Altitude"; -const char* StructureScanComplexItem::_structureHeightFactName = "StructureHeight"; -const char* StructureScanComplexItem::_layersFactName = "Layers"; -const char* StructureScanComplexItem::_gimbalPitchFactName = "GimbalPitch"; -const char* StructureScanComplexItem::_gimbalYawFactName = "GimbalYaw"; - -const char* StructureScanComplexItem::jsonComplexItemTypeValue = "StructureScan"; -const char* StructureScanComplexItem::_jsonCameraCalcKey = "CameraCalc"; -const char* StructureScanComplexItem::_jsonAltitudeRelativeKey = "altitudeRelative"; - -QMap StructureScanComplexItem::_metaDataMap; - -StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, QObject* parent) - : ComplexMissionItem (vehicle, parent) +const char* StructureScanComplexItem::settingsGroup = "StructureScan"; +const char* StructureScanComplexItem::altitudeName = "Altitude"; +const char* StructureScanComplexItem::structureHeightName = "StructureHeight"; +const char* StructureScanComplexItem::layersName = "Layers"; + +const char* StructureScanComplexItem::jsonComplexItemTypeValue = "StructureScan"; +const char* StructureScanComplexItem::_jsonCameraCalcKey = "CameraCalc"; +const char* StructureScanComplexItem::_jsonAltitudeRelativeKey = "altitudeRelative"; + +StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlFile, QObject* parent) + : ComplexMissionItem (vehicle, flyView, parent) + , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/StructureScan.SettingsGroup.json"), this /* QObject parent */)) , _sequenceNumber (0) , _dirty (false) , _altitudeRelative (true) @@ -42,35 +40,17 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, QObject* pa , _ignoreRecalc (false) , _scanDistance (0.0) , _cameraShots (0) - , _cameraMinTriggerInterval (0) - , _cameraCalc (vehicle) - , _altitudeFact (0, _altitudeFactName, FactMetaData::valueTypeDouble) - , _layersFact (0, _layersFactName, FactMetaData::valueTypeUint32) - , _gimbalPitchFact (0, _gimbalPitchFactName, FactMetaData::valueTypeDouble) - , _gimbalYawFact (0, _gimbalYawFactName, FactMetaData::valueTypeDouble) + , _cameraCalc (vehicle, settingsGroup) + , _altitudeFact (settingsGroup, _metaDataMap[altitudeName]) + , _structureHeightFact (settingsGroup, _metaDataMap[structureHeightName]) + , _layersFact (settingsGroup, _metaDataMap[layersName]) { _editorQml = "qrc:/qml/StructureScanEditor.qml"; - if (_metaDataMap.isEmpty()) { - _metaDataMap = FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/StructureScan.SettingsGroup.json"), NULL /* QObject parent */); - } - - _altitudeFact.setMetaData (_metaDataMap[_altitudeFactName]); - _layersFact.setMetaData (_metaDataMap[_layersFactName]); - _gimbalPitchFact.setMetaData(_metaDataMap[_gimbalPitchFactName]); - _gimbalYawFact.setMetaData (_metaDataMap[_gimbalYawFactName]); - - _altitudeFact.setRawValue (_altitudeFact.rawDefaultValue()); - _layersFact.setRawValue (_layersFact.rawDefaultValue()); - _gimbalPitchFact.setRawValue(_gimbalPitchFact.rawDefaultValue()); - _gimbalYawFact.setRawValue (_gimbalYawFact.rawDefaultValue()); - _altitudeFact.setRawValue(qgcApp()->toolbox()->settingsManager()->appSettings()->defaultMissionItemAltitude()->rawValue()); connect(&_altitudeFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); connect(&_layersFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); - connect(&_gimbalPitchFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); - connect(&_gimbalYawFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); connect(&_layersFact, &Fact::valueChanged, this, &StructureScanComplexItem::_recalcLayerInfo); connect(&_structureHeightFact, &Fact::valueChanged, this, &StructureScanComplexItem::_recalcLayerInfo); @@ -89,13 +69,19 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, QObject* pa connect(&_flightPolygon, &QGCMapPolygon::pathChanged, this, &StructureScanComplexItem::_flightPathChanged); connect(_cameraCalc.distanceToSurface(), &Fact::valueChanged, this, &StructureScanComplexItem::_rebuildFlightPolygon); - connect(&_cameraCalc, &CameraCalc::cameraNameChanged, this, &StructureScanComplexItem::_resetGimbal); connect(&_flightPolygon, &QGCMapPolygon::pathChanged, this, &StructureScanComplexItem::_recalcCameraShots); connect(_cameraCalc.adjustedFootprintSide(), &Fact::valueChanged, this, &StructureScanComplexItem::_recalcCameraShots); connect(&_layersFact, &Fact::valueChanged, this, &StructureScanComplexItem::_recalcCameraShots); _recalcLayerInfo(); + + if (!kmlFile.isEmpty()) { + _structurePolygon.loadKMLFile(kmlFile); + _structurePolygon.setDirty(false); + } + + setDirty(false); } void StructureScanComplexItem::_setScanDistance(double scanDistance) @@ -132,9 +118,9 @@ int StructureScanComplexItem::lastSequenceNumber(void) const { return _sequenceNumber + (_layersFact.rawValue().toInt() * - ((_flightPolygon.count() + 1) + // 1 waypoint for each polygon vertex + 1 to go back to first polygon vertex for each layer - 2)) + // Camera trigger start/stop for each layer - 1; // Gimbal control command + ((_flightPolygon.count() + 1) + // 1 waypoint for each polygon vertex + 1 to go back to first polygon vertex for each layer + 2)) + // Camera trigger start/stop for each layer + 2; // ROI_WPNEXT_OFFSET and ROI_NONE commands } void StructureScanComplexItem::setDirty(bool dirty) @@ -154,12 +140,10 @@ void StructureScanComplexItem::save(QJsonArray& missionItems) saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; - saveObject[_gimbalPitchFactName] = _gimbalPitchFact.rawValue().toDouble(); - saveObject[_gimbalYawFactName] = _gimbalYawFact.rawValue().toDouble(); - saveObject[_altitudeFactName] = _altitudeFact.rawValue().toDouble(); - saveObject[_structureHeightFactName] = _structureHeightFact.rawValue().toDouble(); - saveObject[_jsonAltitudeRelativeKey] = _altitudeRelative; - saveObject[_layersFactName] = _layersFact.rawValue().toDouble(); + saveObject[altitudeName] = _altitudeFact.rawValue().toDouble(); + saveObject[structureHeightName] = _structureHeightFact.rawValue().toDouble(); + saveObject[_jsonAltitudeRelativeKey] = _altitudeRelative; + saveObject[layersName] = _layersFact.rawValue().toDouble(); QJsonObject cameraCalcObject; _cameraCalc.save(cameraCalcObject); @@ -186,12 +170,10 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen { VisualMissionItem::jsonTypeKey, QJsonValue::String, true }, { ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true }, { QGCMapPolygon::jsonPolygonKey, QJsonValue::Array, true }, - { _gimbalPitchFactName, QJsonValue::Double, true }, - { _gimbalYawFactName, QJsonValue::Double, true }, - { _altitudeFactName, QJsonValue::Double, true }, - { _structureHeightFactName, QJsonValue::Double, true }, + { altitudeName, QJsonValue::Double, true }, + { structureHeightName, QJsonValue::Double, true }, { _jsonAltitudeRelativeKey, QJsonValue::Bool, true }, - { _layersFactName, QJsonValue::Double, true }, + { layersName, QJsonValue::Double, true }, { _jsonCameraCalcKey, QJsonValue::Object, true }, }; if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) { @@ -220,10 +202,8 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen return false; } - _gimbalPitchFact.setRawValue(complexObject[_gimbalPitchFactName].toDouble()); - _gimbalYawFact.setRawValue (complexObject[_gimbalYawFactName].toDouble()); - _altitudeFact.setRawValue (complexObject[_altitudeFactName].toDouble()); - _layersFact.setRawValue (complexObject[_layersFactName].toDouble()); + _altitudeFact.setRawValue (complexObject[altitudeName].toDouble()); + _layersFact.setRawValue (complexObject[layersName].toDouble()); _altitudeRelative = complexObject[_jsonAltitudeRelativeKey].toBool(true); if (!_structurePolygon.loadFromJson(complexObject, true /* required */, errorString)) { @@ -268,13 +248,11 @@ void StructureScanComplexItem::appendMissionItems(QList& items, QO double baseAltitude = _altitudeFact.rawValue().toDouble(); MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_DO_MOUNT_CONTROL, + MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET, MAV_FRAME_MISSION, - _gimbalPitchFact.rawValue().toDouble(), - 0, // Gimbal roll - _gimbalYawFact.rawValue().toDouble(), - 0, 0, 0, // param 4-6 not used - MAV_MOUNT_MODE_MAVLINK_TARGETING, + 0, 0, 0, 0, // param 1-4 not used + 0, 0, // Pitch and Roll stay in standard orientation + 90, // 90 degreee yaw offset to point to structure true, // autoContinue false, // isCurrentItem missionItemParent); @@ -349,6 +327,15 @@ void StructureScanComplexItem::appendMissionItems(QList& items, QO missionItemParent); items.append(item); } + + item = new MissionItem(seqNum++, + MAV_CMD_DO_SET_ROI_NONE, + MAV_FRAME_MISSION, + 0, 0, 0,0, 0, 0, 0, // param 1-7 not used + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); } int StructureScanComplexItem::cameraShots(void) const @@ -447,12 +434,6 @@ void StructureScanComplexItem::_recalcCameraShots(void) _setCameraShots(cameraShots * _layersFact.rawValue().toInt()); } -void StructureScanComplexItem::_resetGimbal(void) -{ - _gimbalPitchFact.setCookedValue(0); - _gimbalYawFact.setCookedValue(90); -} - void StructureScanComplexItem::setAltitudeRelative(bool altitudeRelative) { if (altitudeRelative != _altitudeRelative) { diff --git a/src/MissionManager/StructureScanComplexItem.h b/src/MissionManager/StructureScanComplexItem.h index f7faadcac21ca90d97f064cd205972f1e7d6c429..904f57e4c3c1bc9cacc6f3035705863d949a89c4 100644 --- a/src/MissionManager/StructureScanComplexItem.h +++ b/src/MissionManager/StructureScanComplexItem.h @@ -25,18 +25,18 @@ class StructureScanComplexItem : public ComplexMissionItem Q_OBJECT public: - StructureScanComplexItem(Vehicle* vehicle, QObject* parent = NULL); + /// @param vehicle Vehicle which this is being contructed for + /// @param flyView true: Created for use in the Fly View, false: Created for use in the Plan View + /// @param kmlFile Polygon comes from this file, empty for default polygon + StructureScanComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlFile, QObject* parent); Q_PROPERTY(CameraCalc* cameraCalc READ cameraCalc CONSTANT) - Q_PROPERTY(Fact* gimbalPitch READ gimbalPitch CONSTANT) - Q_PROPERTY(Fact* gimbalYaw READ gimbalYaw CONSTANT) Q_PROPERTY(Fact* altitude READ altitude CONSTANT) Q_PROPERTY(Fact* structureHeight READ structureHeight CONSTANT) Q_PROPERTY(Fact* layers READ layers CONSTANT) Q_PROPERTY(bool altitudeRelative READ altitudeRelative WRITE setAltitudeRelative NOTIFY altitudeRelativeChanged) Q_PROPERTY(int cameraShots READ cameraShots NOTIFY cameraShotsChanged) Q_PROPERTY(double timeBetweenShots READ timeBetweenShots NOTIFY timeBetweenShotsChanged) - Q_PROPERTY(double cameraMinTriggerInterval MEMBER _cameraMinTriggerInterval NOTIFY cameraMinTriggerIntervalChanged) Q_PROPERTY(QGCMapPolygon* structurePolygon READ structurePolygon CONSTANT) Q_PROPERTY(QGCMapPolygon* flightPolygon READ flightPolygon CONSTANT) @@ -47,8 +47,6 @@ public: bool altitudeRelative (void) const { return _altitudeRelative; } int cameraShots (void) const; - Fact* gimbalPitch (void) { return &_gimbalPitchFact; } - Fact* gimbalYaw (void) { return &_gimbalYawFact; } double timeBetweenShots (void); QGCMapPolygon* structurePolygon (void) { return &_structurePolygon; } QGCMapPolygon* flightPolygon (void) { return &_flightPolygon; } @@ -96,10 +94,14 @@ public: static const char* jsonComplexItemTypeValue; + static const char* settingsGroup; + static const char* altitudeName; + static const char* structureHeightName; + static const char* layersName; + signals: void cameraShotsChanged (int cameraShots); void timeBetweenShotsChanged (void); - void cameraMinTriggerIntervalChanged(double cameraMinTriggerInterval); void altitudeRelativeChanged (bool altitudeRelative); private slots: @@ -111,7 +113,6 @@ private slots: void _updateCoordinateAltitudes (void); void _rebuildFlightPolygon (void); void _recalcCameraShots (void); - void _resetGimbal (void); void _recalcLayerInfo (void); private: @@ -120,6 +121,8 @@ private: void _setCameraShots(int cameraShots); double _triggerDistance(void) const; + QMap _metaDataMap; + int _sequenceNumber; bool _dirty; QGCMapPolygon _structurePolygon; @@ -131,23 +134,13 @@ private: double _scanDistance; int _cameraShots; double _timeBetweenShots; - double _cameraMinTriggerInterval; double _cruiseSpeed; CameraCalc _cameraCalc; - static QMap _metaDataMap; - - Fact _altitudeFact; - Fact _structureHeightFact; - Fact _layersFact; - Fact _gimbalPitchFact; - Fact _gimbalYawFact; - static const char* _altitudeFactName; - static const char* _structureHeightFactName; - static const char* _layersFactName; - static const char* _gimbalPitchFactName; - static const char* _gimbalYawFactName; + SettingsFact _altitudeFact; + SettingsFact _structureHeightFact; + SettingsFact _layersFact; static const char* _jsonCameraCalcKey; static const char* _jsonAltitudeRelativeKey; diff --git a/src/MissionManager/StructureScanComplexItemTest.cc b/src/MissionManager/StructureScanComplexItemTest.cc index 9058f1ae93f1ae30dce5a6bd4c0c892ad3fa147c..13b0173ac1b30057f457bdbbaa88bd036c8c3d7b 100644 --- a/src/MissionManager/StructureScanComplexItemTest.cc +++ b/src/MissionManager/StructureScanComplexItemTest.cc @@ -24,7 +24,7 @@ void StructureScanComplexItemTest::init(void) _rgSignals[dirtyChangedIndex] = SIGNAL(dirtyChanged(bool)); _offlineVehicle = new Vehicle(MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, qgcApp()->toolbox()->firmwarePluginManager(), this); - _structureScanItem = new StructureScanComplexItem(_offlineVehicle, this); + _structureScanItem = new StructureScanComplexItem(_offlineVehicle, false /* flyView */, QString() /* kmlFile */, this /* parent */); _structureScanItem->setDirty(false); _multiSpy = new MultiSignalSpy(); @@ -59,7 +59,7 @@ void StructureScanComplexItemTest::_testDirty(void) // These facts should set dirty when changed QList rgFacts; - rgFacts << _structureScanItem->gimbalPitch() << _structureScanItem->gimbalYaw() << _structureScanItem->altitude() << _structureScanItem->layers(); + rgFacts << _structureScanItem->altitude() << _structureScanItem->layers(); foreach(Fact* fact, rgFacts) { qDebug() << fact->name(); QVERIFY(!_structureScanItem->dirty()); @@ -92,9 +92,7 @@ void StructureScanComplexItemTest::_initItem(void) mapPolygon->appendVertex(vertex); } - _structureScanItem->cameraCalc()->setCameraName(CameraCalc::manualCameraName()); - _structureScanItem->gimbalPitch()->setCookedValue(45); - _structureScanItem->gimbalYaw()->setCookedValue(45); + _structureScanItem->cameraCalc()->cameraName()->setRawValue(CameraCalc::manualCameraName()); _structureScanItem->layers()->setCookedValue(2); _structureScanItem->setDirty(false); @@ -111,9 +109,7 @@ void StructureScanComplexItemTest::_validateItem(StructureScanComplexItem* item) QCOMPARE(expectedVertex, actualVertex); } - QCOMPARE(_structureScanItem->cameraCalc()->cameraName() , CameraCalc::manualCameraName()); - QCOMPARE(item->gimbalPitch()->cookedValue().toDouble(), 45.0); - QCOMPARE(item->gimbalYaw()->cookedValue().toDouble(), 45.0); + QVERIFY(_structureScanItem->cameraCalc()->isManualCamera()); QCOMPARE(item->layers()->cookedValue().toInt(), 2); } @@ -125,24 +121,13 @@ void StructureScanComplexItemTest::_testSaveLoad(void) _structureScanItem->save(items); QString errorString; - StructureScanComplexItem* newItem = new StructureScanComplexItem(_offlineVehicle, this); + StructureScanComplexItem* newItem = new StructureScanComplexItem(_offlineVehicle, false /* flyView */, QString() /* kmlFile */, this /* parent */); QVERIFY(newItem->load(items[0].toObject(), 10, errorString)); QVERIFY(errorString.isEmpty()); _validateItem(newItem); newItem->deleteLater(); } -void StructureScanComplexItemTest::_testGimbalAngleUpdate(void) -{ - // This sets the item to CameraCalc::CameraSpecNone and non-standard gimbal angles - _initItem(); - - // Switching to a camera specific setup should set gimbal angles to defaults - _structureScanItem->cameraCalc()->setCameraName(CameraCalc::customCameraName()); - QCOMPARE(_structureScanItem->gimbalPitch()->cookedValue().toDouble(), 0.0); - QCOMPARE(_structureScanItem->gimbalYaw()->cookedValue().toDouble(), 90.0); -} - void StructureScanComplexItemTest::_testItemCount(void) { QList items; diff --git a/src/MissionManager/StructureScanComplexItemTest.h b/src/MissionManager/StructureScanComplexItemTest.h index c6c42432b90e050aabb27425af7b5d2b5bf91453..06f5e1b1a3ac2de335b7c9cee033547877749266 100644 --- a/src/MissionManager/StructureScanComplexItemTest.h +++ b/src/MissionManager/StructureScanComplexItemTest.h @@ -27,7 +27,6 @@ protected: private slots: void _testDirty(void); void _testSaveLoad(void); - void _testGimbalAngleUpdate(void); void _testItemCount(void); private: diff --git a/src/MissionManager/Survey.SettingsGroup.json b/src/MissionManager/Survey.SettingsGroup.json index 8287aa689736c08bc80d1ffabc595fabca0f9f24..2690fdd856512aee39670eb84582b81150dbea96 100644 --- a/src/MissionManager/Survey.SettingsGroup.json +++ b/src/MissionManager/Survey.SettingsGroup.json @@ -1,24 +1,4 @@ [ -{ - "name": "ManualGrid", - "shortDescription": "Specify all parameters for grid generation.", - "type": "bool", - "defaultValue": 1 -}, -{ - "name": "GridAltitude", - "shortDescription": "Altitude for all waypoints within the grid.", - "type": "double", - "units": "m", - "decimalPlaces": 1, - "defaultValue": 50 -}, -{ - "name": "GridAltitudeRelative", - "shortDescription": "Altitude for all waypoints within the grid is relative to home.", - "type": "bool", - "defaultValue": 1 -}, { "name": "GridAngle", "shortDescription": "Angle for parallel lines of grid.", @@ -30,140 +10,9 @@ "defaultValue": 0 }, { - "name": "GridSpacing", - "shortDescription": "Amount of spacing in between parallel grid lines.", - "type": "double", - "decimalPlaces": 2, - "min": 0.1, - "units": "m", - "defaultValue": 30 -}, -{ - "name": "TurnaroundDist", - "shortDescription": "Amount of additional distance to add outside the grid area for vehicle turnaround.", - "type": "double", - "decimalPlaces": 2, - "min": 0, - "units": "m", - "defaultValue": 30 -}, -{ - "name": "GroundResolution", - "shortDescription": "Resolution of image in relationship to ground distance.", - "type": "double", - "decimalPlaces": 2, - "min": 0, - "units": "cm/px", - "defaultValue": 3 -}, -{ - "name": "FrontalOverlap", - "shortDescription": "Amount of overlap between images in the forward facing direction.", - "type": "double", - "decimalPlaces": 0, - "min": 0, - "max": 85, - "units": "%", - "defaultValue": 70 -}, -{ - "name": "SideOverlap", - "shortDescription": "Amount of overlap between images in the side facing direction.", - "type": "double", - "decimalPlaces": 0, - "min": 0, - "max": 85, - "units": "%", - "defaultValue": 70 -}, -{ - "name": "CameraSensorWidth", - "shortDescription": "Amount of overlap between images in the side facing direction.", - "type": "double", - "decimalPlaces": 2, - "min": 1, - "units": "mm", - "defaultValue": 6.17 -}, -{ - "name": "CameraSensorHeight", - "shortDescription": "Amount of overlap between images in the side facing direction.", - "type": "double", - "decimalPlaces": 2, - "min": 1, - "units": "mm", - "defaultValue": 4.55 -}, -{ - "name": "CameraResolutionWidth", - "shortDescription": "Camera resolution width.", - "type": "uint32", - "min": 1, - "units": "px", - "defaultValue": 4000 -}, -{ - "name": "CameraResolutionHeight", - "shortDescription": "Camera resolution height.", - "type": "uint32", - "min": 1, - "units": "px", - "defaultValue": 3000 -}, -{ - "name": "CameraFocalLength", - "shortDescription": "Focal length of camera lens.", - "type": "double", - "decimalPlaces": 1, - "min": 1, - "units": "mm", - "defaultValue": 4.5 -}, -{ - "name": "CameraTriggerDistance", - "shortDescription": "Distance between each triggering of the camera. 0 specifies not camera trigger.", - "type": "double", - "decimalPlaces": 2, - "min": 0, - "units": "m", - "defaultValue": 25 -}, -{ - "name": "CameraTriggerInTurnaround", - "shortDescription": "Camera continues taking images in turnarounds.", - "type": "bool", - "defaultValue": true -}, -{ - "name": "HoverAndCapture", - "shortDescription": "Hover at each image point and take image", - "type": "bool", - "defaultValue": false -}, -{ - "name": "CameraOrientationLandscape", - "shortDescription": "Camera on vehicle is in landscape orientation.", - "type": "bool", - "defaultValue": 1 -}, -{ - "name": "FixedValueIsAltitude", - "shortDescription": "The altitude is kep constant while ground resolution changes.", + "name": "FlyAlternateTransects", + "shortDescription": "Fly every other transect in each pass.", "type": "bool", "defaultValue": false -}, -{ - "name": "Camera", - "shortDescription": "Camera selected for Survey.", - "type": "string", - "defaultValue": "" -}, -{ - "name": "GridEntryLocation", - "shortDescription": "Location for entry point into survey area", - "type": "uint32", - "enumStrings": "Position 1,Position 2,Position 3,Position 4", - "enumValues": "0,1,2,3", - "defaultValue": 0 } ] diff --git a/src/MissionManager/SurveyComplexItem.cc b/src/MissionManager/SurveyComplexItem.cc new file mode 100644 index 0000000000000000000000000000000000000000..ad96827c787b1b211739870ffe6d3585937c3d43 --- /dev/null +++ b/src/MissionManager/SurveyComplexItem.cc @@ -0,0 +1,1447 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + + +#include "SurveyComplexItem.h" +#include "JsonHelper.h" +#include "MissionController.h" +#include "QGCGeo.h" +#include "QGroundControlQmlGlobal.h" +#include "QGCQGeoCoordinate.h" +#include "SettingsManager.h" +#include "AppSettings.h" + +#include + +QGC_LOGGING_CATEGORY(SurveyComplexItemLog, "SurveyComplexItemLog") + +const char* SurveyComplexItem::jsonComplexItemTypeValue = "survey"; +const char* SurveyComplexItem::jsonV3ComplexItemTypeValue = "survey"; + +const char* SurveyComplexItem::settingsGroup = "Survey"; +const char* SurveyComplexItem::gridAngleName = "GridAngle"; +const char* SurveyComplexItem::gridEntryLocationName = "GridEntryLocation"; +const char* SurveyComplexItem::flyAlternateTransectsName = "FlyAlternateTransects"; + +const char* SurveyComplexItem::_jsonGridAngleKey = "angle"; +const char* SurveyComplexItem::_jsonEntryPointKey = "entryLocation"; + +const char* SurveyComplexItem::_jsonV3GridObjectKey = "grid"; +const char* SurveyComplexItem::_jsonV3GridAltitudeKey = "altitude"; +const char* SurveyComplexItem::_jsonV3GridAltitudeRelativeKey = "relativeAltitude"; +const char* SurveyComplexItem::_jsonV3GridAngleKey = "angle"; +const char* SurveyComplexItem::_jsonV3GridSpacingKey = "spacing"; +const char* SurveyComplexItem::_jsonV3EntryPointKey = "entryLocation"; +const char* SurveyComplexItem::_jsonV3TurnaroundDistKey = "turnAroundDistance"; +const char* SurveyComplexItem::_jsonV3CameraTriggerDistanceKey = "cameraTriggerDistance"; +const char* SurveyComplexItem::_jsonV3CameraTriggerInTurnaroundKey = "cameraTriggerInTurnaround"; +const char* SurveyComplexItem::_jsonV3HoverAndCaptureKey = "hoverAndCapture"; +const char* SurveyComplexItem::_jsonV3GroundResolutionKey = "groundResolution"; +const char* SurveyComplexItem::_jsonV3FrontalOverlapKey = "imageFrontalOverlap"; +const char* SurveyComplexItem::_jsonV3SideOverlapKey = "imageSideOverlap"; +const char* SurveyComplexItem::_jsonV3CameraSensorWidthKey = "sensorWidth"; +const char* SurveyComplexItem::_jsonV3CameraSensorHeightKey = "sensorHeight"; +const char* SurveyComplexItem::_jsonV3CameraResolutionWidthKey = "resolutionWidth"; +const char* SurveyComplexItem::_jsonV3CameraResolutionHeightKey = "resolutionHeight"; +const char* SurveyComplexItem::_jsonV3CameraFocalLengthKey = "focalLength"; +const char* SurveyComplexItem::_jsonV3CameraMinTriggerIntervalKey = "minTriggerInterval"; +const char* SurveyComplexItem::_jsonV3CameraObjectKey = "camera"; +const char* SurveyComplexItem::_jsonV3CameraNameKey = "name"; +const char* SurveyComplexItem::_jsonV3ManualGridKey = "manualGrid"; +const char* SurveyComplexItem::_jsonV3CameraOrientationLandscapeKey = "orientationLandscape"; +const char* SurveyComplexItem::_jsonV3FixedValueIsAltitudeKey = "fixedValueIsAltitude"; +const char* SurveyComplexItem::_jsonV3Refly90DegreesKey = "refly90Degrees"; +const char* SurveyComplexItem::_jsonFlyAlternateTransectsKey = "flyAlternateTransects"; + +SurveyComplexItem::SurveyComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlFile, QObject* parent) + : TransectStyleComplexItem (vehicle, flyView, settingsGroup, parent) + , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/Survey.SettingsGroup.json"), this)) + , _gridAngleFact (settingsGroup, _metaDataMap[gridAngleName]) + , _flyAlternateTransectsFact(settingsGroup, _metaDataMap[flyAlternateTransectsName]) + , _entryPoint (EntryLocationTopLeft) +{ + _editorQml = "qrc:/qml/SurveyItemEditor.qml"; + + // If the user hasn't changed turnaround from the default (which is a fixed wing default) and we are multi-rotor set the multi-rotor default. + // NULL check since object creation during unit testing passes NULL for vehicle + if (_vehicle && _vehicle->multiRotor() && _turnAroundDistanceFact.rawValue().toDouble() == _turnAroundDistanceFact.rawDefaultValue().toDouble()) { + // Note this is set to 10 meters to work around a problem with PX4 Pro turnaround behavior. Don't change unless firmware gets better as well. + _turnAroundDistanceFact.setRawValue(10); + } + + // We override the altitude to the mission default + if (_cameraCalc.isManualCamera() || !_cameraCalc.valueSetIsDistance()->rawValue().toBool()) { + _cameraCalc.distanceToSurface()->setRawValue(qgcApp()->toolbox()->settingsManager()->appSettings()->defaultMissionItemAltitude()->rawValue()); + } + + connect(&_gridAngleFact, &Fact::valueChanged, this, &SurveyComplexItem::_setDirty); + connect(&_flyAlternateTransectsFact,&Fact::valueChanged, this, &SurveyComplexItem::_setDirty); + connect(this, &SurveyComplexItem::refly90DegreesChanged, this, &SurveyComplexItem::_setDirty); + + connect(&_gridAngleFact, &Fact::valueChanged, this, &SurveyComplexItem::_rebuildTransects); + connect(&_flyAlternateTransectsFact,&Fact::valueChanged, this, &SurveyComplexItem::_rebuildTransects); + connect(this, &SurveyComplexItem::refly90DegreesChanged, this, &SurveyComplexItem::_rebuildTransects); + + // FIXME: Shouldn't these be in TransectStyleComplexItem? They are also in CorridorScanComplexItem constructur + connect(&_cameraCalc, &CameraCalc::distanceToSurfaceRelativeChanged, this, &SurveyComplexItem::coordinateHasRelativeAltitudeChanged); + connect(&_cameraCalc, &CameraCalc::distanceToSurfaceRelativeChanged, this, &SurveyComplexItem::exitCoordinateHasRelativeAltitudeChanged); + + if (!kmlFile.isEmpty()) { + _surveyAreaPolygon.loadKMLFile(kmlFile); + _surveyAreaPolygon.setDirty(false); + } + setDirty(false); +} + +void SurveyComplexItem::save(QJsonArray& planItems) +{ + QJsonObject saveObject; + + _save(saveObject); + + saveObject[JsonHelper::jsonVersionKey] = 4; + saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; + saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; + saveObject[_jsonGridAngleKey] = _gridAngleFact.rawValue().toDouble(); + saveObject[_jsonFlyAlternateTransectsKey] = _flyAlternateTransectsFact.rawValue().toBool(); + saveObject[_jsonEntryPointKey] = _entryPoint; + + // Polygon shape + _surveyAreaPolygon.saveToJson(saveObject); + + planItems.append(saveObject); +} + +bool SurveyComplexItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString) +{ + // We need to pull version first to determine what validation/conversion needs to be performed + QList versionKeyInfoList = { + { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, + }; + if (!JsonHelper::validateKeys(complexObject, versionKeyInfoList, errorString)) { + return false; + } + + int version = complexObject[JsonHelper::jsonVersionKey].toInt(); + if (version < 2 || version > 4) { + errorString = tr("Survey items do not support version %1").arg(version); + return false; + } + + if (version == 4) { + if (!_loadV4(complexObject, sequenceNumber, errorString)) { + return false; + } + } else { + // Must be v2 or v3 + QJsonObject v3ComplexObject = complexObject; + if (version == 2) { + // Convert to v3 + if (v3ComplexObject.contains(VisualMissionItem::jsonTypeKey) && v3ComplexObject[VisualMissionItem::jsonTypeKey].toString() == QStringLiteral("survey")) { + v3ComplexObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; + v3ComplexObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; + } + } + if (!_loadV3(complexObject, sequenceNumber, errorString)) { + return false; + } + } + + _rebuildTransects(); + + return true; +} + +bool SurveyComplexItem::_loadV4(const QJsonObject& complexObject, int sequenceNumber, QString& errorString) +{ + QList keyInfoList = { + { VisualMissionItem::jsonTypeKey, QJsonValue::String, true }, + { ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true }, + { _jsonEntryPointKey, QJsonValue::Double, true }, + { _jsonGridAngleKey, QJsonValue::Double, true }, + { _jsonFlyAlternateTransectsKey, QJsonValue::Bool, false }, + }; + if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) { + return false; + } + + QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString(); + QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString(); + if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonComplexItemTypeValue) { + errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType); + return false; + } + + _ignoreRecalc = true; + + setSequenceNumber(sequenceNumber); + + if (!_surveyAreaPolygon.loadFromJson(complexObject, true /* required */, errorString)) { + _surveyAreaPolygon.clear(); + return false; + } + + if (!_load(complexObject, errorString)) { + _ignoreRecalc = false; + return false; + } + + _gridAngleFact.setRawValue (complexObject[_jsonGridAngleKey].toDouble()); + _flyAlternateTransectsFact.setRawValue (complexObject[_jsonFlyAlternateTransectsKey].toBool(false)); + + _entryPoint = complexObject[_jsonEntryPointKey].toInt(); + + _ignoreRecalc = false; + + return true; +} + +bool SurveyComplexItem::_loadV3(const QJsonObject& complexObject, int sequenceNumber, QString& errorString) +{ + QList mainKeyInfoList = { + { VisualMissionItem::jsonTypeKey, QJsonValue::String, true }, + { ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true }, + { QGCMapPolygon::jsonPolygonKey, QJsonValue::Array, true }, + { _jsonV3GridObjectKey, QJsonValue::Object, true }, + { _jsonV3CameraObjectKey, QJsonValue::Object, false }, + { _jsonV3CameraTriggerDistanceKey, QJsonValue::Double, true }, + { _jsonV3ManualGridKey, QJsonValue::Bool, true }, + { _jsonV3FixedValueIsAltitudeKey, QJsonValue::Bool, true }, + { _jsonV3HoverAndCaptureKey, QJsonValue::Bool, false }, + { _jsonV3Refly90DegreesKey, QJsonValue::Bool, false }, + { _jsonV3CameraTriggerInTurnaroundKey, QJsonValue::Bool, false }, // Should really be required, but it was missing from initial code due to bug + }; + if (!JsonHelper::validateKeys(complexObject, mainKeyInfoList, errorString)) { + return false; + } + + QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString(); + QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString(); + if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonV3ComplexItemTypeValue) { + errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType); + return false; + } + + _ignoreRecalc = true; + + setSequenceNumber(sequenceNumber); + + _hoverAndCaptureFact.setRawValue (complexObject[_jsonV3HoverAndCaptureKey].toBool(false)); + _refly90DegreesFact.setRawValue (complexObject[_jsonV3Refly90DegreesKey].toBool(false)); + _cameraTriggerInTurnAroundFact.setRawValue (complexObject[_jsonV3CameraTriggerInTurnaroundKey].toBool(true)); + + _cameraCalc.valueSetIsDistance()->setRawValue (complexObject[_jsonV3FixedValueIsAltitudeKey].toBool(true)); + _cameraCalc.setDistanceToSurfaceRelative (complexObject[_jsonV3GridAltitudeRelativeKey].toBool(true)); + + bool manualGrid = complexObject[_jsonV3ManualGridKey].toBool(true); + + QList gridKeyInfoList = { + { _jsonV3GridAltitudeKey, QJsonValue::Double, true }, + { _jsonV3GridAltitudeRelativeKey, QJsonValue::Bool, true }, + { _jsonV3GridAngleKey, QJsonValue::Double, true }, + { _jsonV3GridSpacingKey, QJsonValue::Double, true }, + { _jsonEntryPointKey, QJsonValue::Double, false }, + { _jsonV3TurnaroundDistKey, QJsonValue::Double, true }, + }; + QJsonObject gridObject = complexObject[_jsonV3GridObjectKey].toObject(); + if (!JsonHelper::validateKeys(gridObject, gridKeyInfoList, errorString)) { + _ignoreRecalc = false; + return false; + } + + _gridAngleFact.setRawValue (gridObject[_jsonV3GridAngleKey].toDouble()); + _turnAroundDistanceFact.setRawValue (gridObject[_jsonV3TurnaroundDistKey].toDouble()); + + if (gridObject.contains(_jsonEntryPointKey)) { + _entryPoint = gridObject[_jsonEntryPointKey].toDouble(); + } else { + _entryPoint = EntryLocationTopRight; + } + + _cameraCalc.distanceToSurface()->setRawValue (gridObject[_jsonV3GridAltitudeKey].toDouble()); + _cameraCalc.adjustedFootprintSide()->setRawValue (gridObject[_jsonV3GridSpacingKey].toDouble()); + _cameraCalc.adjustedFootprintFrontal()->setRawValue (complexObject[_jsonV3CameraTriggerDistanceKey].toDouble()); + + if (manualGrid) { + _cameraCalc.cameraName()->setRawValue(_cameraCalc.manualCameraName()); + } else { + if (!complexObject.contains(_jsonV3CameraObjectKey)) { + errorString = tr("%1 but %2 object is missing").arg("manualGrid = false").arg("camera"); + _ignoreRecalc = false; + return false; + } + + QJsonObject cameraObject = complexObject[_jsonV3CameraObjectKey].toObject(); + + // Older code had typo on "imageSideOverlap" incorrectly being "imageSizeOverlap" + QString incorrectImageSideOverlap = "imageSizeOverlap"; + if (cameraObject.contains(incorrectImageSideOverlap)) { + cameraObject[_jsonV3SideOverlapKey] = cameraObject[incorrectImageSideOverlap]; + cameraObject.remove(incorrectImageSideOverlap); + } + + QList cameraKeyInfoList = { + { _jsonV3GroundResolutionKey, QJsonValue::Double, true }, + { _jsonV3FrontalOverlapKey, QJsonValue::Double, true }, + { _jsonV3SideOverlapKey, QJsonValue::Double, true }, + { _jsonV3CameraSensorWidthKey, QJsonValue::Double, true }, + { _jsonV3CameraSensorHeightKey, QJsonValue::Double, true }, + { _jsonV3CameraResolutionWidthKey, QJsonValue::Double, true }, + { _jsonV3CameraResolutionHeightKey, QJsonValue::Double, true }, + { _jsonV3CameraFocalLengthKey, QJsonValue::Double, true }, + { _jsonV3CameraNameKey, QJsonValue::String, true }, + { _jsonV3CameraOrientationLandscapeKey, QJsonValue::Bool, true }, + { _jsonV3CameraMinTriggerIntervalKey, QJsonValue::Double, false }, + }; + if (!JsonHelper::validateKeys(cameraObject, cameraKeyInfoList, errorString)) { + _ignoreRecalc = false; + return false; + } + + _cameraCalc.cameraName()->setRawValue (cameraObject[_jsonV3CameraNameKey].toString()); + _cameraCalc.landscape()->setRawValue (cameraObject[_jsonV3CameraOrientationLandscapeKey].toBool(true)); + _cameraCalc.frontalOverlap()->setRawValue (cameraObject[_jsonV3FrontalOverlapKey].toInt()); + _cameraCalc.sideOverlap()->setRawValue (cameraObject[_jsonV3SideOverlapKey].toInt()); + _cameraCalc.sensorWidth()->setRawValue (cameraObject[_jsonV3CameraSensorWidthKey].toDouble()); + _cameraCalc.sensorHeight()->setRawValue (cameraObject[_jsonV3CameraSensorHeightKey].toDouble()); + _cameraCalc.focalLength()->setRawValue (cameraObject[_jsonV3CameraFocalLengthKey].toDouble()); + _cameraCalc.imageWidth()->setRawValue (cameraObject[_jsonV3CameraResolutionWidthKey].toInt()); + _cameraCalc.imageHeight()->setRawValue (cameraObject[_jsonV3CameraResolutionHeightKey].toInt()); + _cameraCalc.minTriggerInterval()->setRawValue (cameraObject[_jsonV3CameraMinTriggerIntervalKey].toDouble(0)); + _cameraCalc.imageDensity()->setRawValue (cameraObject[_jsonV3GroundResolutionKey].toDouble()); + _cameraCalc.fixedOrientation()->setRawValue (false); + } + + // Polygon shape + /// Load a polygon from json + /// @param json Json object to load from + /// @param required true: no polygon in object will generate error + /// @param errorString Error string if return is false + /// @return true: success, false: failure (errorString set) + if (!_surveyAreaPolygon.loadFromJson(complexObject, true /* required */, errorString)) { + _surveyAreaPolygon.clear(); + _ignoreRecalc = false; + return false; + } + + _ignoreRecalc = false; + + return true; +} + +/// Reverse the order of the transects. First transect becomes last and so forth. +void SurveyComplexItem::_reverseTransectOrder(QList>& transects) +{ + QList> rgReversedTransects; + for (int i=transects.count() - 1; i>=0; i--) { + rgReversedTransects.append(transects[i]); + } + transects = rgReversedTransects; +} + +/// Reverse the order of all points withing each transect, First point becomes last and so forth. +void SurveyComplexItem::_reverseInternalTransectPoints(QList>& transects) +{ + for (int i=0; i rgReversedCoords; + QList& rgOriginalCoords = transects[i]; + for (int j=rgOriginalCoords.count()-1; j>=0; j--) { + rgReversedCoords.append(rgOriginalCoords[j]); + } + transects[i] = rgReversedCoords; + } +} + +/// Reorders the transects such that the first transect is the shortest distance to the specified coordinate +/// and the first point within that transect is the shortest distance to the specified coordinate. +/// @param distanceCoord Coordinate to measure distance against +/// @param transects Transects to test and reorder +void SurveyComplexItem::_optimizeTransectsForShortestDistance(const QGeoCoordinate& distanceCoord, QList>& transects) +{ + double rgTransectDistance[4]; + rgTransectDistance[0] = transects.first().first().distanceTo(distanceCoord); + rgTransectDistance[1] = transects.first().last().distanceTo(distanceCoord); + rgTransectDistance[2] = transects.last().first().distanceTo(distanceCoord); + rgTransectDistance[3] = transects.last().last().distanceTo(distanceCoord); + + int shortestIndex = 0; + double shortestDistance = rgTransectDistance[0]; + for (int i=1; i<3; i++) { + if (rgTransectDistance[i] < shortestDistance) { + shortestIndex = i; + shortestDistance = rgTransectDistance[i]; + } + } + + if (shortestIndex > 1) { + // We need to reverse the order of segments + _reverseTransectOrder(transects); + } + if (shortestIndex & 1) { + // We need to reverse the points within each segment + _reverseInternalTransectPoints(transects); + } +} + +qreal SurveyComplexItem::_ccw(QPointF pt1, QPointF pt2, QPointF pt3) +{ + return (pt2.x()-pt1.x())*(pt3.y()-pt1.y()) - (pt2.y()-pt1.y())*(pt3.x()-pt1.x()); +} + +qreal SurveyComplexItem::_dp(QPointF pt1, QPointF pt2) +{ + return (pt2.x()-pt1.x())/qSqrt((pt2.x()-pt1.x())*(pt2.x()-pt1.x()) + (pt2.y()-pt1.y())*(pt2.y()-pt1.y())); +} + +void SurveyComplexItem::_swapPoints(QList& points, int index1, int index2) +{ + QPointF temp = points[index1]; + points[index1] = points[index2]; + points[index2] = temp; +} + +/// Returns true if the current grid angle generates north/south oriented transects +bool SurveyComplexItem::_gridAngleIsNorthSouthTransects() +{ + // Grid angle ranges from -360<->360 + double gridAngle = qAbs(_gridAngleFact.rawValue().toDouble()); + return gridAngle < 45.0 || (gridAngle > 360.0 - 45.0) || (gridAngle > 90.0 + 45.0 && gridAngle < 270.0 - 45.0); +} + +void SurveyComplexItem::_adjustTransectsToEntryPointLocation(QList>& transects) +{ + if (transects.count() == 0) { + return; + } + + bool reversePoints = false; + bool reverseTransects = false; + + if (_entryPoint == EntryLocationBottomLeft || _entryPoint == EntryLocationBottomRight) { + reversePoints = true; + } + if (_entryPoint == EntryLocationTopRight || _entryPoint == EntryLocationBottomRight) { + reverseTransects = true; + } + + if (reversePoints) { + qCDebug(SurveyComplexItemLog) << "_adjustTransectsToEntryPointLocation Reverse Points"; + _reverseInternalTransectPoints(transects); + } + if (reverseTransects) { + qCDebug(SurveyComplexItemLog) << "_adjustTransectsToEntryPointLocation Reverse Transects"; + _reverseTransectOrder(transects); + } + + qCDebug(SurveyComplexItemLog) << "_adjustTransectsToEntryPointLocation Modified entry point:entryLocation" << transects.first().first() << _entryPoint; +} + +#if 0 +void SurveyComplexItem::_generateGrid(void) +{ + if (_ignoreRecalc) { + return; + } + + if (_mapPolygon.count() < 3 || _gridSpacingFact.rawValue().toDouble() <= 0) { + _clearInternal(); + return; + } + + _simpleGridPoints.clear(); + _transectSegments.clear(); + _reflyTransectSegments.clear(); + _additionalFlightDelaySeconds = 0; + + QList polygonPoints; + QList> transectSegments; + + // Convert polygon to NED + QGeoCoordinate tangentOrigin = _mapPolygon.pathModel().value(0)->coordinate(); + qCDebug(SurveyComplexItemLog) << "Convert polygon to NED - tangentOrigin" << tangentOrigin; + for (int i=0; i<_mapPolygon.count(); i++) { + double y, x, down; + QGeoCoordinate vertex = _mapPolygon.pathModel().value(i)->coordinate(); + if (i == 0) { + // This avoids a nan calculation that comes out of convertGeoToNed + x = y = 0; + } else { + convertGeoToNed(vertex, tangentOrigin, &y, &x, &down); + } + polygonPoints += QPointF(x, y); + qCDebug(SurveyComplexItemLog) << "vertex:x:y" << vertex << polygonPoints.last().x() << polygonPoints.last().y(); + } + + double coveredArea = 0.0; + for (int i=0; i(); + QGeoCoordinate coord2 = _simpleGridPoints[i].value(); + surveyDistance += coord1.distanceTo(coord2); + } + _setSurveyDistance(surveyDistance); + + if (cameraShots == 0 && _triggerCamera()) { + cameraShots = (int)floor(surveyDistance / _triggerDistance()); + // Take into account immediate camera trigger at waypoint entry + cameraShots++; + } + _setCameraShots(cameraShots); + + if (_hoverAndCaptureEnabled()) { + _additionalFlightDelaySeconds = cameraShots * _hoverAndCaptureDelaySeconds; + } + emit additionalTimeDelayChanged(); + + emit gridPointsChanged(); + + // Determine command count for lastSequenceNumber + _missionCommandCount = _calcMissionCommandCount(_transectSegments); + _missionCommandCount += _calcMissionCommandCount(_reflyTransectSegments); + emit lastSequenceNumberChanged(lastSequenceNumber()); + + // Set exit coordinate + if (_simpleGridPoints.count()) { + QGeoCoordinate coordinate = _simpleGridPoints.first().value(); + coordinate.setAltitude(_gridAltitudeFact.rawValue().toDouble()); + setCoordinate(coordinate); + QGeoCoordinate exitCoordinate = _simpleGridPoints.last().value(); + exitCoordinate.setAltitude(_gridAltitudeFact.rawValue().toDouble()); + _setExitCoordinate(exitCoordinate); + } + + setDirty(true); +} +#endif + +QPointF SurveyComplexItem::_rotatePoint(const QPointF& point, const QPointF& origin, double angle) +{ + QPointF rotated; + double radians = (M_PI / 180.0) * -angle; + + rotated.setX(((point.x() - origin.x()) * cos(radians)) - ((point.y() - origin.y()) * sin(radians)) + origin.x()); + rotated.setY(((point.x() - origin.x()) * sin(radians)) + ((point.y() - origin.y()) * cos(radians)) + origin.y()); + + return rotated; +} + +void SurveyComplexItem::_intersectLinesWithRect(const QList& lineList, const QRectF& boundRect, QList& resultLines) +{ + QLineF topLine (boundRect.topLeft(), boundRect.topRight()); + QLineF bottomLine (boundRect.bottomLeft(), boundRect.bottomRight()); + QLineF leftLine (boundRect.topLeft(), boundRect.bottomLeft()); + QLineF rightLine (boundRect.topRight(), boundRect.bottomRight()); + + for (int i=0; i& lineList, const QPolygonF& polygon, QList& resultLines) +{ + resultLines.clear(); + + for (int i=0; i intersections; + + // Intersect the line with all the polygon edges + for (int j=0; j 1) { + QPointF firstPoint; + QPointF secondPoint; + double currentMaxDistance = 0; + + for (int i=0; i currentMaxDistance) { + firstPoint = intersections[i]; + secondPoint = intersections[j]; + currentMaxDistance = newMaxDistance; + } + } + } + + resultLines += QLineF(firstPoint, secondPoint); + } + } +} + +/// Adjust the line segments such that they are all going the same direction with respect to going from P1->P2 +void SurveyComplexItem::_adjustLineDirection(const QList& lineList, QList& resultLines) +{ + qreal firstAngle = 0; + for (int i=0; i 1.0) { + adjustedLine.setP1(line.p2()); + adjustedLine.setP2(line.p1()); + } else { + adjustedLine = line; + } + + resultLines += adjustedLine; + } +} + +double SurveyComplexItem::_clampGridAngle90(double gridAngle) +{ + // Clamp grid angle to -90<->90. This prevents transects from being rotated to a reversed order. + if (gridAngle > 90.0) { + gridAngle -= 180.0; + } else if (gridAngle < -90.0) { + gridAngle += 180; + } + return gridAngle; +} + +#if 0 +int SurveyComplexItem::_gridGenerator(const QList& polygonPoints, QList>& transectSegments, bool refly) +{ + int cameraShots = 0; + + double gridAngle = _gridAngleFact.rawValue().toDouble(); + double gridSpacing = _gridSpacingFact.rawValue().toDouble(); + + gridAngle = _clampGridAngle90(gridAngle); + gridAngle += refly ? 90 : 0; + qCDebug(SurveyComplexItemLog) << "Clamped grid angle" << gridAngle; + + qCDebug(SurveyComplexItemLog) << "SurveyComplexItem::_gridGenerator gridSpacing:gridAngle:refly" << gridSpacing << gridAngle << refly; + + transectSegments.clear(); + + // Convert polygon to bounding rect + + qCDebug(SurveyComplexItemLog) << "Polygon"; + QPolygonF polygon; + for (int i=0; i lineList; + + // Transects are generated to be as long as the largest width/height of the bounding rect plus some fudge factor. + // This way they will always be guaranteed to intersect with a polygon edge no matter what angle they are rotated to. + // They are initially generated with the transects flowing from west to east and then points within the transect north to south. + double maxWidth = qMax(boundingRect.width(), boundingRect.height()) + 2000.0; + double halfWidth = maxWidth / 2.0; + double transectX = boundingCenter.x() - halfWidth; + double transectXMax = transectX + maxWidth; + while (transectX < transectXMax) { + double transectYTop = boundingCenter.y() - halfWidth; + double transectYBottom = boundingCenter.y() + halfWidth; + + lineList += QLineF(_rotatePoint(QPointF(transectX, transectYTop), boundingCenter, gridAngle), _rotatePoint(QPointF(transectX, transectYBottom), boundingCenter, gridAngle)); + transectX += gridSpacing; + } + + // Now intersect the lines with the polygon + QList intersectLines; +#if 1 + _intersectLinesWithPolygon(lineList, polygon, intersectLines); +#else + // This is handy for debugging grid problems, not for release + intersectLines = lineList; +#endif + + // Less than two transects intersected with the polygon: + // Create a single transect which goes through the center of the polygon + // Intersect it with the polygon + if (intersectLines.count() < 2) { + _mapPolygon.center(); + QLineF firstLine = lineList.first(); + QPointF lineCenter = firstLine.pointAt(0.5); + QPointF centerOffset = boundingCenter - lineCenter; + firstLine.translate(centerOffset); + lineList.clear(); + lineList.append(firstLine); + intersectLines = lineList; + _intersectLinesWithPolygon(lineList, polygon, intersectLines); + } + + // Make sure all lines are going to same direction. Polygon intersection leads to line which + // can be in varied directions depending on the order of the intesecting sides. + QList resultLines; + _adjustLineDirection(intersectLines, resultLines); + + // Calc camera shots here if there are no images in turnaround + if (_triggerCamera() && !_imagesEverywhere()) { + for (int i=0; i transectPoints; + const QLineF& line = resultLines[i]; + + float turnaroundPosition = _turnaroundDistance() / line.length(); + + if (i & 1) { + transectLine = QLineF(line.p2(), line.p1()); + } else { + transectLine = QLineF(line.p1(), line.p2()); + } + + // Build the points along the transect + + if (_hasTurnaround()) { + transectPoints.append(transectLine.pointAt(-turnaroundPosition)); + } + + // Polygon entry point + transectPoints.append(transectLine.p1()); + + // For hover and capture we need points for each camera location + if (_triggerCamera() && _hoverAndCaptureEnabled()) { + if (_triggerDistance() < transectLine.length()) { + int innerPoints = floor(transectLine.length() / _triggerDistance()); + qCDebug(SurveyComplexItemLog) << "innerPoints" << innerPoints; + float transectPositionIncrement = _triggerDistance() / transectLine.length(); + for (int i=0; i& items, int seqNum, QGeoCoordinate& coord, CameraTriggerCode cameraTrigger, QObject* missionItemParent) +{ + double altitude = _gridAltitudeFact.rawValue().toDouble(); + bool altitudeRelative = _gridAltitudeRelativeFact.rawValue().toBool(); + + qCDebug(SurveyComplexItemLog) << "_appendWaypointToMission seq:trigger" << seqNum << (cameraTrigger != CameraTriggerNone); + + MissionItem* item = new MissionItem(seqNum++, + MAV_CMD_NAV_WAYPOINT, + altitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, + cameraTrigger == CameraTriggerHoverAndCapture ? _hoverAndCaptureDelaySeconds : 0, // Hold time (delay for hover and capture to settle vehicle before image is taken) + 0.0, 0.0, + std::numeric_limits::quiet_NaN(), // Yaw unchanged + coord.latitude(), + coord.longitude(), + altitude, + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); + + switch (cameraTrigger) { + case CameraTriggerOff: + case CameraTriggerOn: + item = new MissionItem(seqNum++, + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + MAV_FRAME_MISSION, + cameraTrigger == CameraTriggerOn ? _triggerDistance() : 0, + 0, // shutter integration (ignore) + cameraTrigger == CameraTriggerOn ? 1 : 0, // trigger immediately when starting + 0, 0, 0, 0, // param 4-7 unused + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); + break; + case CameraTriggerHoverAndCapture: + item = new MissionItem(seqNum++, + MAV_CMD_IMAGE_START_CAPTURE, + MAV_FRAME_MISSION, + 0, // Reserved (Set to 0) + 0, // Interval (none) + 1, // Take 1 photo + NAN, NAN, NAN, NAN, // param 4-7 reserved + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); +#if 0 + // This generates too many commands. Pulling out for now, to see if image quality is still high enough. + item = new MissionItem(seqNum++, + MAV_CMD_NAV_DELAY, + MAV_FRAME_MISSION, + 0.5, // Delay in seconds, give some time for image to be taken + -1, -1, -1, // No time + 0, 0, 0, // Param 5-7 unused + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); +#endif + default: + break; + } + + return seqNum; +} +#endif + +bool SurveyComplexItem::_nextTransectCoord(const QList& transectPoints, int pointIndex, QGeoCoordinate& coord) +{ + if (pointIndex > transectPoints.count()) { + qWarning() << "Bad grid generation"; + return false; + } + + coord = transectPoints[pointIndex]; + return true; +} + +void SurveyComplexItem::_buildAndAppendMissionItems(QList& items, QObject* missionItemParent) +{ + qCDebug(SurveyComplexItemLog) << "_buildAndAppendMissionItems"; + + // Now build the mission items from the transect points + + MissionItem* item; + int seqNum = _sequenceNumber; + bool imagesEverywhere = _cameraTriggerInTurnAroundFact.rawValue().toBool(); + bool addTriggerAtBeginning = imagesEverywhere; + bool firstOverallPoint = true; + + MAV_FRAME mavFrame = followTerrain() || !_cameraCalc.distanceToSurfaceRelative() ? MAV_FRAME_GLOBAL : MAV_FRAME_GLOBAL_RELATIVE_ALT; + + foreach (const QList& transect, _transects) { + bool entryPoint = true; + + foreach (const CoordInfo_t& transectCoordInfo, transect) { + item = new MissionItem(seqNum++, + MAV_CMD_NAV_WAYPOINT, + mavFrame, + 0, // No hold time + 0.0, // No acceptance radius specified + 0.0, // Pass through waypoint + std::numeric_limits::quiet_NaN(), // Yaw unchanged + transectCoordInfo.coord.latitude(), + transectCoordInfo.coord.longitude(), + transectCoordInfo.coord.altitude(), + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); + + if (firstOverallPoint && addTriggerAtBeginning) { + // Start triggering + addTriggerAtBeginning = false; + item = new MissionItem(seqNum++, + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + MAV_FRAME_MISSION, + _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble(), // trigger distance + 0, // shutter integration (ignore) + 1, // trigger immediately when starting + 0, 0, 0, 0, // param 4-7 unused + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); + } + firstOverallPoint = false; + + if (transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEdge && !imagesEverywhere) { + if (entryPoint) { + // Start of transect, start triggering + item = new MissionItem(seqNum++, + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + MAV_FRAME_MISSION, + _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble(), // trigger distance + 0, // shutter integration (ignore) + 1, // trigger immediately when starting + 0, 0, 0, 0, // param 4-7 unused + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); + } else { + // End of transect, stop triggering + item = new MissionItem(seqNum++, + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + MAV_FRAME_MISSION, + 0, // stop triggering + 0, // shutter integration (ignore) + 0, // trigger immediately when starting + 0, 0, 0, 0, // param 4-7 unused + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); + } + entryPoint = !entryPoint; + } + } + } + + if (imagesEverywhere) { + // Stop triggering + MissionItem* item = new MissionItem(seqNum++, + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + MAV_FRAME_MISSION, + 0, // stop triggering + 0, // shutter integration (ignore) + 0, // trigger immediately when starting + 0, 0, 0, 0, // param 4-7 unused + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); + } +} + +#if 0 +bool SurveyComplexItem::_buildAndAppendMissionItems(QList& items, QObject* missionItemParent) +{ + int seqNum = _sequenceNumber; + bool firstWaypointTrigger = false; + +#if 1 + // FIXME: Hack + bool hasRefly = false; + bool buildRefly = false; +#endif + + qCDebug(SurveyComplexItemLog) << QStringLiteral("hasTurnaround(%1) triggerCamera(%2) hoverAndCapture(%3) imagesEverywhere(%4) hasRefly(%5) buildRefly(%6) ").arg(_hasTurnaround()).arg(_triggerCamera()).arg(_hoverAndCaptureEnabled()).arg(_imagesEverywhere()).arg(hasRefly).arg(buildRefly); + +#if 0 + QList>& transectSegments = buildRefly ? _reflyTransectSegments : _transectSegments; +#endif + + if (!buildRefly && _imagesEverywhere()) { + firstWaypointTrigger = true; + } + + foreach (const QList& transect, _transects) { + int pointIndex = 0; + QGeoCoordinate coord; + CameraTriggerCode cameraTrigger; + + qCDebug(SurveyComplexItemLog) << "transect.count" << transect.count(); + + if (_hasTurnaround()) { + // Add entry turnaround point + if (!_nextTransectCoord(segment, pointIndex++, coord)) { + return false; + } + seqNum = _appendWaypointToMission(items, seqNum, coord, firstWaypointTrigger ? CameraTriggerOn : CameraTriggerNone, missionItemParent); + firstWaypointTrigger = false; + } + + // Add polygon entry point + if (!_nextTransectCoord(segment, pointIndex++, coord)) { + return false; + } + if (firstWaypointTrigger) { + cameraTrigger = CameraTriggerOn; + } else { + cameraTrigger = _imagesEverywhere() || !_triggerCamera() ? CameraTriggerNone : (_hoverAndCaptureEnabled() ? CameraTriggerHoverAndCapture : CameraTriggerOn); + } + seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent); + firstWaypointTrigger = false; + + // Add internal hover and capture points + if (_hoverAndCaptureEnabled()) { + int lastHoverAndCaptureIndex = segment.count() - 1 - (_hasTurnaround() ? 1 : 0); + qCDebug(SurveyComplexItemLog) << "lastHoverAndCaptureIndex" << lastHoverAndCaptureIndex; + for (; pointIndex < lastHoverAndCaptureIndex; pointIndex++) { + if (!_nextTransectCoord(segment, pointIndex, coord)) { + return false; + } + seqNum = _appendWaypointToMission(items, seqNum, coord, CameraTriggerHoverAndCapture, missionItemParent); + } + } + + // Add polygon exit point + if (!_nextTransectCoord(segment, pointIndex++, coord)) { + return false; + } + cameraTrigger = _imagesEverywhere() || !_triggerCamera() ? CameraTriggerNone : (_hoverAndCaptureEnabled() ? CameraTriggerNone : CameraTriggerOff); + seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent); + + if (_hasTurnaround()) { + // Add exit turnaround point + if (!_nextTransectCoord(segment, pointIndex++, coord)) { + return false; + } + seqNum = _appendWaypointToMission(items, seqNum, coord, CameraTriggerNone, missionItemParent); + } + + qCDebug(SurveyComplexItemLog) << "last PointIndex" << pointIndex; + } + + if (((hasRefly && buildRefly) || !hasRefly) && _imagesEverywhere()) { + // Turn off camera at end of survey + MissionItem* item = new MissionItem(seqNum++, + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + MAV_FRAME_MISSION, + 0.0, // trigger distance (off) + 0, 0, 0, 0, 0, 0, // param 2-7 unused + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); + } + + return true; +} +#endif + +#if 0 +void SurveyComplexItem::appendMissionItems(QList& items, QObject* missionItemParent) +{ + int seqNum = _sequenceNumber; + bool refly = refly90Degrees()->rawValue().toBool(); + + if (!_appendMissionItemsWorker(items, missionItemParent, seqNum, refly, false /* buildRefly */)) { + return; + } + + if (refly) { + _appendMissionItemsWorker(items, missionItemParent, seqNum, refly, true /* buildRefly */); + } +} +#endif + +bool SurveyComplexItem::_hasTurnaround(void) const +{ + return _turnaroundDistance() > 0; +} + +double SurveyComplexItem::_turnaroundDistance(void) const +{ + return _turnAroundDistanceFact.rawValue().toDouble(); +} + +#if 0 +bool SurveyComplexItem::_triggerCamera(void) const +{ + return _triggerDistance() > 0; +} + +bool SurveyComplexItem::_imagesEverywhere(void) const +{ + return _triggerCamera() && _cameraTriggerInTurnaroundFact.rawValue().toBool(); +} + +bool SurveyComplexItem::_hoverAndCaptureEnabled(void) const +{ + return hoverAndCaptureAllowed() && !_imagesEverywhere() && _triggerCamera() && _hoverAndCaptureFact.rawValue().toBool(); +} +#endif + +void SurveyComplexItem::_rebuildTransectsPhase1(void) +{ + _rebuildTransectsPhase1Worker(false /* refly */); + if (_refly90DegreesFact.rawValue().toBool()) { + _rebuildTransectsPhase1Worker(true /* refly */); + } +} + +void SurveyComplexItem::_rebuildTransectsPhase1Worker(bool refly) +{ + if (_ignoreRecalc) { + return; + } + + // If the transects are getting rebuilt then any previously loaded mission items are now invalid + if (_loadedMissionItemsParent) { + _loadedMissionItems.clear(); + _loadedMissionItemsParent->deleteLater(); + _loadedMissionItemsParent = NULL; + } + + // First pass will clear old transect data, refly will append to existing data + if (!refly) { + _transects.clear(); + _transectsPathHeightInfo.clear(); + } + + if (_surveyAreaPolygon.count() < 3) { + return; + } + + // Convert polygon to NED + + QList polygonPoints; + QGeoCoordinate tangentOrigin = _surveyAreaPolygon.pathModel().value(0)->coordinate(); + qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 Convert polygon to NED - _surveyAreaPolygon.count():tangentOrigin" << _surveyAreaPolygon.count() << tangentOrigin; + for (int i=0; i<_surveyAreaPolygon.count(); i++) { + double y, x, down; + QGeoCoordinate vertex = _surveyAreaPolygon.pathModel().value(i)->coordinate(); + if (i == 0) { + // This avoids a nan calculation that comes out of convertGeoToNed + x = y = 0; + } else { + convertGeoToNed(vertex, tangentOrigin, &y, &x, &down); + } + polygonPoints += QPointF(x, y); + qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 vertex:x:y" << vertex << polygonPoints.last().x() << polygonPoints.last().y(); + } + + // Generate transects + + double gridAngle = _gridAngleFact.rawValue().toDouble(); + double gridSpacing = _cameraCalc.adjustedFootprintSide()->rawValue().toDouble(); + + gridAngle = _clampGridAngle90(gridAngle); + gridAngle += refly ? 90 : 0; + qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 Clamped grid angle" << gridAngle; + + qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 gridSpacing:gridAngle:refly" << gridSpacing << gridAngle << refly; + + // Convert polygon to bounding rect + + qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 Polygon"; + QPolygonF polygon; + for (int i=0; i lineList; + + // Transects are generated to be as long as the largest width/height of the bounding rect plus some fudge factor. + // This way they will always be guaranteed to intersect with a polygon edge no matter what angle they are rotated to. + // They are initially generated with the transects flowing from west to east and then points within the transect north to south. + double maxWidth = qMax(boundingRect.width(), boundingRect.height()) + 2000.0; + double halfWidth = maxWidth / 2.0; + double transectX = boundingCenter.x() - halfWidth; + double transectXMax = transectX + maxWidth; + while (transectX < transectXMax) { + double transectYTop = boundingCenter.y() - halfWidth; + double transectYBottom = boundingCenter.y() + halfWidth; + + lineList += QLineF(_rotatePoint(QPointF(transectX, transectYTop), boundingCenter, gridAngle), _rotatePoint(QPointF(transectX, transectYBottom), boundingCenter, gridAngle)); + transectX += gridSpacing; + } + + // Now intersect the lines with the polygon + QList intersectLines; +#if 1 + _intersectLinesWithPolygon(lineList, polygon, intersectLines); +#else + // This is handy for debugging grid problems, not for release + intersectLines = lineList; +#endif + + // Less than two transects intersected with the polygon: + // Create a single transect which goes through the center of the polygon + // Intersect it with the polygon + if (intersectLines.count() < 2) { + _surveyAreaPolygon.center(); + QLineF firstLine = lineList.first(); + QPointF lineCenter = firstLine.pointAt(0.5); + QPointF centerOffset = boundingCenter - lineCenter; + firstLine.translate(centerOffset); + lineList.clear(); + lineList.append(firstLine); + intersectLines = lineList; + _intersectLinesWithPolygon(lineList, polygon, intersectLines); + } + + // Make sure all lines are going the same direction. Polygon intersection leads to lines which + // can be in varied directions depending on the order of the intesecting sides. + QList resultLines; + _adjustLineDirection(intersectLines, resultLines); + + // Convert from NED to Geo + QList> transects; + foreach (const QLineF& line, resultLines) { + QGeoCoordinate coord; + QList transect; + + convertNedToGeo(line.p1().y(), line.p1().x(), 0, tangentOrigin, &coord); + transect.append(coord); + convertNedToGeo(line.p2().y(), line.p2().x(), 0, tangentOrigin, &coord); + transect.append(coord); + + transects.append(transect); + } + + _adjustTransectsToEntryPointLocation(transects); + + if (refly) { + _optimizeTransectsForShortestDistance(_transects.last().last().coord, transects); + } + + if (_flyAlternateTransectsFact.rawValue().toBool()) { + QList> alternatingTransects; + for (int i=0; i0; i--) { + if (i & 1) { + alternatingTransects.append(transects[i]); + } + } + transects = alternatingTransects; + } + + // Adjust to lawnmower pattern + bool reverseVertices = false; + for (int i=0; i transectVertices = transects[i]; + if (reverseVertices) { + reverseVertices = false; + QList reversedVertices; + for (int j=transectVertices.count()-1; j>=0; j--) { + reversedVertices.append(transectVertices[j]); + } + transectVertices = reversedVertices; + } else { + reverseVertices = true; + } + transects[i] = transectVertices; + } + + // Convert to CoordInfo transects and append to _transects + foreach (const QList& transect, transects) { + QGeoCoordinate coord; + QList coordInfoTransect; + TransectStyleComplexItem::CoordInfo_t coordInfo; + + coordInfo = { transect[0], CoordTypeSurveyEdge }; + coordInfoTransect.append(coordInfo); + coordInfo = { transect[1], CoordTypeSurveyEdge }; + coordInfoTransect.append(coordInfo); + + + // Extend the transect ends for turnaround + if (_hasTurnaround()) { + QGeoCoordinate turnaroundCoord; + double turnAroundDistance = _turnAroundDistanceFact.rawValue().toDouble(); + + double azimuth = transect[0].azimuthTo(transect[1]); + turnaroundCoord = transect[0].atDistanceAndAzimuth(-turnAroundDistance, azimuth); + turnaroundCoord.setAltitude(qQNaN()); + TransectStyleComplexItem::CoordInfo_t coordInfo = { turnaroundCoord, CoordTypeTurnaround }; + coordInfoTransect.prepend(coordInfo); + + azimuth = transect.last().azimuthTo(transect[transect.count() - 2]); + turnaroundCoord = transect.last().atDistanceAndAzimuth(-turnAroundDistance, azimuth); + turnaroundCoord.setAltitude(qQNaN()); + coordInfo = { turnaroundCoord, CoordTypeTurnaround }; + coordInfoTransect.append(coordInfo); + } + + _transects.append(coordInfoTransect); + } +} + +void SurveyComplexItem::_rebuildTransectsPhase2(void) +{ + // Calculate distance flown for complex item + _complexDistance = 0; + for (int i=0; i<_visualTransectPoints.count() - 1; i++) { + _complexDistance += _visualTransectPoints[i].value().distanceTo(_visualTransectPoints[i+1].value()); + } + + double triggerDistance = _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble(); + if (_cameraTriggerInTurnAroundFact.rawValue().toBool()) { + _cameraShots = qCeil(_complexDistance / triggerDistance); + } else { + _cameraShots = 0; + foreach (const QList& transect, _transects) { + QGeoCoordinate firstCameraCoord, lastCameraCoord; + if (_hasTurnaround()) { + firstCameraCoord = transect[1].coord; + lastCameraCoord = transect[transect.count() - 2].coord; + } else { + firstCameraCoord = transect.first().coord; + lastCameraCoord = transect.last().coord; + } + _cameraShots += qCeil(firstCameraCoord.distanceTo(lastCameraCoord) / triggerDistance); + } + } + + _coordinate = _visualTransectPoints.count() ? _visualTransectPoints.first().value() : QGeoCoordinate(); + _exitCoordinate = _visualTransectPoints.count() ? _visualTransectPoints.last().value() : QGeoCoordinate(); + + emit cameraShotsChanged(); + emit complexDistanceChanged(); + emit coordinateChanged(_coordinate); + emit exitCoordinateChanged(_exitCoordinate); +} + +// FIXME: This same exact code is in Corridor Scan. Move to TransectStyleComplex? +void SurveyComplexItem::applyNewAltitude(double newAltitude) +{ + _cameraCalc.valueSetIsDistance()->setRawValue(true); + _cameraCalc.distanceToSurface()->setRawValue(newAltitude); + _cameraCalc.setDistanceToSurfaceRelative(true); +} + +bool SurveyComplexItem::readyForSave(void) const +{ + return TransectStyleComplexItem::readyForSave(); +} + +void SurveyComplexItem::appendMissionItems(QList& items, QObject* missionItemParent) +{ + if (_loadedMissionItems.count()) { + // We have mission items from the loaded plan, use those + _appendLoadedMissionItems(items, missionItemParent); + } else { + // Build the mission items on the fly + _buildAndAppendMissionItems(items, missionItemParent); + } +} + +void SurveyComplexItem::_appendLoadedMissionItems(QList& items, QObject* missionItemParent) +{ + qCDebug(SurveyComplexItemLog) << "_appendLoadedMissionItems"; + + int seqNum = _sequenceNumber; + + foreach (const MissionItem* loadedMissionItem, _loadedMissionItems) { + MissionItem* item = new MissionItem(*loadedMissionItem, missionItemParent); + item->setSequenceNumber(seqNum++); + items.append(item); + } +} + +void SurveyComplexItem::rotateEntryPoint(void) +{ + if (_entryPoint == EntryLocationLast) { + _entryPoint = EntryLocationFirst; + } else { + _entryPoint++; + } + + _rebuildTransects(); + + setDirty(true); +} + +double SurveyComplexItem::timeBetweenShots(void) +{ + return _cruiseSpeed == 0 ? 0 : _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / _cruiseSpeed; +} + diff --git a/src/MissionManager/SurveyComplexItem.h b/src/MissionManager/SurveyComplexItem.h new file mode 100644 index 0000000000000000000000000000000000000000..7935b484502f616c156a41ef953a03019d80b9a1 --- /dev/null +++ b/src/MissionManager/SurveyComplexItem.h @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "TransectStyleComplexItem.h" +#include "MissionItem.h" +#include "SettingsFact.h" +#include "QGCLoggingCategory.h" + +Q_DECLARE_LOGGING_CATEGORY(SurveyComplexItemLog) + +class SurveyComplexItem : public TransectStyleComplexItem +{ + Q_OBJECT + +public: + /// @param vehicle Vehicle which this is being contructed for + /// @param flyView true: Created for use in the Fly View, false: Created for use in the Plan View + /// @param kmlFile Polygon comes from this file, empty for default polygon + SurveyComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlFile, QObject* parent); + + Q_PROPERTY(Fact* gridAngle READ gridAngle CONSTANT) + Q_PROPERTY(Fact* flyAlternateTransects READ flyAlternateTransects CONSTANT) + + Fact* gridAngle (void) { return &_gridAngleFact; } + Fact* flyAlternateTransects (void) { return &_flyAlternateTransectsFact; } + + Q_INVOKABLE void rotateEntryPoint(void); + + // Overrides from ComplexMissionItem + bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final; + QString mapVisualQML (void) const final { return QStringLiteral("SurveyMapVisual.qml"); } + + // Overrides from TransectStyleComplexItem + void save (QJsonArray& planItems) final; + bool specifiesCoordinate (void) const final { return true; } + void appendMissionItems (QList& items, QObject* missionItemParent) final; + void applyNewAltitude (double newAltitude) final; + double timeBetweenShots (void) final; + + // Overrides from VisualMissionionItem + QString commandDescription (void) const final { return tr("Survey"); } + QString commandName (void) const final { return tr("Survey"); } + QString abbreviation (void) const final { return tr("S"); } + bool readyForSave (void) const final; + + // Must match json spec for GridEntryLocation + enum EntryLocation { + EntryLocationFirst, + EntryLocationTopLeft = EntryLocationFirst, + EntryLocationTopRight, + EntryLocationBottomLeft, + EntryLocationBottomRight, + EntryLocationLast = EntryLocationBottomRight + }; + + static const char* jsonComplexItemTypeValue; + static const char* settingsGroup; + static const char* gridAngleName; + static const char* gridEntryLocationName; + static const char* flyAlternateTransectsName; + + static const char* jsonV3ComplexItemTypeValue; + +signals: + void refly90DegreesChanged(bool refly90Degrees); + +private slots: + // Overrides from TransectStyleComplexItem + void _rebuildTransectsPhase1(void) final; + void _rebuildTransectsPhase2(void) final; + +private: + enum CameraTriggerCode { + CameraTriggerNone, + CameraTriggerOn, + CameraTriggerOff, + CameraTriggerHoverAndCapture + }; + + QPointF _rotatePoint(const QPointF& point, const QPointF& origin, double angle); + void _intersectLinesWithRect(const QList& lineList, const QRectF& boundRect, QList& resultLines); + void _intersectLinesWithPolygon(const QList& lineList, const QPolygonF& polygon, QList& resultLines); + void _adjustLineDirection(const QList& lineList, QList& resultLines); + int _appendWaypointToMission(QList& items, int seqNum, QGeoCoordinate& coord, CameraTriggerCode cameraTrigger, QObject* missionItemParent); + bool _nextTransectCoord(const QList& transectPoints, int pointIndex, QGeoCoordinate& coord); + bool _appendMissionItemsWorker(QList& items, QObject* missionItemParent, int& seqNum, bool hasRefly, bool buildRefly); + void _optimizeTransectsForShortestDistance(const QGeoCoordinate& distanceCoord, QList>& transects); + qreal _ccw(QPointF pt1, QPointF pt2, QPointF pt3); + qreal _dp(QPointF pt1, QPointF pt2); + void _swapPoints(QList& points, int index1, int index2); + void _reverseTransectOrder(QList>& transects); + void _reverseInternalTransectPoints(QList>& transects); + void _adjustTransectsToEntryPointLocation(QList>& transects); + bool _gridAngleIsNorthSouthTransects(); + double _clampGridAngle90(double gridAngle); + void _buildAndAppendMissionItems(QList& items, QObject* missionItemParent); + void _appendLoadedMissionItems (QList& items, QObject* missionItemParent); + bool _imagesEverywhere(void) const; + bool _triggerCamera(void) const; + bool _hasTurnaround(void) const; + double _turnaroundDistance(void) const; + bool _hoverAndCaptureEnabled(void) const; + bool _loadV3(const QJsonObject& complexObject, int sequenceNumber, QString& errorString); + bool _loadV4(const QJsonObject& complexObject, int sequenceNumber, QString& errorString); + void _rebuildTransectsPhase1Worker(bool refly); + + QMap _metaDataMap; + + SettingsFact _gridAngleFact; + SettingsFact _flyAlternateTransectsFact; + int _entryPoint; + + static const char* _jsonGridAngleKey; + static const char* _jsonEntryPointKey; + static const char* _jsonFlyAlternateTransectsKey; + + static const char* _jsonV3GridObjectKey; + static const char* _jsonV3GridAltitudeKey; + static const char* _jsonV3GridAltitudeRelativeKey; + static const char* _jsonV3GridAngleKey; + static const char* _jsonV3GridSpacingKey; + static const char* _jsonV3EntryPointKey; + static const char* _jsonV3TurnaroundDistKey; + static const char* _jsonV3CameraTriggerDistanceKey; + static const char* _jsonV3CameraTriggerInTurnaroundKey; + static const char* _jsonV3HoverAndCaptureKey; + static const char* _jsonV3GroundResolutionKey; + static const char* _jsonV3FrontalOverlapKey; + static const char* _jsonV3SideOverlapKey; + static const char* _jsonV3CameraSensorWidthKey; + static const char* _jsonV3CameraSensorHeightKey; + static const char* _jsonV3CameraResolutionWidthKey; + static const char* _jsonV3CameraResolutionHeightKey; + static const char* _jsonV3CameraFocalLengthKey; + static const char* _jsonV3CameraMinTriggerIntervalKey; + static const char* _jsonV3ManualGridKey; + static const char* _jsonV3CameraObjectKey; + static const char* _jsonV3CameraNameKey; + static const char* _jsonV3CameraOrientationLandscapeKey; + static const char* _jsonV3FixedValueIsAltitudeKey; + static const char* _jsonV3Refly90DegreesKey; + + + static const int _hoverAndCaptureDelaySeconds = 4; +}; diff --git a/src/MissionManager/SurveyComplexItemTest.cc b/src/MissionManager/SurveyComplexItemTest.cc new file mode 100644 index 0000000000000000000000000000000000000000..ca86135c39a8fde26b74062d72b3f47c4d14b686 --- /dev/null +++ b/src/MissionManager/SurveyComplexItemTest.cc @@ -0,0 +1,187 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "SurveyComplexItemTest.h" +#include "QGCApplication.h" + +SurveyComplexItemTest::SurveyComplexItemTest(void) + : _offlineVehicle(NULL) +{ + _polyPoints << QGeoCoordinate(47.633550640000003, -122.08982199) << QGeoCoordinate(47.634129020000003, -122.08887249) << + QGeoCoordinate(47.633619320000001, -122.08811074) << QGeoCoordinate(47.633189139999999, -122.08900124); +} + +void SurveyComplexItemTest::init(void) +{ + UnitTest::init(); + + _rgSurveySignals[surveyVisualTransectPointsChangedIndex] = SIGNAL(visualTransectPointsChanged()); + _rgSurveySignals[surveyCameraShotsChangedIndex] = SIGNAL(cameraShotsChanged()); + _rgSurveySignals[surveyCoveredAreaChangedIndex] = SIGNAL(coveredAreaChanged()); + _rgSurveySignals[surveyTimeBetweenShotsChangedIndex] = SIGNAL(timeBetweenShotsChanged()); + _rgSurveySignals[surveyRefly90DegreesChangedIndex] = SIGNAL(refly90DegreesChanged(bool)); + _rgSurveySignals[surveyDirtyChangedIndex] = SIGNAL(dirtyChanged(bool)); + + _offlineVehicle = new Vehicle(MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, qgcApp()->toolbox()->firmwarePluginManager(), this); + _surveyItem = new SurveyComplexItem(_offlineVehicle, false /* flyView */, QString() /* kmlFile */, this /* parent */); + _surveyItem->turnAroundDistance()->setRawValue(0); // Unit test written for no turnaround distance + _surveyItem->setDirty(false); + _mapPolygon = _surveyItem->surveyAreaPolygon(); + + // It's important to check that the right signals are emitted at the right time since that drives ui change. + // It's also important to check that things are not being over-signalled when they should not be, since that can lead + // to incorrect ui or perf impact of uneeded signals propogating ui change. + + _multiSpy = new MultiSignalSpy(); + Q_CHECK_PTR(_multiSpy); + QCOMPARE(_multiSpy->init(_surveyItem, _rgSurveySignals, _cSurveySignals), true); +} + +void SurveyComplexItemTest::cleanup(void) +{ + delete _surveyItem; + delete _offlineVehicle; + delete _multiSpy; +} + +void SurveyComplexItemTest::_testDirty(void) +{ + QVERIFY(!_surveyItem->dirty()); + _surveyItem->setDirty(false); + QVERIFY(!_surveyItem->dirty()); + QVERIFY(_multiSpy->checkNoSignals()); + + _surveyItem->setDirty(true); + QVERIFY(_surveyItem->dirty()); + QVERIFY(_multiSpy->checkOnlySignalByMask(surveyDirtyChangedMask)); + QVERIFY(_multiSpy->pullBoolFromSignalIndex(surveyDirtyChangedIndex)); + _multiSpy->clearAllSignals(); + + _surveyItem->setDirty(false); + QVERIFY(!_surveyItem->dirty()); + QVERIFY(_multiSpy->checkOnlySignalByMask(surveyDirtyChangedMask)); + _multiSpy->clearAllSignals(); + + // These facts should set dirty when changed + QList rgFacts; + rgFacts << _surveyItem->gridAngle() << _surveyItem->flyAlternateTransects(); + foreach(Fact* fact, rgFacts) { + qDebug() << fact->name(); + QVERIFY(!_surveyItem->dirty()); + if (fact->typeIsBool()) { + fact->setRawValue(!fact->rawValue().toBool()); + } else { + fact->setRawValue(fact->rawValue().toDouble() + 1); + } + QVERIFY(_multiSpy->checkSignalByMask(surveyDirtyChangedMask)); + QVERIFY(_multiSpy->pullBoolFromSignalIndex(surveyDirtyChangedIndex)); + _surveyItem->setDirty(false); + _multiSpy->clearAllSignals(); + } + rgFacts.clear(); +} + +// Clamp expected grid angle from 0<->180. We don't care about opposite angles like 90/270 +double SurveyComplexItemTest::_clampGridAngle180(double gridAngle) +{ + if (gridAngle >= 0.0) { + if (gridAngle == 360.0) { + gridAngle = 0.0; + } else if (gridAngle >= 180.0) { + gridAngle -= 180.0; + } + } else { + if (gridAngle < -180.0) { + gridAngle += 360.0; + } else { + gridAngle += 180.0; + } + } + return gridAngle; +} + +void SurveyComplexItemTest::_setPolygon(void) +{ + for (int i=0; i<_polyPoints.count(); i++) { + QGeoCoordinate& vertex = _polyPoints[i]; + _mapPolygon->appendVertex(vertex); + } +} + +void SurveyComplexItemTest::_testGridAngle(void) +{ + _setPolygon(); + + for (double gridAngle=-360.0; gridAngle<=360.0; gridAngle++) { + _surveyItem->gridAngle()->setRawValue(gridAngle); + + QVariantList gridPoints = _surveyItem->visualTransectPoints(); + QGeoCoordinate firstTransectEntry = gridPoints[0].value(); + QGeoCoordinate firstTransectExit = gridPoints[1].value(); + double azimuth = firstTransectEntry.azimuthTo(firstTransectExit); + //qDebug() << gridAngle << azimuth << _clampGridAngle180(gridAngle) << _clampGridAngle180(azimuth); + QCOMPARE((int)_clampGridAngle180(gridAngle), (int)_clampGridAngle180(azimuth)); + } +} + +void SurveyComplexItemTest::_testEntryLocation(void) +{ + _setPolygon(); + + for (double gridAngle=-360.0; gridAngle<=360.0; gridAngle++) { + _surveyItem->gridAngle()->setRawValue(gridAngle); + + // Validate that each entry location is unique + QList rgSeenEntryCoords; + for (int rotateCount=0; rotateCount<3; rotateCount++) { + _surveyItem->rotateEntryPoint(); + QVERIFY(!rgSeenEntryCoords.contains(_surveyItem->coordinate())); + rgSeenEntryCoords << _surveyItem->coordinate(); + } + + _surveyItem->rotateEntryPoint(); // Rotate back for first entry point + rgSeenEntryCoords.clear(); + } +} + + +void SurveyComplexItemTest::_testItemCount(void) +{ + QList items; + + _setPolygon(); + + _surveyItem->hoverAndCapture()->setRawValue(false); + _surveyItem->cameraTriggerInTurnAround()->setRawValue(false); + _surveyItem->refly90Degrees()->setRawValue(false); + _surveyItem->appendMissionItems(items, this); + QCOMPARE(items.count() - 1, _surveyItem->lastSequenceNumber()); + items.clear(); + + _surveyItem->hoverAndCapture()->setRawValue(false); + _surveyItem->cameraTriggerInTurnAround()->setRawValue(true); + _surveyItem->refly90Degrees()->setRawValue(false); + _surveyItem->appendMissionItems(items, this); + QCOMPARE(items.count() - 1, _surveyItem->lastSequenceNumber()); + items.clear(); + + _surveyItem->hoverAndCapture()->setRawValue(true); + _surveyItem->cameraTriggerInTurnAround()->setRawValue(false); + _surveyItem->refly90Degrees()->setRawValue(false); + _surveyItem->appendMissionItems(items, this); + QCOMPARE(items.count() - 1, _surveyItem->lastSequenceNumber()); + items.clear(); + + _surveyItem->hoverAndCapture()->setRawValue(true); + _surveyItem->cameraTriggerInTurnAround()->setRawValue(false); + _surveyItem->refly90Degrees()->setRawValue(true); + _surveyItem->appendMissionItems(items, this); + QCOMPARE(items.count() - 1, _surveyItem->lastSequenceNumber()); + items.clear(); +} diff --git a/src/MissionManager/SurveyComplexItemTest.h b/src/MissionManager/SurveyComplexItemTest.h new file mode 100644 index 0000000000000000000000000000000000000000..847bc7381f075653a29c7fc6ed345d891059d02b --- /dev/null +++ b/src/MissionManager/SurveyComplexItemTest.h @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "UnitTest.h" +#include "TCPLink.h" +#include "MultiSignalSpy.h" +#include "SurveyComplexItem.h" + +#include + +/// Unit test for SurveyComplexItem +class SurveyComplexItemTest : public UnitTest +{ + Q_OBJECT + +public: + SurveyComplexItemTest(void); + +protected: + void init(void) final; + void cleanup(void) final; + +private slots: + void _testDirty(void); + void _testGridAngle(void); + void _testEntryLocation(void); + void _testItemCount(void); + +private: + + double _clampGridAngle180(double gridAngle); + void _setPolygon(void); + + // SurveyComplexItem signals + + enum { + surveyVisualTransectPointsChangedIndex = 0, + surveyCameraShotsChangedIndex, + surveyCoveredAreaChangedIndex, + surveyTimeBetweenShotsChangedIndex, + surveyRefly90DegreesChangedIndex, + surveyDirtyChangedIndex, + surveyMaxSignalIndex + }; + + enum { + surveyVisualTransectPointsChangedMask = 1 << surveyVisualTransectPointsChangedIndex, + surveyCameraShotsChangedMask = 1 << surveyCameraShotsChangedIndex, + surveyCoveredAreaChangedMask = 1 << surveyCoveredAreaChangedIndex, + surveyTimeBetweenShotsChangedMask = 1 << surveyTimeBetweenShotsChangedIndex, + surveyRefly90DegreesChangedMask = 1 << surveyRefly90DegreesChangedIndex, + surveyDirtyChangedMask = 1 << surveyDirtyChangedIndex + }; + + static const size_t _cSurveySignals = surveyMaxSignalIndex; + const char* _rgSurveySignals[_cSurveySignals]; + + Vehicle* _offlineVehicle; + MultiSignalSpy* _multiSpy; + SurveyComplexItem* _surveyItem; + QGCMapPolygon* _mapPolygon; + QList _polyPoints; +}; diff --git a/src/MissionManager/SurveyMissionItem.cc b/src/MissionManager/SurveyMissionItem.cc deleted file mode 100644 index 1e8b5e9f4590bde163effb055a5212090f76e4b0..0000000000000000000000000000000000000000 --- a/src/MissionManager/SurveyMissionItem.cc +++ /dev/null @@ -1,1309 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2016 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - - -#include "SurveyMissionItem.h" -#include "JsonHelper.h" -#include "MissionController.h" -#include "QGCGeo.h" -#include "QGroundControlQmlGlobal.h" -#include "QGCQGeoCoordinate.h" -#include "SettingsManager.h" -#include "AppSettings.h" - -#include - -QGC_LOGGING_CATEGORY(SurveyMissionItemLog, "SurveyMissionItemLog") - -const char* SurveyMissionItem::jsonComplexItemTypeValue = "survey"; - -const char* SurveyMissionItem::_jsonGridObjectKey = "grid"; -const char* SurveyMissionItem::_jsonGridAltitudeKey = "altitude"; -const char* SurveyMissionItem::_jsonGridAltitudeRelativeKey = "relativeAltitude"; -const char* SurveyMissionItem::_jsonGridAngleKey = "angle"; -const char* SurveyMissionItem::_jsonGridSpacingKey = "spacing"; -const char* SurveyMissionItem::_jsonGridEntryLocationKey = "entryLocation"; -const char* SurveyMissionItem::_jsonTurnaroundDistKey = "turnAroundDistance"; -const char* SurveyMissionItem::_jsonCameraTriggerDistanceKey = "cameraTriggerDistance"; -const char* SurveyMissionItem::_jsonCameraTriggerInTurnaroundKey = "cameraTriggerInTurnaround"; -const char* SurveyMissionItem::_jsonHoverAndCaptureKey = "hoverAndCapture"; -const char* SurveyMissionItem::_jsonGroundResolutionKey = "groundResolution"; -const char* SurveyMissionItem::_jsonFrontalOverlapKey = "imageFrontalOverlap"; -const char* SurveyMissionItem::_jsonSideOverlapKey = "imageSideOverlap"; -const char* SurveyMissionItem::_jsonCameraSensorWidthKey = "sensorWidth"; -const char* SurveyMissionItem::_jsonCameraSensorHeightKey = "sensorHeight"; -const char* SurveyMissionItem::_jsonCameraResolutionWidthKey = "resolutionWidth"; -const char* SurveyMissionItem::_jsonCameraResolutionHeightKey = "resolutionHeight"; -const char* SurveyMissionItem::_jsonCameraFocalLengthKey = "focalLength"; -const char* SurveyMissionItem::_jsonCameraMinTriggerIntervalKey = "minTriggerInterval"; -const char* SurveyMissionItem::_jsonCameraObjectKey = "camera"; -const char* SurveyMissionItem::_jsonCameraNameKey = "name"; -const char* SurveyMissionItem::_jsonManualGridKey = "manualGrid"; -const char* SurveyMissionItem::_jsonCameraOrientationLandscapeKey = "orientationLandscape"; -const char* SurveyMissionItem::_jsonFixedValueIsAltitudeKey = "fixedValueIsAltitude"; -const char* SurveyMissionItem::_jsonRefly90DegreesKey = "refly90Degrees"; - -const char* SurveyMissionItem::settingsGroup = "Survey"; -const char* SurveyMissionItem::manualGridName = "ManualGrid"; -const char* SurveyMissionItem::gridAltitudeName = "GridAltitude"; -const char* SurveyMissionItem::gridAltitudeRelativeName = "GridAltitudeRelative"; -const char* SurveyMissionItem::gridAngleName = "GridAngle"; -const char* SurveyMissionItem::gridSpacingName = "GridSpacing"; -const char* SurveyMissionItem::gridEntryLocationName = "GridEntryLocation"; -const char* SurveyMissionItem::turnaroundDistName = "TurnaroundDist"; -const char* SurveyMissionItem::cameraTriggerDistanceName = "CameraTriggerDistance"; -const char* SurveyMissionItem::cameraTriggerInTurnaroundName = "CameraTriggerInTurnaround"; -const char* SurveyMissionItem::hoverAndCaptureName = "HoverAndCapture"; -const char* SurveyMissionItem::groundResolutionName = "GroundResolution"; -const char* SurveyMissionItem::frontalOverlapName = "FrontalOverlap"; -const char* SurveyMissionItem::sideOverlapName = "SideOverlap"; -const char* SurveyMissionItem::cameraSensorWidthName = "CameraSensorWidth"; -const char* SurveyMissionItem::cameraSensorHeightName = "CameraSensorHeight"; -const char* SurveyMissionItem::cameraResolutionWidthName = "CameraResolutionWidth"; -const char* SurveyMissionItem::cameraResolutionHeightName = "CameraResolutionHeight"; -const char* SurveyMissionItem::cameraFocalLengthName = "CameraFocalLength"; -const char* SurveyMissionItem::cameraTriggerName = "CameraTrigger"; -const char* SurveyMissionItem::cameraOrientationLandscapeName = "CameraOrientationLandscape"; -const char* SurveyMissionItem::fixedValueIsAltitudeName = "FixedValueIsAltitude"; -const char* SurveyMissionItem::cameraName = "Camera"; - -SurveyMissionItem::SurveyMissionItem(Vehicle* vehicle, QObject* parent) - : ComplexMissionItem(vehicle, parent) - , _sequenceNumber(0) - , _dirty(false) - , _mapPolygon(this) - , _cameraOrientationFixed(false) - , _missionCommandCount(0) - , _refly90Degrees(false) - , _additionalFlightDelaySeconds(0) - , _cameraMinTriggerInterval(0) - , _ignoreRecalc(false) - , _surveyDistance(0.0) - , _cameraShots(0) - , _coveredArea(0.0) - , _timeBetweenShots(0.0) - , _metaDataMap(FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/Survey.SettingsGroup.json"), this)) - , _manualGridFact (settingsGroup, _metaDataMap[manualGridName]) - , _gridAltitudeFact (settingsGroup, _metaDataMap[gridAltitudeName]) - , _gridAltitudeRelativeFact (settingsGroup, _metaDataMap[gridAltitudeRelativeName]) - , _gridAngleFact (settingsGroup, _metaDataMap[gridAngleName]) - , _gridSpacingFact (settingsGroup, _metaDataMap[gridSpacingName]) - , _gridEntryLocationFact (settingsGroup, _metaDataMap[gridEntryLocationName]) - , _turnaroundDistFact (settingsGroup, _metaDataMap[turnaroundDistName]) - , _cameraTriggerDistanceFact (settingsGroup, _metaDataMap[cameraTriggerDistanceName]) - , _cameraTriggerInTurnaroundFact (settingsGroup, _metaDataMap[cameraTriggerInTurnaroundName]) - , _hoverAndCaptureFact (settingsGroup, _metaDataMap[hoverAndCaptureName]) - , _groundResolutionFact (settingsGroup, _metaDataMap[groundResolutionName]) - , _frontalOverlapFact (settingsGroup, _metaDataMap[frontalOverlapName]) - , _sideOverlapFact (settingsGroup, _metaDataMap[sideOverlapName]) - , _cameraSensorWidthFact (settingsGroup, _metaDataMap[cameraSensorWidthName]) - , _cameraSensorHeightFact (settingsGroup, _metaDataMap[cameraSensorHeightName]) - , _cameraResolutionWidthFact (settingsGroup, _metaDataMap[cameraResolutionWidthName]) - , _cameraResolutionHeightFact (settingsGroup, _metaDataMap[cameraResolutionHeightName]) - , _cameraFocalLengthFact (settingsGroup, _metaDataMap[cameraFocalLengthName]) - , _cameraOrientationLandscapeFact (settingsGroup, _metaDataMap[cameraOrientationLandscapeName]) - , _fixedValueIsAltitudeFact (settingsGroup, _metaDataMap[fixedValueIsAltitudeName]) - , _cameraFact (settingsGroup, _metaDataMap[cameraName]) -{ - _editorQml = "qrc:/qml/SurveyItemEditor.qml"; - - // If the user hasn't changed turnaround from the default (which is a fixed wing default) and we are multi-rotor set the multi-rotor default. - // NULL check since object creation during unit testing passes NULL for vehicle - if (_vehicle && _vehicle->multiRotor() && _turnaroundDistFact.rawValue().toDouble() == _turnaroundDistFact.rawDefaultValue().toDouble()) { - // Note this is set to 10 meters to work around a problem with PX4 Pro turnaround behavior. Don't change unless firmware gets better as well. - _turnaroundDistFact.setRawValue(10); - } - - // We override the grid altitude to the mission default - if (_manualGridFact.rawValue().toBool() || _fixedValueIsAltitudeFact.rawValue().toBool()) { - _gridAltitudeFact.setRawValue(qgcApp()->toolbox()->settingsManager()->appSettings()->defaultMissionItemAltitude()->rawValue()); - } - - connect(&_gridSpacingFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); - connect(&_gridAngleFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); - connect(&_gridEntryLocationFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); - connect(&_turnaroundDistFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); - connect(&_cameraTriggerDistanceFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); - connect(&_cameraTriggerInTurnaroundFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); - connect(&_hoverAndCaptureFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); - connect(this, &SurveyMissionItem::refly90DegreesChanged, this, &SurveyMissionItem::_generateGrid); - - connect(&_gridAltitudeFact, &Fact::valueChanged, this, &SurveyMissionItem::_updateCoordinateAltitude); - - connect(&_gridAltitudeRelativeFact, &Fact::valueChanged, this, &SurveyMissionItem::_setDirty); - - // Signal to Qml when camera value changes so it can recalc - connect(&_groundResolutionFact, &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged); - connect(&_frontalOverlapFact, &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged); - connect(&_sideOverlapFact, &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged); - connect(&_cameraSensorWidthFact, &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged); - connect(&_cameraSensorHeightFact, &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged); - connect(&_cameraResolutionWidthFact, &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged); - connect(&_cameraResolutionHeightFact, &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged); - connect(&_cameraFocalLengthFact, &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged); - connect(&_cameraOrientationLandscapeFact, &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged); - - connect(&_cameraTriggerDistanceFact, &Fact::valueChanged, this, &SurveyMissionItem::timeBetweenShotsChanged); - - connect(&_mapPolygon, &QGCMapPolygon::dirtyChanged, this, &SurveyMissionItem::_polygonDirtyChanged); - connect(&_mapPolygon, &QGCMapPolygon::pathChanged, this, &SurveyMissionItem::_generateGrid); -} - -void SurveyMissionItem::_setSurveyDistance(double surveyDistance) -{ - if (!qFuzzyCompare(_surveyDistance, surveyDistance)) { - _surveyDistance = surveyDistance; - emit complexDistanceChanged(); - } -} - -void SurveyMissionItem::_setCameraShots(int cameraShots) -{ - if (_cameraShots != cameraShots) { - _cameraShots = cameraShots; - emit cameraShotsChanged(this->cameraShots()); - } -} - -void SurveyMissionItem::_setCoveredArea(double coveredArea) -{ - if (!qFuzzyCompare(_coveredArea, coveredArea)) { - _coveredArea = coveredArea; - emit coveredAreaChanged(_coveredArea); - } -} - -void SurveyMissionItem::_clearInternal(void) -{ - // Bug workaround - while (_simpleGridPoints.count() > 1) { - _simpleGridPoints.takeLast(); - } - emit gridPointsChanged(); - _simpleGridPoints.clear(); - _transectSegments.clear(); - - _missionCommandCount = 0; - - setDirty(true); - - emit specifiesCoordinateChanged(); - emit lastSequenceNumberChanged(lastSequenceNumber()); -} - -int SurveyMissionItem::lastSequenceNumber(void) const -{ - return _sequenceNumber + _missionCommandCount; -} - -void SurveyMissionItem::setCoordinate(const QGeoCoordinate& coordinate) -{ - if (_coordinate != coordinate) { - _coordinate = coordinate; - emit coordinateChanged(_coordinate); - } -} - -void SurveyMissionItem::setDirty(bool dirty) -{ - if (_dirty != dirty) { - _dirty = dirty; - emit dirtyChanged(_dirty); - } -} - -void SurveyMissionItem::save(QJsonArray& missionItems) -{ - QJsonObject saveObject; - - saveObject[JsonHelper::jsonVersionKey] = 3; - saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; - saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; - saveObject[_jsonManualGridKey] = _manualGridFact.rawValue().toBool(); - saveObject[_jsonFixedValueIsAltitudeKey] = _fixedValueIsAltitudeFact.rawValue().toBool(); - saveObject[_jsonHoverAndCaptureKey] = _hoverAndCaptureFact.rawValue().toBool(); - saveObject[_jsonRefly90DegreesKey] = _refly90Degrees; - saveObject[_jsonCameraTriggerDistanceKey] = _cameraTriggerDistanceFact.rawValue().toDouble(); - saveObject[_jsonCameraTriggerInTurnaroundKey] = _cameraTriggerInTurnaroundFact.rawValue().toBool(); - - QJsonObject gridObject; - gridObject[_jsonGridAltitudeKey] = _gridAltitudeFact.rawValue().toDouble(); - gridObject[_jsonGridAltitudeRelativeKey] = _gridAltitudeRelativeFact.rawValue().toBool(); - gridObject[_jsonGridAngleKey] = _gridAngleFact.rawValue().toDouble(); - gridObject[_jsonGridSpacingKey] = _gridSpacingFact.rawValue().toDouble(); - gridObject[_jsonGridEntryLocationKey] = _gridEntryLocationFact.rawValue().toDouble(); - gridObject[_jsonTurnaroundDistKey] = _turnaroundDistFact.rawValue().toDouble(); - - saveObject[_jsonGridObjectKey] = gridObject; - - if (!_manualGridFact.rawValue().toBool()) { - QJsonObject cameraObject; - cameraObject[_jsonCameraNameKey] = _cameraFact.rawValue().toString(); - cameraObject[_jsonCameraOrientationLandscapeKey] = _cameraOrientationLandscapeFact.rawValue().toBool(); - cameraObject[_jsonCameraSensorWidthKey] = _cameraSensorWidthFact.rawValue().toDouble(); - cameraObject[_jsonCameraSensorHeightKey] = _cameraSensorHeightFact.rawValue().toDouble(); - cameraObject[_jsonCameraResolutionWidthKey] = _cameraResolutionWidthFact.rawValue().toDouble(); - cameraObject[_jsonCameraResolutionHeightKey] = _cameraResolutionHeightFact.rawValue().toDouble(); - cameraObject[_jsonCameraFocalLengthKey] = _cameraFocalLengthFact.rawValue().toDouble(); - cameraObject[_jsonCameraMinTriggerIntervalKey] = _cameraMinTriggerInterval; - cameraObject[_jsonGroundResolutionKey] = _groundResolutionFact.rawValue().toDouble(); - cameraObject[_jsonFrontalOverlapKey] = _frontalOverlapFact.rawValue().toInt(); - cameraObject[_jsonSideOverlapKey] = _sideOverlapFact.rawValue().toInt(); - - saveObject[_jsonCameraObjectKey] = cameraObject; - } - - // Polygon shape - _mapPolygon.saveToJson(saveObject); - - missionItems.append(saveObject); -} - -void SurveyMissionItem::setSequenceNumber(int sequenceNumber) -{ - if (_sequenceNumber != sequenceNumber) { - _sequenceNumber = sequenceNumber; - emit sequenceNumberChanged(sequenceNumber); - emit lastSequenceNumberChanged(lastSequenceNumber()); - } -} - -bool SurveyMissionItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString) -{ - QJsonObject v2Object = complexObject; - - // We need to pull version first to determine what validation/conversion needs to be performed. - QList versionKeyInfoList = { - { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, - }; - if (!JsonHelper::validateKeys(v2Object, versionKeyInfoList, errorString)) { - return false; - } - - int version = v2Object[JsonHelper::jsonVersionKey].toInt(); - if (version != 2 && version != 3) { - errorString = tr("%1 does not support this version of survey items").arg(qgcApp()->applicationName()); - return false; - } - if (version == 2) { - // Convert to v3 - if (v2Object.contains(VisualMissionItem::jsonTypeKey) && v2Object[VisualMissionItem::jsonTypeKey].toString() == QStringLiteral("survey")) { - v2Object[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; - v2Object[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; - } - } - - QList mainKeyInfoList = { - { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, - { VisualMissionItem::jsonTypeKey, QJsonValue::String, true }, - { ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true }, - { QGCMapPolygon::jsonPolygonKey, QJsonValue::Array, true }, - { _jsonGridObjectKey, QJsonValue::Object, true }, - { _jsonCameraObjectKey, QJsonValue::Object, false }, - { _jsonCameraTriggerDistanceKey, QJsonValue::Double, true }, - { _jsonManualGridKey, QJsonValue::Bool, true }, - { _jsonFixedValueIsAltitudeKey, QJsonValue::Bool, true }, - { _jsonHoverAndCaptureKey, QJsonValue::Bool, false }, - { _jsonRefly90DegreesKey, QJsonValue::Bool, false }, - { _jsonCameraTriggerInTurnaroundKey, QJsonValue::Bool, false }, // Should really be required, but it was missing from initial code due to bug - }; - if (!JsonHelper::validateKeys(v2Object, mainKeyInfoList, errorString)) { - return false; - } - - QString itemType = v2Object[VisualMissionItem::jsonTypeKey].toString(); - QString complexType = v2Object[ComplexMissionItem::jsonComplexItemTypeKey].toString(); - if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonComplexItemTypeValue) { - errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType); - return false; - } - - _ignoreRecalc = true; - - _mapPolygon.clear(); - - setSequenceNumber(sequenceNumber); - - _manualGridFact.setRawValue (v2Object[_jsonManualGridKey].toBool(true)); - _fixedValueIsAltitudeFact.setRawValue (v2Object[_jsonFixedValueIsAltitudeKey].toBool(true)); - _gridAltitudeRelativeFact.setRawValue (v2Object[_jsonGridAltitudeRelativeKey].toBool(true)); - _hoverAndCaptureFact.setRawValue (v2Object[_jsonHoverAndCaptureKey].toBool(false)); - _cameraTriggerInTurnaroundFact.setRawValue (v2Object[_jsonCameraTriggerInTurnaroundKey].toBool(true)); - - _refly90Degrees = v2Object[_jsonRefly90DegreesKey].toBool(false); - - QList gridKeyInfoList = { - { _jsonGridAltitudeKey, QJsonValue::Double, true }, - { _jsonGridAltitudeRelativeKey, QJsonValue::Bool, true }, - { _jsonGridAngleKey, QJsonValue::Double, true }, - { _jsonGridSpacingKey, QJsonValue::Double, true }, - { _jsonGridEntryLocationKey, QJsonValue::Double, false }, - { _jsonTurnaroundDistKey, QJsonValue::Double, true }, - }; - QJsonObject gridObject = v2Object[_jsonGridObjectKey].toObject(); - if (!JsonHelper::validateKeys(gridObject, gridKeyInfoList, errorString)) { - return false; - } - _gridAltitudeFact.setRawValue (gridObject[_jsonGridAltitudeKey].toDouble()); - _gridAngleFact.setRawValue (gridObject[_jsonGridAngleKey].toDouble()); - _gridSpacingFact.setRawValue (gridObject[_jsonGridSpacingKey].toDouble()); - _turnaroundDistFact.setRawValue (gridObject[_jsonTurnaroundDistKey].toDouble()); - _cameraTriggerDistanceFact.setRawValue (v2Object[_jsonCameraTriggerDistanceKey].toDouble()); - if (gridObject.contains(_jsonGridEntryLocationKey)) { - _gridEntryLocationFact.setRawValue(gridObject[_jsonGridEntryLocationKey].toDouble()); - } else { - _gridEntryLocationFact.setRawValue(_gridEntryLocationFact.rawDefaultValue()); - } - - if (!_manualGridFact.rawValue().toBool()) { - if (!v2Object.contains(_jsonCameraObjectKey)) { - errorString = tr("%1 but %2 object is missing").arg("manualGrid = false").arg("camera"); - return false; - } - - QJsonObject cameraObject = v2Object[_jsonCameraObjectKey].toObject(); - - // Older code had typo on "imageSideOverlap" incorrectly being "imageSizeOverlap" - QString incorrectImageSideOverlap = "imageSizeOverlap"; - if (cameraObject.contains(incorrectImageSideOverlap)) { - cameraObject[_jsonSideOverlapKey] = cameraObject[incorrectImageSideOverlap]; - cameraObject.remove(incorrectImageSideOverlap); - } - - QList cameraKeyInfoList = { - { _jsonGroundResolutionKey, QJsonValue::Double, true }, - { _jsonFrontalOverlapKey, QJsonValue::Double, true }, - { _jsonSideOverlapKey, QJsonValue::Double, true }, - { _jsonCameraSensorWidthKey, QJsonValue::Double, true }, - { _jsonCameraSensorHeightKey, QJsonValue::Double, true }, - { _jsonCameraResolutionWidthKey, QJsonValue::Double, true }, - { _jsonCameraResolutionHeightKey, QJsonValue::Double, true }, - { _jsonCameraFocalLengthKey, QJsonValue::Double, true }, - { _jsonCameraNameKey, QJsonValue::String, true }, - { _jsonCameraOrientationLandscapeKey, QJsonValue::Bool, true }, - { _jsonCameraMinTriggerIntervalKey, QJsonValue::Double, false }, - }; - if (!JsonHelper::validateKeys(cameraObject, cameraKeyInfoList, errorString)) { - return false; - } - - _cameraFact.setRawValue(cameraObject[_jsonCameraNameKey].toString()); - _cameraOrientationLandscapeFact.setRawValue(cameraObject[_jsonCameraOrientationLandscapeKey].toBool(true)); - - _groundResolutionFact.setRawValue (cameraObject[_jsonGroundResolutionKey].toDouble()); - _frontalOverlapFact.setRawValue (cameraObject[_jsonFrontalOverlapKey].toInt()); - _sideOverlapFact.setRawValue (cameraObject[_jsonSideOverlapKey].toInt()); - _cameraSensorWidthFact.setRawValue (cameraObject[_jsonCameraSensorWidthKey].toDouble()); - _cameraSensorHeightFact.setRawValue (cameraObject[_jsonCameraSensorHeightKey].toDouble()); - _cameraResolutionWidthFact.setRawValue (cameraObject[_jsonCameraResolutionWidthKey].toDouble()); - _cameraResolutionHeightFact.setRawValue (cameraObject[_jsonCameraResolutionHeightKey].toDouble()); - _cameraFocalLengthFact.setRawValue (cameraObject[_jsonCameraFocalLengthKey].toDouble()); - _cameraMinTriggerInterval = cameraObject[_jsonCameraMinTriggerIntervalKey].toDouble(0); - } - - // Polygon shape - /// Load a polygon from json - /// @param json Json object to load from - /// @param required true: no polygon in object will generate error - /// @param errorString Error string if return is false - /// @return true: success, false: failure (errorString set) - if (!_mapPolygon.loadFromJson(v2Object, true /* required */, errorString)) { - _mapPolygon.clear(); - return false; - } - - _ignoreRecalc = false; - _generateGrid(); - - return true; -} - -double SurveyMissionItem::greatestDistanceTo(const QGeoCoordinate &other) const -{ - double greatestDistance = 0.0; - for (int i=0; i<_simpleGridPoints.count(); i++) { - QGeoCoordinate currentCoord = _simpleGridPoints[i].value(); - double distance = currentCoord.distanceTo(other); - if (distance > greatestDistance) { - greatestDistance = distance; - } - } - return greatestDistance; -} - -void SurveyMissionItem::_setExitCoordinate(const QGeoCoordinate& coordinate) -{ - if (_exitCoordinate != coordinate) { - _exitCoordinate = coordinate; - emit exitCoordinateChanged(coordinate); - } -} - -bool SurveyMissionItem::specifiesCoordinate(void) const -{ - return _mapPolygon.count() > 2; -} - -void _calcCameraShots() -{ - -} - -void SurveyMissionItem::_convertTransectToGeo(const QList>& transectSegmentsNED, const QGeoCoordinate& tangentOrigin, QList>& transectSegmentsGeo) -{ - transectSegmentsGeo.clear(); - - for (int i=0; i transectCoords; - const QList& transectPoints = transectSegmentsNED[i]; - - for (int j=0; j>& transects) -{ - QList> rgReversedTransects; - for (int i=transects.count() - 1; i>=0; i--) { - rgReversedTransects.append(transects[i]); - } - transects = rgReversedTransects; -} - -/// Reverse the order of all points withing each transect, First point becomes last and so forth. -void SurveyMissionItem::_reverseInternalTransectPoints(QList>& transects) -{ - for (int i=0; i rgReversedCoords; - QList& rgOriginalCoords = transects[i]; - for (int j=rgOriginalCoords.count()-1; j>=0; j--) { - rgReversedCoords.append(rgOriginalCoords[j]); - } - transects[i] = rgReversedCoords; - } -} - -/// Reorders the transects such that the first transect is the shortest distance to the specified coordinate -/// and the first point within that transect is the shortest distance to the specified coordinate. -/// @param distanceCoord Coordinate to measure distance against -/// @param transects Transects to test and reorder -void SurveyMissionItem::_optimizeTransectsForShortestDistance(const QGeoCoordinate& distanceCoord, QList>& transects) -{ - double rgTransectDistance[4]; - rgTransectDistance[0] = transects.first().first().distanceTo(distanceCoord); - rgTransectDistance[1] = transects.first().last().distanceTo(distanceCoord); - rgTransectDistance[2] = transects.last().first().distanceTo(distanceCoord); - rgTransectDistance[3] = transects.last().last().distanceTo(distanceCoord); - - int shortestIndex = 0; - double shortestDistance = rgTransectDistance[0]; - for (int i=1; i<3; i++) { - if (rgTransectDistance[i] < shortestDistance) { - shortestIndex = i; - shortestDistance = rgTransectDistance[i]; - } - } - - if (shortestIndex > 1) { - // We need to reverse the order of segments - _reverseTransectOrder(transects); - } - if (shortestIndex & 1) { - // We need to reverse the points within each segment - _reverseInternalTransectPoints(transects); - } -} - -void SurveyMissionItem::_appendGridPointsFromTransects(QList>& rgTransectSegments) -{ - qCDebug(SurveyMissionItemLog) << "Entry point _appendGridPointsFromTransects" << rgTransectSegments.first().first(); - - for (int i=0; i& points, int index1, int index2) -{ - QPointF temp = points[index1]; - points[index1] = points[index2]; - points[index2] = temp; -} - -QList SurveyMissionItem::_convexPolygon(const QList& polygon) -{ - // We use the Graham scan algorithem to convert the possibly concave polygon to a convex polygon - // https://en.wikipedia.org/wiki/Graham_scan - - QList workPolygon(polygon); - - // First point must be lowest y-coordinate point - for (int i=1; i angle) { - _swapPoints(workPolygon, i, j); - angle = _dp(workPolygon[0], workPolygon[j]); - } - } - } - - // Perform the the Graham scan - - workPolygon.insert(0, workPolygon.last()); // Sentinel for algo stop - int convexCount = 1; // Number of points on the convex hull. - - for (int i=2; i<=polygon.count(); i++) { - while (_ccw(workPolygon[convexCount-1], workPolygon[convexCount], workPolygon[i]) <= 0) { - if (convexCount > 1) { - convexCount -= 1; - } else if (i == polygon.count()) { - break; - } else { - i++; - } - } - convexCount++; - _swapPoints(workPolygon, convexCount, i); - } - - return workPolygon.mid(1, convexCount); -} - -/// Returns true if the current grid angle generates north/south oriented transects -bool SurveyMissionItem::_gridAngleIsNorthSouthTransects() -{ - // Grid angle ranges from -360<->360 - double gridAngle = qAbs(_gridAngleFact.rawValue().toDouble()); - return gridAngle < 45.0 || (gridAngle > 360.0 - 45.0) || (gridAngle > 90.0 + 45.0 && gridAngle < 270.0 - 45.0); -} - -void SurveyMissionItem::_adjustTransectsToEntryPointLocation(QList>& transects) -{ - if (transects.count() == 0) { - return; - } - - int entryLocation = _gridEntryLocationFact.rawValue().toInt(); - bool reversePoints = false; - bool reverseTransects = false; - - if (entryLocation == EntryLocationBottomLeft || entryLocation == EntryLocationBottomRight) { - reversePoints = true; - } - if (entryLocation == EntryLocationTopRight || entryLocation == EntryLocationBottomRight) { - reverseTransects = true; - } - - if (reversePoints) { - qCDebug(SurveyMissionItemLog) << "Reverse Points"; - _reverseInternalTransectPoints(transects); - } - if (reverseTransects) { - qCDebug(SurveyMissionItemLog) << "Reverse Transects"; - _reverseTransectOrder(transects); - } - - qCDebug(SurveyMissionItemLog) << "Modified entry point" << transects.first().first(); -} - -int SurveyMissionItem::_calcMissionCommandCount(QList>& transectSegments) -{ - int missionCommandCount= 0; - for (int i=0; i& transectSegment = transectSegments[i]; - - missionCommandCount += transectSegment.count(); // This accounts for all waypoints - if (_hoverAndCaptureEnabled()) { - // Internal camera trigger points are entry point, plus all points before exit point - missionCommandCount += transectSegment.count() - (_hasTurnaround() ? 2 : 0) - 1; - } else if (_triggerCamera() && !_imagesEverywhere()) { - // Camera on/off at entry/exit of each transect - missionCommandCount += 2; - } - } - if (transectSegments.count() && _triggerCamera() && _imagesEverywhere()) { - // Camera on/off for entire survey - missionCommandCount += 2; - } - - return missionCommandCount; -} -void SurveyMissionItem::_generateGrid(void) -{ - if (_ignoreRecalc) { - return; - } - - if (_mapPolygon.count() < 3 || _gridSpacingFact.rawValue().toDouble() <= 0) { - _clearInternal(); - return; - } - - _simpleGridPoints.clear(); - _transectSegments.clear(); - _reflyTransectSegments.clear(); - _additionalFlightDelaySeconds = 0; - - QList polygonPoints; - QList> transectSegments; - - // Convert polygon to NED - QGeoCoordinate tangentOrigin = _mapPolygon.pathModel().value(0)->coordinate(); - qCDebug(SurveyMissionItemLog) << "Convert polygon to NED - tangentOrigin" << tangentOrigin; - for (int i=0; i<_mapPolygon.count(); i++) { - double y, x, down; - QGeoCoordinate vertex = _mapPolygon.pathModel().value(i)->coordinate(); - if (i == 0) { - // This avoids a nan calculation that comes out of convertGeoToNed - x = y = 0; - } else { - convertGeoToNed(vertex, tangentOrigin, &y, &x, &down); - } - polygonPoints += QPointF(x, y); - qCDebug(SurveyMissionItemLog) << "vertex:x:y" << vertex << polygonPoints.last().x() << polygonPoints.last().y(); - } - - polygonPoints = _convexPolygon(polygonPoints); - - double coveredArea = 0.0; - for (int i=0; i(); - QGeoCoordinate coord2 = _simpleGridPoints[i].value(); - surveyDistance += coord1.distanceTo(coord2); - } - _setSurveyDistance(surveyDistance); - - if (cameraShots == 0 && _triggerCamera()) { - cameraShots = (int)floor(surveyDistance / _triggerDistance()); - // Take into account immediate camera trigger at waypoint entry - cameraShots++; - } - _setCameraShots(cameraShots); - - if (_hoverAndCaptureEnabled()) { - _additionalFlightDelaySeconds = cameraShots * _hoverAndCaptureDelaySeconds; - } - emit additionalTimeDelayChanged(); - - emit gridPointsChanged(); - - // Determine command count for lastSequenceNumber - _missionCommandCount = _calcMissionCommandCount(_transectSegments); - _missionCommandCount += _calcMissionCommandCount(_reflyTransectSegments); - emit lastSequenceNumberChanged(lastSequenceNumber()); - - // Set exit coordinate - if (_simpleGridPoints.count()) { - QGeoCoordinate coordinate = _simpleGridPoints.first().value(); - coordinate.setAltitude(_gridAltitudeFact.rawValue().toDouble()); - setCoordinate(coordinate); - QGeoCoordinate exitCoordinate = _simpleGridPoints.last().value(); - exitCoordinate.setAltitude(_gridAltitudeFact.rawValue().toDouble()); - _setExitCoordinate(exitCoordinate); - } - - setDirty(true); -} - -void SurveyMissionItem::_updateCoordinateAltitude(void) -{ - _coordinate.setAltitude(_gridAltitudeFact.rawValue().toDouble()); - _exitCoordinate.setAltitude(_gridAltitudeFact.rawValue().toDouble()); - emit coordinateChanged(_coordinate); - emit exitCoordinateChanged(_exitCoordinate); - setDirty(true); -} - -QPointF SurveyMissionItem::_rotatePoint(const QPointF& point, const QPointF& origin, double angle) -{ - QPointF rotated; - double radians = (M_PI / 180.0) * -angle; - - rotated.setX(((point.x() - origin.x()) * cos(radians)) - ((point.y() - origin.y()) * sin(radians)) + origin.x()); - rotated.setY(((point.x() - origin.x()) * sin(radians)) + ((point.y() - origin.y()) * cos(radians)) + origin.y()); - - return rotated; -} - -void SurveyMissionItem::_intersectLinesWithRect(const QList& lineList, const QRectF& boundRect, QList& resultLines) -{ - QLineF topLine (boundRect.topLeft(), boundRect.topRight()); - QLineF bottomLine (boundRect.bottomLeft(), boundRect.bottomRight()); - QLineF leftLine (boundRect.topLeft(), boundRect.bottomLeft()); - QLineF rightLine (boundRect.topRight(), boundRect.bottomRight()); - - for (int i=0; i& lineList, const QPolygonF& polygon, QList& resultLines) -{ - resultLines.clear(); - for (int i=0; iP2 -void SurveyMissionItem::_adjustLineDirection(const QList& lineList, QList& resultLines) -{ - qreal firstAngle = 0; - for (int i=0; i 1.0) { - adjustedLine.setP1(line.p2()); - adjustedLine.setP2(line.p1()); - } else { - adjustedLine = line; - } - - resultLines += adjustedLine; - } -} - -double SurveyMissionItem::_clampGridAngle90(double gridAngle) -{ - // Clamp grid angle to -90<->90. This prevents transects from being rotated to a reversed order. - if (gridAngle > 90.0) { - gridAngle -= 180.0; - } else if (gridAngle < -90.0) { - gridAngle += 180; - } - return gridAngle; -} - -int SurveyMissionItem::_gridGenerator(const QList& polygonPoints, QList>& transectSegments, bool refly) -{ - int cameraShots = 0; - - double gridAngle = _gridAngleFact.rawValue().toDouble(); - double gridSpacing = _gridSpacingFact.rawValue().toDouble(); - - gridAngle = _clampGridAngle90(gridAngle); - gridAngle += refly ? 90 : 0; - qCDebug(SurveyMissionItemLog) << "Clamped grid angle" << gridAngle; - - qCDebug(SurveyMissionItemLog) << "SurveyMissionItem::_gridGenerator gridSpacing:gridAngle:refly" << gridSpacing << gridAngle << refly; - - transectSegments.clear(); - - // Convert polygon to bounding rect - - qCDebug(SurveyMissionItemLog) << "Polygon"; - QPolygonF polygon; - for (int i=0; i lineList; - - // Transects are generated to be as long as the largest width/height of the bounding rect plus some fudge factor. - // This way they will always be guaranteed to intersect with a polygon edge no matter what angle they are rotated to. - // They are initially generated with the transects flowing from west to east and then points within the transect north to south. - double maxWidth = qMax(boundingRect.width(), boundingRect.height()) + 2000.0; - double halfWidth = maxWidth / 2.0; - double transectX = boundingCenter.x() - halfWidth; - double transectXMax = transectX + maxWidth; - while (transectX < transectXMax) { - double transectYTop = boundingCenter.y() - halfWidth; - double transectYBottom = boundingCenter.y() + halfWidth; - - lineList += QLineF(_rotatePoint(QPointF(transectX, transectYTop), boundingCenter, gridAngle), _rotatePoint(QPointF(transectX, transectYBottom), boundingCenter, gridAngle)); - transectX += gridSpacing; - } - - // Now intersect the lines with the polygon - QList intersectLines; -#if 1 - _intersectLinesWithPolygon(lineList, polygon, intersectLines); -#else - // This is handy for debugging grid problems, not for release - intersectLines = lineList; -#endif - - // Less than two transects intersected with the polygon: - // Create a single transect which goes through the center of the polygon - // Intersect it with the polygon - if (intersectLines.count() < 2) { - _mapPolygon.center(); - QLineF firstLine = lineList.first(); - QPointF lineCenter = firstLine.pointAt(0.5); - QPointF centerOffset = boundingCenter - lineCenter; - firstLine.translate(centerOffset); - lineList.clear(); - lineList.append(firstLine); - intersectLines = lineList; - _intersectLinesWithPolygon(lineList, polygon, intersectLines); - } - - // Make sure all lines are going to same direction. Polygon intersection leads to line which - // can be in varied directions depending on the order of the intesecting sides. - QList resultLines; - _adjustLineDirection(intersectLines, resultLines); - - // Calc camera shots here if there are no images in turnaround - if (_triggerCamera() && !_imagesEverywhere()) { - for (int i=0; i transectPoints; - const QLineF& line = resultLines[i]; - - float turnaroundPosition = _turnaroundDistance() / line.length(); - - if (i & 1) { - transectLine = QLineF(line.p2(), line.p1()); - } else { - transectLine = QLineF(line.p1(), line.p2()); - } - - // Build the points along the transect - - if (_hasTurnaround()) { - transectPoints.append(transectLine.pointAt(-turnaroundPosition)); - } - - // Polygon entry point - transectPoints.append(transectLine.p1()); - - // For hover and capture we need points for each camera location - if (_triggerCamera() && _hoverAndCaptureEnabled()) { - if (_triggerDistance() < transectLine.length()) { - int innerPoints = floor(transectLine.length() / _triggerDistance()); - qCDebug(SurveyMissionItemLog) << "innerPoints" << innerPoints; - float transectPositionIncrement = _triggerDistance() / transectLine.length(); - for (int i=0; i& items, int seqNum, QGeoCoordinate& coord, CameraTriggerCode cameraTrigger, QObject* missionItemParent) -{ - double altitude = _gridAltitudeFact.rawValue().toDouble(); - bool altitudeRelative = _gridAltitudeRelativeFact.rawValue().toBool(); - - qCDebug(SurveyMissionItemLog) << "_appendWaypointToMission seq:trigger" << seqNum << (cameraTrigger != CameraTriggerNone); - - MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_NAV_WAYPOINT, - altitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, - cameraTrigger == CameraTriggerHoverAndCapture ? _hoverAndCaptureDelaySeconds : 0, // Hold time (delay for hover and capture to settle vehicle before image is taken) - 0.0, 0.0, - std::numeric_limits::quiet_NaN(), // Yaw unchanged - coord.latitude(), - coord.longitude(), - altitude, - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - - switch (cameraTrigger) { - case CameraTriggerOff: - case CameraTriggerOn: - item = new MissionItem(seqNum++, - MAV_CMD_DO_SET_CAM_TRIGG_DIST, - MAV_FRAME_MISSION, - cameraTrigger == CameraTriggerOn ? _triggerDistance() : 0, - 0, // shutter integration (ignore) - cameraTrigger == CameraTriggerOn ? 1 : 0, // trigger immediately when starting - 0, 0, 0, 0, // param 4-7 unused - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - break; - case CameraTriggerHoverAndCapture: - item = new MissionItem(seqNum++, - MAV_CMD_IMAGE_START_CAPTURE, - MAV_FRAME_MISSION, - 0, // Reserved (Set to 0) - 0, // Interval (none) - 1, // Take 1 photo - NAN, NAN, NAN, NAN, // param 4-7 reserved - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); -#if 0 - // This generates too many commands. Pulling out for now, to see if image quality is still high enough. - item = new MissionItem(seqNum++, - MAV_CMD_NAV_DELAY, - MAV_FRAME_MISSION, - 0.5, // Delay in seconds, give some time for image to be taken - -1, -1, -1, // No time - 0, 0, 0, // Param 5-7 unused - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); -#endif - default: - break; - } - - return seqNum; -} - -bool SurveyMissionItem::_nextTransectCoord(const QList& transectPoints, int pointIndex, QGeoCoordinate& coord) -{ - if (pointIndex > transectPoints.count()) { - qWarning() << "Bad grid generation"; - return false; - } - - coord = transectPoints[pointIndex]; - return true; -} - -/// Appends the mission items for the survey -/// @param items Mission items are appended to this list -/// @param missionItemParent Parent object for newly created MissionItem objects -/// @param seqNum[in,out] Sequence number to start from -/// @param hasRefly true: misison has a refly section -/// @param buildRefly: true: build the refly section, false: build the first section -/// @return false: Generation failed -bool SurveyMissionItem::_appendMissionItemsWorker(QList& items, QObject* missionItemParent, int& seqNum, bool hasRefly, bool buildRefly) -{ - bool firstWaypointTrigger = false; - - qCDebug(SurveyMissionItemLog) << QStringLiteral("hasTurnaround(%1) triggerCamera(%2) hoverAndCapture(%3) imagesEverywhere(%4) hasRefly(%5) buildRefly(%6) ").arg(_hasTurnaround()).arg(_triggerCamera()).arg(_hoverAndCaptureEnabled()).arg(_imagesEverywhere()).arg(hasRefly).arg(buildRefly); - - QList>& transectSegments = buildRefly ? _reflyTransectSegments : _transectSegments; - - if (!buildRefly && _imagesEverywhere()) { - firstWaypointTrigger = true; - } - - for (int segmentIndex=0; segmentIndex& segment = transectSegments[segmentIndex]; - - qCDebug(SurveyMissionItemLog) << "segment.count" << segment.count(); - - if (_hasTurnaround()) { - // Add entry turnaround point - if (!_nextTransectCoord(segment, pointIndex++, coord)) { - return false; - } - seqNum = _appendWaypointToMission(items, seqNum, coord, firstWaypointTrigger ? CameraTriggerOn : CameraTriggerNone, missionItemParent); - firstWaypointTrigger = false; - } - - // Add polygon entry point - if (!_nextTransectCoord(segment, pointIndex++, coord)) { - return false; - } - if (firstWaypointTrigger) { - cameraTrigger = CameraTriggerOn; - } else { - cameraTrigger = _imagesEverywhere() || !_triggerCamera() ? CameraTriggerNone : (_hoverAndCaptureEnabled() ? CameraTriggerHoverAndCapture : CameraTriggerOn); - } - seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent); - firstWaypointTrigger = false; - - // Add internal hover and capture points - if (_hoverAndCaptureEnabled()) { - int lastHoverAndCaptureIndex = segment.count() - 1 - (_hasTurnaround() ? 1 : 0); - qCDebug(SurveyMissionItemLog) << "lastHoverAndCaptureIndex" << lastHoverAndCaptureIndex; - for (; pointIndex < lastHoverAndCaptureIndex; pointIndex++) { - if (!_nextTransectCoord(segment, pointIndex, coord)) { - return false; - } - seqNum = _appendWaypointToMission(items, seqNum, coord, CameraTriggerHoverAndCapture, missionItemParent); - } - } - - // Add polygon exit point - if (!_nextTransectCoord(segment, pointIndex++, coord)) { - return false; - } - cameraTrigger = _imagesEverywhere() || !_triggerCamera() ? CameraTriggerNone : (_hoverAndCaptureEnabled() ? CameraTriggerNone : CameraTriggerOff); - seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent); - - if (_hasTurnaround()) { - // Add exit turnaround point - if (!_nextTransectCoord(segment, pointIndex++, coord)) { - return false; - } - seqNum = _appendWaypointToMission(items, seqNum, coord, CameraTriggerNone, missionItemParent); - } - - qCDebug(SurveyMissionItemLog) << "last PointIndex" << pointIndex; - } - - if (((hasRefly && buildRefly) || !hasRefly) && _imagesEverywhere()) { - // Turn off camera at end of survey - MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_DO_SET_CAM_TRIGG_DIST, - MAV_FRAME_MISSION, - 0.0, // trigger distance (off) - 0, 0, 0, 0, 0, 0, // param 2-7 unused - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - } - - return true; -} - -void SurveyMissionItem::appendMissionItems(QList& items, QObject* missionItemParent) -{ - int seqNum = _sequenceNumber; - - if (!_appendMissionItemsWorker(items, missionItemParent, seqNum, _refly90Degrees, false /* buildRefly */)) { - return; - } - - if (_refly90Degrees) { - _appendMissionItemsWorker(items, missionItemParent, seqNum, _refly90Degrees, true /* buildRefly */); - } -} - -int SurveyMissionItem::cameraShots(void) const -{ - return _triggerCamera() ? _cameraShots : 0; -} - -void SurveyMissionItem::_cameraValueChanged(void) -{ - emit cameraValueChanged(); -} - -double SurveyMissionItem::timeBetweenShots(void) const -{ - return _cruiseSpeed == 0 ? 0 : _triggerDistance() / _cruiseSpeed; -} - -void SurveyMissionItem::setMissionFlightStatus(MissionController::MissionFlightStatus_t& missionFlightStatus) -{ - ComplexMissionItem::setMissionFlightStatus(missionFlightStatus); - if (!qFuzzyCompare(_cruiseSpeed, missionFlightStatus.vehicleSpeed)) { - _cruiseSpeed = missionFlightStatus.vehicleSpeed; - emit timeBetweenShotsChanged(); - } -} - -void SurveyMissionItem::_setDirty(void) -{ - setDirty(true); -} - -bool SurveyMissionItem::hoverAndCaptureAllowed(void) const -{ - return _vehicle->multiRotor() || _vehicle->vtol(); -} - -double SurveyMissionItem::_triggerDistance(void) const { - return _cameraTriggerDistanceFact.rawValue().toDouble(); -} - -bool SurveyMissionItem::_triggerCamera(void) const -{ - return _triggerDistance() > 0; -} - -bool SurveyMissionItem::_imagesEverywhere(void) const -{ - return _triggerCamera() && _cameraTriggerInTurnaroundFact.rawValue().toBool(); -} - -bool SurveyMissionItem::_hoverAndCaptureEnabled(void) const -{ - return hoverAndCaptureAllowed() && !_imagesEverywhere() && _triggerCamera() && _hoverAndCaptureFact.rawValue().toBool(); -} - -bool SurveyMissionItem::_hasTurnaround(void) const -{ - return _turnaroundDistance() > 0; -} - -double SurveyMissionItem::_turnaroundDistance(void) const -{ - return _turnaroundDistFact.rawValue().toDouble(); -} - -void SurveyMissionItem::applyNewAltitude(double newAltitude) -{ - _fixedValueIsAltitudeFact.setRawValue(true); - _gridAltitudeFact.setRawValue(newAltitude); -} - -void SurveyMissionItem::setRefly90Degrees(bool refly90Degrees) -{ - if (refly90Degrees != _refly90Degrees) { - _refly90Degrees = refly90Degrees; - emit refly90DegreesChanged(refly90Degrees); - } -} - -void SurveyMissionItem::_polygonDirtyChanged(bool dirty) -{ - if (dirty) { - setDirty(true); - } -} diff --git a/src/MissionManager/SurveyMissionItem.h b/src/MissionManager/SurveyMissionItem.h deleted file mode 100644 index d2096ea1fe2d3a86b4bbacd0c3e575ca05d15492..0000000000000000000000000000000000000000 --- a/src/MissionManager/SurveyMissionItem.h +++ /dev/null @@ -1,302 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2016 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - - -#ifndef SurveyMissionItem_H -#define SurveyMissionItem_H - -#include "ComplexMissionItem.h" -#include "MissionItem.h" -#include "SettingsFact.h" -#include "QGCLoggingCategory.h" -#include "QGCMapPolygon.h" - -Q_DECLARE_LOGGING_CATEGORY(SurveyMissionItemLog) - -class SurveyMissionItem : public ComplexMissionItem -{ - Q_OBJECT - -public: - SurveyMissionItem(Vehicle* vehicle, QObject* parent = NULL); - - Q_PROPERTY(Fact* gridAltitude READ gridAltitude CONSTANT) - Q_PROPERTY(Fact* gridAltitudeRelative READ gridAltitudeRelative CONSTANT) - Q_PROPERTY(Fact* gridAngle READ gridAngle CONSTANT) - Q_PROPERTY(Fact* gridSpacing READ gridSpacing CONSTANT) - Q_PROPERTY(Fact* gridEntryLocation READ gridEntryLocation CONSTANT) - Q_PROPERTY(Fact* turnaroundDist READ turnaroundDist CONSTANT) - Q_PROPERTY(Fact* cameraTriggerDistance READ cameraTriggerDistance CONSTANT) - Q_PROPERTY(Fact* cameraTriggerInTurnaround READ cameraTriggerInTurnaround CONSTANT) - Q_PROPERTY(Fact* hoverAndCapture READ hoverAndCapture CONSTANT) - Q_PROPERTY(Fact* groundResolution READ groundResolution CONSTANT) - Q_PROPERTY(Fact* frontalOverlap READ frontalOverlap CONSTANT) - Q_PROPERTY(Fact* sideOverlap READ sideOverlap CONSTANT) - Q_PROPERTY(Fact* cameraSensorWidth READ cameraSensorWidth CONSTANT) - Q_PROPERTY(Fact* cameraSensorHeight READ cameraSensorHeight CONSTANT) - Q_PROPERTY(Fact* cameraResolutionWidth READ cameraResolutionWidth CONSTANT) - Q_PROPERTY(Fact* cameraResolutionHeight READ cameraResolutionHeight CONSTANT) - Q_PROPERTY(Fact* cameraFocalLength READ cameraFocalLength CONSTANT) - Q_PROPERTY(Fact* cameraOrientationLandscape READ cameraOrientationLandscape CONSTANT) - Q_PROPERTY(Fact* fixedValueIsAltitude READ fixedValueIsAltitude CONSTANT) - Q_PROPERTY(Fact* manualGrid READ manualGrid CONSTANT) - Q_PROPERTY(Fact* camera READ camera CONSTANT) - - Q_PROPERTY(bool cameraOrientationFixed MEMBER _cameraOrientationFixed NOTIFY cameraOrientationFixedChanged) - Q_PROPERTY(bool hoverAndCaptureAllowed READ hoverAndCaptureAllowed CONSTANT) - Q_PROPERTY(bool refly90Degrees READ refly90Degrees WRITE setRefly90Degrees NOTIFY refly90DegreesChanged) - Q_PROPERTY(double cameraMinTriggerInterval MEMBER _cameraMinTriggerInterval NOTIFY cameraMinTriggerIntervalChanged) - - Q_PROPERTY(double timeBetweenShots READ timeBetweenShots NOTIFY timeBetweenShotsChanged) - Q_PROPERTY(QVariantList gridPoints READ gridPoints NOTIFY gridPointsChanged) - Q_PROPERTY(int cameraShots READ cameraShots NOTIFY cameraShotsChanged) - Q_PROPERTY(double coveredArea READ coveredArea NOTIFY coveredAreaChanged) - - Q_PROPERTY(QGCMapPolygon* mapPolygon READ mapPolygon CONSTANT) - - QVariantList gridPoints (void) { return _simpleGridPoints; } - - Fact* manualGrid (void) { return &_manualGridFact; } - Fact* gridAltitude (void) { return &_gridAltitudeFact; } - Fact* gridAltitudeRelative (void) { return &_gridAltitudeRelativeFact; } - Fact* gridAngle (void) { return &_gridAngleFact; } - Fact* gridSpacing (void) { return &_gridSpacingFact; } - Fact* gridEntryLocation (void) { return &_gridEntryLocationFact; } - Fact* turnaroundDist (void) { return &_turnaroundDistFact; } - Fact* cameraTriggerDistance (void) { return &_cameraTriggerDistanceFact; } - Fact* cameraTriggerInTurnaround (void) { return &_cameraTriggerInTurnaroundFact; } - Fact* hoverAndCapture (void) { return &_hoverAndCaptureFact; } - Fact* groundResolution (void) { return &_groundResolutionFact; } - Fact* frontalOverlap (void) { return &_frontalOverlapFact; } - Fact* sideOverlap (void) { return &_sideOverlapFact; } - Fact* cameraSensorWidth (void) { return &_cameraSensorWidthFact; } - Fact* cameraSensorHeight (void) { return &_cameraSensorHeightFact; } - Fact* cameraResolutionWidth (void) { return &_cameraResolutionWidthFact; } - Fact* cameraResolutionHeight (void) { return &_cameraResolutionHeightFact; } - Fact* cameraFocalLength (void) { return &_cameraFocalLengthFact; } - Fact* cameraOrientationLandscape(void) { return &_cameraOrientationLandscapeFact; } - Fact* fixedValueIsAltitude (void) { return &_fixedValueIsAltitudeFact; } - Fact* camera (void) { return &_cameraFact; } - - int cameraShots (void) const; - double coveredArea (void) const { return _coveredArea; } - double timeBetweenShots (void) const; - bool hoverAndCaptureAllowed (void) const; - bool refly90Degrees (void) const { return _refly90Degrees; } - QGCMapPolygon* mapPolygon (void) { return &_mapPolygon; } - - void setRefly90Degrees(bool refly90Degrees); - - // Overrides from ComplexMissionItem - - double complexDistance (void) const final { return _surveyDistance; } - double additionalTimeDelay (void) const final { return _additionalFlightDelaySeconds; } - int lastSequenceNumber (void) const final; - bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final; - double greatestDistanceTo (const QGeoCoordinate &other) const final; - QString mapVisualQML (void) const final { return QStringLiteral("SurveyMapVisual.qml"); } - - // Overrides from VisualMissionItem - - bool dirty (void) const final { return _dirty; } - bool isSimpleItem (void) const final { return false; } - bool isStandaloneCoordinate (void) const final { return false; } - bool specifiesCoordinate (void) const final; - bool specifiesAltitudeOnly (void) const final { return false; } - QString commandDescription (void) const final { return "Survey"; } - QString commandName (void) const final { return "Survey"; } - QString abbreviation (void) const final { return "S"; } - QGeoCoordinate coordinate (void) const final { return _coordinate; } - QGeoCoordinate exitCoordinate (void) const final { return _exitCoordinate; } - int sequenceNumber (void) const final { return _sequenceNumber; } - double specifiedFlightSpeed (void) final { return std::numeric_limits::quiet_NaN(); } - double specifiedGimbalYaw (void) final { return std::numeric_limits::quiet_NaN(); } - double specifiedGimbalPitch (void) final { return std::numeric_limits::quiet_NaN(); } - void appendMissionItems (QList& items, QObject* missionItemParent) final; - void setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final; - void applyNewAltitude (double newAltitude) final; - - bool coordinateHasRelativeAltitude (void) const final { return _gridAltitudeRelativeFact.rawValue().toBool(); } - bool exitCoordinateHasRelativeAltitude (void) const final { return _gridAltitudeRelativeFact.rawValue().toBool(); } - bool exitCoordinateSameAsEntry (void) const final { return false; } - - void setDirty (bool dirty) final; - void setCoordinate (const QGeoCoordinate& coordinate) final; - void setSequenceNumber (int sequenceNumber) final; - void setTurnaroundDist (double dist) { _turnaroundDistFact.setRawValue(dist); } - void save (QJsonArray& missionItems) final; - - // Must match json spec for GridEntryLocation - enum EntryLocation { - EntryLocationTopLeft, - EntryLocationTopRight, - EntryLocationBottomLeft, - EntryLocationBottomRight, - }; - - static const char* jsonComplexItemTypeValue; - - static const char* settingsGroup; - static const char* manualGridName; - static const char* gridAltitudeName; - static const char* gridAltitudeRelativeName; - static const char* gridAngleName; - static const char* gridSpacingName; - static const char* gridEntryLocationName; - static const char* turnaroundDistName; - static const char* cameraTriggerDistanceName; - static const char* cameraTriggerInTurnaroundName; - static const char* hoverAndCaptureName; - static const char* groundResolutionName; - static const char* frontalOverlapName; - static const char* sideOverlapName; - static const char* cameraSensorWidthName; - static const char* cameraSensorHeightName; - static const char* cameraResolutionWidthName; - static const char* cameraResolutionHeightName; - static const char* cameraFocalLengthName; - static const char* cameraTriggerName; - static const char* cameraOrientationLandscapeName; - static const char* fixedValueIsAltitudeName; - static const char* cameraName; - -signals: - void gridPointsChanged (void); - void cameraShotsChanged (int cameraShots); - void coveredAreaChanged (double coveredArea); - void cameraValueChanged (void); - void gridTypeChanged (QString gridType); - void timeBetweenShotsChanged (void); - void cameraOrientationFixedChanged (bool cameraOrientationFixed); - void refly90DegreesChanged (bool refly90Degrees); - void cameraMinTriggerIntervalChanged (double cameraMinTriggerInterval); - -private slots: - void _setDirty(void); - void _polygonDirtyChanged(bool dirty); - void _clearInternal(void); - -private: - enum CameraTriggerCode { - CameraTriggerNone, - CameraTriggerOn, - CameraTriggerOff, - CameraTriggerHoverAndCapture - }; - - void _setExitCoordinate(const QGeoCoordinate& coordinate); - void _generateGrid(void); - void _updateCoordinateAltitude(void); - int _gridGenerator(const QList& polygonPoints, QList>& transectSegments, bool refly); - QPointF _rotatePoint(const QPointF& point, const QPointF& origin, double angle); - void _intersectLinesWithRect(const QList& lineList, const QRectF& boundRect, QList& resultLines); - void _intersectLinesWithPolygon(const QList& lineList, const QPolygonF& polygon, QList& resultLines); - void _adjustLineDirection(const QList& lineList, QList& resultLines); - void _setSurveyDistance(double surveyDistance); - void _setCameraShots(int cameraShots); - void _setCoveredArea(double coveredArea); - void _cameraValueChanged(void); - int _appendWaypointToMission(QList& items, int seqNum, QGeoCoordinate& coord, CameraTriggerCode cameraTrigger, QObject* missionItemParent); - bool _nextTransectCoord(const QList& transectPoints, int pointIndex, QGeoCoordinate& coord); - double _triggerDistance(void) const; - bool _triggerCamera(void) const; - bool _imagesEverywhere(void) const; - bool _hoverAndCaptureEnabled(void) const; - bool _hasTurnaround(void) const; - double _turnaroundDistance(void) const; - void _convertTransectToGeo(const QList>& transectSegmentsNED, const QGeoCoordinate& tangentOrigin, QList>& transectSegmentsGeo); - bool _appendMissionItemsWorker(QList& items, QObject* missionItemParent, int& seqNum, bool hasRefly, bool buildRefly); - void _optimizeTransectsForShortestDistance(const QGeoCoordinate& distanceCoord, QList>& transects); - void _appendGridPointsFromTransects(QList>& rgTransectSegments); - qreal _ccw(QPointF pt1, QPointF pt2, QPointF pt3); - qreal _dp(QPointF pt1, QPointF pt2); - void _swapPoints(QList& points, int index1, int index2); - QList _convexPolygon(const QList& polygon); - void _reverseTransectOrder(QList>& transects); - void _reverseInternalTransectPoints(QList>& transects); - void _adjustTransectsToEntryPointLocation(QList>& transects); - bool _gridAngleIsNorthSouthTransects(); - double _clampGridAngle90(double gridAngle); - int _calcMissionCommandCount(QList>& transectSegments); - - int _sequenceNumber; - bool _dirty; - QGCMapPolygon _mapPolygon; - QVariantList _simpleGridPoints; ///< Grid points for drawing simple grid visuals - QList> _transectSegments; ///< Internal transect segments including grid exit, turnaround and internal camera points - QList> _reflyTransectSegments; ///< Refly segments - QGeoCoordinate _coordinate; - QGeoCoordinate _exitCoordinate; - bool _cameraOrientationFixed; - int _missionCommandCount; - bool _refly90Degrees; - double _additionalFlightDelaySeconds; - double _cameraMinTriggerInterval; - - bool _ignoreRecalc; - double _surveyDistance; - int _cameraShots; - double _coveredArea; - double _timeBetweenShots; - double _cruiseSpeed; - - QMap _metaDataMap; - - SettingsFact _manualGridFact; - SettingsFact _gridAltitudeFact; - SettingsFact _gridAltitudeRelativeFact; - SettingsFact _gridAngleFact; - SettingsFact _gridSpacingFact; - SettingsFact _gridEntryLocationFact; - SettingsFact _turnaroundDistFact; - SettingsFact _cameraTriggerDistanceFact; - SettingsFact _cameraTriggerInTurnaroundFact; - SettingsFact _hoverAndCaptureFact; - SettingsFact _groundResolutionFact; - SettingsFact _frontalOverlapFact; - SettingsFact _sideOverlapFact; - SettingsFact _cameraSensorWidthFact; - SettingsFact _cameraSensorHeightFact; - SettingsFact _cameraResolutionWidthFact; - SettingsFact _cameraResolutionHeightFact; - SettingsFact _cameraFocalLengthFact; - SettingsFact _cameraOrientationLandscapeFact; - SettingsFact _fixedValueIsAltitudeFact; - SettingsFact _cameraFact; - - static const char* _jsonGridObjectKey; - static const char* _jsonGridAltitudeKey; - static const char* _jsonGridAltitudeRelativeKey; - static const char* _jsonGridAngleKey; - static const char* _jsonGridSpacingKey; - static const char* _jsonGridEntryLocationKey; - static const char* _jsonTurnaroundDistKey; - static const char* _jsonCameraTriggerDistanceKey; - static const char* _jsonCameraTriggerInTurnaroundKey; - static const char* _jsonHoverAndCaptureKey; - static const char* _jsonGroundResolutionKey; - static const char* _jsonFrontalOverlapKey; - static const char* _jsonSideOverlapKey; - static const char* _jsonCameraSensorWidthKey; - static const char* _jsonCameraSensorHeightKey; - static const char* _jsonCameraResolutionWidthKey; - static const char* _jsonCameraResolutionHeightKey; - static const char* _jsonCameraFocalLengthKey; - static const char* _jsonCameraMinTriggerIntervalKey; - static const char* _jsonManualGridKey; - static const char* _jsonCameraObjectKey; - static const char* _jsonCameraNameKey; - static const char* _jsonCameraOrientationLandscapeKey; - static const char* _jsonFixedValueIsAltitudeKey; - static const char* _jsonRefly90DegreesKey; - - static const int _hoverAndCaptureDelaySeconds = 2; -}; - -#endif diff --git a/src/MissionManager/SurveyMissionItemTest.cc b/src/MissionManager/SurveyMissionItemTest.cc deleted file mode 100644 index 0f021051f250db374d7401a0378ea6e8aa72afb9..0000000000000000000000000000000000000000 --- a/src/MissionManager/SurveyMissionItemTest.cc +++ /dev/null @@ -1,287 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2016 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - -#include "SurveyMissionItemTest.h" -#include "QGCApplication.h" - -SurveyMissionItemTest::SurveyMissionItemTest(void) - : _offlineVehicle(NULL) -{ - _polyPoints << QGeoCoordinate(47.633550640000003, -122.08982199) << QGeoCoordinate(47.634129020000003, -122.08887249) << - QGeoCoordinate(47.633619320000001, -122.08811074) << QGeoCoordinate(47.633189139999999, -122.08900124); -} - -void SurveyMissionItemTest::init(void) -{ - UnitTest::init(); - - _rgSurveySignals[gridPointsChangedIndex] = SIGNAL(gridPointsChanged()); - _rgSurveySignals[cameraShotsChangedIndex] = SIGNAL(cameraShotsChanged(int)); - _rgSurveySignals[coveredAreaChangedIndex] = SIGNAL(coveredAreaChanged(double)); - _rgSurveySignals[cameraValueChangedIndex] = SIGNAL(cameraValueChanged()); - _rgSurveySignals[gridTypeChangedIndex] = SIGNAL(gridTypeChanged(QString)); - _rgSurveySignals[timeBetweenShotsChangedIndex] = SIGNAL(timeBetweenShotsChanged()); - _rgSurveySignals[cameraOrientationFixedChangedIndex] = SIGNAL(cameraOrientationFixedChanged(bool)); - _rgSurveySignals[refly90DegreesChangedIndex] = SIGNAL(refly90DegreesChanged(bool)); - _rgSurveySignals[dirtyChangedIndex] = SIGNAL(dirtyChanged(bool)); - - _offlineVehicle = new Vehicle(MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, qgcApp()->toolbox()->firmwarePluginManager(), this); - _surveyItem = new SurveyMissionItem(_offlineVehicle, this); - _surveyItem->setTurnaroundDist(0); // Unit test written for no turnaround distance - _surveyItem->setDirty(false); - _mapPolygon = _surveyItem->mapPolygon(); - - // It's important to check that the right signals are emitted at the right time since that drives ui change. - // It's also important to check that things are not being over-signalled when they should not be, since that can lead - // to incorrect ui or perf impact of uneeded signals propogating ui change. - - _multiSpy = new MultiSignalSpy(); - Q_CHECK_PTR(_multiSpy); - QCOMPARE(_multiSpy->init(_surveyItem, _rgSurveySignals, _cSurveySignals), true); -} - -void SurveyMissionItemTest::cleanup(void) -{ - delete _surveyItem; - delete _offlineVehicle; - delete _multiSpy; -} - -void SurveyMissionItemTest::_testDirty(void) -{ - QVERIFY(!_surveyItem->dirty()); - _surveyItem->setDirty(false); - QVERIFY(!_surveyItem->dirty()); - QVERIFY(_multiSpy->checkNoSignals()); - - _surveyItem->setDirty(true); - QVERIFY(_surveyItem->dirty()); - QVERIFY(_multiSpy->checkOnlySignalByMask(dirtyChangedMask)); - QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); - _multiSpy->clearAllSignals(); - - _surveyItem->setDirty(false); - QVERIFY(!_surveyItem->dirty()); - QVERIFY(_multiSpy->checkOnlySignalByMask(dirtyChangedMask)); - QVERIFY(!_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); - _multiSpy->clearAllSignals(); - - // These facts should set dirty when changed - QList rgFacts; - rgFacts << _surveyItem->gridAltitude() << _surveyItem->gridAngle() << _surveyItem->gridSpacing() << _surveyItem->turnaroundDist() << _surveyItem->cameraTriggerDistance() << - _surveyItem->gridAltitudeRelative() << _surveyItem->cameraTriggerInTurnaround() << _surveyItem->hoverAndCapture(); - foreach(Fact* fact, rgFacts) { - qDebug() << fact->name(); - QVERIFY(!_surveyItem->dirty()); - if (fact->typeIsBool()) { - fact->setRawValue(!fact->rawValue().toBool()); - } else { - fact->setRawValue(fact->rawValue().toDouble() + 1); - } - QVERIFY(_multiSpy->checkSignalByMask(dirtyChangedMask)); - QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); - _surveyItem->setDirty(false); - _multiSpy->clearAllSignals(); - } - rgFacts.clear(); - - // These facts should not change dirty bit - rgFacts << _surveyItem->groundResolution() << _surveyItem->frontalOverlap() << _surveyItem->sideOverlap() << _surveyItem->cameraSensorWidth() << _surveyItem->cameraSensorHeight() << - _surveyItem->cameraResolutionWidth() << _surveyItem->cameraResolutionHeight() << _surveyItem->cameraFocalLength() << _surveyItem->cameraOrientationLandscape() << - _surveyItem->fixedValueIsAltitude() << _surveyItem->camera() << _surveyItem->manualGrid(); - foreach(Fact* fact, rgFacts) { - qDebug() << fact->name(); - QVERIFY(!_surveyItem->dirty()); - if (fact->typeIsBool()) { - fact->setRawValue(!fact->rawValue().toBool()); - } else { - fact->setRawValue(fact->rawValue().toDouble() + 1); - } - QVERIFY(_multiSpy->checkNoSignalByMask(dirtyChangedMask)); - QVERIFY(!_surveyItem->dirty()); - _multiSpy->clearAllSignals(); - } - rgFacts.clear(); -} - -void SurveyMissionItemTest::_testCameraValueChanged(void) -{ - // These facts should trigger cameraValueChanged when changed - QList rgFacts; - rgFacts << _surveyItem->groundResolution() << _surveyItem->frontalOverlap() << _surveyItem->sideOverlap() << _surveyItem->cameraSensorWidth() << _surveyItem->cameraSensorHeight() << - _surveyItem->cameraResolutionWidth() << _surveyItem->cameraResolutionHeight() << _surveyItem->cameraFocalLength() << _surveyItem->cameraOrientationLandscape(); - foreach(Fact* fact, rgFacts) { - qDebug() << fact->name(); - if (fact->typeIsBool()) { - fact->setRawValue(!fact->rawValue().toBool()); - } else { - fact->setRawValue(fact->rawValue().toDouble() + 1); - } - QVERIFY(_multiSpy->checkSignalByMask(cameraValueChangedMask)); - _multiSpy->clearAllSignals(); - } - rgFacts.clear(); - - // These facts should not trigger cameraValueChanged - rgFacts << _surveyItem->gridAltitude() << _surveyItem->gridAngle() << _surveyItem->gridSpacing() << _surveyItem->turnaroundDist() << _surveyItem->cameraTriggerDistance() << - _surveyItem->gridAltitudeRelative() << _surveyItem->cameraTriggerInTurnaround() << _surveyItem->hoverAndCapture() << - _surveyItem->fixedValueIsAltitude() << _surveyItem->camera() << _surveyItem->manualGrid(); - foreach(Fact* fact, rgFacts) { - qDebug() << fact->name(); - if (fact->typeIsBool()) { - fact->setRawValue(!fact->rawValue().toBool()); - } else { - fact->setRawValue(fact->rawValue().toDouble() + 1); - } - QVERIFY(_multiSpy->checkNoSignalByMask(cameraValueChangedMask)); - _multiSpy->clearAllSignals(); - } - rgFacts.clear(); -} - -void SurveyMissionItemTest::_testCameraTrigger(void) -{ -#if 0 - QCOMPARE(_surveyItem->property("cameraTrigger").toBool(), true); - - // Set up a grid - - for (int i=0; i<3; i++) { - _mapPolygon->appendVertex(_polyPoints[i]); - } - - _surveyItem->setDirty(false); - _multiSpy->clearAllSignals(); - - int lastSeq = _surveyItem->lastSequenceNumber(); - QVERIFY(lastSeq > 0); - - // Turning off camera triggering should remove two camera trigger mission items, this should trigger: - // lastSequenceNumberChanged - // dirtyChanged - - _surveyItem->setProperty("cameraTrigger", false); - QVERIFY(_multiSpy->checkOnlySignalByMask(lastSequenceNumberChangedMask | dirtyChangedMask | cameraTriggerChangedMask)); - QCOMPARE(_multiSpy->pullIntFromSignalIndex(lastSequenceNumberChangedIndex), lastSeq - 2); - - _surveyItem->setDirty(false); - _multiSpy->clearAllSignals(); - - // Turn on camera triggering and make sure things go back to previous count - - _surveyItem->setProperty("cameraTrigger", true); - QVERIFY(_multiSpy->checkOnlySignalByMask(lastSequenceNumberChangedMask | dirtyChangedMask | cameraTriggerChangedMask)); - QCOMPARE(_multiSpy->pullIntFromSignalIndex(lastSequenceNumberChangedIndex), lastSeq); -#endif -} - -// Clamp expected grid angle from 0<->180. We don't care about opposite angles like 90/270 -double SurveyMissionItemTest::_clampGridAngle180(double gridAngle) -{ - if (gridAngle >= 0.0) { - if (gridAngle == 360.0) { - gridAngle = 0.0; - } else if (gridAngle >= 180.0) { - gridAngle -= 180.0; - } - } else { - if (gridAngle < -180.0) { - gridAngle += 360.0; - } else { - gridAngle += 180.0; - } - } - return gridAngle; -} - -void SurveyMissionItemTest::_setPolygon(void) -{ - for (int i=0; i<_polyPoints.count(); i++) { - QGeoCoordinate& vertex = _polyPoints[i]; - _mapPolygon->appendVertex(vertex); - } -} - -void SurveyMissionItemTest::_testGridAngle(void) -{ - _setPolygon(); - - for (double gridAngle=-360.0; gridAngle<=360.0; gridAngle++) { - _surveyItem->gridAngle()->setRawValue(gridAngle); - - QVariantList gridPoints = _surveyItem->gridPoints(); - QGeoCoordinate firstTransectEntry = gridPoints[0].value(); - QGeoCoordinate firstTransectExit = gridPoints[1].value(); - double azimuth = firstTransectEntry.azimuthTo(firstTransectExit); - //qDebug() << gridAngle << azimuth << _clampGridAngle180(gridAngle) << _clampGridAngle180(azimuth); - QCOMPARE((int)_clampGridAngle180(gridAngle), (int)_clampGridAngle180(azimuth)); - } -} - -void SurveyMissionItemTest::_testEntryLocation(void) -{ - _setPolygon(); - - for (double gridAngle=-360.0; gridAngle<=360.0; gridAngle++) { - _surveyItem->gridAngle()->setRawValue(gridAngle); - - QList rgSeenEntryCoords; - QList rgEntryLocation; - rgEntryLocation << SurveyMissionItem::EntryLocationTopLeft - << SurveyMissionItem::EntryLocationTopRight - << SurveyMissionItem::EntryLocationBottomLeft - << SurveyMissionItem::EntryLocationBottomRight; - - // Validate that each entry location is unique - for (int i=0; igridEntryLocation()->setRawValue(entryLocation); - QVERIFY(!rgSeenEntryCoords.contains(_surveyItem->coordinate())); - rgSeenEntryCoords << _surveyItem->coordinate(); - } - rgSeenEntryCoords.clear(); - } -} - - -void SurveyMissionItemTest::_testItemCount(void) -{ - QList items; - - _setPolygon(); - - _surveyItem->hoverAndCapture()->setRawValue(false); - _surveyItem->cameraTriggerInTurnaround()->setRawValue(false); - _surveyItem->setRefly90Degrees(false); - _surveyItem->appendMissionItems(items, this); - QCOMPARE(items.count(), _surveyItem->lastSequenceNumber()); - items.clear(); - - _surveyItem->hoverAndCapture()->setRawValue(false); - _surveyItem->cameraTriggerInTurnaround()->setRawValue(true); - _surveyItem->setRefly90Degrees(false); - _surveyItem->appendMissionItems(items, this); - QCOMPARE(items.count(), _surveyItem->lastSequenceNumber()); - items.clear(); - - _surveyItem->hoverAndCapture()->setRawValue(true); - _surveyItem->cameraTriggerInTurnaround()->setRawValue(false); - _surveyItem->setRefly90Degrees(false); - _surveyItem->appendMissionItems(items, this); - QCOMPARE(items.count(), _surveyItem->lastSequenceNumber()); - items.clear(); - - _surveyItem->hoverAndCapture()->setRawValue(true); - _surveyItem->cameraTriggerInTurnaround()->setRawValue(false); - _surveyItem->setRefly90Degrees(true); - _surveyItem->appendMissionItems(items, this); - QCOMPARE(items.count(), _surveyItem->lastSequenceNumber()); - items.clear(); -} diff --git a/src/MissionManager/SurveyMissionItemTest.h b/src/MissionManager/SurveyMissionItemTest.h deleted file mode 100644 index 0fe45131f79d17f2446e8218a40d1d6ad06d9685..0000000000000000000000000000000000000000 --- a/src/MissionManager/SurveyMissionItemTest.h +++ /dev/null @@ -1,80 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2016 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - - -#ifndef SurveyMissionItemTest_H -#define SurveyMissionItemTest_H - -#include "UnitTest.h" -#include "TCPLink.h" -#include "MultiSignalSpy.h" -#include "SurveyMissionItem.h" - -#include - -/// Unit test for SurveyMissionItem -class SurveyMissionItemTest : public UnitTest -{ - Q_OBJECT - -public: - SurveyMissionItemTest(void); - -protected: - void init(void) final; - void cleanup(void) final; - -private slots: - void _testDirty(void); - void _testCameraValueChanged(void); - void _testCameraTrigger(void); - void _testGridAngle(void); - void _testEntryLocation(void); - void _testItemCount(void); - -private: - double _clampGridAngle180(double gridAngle); - void _setPolygon(void); - - enum { - gridPointsChangedIndex = 0, - cameraShotsChangedIndex, - coveredAreaChangedIndex, - cameraValueChangedIndex, - gridTypeChangedIndex, - timeBetweenShotsChangedIndex, - cameraOrientationFixedChangedIndex, - refly90DegreesChangedIndex, - dirtyChangedIndex, - maxSignalIndex - }; - - enum { - gridPointsChangedMask = 1 << gridPointsChangedIndex, - cameraShotsChangedMask = 1 << cameraShotsChangedIndex, - coveredAreaChangedMask = 1 << coveredAreaChangedIndex, - cameraValueChangedMask = 1 << cameraValueChangedIndex, - gridTypeChangedMask = 1 << gridTypeChangedIndex, - timeBetweenShotsChangedMask = 1 << timeBetweenShotsChangedIndex, - cameraOrientationFixedChangedMask = 1 << cameraOrientationFixedChangedIndex, - refly90DegreesChangedMask = 1 << refly90DegreesChangedIndex, - dirtyChangedMask = 1 << dirtyChangedIndex - }; - - static const size_t _cSurveySignals = maxSignalIndex; - const char* _rgSurveySignals[_cSurveySignals]; - - Vehicle* _offlineVehicle; - MultiSignalSpy* _multiSpy; - SurveyMissionItem* _surveyItem; - QGCMapPolygon* _mapPolygon; - QList _polyPoints; -}; - -#endif diff --git a/src/MissionManager/TransectStyle.SettingsGroup.json b/src/MissionManager/TransectStyle.SettingsGroup.json index d949f6ba24ec433e4aa073df3692bd887c904882..fe86299eab363ec46080c5fdf63628ec168b68e8 100644 --- a/src/MissionManager/TransectStyle.SettingsGroup.json +++ b/src/MissionManager/TransectStyle.SettingsGroup.json @@ -34,5 +34,32 @@ "shortDescription": "Refly the pattern at a 90 degree angle", "type": "bool", "defaultValue": false +}, +{ + "name": "TerrainAdjustTolerance", + "shortDescription": "If adjacent terrain waypoints are within this tolerence they will be removed.", + "type": "double", + "decimalPlaces": 2, + "min": 0, + "units": "m", + "defaultValue": 10 +}, +{ + "name": "TerrainAdjustMaxClimbRate", + "shortDescription": "The maximum climb rate from one waypoint to another when adjusting for terrain. Set to 0 for no max.", + "type": "double", + "decimalPlaces": 2, + "min": 0, + "units": "m/s", + "defaultValue": 0 +}, +{ + "name": "TerrainAdjustMaxDescentRate", + "shortDescription": "The maximum descent rate from one waypoint to another when adjusting for terrain. Set to 0 for no max.", + "type": "double", + "decimalPlaces": 2, + "min": 0, + "units": "m/s", + "defaultValue": 0 } ] diff --git a/src/MissionManager/TransectStyleComplexItem.cc b/src/MissionManager/TransectStyleComplexItem.cc index 27c1f54018ff3df37873a45df5a86ffbe5fd83b7..e22dd92b2f9195c1fa1dc02cf41e3de1ca503cd0 100644 --- a/src/MissionManager/TransectStyleComplexItem.cc +++ b/src/MissionManager/TransectStyleComplexItem.cc @@ -26,41 +26,54 @@ const char* TransectStyleComplexItem::turnAroundDistanceMultiRotorName = "Tur const char* TransectStyleComplexItem::cameraTriggerInTurnAroundName = "CameraTriggerInTurnAround"; const char* TransectStyleComplexItem::hoverAndCaptureName = "HoverAndCapture"; const char* TransectStyleComplexItem::refly90DegreesName = "Refly90Degrees"; - -const char* TransectStyleComplexItem::_jsonCameraCalcKey = "CameraCalc"; - -TransectStyleComplexItem::TransectStyleComplexItem(Vehicle* vehicle, QString settingsGroup, QObject* parent) - : ComplexMissionItem (vehicle, parent) - , _settingsGroup (settingsGroup) - , _sequenceNumber (0) - , _dirty (false) - , _ignoreRecalc (false) - , _complexDistance (0) - , _cameraShots (0) - , _cameraMinTriggerInterval (0) - , _cameraCalc (vehicle) - , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/TransectStyle.SettingsGroup.json"), this)) - , _turnAroundDistanceFact (_settingsGroup, _metaDataMap[_vehicle->multiRotor() ? turnAroundDistanceMultiRotorName : turnAroundDistanceName]) - , _cameraTriggerInTurnAroundFact(_settingsGroup, _metaDataMap[cameraTriggerInTurnAroundName]) - , _hoverAndCaptureFact (_settingsGroup, _metaDataMap[hoverAndCaptureName]) - , _refly90DegreesFact (_settingsGroup, _metaDataMap[refly90DegreesName]) +const char* TransectStyleComplexItem::terrainAdjustToleranceName = "TerrainAdjustTolerance"; +const char* TransectStyleComplexItem::terrainAdjustMaxClimbRateName = "TerrainAdjustMaxClimbRate"; +const char* TransectStyleComplexItem::terrainAdjustMaxDescentRateName = "TerrainAdjustMaxDescentRate"; + +const char* TransectStyleComplexItem::_jsonTransectStyleComplexItemKey = "TransectStyleComplexItem"; +const char* TransectStyleComplexItem::_jsonCameraCalcKey = "CameraCalc"; +const char* TransectStyleComplexItem::_jsonVisualTransectPointsKey = "VisualTransectPoints"; +const char* TransectStyleComplexItem::_jsonItemsKey = "Items"; +const char* TransectStyleComplexItem::_jsonFollowTerrainKey = "FollowTerrain"; + +const int TransectStyleComplexItem::_terrainQueryTimeoutMsecs = 1000; + +TransectStyleComplexItem::TransectStyleComplexItem(Vehicle* vehicle, bool flyView, QString settingsGroup, QObject* parent) + : ComplexMissionItem (vehicle, flyView, parent) + , _sequenceNumber (0) + , _dirty (false) + , _terrainPolyPathQuery (NULL) + , _ignoreRecalc (false) + , _complexDistance (0) + , _cameraShots (0) + , _cameraCalc (vehicle, settingsGroup) + , _followTerrain (false) + , _loadedMissionItemsParent (NULL) + , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/TransectStyle.SettingsGroup.json"), this)) + , _turnAroundDistanceFact (settingsGroup, _metaDataMap[_vehicle->multiRotor() ? turnAroundDistanceMultiRotorName : turnAroundDistanceName]) + , _cameraTriggerInTurnAroundFact (settingsGroup, _metaDataMap[cameraTriggerInTurnAroundName]) + , _hoverAndCaptureFact (settingsGroup, _metaDataMap[hoverAndCaptureName]) + , _refly90DegreesFact (settingsGroup, _metaDataMap[refly90DegreesName]) + , _terrainAdjustToleranceFact (settingsGroup, _metaDataMap[terrainAdjustToleranceName]) + , _terrainAdjustMaxClimbRateFact (settingsGroup, _metaDataMap[terrainAdjustMaxClimbRateName]) + , _terrainAdjustMaxDescentRateFact (settingsGroup, _metaDataMap[terrainAdjustMaxDescentRateName]) { + _terrainQueryTimer.setInterval(_terrainQueryTimeoutMsecs); + _terrainQueryTimer.setSingleShot(true); + connect(&_terrainQueryTimer, &QTimer::timeout, this, &TransectStyleComplexItem::_reallyQueryTransectsPathHeightInfo); + connect(&_turnAroundDistanceFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); connect(&_hoverAndCaptureFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); connect(&_refly90DegreesFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); + connect(this, &TransectStyleComplexItem::followTerrainChanged, this, &TransectStyleComplexItem::_rebuildTransects); + connect(&_terrainAdjustMaxClimbRateFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); + connect(&_terrainAdjustMaxDescentRateFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); + connect(&_terrainAdjustToleranceFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); connect(&_surveyAreaPolygon, &QGCMapPolygon::pathChanged, this, &TransectStyleComplexItem::_rebuildTransects); connect(&_cameraTriggerInTurnAroundFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); connect(_cameraCalc.adjustedFootprintSide(), &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); connect(_cameraCalc.adjustedFootprintFrontal(), &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); - connect(&_turnAroundDistanceFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged); - connect(&_hoverAndCaptureFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged); - connect(&_refly90DegreesFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged); - connect(&_surveyAreaPolygon, &QGCMapPolygon::pathChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged); - connect(&_cameraTriggerInTurnAroundFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged); - connect(_cameraCalc.adjustedFootprintSide(), &Fact::valueChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged); - connect(_cameraCalc.adjustedFootprintFrontal(), &Fact::valueChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged); - connect(&_turnAroundDistanceFact, &Fact::valueChanged, this, &TransectStyleComplexItem::complexDistanceChanged); connect(&_hoverAndCaptureFact, &Fact::valueChanged, this, &TransectStyleComplexItem::complexDistanceChanged); connect(&_refly90DegreesFact, &Fact::valueChanged, this, &TransectStyleComplexItem::complexDistanceChanged); @@ -75,6 +88,10 @@ TransectStyleComplexItem::TransectStyleComplexItem(Vehicle* vehicle, QString set connect(&_cameraTriggerInTurnAroundFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_setDirty); connect(&_hoverAndCaptureFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_setDirty); connect(&_refly90DegreesFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_setDirty); + connect(this, &TransectStyleComplexItem::followTerrainChanged, this, &TransectStyleComplexItem::_setDirty); + connect(&_terrainAdjustMaxClimbRateFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_setDirty); + connect(&_terrainAdjustMaxDescentRateFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_setDirty); + connect(&_terrainAdjustToleranceFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_setDirty); connect(&_surveyAreaPolygon, &QGCMapPolygon::pathChanged, this, &TransectStyleComplexItem::_setDirty); connect(&_surveyAreaPolygon, &QGCMapPolygon::dirtyChanged, this, &TransectStyleComplexItem::_setIfDirty); @@ -82,8 +99,15 @@ TransectStyleComplexItem::TransectStyleComplexItem(Vehicle* vehicle, QString set connect(&_surveyAreaPolygon, &QGCMapPolygon::pathChanged, this, &TransectStyleComplexItem::coveredAreaChanged); - connect(this, &TransectStyleComplexItem::transectPointsChanged, this, &TransectStyleComplexItem::complexDistanceChanged); - connect(this, &TransectStyleComplexItem::transectPointsChanged, this, &TransectStyleComplexItem::greatestDistanceToChanged); + connect(&_cameraCalc, &CameraCalc::distanceToSurfaceRelativeChanged, this, &TransectStyleComplexItem::coordinateHasRelativeAltitudeChanged); + connect(&_cameraCalc, &CameraCalc::distanceToSurfaceRelativeChanged, this, &TransectStyleComplexItem::exitCoordinateHasRelativeAltitudeChanged); + + connect(this, &TransectStyleComplexItem::visualTransectPointsChanged, this, &TransectStyleComplexItem::complexDistanceChanged); + connect(this, &TransectStyleComplexItem::visualTransectPointsChanged, this, &TransectStyleComplexItem::greatestDistanceToChanged); + + connect(this, &TransectStyleComplexItem::followTerrainChanged, this, &TransectStyleComplexItem::_followTerrainChanged); + + setDirty(false); } void TransectStyleComplexItem::_setCameraShots(int cameraShots) @@ -108,14 +132,45 @@ void TransectStyleComplexItem::setDirty(bool dirty) void TransectStyleComplexItem::_save(QJsonObject& complexObject) { - complexObject[turnAroundDistanceName] = _turnAroundDistanceFact.rawValue().toDouble(); - complexObject[cameraTriggerInTurnAroundName] = _cameraTriggerInTurnAroundFact.rawValue().toBool(); - complexObject[hoverAndCaptureName] = _hoverAndCaptureFact.rawValue().toBool(); - complexObject[refly90DegreesName] = _refly90DegreesFact.rawValue().toBool(); + QJsonObject innerObject; + + innerObject[JsonHelper::jsonVersionKey] = 1; + innerObject[turnAroundDistanceName] = _turnAroundDistanceFact.rawValue().toDouble(); + innerObject[cameraTriggerInTurnAroundName] = _cameraTriggerInTurnAroundFact.rawValue().toBool(); + innerObject[hoverAndCaptureName] = _hoverAndCaptureFact.rawValue().toBool(); + innerObject[refly90DegreesName] = _refly90DegreesFact.rawValue().toBool(); + innerObject[_jsonFollowTerrainKey] = _followTerrain; + + if (_followTerrain) { + innerObject[terrainAdjustToleranceName] = _terrainAdjustToleranceFact.rawValue().toDouble(); + innerObject[terrainAdjustMaxClimbRateName] = _terrainAdjustMaxClimbRateFact.rawValue().toDouble(); + innerObject[terrainAdjustMaxDescentRateName] = _terrainAdjustMaxDescentRateFact.rawValue().toDouble(); + } QJsonObject cameraCalcObject; _cameraCalc.save(cameraCalcObject); - complexObject[_jsonCameraCalcKey] = cameraCalcObject; + innerObject[_jsonCameraCalcKey] = cameraCalcObject; + + QJsonValue transectPointsJson; + + // Save transects polyline + JsonHelper::saveGeoCoordinateArray(_visualTransectPoints, false /* writeAltitude */, transectPointsJson); + innerObject[_jsonVisualTransectPointsKey] = transectPointsJson; + + // Save the interal mission items + QJsonArray missionItemsJsonArray; + QObject* missionItemParent = new QObject(); + QList missionItems; + appendMissionItems(missionItems, missionItemParent); + foreach (const MissionItem* missionItem, missionItems) { + QJsonObject missionItemJsonObject; + missionItem->save(missionItemJsonObject); + missionItemsJsonArray.append(missionItemJsonObject); + } + missionItemParent->deleteLater(); + innerObject[_jsonItemsKey] = missionItemsJsonArray; + + complexObject[_jsonTransectStyleComplexItemKey] = innerObject; } void TransectStyleComplexItem::setSequenceNumber(int sequenceNumber) @@ -129,25 +184,86 @@ void TransectStyleComplexItem::setSequenceNumber(int sequenceNumber) bool TransectStyleComplexItem::_load(const QJsonObject& complexObject, QString& errorString) { + QList keyInfoList = { + { _jsonTransectStyleComplexItemKey, QJsonValue::Object, true }, + }; + if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) { + return false; + } + + // The TransectStyleComplexItem is a sub-object of the main complex item object + QJsonObject innerObject = complexObject[_jsonTransectStyleComplexItemKey].toObject(); + + if (innerObject.contains(JsonHelper::jsonVersionKey)) { + int version = innerObject[JsonHelper::jsonVersionKey].toInt(); + if (version != 1) { + errorString = tr("TransectStyleComplexItem version %2 not supported").arg(version); + return false; + } + } + + QList innerKeyInfoList = { + { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, { turnAroundDistanceName, QJsonValue::Double, true }, { cameraTriggerInTurnAroundName, QJsonValue::Bool, true }, { hoverAndCaptureName, QJsonValue::Bool, true }, { refly90DegreesName, QJsonValue::Bool, true }, { _jsonCameraCalcKey, QJsonValue::Object, true }, + { _jsonVisualTransectPointsKey, QJsonValue::Array, true }, + { _jsonItemsKey, QJsonValue::Array, true }, + { _jsonFollowTerrainKey, QJsonValue::Bool, true }, }; - if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) { + if (!JsonHelper::validateKeys(innerObject, innerKeyInfoList, errorString)) { + return false; + } + + // Load visual transect points + if (!JsonHelper::loadGeoCoordinateArray(innerObject[_jsonVisualTransectPointsKey], false /* altitudeRequired */, _visualTransectPoints, errorString)) { return false; } + _coordinate = _visualTransectPoints.count() ? _visualTransectPoints.first().value() : QGeoCoordinate(); + _exitCoordinate = _visualTransectPoints.count() ? _visualTransectPoints.last().value() : QGeoCoordinate(); + + // Load generated mission items + _loadedMissionItemsParent = new QObject(this); + QJsonArray missionItemsJsonArray = innerObject[_jsonItemsKey].toArray(); + foreach (const QJsonValue& missionItemJson, missionItemsJsonArray) { + MissionItem* missionItem = new MissionItem(_loadedMissionItemsParent); + if (!missionItem->load(missionItemJson.toObject(), 0 /* sequenceNumber */, errorString)) { + _loadedMissionItemsParent->deleteLater(); + _loadedMissionItemsParent = NULL; + return false; + } + _loadedMissionItems.append(missionItem); + } - if (!_cameraCalc.load(complexObject[_jsonCameraCalcKey].toObject(), errorString)) { + // Load CameraCalc data + if (!_cameraCalc.load(innerObject[_jsonCameraCalcKey].toObject(), errorString)) { return false; } - _turnAroundDistanceFact.setRawValue (complexObject[turnAroundDistanceName].toDouble()); - _cameraTriggerInTurnAroundFact.setRawValue (complexObject[cameraTriggerInTurnAroundName].toBool()); - _hoverAndCaptureFact.setRawValue (complexObject[hoverAndCaptureName].toBool()); - _hoverAndCaptureFact.setRawValue (complexObject[refly90DegreesName].toBool()); + // Load TransectStyleComplexItem individual values + _turnAroundDistanceFact.setRawValue (innerObject[turnAroundDistanceName].toDouble()); + _cameraTriggerInTurnAroundFact.setRawValue (innerObject[cameraTriggerInTurnAroundName].toBool()); + _hoverAndCaptureFact.setRawValue (innerObject[hoverAndCaptureName].toBool()); + _refly90DegreesFact.setRawValue (innerObject[refly90DegreesName].toBool()); + _followTerrain = innerObject[_jsonFollowTerrainKey].toBool(); + + if (_followTerrain) { + QList followTerrainKeyInfoList = { + { terrainAdjustToleranceName, QJsonValue::Double, true }, + { terrainAdjustMaxClimbRateName, QJsonValue::Double, true }, + { terrainAdjustMaxDescentRateName, QJsonValue::Double, true }, + }; + if (!JsonHelper::validateKeys(innerObject, followTerrainKeyInfoList, errorString)) { + return false; + } + + _terrainAdjustToleranceFact.setRawValue (innerObject[terrainAdjustToleranceName].toDouble()); + _terrainAdjustMaxClimbRateFact.setRawValue (innerObject[terrainAdjustMaxClimbRateName].toDouble()); + _terrainAdjustMaxDescentRateFact.setRawValue (innerObject[terrainAdjustMaxDescentRateName].toDouble()); + } return true; } @@ -155,8 +271,8 @@ bool TransectStyleComplexItem::_load(const QJsonObject& complexObject, QString& double TransectStyleComplexItem::greatestDistanceTo(const QGeoCoordinate &other) const { double greatestDistance = 0.0; - for (int i=0; i<_transectPoints.count(); i++) { - QGeoCoordinate vertex = _transectPoints[i].value(); + for (int i=0; i<_visualTransectPoints.count(); i++) { + QGeoCoordinate vertex = _visualTransectPoints[i].value(); double distance = vertex.distanceTo(other); if (distance > greatestDistance) { greatestDistance = distance; @@ -194,22 +310,12 @@ void TransectStyleComplexItem::applyNewAltitude(double newAltitude) //_altitudeFact.setRawValue(newAltitude); } -double TransectStyleComplexItem::timeBetweenShots(void) -{ - return _cruiseSpeed == 0 ? 0 : _cameraCalc.adjustedFootprintSide()->rawValue().toDouble() / _cruiseSpeed; -} - void TransectStyleComplexItem::_updateCoordinateAltitudes(void) { emit coordinateChanged(coordinate()); emit exitCoordinateChanged(exitCoordinate()); } -void TransectStyleComplexItem::_signalLastSequenceNumberChanged(void) -{ - emit lastSequenceNumberChanged(lastSequenceNumber()); -} - double TransectStyleComplexItem::coveredArea(void) const { return _surveyAreaPolygon.area(); @@ -230,3 +336,382 @@ bool TransectStyleComplexItem::hoverAndCaptureAllowed(void) const return _vehicle->multiRotor() || _vehicle->vtol(); } +void TransectStyleComplexItem::_rebuildTransects(void) +{ + if (_ignoreRecalc) { + return; + } + + _rebuildTransectsPhase1(); + + if (_followTerrain) { + // Query the terrain data. Once available terrain heights will be calculated + _queryTransectsPathHeightInfo(); + } else { + // Not following terrain, just add requested altitude to coords + double requestedAltitude = _cameraCalc.distanceToSurface()->rawValue().toDouble(); + + for (int i=0; i<_transects.count(); i++) { + QList& transect = _transects[i]; + + for (int j=0; j& transect, _transects) { + foreach (const CoordInfo_t& coordInfo, transect) { + _visualTransectPoints.append(QVariant::fromValue(coordInfo.coord)); + } + } + emit visualTransectPointsChanged(); + + _coordinate = _visualTransectPoints.count() ? _visualTransectPoints.first().value() : QGeoCoordinate(); + _exitCoordinate = _visualTransectPoints.count() ? _visualTransectPoints.last().value() : QGeoCoordinate(); + emit coordinateChanged(_coordinate); + emit exitCoordinateChanged(_exitCoordinate); + + _rebuildTransectsPhase2(); + + emit lastSequenceNumberChanged(lastSequenceNumber()); + emit timeBetweenShotsChanged(); +} + +void TransectStyleComplexItem::_queryTransectsPathHeightInfo(void) +{ + _transectsPathHeightInfo.clear(); + if (_terrainPolyPathQuery) { + // Toss previous query + _terrainPolyPathQuery->deleteLater(); + _terrainPolyPathQuery = NULL; + } + + if (_transects.count()) { + // We don't actually send the query until this timer times out. This way we only send + // the latest request if we get a bunch in a row. + _terrainQueryTimer.start(); + } +} + +void TransectStyleComplexItem::_reallyQueryTransectsPathHeightInfo(void) +{ + // Append all transects into a single PolyPath query + + QList transectPoints; + + foreach (const QList& transect, _transects) { + foreach (const CoordInfo_t& coordInfo, transect) { + transectPoints.append(coordInfo.coord); + } + } + + if (transectPoints.count() > 1) { + _terrainPolyPathQuery = new TerrainPolyPathQuery(this); + connect(_terrainPolyPathQuery, &TerrainPolyPathQuery::terrainData, this, &TransectStyleComplexItem::_polyPathTerrainData); + _terrainPolyPathQuery->requestData(transectPoints); + } +} + +void TransectStyleComplexItem::_polyPathTerrainData(bool success, const QList& rgPathHeightInfo) +{ + _transectsPathHeightInfo.clear(); + + if (success) { + // Break out into individual transects + int pathHeightIndex = 0; + for (int i=0; i<_transects.count(); i++) { + _transectsPathHeightInfo.append(QList()); + int cPathHeight = _transects[i].count() - 1; + while (cPathHeight-- > 0) { + _transectsPathHeightInfo[i].append(rgPathHeightInfo[pathHeightIndex++]); + } + pathHeightIndex++; // There is an extra on between each transect + } + + // Now that we have terrain data we can adjust + _adjustTransectsForTerrain(); + } +} + +bool TransectStyleComplexItem::readyForSave(void) const +{ + // Make sure we have the terrain data we need + return _followTerrain ? _transectsPathHeightInfo.count() : true; +} + +void TransectStyleComplexItem::_adjustTransectsForTerrain(void) +{ + if (_followTerrain) { + if (!readyForSave()) { + qCWarning(TransectStyleComplexItemLog) << "_adjustTransectPointsForTerrain called when terrain data not ready"; + qgcApp()->showMessage(tr("INTERNAL ERROR: TransectStyleComplexItem::_adjustTransectPointsForTerrain called when terrain data not ready. Plan will be incorrect.")); + return; + } + + // First step is add all interstitial points at max resolution + for (int i=0; i<_transects.count(); i++) { + _addInterstitialTerrainPoints(_transects[i], _transectsPathHeightInfo[i]); + } + + for (int i=0; i<_transects.count(); i++) { + _adjustForMaxRates(_transects[i]); + } + + for (int i=0; i<_transects.count(); i++) { + _adjustForTolerance(_transects[i]); + } + + emit lastSequenceNumberChanged(lastSequenceNumber()); + } +} + +/// Returns the altitude in between the two points on a line. +/// @param precentTowardsTo Example: .25 = twenty five percent along the distance of from to to +double TransectStyleComplexItem::_altitudeBetweenCoords(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord, double percentTowardsTo) +{ + double fromAlt = fromCoord.altitude(); + double toAlt = toCoord.altitude(); + double altDiff = toAlt - fromAlt; + return fromAlt + (altDiff * percentTowardsTo); +} + +/// Determine the maximum height within the PathHeightInfo +/// @param fromIndex First height index to consider +/// @param fromIndex Last height index to consider +/// @param[out] maxHeight Maximum height +/// @return index within PathHeightInfo_t.heights which contains maximum height, -1 no height data in between from and to indices +int TransectStyleComplexItem::_maxPathHeight(const TerrainPathQuery::PathHeightInfo_t& pathHeightInfo, int fromIndex, int toIndex, double& maxHeight) +{ + maxHeight = qQNaN(); + + if (toIndex - fromIndex < 2) { + return -1; + } + + fromIndex++; + toIndex--; + + int maxIndex = fromIndex; + maxHeight = pathHeightInfo.heights[fromIndex]; + + for (int i=fromIndex; i<=toIndex; i++) { + if (pathHeightInfo.heights[i] > maxHeight) { + maxIndex = i; + maxHeight = pathHeightInfo.heights[i]; + } + } + + return maxIndex; +} + +void TransectStyleComplexItem::_adjustForMaxRates(QList& transect) +{ + double maxClimbRate = _terrainAdjustMaxClimbRateFact.rawValue().toDouble(); + double maxDescentRate = _terrainAdjustMaxDescentRateFact.rawValue().toDouble(); + double flightSpeed = _missionFlightStatus.vehicleSpeed; + + if (qIsNaN(flightSpeed) || (maxClimbRate == 0 && maxDescentRate == 0)) { + if (qIsNaN(flightSpeed)) { + qWarning() << "TransectStyleComplexItem::_adjustForMaxRates called with flightSpeed = NaN"; + } + return; + } + + if (maxClimbRate > 0) { + // Adjust climb rates + bool climbRateAdjusted; + do { + //qDebug() << "climbrate pass"; + climbRateAdjusted = false; + for (int i=0; i 0 && climbRate - maxClimbRate > 0.1) { + double maxAltitudeDelta = maxClimbRate * seconds; + fromCoord.setAltitude(toCoord.altitude() - maxAltitudeDelta); + //qDebug() << "Adjusting"; + climbRateAdjusted = true; + } + } + } while (climbRateAdjusted); + } + + if (maxDescentRate > 0) { + // Adjust descent rates + bool descentRateAdjusted; + maxDescentRate = -maxDescentRate; + do { + //qDebug() << "descent rate pass"; + descentRateAdjusted = false; + for (int i=1; i& transect) +{ + QList adjustedPoints; + + double tolerance = _terrainAdjustToleranceFact.rawValue().toDouble(); + + int coordIndex = 0; + while (coordIndex < transect.count()) { + const CoordInfo_t& fromCoordInfo = transect[coordIndex]; + + adjustedPoints.append(fromCoordInfo); + + // Walk forward until we fall out of tolerence or find a fixed point + while (++coordIndex < transect.count()) { + const CoordInfo_t& toCoordInfo = transect[coordIndex]; + if (toCoordInfo.coordType != CoordTypeInteriorTerrainAdded || qAbs(fromCoordInfo.coord.altitude() - toCoordInfo.coord.altitude()) > tolerance) { + adjustedPoints.append(toCoordInfo); + coordIndex++; + break; + } + } + } + +#if 0 + qDebug() << "_adjustForTolerance"; + foreach (const TransectStyleComplexItem::CoordInfo_t& coordInfo, adjustedPoints) { + qDebug() << coordInfo.coordType; + } +#endif + + transect = adjustedPoints; +} + +void TransectStyleComplexItem::_addInterstitialTerrainPoints(QList& transect, const QList& transectPathHeightInfo) +{ + QList adjustedTransect; + + double requestedAltitude = _cameraCalc.distanceToSurface()->rawValue().toDouble(); + + for (int i=0; i& rgCoordInfo, _transects) { + itemCount += rgCoordInfo.count(); + } + + if (_cameraTriggerInTurnAroundFact.rawValue().toBool()) { + // Only one camera start and on camera stop + itemCount += 2; + } else { + // Each transect will have a camera start and stop in it + itemCount += _transects.count() * 2; + } + + return _sequenceNumber + itemCount - 1; + } +} + +bool TransectStyleComplexItem::coordinateHasRelativeAltitude(void) const +{ + return _cameraCalc.distanceToSurfaceRelative(); +} + +bool TransectStyleComplexItem::exitCoordinateHasRelativeAltitude(void) const +{ + return coordinateHasRelativeAltitude(); +} + +void TransectStyleComplexItem::_followTerrainChanged(bool followTerrain) +{ + _cameraCalc.setDistanceToSurfaceRelative(!followTerrain); + if (followTerrain) { + _refly90DegreesFact.setRawValue(false); + _hoverAndCaptureFact.setRawValue(false); + } +} diff --git a/src/MissionManager/TransectStyleComplexItem.h b/src/MissionManager/TransectStyleComplexItem.h index d624557eb4509d5736600147aa364a7b1253bb29..7d35574959bb7a5006883beb71a9c997f931f585 100644 --- a/src/MissionManager/TransectStyleComplexItem.h +++ b/src/MissionManager/TransectStyleComplexItem.h @@ -16,6 +16,7 @@ #include "QGCMapPolyline.h" #include "QGCMapPolygon.h" #include "CameraCalc.h" +#include "TerrainQuery.h" Q_DECLARE_LOGGING_CATEGORY(TransectStyleComplexItemLog) @@ -24,40 +25,50 @@ class TransectStyleComplexItem : public ComplexMissionItem Q_OBJECT public: - TransectStyleComplexItem(Vehicle* vehicle, QString settignsGroup, QObject* parent = NULL); - - Q_PROPERTY(QGCMapPolygon* surveyAreaPolygon READ surveyAreaPolygon CONSTANT) - Q_PROPERTY(CameraCalc* cameraCalc READ cameraCalc CONSTANT) - Q_PROPERTY(Fact* turnAroundDistance READ turnAroundDistance CONSTANT) - Q_PROPERTY(Fact* cameraTriggerInTurnAround READ cameraTriggerInTurnAround CONSTANT) - Q_PROPERTY(Fact* hoverAndCapture READ hoverAndCapture CONSTANT) - Q_PROPERTY(Fact* refly90Degrees READ refly90Degrees CONSTANT) - - Q_PROPERTY(int cameraShots READ cameraShots NOTIFY cameraShotsChanged) - Q_PROPERTY(double timeBetweenShots READ timeBetweenShots NOTIFY timeBetweenShotsChanged) - Q_PROPERTY(double coveredArea READ coveredArea NOTIFY coveredAreaChanged) - Q_PROPERTY(double cameraMinTriggerInterval READ cameraMinTriggerInterval NOTIFY cameraMinTriggerIntervalChanged) - Q_PROPERTY(bool hoverAndCaptureAllowed READ hoverAndCaptureAllowed CONSTANT) - Q_PROPERTY(QVariantList transectPoints READ transectPoints NOTIFY transectPointsChanged) + TransectStyleComplexItem(Vehicle* vehicle, bool flyView, QString settignsGroup, QObject* parent); + + Q_PROPERTY(QGCMapPolygon* surveyAreaPolygon READ surveyAreaPolygon CONSTANT) + Q_PROPERTY(CameraCalc* cameraCalc READ cameraCalc CONSTANT) + Q_PROPERTY(Fact* turnAroundDistance READ turnAroundDistance CONSTANT) + Q_PROPERTY(Fact* cameraTriggerInTurnAround READ cameraTriggerInTurnAround CONSTANT) + Q_PROPERTY(Fact* hoverAndCapture READ hoverAndCapture CONSTANT) + Q_PROPERTY(Fact* refly90Degrees READ refly90Degrees CONSTANT) + + Q_PROPERTY(int cameraShots READ cameraShots NOTIFY cameraShotsChanged) + Q_PROPERTY(double timeBetweenShots READ timeBetweenShots NOTIFY timeBetweenShotsChanged) + Q_PROPERTY(double coveredArea READ coveredArea NOTIFY coveredAreaChanged) + Q_PROPERTY(bool hoverAndCaptureAllowed READ hoverAndCaptureAllowed CONSTANT) + Q_PROPERTY(QVariantList visualTransectPoints READ visualTransectPoints NOTIFY visualTransectPointsChanged) + + Q_PROPERTY(bool followTerrain READ followTerrain WRITE setFollowTerrain NOTIFY followTerrainChanged) + Q_PROPERTY(Fact* terrainAdjustTolerance READ terrainAdjustTolerance CONSTANT) + Q_PROPERTY(Fact* terrainAdjustMaxDescentRate READ terrainAdjustMaxDescentRate CONSTANT) + Q_PROPERTY(Fact* terrainAdjustMaxClimbRate READ terrainAdjustMaxClimbRate CONSTANT) QGCMapPolygon* surveyAreaPolygon (void) { return &_surveyAreaPolygon; } CameraCalc* cameraCalc (void) { return &_cameraCalc; } - QVariantList transectPoints (void) { return _transectPoints; } + QVariantList visualTransectPoints(void) { return _visualTransectPoints; } - Fact* turnAroundDistance (void) { return &_turnAroundDistanceFact; } - Fact* cameraTriggerInTurnAround (void) { return &_cameraTriggerInTurnAroundFact; } - Fact* hoverAndCapture (void) { return &_hoverAndCaptureFact; } - Fact* refly90Degrees (void) { return &_refly90DegreesFact; } + Fact* turnAroundDistance (void) { return &_turnAroundDistanceFact; } + Fact* cameraTriggerInTurnAround (void) { return &_cameraTriggerInTurnAroundFact; } + Fact* hoverAndCapture (void) { return &_hoverAndCaptureFact; } + Fact* refly90Degrees (void) { return &_refly90DegreesFact; } + Fact* terrainAdjustTolerance (void) { return &_terrainAdjustToleranceFact; } + Fact* terrainAdjustMaxDescentRate (void) { return &_terrainAdjustMaxClimbRateFact; } + Fact* terrainAdjustMaxClimbRate (void) { return &_terrainAdjustMaxDescentRateFact; } int cameraShots (void) const { return _cameraShots; } - double timeBetweenShots (void); double coveredArea (void) const; - double cameraMinTriggerInterval(void) const { return _cameraMinTriggerInterval; } bool hoverAndCaptureAllowed (void) const; + bool followTerrain (void) const { return _followTerrain; } + + virtual double timeBetweenShots (void) { return 0; } // Most be overridden. Implementation here is needed for unit testing. + + void setFollowTerrain(bool followTerrain); // Overrides from ComplexMissionItem - int lastSequenceNumber (void) const override = 0; + int lastSequenceNumber (void) const final; QString mapVisualQML (void) const override = 0; bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) override = 0; @@ -66,7 +77,7 @@ public: // Overrides from VisualMissionItem - void save (QJsonArray& missionItems) override = 0; + void save (QJsonArray& planItems) override = 0; bool specifiesCoordinate (void) const override = 0; void appendMissionItems (QList& items, QObject* missionItemParent) override = 0; void applyNewAltitude (double newAltitude) override = 0; @@ -75,9 +86,6 @@ public: bool isSimpleItem (void) const final { return false; } bool isStandaloneCoordinate (void) const final { return false; } bool specifiesAltitudeOnly (void) const final { return false; } - QString commandDescription (void) const final { return tr("Corridor Scan"); } - QString commandName (void) const final { return tr("Corridor Scan"); } - QString abbreviation (void) const final { return "S"; } QGeoCoordinate coordinate (void) const final { return _coordinate; } QGeoCoordinate exitCoordinate (void) const final { return _exitCoordinate; } int sequenceNumber (void) const final { return _sequenceNumber; } @@ -85,9 +93,13 @@ public: double specifiedGimbalYaw (void) final { return std::numeric_limits::quiet_NaN(); } double specifiedGimbalPitch (void) final { return std::numeric_limits::quiet_NaN(); } void setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final; + bool readyForSave (void) const override; + QString commandDescription (void) const override { return tr("Transect"); } + QString commandName (void) const override { return tr("Transect"); } + QString abbreviation (void) const override { return tr("T"); } - bool coordinateHasRelativeAltitude (void) const final { return true /*_altitudeRelative*/; } - bool exitCoordinateHasRelativeAltitude (void) const final { return true /*_altitudeRelative*/; } + bool coordinateHasRelativeAltitude (void) const final; + bool exitCoordinateHasRelativeAltitude (void) const final; bool exitCoordinateSameAsEntry (void) const final { return false; } void setDirty (bool dirty) final; @@ -99,47 +111,70 @@ public: static const char* cameraTriggerInTurnAroundName; static const char* hoverAndCaptureName; static const char* refly90DegreesName; + static const char* terrainAdjustToleranceName; + static const char* terrainAdjustMaxClimbRateName; + static const char* terrainAdjustMaxDescentRateName; signals: void cameraShotsChanged (void); void timeBetweenShotsChanged (void); - void cameraMinTriggerIntervalChanged(double cameraMinTriggerInterval); - void transectPointsChanged (void); + void visualTransectPointsChanged (void); void coveredAreaChanged (void); + void followTerrainChanged (bool followTerrain); protected slots: - virtual void _rebuildTransects (void) = 0; + virtual void _rebuildTransectsPhase1 (void) = 0; ///< Rebuilds the _transects array + virtual void _rebuildTransectsPhase2 (void) = 0; ///< Adjust values associated with _transects array void _setDirty (void); void _setIfDirty (bool dirty); void _updateCoordinateAltitudes (void); - void _signalLastSequenceNumberChanged (void); + void _polyPathTerrainData (bool success, const QList& rgPathHeightInfo); + void _rebuildTransects (void); protected: - void _save (QJsonObject& saveObject); - bool _load (const QJsonObject& complexObject, QString& errorString); - void _setExitCoordinate (const QGeoCoordinate& coordinate); - void _setCameraShots (int cameraShots); - double _triggerDistance (void) const; - int _transectCount (void) const; - bool _hasTurnaround (void) const; - double _turnaroundDistance (void) const; - - QString _settingsGroup; + void _save (QJsonObject& saveObject); + bool _load (const QJsonObject& complexObject, QString& errorString); + void _setExitCoordinate (const QGeoCoordinate& coordinate); + void _setCameraShots (int cameraShots); + double _triggerDistance (void) const; + bool _hasTurnaround (void) const; + double _turnaroundDistance (void) const; + int _sequenceNumber; bool _dirty; QGeoCoordinate _coordinate; QGeoCoordinate _exitCoordinate; - QVariantList _transectPoints; QGCMapPolygon _surveyAreaPolygon; + enum CoordType { + CoordTypeInterior, + CoordTypeInteriorTerrainAdded, + CoordTypeSurveyEdge, + CoordTypeTurnaround + }; + + typedef struct { + QGeoCoordinate coord; + CoordType coordType; + } CoordInfo_t; + + QVariantList _visualTransectPoints; + QList> _transects; + QList> _transectsPathHeightInfo; + TerrainPolyPathQuery* _terrainPolyPathQuery; + QTimer _terrainQueryTimer; + bool _ignoreRecalc; double _complexDistance; int _cameraShots; double _timeBetweenShots; - double _cameraMinTriggerInterval; double _cruiseSpeed; CameraCalc _cameraCalc; + bool _followTerrain; + + QObject* _loadedMissionItemsParent; ///< Parent for all items in _loadedMissionItems for simpler delete + QList _loadedMissionItems; ///< Mission items loaded from plan file QMap _metaDataMap; @@ -147,6 +182,28 @@ protected: SettingsFact _cameraTriggerInTurnAroundFact; SettingsFact _hoverAndCaptureFact; SettingsFact _refly90DegreesFact; + SettingsFact _terrainAdjustToleranceFact; + SettingsFact _terrainAdjustMaxClimbRateFact; + SettingsFact _terrainAdjustMaxDescentRateFact; static const char* _jsonCameraCalcKey; + static const char* _jsonTransectStyleComplexItemKey; + static const char* _jsonVisualTransectPointsKey; + static const char* _jsonItemsKey; + static const char* _jsonFollowTerrainKey; + + static const int _terrainQueryTimeoutMsecs; + +private slots: + void _reallyQueryTransectsPathHeightInfo(void); + void _followTerrainChanged (bool followTerrain); + +private: + void _queryTransectsPathHeightInfo (void); + void _adjustTransectsForTerrain (void); + void _addInterstitialTerrainPoints (QList& transect, const QList& transectPathHeightInfo); + void _adjustForMaxRates (QList& transect); + void _adjustForTolerance (QList& transect); + double _altitudeBetweenCoords (const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord, double percentTowardsTo); + int _maxPathHeight (const TerrainPathQuery::PathHeightInfo_t& pathHeightInfo, int fromIndex, int toIndex, double& maxHeight); }; diff --git a/src/MissionManager/TransectStyleComplexItemTest.cc b/src/MissionManager/TransectStyleComplexItemTest.cc index a9f2372af32e32225d8bfdbf1612ce063f09f790..99caca57866f72acd39a7cc1d466639f979f6baf 100644 --- a/src/MissionManager/TransectStyleComplexItemTest.cc +++ b/src/MissionManager/TransectStyleComplexItemTest.cc @@ -25,13 +25,16 @@ void TransectStyleComplexItemTest::init(void) _offlineVehicle = new Vehicle(MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, qgcApp()->toolbox()->firmwarePluginManager(), this); _transectStyleItem = new TransectStyleItem(_offlineVehicle, this); + _transectStyleItem->cameraTriggerInTurnAround()->setRawValue(false); + _transectStyleItem->cameraCalc()->cameraName()->setRawValue(_transectStyleItem->cameraCalc()->customCameraName()); + _transectStyleItem->cameraCalc()->valueSetIsDistance()->setRawValue(true); + _transectStyleItem->cameraCalc()->distanceToSurface()->setRawValue(100); _setSurveyAreaPolygon(); _transectStyleItem->setDirty(false); _rgSignals[cameraShotsChangedIndex] = SIGNAL(cameraShotsChanged()); _rgSignals[timeBetweenShotsChangedIndex] = SIGNAL(timeBetweenShotsChanged()); - _rgSignals[cameraMinTriggerIntervalChangedIndex] = SIGNAL(cameraMinTriggerIntervalChanged(double)); - _rgSignals[transectPointsChangedIndex] = SIGNAL(transectPointsChanged()); + _rgSignals[visualTransectPointsChangedIndex] = SIGNAL(visualTransectPointsChanged()); _rgSignals[coveredAreaChangedIndex] = SIGNAL(coveredAreaChanged()); _rgSignals[dirtyChangedIndex] = SIGNAL(dirtyChanged(bool)); _rgSignals[complexDistanceChangedIndex] = SIGNAL(complexDistanceChanged()); @@ -107,36 +110,58 @@ void TransectStyleComplexItemTest::_setSurveyAreaPolygon(void) void TransectStyleComplexItemTest::_testRebuildTransects(void) { // Changing the survey polygon should trigger: - // _rebuildTransects call + // _rebuildTransects calls // coveredAreaChanged signal // lastSequenceNumberChanged signal _adjustSurveAreaPolygon(); - QVERIFY(_transectStyleItem->rebuildTransectsCalled); + QVERIFY(_transectStyleItem->rebuildTransectsPhase1Called); + QVERIFY(_transectStyleItem->rebuildTransectsPhase2Called); QVERIFY(_multiSpy->checkSignalsByMask(coveredAreaChangedMask | lastSequenceNumberChangedMask)); - _transectStyleItem->rebuildTransectsCalled = false; + _transectStyleItem->rebuildTransectsPhase1Called = false; + _transectStyleItem->rebuildTransectsPhase2Called = false; _transectStyleItem->setDirty(false); _multiSpy->clearAllSignals(); // Changes to these facts should trigger: - // _rebuildTransects call + // _rebuildTransects calls // lastSequenceNumberChanged signal QList rgFacts; rgFacts << _transectStyleItem->turnAroundDistance() << _transectStyleItem->cameraTriggerInTurnAround() << _transectStyleItem->hoverAndCapture() << _transectStyleItem->refly90Degrees() - << _transectStyleItem->cameraCalc()->adjustedFootprintSide() - << _transectStyleItem->cameraCalc()->adjustedFootprintFrontal(); + << _transectStyleItem->cameraCalc()->frontalOverlap() + << _transectStyleItem->cameraCalc()->sideOverlap(); foreach(Fact* fact, rgFacts) { qDebug() << fact->name(); changeFactValue(fact); - QVERIFY(_transectStyleItem->rebuildTransectsCalled); + QVERIFY(_transectStyleItem->rebuildTransectsPhase1Called); + QVERIFY(_transectStyleItem->rebuildTransectsPhase2Called); QVERIFY(_multiSpy->checkSignalsByMask(lastSequenceNumberChangedMask)); _transectStyleItem->setDirty(false); _multiSpy->clearAllSignals(); - _transectStyleItem->rebuildTransectsCalled = false; + _transectStyleItem->rebuildTransectsPhase1Called = false; + _transectStyleItem->rebuildTransectsPhase2Called = false; } rgFacts.clear(); + + _transectStyleItem->cameraCalc()->valueSetIsDistance()->setRawValue(false); + _transectStyleItem->rebuildTransectsPhase1Called = false; + _transectStyleItem->rebuildTransectsPhase2Called = false; + changeFactValue(_transectStyleItem->cameraCalc()->imageDensity()); + QVERIFY(_transectStyleItem->rebuildTransectsPhase1Called); + QVERIFY(_transectStyleItem->rebuildTransectsPhase2Called); + QVERIFY(_multiSpy->checkSignalsByMask(lastSequenceNumberChangedMask)); + _multiSpy->clearAllSignals(); + + _transectStyleItem->cameraCalc()->valueSetIsDistance()->setRawValue(true); + _transectStyleItem->rebuildTransectsPhase1Called = false; + _transectStyleItem->rebuildTransectsPhase2Called = false; + changeFactValue(_transectStyleItem->cameraCalc()->distanceToSurface()); + QVERIFY(_transectStyleItem->rebuildTransectsPhase1Called); + QVERIFY(_transectStyleItem->rebuildTransectsPhase2Called); + QVERIFY(_multiSpy->checkSignalsByMask(lastSequenceNumberChangedMask)); + _multiSpy->clearAllSignals(); } void TransectStyleComplexItemTest::_testDistanceSignalling(void) @@ -167,14 +192,44 @@ void TransectStyleComplexItemTest::_adjustSurveAreaPolygon(void) _transectStyleItem->surveyAreaPolygon()->adjustVertex(0, vertex); } +void TransectStyleComplexItemTest::_testAltMode(void) +{ + // Default should be relative + QVERIFY(_transectStyleItem->cameraCalc()->distanceToSurfaceRelative()); + + // Manual camera allows non-relative altitudes, validate that changing back to known + // camera switches back to relative + _transectStyleItem->cameraCalc()->cameraName()->setRawValue(_transectStyleItem->cameraCalc()->manualCameraName()); + _transectStyleItem->cameraCalc()->setDistanceToSurfaceRelative(false); + _transectStyleItem->cameraCalc()->cameraName()->setRawValue(_transectStyleItem->cameraCalc()->customCameraName()); + QVERIFY(_transectStyleItem->cameraCalc()->distanceToSurfaceRelative()); + + // When you turn off terrain following mode make sure that the altitude mode changed back to relative altitudes + _transectStyleItem->cameraCalc()->setDistanceToSurfaceRelative(false); + _transectStyleItem->setFollowTerrain(true); + + QVERIFY(!_transectStyleItem->cameraCalc()->distanceToSurfaceRelative()); + QVERIFY(_transectStyleItem->followTerrain()); + + _transectStyleItem->setFollowTerrain(false); + QVERIFY(_transectStyleItem->cameraCalc()->distanceToSurfaceRelative()); + QVERIFY(!_transectStyleItem->followTerrain()); +} + TransectStyleItem::TransectStyleItem(Vehicle* vehicle, QObject* parent) - : TransectStyleComplexItem (vehicle, QStringLiteral("UnitTestTransect"), parent) - , rebuildTransectsCalled (false) + : TransectStyleComplexItem (vehicle, false /* flyView */, QStringLiteral("UnitTestTransect"), parent) + , rebuildTransectsPhase1Called (false) + , rebuildTransectsPhase2Called (false) { } -void TransectStyleItem::_rebuildTransects(void) +void TransectStyleItem::_rebuildTransectsPhase1(void) +{ + rebuildTransectsPhase1Called = true; +} + +void TransectStyleItem::_rebuildTransectsPhase2(void) { - rebuildTransectsCalled = true; + rebuildTransectsPhase2Called = true; } diff --git a/src/MissionManager/TransectStyleComplexItemTest.h b/src/MissionManager/TransectStyleComplexItemTest.h index d3aeea3202af69898f5d74f5098f99b84182756f..dac201cf7094ab41616f67ee51b06910df3ff926 100644 --- a/src/MissionManager/TransectStyleComplexItemTest.h +++ b/src/MissionManager/TransectStyleComplexItemTest.h @@ -29,9 +29,10 @@ protected: void cleanup(void) final; private slots: - void _testDirty (void); - void _testRebuildTransects (void); - void _testDistanceSignalling (void); + void _testDirty (void); + void _testRebuildTransects (void); + void _testDistanceSignalling(void); + void _testAltMode (void); private: void _setSurveyAreaPolygon (void); @@ -41,8 +42,7 @@ private: // These signals are from TransectStyleComplexItem cameraShotsChangedIndex = 0, timeBetweenShotsChangedIndex, - cameraMinTriggerIntervalChangedIndex, - transectPointsChangedIndex, + visualTransectPointsChangedIndex, coveredAreaChangedIndex, // These signals are from ComplexItem dirtyChangedIndex, @@ -56,18 +56,17 @@ private: enum { // These signals are from TransectStyleComplexItem - cameraShotsChangedMask = 1 << cameraShotsChangedIndex, - timeBetweenShotsChangedMask = 1 << timeBetweenShotsChangedIndex, - cameraMinTriggerIntervalChangedMask = 1 << cameraMinTriggerIntervalChangedIndex, - transectPointsChangedMask = 1 << transectPointsChangedIndex, - coveredAreaChangedMask = 1 << coveredAreaChangedIndex, + cameraShotsChangedMask = 1 << cameraShotsChangedIndex, + timeBetweenShotsChangedMask = 1 << timeBetweenShotsChangedIndex, + visualTransectPointsChangedMask = 1 << visualTransectPointsChangedIndex, + coveredAreaChangedMask = 1 << coveredAreaChangedIndex, // These signals are from ComplexItem - dirtyChangedMask = 1 << dirtyChangedIndex, - complexDistanceChangedMask = 1 << complexDistanceChangedIndex, - greatestDistanceToChangedMask = 1 << greatestDistanceToChangedIndex, - additionalTimeDelayChangedMask = 1 << additionalTimeDelayChangedIndex, + dirtyChangedMask = 1 << dirtyChangedIndex, + complexDistanceChangedMask = 1 << complexDistanceChangedIndex, + greatestDistanceToChangedMask = 1 << greatestDistanceToChangedIndex, + additionalTimeDelayChangedMask = 1 << additionalTimeDelayChangedIndex, // These signals are from VisualMissionItem - lastSequenceNumberChangedMask = 1 << lastSequenceNumberChangedIndex, + lastSequenceNumberChangedMask = 1 << lastSequenceNumberChangedIndex, }; static const size_t _cSignals = maxSignalIndex; @@ -87,7 +86,6 @@ public: TransectStyleItem(Vehicle* vehicle, QObject* parent = NULL); // Overrides from ComplexMissionItem - int lastSequenceNumber (void) const final { return _sequenceNumber; } QString mapVisualQML (void) const final { return QString(); } bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final { Q_UNUSED(complexObject); Q_UNUSED(sequenceNumber); Q_UNUSED(errorString); return false; } @@ -97,9 +95,11 @@ public: void appendMissionItems (QList& items, QObject* missionItemParent) final { Q_UNUSED(items); Q_UNUSED(missionItemParent); } void applyNewAltitude (double newAltitude) final { Q_UNUSED(newAltitude); } - bool rebuildTransectsCalled; + bool rebuildTransectsPhase1Called; + bool rebuildTransectsPhase2Called; private slots: // Overrides from TransectStyleComplexItem - void _rebuildTransects (void) final; + void _rebuildTransectsPhase1(void) final; + void _rebuildTransectsPhase2(void) final; }; diff --git a/src/MissionManager/VisualMissionItem.cc b/src/MissionManager/VisualMissionItem.cc index ba309b72f6ac54b434f2ca2d5aca0f85eab7d591..37707d614749954549eec23a8c401226c3f44bea 100644 --- a/src/MissionManager/VisualMissionItem.cc +++ b/src/MissionManager/VisualMissionItem.cc @@ -15,15 +15,16 @@ #include "FirmwarePluginManager.h" #include "QGCApplication.h" #include "JsonHelper.h" -#include "Terrain.h" +#include "TerrainQuery.h" const char* VisualMissionItem::jsonTypeKey = "type"; const char* VisualMissionItem::jsonTypeSimpleItemValue = "SimpleItem"; const char* VisualMissionItem::jsonTypeComplexItemValue = "ComplexItem"; -VisualMissionItem::VisualMissionItem(Vehicle* vehicle, QObject* parent) +VisualMissionItem::VisualMissionItem(Vehicle* vehicle, bool flyView, QObject* parent) : QObject (parent) , _vehicle (vehicle) + , _flyView (flyView) , _isCurrentItem (false) , _dirty (false) , _homePositionSpecialCase (false) @@ -38,20 +39,13 @@ VisualMissionItem::VisualMissionItem(Vehicle* vehicle, QObject* parent) , _lastLatTerrainQuery (0) , _lastLonTerrainQuery (0) { - - // Don't get terrain altitude information for submarines or boards - if (_vehicle->vehicleType() != MAV_TYPE_SUBMARINE && _vehicle->vehicleType() != MAV_TYPE_SURFACE_BOAT) { - _updateTerrainTimer.setInterval(500); - _updateTerrainTimer.setSingleShot(true); - connect(&_updateTerrainTimer, &QTimer::timeout, this, &VisualMissionItem::_reallyUpdateTerrainAltitude); - - connect(this, &VisualMissionItem::coordinateChanged, this, &VisualMissionItem::_updateTerrainAltitude); - } + _commonInit(); } -VisualMissionItem::VisualMissionItem(const VisualMissionItem& other, QObject* parent) +VisualMissionItem::VisualMissionItem(const VisualMissionItem& other, bool flyView, QObject* parent) : QObject (parent) , _vehicle (NULL) + , _flyView (flyView) , _isCurrentItem (false) , _dirty (false) , _homePositionSpecialCase (false) @@ -63,8 +57,17 @@ VisualMissionItem::VisualMissionItem(const VisualMissionItem& other, QObject* pa { *this = other; + _commonInit(); +} + +void VisualMissionItem::_commonInit(void) +{ // Don't get terrain altitude information for submarines or boats if (_vehicle->vehicleType() != MAV_TYPE_SUBMARINE && _vehicle->vehicleType() != MAV_TYPE_SURFACE_BOAT) { + _updateTerrainTimer.setInterval(500); + _updateTerrainTimer.setSingleShot(true); + connect(&_updateTerrainTimer, &QTimer::timeout, this, &VisualMissionItem::_reallyUpdateTerrainAltitude); + connect(this, &VisualMissionItem::coordinateChanged, this, &VisualMissionItem::_updateTerrainAltitude); } } @@ -160,7 +163,7 @@ void VisualMissionItem::setMissionVehicleYaw(double vehicleYaw) void VisualMissionItem::_updateTerrainAltitude(void) { - if (coordinate().isValid()) { + if (!_flyView && coordinate().isValid()) { // We use a timer so that any additional requests before the timer fires result in only a single request _updateTerrainTimer.start(); } @@ -172,18 +175,18 @@ void VisualMissionItem::_reallyUpdateTerrainAltitude(void) if (coord.isValid() && (qIsNaN(_terrainAltitude) || !qFuzzyCompare(_lastLatTerrainQuery, coord.latitude()) || qFuzzyCompare(_lastLonTerrainQuery, coord.longitude()))) { _lastLatTerrainQuery = coord.latitude(); _lastLonTerrainQuery = coord.longitude(); - ElevationProvider* terrain = new ElevationProvider(this); - connect(terrain, &ElevationProvider::terrainData, this, &VisualMissionItem::_terrainDataReceived); + TerrainAtCoordinateQuery* terrain = new TerrainAtCoordinateQuery(this); + connect(terrain, &TerrainAtCoordinateQuery::terrainData, this, &VisualMissionItem::_terrainDataReceived); QList rgCoord; rgCoord.append(coordinate()); - terrain->queryTerrainData(rgCoord); + terrain->requestData(rgCoord); } } -void VisualMissionItem::_terrainDataReceived(bool success, QList altitudes) +void VisualMissionItem::_terrainDataReceived(bool success, QList heights) { if (success) { - _terrainAltitude = altitudes[0]; + _terrainAltitude = heights[0]; emit terrainAltitudeChanged(_terrainAltitude); sender()->deleteLater(); } diff --git a/src/MissionManager/VisualMissionItem.h b/src/MissionManager/VisualMissionItem.h index c473adce121a907ed16a74ead3f4e0fc92a5a8de..7050c6886aa4ba2026427cf9e314ac78f9c7ae66 100644 --- a/src/MissionManager/VisualMissionItem.h +++ b/src/MissionManager/VisualMissionItem.h @@ -18,7 +18,6 @@ #include "QGCMAVLink.h" #include "QGC.h" -#include "MavlinkQmlSingleton.h" #include "QmlObjectListModel.h" #include "Fact.h" #include "QGCLoggingCategory.h" @@ -34,8 +33,8 @@ class VisualMissionItem : public QObject Q_OBJECT public: - VisualMissionItem(Vehicle* vehicle, QObject* parent = NULL); - VisualMissionItem(const VisualMissionItem& other, QObject* parent = NULL); + VisualMissionItem(Vehicle* vehicle, bool flyView, QObject* parent); + VisualMissionItem(const VisualMissionItem& other, bool flyView, QObject* parent); ~VisualMissionItem(); @@ -43,7 +42,7 @@ public: Q_PROPERTY(bool homePosition READ homePosition CONSTANT) ///< true: This item is being used as a home position indicator Q_PROPERTY(QGeoCoordinate coordinate READ coordinate WRITE setCoordinate NOTIFY coordinateChanged) ///< This is the entry point for a waypoint line into the item. For a simple item it is also the location of the item - Q_PROPERTY(double terrainAltitude READ terrainAltitude NOTIFY terrainAltitudeChanged) ///< The altitude of terrain at the coordinate position, NaN if not known + Q_PROPERTY(double terrainAltitude READ terrainAltitude NOTIFY terrainAltitudeChanged) ///< The altitude of terrain at the coordinate position, NaN if not known Q_PROPERTY(bool coordinateHasRelativeAltitude READ coordinateHasRelativeAltitude NOTIFY coordinateHasRelativeAltitudeChanged) ///< true: coordinate.latitude is relative to home altitude Q_PROPERTY(QGeoCoordinate exitCoordinate READ exitCoordinate NOTIFY exitCoordinateChanged) ///< This is the exit point for a waypoint line coming out of the item. Q_PROPERTY(bool exitCoordinateHasRelativeAltitude READ exitCoordinateHasRelativeAltitude NOTIFY exitCoordinateHasRelativeAltitudeChanged) ///< true: coordinate.latitude is relative to home altitude @@ -67,6 +66,7 @@ public: Q_PROPERTY(double specifiedGimbalPitch READ specifiedGimbalPitch NOTIFY specifiedGimbalPitchChanged) ///< Gimbal pitch, NaN for not specified Q_PROPERTY(double missionGimbalYaw READ missionGimbalYaw NOTIFY missionGimbalYawChanged) ///< Current gimbal yaw state at this point in mission Q_PROPERTY(double missionVehicleYaw READ missionVehicleYaw NOTIFY missionVehicleYawChanged) ///< Expected vehicle yaw at this point in mission + Q_PROPERTY(bool flyView READ flyView CONSTANT) // The following properties are calculated/set by the MissionController recalc methods @@ -88,6 +88,7 @@ public: double distance (void) const { return _distance; } bool isCurrentItem (void) const { return _isCurrentItem; } double terrainAltitude (void) const { return _terrainAltitude; } + bool flyView (void) const { return _flyView; } QmlObjectListModel* childItems(void) { return &_childItems; } @@ -130,6 +131,11 @@ public: virtual void setSequenceNumber (int sequenceNumber) = 0; virtual int lastSequenceNumber (void) const = 0; + /// Specifies whether the item has all the data it needs such that it can be saved. Currently the only + /// case where this returns false is if it has not determined terrain values yet. + /// @return true: Ready to save, false: Still waiting on information + virtual bool readyForSave(void) const { return true; } + /// Save the item(s) in Json format /// @param missionItems Current set of mission items, new items should be appended to the end virtual void save(QJsonArray& missionItems) = 0; @@ -185,6 +191,7 @@ signals: protected: Vehicle* _vehicle; + bool _flyView; bool _isCurrentItem; bool _dirty; bool _homePositionSpecialCase; ///< true: This item is being used as a ui home position indicator @@ -206,9 +213,11 @@ protected: private slots: void _updateTerrainAltitude (void); void _reallyUpdateTerrainAltitude (void); - void _terrainDataReceived (bool success, QList altitudes); + void _terrainDataReceived (bool success, QList heights); private: + void _commonInit(void); + QTimer _updateTerrainTimer; double _lastLatTerrainQuery; double _lastLonTerrainQuery; diff --git a/src/PlanView/CameraCalc.qml b/src/PlanView/CameraCalc.qml index 6f0d33e45b9ec966e36050fc3af9fb0e9b070ee1..95c531f016ebed6cca2051bb760e98e370fbc714 100644 --- a/src/PlanView/CameraCalc.qml +++ b/src/PlanView/CameraCalc.qml @@ -38,11 +38,11 @@ Column { _cameraList.push(_vehicle.staticCameraList[i].name) } gridTypeCombo.model = _cameraList - var knownCameraIndex = gridTypeCombo.find(cameraCalc.cameraName) + var knownCameraIndex = gridTypeCombo.find(cameraCalc.cameraName.value) if (knownCameraIndex !== -1) { gridTypeCombo.currentIndex = knownCameraIndex } else { - console.log("Internal error: Known camera not found", cameraCalc.cameraName) + console.log("Internal error: Known camera not found", cameraCalc.cameraName.value) gridTypeCombo.currentIndex = _gridTypeCustomCamera } } @@ -73,7 +73,7 @@ Column { anchors.right: parent.right model: _cameraList currentIndex: -1 - onActivated: cameraCalc.cameraName = gridTypeCombo.textAt(index) + onActivated: cameraCalc.cameraName.value = gridTypeCombo.textAt(index) } // QGCComboxBox // Camera based grid ui @@ -81,7 +81,7 @@ Column { anchors.left: parent.left anchors.right: parent.right spacing: _margin - visible: cameraCalc.cameraName !== cameraCalc.manualCameraName + visible: !cameraCalc.isManualCamera Row { spacing: _margin @@ -111,7 +111,7 @@ Column { anchors.left: parent.left anchors.right: parent.right spacing: _margin - visible: cameraCalc.cameraName === cameraCalc.customCameraName + visible: cameraCalc.isCustomCamera RowLayout { anchors.left: parent.left @@ -234,7 +234,7 @@ Column { QGCRadioButton { id: fixedImageDensityRadio - text: qsTr("Image density") + text: qsTr("Ground Res") checked: !cameraCalc.valueSetIsDistance.value exclusiveGroup: fixedValueGroup onClicked: cameraCalc.valueSetIsDistance.value = 0 @@ -282,7 +282,7 @@ Column { columnSpacing: _margin rowSpacing: _margin columns: 2 - visible: cameraCalc.cameraName === cameraCalc.manualCameraName + visible: cameraCalc.isManualCamera QGCLabel { text: distanceToSurfaceLabel } FactTextField { diff --git a/src/PlanView/CameraSection.qml b/src/PlanView/CameraSection.qml index b91cd52b5e8d0413969f4bf1140185fb3b5cb3c8..0a01e62e09489081785e110d83f2f4ca54c582c2 100644 --- a/src/PlanView/CameraSection.qml +++ b/src/PlanView/CameraSection.qml @@ -74,6 +74,26 @@ Column { } } + RowLayout { + anchors.left: parent.left + anchors.right: parent.right + spacing: ScreenTools.defaultFontPixelWidth + visible: _camera.cameraModeSupported + + QGCCheckBox { + id: modeCheckBox + text: qsTr("Mode") + checked: _camera.specifyCameraMode + onClicked: _camera.specifyCameraMode = checked + } + FactComboBox { + fact: _camera.cameraMode + indexModel: false + enabled: modeCheckBox.checked + Layout.fillWidth: true + } + } + GridLayout { anchors.left: parent.left anchors.right: parent.right @@ -104,25 +124,5 @@ Column { enabled: gimbalCheckBox.checked } } - - RowLayout { - anchors.left: parent.left - anchors.right: parent.right - spacing: ScreenTools.defaultFontPixelWidth - visible: _camera.cameraModeSupported - - QGCCheckBox { - id: modeCheckBox - text: qsTr("Mode") - checked: _camera.specifyCameraMode - onClicked: _camera.specifyCameraMode = checked - } - FactComboBox { - fact: _camera.cameraMode - indexModel: false - enabled: modeCheckBox.checked - Layout.fillWidth: true - } - } } } diff --git a/src/PlanView/CorridorScanEditor.qml b/src/PlanView/CorridorScanEditor.qml index 72cd5d3d80a7bb1f5672f00b8f6373b85ead7873..ab42e20eaae9b73e3f4d1d846b2234954878194c 100644 --- a/src/PlanView/CorridorScanEditor.qml +++ b/src/PlanView/CorridorScanEditor.qml @@ -13,7 +13,6 @@ import QGroundControl.FactControls 1.0 import QGroundControl.Palette 1.0 import QGroundControl.FlightMap 1.0 -// Editor for Survery mission items Rectangle { id: _root height: visible ? (editorColumn.height + (_margin * 2)) : 0 @@ -25,9 +24,10 @@ Rectangle { //property real availableWidth ///< Width for control //property var missionItem ///< Mission Item for editor - property real _margin: ScreenTools.defaultFontPixelWidth / 2 - property real _fieldWidth: ScreenTools.defaultFontPixelWidth * 10.5 - property var _vehicle: QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle : QGroundControl.multiVehicleManager.offlineEditingVehicle + property real _margin: ScreenTools.defaultFontPixelWidth / 2 + property real _fieldWidth: ScreenTools.defaultFontPixelWidth * 10.5 + property var _vehicle: QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle : QGroundControl.multiVehicleManager.offlineEditingVehicle + property real _cameraMinTriggerInterval: missionItem.cameraCalc.minTriggerInterval.rawValue function polygonCaptureStarted() { missionItem.clearPolygon() @@ -59,12 +59,13 @@ Rectangle { QGCLabel { anchors.left: parent.left anchors.right: parent.right - text: qsTr("WARNING: Photo interval is below minimum interval (%1 secs) supported by camera.").arg(missionItem.cameraMinTriggerInterval.toFixed(1)) + text: qsTr("WARNING: Photo interval is below minimum interval (%1 secs) supported by camera.").arg(_cameraMinTriggerInterval.toFixed(1)) wrapMode: Text.WordWrap color: qgcPal.warningText - visible: missionItem.cameraShots > 0 && missionItem.cameraMinTriggerInterval !== 0 && missionItem.cameraMinTriggerInterval > missionItem.timeBetweenShots + visible: missionItem.cameraShots > 0 && _cameraMinTriggerInterval !== 0 && _cameraMinTriggerInterval > missionItem.timeBetweenShots } + CameraCalc { cameraCalc: missionItem.cameraCalc vehicleFlightIsFrontal: true @@ -107,10 +108,11 @@ Rectangle { QGCCheckBox { id: relAlt - anchors.left: parent.left text: qsTr("Relative altitude") checked: missionItem.cameraCalc.distanceToSurfaceRelative - enabled: missionItem.cameraCalc.isManualCamera + enabled: missionItem.cameraCalc.isManualCamera && !missionItem.followTerrain + visible: QGroundControl.corePlugin.options.showMissionAbsoluteAltitude || (!missionItem.cameraCalc.distanceToSurfaceRelative && !missionItem.followTerrain) + Layout.alignment: Qt.AlignLeft Layout.columnSpan: 2 onClicked: missionItem.cameraCalc.distanceToSurfaceRelative = checked @@ -127,20 +129,56 @@ Rectangle { } SectionHeader { - id: statsHeader - text: qsTr("Statistics") + id: terrainHeader + text: qsTr("Terrain") + checked: missionItem.followTerrain } - Grid { - columns: 2 - columnSpacing: ScreenTools.defaultFontPixelWidth - visible: statsHeader.checked + ColumnLayout { + anchors.left: parent.left + anchors.right: parent.right + spacing: _margin + visible: terrainHeader.checked + + QGCCheckBox { + id: followsTerrainCheckBox + text: qsTr("Vehicle follows terrain") + checked: missionItem.followTerrain + onClicked: missionItem.followTerrain = checked + } + + GridLayout { + Layout.fillWidth: true + columnSpacing: _margin + rowSpacing: _margin + columns: 2 + visible: followsTerrainCheckBox.checked + + QGCLabel { text: qsTr("Tolerance") } + FactTextField { + fact: missionItem.terrainAdjustTolerance + Layout.fillWidth: true + } + + QGCLabel { text: qsTr("Max Climb Rate") } + FactTextField { + fact: missionItem.terrainAdjustMaxClimbRate + Layout.fillWidth: true + } - QGCLabel { text: qsTr("Photo count") } - QGCLabel { text: missionItem.cameraShots } + QGCLabel { text: qsTr("Max Descent Rate") } + FactTextField { + fact: missionItem.terrainAdjustMaxDescentRate + Layout.fillWidth: true + } + } + } - QGCLabel { text: qsTr("Photo interval") } - QGCLabel { text: missionItem.timeBetweenShots.toFixed(1) + " " + qsTr("secs") } + SectionHeader { + id: statsHeader + text: qsTr("Statistics") } + + TransectStyleComplexItemStats { } } // Column } // Rectangle diff --git a/src/PlanView/CorridorScanMapVisual.qml b/src/PlanView/CorridorScanMapVisual.qml index 616120b29c42e6c6a8c8c9fe7a6b6b8469534103..762fb5a209f03bfd8928d9bff14ae6f21485823f 100644 --- a/src/PlanView/CorridorScanMapVisual.qml +++ b/src/PlanView/CorridorScanMapVisual.qml @@ -119,7 +119,7 @@ Item { MapPolyline { line.color: "white" line.width: 2 - path: _missionItem.transectPoints + path: _missionItem.visualTransectPoints } } } diff --git a/src/PlanView/FWLandingPatternEditor.qml b/src/PlanView/FWLandingPatternEditor.qml index b703e9a63fc1cf02a1617871734e52dc7895259b..9ea64587a618ec9e3e2230707f760986620ee2f1 100644 --- a/src/PlanView/FWLandingPatternEditor.qml +++ b/src/PlanView/FWLandingPatternEditor.qml @@ -34,6 +34,8 @@ Rectangle { property real _margin: ScreenTools.defaultFontPixelWidth / 2 property real _spacer: ScreenTools.defaultFontPixelWidth / 2 + ExclusiveGroup { id: distanceGlideGroup } + Column { id: editorColumn anchors.margins: _margin @@ -58,17 +60,6 @@ Rectangle { Item { width: 1; height: _spacer } QGCCheckBox { - id: loiterAltRelative - anchors.right: parent.right - text: qsTr("Altitude relative to home") - checked: missionItem.loiterAltitudeRelative - onClicked: missionItem.loiterAltitudeRelative = checked - } - - Item { width: 1; height: _spacer } - - QGCCheckBox { - anchors.left: loiterAltRelative.left text: qsTr("Loiter clockwise") checked: missionItem.loiterClockwise onClicked: missionItem.loiterClockwise = checked @@ -98,53 +89,33 @@ Rectangle { } QGCRadioButton { - id: useLandingDistance - text: qsTr("Landing dist") - checked: !useFallRate.checked - onClicked: { - useFallRate.checked = false - missionItem.fallRate.value = parseFloat(missionItem.loiterAltitude.value)*100/parseFloat (missionItem.landingDistance.value) - } - Layout.fillWidth: true + id: specifyLandingDistance + text: qsTr("Landing Dist") + checked: missionItem.valueSetIsDistance + exclusiveGroup: distanceGlideGroup + onClicked: missionItem.valueSetIsDistance = checked + Layout.fillWidth: true } FactTextField { - fact: missionItem.landingDistance - enabled: useLandingDistance.checked - Layout.fillWidth: true + fact: missionItem.landingDistance + enabled: specifyLandingDistance.checked + Layout.fillWidth: true } QGCRadioButton { - id: useFallRate - text: qsTr("Descent rate") - checked: !useLandingDistance.checked - onClicked: { - useLandingDistance.checked = false - missionItem.landingDistance.value = parseFloat(missionItem.loiterAltitude.value)*100/parseFloat (missionItem.fallRate.value) - } - Layout.fillWidth: true + id: specifyGlideSlope + text: qsTr("Glide Slope") + checked: !missionItem.valueSetIsDistance + exclusiveGroup: distanceGlideGroup + onClicked: missionItem.valueSetIsDistance = !checked + Layout.fillWidth: true } FactTextField { - fact: missionItem.fallRate - enabled: useFallRate.checked - Layout.fillWidth: true - } - - Connections { - target: missionItem.landingDistance - - onValueChanged: { - missionItem.fallRate.value = parseFloat(missionItem.loiterAltitude.value)*100/parseFloat (missionItem.landingDistance.value) - } - } - - Connections { - target: missionItem.fallRate - - onValueChanged: { - missionItem.landingDistance.value = parseFloat(missionItem.loiterAltitude.value)*100/parseFloat (missionItem.fallRate.value) - } + fact: missionItem.glideSlope + enabled: specifyGlideSlope.checked + Layout.fillWidth: true } } @@ -152,9 +123,10 @@ Rectangle { QGCCheckBox { anchors.right: parent.right - text: qsTr("Altitude relative to home") - checked: missionItem.landingAltitudeRelative - onClicked: missionItem.landingAltitudeRelative = checked + text: qsTr("Altitudes relative to home") + checked: missionItem.altitudesAreRelative + visible: QGroundControl.corePlugin.options.showMissionAbsoluteAltitude || !missionItem.altitudesAreRelative + onClicked: missionItem.altitudesAreRelative = checked } } diff --git a/src/PlanView/FWLandingPatternMapVisual.qml b/src/PlanView/FWLandingPatternMapVisual.qml index b6fc98884562e196a6efabe84b8c4016dbeb816c..21e652df10832b92641a600b4149921b85e755cf 100644 --- a/src/PlanView/FWLandingPatternMapVisual.qml +++ b/src/PlanView/FWLandingPatternMapVisual.qml @@ -36,7 +36,8 @@ Item { readonly property int _flightPathIndex: 0 readonly property int _loiterPointIndex: 1 readonly property int _loiterRadiusIndex: 2 - readonly property int _landPointIndex: 3 + readonly property int _landingAreaIndex: 3 + readonly property int _landPointIndex: 4 function hideItemVisuals() { for (var i=0; i<_itemVisuals.length; i++) { @@ -56,6 +57,9 @@ Item { itemVisual = loiterRadiusComponent.createObject(map) map.addMapItem(itemVisual) _itemVisuals[_loiterRadiusIndex] = itemVisual + itemVisual = landingAreaComponent.createObject(map) + map.addMapItem(itemVisual) + _itemVisuals[_landingAreaIndex] = itemVisual itemVisual = landPointComponent.createObject(map) map.addMapItem(itemVisual) _itemVisuals[_landPointIndex] = itemVisual @@ -97,11 +101,11 @@ Item { Component.onCompleted: { if (_missionItem.landingCoordSet) { showItemVisuals() - if (_missionItem.isCurrentItem) { + if (!_missionItem.flyView && _missionItem.isCurrentItem) { showDragAreas() } _setFlightPath() - } else if (_missionItem.isCurrentItem) { + } else if (!_missionItem.flyView && _missionItem.isCurrentItem) { showMouseArea() } } @@ -116,6 +120,9 @@ Item { target: _missionItem onIsCurrentItemChanged: { + if (_missionItem.flyView) { + return + } if (_missionItem.isCurrentItem) { if (_missionItem.landingCoordSet) { showDragAreas() @@ -129,6 +136,9 @@ Item { } onLandingCoordSetChanged: { + if (_missionItem.flyView) { + return + } if (_missionItem.landingCoordSet) { hideMouseArea() showItemVisuals() @@ -218,7 +228,6 @@ Item { } } - // Loiter radius visual Component { id: loiterRadiusComponent @@ -232,7 +241,6 @@ Item { } } - // Land point Component { id: landPointComponent @@ -252,4 +260,40 @@ Item { } } } + + Component { + id: landingAreaComponent + + MapPolygon { + z: QGroundControl.zOrderMapItems + border.width: 1 + border.color: "black" + color: "green" + opacity: 0.5 + + readonly property real landingWidth: 15 + readonly property real landingLength: 100 + readonly property real angleRadians: Math.atan((landingWidth / 2) / (landingLength / 2)) + readonly property real angleDegrees: (angleRadians * (180 / Math.PI)) + readonly property real hypotenuse: (landingWidth / 2) / Math.sin(angleRadians) + + property real landingAreaAngle: _missionItem.landingCoordinate.azimuthTo(_missionItem.loiterTangentCoordinate) + + function calcPoly() { + path = [ ] + addCoordinate(_missionItem.landingCoordinate.atDistanceAndAzimuth(hypotenuse, landingAreaAngle - angleDegrees)) + addCoordinate(_missionItem.landingCoordinate.atDistanceAndAzimuth(hypotenuse, landingAreaAngle + angleDegrees)) + addCoordinate(_missionItem.landingCoordinate.atDistanceAndAzimuth(hypotenuse, landingAreaAngle + (180 - angleDegrees))) + addCoordinate(_missionItem.landingCoordinate.atDistanceAndAzimuth(hypotenuse, landingAreaAngle - (180 - angleDegrees))) + } + + Component.onCompleted: calcPoly() + + Connections { + target: _missionItem + onLandingCoordinateChanged: calcPoly() + onLoiterTangentCoordinateChanged: calcPoly() + } + } + } } diff --git a/src/PlanView/GeoFenceMapVisuals.qml b/src/PlanView/GeoFenceMapVisuals.qml index 9e41cc65f3555abe0fac680d1254836f2299eb05..371a64a4ca546f00aa288da2e7d77c367918e4cc 100644 --- a/src/PlanView/GeoFenceMapVisuals.qml +++ b/src/PlanView/GeoFenceMapVisuals.qml @@ -28,10 +28,18 @@ Item { property bool planView: false ///< true: visuals showing in plan view property var homePosition - property var _breachReturnPointComponent - property var _mouseAreaComponent + //property var _breachReturnPointComponent + //property var _mouseAreaComponent + property var _paramCircleFenceComponent property var _polygons: myGeoFenceController.polygons property var _circles: myGeoFenceController.circles + property color _borderColor: "orange" + property int _borderWidthInclusion: 2 + property int _borderWidthExclusion: 0 + property color _interiorColorExclusion: "orange" + property color _interiorColorInclusion: "transparent" + property real _interiorOpacityExclusion: 0.2 + property real _interiorOpacityInclusion: 1 function addPolygon(inclusionPolygon) { // Initial polygon is inset to take 2/3rds space @@ -67,14 +75,17 @@ Item { } Component.onCompleted: { - _breachReturnPointComponent = breachReturnPointComponent.createObject(map) - map.addMapItem(_breachReturnPointComponent) - _mouseAreaComponent = mouseAreaComponent.createObject(map) + //_breachReturnPointComponent = breachReturnPointComponent.createObject(map) + //map.addMapItem(_breachReturnPointComponent) + //_mouseAreaComponent = mouseAreaComponent.createObject(map) + _paramCircleFenceComponent = paramCircleFenceComponent.createObject(map) + map.addMapItem(_paramCircleFenceComponent) } Component.onDestruction: { - _breachReturnPointComponent.destroy() - _mouseAreaComponent.destroy() + //_breachReturnPointComponent.destroy() + //_mouseAreaComponent.destroy() + _paramCircleFenceComponent.destroy() } // Mouse area to capture breach return point coordinate @@ -94,10 +105,10 @@ Item { delegate : QGCMapPolygonVisuals { mapControl: map mapPolygon: object - borderWidth: object.inclusion ? 2 : 0 - borderColor: "orange" - interiorColor: object.inclusion ? "transparent" : "orange" - interiorOpacity: object.inclusion ? 1: 0.2 + borderWidth: object.inclusion ? _borderWidthInclusion : _borderWidthExclusion + borderColor: _borderColor + interiorColor: object.inclusion ? _interiorColorInclusion : _interiorColorExclusion + interiorOpacity: object.inclusion ? _interiorOpacityInclusion : _interiorOpacityExclusion } } @@ -107,10 +118,29 @@ Item { delegate : QGCMapCircleVisuals { mapControl: map mapCircle: object - borderWidth: object.inclusion ? 2 : 0 - borderColor: "orange" - interiorColor: object.inclusion ? "transparent" : "orange" - interiorOpacity: object.inclusion ? 1: 0.2 + borderWidth: object.inclusion ? _borderWidthInclusion : _borderWidthExclusion + borderColor: _borderColor + interiorColor: object.inclusion ? _interiorColorInclusion : _interiorColorExclusion + interiorOpacity: object.inclusion ? _interiorOpacityInclusion : _interiorOpacityExclusion + } + } + + // Circular geofence specified from parameter + Component { + id: paramCircleFenceComponent + + MapCircle { + color: _interiorColorInclusion + opacity: _interiorOpacityInclusion + border.color: _borderColor + border.width: _borderWidthInclusion + center: homePosition + radius: _radius + visible: homePosition.isValid && _radius > 0 + + property real _radius: myGeoFenceController.paramCircularFence + + on_RadiusChanged: console.log("_radius", _radius, homePosition.isValid, homePosition) } } diff --git a/src/PlanView/MissionItemEditor.qml b/src/PlanView/MissionItemEditor.qml index 18083de522dc4814967d09be7cf3d9312f2bc0ce..26dee4bedeb7d0ec269b67d127a363a499345ddb 100644 --- a/src/PlanView/MissionItemEditor.qml +++ b/src/PlanView/MissionItemEditor.qml @@ -76,7 +76,7 @@ Rectangle { anchors.verticalCenter: commandPicker.verticalCenter anchors.leftMargin: _margin anchors.left: parent.left - text: missionItem.homePosition ? "H" : missionItem.sequenceNumber + text: missionItem.homePosition ? "P" : missionItem.sequenceNumber color: _outerTextColor } diff --git a/src/PlanView/MissionItemStatus.qml b/src/PlanView/MissionItemStatus.qml index e5a2dd74599ea44fb0ed3024a0381fcbcdbd6b22..00d1cab92223582af387599a701574e14bc83797 100644 --- a/src/PlanView/MissionItemStatus.qml +++ b/src/PlanView/MissionItemStatus.qml @@ -72,9 +72,9 @@ Rectangle { width: display ? (indicator.width + spacing) : 0 visible: display - property real availableHeight: height - indicator.height - property bool showTerrain: !isNaN(object.terrainPercent) - property real _terrainPercent: showTerrain ? object.terrainPercent : 0 + property real availableHeight: height - indicator.height + property bool _terrainAvailable: !isNaN(object.terrainPercent) + property real _terrainPercent: _terrainAvailable ? object.terrainPercent : 1 readonly property bool display: object.specifiesCoordinate && !object.isStandaloneCoordinate readonly property real spacing: ScreenTools.defaultFontPixelWidth * ScreenTools.smallFontPointRatio @@ -83,9 +83,8 @@ Rectangle { anchors.bottom: parent.bottom anchors.horizontalCenter: parent.horizontalCenter width: indicator.width - height: Math.max(availableHeight * _terrainPercent, 1) - color: _terrainPercent > object.altPercent ? "red": qgcPal.text - visible: !isNaN(object.terrainPercent) + height: _terrainAvailable ? Math.max(availableHeight * _terrainPercent, 1) : parent.height + color: _terrainAvailable ? (_terrainPercent > object.altPercent ? "red": qgcPal.text) : "yellow" } MissionItemIndexLabel { diff --git a/src/PlanView/MissionSettingsEditor.qml b/src/PlanView/MissionSettingsEditor.qml index 544fa893fa6c4013a19b8f1dcfa4407a19695859..a9e0f9897e2e90c3b7e1e48d1f9323369a2bd77e 100644 --- a/src/PlanView/MissionSettingsEditor.qml +++ b/src/PlanView/MissionSettingsEditor.qml @@ -36,6 +36,8 @@ Rectangle { property var _fileExtension: QGroundControl.settingsManager.appSettings.missionFileExtension property var _appSettings: QGroundControl.settingsManager.appSettings property bool _waypointsOnlyMode: QGroundControl.corePlugin.options.missionWaypointsOnly + property bool _showCameraSection: !_waypointsOnlyMode || QGroundControl.corePlugin.showAdvancedUI + property bool _simpleMissionStart: QGroundControl.corePlugin.options.showSimpleMissionStart readonly property string _firmwareLabel: qsTr("Firmware") readonly property string _vehicleLabel: qsTr("Vehicle") @@ -52,153 +54,84 @@ Rectangle { anchors.top: parent.top spacing: _margin - Column { - anchors.left: parent.left - anchors.right: parent.right - spacing: _margin - - GridLayout { - anchors.left: parent.left - anchors.right: parent.right - columnSpacing: ScreenTools.defaultFontPixelWidth - rowSpacing: columnSpacing - columns: 2 - - QGCLabel { - text: qsTr("Waypoint alt") - } - FactTextField { - fact: QGroundControl.settingsManager.appSettings.defaultMissionItemAltitude - Layout.fillWidth: true - } - - QGCCheckBox { - id: flightSpeedCheckBox - text: qsTr("Flight speed") - visible: !_missionVehicle.vtol - checked: missionItem.speedSection.specifyFlightSpeed - onClicked: missionItem.speedSection.specifyFlightSpeed = checked - } - FactTextField { - Layout.fillWidth: true - fact: missionItem.speedSection.flightSpeed - visible: flightSpeedCheckBox.visible - enabled: flightSpeedCheckBox.checked - } - } // GridLayout - } - - CameraSection { - id: cameraSection - checked: missionItem.cameraSection.settingsSpecified - } - - QGCLabel { - anchors.left: parent.left - anchors.right: parent.right - text: qsTr("Above camera commands will take affect immediately upon mission start.") - wrapMode: Text.WordWrap - horizontalAlignment: Text.AlignHCenter - font.pointSize: ScreenTools.smallFontPointSize - visible: cameraSection.checked - } - - SectionHeader { - id: missionEndHeader - text: qsTr("Mission End") - checked: true - } - - Column { - anchors.left: parent.left - anchors.right: parent.right - spacing: _margin - visible: missionEndHeader.checked - - QGCCheckBox { - text: qsTr("Return To Launch") - checked: missionItem.missionEndRTL - onClicked: missionItem.missionEndRTL = checked - } - } - - - SectionHeader { - id: vehicleInfoSectionHeader - text: qsTr("Vehicle Info") - visible: _offlineEditing && !_waypointsOnlyMode - checked: false - } - GridLayout { anchors.left: parent.left anchors.right: parent.right columnSpacing: ScreenTools.defaultFontPixelWidth rowSpacing: columnSpacing columns: 2 - visible: vehicleInfoSectionHeader.visible && vehicleInfoSectionHeader.checked QGCLabel { - text: _firmwareLabel - Layout.fillWidth: true - visible: _showOfflineVehicleCombos - } - FactComboBox { - fact: QGroundControl.settingsManager.appSettings.offlineEditingFirmwareType - indexModel: false - Layout.preferredWidth: _fieldWidth - visible: _showOfflineVehicleCombos - enabled: _enableOfflineVehicleCombos + text: qsTr("Waypoint alt") } - - QGCLabel { - text: _vehicleLabel + FactTextField { + fact: QGroundControl.settingsManager.appSettings.defaultMissionItemAltitude Layout.fillWidth: true - visible: _showOfflineVehicleCombos - } - FactComboBox { - fact: QGroundControl.settingsManager.appSettings.offlineEditingVehicleType - indexModel: false - Layout.preferredWidth: _fieldWidth - visible: _showOfflineVehicleCombos - enabled: _enableOfflineVehicleCombos } - QGCLabel { - text: qsTr("Cruise speed") - visible: _showCruiseSpeed - Layout.fillWidth: true + QGCCheckBox { + id: flightSpeedCheckBox + text: qsTr("Flight speed") + visible: !_missionVehicle.vtol && !_simpleMissionStart + checked: missionItem.speedSection.specifyFlightSpeed + onClicked: missionItem.speedSection.specifyFlightSpeed = checked } FactTextField { - fact: QGroundControl.settingsManager.appSettings.offlineEditingCruiseSpeed - visible: _showCruiseSpeed - Layout.preferredWidth: _fieldWidth - } - - QGCLabel { - text: qsTr("Hover speed") - visible: _showHoverSpeed Layout.fillWidth: true + fact: missionItem.speedSection.flightSpeed + visible: flightSpeedCheckBox.visible + enabled: flightSpeedCheckBox.checked } - FactTextField { - fact: QGroundControl.settingsManager.appSettings.offlineEditingHoverSpeed - visible: _showHoverSpeed - Layout.preferredWidth: _fieldWidth - } - } // GridLayout - - SectionHeader { - id: plannedHomePositionSection - text: qsTr("Planned Home Position") - visible: !_vehicleHasHomePosition - checked: false } Column { anchors.left: parent.left anchors.right: parent.right spacing: _margin - visible: plannedHomePositionSection.checked && !_vehicleHasHomePosition + visible: !_simpleMissionStart + + CameraSection { + id: cameraSection + checked: missionItem.cameraSection.settingsSpecified + visible: _showCameraSection + } + + QGCLabel { + anchors.left: parent.left + anchors.right: parent.right + text: qsTr("Above camera commands will take affect immediately upon mission start.") + wrapMode: Text.WordWrap + horizontalAlignment: Text.AlignHCenter + font.pointSize: ScreenTools.smallFontPointSize + visible: _showCameraSection && cameraSection.checked + } + + SectionHeader { + id: missionEndHeader + text: qsTr("Mission End") + checked: true + } + + Column { + anchors.left: parent.left + anchors.right: parent.right + spacing: _margin + visible: missionEndHeader.checked + + QGCCheckBox { + text: qsTr("Return To Launch") + checked: missionItem.missionEndRTL + onClicked: missionItem.missionEndRTL = checked + } + } + + + SectionHeader { + id: vehicleInfoSectionHeader + text: qsTr("Vehicle Info") + visible: _offlineEditing && !_waypointsOnlyMode + checked: false + } GridLayout { anchors.left: parent.left @@ -206,29 +139,100 @@ Rectangle { columnSpacing: ScreenTools.defaultFontPixelWidth rowSpacing: columnSpacing columns: 2 + visible: vehicleInfoSectionHeader.visible && vehicleInfoSectionHeader.checked QGCLabel { - text: qsTr("Altitude") + text: _firmwareLabel + Layout.fillWidth: true + visible: _showOfflineVehicleCombos + } + FactComboBox { + fact: QGroundControl.settingsManager.appSettings.offlineEditingFirmwareType + indexModel: false + Layout.preferredWidth: _fieldWidth + visible: _showOfflineVehicleCombos + enabled: _enableOfflineVehicleCombos + } + + QGCLabel { + text: _vehicleLabel + Layout.fillWidth: true + visible: _showOfflineVehicleCombos + } + FactComboBox { + fact: QGroundControl.settingsManager.appSettings.offlineEditingVehicleType + indexModel: false + Layout.preferredWidth: _fieldWidth + visible: _showOfflineVehicleCombos + enabled: _enableOfflineVehicleCombos + } + + QGCLabel { + text: qsTr("Cruise speed") + visible: _showCruiseSpeed + Layout.fillWidth: true } FactTextField { - fact: missionItem.plannedHomePositionAltitude + fact: QGroundControl.settingsManager.appSettings.offlineEditingCruiseSpeed + visible: _showCruiseSpeed + Layout.preferredWidth: _fieldWidth + } + + QGCLabel { + text: qsTr("Hover speed") + visible: _showHoverSpeed Layout.fillWidth: true } - } + FactTextField { + fact: QGroundControl.settingsManager.appSettings.offlineEditingHoverSpeed + visible: _showHoverSpeed + Layout.preferredWidth: _fieldWidth + } + } // GridLayout - QGCLabel { - width: parent.width - wrapMode: Text.WordWrap - font.pointSize: ScreenTools.smallFontPointSize - text: qsTr("Actual position set by vehicle at flight time.") - horizontalAlignment: Text.AlignHCenter + SectionHeader { + id: plannedHomePositionSection + text: qsTr("Planned Home Position") + visible: !_vehicleHasHomePosition + checked: false } - QGCButton { - text: qsTr("Set Home To Map Center") - onClicked: missionItem.coordinate = map.center - anchors.horizontalCenter: parent.horizontalCenter + Column { + anchors.left: parent.left + anchors.right: parent.right + spacing: _margin + visible: plannedHomePositionSection.checked && !_vehicleHasHomePosition + + GridLayout { + anchors.left: parent.left + anchors.right: parent.right + columnSpacing: ScreenTools.defaultFontPixelWidth + rowSpacing: columnSpacing + columns: 2 + + QGCLabel { + text: qsTr("Altitude") + } + FactTextField { + fact: missionItem.plannedHomePositionAltitude + Layout.fillWidth: true + } + } + + QGCLabel { + width: parent.width + wrapMode: Text.WordWrap + font.pointSize: ScreenTools.smallFontPointSize + text: qsTr("Actual position set by vehicle at flight time.") + horizontalAlignment: Text.AlignHCenter + } + + QGCButton { + text: qsTr("Set Home To Map Center") + onClicked: missionItem.coordinate = map.center + anchors.horizontalCenter: parent.horizontalCenter + } } - } + } // Column } // Column } // Rectangle diff --git a/src/PlanView/PlanToolBar.qml b/src/PlanView/PlanToolBar.qml index 754f3a38442f270112d9c47b376e7c8d9d75c3a9..3487257f0eec2dc4fc3b1d2aa1568b4c47a6d16b 100644 --- a/src/PlanView/PlanToolBar.qml +++ b/src/PlanView/PlanToolBar.qml @@ -116,6 +116,7 @@ Rectangle { if (_controllerProgressPct === 1) { missionStats.visible = false uploadCompleteText.visible = true + progressBar.visible = false resetProgressTimer.start() } else if (_controllerProgressPct > 0) { progressBar.visible = true @@ -128,20 +129,9 @@ Rectangle { onTriggered: { missionStats.visible = true uploadCompleteText.visible = false - progressBar.visible = false } } - Rectangle { - id: progressBar - anchors.left: parent.left - anchors.bottom: parent.bottom - height: 4 - width: _controllerProgressPct * parent.width - color: qgcPal.colorGreen - visible: false - } - QGCLabel { id: uploadCompleteText anchors.top: parent.top @@ -167,11 +157,10 @@ Rectangle { columns: 3 GridLayout { - anchors.verticalCenter: parent.verticalCenter columns: 8 rowSpacing: _rowSpacing columnSpacing: _labelToValueSpacing - Layout.alignment: Qt.AlignHCenter + Layout.alignment: Qt.AlignVCenter | Qt.AlignHCenter QGCLabel { text: qsTr("Selected Waypoint") @@ -222,11 +211,10 @@ Rectangle { } GridLayout { - anchors.verticalCenter: parent.verticalCenter columns: 5 rowSpacing: _rowSpacing columnSpacing: _labelToValueSpacing - Layout.alignment: Qt.AlignHCenter + Layout.alignment: Qt.AlignVCenter | Qt.AlignHCenter QGCLabel { text: qsTr("Total Mission") @@ -259,11 +247,10 @@ Rectangle { } GridLayout { - anchors.verticalCenter: parent.verticalCenter columns: 3 rowSpacing: _rowSpacing columnSpacing: _labelToValueSpacing - Layout.alignment: Qt.AlignHCenter + Layout.alignment: Qt.AlignVCenter | Qt.AlignHCenter visible: _batteryInfoAvailable QGCLabel { @@ -313,5 +300,72 @@ Rectangle { duration: 2000 } } + + // Small mission download progress bar + Rectangle { + id: progressBar + anchors.left: parent.left + anchors.bottom: parent.bottom + height: 4 + width: _controllerProgressPct * parent.width + color: qgcPal.colorGreen + visible: false + } + + /* + Rectangle { + anchors.bottom: parent.bottom + height: toolBar.height * 0.05 + width: _activeVehicle ? _activeVehicle.parameterManager.loadProgress * parent.width : 0 + color: qgcPal.colorGreen + visible: !largeProgressBar.visible + } + */ + + // Large mission download progress bar + Rectangle { + id: largeProgressBar + anchors.bottom: parent.bottom + anchors.left: parent.left + anchors.right: parent.right + height: parent.height + color: qgcPal.window + visible: _showLargeProgress + + property bool _userHide: false + property bool _showLargeProgress: progressBar.visible && !_userHide && qgcPal.globalTheme === QGCPalette.Light + + Connections { + target: QGroundControl.multiVehicleManager + onActiveVehicleChanged: largeProgressBar._userHide = false + } + + Rectangle { + anchors.top: parent.top + anchors.bottom: parent.bottom + width: _controllerProgressPct * parent.width + color: qgcPal.colorGreen + } + + QGCLabel { + anchors.centerIn: parent + text: qsTr("Syncing Mission") + font.pointSize: ScreenTools.largeFontPointSize + } + + QGCLabel { + anchors.margins: _margin + anchors.right: parent.right + anchors.bottom: parent.bottom + text: qsTr("Click anywhere to hide") + + property real _margin: ScreenTools.defaultFontPixelWidth / 2 + } + + MouseArea { + anchors.fill: parent + onClicked: largeProgressBar._userHide = true + } + } } diff --git a/src/PlanView/PlanView.qml b/src/PlanView/PlanView.qml index 16b4209f0a0cb4f949939d6d8052b91d5f757684..278f56443172cfbab7887c4d7fcd96168069ade3 100644 --- a/src/PlanView/PlanView.qml +++ b/src/PlanView/PlanView.qml @@ -23,8 +23,8 @@ import QGroundControl.Controls 1.0 import QGroundControl.FactSystem 1.0 import QGroundControl.FactControls 1.0 import QGroundControl.Palette 1.0 -import QGroundControl.Mavlink 1.0 import QGroundControl.Controllers 1.0 +import QGroundControl.KMLFileHelper 1.0 /// Mission Editor @@ -78,13 +78,18 @@ QGCView { _missionController.setCurrentPlanViewIndex(sequenceNumber, true) } + function insertComplexMissionItemFromKML(complexItemName, kmlFile, index) { + var sequenceNumber = _missionController.insertComplexMissionItemFromKML(complexItemName, kmlFile, index) + _missionController.setCurrentPlanViewIndex(sequenceNumber, true) + } + property bool _firstMissionLoadComplete: false property bool _firstFenceLoadComplete: false property bool _firstRallyLoadComplete: false property bool _firstLoadComplete: false MapFitFunctions { - id: mapFitFunctions + id: mapFitFunctions // The name for this id cannot be changed without breaking references outside of this code. Beware! map: editorMap usePlannedHomePosition: true planMasterController: _planMasterController @@ -158,11 +163,19 @@ QGCView { id: masterController Component.onCompleted: { - start(true /* editMode */) + start(false /* flyView */) _missionController.setCurrentPlanViewIndex(0, true) } + function waitingOnDataMessage() { + _qgcView.showMessage(qsTr("Unable to Save/Upload"), qsTr("Plan is waiting on terrain data from server for correct altitude values."), StandardButton.Ok) + } + function upload() { + if (!readyForSaveSend()) { + waitingOnDataMessage() + return + } if (_activeVehicle && _activeVehicle.armed && _activeVehicle.flightMode === _activeVehicle.missionFlightMode) { _qgcView.showDialog(activeMissionUploadDialogComponent, qsTr("Plan Upload"), _qgcView.showDialogDefaultWidth, StandardButton.Cancel) } else { @@ -174,14 +187,22 @@ QGCView { fileDialog.title = qsTr("Select Plan File") fileDialog.selectExisting = true fileDialog.nameFilters = masterController.loadNameFilters + fileDialog.fileExtension = QGroundControl.settingsManager.appSettings.planFileExtension + fileDialog.fileExtension2 = QGroundControl.settingsManager.appSettings.missionFileExtension fileDialog.openForLoad() } function saveToSelectedFile() { + if (!readyForSaveSend()) { + waitingOnDataMessage() + return + } fileDialog.title = qsTr("Save Plan") - fileDialog.plan = true + fileDialog.planFiles = true fileDialog.selectExisting = false fileDialog.nameFilters = masterController.saveNameFilters + fileDialog.fileExtension = QGroundControl.settingsManager.appSettings.planFileExtension + fileDialog.fileExtension2 = QGroundControl.settingsManager.appSettings.missionFileExtension fileDialog.openForSave() } @@ -189,11 +210,27 @@ QGCView { mapFitFunctions.fitMapViewportToMissionItems() } + function loadKmlFromSelectedFile() { + fileDialog.title = qsTr("Load KML") + fileDialog.planFiles = false + fileDialog.selectExisting = true + fileDialog.nameFilters = masterController.fileKmlFilters + fileDialog.fileExtension = QGroundControl.settingsManager.appSettings.kmlFileExtension + fileDialog.fileExtension2 = "" + fileDialog.openForLoad() + } + function saveKmlToSelectedFile() { + if (!readyForSaveSend()) { + waitingOnDataMessage() + return + } fileDialog.title = qsTr("Save KML") - fileDialog.plan = false + fileDialog.planFiles = false fileDialog.selectExisting = false - fileDialog.nameFilters = masterController.saveKmlFilters + fileDialog.nameFilters = masterController.fileKmlFilters + fileDialog.fileExtension = QGroundControl.settingsManager.appSettings.kmlFileExtension + fileDialog.fileExtension2 = "" fileDialog.openForSave() } } @@ -238,24 +275,96 @@ QGCView { QGCFileDialog { id: fileDialog qgcView: _qgcView - property bool plan: true folder: QGroundControl.settingsManager.appSettings.missionSavePath - fileExtension: QGroundControl.settingsManager.appSettings.planFileExtension - fileExtension2: QGroundControl.settingsManager.appSettings.missionFileExtension + + property bool planFiles: true ///< true: working with plan files, false: working with kml file onAcceptedForSave: { - plan ? masterController.saveToFile(file) : masterController.saveToKml(file) + if (planFiles) { + masterController.saveToFile(file) + } else { + masterController.saveToKml(file) + } close() } onAcceptedForLoad: { - masterController.loadFromFile(file) - masterController.fitViewportToItems() - _missionController.setCurrentPlanViewIndex(0, true) + if (planFiles) { + masterController.loadFromFile(file) + masterController.fitViewportToItems() + _missionController.setCurrentPlanViewIndex(0, true) + } else { + var retList = KMLFileHelper.determineFileContents(file) + if (retList[0] == KMLFileHelper.Error) { + _qgcView.showMessage("Error", retList[1], StandardButton.Ok) + } else if (retList[0] == KMLFileHelper.Polygon) { + kmlPolygonSelectDialogKMLFile = file + _qgcView.showDialog(kmlPolygonSelectDialog, fileDialog.title, _qgcView.showDialogDefaultWidth, StandardButton.Ok | StandardButton.Cancel) + } else if (retList[0] == KMLFileHelper.Polyline) { + insertComplexMissionItemFromKML(_missionController.corridorScanComplexItemName, file, -1) + } + } close() } } + property string kmlPolygonSelectDialogKMLFile + Component { + id: kmlPolygonSelectDialog + + QGCViewDialog { + property var editVehicle: _activeVehicle ? _activeVehicle : QGroundControl.multiVehicleManager.offlineEditingVehicle + + function accept() { + var complexItemName + if (surveyRadio.checked) { + complexItemName = _missionController.surveyComplexItemName + } else { + complexItemName = _missionController.structureScanComplexItemName + } + insertComplexMissionItemFromKML(complexItemName, kmlPolygonSelectDialogKMLFile, -1) + hideDialog() + } + + Component.onCompleted: { + if (editVehicle.fixedWing) { + // Only Survey available + accept() + } + } + + ExclusiveGroup { + id: radioGroup + } + + Column { + anchors.left: parent.left + anchors.right: parent.right + spacing: ScreenTools.defaultFontPixelHeight + + QGCLabel { + anchors.left: parent.left + anchors.right: parent.right + wrapMode: Text.WordWrap + text: qsTr("What would you like to create from the polygon specified by the KML file?") + } + + QGCRadioButton { + id: surveyRadio + text: qsTr("Survey") + checked: true + exclusiveGroup: radioGroup + } + + QGCRadioButton { + text: qsTr("Structure Scan") + exclusiveGroup: radioGroup + visible: !editVehicle.fixedWing + } + } + } + } + Component { id: moveDialog @@ -302,6 +411,7 @@ QGCView { allowGCSLocationCenter: true allowVehicleLocationCenter: true planView: true + qgcView: _qgcView // This is the center rectangle of the map which is not obscured by tools property rect centerViewport: Qt.rect(_leftToolWidth, _toolbarHeight, editorMap.width - _leftToolWidth - _rightPanelWidth, editorMap.height - _statusHeight - _toolbarHeight) @@ -789,7 +899,7 @@ QGCView { } QGCButton { - text: qsTr("Save To File...") + text: qsTr("Save Plan...") Layout.fillWidth: true enabled: !masterController.syncInProgress onClicked: { @@ -799,7 +909,7 @@ QGCView { } QGCButton { - text: qsTr("Load From File...") + text: qsTr("Load Plan...") Layout.fillWidth: true enabled: !masterController.syncInProgress onClicked: { @@ -813,14 +923,16 @@ QGCView { } QGCButton { - text: qsTr("Remove All") + text: qsTr("Load KML...") Layout.fillWidth: true - onClicked: { + enabled: !masterController.syncInProgress + onClicked: { dropPanel.hide() - _qgcView.showDialog(removeAllPromptDialog, qsTr("Remove all"), _qgcView.showDialogDefaultWidth, StandardButton.Yes | StandardButton.No) + masterController.loadKmlFromSelectedFile() } } + QGCButton { text: qsTr("Save KML...") Layout.fillWidth: true @@ -835,6 +947,15 @@ QGCView { masterController.saveKmlToSelectedFile() } } + + QGCButton { + text: qsTr("Remove All") + Layout.fillWidth: true + onClicked: { + dropPanel.hide() + _qgcView.showDialog(removeAllPromptDialog, qsTr("Remove all"), _qgcView.showDialogDefaultWidth, StandardButton.Yes | StandardButton.No) + } + } } } } diff --git a/src/PlanView/SimpleItemEditor.qml b/src/PlanView/SimpleItemEditor.qml index 75c6da4f306b64596bffc941babfa7e0cee4a3a3..7bbe3b8e45d17c6c309df828e982d7cb4a29c569 100644 --- a/src/PlanView/SimpleItemEditor.qml +++ b/src/PlanView/SimpleItemEditor.qml @@ -4,6 +4,7 @@ import QtQuick.Controls.Styles 1.4 import QtQuick.Dialogs 1.2 import QtQuick.Layouts 1.2 +import QGroundControl 1.0 import QGroundControl.ScreenTools 1.0 import QGroundControl.Vehicle 1.0 import QGroundControl.Controls 1.0 @@ -17,6 +18,23 @@ Rectangle { color: qgcPal.windowShadeDark radius: _radius + property bool _specifiesAltitude: missionItem.specifiesAltitude + property real _margin: ScreenTools.defaultFontPixelHeight / 2 + property bool _supportsTerrainFrame: missionItem + + readonly property int _altModeRelative: 0 + readonly property int _altModeAbsolute: 1 + readonly property int _altModeAboveTerrain: 2 + readonly property int _altModeTerrainFrame: 3 + + ExclusiveGroup { + id: altRadios + onCurrentChanged: { + altModeLabel.text = Qt.binding(function() { return current.helpText }) + missionItem.altitudeMode = current.altModeValue + } + } + Column { id: valuesColumn anchors.margins: _margin @@ -64,11 +82,86 @@ Rectangle { } } + Rectangle { + anchors.left: parent.left + anchors.right: parent.right + height: altColumn.y + altColumn.height + _margin + color: qgcPal.windowShade + visible: _specifiesAltitude + + Column { + id: altColumn + anchors.margins: _margin + anchors.top: parent.top + anchors.left: parent.left + anchors.right: parent.right + spacing: _margin + + QGCLabel { + font.pointSize: ScreenTools.smallFontPointSize + text: qsTr("Altitude") + } + + RowLayout { + QGCRadioButton { + text: qsTr("Rel") + exclusiveGroup: altRadios + checked: missionItem.altitudeMode === altModeValue + + readonly property int altModeValue: _altModeRelative + readonly property string helpText: qsTr("Relative to home altitude") + } + QGCRadioButton { + text: qsTr("Abs") + exclusiveGroup: altRadios + checked: missionItem.altitudeMode === altModeValue + visible: QGroundControl.corePlugin.options.showMissionAbsoluteAltitude || missionItem.altitudeMode === altModeValue + + readonly property int altModeValue: _altModeAbsolute + readonly property string helpText: qsTr("Absolute WGS84") + } + QGCRadioButton { + text: qsTr("AGL") + exclusiveGroup: altRadios + checked: missionItem.altitudeMode === altModeValue + + readonly property int altModeValue: _altModeAboveTerrain + property string helpText: qsTr("Calculated from terrain data\nAbs Alt ") + missionItem.amslAltAboveTerrain.valueString + " " + missionItem.amslAltAboveTerrain.units + } + QGCRadioButton { + text: qsTr("TerrF") + exclusiveGroup: altRadios + checked: missionItem.altitudeMode === altModeValue + visible: missionItem.altitudeMode === altModeValue + + readonly property int altModeValue: _altModeTerrainFrame + readonly property string helpText: qsTr("Using terrain reference frame") + } + } + + FactValueSlider { + fact: missionItem.altitude + digitCount: 3 + incrementSlots: 1 + } + + QGCLabel { + id: altModeLabel + anchors.left: parent.left + anchors.right: parent.right + wrapMode: Text.WordWrap + font.pointSize: ScreenTools.smallFontPointSize + } + } + } + GridLayout { anchors.left: parent.left anchors.right: parent.right flow: GridLayout.TopToBottom - rows: missionItem.textFieldFacts.count + missionItem.nanFacts.count + (missionItem.speedSection.available ? 1 : 0) + rows: missionItem.textFieldFacts.count + + missionItem.nanFacts.count + + (missionItem.speedSection.available ? 1 : 0) columns: 2 Repeater { @@ -95,6 +188,7 @@ Rectangle { visible: missionItem.speedSection.available } + Repeater { model: missionItem.textFieldFacts @@ -102,6 +196,7 @@ Rectangle { showUnits: true fact: object Layout.fillWidth: true + enabled: !object.readOnly } } @@ -124,15 +219,6 @@ Rectangle { } } - Repeater { - model: missionItem.checkboxFacts - - FactCheckBox { - text: object.name - fact: object - } - } - CameraSection { checked: missionItem.cameraSection.settingsSpecified visible: missionItem.cameraSection.available diff --git a/src/PlanView/StructureScanEditor.qml b/src/PlanView/StructureScanEditor.qml index 9511c5380af205160d4b615200c0153dfde1a722..9974da55f6c560c9aa52c5c689103ffae9c384a4 100644 --- a/src/PlanView/StructureScanEditor.qml +++ b/src/PlanView/StructureScanEditor.qml @@ -25,9 +25,10 @@ Rectangle { //property real availableWidth ///< Width for control //property var missionItem ///< Mission Item for editor - property real _margin: ScreenTools.defaultFontPixelWidth / 2 - property real _fieldWidth: ScreenTools.defaultFontPixelWidth * 10.5 - property var _vehicle: QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle : QGroundControl.multiVehicleManager.offlineEditingVehicle + property real _margin: ScreenTools.defaultFontPixelWidth / 2 + property real _fieldWidth: ScreenTools.defaultFontPixelWidth * 10.5 + property var _vehicle: QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle : QGroundControl.multiVehicleManager.offlineEditingVehicle + property real _cameraMinTriggerInterval: missionItem.cameraCalc.minTriggerInterval.rawValue function polygonCaptureStarted() { missionItem.clearPolygon() @@ -67,10 +68,10 @@ Rectangle { QGCLabel { anchors.left: parent.left anchors.right: parent.right - text: qsTr("WARNING: Photo interval is below minimum interval (%1 secs) supported by camera.").arg(missionItem.cameraMinTriggerInterval.toFixed(1)) + text: qsTr("WARNING: Photo interval is below minimum interval (%1 secs) supported by camera.").arg(_cameraMinTriggerInterval.toFixed(1)) wrapMode: Text.WordWrap color: qgcPal.warningText - visible: missionItem.cameraShots > 0 && missionItem.cameraMinTriggerInterval !== 0 && missionItem.cameraMinTriggerInterval > missionItem.timeBetweenShots + visible: missionItem.cameraShots > 0 && _cameraMinTriggerInterval !== 0 && _cameraMinTriggerInterval > missionItem.timeBetweenShots } CameraCalc { @@ -81,33 +82,6 @@ Rectangle { sideDistanceLabel: qsTr("Trigger Distance") } - GridLayout { - anchors.left: parent.left - anchors.right: parent.right - columnSpacing: ScreenTools.defaultFontPixelWidth / 2 - rowSpacing: 0 - columns: 3 - visible: missionItem.cameraCalc.isManualCamera - - Item { width: 1; height: 1 } - QGCLabel { text: qsTr("Pitch") } - QGCLabel { text: qsTr("Yaw") } - - QGCLabel { - text: qsTr("Gimbal") - Layout.fillWidth: true - } - FactTextField { - fact: missionItem.gimbalPitch - implicitWidth: ScreenTools.defaultFontPixelWidth * 9 - } - - FactTextField { - fact: missionItem.gimbalYaw - implicitWidth: ScreenTools.defaultFontPixelWidth * 9 - } - } - SectionHeader { id: scanHeader text: qsTr("Scan") @@ -186,6 +160,9 @@ Rectangle { QGCLabel { text: qsTr("Photo interval") } QGCLabel { text: missionItem.timeBetweenShots.toFixed(1) + " " + qsTr("secs") } + + QGCLabel { text: qsTr("Trigger Distance") } + QGCLabel { text: missionItem.cameraCalc.adjustedFootprintSide.valueString + " " + QGroundControl.appSettingsDistanceUnitsString } } } // Column } // Rectangle diff --git a/src/PlanView/SurveyItemEditor.qml b/src/PlanView/SurveyItemEditor.qml index ad47d11ebe470df2c7fc302f299d25c37763517c..8f0c3503b653d2a3b92b45ab3313a104729c17a0 100644 --- a/src/PlanView/SurveyItemEditor.qml +++ b/src/PlanView/SurveyItemEditor.qml @@ -13,7 +13,6 @@ import QGroundControl.FactControls 1.0 import QGroundControl.Palette 1.0 import QGroundControl.FlightMap 1.0 -// Editor for Survery mission items Rectangle { id: _root height: visible ? (editorColumn.height + (_margin * 2)) : 0 @@ -25,92 +24,10 @@ Rectangle { //property real availableWidth ///< Width for control //property var missionItem ///< Mission Item for editor - property real _margin: ScreenTools.defaultFontPixelWidth / 2 - property int _cameraIndex: 1 - property real _fieldWidth: ScreenTools.defaultFontPixelWidth * 10.5 - property var _cameraList: [ qsTr("Manual Grid (no camera specs)"), qsTr("Custom Camera Grid") ] - property var _vehicle: QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle : QGroundControl.multiVehicleManager.offlineEditingVehicle - property var _vehicleCameraList: _vehicle ? _vehicle.staticCameraList : [] - - readonly property int _gridTypeManual: 0 - readonly property int _gridTypeCustomCamera: 1 - readonly property int _gridTypeCamera: 2 - - Component.onCompleted: { - for (var i=0; i<_vehicle.staticCameraList.length; i++) { - _cameraList.push(_vehicle.staticCameraList[i].name) - } - gridTypeCombo.model = _cameraList - if (missionItem.manualGrid.value) { - gridTypeCombo.currentIndex = _gridTypeManual - } else { - var index = -1 - for (index=0; index<_cameraList.length; index++) { - if (_cameraList[index] === missionItem.camera.value) { - break; - } - } - missionItem.cameraOrientationFixed = false - if (index == _cameraList.length) { - gridTypeCombo.currentIndex = _gridTypeCustomCamera - } else { - gridTypeCombo.currentIndex = index - if (index != 1) { - // Specific camera is selected - var camera = _vehicleCameraList[index - _gridTypeCamera] - missionItem.cameraOrientationFixed = camera.fixedOrientation - missionItem.cameraMinTriggerInterval = camera.minTriggerInterval - } - } - } - } - - function recalcFromCameraValues() { - var focalLength = missionItem.cameraFocalLength.rawValue - var sensorWidth = missionItem.cameraSensorWidth.rawValue - var sensorHeight = missionItem.cameraSensorHeight.rawValue - var imageWidth = missionItem.cameraResolutionWidth.rawValue - var imageHeight = missionItem.cameraResolutionHeight.rawValue - - var altitude = missionItem.gridAltitude.rawValue - var groundResolution= missionItem.groundResolution.rawValue - var frontalOverlap = missionItem.frontalOverlap.rawValue - var sideOverlap = missionItem.sideOverlap.rawValue - - if (focalLength <= 0 || sensorWidth <= 0 || sensorHeight <= 0 || imageWidth <= 0 || imageHeight <= 0 || groundResolution <= 0) { - return - } - - var imageSizeSideGround //size in side (non flying) direction of the image on the ground - var imageSizeFrontGround //size in front (flying) direction of the image on the ground - var gridSpacing - var cameraTriggerDistance - - if (missionItem.fixedValueIsAltitude.value) { - groundResolution = (altitude * sensorWidth * 100) / (imageWidth * focalLength) - } else { - altitude = (imageWidth * groundResolution * focalLength) / (sensorWidth * 100) - } - - if (missionItem.cameraOrientationLandscape.value) { - imageSizeSideGround = (imageWidth * groundResolution) / 100 - imageSizeFrontGround = (imageHeight * groundResolution) / 100 - } else { - imageSizeSideGround = (imageHeight * groundResolution) / 100 - imageSizeFrontGround = (imageWidth * groundResolution) / 100 - } - - gridSpacing = imageSizeSideGround * ( (100-sideOverlap) / 100 ) - cameraTriggerDistance = imageSizeFrontGround * ( (100-frontalOverlap) / 100 ) - - if (missionItem.fixedValueIsAltitude.value) { - missionItem.groundResolution.rawValue = groundResolution - } else { - missionItem.gridAltitude.rawValue = altitude - } - missionItem.gridSpacing.rawValue = gridSpacing - missionItem.cameraTriggerDistance.rawValue = cameraTriggerDistance - } + property real _margin: ScreenTools.defaultFontPixelWidth / 2 + property real _fieldWidth: ScreenTools.defaultFontPixelWidth * 10.5 + property var _vehicle: QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle : QGroundControl.multiVehicleManager.offlineEditingVehicle + property real _cameraMinTriggerInterval: missionItem.cameraCalc.minTriggerInterval.rawValue function polygonCaptureStarted() { missionItem.clearPolygon() @@ -129,52 +46,8 @@ Rectangle { function polygonAdjustStarted() { } function polygonAdjustFinished() { } - property bool _noCameraValueRecalc: false ///< Prevents uneeded recalcs - - Connections { - target: missionItem.camera - - onValueChanged: { - if (gridTypeCombo.currentIndex >= _gridTypeCustomCamera && !_noCameraValueRecalc) { - recalcFromCameraValues() - } - } - } - - Connections { - target: missionItem.gridAltitude - - onValueChanged: { - if (gridTypeCombo.currentIndex >= _gridTypeCustomCamera && missionItem.fixedValueIsAltitude.value && !_noCameraValueRecalc) { - recalcFromCameraValues() - } - } - } - - Connections { - target: missionItem - - onCameraValueChanged: { - if (gridTypeCombo.currentIndex >= _gridTypeCustomCamera && !_noCameraValueRecalc) { - recalcFromCameraValues() - } - } - } - QGCPalette { id: qgcPal; colorGroupEnabled: true } - ExclusiveGroup { - id: cameraOrientationGroup - - onCurrentChanged: { - if (gridTypeCombo.currentIndex >= _gridTypeCustomCamera) { - recalcFromCameraValues() - } - } - } - - ExclusiveGroup { id: fixedValueGroup } - Column { id: editorColumn anchors.margins: _margin @@ -186,343 +59,23 @@ Rectangle { QGCLabel { anchors.left: parent.left anchors.right: parent.right - text: qsTr("WARNING: Photo interval is below minimum interval (%1 secs) supported by camera.").arg(missionItem.cameraMinTriggerInterval.toFixed(1)) + text: qsTr("WARNING: Photo interval is below minimum interval (%1 secs) supported by camera.").arg(_cameraMinTriggerInterval.toFixed(1)) wrapMode: Text.WordWrap color: qgcPal.warningText - visible: missionItem.manualGrid.value !== true && missionItem.cameraShots > 0 && missionItem.cameraMinTriggerInterval !== 0 && missionItem.cameraMinTriggerInterval > missionItem.timeBetweenShots - } - - SectionHeader { - id: cameraHeader - text: qsTr("Camera") - showSpacer: false + visible: missionItem.cameraShots > 0 && _cameraMinTriggerInterval !== 0 && _cameraMinTriggerInterval > missionItem.timeBetweenShots } - Column { - anchors.left: parent.left - anchors.right: parent.right - spacing: _margin - visible: cameraHeader.checked - - QGCComboBox { - id: gridTypeCombo - anchors.left: parent.left - anchors.right: parent.right - model: _cameraList - currentIndex: -1 - - onActivated: { - if (index == _gridTypeManual) { - missionItem.manualGrid.value = true - missionItem.fixedValueIsAltitude.value = true - } else if (index == _gridTypeCustomCamera) { - missionItem.manualGrid.value = false - missionItem.camera.value = gridTypeCombo.textAt(index) - missionItem.cameraOrientationFixed = false - missionItem.cameraMinTriggerInterval = 0 - } else { - missionItem.manualGrid.value = false - missionItem.camera.value = gridTypeCombo.textAt(index) - _noCameraValueRecalc = true - var listIndex = index - _gridTypeCamera - missionItem.cameraSensorWidth.rawValue = _vehicleCameraList[listIndex].sensorWidth - missionItem.cameraSensorHeight.rawValue = _vehicleCameraList[listIndex].sensorHeight - missionItem.cameraResolutionWidth.rawValue = _vehicleCameraList[listIndex].imageWidth - missionItem.cameraResolutionHeight.rawValue = _vehicleCameraList[listIndex].imageHeight - missionItem.cameraFocalLength.rawValue = _vehicleCameraList[listIndex].focalLength - missionItem.cameraOrientationLandscape.rawValue = _vehicleCameraList[listIndex].landscape ? 1 : 0 - missionItem.cameraOrientationFixed = _vehicleCameraList[listIndex].fixedOrientation - missionItem.cameraMinTriggerInterval = _vehicleCameraList[listIndex].minTriggerInterval - _noCameraValueRecalc = false - recalcFromCameraValues() - } - } - } - - RowLayout { - anchors.left: parent.left - anchors.right: parent.right - spacing: _margin - visible: missionItem.manualGrid.value - - QGCCheckBox { - id: cameraTriggerDistanceCheckBox - anchors.baseline: cameraTriggerDistanceField.baseline - text: qsTr("Trigger Distance") - checked: missionItem.cameraTriggerDistance.rawValue > 0 - onClicked: { - if (checked) { - missionItem.cameraTriggerDistance.value = missionItem.cameraTriggerDistance.defaultValue - } else { - missionItem.cameraTriggerDistance.value = 0 - } - } - } - - FactTextField { - id: cameraTriggerDistanceField - Layout.fillWidth: true - fact: missionItem.cameraTriggerDistance - enabled: cameraTriggerDistanceCheckBox.checked - } - } - } - - // Camera based grid ui - Column { - anchors.left: parent.left - anchors.right: parent.right - spacing: _margin - visible: gridTypeCombo.currentIndex !== _gridTypeManual - - Row { - spacing: _margin - anchors.horizontalCenter: parent.horizontalCenter - visible: !missionItem.cameraOrientationFixed - - QGCRadioButton { - width: _editFieldWidth - text: "Landscape" - checked: !!missionItem.cameraOrientationLandscape.value - exclusiveGroup: cameraOrientationGroup - onClicked: missionItem.cameraOrientationLandscape.value = 1 - } - - QGCRadioButton { - id: cameraOrientationPortrait - text: "Portrait" - checked: !missionItem.cameraOrientationLandscape.value - exclusiveGroup: cameraOrientationGroup - onClicked: missionItem.cameraOrientationLandscape.value = 0 - } - } - - Column { - id: custCameraCol - anchors.left: parent.left - anchors.right: parent.right - spacing: _margin - visible: gridTypeCombo.currentIndex === _gridTypeCustomCamera - - RowLayout { - anchors.left: parent.left - anchors.right: parent.right - spacing: _margin - Item { Layout.fillWidth: true } - QGCLabel { - Layout.preferredWidth: _root._fieldWidth - text: qsTr("Width") - } - QGCLabel { - Layout.preferredWidth: _root._fieldWidth - text: qsTr("Height") - } - } - - RowLayout { - anchors.left: parent.left - anchors.right: parent.right - spacing: _margin - QGCLabel { text: qsTr("Sensor"); Layout.fillWidth: true } - FactTextField { - Layout.preferredWidth: _root._fieldWidth - fact: missionItem.cameraSensorWidth - } - FactTextField { - Layout.preferredWidth: _root._fieldWidth - fact: missionItem.cameraSensorHeight - } - } - - RowLayout { - anchors.left: parent.left - anchors.right: parent.right - spacing: _margin - QGCLabel { text: qsTr("Image"); Layout.fillWidth: true } - FactTextField { - Layout.preferredWidth: _root._fieldWidth - fact: missionItem.cameraResolutionWidth - } - FactTextField { - Layout.preferredWidth: _root._fieldWidth - fact: missionItem.cameraResolutionHeight - } - } - - RowLayout { - anchors.left: parent.left - anchors.right: parent.right - spacing: _margin - QGCLabel { - text: qsTr("Focal length") - Layout.fillWidth: true - } - FactTextField { - Layout.preferredWidth: _root._fieldWidth - fact: missionItem.cameraFocalLength - } - } - - } // Column - custom camera - - RowLayout { - anchors.left: parent.left - anchors.right: parent.right - spacing: _margin - Item { Layout.fillWidth: true } - QGCLabel { - Layout.preferredWidth: _root._fieldWidth - text: qsTr("Front Lap") - } - QGCLabel { - Layout.preferredWidth: _root._fieldWidth - text: qsTr("Side Lap") - } - } - - RowLayout { - anchors.left: parent.left - anchors.right: parent.right - spacing: _margin - QGCLabel { text: qsTr("Overlap"); Layout.fillWidth: true } - FactTextField { - Layout.preferredWidth: _root._fieldWidth - fact: missionItem.frontalOverlap - } - FactTextField { - Layout.preferredWidth: _root._fieldWidth - fact: missionItem.sideOverlap - } - } - - FactCheckBox { - text: qsTr("Hover and capture image") - fact: missionItem.hoverAndCapture - visible: missionItem.hoverAndCaptureAllowed - onClicked: { - if (checked) { - missionItem.cameraTriggerInTurnaround.rawValue = false - } - } - } - - FactCheckBox { - text: qsTr("Take images in turnarounds") - fact: missionItem.cameraTriggerInTurnaround - enabled: missionItem.hoverAndCaptureAllowed ? !missionItem.hoverAndCapture.rawValue : true - } - - SectionHeader { - id: gridHeader - text: qsTr("Grid") - } - - GridLayout { - anchors.left: parent.left - anchors.right: parent.right - columnSpacing: _margin - rowSpacing: _margin - columns: 2 - visible: gridHeader.checked - - GridLayout { - anchors.left: parent.left - anchors.right: parent.right - columnSpacing: _margin - rowSpacing: _margin - columns: 2 - visible: gridHeader.checked - - QGCLabel { - id: angleText - text: qsTr("Angle") - Layout.fillWidth: true - } - - ToolButton { - id: windRoseButton - anchors.verticalCenter: angleText.verticalCenter - iconSource: qgcPal.globalTheme === QGCPalette.Light ? "/res/wind-roseBlack.svg" : "/res/wind-rose.svg" - visible: _vehicle ? _vehicle.fixedWing : false - - onClicked: { - windRosePie.angle = Number(gridAngleText.text) - var cords = windRoseButton.mapToItem(_root, 0, 0) - windRosePie.popup(cords.x + windRoseButton.width / 2, cords.y + windRoseButton.height / 2) - } - } - } - - FactTextField { - id: gridAngleText - fact: missionItem.gridAngle - Layout.fillWidth: true - } - - QGCLabel { text: qsTr("Turnaround dist") } - FactTextField { - fact: missionItem.turnaroundDist - Layout.fillWidth: true - } - - QGCLabel { - text: qsTr("Entry") - } - FactComboBox { - fact: missionItem.gridEntryLocation - indexModel: false - Layout.fillWidth: true - } - - QGCCheckBox { - text: qsTr("Refly at 90 degree offset") - checked: missionItem.refly90Degrees - onClicked: missionItem.refly90Degrees = checked - Layout.columnSpan: 2 - } - - QGCLabel { - wrapMode: Text.WordWrap - text: qsTr("Select one:") - Layout.preferredWidth: parent.width - Layout.columnSpan: 2 - } - - QGCRadioButton { - id: fixedAltitudeRadio - text: qsTr("Altitude") - checked: !!missionItem.fixedValueIsAltitude.value - exclusiveGroup: fixedValueGroup - onClicked: missionItem.fixedValueIsAltitude.value = 1 - } - - FactTextField { - fact: missionItem.gridAltitude - enabled: fixedAltitudeRadio.checked - Layout.fillWidth: true - } - - QGCRadioButton { - id: fixedGroundResolutionRadio - text: qsTr("Ground res") - checked: !missionItem.fixedValueIsAltitude.value - exclusiveGroup: fixedValueGroup - onClicked: missionItem.fixedValueIsAltitude.value = 0 - } - - FactTextField { - fact: missionItem.groundResolution - enabled: fixedGroundResolutionRadio.checked - Layout.fillWidth: true - } - } + CameraCalc { + cameraCalc: missionItem.cameraCalc + vehicleFlightIsFrontal: true + distanceToSurfaceLabel: qsTr("Altitude") + frontalDistanceLabel: qsTr("Trigger Distance") + sideDistanceLabel: qsTr("Spacing") } - // Manual grid ui SectionHeader { - id: manualGridHeader - text: qsTr("Grid") - visible: gridTypeCombo.currentIndex === _gridTypeManual + id: corridorHeader + text: qsTr("Transects") } GridLayout { @@ -531,69 +84,31 @@ Rectangle { columnSpacing: _margin rowSpacing: _margin columns: 2 - visible: manualGridHeader.visible && manualGridHeader.checked - - RowLayout { - spacing: _margin - - QGCLabel { - id: manualAngleText - text: qsTr("Angle") - Layout.fillWidth: true - } - - ToolButton { - id: manualWindRoseButton - anchors.verticalCenter: manualAngleText.verticalCenter - Layout.columnSpan: 1 - iconSource: qgcPal.globalTheme === QGCPalette.Light ? "/res/wind-roseBlack.svg" : "/res/wind-rose.svg" - visible: _vehicle ? _vehicle.fixedWing : false - - onClicked: { - var cords = manualWindRoseButton.mapToItem(_root, 0, 0) - windRosePie.popup(cords.x + manualWindRoseButton.width / 2, cords.y + manualWindRoseButton.height / 2) - } - } - } + visible: corridorHeader.checked + QGCLabel { text: qsTr("Angle") } FactTextField { - id: manualGridAngleText - fact: missionItem.gridAngle - Layout.fillWidth: true - } - - QGCLabel { text: qsTr("Spacing") } - FactTextField { - fact: missionItem.gridSpacing + fact: missionItem.gridAngle Layout.fillWidth: true } - QGCLabel { text: qsTr("Altitude") } - FactTextField { - fact: missionItem.gridAltitude - Layout.fillWidth: true - } QGCLabel { text: qsTr("Turnaround dist") } FactTextField { - fact: missionItem.turnaroundDist + fact: missionItem.turnAroundDistance Layout.fillWidth: true } - QGCLabel { - text: qsTr("Entry") - visible: !windRoseButton.visible - } - FactComboBox { - id: gridAngleBox - fact: missionItem.gridEntryLocation - visible: !windRoseButton.visible - indexModel: false - Layout.fillWidth: true + + QGCButton { + Layout.columnSpan: 2 + text: qsTr("Rotate Entry Point") + onClicked: missionItem.rotateEntryPoint(); } FactCheckBox { text: qsTr("Hover and capture image") fact: missionItem.hoverAndCapture visible: missionItem.hoverAndCaptureAllowed + enabled: !missionItem.followTerrain Layout.columnSpan: 2 onClicked: { if (checked) { @@ -603,171 +118,94 @@ Rectangle { } FactCheckBox { - text: qsTr("Take images in turnarounds") - fact: missionItem.cameraTriggerInTurnaround - enabled: missionItem.hoverAndCaptureAllowed ? !missionItem.hoverAndCapture.rawValue : true + text: qsTr("Refly at 90 degree offset") + fact: missionItem.refly90Degrees + enabled: !missionItem.followTerrain Layout.columnSpan: 2 } - QGCCheckBox { - text: qsTr("Refly at 90 degree offset") - checked: missionItem.refly90Degrees - onClicked: missionItem.refly90Degrees = checked + FactCheckBox { + text: qsTr("Take images in turnarounds") + fact: missionItem.cameraTriggerInTurnAround + enabled: missionItem.hoverAndCaptureAllowed ? !missionItem.hoverAndCapture.rawValue : true Layout.columnSpan: 2 } FactCheckBox { - anchors.left: parent.left - text: qsTr("Relative altitude") - fact: missionItem.gridAltitudeRelative + text: qsTr("Fly alternate transects") + fact: missionItem.flyAlternateTransects + visible: _vehicle.fixedWing || _vehicle.vtol Layout.columnSpan: 2 } - } - - SectionHeader { - id: statsHeader - text: qsTr("Statistics") } - - Grid { - columns: 2 - columnSpacing: ScreenTools.defaultFontPixelWidth - visible: statsHeader.checked - QGCLabel { text: qsTr("Survey area") } - QGCLabel { text: QGroundControl.squareMetersToAppSettingsAreaUnits(missionItem.coveredArea).toFixed(2) + " " + QGroundControl.appSettingsAreaUnitsString } - - QGCLabel { text: qsTr("Photo count") } - QGCLabel { text: missionItem.cameraShots } + QGCCheckBox { + id: relAlt + Layout.alignment: Qt.AlignLeft + text: qsTr("Relative altitude") + checked: missionItem.cameraCalc.distanceToSurfaceRelative + enabled: missionItem.cameraCalc.isManualCamera && !missionItem.followTerrain + visible: QGroundControl.corePlugin.options.showMissionAbsoluteAltitude || (!missionItem.cameraCalc.distanceToSurfaceRelative && !missionItem.followTerrain) + Layout.columnSpan: 2 + onClicked: missionItem.cameraCalc.distanceToSurfaceRelative = checked - QGCLabel { text: qsTr("Photo interval") } - QGCLabel { - text: { - var timeVal = missionItem.timeBetweenShots - if(!isFinite(timeVal) || missionItem.cameraShots === 0) { - return qsTr("N/A") - } - return timeVal.toFixed(1) + " " + qsTr("secs") + Connections { + target: missionItem.cameraCalc + onDistanceToSurfaceRelativeChanged: relAlt.checked = missionItem.cameraCalc.distanceToSurfaceRelative } } } - } - QGCColoredImage { - id: windRoseArrow - source: "/res/wind-rose-arrow.svg" - visible: windRosePie.visible - width: windRosePie.width / 5 - height: width * 1.454 - smooth: true - color: qgcPal.colorGrey - transform: Rotation { - origin.x: windRoseArrow.width / 2 - origin.y: windRoseArrow.height / 2 - axis { x: 0; y: 0; z: 1 } angle: windRosePie.angle + SectionHeader { + id: terrainHeader + text: qsTr("Terrain") + checked: missionItem.followTerrain } - x: windRosePie.x + Math.sin(- windRosePie.angle*Math.PI/180 - Math.PI/2)*(windRosePie.width/2 - windRoseArrow.width/2) + windRosePie.width / 2 - windRoseArrow.width / 2 - y: windRosePie.y + Math.cos(- windRosePie.angle*Math.PI/180 - Math.PI/2)*(windRosePie.width/2 - windRoseArrow.width/2) + windRosePie.height / 2 - windRoseArrow.height / 2 - z: windRosePie.z + 1 - } - QGCColoredImage { - id: windGuru - source: "/res/wind-guru.svg" - visible: windRosePie.visible - width: windRosePie.width / 3 - height: width * 4.28e-1 - smooth: true - color: qgcPal.colorGrey - transform: Rotation { - origin.x: windGuru.width / 2 - origin.y: windGuru.height / 2 - axis { x: 0; y: 0; z: 1 } angle: windRosePie.angle + 180 - } - x: windRosePie.x + Math.sin(- windRosePie.angle*Math.PI/180 - 3*Math.PI/2)*(windRosePie.width/2) + windRosePie.width / 2 - windGuru.width / 2 - y: windRosePie.y + Math.cos(- windRosePie.angle*Math.PI/180 - 3*Math.PI/2)*(windRosePie.height/2) + windRosePie.height / 2 - windGuru.height / 2 - z: windRosePie.z + 1 - } + ColumnLayout { + anchors.left: parent.left + anchors.right: parent.right + spacing: _margin + visible: terrainHeader.checked - Item { - id: windRosePie - height: 2.6*windRoseButton.height - width: 2.6*windRoseButton.width - visible: false - focus: true - - property string colorCircle: qgcPal.windowShade - property string colorBackground: qgcPal.colorGrey - property real lineWidth: windRoseButton.width / 3 - property real angle: Number(gridAngleText.text) - - Canvas { - id: windRoseCanvas - anchors.fill: parent - - onPaint: { - var ctx = getContext("2d") - var x = width / 2 - var y = height / 2 - var angleWidth = 0.03 * Math.PI - var start = windRosePie.angle*Math.PI/180 - angleWidth - var end = windRosePie.angle*Math.PI/180 + angleWidth - ctx.reset() - - ctx.beginPath() - ctx.arc(x, y, (width / 3) - windRosePie.lineWidth / 2, 0, 2*Math.PI, false) - ctx.lineWidth = windRosePie.lineWidth - ctx.strokeStyle = windRosePie.colorBackground - ctx.stroke() - - ctx.beginPath() - ctx.arc(x, y, (width / 3) - windRosePie.lineWidth / 2, start, end, false) - ctx.lineWidth = windRosePie.lineWidth - ctx.strokeStyle = windRosePie.colorCircle - ctx.stroke() + QGCCheckBox { + id: followsTerrainCheckBox + text: qsTr("Vehicle follows terrain") + checked: missionItem.followTerrain + onClicked: missionItem.followTerrain = checked } - } - onFocusChanged: { - visible = focus - } - - function popup(x, y) { - if (x !== undefined) - windRosePie.x = x - windRosePie.width / 2 - if (y !== undefined) - windRosePie.y = y - windRosePie.height / 2 + GridLayout { + Layout.fillWidth: true + columnSpacing: _margin + rowSpacing: _margin + columns: 2 + visible: followsTerrainCheckBox.checked - windRosePie.visible = true - windRosePie.focus = true - missionItemEditorListView.interactive = false - } + QGCLabel { text: qsTr("Tolerance") } + FactTextField { + fact: missionItem.terrainAdjustTolerance + Layout.fillWidth: true + } - MouseArea { - id: mouseArea - anchors.fill: parent - acceptedButtons: Qt.LeftButton | Qt.RightButton + QGCLabel { text: qsTr("Max Climb Rate") } + FactTextField { + fact: missionItem.terrainAdjustMaxClimbRate + Layout.fillWidth: true + } - onClicked: { - windRosePie.visible = false - missionItemEditorListView.interactive = true - } - onPositionChanged: { - var point = Qt.point(mouseX - parent.width / 2, mouseY - parent.height / 2) - var angle = Math.round(Math.atan2(point.y, point.x) * 180 / Math.PI) - windRoseCanvas.requestPaint() - windRosePie.angle = angle - gridAngleText.text = angle - gridAngleText.editingFinished() - if(angle > -135 && angle <= -45) { - gridAngleBox.activated(2) // or 3 - } else if(angle > -45 && angle <= 45) { - gridAngleBox.activated(2) // or 0 - } else if(angle > 45 && angle <= 135) { - gridAngleBox.activated(1) // or 0 - } else if(angle > 135 || angle <= -135) { - gridAngleBox.activated(1) // or 3 + QGCLabel { text: qsTr("Max Descent Rate") } + FactTextField { + fact: missionItem.terrainAdjustMaxDescentRate + Layout.fillWidth: true } } } - } -} + + SectionHeader { + id: statsHeader + text: qsTr("Statistics") + } + + TransectStyleComplexItemStats { } + } // Column +} // Rectangle diff --git a/src/PlanView/SurveyMapVisual.qml b/src/PlanView/SurveyMapVisual.qml index 7a10c98eee99af5a7b4ab531c95f6b4793716598..dbcc03f3ef3320ff7b393a3dbe45c385de53275d 100644 --- a/src/PlanView/SurveyMapVisual.qml +++ b/src/PlanView/SurveyMapVisual.qml @@ -26,24 +26,24 @@ Item { property var qgcView ///< QGCView to use for popping dialogs property var _missionItem: object - property var _mapPolygon: object.mapPolygon - property var _gridComponent + property var _mapPolygon: object.surveyAreaPolygon + property var _visualTransectsComponent property var _entryCoordinate property var _exitCoordinate signal clicked(int sequenceNumber) function _addVisualElements() { - _gridComponent = gridComponent.createObject(map) + _visualTransectsComponent = visualTransectsComponent.createObject(map) _entryCoordinate = entryPointComponent.createObject(map) _exitCoordinate = exitPointComponent.createObject(map) - map.addMapItem(_gridComponent) + map.addMapItem(_visualTransectsComponent) map.addMapItem(_entryCoordinate) map.addMapItem(_exitCoordinate) } function _destroyVisualElements() { - _gridComponent.destroy() + _visualTransectsComponent.destroy() _entryCoordinate.destroy() _exitCoordinate.destroy() } @@ -100,14 +100,14 @@ Item { interiorOpacity: 0.5 } - // Survey grid lines + // Transect lines Component { - id: gridComponent + id: visualTransectsComponent MapPolyline { line.color: "white" line.width: 2 - path: _missionItem.gridPoints + path: _missionItem.visualTransectPoints } } diff --git a/src/PlanView/TransectStyleComplexItemStats.qml b/src/PlanView/TransectStyleComplexItemStats.qml new file mode 100644 index 0000000000000000000000000000000000000000..0dec0c10661552c7b9f3a846e7b6b6bdf8efbef3 --- /dev/null +++ b/src/PlanView/TransectStyleComplexItemStats.qml @@ -0,0 +1,30 @@ +import QtQuick 2.3 +import QtQuick.Controls 1.2 + +import QGroundControl 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Controls 1.0 + +// Statistics section for TransectStyleComplexItems +Grid { + // The following properties must be available up the hierarchy chain + //property var missionItem ///< Mission Item for editor + + anchors.left: parent.left + anchors.right: parent.right + columns: 2 + columnSpacing: ScreenTools.defaultFontPixelWidth + visible: statsHeader.checked + + QGCLabel { text: qsTr("Survey Area") } + QGCLabel { text: QGroundControl.squareMetersToAppSettingsAreaUnits(missionItem.coveredArea).toFixed(2) + " " + QGroundControl.appSettingsAreaUnitsString } + + QGCLabel { text: qsTr("Photo Count") } + QGCLabel { text: missionItem.cameraShots } + + QGCLabel { text: qsTr("Photo Interval") } + QGCLabel { text: missionItem.timeBetweenShots.toFixed(1) + " " + qsTr("secs") } + + QGCLabel { text: qsTr("Trigger Distance") } + QGCLabel { text: missionItem.cameraCalc.adjustedFootprintFrontal.valueString + " " + missionItem.cameraCalc.adjustedFootprintFrontal.units } +} diff --git a/src/QGC.cc b/src/QGC.cc index dcfa7e2d52a6064334d6bd2938a047b46889de4e..2ea7fdc4fbf4d1e2ea477f982b56c9630a643280 100644 --- a/src/QGC.cc +++ b/src/QGC.cc @@ -30,7 +30,7 @@ qreal groundTimeSeconds() return static_cast(groundTimeMilliseconds()) / 1000.0f; } -float limitAngleToPMPIf(float angle) +float limitAngleToPMPIf(double angle) { if (angle > -20*M_PI && angle < 20*M_PI) { diff --git a/src/QGC.h b/src/QGC.h index f853e37f458952501da955762ba2b66e193ec6a8..0b8b07541b89b7ec3d6698e63e11172a13be2923 100644 --- a/src/QGC.h +++ b/src/QGC.h @@ -33,7 +33,7 @@ quint64 groundTimeMilliseconds(); */ qreal groundTimeSeconds(); /** @brief Returns the angle limited to -pi - pi */ -float limitAngleToPMPIf(float angle); +float limitAngleToPMPIf(double angle); /** @brief Returns the angle limited to -pi - pi */ double limitAngleToPMPId(double angle); diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index ec3d865182f5c6a34c42ab8291559ce7dfa4816f..067fe858f0b7c9f1bc9ab0a2a210aa9c2b918d1a 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -57,7 +57,6 @@ #include "FirmwarePluginManager.h" #include "MultiVehicleManager.h" #include "Vehicle.h" -#include "MavlinkQmlSingleton.h" #include "JoystickConfigController.h" #include "JoystickManager.h" #include "QmlObjectListModel.h" @@ -84,6 +83,8 @@ #include "CameraCalc.h" #include "VisualMissionItem.h" #include "EditPositionDialogController.h" +#include "FactValueSliderListModel.h" +#include "KMLFileHelper.h" #ifndef NO_SERIAL_LINK #include "SerialLink.h" @@ -130,11 +131,6 @@ static QObject* screenToolsControllerSingletonFactory(QQmlEngine*, QJSEngine*) return screenToolsController; } -static QObject* mavlinkQmlSingletonFactory(QQmlEngine*, QJSEngine*) -{ - return new MavlinkQmlSingleton; -} - static QObject* qgroundcontrolQmlGlobalSingletonFactory(QQmlEngine*, QJSEngine*) { // We create this object as a QGCTool even though it isn't in the toolbox @@ -144,31 +140,27 @@ static QObject* qgroundcontrolQmlGlobalSingletonFactory(QQmlEngine*, QJSEngine*) return qmlGlobal; } -/** - * @brief Constructor for the main application. - * - * This constructor initializes and starts the whole application. It takes standard - * command-line parameters - * - * @param argc The number of command-line parameters - * @param argv The string array of parameters - **/ +static QObject* kmlFileHelperSingletonFactory(QQmlEngine*, QJSEngine*) +{ + return new KMLFileHelper; +} QGCApplication::QGCApplication(int &argc, char* argv[], bool unitTesting) #ifdef __mobile__ - : QGuiApplication(argc, argv) - , _qmlAppEngine(NULL) - #else - : QApplication(argc, argv) - #endif - , _runningUnitTests(unitTesting) - , _fakeMobile(false) - , _settingsUpgraded(false) - #ifdef QT_DEBUG - , _testHighDPI(false) - #endif - , _toolbox(NULL) - , _bluetoothAvailable(false) + : QGuiApplication (argc, argv) + , _qmlAppEngine (NULL) +#else + : QApplication (argc, argv) +#endif + , _runningUnitTests (unitTesting) + , _logOutput (false) + , _fakeMobile (false) + , _settingsUpgraded (false) +#ifdef QT_DEBUG + , _testHighDPI (false) +#endif + , _toolbox (NULL) + , _bluetoothAvailable (false) { _app = this; @@ -231,6 +223,7 @@ QGCApplication::QGCApplication(int &argc, char* argv[], bool unitTesting) { "--clear-settings", &fClearSettingsOptions, NULL }, { "--logging", &logging, &loggingOptions }, { "--fake-mobile", &_fakeMobile, NULL }, + { "--log-output", &_logOutput, NULL }, #ifdef QT_DEBUG { "--test-high-dpi", &_testHighDPI, NULL }, #endif @@ -349,6 +342,7 @@ void QGCApplication::_shutdown(void) QGCApplication::~QGCApplication() { // Place shutdown code in _shutdown + _app = NULL; } void QGCApplication::_initCommon(void) @@ -373,6 +367,7 @@ void QGCApplication::_initCommon(void) qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "ParameterManager", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "QGCCameraManager", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "QGCCameraControl", "Reference only"); + qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "LinkInterface", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.JoystickManager", 1, 0, "JoystickManager", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.JoystickManager", 1, 0, "Joystick", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.QGCPositionManager", 1, 0, "QGCPositionManager", "Reference only"); @@ -381,6 +376,7 @@ void QGCApplication::_initCommon(void) qmlRegisterUncreatableType ("QGroundControl.Controllers", 1, 0, "GeoFenceController", "Reference only"); qmlRegisterUncreatableType("QGroundControl.Controllers", 1, 0, "RallyPointController", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.Controllers", 1, 0, "VisualMissionItem", "Reference only"); + qmlRegisterUncreatableType("QGroundControl.FactControls", 1, 0, "FactValueSliderListModel","Reference only"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "ParameterEditorController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "ESP8266ComponentController"); @@ -404,7 +400,7 @@ void QGCApplication::_initCommon(void) // Register Qml Singletons qmlRegisterSingletonType ("QGroundControl", 1, 0, "QGroundControl", qgroundcontrolQmlGlobalSingletonFactory); qmlRegisterSingletonType ("QGroundControl.ScreenToolsController", 1, 0, "ScreenToolsController", screenToolsControllerSingletonFactory); - qmlRegisterSingletonType ("QGroundControl.Mavlink", 1, 0, "Mavlink", mavlinkQmlSingletonFactory); + qmlRegisterSingletonType ("QGroundControl.KMLFileHelper", 1, 0, "KMLFileHelper", kmlFileHelperSingletonFactory); } bool QGCApplication::_initForNormalAppBoot(void) diff --git a/src/QGCApplication.h b/src/QGCApplication.h index f2459a6acd1b6ab034230a680937dc80b8117b8f..0c098f13fe3bd7b8b4cbd173a1df301fe1bdba0e 100644 --- a/src/QGCApplication.h +++ b/src/QGCApplication.h @@ -69,9 +69,12 @@ public: /// @brief Clears the persistent flag to delete all settings the next time QGroundControl is started. void clearDeleteAllSettingsNextBoot(void); - /// @brief Returns truee if unit test are being run + /// @brief Returns true if unit tests are being run bool runningUnitTests(void) { return _runningUnitTests; } + /// @brief Returns true if Qt debug output should be logged to a file + bool logOutput(void) { return _logOutput; } + /// Used to report a missing Parameter. Warning will be displayed to user. Method may be called /// multiple times. void reportMissingParameter(int componentId, const QString& name); @@ -158,6 +161,7 @@ private: #endif bool _runningUnitTests; ///< true: running unit tests, false: normal app + bool _logOutput; ///< true: Log Qt debug output to file static const char* _darkStyleFile; static const char* _lightStyleFile; diff --git a/src/QGCPalette.cc b/src/QGCPalette.cc index ec5ac8fa79e1a44e2e2f4aace3f5d8e01a3840a3..90635f5216f566d2892a52d25de59c7b1c47beda 100644 --- a/src/QGCPalette.cc +++ b/src/QGCPalette.cc @@ -51,7 +51,7 @@ void QGCPalette::_buildMap(void) DECLARE_QGC_COLOR(window, "#ffffff", "#ffffff", "#222222", "#222222") DECLARE_QGC_COLOR(windowShade, "#d9d9d9", "#d9d9d9", "#333333", "#333333") DECLARE_QGC_COLOR(windowShadeDark, "#bdbdbd", "#bdbdbd", "#282828", "#282828") - DECLARE_QGC_COLOR(text, "#9d9d9d", "#000000", "#a0a0a0", "#ffffff") + DECLARE_QGC_COLOR(text, "#9d9d9d", "#000000", "#707070", "#ffffff") DECLARE_QGC_COLOR(warningText, "#cc0808", "#cc0808", "#f85761", "#f85761") DECLARE_QGC_COLOR(button, "#ffffff", "#ffffff", "#707070", "#626270") DECLARE_QGC_COLOR(buttonText, "#9d9d9d", "#000000", "#202020", "#ffffff") diff --git a/src/QmlControls/AppMessages.cc b/src/QmlControls/AppMessages.cc index fa88b30472a456314575b57e19e3290a7fd638a5..fab8b7bd6e1fbaf012b66a9006b70f4da313d2d8 100644 --- a/src/QmlControls/AppMessages.cc +++ b/src/QmlControls/AppMessages.cc @@ -11,8 +11,12 @@ // Allows QGlobalStatic to work on this translation unit #define _LOG_CTOR_ACCESS_ public + #include "AppMessages.h" -#include +#include "QGCApplication.h" +#include "SettingsManager.h" +#include "AppSettings.h" + #include #include #include @@ -87,4 +91,26 @@ void AppLogModel::threadsafeLog(const QString message) const int line = rowCount(); insertRows(line, 1); setData(index(line), message, Qt::DisplayRole); + + if (qgcApp() && qgcApp()->logOutput() && _logFile.fileName().isEmpty()) { + qDebug() << _logFile.fileName().isEmpty() << qgcApp()->logOutput(); + QGCToolbox* toolbox = qgcApp()->toolbox(); + // Be careful of toolbox not being open yet + if (toolbox) { + QString saveDirPath = qgcApp()->toolbox()->settingsManager()->appSettings()->crashSavePath(); + QDir saveDir(saveDirPath); + QString saveFilePath = saveDir.absoluteFilePath(QStringLiteral("QGCConsole.log")); + + _logFile.setFileName(saveFilePath); + if (!_logFile.open(QIODevice::WriteOnly | QIODevice::Text)) { + qgcApp()->showMessage(tr("Open console log output file failed %1 : %2").arg(_logFile.fileName()).arg(_logFile.errorString())); + } + } + } + + if (_logFile.isOpen()) { + QTextStream out(&_logFile); + out << message << "\n"; + _logFile.flush(); + } } diff --git a/src/QmlControls/AppMessages.h b/src/QmlControls/AppMessages.h index ff8c44ec45a00ee4a44eb46b56a79e2d9d40ae10..335b487e49331f419eb47c7447733ebb6d757eea 100644 --- a/src/QmlControls/AppMessages.h +++ b/src/QmlControls/AppMessages.h @@ -13,6 +13,7 @@ #include #include #include +#include // Hackish way to force only this translation unit to have public ctor access #ifndef _LOG_CTOR_ACCESS_ @@ -34,6 +35,9 @@ signals: private slots: void threadsafeLog(const QString message); +private: + QFile _logFile; + _LOG_CTOR_ACCESS_: AppLogModel(); }; diff --git a/src/QmlControls/DropPanel.qml b/src/QmlControls/DropPanel.qml index 91fe3dcc58206c681f06b75c3d2f3d36a5dab191..b3dd2f71f98c2819b62aa6b5b84439fcce181355 100644 --- a/src/QmlControls/DropPanel.qml +++ b/src/QmlControls/DropPanel.qml @@ -35,8 +35,8 @@ Item { readonly property int dropUp: 3 readonly property int dropDown: 4 - readonly property real _arrowBaseWidth: radius // Width of long side of arrow - readonly property real _arrowPointHeight: radius * 0.666 // Height is long side to point + readonly property real _arrowBaseHeight: radius // Height of vertical side of arrow + readonly property real _arrowPointWidth: radius * 0.666 // Distance from vertical side to point readonly property real _dropCornerRadius: ScreenTools.defaultFontPixelWidth * 0.5 readonly property real _dropCornerRadiusX2: _dropCornerRadius * 2 readonly property real _dropMargin: _dropCornerRadius @@ -44,9 +44,10 @@ Item { property var _dropEdgeTopPoint property real _dropEdgeHeight - property alias _dropDownComponent: dropDownLoader.sourceComponent + property alias _dropDownComponent: panelLoader.sourceComponent property real _viewportMaxTop: 0 property real _viewportMaxBottom: parent.parent.height - parent.y + property real _viewportMaxHeight: _viewportMaxBottom - _viewportMaxTop property var _dropPanelCancel function show(panelEdgeTopPoint, panelEdgeHeight, panelComponent) { @@ -70,37 +71,29 @@ Item { } function _calcPositions() { - var dropComponentWidth = dropDownLoader.item.width - var dropComponentHeight = dropDownLoader.item.height - var dropRectWidth = dropComponentWidth + _dropMarginX2 - var dropRectHeight = dropComponentHeight + _dropMarginX2 + var panelComponentWidth = panelLoader.item.width + var panelComponentHeight = panelLoader.item.height - dropItemHolderRect.width = dropRectWidth - dropItemHolderRect.height = dropRectHeight - - dropDownItem.width = dropComponentWidth + _dropMarginX2 - dropDownItem.height = dropComponentHeight + _dropMarginX2 - - dropDownItem.width += _arrowPointHeight - - dropDownItem.y = _dropEdgeTopPoint.y -(dropDownItem.height / 2) + radius - - dropItemHolderRect.y = 0 + dropDownItem.width = panelComponentWidth + _dropMarginX2 + _arrowPointWidth + dropDownItem.height = panelComponentHeight + _dropMarginX2 dropDownItem.x = _dropEdgeTopPoint.x + _dropMargin - dropItemHolderRect.x = _arrowPointHeight + dropDownItem.y = _dropEdgeTopPoint.y -(dropDownItem.height / 2) + radius // Validate that dropdown is within viewport dropDownItem.y = Math.min(dropDownItem.y + dropDownItem.height, _viewportMaxBottom) - dropDownItem.height dropDownItem.y = Math.max(dropDownItem.y, _viewportMaxTop) + // Adjust height to not exceed viewport bounds + dropDownItem.height = Math.min(dropDownItem.height, _viewportMaxHeight - dropDownItem.y) + // Arrow points arrowCanvas.arrowPoint.y = (_dropEdgeTopPoint.y + radius) - dropDownItem.y arrowCanvas.arrowPoint.x = 0 - arrowCanvas.arrowBase1.x = _arrowPointHeight - arrowCanvas.arrowBase1.y = arrowCanvas.arrowPoint.y - (_arrowBaseWidth / 2) + arrowCanvas.arrowBase1.x = _arrowPointWidth + arrowCanvas.arrowBase1.y = arrowCanvas.arrowPoint.y - (_arrowBaseHeight / 2) arrowCanvas.arrowBase2.x = arrowCanvas.arrowBase1.x - arrowCanvas.arrowBase2.y = arrowCanvas.arrowBase1.y + _arrowBaseWidth + arrowCanvas.arrowBase2.y = arrowCanvas.arrowBase1.y + _arrowBaseHeight arrowCanvas.requestPaint() } // function - _calcPositions @@ -117,9 +110,14 @@ Item { } } + // This item is sized to hold the entirety of the drop panel including the arrow point Item { id: dropDownItem + DeadMouseArea { + anchors.fill: parent + } + Canvas { id: arrowCanvas anchors.fill: parent @@ -129,32 +127,47 @@ Item { property point arrowBase2: Qt.point(0, 0) onPaint: { + var panelX = _arrowPointWidth + var panelY = 0 + var panelWidth = parent.width - _arrowPointWidth + var panelHeight = parent.height + var context = getContext("2d") context.reset() context.beginPath() - context.moveTo(dropItemHolderRect.x, dropItemHolderRect.y) - context.lineTo(dropItemHolderRect.x + dropItemHolderRect.width, dropItemHolderRect.y) - context.lineTo(dropItemHolderRect.x + dropItemHolderRect.width, dropItemHolderRect.y + dropItemHolderRect.height) - context.lineTo(dropItemHolderRect.x, dropItemHolderRect.y + dropItemHolderRect.height) + context.moveTo(panelX, panelY) // top left + context.lineTo(panelX + panelWidth, panelY) // top right + context.lineTo(panelX + panelWidth, panelX + panelHeight) // bottom right + context.lineTo(panelX, panelY + panelHeight) // bottom left context.lineTo(arrowBase2.x, arrowBase2.y) context.lineTo(arrowPoint.x, arrowPoint.y) context.lineTo(arrowBase1.x, arrowBase1.y) - context.lineTo(dropItemHolderRect.x, dropItemHolderRect.y) + context.lineTo(panelX, panelY) // top left + context.closePath() context.fillStyle = qgcPal.windowShade context.fill() } } // Canvas - arrowCanvas - Item { - id: dropItemHolderRect + QGCFlickable { + id: panelItemFlickable + anchors.margins: _dropMargin + anchors.leftMargin: _dropMargin + _arrowPointWidth + anchors.fill: parent + flickableDirection: Flickable.VerticalFlick + contentWidth: panelLoader.width + contentHeight: panelLoader.height Loader { - id: dropDownLoader + id: panelLoader x: _dropMargin y: _dropMargin + onHeightChanged: _calcPositions() + onWidthChanged: _calcPositions() + property var dropPanel: _root } } diff --git a/src/QmlControls/EditPositionDialog.qml b/src/QmlControls/EditPositionDialog.qml index bd20d0b51578d86755150a15dac92ed2449f106b..c64669ef49bca126ec0913c37b254d657c6c070a 100644 --- a/src/QmlControls/EditPositionDialog.qml +++ b/src/QmlControls/EditPositionDialog.qml @@ -112,6 +112,17 @@ QGCViewDialog { reject() } } + + QGCButton { + anchors.right: parent.right + text: qsTr("Set From Vehicle Position") + visible: QGroundControl.multiVehicleManager.activeVehicle && QGroundControl.multiVehicleManager.activeVehicle.coordinate.isValid + + onClicked: { + controller.setFromVehicle() + reject() + } + } } // Column } // QGCFlickable } // QGCViewDialog diff --git a/src/QmlControls/EditPositionDialogController.cc b/src/QmlControls/EditPositionDialogController.cc index 1ad11b5c9f05ab431684173c45011b69e7278943..9a1f604f2d197e6216ede49b6b77b07874b9b6a4 100644 --- a/src/QmlControls/EditPositionDialogController.cc +++ b/src/QmlControls/EditPositionDialogController.cc @@ -9,6 +9,7 @@ #include "EditPositionDialogController.h" #include "QGCGeo.h" +#include "QGCApplication.h" const char* EditPositionDialogController::_latitudeFactName = "Latitude"; const char* EditPositionDialogController::_longitudeFactName = "Longitude"; @@ -74,3 +75,10 @@ void EditPositionDialogController::setFromUTM(void) qDebug() << _eastingFact.rawValue().toDouble() << _northingFact.rawValue().toDouble() << _zoneFact.rawValue().toInt() << (_hemisphereFact.rawValue().toInt() == 1) << _coordinate; emit coordinateChanged(_coordinate); } + +void EditPositionDialogController::setFromVehicle(void) +{ + _coordinate = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->coordinate(); + emit coordinateChanged(_coordinate); +} + diff --git a/src/QmlControls/EditPositionDialogController.h b/src/QmlControls/EditPositionDialogController.h index 99ecd44ddcb4ec12c3544d88fa0bcae6eb40080e..ed8d90be9f03abdb9fde6ceee25e4e9242d05bd8 100644 --- a/src/QmlControls/EditPositionDialogController.h +++ b/src/QmlControls/EditPositionDialogController.h @@ -42,6 +42,7 @@ public: Q_INVOKABLE void initValues(void); Q_INVOKABLE void setFromGeo(void); Q_INVOKABLE void setFromUTM(void); + Q_INVOKABLE void setFromVehicle(void); signals: void coordinateChanged(QGeoCoordinate coordinate); diff --git a/src/QmlControls/FactSliderPanel.qml b/src/QmlControls/FactSliderPanel.qml index 98f037ca1b9f529b3aa209ea95504eabd4c78bf7..ac4cc10d722d69cda79475f30eefd20087e0eca9 100644 --- a/src/QmlControls/FactSliderPanel.qml +++ b/src/QmlControls/FactSliderPanel.qml @@ -41,18 +41,6 @@ Column { QGCPalette { id: palette; colorGroupEnabled: enabled } - Component.onCompleted: { - // Qml Sliders have a strange behavior in which they first set Slider::value to some internal - // setting and then set Slider::value to the bound properties value. If you have an onValueChanged - // handler which updates your property with the new value, this first value change will trash - // your bound values. In order to work around this we don't set the values into the Sliders until - // after Qml load is done. We also don't track value changes until Qml load completes. - for (var i=0; i - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - - -#ifndef MavlinkQmlSingleton_H -#define MavlinkQmlSingleton_H - -#include - -class MavlinkQmlSingleton : public QObject -{ - Q_OBJECT - Q_ENUMS(Qml_MAV_CMD) - -public: - // Couldn't figure a way to get MAV_CMD enum out to Qml so I had to copy it. Sigh! - - typedef enum { - MAV_CMD_NAV_WAYPOINT=16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LAND_LOCAL=23, /* Land at local position (local frame only) |Landing target number (if available)| Maximum accepted offset from desired landing position [m] - computed magnitude from spherical coordinates: d = sqrt(x^2 + y^2 + z^2), which gives the maximum accepted distance between the desired landing position and the position where the vehicle is about to land| Landing descend rate [ms^-1]| Desired yaw angle [rad]| Y-axis position [m]| X-axis position [m]| Z-axis / ground level position [m]| */ - MAV_CMD_NAV_TAKEOFF_LOCAL=24, /* Takeoff from local position (local frame only) |Minimum pitch (if airspeed sensor present), desired pitch without sensor [rad]| Empty| Takeoff ascend rate [ms^-1]| Yaw angle [rad] (if magnetometer or another yaw estimation source present), ignored without one of these| Y-axis position [m]| X-axis position [m]| Z-axis position [m]| */ - MAV_CMD_NAV_FOLLOW=25, /* Vehicle following, i.e. this waypoint represents the position of a moving vehicle |Following logic to use (e.g. loitering or sinusoidal following) - depends on specific autopilot implementation| Ground speed of vehicle to be followed| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT=30, /* Continue on the current course and climb/descend to specified altitude. When the altitude is reached continue to the next command (i.e., don't proceed to the next command until the desired altitude is reached. |Empty| Empty| Empty| Empty| Empty| Empty| Desired altitude in meters| */ - MAV_CMD_NAV_LOITER_TO_ALT=31, /* Begin loiter at the specified Latitude and Longitude. If Lat=Lon=0, then loiter at the current position. Don't consider the navigation command complete (don't leave loiter) until the altitude has been reached. Additionally, if the Heading Required parameter is non-zero the aircraft will not leave the loiter until heading toward the next waypoint. |Heading Required (0 = False)| Radius in meters. If positive loiter clockwise, negative counter-clockwise, 0 means no change to standard loiter.| Empty| Empty| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - MAV_CMD_NAV_SPLINE_WAYPOINT=82, /* Navigate to MISSION using a spline path. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Empty| Empty| Empty| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - MAV_CMD_NAV_ALTITUDE_WAIT=83, /* Mission command to wait for an altitude or downwards vertical speed. This is meant for high altitude balloon launches, allowing the aircraft to be idle until either an altitude is reached or a negative vertical speed is reached (indicating early balloon burst). The wiggle time is how often to wiggle the control surfaces to prevent them seizing up. |altitude (m)| descent speed (m/s)| Wiggle Time (s)| Empty| Empty| Empty| Empty| */ - MAV_CMD_NAV_VTOL_TAKEOFF=84, - MAV_CMD_NAV_VTOL_LAND=85, - MAV_CMD_NAV_GUIDED_ENABLE=92, /* hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ - MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Custom mode - this is system specific, please refer to the individual autopilot specifications for details.| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ - MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ - MAV_CMD_DO_FLIGHTTERMINATION=185, /* Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_LAND_START=189, /* Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude is optional, and may be set to 0/0 if not needed. If specified then it will be used to help find the closest landing sequence. |Empty| Empty| Empty| Empty| Latitude| Longitude| Empty| */ - MAV_CMD_DO_RALLY_LAND=190, /* Mission command to perform a landing from a rally point. |Break altitude (meters)| Landing speed (m/s)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_GO_AROUND=191, /* Mission command to safely abort an autonmous landing. |Altitude (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ - MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ - MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ - MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| */ - MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable, 2=disable_floor_only)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_MOTOR_TEST=209, /* Mission command to perform motor test |motor sequence number (a number from 1 to max number of motors on the vehicle)| throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)| throttle| timeout (in seconds)| Empty| Empty| Empty| */ - MAV_CMD_DO_INVERTED_FLIGHT=210, /* Change to/from inverted flight |inverted (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_GRIPPER=211, /* Mission command to operate EPM gripper |gripper number (a number from 1 to max number of grippers on the vehicle)| gripper action (0=release, 1=grab. See GRIPPER_ACTIONS enum)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_AUTOTUNE_ENABLE=212, /* Enable/disable autotune |enable (1: enable, 0:disable)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1, w (1 in null-rotation)| q2 - quaternion param #2, x (0 in null-rotation)| q3 - quaternion param #3, y (0 in null-rotation)| q4 - quaternion param #4, z (0 in null-rotation)| Empty| Empty| Empty| */ - MAV_CMD_DO_GUIDED_MASTER=221, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_GUIDED_LIMITS=222, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| */ - MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty| */ - MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow, 5: second magnetometer| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ - MAV_CMD_PREFLIGHT_UAVCAN=243, /* Trigger UAVCAN config. This command will be only accepted if in pre-flight mode. |1: Trigger actuator ID assignment and direction mapping.| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ - MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM, 2: Reset to defaults| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM, 2: Reset to defaults| Onboard logging: 0: Ignore, 1: Start default rate logging, -1: Stop logging, > 1: start logging with rate of param 3 in Hz (e.g. set to 1000 for 1000 Hz logging)| Reserved| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ - MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ - MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ - MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_GET_HOME_POSITION=410, /* Request the home position from the vehicle. |Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ - MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ - MAV_CMD_GET_MESSAGE_INTERVAL=510, /* Request the interval between messages for a particular MAVLink message ID |The MAVLink message ID| */ - MAV_CMD_SET_MESSAGE_INTERVAL=511, /* Request the interval between messages for a particular MAVLink message ID. This interface replaces REQUEST_DATA_STREAM |The MAVLink message ID| The interval between two messages, in microseconds. Set to -1 to disable and 0 to request default rate.| */ - MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES=520, /* Request autopilot capabilities |1: Request autopilot version| Reserved (all remaining params)| */ - MAV_CMD_IMAGE_START_CAPTURE=2000, /* Start image capture sequence */ - MAV_CMD_IMAGE_STOP_CAPTURE=2001, /* Stop image capture sequence */ - MAV_CMD_DO_TRIGGER_CONTROL=2003, /* Enable or disable on-board camera triggering system. |Trigger enable/disable (0 for disable, 1 for start)| Shutter integration time (in ms)| Reserved| */ - MAV_CMD_VIDEO_START_CAPTURE=2500, /* Starts video capture */ - MAV_CMD_VIDEO_STOP_CAPTURE=2501, /* Stop the current video capture */ - MAV_CMD_PANORAMA_CREATE=2800, /* Create a panorama at the current position |Viewing angle horizontal of the panorama (in degrees, +- 0.5 the total angle)| Viewing angle vertical of panorama (in degrees)| Speed of the horizontal rotation (in degrees per second)| Speed of the vertical rotation (in degrees per second)| */ - MAV_CMD_DO_VTOL_TRANSITION=3000, /* Request VTOL transition |The target VTOL state, as defined by ENUM MAV_VTOL_STATE. Only MAV_VTOL_STATE_MC and MAV_VTOL_STATE_FW can be used.| */ - MAV_CMD_PAYLOAD_PREPARE_DEPLOY=30001, /* Deploy payload on a Lat / Lon / Alt position. This includes the navigation to reach the required release position and velocity. |Operation mode. 0: prepare single payload deploy (overwriting previous requests), but do not execute it. 1: execute payload deploy immediately (rejecting further deploy commands during execution, but allowing abort). 2: add payload deploy to existing deployment list.| Desired approach vector in degrees compass heading (0..360). A negative value indicates the system can define the approach vector at will.| Desired ground speed at release time. This can be overridden by the airframe in case it needs to meet minimum airspeed. A negative value indicates the system can define the ground speed at will.| Minimum altitude clearance to the release position in meters. A negative value indicates the system can define the clearance at will.| Latitude unscaled for MISSION_ITEM or in 1e7 degrees for MISSION_ITEM_INT| Longitude unscaled for MISSION_ITEM or in 1e7 degrees for MISSION_ITEM_INT| Altitude, in meters AMSL| */ - MAV_CMD_PAYLOAD_CONTROL_DEPLOY=30002, /* Control the payload deployment. |Operation mode. 0: Abort deployment, continue normal mission. 1: switch to payload deploment mode. 100: delete first payload deployment request. 101: delete all payload deployment requests.| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ - MAV_CMD_DO_START_MAG_CAL=42424, /* Initiate a magnetometer calibration |uint8_t bitmask of magnetometers (0 means all)| Automatically retry on failure (0=no retry, 1=retry).| Save without user input (0=require input, 1=autosave).| Delay (seconds)| Empty| Empty| Empty| */ - MAV_CMD_DO_ACCEPT_MAG_CAL=42425, /* Initiate a magnetometer calibration |uint8_t bitmask of magnetometers (0 means all)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_CANCEL_MAG_CAL=42426, /* Cancel a running magnetometer calibration |uint8_t bitmask of magnetometers (0 means all)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_ENUM_END=42427, /* | */ - } Qml_MAV_CMD; -}; - -#endif // MavlinkQmlSingleton_H diff --git a/src/QmlControls/MissionItemIndexLabel.qml b/src/QmlControls/MissionItemIndexLabel.qml index 5b0e662f5951b5b58600c406e5a6e7ab6b94e292..ff88b1f56099823c54a03c44195cb886ccdac42d 100644 --- a/src/QmlControls/MissionItemIndexLabel.qml +++ b/src/QmlControls/MissionItemIndexLabel.qml @@ -33,7 +33,7 @@ Canvas { property real _gimbalRadians: degreesToRadians(vehicleYaw + gimbalYaw - 90) property real _labelMargin: 2 property real _labelRadius: _indicatorRadius + _labelMargin - property string _label: index === 0 ? "" : label + property string _label: label.length > 1 ? label : "" property string _index: index === 0 || index === -1 ? label.charAt(0) : index onColorChanged: requestPaint() diff --git a/src/QmlControls/PIDTuning.qml b/src/QmlControls/PIDTuning.qml new file mode 100644 index 0000000000000000000000000000000000000000..6467ce0109451f4ce55df7fc6264c6d63ef1dda2 --- /dev/null +++ b/src/QmlControls/PIDTuning.qml @@ -0,0 +1,429 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.3 +import QtQuick.Controls 1.2 +import QtCharts 2.2 +import QtQuick.Layouts 1.2 + +import QGroundControl 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.FactSystem 1.0 +import QGroundControl.FactControls 1.0 +import QGroundControl.ScreenTools 1.0 + +RowLayout { + layoutDirection: Qt.RightToLeft + + property var tuneList + property var params + + property real _chartHeight: ScreenTools.defaultFontPixelHeight * 20 + property real _margins: ScreenTools.defaultFontPixelHeight / 2 + property string _currentTuneType: tuneList[0] + property real _roll: _activeVehicle.roll.value + property real _rollSetpoint: _activeVehicle.setpoint.roll.value + property real _rollRate: _activeVehicle.rollRate.value + property real _rollRateSetpoint: _activeVehicle.setpoint.rollRate.value + property real _pitch: _activeVehicle.pitch.value + property real _pitchSetpoint: _activeVehicle.setpoint.pitch.value + property real _pitchRate: _activeVehicle.pitchRate.value + property real _pitchRateSetpoint: _activeVehicle.setpoint.pitchRate.value + property real _yaw: _activeVehicle.heading.value + property real _yawSetpoint: _activeVehicle.setpoint.yaw.value + property real _yawRate: _activeVehicle.yawRate.value + property real _yawRateSetpoint: _activeVehicle.setpoint.yawRate.value + property var _valueXAxis: valueXAxis + property var _valueRateXAxis: valueRateXAxis + property var _valueYAxis: valueYAxis + property var _valueRateYAxis: valueRateYAxis + property int _msecs: 0 + property var _savedTuningParamValues: [ ] + + // The following are set when getValues is called + property real _value + property real _valueSetpoint + property real _valueRate + property real _valueRateSetpoint + + readonly property int _tickSeparation: 5 + readonly property int _maxTickSections: 10 + readonly property int _tuneListRollIndex: 0 + readonly property int _tuneListPitchIndex: 1 + readonly property int _tuneListYawIndex: 2 + + function adjustYAxisMin(yAxis, newValue) { + var newMin = Math.min(yAxis.min, newValue) + if (newMin % 5 != 0) { + newMin -= 5 + newMin = Math.floor(newMin / _tickSeparation) * _tickSeparation + } + yAxis.min = newMin + } + + function adjustYAxisMax(yAxis, newValue) { + var newMax = Math.max(yAxis.max, newValue) + if (newMax % 5 != 0) { + newMax += 5 + newMax = Math.floor(newMax / _tickSeparation) * _tickSeparation + } + yAxis.max = newMax + } + + function getValues() { + if (_currentTuneType === tuneList[_tuneListRollIndex]) { + _value = _roll + _valueSetpoint = _rollSetpoint + _valueRate = _rollRate + _valueRateSetpoint = _rollRateSetpoint + } else if (_currentTuneType === tuneList[_tuneListPitchIndex]) { + _value = _pitch + _valueSetpoint = _pitchSetpoint + _valueRate = _pitchRate + _valueRateSetpoint = _pitchRateSetpoint + } else if (_currentTuneType === tuneList[_tuneListYawIndex]) { + _value = _yaw + _valueSetpoint = _yawSetpoint + _valueRate = _yawRate + _valueRateSetpoint = _yawRateSetpoint + } + } + + function resetGraphs() { + valueSeries.removePoints(0, valueSeries.count) + valueSetpointSeries.removePoints(0, valueSetpointSeries.count) + valueRateSeries.removePoints(0, valueRateSeries.count) + valueRateSetpointSeries.removePoints(0, valueRateSetpointSeries.count) + _valueXAxis.min = 0 + _valueXAxis.max = 0 + _valueRateXAxis.min = 0 + _valueRateXAxis.max = 0 + _valueYAxis.min = 0 + _valueYAxis.max = 10 + _valueRateYAxis.min = 0 + _valueRateYAxis.max = 10 + _msecs = 0 + } + + function currentTuneTypeIndex() { + if (_currentTuneType === tuneList[_tuneListRollIndex]) { + return _tuneListRollIndex + } else if (_currentTuneType === tuneList[_tuneListPitchIndex]) { + return _tuneListPitchIndex + } else if (_currentTuneType === tuneList[_tuneListYawIndex]) { + return _tuneListYawIndex + } + } + + // Save the current set of tuning values so we can reset to them + function saveTuningParamValues() { + var tuneTypeIndex = currentTuneTypeIndex() + + _savedTuningParamValues = [ ] + var currentTuneParams = params[tuneTypeIndex] + for (var i=0; i _maxPointCount) { + valueSeries.remove(0) + valueSetpointSeries.remove(0) + valueRateSeries.remove(0) + valueRateSetpointSeries.remove(0) + valueXAxis.min = valueSeries.at(0).x + valueRateXAxis.min = valueSeries.at(0).x + } + */ + } + + property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle + property int _maxPointCount: 10000 / interval + } + + Column { + spacing: _margins + Layout.alignment: Qt.AlignTop + + QGCLabel { text: qsTr("Tuning Axis:") } + + RowLayout { + spacing: _margins + + Repeater { + model: tuneList + QGCRadioButton { + text: modelData + checked: _currentTuneType === modelData + exclusiveGroup: tuneTypeRadios + + onClicked: _currentTuneType = modelData + } + } + } + + Item { width: 1; height: 1 } + + QGCLabel { text: qsTr("Tuning Values:") } + + GridLayout { + rows: factList.length + flow: GridLayout.TopToBottom + rowSpacing: _margins + columnSpacing: _margins + + property var factList: params[tuneList.indexOf(_currentTuneType)] + + Repeater { + model: parent.factList + + QGCLabel { text: modelData.name } + } + + Repeater { + model: parent.factList + + QGCButton { + text: "-" + onClicked: { + var value = modelData.value + modelData.value -= value * adjustPercentModel.get(adjustPercentCombo.currentIndex).value + } + } + } + + Repeater { + model: parent.factList + + FactTextField { + Layout.fillWidth: true + fact: modelData + showUnits: false + } + } + + Repeater { + model: parent.factList + + QGCButton { + text: "+" + onClicked: { + var value = modelData.value + modelData.value += value * adjustPercentModel.get(adjustPercentCombo.currentIndex).value + } + } + } + } + + RowLayout { + QGCLabel { text: qsTr("Increment/Decrement %") } + + QGCComboBox { + id: adjustPercentCombo + model: ListModel { + id: adjustPercentModel + ListElement { text: "5"; value: 0.05 } + ListElement { text: "10"; value: 0.10 } + ListElement { text: "15"; value: 0.15 } + ListElement { text: "20"; value: 0.20 } + } + } + } + Item { width: 1; height: 1 } + + QGCLabel { text: qsTr("Saved Tuning Values:") } + + GridLayout { + rows: savedRepeater.model.length + flow: GridLayout.TopToBottom + rowSpacing: _margins + columnSpacing: _margins + + Repeater { + model: params[tuneList.indexOf(_currentTuneType)] + + QGCLabel { text: modelData.name } + } + + Repeater { + id: savedRepeater + + QGCLabel { text: modelData } + } + } + + RowLayout { + spacing: _margins + + QGCButton { + text: qsTr("Save Values") + onClicked: saveTuningParamValues() + } + + QGCButton { + text: qsTr("Reset To Saved Values") + onClicked: resetToSavedTuningParamValues() + } + } + + Item { width: 1; height: 1 } + + QGCLabel { text: qsTr("Chart:") } + + RowLayout { + spacing: _margins + + QGCButton { + text: qsTr("Clear") + onClicked: resetGraphs() + } + + QGCButton { + text: dataTimer.running ? qsTr("Stop") : qsTr("Start") + onClicked: dataTimer.running = !dataTimer.running + } + } + } + + Column { + Layout.fillWidth: true + + ChartView { + anchors.left: parent.left + anchors.right: parent.right + height: availableHeight / 2 + title: _currentTuneType + antialiasing: true + legend.alignment: Qt.AlignRight + + LineSeries { + id: valueSeries + name: "Response" + axisY: valueYAxis + axisX: valueXAxis + } + + LineSeries { + id: valueSetpointSeries + name: "Command" + axisY: valueYAxis + axisX: valueXAxis + } + } + + ChartView { + anchors.left: parent.left + anchors.right: parent.right + height: availableHeight / 2 + title: _currentTuneType + qsTr(" Rate") + antialiasing: true + legend.alignment: Qt.AlignRight + + LineSeries { + id: valueRateSeries + name: "Response" + axisY: valueRateYAxis + axisX: valueRateXAxis + } + + LineSeries { + id: valueRateSetpointSeries + name: "Command" + axisY: valueRateYAxis + axisX: valueRateXAxis + } + } + } +} // RowLayout diff --git a/src/QmlControls/ParameterEditor.qml b/src/QmlControls/ParameterEditor.qml index c09e3546efa3aca8ea9a11f4a00057bb5a206c17..9fa0fec47affc77b0d4e8c7546d344601b264cf3 100644 --- a/src/QmlControls/ParameterEditor.qml +++ b/src/QmlControls/ParameterEditor.qml @@ -32,6 +32,7 @@ QGCView { property bool _searchFilter: searchText.text.trim() != "" ///< true: showing results of search property var _searchResults ///< List of parameter names from search results property bool _showRCToParam: !ScreenTools.isMobile && QGroundControl.multiVehicleManager.activeVehicle.px4Firmware + property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle ParameterEditorController { id: controller; @@ -104,6 +105,7 @@ QGCView { } MenuItem { text: qsTr("Reset all to defaults") + visible: !_activeVehicle.apmFirmware onTriggered: showDialog(resetToDefaultConfirmComponent, qsTr("Reset All"), qgcView.showDialogDefaultWidth, StandardButton.Cancel | StandardButton.Reset) } MenuSeparator { } @@ -334,7 +336,7 @@ QGCView { QGCViewDialog { function accept() { - QGroundControl.multiVehicleManager.activeVehicle.rebootVehicle() + _activeVehicle.rebootVehicle() hideDialog() } diff --git a/src/QmlControls/ParameterEditorDialog.qml b/src/QmlControls/ParameterEditorDialog.qml index 4a97a174dcc4dbef0a1254f67f63f10028ad9236..404a747d6e183d4be00dd92979f7cd4b1cf8a6c3 100644 --- a/src/QmlControls/ParameterEditorDialog.qml +++ b/src/QmlControls/ParameterEditorDialog.qml @@ -20,13 +20,16 @@ import QGroundControl.FactControls 1.0 import QGroundControl.ScreenTools 1.0 QGCViewDialog { - id: root + id: root + focus: true property Fact fact property bool showRCToParam: false property bool validate: false property string validateValue + signal valueChanged + property real _editFieldWidth: ScreenTools.defaultFontPixelWidth * 20 property bool _longDescriptionAvailable: fact.longDescription != "" property bool _editingParameter: fact.componentId != 0 @@ -41,15 +44,18 @@ QGCViewDialog { if (bitmaskColumn.visible && !manualEntry.checked) { fact.value = bitmaskValue(); fact.valueChanged(fact.value) + valueChanged() hideDialog(); } else if (factCombo.visible && !manualEntry.checked) { fact.enumIndex = factCombo.currentIndex + valueChanged() hideDialog() } else { var errorString = fact.validate(valueField.text, forceSave.checked) if (errorString === "") { fact.value = valueField.text fact.valueChanged(fact.value) + valueChanged() hideDialog() } else { validationError.text = errorString @@ -85,12 +91,8 @@ QGCViewDialog { } } - // set focus to the text field when becoming visible (in case of an Enum, - // the valueField is not visible, but it's not an issue because the combo - // box cannot have a focus) - onVisibleChanged: if (visible && !ScreenTools.isMobile) valueField.forceActiveFocus() - QGCFlickable { + id: flickable anchors.fill: parent contentHeight: _column.y + _column.height flickableDirection: Flickable.VerticalFlick @@ -120,9 +122,10 @@ QGCViewDialog { unitsLabel: fact.units showUnits: fact.units != "" Layout.fillWidth: true - inputMethodHints: ScreenTools.isiOS ? - Qt.ImhNone : // iOS numeric keyboard has not done button, we can't use it - Qt.ImhFormattedNumbersOnly // Forces use of virtual numeric keyboard + focus: true + inputMethodHints: (fact.typeIsString || ScreenTools.isiOS) ? + Qt.ImhNone : // iOS numeric keyboard has no done button, we can't use it + Qt.ImhFormattedNumbersOnly // Forces use of virtual numeric keyboard } QGCButton { diff --git a/src/QmlControls/PreFlightCheckButton.qml b/src/QmlControls/PreFlightCheckButton.qml new file mode 100644 index 0000000000000000000000000000000000000000..b0a26c82ef6f1ec4a1e2d5c3f59926b5e2b7ac76 --- /dev/null +++ b/src/QmlControls/PreFlightCheckButton.qml @@ -0,0 +1,142 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.3 +import QtQuick.Controls 1.2 +import QtQuick.Controls.Styles 1.4 + +import QGroundControl 1.0 +import QGroundControl.Palette 1.0 +import QGroundControl.ScreenTools 1.0 + +/// The PreFlightCheckButton supports creating a button which the user then has to verify/click to confirm a check. +/// It also supports failing the check based on values from within the system: telemetry or QGC app values. These +/// controls are normally placed within a PreFlightCheckGroup. +/// +/// Two types of checks may be included on the button: +/// Manual - This is simply a check which the user must verify and confirm. It is not based on any system state. +/// Telemetry - This type of check can fail due to some state within the system. A telemetry check failure can be +/// a hard stop in that there is no way to pass the checklist until the system state resolves itself. +/// Or it can also optionally be override by the user. +/// If a button uses both manual and telemetry checks, the telemetry check takes precendence and must be passed first. +QGCButton { + property string name: "" + property string manualText: "" ///< text to show for a manual check, "" signals no manual check + property string telemetryTextFailure ///< text to show if telemetry check failed (override not allowed) + property bool telemetryFailure: false ///< true: telemetry check failing, false: telemetry check passing + property bool allowTelemetryFailureOverride: false ///< true: user can click past telemetry failure + property bool passed: _manualState === _statePassed && _telemetryState === _statePassed + + property int _manualState: manualText === "" ? _statePassed : _statePending + property int _telemetryState: _statePassed + property int _horizontalPadding: ScreenTools.defaultFontPixelWidth + property int _verticalPadding: Math.round(ScreenTools.defaultFontPixelHeight / 2) + property real _stateFlagWidth: ScreenTools.defaultFontPixelWidth * 4 + + readonly property int _statePending: 0 ///< Telemetry check is failing or manual check not yet verified, user can click to make it pass + readonly property int _stateFailed: 1 ///< Telemetry check is failing, user cannot click to make it pass + readonly property int _statePassed: 2 ///< Check has passed + + readonly property color _passedColor: Qt.rgba(0.27,0.67,0.42,1) + readonly property color _pendingColor: Qt.rgba(0.9,0.47,0.2,1) + readonly property color _failedColor: Qt.rgba(0.92,0.22,0.22,1) + + property string _text: "" + name +": " + + ((_telemetryState !== _statePassed) ? + telemetryTextFailure : + (_manualState !== _statePassed ? manualText : qsTr("Passed"))) + property color _color: _telemetryState === _statePassed && _manualState === _statePassed ? + _passedColor : + (_telemetryState == _stateFailed ? + _failedColor : + (_telemetryState === _statePending || _manualState === _statePending ? + _pendingColor : + _failedColor)) + + property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle + + width: 40 * ScreenTools.defaultFontPixelWidth + + style: ButtonStyle { + padding { + top: _verticalPadding + bottom: _verticalPadding + left: (_horizontalPadding * 2) + _stateFlagWidth + right: _horizontalPadding + } + + background: Rectangle { + color: qgcPal.button + border.color: qgcPal.button; + + Rectangle { + color: _color + anchors.left: parent.left + anchors.top: parent.top + anchors.bottom: parent.bottom + width: _stateFlagWidth + } + } + + label: Label { + text: _text + wrapMode: Text.WordWrap + horizontalAlignment: Text.AlignHCenter + color: qgcPal.buttonText + } + } + + function _updateTelemetryState() { + if (telemetryFailure) { + // We have a new telemetry failure, reset user pass + _telemetryState = allowTelemetryFailureOverride ? _statePending : _stateFailed + } else { + _telemetryState = _statePassed + } + } + + onTelemetryFailureChanged: _updateTelemetryState() + onAllowTelemetryFailureOverrideChanged: _updateTelemetryState() + + onClicked: { + if (telemetryFailure && !allowTelemetryFailureOverride) { + // No way to proceed past this failure + return + } + if (telemetryFailure && allowTelemetryFailureOverride && _telemetryState !== _statePassed) { + // User is allowed to proceed past this failure + _telemetryState = _statePassed + return + } + if (manualText !== "" && _manualState !== _statePassed) { + // User is confirming a manual check + _manualState = _statePassed + } + } + + onPassedChanged: callButtonPassedChanged() + onParentChanged: callButtonPassedChanged() + + function callButtonPassedChanged() { + if (typeof parent.buttonPassedChanged === "function") { + parent.buttonPassedChanged() + } + } + + function reset() { + _manualState = manualText === "" ? _statePassed : _statePending + if (telemetryFailure) { + _telemetryState = allowTelemetryFailureOverride ? _statePending : _stateFailed + } else { + _telemetryState = _statePassed + } + } + + QGCPalette { id: qgcPal; colorGroupEnabled: enabled } +} diff --git a/src/QmlControls/PreFlightCheckGroup.qml b/src/QmlControls/PreFlightCheckGroup.qml new file mode 100644 index 0000000000000000000000000000000000000000..e1b1dd146da7b3dce3715aa3eea6eacf010fac26 --- /dev/null +++ b/src/QmlControls/PreFlightCheckGroup.qml @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.3 +import QtQml.Models 2.1 + +import QGroundControl.Controls 1.0 +import QGroundControl.ScreenTools 1.0 + +/// A PreFlightCheckGroup manages a set of PreFlightCheckButtons as a single entity. +Column { + property string name + property bool passed: false + + spacing: ScreenTools.defaultFontPixelHeight / 2 + + property alias _checked: header.checked + + onPassedChanged: parent.groupPassedChanged(ObjectModel.index) + + Component.onCompleted: { + enabled = _checked + var moveList = [] + for (var i=2; i + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.3 +import QtQuick.Controls 1.2 +import QtQml.Models 2.1 + +import QGroundControl 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Controls 1.0 + +Rectangle { + width: mainColumn.width + 3*ScreenTools.defaultFontPixelWidth + height: mainColumn.height + ScreenTools.defaultFontPixelHeight + color: qgcPal.windowShade + radius: 3 + + property alias model: checkListRepeater.model + + property bool _passed: false + + // We delay the updates when a group passes so the user can see all items green for a moment prior to hiding + Timer { + id: delayedGroupPassed + interval: 750 + + property int index + + onTriggered: { + var group = checkListRepeater.itemAt(index) + group._checked = false + if (index + 1 < checkListRepeater.count) { + group = checkListRepeater.itemAt(index + 1) + group.enabled = true + group._checked = true + } + for (var i=0; i 0) + tooltip: qsTr("Reset the checklist (e.g. after a vehicle reboot)") + + onClicked: model.reset() + + Image { source:"/qmlimages/MapSyncBlack.svg" ; anchors.fill: parent } + } + } + + // All check list items + Repeater { + id: checkListRepeater + } + } // Column +} //Rectangle diff --git a/src/QmlControls/PreFlightCheckModel.qml b/src/QmlControls/PreFlightCheckModel.qml new file mode 100644 index 0000000000000000000000000000000000000000..c7cfab04204d9e0c2377eac8bcc63fa7ce3de21b --- /dev/null +++ b/src/QmlControls/PreFlightCheckModel.qml @@ -0,0 +1,27 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.3 +import QtQml.Models 2.1 + +ObjectModel { + id: _root + + function reset() { + for (var i=0; i<_root.count; i++) { + var group = _root.get(i) + group.reset() + group.enabled = i === 0 + group._checked = i === 0 + } + } + + Component.onCompleted: reset() + +} diff --git a/src/QmlControls/QGCButton.qml b/src/QmlControls/QGCButton.qml index f9cbe883583a6acd69dcd319a30a09633bb89f35..f7adf355b18b4908e3a15c49410a2dd183191727 100644 --- a/src/QmlControls/QGCButton.qml +++ b/src/QmlControls/QGCButton.qml @@ -6,6 +6,8 @@ import QGroundControl.Palette 1.0 import QGroundControl.ScreenTools 1.0 Button { + activeFocusOnPress: true + property bool primary: false ///< primary button for a group of buttons property real pointSize: ScreenTools.defaultFontPointSize ///< Point size for button text diff --git a/src/QmlControls/QGCCheckBox.qml b/src/QmlControls/QGCCheckBox.qml index 735138d28ec87d8a44f310cc35b72e0217c94054..ed602c1678742f9bf8b8f5fb08e9f0e6def34772 100644 --- a/src/QmlControls/QGCCheckBox.qml +++ b/src/QmlControls/QGCCheckBox.qml @@ -6,6 +6,8 @@ import QGroundControl.Palette 1.0 import QGroundControl.ScreenTools 1.0 CheckBox { + activeFocusOnPress: true + property var __qgcPal: QGCPalette { colorGroupEnabled: enabled } style: CheckBoxStyle { diff --git a/src/QmlControls/QGCFileDialog.qml b/src/QmlControls/QGCFileDialog.qml index ecd0782ca9a2b8bdf997e3832c328266f2d8d852..d70730a070f33004b027a74f7b62a43c5582a35c 100644 --- a/src/QmlControls/QGCFileDialog.qml +++ b/src/QmlControls/QGCFileDialog.qml @@ -15,7 +15,7 @@ Item { visible: false property var qgcView - property string folder + property string folder // Due to Qt bug with file url parsing this must be an absolute path property var nameFilters property string fileExtension // Primary file extension to search for property string fileExtension2: "" // Secondary file extension to search for @@ -75,7 +75,7 @@ Item { // the FileDialog fallback mechanism on android 5.9 builds. HackFileDialog { id: fullFileDialog - folder: "file://" + _root.folder + folder: "file:///" + _root.folder nameFilters: _root.nameFilters ? _root.nameFilters : [] title: _root.title selectExisting: _root.selectExisting diff --git a/src/QmlControls/QGCLabel.qml b/src/QmlControls/QGCLabel.qml index 32176837c8ef2adab8df47ff7790d6c2296c64bc..2700e02d9f7ca87daed8bd788484cc8988dadd4a 100644 --- a/src/QmlControls/QGCLabel.qml +++ b/src/QmlControls/QGCLabel.qml @@ -8,8 +8,6 @@ import QGroundControl.ScreenTools 1.0 Text { QGCPalette { id: __qgcPal; colorGroupEnabled: enabled } - property bool enabled: true - font.pointSize: ScreenTools.defaultFontPointSize font.family: ScreenTools.normalFontFamily color: __qgcPal.text diff --git a/src/QmlControls/QGCRadioButton.qml b/src/QmlControls/QGCRadioButton.qml index ccaaf0e4cb8813a14e1ac83a55f2f7c24387737f..e28d327e349a19888d2d12f3690048e77e90ce34 100644 --- a/src/QmlControls/QGCRadioButton.qml +++ b/src/QmlControls/QGCRadioButton.qml @@ -9,6 +9,7 @@ RadioButton { property var color: qgcPal.text ///< Text color property int textStyle: Text.Normal property color textStyleColor: qgcPal.text + property bool textBold: false property var qgcPal: QGCPalette { colorGroupEnabled: enabled } style: RadioButtonStyle { @@ -35,6 +36,7 @@ RadioButton { text: control.text font.pointSize: ScreenTools.defaultFontPointSize font.family: ScreenTools.normalFontFamily + font.bold: control.textBold antialiasing: true color: control.color style: control.textStyle diff --git a/src/QmlControls/QGCTextField.qml b/src/QmlControls/QGCTextField.qml index 459a775bc3305c3ac4922690b3f7db362b6c3b7c..422a4ffaa24be088aad63f54951c61f6c708fb32 100644 --- a/src/QmlControls/QGCTextField.qml +++ b/src/QmlControls/QGCTextField.qml @@ -7,7 +7,10 @@ import QGroundControl.Palette 1.0 import QGroundControl.ScreenTools 1.0 TextField { - id: root + id: root + textColor: qgcPal.textFieldText + implicitHeight: ScreenTools.implicitTextFieldHeight + activeFocusOnPress: true property bool showUnits: false property bool showHelp: false @@ -17,18 +20,11 @@ TextField { property real _helpLayoutWidth: 0 - Component.onCompleted: { - if (typeof qgcTextFieldforwardKeysTo !== 'undefined') { - root.Keys.forwardTo = [qgcTextFieldforwardKeysTo] - } - } + Component.onCompleted: selectAllIfActiveFocus() + onActiveFocusChanged: selectAllIfActiveFocus() QGCPalette { id: qgcPal; colorGroupEnabled: enabled } - textColor: qgcPal.textFieldText - - implicitHeight: ScreenTools.implicitTextFieldHeight - onEditingFinished: { if (ScreenTools.isMobile) { // Toss focus on mobile after Done on virtual keyboard. Prevent strange interactions. @@ -36,6 +32,12 @@ TextField { } } + function selectAllIfActiveFocus() { + if (activeFocus) { + selectAll() + } + } + QGCLabel { id: unitsLabelWidthGenerator text: unitsLabel @@ -59,7 +61,7 @@ TextField { Rectangle { anchors.fill: parent - border.color: control.activeFocus ? "#47b" : "#999" + border.color: root.activeFocus ? "#47b" : "#999" color: qgcPal.textField } @@ -75,7 +77,7 @@ TextField { onWidthChanged: control._helpLayoutWidth = unitsHelpLayout.width Text { - anchors.verticalCenter: parent.verticalCenter + Layout.alignment: Qt.AlignVCenter text: control.unitsLabel font.pointSize: backgroundItem.showHelp ? ScreenTools.smallFontPointSize : ScreenTools.defaultFontPointSize font.family: ScreenTools.normalFontFamily @@ -85,10 +87,9 @@ TextField { } Rectangle { - anchors.margins: 2 - anchors.top: parent.top - anchors.bottom: parent.bottom - anchors.right: parent.right + Layout.margins: 2 + Layout.fillHeight: true + Layout.alignment: Qt.AlignRight width: height * 0.75 color: control.textColor radius: 2 @@ -114,10 +115,4 @@ TextField { padding.right: control._helpLayoutWidth //control.showUnits ? unitsLabelWidthGenerator.width : control.__contentHeight * 0.333 } - - onActiveFocusChanged: { - if (activeFocus) { - selectAll() - } - } } diff --git a/src/QmlControls/QGCView.qml b/src/QmlControls/QGCView.qml index b50ac571e77367df7375af4404b21f47676c0939..4a4c2cef313167c767b2033012aef975f2cedc94 100644 --- a/src/QmlControls/QGCView.qml +++ b/src/QmlControls/QGCView.qml @@ -68,6 +68,7 @@ FactPanel { "viewPanel": viewPanel }) dialog.setupDialogButtons(buttons) + dialog.focus = true viewPanel.enabled = false } diff --git a/src/QmlControls/QGCViewDialog.qml b/src/QmlControls/QGCViewDialog.qml index 0c1e02b1c23ad98b26bc6d6daa8bc78a2b1b63e1..228432734d119843e2b6832008775ca1c5e0aae0 100644 --- a/src/QmlControls/QGCViewDialog.qml +++ b/src/QmlControls/QGCViewDialog.qml @@ -17,8 +17,6 @@ import QGroundControl.FactControls 1.0 import QGroundControl.ScreenTools 1.0 FactPanel { - property var qgcTextFieldforwardKeysTo: this ///< Causes all QGCTextFields to forward keys here if they have focus - property real defaultTextWidth: ScreenTools.defaultFontPixelWidth property real defaultTextHeight: ScreenTools.defaultFontPixelHeight diff --git a/src/QmlControls/QGCViewDialogContainer.qml b/src/QmlControls/QGCViewDialogContainer.qml index dda82095d2e11763365cad8d9cb7d3e6790e937b..f22dc7a55a275bee3dfb746191d2ca8f5d2a3996 100644 --- a/src/QmlControls/QGCViewDialogContainer.qml +++ b/src/QmlControls/QGCViewDialogContainer.qml @@ -15,9 +15,10 @@ import QGroundControl.Controls 1.0 import QGroundControl.Palette 1.0 import QGroundControl.ScreenTools 1.0 -Item { - id: _root - z: 5000 +FocusScope { + id: _root + z: 5000 + focus: true property alias dialogWidth: _dialogPanel.width property alias dialogTitle: titleLabel.text @@ -182,6 +183,7 @@ Item { anchors.top: _spacer.bottom anchors.bottom: parent.bottom sourceComponent: _dialogComponent + focus: true property bool acceptAllowed: _acceptButton.visible property bool rejectAllowed: _rejectButton.visible diff --git a/src/QmlControls/QGroundControl.Controls.qmldir b/src/QmlControls/QGroundControl.Controls.qmldir index 65b86d035be9ab5cd631f4990a6c045c8b953843..cac9e41c9d2cb54bd8e893a335c731a0d9ea44ad 100644 --- a/src/QmlControls/QGroundControl.Controls.qmldir +++ b/src/QmlControls/QGroundControl.Controls.qmldir @@ -32,7 +32,12 @@ OfflineMapButton 1.0 OfflineMapButton.qml PageView 1.0 PageView.qml ParameterEditor 1.0 ParameterEditor.qml ParameterEditorDialog 1.0 ParameterEditorDialog.qml +PIDTuning 1.0 PIDTuning.qml PlanToolBar 1.0 PlanToolBar.qml +PreFlightCheckButton 1.0 PreFlightCheckButton.qml +PreFlightCheckGroup 1.0 PreFlightCheckGroup.qml +PreFlightCheckList 1.0 PreFlightCheckList.qml +PreFlightCheckModel 1.0 PreFlightCheckModel.qml QGCButton 1.0 QGCButton.qml QGCCheckBox 1.0 QGCCheckBox.qml QGCColoredImage 1.0 QGCColoredImage.qml @@ -71,6 +76,7 @@ SimpleItemMapVisuals 1.0 SimpleItemMapVisuals.qml SliderSwitch 1.0 SliderSwitch.qml SubMenuButton 1.0 SubMenuButton.qml SurveyMapVisuals 1.0 SurveyMapVisuals.qml +TransectStyleComplexItemStats 1.0 TransectStyleComplexItemStats.qml ToolStrip 1.0 ToolStrip.qml VehicleRotationCal 1.0 VehicleRotationCal.qml VehicleSummaryRow 1.0 VehicleSummaryRow.qml diff --git a/src/QmlControls/QmlObjectListModel.cc b/src/QmlControls/QmlObjectListModel.cc index 0b1b28e3c9ca27196140c5d114bd609ead11055e..09685f5896b0f33f137892e63e944acdd61e1073 100644 --- a/src/QmlControls/QmlObjectListModel.cc +++ b/src/QmlControls/QmlObjectListModel.cc @@ -107,8 +107,6 @@ bool QmlObjectListModel::removeRows(int position, int rows, const QModelIndex& p beginRemoveRows(QModelIndex(), position, position + rows - 1); for (int row=0; rowdeleteLater(); _objectList.removeAt(position); } endRemoveRows(); @@ -178,11 +176,42 @@ void QmlObjectListModel::insert(int i, QObject* object) setDirty(true); } +void QmlObjectListModel::insert(int i, QList objects) +{ + if (i < 0 || i > _objectList.count()) { + qWarning() << "Invalid index index:count" << i << _objectList.count(); + } + + int j = i; + foreach (QObject* object, objects) { + QQmlEngine::setObjectOwnership(object, QQmlEngine::CppOwnership); + + // Look for a dirtyChanged signal on the object + if (object->metaObject()->indexOfSignal(QMetaObject::normalizedSignature("dirtyChanged(bool)")) != -1) { + if (!_skipDirtyFirstItem || j != 0) { + QObject::connect(object, SIGNAL(dirtyChanged(bool)), this, SLOT(_childDirtyChanged(bool))); + } + } + j++; + + _objectList.insert(i, object); + } + + insertRows(i, objects.count()); + + setDirty(true); +} + void QmlObjectListModel::append(QObject* object) { insert(_objectList.count(), object); } +void QmlObjectListModel::append(QList objects) +{ + insert(_objectList.count(), objects); +} + QObjectList QmlObjectListModel::swapObjectList(const QObjectList& newlist) { QObjectList oldlist(_objectList); diff --git a/src/QmlControls/QmlObjectListModel.h b/src/QmlControls/QmlObjectListModel.h index 78a202dd0db9054179ea3e83aa217552a6f76def..976405ee5794118ae6bca4c8bf2d2d9fc7830aba 100644 --- a/src/QmlControls/QmlObjectListModel.h +++ b/src/QmlControls/QmlObjectListModel.h @@ -37,11 +37,13 @@ public: void setDirty(bool dirty); void append(QObject* object); + void append(QList objects); QObjectList swapObjectList(const QObjectList& newlist); void clear(void); QObject* removeAt(int i); QObject* removeOne(QObject* object) { return removeAt(indexOf(object)); } void insert(int i, QObject* object); + void insert(int i, QList objects); QObject* operator[](int i); const QObject* operator[](int i) const; bool contains(QObject* object) { return _objectList.indexOf(object) != -1; } @@ -63,12 +65,12 @@ private slots: private: // Overrides from QAbstractListModel - virtual int rowCount(const QModelIndex & parent = QModelIndex()) const; - virtual QVariant data(const QModelIndex & index, int role = Qt::DisplayRole) const; - virtual QHash roleNames(void) const; - virtual bool insertRows(int position, int rows, const QModelIndex &index = QModelIndex()); - virtual bool removeRows(int position, int rows, const QModelIndex &index = QModelIndex()); - virtual bool setData(const QModelIndex &index, const QVariant &value, int role = Qt::EditRole); + int rowCount(const QModelIndex & parent = QModelIndex()) const override; + QVariant data(const QModelIndex & index, int role = Qt::DisplayRole) const override; + QHash roleNames(void) const override; + bool insertRows(int position, int rows, const QModelIndex &index = QModelIndex()) override; + bool removeRows(int position, int rows, const QModelIndex &index = QModelIndex()) override; + bool setData(const QModelIndex &index, const QVariant &value, int role = Qt::EditRole) override; private: QList _objectList; diff --git a/src/PlanView/SectionHeader.qml b/src/QmlControls/SectionHeader.qml similarity index 83% rename from src/PlanView/SectionHeader.qml rename to src/QmlControls/SectionHeader.qml index 9f0b6b8a7a582e3f0d9ef43dd17607f7b6559f2d..06c9a2ca533ccb57c9f5a810bd242c016ab98931 100644 --- a/src/PlanView/SectionHeader.qml +++ b/src/QmlControls/SectionHeader.qml @@ -24,7 +24,7 @@ FocusScope { exclusiveGroup.bindCheckable(_root) } - QGCPalette { id: qgcPal; colorGroupEnabled: true } + QGCPalette { id: qgcPal; colorGroupEnabled: enabled } QGCMouseArea { anchors.fill: parent @@ -46,16 +46,15 @@ FocusScope { } QGCLabel { - id: label - anchors.left: parent.left - anchors.right: parent.right + id: label + Layout.fillWidth: true QGCColoredImage { id: image - width: label.height / 2 - height: width anchors.right: parent.right anchors.verticalCenter: parent.verticalCenter + width: label.height / 2 + height: width source: "/qmlimages/arrow-down.png" color: qgcPal.text visible: !_root.checked @@ -63,10 +62,9 @@ FocusScope { } Rectangle { - anchors.left: parent.left - anchors.right: parent.right - height: 1 - color: qgcPal.text + Layout.fillWidth: true + height: 1 + color: qgcPal.text } } } diff --git a/src/QmlControls/SliderSwitch.qml b/src/QmlControls/SliderSwitch.qml index 3aef291bbf64ea7368c11cb38a6a7024af6bc9de..2fcf738dc6c88df66e0d67bba9f29b1e01ee10f3 100644 --- a/src/QmlControls/SliderSwitch.qml +++ b/src/QmlControls/SliderSwitch.qml @@ -11,7 +11,7 @@ Rectangle { implicitWidth: label.contentWidth + (_diameter * 2.5) + (_border * 4) implicitHeight: label.height * 2.5 radius: height /2 - color: qgcPal.text + color: qgcPal.windowShade signal accept ///< Action confirmed signal reject ///< Action rejected @@ -29,7 +29,7 @@ Rectangle { anchors.horizontalCenter: parent.horizontalCenter anchors.verticalCenter: parent.verticalCenter text: confirmText - color: qgcPal.window + color: qgcPal.buttonText } Rectangle { @@ -39,8 +39,7 @@ Rectangle { height: _diameter width: _diameter radius: _diameter / 2 - color: qgcPal.windowShade - opacity: 0.8 + color: qgcPal.primaryButton QGCColoredImage { anchors.centerIn: parent @@ -50,7 +49,7 @@ Rectangle { fillMode: Image.PreserveAspectFit smooth: false mipmap: false - color: qgcPal.text + color: qgcPal.buttonText cache: false source: "/res/ArrowRight.svg" } diff --git a/src/QtLocationPlugin/QGCMapEngine.cpp b/src/QtLocationPlugin/QGCMapEngine.cpp index 2b694f519d4da682ae44f28042ce1b3a194cc2d4..fcd219fbd32954e31f46bd14b29aa9f1a7e92f4d 100644 --- a/src/QtLocationPlugin/QGCMapEngine.cpp +++ b/src/QtLocationPlugin/QGCMapEngine.cpp @@ -92,6 +92,12 @@ stQGeoTileCacheQGCMapTypes kEsriTypes[] = { #define NUM_ESRIMAPS (sizeof(kEsriTypes) / sizeof(stQGeoTileCacheQGCMapTypes)) +stQGeoTileCacheQGCMapTypes kElevationTypes[] = { + {"Airmap Elevation Data", UrlFactory::AirmapElevation} +}; + +#define NUM_ELEVMAPS (sizeof(kElevationTypes) / sizeof(stQGeoTileCacheQGCMapTypes)) + static const char* kMaxDiskCacheKey = "MaxDiskCache"; static const char* kMaxMemCacheKey = "MaxMemoryCache"; @@ -106,6 +112,9 @@ getQGCMapEngine() return kMapEngine; } +//----------------------------------------------------------------------------- +const double QGCMapEngine::srtm1TileSize = 0.01; + //----------------------------------------------------------------------------- void destroyMapEngine() @@ -297,10 +306,17 @@ QGCMapEngine::getTileCount(int zoom, double topleftLon, double topleftLat, doubl if(zoom < 1) zoom = 1; if(zoom > MAX_MAP_ZOOM) zoom = MAX_MAP_ZOOM; QGCTileSet set; - set.tileX0 = long2tileX(topleftLon, zoom); - set.tileY0 = lat2tileY(topleftLat, zoom); - set.tileX1 = long2tileX(bottomRightLon, zoom); - set.tileY1 = lat2tileY(bottomRightLat, zoom); + if (mapType != UrlFactory::AirmapElevation) { + set.tileX0 = long2tileX(topleftLon, zoom); + set.tileY0 = lat2tileY(topleftLat, zoom); + set.tileX1 = long2tileX(bottomRightLon, zoom); + set.tileY1 = lat2tileY(bottomRightLat, zoom); + } else { + set.tileX0 = long2elevationTileX(topleftLon, zoom); + set.tileY0 = lat2elevationTileY(bottomRightLat, zoom); + set.tileX1 = long2elevationTileX(bottomRightLon, zoom); + set.tileY1 = lat2elevationTileY(topleftLat, zoom); + } set.tileCount = (quint64)((quint64)set.tileX1 - (quint64)set.tileX0 + 1) * (quint64)((quint64)set.tileY1 - (quint64)set.tileY0 + 1); set.tileSize = UrlFactory::averageSizeForType(mapType) * set.tileCount; return set; @@ -320,6 +336,22 @@ QGCMapEngine::lat2tileY(double lat, int z) return (int)(floor((1.0 - log( tan(lat * M_PI/180.0) + 1.0 / cos(lat * M_PI/180.0)) / M_PI) / 2.0 * pow(2.0, z))); } +//----------------------------------------------------------------------------- +int +QGCMapEngine::long2elevationTileX(double lon, int z) +{ + Q_UNUSED(z); + return (int)(floor((lon + 180.0) / srtm1TileSize)); +} + +//----------------------------------------------------------------------------- +int +QGCMapEngine::lat2elevationTileY(double lat, int z) +{ + Q_UNUSED(z); + return (int)(floor((lat + 90.0) / srtm1TileSize)); +} + //----------------------------------------------------------------------------- UrlFactory::MapType QGCMapEngine::getTypeFromName(const QString& name) @@ -337,6 +369,10 @@ QGCMapEngine::getTypeFromName(const QString& name) if(name.compare(kEsriTypes[i].name, Qt::CaseInsensitive) == 0) return kEsriTypes[i].type; } + for(i = 0; i < NUM_ELEVMAPS; i++) { + if(name.compare(kElevationTypes[i].name, Qt::CaseInsensitive) == 0) + return kElevationTypes[i].type; + } return UrlFactory::Invalid; } @@ -471,6 +507,7 @@ QGCMapEngine::concurrentDownloads(UrlFactory::MapType type) case UrlFactory::EsriWorldStreet: case UrlFactory::EsriWorldSatellite: case UrlFactory::EsriTerrain: + case UrlFactory::AirmapElevation: return 12; /* case UrlFactory::MapQuestMap: diff --git a/src/QtLocationPlugin/QGCMapEngine.h b/src/QtLocationPlugin/QGCMapEngine.h index e0fd28036ce5dc4cabd1bec0383f5708878d4b0a..352fcbd198f5ac67bc9fc3fa57834b34e7d5275d 100644 --- a/src/QtLocationPlugin/QGCMapEngine.h +++ b/src/QtLocationPlugin/QGCMapEngine.h @@ -94,12 +94,17 @@ public: static QGCTileSet getTileCount (int zoom, double topleftLon, double topleftLat, double bottomRightLon, double bottomRightLat, UrlFactory::MapType mapType); static int long2tileX (double lon, int z); static int lat2tileY (double lat, int z); + static int long2elevationTileX (double lon, int z); + static int lat2elevationTileY (double lat, int z); static QString getTileHash (UrlFactory::MapType type, int x, int y, int z); static UrlFactory::MapType getTypeFromName (const QString &name); static QString bigSizeToString (quint64 size); static QString numberToString (quint64 number); static int concurrentDownloads (UrlFactory::MapType type); + /// size of an elevation tile in degree + static const double srtm1TileSize; + private slots: void _updateTotals (quint32 totaltiles, quint64 totalsize, quint32 defaulttiles, quint64 defaultsize); void _pruned (); diff --git a/src/QtLocationPlugin/QGCMapTileSet.cpp b/src/QtLocationPlugin/QGCMapTileSet.cpp index 70e70f2e8cd2c0ea5ec6c9e260e775f0b594aa29..f2ac50f9e91642f8ec8c193ef837b32e96d0bd8e 100644 --- a/src/QtLocationPlugin/QGCMapTileSet.cpp +++ b/src/QtLocationPlugin/QGCMapTileSet.cpp @@ -19,6 +19,7 @@ #include "QGCMapEngine.h" #include "QGCMapTileSet.h" #include "QGCMapEngineManager.h" +#include "TerrainTile.h" #include #include @@ -282,6 +283,9 @@ QGCCachedTileSet::_networkReplyFinished() qCDebug(QGCCachedTileSetLog) << "Tile fetched" << hash; QByteArray image = reply->readAll(); UrlFactory::MapType type = getQGCMapEngine()->hashToType(hash); + if (type == UrlFactory::MapType::AirmapElevation) { + image = TerrainTile::serialize(image); + } QString format = getQGCMapEngine()->urlFactory()->getImageFormat(type, image); if(!format.isEmpty()) { //-- Cache tile diff --git a/src/QtLocationPlugin/QGCMapUrlEngine.cpp b/src/QtLocationPlugin/QGCMapUrlEngine.cpp index 89ee5cbdee635cd8f2c9300d79b2caadc97d4986..c0c2732da7eb73606a925422da50bb2fe4d0a736 100644 --- a/src/QtLocationPlugin/QGCMapUrlEngine.cpp +++ b/src/QtLocationPlugin/QGCMapUrlEngine.cpp @@ -122,6 +122,9 @@ UrlFactory::getImageFormat(MapType type, const QByteArray& image) case BingHybrid: format = "jpg"; break; + case AirmapElevation: + format = "bin"; + break; default: qWarning("UrlFactory::getImageFormat() Unknown map id %d", type); break; @@ -183,6 +186,10 @@ UrlFactory::getTileURL(MapType type, int x, int y, int zoom, QNetworkAccessManag } return request; + case AirmapElevation: + request.setRawHeader("Referrer", "https://api.airmap.com/"); + break; + default: break; } @@ -262,7 +269,7 @@ UrlFactory::_getURL(MapType type, int x, int y, int zoom, QNetworkAccessManager* #endif case StatkartTopo: { - return QString("http://opencache.statkart.no/gatekeeper/gk/gk.open_gmaps?layers=topo2&zoom=%1&x=%2&y=%3").arg(zoom).arg(x).arg(y); + return QString("http://opencache.statkart.no/gatekeeper/gk/gk.open_gmaps?layers=topo4&zoom=%1&x=%2&y=%3").arg(zoom).arg(x).arg(y); } break; case EniroTopo: @@ -403,6 +410,14 @@ UrlFactory::_getURL(MapType type, int x, int y, int zoom, QNetworkAccessManager* } } break; + case AirmapElevation: + { + return QString("https://api.airmap.com/elevation/v1/ele/carpet?points=%1,%2,%3,%4").arg(static_cast(y)*QGCMapEngine::srtm1TileSize - 90.0).arg( + static_cast(x)*QGCMapEngine::srtm1TileSize - 180.0).arg( + static_cast(y + 1)*QGCMapEngine::srtm1TileSize - 90.0).arg( + static_cast(x + 1)*QGCMapEngine::srtm1TileSize - 180.0); + } + break; default: qWarning("Unknown map id %d\n", type); @@ -545,6 +560,7 @@ UrlFactory::_tryCorrectGoogleVersions(QNetworkAccessManager* networkManager) #define AVERAGE_MAPBOX_SAT_MAP 15739 #define AVERAGE_MAPBOX_STREET_MAP 5648 #define AVERAGE_TILE_SIZE 13652 +#define AVERAGE_AIRMAP_ELEV_SIZE 2786 //----------------------------------------------------------------------------- quint32 @@ -568,6 +584,8 @@ UrlFactory::averageSizeForType(MapType type) case MapboxStreetsBasic: case MapboxRunBikeHike: return AVERAGE_MAPBOX_STREET_MAP; + case AirmapElevation: + return AVERAGE_AIRMAP_ELEV_SIZE; case GoogleLabels: case MapboxDark: case MapboxLight: diff --git a/src/QtLocationPlugin/QGCMapUrlEngine.h b/src/QtLocationPlugin/QGCMapUrlEngine.h index 193e63e80114f4a413d5d5d8e50fd8ed8e8c5f12..1dc2281d618add8d7f23f304162527512b753191 100644 --- a/src/QtLocationPlugin/QGCMapUrlEngine.h +++ b/src/QtLocationPlugin/QGCMapUrlEngine.h @@ -73,7 +73,9 @@ public: EsriWorldStreet = 7000, EsriWorldSatellite = 7001, - EsriTerrain = 7002 + EsriTerrain = 7002, + + AirmapElevation = 8000 }; UrlFactory (); diff --git a/src/QtLocationPlugin/QGCTileCacheWorker.cpp b/src/QtLocationPlugin/QGCTileCacheWorker.cpp index 074bcf0a6a24be516fa9a3109c2eb69bc0a3c484..0f847311dacce3841cb7e0bc1d45e9cd25978b87 100644 --- a/src/QtLocationPlugin/QGCTileCacheWorker.cpp +++ b/src/QtLocationPlugin/QGCTileCacheWorker.cpp @@ -188,18 +188,15 @@ QGCCacheWorker::run() //-- Wait a bit before shutting things down _waitmutex.lock(); int timeout = 5000; - if(!_waitc.wait(&_waitmutex, timeout)) - { - _waitmutex.unlock(); - _mutex.lock(); - //-- If nothing to do, close db and leave thread - if(!_taskQueue.count()) { - _mutex.unlock(); - break; - } + _waitc.wait(&_waitmutex, timeout); + _waitmutex.unlock(); + _mutex.lock(); + //-- If nothing to do, close db and leave thread + if(!_taskQueue.count()) { _mutex.unlock(); + break; } - _waitmutex.unlock(); + _mutex.unlock(); } } if(_db) { @@ -1094,15 +1091,40 @@ QGCCacheWorker::_createDB(QSqlDatabase* db, bool createDefault) void QGCCacheWorker::_testInternet() { + /* + To test if you have Internet connection, the code tests a connection to + 8.8.8.8:53 (google DNS). It appears that some routers are now blocking TCP + connections to port 53. So instead, we use a TCP connection to "github.com" + (80). On exit, if the look up for “github.com” is under way, a call to abort + the lookup is made. This abort call on Android has no effect, and the code + blocks for a full minute. So to work around the issue, we continue a direct + TCP connection to 8.8.8.8:53 on Android and do the lookup/connect on the + other platforms. + */ +#if defined(__android__) + QTcpSocket socket; + socket.connectToHost("8.8.8.8", 53); + if (socket.waitForConnected(2000)) { + qCDebug(QGCTileCacheLog) << "Yes Internet Access"; + emit internetStatus(true); + return; + } + qWarning() << "No Internet Access"; + emit internetStatus(false); +#else if(!_hostLookupID) { _hostLookupID = QHostInfo::lookupHost("www.github.com", this, SLOT(_lookupReady(QHostInfo))); } +#endif } //----------------------------------------------------------------------------- void QGCCacheWorker::_lookupReady(QHostInfo info) { +#if defined(__android__) + Q_UNUSED(info); +#else _hostLookupID = 0; if(info.error() == QHostInfo::NoError && info.addresses().size()) { QTcpSocket socket; @@ -1118,4 +1140,5 @@ QGCCacheWorker::_lookupReady(QHostInfo info) } qWarning() << "No Internet Access"; emit internetStatus(false); +#endif } diff --git a/src/QtLocationPlugin/QGeoMapReplyQGC.cpp b/src/QtLocationPlugin/QGeoMapReplyQGC.cpp index 2c88231c972402883d38db751f7e2e1170d80a22..eaee21fc02da06db6a80ccc0cab858ffc2b27257 100644 --- a/src/QtLocationPlugin/QGeoMapReplyQGC.cpp +++ b/src/QtLocationPlugin/QGeoMapReplyQGC.cpp @@ -51,6 +51,7 @@ #include #include #include +#include "TerrainTile.h" int QGeoTiledMapReplyQGC::_requestCount = 0; @@ -104,6 +105,7 @@ QGeoTiledMapReplyQGC::abort() _timer.stop(); if (_reply) _reply->abort(); + emit aborted(); } //----------------------------------------------------------------------------- @@ -112,19 +114,32 @@ QGeoTiledMapReplyQGC::networkReplyFinished() { _timer.stop(); if (!_reply) { + emit aborted(); return; } if (_reply->error() != QNetworkReply::NoError) { + emit aborted(); return; } QByteArray a = _reply->readAll(); - setMapImageData(a); QString format = getQGCMapEngine()->urlFactory()->getImageFormat((UrlFactory::MapType)tileSpec().mapId(), a); - if(!format.isEmpty()) { - setMapImageFormat(format); - getQGCMapEngine()->cacheTile((UrlFactory::MapType)tileSpec().mapId(), tileSpec().x(), tileSpec().y(), tileSpec().zoom(), a, format); + //-- Test for a specialized, elevation data (not map tile) + if ((UrlFactory::MapType)tileSpec().mapId() == UrlFactory::MapType::AirmapElevation) { + a = TerrainTile::serialize(a); + //-- Cache it if valid + if(!a.isEmpty()) { + getQGCMapEngine()->cacheTile(UrlFactory::MapType::AirmapElevation, tileSpec().x(), tileSpec().y(), tileSpec().zoom(), a, format); + } + emit terrainDone(a, QNetworkReply::NoError); + } else { + //-- This is a map tile. Process and cache it if valid. + setMapImageData(a); + if(!format.isEmpty()) { + setMapImageFormat(format); + getQGCMapEngine()->cacheTile((UrlFactory::MapType)tileSpec().mapId(), tileSpec().x(), tileSpec().y(), tileSpec().zoom(), a, format); + } + setFinished(true); } - setFinished(true); _clearReply(); } @@ -136,11 +151,17 @@ QGeoTiledMapReplyQGC::networkReplyError(QNetworkReply::NetworkError error) if (!_reply) { return; } - if (error != QNetworkReply::OperationCanceledError) { - qWarning() << "Fetch tile error:" << _reply->errorString(); - setError(QGeoTiledMapReply::CommunicationError, _reply->errorString()); + //-- Test for a specialized, elevation data (not map tile) + if ((UrlFactory::MapType)tileSpec().mapId() == UrlFactory::MapType::AirmapElevation) { + emit terrainDone(QByteArray(), error); + } else { + //-- Regular map tile + if (error != QNetworkReply::OperationCanceledError) { + qWarning() << "Fetch tile error:" << _reply->errorString(); + setError(QGeoTiledMapReply::CommunicationError, _reply->errorString()); + } + setFinished(true); } - setFinished(true); _clearReply(); } @@ -149,8 +170,12 @@ void QGeoTiledMapReplyQGC::cacheError(QGCMapTask::TaskType type, QString /*errorString*/) { if(!getQGCMapEngine()->isInternetActive()) { - setError(QGeoTiledMapReply::CommunicationError, "Network not available"); - setFinished(true); + if ((UrlFactory::MapType)tileSpec().mapId() == UrlFactory::MapType::AirmapElevation) { + emit terrainDone(QByteArray(), QNetworkReply::NetworkSessionFailedError); + } else { + setError(QGeoTiledMapReply::CommunicationError, "Network not available"); + setFinished(true); + } } else { if(type != QGCMapTask::taskFetchTile) { qWarning() << "QGeoTiledMapReplyQGC::cacheError() for wrong task"; @@ -181,10 +206,16 @@ QGeoTiledMapReplyQGC::cacheError(QGCMapTask::TaskType type, QString /*errorStrin void QGeoTiledMapReplyQGC::cacheReply(QGCCacheTile* tile) { - setMapImageData(tile->img()); - setMapImageFormat(tile->format()); - setFinished(true); - setCached(true); + //-- Test for a specialized, elevation data (not map tile) + if ((UrlFactory::MapType)tileSpec().mapId() == UrlFactory::MapType::AirmapElevation) { + emit terrainDone(tile->img(), QNetworkReply::NoError); + } else { + //-- Regular map tile + setMapImageData(tile->img()); + setMapImageFormat(tile->format()); + setFinished(true); + setCached(true); + } tile->deleteLater(); } @@ -195,4 +226,5 @@ QGeoTiledMapReplyQGC::timeout() if(_reply) { _reply->abort(); } + emit aborted(); } diff --git a/src/QtLocationPlugin/QGeoMapReplyQGC.h b/src/QtLocationPlugin/QGeoMapReplyQGC.h index dcde1f0b3b7fe8984b820038aa9fb898d129b6ff..9f5e812f62e7ed3062da4ce1cc82de0b96bd8803 100644 --- a/src/QtLocationPlugin/QGeoMapReplyQGC.h +++ b/src/QtLocationPlugin/QGeoMapReplyQGC.h @@ -61,6 +61,9 @@ public: ~QGeoTiledMapReplyQGC(); void abort(); +signals: + void terrainDone (QByteArray responseBytes, QNetworkReply::NetworkError error); + private slots: void networkReplyFinished (); void networkReplyError (QNetworkReply::NetworkError error); diff --git a/src/QtLocationPlugin/QMLControl/OfflineMap.qml b/src/QtLocationPlugin/QMLControl/OfflineMap.qml index 187c12a2dc061b89654b9a39cbc609990079da14..e7c8c87a098dc4de217b8998c239ac7bd7f82f13 100644 --- a/src/QtLocationPlugin/QMLControl/OfflineMap.qml +++ b/src/QtLocationPlugin/QMLControl/OfflineMap.qml @@ -213,6 +213,31 @@ QGCView { } } + QGCFileDialog { + id: fileDialog + qgcView: offlineMapView + folder: QGroundControl.settingsManager.appSettings.missionSavePath + nameFilters: ["Tile Sets (*.qgctiledb)"] + fileExtension: "qgctiledb" + + onAcceptedForSave: { + if (QGroundControl.mapEngineManager.exportSets(file)) { + rootLoader.sourceComponent = exportToDiskProgress + } else { + showList() + } + close() + } + + onAcceptedForLoad: { + if(!QGroundControl.mapEngineManager.importSets(file)) { + showList(); + mainWindow.enableToolbar() + } + close() + } + } + MessageDialog { id: errorDialog visible: false @@ -339,6 +364,7 @@ QGCView { allowVehicleLocationCenter: false gesture.flickDeceleration: 3000 mapName: "OfflineMap" + qgcView: offlineMapView property bool isSatelliteMap: activeMapType.name.indexOf("Satellite") > -1 || activeMapType.name.indexOf("Hybrid") > -1 @@ -433,7 +459,7 @@ QGCView { Row { spacing: ScreenTools.defaultFontPixelWidth anchors.horizontalCenter: parent.horizontalCenter - visible: !_defaultSet + visible: !_defaultSet && mapType !== "Airmap Elevation Data" QGCLabel { text: qsTr("Zoom Levels:"); width: infoView._labelWidth; } QGCLabel { text: offlineMapView._currentSelection ? (offlineMapView._currentSelection.minZoom + " - " + offlineMapView._currentSelection.maxZoom) : ""; horizontalAlignment: Text.AlignRight; width: infoView._valueWidth; } } @@ -730,6 +756,16 @@ QGCView { } } } + QGCCheckBox { + anchors.left: parent.left + anchors.right: parent.right + text: qsTr("Fetch elevation data") + checked: QGroundControl.mapEngineManager.fetchElevation + onClicked: { + QGroundControl.mapEngineManager.fetchElevation = checked + handleChanges() + } + } } Rectangle { @@ -1034,10 +1070,9 @@ QGCView { width: _bigButtonSize enabled: QGroundControl.mapEngineManager.selectedCount > 0 onClicked: { - showList(); - if(QGroundControl.mapEngineManager.exportSets()) { - rootLoader.sourceComponent = exportToDiskProgress - } + fileDialog.title = qsTr("Export Tile Set") + fileDialog.selectExisting = false + fileDialog.openForSave() } } QGCButton { @@ -1190,11 +1225,10 @@ QGCView { text: qsTr("Import") width: _bigButtonSize * 1.25 onClicked: { - if(!QGroundControl.mapEngineManager.importSets()) { - showList(); - mainWindow.enableToolbar() - rootLoader.sourceComponent = null - } + rootLoader.sourceComponent = null + fileDialog.title = qsTr("Import Tile Set") + fileDialog.selectExisting = true + fileDialog.openForLoad() } } QGCButton { diff --git a/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc b/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc index 52037ae32bfb22819f4460022623e403cc8e35ce..926845cf53cf35776af275b5fdf4c694ba13b705 100644 --- a/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc +++ b/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc @@ -41,6 +41,7 @@ QGCMapEngineManager::QGCMapEngineManager(QGCApplication* app, QGCToolbox* toolbo , _setID(UINT64_MAX) , _freeDiskSpace(0) , _diskSpace(0) + , _fetchElevation(true) , _actionProgress(0) , _importAction(ActionNone) , _importReplace(false) @@ -82,6 +83,10 @@ QGCMapEngineManager::updateForCurrentView(double lon0, double lat0, double lon1, QGCTileSet set = QGCMapEngine::getTileCount(z, lon0, lat0, lon1, lat1, mapType); _totalSet += set; } + if (_fetchElevation) { + QGCTileSet set = QGCMapEngine::getTileCount(1, lon0, lat0, lon1, lat1, UrlFactory::AirmapElevation); + _totalSet += set; + } emit tileX0Changed(); emit tileX1Changed(); emit tileY0Changed(); @@ -157,6 +162,26 @@ QGCMapEngineManager::startDownload(const QString& name, const QString& mapType) } else { qWarning() << "QGCMapEngineManager::startDownload() No Tiles to save"; } + if (mapType != "Airmap Elevation Data" && _fetchElevation) { + QGCCachedTileSet* set = new QGCCachedTileSet(name + " Elevation"); + set->setMapTypeStr("Airmap Elevation Data"); + set->setTopleftLat(_topleftLat); + set->setTopleftLon(_topleftLon); + set->setBottomRightLat(_bottomRightLat); + set->setBottomRightLon(_bottomRightLon); + set->setMinZoom(1); + set->setMaxZoom(1); + set->setTotalTileSize(_totalSet.tileSize); + set->setTotalTileCount(_totalSet.tileCount); + set->setType(QGCMapEngine::getTypeFromName("Airmap Elevation Data")); + QGCCreateTileSetTask* task = new QGCCreateTileSetTask(set); + //-- Create Tile Set (it will also create a list of tiles to download) + connect(task, &QGCCreateTileSetTask::tileSetSaved, this, &QGCMapEngineManager::_tileSetSaved); + connect(task, &QGCMapTask::error, this, &QGCMapEngineManager::taskError); + getQGCMapEngine()->addTask(task); + } else { + qWarning() << "QGCMapEngineManager::startDownload() No Tiles to save"; + } } //----------------------------------------------------------------------------- diff --git a/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h b/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h index 0d1b4199f5719f6ee5fc6446af652aae1c8f0d60..3a5d283d9e0d0e855a6f44e7ef768ceabcbf73cd 100644 --- a/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h +++ b/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h @@ -50,6 +50,7 @@ public: Q_PROPERTY(quint32 maxMemCache READ maxMemCache WRITE setMaxMemCache NOTIFY maxMemCacheChanged) Q_PROPERTY(quint32 maxDiskCache READ maxDiskCache WRITE setMaxDiskCache NOTIFY maxDiskCacheChanged) Q_PROPERTY(QString errorMessage READ errorMessage NOTIFY errorMessageChanged) + Q_PROPERTY(bool fetchElevation READ fetchElevation WRITE setFetchElevation NOTIFY fetchElevationChanged) //-- Disk Space in MB Q_PROPERTY(quint32 freeDiskSpace READ freeDiskSpace NOTIFY freeDiskSpaceChanged) Q_PROPERTY(quint32 diskSpace READ diskSpace CONSTANT) @@ -88,6 +89,7 @@ public: quint32 maxMemCache (); quint32 maxDiskCache (); QString errorMessage () { return _errorMessage; } + bool fetchElevation () { return _fetchElevation; } quint64 freeDiskSpace () { return _freeDiskSpace; } quint64 diskSpace () { return _diskSpace; } int selectedCount (); @@ -100,6 +102,7 @@ public: void setImportReplace (bool replace) { _importReplace = replace; emit importReplaceChanged(); } void setImportAction (ImportAction action) {_importAction = action; emit importActionChanged(); } void setErrorMessage (const QString& error) { _errorMessage = error; emit errorMessageChanged(); } + void setFetchElevation (bool fetchElevation) { _fetchElevation = fetchElevation; emit fetchElevationChanged(); } // Override from QGCTool void setToolbox(QGCToolbox *toolbox); @@ -115,6 +118,7 @@ signals: void maxMemCacheChanged (); void maxDiskCacheChanged (); void errorMessageChanged (); + void fetchElevationChanged (); void freeDiskSpaceChanged (); void selectedCountChanged (); void actionProgressChanged (); @@ -149,6 +153,7 @@ private: quint32 _diskSpace; QmlObjectListModel _tileSets; QString _errorMessage; + bool _fetchElevation; int _actionProgress; ImportAction _importAction; bool _importReplace; diff --git a/src/Settings/App.SettingsGroup.json b/src/Settings/App.SettingsGroup.json index 81520a00d83d844c8af5b5436cfe0616471935de..32d51f2bb4437097208fce78c1eec822d599a445 100644 --- a/src/Settings/App.SettingsGroup.json +++ b/src/Settings/App.SettingsGroup.json @@ -117,6 +117,13 @@ "type": "bool", "defaultValue": false }, +{ + "name": "UseChecklist", + "shortDescription": "Use preflight checklist", + "longDescription": "If this option is enabled the preflight checklist will be used.", + "type": "bool", + "defaultValue": false +}, { "name": "BaseDeviceFontPointSize", "shortDescription": "Application font size", @@ -183,5 +190,13 @@ "shortDescription": "Default firmware type for flashing", "type": "uint32", "defaultValue": 12 +}, +{ + "name": "FollowTarget", + "shortDescription": "Stream GCS' coordinates to Autopilot", + "type": "uint32", + "enumStrings": "Never,Always,When in Follow Me Flight Mode", + "enumValues": "0,1,2", + "defaultValue": 0 } ] diff --git a/src/Settings/AppSettings.cc b/src/Settings/AppSettings.cc index 3e8b950e88401411b79425f9ede36e74db97f3eb..b9f341c7d62cc8a9e582c1bd4eb316af43269faf 100644 --- a/src/Settings/AppSettings.cc +++ b/src/Settings/AppSettings.cc @@ -15,7 +15,9 @@ #include #include -const char* AppSettings::appSettingsGroupName = "App"; +const char* AppSettings::name = "App"; +const char* AppSettings::settingsGroup = ""; // settings are in root group + const char* AppSettings::offlineEditingFirmwareTypeSettingsName = "OfflineEditingFirmwareType"; const char* AppSettings::offlineEditingVehicleTypeSettingsName = "OfflineEditingVehicleType"; const char* AppSettings::offlineEditingCruiseSpeedSettingsName = "OfflineEditingCruiseSpeed"; @@ -33,10 +35,12 @@ const char* AppSettings::indoorPaletteName = "StyleIs const char* AppSettings::showLargeCompassName = "ShowLargeCompass"; const char* AppSettings::savePathName = "SavePath"; const char* AppSettings::autoLoadMissionsName = "AutoLoadMissions"; +const char* AppSettings::useChecklistName = "UseChecklist"; const char* AppSettings::mapboxTokenName = "MapboxToken"; const char* AppSettings::esriTokenName = "EsriToken"; const char* AppSettings::defaultFirmwareTypeName = "DefaultFirmwareType"; const char* AppSettings::gstDebugName = "GstreamerDebugLevel"; +const char* AppSettings::followTargetName = "FollowTarget"; const char* AppSettings::parameterFileExtension = "params"; const char* AppSettings::planFileExtension = "plan"; @@ -53,30 +57,33 @@ const char* AppSettings::telemetryDirectory = "Telemetry"; const char* AppSettings::missionDirectory = "Missions"; const char* AppSettings::logDirectory = "Logs"; const char* AppSettings::videoDirectory = "Video"; +const char* AppSettings::crashDirectory = "CrashLogs"; AppSettings::AppSettings(QObject* parent) - : SettingsGroup(appSettingsGroupName, QString() /* root settings group */, parent) - , _offlineEditingFirmwareTypeFact(NULL) - , _offlineEditingVehicleTypeFact(NULL) - , _offlineEditingCruiseSpeedFact(NULL) - , _offlineEditingHoverSpeedFact(NULL) - , _offlineEditingAscentSpeedFact(NULL) - , _offlineEditingDescentSpeedFact(NULL) - , _batteryPercentRemainingAnnounceFact(NULL) - , _defaultMissionItemAltitudeFact(NULL) - , _telemetrySaveFact(NULL) - , _telemetrySaveNotArmedFact(NULL) - , _audioMutedFact(NULL) - , _virtualJoystickFact(NULL) - , _appFontPointSizeFact(NULL) - , _indoorPaletteFact(NULL) - , _showLargeCompassFact(NULL) - , _savePathFact(NULL) - , _autoLoadMissionsFact(NULL) - , _mapboxTokenFact(NULL) - , _esriTokenFact(NULL) - , _defaultFirmwareTypeFact(NULL) - , _gstDebugFact(NULL) + : SettingsGroup (name, settingsGroup, parent) + , _offlineEditingFirmwareTypeFact (NULL) + , _offlineEditingVehicleTypeFact (NULL) + , _offlineEditingCruiseSpeedFact (NULL) + , _offlineEditingHoverSpeedFact (NULL) + , _offlineEditingAscentSpeedFact (NULL) + , _offlineEditingDescentSpeedFact (NULL) + , _batteryPercentRemainingAnnounceFact (NULL) + , _defaultMissionItemAltitudeFact (NULL) + , _telemetrySaveFact (NULL) + , _telemetrySaveNotArmedFact (NULL) + , _audioMutedFact (NULL) + , _virtualJoystickFact (NULL) + , _appFontPointSizeFact (NULL) + , _indoorPaletteFact (NULL) + , _showLargeCompassFact (NULL) + , _savePathFact (NULL) + , _autoLoadMissionsFact (NULL) + , _useChecklistFact (NULL) + , _mapboxTokenFact (NULL) + , _esriTokenFact (NULL) + , _defaultFirmwareTypeFact (NULL) + , _gstDebugFact (NULL) + , _followTargetFact (NULL) { QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); qmlRegisterUncreatableType("QGroundControl.SettingsManager", 1, 0, "AppSettings", "Reference only"); @@ -118,6 +125,7 @@ void AppSettings::_checkSavePathDirectories(void) savePathDir.mkdir(missionDirectory); savePathDir.mkdir(logDirectory); savePathDir.mkdir(videoDirectory); + savePathDir.mkdir(crashDirectory); } } @@ -216,6 +224,15 @@ Fact* AppSettings::audioMuted(void) return _audioMutedFact; } +Fact* AppSettings::useChecklist(void) +{ + if (!_useChecklistFact) { + _useChecklistFact = _createSettingsFact(useChecklistName); + } + + return _useChecklistFact; +} + Fact* AppSettings::appFontPointSize(void) { if (!_appFontPointSizeFact) { @@ -279,67 +296,68 @@ Fact* AppSettings::savePath(void) QString AppSettings::missionSavePath(void) { - QString fullPath; - QString path = savePath()->rawValue().toString(); if (!path.isEmpty() && QDir(path).exists()) { QDir dir(path); return dir.filePath(missionDirectory); } - return fullPath; + return QString(); } QString AppSettings::parameterSavePath(void) { - QString fullPath; - QString path = savePath()->rawValue().toString(); if (!path.isEmpty() && QDir(path).exists()) { QDir dir(path); return dir.filePath(parameterDirectory); } - return fullPath; + return QString(); } QString AppSettings::telemetrySavePath(void) { - QString fullPath; - QString path = savePath()->rawValue().toString(); if (!path.isEmpty() && QDir(path).exists()) { QDir dir(path); return dir.filePath(telemetryDirectory); } - return fullPath; + return QString(); } QString AppSettings::logSavePath(void) { - QString fullPath; - QString path = savePath()->rawValue().toString(); if (!path.isEmpty() && QDir(path).exists()) { QDir dir(path); return dir.filePath(logDirectory); } - return fullPath; + return QString(); } QString AppSettings::videoSavePath(void) { - QString fullPath; - QString path = savePath()->rawValue().toString(); if (!path.isEmpty() && QDir(path).exists()) { QDir dir(path); return dir.filePath(videoDirectory); } - return fullPath; + return QString(); +} + +QString AppSettings::crashSavePath(void) +{ + QString path = savePath()->rawValue().toString(); + if (!path.isEmpty() && QDir(path).exists()) { + QDir dir(path); + return dir.filePath(crashDirectory); + } + + return QString(); } Fact* AppSettings::autoLoadMissions(void) @@ -400,3 +418,13 @@ Fact* AppSettings::defaultFirmwareType(void) return _defaultFirmwareTypeFact; } + +Fact* AppSettings::followTarget(void) +{ + if (!_followTargetFact) { + _followTargetFact = _createSettingsFact(followTargetName); + } + + return _followTargetFact; +} + diff --git a/src/Settings/AppSettings.h b/src/Settings/AppSettings.h index 6b421aa35ddac5ce78e53ba0a85d3920af3992c7..327ce30950dcb013741cfa63c7dbdd4db8d4d1b6 100644 --- a/src/Settings/AppSettings.h +++ b/src/Settings/AppSettings.h @@ -37,16 +37,19 @@ public: Q_PROPERTY(Fact* showLargeCompass READ showLargeCompass CONSTANT) Q_PROPERTY(Fact* savePath READ savePath CONSTANT) Q_PROPERTY(Fact* autoLoadMissions READ autoLoadMissions CONSTANT) + Q_PROPERTY(Fact* useChecklist READ useChecklist CONSTANT) Q_PROPERTY(Fact* mapboxToken READ mapboxToken CONSTANT) Q_PROPERTY(Fact* esriToken READ esriToken CONSTANT) Q_PROPERTY(Fact* defaultFirmwareType READ defaultFirmwareType CONSTANT) Q_PROPERTY(Fact* gstDebug READ gstDebug CONSTANT) + Q_PROPERTY(Fact* followTarget READ followTarget CONSTANT) Q_PROPERTY(QString missionSavePath READ missionSavePath NOTIFY savePathsChanged) Q_PROPERTY(QString parameterSavePath READ parameterSavePath NOTIFY savePathsChanged) Q_PROPERTY(QString telemetrySavePath READ telemetrySavePath NOTIFY savePathsChanged) Q_PROPERTY(QString logSavePath READ logSavePath NOTIFY savePathsChanged) Q_PROPERTY(QString videoSavePath READ videoSavePath NOTIFY savePathsChanged) + Q_PROPERTY(QString crashSavePath READ crashSavePath NOTIFY savePathsChanged) Q_PROPERTY(QString planFileExtension MEMBER planFileExtension CONSTANT) Q_PROPERTY(QString missionFileExtension MEMBER missionFileExtension CONSTANT) @@ -73,21 +76,25 @@ public: Fact* showLargeCompass (void); Fact* savePath (void); Fact* autoLoadMissions (void); + Fact* useChecklist (void); Fact* mapboxToken (void); Fact* esriToken (void); Fact* defaultFirmwareType (void); Fact* gstDebug (void); + Fact* followTarget (void); QString missionSavePath (void); QString parameterSavePath (void); QString telemetrySavePath (void); QString logSavePath (void); QString videoSavePath (void); + QString crashSavePath (void); static MAV_AUTOPILOT offlineEditingFirmwareTypeFromFirmwareType(MAV_AUTOPILOT firmwareType); static MAV_TYPE offlineEditingVehicleTypeFromVehicleType(MAV_TYPE vehicleType); - static const char* appSettingsGroupName; + static const char* name; + static const char* settingsGroup; static const char* offlineEditingFirmwareTypeSettingsName; static const char* offlineEditingVehicleTypeSettingsName; @@ -106,10 +113,12 @@ public: static const char* showLargeCompassName; static const char* savePathName; static const char* autoLoadMissionsName; + static const char* useChecklistName; static const char* mapboxTokenName; static const char* esriTokenName; static const char* defaultFirmwareTypeName; static const char* gstDebugName; + static const char* followTargetName; // Application wide file extensions static const char* parameterFileExtension; @@ -128,6 +137,7 @@ public: static const char* missionDirectory; static const char* logDirectory; static const char* videoDirectory; + static const char* crashDirectory; signals: void savePathsChanged(void); @@ -154,10 +164,12 @@ private: SettingsFact* _showLargeCompassFact; SettingsFact* _savePathFact; SettingsFact* _autoLoadMissionsFact; + SettingsFact* _useChecklistFact; SettingsFact* _mapboxTokenFact; SettingsFact* _esriTokenFact; SettingsFact* _defaultFirmwareTypeFact; SettingsFact* _gstDebugFact; + SettingsFact* _followTargetFact; }; #endif diff --git a/src/Settings/AutoConnectSettings.cc b/src/Settings/AutoConnectSettings.cc index cb5a403b2ff340e5c0ffdf582536f76be1f892c9..ef7050712d2c902225ebdab52ab7962e53611d22 100644 --- a/src/Settings/AutoConnectSettings.cc +++ b/src/Settings/AutoConnectSettings.cc @@ -13,7 +13,8 @@ #include #include -const char* AutoConnectSettings::_settingsGroup = "LinkManager"; +const char* AutoConnectSettings::name = "AutoConnect"; +const char* AutoConnectSettings::settingsGroup = "LinkManager"; const char* AutoConnectSettings:: autoConnectUDPSettingsName = "AutoconnectUDP"; const char* AutoConnectSettings:: autoConnectPixhawkSettingsName = "AutoconnectPixhawk"; @@ -27,10 +28,9 @@ const char* AutoConnectSettings:: udpListenPortName = "Autocon const char* AutoConnectSettings:: udpTargetHostIPName = "AutoconnectUDPTargetHostIP"; const char* AutoConnectSettings:: udpTargetHostPortName = "AutoconnectUDPTargetHostPort"; -const char* AutoConnectSettings::autoConnectSettingsGroupName = "AutoConnect"; AutoConnectSettings::AutoConnectSettings(QObject* parent) - : SettingsGroup (autoConnectSettingsGroupName, _settingsGroup, parent) + : SettingsGroup (name, settingsGroup, parent) , _autoConnectUDPFact (NULL) , _autoConnectPixhawkFact (NULL) , _autoConnectSiKRadioFact (NULL) diff --git a/src/Settings/AutoConnectSettings.h b/src/Settings/AutoConnectSettings.h index 74081bd76e9ab8107d91972f31b3afeb7f33c2c5..039d3b90b181a3aa05ca76500fca3e0d20b662e0 100644 --- a/src/Settings/AutoConnectSettings.h +++ b/src/Settings/AutoConnectSettings.h @@ -43,7 +43,8 @@ public: Fact* udpTargetHostIP (void); Fact* udpTargetHostPort (void); - static const char* autoConnectSettingsGroupName; + static const char* name; + static const char* settingsGroup; static const char* autoConnectUDPSettingsName; static const char* autoConnectPixhawkSettingsName; @@ -69,8 +70,6 @@ private: SettingsFact* _udpListenPortFact; SettingsFact* _udpTargetHostIPFact; SettingsFact* _udpTargetHostPortFact; - - static const char* _settingsGroup; }; #endif diff --git a/src/Settings/BrandImageSettings.cc b/src/Settings/BrandImageSettings.cc index 20bdc6d3f3e81a8740ffdc64b34fd527e711bf2b..f660869c7e8487a4f2e2077c456839aaf4f98855 100644 --- a/src/Settings/BrandImageSettings.cc +++ b/src/Settings/BrandImageSettings.cc @@ -12,12 +12,14 @@ #include #include -const char* BrandImageSettings::brandImageSettingsGroupName = "BrandImage"; -const char* BrandImageSettings::userBrandImageIndoorName = "UserBrandImageIndoor"; -const char* BrandImageSettings::userBrandImageOutdoorName = "UserBrandImageOutdoor"; +const char* BrandImageSettings::name = "BrandImage"; +const char* BrandImageSettings::settingsGroup = ""; // settings are in root group + +const char* BrandImageSettings::userBrandImageIndoorName = "UserBrandImageIndoor"; +const char* BrandImageSettings::userBrandImageOutdoorName = "UserBrandImageOutdoor"; BrandImageSettings::BrandImageSettings(QObject* parent) - : SettingsGroup(brandImageSettingsGroupName, QString() /* root settings group */, parent) + : SettingsGroup(name, settingsGroup, parent) , _userBrandImageIndoorFact(NULL) , _userBrandImageOutdoorFact(NULL) { diff --git a/src/Settings/BrandImageSettings.h b/src/Settings/BrandImageSettings.h index c2f66b3d36203b2574329aa5c46f875d41c51228..a1bb1b581fdf28d6543cb1fe15f3c1c472a530f3 100644 --- a/src/Settings/BrandImageSettings.h +++ b/src/Settings/BrandImageSettings.h @@ -25,7 +25,8 @@ public: Fact* userBrandImageIndoor (void); Fact* userBrandImageOutdoor (void); - static const char* brandImageSettingsGroupName; + static const char* name; + static const char* settingsGroup; static const char* userBrandImageIndoorName; static const char* userBrandImageOutdoorName; diff --git a/src/Settings/FlightMapSettings.cc b/src/Settings/FlightMapSettings.cc index 7f501614a2029cca46d829aa17880c6f78f8ab94..d2af25689c1b94858cdcc1e43735862240e2896c 100644 --- a/src/Settings/FlightMapSettings.cc +++ b/src/Settings/FlightMapSettings.cc @@ -16,13 +16,14 @@ #include #include -const char* FlightMapSettings::flightMapSettingsGroupName = "FlightMap"; -const char* FlightMapSettings::mapProviderSettingsName = "MapProvider"; -const char* FlightMapSettings::mapTypeSettingsName = "MapType"; -const char* FlightMapSettings::_settingsGroupName = "FlightMap"; +const char* FlightMapSettings::name = "FlightMap"; +const char* FlightMapSettings::settingsGroup = "FlightMap"; + +const char* FlightMapSettings::mapProviderSettingsName = "MapProvider"; +const char* FlightMapSettings::mapTypeSettingsName = "MapType"; FlightMapSettings::FlightMapSettings(QObject* parent) - : SettingsGroup(flightMapSettingsGroupName, QString(_settingsGroupName) /* root settings group */, parent) + : SettingsGroup(name, settingsGroup, parent) , _mapProviderFact(NULL) , _mapTypeFact(NULL) { diff --git a/src/Settings/FlightMapSettings.h b/src/Settings/FlightMapSettings.h index d7dc2a373a207479d408e090f834bece491124ef..edbc79391889bb6736398950e1c9c3afae0c70ef 100644 --- a/src/Settings/FlightMapSettings.h +++ b/src/Settings/FlightMapSettings.h @@ -43,7 +43,9 @@ public: Fact* mapProvider (void); Fact* mapType (void); - static const char* flightMapSettingsGroupName; + static const char* name; + static const char* settingsGroup; + static const char* mapProviderSettingsName; static const char* mapTypeSettingsName; @@ -61,8 +63,6 @@ private: SettingsFact* _mapTypeFact; QStringList _savedMapTypeStrings; QVariantList _savedMapTypeValues; - - static const char* _settingsGroupName; }; #endif diff --git a/src/Settings/GuidedSettings.cc b/src/Settings/GuidedSettings.cc index 3aa93e2af65f3ea434eb2b2b0a6706c3836ede60..9c81519bb8f12de286da1a703871879f825a71b2 100644 --- a/src/Settings/GuidedSettings.cc +++ b/src/Settings/GuidedSettings.cc @@ -15,14 +15,16 @@ #include #include -const char* GuidedSettings::guidedSettingsGroupName = "Guided"; +const char* GuidedSettings::name = "Guided"; +const char* GuidedSettings::settingsGroup = ""; // settings are in root group + const char* GuidedSettings::fixedWingMinimumAltitudeName = "FixedWingMinimumAltitude"; const char* GuidedSettings::fixedWingMaximumAltitudeName = "FixedWingMaximumAltitude"; const char* GuidedSettings::vehicleMinimumAltitudeName = "VehicleMinimumAltitude"; const char* GuidedSettings::vehicleMaximumAltitudeName = "VehicleMaximumAltitude"; GuidedSettings::GuidedSettings(QObject* parent) - : SettingsGroup(guidedSettingsGroupName, QString() /* root settings group */, parent) + : SettingsGroup(name, settingsGroup, parent) , _fixedWingMinimumAltitudeFact (NULL) , _fixedWingMaximumAltitudeFact (NULL) , _vehicleMinimumAltitudeFact (NULL) diff --git a/src/Settings/GuidedSettings.h b/src/Settings/GuidedSettings.h index bc6b29de02dc7226fd0b1cece86d8402dfb13f30..52bba3829a2de7f36872d10eafc63d5bc0ad8030 100644 --- a/src/Settings/GuidedSettings.h +++ b/src/Settings/GuidedSettings.h @@ -31,7 +31,8 @@ public: Fact* vehicleMinimumAltitude (void); Fact* vehicleMaximumAltitude (void); - static const char* guidedSettingsGroupName; + static const char* name; + static const char* settingsGroup; static const char* fixedWingMinimumAltitudeName; static const char* fixedWingMaximumAltitudeName; diff --git a/src/Settings/RTKSettings.cc b/src/Settings/RTKSettings.cc index 1e8699162038ee57fb7dc005b840006a056397bd..c876da317af4f84f621dbfa08dd59477f10945a3 100644 --- a/src/Settings/RTKSettings.cc +++ b/src/Settings/RTKSettings.cc @@ -12,12 +12,14 @@ #include #include -const char* RTKSettings::RTKSettingsGroupName = "RTK"; +const char* RTKSettings::name = "RTK"; +const char* RTKSettings::settingsGroup = "RTK"; + const char* RTKSettings::surveyInAccuracyLimitName = "SurveyInAccuracyLimit"; const char* RTKSettings::surveyInMinObservationDurationName = "SurveyInMinObservationDuration"; RTKSettings::RTKSettings(QObject* parent) - : SettingsGroup(RTKSettingsGroupName, QString(RTKSettingsGroupName), parent) + : SettingsGroup(name, settingsGroup, parent) , _surveyInAccuracyLimitFact(NULL) , _surveyInMinObservationDurationFact(NULL) { diff --git a/src/Settings/RTKSettings.h b/src/Settings/RTKSettings.h index 467df27a1433cae4effe5294098e0de758334f54..6688c4ce7a170afb558008418e4e4241ebbd7b74 100644 --- a/src/Settings/RTKSettings.h +++ b/src/Settings/RTKSettings.h @@ -24,7 +24,8 @@ public: Fact* surveyInAccuracyLimit (void); Fact* surveyInMinObservationDuration(void); - static const char* RTKSettingsGroupName; + static const char* name; + static const char* settingsGroup; static const char* surveyInAccuracyLimitName; static const char* surveyInMinObservationDurationName; diff --git a/src/Settings/SettingsGroup.cc b/src/Settings/SettingsGroup.cc index e0f1ac231397146c46dbd31c3be6b44158b2d272..0e56bdfb408cf550d68a1c00d45cff1d81a57016 100644 --- a/src/Settings/SettingsGroup.cc +++ b/src/Settings/SettingsGroup.cc @@ -12,18 +12,18 @@ #include "QGCApplication.h" SettingsGroup::SettingsGroup(const QString& name, const QString& settingsGroup, QObject* parent) - : QObject(parent) - , _name(name) + : QObject (parent) + , _name (name) , _settingsGroup(settingsGroup) - , _visible(qgcApp()->toolbox()->corePlugin()->overrideSettingsGroupVisibility(name)) + , _visible (qgcApp()->toolbox()->corePlugin()->overrideSettingsGroupVisibility(_name)) { QString jsonNameFormat(":/json/%1.SettingsGroup.json"); - _nameToMetaDataMap = FactMetaData::createMapFromJsonFile(jsonNameFormat.arg(name), this); + _nameToMetaDataMap = FactMetaData::createMapFromJsonFile(jsonNameFormat.arg(_name), this); } -SettingsFact* SettingsGroup::_createSettingsFact(const QString& name) +SettingsFact* SettingsGroup::_createSettingsFact(const QString& factName) { - return new SettingsFact(_settingsGroup, _nameToMetaDataMap[name], this); + return new SettingsFact(_settingsGroup, _nameToMetaDataMap[factName], this); } diff --git a/src/Settings/SettingsGroup.h b/src/Settings/SettingsGroup.h index 048c0b1132fdb721cb48c5f962ad505776f2c934..db85bb427334d2effea47d2cc25b4c40b504ba30 100644 --- a/src/Settings/SettingsGroup.h +++ b/src/Settings/SettingsGroup.h @@ -31,10 +31,10 @@ public: Q_PROPERTY(bool visible MEMBER _visible CONSTANT) protected: - SettingsFact* _createSettingsFact(const QString& name); + SettingsFact* _createSettingsFact(const QString& factName); - QString _name; - QString _settingsGroup; + QString _name; ///< Name for group. Used to generate name for loaded json meta data file. + QString _settingsGroup; ///< QSettings group which contains these settings. empty for settings in root bool _visible; QMap _nameToMetaDataMap; diff --git a/src/Settings/UnitsSettings.cc b/src/Settings/UnitsSettings.cc index cd5988891cbfadc336882b45e70f667bf769a0c9..bab937858c3e79b8b3a83169f36f0152326f314c 100644 --- a/src/Settings/UnitsSettings.cc +++ b/src/Settings/UnitsSettings.cc @@ -12,14 +12,16 @@ #include #include -const char* UnitsSettings::unitsSettingsGroupName = "Units"; -const char* UnitsSettings::distanceUnitsSettingsName = "DistanceUnits"; -const char* UnitsSettings::areaUnitsSettingsName = "AreaUnits"; -const char* UnitsSettings::speedUnitsSettingsName = "SpeedUnits"; -const char* UnitsSettings::temperatureUnitsSettingsName = "TemperatureUnits"; +const char* UnitsSettings::name = "Units"; +const char* UnitsSettings::settingsGroup = ""; // settings are in root group + +const char* UnitsSettings::distanceUnitsSettingsName = "DistanceUnits"; +const char* UnitsSettings::areaUnitsSettingsName = "AreaUnits"; +const char* UnitsSettings::speedUnitsSettingsName = "SpeedUnits"; +const char* UnitsSettings::temperatureUnitsSettingsName = "TemperatureUnits"; UnitsSettings::UnitsSettings(QObject* parent) - : SettingsGroup(unitsSettingsGroupName, QString() /* root settings group */, parent) + : SettingsGroup(name, settingsGroup, parent) , _distanceUnitsFact(NULL) , _areaUnitsFact(NULL) , _speedUnitsFact(NULL) @@ -44,7 +46,7 @@ Fact* UnitsSettings::distanceUnits(void) metaData->setEnumInfo(enumStrings, enumValues); metaData->setRawDefaultValue(DistanceUnitsMeters); - _distanceUnitsFact = new SettingsFact(QString() /* no settings group */, metaData, this); + _distanceUnitsFact = new SettingsFact(_settingsGroup, metaData, this); } @@ -67,7 +69,7 @@ Fact* UnitsSettings::areaUnits(void) metaData->setEnumInfo(enumStrings, enumValues); metaData->setRawDefaultValue(AreaUnitsSquareMeters); - _areaUnitsFact = new SettingsFact(QString() /* no settings group */, metaData, this); + _areaUnitsFact = new SettingsFact(_settingsGroup, metaData, this); } return _areaUnitsFact; @@ -89,7 +91,7 @@ Fact* UnitsSettings::speedUnits(void) metaData->setEnumInfo(enumStrings, enumValues); metaData->setRawDefaultValue(SpeedUnitsMetersPerSecond); - _speedUnitsFact = new SettingsFact(QString() /* no settings group */, metaData, this); + _speedUnitsFact = new SettingsFact(_settingsGroup, metaData, this); } return _speedUnitsFact; @@ -110,7 +112,7 @@ Fact* UnitsSettings::temperatureUnits(void) metaData->setEnumInfo(enumStrings, enumValues); metaData->setRawDefaultValue(TemperatureUnitsCelsius); - _temperatureUnitsFact = new SettingsFact(QString() /* no settings group */, metaData, this); + _temperatureUnitsFact = new SettingsFact(_settingsGroup, metaData, this); } return _temperatureUnitsFact; diff --git a/src/Settings/UnitsSettings.h b/src/Settings/UnitsSettings.h index 1d09201c5cc49533689ddb620e77b1c352a58300..28c3d3dc93a08e15c2a9103dcaa563bc8a6e3c18 100644 --- a/src/Settings/UnitsSettings.h +++ b/src/Settings/UnitsSettings.h @@ -61,7 +61,8 @@ public: Fact* speedUnits (void); Fact* temperatureUnits (void); - static const char* unitsSettingsGroupName; + static const char* name; + static const char* settingsGroup; static const char* distanceUnitsSettingsName; static const char* areaUnitsSettingsName; diff --git a/src/Settings/VideoSettings.cc b/src/Settings/VideoSettings.cc index cb6ab514f1eeca4f5250f42d4d82017caf65d53e..a76d588a2d6dded9d5130cbef798a04c0069cb36 100644 --- a/src/Settings/VideoSettings.cc +++ b/src/Settings/VideoSettings.cc @@ -17,7 +17,8 @@ #include #endif -const char* VideoSettings::videoSettingsGroupName = "Video"; +const char* VideoSettings::name = "Video"; +const char* VideoSettings::settingsGroup = ""; // settings are in root group const char* VideoSettings::videoSourceName = "VideoSource"; const char* VideoSettings::udpPortName = "VideoUDPPort"; @@ -40,7 +41,7 @@ const char* VideoSettings::videoSourceRTSP = "RTSP Video Stream"; const char* VideoSettings::videoSourceTCP = "TCP-MPEG2 Video Stream"; VideoSettings::VideoSettings(QObject* parent) - : SettingsGroup(videoSettingsGroupName, QString() /* root settings group */, parent) + : SettingsGroup(name, settingsGroup, parent) , _videoSourceFact(NULL) , _udpPortFact(NULL) , _tcpUrlFact(NULL) diff --git a/src/Settings/VideoSettings.h b/src/Settings/VideoSettings.h index 63b6840d165d5d110b478f0e34608c53f70dec5b..a8933972191192521e988a9444ddd49f50aacdbf 100644 --- a/src/Settings/VideoSettings.h +++ b/src/Settings/VideoSettings.h @@ -49,7 +49,8 @@ public: Fact* disableWhenDisarmed (void); bool streamConfigured (void); - static const char* videoSettingsGroupName; + static const char* name; + static const char* settingsGroup; static const char* videoSourceName; static const char* udpPortName; diff --git a/src/Terrain.cc b/src/Terrain.cc deleted file mode 100644 index cd2480cf9c53a4a0512514f05f9909b191fc37f7..0000000000000000000000000000000000000000 --- a/src/Terrain.cc +++ /dev/null @@ -1,222 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2016 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - -#include "Terrain.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -QGC_LOGGING_CATEGORY(ElevationProviderLog, "ElevationProviderLog") - -Q_GLOBAL_STATIC(TerrainBatchManager, _terrainBatchManager) - -TerrainBatchManager::TerrainBatchManager(void) -{ - _batchTimer.setSingleShot(true); - _batchTimer.setInterval(_batchTimeout); - connect(&_batchTimer, &QTimer::timeout, this, &TerrainBatchManager::_sendNextBatch); -} - -void TerrainBatchManager::addQuery(ElevationProvider* elevationProvider, const QList& coordinates) -{ - if (coordinates.length() > 0) { - qCDebug(ElevationProviderLog) << "addQuery: elevationProvider:coordinates.count" << elevationProvider << coordinates.count(); - connect(elevationProvider, &ElevationProvider::destroyed, this, &TerrainBatchManager::_elevationProviderDestroyed); - QueuedRequestInfo_t queuedRequestInfo = { elevationProvider, coordinates }; - _requestQueue.append(queuedRequestInfo); - if (!_batchTimer.isActive()) { - _batchTimer.start(); - } - } -} - -void TerrainBatchManager::_sendNextBatch(void) -{ - qCDebug(ElevationProviderLog) << "_sendNextBatch _state:_requestQueue.count:_sentRequests.count" << _stateToString(_state) << _requestQueue.count() << _sentRequests.count(); - - if (_state != State::Idle) { - // Waiting for last download the complete, wait some more - _batchTimer.start(); - return; - } - - if (_requestQueue.count() == 0) { - return; - } - - _sentRequests.clear(); - - // Convert coordinates to point strings for json query - QString points; - foreach (const QueuedRequestInfo_t& requestInfo, _requestQueue) { - SentRequestInfo_t sentRequestInfo = { requestInfo.elevationProvider, false, requestInfo.coordinates.count() }; - qCDebug(ElevationProviderLog) << "Building request: coordinate count" << requestInfo.coordinates.count(); - _sentRequests.append(sentRequestInfo); - - foreach (const QGeoCoordinate& coord, requestInfo.coordinates) { - points += QString::number(coord.latitude(), 'f', 10) + "," - + QString::number(coord.longitude(), 'f', 10) + ","; - } - - } - points = points.mid(0, points.length() - 1); // remove the last ',' from string - _requestQueue.clear(); - - QUrlQuery query; - query.addQueryItem(QStringLiteral("points"), points); - QUrl url(QStringLiteral("https://api.airmap.com/elevation/stage/srtm1/ele")); - url.setQuery(query); - - QNetworkRequest request(url); - - QNetworkProxy tProxy; - tProxy.setType(QNetworkProxy::DefaultProxy); - _networkManager.setProxy(tProxy); - - QNetworkReply* networkReply = _networkManager.get(request); - if (!networkReply) { - _batchFailed(); - return; - } - - connect(networkReply, &QNetworkReply::finished, this, &TerrainBatchManager::_requestFinished); - - _state = State::Downloading; -} - -void TerrainBatchManager::_batchFailed(void) -{ - QList noAltitudes; - - foreach (const SentRequestInfo_t& sentRequestInfo, _sentRequests) { - if (!sentRequestInfo.providerDestroyed) { - disconnect(sentRequestInfo.elevationProvider, &ElevationProvider::destroyed, this, &TerrainBatchManager::_elevationProviderDestroyed); - sentRequestInfo.elevationProvider->_signalTerrainData(false, noAltitudes); - } - } - _sentRequests.clear(); -} - -void TerrainBatchManager::_requestFinished() -{ - QNetworkReply* reply = qobject_cast(QObject::sender()); - - _state = State::Idle; - - // When an error occurs we still end up here - if (reply->error() != QNetworkReply::NoError) { - qCDebug(ElevationProviderLog) << "_requestFinished error:" << reply->error(); - _batchFailed(); - reply->deleteLater(); - return; - } - - QByteArray responseBytes = reply->readAll(); - - QJsonParseError parseError; - QJsonDocument responseJson = QJsonDocument::fromJson(responseBytes, &parseError); - if (parseError.error != QJsonParseError::NoError) { - qCDebug(ElevationProviderLog) << "_requestFinished unable to parse json:" << parseError.errorString(); - _batchFailed(); - reply->deleteLater(); - return; - } - - QJsonObject rootObject = responseJson.object(); - QString status = rootObject["status"].toString(); - if (status != "success") { - qCDebug(ElevationProviderLog) << "_requestFinished status != success:" << status; - _batchFailed(); - reply->deleteLater(); - return; - } - - QList altitudes; - const QJsonArray& dataArray = rootObject["data"].toArray(); - for (int i = 0; i < dataArray.count(); i++) { - altitudes.push_back(dataArray[i].toDouble()); - } - - int currentIndex = 0; - foreach (const SentRequestInfo_t& sentRequestInfo, _sentRequests) { - if (!sentRequestInfo.providerDestroyed) { - disconnect(sentRequestInfo.elevationProvider, &ElevationProvider::destroyed, this, &TerrainBatchManager::_elevationProviderDestroyed); - QList requestAltitudes = altitudes.mid(currentIndex, sentRequestInfo.cCoord); - sentRequestInfo.elevationProvider->_signalTerrainData(true, requestAltitudes); - currentIndex += sentRequestInfo.cCoord; - } - } - _sentRequests.clear(); - - reply->deleteLater(); -} - -void TerrainBatchManager::_elevationProviderDestroyed(QObject* elevationProvider) -{ - // Remove/Mark deleted objects queries from queues - - qCDebug(ElevationProviderLog) << "_elevationProviderDestroyed elevationProvider" << elevationProvider; - - int i = 0; - while (i < _requestQueue.count()) { - const QueuedRequestInfo_t& requestInfo = _requestQueue[i]; - if (requestInfo.elevationProvider == elevationProvider) { - qCDebug(ElevationProviderLog) << "Removing deleted provider from _requestQueue index:elevationProvider" << i << requestInfo.elevationProvider; - _requestQueue.removeAt(i); - } else { - i++; - } - } - - for (int i=0; i<_sentRequests.count(); i++) { - SentRequestInfo_t& sentRequestInfo = _sentRequests[i]; - if (sentRequestInfo.elevationProvider == elevationProvider) { - qCDebug(ElevationProviderLog) << "Zombieing deleted provider from _sentRequests index:elevatationProvider" << sentRequestInfo.elevationProvider; - sentRequestInfo.providerDestroyed = true; - } - } -} - -QString TerrainBatchManager::_stateToString(State state) -{ - switch (state) { - case State::Idle: - return QStringLiteral("Idle"); - case State::Downloading: - return QStringLiteral("Downloading"); - } - - return QStringLiteral("State unknown"); -} - -ElevationProvider::ElevationProvider(QObject* parent) - : QObject(parent) -{ - -} -void ElevationProvider::queryTerrainData(const QList& coordinates) -{ - if (coordinates.length() == 0) { - return; - } - - _terrainBatchManager->addQuery(this, coordinates); -} - -void ElevationProvider::_signalTerrainData(bool success, QList& altitudes) -{ - emit terrainData(success, altitudes); -} diff --git a/src/Terrain.h b/src/Terrain.h deleted file mode 100644 index 92f3f330a98574e3debf9661a1492d0958220e28..0000000000000000000000000000000000000000 --- a/src/Terrain.h +++ /dev/null @@ -1,83 +0,0 @@ -/**************************************************************************** - * - * (c) 2017 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - -#pragma once - -#include "QGCLoggingCategory.h" - -#include -#include -#include -#include - -Q_DECLARE_LOGGING_CATEGORY(ElevationProviderLog) - -class ElevationProvider; - -/// Used internally by ElevationProvider to batch requests together -class TerrainBatchManager : public QObject { - Q_OBJECT - -public: - TerrainBatchManager(void); - - void addQuery(ElevationProvider* elevationProvider, const QList& coordinates); - -private slots: - void _sendNextBatch (void); - void _requestFinished (void); - void _elevationProviderDestroyed (QObject* elevationProvider); - -private: - typedef struct { - ElevationProvider* elevationProvider; - QList coordinates; - } QueuedRequestInfo_t; - - typedef struct { - ElevationProvider* elevationProvider; - bool providerDestroyed; - int cCoord; - } SentRequestInfo_t; - - - enum class State { - Idle, - Downloading, - }; - - void _batchFailed(void); - QString _stateToString(State state); - - QList _requestQueue; - QList _sentRequests; - State _state = State::Idle; - QNetworkAccessManager _networkManager; - const int _batchTimeout = 500; - QTimer _batchTimer; -}; - -/// NOTE: ElevationProvider is not thread safe. All instances/calls to ElevationProvider must be on main thread. -class ElevationProvider : public QObject -{ - Q_OBJECT -public: - ElevationProvider(QObject* parent = NULL); - - /// Async elevation query for a list of lon,lat coordinates. When the query is done, the terrainData() signal - /// is emitted. - /// @param coordinates to query - void queryTerrainData(const QList& coordinates); - - // Internal method - void _signalTerrainData(bool success, QList& altitudes); - -signals: - void terrainData(bool success, QList altitudes); -}; diff --git a/src/Terrain/TerrainQuery.cc b/src/Terrain/TerrainQuery.cc new file mode 100644 index 0000000000000000000000000000000000000000..ec9c96c350d906b4ff500b68ba3ec78f4272f5e5 --- /dev/null +++ b/src/Terrain/TerrainQuery.cc @@ -0,0 +1,706 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "TerrainQuery.h" +#include "QGCMapEngine.h" +#include "QGeoMapReplyQGC.h" +#include "QGCApplication.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +QGC_LOGGING_CATEGORY(TerrainQueryLog, "TerrainQueryLog") +QGC_LOGGING_CATEGORY(TerrainQueryVerboseLog, "TerrainQueryVerboseLog") + +Q_GLOBAL_STATIC(TerrainAtCoordinateBatchManager, _TerrainAtCoordinateBatchManager) +Q_GLOBAL_STATIC(TerrainTileManager, _terrainTileManager) + +TerrainAirMapQuery::TerrainAirMapQuery(QObject* parent) + : TerrainQueryInterface(parent) +{ + +} + +void TerrainAirMapQuery::requestCoordinateHeights(const QList& coordinates) +{ + if (qgcApp()->runningUnitTests()) { + emit coordinateHeights(false, QList()); + return; + } + + QString points; + foreach (const QGeoCoordinate& coord, coordinates) { + points += QString::number(coord.latitude(), 'f', 10) + "," + + QString::number(coord.longitude(), 'f', 10) + ","; + } + points = points.mid(0, points.length() - 1); // remove the last ',' from string + + QUrlQuery query; + query.addQueryItem(QStringLiteral("points"), points); + + _queryMode = QueryModeCoordinates; + _sendQuery(QString() /* path */, query); +} + +void TerrainAirMapQuery::requestPathHeights(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord) +{ + if (qgcApp()->runningUnitTests()) { + emit pathHeights(false, qQNaN(), qQNaN(), QList()); + return; + } + + QString points; + points += QString::number(fromCoord.latitude(), 'f', 10) + "," + + QString::number(fromCoord.longitude(), 'f', 10) + ","; + points += QString::number(toCoord.latitude(), 'f', 10) + "," + + QString::number(toCoord.longitude(), 'f', 10); + + QUrlQuery query; + query.addQueryItem(QStringLiteral("points"), points); + + _queryMode = QueryModePath; + _sendQuery(QStringLiteral("/path"), query); +} + +void TerrainAirMapQuery::requestCarpetHeights(const QGeoCoordinate& swCoord, const QGeoCoordinate& neCoord, bool statsOnly) +{ + if (qgcApp()->runningUnitTests()) { + emit carpetHeights(false, qQNaN(), qQNaN(), QList>()); + return; + } + + QString points; + points += QString::number(swCoord.latitude(), 'f', 10) + "," + + QString::number(swCoord.longitude(), 'f', 10) + ","; + points += QString::number(neCoord.latitude(), 'f', 10) + "," + + QString::number(neCoord.longitude(), 'f', 10); + + QUrlQuery query; + query.addQueryItem(QStringLiteral("points"), points); + + _queryMode = QueryModeCarpet; + _carpetStatsOnly = statsOnly; + + _sendQuery(QStringLiteral("/carpet"), query); +} + +void TerrainAirMapQuery::_sendQuery(const QString& path, const QUrlQuery& urlQuery) +{ + QUrl url(QStringLiteral("https://api.airmap.com/elevation/v1/ele") + path); + qCDebug(TerrainQueryLog) << "_sendQuery" << url; + url.setQuery(urlQuery); + + QNetworkRequest request(url); + + QNetworkProxy tProxy; + tProxy.setType(QNetworkProxy::DefaultProxy); + _networkManager.setProxy(tProxy); + + QNetworkReply* networkReply = _networkManager.get(request); + if (!networkReply) { + qCDebug(TerrainQueryLog) << "QNetworkManager::Get did not return QNetworkReply"; + _requestFailed(); + return; + } + + connect(networkReply, &QNetworkReply::finished, this, &TerrainAirMapQuery::_requestFinished); + connect(networkReply, QOverload::of(&QNetworkReply::error), this, &TerrainAirMapQuery::_requestError); +} + +void TerrainAirMapQuery::_requestError(QNetworkReply::NetworkError code) +{ + QNetworkReply* reply = qobject_cast(QObject::sender()); + + if (code != QNetworkReply::NoError) { + qCDebug(TerrainQueryLog) << "_requestError error:url:data" << reply->error() << reply->url() << reply->readAll(); + return; + } +} + +void TerrainAirMapQuery::_requestFinished(void) +{ + QNetworkReply* reply = qobject_cast(QObject::sender()); + + if (reply->error() != QNetworkReply::NoError) { + qCDebug(TerrainQueryLog) << "_requestFinished error:url:data" << reply->error() << reply->url() << reply->readAll(); + reply->deleteLater(); + _requestFailed(); + return; + } + + QByteArray responseBytes = reply->readAll(); + reply->deleteLater(); + + // Convert the response to Json + QJsonParseError parseError; + QJsonDocument responseJson = QJsonDocument::fromJson(responseBytes, &parseError); + if (parseError.error != QJsonParseError::NoError) { + qCDebug(TerrainQueryLog) << "_requestFinished unable to parse json:" << parseError.errorString(); + _requestFailed(); + return; + } + + // Check airmap reponse status + QJsonObject rootObject = responseJson.object(); + QString status = rootObject["status"].toString(); + if (status != "success") { + qCDebug(TerrainQueryLog) << "_requestFinished status != success:" << status; + _requestFailed(); + return; + } + + // Send back data + const QJsonValue& jsonData = rootObject["data"]; + qCDebug(TerrainQueryLog) << "_requestFinished success"; + switch (_queryMode) { + case QueryModeCoordinates: + emit _parseCoordinateData(jsonData); + break; + case QueryModePath: + emit _parsePathData(jsonData); + break; + case QueryModeCarpet: + emit _parseCarpetData(jsonData); + break; + } +} + +void TerrainAirMapQuery::_requestFailed(void) +{ + switch (_queryMode) { + case QueryModeCoordinates: + emit coordinateHeights(false /* success */, QList() /* heights */); + break; + case QueryModePath: + emit pathHeights(false /* success */, qQNaN() /* latStep */, qQNaN() /* lonStep */, QList() /* heights */); + break; + case QueryModeCarpet: + emit carpetHeights(false /* success */, qQNaN() /* minHeight */, qQNaN() /* maxHeight */, QList>() /* carpet */); + break; + } +} + +void TerrainAirMapQuery::_parseCoordinateData(const QJsonValue& coordinateJson) +{ + QList heights; + const QJsonArray& dataArray = coordinateJson.toArray(); + for (int i = 0; i < dataArray.count(); i++) { + heights.append(dataArray[i].toDouble()); + } + + emit coordinateHeights(true /* success */, heights); +} + +void TerrainAirMapQuery::_parsePathData(const QJsonValue& pathJson) +{ + QJsonObject jsonObject = pathJson.toArray()[0].toObject(); + QJsonArray stepArray = jsonObject["step"].toArray(); + QJsonArray profileArray = jsonObject["profile"].toArray(); + + double latStep = stepArray[0].toDouble(); + double lonStep = stepArray[1].toDouble(); + + QList heights; + foreach (const QJsonValue& profileValue, profileArray) { + heights.append(profileValue.toDouble()); + } + + emit pathHeights(true /* success */, latStep, lonStep, heights); +} + +void TerrainAirMapQuery::_parseCarpetData(const QJsonValue& carpetJson) +{ + QJsonObject jsonObject = carpetJson.toArray()[0].toObject(); + + QJsonObject statsObject = jsonObject["stats"].toObject(); + double minHeight = statsObject["min"].toDouble(); + double maxHeight = statsObject["min"].toDouble(); + + QList> carpet; + if (!_carpetStatsOnly) { + QJsonArray carpetArray = jsonObject["carpet"].toArray(); + + for (int i=0; i()); + + for (int j=0; j& coordinates) +{ + if (qgcApp()->runningUnitTests()) { + emit coordinateHeights(false, QList()); + return; + } + + if (coordinates.length() == 0) { + return; + } + + _terrainTileManager->addCoordinateQuery(this, coordinates); +} + +void TerrainOfflineAirMapQuery::requestPathHeights(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord) +{ + if (qgcApp()->runningUnitTests()) { + emit pathHeights(false, qQNaN(), qQNaN(), QList()); + return; + } + + _terrainTileManager->addPathQuery(this, fromCoord, toCoord); +} + +void TerrainOfflineAirMapQuery::requestCarpetHeights(const QGeoCoordinate& swCoord, const QGeoCoordinate& neCoord, bool statsOnly) +{ + if (qgcApp()->runningUnitTests()) { + emit carpetHeights(false, qQNaN(), qQNaN(), QList>()); + return; + } + + // TODO + Q_UNUSED(swCoord); + Q_UNUSED(neCoord); + Q_UNUSED(statsOnly); + qWarning() << "Carpet queries are currently not supported from offline air map data"; +} + +void TerrainOfflineAirMapQuery::_signalCoordinateHeights(bool success, QList heights) +{ + emit coordinateHeights(success, heights); +} + +void TerrainOfflineAirMapQuery::_signalPathHeights(bool success, double latStep, double lonStep, const QList& heights) +{ + emit pathHeights(success, latStep, lonStep, heights); +} + +void TerrainOfflineAirMapQuery::_signalCarpetHeights(bool success, double minHeight, double maxHeight, const QList>& carpet) +{ + emit carpetHeights(success, minHeight, maxHeight, carpet); +} + +TerrainTileManager::TerrainTileManager(void) +{ + +} + +void TerrainTileManager::addCoordinateQuery(TerrainOfflineAirMapQuery* terrainQueryInterface, const QList& coordinates) +{ + if (coordinates.length() > 0) { + QList altitudes; + + if (!_getAltitudesForCoordinates(coordinates, altitudes)) { + QueuedRequestInfo_t queuedRequestInfo = { terrainQueryInterface, QueryMode::QueryModeCoordinates, coordinates }; + _requestQueue.append(queuedRequestInfo); + return; + } + + qCDebug(TerrainQueryLog) << "All altitudes taken from cached data"; + terrainQueryInterface->_signalCoordinateHeights(coordinates.count() == altitudes.count(), altitudes); + } +} + +void TerrainTileManager::addPathQuery(TerrainOfflineAirMapQuery* terrainQueryInterface, const QGeoCoordinate &startPoint, const QGeoCoordinate &endPoint) +{ + QList coordinates; + double lat = startPoint.latitude(); + double lon = startPoint.longitude(); + double latDiff = endPoint.latitude() - lat; + double lonDiff = endPoint.longitude() - lon; + double steps = ceil(endPoint.distanceTo(startPoint) / TerrainTile::terrainAltitudeSpacing); + for (double i = 0.0; i <= steps; i = i + 1) { + coordinates.append(QGeoCoordinate(lat + latDiff * i / steps, lon + lonDiff * i / steps)); + } + + QList altitudes; + if (!_getAltitudesForCoordinates(coordinates, altitudes)) { + QueuedRequestInfo_t queuedRequestInfo = { terrainQueryInterface, QueryMode::QueryModePath, coordinates }; + _requestQueue.append(queuedRequestInfo); + return; + } + + qCDebug(TerrainQueryLog) << "All altitudes taken from cached data"; + double stepLat = 0; + double stepLon = 0; + if (coordinates.count() > 1) { + stepLat = coordinates[1].latitude() - coordinates[0].latitude(); + stepLon = coordinates[1].longitude() - coordinates[0].longitude(); + } + terrainQueryInterface->_signalPathHeights(coordinates.count() == altitudes.count(), stepLat, stepLon, altitudes); +} + +bool TerrainTileManager::_getAltitudesForCoordinates(const QList& coordinates, QList& altitudes) +{ + foreach (const QGeoCoordinate& coordinate, coordinates) { + QString tileHash = _getTileHash(coordinate); + _tilesMutex.lock(); + if (!_tiles.contains(tileHash)) { + qCDebug(TerrainQueryLog) << "Need to download tile " << tileHash; + + // Schedule the fetch task + if (_state != State::Downloading) { + QNetworkRequest request = getQGCMapEngine()->urlFactory()->getTileURL(UrlFactory::AirmapElevation, QGCMapEngine::long2elevationTileX(coordinate.longitude(), 1), QGCMapEngine::lat2elevationTileY(coordinate.latitude(), 1), 1, &_networkManager); + QGeoTileSpec spec; + spec.setX(QGCMapEngine::long2elevationTileX(coordinate.longitude(), 1)); + spec.setY(QGCMapEngine::lat2elevationTileY(coordinate.latitude(), 1)); + spec.setZoom(1); + spec.setMapId(UrlFactory::AirmapElevation); + QGeoTiledMapReplyQGC* reply = new QGeoTiledMapReplyQGC(&_networkManager, request, spec); + connect(reply, &QGeoTiledMapReplyQGC::terrainDone, this, &TerrainTileManager::_terrainDone); + _state = State::Downloading; + } + _tilesMutex.unlock(); + + return false; + } else { + if (_tiles[tileHash].isIn(coordinate)) { + altitudes.push_back(_tiles[tileHash].elevation(coordinate)); + } else { + qCDebug(TerrainQueryLog) << "Error: coordinate not in tile region"; + altitudes.push_back(-1.0); + } + } + _tilesMutex.unlock(); + } + return true; +} + +void TerrainTileManager::_tileFailed(void) +{ + QList noAltitudes; + + foreach (const QueuedRequestInfo_t& requestInfo, _requestQueue) { + if (requestInfo.queryMode == QueryMode::QueryModeCoordinates) { + requestInfo.terrainQueryInterface->_signalCoordinateHeights(false, noAltitudes); + } + } + _requestQueue.clear(); +} + +void TerrainTileManager::_terrainDone(QByteArray responseBytes, QNetworkReply::NetworkError error) +{ + QGeoTiledMapReplyQGC* reply = qobject_cast(QObject::sender()); + _state = State::Idle; + + if (!reply) { + qCDebug(TerrainQueryLog) << "Elevation tile fetched but invalid reply data type."; + return; + } + + // remove from download queue + QGeoTileSpec spec = reply->tileSpec(); + QString hash = QGCMapEngine::getTileHash(UrlFactory::AirmapElevation, spec.x(), spec.y(), spec.zoom()); + + // handle potential errors + if (error != QNetworkReply::NoError) { + qCDebug(TerrainQueryLog) << "Elevation tile fetching returned error (" << error << ")"; + _tileFailed(); + reply->deleteLater(); + return; + } + if (responseBytes.isEmpty()) { + qCDebug(TerrainQueryLog) << "Error in fetching elevation tile. Empty response."; + _tileFailed(); + reply->deleteLater(); + return; + } + + qWarning() << "Received some bytes of terrain data: " << responseBytes.size(); + + TerrainTile* terrainTile = new TerrainTile(responseBytes); + if (terrainTile->isValid()) { + _tilesMutex.lock(); + if (!_tiles.contains(hash)) { + _tiles.insert(hash, *terrainTile); + } else { + delete terrainTile; + } + _tilesMutex.unlock(); + } else { + qCDebug(TerrainQueryLog) << "Received invalid tile"; + } + reply->deleteLater(); + + // now try to query the data again + for (int i = _requestQueue.count() - 1; i >= 0; i--) { + QList altitudes; + if (_getAltitudesForCoordinates(_requestQueue[i].coordinates, altitudes)) { + if (_requestQueue[i].queryMode == QueryMode::QueryModeCoordinates) { + _requestQueue[i].terrainQueryInterface->_signalCoordinateHeights(_requestQueue[i].coordinates.count() == altitudes.count(), altitudes); + } + _requestQueue.removeAt(i); + } + } +} + +QString TerrainTileManager::_getTileHash(const QGeoCoordinate& coordinate) +{ + QString ret = QGCMapEngine::getTileHash(UrlFactory::AirmapElevation, QGCMapEngine::long2elevationTileX(coordinate.longitude(), 1), QGCMapEngine::lat2elevationTileY(coordinate.latitude(), 1), 1); + qCDebug(TerrainQueryVerboseLog) << "Computing unique tile hash for " << coordinate << ret; + + return ret; +} + +TerrainAtCoordinateBatchManager::TerrainAtCoordinateBatchManager(void) +{ + _batchTimer.setSingleShot(true); + _batchTimer.setInterval(_batchTimeout); + connect(&_batchTimer, &QTimer::timeout, this, &TerrainAtCoordinateBatchManager::_sendNextBatch); + connect(&_terrainQuery, &TerrainQueryInterface::coordinateHeights, this, &TerrainAtCoordinateBatchManager::_coordinateHeights); +} + +void TerrainAtCoordinateBatchManager::addQuery(TerrainAtCoordinateQuery* terrainAtCoordinateQuery, const QList& coordinates) +{ + if (coordinates.length() > 0) { + connect(terrainAtCoordinateQuery, &TerrainAtCoordinateQuery::destroyed, this, &TerrainAtCoordinateBatchManager::_queryObjectDestroyed); + QueuedRequestInfo_t queuedRequestInfo = { terrainAtCoordinateQuery, coordinates }; + _requestQueue.append(queuedRequestInfo); + if (!_batchTimer.isActive()) { + _batchTimer.start(); + } + } +} + +void TerrainAtCoordinateBatchManager::_sendNextBatch(void) +{ + qCDebug(TerrainQueryLog) << "_sendNextBatch _state:_requestQueue.count:_sentRequests.count" << _stateToString(_state) << _requestQueue.count() << _sentRequests.count(); + + if (_state != State::Idle) { + // Waiting for last download the complete, wait some more + qCDebug(TerrainQueryLog) << "_sendNextBatch restarting timer"; + _batchTimer.start(); + return; + } + + if (_requestQueue.count() == 0) { + return; + } + + _sentRequests.clear(); + + // Convert coordinates to point strings for json query + QList coords; + int requestQueueAdded = 0; + foreach (const QueuedRequestInfo_t& requestInfo, _requestQueue) { + SentRequestInfo_t sentRequestInfo = { requestInfo.terrainAtCoordinateQuery, false, requestInfo.coordinates.count() }; + _sentRequests.append(sentRequestInfo); + coords += requestInfo.coordinates; + requestQueueAdded++; + if (coords.count() > 50) { + break; + } + } + _requestQueue = _requestQueue.mid(requestQueueAdded); + qCDebug(TerrainQueryLog) << "TerrainAtCoordinateBatchManager::_sendNextBatch - batch count:request queue count" << coords.count() << _requestQueue.count(); + + _state = State::Downloading; + _terrainQuery.requestCoordinateHeights(coords); +} + +void TerrainAtCoordinateBatchManager::_batchFailed(void) +{ + QList noHeights; + + foreach (const SentRequestInfo_t& sentRequestInfo, _sentRequests) { + if (!sentRequestInfo.queryObjectDestroyed) { + disconnect(sentRequestInfo.terrainAtCoordinateQuery, &TerrainAtCoordinateQuery::destroyed, this, &TerrainAtCoordinateBatchManager::_queryObjectDestroyed); + sentRequestInfo.terrainAtCoordinateQuery->_signalTerrainData(false, noHeights); + } + } + _sentRequests.clear(); +} + +void TerrainAtCoordinateBatchManager::_queryObjectDestroyed(QObject* terrainAtCoordinateQuery) +{ + // Remove/Mark deleted objects queries from queues + + qCDebug(TerrainQueryLog) << "_TerrainAtCoordinateQueryDestroyed TerrainAtCoordinateQuery" << terrainAtCoordinateQuery; + + int i = 0; + while (i < _requestQueue.count()) { + const QueuedRequestInfo_t& requestInfo = _requestQueue[i]; + if (requestInfo.terrainAtCoordinateQuery == terrainAtCoordinateQuery) { + qCDebug(TerrainQueryLog) << "Removing deleted provider from _requestQueue index:terrainAtCoordinateQuery" << i << requestInfo.terrainAtCoordinateQuery; + _requestQueue.removeAt(i); + } else { + i++; + } + } + + for (int i=0; i<_sentRequests.count(); i++) { + SentRequestInfo_t& sentRequestInfo = _sentRequests[i]; + if (sentRequestInfo.terrainAtCoordinateQuery == terrainAtCoordinateQuery) { + qCDebug(TerrainQueryLog) << "Zombieing deleted provider from _sentRequests index:terrainAtCoordinateQuery" << sentRequestInfo.terrainAtCoordinateQuery; + sentRequestInfo.queryObjectDestroyed = true; + } + } +} + +QString TerrainAtCoordinateBatchManager::_stateToString(State state) +{ + switch (state) { + case State::Idle: + return QStringLiteral("Idle"); + case State::Downloading: + return QStringLiteral("Downloading"); + } + + return QStringLiteral("State unknown"); +} + +void TerrainAtCoordinateBatchManager::_coordinateHeights(bool success, QList heights) +{ + _state = State::Idle; + + qCDebug(TerrainQueryLog) << "_coordinateHeights success:count" << success << heights.count(); + + if (!success) { + _batchFailed(); + return; + } + + int currentIndex = 0; + foreach (const SentRequestInfo_t& sentRequestInfo, _sentRequests) { + if (!sentRequestInfo.queryObjectDestroyed) { + qCDebug(TerrainQueryVerboseLog) << "TerrainAtCoordinateBatchManager::_coordinateHeights returned TerrainCoordinateQuery:count" << sentRequestInfo.terrainAtCoordinateQuery << sentRequestInfo.cCoord; + disconnect(sentRequestInfo.terrainAtCoordinateQuery, &TerrainAtCoordinateQuery::destroyed, this, &TerrainAtCoordinateBatchManager::_queryObjectDestroyed); + QList requestAltitudes = heights.mid(currentIndex, sentRequestInfo.cCoord); + sentRequestInfo.terrainAtCoordinateQuery->_signalTerrainData(true, requestAltitudes); + currentIndex += sentRequestInfo.cCoord; + } + } + _sentRequests.clear(); + + if (_requestQueue.count()) { + _batchTimer.start(); + } +} + +TerrainAtCoordinateQuery::TerrainAtCoordinateQuery(QObject* parent) + : QObject(parent) +{ + +} +void TerrainAtCoordinateQuery::requestData(const QList& coordinates) +{ + if (coordinates.length() == 0) { + return; + } + + _TerrainAtCoordinateBatchManager->addQuery(this, coordinates); +} + +void TerrainAtCoordinateQuery::_signalTerrainData(bool success, QList& heights) +{ + emit terrainData(success, heights); +} + +TerrainPathQuery::TerrainPathQuery(QObject* parent) + : QObject(parent) +{ + qRegisterMetaType(); + connect(&_terrainQuery, &TerrainQueryInterface::pathHeights, this, &TerrainPathQuery::_pathHeights); +} + +void TerrainPathQuery::requestData(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord) +{ + _terrainQuery.requestPathHeights(fromCoord, toCoord); +} + +void TerrainPathQuery::_pathHeights(bool success, double latStep, double lonStep, const QList& heights) +{ + PathHeightInfo_t pathHeightInfo; + pathHeightInfo.latStep = latStep; + pathHeightInfo.lonStep = lonStep; + pathHeightInfo.heights = heights; + emit terrainData(success, pathHeightInfo); +} + +TerrainPolyPathQuery::TerrainPolyPathQuery(QObject* parent) + : QObject (parent) + , _curIndex (0) +{ + connect(&_pathQuery, &TerrainPathQuery::terrainData, this, &TerrainPolyPathQuery::_terrainDataReceived); +} + +void TerrainPolyPathQuery::requestData(const QVariantList& polyPath) +{ + QList path; + + foreach (const QVariant& geoVar, polyPath) { + path.append(geoVar.value()); + } + + requestData(path); +} + +void TerrainPolyPathQuery::requestData(const QList& polyPath) +{ + qCDebug(TerrainQueryLog) << "TerrainPolyPathQuery::requestData count" << polyPath.count(); + + // Kick off first request + _rgCoords = polyPath; + _curIndex = 0; + _pathQuery.requestData(_rgCoords[0], _rgCoords[1]); +} + +void TerrainPolyPathQuery::_terrainDataReceived(bool success, const TerrainPathQuery::PathHeightInfo_t& pathHeightInfo) +{ + qCDebug(TerrainQueryLog) << "TerrainPolyPathQuery::_terrainDataReceived success:_curIndex" << success << _curIndex; + + if (!success) { + _rgPathHeightInfo.clear(); + emit terrainData(false /* success */, _rgPathHeightInfo); + return; + } + + _rgPathHeightInfo.append(pathHeightInfo); + + if (++_curIndex >= _rgCoords.count() - 1) { + // We've finished all requests + qCDebug(TerrainQueryLog) << "TerrainPolyPathQuery::_terrainDataReceived complete"; + emit terrainData(true /* success */, _rgPathHeightInfo); + } else { + _pathQuery.requestData(_rgCoords[_curIndex], _rgCoords[_curIndex+1]); + } +} + +TerrainCarpetQuery::TerrainCarpetQuery(QObject* parent) + : QObject(parent) +{ + connect(&_terrainQuery, &TerrainQueryInterface::carpetHeights, this, &TerrainCarpetQuery::terrainData); +} + +void TerrainCarpetQuery::requestData(const QGeoCoordinate& swCoord, const QGeoCoordinate& neCoord, bool statsOnly) +{ + _terrainQuery.requestCarpetHeights(swCoord, neCoord, statsOnly); +} diff --git a/src/Terrain/TerrainQuery.h b/src/Terrain/TerrainQuery.h new file mode 100644 index 0000000000000000000000000000000000000000..f41c9be0bc66c5a19d4bc70e512e915c4fa73434 --- /dev/null +++ b/src/Terrain/TerrainQuery.h @@ -0,0 +1,295 @@ +/**************************************************************************** + * + * (c) 2017 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "TerrainTile.h" +#include "QGCMapEngineData.h" +#include "QGCLoggingCategory.h" + +#include +#include +#include +#include +#include +#include + +Q_DECLARE_LOGGING_CATEGORY(TerrainQueryLog) +Q_DECLARE_LOGGING_CATEGORY(TerrainQueryVerboseLog) + +class TerrainAtCoordinateQuery; + +/// Base class for offline/online terrain queries +class TerrainQueryInterface : public QObject +{ + Q_OBJECT + +public: + TerrainQueryInterface(QObject* parent) : QObject(parent) { } + + /// Request terrain heights for specified coodinates. + /// Signals: coordinateHeights when data is available + virtual void requestCoordinateHeights(const QList& coordinates) = 0; + + /// Requests terrain heights along the path specified by the two coordinates. + /// Signals: pathHeights + /// @param coordinates to query + virtual void requestPathHeights(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord) = 0; + + /// Request terrain heights for the rectangular area specified. + /// Signals: carpetHeights when data is available + /// @param swCoord South-West bound of rectangular area to query + /// @param neCoord North-East bound of rectangular area to query + /// @param statsOnly true: Return only stats, no carpet data + virtual void requestCarpetHeights(const QGeoCoordinate& swCoord, const QGeoCoordinate& neCoord, bool statsOnly) = 0; + +signals: + void coordinateHeights(bool success, QList heights); + void pathHeights(bool success, double latStep, double lonStep, const QList& heights); + void carpetHeights(bool success, double minHeight, double maxHeight, const QList>& carpet); +}; + +/// AirMap online implementation of terrain queries +class TerrainAirMapQuery : public TerrainQueryInterface { + Q_OBJECT + +public: + TerrainAirMapQuery(QObject* parent = NULL); + + // Overrides from TerrainQueryInterface + void requestCoordinateHeights (const QList& coordinates) final; + void requestPathHeights (const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord) final; + void requestCarpetHeights (const QGeoCoordinate& swCoord, const QGeoCoordinate& neCoord, bool statsOnly) final; + +private slots: + void _requestError (QNetworkReply::NetworkError code); + void _requestFinished (); + +private: + void _sendQuery (const QString& path, const QUrlQuery& urlQuery); + void _requestFailed (void); + void _parseCoordinateData (const QJsonValue& coordinateJson); + void _parsePathData (const QJsonValue& pathJson); + void _parseCarpetData (const QJsonValue& carpetJson); + + enum QueryMode { + QueryModeCoordinates, + QueryModePath, + QueryModeCarpet + }; + + QNetworkAccessManager _networkManager; + QueryMode _queryMode; + bool _carpetStatsOnly; +}; + +/// AirMap offline cachable implementation of terrain queries +class TerrainOfflineAirMapQuery : public TerrainQueryInterface { + Q_OBJECT + +public: + TerrainOfflineAirMapQuery(QObject* parent = NULL); + + // Overrides from TerrainQueryInterface + void requestCoordinateHeights(const QList& coordinates) final; + void requestPathHeights(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord) final; + void requestCarpetHeights(const QGeoCoordinate& swCoord, const QGeoCoordinate& neCoord, bool statsOnly) final; + + // Internal methods + void _signalCoordinateHeights(bool success, QList heights); + void _signalPathHeights(bool success, double latStep, double lonStep, const QList& heights); + void _signalCarpetHeights(bool success, double minHeight, double maxHeight, const QList>& carpet); +}; + +/// Used internally by TerrainOfflineAirMapQuery to manage terrain tiles +class TerrainTileManager : public QObject { + Q_OBJECT + +public: + TerrainTileManager(void); + + void addCoordinateQuery (TerrainOfflineAirMapQuery* terrainQueryInterface, const QList& coordinates); + void addPathQuery (TerrainOfflineAirMapQuery* terrainQueryInterface, const QGeoCoordinate& startPoint, const QGeoCoordinate& endPoint); + +private slots: + void _terrainDone (QByteArray responseBytes, QNetworkReply::NetworkError error); + +private: + enum class State { + Idle, + Downloading, + }; + + enum QueryMode { + QueryModeCoordinates, + QueryModePath, + QueryModeCarpet + }; + + typedef struct { + TerrainOfflineAirMapQuery* terrainQueryInterface; + QueryMode queryMode; + QList coordinates; + } QueuedRequestInfo_t; + + void _tileFailed(void); + bool _getAltitudesForCoordinates(const QList& coordinates, QList& altitudes); + QString _getTileHash(const QGeoCoordinate& coordinate); /// Method to create a unique string for each tile + + QList _requestQueue; + State _state = State::Idle; + QNetworkAccessManager _networkManager; + + QMutex _tilesMutex; + QHash _tiles; +}; + +/// Used internally by TerrainAtCoordinateQuery to batch coordinate requests together +class TerrainAtCoordinateBatchManager : public QObject { + Q_OBJECT + +public: + TerrainAtCoordinateBatchManager(void); + + void addQuery(TerrainAtCoordinateQuery* terrainAtCoordinateQuery, const QList& coordinates); + +private slots: + void _sendNextBatch (void); + void _queryObjectDestroyed (QObject* elevationProvider); + void _coordinateHeights (bool success, QList heights); + +private: + typedef struct { + TerrainAtCoordinateQuery* terrainAtCoordinateQuery; + QList coordinates; + } QueuedRequestInfo_t; + + typedef struct { + TerrainAtCoordinateQuery* terrainAtCoordinateQuery; + bool queryObjectDestroyed; + int cCoord; + } SentRequestInfo_t; + + + enum class State { + Idle, + Downloading, + }; + + void _batchFailed(void); + QString _stateToString(State state); + + QList _requestQueue; + QList _sentRequests; + State _state = State::Idle; + const int _batchTimeout = 500; + QTimer _batchTimer; + TerrainOfflineAirMapQuery _terrainQuery; +}; + +/// NOTE: TerrainAtCoordinateQuery is not thread safe. All instances/calls to ElevationProvider must be on main thread. +class TerrainAtCoordinateQuery : public QObject +{ + Q_OBJECT +public: + TerrainAtCoordinateQuery(QObject* parent = NULL); + + /// Async terrain query for a list of lon,lat coordinates. When the query is done, the terrainData() signal + /// is emitted. + /// @param coordinates to query + void requestData(const QList& coordinates); + + // Internal method + void _signalTerrainData(bool success, QList& heights); + +signals: + void terrainData(bool success, QList heights); +}; + +class TerrainPathQuery : public QObject +{ + Q_OBJECT + +public: + TerrainPathQuery(QObject* parent = NULL); + + /// Async terrain query for terrain heights between two lat/lon coordinates. When the query is done, the terrainData() signal + /// is emitted. + /// @param coordinates to query + void requestData(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord); + + typedef struct { + double latStep; ///< Amount of latitudinal distance between each returned height + double lonStep; ///< Amount of longitudinal distance between each returned height + QList heights; ///< Terrain heights along path + } PathHeightInfo_t; + +signals: + /// Signalled when terrain data comes back from server + void terrainData(bool success, const PathHeightInfo_t& pathHeightInfo); + +private slots: + void _pathHeights(bool success, double latStep, double lonStep, const QList& heights); + +private: + TerrainOfflineAirMapQuery _terrainQuery; +}; + +Q_DECLARE_METATYPE(TerrainPathQuery::PathHeightInfo_t) + +class TerrainPolyPathQuery : public QObject +{ + Q_OBJECT + +public: + TerrainPolyPathQuery(QObject* parent = NULL); + + /// Async terrain query for terrain heights for the paths between each specified QGeoCoordinate. + /// When the query is done, the terrainData() signal is emitted. + /// @param polyPath List of QGeoCoordinate + void requestData(const QVariantList& polyPath); + void requestData(const QList& polyPath); + +signals: + /// Signalled when terrain data comes back from server + void terrainData(bool success, const QList& rgPathHeightInfo); + +private slots: + void _terrainDataReceived(bool success, const TerrainPathQuery::PathHeightInfo_t& pathHeightInfo); + +private: + int _curIndex; + QList _rgCoords; + QList _rgPathHeightInfo; + TerrainPathQuery _pathQuery; +}; + + +class TerrainCarpetQuery : public QObject +{ + Q_OBJECT + +public: + TerrainCarpetQuery(QObject* parent = NULL); + + /// Async terrain query for terrain information bounded by the specifed corners. + /// When the query is done, the terrainData() signal is emitted. + /// @param swCoord South-West bound of rectangular area to query + /// @param neCoord North-East bound of rectangular area to query + /// @param statsOnly true: Return only stats, no carpet data + void requestData(const QGeoCoordinate& swCoord, const QGeoCoordinate& neCoord, bool statsOnly); + +signals: + /// Signalled when terrain data comes back from server + void terrainData(bool success, double minHeight, double maxHeight, const QList>& carpet); + +private: + TerrainAirMapQuery _terrainQuery; +}; + diff --git a/src/TerrainTile.cc b/src/TerrainTile.cc new file mode 100644 index 0000000000000000000000000000000000000000..9c17ed24a231adff25d8c780acd537d72cf2e715 --- /dev/null +++ b/src/TerrainTile.cc @@ -0,0 +1,298 @@ +#include "TerrainTile.h" +#include "JsonHelper.h" +#include "QGCMapEngine.h" + +#include +#include +#include +#include + +QGC_LOGGING_CATEGORY(TerrainTileLog, "TerrainTileLog") + +const char* TerrainTile::_jsonStatusKey = "status"; +const char* TerrainTile::_jsonDataKey = "data"; +const char* TerrainTile::_jsonBoundsKey = "bounds"; +const char* TerrainTile::_jsonSouthWestKey = "sw"; +const char* TerrainTile::_jsonNorthEastKey = "ne"; +const char* TerrainTile::_jsonStatsKey = "stats"; +const char* TerrainTile::_jsonMaxElevationKey = "max"; +const char* TerrainTile::_jsonMinElevationKey = "min"; +const char* TerrainTile::_jsonAvgElevationKey = "avg"; +const char* TerrainTile::_jsonCarpetKey = "carpet"; + +TerrainTile::TerrainTile() + : _minElevation(-1.0) + , _maxElevation(-1.0) + , _avgElevation(-1.0) + , _data(NULL) + , _gridSizeLat(-1) + , _gridSizeLon(-1) + , _isValid(false) +{ + +} + +TerrainTile::~TerrainTile() +{ + if (_data) { + for (int i = 0; i < _gridSizeLat; i++) { + delete _data[i]; + } + delete _data; + _data = NULL; + } +} + + +TerrainTile::TerrainTile(QByteArray byteArray) + : _minElevation(-1.0) + , _maxElevation(-1.0) + , _avgElevation(-1.0) + , _data(NULL) + , _gridSizeLat(-1) + , _gridSizeLon(-1) + , _isValid(false) +{ + QDataStream stream(byteArray); + + float lat,lon; + if (stream.atEnd()) { + qWarning() << "Terrain tile binary data does not contain all data"; + return; + } + stream >> lat; + if (stream.atEnd()) { + qWarning() << "Terrain tile binary data does not contain all data"; + return; + } + stream >> lon; + _southWest.setLatitude(lat); + _southWest.setLongitude(lon); + + if (stream.atEnd()) { + qWarning() << "Terrain tile binary data does not contain all data"; + return; + } + stream >> lat; + if (stream.atEnd()) { + qWarning() << "Terrain tile binary data does not contain all data"; + return; + } + stream >> lon; + _northEast.setLatitude(lat); + _northEast.setLongitude(lon); + + if (stream.atEnd()) { + qWarning() << "Terrain tile binary data does not contain all data"; + return; + } + stream >> _minElevation; + if (stream.atEnd()) { + qWarning() << "Terrain tile binary data does not contain all data"; + return; + } + stream >> _maxElevation; + if (stream.atEnd()) { + qWarning() << "Terrain tile binary data does not contain all data"; + return; + } + stream >> _avgElevation; + if (stream.atEnd()) { + qWarning() << "Terrain tile binary data does not contain all data"; + return; + } + stream >> _gridSizeLat; + if (stream.atEnd()) { + qWarning() << "Terrain tile binary data does not contain all data"; + return; + } + stream >> _gridSizeLon; + + qCDebug(TerrainTileLog) << "Loading terrain tile: " << _southWest << " - " << _northEast; + qCDebug(TerrainTileLog) << "min:max:avg:sizeLat:sizeLon" << _minElevation << _maxElevation << _avgElevation << _gridSizeLat << _gridSizeLon; + + for (int i = 0; i < _gridSizeLat; i++) { + if (i == 0) { + _data = new int16_t*[_gridSizeLat]; + for (int k = 0; k < _gridSizeLat; k++) { + _data[k] = new int16_t[_gridSizeLon]; + } + } + for (int j = 0; j < _gridSizeLon; j++) { + if (stream.atEnd()) { + qWarning() << "Terrain tile binary data does not contain all data"; + return; + } + stream >> _data[i][j]; + } + } + + _isValid = true; +} + + +bool TerrainTile::isIn(const QGeoCoordinate& coordinate) const +{ + if (!_isValid) { + qCDebug(TerrainTileLog) << "isIn requested, but tile not valid"; + return false; + } + bool ret = coordinate.latitude() >= _southWest.latitude() && coordinate.longitude() >= _southWest.longitude() && + coordinate.latitude() <= _northEast.latitude() && coordinate.longitude() <= _northEast.longitude(); + qCDebug(TerrainTileLog) << "Checking isIn: " << coordinate << " , in sw " << _southWest << " , ne " << _northEast << ": " << ret; + return ret; +} + +double TerrainTile::elevation(const QGeoCoordinate& coordinate) const +{ + if (_isValid) { + qCDebug(TerrainTileLog) << "elevation: " << coordinate << " , in sw " << _southWest << " , ne " << _northEast; + // Get the index at resolution of 1 arc second + int indexLat = _latToDataIndex(coordinate.latitude()); + int indexLon = _lonToDataIndex(coordinate.longitude()); + if (indexLat == -1 || indexLon == -1) { + qCWarning(TerrainTileLog) << "Internal error indexLat:indexLon == -1" << indexLat << indexLon; + return -1.0; + } + qCDebug(TerrainTileLog) << "indexLat:indexLon" << indexLat << indexLon << "elevation" << _data[indexLat][indexLon]; + return static_cast(_data[indexLat][indexLon]); + } else { + qCDebug(TerrainTileLog) << "Asking for elevation, but no valid data."; + return -1.0; + } +} + +QGeoCoordinate TerrainTile::centerCoordinate(void) const +{ + return _southWest.atDistanceAndAzimuth(_southWest.distanceTo(_northEast) / 2.0, _southWest.azimuthTo(_northEast)); +} + +QByteArray TerrainTile::serialize(QByteArray input) +{ + QJsonParseError parseError; + QJsonDocument document = QJsonDocument::fromJson(input, &parseError); + if (parseError.error != QJsonParseError::NoError) { + QByteArray emptyArray; + return emptyArray; + } + + QByteArray byteArray; + QDataStream stream(&byteArray, QIODevice::WriteOnly); + if (!document.isObject()) { + qCDebug(TerrainTileLog) << "Terrain tile json doc is no object"; + QByteArray emptyArray; + return emptyArray; + } + QJsonObject rootObject = document.object(); + + QString errorString; + QList rootVersionKeyInfoList = { + { _jsonStatusKey, QJsonValue::String, true }, + { _jsonDataKey, QJsonValue::Object, true }, + }; + if (!JsonHelper::validateKeys(rootObject, rootVersionKeyInfoList, errorString)) { + qCDebug(TerrainTileLog) << "Error in reading json: " << errorString; + QByteArray emptyArray; + return emptyArray; + } + + if (rootObject[_jsonStatusKey].toString() != "success") { + qCDebug(TerrainTileLog) << "Invalid terrain tile."; + QByteArray emptyArray; + return emptyArray; + } + const QJsonObject& dataObject = rootObject[_jsonDataKey].toObject(); + QList dataVersionKeyInfoList = { + { _jsonBoundsKey, QJsonValue::Object, true }, + { _jsonStatsKey, QJsonValue::Object, true }, + { _jsonCarpetKey, QJsonValue::Array, true }, + }; + if (!JsonHelper::validateKeys(dataObject, dataVersionKeyInfoList, errorString)) { + qCDebug(TerrainTileLog) << "Error in reading json: " << errorString; + QByteArray emptyArray; + return emptyArray; + } + + // Bounds + const QJsonObject& boundsObject = dataObject[_jsonBoundsKey].toObject(); + QList boundsVersionKeyInfoList = { + { _jsonSouthWestKey, QJsonValue::Array, true }, + { _jsonNorthEastKey, QJsonValue::Array, true }, + }; + if (!JsonHelper::validateKeys(boundsObject, boundsVersionKeyInfoList, errorString)) { + qCDebug(TerrainTileLog) << "Error in reading json: " << errorString; + QByteArray emptyArray; + return emptyArray; + } + const QJsonArray& swArray = boundsObject[_jsonSouthWestKey].toArray(); + const QJsonArray& neArray = boundsObject[_jsonNorthEastKey].toArray(); + if (swArray.count() < 2 || neArray.count() < 2 ) { + qCDebug(TerrainTileLog) << "Incomplete bounding location"; + QByteArray emptyArray; + return emptyArray; + } + stream << static_cast(swArray[0].toDouble()); + stream << static_cast(swArray[1].toDouble()); + stream << static_cast(neArray[0].toDouble()); + stream << static_cast(neArray[1].toDouble()); + + // Stats + const QJsonObject& statsObject = dataObject[_jsonStatsKey].toObject(); + QList statsVersionKeyInfoList = { + { _jsonMinElevationKey, QJsonValue::Double, true }, + { _jsonMaxElevationKey, QJsonValue::Double, true }, + { _jsonAvgElevationKey, QJsonValue::Double, true }, + }; + if (!JsonHelper::validateKeys(statsObject, statsVersionKeyInfoList, errorString)) { + qCDebug(TerrainTileLog) << "Error in reading json: " << errorString; + QByteArray emptyArray; + return emptyArray; + } + stream << static_cast(statsObject[_jsonMinElevationKey].toInt()); + stream << static_cast(statsObject[_jsonMaxElevationKey].toInt()); + stream << static_cast(statsObject[_jsonAvgElevationKey].toDouble()); + + // Carpet + const QJsonArray& carpetArray = dataObject[_jsonCarpetKey].toArray(); + int gridSizeLat = carpetArray.count(); + stream << static_cast(gridSizeLat); + int gridSizeLon = 0; + qCDebug(TerrainTileLog) << "Received tile has size in latitude direction: " << carpetArray.count(); + for (int i = 0; i < gridSizeLat; i++) { + const QJsonArray& row = carpetArray[i].toArray(); + if (i == 0) { + gridSizeLon = row.count(); + stream << static_cast(gridSizeLon); + qCDebug(TerrainTileLog) << "Received tile has size in longitued direction: " << row.count(); + } + if (row.count() < gridSizeLon) { + qCDebug(TerrainTileLog) << "Expected row array of " << gridSizeLon << ", instead got " << row.count(); + QByteArray emptyArray; + return emptyArray; + } + for (int j = 0; j < gridSizeLon; j++) { + stream << static_cast(row[j].toDouble()); + } + } + + return byteArray; +} + + +int TerrainTile::_latToDataIndex(double latitude) const +{ + if (isValid() && _southWest.isValid() && _northEast.isValid()) { + return qRound((latitude - _southWest.latitude()) / (_northEast.latitude() - _southWest.latitude()) * (_gridSizeLat - 1)); + } else { + return -1; + } +} + +int TerrainTile::_lonToDataIndex(double longitude) const +{ + if (isValid() && _southWest.isValid() && _northEast.isValid()) { + return qRound((longitude - _southWest.longitude()) / (_northEast.longitude() - _southWest.longitude()) * (_gridSizeLon - 1)); + } else { + return -1; + } +} diff --git a/src/TerrainTile.h b/src/TerrainTile.h new file mode 100644 index 0000000000000000000000000000000000000000..adf217df8574e6f8f601d43aa3a3c6a6905ef4e4 --- /dev/null +++ b/src/TerrainTile.h @@ -0,0 +1,126 @@ +#ifndef TERRAINTILE_H +#define TERRAINTILE_H + +#include "QGCLoggingCategory.h" + +#include + +Q_DECLARE_LOGGING_CATEGORY(TerrainTileLog) + +/** + * @brief The TerrainTile class + * + * Implements an interface for https://developers.airmap.com/v2.0/docs/elevation-api + */ + +class TerrainTile +{ +public: + TerrainTile(); + ~TerrainTile(); + + /** + * Constructor from json doc with elevation data (either from file or web) + * + * @param document + */ + TerrainTile(QJsonDocument document); + + /** + * Constructor from serialized elevation data (either from file or web) + * + * @param document + */ + TerrainTile(QByteArray byteArray); + + /** + * Check for whether a coordinate lies within this tile + * + * @param coordinate + * @return true if within + */ + bool isIn(const QGeoCoordinate& coordinate) const; + + /** + * Check whether valid data is loaded + * + * @return true if data is valid + */ + bool isValid(void) const { return _isValid; } + + /** + * Evaluates the elevation at the given coordinate + * + * @param coordinate + * @return elevation + */ + double elevation(const QGeoCoordinate& coordinate) const; + + /** + * Accessor for the minimum elevation of the tile + * + * @return minimum elevation + */ + double minElevation(void) const { return _minElevation; } + + /** + * Accessor for the maximum elevation of the tile + * + * @return maximum elevation + */ + double maxElevation(void) const { return _maxElevation; } + + /** + * Accessor for the average elevation of the tile + * + * @return average elevation + */ + double avgElevation(void) const { return _avgElevation; } + + /** + * Accessor for the center coordinate + * + * @return center coordinate + */ + QGeoCoordinate centerCoordinate(void) const; + + /** + * Serialize data + * + * @return serialized data + */ + static QByteArray serialize(QByteArray input); + + /// Approximate spacing of the elevation data measurement points + static constexpr double terrainAltitudeSpacing = 30.0; + +private: + inline int _latToDataIndex(double latitude) const; + inline int _lonToDataIndex(double longitude) const; + + QGeoCoordinate _southWest; /// South west corner of the tile + QGeoCoordinate _northEast; /// North east corner of the tile + + int16_t _minElevation; /// Minimum elevation in tile + int16_t _maxElevation; /// Maximum elevation in tile + float _avgElevation; /// Average elevation of the tile + + int16_t** _data; /// 2D elevation data array + int16_t _gridSizeLat; /// data grid size in latitude direction + int16_t _gridSizeLon; /// data grid size in longitude direction + bool _isValid; /// data loaded is valid + + // Json keys + static const char* _jsonStatusKey; + static const char* _jsonDataKey; + static const char* _jsonBoundsKey; + static const char* _jsonSouthWestKey; + static const char* _jsonNorthEastKey; + static const char* _jsonStatsKey; + static const char* _jsonMaxElevationKey; + static const char* _jsonMinElevationKey; + static const char* _jsonAvgElevationKey; + static const char* _jsonCarpetKey; +}; + +#endif // TERRAINTILE_H diff --git a/src/Vehicle/BatteryFact.json b/src/Vehicle/BatteryFact.json index 52144355398bbfde975476e481f3256e042a6929..d2a8d47773c940bd1bb30680853f8bba5cd72df7 100644 --- a/src/Vehicle/BatteryFact.json +++ b/src/Vehicle/BatteryFact.json @@ -30,8 +30,8 @@ { "name": "temperature", "shortDescription": "Temperature", - "type": "int32", - "decimalPlaces": 2, + "type": "double", + "decimalPlaces": 0, "units": "C" }, { @@ -46,5 +46,19 @@ "type": "float", "decimalPlaces": 2, "units": "W" +}, +{ + "name": "timeRemaining", + "shortDescription": "Time Remaining", + "type": "int32", + "units": "s" +}, +{ + "name": "chargeState", + "shortDescription": "Charge State", + "type": "uint8", + "enumStrings": "Low Battery State Not Provided,Normal Operation,Low Battery State,Critical Battery State,Emergency Battery State,Battery Failed,Battery Unhealthy", + "enumValues": "0,1,2,3,4,5,6", + "decimalPlaces": 0 } ] diff --git a/src/Vehicle/DistanceSensorFact.json b/src/Vehicle/DistanceSensorFact.json new file mode 100644 index 0000000000000000000000000000000000000000..97f2837e49065a48f6f9ca939c641ea40e3eec78 --- /dev/null +++ b/src/Vehicle/DistanceSensorFact.json @@ -0,0 +1,72 @@ +[ +{ + "name": "rotationNone", + "shortDescription": "Forward", + "type": "double", + "decimalPlaces": 2, + "units": "m" +}, +{ + "name": "rotationYaw45", + "shortDescription": "Forward/Right", + "type": "double", + "decimalPlaces": 2, + "units": "m" +}, +{ + "name": "rotationYaw90", + "shortDescription": "Right", + "type": "double", + "decimalPlaces": 2, + "units": "m" +}, +{ + "name": "rotationYaw135", + "shortDescription": "Rear/Right", + "type": "double", + "decimalPlaces": 2, + "units": "m" +}, +{ + "name": "rotationYaw180", + "shortDescription": "Rear", + "type": "double", + "decimalPlaces": 2, + "units": "m" +}, +{ + "name": "rotationYaw225", + "shortDescription": "Rear/Left", + "type": "double", + "decimalPlaces": 2, + "units": "m" +}, +{ + "name": "rotationYaw270", + "shortDescription": "Left", + "type": "double", + "decimalPlaces": 2, + "units": "m" +}, +{ + "name": "rotationYaw315", + "shortDescription": "Forward/Left", + "type": "double", + "decimalPlaces": 2, + "units": "m" +}, +{ + "name": "rotationPitch90", + "shortDescription": "Up", + "type": "double", + "decimalPlaces": 2, + "units": "m" +}, +{ + "name": "rotationPitch270", + "shortDescription": "Down", + "type": "double", + "decimalPlaces": 2, + "units": "m" +} +] diff --git a/src/Vehicle/MAVLinkLogManager.cc b/src/Vehicle/MAVLinkLogManager.cc index 756f432e0381ac3871513d1d0aff2147f0af40e7..eca0bc3b3135fd765543eda0fb6b35ad2efc0fc4 100644 --- a/src/Vehicle/MAVLinkLogManager.cc +++ b/src/Vehicle/MAVLinkLogManager.cc @@ -309,6 +309,7 @@ MAVLinkLogManager::MAVLinkLogManager(QGCApplication* app, QGCToolbox* toolbox) , _deleteAfterUpload(false) , _windSpeed(-1) , _publicLog(false) + , _logginDenied(false) { //-- Get saved settings QSettings settings; @@ -613,7 +614,7 @@ MAVLinkLogManager::cancelUpload() void MAVLinkLogManager::startLogging() { - if(_vehicle && _vehicle->px4Firmware()) { + if(_vehicle && _vehicle->px4Firmware() && !_logginDenied) { if(_createNewLog()) { _vehicle->startMavlinkLog(); _logRunning = true; @@ -847,6 +848,8 @@ MAVLinkLogManager::_activeVehicleChanged(Vehicle* vehicle) // Connect new system if(vehicle && vehicle->px4Firmware()) { _vehicle = vehicle; + //-- Reset logging denied flag as well + _logginDenied = false; connect(_vehicle, &Vehicle::armedChanged, this, &MAVLinkLogManager::_armedChanged); connect(_vehicle, &Vehicle::mavlinkLogData, this, &MAVLinkLogManager::_mavlinkLogData); connect(_vehicle, &Vehicle::mavCommandResult, this, &MAVLinkLogManager::_mavCommandResult); @@ -888,7 +891,12 @@ MAVLinkLogManager::_mavCommandResult(int vehicleId, int component, int command, qCWarning(MAVLinkLogManagerLog) << "Stop MAVLink log command failed."; } else { //-- Could not start logging for some reason. - qCWarning(MAVLinkLogManagerLog) << "Start MAVLink log command failed."; + if(result == MAV_RESULT_DENIED) { + _logginDenied = true; + qCWarning(MAVLinkLogManagerLog) << "Start MAVLink log command denied."; + } else { + qCWarning(MAVLinkLogManagerLog) << "Start MAVLink log command failed."; + } _discardLog(); } } diff --git a/src/Vehicle/MAVLinkLogManager.h b/src/Vehicle/MAVLinkLogManager.h index 507b8bea8f3736a0bd27f51f1914cc255094b849..ba47065f64ba0c7825957b048a8d5937d3c7c398 100644 --- a/src/Vehicle/MAVLinkLogManager.h +++ b/src/Vehicle/MAVLinkLogManager.h @@ -140,7 +140,7 @@ public: bool enableAutoStart () { return _enableAutoStart; } bool uploading (); bool logRunning () { return _logRunning; } - bool canStartLog () { return _vehicle != NULL; } + bool canStartLog () { return _vehicle != NULL && !_logginDenied; } bool deleteAfterUpload () { return _deleteAfterUpload; } bool publicLog () { return _publicLog; } int windSpeed () { return _windSpeed; } @@ -226,6 +226,7 @@ private: QString _rating; bool _publicLog; QString _ulogExtension; + bool _logginDenied; }; diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index cbe4bdea6ee1eb4de6842f290ff453a9e73d7b2d..dd84995872a18a3cd00f71c81d0121e855370b83 100644 --- a/src/Vehicle/MultiVehicleManager.cc +++ b/src/Vehicle/MultiVehicleManager.cc @@ -74,6 +74,14 @@ void MultiVehicleManager::setToolbox(QGCToolbox *toolbox) void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleFirmwareType, int vehicleType) { + // Special case PX4 Flow since depending on firmware it can have different settings. We force to the PX4 Firmware settings. + if (link->isPX4Flow()) { + vehicleId = 81; + componentId = 50;//MAV_COMP_ID_AUTOPILOT1; + vehicleFirmwareType = MAV_AUTOPILOT_GENERIC; + vehicleType = 0; + } + if (componentId != MAV_COMP_ID_AUTOPILOT1) { // Special case for PX4 Flow if (vehicleId != 81 || componentId != 50) { @@ -138,9 +146,6 @@ void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicle setActiveVehicle(vehicle); } - // Mark link as active - link->setActive(true); - #if defined (__ios__) || defined(__android__) if(_vehicles.count() == 1) { //-- Once a vehicle is connected, keep screen from going off diff --git a/src/Vehicle/SetpointFact.json b/src/Vehicle/SetpointFact.json new file mode 100644 index 0000000000000000000000000000000000000000..58fea58334a8866ee4661454452bee925e16a99d --- /dev/null +++ b/src/Vehicle/SetpointFact.json @@ -0,0 +1,44 @@ +[ +{ + "name": "roll", + "shortDescription": "Roll Setpoint", + "type": "double", + "decimalPlaces": 1, + "units": "deg" +}, +{ + "name": "pitch", + "shortDescription": "Pitch Setpoint", + "type": "double", + "decimalPlaces": 1, + "units": "deg" +}, +{ + "name": "yaw", + "shortDescription": "Yaw Setpoint", + "type": "double", + "decimalPlaces": 1, + "units": "deg" +}, +{ + "name": "rollRate", + "shortDescription": "Roll Rate Setpoint", + "type": "double", + "decimalPlaces": 1, + "units": "deg/s" +}, +{ + "name": "pitchRate", + "shortDescription": "Pitch Rate Setpoint", + "type": "double", + "decimalPlaces": 1, + "units": "deg/s" +}, +{ + "name": "yawRate", + "shortDescription": "Yaw Rate Setpoint", + "type": "double", + "decimalPlaces": 1, + "units": "deg/s" +} +] diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index a05d86c8255aec861165578fadc8e2ebeb7091d5..01a66ea2c09c807eeb7523a3f937dc83e508902f 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -10,6 +10,7 @@ #include #include #include +#include #include "Vehicle.h" #include "MAVLinkProtocol.h" @@ -54,6 +55,9 @@ const char* Vehicle::_joystickEnabledSettingsKey = "JoystickEnabled"; const char* Vehicle::_rollFactName = "roll"; const char* Vehicle::_pitchFactName = "pitch"; const char* Vehicle::_headingFactName = "heading"; +const char* Vehicle::_rollRateFactName = "rollRate"; +const char* Vehicle::_pitchRateFactName = "pitchRate"; +const char* Vehicle::_yawRateFactName = "yawRate"; const char* Vehicle::_airSpeedFactName = "airSpeed"; const char* Vehicle::_groundSpeedFactName = "groundSpeed"; const char* Vehicle::_climbRateFactName = "climbRate"; @@ -64,12 +68,14 @@ const char* Vehicle::_flightTimeFactName = "flightTime"; const char* Vehicle::_distanceToHomeFactName = "distanceToHome"; const char* Vehicle::_hobbsFactName = "hobbs"; -const char* Vehicle::_gpsFactGroupName = "gps"; -const char* Vehicle::_batteryFactGroupName = "battery"; -const char* Vehicle::_windFactGroupName = "wind"; -const char* Vehicle::_vibrationFactGroupName = "vibration"; -const char* Vehicle::_temperatureFactGroupName = "temperature"; -const char* Vehicle::_clockFactGroupName = "clock"; +const char* Vehicle::_gpsFactGroupName = "gps"; +const char* Vehicle::_battery1FactGroupName = "battery"; +const char* Vehicle::_battery2FactGroupName = "battery2"; +const char* Vehicle::_windFactGroupName = "wind"; +const char* Vehicle::_vibrationFactGroupName = "vibration"; +const char* Vehicle::_temperatureFactGroupName = "temperature"; +const char* Vehicle::_clockFactGroupName = "clock"; +const char* Vehicle::_distanceSensorFactGroupName = "distanceSensor"; Vehicle::Vehicle(LinkInterface* link, int vehicleId, @@ -128,6 +134,7 @@ Vehicle::Vehicle(LinkInterface* link, , _vehicleCapabilitiesKnown(false) , _capabilityBits(0) , _highLatencyLink(false) + , _receivingAttitudeQuaternion(false) , _cameras(NULL) , _connectionLost(false) , _connectionLostEnabled(true) @@ -163,9 +170,13 @@ Vehicle::Vehicle(LinkInterface* link, , _gitHash(versionNotSetValue) , _uid(0) , _lastAnnouncedLowBatteryPercent(100) + , _priorityLinkCommanded(false) , _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble) , _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble) , _headingFact (0, _headingFactName, FactMetaData::valueTypeDouble) + , _rollRateFact (0, _rollRateFactName, FactMetaData::valueTypeDouble) + , _pitchRateFact (0, _pitchRateFactName, FactMetaData::valueTypeDouble) + , _yawRateFact (0, _yawRateFactName, FactMetaData::valueTypeDouble) , _groundSpeedFact (0, _groundSpeedFactName, FactMetaData::valueTypeDouble) , _airSpeedFact (0, _airSpeedFactName, FactMetaData::valueTypeDouble) , _climbRateFact (0, _climbRateFactName, FactMetaData::valueTypeDouble) @@ -176,14 +187,14 @@ Vehicle::Vehicle(LinkInterface* link, , _distanceToHomeFact (0, _distanceToHomeFactName, FactMetaData::valueTypeDouble) , _hobbsFact (0, _hobbsFactName, FactMetaData::valueTypeString) , _gpsFactGroup(this) - , _batteryFactGroup(this) + , _battery1FactGroup(this) + , _battery2FactGroup(this) , _windFactGroup(this) , _vibrationFactGroup(this) , _temperatureFactGroup(this) , _clockFactGroup(this) + , _distanceSensorFactGroup(this) { - _addLink(link); - connect(_joystickManager, &JoystickManager::activeJoystickChanged, this, &Vehicle::_loadSettings); connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::activeVehicleAvailableChanged, this, &Vehicle::_loadSettings); @@ -191,6 +202,8 @@ Vehicle::Vehicle(LinkInterface* link, connect(_mavlink, &MAVLinkProtocol::messageReceived, this, &Vehicle::_mavlinkMessageReceived); + _addLink(link); + connect(this, &Vehicle::_sendMessageOnLinkOnThread, this, &Vehicle::_sendMessageOnLink, Qt::QueuedConnection); connect(this, &Vehicle::flightModeChanged, this, &Vehicle::_handleFlightModeChanged); connect(this, &Vehicle::armedChanged, this, &Vehicle::_announceArmedChanged); @@ -213,12 +226,6 @@ Vehicle::Vehicle(LinkInterface* link, _prearmErrorTimer.setInterval(_prearmErrorTimeoutMSecs); _prearmErrorTimer.setSingleShot(true); - // Connection Lost timer - _connectionLostTimer.setInterval(_connectionLostTimeoutMSecs); - _connectionLostTimer.setSingleShot(false); - _connectionLostTimer.start(); - connect(&_connectionLostTimer, &QTimer::timeout, this, &Vehicle::_connectionLostTimeout); - // Send MAV_CMD ack timer _mavCommandAckTimer.setSingleShot(true); _mavCommandAckTimer.setInterval(_highLatencyLink ? _mavCommandAckTimeoutMSecsHighLatency : _mavCommandAckTimeoutMSecs); @@ -229,12 +236,9 @@ Vehicle::Vehicle(LinkInterface* link, // Listen for system messages connect(_toolbox->uasMessageHandler(), &UASMessageHandler::textMessageCountChanged, this, &Vehicle::_handleTextMessage); connect(_toolbox->uasMessageHandler(), &UASMessageHandler::textMessageReceived, this, &Vehicle::_handletextMessageReceived); - // Now connect the new UAS - connect(_mav, SIGNAL(attitudeChanged (UASInterface*,double,double,double,quint64)), this, SLOT(_updateAttitude(UASInterface*, double, double, double, quint64))); - connect(_mav, SIGNAL(attitudeChanged (UASInterface*,int,double,double,double,quint64)), this, SLOT(_updateAttitude(UASInterface*,int,double, double, double, quint64))); - if (_highLatencyLink) { - // High latency links don't request information + if (_highLatencyLink || link->isPX4Flow()) { + // These links don't request information _setMaxProtoVersion(100); _setCapabilities(0); _initialPlanRequestComplete = true; @@ -315,6 +319,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _vehicleCapabilitiesKnown(true) , _capabilityBits(_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA ? 0 : MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY) , _highLatencyLink(false) + , _receivingAttitudeQuaternion(false) , _cameras(NULL) , _connectionLost(false) , _connectionLostEnabled(true) @@ -353,6 +358,9 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble) , _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble) , _headingFact (0, _headingFactName, FactMetaData::valueTypeDouble) + , _rollRateFact (0, _rollRateFactName, FactMetaData::valueTypeDouble) + , _pitchRateFact (0, _pitchRateFactName, FactMetaData::valueTypeDouble) + , _yawRateFact (0, _yawRateFactName, FactMetaData::valueTypeDouble) , _groundSpeedFact (0, _groundSpeedFactName, FactMetaData::valueTypeDouble) , _airSpeedFact (0, _airSpeedFactName, FactMetaData::valueTypeDouble) , _climbRateFact (0, _climbRateFactName, FactMetaData::valueTypeDouble) @@ -363,10 +371,12 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _distanceToHomeFact (0, _distanceToHomeFactName, FactMetaData::valueTypeDouble) , _hobbsFact (0, _hobbsFactName, FactMetaData::valueTypeString) , _gpsFactGroup(this) - , _batteryFactGroup(this) + , _battery1FactGroup(this) + , _battery2FactGroup(this) , _windFactGroup(this) , _vibrationFactGroup(this) , _clockFactGroup(this) + , _distanceSensorFactGroup(this) { _commonInit(); _firmwarePlugin->initializeVehicle(this); @@ -408,11 +418,17 @@ void Vehicle::_commonInit(void) connect(_settingsManager->appSettings()->offlineEditingCruiseSpeed(), &Fact::rawValueChanged, this, &Vehicle::_offlineCruiseSpeedSettingChanged); connect(_settingsManager->appSettings()->offlineEditingHoverSpeed(), &Fact::rawValueChanged, this, &Vehicle::_offlineHoverSpeedSettingChanged); + // Flight modes can differ based on advanced mode + connect(_toolbox->corePlugin(), &QGCCorePlugin::showAdvancedUIChanged, this, &Vehicle::flightModesChanged); + // Build FactGroup object model _addFact(&_rollFact, _rollFactName); _addFact(&_pitchFact, _pitchFactName); _addFact(&_headingFact, _headingFactName); + _addFact(&_rollRateFact, _rollRateFactName); + _addFact(&_pitchRateFact, _pitchRateFactName); + _addFact(&_yawRateFact, _yawRateFactName); _addFact(&_groundSpeedFact, _groundSpeedFactName); _addFact(&_airSpeedFact, _airSpeedFactName); _addFact(&_climbRateFact, _climbRateFactName); @@ -425,12 +441,14 @@ void Vehicle::_commonInit(void) _hobbsFact.setRawValue(QVariant(QString("0000:00:00"))); _addFact(&_hobbsFact, _hobbsFactName); - _addFactGroup(&_gpsFactGroup, _gpsFactGroupName); - _addFactGroup(&_batteryFactGroup, _batteryFactGroupName); - _addFactGroup(&_windFactGroup, _windFactGroupName); - _addFactGroup(&_vibrationFactGroup, _vibrationFactGroupName); - _addFactGroup(&_temperatureFactGroup, _temperatureFactGroupName); - _addFactGroup(&_clockFactGroup, _clockFactGroupName); + _addFactGroup(&_gpsFactGroup, _gpsFactGroupName); + _addFactGroup(&_battery1FactGroup, _battery1FactGroupName); + _addFactGroup(&_battery2FactGroup, _battery2FactGroupName); + _addFactGroup(&_windFactGroup, _windFactGroupName); + _addFactGroup(&_vibrationFactGroup, _vibrationFactGroupName); + _addFactGroup(&_temperatureFactGroup, _temperatureFactGroupName); + _addFactGroup(&_clockFactGroup, _clockFactGroupName); + _addFactGroup(&_distanceSensorFactGroup, _distanceSensorFactGroupName); // Add firmware-specific fact groups, if provided QMap* fwFactGroups = _firmwarePlugin->factGroups(); @@ -474,6 +492,7 @@ void Vehicle::prepareDelete() void Vehicle::_offlineFirmwareTypeSettingChanged(QVariant value) { _firmwareType = static_cast(value.toInt()); + _firmwarePlugin = _firmwarePluginManager->firmwarePluginForAutopilot(_firmwareType, _vehicleType); emit firmwareTypeChanged(); if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { _capabilityBits = 0; @@ -585,13 +604,6 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes } } - - // Mark this vehicle as active - but only if the traffic is coming from - // the actual vehicle - if (message.sysid == _id) { - _connectionActive(); - } - // Give the plugin a change to adjust the message contents if (!_firmwarePlugin->adjustIncomingMavlinkMessage(this, &message)) { return; @@ -696,6 +708,21 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_HIGH_LATENCY2: _handleHighLatency2(message); break; + case MAVLINK_MSG_ID_ATTITUDE: + _handleAttitude(message); + break; + case MAVLINK_MSG_ID_ATTITUDE_QUATERNION: + _handleAttitudeQuaternion(message); + break; + case MAVLINK_MSG_ID_ATTITUDE_TARGET: + _handleAttitudeTarget(message); + break; + case MAVLINK_MSG_ID_DISTANCE_SENSOR: + _handleDistanceSensor(message); + break; + case MAVLINK_MSG_ID_PING: + _handlePing(link, message); + break; case MAVLINK_MSG_ID_SERIAL_CONTROL: { @@ -760,6 +787,120 @@ void Vehicle::_handleVfrHud(mavlink_message_t& message) _climbRateFact.setRawValue(qIsNaN(vfrHud.climb) ? 0 : vfrHud.climb); } +void Vehicle::_handleDistanceSensor(mavlink_message_t& message) +{ + mavlink_distance_sensor_t distanceSensor; + + mavlink_msg_distance_sensor_decode(&message, &distanceSensor);\ + + if (!_distanceSensorFactGroup.idSet()) { + _distanceSensorFactGroup.setIdSet(true); + _distanceSensorFactGroup.setId(distanceSensor.id); + } + + if (_distanceSensorFactGroup.id() != distanceSensor.id) { + // We can only handle a single sensor reporting + return; + } + + struct orientation2Fact_s { + MAV_SENSOR_ORIENTATION orientation; + Fact* fact; + }; + + orientation2Fact_s rgOrientation2Fact[] = + { + { MAV_SENSOR_ROTATION_NONE, _distanceSensorFactGroup.rotationNone() }, + { MAV_SENSOR_ROTATION_YAW_45, _distanceSensorFactGroup.rotationYaw45() }, + { MAV_SENSOR_ROTATION_YAW_90, _distanceSensorFactGroup.rotationYaw90() }, + { MAV_SENSOR_ROTATION_YAW_135, _distanceSensorFactGroup.rotationYaw135() }, + { MAV_SENSOR_ROTATION_YAW_180, _distanceSensorFactGroup.rotationYaw180() }, + { MAV_SENSOR_ROTATION_YAW_225, _distanceSensorFactGroup.rotationYaw225() }, + { MAV_SENSOR_ROTATION_YAW_270, _distanceSensorFactGroup.rotationYaw270() }, + { MAV_SENSOR_ROTATION_YAW_315, _distanceSensorFactGroup.rotationYaw315() }, + { MAV_SENSOR_ROTATION_PITCH_90, _distanceSensorFactGroup.rotationPitch90() }, + { MAV_SENSOR_ROTATION_PITCH_270, _distanceSensorFactGroup.rotationPitch270() }, + }; + + for (size_t i=0; isetRawValue(distanceSensor.current_distance / 100.0); // cm to meters + } + } +} + +void Vehicle::_handleAttitudeTarget(mavlink_message_t& message) +{ + mavlink_attitude_target_t attitudeTarget; + + mavlink_msg_attitude_target_decode(&message, &attitudeTarget); + + float roll, pitch, yaw; + mavlink_quaternion_to_euler(attitudeTarget.q, &roll, &pitch, &yaw); + + _setpointFactGroup.roll()->setRawValue(qRadiansToDegrees(roll)); + _setpointFactGroup.pitch()->setRawValue(qRadiansToDegrees(pitch)); + _setpointFactGroup.yaw()->setRawValue(qRadiansToDegrees(yaw)); + + _setpointFactGroup.rollRate()->setRawValue(qRadiansToDegrees(attitudeTarget.body_roll_rate)); + _setpointFactGroup.pitchRate()->setRawValue(qRadiansToDegrees(attitudeTarget.body_pitch_rate)); + _setpointFactGroup.yawRate()->setRawValue(qRadiansToDegrees(attitudeTarget.body_yaw_rate)); +} + +void Vehicle::_handleAttitudeWorker(double rollRadians, double pitchRadians, double yawRadians) +{ + double roll, pitch, yaw; + + roll = QGC::limitAngleToPMPIf(rollRadians); + pitch = QGC::limitAngleToPMPIf(pitchRadians); + yaw = QGC::limitAngleToPMPIf(yawRadians); + + roll = qRadiansToDegrees(roll); + pitch = qRadiansToDegrees(pitch); + yaw = qRadiansToDegrees(yaw); + + if (yaw < 0.0) { + yaw += 360.0; + } + // truncate to integer so widget never displays 360 + yaw = trunc(yaw); + + _rollFact.setRawValue(roll); + _pitchFact.setRawValue(pitch); + _headingFact.setRawValue(yaw); +} + +void Vehicle::_handleAttitude(mavlink_message_t& message) +{ + if (_receivingAttitudeQuaternion) { + return; + } + + mavlink_attitude_t attitude; + mavlink_msg_attitude_decode(&message, &attitude); + + _handleAttitudeWorker(attitude.roll, attitude.pitch, attitude.yaw); +} + +void Vehicle::_handleAttitudeQuaternion(mavlink_message_t& message) +{ + _receivingAttitudeQuaternion = true; + + mavlink_attitude_quaternion_t attitudeQuaternion; + mavlink_msg_attitude_quaternion_decode(&message, &attitudeQuaternion); + + float roll, pitch, yaw; + float q[] = { attitudeQuaternion.q1, attitudeQuaternion.q2, attitudeQuaternion.q3, attitudeQuaternion.q4 }; + mavlink_quaternion_to_euler(q, &roll, &pitch, &yaw); + + _handleAttitudeWorker(roll, pitch, yaw); + + rollRate()->setRawValue(qRadiansToDegrees(attitudeQuaternion.rollspeed)); + pitchRate()->setRawValue(qRadiansToDegrees(attitudeQuaternion.pitchspeed)); + yawRate()->setRawValue(qRadiansToDegrees(attitudeQuaternion.yawspeed)); +} + void Vehicle::_handleGpsRawInt(mavlink_message_t& message) { mavlink_gps_raw_int_t gpsRawInt; @@ -769,11 +910,11 @@ void Vehicle::_handleGpsRawInt(mavlink_message_t& message) if (gpsRawInt.fix_type >= GPS_FIX_TYPE_3D_FIX) { if (!_globalPositionIntMessageAvailable) { - //-- Set these here and emit a single signal instead of 3 for the same variable (_coordinate) - _coordinate.setLatitude(gpsRawInt.lat / (double)1E7); - _coordinate.setLongitude(gpsRawInt.lon / (double)1E7); - _coordinate.setAltitude(gpsRawInt.alt / 1000.0); - emit coordinateChanged(_coordinate); + QGeoCoordinate newPosition(gpsRawInt.lat / (double)1E7, gpsRawInt.lon / (double)1E7, gpsRawInt.alt / 1000.0); + if (newPosition != _coordinate) { + _coordinate = newPosition; + emit coordinateChanged(_coordinate); + } _altitudeAMSLFact.setRawValue(gpsRawInt.alt / 1000.0); } } @@ -802,11 +943,11 @@ void Vehicle::_handleGlobalPositionInt(mavlink_message_t& message) } _globalPositionIntMessageAvailable = true; - //-- Set these here and emit a single signal instead of 3 for the same variable (_coordinate) - _coordinate.setLatitude(globalPositionInt.lat / (double)1E7); - _coordinate.setLongitude(globalPositionInt.lon / (double)1E7); - _coordinate.setAltitude(globalPositionInt.alt / 1000.0); - emit coordinateChanged(_coordinate); + QGeoCoordinate newPosition(globalPositionInt.lat / (double)1E7, globalPositionInt.lon / (double)1E7, globalPositionInt.alt / 1000.0); + if (newPosition != _coordinate) { + _coordinate = newPosition; + emit coordinateChanged(_coordinate); + } } void Vehicle::_handleHighLatency2(mavlink_message_t& message) @@ -847,7 +988,7 @@ void Vehicle::_handleHighLatency2(mavlink_message_t& message) _windFactGroup.direction()->setRawValue((double)highLatency2.wind_heading * 2.0); _windFactGroup.speed()->setRawValue((double)highLatency2.windspeed / 5.0); - _batteryFactGroup.percentRemaining()->setRawValue(highLatency2.battery); + _battery1FactGroup.percentRemaining()->setRawValue(highLatency2.battery); _temperatureFactGroup.temperature1()->setRawValue(highLatency2.temperature_air); @@ -869,18 +1010,6 @@ void Vehicle::_handleHighLatency2(mavlink_message_t& message) { HL_FAILURE_FLAG_3D_ACCEL, MAV_SYS_STATUS_SENSOR_3D_ACCEL }, { HL_FAILURE_FLAG_3D_GYRO, MAV_SYS_STATUS_SENSOR_3D_GYRO }, { HL_FAILURE_FLAG_3D_MAG, MAV_SYS_STATUS_SENSOR_3D_MAG }, -#if 0 - // FIXME: These don't currently map to existing sensor health bits. Support needs to be added to show them - // on health page of instrument panel as well. - { HL_FAILURE_FLAG_TERRAIN=64, /* Terrain subsystem failure. | */ - { HL_FAILURE_FLAG_BATTERY=128, /* Battery failure/critical low battery. | */ - { HL_FAILURE_FLAG_RC_RECEIVER=256, /* RC receiver failure/no rc connection. | */ - { HL_FAILURE_FLAG_OFFBOARD_LINK=512, /* Offboard link failure. | */ - { HL_FAILURE_FLAG_ENGINE=1024, /* Engine failure. | */ - { HL_FAILURE_FLAG_GEOFENCE=2048, /* Geofence violation. | */ - { HL_FAILURE_FLAG_ESTIMATOR=4096, /* Estimator failure, for example measurement rejection or large variances. | */ - { HL_FAILURE_FLAG_MISSION=8192, /* Mission failure. | */ -#endif }; // Map from MAV_FAILURE bits to standard SYS_STATUS message handling @@ -1067,6 +1196,7 @@ void Vehicle::_handleCommandLong(mavlink_message_t& message) if (sl && sl->getSerialConfig()->usbDirect()) { qDebug() << "Disconnecting:" << _links.at(i)->getName(); qgcApp()->toolbox()->linkManager()->disconnectLink(_links.at(i)); + emit linksChanged(); } } } @@ -1139,31 +1269,43 @@ void Vehicle::_handleWind(mavlink_message_t& message) mavlink_wind_t wind; mavlink_msg_wind_decode(&message, &wind); - _windFactGroup.direction()->setRawValue(wind.direction); + // We don't want negative wind angles + float direction = wind.direction; + if (direction < 0) { + direction += 360; + } + _windFactGroup.direction()->setRawValue(direction); _windFactGroup.speed()->setRawValue(wind.speed); _windFactGroup.verticalSpeed()->setRawValue(wind.speed_z); } #endif +bool Vehicle::_apmArmingNotRequired(void) +{ + QString armingRequireParam("ARMING_REQUIRE"); + return _parameterManager->parameterExists(FactSystem::defaultComponentId, armingRequireParam) && + _parameterManager->getParameter(FactSystem::defaultComponentId, armingRequireParam)->rawValue().toInt() == 0; +} + void Vehicle::_handleSysStatus(mavlink_message_t& message) { mavlink_sys_status_t sysStatus; mavlink_msg_sys_status_decode(&message, &sysStatus); if (sysStatus.current_battery == -1) { - _batteryFactGroup.current()->setRawValue(VehicleBatteryFactGroup::_currentUnavailable); + _battery1FactGroup.current()->setRawValue(VehicleBatteryFactGroup::_currentUnavailable); } else { // Current is in Amps, current_battery is 10 * milliamperes (1 = 10 milliampere) - _batteryFactGroup.current()->setRawValue((float)sysStatus.current_battery / 100.0f); + _battery1FactGroup.current()->setRawValue((float)sysStatus.current_battery / 100.0f); } if (sysStatus.voltage_battery == UINT16_MAX) { - _batteryFactGroup.voltage()->setRawValue(VehicleBatteryFactGroup::_voltageUnavailable); + _battery1FactGroup.voltage()->setRawValue(VehicleBatteryFactGroup::_voltageUnavailable); } else { - _batteryFactGroup.voltage()->setRawValue((double)sysStatus.voltage_battery / 1000.0); + _battery1FactGroup.voltage()->setRawValue((double)sysStatus.voltage_battery / 1000.0); // current_battery is 10 mA and voltage_battery is 1mV. (10/1e3 times 1/1e3 = 1/1e5) - _batteryFactGroup.instantPower()->setRawValue((float)(sysStatus.current_battery*sysStatus.voltage_battery)/(100000.0)); + _battery1FactGroup.instantPower()->setRawValue((float)(sysStatus.current_battery*sysStatus.voltage_battery)/(100000.0)); } - _batteryFactGroup.percentRemaining()->setRawValue(sysStatus.battery_remaining); + _battery1FactGroup.percentRemaining()->setRawValue(sysStatus.battery_remaining); if (sysStatus.battery_remaining > 0) { if (sysStatus.battery_remaining < _settingsManager->appSettings()->batteryPercentRemainingAnnounce()->rawValue().toInt() && @@ -1173,14 +1315,31 @@ void Vehicle::_handleSysStatus(mavlink_message_t& message) _lastAnnouncedLowBatteryPercent = sysStatus.battery_remaining; } - _onboardControlSensorsPresent = sysStatus.onboard_control_sensors_present; - _onboardControlSensorsEnabled = sysStatus.onboard_control_sensors_enabled; - _onboardControlSensorsHealth = sysStatus.onboard_control_sensors_health; + if (_onboardControlSensorsPresent != sysStatus.onboard_control_sensors_present) { + _onboardControlSensorsPresent = sysStatus.onboard_control_sensors_present; + emit sensorsPresentBitsChanged(_onboardControlSensorsPresent); + } + if (_onboardControlSensorsEnabled != sysStatus.onboard_control_sensors_enabled) { + _onboardControlSensorsEnabled = sysStatus.onboard_control_sensors_enabled; + emit sensorsEnabledBitsChanged(_onboardControlSensorsEnabled); + } + if (_onboardControlSensorsHealth != sysStatus.onboard_control_sensors_health) { + _onboardControlSensorsHealth = sysStatus.onboard_control_sensors_health; + emit sensorsHealthBitsChanged(_onboardControlSensorsHealth); + } + + // ArduPilot firmare has a strange case when ARMING_REQUIRE=0. This means the vehicle is always armed but the motors are not + // really powered up until the safety button is pressed. Because of this we can't depend on the heartbeat to tell us the true + // armed (and dangerous) state. We must instead rely on SYS_STATUS telling us that the motors are enabled. + if (apmFirmware() && _apmArmingNotRequired()) { + _updateArmed(_onboardControlSensorsEnabled & MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS); + } uint32_t newSensorsUnhealthy = _onboardControlSensorsEnabled & ~_onboardControlSensorsHealth; if (newSensorsUnhealthy != _onboardControlSensorsUnhealthy) { _onboardControlSensorsUnhealthy = newSensorsUnhealthy; emit unhealthySensorsChanged(); + emit sensorsUnhealthyBitsChanged(_onboardControlSensorsUnhealthy); } } @@ -1189,15 +1348,17 @@ void Vehicle::_handleBatteryStatus(mavlink_message_t& message) mavlink_battery_status_t bat_status; mavlink_msg_battery_status_decode(&message, &bat_status); + VehicleBatteryFactGroup& batteryFactGroup = bat_status.id == 0 ? _battery1FactGroup : _battery2FactGroup; + if (bat_status.temperature == INT16_MAX) { - _batteryFactGroup.temperature()->setRawValue(VehicleBatteryFactGroup::_temperatureUnavailable); + batteryFactGroup.temperature()->setRawValue(VehicleBatteryFactGroup::_temperatureUnavailable); } else { - _batteryFactGroup.temperature()->setRawValue((double)bat_status.temperature / 100.0); + batteryFactGroup.temperature()->setRawValue((double)bat_status.temperature / 100.0); } if (bat_status.current_consumed == -1) { - _batteryFactGroup.mahConsumed()->setRawValue(VehicleBatteryFactGroup::_mahConsumedUnavailable); + batteryFactGroup.mahConsumed()->setRawValue(VehicleBatteryFactGroup::_mahConsumedUnavailable); } else { - _batteryFactGroup.mahConsumed()->setRawValue(bat_status.current_consumed); + batteryFactGroup.mahConsumed()->setRawValue(bat_status.current_consumed); } int cellCount = 0; @@ -1210,7 +1371,21 @@ void Vehicle::_handleBatteryStatus(mavlink_message_t& message) cellCount = -1; } - _batteryFactGroup.cellCount()->setRawValue(cellCount); + batteryFactGroup.cellCount()->setRawValue(cellCount); + + //-- Time remaining in seconds (0 means not supported) + batteryFactGroup.timeRemaining()->setRawValue(bat_status.time_remaining); + //-- Battery charge state (0 means not supported) + if(bat_status.charge_state <= MAV_BATTERY_CHARGE_STATE_UNHEALTHY) { + batteryFactGroup.chargeState()->setRawValue(bat_status.charge_state); + } else { + batteryFactGroup.chargeState()->setRawValue(0); + } + //-- TODO: Somewhere, actions would be taken based on this chargeState: + // MAV_BATTERY_CHARGE_STATE_CRITICAL: Battery state is critical, return / abort immediately + // MAV_BATTERY_CHARGE_STATE_EMERGENCY: Battery state is too low for ordinary abortion, fastest possible emergency stop preventing damage + // MAV_BATTERY_CHARGE_STATE_FAILED: Battery failed, damage unavoidable + // MAV_BATTERY_CHARGE_STATE_UNHEALTHY: Battery is diagnosed to be broken or an error occurred, usage is discouraged / prohibited } void Vehicle::_setHomePosition(QGeoCoordinate& homeCoord) @@ -1233,20 +1408,10 @@ void Vehicle::_handleHomePosition(mavlink_message_t& message) _setHomePosition(newHomePosition); } -void Vehicle::_handleHeartbeat(mavlink_message_t& message) +void Vehicle::_updateArmed(bool armed) { - if (message.compid != _defaultComponentId) { - return; - } - - mavlink_heartbeat_t heartbeat; - - mavlink_msg_heartbeat_decode(&message, &heartbeat); - - bool newArmed = heartbeat.base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY; - - if (_armed != newArmed) { - _armed = newArmed; + if (_armed != armed) { + _armed = armed; emit armedChanged(_armed); // We are transitioning to the armed state, begin tracking trajectory points for the map @@ -1262,6 +1427,49 @@ void Vehicle::_handleHeartbeat(mavlink_message_t& message) } } } +} + +void Vehicle::_handlePing(LinkInterface* link, mavlink_message_t& message) +{ + mavlink_ping_t ping; + mavlink_message_t msg; + + mavlink_msg_ping_decode(&message, &ping); + mavlink_msg_ping_pack_chan(_mavlink->getSystemId(), + _mavlink->getComponentId(), + priorityLink()->mavlinkChannel(), + &msg, + ping.time_usec, + ping.seq, + message.sysid, + message.compid); + sendMessageOnLink(link, msg); +} + +void Vehicle::_handleHeartbeat(mavlink_message_t& message) +{ + if (message.compid != _defaultComponentId) { + return; + } + + mavlink_heartbeat_t heartbeat; + + mavlink_msg_heartbeat_decode(&message, &heartbeat); + + bool newArmed = heartbeat.base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY; + + // ArduPilot firmare has a strange case when ARMING_REQUIRE=0. This means the vehicle is always armed but the motors are not + // really powered up until the safety button is pressed. Because of this we can't depend on the heartbeat to tell us the true + // armed (and dangerous) state. We must instead rely on SYS_STATUS telling us that the motors are enabled. + if (apmFirmware()) { + if (!_apmArmingNotRequired() || !(_onboardControlSensorsPresent & MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS)) { + // If ARMING_REQUIRE!=0 or we haven't seen motor output status yet we use the hearbeat info for armed + _updateArmed(newArmed); + } + } else { + // Non-ArduPilot always updates from armed state in heartbeat + _updateArmed(newArmed); + } if (heartbeat.base_mode != _base_mode || heartbeat.custom_mode != _custom_mode) { QString previousFlightMode; @@ -1453,11 +1661,16 @@ void Vehicle::_addLink(LinkInterface* link) if (!_containsLink(link)) { qCDebug(VehicleLog) << "_addLink:" << QString("%1").arg((ulong)link, 0, 16); _links += link; - _updatePriorityLink(); - _updateHighLatencyLink(); + if (_links.count() <= 1) { + _updatePriorityLink(true /* updateActive */, false /* sendCommand */); + } else { + _updatePriorityLink(true /* updateActive */, true /* sendCommand */); + } + connect(_toolbox->linkManager(), &LinkManager::linkInactive, this, &Vehicle::_linkInactiveOrDeleted); connect(_toolbox->linkManager(), &LinkManager::linkDeleted, this, &Vehicle::_linkInactiveOrDeleted); connect(link, &LinkInterface::highLatencyChanged, this, &Vehicle::_updateHighLatencyLink); + connect(link, &LinkInterface::activeChanged, this, &Vehicle::_linkActiveChanged); } } @@ -1466,7 +1679,12 @@ void Vehicle::_linkInactiveOrDeleted(LinkInterface* link) qCDebug(VehicleLog) << "_linkInactiveOrDeleted linkCount" << _links.count(); _links.removeOne(link); - _updatePriorityLink(); + + if (_priorityLink.data() == link) { + _priorityLink.clear(); + } + + _updatePriorityLink(true /* updateActive */, true /* sendCommand */); if (_links.count() == 0 && !_allLinksInactiveSent) { qCDebug(VehicleLog) << "All links inactive"; @@ -1512,8 +1730,19 @@ void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message) emit messagesSentChanged(); } -void Vehicle::_updatePriorityLink(void) +void Vehicle::_updatePriorityLink(bool updateActive, bool sendCommand) { + emit linksPropertiesChanged(); + + // if the priority link is commanded and still active don't change anything + if (_priorityLinkCommanded) { + if (_priorityLink.data()->link_active(_id)) { + return; + } else { + _priorityLinkCommanded = false; + } + } + LinkInterface* newPriorityLink = NULL; // This routine specifically does not clear _priorityLink when there are no links remaining. @@ -1526,7 +1755,7 @@ void Vehicle::_updatePriorityLink(void) // Check for the existing priority link to still be valid for (int i=0; i<_links.count(); i++) { if (_priorityLink.data() == _links[i]) { - if (!_priorityLink.data()->highLatency()) { + if (!_priorityLink.data()->highLatency() && _priorityLink->link_active(_id)) { // Link is still valid. Continue to use it unless it is high latency. In that case we still look for a better // link to use as priority link. return; @@ -1535,12 +1764,13 @@ void Vehicle::_updatePriorityLink(void) } // The previous priority link is no longer valid. We must no find the best link available in this priority order: - // Direct USB connection - // Not a high latency link + // First active direct USB connection + // Any active non high latency link + // An active high latency link // Any link #ifndef NO_SERIAL_LINK - // Search for direct usb connection + // Search for an active direct usb connection for (int i=0; i<_links.count(); i++) { LinkInterface* link = _links[i]; SerialLink* pSerialLink = qobject_cast(link); @@ -1549,7 +1779,7 @@ void Vehicle::_updatePriorityLink(void) if (config) { SerialConfiguration* pSerialConfig = qobject_cast(config); if (pSerialConfig && pSerialConfig->usbDirect()) { - if (_priorityLink.data() != link) { + if (_priorityLink.data() != link && link->link_active(_id)) { newPriorityLink = link; break; } @@ -1561,10 +1791,21 @@ void Vehicle::_updatePriorityLink(void) #endif if (!newPriorityLink) { - // Search for non-high latency link + // Search for an active non-high latency link for (int i=0; i<_links.count(); i++) { LinkInterface* link = _links[i]; - if (!link->highLatency()) { + if (!link->highLatency() && link->link_active(_id)) { + newPriorityLink = link; + break; + } + } + } + + if (!newPriorityLink) { + // Search for an active high latency link + for (int i=0; i<_links.count(); i++) { + LinkInterface* link = _links[i]; + if (link->highLatency() && link->link_active(_id)) { newPriorityLink = link; break; } @@ -1576,35 +1817,19 @@ void Vehicle::_updatePriorityLink(void) newPriorityLink = _links[0]; } - _priorityLink = _toolbox->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink); - _updateHighLatencyLink(); -} + if (_priorityLink.data() != newPriorityLink) { + if (_priorityLink) { + qgcApp()->showMessage((tr("switch to %2 as priority link")).arg(newPriorityLink->getName())); + } + _priorityLink = _toolbox->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink); -void Vehicle::_updateAttitude(UASInterface*, double roll, double pitch, double yaw, quint64) -{ - if (qIsInf(roll)) { - _rollFact.setRawValue(0); - } else { - _rollFact.setRawValue(roll * (180.0 / M_PI)); - } - if (qIsInf(pitch)) { - _pitchFact.setRawValue(0); - } else { - _pitchFact.setRawValue(pitch * (180.0 / M_PI)); - } - if (qIsInf(yaw)) { - _headingFact.setRawValue(0); - } else { - yaw = yaw * (180.0 / M_PI); - if (yaw < 0.0) yaw += 360.0; - // truncate to integer so widget never displays 360 - _headingFact.setRawValue(trunc(yaw)); - } -} + _updateHighLatencyLink(sendCommand); -void Vehicle::_updateAttitude(UASInterface* uas, int, double roll, double pitch, double yaw, quint64 timestamp) -{ - _updateAttitude(uas, roll, pitch, yaw, timestamp); + emit priorityLinkNameChanged(_priorityLink->getName()); + if (updateActive) { + _linkActiveChanged(_priorityLink.data(), _priorityLink->link_active(_id), _id); + } + } } int Vehicle::motorCount(void) @@ -1638,19 +1863,6 @@ bool Vehicle::xConfigMotors(void) return _firmwarePlugin->multiRotorXConfig(this); } -/* - * Internal - */ - -QString Vehicle::getMavIconColor() -{ - // TODO: Not using because not only the colors are ghastly, it doesn't respect dark/light palette - if(_mav) - return _mav->getColor().name(); - else - return QString("black"); -} - QString Vehicle::formatedMessages() { QString messages; @@ -1782,6 +1994,7 @@ void Vehicle::_loadSettings(void) // Joystick enabled is a global setting so first make sure there are any joysticks connected if (_toolbox->joystickManager()->joysticks().count()) { setJoystickEnabled(settings.value(_joystickEnabledSettingsKey, false).toBool()); + _startJoystick(true); } } @@ -1834,7 +2047,6 @@ bool Vehicle::joystickEnabled(void) void Vehicle::setJoystickEnabled(bool enabled) { _joystickEnabled = enabled; - _startJoystick(_joystickEnabled); _saveSettings(); emit joystickEnabledChanged(_joystickEnabled); } @@ -1844,9 +2056,7 @@ void Vehicle::_startJoystick(bool start) Joystick* joystick = _joystickManager->activeJoystick(); if (joystick) { if (start) { - if (_joystickEnabled) { - joystick->startPolling(this); - } + joystick->startPolling(this); } else { joystick->stopPolling(); } @@ -1862,6 +2072,7 @@ void Vehicle::setActive(bool active) { if (_active != active) { _active = active; + _startJoystick(false); emit activeChanged(_active); } } @@ -1920,6 +2131,53 @@ void Vehicle::setFlightMode(const QString& flightMode) } } +QString Vehicle::priorityLinkName(void) const +{ + if (_priorityLink) { + return _priorityLink->getName(); + } + + return "none"; +} + +QVariantList Vehicle::links(void) const { + QVariantList ret; + + foreach( const auto &item, _links ) + ret << QVariant::fromValue(item); + + return ret; +} + +void Vehicle::setPriorityLinkByName(const QString& priorityLinkName) +{ + if (!_priorityLink) { + return; + } + + if (priorityLinkName == _priorityLink->getName()) { + // The link did not change + return; + } + + LinkInterface* newPriorityLink = NULL; + + + for (int i=0; i<_links.count(); i++) { + if (_links[i]->getName() == priorityLinkName) { + newPriorityLink = _links[i]; + } + } + + if (newPriorityLink) { + _priorityLinkCommanded = true; + _priorityLink = _toolbox->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink); + _updateHighLatencyLink(true); + emit priorityLinkNameChanged(_priorityLink->getName()); + _linkActiveChanged(_priorityLink.data(), _priorityLink->link_active(_id), _id); + } +} + bool Vehicle::hilMode(void) { return _base_mode & MAV_MODE_FLAG_HIL_ENABLED; @@ -2132,17 +2390,38 @@ void Vehicle::_rallyPointLoadComplete(void) void Vehicle::_parametersReady(bool parametersReady) { + // Try to set current unix time to the vehicle + _sendQGCTimeToVehicle(); + // Send time twice, more likely to get to the vehicle on a noisy link + _sendQGCTimeToVehicle(); if (parametersReady) { _setupAutoDisarmSignalling(); _startPlanRequest(); } } +void Vehicle::_sendQGCTimeToVehicle(void) +{ + mavlink_message_t msg; + mavlink_system_time_t cmd; + + // Timestamp of the master clock in microseconds since UNIX epoch. + cmd.time_unix_usec = QDateTime::currentDateTime().currentMSecsSinceEpoch()*1000; + // Timestamp of the component clock since boot time in milliseconds (Not necessary). + cmd.time_boot_ms = 0; + mavlink_msg_system_time_encode_chan(_mavlink->getSystemId(), + _mavlink->getComponentId(), + priorityLink()->mavlinkChannel(), + &msg, + &cmd); + + sendMessageOnLink(priorityLink(), msg); +} + void Vehicle::disconnectInactiveVehicle(void) { // Vehicle is no longer communicating with us, disconnect all links - LinkManager* linkMgr = _toolbox->linkManager(); for (int i=0; i<_links.count(); i++) { // FIXME: This linkInUse check is a hack fix for multiple vehicles on the same link. @@ -2151,6 +2430,7 @@ void Vehicle::disconnectInactiveVehicle(void) linkMgr->disconnectLink(_links[i]); } } + emit linksChanged(); } void Vehicle::_imageReady(UASInterface*) @@ -2198,43 +2478,62 @@ void Vehicle::setConnectionLostEnabled(bool connectionLostEnabled) } } -void Vehicle::_connectionLostTimeout(void) +void Vehicle::_linkActiveChanged(LinkInterface *link, bool active, int vehicleID) { - if (highLatencyLink()) { - // No connection timeout on high latency links + // only continue if the vehicle id is correct + if (vehicleID != _id) { return; } - if (_connectionLostEnabled && !_connectionLost) { - _connectionLost = true; - _heardFrom = false; - _maxProtoVersion = 0; - emit connectionLostChanged(true); - _say(QString(tr("%1 communication lost")).arg(_vehicleIdSpeech())); - if (_autoDisconnect) { + emit linksPropertiesChanged(); - // Reset link state - for (int i = 0; i < _links.length(); i++) { - _mavlink->resetMetadataForLink(_links.at(i)); + if (link == _priorityLink) { + if (active && _connectionLost) { + // communication to priority link regained + _connectionLost = false; + emit connectionLostChanged(false); + qgcApp()->showMessage((tr("%1 communication to %2 link %3 regained")).arg(_vehicleIdSpeech()).arg((_links.count() > 1) ? "priority" : "").arg(link->getName())); + + if (_priorityLink->highLatency()) { + _setMaxProtoVersion(100); + } else { + // Re-negotiate protocol version for the link + sendMavCommand(MAV_COMP_ID_ALL, // Don't know default component id yet. + MAV_CMD_REQUEST_PROTOCOL_VERSION, + false, // No error shown if fails + 1); // Request protocol version } - disconnectInactiveVehicle(); - } - } -} -void Vehicle::_connectionActive(void) -{ - _connectionLostTimer.start(); - if (_connectionLost) { - _connectionLost = false; - emit connectionLostChanged(false); - _say(QString(tr("%1 communication regained")).arg(_vehicleIdSpeech())); + } else if (!active && !_connectionLost) { + // communication to priority link lost + qgcApp()->showMessage((tr("%1 communication to %2 link %3 lost")).arg(_vehicleIdSpeech()).arg((_links.count() > 1) ? "priority" : "").arg(link->getName())); - // Re-negotiate protocol version for the link - sendMavCommand(MAV_COMP_ID_ALL, // Don't know default component id yet. - MAV_CMD_REQUEST_PROTOCOL_VERSION, - false, // No error shown if fails - 1); // Request protocol version + _updatePriorityLink(false /* updateActive */, true /* sendCommand */); + + if (link == _priorityLink) { + _say(QString(tr("%1 communication lost")).arg(_vehicleIdSpeech())); + qgcApp()->showMessage((tr("%1 communication lost")).arg(_vehicleIdSpeech())); + + if (_connectionLostEnabled) { + _connectionLost = true; + _heardFrom = false; + _maxProtoVersion = 0; + emit connectionLostChanged(true); + + if (_autoDisconnect) { + // Reset link state + for (int i = 0; i < _links.length(); i++) { + _mavlink->resetMetadataForLink(_links.at(i)); + } + + disconnectInactiveVehicle(); + } + } + } + } + } else { + qgcApp()->showMessage((tr("%1 communication to auxiliary link %2 %3")).arg(_vehicleIdSpeech()).arg(link->getName()).arg(active ? "regained" : "lost")); + _updatePriorityLink(false /* updateActive */, true /* sendCommand */); } } @@ -2293,6 +2592,11 @@ bool Vehicle::supportsMotorInterference(void) const return _firmwarePlugin->supportsMotorInterference(); } +bool Vehicle::supportsTerrainFrame(void) const +{ + return _firmwarePlugin->supportsTerrainFrame(); +} + QString Vehicle::vehicleTypeName() const { static QMap typeNames = { { MAV_TYPE_GENERIC, tr("Generic micro air vehicle" )}, @@ -2408,7 +2712,6 @@ void Vehicle::guidedModeTakeoff(double altitudeRelative) qgcApp()->showMessage(guided_mode_not_supported_by_vehicle); return; } - setGuidedMode(true); _firmwarePlugin->guidedModeTakeoff(this, altitudeRelative); } @@ -3105,12 +3408,23 @@ void Vehicle::_vehicleParamLoaded(bool ready) } } -void Vehicle::_updateHighLatencyLink(void) +void Vehicle::_updateHighLatencyLink(bool sendCommand) { + if (!_priorityLink) { + return; + } + if (_priorityLink->highLatency() != _highLatencyLink) { _highLatencyLink = _priorityLink->highLatency(); _mavCommandAckTimer.setInterval(_highLatencyLink ? _mavCommandAckTimeoutMSecsHighLatency : _mavCommandAckTimeoutMSecs); emit highLatencyLinkChanged(_highLatencyLink); + + if (sendCommand) { + sendMavCommand(defaultComponentId(), + MAV_CMD_CONTROL_HIGH_LATENCY, + true, + _highLatencyLink ? 1.0f : 0.0f); // request start/stop transmitting over high latency telemetry + } } } @@ -3124,6 +3438,8 @@ const char* VehicleBatteryFactGroup::_currentFactName = "cur const char* VehicleBatteryFactGroup::_temperatureFactName = "temperature"; const char* VehicleBatteryFactGroup::_cellCountFactName = "cellCount"; const char* VehicleBatteryFactGroup::_instantPowerFactName = "instantPower"; +const char* VehicleBatteryFactGroup::_timeRemainingFactName = "timeRemaining"; +const char* VehicleBatteryFactGroup::_chargeStateFactName = "chargeState"; const char* VehicleBatteryFactGroup::_settingsGroup = "Vehicle.battery"; @@ -3144,6 +3460,8 @@ VehicleBatteryFactGroup::VehicleBatteryFactGroup(QObject* parent) , _temperatureFact (0, _temperatureFactName, FactMetaData::valueTypeDouble) , _cellCountFact (0, _cellCountFactName, FactMetaData::valueTypeInt32) , _instantPowerFact (0, _instantPowerFactName, FactMetaData::valueTypeFloat) + , _timeRemainingFact (0, _timeRemainingFactName, FactMetaData::valueTypeInt32) + , _chargeStateFact (0, _chargeStateFactName, FactMetaData::valueTypeUint8) { _addFact(&_voltageFact, _voltageFactName); _addFact(&_percentRemainingFact, _percentRemainingFactName); @@ -3152,6 +3470,8 @@ VehicleBatteryFactGroup::VehicleBatteryFactGroup(QObject* parent) _addFact(&_temperatureFact, _temperatureFactName); _addFact(&_cellCountFact, _cellCountFactName); _addFact(&_instantPowerFact, _instantPowerFactName); + _addFact(&_timeRemainingFact, _timeRemainingFactName); + _addFact(&_chargeStateFact, _chargeStateFactName); // Start out as not available _voltageFact.setRawValue (_voltageUnavailable); @@ -3212,7 +3532,6 @@ VehicleVibrationFactGroup::VehicleVibrationFactGroup(QObject* parent) _zAxisFact.setRawValue(std::numeric_limits::quiet_NaN()); } - const char* VehicleTemperatureFactGroup::_temperature1FactName = "temperature1"; const char* VehicleTemperatureFactGroup::_temperature2FactName = "temperature2"; const char* VehicleTemperatureFactGroup::_temperature3FactName = "temperature3"; @@ -3256,3 +3575,84 @@ void VehicleClockFactGroup::_updateAllValues(void) FactGroup::_updateAllValues(); } + +const char* VehicleSetpointFactGroup::_rollFactName = "roll"; +const char* VehicleSetpointFactGroup::_pitchFactName = "pitch"; +const char* VehicleSetpointFactGroup::_yawFactName = "yaw"; +const char* VehicleSetpointFactGroup::_rollRateFactName = "rollRate"; +const char* VehicleSetpointFactGroup::_pitchRateFactName = "pitchRate"; +const char* VehicleSetpointFactGroup::_yawRateFactName = "yawRate"; + +VehicleSetpointFactGroup::VehicleSetpointFactGroup(QObject* parent) + : FactGroup (1000, ":/json/Vehicle/SetpointFact.json", parent) + , _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble) + , _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble) + , _yawFact (0, _yawFactName, FactMetaData::valueTypeDouble) + , _rollRateFact (0, _rollRateFactName, FactMetaData::valueTypeDouble) + , _pitchRateFact(0, _pitchRateFactName, FactMetaData::valueTypeDouble) + , _yawRateFact (0, _yawRateFactName, FactMetaData::valueTypeDouble) +{ + _addFact(&_rollFact, _rollFactName); + _addFact(&_pitchFact, _pitchFactName); + _addFact(&_yawFact, _yawFactName); + _addFact(&_rollRateFact, _rollRateFactName); + _addFact(&_pitchRateFact, _pitchRateFactName); + _addFact(&_yawRateFact, _yawRateFactName); + + // Start out as not available "--.--" + _rollFact.setRawValue(std::numeric_limits::quiet_NaN()); + _pitchFact.setRawValue(std::numeric_limits::quiet_NaN()); + _yawFact.setRawValue(std::numeric_limits::quiet_NaN()); + _rollRateFact.setRawValue(std::numeric_limits::quiet_NaN()); + _pitchRateFact.setRawValue(std::numeric_limits::quiet_NaN()); + _yawRateFact.setRawValue(std::numeric_limits::quiet_NaN()); +} + +const char* VehicleDistanceSensorFactGroup::_rotationNoneFactName = "rotationNone"; +const char* VehicleDistanceSensorFactGroup::_rotationYaw45FactName = "rotationYaw45"; +const char* VehicleDistanceSensorFactGroup::_rotationYaw90FactName = "rotationYaw90"; +const char* VehicleDistanceSensorFactGroup::_rotationYaw135FactName = "rotationYaw135"; +const char* VehicleDistanceSensorFactGroup::_rotationYaw180FactName = "rotationYaw180"; +const char* VehicleDistanceSensorFactGroup::_rotationYaw225FactName = "rotationYaw225"; +const char* VehicleDistanceSensorFactGroup::_rotationYaw270FactName = "rotationYaw270"; +const char* VehicleDistanceSensorFactGroup::_rotationYaw315FactName = "rotationYaw315"; +const char* VehicleDistanceSensorFactGroup::_rotationPitch90FactName = "rotationPitch90"; +const char* VehicleDistanceSensorFactGroup::_rotationPitch270FactName = "rotationPitch270"; + +VehicleDistanceSensorFactGroup::VehicleDistanceSensorFactGroup(QObject* parent) + : FactGroup (1000, ":/json/Vehicle/DistanceSensorFact.json", parent) + , _rotationNoneFact (0, _rotationNoneFactName, FactMetaData::valueTypeDouble) + , _rotationYaw45Fact (0, _rotationYaw45FactName, FactMetaData::valueTypeDouble) + , _rotationYaw90Fact (0, _rotationYaw90FactName, FactMetaData::valueTypeDouble) + , _rotationYaw135Fact (0, _rotationYaw135FactName, FactMetaData::valueTypeDouble) + , _rotationYaw180Fact (0, _rotationYaw180FactName, FactMetaData::valueTypeDouble) + , _rotationYaw225Fact (0, _rotationYaw225FactName, FactMetaData::valueTypeDouble) + , _rotationYaw270Fact (0, _rotationYaw270FactName, FactMetaData::valueTypeDouble) + , _rotationYaw315Fact (0, _rotationYaw315FactName, FactMetaData::valueTypeDouble) + , _rotationPitch90Fact (0, _rotationPitch90FactName, FactMetaData::valueTypeDouble) + , _rotationPitch270Fact (0, _rotationPitch270FactName, FactMetaData::valueTypeDouble) + , _idSet (false) + , _id (0) +{ + _addFact(&_rotationNoneFact, _rotationNoneFactName); + _addFact(&_rotationYaw45Fact, _rotationYaw45FactName); + _addFact(&_rotationYaw90Fact, _rotationYaw90FactName); + _addFact(&_rotationYaw135Fact, _rotationYaw135FactName); + _addFact(&_rotationYaw180Fact, _rotationYaw180FactName); + _addFact(&_rotationYaw225Fact, _rotationYaw225FactName); + _addFact(&_rotationYaw270Fact, _rotationYaw270FactName); + _addFact(&_rotationYaw315Fact, _rotationYaw315FactName); + _addFact(&_rotationPitch90Fact, _rotationPitch90FactName); + _addFact(&_rotationPitch270Fact, _rotationPitch270FactName); + + // Start out as not available "--.--" + _rotationNoneFact.setRawValue(std::numeric_limits::quiet_NaN()); + _rotationYaw45Fact.setRawValue(std::numeric_limits::quiet_NaN()); + _rotationYaw135Fact.setRawValue(std::numeric_limits::quiet_NaN()); + _rotationYaw90Fact.setRawValue(std::numeric_limits::quiet_NaN()); + _rotationYaw180Fact.setRawValue(std::numeric_limits::quiet_NaN()); + _rotationYaw225Fact.setRawValue(std::numeric_limits::quiet_NaN()); + _rotationYaw270Fact.setRawValue(std::numeric_limits::quiet_NaN()); + _rotationPitch90Fact.setRawValue(std::numeric_limits::quiet_NaN()); + _rotationPitch270Fact.setRawValue(std::numeric_limits::quiet_NaN()); +} diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index ba867511334781bb2cd8a4251c59709ebc10fc23..e2b8a646bb7bf6f02e4134be03e100aa0b3e298e 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -10,8 +10,8 @@ #pragma once #include +#include #include -#include #include "FactGroup.h" #include "LinkInterface.h" @@ -40,6 +40,104 @@ Q_DECLARE_LOGGING_CATEGORY(VehicleLog) class Vehicle; +class VehicleDistanceSensorFactGroup : public FactGroup +{ + Q_OBJECT + +public: + VehicleDistanceSensorFactGroup(QObject* parent = NULL); + + Q_PROPERTY(Fact* rotationNone READ rotationNone CONSTANT) + Q_PROPERTY(Fact* rotationYaw45 READ rotationYaw45 CONSTANT) + Q_PROPERTY(Fact* rotationYaw90 READ rotationYaw90 CONSTANT) + Q_PROPERTY(Fact* rotationYaw135 READ rotationYaw135 CONSTANT) + Q_PROPERTY(Fact* rotationYaw180 READ rotationYaw180 CONSTANT) + Q_PROPERTY(Fact* rotationYaw225 READ rotationYaw225 CONSTANT) + Q_PROPERTY(Fact* rotationYaw270 READ rotationYaw270 CONSTANT) + Q_PROPERTY(Fact* rotationYaw315 READ rotationYaw315 CONSTANT) + Q_PROPERTY(Fact* rotationPitch90 READ rotationPitch90 CONSTANT) + Q_PROPERTY(Fact* rotationPitch270 READ rotationPitch270 CONSTANT) + + Fact* rotationNone (void) { return &_rotationNoneFact; } + Fact* rotationYaw45 (void) { return &_rotationYaw45Fact; } + Fact* rotationYaw90 (void) { return &_rotationYaw90Fact; } + Fact* rotationYaw135 (void) { return &_rotationYaw90Fact; } + Fact* rotationYaw180 (void) { return &_rotationYaw180Fact; } + Fact* rotationYaw225 (void) { return &_rotationYaw180Fact; } + Fact* rotationYaw270 (void) { return &_rotationYaw270Fact; } + Fact* rotationYaw315 (void) { return &_rotationYaw315Fact; } + Fact* rotationPitch90 (void) { return &_rotationPitch90Fact; } + Fact* rotationPitch270 (void) { return &_rotationPitch270Fact; } + + bool idSet(void) { return _idSet; } + void setIdSet(bool idSet) { _idSet = idSet; } + uint8_t id(void) { return _id; } + void setId(uint8_t id) { _id = id; } + + static const char* _rotationNoneFactName; + static const char* _rotationYaw45FactName; + static const char* _rotationYaw90FactName; + static const char* _rotationYaw135FactName; + static const char* _rotationYaw180FactName; + static const char* _rotationYaw225FactName; + static const char* _rotationYaw270FactName; + static const char* _rotationYaw315FactName; + static const char* _rotationPitch90FactName; + static const char* _rotationPitch270FactName; + +private: + Fact _rotationNoneFact; + Fact _rotationYaw45Fact; + Fact _rotationYaw90Fact; + Fact _rotationYaw135Fact; + Fact _rotationYaw180Fact; + Fact _rotationYaw225Fact; + Fact _rotationYaw270Fact; + Fact _rotationYaw315Fact; + Fact _rotationPitch90Fact; + Fact _rotationPitch270Fact; + + bool _idSet; // true: _id is set to seen sensor id + uint8_t _id; // The id for the sensor being tracked. Current support for only a single sensor. +}; + +class VehicleSetpointFactGroup : public FactGroup +{ + Q_OBJECT + +public: + VehicleSetpointFactGroup(QObject* parent = NULL); + + Q_PROPERTY(Fact* roll READ roll CONSTANT) + Q_PROPERTY(Fact* pitch READ pitch CONSTANT) + Q_PROPERTY(Fact* yaw READ yaw CONSTANT) + Q_PROPERTY(Fact* rollRate READ rollRate CONSTANT) + Q_PROPERTY(Fact* pitchRate READ pitchRate CONSTANT) + Q_PROPERTY(Fact* yawRate READ yawRate CONSTANT) + + Fact* roll (void) { return &_rollFact; } + Fact* pitch (void) { return &_pitchFact; } + Fact* yaw (void) { return &_yawFact; } + Fact* rollRate (void) { return &_rollRateFact; } + Fact* pitchRate (void) { return &_pitchRateFact; } + Fact* yawRate (void) { return &_yawRateFact; } + + static const char* _rollFactName; + static const char* _pitchFactName; + static const char* _yawFactName; + static const char* _rollRateFactName; + static const char* _pitchRateFactName; + static const char* _yawRateFactName; + +private: + Fact _rollFact; + Fact _pitchFact; + Fact _yawFact; + Fact _rollRateFact; + Fact _pitchRateFact; + Fact _yawRateFact; +}; + class VehicleVibrationFactGroup : public FactGroup { Q_OBJECT @@ -157,6 +255,8 @@ public: Q_PROPERTY(Fact* temperature READ temperature CONSTANT) Q_PROPERTY(Fact* cellCount READ cellCount CONSTANT) Q_PROPERTY(Fact* instantPower READ instantPower CONSTANT) + Q_PROPERTY(Fact* timeRemaining READ timeRemaining CONSTANT) + Q_PROPERTY(Fact* chargeState READ chargeState CONSTANT) Fact* voltage (void) { return &_voltageFact; } Fact* percentRemaining (void) { return &_percentRemainingFact; } @@ -165,7 +265,8 @@ public: Fact* temperature (void) { return &_temperatureFact; } Fact* cellCount (void) { return &_cellCountFact; } Fact* instantPower (void) { return &_instantPowerFact; } - + Fact* timeRemaining (void) { return &_timeRemainingFact; } + Fact* chargeState (void) { return &_chargeStateFact; } static const char* _voltageFactName; static const char* _percentRemainingFactName; @@ -174,6 +275,8 @@ public: static const char* _temperatureFactName; static const char* _cellCountFactName; static const char* _instantPowerFactName; + static const char* _timeRemainingFactName; + static const char* _chargeStateFactName; static const char* _settingsGroup; @@ -193,6 +296,8 @@ private: Fact _temperatureFact; Fact _cellCountFact; Fact _instantPowerFact; + Fact _timeRemainingFact; + Fact _chargeStateFact; }; class VehicleTemperatureFactGroup : public FactGroup @@ -271,6 +376,37 @@ public: ~Vehicle(); + /// Sensor bits from sensors*Bits properties + enum MavlinkSysStatus { + SysStatusSensor3dGyro = MAV_SYS_STATUS_SENSOR_3D_GYRO, + SysStatusSensor3dAccel = MAV_SYS_STATUS_SENSOR_3D_ACCEL, + SysStatusSensor3dMag = MAV_SYS_STATUS_SENSOR_3D_MAG, + SysStatusSensorAbsolutePressure = MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE, + SysStatusSensorDifferentialPressure = MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE, + SysStatusSensorGPS = MAV_SYS_STATUS_SENSOR_GPS, + SysStatusSensorOpticalFlow = MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW, + SysStatusSensorVisionPosition = MAV_SYS_STATUS_SENSOR_VISION_POSITION, + SysStatusSensorLaserPosition = MAV_SYS_STATUS_SENSOR_LASER_POSITION, + SysStatusSensorExternalGroundTruth = MAV_SYS_STATUS_SENSOR_EXTERNAL_GROUND_TRUTH, + SysStatusSensorAngularRateControl = MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL, + SysStatusSensorAttitudeStabilization = MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION, + SysStatusSensorYawPosition = MAV_SYS_STATUS_SENSOR_YAW_POSITION, + SysStatusSensorZAltitudeControl = MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL, + SysStatusSensorXYPositionControl = MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL, + SysStatusSensorMotorOutputs = MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS, + SysStatusSensorRCReceiver = MAV_SYS_STATUS_SENSOR_RC_RECEIVER, + SysStatusSensor3dGyro2 = MAV_SYS_STATUS_SENSOR_3D_GYRO2, + SysStatusSensor3dAccel2 = MAV_SYS_STATUS_SENSOR_3D_ACCEL2, + SysStatusSensor3dMag2 = MAV_SYS_STATUS_SENSOR_3D_MAG2, + SysStatusSensorGeoFence = MAV_SYS_STATUS_GEOFENCE, + SysStatusSensorAHRS = MAV_SYS_STATUS_AHRS, + SysStatusSensorTerrain = MAV_SYS_STATUS_TERRAIN, + SysStatusSensorReverseMotor = MAV_SYS_STATUS_REVERSE_MOTOR, + SysStatusSensorLogging = MAV_SYS_STATUS_LOGGING, + SysStatusSensorBattery = MAV_SYS_STATUS_SENSOR_BATTERY, + }; + Q_ENUM(MavlinkSysStatus) + Q_PROPERTY(int id READ id CONSTANT) Q_PROPERTY(AutoPilotPlugin* autopilot MEMBER _autopilotPlugin CONSTANT) Q_PROPERTY(QGeoCoordinate coordinate READ coordinate NOTIFY coordinateChanged) @@ -278,7 +414,7 @@ public: Q_PROPERTY(bool armed READ armed WRITE setArmed NOTIFY armedChanged) Q_PROPERTY(bool autoDisarm READ autoDisarm NOTIFY autoDisarmChanged) Q_PROPERTY(bool flightModeSetAvailable READ flightModeSetAvailable CONSTANT) - Q_PROPERTY(QStringList flightModes READ flightModes CONSTANT) + Q_PROPERTY(QStringList flightModes READ flightModes NOTIFY flightModesChanged) Q_PROPERTY(QString flightMode READ flightMode WRITE setFlightMode NOTIFY flightModeChanged) Q_PROPERTY(bool hilMode READ hilMode WRITE setHilMode NOTIFY hilModeChanged) Q_PROPERTY(QmlObjectListModel* trajectoryPoints READ trajectoryPoints CONSTANT) @@ -328,6 +464,10 @@ public: Q_PROPERTY(QString brandImageIndoor READ brandImageIndoor NOTIFY firmwareTypeChanged) Q_PROPERTY(QString brandImageOutdoor READ brandImageOutdoor NOTIFY firmwareTypeChanged) Q_PROPERTY(QStringList unhealthySensors READ unhealthySensors NOTIFY unhealthySensorsChanged) + Q_PROPERTY(int sensorsPresentBits READ sensorsPresentBits NOTIFY sensorsPresentBitsChanged) + Q_PROPERTY(int sensorsEnabledBits READ sensorsEnabledBits NOTIFY sensorsEnabledBitsChanged) + Q_PROPERTY(int sensorsHealthBits READ sensorsHealthBits NOTIFY sensorsHealthBitsChanged) + Q_PROPERTY(int sensorsUnhealthyBits READ sensorsUnhealthyBits NOTIFY sensorsUnhealthyBitsChanged) ///< Combination of enabled and health Q_PROPERTY(QString missionFlightMode READ missionFlightMode CONSTANT) Q_PROPERTY(QString pauseFlightMode READ pauseFlightMode CONSTANT) Q_PROPERTY(QString rtlFlightMode READ rtlFlightMode CONSTANT) @@ -353,6 +493,10 @@ public: Q_PROPERTY(QString hobbsMeter READ hobbsMeter NOTIFY hobbsMeterChanged) Q_PROPERTY(bool vtolInFwdFlight READ vtolInFwdFlight WRITE setVtolInFwdFlight NOTIFY vtolInFwdFlightChanged) Q_PROPERTY(bool highLatencyLink READ highLatencyLink NOTIFY highLatencyLinkChanged) + Q_PROPERTY(bool supportsTerrainFrame READ supportsTerrainFrame NOTIFY firmwareTypeChanged) + Q_PROPERTY(QString priorityLinkName READ priorityLinkName WRITE setPriorityLinkByName NOTIFY priorityLinkNameChanged) + Q_PROPERTY(QVariantList links READ links NOTIFY linksChanged) + Q_PROPERTY(LinkInterface* priorityLink READ priorityLink NOTIFY priorityLinkNameChanged) // Vehicle state used for guided control Q_PROPERTY(bool flying READ flying NOTIFY flyingChanged) ///< Vehicle is flying @@ -361,7 +505,7 @@ public: Q_PROPERTY(bool guidedModeSupported READ guidedModeSupported CONSTANT) ///< Guided mode commands are supported by this vehicle Q_PROPERTY(bool pauseVehicleSupported READ pauseVehicleSupported CONSTANT) ///< Pause vehicle command is supported Q_PROPERTY(bool orbitModeSupported READ orbitModeSupported CONSTANT) ///< Orbit mode is supported by this vehicle - Q_PROPERTY(bool takeoffVehicleSupported READ takeoffVehicleSupported CONSTANT) ///< Guided takeoff supported + Q_PROPERTY(bool takeoffVehicleSupported READ takeoffVehicleSupported CONSTANT) ///< Guided takeoff supported Q_PROPERTY(ParameterManager* parameterManager READ parameterManager CONSTANT) @@ -370,6 +514,9 @@ public: Q_PROPERTY(Fact* roll READ roll CONSTANT) Q_PROPERTY(Fact* pitch READ pitch CONSTANT) Q_PROPERTY(Fact* heading READ heading CONSTANT) + Q_PROPERTY(Fact* rollRate READ rollRate CONSTANT) + Q_PROPERTY(Fact* pitchRate READ pitchRate CONSTANT) + Q_PROPERTY(Fact* yawRate READ yawRate CONSTANT) Q_PROPERTY(Fact* groundSpeed READ groundSpeed CONSTANT) Q_PROPERTY(Fact* airSpeed READ airSpeed CONSTANT) Q_PROPERTY(Fact* climbRate READ climbRate CONSTANT) @@ -380,11 +527,13 @@ public: Q_PROPERTY(Fact* hobbs READ hobbs CONSTANT) Q_PROPERTY(FactGroup* gps READ gpsFactGroup CONSTANT) - Q_PROPERTY(FactGroup* battery READ batteryFactGroup CONSTANT) + Q_PROPERTY(FactGroup* battery READ battery1FactGroup CONSTANT) + Q_PROPERTY(FactGroup* battery2 READ battery2FactGroup CONSTANT) Q_PROPERTY(FactGroup* wind READ windFactGroup CONSTANT) Q_PROPERTY(FactGroup* vibration READ vibrationFactGroup CONSTANT) Q_PROPERTY(FactGroup* temperature READ temperatureFactGroup CONSTANT) Q_PROPERTY(FactGroup* clock READ clockFactGroup CONSTANT) + Q_PROPERTY(FactGroup* setpoint READ setpointFactGroup CONSTANT) Q_PROPERTY(int firmwareMajorVersion READ firmwareMajorVersion NOTIFY firmwareVersionChanged) Q_PROPERTY(int firmwareMinorVersion READ firmwareMinorVersion NOTIFY firmwareVersionChanged) @@ -407,8 +556,6 @@ public: /// @return -1: reserver all buttons, >0 number of buttons to reserve Q_PROPERTY(int manualControlReservedButtonCount READ manualControlReservedButtonCount CONSTANT) - Q_INVOKABLE QString getMavIconColor(); - // Called when the message drop-down is invoked to clear current count Q_INVOKABLE void resetMessages(); @@ -548,6 +695,10 @@ public: QString flightMode(void) const; void setFlightMode(const QString& flightMode); + QString priorityLinkName(void) const; + QVariantList links(void) const; + void setPriorityLinkByName(const QString& priorityLinkName); + bool hilMode(void); void setHilMode(bool hilMode); @@ -557,11 +708,12 @@ public: bool rover(void) const; bool sub(void) const; - bool supportsThrottleModeCenterZero(void) const; - bool supportsNegativeThrust(void) const; - bool supportsRadio(void) const; - bool supportsJSButton(void) const; - bool supportsMotorInterference(void) const; + bool supportsThrottleModeCenterZero (void) const; + bool supportsNegativeThrust (void) const; + bool supportsRadio (void) const; + bool supportsJSButton (void) const; + bool supportsMotorInterference (void) const; + bool supportsTerrainFrame (void) const; void setGuidedMode(bool guidedMode); @@ -622,6 +774,10 @@ public: QString brandImageIndoor () const; QString brandImageOutdoor () const; QStringList unhealthySensors () const; + int sensorsPresentBits () const { return _onboardControlSensorsPresent; } + int sensorsEnabledBits () const { return _onboardControlSensorsEnabled; } + int sensorsHealthBits () const { return _onboardControlSensorsHealth; } + int sensorsUnhealthyBits () const { return _onboardControlSensorsUnhealthy; } QString missionFlightMode () const; QString pauseFlightMode () const; QString rtlFlightMode () const; @@ -645,8 +801,11 @@ public: unsigned maxProtoVersion () const { return _maxProtoVersion; } Fact* roll (void) { return &_rollFact; } - Fact* heading (void) { return &_headingFact; } Fact* pitch (void) { return &_pitchFact; } + Fact* heading (void) { return &_headingFact; } + Fact* rollRate (void) { return &_rollRateFact; } + Fact* pitchRate (void) { return &_pitchRateFact; } + Fact* yawRate (void) { return &_yawRateFact; } Fact* airSpeed (void) { return &_airSpeedFact; } Fact* groundSpeed (void) { return &_groundSpeedFact; } Fact* climbRate (void) { return &_climbRateFact; } @@ -656,12 +815,15 @@ public: Fact* distanceToHome (void) { return &_distanceToHomeFact; } Fact* hobbs (void) { return &_hobbsFact; } - FactGroup* gpsFactGroup (void) { return &_gpsFactGroup; } - FactGroup* batteryFactGroup (void) { return &_batteryFactGroup; } - FactGroup* windFactGroup (void) { return &_windFactGroup; } - FactGroup* vibrationFactGroup (void) { return &_vibrationFactGroup; } - FactGroup* temperatureFactGroup (void) { return &_temperatureFactGroup; } - FactGroup* clockFactGroup (void) { return &_clockFactGroup; } + FactGroup* gpsFactGroup (void) { return &_gpsFactGroup; } + FactGroup* battery1FactGroup (void) { return &_battery1FactGroup; } + FactGroup* battery2FactGroup (void) { return &_battery2FactGroup; } + FactGroup* windFactGroup (void) { return &_windFactGroup; } + FactGroup* vibrationFactGroup (void) { return &_vibrationFactGroup; } + FactGroup* temperatureFactGroup (void) { return &_temperatureFactGroup; } + FactGroup* clockFactGroup (void) { return &_clockFactGroup; } + FactGroup* setpointFactGroup (void) { return &_setpointFactGroup; } + FactGroup* distanceSensorFactGroup (void) { return &_distanceSensorFactGroup; } void setConnectionLostEnabled(bool connectionLostEnabled); @@ -789,6 +951,9 @@ signals: void capabilityBitsChanged(uint64_t capabilityBits); void toolBarIndicatorsChanged(void); void highLatencyLinkChanged(bool highLatencyLink); + void priorityLinkNameChanged(const QString& priorityLinkName); + void linksChanged(void); + void linksPropertiesChanged(void); void messagesReceivedChanged (); void messagesSentChanged (); @@ -815,6 +980,11 @@ signals: void telemetryLNoiseChanged (int value); void telemetryRNoiseChanged (int value); void autoDisarmChanged (void); + void flightModesChanged (void); + void sensorsPresentBitsChanged (int sensorsPresentBits); + void sensorsEnabledBitsChanged (int sensorsEnabledBits); + void sensorsHealthBitsChanged (int sensorsHealthBits); + void sensorsUnhealthyBitsChanged(int sensorsUnhealthyBits); void firmwareVersionChanged(void); void firmwareCustomVersionChanged(void); @@ -865,17 +1035,12 @@ private slots: void _offlineVehicleTypeSettingChanged(QVariant value); void _offlineCruiseSpeedSettingChanged(QVariant value); void _offlineHoverSpeedSettingChanged(QVariant value); - void _updateHighLatencyLink(void); + void _updateHighLatencyLink(bool sendCommand = true); void _handleTextMessage (int newCount); void _handletextMessageReceived (UASMessage* message); - /** @brief Attitude from main autopilot / system state */ - void _updateAttitude (UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp); - /** @brief Attitude from one specific component / redundant autopilot */ - void _updateAttitude (UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 timestamp); /** @brief A new camera image has arrived */ void _imageReady (UASInterface* uas); - void _connectionLostTimeout(void); void _prearmErrorTimeout(void); void _missionLoadComplete(void); void _geoFenceLoadComplete(void); @@ -886,6 +1051,7 @@ private slots: void _updateDistanceToHome(void); void _updateHobbsMeter(void); void _vehicleParamLoaded(bool ready); + void _sendQGCTimeToVehicle(void); private: bool _containsLink(LinkInterface* link); @@ -893,6 +1059,7 @@ private: void _loadSettings(void); void _saveSettings(void); void _startJoystick(bool start); + void _handlePing(LinkInterface* link, mavlink_message_t& message); void _handleHomePosition(mavlink_message_t& message); void _handleHeartbeat(mavlink_message_t& message); void _handleRadioStatus(mavlink_message_t& message); @@ -916,6 +1083,11 @@ private: void _handleScaledPressure2(mavlink_message_t& message); void _handleScaledPressure3(mavlink_message_t& message); void _handleHighLatency2(mavlink_message_t& message); + void _handleAttitudeWorker(double rollRadians, double pitchRadians, double yawRadians); + void _handleAttitude(mavlink_message_t& message); + void _handleAttitudeQuaternion(mavlink_message_t& message); + void _handleAttitudeTarget(mavlink_message_t& message); + void _handleDistanceSensor(mavlink_message_t& message); // ArduPilot dialect messages #if !defined(NO_ARDUPILOT_DIALECT) void _handleCameraFeedback(const mavlink_message_t& message); @@ -928,18 +1100,20 @@ private: void _rallyPointManagerError(int errorCode, const QString& errorMsg); void _mapTrajectoryStart(void); void _mapTrajectoryStop(void); - void _connectionActive(void); + void _linkActiveChanged(LinkInterface* link, bool active, int vehicleID); void _say(const QString& text); QString _vehicleIdSpeech(void); void _handleMavlinkLoggingData(mavlink_message_t& message); void _handleMavlinkLoggingDataAcked(mavlink_message_t& message); void _ackMavlinkLogData(uint16_t sequence); void _sendNextQueuedMavCommand(void); - void _updatePriorityLink(void); + void _updatePriorityLink(bool updateActive, bool sendCommand); void _commonInit(void); void _startPlanRequest(void); void _setupAutoDisarmSignalling(void); void _setCapabilities(uint64_t capabilityBits); + void _updateArmed(bool armed); + bool _apmArmingNotRequired(void); int _id; ///< Mavlink system id int _defaultComponentId; @@ -1001,6 +1175,7 @@ private: bool _vehicleCapabilitiesKnown; uint64_t _capabilityBits; bool _highLatencyLink; + bool _receivingAttitudeQuaternion; QGCCameraManager* _cameras; @@ -1025,8 +1200,6 @@ private: // Lost connection handling bool _connectionLost; bool _connectionLostEnabled; - static const int _connectionLostTimeoutMSecs = 3500; // Signal connection lost after 3.5 seconds of missed heartbeat - QTimer _connectionLostTimer; bool _initialPlanRequestComplete; @@ -1100,12 +1273,16 @@ private: int _lastAnnouncedLowBatteryPercent; SharedLinkInterfacePointer _priorityLink; // We always keep a reference to the priority link to manage shutdown ordering + bool _priorityLinkCommanded; // FactGroup facts Fact _rollFact; Fact _pitchFact; Fact _headingFact; + Fact _rollRateFact; + Fact _pitchRateFact; + Fact _yawRateFact; Fact _groundSpeedFact; Fact _airSpeedFact; Fact _climbRateFact; @@ -1116,16 +1293,22 @@ private: Fact _distanceToHomeFact; Fact _hobbsFact; - VehicleGPSFactGroup _gpsFactGroup; - VehicleBatteryFactGroup _batteryFactGroup; - VehicleWindFactGroup _windFactGroup; - VehicleVibrationFactGroup _vibrationFactGroup; - VehicleTemperatureFactGroup _temperatureFactGroup; - VehicleClockFactGroup _clockFactGroup; + VehicleGPSFactGroup _gpsFactGroup; + VehicleBatteryFactGroup _battery1FactGroup; + VehicleBatteryFactGroup _battery2FactGroup; + VehicleWindFactGroup _windFactGroup; + VehicleVibrationFactGroup _vibrationFactGroup; + VehicleTemperatureFactGroup _temperatureFactGroup; + VehicleClockFactGroup _clockFactGroup; + VehicleSetpointFactGroup _setpointFactGroup; + VehicleDistanceSensorFactGroup _distanceSensorFactGroup; static const char* _rollFactName; static const char* _pitchFactName; static const char* _headingFactName; + static const char* _rollRateFactName; + static const char* _pitchRateFactName; + static const char* _yawRateFactName; static const char* _groundSpeedFactName; static const char* _airSpeedFactName; static const char* _climbRateFactName; @@ -1137,11 +1320,13 @@ private: static const char* _hobbsFactName; static const char* _gpsFactGroupName; - static const char* _batteryFactGroupName; + static const char* _battery1FactGroupName; + static const char* _battery2FactGroupName; static const char* _windFactGroupName; static const char* _vibrationFactGroupName; static const char* _temperatureFactGroupName; static const char* _clockFactGroupName; + static const char* _distanceSensorFactGroupName; static const int _vehicleUIUpdateRateMSecs = 100; diff --git a/src/Vehicle/VehicleFact.json b/src/Vehicle/VehicleFact.json index ae6d59c50df1eedad6ff82c6ecbaced37120cabe..b385571291d9761568ca574c5c22ede9a3b3334c 100644 --- a/src/Vehicle/VehicleFact.json +++ b/src/Vehicle/VehicleFact.json @@ -20,6 +20,27 @@ "decimalPlaces": 0, "units": "deg" }, +{ + "name": "rollRate", + "shortDescription": "Roll Rate", + "type": "double", + "decimalPlaces": 1, + "units": "deg/s" +}, +{ + "name": "pitchRate", + "shortDescription": "Pitch Rate", + "type": "double", + "decimalPlaces": 1, + "units": "deg/s" +}, +{ + "name": "yawRate", + "shortDescription": "Yaw Rate", + "type": "double", + "decimalPlaces": 1, + "units": "deg/s" +}, { "name": "groundSpeed", "shortDescription": "Ground Speed", diff --git a/src/VehicleSetup/FirmwareUpgrade.qml b/src/VehicleSetup/FirmwareUpgrade.qml index 646bfcce3218d25381242ea58b7e8234fb56ab27..046312f4967f4d72133e213e75a7fe1e76d895a2 100644 --- a/src/VehicleSetup/FirmwareUpgrade.qml +++ b/src/VehicleSetup/FirmwareUpgrade.qml @@ -56,7 +56,7 @@ SetupPage { readonly property int _defaultFimwareTypeAPM: 3 property var _defaultFirmwareFact: QGroundControl.settingsManager.appSettings.defaultFirmwareType - property bool _defaultFirmwareIsPX4: _defaultFirmwareFact.rawValue === _defaultFimwareTypePX4 + property bool _defaultFirmwareIsPX4: true property string firmwareWarningMessage property bool controllerCompleted: false @@ -134,6 +134,7 @@ SetupPage { // We can only start the board search when the Qml and Controller are completely done loading controller.startBoardSearch() } + _defaultFirmwareIsPX4 = _defaultFirmwareFact.rawValue === _defaultFimwareTypePX4 // we don't want this to be bound and change as radios are selected } Component { @@ -160,7 +161,8 @@ SetupPage { } else { versionString = controller.px4StableVersion } - px4FlightStack.text = qsTr("PX4 Flight Stack ") + versionString + px4FlightStackRadio1.text = qsTr("PX4 Flight Stack ") + versionString + px4FlightStackRadio2.text = qsTr("PX4 Flight Stack ") + versionString } Component.onCompleted: updatePX4VersionDisplay() @@ -168,18 +170,22 @@ SetupPage { function accept() { hideDialog() if (_singleFirmwareMode) { - controller.flashSingleFirmwareMode() + controller.flashSingleFirmwareMode(controller.selectedFirmwareType) } else { - var stack = apmFlightStack.checked ? FirmwareUpgradeController.AutoPilotStackAPM : FirmwareUpgradeController.AutoPilotStackPX4 - if (px4Flow) { - stack = FirmwareUpgradeController.PX4Flow - } - + var stack var firmwareType = firmwareVersionCombo.model.get(firmwareVersionCombo.currentIndex).firmwareType var vehicleType = FirmwareUpgradeController.DefaultVehicleFirmware - if (apmFlightStack.checked) { - vehicleType = controller.vehicleTypeFromVersionIndex(vehicleTypeSelectionCombo.currentIndex) + + if (px4Flow) { + stack = px4FlowTypeSelectionCombo.model.get(px4FlowTypeSelectionCombo.currentIndex).stackType + vehicleType = FirmwareUpgradeController.DefaultVehicleFirmware + } else { + stack = apmFlightStack.checked ? FirmwareUpgradeController.AutoPilotStackAPM : FirmwareUpgradeController.AutoPilotStackPX4 + if (apmFlightStack.checked) { + vehicleType = controller.vehicleTypeFromVersionIndex(vehicleTypeSelectionCombo.currentIndex) + } } + controller.flash(stack, firmwareType, vehicleType) } } @@ -214,6 +220,19 @@ SetupPage { } } + ListModel { + id: px4FlowFirmwareList + + ListElement { + text: qsTr("PX4 Pro") + stackType: FirmwareUpgradeController.PX4FlowPX4 + } + ListElement { + text: qsTr("ArduPilot") + stackType: FirmwareUpgradeController.PX4FlowAPM + } + } + ListModel { id: px4FlowTypeList @@ -249,7 +268,7 @@ SetupPage { wrapMode: Text.WordWrap text: _singleFirmwareMode ? _singleFirmwareLabel : (px4Flow ? _px4FlowLabel : _pixhawkLabel) - readonly property string _px4FlowLabel: qsTr("Detected PX4 Flow board. You can select from the following firmware:") + readonly property string _px4FlowLabel: qsTr("Detected PX4 Flow board. The firmware you use on the PX4 Flow must match the AutoPilot firmware type you are using on the vehicle:") readonly property string _pixhawkLabel: qsTr("Detected Pixhawk board. You can select from the following flight stacks:") readonly property string _singleFirmwareLabel: qsTr("Press Ok to upgrade your vehicle.") } @@ -265,19 +284,16 @@ SetupPage { firmwareVersionCombo.currentIndex = 0 } - Component.onCompleted: { - if (_defaultFirmwareIsPX4) { - px4FlightStack.checked = true - } else { - apmFlightStack.checked = true - } - } + // The following craziness of three radio buttons to represent two radio buttons is so that the + // order can be changed such that the default firmware button is always on the top QGCRadioButton { - id: px4FlightStack - exclusiveGroup: firmwareGroup + id: px4FlightStackRadio1 + exclusiveGroup: _defaultFirmwareIsPX4 ? firmwareGroup : null text: qsTr("PX4 Flight Stack ") - visible: !_singleFirmwareMode && !px4Flow + textBold: _defaultFirmwareIsPX4 + checked: _defaultFirmwareIsPX4 + visible: _defaultFirmwareIsPX4 && !_singleFirmwareMode && !px4Flow onClicked: { _defaultFirmwareFact.rawValue = _defaultFimwareTypePX4 @@ -289,6 +305,8 @@ SetupPage { id: apmFlightStack exclusiveGroup: firmwareGroup text: qsTr("ArduPilot Flight Stack") + textBold: !_defaultFirmwareIsPX4 + checked: !_defaultFirmwareIsPX4 visible: !_singleFirmwareMode && !px4Flow onClicked: { @@ -297,14 +315,35 @@ SetupPage { } } + QGCRadioButton { + id: px4FlightStackRadio2 + exclusiveGroup: _defaultFirmwareIsPX4 ? null : firmwareGroup + text: qsTr("PX4 Flight Stack ") + visible: !_defaultFirmwareIsPX4 && !_singleFirmwareMode && !px4Flow + + onClicked: { + _defaultFirmwareFact.rawValue = _defaultFimwareTypePX4 + parent.firmwareVersionChanged(firmwareTypeList) + } + } + QGCComboBox { id: vehicleTypeSelectionCombo anchors.left: parent.left anchors.right: parent.right - visible: apmFlightStack.checked + visible: !px4Flow && apmFlightStack.checked model: controller.apmAvailableVersions } + QGCComboBox { + id: px4FlowTypeSelectionCombo + anchors.left: parent.left + anchors.right: parent.right + visible: px4Flow + model: px4FlowFirmwareList + currentIndex: _defaultFirmwareIsPX4 ? 0 : 1 + } + Row { width: parent.width spacing: ScreenTools.defaultFontPixelWidth / 2 @@ -349,11 +388,11 @@ SetupPage { anchors.left: parent.left anchors.right: parent.right visible: showFirmwareTypeSelection - model: _singleFirmwareMode ? singleFirmwareModeTypeList: (px4Flow ? px4FlowTypeList : firmwareTypeList) + model: _singleFirmwareMode ? singleFirmwareModeTypeList : (px4Flow ? px4FlowTypeList : firmwareTypeList) currentIndex: controller.selectedFirmwareType onActivated: { - controller.selectedFirmwareType = index + controller.selectedFirmwareType = model.get(index).firmwareType if (model.get(index).firmwareType === FirmwareUpgradeController.BetaFirmware) { firmwareVersionWarningLabel.visible = true firmwareVersionWarningLabel.text = qsTr("WARNING: BETA FIRMWARE. ") + diff --git a/src/VehicleSetup/FirmwareUpgradeController.cc b/src/VehicleSetup/FirmwareUpgradeController.cc index f417e86ab20abd946667b818eba8579507a818df..2bc5ac052db895ff1f3fb442e70f875e8a956777 100644 --- a/src/VehicleSetup/FirmwareUpgradeController.cc +++ b/src/VehicleSetup/FirmwareUpgradeController.cc @@ -99,6 +99,7 @@ void FirmwareUpgradeController::flash(AutoPilotStackType_t stackType, FirmwareType_t firmwareType, FirmwareVehicleType_t vehicleType) { + qCDebug(FirmwareUpgradeLog) << "_flash stackType:firmwareType:vehicleType" << stackType << firmwareType << vehicleType; FirmwareIdentifier firmwareId = FirmwareIdentifier(stackType, firmwareType, vehicleType); if (_bootloaderFound) { _getFirmwareFile(firmwareId); @@ -114,9 +115,9 @@ void FirmwareUpgradeController::flash(const FirmwareIdentifier& firmwareId) flash(firmwareId.autopilotStackType, firmwareId.firmwareType, firmwareId.firmwareVehicleType); } -void FirmwareUpgradeController::flashSingleFirmwareMode(void) +void FirmwareUpgradeController::flashSingleFirmwareMode(FirmwareType_t firmwareType) { - flash(SingleFirmwareMode, StableFirmware, DefaultVehicleFirmware); + flash(SingleFirmwareMode, firmwareType, DefaultVehicleFirmware); } void FirmwareUpgradeController::cancel(void) @@ -351,7 +352,8 @@ void FirmwareUpgradeController::_initFirmwareHash() /////////////////////////////// px4flow firmwares /////////////////////////////////////// FirmwareToUrlElement_t rgPX4FLowFirmwareArray[] = { - { PX4Flow, StableFirmware, DefaultVehicleFirmware, "http://px4-travis.s3.amazonaws.com/Flow/master/px4flow.px4" }, + { PX4FlowPX4, StableFirmware, DefaultVehicleFirmware, "http://px4-travis.s3.amazonaws.com/Flow/master/px4flow.px4" }, + { PX4FlowAPM, StableFirmware, DefaultVehicleFirmware, "http://firmware.ardupilot.org/Tools/PX4Flow/px4flow-klt-latest.px4" }, }; /////////////////////////////// 3dr radio firmwares /////////////////////////////////////// diff --git a/src/VehicleSetup/FirmwareUpgradeController.h b/src/VehicleSetup/FirmwareUpgradeController.h index 80b1d563208543e6a6a4d1820a8defb42e11aaa2..2193636e1294b09438287a45351512c75915ed3e 100644 --- a/src/VehicleSetup/FirmwareUpgradeController.h +++ b/src/VehicleSetup/FirmwareUpgradeController.h @@ -40,7 +40,8 @@ public: typedef enum { AutoPilotStackPX4, AutoPilotStackAPM, - PX4Flow, + PX4FlowPX4, + PX4FlowAPM, ThreeDRRadio, SingleFirmwareMode } AutoPilotStackType_t; @@ -123,7 +124,7 @@ public: FirmwareVehicleType_t vehicleType = DefaultVehicleFirmware ); /// Called to flash when upgrade is running in singleFirmwareMode - Q_INVOKABLE void flashSingleFirmwareMode(void); + Q_INVOKABLE void flashSingleFirmwareMode(FirmwareType_t firmwareType); Q_INVOKABLE FirmwareVehicleType_t vehicleTypeFromVersionIndex(int index); diff --git a/src/VehicleSetup/JoystickConfig.qml b/src/VehicleSetup/JoystickConfig.qml index df9a8d22c6264ea186f9275efda8f3c41b012f2e..396a1edcaa2967150a34e2a2f519f7d775f17ec3 100644 --- a/src/VehicleSetup/JoystickConfig.qml +++ b/src/VehicleSetup/JoystickConfig.qml @@ -366,13 +366,14 @@ SetupPage { id: enabledCheckBox enabled: _activeJoystick ? _activeJoystick.calibrated : false text: _activeJoystick ? _activeJoystick.calibrated ? qsTr("Enable joystick input") : qsTr("Enable not allowed (Calibrate First)") : "" - - onClicked: _activeJoystick.outputEnabled = checked + onClicked: _activeVehicle.joystickEnabled = checked + Component.onCompleted: checked = _activeVehicle.joystickEnabled Connections { - target: _activeJoystick - onOutputEnabledChanged: { - enabledCheckBox.checked=enabled + target: _activeVehicle + + onJoystickEnabledChanged: { + enabledCheckBox.checked = _activeVehicle.joystickEnabled } } @@ -530,6 +531,40 @@ SetupPage { } } + Row { + width: parent.width + spacing: ScreenTools.defaultFontPixelWidth + visible: advancedSettings.checked + QGCLabel { + text: qsTr("Message frequency (Hz):") + anchors.verticalCenter: parent.verticalCenter + } + QGCTextField { + text: _activeJoystick.frequency + validator: DoubleValidator { bottom: 0.25; top: 100.0; } + inputMethodHints: Qt.ImhFormattedNumbersOnly + onEditingFinished: { + _activeJoystick.frequency = parseFloat(text) + } + } + } + + Row { + width: parent.width + spacing: ScreenTools.defaultFontPixelWidth + visible: advancedSettings.checked + QGCCheckBox { + id: joystickCircleCorrection + checked: _activeVehicle.joystickMode != 0 + text: qsTr("Enable circle correction") + + Component.onCompleted: checked = _activeJoystick.circleCorrection + onClicked: { + _activeJoystick.circleCorrection = checked + } + } + } + Row { width: parent.width spacing: ScreenTools.defaultFontPixelWidth diff --git a/src/VehicleSetup/SetupView.qml b/src/VehicleSetup/SetupView.qml index 9b94e26c72a696d83d3ecf87b62fc3b82a605b73..e5f0f86ea6b9c68137a51d4279511231cedd9493 100644 --- a/src/VehicleSetup/SetupView.qml +++ b/src/VehicleSetup/SetupView.qml @@ -210,8 +210,7 @@ Rectangle { spacing: _defaultTextHeight / 2 QGCLabel { - anchors.left: parent.left - anchors.right: parent.right + Layout.fillWidth: true text: qsTr("Vehicle Setup") wrapMode: Text.WordWrap horizontalAlignment: Text.AlignHCenter @@ -259,7 +258,7 @@ Rectangle { SubMenuButton { id: px4FlowButton exclusiveGroup: setupButtonGroup - visible: QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle.genericFirmware : false + visible: QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle.priorityLink.isPX4Flow : false setupIndicator: false text: qsTr("PX4Flow") Layout.fillWidth: true diff --git a/src/VehicleSetup/VehicleComponent.cc b/src/VehicleSetup/VehicleComponent.cc index 3afc798ace843aa43f4ed8909e2db2cd4960dd67..071940bc0f38a233fd58e58f4cd0a4d13dcac81b 100644 --- a/src/VehicleSetup/VehicleComponent.cc +++ b/src/VehicleSetup/VehicleComponent.cc @@ -66,9 +66,3 @@ void VehicleComponent::_triggerUpdated(QVariant /*value*/) { emit setupCompleteChanged(setupComplete()); } - -bool VehicleComponent::allowSetupWhileArmed(void) const -{ - // Default is to not allow setup while armed - return false; -} diff --git a/src/VehicleSetup/VehicleComponent.h b/src/VehicleSetup/VehicleComponent.h index f452467ab7955e319c2aabb4779ad496bbc65914..9b6488ed0bf025929360d3d3136910d3ac1a454e 100644 --- a/src/VehicleSetup/VehicleComponent.h +++ b/src/VehicleSetup/VehicleComponent.h @@ -37,7 +37,8 @@ class VehicleComponent : public QObject Q_PROPERTY(QUrl setupSource READ setupSource NOTIFY setupSourceChanged) Q_PROPERTY(QUrl summaryQmlSource READ summaryQmlSource CONSTANT) Q_PROPERTY(bool allowSetupWhileArmed READ allowSetupWhileArmed CONSTANT) - + Q_PROPERTY(bool allowSetupWhileFlying READ allowSetupWhileFlying CONSTANT) + public: VehicleComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent = NULL); ~VehicleComponent(); @@ -51,8 +52,11 @@ public: virtual QUrl summaryQmlSource(void) const = 0; // @return true: Setup panel can be shown while vehicle is armed - virtual bool allowSetupWhileArmed(void) const; + virtual bool allowSetupWhileArmed(void) const { return false; } // Defaults to false + // @return true: Setup panel can be shown while vehicle is flying (and armed) + virtual bool allowSetupWhileFlying(void) const { return false; } // Defaults to false + virtual void addSummaryQmlComponent(QQmlContext* context, QQuickItem* parent); /// @brief Returns an list of parameter names for which a change should cause the setupCompleteChanged diff --git a/src/VideoStreaming/README.md b/src/VideoStreaming/README.md index 585cb9cf8805fa004d62073531028acf94961d8f..eb8d8f45c6936ee13c23a4d38b762d94401e98d2 100644 --- a/src/VideoStreaming/README.md +++ b/src/VideoStreaming/README.md @@ -40,6 +40,9 @@ list=$(apt-cache --names-only search ^gstreamer1.0-* | awk '{ print $1 }' | grep ``` sudo apt-get install $list ``` +``` +sudo apt-get install libgstreamer-plugins-base1.0-dev +``` The build system is setup to use pkgconfig and it will find the necessary headers and libraries automatically. diff --git a/src/VideoStreaming/VideoReceiver.cc b/src/VideoStreaming/VideoReceiver.cc index 7f46d85650258331bfd533f0c89ed9d4e2d68e2c..97455b3833067282eb15c5aace44cb0f44b53651 100644 --- a/src/VideoStreaming/VideoReceiver.cc +++ b/src/VideoStreaming/VideoReceiver.cc @@ -63,6 +63,7 @@ VideoReceiver::VideoReceiver(QObject* parent) , _videoSink(NULL) , _socket(NULL) , _serverPresent(false) + , _rtspTestInterval_ms(5000) #endif , _videoSurface(NULL) , _videoRunning(false) @@ -164,9 +165,9 @@ VideoReceiver::_socketError(QAbstractSocket::SocketError socketError) Q_UNUSED(socketError); _socket->deleteLater(); _socket = NULL; - //-- Try again in 5 seconds + //-- Try again in a while if(_videoSettings->streamEnabled()->rawValue().toBool()) { - _timer.start(5000); + _timer.start(_rtspTestInterval_ms); } } #endif @@ -194,7 +195,7 @@ VideoReceiver::_timeout() connect(_socket, static_cast(&QTcpSocket::error), this, &VideoReceiver::_socketError); connect(_socket, &QTcpSocket::connected, this, &VideoReceiver::_connected); _socket->connectToHost(url.host(), url.port()); - _timer.start(5000); + _timer.start(_rtspTestInterval_ms); } } #endif @@ -218,6 +219,7 @@ VideoReceiver::start() return; } #if defined(QGC_GST_STREAMING) + _stop = false; qCDebug(VideoReceiverLog) << "start()"; if (_uri.isEmpty()) { @@ -433,6 +435,7 @@ void VideoReceiver::stop() { #if defined(QGC_GST_STREAMING) + _stop = true; qCDebug(VideoReceiverLog) << "stop()"; if(!_streaming) { _shutdownPipeline(); @@ -674,6 +677,14 @@ VideoReceiver::startRecording(const QString &videoFile) gst_element_sync_state_with_parent(_sink->mux); gst_element_sync_state_with_parent(_sink->filesink); + // Install a probe on the recording branch to drop buffers until we hit our first keyframe + // When we hit our first keyframe, we can offset the timestamps appropriately according to the first keyframe time + // This will ensure the first frame is a keyframe at t=0, and decoding can begin immediately on playback + GstPad* probepad = gst_element_get_static_pad(_sink->queue, "src"); + gst_pad_add_probe(probepad, (GstPadProbeType)(GST_PAD_PROBE_TYPE_BUFFER /* | GST_PAD_PROBE_TYPE_BLOCK */), _keyframeWatch, this, NULL); // to drop the buffer or to block the buffer? + gst_object_unref(probepad); + + // Link the recording branch to the pipeline GstPad* sinkpad = gst_element_get_static_pad(_sink->queue, "sink"); gst_pad_link(_sink->teepad, sinkpad); gst_object_unref(sinkpad); @@ -802,6 +813,33 @@ VideoReceiver::_unlinkCallBack(GstPad* pad, GstPadProbeInfo* info, gpointer user } #endif +//----------------------------------------------------------------------------- +#if defined(QGC_GST_STREAMING) +GstPadProbeReturn +VideoReceiver::_keyframeWatch(GstPad* pad, GstPadProbeInfo* info, gpointer user_data) +{ + Q_UNUSED(pad); + if(info != NULL && user_data != NULL) { + GstBuffer* buf = gst_pad_probe_info_get_buffer(info); + if(GST_BUFFER_FLAG_IS_SET(buf, GST_BUFFER_FLAG_DELTA_UNIT)) { // wait for a keyframe + return GST_PAD_PROBE_DROP; + } else { + VideoReceiver* pThis = (VideoReceiver*)user_data; + // reset the clock + GstClock* clock = gst_pipeline_get_clock(GST_PIPELINE(pThis->_pipeline)); + GstClockTime time = gst_clock_get_time(clock); + gst_object_unref(clock); + gst_element_set_base_time(pThis->_pipeline, time); // offset pipeline timestamps to start at zero again + buf->dts = 0; // The offset will not apply to this current buffer, our first frame, timestamp is zero + buf->pts = 0; + qCDebug(VideoReceiverLog) << "Got keyframe, stop dropping buffers"; + } + } + + return GST_PAD_PROBE_REMOVE; +} +#endif + //----------------------------------------------------------------------------- void VideoReceiver::_updateTimer() @@ -835,9 +873,11 @@ VideoReceiver::_updateTimer() } if(elapsed > (time_t)timeout && _videoSurface) { stop(); + // We want to start it back again with _updateTimer + _stop = false; } } else { - if(!running() && !_uri.isEmpty() && _videoSettings->streamEnabled()->rawValue().toBool()) { + if(!_stop && !running() && !_uri.isEmpty() && _videoSettings->streamEnabled()->rawValue().toBool()) { start(); } } diff --git a/src/VideoStreaming/VideoReceiver.h b/src/VideoStreaming/VideoReceiver.h index 843abdf144ea6b0d3e18529ea9abfbd06df653cb..edecf8502ba3fe1b71a72b19d4d50ae791c7f690 100644 --- a/src/VideoStreaming/VideoReceiver.h +++ b/src/VideoStreaming/VideoReceiver.h @@ -49,54 +49,54 @@ public: ~VideoReceiver(); #if defined(QGC_GST_STREAMING) - bool running () { return _running; } - bool recording () { return _recording; } - bool streaming () { return _streaming; } - bool starting () { return _starting; } - bool stopping () { return _stopping; } + virtual bool running () { return _running; } + virtual bool recording () { return _recording; } + virtual bool streaming () { return _streaming; } + virtual bool starting () { return _starting; } + virtual bool stopping () { return _stopping; } #endif - VideoSurface* videoSurface () { return _videoSurface; } - bool videoRunning () { return _videoRunning; } - QString imageFile () { return _imageFile; } - QString videoFile () { return _videoFile; } - bool showFullScreen () { return _showFullScreen; } + virtual VideoSurface* videoSurface () { return _videoSurface; } + virtual bool videoRunning () { return _videoRunning; } + virtual QString imageFile () { return _imageFile; } + virtual QString videoFile () { return _videoFile; } + virtual bool showFullScreen () { return _showFullScreen; } - void grabImage (QString imageFile); + virtual void grabImage (QString imageFile); - void setShowFullScreen (bool show) { _showFullScreen = show; emit showFullScreenChanged(); } + virtual void setShowFullScreen (bool show) { _showFullScreen = show; emit showFullScreenChanged(); } signals: - void videoRunningChanged (); - void imageFileChanged (); - void videoFileChanged (); - void showFullScreenChanged (); + void videoRunningChanged (); + void imageFileChanged (); + void videoFileChanged (); + void showFullScreenChanged (); #if defined(QGC_GST_STREAMING) - void recordingChanged (); - void msgErrorReceived (); - void msgEOSReceived (); - void msgStateChangedReceived (); + void recordingChanged (); + void msgErrorReceived (); + void msgEOSReceived (); + void msgStateChangedReceived (); #endif public slots: - void start (); - void stop (); - void setUri (const QString& uri); - void stopRecording (); - void startRecording (const QString& videoFile = QString()); - -private slots: - void _updateTimer (); + virtual void start (); + virtual void stop (); + virtual void setUri (const QString& uri); + virtual void stopRecording (); + virtual void startRecording (const QString& videoFile = QString()); + +protected slots: + virtual void _updateTimer (); #if defined(QGC_GST_STREAMING) - void _timeout (); - void _connected (); - void _socketError (QAbstractSocket::SocketError socketError); - void _handleError (); - void _handleEOS (); - void _handleStateChanged (); + virtual void _timeout (); + virtual void _connected (); + virtual void _socketError (QAbstractSocket::SocketError socketError); + virtual void _handleError (); + virtual void _handleEOS (); + virtual void _handleStateChanged (); #endif -private: +protected: #if defined(QGC_GST_STREAMING) typedef struct @@ -114,16 +114,19 @@ private: bool _streaming; bool _starting; bool _stopping; + bool _stop; Sink* _sink; GstElement* _tee; static gboolean _onBusMessage (GstBus* bus, GstMessage* message, gpointer user_data); static GstPadProbeReturn _unlinkCallBack (GstPad* pad, GstPadProbeInfo* info, gpointer user_data); - void _detachRecordingBranch (GstPadProbeInfo* info); - void _shutdownRecordingBranch(); - void _shutdownPipeline (); - void _cleanupOldVideos (); - void _setVideoSink (GstElement* sink); + static GstPadProbeReturn _keyframeWatch (GstPad* pad, GstPadProbeInfo* info, gpointer user_data); + + virtual void _detachRecordingBranch (GstPadProbeInfo* info); + virtual void _shutdownRecordingBranch(); + virtual void _shutdownPipeline (); + virtual void _cleanupOldVideos (); + virtual void _setVideoSink (GstElement* sink); GstElement* _pipeline; GstElement* _pipelineStopRec; @@ -134,6 +137,7 @@ private: QTimer _timer; QTcpSocket* _socket; bool _serverPresent; + int _rtspTestInterval_ms; #endif diff --git a/src/VideoStreaming/VideoStreaming.pri b/src/VideoStreaming/VideoStreaming.pri index b6b4324f3471fbbec7ca0601729022ae8c865d42..9474c54baa128b267ff61a95a779148c3223e989 100644 --- a/src/VideoStreaming/VideoStreaming.pri +++ b/src/VideoStreaming/VideoStreaming.pri @@ -181,24 +181,7 @@ VideoEnabled { } else { LinuxBuild|MacBuild|iOSBuild|WindowsBuild|AndroidBuild { message("Skipping support for video streaming (GStreamer libraries not installed)") - MacBuild { - message(" You can download it from http://gstreamer.freedesktop.org/data/pkg/osx/") - message(" Select the devel package and install it (gstreamer-1.0-devel-1.x.x-x86_64.pkg)") - message(" It will be installed in /Libraries/Frameworks") - } - LinuxBuild { - message(" You can install it using apt-get") - message(" sudo apt-get install gstreamer1.0*") - } - WindowsBuild { - message(" You can download it from http://gstreamer.freedesktop.org/data/pkg/windows/") - message(" Select the devel AND runtime packages and install them (x86, not the 64-Bit)") - message(" It will be installed in C:/gstreamer. You need to update you PATH to point to the bin directory.") - } - AndroidBuild { - message(" You can download it from http://gstreamer.freedesktop.org/data/pkg/android/") - message(" Uncompress the archive into the qgc root source directory (same directory where qgroundcontrol.pro is found.") - } + message("Installation instructions here: https://github.com/mavlink/qgroundcontrol/blob/master/src/VideoStreaming/README.md") } else { message("Skipping support for video streaming (Unsupported platform)") } diff --git a/src/VideoStreaming/gstqtvideosink/painters/videonode.cpp b/src/VideoStreaming/gstqtvideosink/painters/videonode.cpp index 87b415c15c3c6bf6ce7c40ad87f9d00d38f88c73..1a4cd1ab02bb7e2bdea3636014c2093e691b721f 100644 --- a/src/VideoStreaming/gstqtvideosink/painters/videonode.cpp +++ b/src/VideoStreaming/gstqtvideosink/painters/videonode.cpp @@ -52,14 +52,18 @@ void VideoNode::setMaterialTypeSolidBlack() void VideoNode::setCurrentFrame(GstBuffer* buffer) { - Q_ASSERT (m_materialType == MaterialTypeVideo); + if (m_materialType != MaterialTypeVideo) { + return; + } static_cast(material())->setCurrentFrame(buffer); markDirty(DirtyMaterial); } void VideoNode::updateColors(int brightness, int contrast, int hue, int saturation) { - Q_ASSERT (m_materialType == MaterialTypeVideo); + if (m_materialType != MaterialTypeVideo) { + return; + } static_cast(material())->updateColors(brightness, contrast, hue, saturation); markDirty(DirtyMaterial); } diff --git a/src/ViewWidgets/CustomCommandWidget.qml b/src/ViewWidgets/CustomCommandWidget.qml index e937aef5ee105664e42899e091a72c9a8ce64c83..4d4ee60d7688183ba315e772a6f0d9792cafd4d4 100644 --- a/src/ViewWidgets/CustomCommandWidget.qml +++ b/src/ViewWidgets/CustomCommandWidget.qml @@ -34,7 +34,7 @@ QGCView { "So make sure to test your changes thoroughly before using them in flight.

" + "

Click 'Load Custom Qml file' to provide your custom qml file.

" + "

Click 'Reset' to reset to none.

" + - "

Example usage: http://https://dev.qgroundcontrol.com/en/tools/custom_command_widget.html

" + "

Example usage: https://dev.qgroundcontrol.com/en/tools/custom_command_widget.html

" QGCPalette { id: qgcPal; colorGroupEnabled: enabled } @@ -87,15 +87,20 @@ QGCView { } } } - - QGCLabel { - id: textOutput + QGCFlickable { + id: container anchors.fill: loader - wrapMode: Text.WordWrap - textFormat: Text.RichText + contentHeight: textOutput.height + flickableDirection: QGCFlickable.VerticalFlick visible: !loader.visible + QGCLabel { + id: textOutput + width: container.width + wrapMode: Text.WordWrap + textFormat: Text.RichText + onLinkActivated: Qt.openUrlExternally(link) + } } - Row { id: buttonRow spacing: ScreenTools.defaultFontPixelWidth @@ -115,6 +120,7 @@ QGCView { onClicked: controller.clearQmlFile() } } + } - } + } } diff --git a/src/api/QGCCorePlugin.cc b/src/api/QGCCorePlugin.cc index 58b06834e82948323797611d4de59f72d147f161..9031c90560b174205d3c0d6ff52f9a529a3fef20 100644 --- a/src/api/QGCCorePlugin.cc +++ b/src/api/QGCCorePlugin.cc @@ -15,6 +15,7 @@ #include "SettingsManager.h" #include "AppMessages.h" #include "QmlObjectListModel.h" +#include "VideoReceiver.h" #include #include @@ -190,8 +191,13 @@ bool QGCCorePlugin::overrideSettingsGroupVisibility(QString name) return true; } -bool QGCCorePlugin::adjustSettingMetaData(FactMetaData& metaData) +bool QGCCorePlugin::adjustSettingMetaData(const QString& settingsGroup, FactMetaData& metaData) { + if (settingsGroup != AppSettings::settingsGroup) { + // All changes refer to AppSettings + return true; + } + //-- Default Palette if (metaData.name() == AppSettings::indoorPaletteName) { QVariant outdoorPalette; @@ -281,3 +287,8 @@ QmlObjectListModel* QGCCorePlugin::customMapItems(void) { return &_p->_emptyCustomMapItems; } + +VideoReceiver* QGCCorePlugin::createVideoReceiver(QObject* parent) +{ + return new VideoReceiver(parent); +} diff --git a/src/api/QGCCorePlugin.h b/src/api/QGCCorePlugin.h index 193253cc560b126aa95175861044562d852ca396..b68f3245798d2bb231748da8caefbf00c0fdff6e 100644 --- a/src/api/QGCCorePlugin.h +++ b/src/api/QGCCorePlugin.h @@ -21,8 +21,6 @@ /// @brief Core Plugin Interface for QGroundControl /// @author Gus Grubba -// Work In Progress - class QGCApplication; class QGCOptions; class QGCSettings; @@ -33,6 +31,8 @@ class QQmlApplicationEngine; class Vehicle; class LinkInterface; class QmlObjectListModel; +class VideoReceiver; + class QGCCorePlugin : public QGCTool { Q_OBJECT @@ -68,14 +68,15 @@ public: virtual QGCOptions* options(void); /// Allows the core plugin to override the visibility for a settings group - /// @param name - Setting group name + /// @param name - SettingsGroup name /// @return true: Show settings ui, false: Hide settings ui virtual bool overrideSettingsGroupVisibility(QString name); /// Allows the core plugin to override the setting meta data before the setting fact is created. + /// @param settingsGroup - QSettings group which contains this item /// @param metaData - MetaData for setting fact /// @return true: Setting should be visible in ui, false: Setting should not be shown in ui - virtual bool adjustSettingMetaData(FactMetaData& metaData); + virtual bool adjustSettingMetaData(const QString& settingsGroup, FactMetaData& metaData); /// Return the resource file which contains the brand image for for Indoor theme. virtual QString brandImageIndoor(void) const { return QString(); } @@ -98,6 +99,9 @@ public: /// Allows the plugin to override the creation of the root (native) window. virtual QQmlApplicationEngine* createRootWindow(QObject* parent); + /// Allows the plugin to override the creation of VideoReceiver. + virtual VideoReceiver* createVideoReceiver(QObject* parent); + /// Allows the plugin to see all mavlink traffic to a vehicle /// @return true: Allow vehicle to continue processing, false: Vehicle should not process message virtual bool mavlinkMessage(Vehicle* vehicle, LinkInterface* link, mavlink_message_t message); diff --git a/src/api/QGCOptions.h b/src/api/QGCOptions.h index a441b0139c5e68d335075d07be7163308e16beb3..56f0546829022dee0c3cb4319bc08f046eea03d1 100644 --- a/src/api/QGCOptions.h +++ b/src/api/QGCOptions.h @@ -46,6 +46,9 @@ public: Q_PROPERTY(bool showOfflineMapImport READ showOfflineMapImport NOTIFY showOfflineMapImportChanged) Q_PROPERTY(bool useMobileFileDialog READ useMobileFileDialog CONSTANT) Q_PROPERTY(bool showMissionStatus READ showMissionStatus CONSTANT) + Q_PROPERTY(bool guidedActionsRequireRCRSSI READ guidedActionsRequireRCRSSI CONSTANT) + Q_PROPERTY(bool showMissionAbsoluteAltitude READ showMissionAbsoluteAltitude NOTIFY showMissionAbsoluteAltitudeChanged) + Q_PROPERTY(bool showSimpleMissionStart READ showSimpleMissionStart NOTIFY showSimpleMissionStartChanged) /// Should QGC hide its settings menu and colapse it into one single menu (Settings and Vehicle Setup)? /// @return true if QGC should consolidate both menus into one. @@ -82,14 +85,15 @@ public: virtual bool guidedBarShowOrbit () const { return true; } virtual bool missionWaypointsOnly () const { return false; } ///< true: Only allow waypoints and complex items in Plan virtual bool multiVehicleEnabled () const { return true; } ///< false: multi vehicle support is disabled + virtual bool guidedActionsRequireRCRSSI () const { return false; } ///< true: Guided actions will be disabled is there is no RC RSSI + virtual bool showOfflineMapExport () const { return true; } + virtual bool showOfflineMapImport () const { return true; } + virtual bool showMissionAbsoluteAltitude () const { return true; } + virtual bool showSimpleMissionStart () const { return false; } #if defined(__mobile__) - virtual bool showOfflineMapExport () const { return false; } - virtual bool showOfflineMapImport () const { return false; } virtual bool useMobileFileDialog () const { return true;} #else - virtual bool showOfflineMapExport () const { return true; } - virtual bool showOfflineMapImport () const { return true; } virtual bool useMobileFileDialog () const { return false;} #endif @@ -111,6 +115,8 @@ signals: void multiVehicleEnabledChanged (bool multiVehicleEnabled); void showOfflineMapExportChanged (); void showOfflineMapImportChanged (); + void showMissionAbsoluteAltitudeChanged (); + void showSimpleMissionStartChanged (); private: CustomInstrumentWidget* _defaultInstrumentWidget; diff --git a/src/comm/BluetoothLink.cc b/src/comm/BluetoothLink.cc index f1b9cc46e83232564f1cadcd8185f37cd88aad13..e6606987fe7c0037c9a2abeb8aa57d5ff6efd831 100644 --- a/src/comm/BluetoothLink.cc +++ b/src/comm/BluetoothLink.cc @@ -75,30 +75,25 @@ QString BluetoothLink::getName() const void BluetoothLink::_writeBytes(const QByteArray bytes) { - if(_targetSocket) - { - if(_targetSocket->isWritable()) - { - if(_targetSocket->write(bytes) > 0) { - _logOutputDataRate(bytes.size(), QDateTime::currentMSecsSinceEpoch()); - } - else - qWarning() << "Bluetooth write error"; + if (_targetSocket) { + if(_targetSocket->write(bytes) > 0) { + _logOutputDataRate(bytes.size(), QDateTime::currentMSecsSinceEpoch()); + } else { + qWarning() << "Bluetooth write error"; } - else - qWarning() << "Bluetooth not writable error"; } } void BluetoothLink::readBytes() { - while (_targetSocket->bytesAvailable() > 0) - { - QByteArray datagram; - datagram.resize(_targetSocket->bytesAvailable()); - _targetSocket->read(datagram.data(), datagram.size()); - emit bytesReceived(this, datagram); - _logInputDataRate(datagram.length(), QDateTime::currentMSecsSinceEpoch()); + if (_targetSocket) { + while (_targetSocket->bytesAvailable() > 0) { + QByteArray datagram; + datagram.resize(_targetSocket->bytesAvailable()); + _targetSocket->read(datagram.data(), datagram.size()); + emit bytesReceived(this, datagram); + _logInputDataRate(datagram.length(), QDateTime::currentMSecsSinceEpoch()); + } } } @@ -114,7 +109,7 @@ void BluetoothLink::_disconnect(void) #endif if(_targetSocket) { - delete _targetSocket; + _targetSocket->deleteLater(); _targetSocket = NULL; emit disconnected(); } @@ -249,8 +244,8 @@ BluetoothConfiguration::BluetoothConfiguration(const QString& name) BluetoothConfiguration::BluetoothConfiguration(BluetoothConfiguration* source) : LinkConfiguration(source) , _deviceDiscover(NULL) + , _device(source->device()) { - _device = source->device(); } BluetoothConfiguration::~BluetoothConfiguration() diff --git a/src/comm/HeartbeatTimer.cc b/src/comm/HeartbeatTimer.cc new file mode 100644 index 0000000000000000000000000000000000000000..7090b736701685f346f3787bfeaf563ca500a464 --- /dev/null +++ b/src/comm/HeartbeatTimer.cc @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * (c) 2009-2018 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "HeartbeatTimer.h" + +#include + +HeartbeatTimer::HeartbeatTimer(int vehicle_id, bool high_latency) : + _active(true), + _timer(new QTimer), + _vehicleID(vehicle_id), + _high_latency(high_latency) +{ + if (!high_latency) { + _timer->setInterval(_heartbeatReceivedTimeoutMSecs); + _timer->setSingleShot(true); + _timer->start(); + } + emit activeChanged(true, _vehicleID); + QObject::connect(_timer, &QTimer::timeout, this, &HeartbeatTimer::timerTimeout); +} + +HeartbeatTimer::~HeartbeatTimer() { + if (_timer) { + QObject::disconnect(_timer, &QTimer::timeout, this, &HeartbeatTimer::timerTimeout); + _timer->stop(); + delete _timer; + _timer = nullptr; + } + + emit activeChanged(false, _vehicleID); +} + +void HeartbeatTimer::restartTimer() +{ + if (!_active) { + _active = true; + emit activeChanged(true, _vehicleID); + } + + _timer->start(); +} + +void HeartbeatTimer::timerTimeout() +{ + if (!_high_latency) { + if (_active) { + _active = false; + emit activeChanged(false, _vehicleID); + } + emit heartbeatTimeout(_vehicleID); + } +} diff --git a/src/comm/HeartbeatTimer.h b/src/comm/HeartbeatTimer.h new file mode 100644 index 0000000000000000000000000000000000000000..c62a63f695d43e9d370a021e391fd632cefbad10 --- /dev/null +++ b/src/comm/HeartbeatTimer.h @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * (c) 2009-2018 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#ifndef _HEARTBEATTIMER_H_ +#define _HEARTBEATTIMER_H_ + +#include +#include + +/** + * @brief The HeartbeatTimer class + * + * Track the heartbeat for a single vehicle on one link. + * As long as regular heartbeats are received the status is active. On the timer timeout + * status is set to inactive. On any status change the activeChanged signal is emitted. + * If high_latency is true then active is always true. + */ +class HeartbeatTimer : public QObject +{ + Q_OBJECT + +public: + /** + * @brief HeartbeatTimer + * + * Constructor + * + * @param vehicle_id: The vehicle ID for which the heartbeat will be tracked. + * @param high_latency: Indicates if the link is a high latency one. + */ + HeartbeatTimer(int vehicle_id, bool high_latency); + + ~HeartbeatTimer(); + + /** + * @brief getActive + * @return The current value of active + */ + bool getActive() const { return _active; } + + /** + * @brief getVehicleID + * @return The vehicle ID + */ + int getVehicleID() const { return _vehicleID; } + + /** + * @brief restartTimer + * + * Restarts the timer and emits the signal if the timer was previously inactive + */ + void restartTimer(); + +signals: + /** + * @brief heartbeatTimeout + * + * Emitted if no heartbeat is received over the specified time. + * + * @param vehicle_id: The vehicle ID for which the heartbeat timed out. + */ + void heartbeatTimeout(int vehicle_id); + + /** + * @brief activeChanged + * + * Emitted if the active status changes. + * + * @param active: The new value of the active state. + * @param vehicle_id: The vehicle ID for which the active state changed. + */ + void activeChanged(bool active, int vehicle_id); +private slots: + /** + * @brief timerTimeout + * + * Handle the timer timeout. + * + * Emit the signals according to the current state for regular links. + * Do nothing for a high latency link. + */ + void timerTimeout(); + +private: + bool _active = false; // The state of active. Is true if the timer has not timed out. + QTimer* _timer = nullptr; // Single shot timer + int _vehicleID = -1; // Vehicle ID for which the heartbeat is tracked. + bool _high_latency = false; // Indicates if the link is a high latency link or not. + + static const int _heartbeatReceivedTimeoutMSecs = 3500; // Signal connection lost after 3.5 seconds of no messages +}; + +#endif // _HEARTBEATTIMER_H_ diff --git a/src/comm/LinkInterface.cc b/src/comm/LinkInterface.cc index f2920712efce1b076baafeee3637bcf9fca352c5..13da6a8508bee26ab22d69e082a70febe9279111 100644 --- a/src/comm/LinkInterface.cc +++ b/src/comm/LinkInterface.cc @@ -10,6 +10,28 @@ #include "LinkInterface.h" #include "QGCApplication.h" +bool LinkInterface::active() const +{ + QMapIterator iter(_heartbeatTimers); + while (iter.hasNext()) { + iter.next(); + if (iter.value()->getActive()) { + return true; + } + } + + return false; +} + +bool LinkInterface::link_active(int vehicle_id) const +{ + if (_heartbeatTimers.contains(vehicle_id)) { + return _heartbeatTimers.value(vehicle_id)->getActive(); + } else { + return false; + } +} + /// mavlink channel to use for this link, as used by mavlink_parse_char. The mavlink channel is only /// set into the link when it is added to LinkManager uint8_t LinkInterface::mavlinkChannel(void) const @@ -20,15 +42,17 @@ uint8_t LinkInterface::mavlinkChannel(void) const return _mavlinkChannel; } // Links are only created by LinkManager so constructor is not public -LinkInterface::LinkInterface(SharedLinkConfigurationPointer& config) +LinkInterface::LinkInterface(SharedLinkConfigurationPointer& config, bool isPX4Flow) : QThread (0) , _config (config) , _highLatency (config->isHighLatency()) , _mavlinkChannelSet (false) - , _active (false) , _enableRateCollection (false) , _decodedFirstMavlinkPacket(false) + , _isPX4Flow (isPX4Flow) { + QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); + _config->setLink(this); // Initialize everything for the data rate calculation buffers. @@ -159,3 +183,29 @@ void LinkInterface::_setMavlinkChannel(uint8_t channel) _mavlinkChannelSet = true; _mavlinkChannel = channel; } + +void LinkInterface::_activeChanged(bool active, int vehicle_id) +{ + emit activeChanged(this, active, vehicle_id); +} + +void LinkInterface::startHeartbeatTimer(int vehicle_id) { + if (_heartbeatTimers.contains(vehicle_id)) { + _heartbeatTimers.value(vehicle_id)->restartTimer(); + } else { + _heartbeatTimers.insert(vehicle_id, new HeartbeatTimer(vehicle_id, _highLatency)); + QObject::connect(_heartbeatTimers.value(vehicle_id), &HeartbeatTimer::activeChanged, this, &LinkInterface::_activeChanged); + } +} + +void LinkInterface::stopHeartbeatTimer() { + QMapIterator iter(_heartbeatTimers); + while (iter.hasNext()) { + iter.next(); + QObject::disconnect(iter.value(), &HeartbeatTimer::activeChanged, this, &LinkInterface::_activeChanged); + delete _heartbeatTimers[iter.key()]; + _heartbeatTimers[iter.key()] = nullptr; + } + + _heartbeatTimers.clear(); +} diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h index 001dc0debb900d276d73280c108bb36e8025a304..0597d3ec4f3746c44f86864b72218646e195dd89 100644 --- a/src/comm/LinkInterface.h +++ b/src/comm/LinkInterface.h @@ -17,9 +17,11 @@ #include #include #include +#include #include "QGCMAVLink.h" #include "LinkConfiguration.h" +#include "HeartbeatTimer.h" class LinkManager; @@ -36,13 +38,20 @@ class LinkInterface : public QThread friend class LinkManager; public: - ~LinkInterface() { _config->setLink(NULL); } + virtual ~LinkInterface() { + stopHeartbeatTimer(); + _config->setLink(NULL); + } + + Q_PROPERTY(bool active READ active NOTIFY activeChanged) + Q_PROPERTY(bool isPX4Flow READ isPX4Flow CONSTANT) - Q_PROPERTY(bool active READ active WRITE setActive NOTIFY activeChanged) + Q_INVOKABLE bool link_active(int vehicle_id) const; + Q_INVOKABLE bool getHighLatency(void) const { return _highLatency; } // Property accessors - bool active(void) { return _active; } - void setActive(bool active) { _active = active; emit activeChanged(active); } + bool active() const; + bool isPX4Flow(void) const { return _isPX4Flow; } LinkConfiguration* getLinkConfiguration(void) { return _config.data(); } @@ -51,7 +60,7 @@ public: /** * @brief Get the human readable name of this link */ - virtual QString getName() const = 0; + Q_INVOKABLE virtual QString getName() const = 0; virtual void requestReset() = 0; @@ -149,10 +158,12 @@ public slots: private slots: virtual void _writeBytes(const QByteArray) = 0; + + void _activeChanged(bool active, int vehicle_id); signals: void autoconnectChanged(bool autoconnect); - void activeChanged(bool active); + void activeChanged(LinkInterface* link, bool active, int vehicle_id); void _invokeWriteBytes(QByteArray); void highLatencyChanged(bool highLatency); @@ -193,7 +204,7 @@ signals: protected: // Links are only created by LinkManager so constructor is not public - LinkInterface(SharedLinkConfigurationPointer& config); + LinkInterface(SharedLinkConfigurationPointer& config, bool isPX4Flow = false); /// This function logs the send times and amounts of datas for input. Data is used for calculating /// the transmission rate. @@ -248,10 +259,25 @@ private: virtual bool _connect(void) = 0; virtual void _disconnect(void) = 0; - + /// Sets the mavlink channel to use for this link void _setMavlinkChannel(uint8_t channel); + /** + * @brief startHeartbeatTimer + * + * Start/restart the heartbeat timer for the specific vehicle. + * If no timer exists an instance is allocated. + */ + void startHeartbeatTimer(int vehicle_id); + + /** + * @brief stopHeartbeatTimer + * + * Stop and deallocate the heartbeat timers for all vehicles if any exists. + */ + void stopHeartbeatTimer(); + bool _mavlinkChannelSet; ///< true: _mavlinkChannel has been set uint8_t _mavlinkChannel; ///< mavlink channel to use for this link, as used by mavlink_parse_char @@ -273,9 +299,11 @@ private: mutable QMutex _dataRateMutex; // Mutex for accessing the data rate member variables - bool _active; ///< true: link is actively receiving mavlink messages bool _enableRateCollection; bool _decodedFirstMavlinkPacket; ///< true: link has correctly decoded it's first mavlink packet + bool _isPX4Flow; + + QMap _heartbeatTimers; }; typedef QSharedPointer SharedLinkInterfacePointer; diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index a425e1a81ef45e4f1e32b975dc55868cba74b8e0..20612542d8d993a1811295a730849f7dbe186c21 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -80,6 +80,8 @@ void LinkManager::setToolbox(QGCToolbox *toolbox) _autoConnectSettings = toolbox->settingsManager()->autoConnectSettings(); _mavlinkProtocol = _toolbox->mavlinkProtocol(); + connect(_mavlinkProtocol, &MAVLinkProtocol::vehicleHeartbeatInfo, this, &LinkManager::_heartbeatReceived); + connect(&_portListTimer, &QTimer::timeout, this, &LinkManager::_updateAutoConnectLinks); _portListTimer.start(_autoconnectUpdateTimerMSecs); // timeout must be long enough to get past bootloader on second pass @@ -95,7 +97,7 @@ void LinkManager::createConnectedLink(LinkConfiguration* config) } } -LinkInterface* LinkManager::createConnectedLink(SharedLinkConfigurationPointer& config) +LinkInterface* LinkManager::createConnectedLink(SharedLinkConfigurationPointer& config, bool isPX4Flow) { if (!config) { qWarning() << "LinkManager::createConnectedLink called with NULL config"; @@ -109,7 +111,7 @@ LinkInterface* LinkManager::createConnectedLink(SharedLinkConfigurationPointer& { SerialConfiguration* serialConfig = dynamic_cast(config.data()); if (serialConfig) { - pLink = new SerialLink(config); + pLink = new SerialLink(config, isPX4Flow); if (serialConfig->usbDirect()) { _activeLinkCheckList.append((SerialLink*)pLink); if (!_activeLinkCheckTimer.isActive()) { @@ -119,6 +121,8 @@ LinkInterface* LinkManager::createConnectedLink(SharedLinkConfigurationPointer& } } break; +#else + Q_UNUSED(isPX4Flow) #endif case LinkConfiguration::TypeUdp: pLink = new UDPLink(config); @@ -172,7 +176,7 @@ LinkInterface* LinkManager::createConnectedLink(const QString& name) void LinkManager::_addLink(LinkInterface* link) { if (thread() != QThread::currentThread()) { - qWarning() << "_deleteLink called from incorrect thread"; + qWarning() << "_addLink called from incorrect thread"; return; } @@ -338,6 +342,7 @@ void LinkManager::saveLinkConfigurationList() settings.setValue(root + "/name", linkConfig->name()); settings.setValue(root + "/type", linkConfig->type()); settings.setValue(root + "/auto", linkConfig->isAutoConnect()); + settings.setValue(root + "/high_latency", linkConfig->isHighLatency()); // Have the instance save its own values linkConfig->saveSettings(settings, root); } @@ -369,6 +374,7 @@ void LinkManager::loadLinkConfigurationList() if(!name.isEmpty()) { LinkConfiguration* pLink = NULL; bool autoConnect = settings.value(root + "/auto").toBool(); + bool highLatency = settings.value(root + "/high_latency").toBool(); switch((LinkConfiguration::LinkType)type) { #ifndef NO_SERIAL_LINK case LinkConfiguration::TypeSerial: @@ -403,6 +409,7 @@ void LinkManager::loadLinkConfigurationList() if(pLink) { //-- Have the instance load its own values pLink->setAutoConnect(autoConnect); + pLink->setHighLatency(highLatency); pLink->loadSettings(settings, root); addConfiguration(pLink); linksChanged = true; @@ -594,7 +601,7 @@ void LinkManager::_updateAutoConnectLinks(void) pSerialConfig->setDynamic(true); pSerialConfig->setPortName(portInfo.systemLocation()); _sharedAutoconnectConfigurations.append(SharedLinkConfigurationPointer(pSerialConfig)); - createConnectedLink(_sharedAutoconnectConfigurations.last()); + createConnectedLink(_sharedAutoconnectConfigurations.last(), boardType == QGCSerialPortInfo::BoardTypePX4Flow); } } } @@ -1000,3 +1007,11 @@ void LinkManager::_freeMavlinkChannel(int channel) { _mavlinkChannelsUsedBitMask &= ~(1 << channel); } + +void LinkManager::_heartbeatReceived(LinkInterface* link, int vehicleId, int componentId, int vehicleFirmwareType, int vehicleType) { + Q_UNUSED(componentId); + Q_UNUSED(vehicleFirmwareType); + Q_UNUSED(vehicleType); + + link->startHeartbeatTimer(vehicleId); +} diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index bd719e364305cc1dc8048d5b55a75af7ec4521e7..8c21ed5dac0542418aaa0c5e9affa93cd74461a7 100644 --- a/src/comm/LinkManager.h +++ b/src/comm/LinkManager.h @@ -104,7 +104,7 @@ public: /// Creates, connects (and adds) a link based on the given configuration instance. /// Link takes ownership of config. - LinkInterface* createConnectedLink(SharedLinkConfigurationPointer& config); + LinkInterface* createConnectedLink(SharedLinkConfigurationPointer& config, bool isPX4Flow = false); // This should only be used by Qml code Q_INVOKABLE void createConnectedLink(LinkConfiguration* config); @@ -204,6 +204,8 @@ private: SerialConfiguration* _autoconnectConfigurationsContainsPort(const QString& portName); #endif + void _heartbeatReceived(LinkInterface* link, int vehicleId, int componentId, int vehicleFirmwareType, int vehicleType); + bool _configUpdateSuspended; ///< true: stop updating configuration list bool _configurationsLoaded; ///< true: Link configurations have been loaded bool _connectionsSuspended; ///< true: all new connections should not be allowed diff --git a/src/comm/LogReplayLink.cc b/src/comm/LogReplayLink.cc index 32756a8c055e1d98eed977aa51d9e5e8236a194f..7c2cc4f9b0902d71a8c5e24662cce93e6d2b83dc 100644 --- a/src/comm/LogReplayLink.cc +++ b/src/comm/LogReplayLink.cc @@ -367,7 +367,9 @@ void LogReplayLink::_readNextLogEntry(void) quint64 currentTimeMSecs = (quint64)QDateTime::currentMSecsSinceEpoch(); timeToNextExecutionMSecs = desiredPacedTimeMSecs - currentTimeMSecs; } - + + emit currentLogTimeSecs((_logCurrentTimeUSecs - _logStartTimeUSecs) / 1000000); + // And schedule the next execution of this function. _readTickTimer.start(timeToNextExecutionMSecs); } diff --git a/src/comm/LogReplayLink.h b/src/comm/LogReplayLink.h index d5d863d5a750f31c7af3e7757d3a104a677b8adf..1ae85d5c9ea878ac55d7df99cb53e709f4ef791a 100644 --- a/src/comm/LogReplayLink.h +++ b/src/comm/LogReplayLink.h @@ -94,6 +94,7 @@ signals: void playbackAtEnd(void); void playbackError(void); void playbackPercentCompleteChanged(int percentComplete); + void currentLogTimeSecs(int secs); // Internal signals void _playOnThread(void); diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 9ad225d6a9e48914c7d531b5aaf8cf026421bdc5..53a381b81ebe55050b5bea5472e820a18408419c 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -35,9 +35,15 @@ QGC_LOGGING_CATEGORY(MockLinkVerboseLog, "MockLinkVerboseLog") // Vehicle position is set close to default Gazebo vehicle location. This allows for multi-vehicle // testing of a gazebo vehicle and a mocklink vehicle -double MockLink::_defaultVehicleLatitude = 47.397f; -double MockLink::_defaultVehicleLongitude = 8.5455f; -double MockLink::_defaultVehicleAltitude = 488.056f; +#if 1 +double MockLink::_defaultVehicleLatitude = 47.397; +double MockLink::_defaultVehicleLongitude = 8.5455; +double MockLink::_defaultVehicleAltitude = 488.056; +#else +double MockLink::_defaultVehicleLatitude = 47.6333022928789; +double MockLink::_defaultVehicleLongitude = -122.08833157994995; +double MockLink::_defaultVehicleAltitude = 19.0; +#endif int MockLink::_nextVehicleSystemId = 128; const char* MockLink::_failParam = "COM_FLTMODE6"; diff --git a/src/comm/QGCFlightGearLink.cc b/src/comm/QGCFlightGearLink.cc index ea141ca7970dc729d5ffe960dcfcd24d406ebc38..890ca1e83fc8f470d4a772e46ef6a823d30f9ecb 100644 --- a/src/comm/QGCFlightGearLink.cc +++ b/src/comm/QGCFlightGearLink.cc @@ -544,15 +544,9 @@ bool QGCFlightGearLink::parseUIArguments(QString uiArgs, QStringList& argList) } else if (chr == '\"') { // Flip the state of being in a quoted string. Note that we specifically do not add the // quote to the string. This replicates standards command line parsing behaviour. - if (chr == '\"') { - inQuotedString = !inQuotedString; - } + inQuotedString = !inQuotedString; previousSpace = false; } else { - // Flip the state of being in a quoted string - if (chr == '\"') { - inQuotedString = !inQuotedString; - } previousSpace = false; currentArg += chr; } diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc index ea5c8f8148f4f7142f6c6e584b6f16ba784f5e54..f65a7fed7e03805a8170c81e69ad36871ab4f13f 100644 --- a/src/comm/QGCXPlaneLink.cc +++ b/src/comm/QGCXPlaneLink.cc @@ -1064,7 +1064,7 @@ void QGCXPlaneLink::setRandomPosition() _vehicle->altitudeAMSL()->rawValue().toDouble() + offAlt, _vehicle->roll()->rawValue().toDouble(), _vehicle->pitch()->rawValue().toDouble(), - _vehicle->uas()->getYaw()); + _vehicle->heading()->rawValue().toDouble()); } void QGCXPlaneLink::setRandomAttitude() diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index 3ae34a3e99ed37f21fe33b9801644f2d8ec85f12..015ec95c4e22539bcba0ab7f5db7844549f83634 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -30,8 +30,8 @@ QGC_LOGGING_CATEGORY(SerialLinkLog, "SerialLinkLog") static QStringList kSupportedBaudRates; -SerialLink::SerialLink(SharedLinkConfigurationPointer& config) - : LinkInterface(config) +SerialLink::SerialLink(SharedLinkConfigurationPointer& config, bool isPX4Flow) + : LinkInterface(config, isPX4Flow) , _port(NULL) , _bytesRead(0) , _stopp(false) @@ -57,10 +57,6 @@ void SerialLink::requestReset() SerialLink::~SerialLink() { _disconnect(); - if (_port) { - delete _port; - } - _port = NULL; } bool SerialLink::_isBootloader() @@ -92,6 +88,7 @@ void SerialLink::_writeBytes(const QByteArray data) _port->write(data); } else { // Error occurred + qWarning() << "Serial port not writeable"; _emitLinkError(tr("Could not send data - link %1 is disconnected!").arg(getName())); } } @@ -105,7 +102,7 @@ void SerialLink::_disconnect(void) { if (_port) { _port->close(); - delete _port; + _port->deleteLater(); _port = NULL; } @@ -199,7 +196,7 @@ bool SerialLink::_hardwareConnect(QSerialPort::SerialPortError& error, QString& } } - _port = new QSerialPort(_serialConfig->portName()); + _port = new QSerialPort(_serialConfig->portName(), this); QObject::connect(_port, static_cast(&QSerialPort::error), this, &SerialLink::linkError); @@ -261,12 +258,18 @@ bool SerialLink::_hardwareConnect(QSerialPort::SerialPortError& error, QString& void SerialLink::_readBytes(void) { - qint64 byteCount = _port->bytesAvailable(); - if (byteCount) { - QByteArray buffer; - buffer.resize(byteCount); - _port->read(buffer.data(), buffer.size()); - emit bytesReceived(this, buffer); + if (_port && _port->isOpen()) { + qint64 byteCount = _port->bytesAvailable(); + if (byteCount) { + QByteArray buffer; + buffer.resize(byteCount); + _port->read(buffer.data(), buffer.size()); + emit bytesReceived(this, buffer); + } + } else { + // Error occurred + qWarning() << "Serial port not readable"; + _emitLinkError(tr("Could not read data - link %1 is disconnected!").arg(getName())); } } @@ -306,7 +309,7 @@ bool SerialLink::isConnected() const QString SerialLink::getName() const { - return _serialConfig->portName(); + return _serialConfig->name(); } /** diff --git a/src/comm/SerialLink.h b/src/comm/SerialLink.h index 9430f20686586fd8193b571ef6b67db5944f3dac..82fe598d8cc5667839e1d134a95e7abca0440878 100644 --- a/src/comm/SerialLink.h +++ b/src/comm/SerialLink.h @@ -169,7 +169,7 @@ private slots: private: // Links are only created/destroyed by LinkManager so constructor/destructor is not public - SerialLink(SharedLinkConfigurationPointer& config); + SerialLink(SharedLinkConfigurationPointer& config, bool isPX4Flow = false); ~SerialLink(); // From LinkInterface diff --git a/src/comm/TCPLink.cc b/src/comm/TCPLink.cc index 5760fb755f6d0050e9a1a8a405d60660d83dd26e..d91cdb145d5b4ed9ff79092a22145264f46505d2 100644 --- a/src/comm/TCPLink.cc +++ b/src/comm/TCPLink.cc @@ -78,11 +78,11 @@ void TCPLink::_writeBytes(const QByteArray data) #ifdef TCPLINK_READWRITE_DEBUG _writeDebugBytes(data); #endif - if (!_socket) - return; - _socket->write(data); - _logOutputDataRate(data.size(), QDateTime::currentMSecsSinceEpoch()); + if (_socket) { + _socket->write(data); + _logOutputDataRate(data.size(), QDateTime::currentMSecsSinceEpoch()); + } } /** @@ -93,17 +93,19 @@ void TCPLink::_writeBytes(const QByteArray data) **/ void TCPLink::readBytes() { - qint64 byteCount = _socket->bytesAvailable(); - if (byteCount) - { - QByteArray buffer; - buffer.resize(byteCount); - _socket->read(buffer.data(), buffer.size()); - emit bytesReceived(this, buffer); - _logInputDataRate(byteCount, QDateTime::currentMSecsSinceEpoch()); + if (_socket) { + qint64 byteCount = _socket->bytesAvailable(); + if (byteCount) + { + QByteArray buffer; + buffer.resize(byteCount); + _socket->read(buffer.data(), buffer.size()); + emit bytesReceived(this, buffer); + _logInputDataRate(byteCount, QDateTime::currentMSecsSinceEpoch()); #ifdef TCPLINK_READWRITE_DEBUG - writeDebugBytes(buffer.data(), buffer.size()); + writeDebugBytes(buffer.data(), buffer.size()); #endif + } } } @@ -118,9 +120,9 @@ void TCPLink::_disconnect(void) wait(); if (_socket) { _socketIsConnected = false; - _socket->deleteLater(); // Make sure delete happens on correct thread _socket->disconnectFromHost(); // Disconnect tcp _socket->waitForDisconnected(); + _socket->deleteLater(); // Make sure delete happens on correct thread _socket = NULL; emit disconnected(); } diff --git a/src/comm/UDPLink.cc b/src/comm/UDPLink.cc index ea80e3edb9e3cee95712775d40536a87ea5cf11d..58c32faf338fc2805931db2e5339d4d76281779e 100644 --- a/src/comm/UDPLink.cc +++ b/src/comm/UDPLink.cc @@ -68,27 +68,6 @@ static QString get_ip_address(const QString& address) return QString(""); } -static bool is_ip_local(const QHostAddress& add) -{ - // In simulation and testing setups the vehicle and the GCS can be - // running on the same host. This leads to packets arriving through - // the local network or the loopback adapter, which makes it look - // like the vehicle is connected through two different links, - // complicating routing. - // - // We detect this case and force all traffic to a simulated instance - // onto the local loopback interface. - // Run through all IPv4 interfaces and check if their canonical - // IP address in string representation matches the source IP address - foreach (const QHostAddress &address, QNetworkInterface::allAddresses()) { - if (address == add) { - // This is a local address of the same host - return true; - } - } - return false; -} - static bool contains_target(const QList list, const QHostAddress& address, quint16 port) { foreach(UDPCLient* target, list) { @@ -112,6 +91,9 @@ UDPLink::UDPLink(SharedLinkConfigurationPointer& config) if (!_udpConfig) { qWarning() << "Internal error"; } + foreach (const QHostAddress &address, QNetworkInterface::allAddresses()) { + _localAddress.append(QHostAddress(address)); + } moveToThread(this); } @@ -158,10 +140,36 @@ QString UDPLink::getName() const return _udpConfig->name(); } +bool UDPLink::_isIpLocal(const QHostAddress& add) +{ + // In simulation and testing setups the vehicle and the GCS can be + // running on the same host. This leads to packets arriving through + // the local network or the loopback adapter, which makes it look + // like the vehicle is connected through two different links, + // complicating routing. + // + // We detect this case and force all traffic to a simulated instance + // onto the local loopback interface. + // Run through all IPv4 interfaces and check if their canonical + // IP address in string representation matches the source IP address + // + // On Windows, this is a very expensive call only Redmond would know + // why. As such, we make it once and keep the list locally. If a new + // interface shows up after we start, it won't be on this list. + foreach (const QHostAddress &address, _localAddress) { + if (address == add) { + // This is a local address of the same host + return true; + } + } + return false; +} + void UDPLink::_writeBytes(const QByteArray data) { - if (!_socket) + if (!_socket) { return; + } // Send to all manually targeted systems foreach(UDPCLient* target, _udpConfig->targetHosts()) { // Skip it if it's part of the session clients below @@ -177,6 +185,7 @@ void UDPLink::_writeBytes(const QByteArray data) void UDPLink::_writeDataGram(const QByteArray data, const UDPCLient* target) { + //qDebug() << "UDP Out" << target->address << target->port; if(_socket->writeDatagram(data, target->address, target->port) < 0) { qWarning() << "Error writing to" << target->address << target->port; } else { @@ -193,6 +202,9 @@ void UDPLink::_writeDataGram(const QByteArray data, const UDPCLient* target) **/ void UDPLink::readBytes() { + if (!_socket) { + return; + } QByteArray databuffer; while (_socket->hasPendingDatagrams()) { @@ -200,6 +212,7 @@ void UDPLink::readBytes() datagram.resize(_socket->pendingDatagramSize()); QHostAddress sender; quint16 senderPort; + //-- Note: This call is broken in Qt 5.9.3 on Windows. It always returns a blank sender and 0 for the port. _socket->readDatagram(datagram.data(), datagram.size(), &sender, &senderPort); databuffer.append(datagram); //-- Wait a bit before sending it over @@ -213,7 +226,7 @@ void UDPLink::readBytes() // would trigger this. // Add host to broadcast list if not yet present, or update its port QHostAddress asender = sender; - if(is_ip_local(sender)) { + if(_isIpLocal(sender)) { asender = QHostAddress(QString("127.0.0.1")); } if(!contains_target(_sessionTargets, asender, senderPort)) { @@ -272,7 +285,7 @@ bool UDPLink::_hardwareConnect() _socket = NULL; } QHostAddress host = QHostAddress::AnyIPv4; - _socket = new QUdpSocket(); + _socket = new QUdpSocket(this); _socket->setProxy(QNetworkProxy::NoProxy); _connectState = _socket->bind(host, _udpConfig->localPort(), QAbstractSocket::ReuseAddressHint | QUdpSocket::ShareAddress); if (_connectState) { diff --git a/src/comm/UDPLink.h b/src/comm/UDPLink.h index 6e12c6af7bdef0ad4b2a951cc1ee2f5d1ef94e4e..e88782e04c884fcca9c2746bad1972f2b7eb4da8 100644 --- a/src/comm/UDPLink.h +++ b/src/comm/UDPLink.h @@ -184,6 +184,7 @@ private: bool _connect (void) override; void _disconnect (void) override; + bool _isIpLocal (const QHostAddress& add); bool _hardwareConnect (); void _restartConnection (); void _registerZeroconf (uint16_t port, const std::string& regType); @@ -194,11 +195,12 @@ private: DNSServiceRef _dnssServiceRef; #endif - bool _running; - QUdpSocket* _socket; - UDPConfiguration* _udpConfig; - bool _connectState; - QList _sessionTargets; + bool _running; + QUdpSocket* _socket; + UDPConfiguration* _udpConfig; + bool _connectState; + QList _sessionTargets; + QList _localAddress; }; diff --git a/src/comm/USBBoardInfo.json b/src/comm/USBBoardInfo.json index 535b81910f8f90210d4cbe31ec06f8110bd30cef..53013b6aacca59e6b26c9903bf8d9d522460d8f6 100644 --- a/src/comm/USBBoardInfo.json +++ b/src/comm/USBBoardInfo.json @@ -18,6 +18,7 @@ { "vendorID": 9900, "productID": 64, "boardClass": "Pixhawk", "name": "TAP V1" }, { "vendorID": 9900, "productID": 65, "boardClass": "Pixhawk", "name": "ASC V1" }, { "vendorID": 9900, "productID": 22, "boardClass": "Pixhawk", "name": "Crazyflie 2" }, + { "vendorID": 9900, "productID": 1, "boardClass": "Pixhawk", "name": "Omnibus F4 SD" }, { "vendorID": 9900, "productID": 21, "boardClass": "PX4 Flow", "name": "PX4 Flow" }, @@ -53,6 +54,7 @@ { "regExp": "^PX4 FMU", "boardClass": "Pixhawk" }, { "regExp": "^PX4 Crazyflie v2.0", "boardClass": "Pixhawk" }, { "regExp": "^Crazyflie BL", "boardClass": "Pixhawk" }, + { "regExp": "^PX4 OmnibusF4SD", "boardClass": "Pixhawk" }, { "regExp": "PX4.*Flow", "boardClass": "PX4 Flow" }, { "regExp": "^FT231X USB UART$", "boardClass": "SiK Radio" }, { "regExp": "USB UART$", "boardClass": "SiK Radio", "androidOnly": true, "comment": "Very broad fallback, too dangerous for non-android" } diff --git a/src/qgcunittest/UnitTest.cc b/src/qgcunittest/UnitTest.cc index bf0f0854c8d49628a2742332b7ae2670baaf55a9..a39b8cd4db99b7f436af89ed4b0179d7224d1c37 100644 --- a/src/qgcunittest/UnitTest.cc +++ b/src/qgcunittest/UnitTest.cc @@ -524,11 +524,14 @@ bool UnitTest::doubleNaNCompare(double value1, double value2) } } -void UnitTest::changeFactValue(Fact* fact) +void UnitTest::changeFactValue(Fact* fact,double increment) { if (fact->typeIsBool()) { fact->setRawValue(!fact->rawValue().toBool()); } else { - fact->setRawValue(fact->rawValue().toDouble() + 1); + if (increment == 0) { + increment = 1; + } + fact->setRawValue(fact->rawValue().toDouble() + increment); } } diff --git a/src/qgcunittest/UnitTest.h b/src/qgcunittest/UnitTest.h index 6690284e6f262ca42d62a3538383c948a2375e3b..34b18484e2e96ffb9817a0235ab34f187d8b3185 100644 --- a/src/qgcunittest/UnitTest.h +++ b/src/qgcunittest/UnitTest.h @@ -98,7 +98,8 @@ public: static bool doubleNaNCompare(double value1, double value2); /// Changes the Facts rawValue such that it emits a valueChanged signal. - void changeFactValue(Fact* fact); + /// @param increment 0 use standard increment, other increment by specified amount if double value + void changeFactValue(Fact* fact, double increment = 0); protected slots: diff --git a/src/qgcunittest/UnitTestList.cc b/src/qgcunittest/UnitTestList.cc index 97f11ce3a0dfa6ed4519556ebd1701152a21a5dd..834e7b1c5904eb88fcc8dab82cdb86fd541f1b84 100644 --- a/src/qgcunittest/UnitTestList.cc +++ b/src/qgcunittest/UnitTestList.cc @@ -20,7 +20,7 @@ #include "MessageBoxTest.h" #include "MissionItemTest.h" #include "SimpleMissionItemTest.h" -#include "SurveyMissionItemTest.h" +#include "SurveyComplexItemTest.h" #include "MissionControllerTest.h" #include "MissionManagerTest.h" #include "RadioConfigTest.h" @@ -63,7 +63,7 @@ UT_REGISTER_TEST(ParameterManagerTest) UT_REGISTER_TEST(MissionCommandTreeTest) UT_REGISTER_TEST(LogDownloadTest) UT_REGISTER_TEST(SendMavCommandTest) -UT_REGISTER_TEST(SurveyMissionItemTest) +UT_REGISTER_TEST(SurveyComplexItemTest) UT_REGISTER_TEST(CameraSectionTest) UT_REGISTER_TEST(SpeedSectionTest) UT_REGISTER_TEST(PlanMasterControllerTest) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 1258ead866b14b941d22a1ed182b83082b822de7..cec9a70b7f08b274998d6e675772e309ad9efc1d 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -73,10 +73,6 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi attitudeStamped(false), lastAttitude(0), - roll(0.0), - pitch(0.0), - yaw(0.0), - imagePackets(0), // We must initialize to 0, otherwise extended data packets maybe incorrectly thought to be images blockHomePositionChanges(false), @@ -133,7 +129,6 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi #ifndef __mobile__ connect(_vehicle, &Vehicle::mavlinkMessageReceived, &fileManager, &FileManager::receiveMessage); - color = UASInterface::getNextColor(); #endif } @@ -184,7 +179,7 @@ void UAS::receiveMessage(mavlink_message_t message) // Only accept messages from this system (condition 1) // and only then if a) attitudeStamped is disabled OR b) attitudeStamped is enabled // and we already got one attitude packet - if (message.sysid == uasId && (!attitudeStamped || (attitudeStamped && (lastAttitude != 0)) || message.msgid == MAVLINK_MSG_ID_ATTITUDE)) + if (message.sysid == uasId && (!attitudeStamped || lastAttitude != 0 || message.msgid == MAVLINK_MSG_ID_ATTITUDE)) { QString uasState; QString stateDescription; @@ -275,86 +270,6 @@ void UAS::receiveMessage(mavlink_message_t message) emit valueChanged(uasId, name.arg("drop_rate_comm"), "%", state.drop_rate_comm/100.0f, time); } break; - case MAVLINK_MSG_ID_ATTITUDE: - { - mavlink_attitude_t attitude; - mavlink_msg_attitude_decode(&message, &attitude); - quint64 time = getUnixReferenceTime(attitude.time_boot_ms); - - emit attitudeChanged(this, message.compid, QGC::limitAngleToPMPIf(attitude.roll), QGC::limitAngleToPMPIf(attitude.pitch), QGC::limitAngleToPMPIf(attitude.yaw), time); - - if (!wrongComponent) - { - lastAttitude = time; - setRoll(QGC::limitAngleToPMPIf(attitude.roll)); - setPitch(QGC::limitAngleToPMPIf(attitude.pitch)); - setYaw(QGC::limitAngleToPMPIf(attitude.yaw)); - - attitudeKnown = true; - emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time); - } - } - break; - case MAVLINK_MSG_ID_ATTITUDE_QUATERNION: - { - mavlink_attitude_quaternion_t attitude; - mavlink_msg_attitude_quaternion_decode(&message, &attitude); - quint64 time = getUnixReferenceTime(attitude.time_boot_ms); - - double a = attitude.q1; - double b = attitude.q2; - double c = attitude.q3; - double d = attitude.q4; - - double aSq = a * a; - double bSq = b * b; - double cSq = c * c; - double dSq = d * d; - float dcm[3][3]; - dcm[0][0] = aSq + bSq - cSq - dSq; - dcm[0][1] = 2.0 * (b * c - a * d); - dcm[0][2] = 2.0 * (a * c + b * d); - dcm[1][0] = 2.0 * (b * c + a * d); - dcm[1][1] = aSq - bSq + cSq - dSq; - dcm[1][2] = 2.0 * (c * d - a * b); - dcm[2][0] = 2.0 * (b * d - a * c); - dcm[2][1] = 2.0 * (a * b + c * d); - dcm[2][2] = aSq - bSq - cSq + dSq; - - float phi, theta, psi; - theta = asin(-dcm[2][0]); - - if (fabs(theta - M_PI_2) < 1.0e-3f) { - phi = 0.0f; - psi = (atan2(dcm[1][2] - dcm[0][1], - dcm[0][2] + dcm[1][1]) + phi); - - } else if (fabs(theta + M_PI_2) < 1.0e-3f) { - phi = 0.0f; - psi = atan2f(dcm[1][2] - dcm[0][1], - dcm[0][2] + dcm[1][1] - phi); - - } else { - phi = atan2f(dcm[2][1], dcm[2][2]); - psi = atan2f(dcm[1][0], dcm[0][0]); - } - - emit attitudeChanged(this, message.compid, QGC::limitAngleToPMPIf(phi), - QGC::limitAngleToPMPIf(theta), - QGC::limitAngleToPMPIf(psi), time); - - if (!wrongComponent) - { - lastAttitude = time; - setRoll(QGC::limitAngleToPMPIf(phi)); - setPitch(QGC::limitAngleToPMPIf(theta)); - setYaw(QGC::limitAngleToPMPIf(psi)); - - attitudeKnown = true; - emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time); - } - } - break; case MAVLINK_MSG_ID_HIL_CONTROLS: { mavlink_hil_controls_t hil; @@ -362,27 +277,6 @@ void UAS::receiveMessage(mavlink_message_t message) emit hilControlsChanged(hil.time_usec, hil.roll_ailerons, hil.pitch_elevator, hil.yaw_rudder, hil.throttle, hil.mode, hil.nav_mode); } break; - case MAVLINK_MSG_ID_VFR_HUD: - { - mavlink_vfr_hud_t hud; - mavlink_msg_vfr_hud_decode(&message, &hud); - quint64 time = getUnixTime(); - - if (!attitudeKnown) - { - setYaw(QGC::limitAngleToPMPId((((double)hud.heading)/180.0)*M_PI)); - emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time); - } - } - break; - case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE: - { - mavlink_global_vision_position_estimate_t pos; - mavlink_msg_global_vision_position_estimate_decode(&message, &pos); - quint64 time = getUnixTime(pos.usec); - emit attitudeChanged(this, message.compid, pos.roll, pos.pitch, pos.yaw, time); - } - break; case MAVLINK_MSG_ID_PARAM_VALUE: { diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 421941a52d7531daf7ff9b274712c6b79525289c..92cb0cf1623901a33e1a16b80be566e837b6a386 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -60,46 +60,9 @@ public: /** @brief The time interval the robot is switched on */ quint64 getUptime() const; - Q_PROPERTY(double roll READ getRoll WRITE setRoll NOTIFY rollChanged) - Q_PROPERTY(double pitch READ getPitch WRITE setPitch NOTIFY pitchChanged) - Q_PROPERTY(double yaw READ getYaw WRITE setYaw NOTIFY yawChanged) - - /// Vehicle is about to go away - void shutdownVehicle(void); - - void setRoll(double val) - { - roll = val; - emit rollChanged(val,"roll"); - } - - double getRoll() const - { - return roll; - } - - void setPitch(double val) - { - pitch = val; - emit pitchChanged(val,"pitch"); - } - - double getPitch() const - { - return pitch; - } - - void setYaw(double val) - { - yaw = val; - emit yawChanged(val,"yaw"); - } - - double getYaw() const - { - return yaw; - } - + /// Vehicle is about to go away + void shutdownVehicle(void); + // Setters for HIL noise variance void setXaccVar(float var){ xacc_var = var; @@ -196,9 +159,6 @@ protected: //COMMENTS FOR TEST UNIT bool attitudeKnown; ///< True if attitude was received, false else bool attitudeStamped; ///< Should arriving data be timestamped with the last attitude? This helps with broken system time clocks on the MAV quint64 lastAttitude; ///< Timestamp of last attitude measurement - double roll; - double pitch; - double yaw; // dongfang: This looks like a candidate for being moved off to a separate class. /// IMAGING diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index af0257e7f47833b8d2ac8e10b2083fc685368d00..ffee6b2f6af55657a173572dc4698b7209a4127a 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -45,62 +45,12 @@ public: /** @brief The time interval the robot is switched on **/ virtual quint64 getUptime() const = 0; - virtual double getRoll() const = 0; - virtual double getPitch() const = 0; - virtual double getYaw() const = 0; - #ifndef __mobile__ virtual FileManager* getFileManager() = 0; #endif - /** - * @brief Get the color for this UAS - * - * This static function holds a color map that allows to draw a new color for each robot - * - * @return The next color in the color map. The map holds 20 colors and starts from the beginning - * if the colors are exceeded. - */ -#if !defined(__mobile__) - static QColor getNextColor() { - /* Create color map */ - static QList colors = QList() - << QColor(231,72,28) - << QColor(104,64,240) - << QColor(203,254,121) - << QColor(161,252,116) - << QColor(232,33,47) - << QColor(116,251,110) - << QColor(234,38,107) - << QColor(104,250,138) - << QColor(235,43,165) - << QColor(98,248,176) - << QColor(236,48,221) - << QColor(92,247,217) - << QColor(200,54,238) - << QColor(87,231,246) - << QColor(151,59,239) - << QColor(81,183,244) - << QColor(75,133,243) - << QColor(242,255,128) - << QColor(230,126,23); - - static int nextColor = -1; - if(nextColor == 18){//if at the end of the list - nextColor = -1;//go back to the beginning - } - nextColor++; - return colors[nextColor];//return the next color - } -#endif - virtual QMap getComponents() = 0; - QColor getColor() - { - return color; - } - enum StartCalibrationType { StartCalibrationRadio, StartCalibrationGyro, @@ -160,9 +110,6 @@ public slots: /** @brief Send command to disable all bindings/maps between RC and parameters */ virtual void unsetRCToParameterMap() = 0; -protected: - QColor color; - signals: /** @brief A text message from the system has been received */ void textMessageReceived(int uasid, int componentid, int severity, QString text); @@ -213,8 +160,6 @@ signals: */ void batteryChanged(UASInterface* uas, double voltage, double current, double percent, int seconds); void statusChanged(UASInterface* uas, QString status); - void attitudeChanged(UASInterface*, double roll, double pitch, double yaw, quint64 usec); - void attitudeChanged(UASInterface*, int component, double roll, double pitch, double yaw, quint64 usec); void imageStarted(int imgid, int width, int height, int depth, int channels); void imageDataReceived(int imgid, const unsigned char* imageData, int length, int startIndex); diff --git a/src/ui/AppSettings.qml b/src/ui/AppSettings.qml index 3b9d5a02972b80eaeca5525092d2d915af022959..3f1f533e337cf53501891a1e8659b5cce43f570c 100644 --- a/src/ui/AppSettings.qml +++ b/src/ui/AppSettings.qml @@ -58,8 +58,7 @@ Rectangle { property real _maxButtonWidth: 0 QGCLabel { - anchors.left: parent.left - anchors.right: parent.right + Layout.fillWidth: true text: qsTr("Application Settings") wrapMode: Text.WordWrap horizontalAlignment: Text.AlignHCenter diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 457948e579289aeaade308589b500707e240ac29..9fa3240ba948b203d0e328f31a28a93134b9f5ed 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -67,7 +67,6 @@ enum DockWidgetTypes { MAVLINK_INSPECTOR, CUSTOM_COMMAND, ONBOARD_FILES, - DEPRECATED_WIDGET, HIL_CONFIG, ANALYZE }; @@ -76,7 +75,6 @@ static const char *rgDockWidgetNames[] = { "MAVLink Inspector", "Custom Command", "Onboard Files", - "Deprecated Widget", "HIL Config", "Analyze" }; diff --git a/src/ui/MainWindowHybrid.qml b/src/ui/MainWindowHybrid.qml index 8f58d2253d736dbbcfef220880797d5476b8cd6a..a320d9092132ebd755d0e078aa1358c7cc41ddc4 100644 --- a/src/ui/MainWindowHybrid.qml +++ b/src/ui/MainWindowHybrid.qml @@ -22,16 +22,19 @@ Item { } function attemptWindowClose() { - try { - mainWindowInner.item.attemptWindowClose() - } - finally { + if(!mainWindowInner.item) { controller.reallyClose() + } else { + mainWindowInner.item.attemptWindowClose() } } function showMessage(message) { - mainWindowInner.item.showMessage(message) + if(mainWindowInner.item) { + mainWindowInner.item.showMessage(message) + } else { + console.log(message) + } } Loader { diff --git a/src/ui/MainWindowInner.qml b/src/ui/MainWindowInner.qml index e4b6069bda104a8f54a49074eef213ca97767512..4a8af1113edf204397dd116f263e5bb7c5655c07 100644 --- a/src/ui/MainWindowInner.qml +++ b/src/ui/MainWindowInner.qml @@ -175,7 +175,7 @@ Item { MessageDialog { id: activeConnectionsCloseDialog title: qsTr("%1 close").arg(QGroundControl.appName) - text: qsTr("There are still active connections to vehicles. Do you want to disconnect these before closing?") + text: qsTr("There are still active connections to vehicles. Are you sure you want to exit?") standardButtons: StandardButton.Yes | StandardButton.Cancel modality: Qt.ApplicationModal visible: false @@ -212,10 +212,10 @@ Item { } } - function formatMessage(message) { - message = message.replace(new RegExp("<#E>", "g"), "color: #f95e5e; font: " + (ScreenTools.defaultFontPointSize.toFixed(0) - 1) + "pt monospace;"); - message = message.replace(new RegExp("<#I>", "g"), "color: #f9b55e; font: " + (ScreenTools.defaultFontPointSize.toFixed(0) - 1) + "pt monospace;"); - message = message.replace(new RegExp("<#N>", "g"), "color: #ffffff; font: " + (ScreenTools.defaultFontPointSize.toFixed(0) - 1) + "pt monospace;"); + function formatMessage(message) { + message = message.replace(new RegExp("<#E>", "g"), "color: " + qgcPal.warningText + "; font: " + (ScreenTools.defaultFontPointSize.toFixed(0) - 1) + "pt monospace;"); + message = message.replace(new RegExp("<#I>", "g"), "color: " + qgcPal.warningText + "; font: " + (ScreenTools.defaultFontPointSize.toFixed(0) - 1) + "pt monospace;"); + message = message.replace(new RegExp("<#N>", "g"), "color: " + qgcPal.text + "; font: " + (ScreenTools.defaultFontPointSize.toFixed(0) - 1) + "pt monospace;"); return message; } @@ -400,22 +400,22 @@ Item { //------------------------------------------------------------------------- //-- System Message Area Rectangle { - id: messageArea + id: messageArea + width: mainWindow.width * 0.5 + height: mainWindow.height * 0.5 + anchors.horizontalCenter: parent.horizontalCenter + anchors.top: parent.top + anchors.topMargin: toolBar.height + ScreenTools.defaultFontPixelHeight + radius: ScreenTools.defaultFontPixelHeight * 0.5 + color: qgcPal.window + border.color: qgcPal.text + visible: false + function close() { currentPopUp = null messageText.text = "" messageArea.visible = false } - width: mainWindow.width * 0.5 - height: mainWindow.height * 0.5 - color: Qt.rgba(0,0,0,0.8) - visible: false - radius: ScreenTools.defaultFontPixelHeight * 0.5 - border.color: "#808080" - border.width: 2 - anchors.horizontalCenter: parent.horizontalCenter - anchors.top: parent.top - anchors.topMargin: toolBar.height + ScreenTools.defaultFontPixelHeight MouseArea { // This MouseArea prevents the Map below it from getting Mouse events. Without this // things like mousewheel will scroll the Flickable and then scroll the map as well. @@ -435,11 +435,11 @@ Item { id: messageText readOnly: true textFormat: TextEdit.RichText - color: "white" + color: qgcPal.text } } //-- Dismiss System Message - Image { + QGCColoredImage { anchors.margins: ScreenTools.defaultFontPixelHeight * 0.5 anchors.top: parent.top anchors.right: parent.right @@ -450,6 +450,7 @@ Item { fillMode: Image.PreserveAspectFit mipmap: true smooth: true + color: qgcPal.text MouseArea { anchors.fill: parent anchors.margins: ScreenTools.isMobile ? -ScreenTools.defaultFontPixelHeight : 0 @@ -459,7 +460,7 @@ Item { } } //-- Clear Messages - Image { + QGCColoredImage { anchors.bottom: parent.bottom anchors.right: parent.right anchors.margins: ScreenTools.defaultFontPixelHeight * 0.5 @@ -470,6 +471,7 @@ Item { fillMode: Image.PreserveAspectFit mipmap: true smooth: true + color: qgcPal.text MouseArea { anchors.fill: parent onClicked: { diff --git a/src/ui/QGCMAVLinkLogPlayer.cc b/src/ui/QGCMAVLinkLogPlayer.cc index cc79b222fafc41998f102479f7b41c5dbb600cca..6a7d83b6eefe3cb054a0463bdff7d206823a0cc7 100644 --- a/src/ui/QGCMAVLinkLogPlayer.cc +++ b/src/ui/QGCMAVLinkLogPlayer.cc @@ -15,10 +15,11 @@ #include "QGCQFileDialog.h" #include "QGCMessageBox.h" -QGCMAVLinkLogPlayer::QGCMAVLinkLogPlayer(QWidget *parent) : - QWidget(parent), - _replayLink(NULL), - _ui(new Ui::QGCMAVLinkLogPlayer) +QGCMAVLinkLogPlayer::QGCMAVLinkLogPlayer(QWidget *parent) + : QWidget (parent) + , _replayLink (NULL) + , _lastCurrentTime (0) + , _ui (new Ui::QGCMAVLinkLogPlayer) { _ui->setupUi(this); _ui->horizontalLayout->setAlignment(Qt::AlignTop); @@ -81,8 +82,6 @@ void QGCMAVLinkLogPlayer::_selectLogFileForPlayback(void) return; } - LinkInterface* createConnectedLink(LinkConfiguration* config); - LogReplayLinkConfiguration* linkConfig = new LogReplayLinkConfiguration(QString("Log Replay")); linkConfig->setLogFilename(logFilename); linkConfig->setName(linkConfig->logFilenameShort()); @@ -92,11 +91,12 @@ void QGCMAVLinkLogPlayer::_selectLogFileForPlayback(void) SharedLinkConfigurationPointer sharedConfig = linkMgr->addConfiguration(linkConfig); _replayLink = (LogReplayLink*)qgcApp()->toolbox()->linkManager()->createConnectedLink(sharedConfig); - connect(_replayLink, &LogReplayLink::logFileStats, this, &QGCMAVLinkLogPlayer::_logFileStats); - connect(_replayLink, &LogReplayLink::playbackStarted, this, &QGCMAVLinkLogPlayer::_playbackStarted); - connect(_replayLink, &LogReplayLink::playbackPaused, this, &QGCMAVLinkLogPlayer::_playbackPaused); - connect(_replayLink, &LogReplayLink::playbackPercentCompleteChanged, this, &QGCMAVLinkLogPlayer::_playbackPercentCompleteChanged); - connect(_replayLink, &LogReplayLink::disconnected, this, &QGCMAVLinkLogPlayer::_replayLinkDisconnected); + connect(_replayLink, &LogReplayLink::logFileStats, this, &QGCMAVLinkLogPlayer::_logFileStats); + connect(_replayLink, &LogReplayLink::playbackStarted, this, &QGCMAVLinkLogPlayer::_playbackStarted); + connect(_replayLink, &LogReplayLink::playbackPaused, this, &QGCMAVLinkLogPlayer::_playbackPaused); + connect(_replayLink, &LogReplayLink::playbackPercentCompleteChanged, this, &QGCMAVLinkLogPlayer::_playbackPercentCompleteChanged); + connect(_replayLink, &LogReplayLink::currentLogTimeSecs, this, &QGCMAVLinkLogPlayer::_setCurrentLogTime); + connect(_replayLink, &LogReplayLink::disconnected, this, &QGCMAVLinkLogPlayer::_replayLinkDisconnected); _ui->positionSlider->setValue(0); #if 0 @@ -133,7 +133,7 @@ void QGCMAVLinkLogPlayer::_logFileStats(bool logTimestamped, ///< tru _logDurationSeconds = logDurationSeconds; - _ui->logStatsLabel->setText(_secondsToHMS(logDurationSeconds)); + _ui->logLengthTime->setText(_secondsToHMS(logDurationSeconds)); } /// Signalled from LogReplayLink when replay starts @@ -208,3 +208,11 @@ void QGCMAVLinkLogPlayer::_replayLinkDisconnected(void) _enablePlaybackControls(false); _replayLink = NULL; } + +void QGCMAVLinkLogPlayer::_setCurrentLogTime(int secs) +{ + if (secs != _lastCurrentTime) { + _lastCurrentTime = secs; + _ui->logCurrentTime->setText(_secondsToHMS(secs)); + } +} diff --git a/src/ui/QGCMAVLinkLogPlayer.h b/src/ui/QGCMAVLinkLogPlayer.h index da63a27e5bf0e060ff9f28453a996742ba3c881d..f83d268347b3a765d180372caa99d411f13a81f2 100644 --- a/src/ui/QGCMAVLinkLogPlayer.h +++ b/src/ui/QGCMAVLinkLogPlayer.h @@ -42,6 +42,7 @@ private slots: void _playbackPercentCompleteChanged(int percentComplete); void _playbackError(void); void _replayLinkDisconnected(void); + void _setCurrentLogTime(int secs); private: void _finishPlayback(void); @@ -50,6 +51,7 @@ private: LogReplayLink* _replayLink; int _logDurationSeconds; + int _lastCurrentTime; Ui::QGCMAVLinkLogPlayer* _ui; }; diff --git a/src/ui/QGCMAVLinkLogPlayer.ui b/src/ui/QGCMAVLinkLogPlayer.ui index 6693ac3366b171efa74c07c2a03296c90f86f714..f80f6ac95533a8a71b45360e7c7eedfea3560d73 100644 --- a/src/ui/QGCMAVLinkLogPlayer.ui +++ b/src/ui/QGCMAVLinkLogPlayer.ui @@ -27,7 +27,7 @@ 0 - + @@ -89,6 +89,13 @@ + + + + + + + diff --git a/src/ui/QGCMapRCToParamDialog.cpp b/src/ui/QGCMapRCToParamDialog.cpp index 309681c8cc6f105f980aaa49a8a63cd5d5d1d83d..61291b738d56a6603e79e63ac021b716cfde813c 100644 --- a/src/ui/QGCMapRCToParamDialog.cpp +++ b/src/ui/QGCMapRCToParamDialog.cpp @@ -43,7 +43,7 @@ QGCMapRCToParamDialog::QGCMapRCToParamDialog(QString param_id, UASInterface *mav ui->paramIdLabel->setText(param_id); - connect(paramFact, &Fact::valueChanged, this, &QGCMapRCToParamDialog::_parameterUpdated); + connect(paramFact, &Fact::vehicleUpdated, this, &QGCMapRCToParamDialog::_parameterUpdated); vehicle->parameterManager()->refreshParameter(FactSystem::defaultComponentId, param_id); } diff --git a/src/ui/preferences/GeneralSettings.qml b/src/ui/preferences/GeneralSettings.qml index e74cfe334f5cfb5fe4d05d06a2c5604a449a5b41..9c78b413244111a6d808ceb1bdb2f9a7920de0d2 100644 --- a/src/ui/preferences/GeneralSettings.qml +++ b/src/ui/preferences/GeneralSettings.qml @@ -12,7 +12,6 @@ import QtQuick 2.3 import QtQuick.Controls 1.2 import QtQuick.Controls.Styles 1.4 import QtQuick.Dialogs 1.2 -import QtMultimedia 5.5 import QtQuick.Layouts 1.2 import QGroundControl 1.0 @@ -41,6 +40,7 @@ QGCView { property real _editFieldWidth: ScreenTools.defaultFontPixelWidth * 30 property Fact _mapProvider: QGroundControl.settingsManager.flightMapSettings.mapProvider property Fact _mapType: QGroundControl.settingsManager.flightMapSettings.mapType + property Fact _followTarget: QGroundControl.settingsManager.appSettings.followTarget property real _panelWidth: _qgcView.width * _internalWidthRatio readonly property real _internalWidthRatio: 0.8 @@ -245,6 +245,23 @@ QGCView { } } //----------------------------------------------------------------- + //-- Follow Target + Row { + spacing: ScreenTools.defaultFontPixelWidth + visible: _followTarget.visible + QGCLabel { + text: qsTr("Stream GCS Position:") + width: _labelWidth + anchors.verticalCenter: parent.verticalCenter + } + FactComboBox { + width: _editFieldWidth + fact: _followTarget + indexModel: false + anchors.verticalCenter: parent.verticalCenter + } + } + //----------------------------------------------------------------- //-- Audio preferences FactCheckBox { text: qsTr("Mute all audio output") @@ -367,11 +384,11 @@ QGCView { visible: _savePath.visible && !ScreenTools.isMobile QGCLabel { - anchors.baseline: savePathBrowse.baseline - text: qsTr("File Save Path:") + Layout.alignment: Qt.AlignVCenter + text: qsTr("File Save Path:") } QGCLabel { - anchors.baseline: savePathBrowse.baseline + Layout.alignment: Qt.AlignVCenter Layout.maximumWidth: _panelWidth * 0.5 elide: Text.ElideMiddle text: _savePath.rawValue === "" ? qsTr("") : _savePath.value @@ -393,6 +410,16 @@ QGCView { } } } + + //----------------------------------------------------------------- + //-- Checklist Settings + FactCheckBox { + text: qsTr("Use preflight checklist") + fact: _useChecklist + visible: _useChecklist.visible + + property Fact _useChecklist: QGroundControl.settingsManager.appSettings.useChecklist + } } } diff --git a/src/ui/preferences/SerialSettings.qml b/src/ui/preferences/SerialSettings.qml index 8438549d070b9ad7a6a3b8306bc3168c26accf53..0b9ba0f31e164ec3db0806003826c4e7720d8a20 100644 --- a/src/ui/preferences/SerialSettings.qml +++ b/src/ui/preferences/SerialSettings.qml @@ -59,26 +59,41 @@ Item { id: commPortCombo anchors.verticalCenter: parent.verticalCenter width: _secondColumn - model: QGroundControl.linkManager.serialPortStrings visible: QGroundControl.linkManager.serialPortStrings.length > 0 + onActivated: { if (index != -1) { - subEditConfig.portName = QGroundControl.linkManager.serialPorts[index] + if (index >= QGroundControl.linkManager.serialPortStrings.length) { + // This item was adding at the end, must use added text as name + subEditConfig.portName = commPortCombo.textAt(index) + } else { + subEditConfig.portName = QGroundControl.linkManager.serialPorts[index] + } } } Component.onCompleted: { - if(subEditConfig != null) { - if(subEditConfig.portDisplayName === "" && QGroundControl.linkManager.serialPorts.length > 0) + var index + var serialPorts = [ ] + + for (var i=0; i 0) { subEditConfig.portName = QGroundControl.linkManager.serialPorts[0] - var index = commPortCombo.find(subEditConfig.portDisplayName) + } + index = serialPorts.indexOf(subEditConfig.portDisplayName) if (index === -1) { - console.warn(qsTr("Serial Port not present"), subEditConfig.portName) - } else { - commPortCombo.currentIndex = index + serialPorts.push(subEditConfig.portName) + index = serialPorts.indexOf(subEditConfig.portName) } } else { - commPortCombo.currentIndex = 0 + index = 0 } + + commPortCombo.model = serialPorts + commPortCombo.currentIndex = index } } } diff --git a/src/ui/toolbar/LinkIndicator.qml b/src/ui/toolbar/LinkIndicator.qml new file mode 100644 index 0000000000000000000000000000000000000000..a7238d44d6a85941ed1b92304d119ca03e973c88 --- /dev/null +++ b/src/ui/toolbar/LinkIndicator.qml @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + + +import QtQuick 2.3 +import QtQuick.Controls 1.2 + +import QGroundControl 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.MultiVehicleManager 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Palette 1.0 +import QGroundControl.Vehicle 1.0 + +//------------------------------------------------------------------------- +//-- Link Indicator +Item { + anchors.top: parent.top + anchors.bottom: parent.bottom + width: priorityLinkSelector.width + + property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle + property bool _visible: false + + QGCLabel { + id: priorityLinkSelector + text: _activeVehicle ? _activeVehicle.priorityLinkName : qsTr("N/A", "No data to display") + font.pointSize: ScreenTools.mediumFontPointSize + color: qgcPal.buttonText + anchors.verticalCenter: parent.verticalCenter + visible: _visible + Menu { + id: linkSelectionMenu + } + Component { + id: linkSelectionMenuItemComponent + MenuItem { + onTriggered: _activeVehicle.priorityLinkName = text + } + } + property var linkSelectionMenuItems: [] + function updatelinkSelectionMenu() { + if (_activeVehicle) { + // Remove old menu items + for (var i = 0; i < linkSelectionMenuItems.length; i++) { + linkSelectionMenu.removeItem(linkSelectionMenuItems[i]) + } + linkSelectionMenuItems.length = 0 + + // Add new items + var has_hl = false; + var links = _activeVehicle.links + for (var i = 0; i < links.length; i++) { + var menuItem = linkSelectionMenuItemComponent.createObject(null, { "text": links[i].getName(), "enabled": links[i].link_active(_activeVehicle.id)}) + linkSelectionMenuItems.push(menuItem) + linkSelectionMenu.insertItem(i, menuItem) + + if (links[i].getHighLatency()) { + has_hl = true + } + } + + _visible = links.length > 1 && has_hl + } + } + + Component.onCompleted: priorityLinkSelector.updatelinkSelectionMenu() + + Connections { + target: QGroundControl.multiVehicleManager + onActiveVehicleChanged: priorityLinkSelector.updatelinkSelectionMenu() + } + + Connections { + target: _activeVehicle + onLinksChanged: priorityLinkSelector.updatelinkSelectionMenu() + } + + Connections { + target: _activeVehicle + onLinksPropertiesChanged: priorityLinkSelector.updatelinkSelectionMenu() + } + + MouseArea { + visible: _activeVehicle + anchors.fill: parent + onClicked: linkSelectionMenu.popup() + } + } +} diff --git a/src/ui/toolbar/MainToolBar.qml b/src/ui/toolbar/MainToolBar.qml index dffaa83e75d05011b7ccd8f091acede2d90cd442..7d737e0f66cc4c1919f597d291649920a9b8b02a 100644 --- a/src/ui/toolbar/MainToolBar.qml +++ b/src/ui/toolbar/MainToolBar.qml @@ -81,10 +81,9 @@ Rectangle { //--------------------------------------------- // Toolbar Row Row { - id: viewRow - anchors.top: parent.top - anchors.bottom: parent.bottom - spacing: ScreenTools.defaultFontPixelWidth / 2 + id: viewRow + Layout.fillHeight: true + spacing: ScreenTools.defaultFontPixelWidth / 2 ExclusiveGroup { id: mainActionGroup } @@ -153,7 +152,7 @@ Rectangle { width: ScreenTools.defaultFontPixelHeight * 8 text: "Vehicle " + (_activeVehicle ? _activeVehicle.id : "None") visible: QGroundControl.multiVehicleManager.vehicles.count > 1 - anchors.verticalCenter: parent.verticalCenter + Layout.alignment: Qt.AlignVCenter menu: vehicleMenu @@ -175,14 +174,15 @@ Rectangle { property var vehicleMenuItems: [] function updateVehicleMenu() { + var i; // Remove old menu items - for (var i = 0; i < vehicleMenuItems.length; i++) { + for (i = 0; i < vehicleMenuItems.length; i++) { vehicleMenu.removeItem(vehicleMenuItems[i]) } vehicleMenuItems.length = 0 // Add new items - for (var i=0; i