diff --git a/.gitmodules b/.gitmodules index 818cbaa50afd0f0b313b92d202a4b53949d03803..1b16c8bb9a10af018923cff13c07b1add88a8e5d 100644 --- a/.gitmodules +++ b/.gitmodules @@ -6,4 +6,4 @@ url = https://github.com/PX4/GpsDrivers.git [submodule "libs/mavlink/include/mavlink/v2.0"] path = libs/mavlink/include/mavlink/v2.0 - url = git://github.com/mavlink/c_library_v2.git + url = https://github.com/mavlink/c_library_v2.git diff --git a/.travis.yml b/.travis.yml index bfc0d3ead62c190aaec7b99fe4921cd74322deff..fbdd220804121a746d83cf477e99685374db30e2 100644 --- a/.travis.yml +++ b/.travis.yml @@ -173,15 +173,10 @@ script: fi #- ccache -s - # unit tests linux/osx - - if [ "${TRAVIS_BRANCH}" != "master" ]; then - if [[ "${SPEC}" = "linux-g++-64" && "${CONFIG}" = "debug" ]]; then - mkdir -p ~/.config/QtProject/ && cp ${TRAVIS_BUILD_DIR}/test/qtlogging.ini ~/.config/QtProject/ && - ./debug/QGroundControl --unittest; - elif [[ "${SPEC}" = "macx-clang" && "${CONFIG}" = "debug" ]]; then - mkdir -p ~/Library/Preferences/QtProject/ && cp ${TRAVIS_BUILD_DIR}/test/qtlogging.ini ~/Library/Preferences/QtProject/ && - ./debug/qgroundcontrol.app/Contents/MacOS/QGroundControl --unittest; - fi + # unit tests linux + - if [[ "${SPEC}" = "linux-g++-64" && "${CONFIG}" = "debug" ]]; then + mkdir -p ~/.config/QtProject/ && cp ${TRAVIS_BUILD_DIR}/test/qtlogging.ini ~/.config/QtProject/ && + ./debug/qgroundcontrol-start.sh --unittest; fi after_success: diff --git a/QGCSetup.pri b/QGCSetup.pri index 5328be08cc2ab0eeaa55bb1c7dc009b41f70c24d..fb87679a04c0abd54fed8a344c4198ffe9bc636c 100644 --- a/QGCSetup.pri +++ b/QGCSetup.pri @@ -69,98 +69,94 @@ WindowsBuild { QMAKE_POST_LINK += $$escape_expand(\\n) $$QMAKE_COPY \"$$COPY_FILE\" \"$$DESTDIR_WIN\" } - ReleaseBuild { + ReleaseBuild { # Copy Visual Studio DLLs # Note that this is only done for release because the debugging versions of these DLLs cannot be redistributed. win32-msvc2010 { QMAKE_POST_LINK += $$escape_expand(\\n) $$QMAKE_COPY \"C:\\Windows\\System32\\msvcp100.dll\" \"$$DESTDIR_WIN\" QMAKE_POST_LINK += $$escape_expand(\\n) $$QMAKE_COPY \"C:\\Windows\\System32\\msvcr100.dll\" \"$$DESTDIR_WIN\" - } - else:win32-msvc2012 { + + } else:win32-msvc2012 { QMAKE_POST_LINK += $$escape_expand(\\n) $$QMAKE_COPY \"C:\\Windows\\System32\\msvcp110.dll\" \"$$DESTDIR_WIN\" QMAKE_POST_LINK += $$escape_expand(\\n) $$QMAKE_COPY \"C:\\Windows\\System32\\msvcr110.dll\" \"$$DESTDIR_WIN\" - } - else:win32-msvc2013 { + + } else:win32-msvc2013 { QMAKE_POST_LINK += $$escape_expand(\\n) $$QMAKE_COPY \"C:\\Windows\\System32\\msvcp120.dll\" \"$$DESTDIR_WIN\" QMAKE_POST_LINK += $$escape_expand(\\n) $$QMAKE_COPY \"C:\\Windows\\System32\\msvcr120.dll\" \"$$DESTDIR_WIN\" - } - else:win32-msvc2015 { + + } else:win32-msvc2015 { QMAKE_POST_LINK += $$escape_expand(\\n) $$QMAKE_COPY \"C:\\Windows\\System32\\msvcp140.dll\" \"$$DESTDIR_WIN\" QMAKE_POST_LINK += $$escape_expand(\\n) $$QMAKE_COPY \"C:\\Windows\\System32\\vcruntime140.dll\" \"$$DESTDIR_WIN\" - } - else { + + } else { error("Visual studio version not supported, installation cannot be completed.") } } DEPLOY_TARGET = $$shell_quote($$shell_path($$DESTDIR_WIN\\$${TARGET}.exe)) QMAKE_POST_LINK += $$escape_expand(\\n) $$QT_BIN_DIR\\windeployqt --no-compiler-runtime --qmldir=$${BASEDIR_WIN}\\src $${DEPLOY_TARGET} - } LinuxBuild { - installer { - QMAKE_POST_LINK += && mkdir -p $$DESTDIR/Qt/libs && mkdir -p $$DESTDIR/Qt/plugins - - # QT_INSTALL_LIBS - QT_LIB_LIST = \ - libQt5Core.so.5 \ - libQt5DBus.so.5 \ - libQt5Gui.so.5 \ - libQt5Location.so.5 \ - libQt5Multimedia.so.5 \ - libQt5MultimediaQuick_p.so.5 \ - libQt5Network.so.5 \ - libQt5OpenGL.so.5 \ - libQt5Positioning.so.5 \ - libQt5PrintSupport.so.5 \ - libQt5Qml.so.5 \ - libQt5Quick.so.5 \ - libQt5QuickWidgets.so.5 \ - libQt5SerialPort.so.5 \ - libQt5Sql.so.5 \ - libQt5Svg.so.5 \ - libQt5Test.so.5 \ - libQt5Widgets.so.5 \ - libQt5XcbQpa.so.5 - - !contains(DEFINES, __rasp_pi2__) { - QT_LIB_LIST += \ - libicudata.so.56 \ - libicui18n.so.56 \ - libicuuc.so.56 - } + QMAKE_POST_LINK += && mkdir -p $$DESTDIR/Qt/libs && mkdir -p $$DESTDIR/Qt/plugins + + # QT_INSTALL_LIBS + QT_LIB_LIST = \ + libQt5Core.so.5 \ + libQt5DBus.so.5 \ + libQt5Gui.so.5 \ + libQt5Location.so.5 \ + libQt5Multimedia.so.5 \ + libQt5MultimediaQuick_p.so.5 \ + libQt5Network.so.5 \ + libQt5OpenGL.so.5 \ + libQt5Positioning.so.5 \ + libQt5PrintSupport.so.5 \ + libQt5Qml.so.5 \ + libQt5Quick.so.5 \ + libQt5QuickWidgets.so.5 \ + libQt5SerialPort.so.5 \ + libQt5Sql.so.5 \ + libQt5Svg.so.5 \ + libQt5Test.so.5 \ + libQt5Widgets.so.5 \ + libQt5XcbQpa.so.5 + + !contains(DEFINES, __rasp_pi2__) { + QT_LIB_LIST += \ + libicudata.so.56 \ + libicui18n.so.56 \ + libicuuc.so.56 + } - for(QT_LIB, QT_LIB_LIST) { - QMAKE_POST_LINK += && $$QMAKE_COPY --dereference $$[QT_INSTALL_LIBS]/$$QT_LIB $$DESTDIR/Qt/libs/ - } + for(QT_LIB, QT_LIB_LIST) { + QMAKE_POST_LINK += && $$QMAKE_COPY --dereference $$[QT_INSTALL_LIBS]/$$QT_LIB $$DESTDIR/Qt/libs/ + } - # QT_INSTALL_PLUGINS - QT_PLUGIN_LIST = \ - bearer \ - geoservices \ - iconengines \ - imageformats \ - platforminputcontexts \ - platforms \ - position \ - sqldrivers - - !contains(DEFINES, __rasp_pi2__) { - QT_PLUGIN_LIST += xcbglintegrations - } + # QT_INSTALL_PLUGINS + QT_PLUGIN_LIST = \ + bearer \ + geoservices \ + iconengines \ + imageformats \ + platforminputcontexts \ + platforms \ + position \ + sqldrivers + + !contains(DEFINES, __rasp_pi2__) { + QT_PLUGIN_LIST += xcbglintegrations + } - for(QT_PLUGIN, QT_PLUGIN_LIST) { - QMAKE_POST_LINK += && $$QMAKE_COPY --dereference --recursive $$[QT_INSTALL_PLUGINS]/$$QT_PLUGIN $$DESTDIR/Qt/plugins/ - } + for(QT_PLUGIN, QT_PLUGIN_LIST) { + QMAKE_POST_LINK += && $$QMAKE_COPY --dereference --recursive $$[QT_INSTALL_PLUGINS]/$$QT_PLUGIN $$DESTDIR/Qt/plugins/ + } - # QT_INSTALL_QML - QMAKE_POST_LINK += && $$QMAKE_COPY --dereference --recursive $$[QT_INSTALL_QML] $$DESTDIR/Qt/ + # QT_INSTALL_QML + QMAKE_POST_LINK += && $$QMAKE_COPY --dereference --recursive $$[QT_INSTALL_QML] $$DESTDIR/Qt/ - # QGroundControl start script - QMAKE_POST_LINK += && $$QMAKE_COPY $$BASEDIR/deploy/qgroundcontrol-start.sh $$DESTDIR - QMAKE_POST_LINK += && $$QMAKE_COPY $$BASEDIR/deploy/qgroundcontrol.desktop $$DESTDIR - QMAKE_POST_LINK += && $$QMAKE_COPY $$BASEDIR/resources/icons/qgroundcontrol.png $$DESTDIR - } + # QGroundControl start script + QMAKE_POST_LINK += && $$QMAKE_COPY $$BASEDIR/deploy/qgroundcontrol-start.sh $$DESTDIR + QMAKE_POST_LINK += && $$QMAKE_COPY $$BASEDIR/deploy/qgroundcontrol.desktop $$DESTDIR + QMAKE_POST_LINK += && $$QMAKE_COPY $$BASEDIR/resources/icons/qgroundcontrol.png $$DESTDIR } - diff --git a/README.md b/README.md index 6a76b551056e4a06564071626049d408ebbe6024..6ee71f981b81e24ba837983eb05612267ebfbc3f 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,5 @@ # QGroundControl Ground Control Station + ## Open Source Micro Air Vehicle Ground Control Station [![Releases](https://img.shields.io/github/release/mavlink/QGroundControl.svg)](https://github.com/mavlink/QGroundControl/releases) @@ -10,7 +11,7 @@ The license terms are set in the COPYING.md file. * Project: - + * Files: @@ -27,7 +28,7 @@ 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. ### User Manual -https://donlakeflyer.gitbooks.io/qgroundcontrol-user-guide/content/ +https://docs.qgroundcontrol.com/en/ ### Supported Builds @@ -41,7 +42,7 @@ QGroundControl builds are supported for OSX, Linux, Windows, iOS and Android. QG * Qt version: 5.7.1 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. +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.7.1** NOT 5.4.x, 5.6.x, 5.8.x, etc. * 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. @@ -80,7 +81,7 @@ A Vagrantfile is provided to build QGroundControl using the [Vagrant](https://ww * 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. +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. diff --git a/UnitTest.qrc b/UnitTest.qrc index 599007da62aa4d0d25999b4d9454f1560d72c4ee..3332cf87698fa45ef6d3034f368cea05b7861e57 100644 --- a/UnitTest.qrc +++ b/UnitTest.qrc @@ -6,5 +6,7 @@ src/MissionManager/UnitTest/MavCmdInfoRover.json src/MissionManager/UnitTest/MavCmdInfoSub.json src/MissionManager/UnitTest/MavCmdInfoVTOL.json + src/MissionManager/UnitTest/MissionPlanner.waypoints + src/MissionManager/UnitTest/OldFileFormat.mission diff --git a/libs/mavlink/include/mavlink/v2.0 b/libs/mavlink/include/mavlink/v2.0 index fb411385d5f00300ab5d04c5adb08e7e17670d58..2faa6d49834aa203e2a3eeeeed588728fb14c431 160000 --- a/libs/mavlink/include/mavlink/v2.0 +++ b/libs/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit fb411385d5f00300ab5d04c5adb08e7e17670d58 +Subproject commit 2faa6d49834aa203e2a3eeeeed588728fb14c431 diff --git a/qgcresources.qrc b/qgcresources.qrc index ab45927ff935a07ce0046e67ef6211f677c9e1cc..25e0e8baa1608979a8cf2025ba56125edbd19d3b 100644 --- a/qgcresources.qrc +++ b/qgcresources.qrc @@ -154,6 +154,7 @@ src/ui/toolbar/Images/TelemRSSI.svg src/ui/toolbar/Images/Yield.svg src/ui/toolbar/Images/CameraIcon.svg + src/ui/toolbar/Images/Joystick.png src/MissionManager/CogWheel.svg src/AutoPilotPlugins/Common/Images/StationMode.svg src/AutoPilotPlugins/Common/Images/APMode.svg @@ -192,6 +193,10 @@ resources/takeoff.svg resources/TrashDelete.svg resources/waves.svg + resources/wind-guru.svg + resources/wind-rose.svg + resources/wind-roseBlack.svg + resources/wind-rose-arrow.svg resources/XDelete.svg resources/XDeleteBlack.svg resources/icons/qgroundcontrol.ico diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index d4926a7890788219bdbf1326487748514c4cd737..9b2c5cffc76635ecfd42f8036dde08ceddfabe68 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -390,6 +390,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin { src/MissionManager/MissionControllerTest.h \ src/MissionManager/MissionItemTest.h \ src/MissionManager/MissionManagerTest.h \ + src/MissionManager/PlanMasterControllerTest.h \ src/MissionManager/SectionTest.h \ src/MissionManager/SimpleMissionItemTest.h \ src/MissionManager/SpeedSectionTest.h \ @@ -422,6 +423,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin { src/MissionManager/MissionControllerTest.cc \ src/MissionManager/MissionItemTest.cc \ src/MissionManager/MissionManagerTest.cc \ + src/MissionManager/PlanMasterControllerTest.cc \ src/MissionManager/SectionTest.cc \ src/MissionManager/SimpleMissionItemTest.cc \ src/MissionManager/SpeedSectionTest.cc \ @@ -804,6 +806,8 @@ HEADERS+= \ src/AutoPilotPlugins/Common/MixersComponent.h \ src/AutoPilotPlugins/Common/MotorComponent.h \ src/AutoPilotPlugins/Common/RadioComponentController.h \ + src/AutoPilotPlugins/Common/SyslinkComponent.h \ + src/AutoPilotPlugins/Common/SyslinkComponentController.h \ src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.h \ src/FirmwarePlugin/CameraMetaData.h \ src/FirmwarePlugin/FirmwarePlugin.h \ @@ -827,6 +831,8 @@ SOURCES += \ src/AutoPilotPlugins/Common/MixersComponent.cc \ src/AutoPilotPlugins/Common/MotorComponent.cc \ src/AutoPilotPlugins/Common/RadioComponentController.cc \ + src/AutoPilotPlugins/Common/SyslinkComponent.cc \ + src/AutoPilotPlugins/Common/SyslinkComponentController.cc \ src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc \ src/FirmwarePlugin/CameraMetaData.cc \ src/FirmwarePlugin/FirmwarePlugin.cc \ diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index 643946656b2e6bb763e1e10bb77422e5fb956015..098ec02e246a0d740cb10deadafdd045bc5fc776 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -10,6 +10,7 @@ src/ui/toolbar/ModeIndicator.qml src/ui/toolbar/RCRSSIIndicator.qml src/ui/toolbar/TelemetryRSSIIndicator.qml + src/ui/toolbar/JoystickIndicator.qml src/AutoPilotPlugins/PX4/AirframeComponent.qml @@ -23,6 +24,7 @@ src/ui/preferences/DebugWindow.qml src/AutoPilotPlugins/Common/ESP8266Component.qml src/AutoPilotPlugins/Common/ESP8266ComponentSummary.qml + src/AutoPilotPlugins/Common/SyslinkComponent.qml src/VehicleSetup/FirmwareUpgrade.qml src/FlightDisplay/FlightDisplayViewDummy.qml src/FlightDisplay/FlightDisplayViewUVC.qml diff --git a/resources/wind-guru.svg b/resources/wind-guru.svg new file mode 100644 index 0000000000000000000000000000000000000000..f232bff5ac1297c8e06e178f53385bfe4ef85c1d --- /dev/null +++ b/resources/wind-guru.svg @@ -0,0 +1,35 @@ + + + + + + + image/svg+xml + + + + + + + + + diff --git a/resources/wind-rose-arrow.svg b/resources/wind-rose-arrow.svg new file mode 100644 index 0000000000000000000000000000000000000000..94830efecccf4810874884e3c2a89c64743ce6d5 --- /dev/null +++ b/resources/wind-rose-arrow.svg @@ -0,0 +1,62 @@ + + + + + + + + image/svg+xml + + + + + + + + + + + diff --git a/resources/wind-rose.svg b/resources/wind-rose.svg new file mode 100644 index 0000000000000000000000000000000000000000..c00875821c959d17f973bc3f5292e72d19916fa9 --- /dev/null +++ b/resources/wind-rose.svg @@ -0,0 +1,59 @@ + + + + + + + + + image/svg+xml + + + + + + + + diff --git a/resources/wind-roseBlack.svg b/resources/wind-roseBlack.svg new file mode 100644 index 0000000000000000000000000000000000000000..7d13f530efb8d193a05a203ca8d6c070849bb669 --- /dev/null +++ b/resources/wind-roseBlack.svg @@ -0,0 +1,59 @@ + + + + + + + + image/svg+xml + + + + + + + + + diff --git a/src/AnalyzeView/ExifParser.cc b/src/AnalyzeView/ExifParser.cc index 2b4ccf0639efdc652067f26af996473aea34163f..3dddf642e3a313ca0e8bf531dd02b1c264231071 100644 --- a/src/AnalyzeView/ExifParser.cc +++ b/src/AnalyzeView/ExifParser.cc @@ -71,7 +71,7 @@ bool ExifParser::write(QByteArray &buf, QGeoCoordinate coordinate) conversionPointer = reinterpret_cast(buf.mid(nextIfdOffsetInd, 2).data()); uint16_t nextIfdOffset = *conversionPointer; - // Definition of usefull unions and structs + // Definition of useful unions and structs union char2uint32_u { char c[4]; uint32_t i; diff --git a/src/AnalyzeView/MavlinkConsoleController.cc b/src/AnalyzeView/MavlinkConsoleController.cc index 072d89c209e924c7020f92ca526f21efc1c5bb22..03e85b655e4563454f556083c924be420549bbc5 100644 --- a/src/AnalyzeView/MavlinkConsoleController.cc +++ b/src/AnalyzeView/MavlinkConsoleController.cc @@ -80,9 +80,10 @@ MavlinkConsoleController::_receiveData(uint8_t device, uint8_t, uint16_t, uint32 void MavlinkConsoleController::_sendSerialData(QByteArray data, bool close) { - Q_ASSERT(_vehicle); - if (!_vehicle) + if (!_vehicle) { + qWarning() << "Internal error"; return; + } // Send maximum sized chunks until the complete buffer is transmitted while(data.size()) { diff --git a/src/AutoPilotPlugins/APM/APMCameraComponent.qml b/src/AutoPilotPlugins/APM/APMCameraComponent.qml index bd9308ff3661e88ae00a9f25fb042fd1612bc487..121cfdf728021863264103e7af73ea0621ad2aa2 100644 --- a/src/AutoPilotPlugins/APM/APMCameraComponent.qml +++ b/src/AutoPilotPlugins/APM/APMCameraComponent.qml @@ -92,19 +92,12 @@ SetupPage { } function setGimbalSettingsServoInfo(loader, channel) { - var rcPrefix = "RC" + channel + "_" + var rcPrefix = "r.SERVO" + channel + "_" loader.gimbalOutIndex = channel - 4 loader.servoPWMMinFact = controller.getParameterFact(-1, rcPrefix + "MIN") loader.servoPWMMaxFact = controller.getParameterFact(-1, rcPrefix + "MAX") - if (controller.parameterExists(-1, "RC5_REVERSED")) { - // Newer firmware parameter - loader.servoReverseFact = controller.getParameterFact(-1, rcPrefix + "REVERSED") - } else { - // Older firmware parameter - loader.servoReverseFact = controller.getParameterFact(-1, rcPrefix + "REV") - } - + loader.servoReverseFact = controller.getParameterFact(-1, rcPrefix + "REVERSED") } /// Gimbal output channels are stored in SERVO#_FUNCTION parameters. We need to loop through those diff --git a/src/AutoPilotPlugins/APM/APMSafetyComponentSub.qml b/src/AutoPilotPlugins/APM/APMSafetyComponentSub.qml index b9b74112740059407b71064c995bb98fe73a09fc..e72e6c562c56a24d3a0cf3ba1b02ccb04e112034 100644 --- a/src/AutoPilotPlugins/APM/APMSafetyComponentSub.qml +++ b/src/AutoPilotPlugins/APM/APMSafetyComponentSub.qml @@ -34,15 +34,15 @@ SetupPage { QGCPalette { id: ggcPal; colorGroupEnabled: true } - property Fact _failsafeGCSEnable: controller.getParameterFact(-1, "FS_GCS_ENABLE") - property Fact _failsafeLeakEnable: controller.getParameterFact(-1, "FS_LEAK_ENABLE") + property Fact _failsafeGCSEnable: controller.getParameterFact(-1, "FS_GCS_ENABLE") + property Fact _failsafeLeakEnable: controller.getParameterFact(-1, "FS_LEAK_ENABLE") property Fact _failsafePressureEnable: controller.getParameterFact(-1, "FS_PRESS_ENABLE") property Fact _failsafePressureValue: controller.getParameterFact(-1, "FS_PRESS_MAX") - property Fact _failsafeTempEnable: controller.getParameterFact(-1, "FS_TEMP_ENABLE") - property Fact _failsafeTempValue: controller.getParameterFact(-1, "FS_TEMP_MAX") + property Fact _failsafeTempEnable: controller.getParameterFact(-1, "FS_TEMP_ENABLE") + property Fact _failsafeTempValue: controller.getParameterFact(-1, "FS_TEMP_MAX") property Fact _fenceAction: controller.getParameterFact(-1, "FENCE_ACTION") - property Fact _fenceAltMax: controller.getParameterFact(-1, "FENCE_ALT_MAX") + property Fact _fenceAltMax: controller.getParameterFact(-1, "r.FENCE_ALT_MIN") property Fact _fenceEnable: controller.getParameterFact(-1, "FENCE_ENABLE") property Fact _fenceMargin: controller.getParameterFact(-1, "FENCE_MARGIN") property Fact _fenceType: controller.getParameterFact(-1, "FENCE_TYPE") diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc index 9c0837d26ba74a86324c1cf338838c20ac91e277..e3e6ca5e318375e6708cfb8de3d712283cd6500b 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc +++ b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc @@ -607,7 +607,8 @@ void APMSensorsComponentController::nextClicked(void) _vehicle->priorityLink()->mavlinkChannel(), &msg, 0, // command - 1); // result + 1, // result + 0); // progress _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); @@ -677,7 +678,7 @@ void APMSensorsComponentController::_handleMagCalProgress(mavlink_message_t& mes mavlink_msg_mag_cal_progress_decode(&message, &magCalProgress); qCDebug(APMSensorsComponentControllerVerboseLog) << "_handleMagCalProgress id:mask:pct" - << magCalProgress.compass_id << magCalProgress.cal_mask << magCalProgress.completion_pct; + << magCalProgress.compass_id << magCalProgress.cal_mask << magCalProgress.completion_pct; // How many compasses are we calibrating? int compassCalCount = 0; @@ -705,7 +706,7 @@ void APMSensorsComponentController::_handleMagCalReport(mavlink_message_t& messa mavlink_msg_mag_cal_report_decode(&message, &magCalReport); qCDebug(APMSensorsComponentControllerVerboseLog) << "_handleMagCalReport id:mask:status:fitness" - << magCalReport.compass_id << magCalReport.cal_mask << magCalReport.cal_status << magCalReport.fitness; + << magCalReport.compass_id << magCalReport.cal_mask << magCalReport.cal_status << magCalReport.fitness; bool additionalCompassCompleted = false; if (magCalReport.compass_id < 3 && !_rgCompassCalComplete[magCalReport.compass_id]) { diff --git a/src/AutoPilotPlugins/AutoPilotPlugin.cc b/src/AutoPilotPlugins/AutoPilotPlugin.cc index 4f938d0f984b9c11b0e8d67a5ad0237184c154c9..666734cd92aa0adf85cbe69dcd5f3a1884a032d7 100644 --- a/src/AutoPilotPlugins/AutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/AutoPilotPlugin.cc @@ -21,7 +21,7 @@ AutoPilotPlugin::AutoPilotPlugin(Vehicle* vehicle, QObject* parent) : QObject(parent) , _vehicle(vehicle) , _firmwarePlugin(vehicle->firmwarePlugin()) - , _setupComplete(false) + , _setupComplete(false) { } @@ -33,27 +33,29 @@ AutoPilotPlugin::~AutoPilotPlugin() void AutoPilotPlugin::_recalcSetupComplete(void) { - bool newSetupComplete = true; - - foreach(const QVariant componentVariant, vehicleComponents()) { - VehicleComponent* component = qobject_cast(qvariant_cast(componentVariant)); - Q_ASSERT(component); - - if (!component->setupComplete()) { - newSetupComplete = false; - break; - } - } - - if (_setupComplete != newSetupComplete) { - _setupComplete = newSetupComplete; - emit setupCompleteChanged(_setupComplete); - } + bool newSetupComplete = true; + + foreach(const QVariant componentVariant, vehicleComponents()) { + VehicleComponent* component = qobject_cast(qvariant_cast(componentVariant)); + if (component) { + if (!component->setupComplete()) { + newSetupComplete = false; + break; + } + } else { + qWarning() << "AutoPilotPlugin::_recalcSetupComplete Incorrectly typed VehicleComponent"; + } + } + + if (_setupComplete != newSetupComplete) { + _setupComplete = newSetupComplete; + emit setupCompleteChanged(_setupComplete); + } } bool AutoPilotPlugin::setupComplete(void) { - return _setupComplete; + return _setupComplete; } void AutoPilotPlugin::parametersReadyPreChecks(void) diff --git a/src/AutoPilotPlugins/Common/SyslinkComponent.cc b/src/AutoPilotPlugins/Common/SyslinkComponent.cc new file mode 100644 index 0000000000000000000000000000000000000000..ea1e28e371b3a56908a513a2cbb5b19c4b20c5fa --- /dev/null +++ b/src/AutoPilotPlugins/Common/SyslinkComponent.cc @@ -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. + * + ****************************************************************************/ + + +#include "SyslinkComponent.h" +#include "AutoPilotPlugin.h" + +SyslinkComponent::SyslinkComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent) + : VehicleComponent(vehicle, autopilot, parent) + , _name(tr("Syslink")) +{ + +} + +QString SyslinkComponent::name(void) const +{ + return _name; +} + +QString SyslinkComponent::description(void) const +{ + return tr("The Syslink Component is used to setup the radio connection on Crazyflies."); +} + +QString SyslinkComponent::iconResource(void) const +{ + return "/qmlimages/wifi.svg"; +} + +bool SyslinkComponent::requiresSetup(void) const +{ + return false; +} + +bool SyslinkComponent::setupComplete(void) const +{ + return true; +} + +QStringList SyslinkComponent::setupCompleteChangedTriggerList(void) const +{ + return QStringList(); +} + +QUrl SyslinkComponent::setupSource(void) const +{ + return QUrl::fromUserInput("qrc:/qml/SyslinkComponent.qml"); +} + +QUrl SyslinkComponent::summaryQmlSource(void) const +{ + return QUrl(); +} + +QString SyslinkComponent::prerequisiteSetup(void) const +{ + return QString(); +} diff --git a/src/AutoPilotPlugins/Common/SyslinkComponent.h b/src/AutoPilotPlugins/Common/SyslinkComponent.h new file mode 100644 index 0000000000000000000000000000000000000000..8569b1abe87f3e67c7c610cbbc7f7dab4a505cc3 --- /dev/null +++ b/src/AutoPilotPlugins/Common/SyslinkComponent.h @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * (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 SyslinkComponent_H +#define SyslinkComponent_H + +#include "VehicleComponent.h" + +class SyslinkComponent : public VehicleComponent +{ + Q_OBJECT +public: + SyslinkComponent (Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent = NULL); + + // Virtuals from VehicleComponent + QStringList setupCompleteChangedTriggerList() const; + + // Virtuals from VehicleComponent + QString name () const; + QString description () const; + QString iconResource () const; + bool requiresSetup () const; + bool setupComplete () const; + QUrl setupSource () const; + QUrl summaryQmlSource () const; + QString prerequisiteSetup () const; + +private: + const QString _name; + QVariantList _summaryItems; +}; + +#endif diff --git a/src/AutoPilotPlugins/Common/SyslinkComponent.qml b/src/AutoPilotPlugins/Common/SyslinkComponent.qml new file mode 100644 index 0000000000000000000000000000000000000000..d2ae061276e6e3e31f9e4e15d4e136ceaa84bb64 --- /dev/null +++ b/src/AutoPilotPlugins/Common/SyslinkComponent.qml @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * (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.2 +import QtQuick.Controls 1.2 +import QtQuick.Dialogs 1.2 +import QtQuick.Layouts 1.2 + +import QGroundControl.FactSystem 1.0 +import QGroundControl.FactControls 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Controllers 1.0 + +SetupPage { + id: syslinkPage + pageComponent: pageComponent + + Component { + id: pageComponent + + Column { + id: innerColumn + width: availableWidth + spacing: ScreenTools.defaultFontPixelHeight * 0.5 + + property int textEditWidth: ScreenTools.defaultFontPixelWidth * 12 + + + SyslinkComponentController { + id: controller + factPanel: syslinkPage.viewPanel + } + + QGCLabel { + text: qsTr("Radio Settings") + font.family: ScreenTools.demiboldFontFamily + } + + Rectangle { + width: parent.width + height: radioGrid.height + ScreenTools.defaultFontPixelHeight + color: qgcPal.windowShade + + GridLayout { + id: radioGrid + anchors.margins: ScreenTools.defaultFontPixelHeight / 2 + anchors.left: parent.left + anchors.top: parent.top + columns: 2 + columnSpacing: ScreenTools.defaultFontPixelWidth + + QGCLabel { + text: qsTr("Channel") + } + + QGCTextField { + id: channelField + width: textEditWidth + text: controller.radioChannel + validator: IntValidator {bottom: 0; top: 125;} + inputMethodHints: Qt.ImhDigitsOnly + onEditingFinished: { + controller.radioChannel = text + } + } + + QGCLabel { + id: channelHelp + Layout.columnSpan: radioGrid.columns + Layout.fillWidth: true + font.pointSize: ScreenTools.smallFontPointSize + wrapMode: Text.WordWrap + text: "Channel can be between 0 and 125" + } + + QGCLabel { + id: addressLabel + text: qsTr("Address") + } + + QGCTextField { + id: addressField + width: textEditWidth + text: controller.radioAddress + maximumLength: 10 + validator: RegExpValidator { regExp: /^[0-9A-Fa-f]*$/ } + onEditingFinished: { + controller.radioAddress = text + } + } + + QGCLabel { + id: addressHelp + Layout.columnSpan: radioGrid.columns + Layout.fillWidth: true + font.pointSize: ScreenTools.smallFontPointSize + wrapMode: Text.WordWrap + text: "Address in hex. Default is E7E7E7E7E7." + } + + + QGCLabel { + id: rateLabel + text: qsTr("Data Rate") + } + + QGCComboBox { + id: rateField + Layout.fillWidth: true + model: controller.radioRates + currentIndex: controller.radioRate + onActivated: { + controller.radioRate = index + } + } + + QGCButton { + text: "Restore Defaults" + width: textEditWidth + onClicked: { + controller.resetDefaults() + } + } + + } // Grid + } // Rectangle - Radio Settings + + + } + } +} diff --git a/src/AutoPilotPlugins/Common/SyslinkComponentController.cc b/src/AutoPilotPlugins/Common/SyslinkComponentController.cc new file mode 100644 index 0000000000000000000000000000000000000000..63145c07435b1008a8f002fa9439ec7a9a0f77dc --- /dev/null +++ b/src/AutoPilotPlugins/Common/SyslinkComponentController.cc @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * (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 "SyslinkComponentController.h" +#include "QGCApplication.h" +#include "UAS.h" +#include "ParameterManager.h" + +#include +#include + +QGC_LOGGING_CATEGORY(SyslinkComponentControllerLog, "SyslinkComponentControllerLog") + +//----------------------------------------------------------------------------- +SyslinkComponentController::SyslinkComponentController() +{ + _dataRates.append("750Kb/s"); + _dataRates.append("1Mb/s"); + _dataRates.append("2Mb/s"); + + Fact* chan = getParameterFact(_vehicle->id(), "SLNK_RADIO_CHAN"); + connect(chan, &Fact::valueChanged, this, &SyslinkComponentController::_channelChanged); + Fact* rate = getParameterFact(_vehicle->id(), "SLNK_RADIO_RATE"); + connect(rate, &Fact::valueChanged, this, &SyslinkComponentController::_rateChanged); + Fact* addr1 = getParameterFact(_vehicle->id(), "SLNK_RADIO_ADDR1"); + connect(addr1, &Fact::valueChanged, this, &SyslinkComponentController::_addressChanged); + Fact* addr2 = getParameterFact(_vehicle->id(), "SLNK_RADIO_ADDR2"); + connect(addr2, &Fact::valueChanged, this, &SyslinkComponentController::_addressChanged); +} + +//----------------------------------------------------------------------------- +SyslinkComponentController::~SyslinkComponentController() +{ + +} + +//----------------------------------------------------------------------------- +int +SyslinkComponentController::radioChannel() +{ + return getParameterFact(_vehicle->id(), "SLNK_RADIO_CHAN")->rawValue().toUInt(); +} + +//----------------------------------------------------------------------------- +void +SyslinkComponentController::setRadioChannel(int num) +{ + Fact* f = getParameterFact(_vehicle->id(), "SLNK_RADIO_CHAN"); + f->setRawValue(QVariant(num)); +} + +//----------------------------------------------------------------------------- +QString +SyslinkComponentController::radioAddress() +{ + uint32_t val_uh = getParameterFact(_vehicle->id(), "SLNK_RADIO_ADDR1")->rawValue().toUInt(); + uint32_t val_lh = getParameterFact(_vehicle->id(), "SLNK_RADIO_ADDR2")->rawValue().toUInt(); + uint64_t val = (((uint64_t) val_uh) << 32) | ((uint64_t) val_lh); + + return QString().number(val, 16); +} + +//----------------------------------------------------------------------------- +void +SyslinkComponentController::setRadioAddress(QString str) +{ + Fact *uh = getParameterFact(_vehicle->id(), "SLNK_RADIO_ADDR1"); + Fact *lh = getParameterFact(_vehicle->id(), "SLNK_RADIO_ADDR2"); + + uint64_t val = str.toULongLong(0, 16); + + uint32_t val_uh = val >> 32; + uint32_t val_lh = val & 0xFFFFFFFF; + + uh->setRawValue(QVariant(val_uh)); + lh->setRawValue(QVariant(val_lh)); +} + +//----------------------------------------------------------------------------- +int +SyslinkComponentController::radioRate() +{ + return getParameterFact(_vehicle->id(), "SLNK_RADIO_RATE")->rawValue().toInt(); +} + +//----------------------------------------------------------------------------- +void +SyslinkComponentController::setRadioRate(int idx) +{ + if(idx >= 0 && idx <= 2 && idx != radioRate()) { + Fact* r = getParameterFact(_vehicle->id(), "SLNK_RADIO_RATE"); + r->setRawValue(idx); + } +} + +//----------------------------------------------------------------------------- +void +SyslinkComponentController::resetDefaults() +{ + Fact* chan = getParameterFact(_vehicle->id(), "SLNK_RADIO_CHAN"); + Fact* rate = getParameterFact(_vehicle->id(), "SLNK_RADIO_RATE"); + Fact* addr1 = getParameterFact(_vehicle->id(), "SLNK_RADIO_ADDR1"); + Fact* addr2 = getParameterFact(_vehicle->id(), "SLNK_RADIO_ADDR2"); + + chan->setRawValue(chan->rawDefaultValue()); + rate->setRawValue(rate->rawDefaultValue()); + addr1->setRawValue(addr1->rawDefaultValue()); + addr2->setRawValue(addr2->rawDefaultValue()); +} + +//----------------------------------------------------------------------------- +void +SyslinkComponentController::_channelChanged(QVariant) +{ + emit radioChannelChanged(); +} + +//----------------------------------------------------------------------------- +void +SyslinkComponentController::_addressChanged(QVariant) +{ + emit radioAddressChanged(); +} + +//----------------------------------------------------------------------------- +void +SyslinkComponentController::_rateChanged(QVariant) +{ + emit radioRateChanged(); +} + diff --git a/src/AutoPilotPlugins/Common/SyslinkComponentController.h b/src/AutoPilotPlugins/Common/SyslinkComponentController.h new file mode 100644 index 0000000000000000000000000000000000000000..b8180a0b79b26865ca4c055b1c8d483011bbf275 --- /dev/null +++ b/src/AutoPilotPlugins/Common/SyslinkComponentController.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * (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 SyslinkComponentController_H +#define SyslinkComponentController_H + +#include "FactPanelController.h" +#include "UASInterface.h" +#include "QGCLoggingCategory.h" +#include "AutoPilotPlugin.h" + +Q_DECLARE_LOGGING_CATEGORY(SyslinkComponentControllerLog) + +namespace Ui { + class SyslinkComponentController; +} + +class SyslinkComponentController : public FactPanelController +{ + Q_OBJECT + +public: + SyslinkComponentController (); + ~SyslinkComponentController (); + + Q_PROPERTY(int radioChannel READ radioChannel WRITE setRadioChannel NOTIFY radioChannelChanged) + Q_PROPERTY(QString radioAddress READ radioAddress WRITE setRadioAddress NOTIFY radioAddressChanged) + Q_PROPERTY(int radioRate READ radioRate WRITE setRadioRate NOTIFY radioRateChanged) + Q_PROPERTY(QStringList radioRates READ radioRates CONSTANT) + Q_PROPERTY(Vehicle* vehicle READ vehicle CONSTANT) + + Q_INVOKABLE void resetDefaults(); + + int radioChannel (); + QString radioAddress (); + int radioRate (); + QStringList radioRates () { return _dataRates; } + Vehicle* vehicle () { return _vehicle; } + + void setRadioChannel (int num); + void setRadioAddress (QString str); + void setRadioRate (int idx); + + +signals: + void radioChannelChanged (); + void radioAddressChanged (); + void radioRateChanged (); + +private slots: + void _channelChanged (QVariant value); + void _addressChanged (QVariant value); + void _rateChanged (QVariant value); + +private: + QStringList _dataRates; + +}; + +#endif // SyslinkComponentController_H diff --git a/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc b/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc index d4be51de55a2fcaf318230ddce577c1370debc32..78b84a4b33ff936c1d8f7776f35fe2d55a803309 100644 --- a/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc @@ -16,7 +16,9 @@ GenericAutoPilotPlugin::GenericAutoPilotPlugin(Vehicle* vehicle, QObject* parent) : AutoPilotPlugin(vehicle, parent) { - Q_ASSERT(vehicle); + if (!vehicle) { + qWarning() << "Internal error"; + } } const QVariantList& GenericAutoPilotPlugin::vehicleComponents(void) diff --git a/src/AutoPilotPlugins/PX4/AirframeComponent.cc b/src/AutoPilotPlugins/PX4/AirframeComponent.cc index bb7ae98e9ff68fd1ca4a10ec422867798b7cadf8..67b356c859cfc91d573a32b5b088d128f53417f5 100644 --- a/src/AutoPilotPlugins/PX4/AirframeComponent.cc +++ b/src/AutoPilotPlugins/PX4/AirframeComponent.cc @@ -15,96 +15,11 @@ #include "QGCQmlWidgetHolder.h" #include "ParameterManager.h" -#if 0 -// Broken by latest mavlink module changes. Not used yet. Comment out for now. -// Discussing mavlink fix. -struct mavType { - int type; - const char* description; -}; - -/// @brief Used to translate from MAV_TYPE_* id to user string -static const struct mavType mavTypeInfo[] = { - { MAV_TYPE_GENERIC, "Generic" }, - { MAV_TYPE_FIXED_WING, "Fixed Wing" }, - { MAV_TYPE_QUADROTOR, "Quadrotor" }, - { MAV_TYPE_COAXIAL, "Coaxial" }, - { MAV_TYPE_HELICOPTER, "Helicopter"}, - { MAV_TYPE_ANTENNA_TRACKER, "Antenna Tracker" }, - { MAV_TYPE_GCS, "Ground Control Station" }, - { MAV_TYPE_AIRSHIP, "Airship" }, - { MAV_TYPE_FREE_BALLOON, "Free Balloon" }, - { MAV_TYPE_ROCKET, "Rocket" }, - { MAV_TYPE_GROUND_ROVER, "Ground Rover" }, - { MAV_TYPE_SURFACE_BOAT, "Boat" }, - { MAV_TYPE_SUBMARINE, "Submarine" }, - { MAV_TYPE_HEXAROTOR, "Hexarotor" }, - { MAV_TYPE_OCTOROTOR, "Octorotor" }, - { MAV_TYPE_TRICOPTER, "Tricopter" }, - { MAV_TYPE_FLAPPING_WING, "Flapping Wing" }, - { MAV_TYPE_KITE, "Kite" }, - { MAV_TYPE_ONBOARD_CONTROLLER, "Onbard companion controller" }, - { MAV_TYPE_VTOL_DUOROTOR, "Two-rotor VTOL" }, - { MAV_TYPE_VTOL_QUADROTOR, "Quad-rotor VTOL" }, - { MAV_TYPE_VTOL_RESERVED1, "Reserved" }, - { MAV_TYPE_VTOL_RESERVED2, "Reserved" }, - { MAV_TYPE_VTOL_RESERVED3, "Reserved" }, - { MAV_TYPE_VTOL_RESERVED4, "Reserved" }, - { MAV_TYPE_VTOL_RESERVED5, "Reserved" }, - { MAV_TYPE_GIMBAL, "Gimbal" }, -}; -static size_t cMavTypes = sizeof(mavTypeInfo) / sizeof(mavTypeInfo[0]); -#endif - AirframeComponent::AirframeComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent) : VehicleComponent(vehicle, autopilot, parent), _name(tr("Airframe")) { -#if 0 - // Broken by latest mavlink module changes. Not used yet. Comment out for now. - // Discussing mavlink fix. - Q_UNUSED(mavTypeInfo); // Keeping this around for later use - - // Validate that our mavTypeInfo array hasn't gotten out of sync - - qDebug() << cMavTypes << MAV_TYPE_ENUM_END; - Q_ASSERT(cMavTypes == MAV_TYPE_ENUM_END); - - static const int mavTypes[] = { - MAV_TYPE_GENERIC, - MAV_TYPE_FIXED_WING, - MAV_TYPE_QUADROTOR, - MAV_TYPE_COAXIAL, - MAV_TYPE_HELICOPTER, - MAV_TYPE_ANTENNA_TRACKER, - MAV_TYPE_GCS, - MAV_TYPE_AIRSHIP, - MAV_TYPE_FREE_BALLOON, - MAV_TYPE_ROCKET, - MAV_TYPE_GROUND_ROVER, - MAV_TYPE_SURFACE_BOAT, - MAV_TYPE_SUBMARINE, - MAV_TYPE_HEXAROTOR, - MAV_TYPE_OCTOROTOR, - MAV_TYPE_TRICOPTER, - MAV_TYPE_FLAPPING_WING, - MAV_TYPE_KITE, - MAV_TYPE_ONBOARD_CONTROLLER, - MAV_TYPE_VTOL_DUOROTOR, - MAV_TYPE_VTOL_QUADROTOR, - MAV_TYPE_VTOL_RESERVED1, - MAV_TYPE_VTOL_RESERVED2, - MAV_TYPE_VTOL_RESERVED3, - MAV_TYPE_VTOL_RESERVED4, - MAV_TYPE_VTOL_RESERVED5, - MAV_TYPE_GIMBAL, - }; - Q_UNUSED(mavTypes); // Keeping this around for later use - - for (size_t i=0; iautostartId) { - Q_ASSERT(!autostartFound); + if (autostartFound) { + qWarning() << "AirframeComponentController::AirframeComponentController duplicate ids found:" << _autostartId; + } autostartFound = true; _currentAirframeType = pType->name; _currentVehicleName = pInfo->name; diff --git a/src/AutoPilotPlugins/PX4/PX4AirframeLoader.cc b/src/AutoPilotPlugins/PX4/PX4AirframeLoader.cc index e542694b29f456caa7b0bb4802e3a38f8964d7ce..c7296609927e41f126cc06b9ed51fc1347b9b4f2 100644 --- a/src/AutoPilotPlugins/PX4/PX4AirframeLoader.cc +++ b/src/AutoPilotPlugins/PX4/PX4AirframeLoader.cc @@ -30,7 +30,6 @@ PX4AirframeLoader::PX4AirframeLoader(AutoPilotPlugin* autopilot, UASInterface* u Q_UNUSED(autopilot); Q_UNUSED(uas); Q_UNUSED(parent); - Q_ASSERT(uas); } QString PX4AirframeLoader::aiframeMetaDataFile(void) @@ -51,7 +50,10 @@ void PX4AirframeLoader::loadAirframeMetaData(void) qCDebug(PX4AirframeLoaderLog) << "Loading PX4 airframe fact meta data"; - Q_ASSERT(AirframeComponentAirframes::get().count() == 0); + if (AirframeComponentAirframes::get().count() != 0) { + qCWarning(PX4AirframeLoaderLog) << "Internal error"; + return; + } QString airframeFilename; @@ -67,11 +69,12 @@ void PX4AirframeLoader::loadAirframeMetaData(void) qCDebug(PX4AirframeLoaderLog) << "Loading meta data file:" << airframeFilename; QFile xmlFile(airframeFilename); - Q_ASSERT(xmlFile.exists()); + if (!xmlFile.exists()) { + qCWarning(PX4AirframeLoaderLog) << "Internal error"; + return; + } bool success = xmlFile.open(QIODevice::ReadOnly); - Q_UNUSED(success); - Q_ASSERT(success); if (!success) { qCWarning(PX4AirframeLoaderLog) << "Failed opening airframe XML"; diff --git a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc index 383e5f703b558512e8a6ebe761be893c27306446..1b1dd0a8fe241c8e369d3e6ce50bd2a665487c63 100644 --- a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc @@ -40,8 +40,12 @@ PX4AutoPilotPlugin::PX4AutoPilotPlugin(Vehicle* vehicle, QObject* parent) , _motorComponent(NULL) , _tuningComponent(NULL) , _mixersComponent(NULL) + , _syslinkComponent(NULL) { - Q_ASSERT(vehicle); + if (!vehicle) { + qWarning() << "Internal error"; + return; + } _airframeFacts = new PX4AirframeLoader(this, _vehicle->uas(), this); Q_CHECK_PTR(_airframeFacts); @@ -57,68 +61,76 @@ PX4AutoPilotPlugin::~PX4AutoPilotPlugin() const QVariantList& PX4AutoPilotPlugin::vehicleComponents(void) { if (_components.count() == 0 && !_incorrectParameterVersion) { - Q_ASSERT(_vehicle); - - if (_vehicle->parameterManager()->parametersReady()) { - _airframeComponent = new AirframeComponent(_vehicle, this); - _airframeComponent->setupTriggerSignals(); - _components.append(QVariant::fromValue((VehicleComponent*)_airframeComponent)); - - _radioComponent = new PX4RadioComponent(_vehicle, this); - _radioComponent->setupTriggerSignals(); - _components.append(QVariant::fromValue((VehicleComponent*)_radioComponent)); - - if (!_vehicle->hilMode()) { - _sensorsComponent = new SensorsComponent(_vehicle, this); - _sensorsComponent->setupTriggerSignals(); - _components.append(QVariant::fromValue((VehicleComponent*)_sensorsComponent)); - } - - _flightModesComponent = new FlightModesComponent(_vehicle, this); - _flightModesComponent->setupTriggerSignals(); - _components.append(QVariant::fromValue((VehicleComponent*)_flightModesComponent)); - - _powerComponent = new PowerComponent(_vehicle, this); - _powerComponent->setupTriggerSignals(); - _components.append(QVariant::fromValue((VehicleComponent*)_powerComponent)); + if (_vehicle) { + if (_vehicle->parameterManager()->parametersReady()) { + _airframeComponent = new AirframeComponent(_vehicle, this); + _airframeComponent->setupTriggerSignals(); + _components.append(QVariant::fromValue((VehicleComponent*)_airframeComponent)); + + _radioComponent = new PX4RadioComponent(_vehicle, this); + _radioComponent->setupTriggerSignals(); + _components.append(QVariant::fromValue((VehicleComponent*)_radioComponent)); + + if (!_vehicle->hilMode()) { + _sensorsComponent = new SensorsComponent(_vehicle, this); + _sensorsComponent->setupTriggerSignals(); + _components.append(QVariant::fromValue((VehicleComponent*)_sensorsComponent)); + } + + _flightModesComponent = new FlightModesComponent(_vehicle, this); + _flightModesComponent->setupTriggerSignals(); + _components.append(QVariant::fromValue((VehicleComponent*)_flightModesComponent)); + + _powerComponent = new PowerComponent(_vehicle, this); + _powerComponent->setupTriggerSignals(); + _components.append(QVariant::fromValue((VehicleComponent*)_powerComponent)); #if 0 - // Coming soon - _motorComponent = new MotorComponent(_vehicle, this); - _motorComponent->setupTriggerSignals(); - _components.append(QVariant::fromValue((VehicleComponent*)_motorComponent)); + // Coming soon + _motorComponent = new MotorComponent(_vehicle, this); + _motorComponent->setupTriggerSignals(); + _components.append(QVariant::fromValue((VehicleComponent*)_motorComponent)); #endif - _safetyComponent = new SafetyComponent(_vehicle, this); - _safetyComponent->setupTriggerSignals(); - _components.append(QVariant::fromValue((VehicleComponent*)_safetyComponent)); + _safetyComponent = new SafetyComponent(_vehicle, this); + _safetyComponent->setupTriggerSignals(); + _components.append(QVariant::fromValue((VehicleComponent*)_safetyComponent)); - _tuningComponent = new PX4TuningComponent(_vehicle, this); - _tuningComponent->setupTriggerSignals(); - _components.append(QVariant::fromValue((VehicleComponent*)_tuningComponent)); + _tuningComponent = new PX4TuningComponent(_vehicle, this); + _tuningComponent->setupTriggerSignals(); + _components.append(QVariant::fromValue((VehicleComponent*)_tuningComponent)); #if 0 - // Coming soon - _mixersComponent = new MixersComponent(_vehicle, this); - _mixersComponent->setupTriggerSignals(); - _components.append(QVariant::fromValue((VehicleComponent*)_mixersComponent)); + // Coming soon + _mixersComponent = new MixersComponent(_vehicle, this); + _mixersComponent->setupTriggerSignals(); + _components.append(QVariant::fromValue((VehicleComponent*)_mixersComponent)); #endif - //-- Is there support for cameras? - if(_vehicle->parameterManager()->parameterExists(_vehicle->id(), "TRIG_MODE")) { - _cameraComponent = new CameraComponent(_vehicle, this); - _cameraComponent->setupTriggerSignals(); - _components.append(QVariant::fromValue((VehicleComponent*)_cameraComponent)); + //-- Is there support for cameras? + if(_vehicle->parameterManager()->parameterExists(_vehicle->id(), "TRIG_MODE")) { + _cameraComponent = new CameraComponent(_vehicle, this); + _cameraComponent->setupTriggerSignals(); + _components.append(QVariant::fromValue((VehicleComponent*)_cameraComponent)); + } + + //-- Is there an ESP8266 Connected? + if(_vehicle->parameterManager()->parameterExists(MAV_COMP_ID_UDP_BRIDGE, "SW_VER")) { + _esp8266Component = new ESP8266Component(_vehicle, this); + _esp8266Component->setupTriggerSignals(); + _components.append(QVariant::fromValue((VehicleComponent*)_esp8266Component)); + } + } else { + qWarning() << "Call to vehicleCompenents prior to parametersReady"; } - //-- Is there an ESP8266 Connected? - if(_vehicle->parameterManager()->parameterExists(MAV_COMP_ID_UDP_BRIDGE, "SW_VER")) { - _esp8266Component = new ESP8266Component(_vehicle, this); - _esp8266Component->setupTriggerSignals(); - _components.append(QVariant::fromValue((VehicleComponent*)_esp8266Component)); + if(_vehicle->parameterManager()->parameterExists(_vehicle->id(), "SLNK_RADIO_CHAN")) { + _syslinkComponent = new SyslinkComponent(_vehicle, this); + _syslinkComponent->setupTriggerSignals(); + _components.append(QVariant::fromValue((VehicleComponent*)_syslinkComponent)); } } else { - qWarning() << "Call to vehicleCompenents prior to parametersReady"; + qWarning() << "Internal error"; } } diff --git a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h index ee85d9775acf0630c62cc441c5cc0a03c481b966..f886176e6983e3e22f37a0e7143868086a3c94b6 100644 --- a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h +++ b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h @@ -24,6 +24,7 @@ #include "MotorComponent.h" #include "PX4TuningComponent.h" #include "MixersComponent.h" +#include "SyslinkComponent.h" #include "Vehicle.h" #include @@ -44,7 +45,6 @@ public: const QVariantList& vehicleComponents(void) override; void parametersReadyPreChecks(void) override; QString prerequisiteSetup(VehicleComponent* component) const override; - protected: bool _incorrectParameterVersion; ///< true: parameter version incorrect, setup not allowed PX4AirframeLoader* _airframeFacts; @@ -59,7 +59,7 @@ protected: MotorComponent* _motorComponent; PX4TuningComponent* _tuningComponent; MixersComponent* _mixersComponent; - + SyslinkComponent* _syslinkComponent; private: QVariantList _components; }; diff --git a/src/AutoPilotPlugins/PX4/SafetyComponent.qml b/src/AutoPilotPlugins/PX4/SafetyComponent.qml index 80b5307914810ea0918fe50175967fc7f1422f6b..5e7b99a9060f261c73d631433329bc42a5588f2e 100644 --- a/src/AutoPilotPlugins/PX4/SafetyComponent.qml +++ b/src/AutoPilotPlugins/PX4/SafetyComponent.qml @@ -123,7 +123,7 @@ SetupPage { sourceSize.height: height mipmap: true fillMode: Image.PreserveAspectFit - source: "/qmlimages/check.png"///qgcPal.globalTheme === qgcPal.Light ? "/qmlimages/LowBatteryLight.svg" : "/qmlimages/LowBattery.svg" + source: qgcPal.globalTheme === qgcPal.Light ? "/qmlimages/LowBatteryLight.svg" : "/qmlimages/LowBattery.svg" Layout.rowSpan: 3 Layout.minimumWidth: _imageWidth } @@ -177,7 +177,7 @@ SetupPage { sourceSize.height: height mipmap: true fillMode: Image.PreserveAspectFit - //source: qgcPal.globalTheme === qgcPal.Light ? "/qmlimages/RCLossLight.svg" : "/qmlimages/RCLoss.svg" + source: qgcPal.globalTheme === qgcPal.Light ? "/qmlimages/RCLossLight.svg" : "/qmlimages/RCLoss.svg" Layout.rowSpan: 3 Layout.minimumWidth: _imageWidth } @@ -222,7 +222,7 @@ SetupPage { sourceSize.height: height mipmap: true fillMode: Image.PreserveAspectFit - //source: qgcPal.globalTheme === qgcPal.Light ? "/qmlimages/DatalinkLossLight.svg" : "/qmlimages/DatalinkLoss.svg" + source: qgcPal.globalTheme === qgcPal.Light ? "/qmlimages/DatalinkLossLight.svg" : "/qmlimages/DatalinkLoss.svg" Layout.rowSpan: 3 Layout.minimumWidth: _imageWidth } @@ -267,7 +267,7 @@ SetupPage { sourceSize.height: height mipmap: true fillMode: Image.PreserveAspectFit - //source: qgcPal.globalTheme === qgcPal.Light ? "/qmlimages/GeoFenceLight.svg" : "/qmlimages/GeoFence.svg" + source: qgcPal.globalTheme === qgcPal.Light ? "/qmlimages/GeoFenceLight.svg" : "/qmlimages/GeoFence.svg" Layout.rowSpan: 3 Layout.minimumWidth: _imageWidth } @@ -330,7 +330,7 @@ SetupPage { sourceSize.height: height mipmap: true fillMode: Image.PreserveAspectFit - //source: controller.vehicle.fixedWing ? "/qmlimages/ReturnToHomeAltitude.svg" : "/qmlimages/ReturnToHomeAltitudeCopter.svg" + source: controller.vehicle.fixedWing ? "/qmlimages/ReturnToHomeAltitude.svg" : "/qmlimages/ReturnToHomeAltitudeCopter.svg" Layout.rowSpan: 7 Layout.minimumWidth: _imageWidth } @@ -424,7 +424,7 @@ SetupPage { sourceSize.height: height mipmap: true fillMode: Image.PreserveAspectFit - //source: controller.vehicle.fixedWing ? "/qmlimages/LandMode.svg" : "/qmlimages/LandModeCopter.svg" + source: controller.vehicle.fixedWing ? "/qmlimages/LandMode.svg" : "/qmlimages/LandModeCopter.svg" Layout.rowSpan: landVelocityLabel.visible ? 2 : 1 Layout.minimumWidth: _imageWidth } diff --git a/src/AutoPilotPlugins/PX4/SensorsComponentController.cc b/src/AutoPilotPlugins/PX4/SensorsComponentController.cc index 7150e49c4bd2c7bdafa77d988eeb6125092a2c42..b0016e76d29a68a1fb8e46d254676b88685b9d12 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponentController.cc +++ b/src/AutoPilotPlugins/PX4/SensorsComponentController.cc @@ -73,7 +73,10 @@ bool SensorsComponentController::usingUDPLink(void) /// Appends the specified text to the status log area in the ui void SensorsComponentController::_appendStatusLog(const QString& text) { - Q_ASSERT(_statusLog); + if (!_statusLog) { + qWarning() << "Internal error"; + return; + } QVariant returnedValue; QVariant varText = text; @@ -229,8 +232,11 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in bool ok; int p = percent.toInt(&ok); if (ok) { - Q_ASSERT(_progressBar); - _progressBar->setProperty("value", (float)(p / 100.0)); + if (_progressBar) { + _progressBar->setProperty("value", (float)(p / 100.0)); + } else { + qWarning() << "Internal error"; + } } return; } @@ -324,7 +330,7 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in _gyroCalInProgress = true; _orientationCalDownSideVisible = true; } else { - Q_ASSERT(false); + qWarning() << "Unknown calibration message type" << text; } emit orientationCalSidesDoneChanged(); emit orientationCalSidesVisibleChanged(); diff --git a/src/FactSystem/Fact.cc b/src/FactSystem/Fact.cc index 234579be4329a359daebec4d657b99a6f1093842..606a0ce236e3f5e485016a5db883fd6e9241ae1e 100644 --- a/src/FactSystem/Fact.cc +++ b/src/FactSystem/Fact.cc @@ -270,6 +270,18 @@ QString Fact::_variantToString(const QVariant& variant, int decimalPlaces) const } } break; + case FactMetaData::valueTypeElapsedTimeInSeconds: + { + double dValue = variant.toDouble(); + if (qIsNaN(dValue)) { + valueString = QStringLiteral("--:--:--"); + } else { + QTime time(0, 0, 0, 0); + time = time.addSecs(dValue); + valueString = time.toString(QStringLiteral("hh:mm:ss")); + } + } + break; default: valueString = variant.toString(); break; diff --git a/src/FactSystem/FactMetaData.cc b/src/FactSystem/FactMetaData.cc index 2fc884185838f63b133cad5ade8eb08797d427e7..b55b1b954e29ab4d230da217474c022c4a748737 100644 --- a/src/FactSystem/FactMetaData.cc +++ b/src/FactSystem/FactMetaData.cc @@ -34,9 +34,10 @@ const qreal FactMetaData::UnitConsts_s::inchesToCentimeters = 2.54; // Built in translations for all Facts const FactMetaData::BuiltInTranslation_s FactMetaData::_rgBuiltInTranslations[] = { - { "centi-degrees", "deg", FactMetaData::_centiDegreesToDegrees, FactMetaData::_degreesToCentiDegrees }, - { "radians", "deg", FactMetaData::_radiansToDegrees, FactMetaData::_degreesToRadians }, - { "norm", "%", FactMetaData::_normToPercent, FactMetaData::_percentToNorm }, + { "centi-degrees", "deg", FactMetaData::_centiDegreesToDegrees, FactMetaData::_degreesToCentiDegrees }, + { "radians", "deg", FactMetaData::_radiansToDegrees, FactMetaData::_degreesToRadians }, + { "gimbal-degrees", "deg", FactMetaData::_mavlinkGimbalDegreesToUserGimbalDegrees, FactMetaData::_userGimbalDegreesToMavlinkGimbalDegrees }, + { "norm", "%", FactMetaData::_normToPercent, FactMetaData::_percentToNorm }, }; // Translations driven by app settings @@ -209,6 +210,8 @@ QVariant FactMetaData::_minForType(void) const return QVariant(); case valueTypeBool: return QVariant(0); + case valueTypeElapsedTimeInSeconds: + return QVariant(0.0); } // Make windows compiler happy, even switch is full cased @@ -232,6 +235,7 @@ QVariant FactMetaData::_maxForType(void) const return QVariant(std::numeric_limits::max()); case valueTypeFloat: return QVariant(std::numeric_limits::max()); + case valueTypeElapsedTimeInSeconds: case valueTypeDouble: return QVariant(std::numeric_limits::max()); case valueTypeString: @@ -256,36 +260,34 @@ bool FactMetaData::convertAndValidateRaw(const QVariant& rawValue, bool convertO case FactMetaData::valueTypeInt32: typedValue = QVariant(rawValue.toInt(&convertOk)); if (!convertOnly && convertOk) { - if (rawMin() > typedValue || typedValue > rawMax()) { + if (typedValue < rawMin() || typedValue > rawMax()) { errorString = QString("Value must be within %1 and %2").arg(cookedMin().toInt()).arg(cookedMax().toInt()); } } break; - case FactMetaData::valueTypeUint8: case FactMetaData::valueTypeUint16: case FactMetaData::valueTypeUint32: typedValue = QVariant(rawValue.toUInt(&convertOk)); if (!convertOnly && convertOk) { - if (rawMin() > typedValue || typedValue > rawMax()) { + if (typedValue < rawMin() || typedValue > rawMax()) { errorString = QString("Value must be within %1 and %2").arg(cookedMin().toUInt()).arg(cookedMax().toUInt()); } } break; - case FactMetaData::valueTypeFloat: typedValue = QVariant(rawValue.toFloat(&convertOk)); if (!convertOnly && convertOk) { - if (rawMin() > typedValue || typedValue > rawMax()) { + if (typedValue < rawMin() || typedValue > rawMax()) { errorString = QString("Value must be within %1 and %2").arg(cookedMin().toFloat()).arg(cookedMax().toFloat()); } } break; - + case FactMetaData::valueTypeElapsedTimeInSeconds: case FactMetaData::valueTypeDouble: typedValue = QVariant(rawValue.toDouble(&convertOk)); if (!convertOnly && convertOk) { - if (rawMin() > typedValue || typedValue > rawMax()) { + if (typedValue < rawMin() || typedValue > rawMax()) { errorString = QString("Value must be within %1 and %2").arg(cookedMin().toDouble()).arg(cookedMax().toDouble()); } } @@ -324,7 +326,6 @@ bool FactMetaData::convertAndValidateCooked(const QVariant& cookedValue, bool co } } break; - case FactMetaData::valueTypeUint8: case FactMetaData::valueTypeUint16: case FactMetaData::valueTypeUint32: @@ -335,7 +336,6 @@ bool FactMetaData::convertAndValidateCooked(const QVariant& cookedValue, bool co } } break; - case FactMetaData::valueTypeFloat: typedValue = QVariant(cookedValue.toFloat(&convertOk)); if (!convertOnly && convertOk) { @@ -344,7 +344,7 @@ bool FactMetaData::convertAndValidateCooked(const QVariant& cookedValue, bool co } } break; - + case FactMetaData::valueTypeElapsedTimeInSeconds: case FactMetaData::valueTypeDouble: typedValue = QVariant(cookedValue.toDouble(&convertOk)); if (!convertOnly && convertOk) { @@ -455,6 +455,20 @@ QVariant FactMetaData::_degreesToCentiDegrees(const QVariant& degrees) return QVariant(qRound(degrees.toReal() * 100.0)); } +QVariant FactMetaData::_userGimbalDegreesToMavlinkGimbalDegrees(const QVariant& userGimbalDegrees) +{ + // User facing gimbal degree values are from 0 (level) to 90 (straight down) + // Mavlink gimbal degree values are from 0 (level) to -90 (straight down) + return userGimbalDegrees.toDouble() * -1.0; +} + +QVariant FactMetaData::_mavlinkGimbalDegreesToUserGimbalDegrees(const QVariant& mavlinkGimbalDegrees) +{ + // User facing gimbal degree values are from 0 (level) to 90 (straight down) + // Mavlink gimbal degree values are from 0 (level) to -90 (straight down) + return mavlinkGimbalDegrees.toDouble() * -1.0; +} + QVariant FactMetaData::_metersToFeet(const QVariant& meters) { return QVariant(meters.toDouble() * 1.0/constants.feetToMeters); @@ -589,7 +603,8 @@ FactMetaData::ValueType_t FactMetaData::stringToType(const QString& typeString, << QStringLiteral("Float") << QStringLiteral("Double") << QStringLiteral("String") - << QStringLiteral("Bool"); + << QStringLiteral("Bool") + << QStringLiteral("ElapsedSeconds"); knownTypes << valueTypeUint8 << valueTypeInt8 @@ -600,7 +615,8 @@ FactMetaData::ValueType_t FactMetaData::stringToType(const QString& typeString, << valueTypeFloat << valueTypeDouble << valueTypeString - << valueTypeBool; + << valueTypeBool + << valueTypeElapsedTimeInSeconds; for (int i=0; i FactMetaData::createMapFromJsonFile(const QString& return metaDataMap; } + +QVariant FactMetaData::cookedMax(void) const +{ + // We have to be careful with cooked min/max. Running the raw values through the translator could flip min and max. + QVariant cookedMax = _rawTranslator(_rawMax); + QVariant cookedMin = _rawTranslator(_rawMin); + if (cookedMax < cookedMin) { + // We need to flip + return cookedMin; + } else { + return cookedMax; + } +} + +QVariant FactMetaData::cookedMin(void) const +{ + // We have to be careful with cooked min/max. Running the raw values through the translator could flip min and max. + QVariant cookedMax = _rawTranslator(_rawMax); + QVariant cookedMin = _rawTranslator(_rawMin); + if (cookedMax < cookedMin) { + // We need to flip + return cookedMax; + } else { + return cookedMin; + } +} diff --git a/src/FactSystem/FactMetaData.h b/src/FactSystem/FactMetaData.h index 60f4e6c60f1f1e654f1859dc7740e2570e18bae5..f4f88d5f1f0422eedc86150884f1f6efac87a35d 100644 --- a/src/FactSystem/FactMetaData.h +++ b/src/FactSystem/FactMetaData.h @@ -39,7 +39,8 @@ public: valueTypeFloat, valueTypeDouble, valueTypeString, - valueTypeBool + valueTypeBool, + valueTypeElapsedTimeInSeconds, // Internally stored as double, valueString displays as HH:MM:SS } ValueType_t; typedef QVariant (*Translator)(const QVariant& from); @@ -83,10 +84,10 @@ public: QString group (void) const { return _group; } QString longDescription (void) const { return _longDescription;} QVariant rawMax (void) const { return _rawMax; } - QVariant cookedMax (void) const { return _rawTranslator(_rawMax); } + QVariant cookedMax (void) const; bool maxIsDefaultForType (void) const { return _maxIsDefaultForType; } QVariant rawMin (void) const { return _rawMin; } - QVariant cookedMin (void) const { return _rawTranslator(_rawMin); } + QVariant cookedMin (void) const; bool minIsDefaultForType (void) const { return _minIsDefaultForType; } QString name (void) const { return _name; } QString shortDescription (void) const { return _shortDescription; } @@ -155,6 +156,8 @@ private: static QVariant _radiansToDegrees(const QVariant& radians); static QVariant _centiDegreesToDegrees(const QVariant& centiDegrees); static QVariant _degreesToCentiDegrees(const QVariant& degrees); + static QVariant _userGimbalDegreesToMavlinkGimbalDegrees(const QVariant& userGimbalDegrees); + static QVariant _mavlinkGimbalDegreesToUserGimbalDegrees(const QVariant& mavlinkGimbalDegrees); static QVariant _metersToFeet(const QVariant& meters); static QVariant _feetToMeters(const QVariant& feet); static QVariant _squareMetersToSquareKilometers(const QVariant& squareMeters); diff --git a/src/FactSystem/ParameterManager.cc b/src/FactSystem/ParameterManager.cc index 2546f6b2de106f0b9a220736fe33cc82a7618a7c..a0a3c8613ef288be0191331c487673bd9465bb87 100644 --- a/src/FactSystem/ParameterManager.cc +++ b/src/FactSystem/ParameterManager.cc @@ -111,7 +111,7 @@ void ParameterManager::_parameterUpdate(int vehicleId, int componentId, QString // ArduPilot has this strange behavior of streaming parameters that we didn't ask for. This even happens before it responds to the // PARAM_REQUEST_LIST. We disregard any of this until the initial request is responded to. if (parameterId == 65535 && parameterName != "_HASH_CHECK" && _initialRequestTimeoutTimer.isActive()) { - qCDebug(ParameterManagerVerbose1Log) << "Disregarding unrequested param prior to intial list response" << parameterName; + qCDebug(ParameterManagerVerbose1Log) << "Disregarding unrequested param prior to initial list response" << parameterName; return; } @@ -303,11 +303,15 @@ void ParameterManager::_parameterUpdate(int vehicleId, int componentId, QString _dataMutex.unlock(); - Q_ASSERT(_mapParameterName2Variant[componentId].contains(parameterName)); - - Fact* fact = _mapParameterName2Variant[componentId][parameterName].value(); - Q_ASSERT(fact); - fact->_containerSetRawValue(value); + Fact* fact = NULL; + if (_mapParameterName2Variant[componentId].contains(parameterName)) { + fact = _mapParameterName2Variant[componentId][parameterName].value(); + } + if (fact) { + fact->_containerSetRawValue(value); + } else { + qWarning() << "Internal error"; + } if (componentParamsComplete) { if (componentId == _vehicle->defaultComponentId()) { @@ -352,18 +356,24 @@ void ParameterManager::_parameterUpdate(int vehicleId, int componentId, QString void ParameterManager::_valueUpdated(const QVariant& value) { Fact* fact = qobject_cast(sender()); - Q_ASSERT(fact); + if (!fact) { + qWarning() << "Internal error"; + return; + } int componentId = fact->componentId(); QString name = fact->name(); _dataMutex.lock(); - Q_ASSERT(_waitingWriteParamNameMap.contains(componentId)); - _waitingWriteParamNameMap[componentId].remove(name); // Remove any old entry - _waitingWriteParamNameMap[componentId][name] = 0; // Add new entry and set retry count - _waitingParamTimeoutTimer.start(); - _saveRequired = true; + if (_waitingWriteParamNameMap.contains(componentId)) { + _waitingWriteParamNameMap[componentId].remove(name); // Remove any old entry + _waitingWriteParamNameMap[componentId][name] = 0; // Add new entry and set retry count + _waitingParamTimeoutTimer.start(); + _saveRequired = true; + } else { + qWarning() << "Internal error"; + } _dataMutex.unlock(); @@ -397,7 +407,6 @@ void ParameterManager::refreshAllParameters(uint8_t componentId) _dataMutex.unlock(); MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); - Q_ASSERT(mavlink); mavlink_message_t msg; mavlink_msg_param_request_list_pack_chan(mavlink->getSystemId(), @@ -432,8 +441,6 @@ void ParameterManager::refreshParameter(int componentId, const QString& name) _dataMutex.lock(); - Q_ASSERT(_waitingReadParamNameMap.contains(componentId)); - if (_waitingReadParamNameMap.contains(componentId)) { QString mappedParamName = _remapParamNameToVersion(name); @@ -441,6 +448,8 @@ void ParameterManager::refreshParameter(int componentId, const QString& name) _waitingReadParamNameMap[componentId][mappedParamName] = 0; // Add new wait entry and update retry count qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "restarting _waitingParamTimeout"; _waitingParamTimeoutTimer.start(); + } else { + qWarning() << "Internal error"; } _dataMutex.unlock(); diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 3987eeff945da9da05a40afdf2f7150491fabebe..3ca2eebbbb4ace2b6acff4c85d50dbb540cc9626 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -139,9 +139,15 @@ QString APMCustomMode::modeString() const return mode; } +APMFirmwarePluginInstanceData::APMFirmwarePluginInstanceData(QObject* parent) + : QObject(parent) + , textSeverityAdjustmentNeeded(false) +{ + +} + APMFirmwarePlugin::APMFirmwarePlugin(void) : _coaxialMotors(false) - , _textSeverityAdjustmentNeeded(false) { qmlRegisterType ("QGroundControl.Controllers", 1, 0, "APMFlightModesComponentController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "APMAirframeComponentController"); @@ -321,6 +327,7 @@ void APMFirmwarePlugin::_handleOutgoingParamSet(Vehicle* vehicle, LinkInterface* bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_message_t* message) { QString messageText; + APMFirmwarePluginInstanceData* instanceData = qobject_cast(vehicle->firmwarePluginInstanceData()); mavlink_statustext_t statusText; mavlink_msg_statustext_decode(message, &statusText); @@ -334,7 +341,7 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_mess if (messageText.contains(APM_COPTER_REXP) || messageText.contains(APM_PLANE_REXP) || messageText.contains(APM_ROVER_REXP) || messageText.contains(APM_SUB_REXP)) { // found version string APMFirmwareVersion firmwareVersion(messageText); - _textSeverityAdjustmentNeeded = _isTextSeverityAdjustmentNeeded(firmwareVersion); + instanceData->textSeverityAdjustmentNeeded = _isTextSeverityAdjustmentNeeded(firmwareVersion); vehicle->setFirmwareVersion(firmwareVersion.majorNumber(), firmwareVersion.minorNumber(), firmwareVersion.patchNumber()); @@ -378,7 +385,7 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_mess } // adjust mesasge if needed - if (_textSeverityAdjustmentNeeded) { + if (instanceData->textSeverityAdjustmentNeeded) { _adjustSeverity(message); } @@ -416,10 +423,10 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_mess if (messageText.startsWith("PreArm")) { // ArduPilot PreArm messages can come across very frequently especially on Solo, which seems to send them once a second. // Filter them out if they come too quickly. - if (_noisyPrearmMap.contains(messageText) && _noisyPrearmMap[messageText].msecsTo(QTime::currentTime()) < (10 * 1000)) { + if (instanceData->noisyPrearmMap.contains(messageText) && instanceData->noisyPrearmMap[messageText].msecsTo(QTime::currentTime()) < (10 * 1000)) { return false; } - _noisyPrearmMap[messageText] = QTime::currentTime(); + instanceData->noisyPrearmMap[messageText] = QTime::currentTime(); vehicle->setPrearmError(messageText); } @@ -444,7 +451,7 @@ void APMFirmwarePlugin::_handleIncomingHeartbeat(Vehicle* vehicle, mavlink_messa } } - vehicle->setFlying(flying); + vehicle->_setFlying(flying); } bool APMFirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message) @@ -578,6 +585,8 @@ void APMFirmwarePlugin::_adjustCalibrationMessageSeverity(mavlink_message_t* mes void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle) { + vehicle->setFirmwarePluginInstanceData(new APMFirmwarePluginInstanceData); + if (vehicle->isOfflineEditingVehicle()) { switch (vehicle->vehicleType()) { case MAV_TYPE_QUADROTOR: diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index f53f1fa8215782c0f841aae16f8461aa8dc23e4c..ce46554f0dd46c7bc2b17a5a09ed40cf698c2522 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -120,13 +120,24 @@ private: void _handleOutgoingParamSet(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message); void _soloVideoHandshake(Vehicle* vehicle); - bool _textSeverityAdjustmentNeeded; + // Any instance data here must be global to all vehicles + // Vehicle specific data should go into APMFirmwarePluginInstanceData + QList _supportedModes; - QMap _noisyPrearmMap; static const char* _artooIP; static const int _artooVideoHandshakePort; +}; + +class APMFirmwarePluginInstanceData : public QObject +{ + Q_OBJECT + +public: + APMFirmwarePluginInstanceData(QObject* parent = NULL); + bool textSeverityAdjustmentNeeded; + QMap noisyPrearmMap; }; #endif diff --git a/src/FirmwarePlugin/APM/APMGeoFenceManager.cc b/src/FirmwarePlugin/APM/APMGeoFenceManager.cc index 181929caddac22e368c94a2be97e5d49a22e9cab..ef35e2d43c37298229d000173ede2e2a9e8d5e93 100644 --- a/src/FirmwarePlugin/APM/APMGeoFenceManager.cc +++ b/src/FirmwarePlugin/APM/APMGeoFenceManager.cc @@ -82,6 +82,7 @@ void APMGeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, QmlOb // Total point count, +1 polygon close in last index, +1 for breach in index 0 _cWriteFencePoints = validatedPolygonCount ? validatedPolygonCount + 1 + 1 : 0; + qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::sendToVehicle validatedPolygonCount:_cWriteFencePoints" << validatedPolygonCount << _cWriteFencePoints; _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceTotalParam)->setRawValue(_cWriteFencePoints); // FIXME: No validation of correct fence received @@ -89,7 +90,7 @@ void APMGeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, QmlOb _sendFencePoint(index); } - emit loadComplete(_breachReturnPoint, _polygon); + emit sendComplete(false /* error */); } void APMGeoFenceManager::loadFromVehicle(void) @@ -132,7 +133,7 @@ void APMGeoFenceManager::loadFromVehicle(void) _requestFencePoint(_currentFencePoint); } -/// Called when a new mavlink message for out vehicle is received +/// Called when a new mavlink message for our vehicle is received void APMGeoFenceManager::_mavlinkMessageReceived(const mavlink_message_t& message) { if (message.msgid == MAVLINK_MSG_ID_FENCE_POINT) { @@ -326,7 +327,10 @@ void APMGeoFenceManager::_parametersReady(void) void APMGeoFenceManager::removeAll(void) { + qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::removeAll"; + QmlObjectListModel emptyPolygon; sendToVehicle(_breachReturnPoint, emptyPolygon); + emit removeAllComplete(false /* error */); } diff --git a/src/FirmwarePlugin/APM/APMParameterMetaData.cc b/src/FirmwarePlugin/APM/APMParameterMetaData.cc index ac9a11fda88e0de49edfa2c5a391bd041960d692..66579bf68d23a922250fc9c4da01d42953758d74 100644 --- a/src/FirmwarePlugin/APM/APMParameterMetaData.cc +++ b/src/FirmwarePlugin/APM/APMParameterMetaData.cc @@ -52,6 +52,7 @@ QVariant APMParameterMetaData::_stringToTypedVariant(const QString& string, case FactMetaData::valueTypeFloat: convertTo = QMetaType::Float; break; + case FactMetaData::valueTypeElapsedTimeInSeconds: case FactMetaData::valueTypeDouble: convertTo = QVariant::Double; break; diff --git a/src/FirmwarePlugin/APM/APMRallyPointManager.cc b/src/FirmwarePlugin/APM/APMRallyPointManager.cc index 0e44314d93e138fca67f63c3478036143c4dd476..3e1044aad408c2d874131b5c8626c8762c1670bb 100644 --- a/src/FirmwarePlugin/APM/APMRallyPointManager.cc +++ b/src/FirmwarePlugin/APM/APMRallyPointManager.cc @@ -48,7 +48,7 @@ void APMRallyPointManager::sendToVehicle(const QList& rgPoints) _sendRallyPoint(index); } - emit loadComplete(_rgPoints); + emit sendComplete(false /* error */); } void APMRallyPointManager::loadFromVehicle(void) @@ -60,7 +60,7 @@ void APMRallyPointManager::loadFromVehicle(void) _rgPoints.clear(); _cReadRallyPoints = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _rallyTotalParam)->rawValue().toInt(); - qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::loadFromVehicle" << _cReadRallyPoints; + qCDebug(RallyPointManagerLog) << "APMRallyPointManager::loadFromVehicle - point count" << _cReadRallyPoints; if (_cReadRallyPoints == 0) { emit loadComplete(_rgPoints); return; @@ -152,7 +152,10 @@ bool APMRallyPointManager::rallyPointsSupported(void) const void APMRallyPointManager::removeAll(void) { + qCDebug(RallyPointManagerLog) << "APMRallyPointManager::removeAll"; + QList noRallyPoints; sendToVehicle(noRallyPoints); + emit removeAllComplete(false /* error */); } diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index 57094a7bd5c01ca1a53623e9d1f7a688067be82a..4d07f66673ac31ab1ce182c16f0986e2e9d2cb13 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -108,6 +108,39 @@ ArduCopterFirmwarePlugin::ArduCopterFirmwarePlugin(void) remapV3_5["SERVO13_FUNCTION"] = QStringLiteral("RC13_FUNCTION"); remapV3_5["SERVO14_FUNCTION"] = QStringLiteral("RC14_FUNCTION"); + remapV3_5["SERVO5_MIN"] = QStringLiteral("RC5_MIN"); + remapV3_5["SERVO6_MIN"] = QStringLiteral("RC6_MIN"); + remapV3_5["SERVO7_MIN"] = QStringLiteral("RC7_MIN"); + remapV3_5["SERVO8_MIN"] = QStringLiteral("RC8_MIN"); + remapV3_5["SERVO9_MIN"] = QStringLiteral("RC9_MIN"); + remapV3_5["SERVO10_MIN"] = QStringLiteral("RC10_MIN"); + remapV3_5["SERVO11_MIN"] = QStringLiteral("RC11_MIN"); + remapV3_5["SERVO12_MIN"] = QStringLiteral("RC12_MIN"); + remapV3_5["SERVO13_MIN"] = QStringLiteral("RC13_MIN"); + remapV3_5["SERVO14_MIN"] = QStringLiteral("RC14_MIN"); + + remapV3_5["SERVO5_MAX"] = QStringLiteral("RC5_MAX"); + remapV3_5["SERVO6_MAX"] = QStringLiteral("RC6_MAX"); + remapV3_5["SERVO7_MAX"] = QStringLiteral("RC7_MAX"); + remapV3_5["SERVO8_MAX"] = QStringLiteral("RC8_MAX"); + remapV3_5["SERVO9_MAX"] = QStringLiteral("RC9_MAX"); + remapV3_5["SERVO10_MAX"] = QStringLiteral("RC10_MAX"); + remapV3_5["SERVO11_MAX"] = QStringLiteral("RC11_MAX"); + remapV3_5["SERVO12_MAX"] = QStringLiteral("RC12_MAX"); + remapV3_5["SERVO13_MAX"] = QStringLiteral("RC13_MAX"); + remapV3_5["SERVO14_MAX"] = QStringLiteral("RC14_MAX"); + + remapV3_5["SERVO5_REVERSED"] = QStringLiteral("RC5_REVERSED"); + remapV3_5["SERVO6_REVERSED"] = QStringLiteral("RC6_REVERSED"); + remapV3_5["SERVO7_REVERSED"] = QStringLiteral("RC7_REVERSED"); + remapV3_5["SERVO8_REVERSED"] = QStringLiteral("RC8_REVERSED"); + remapV3_5["SERVO9_REVERSED"] = QStringLiteral("RC9_REVERSED"); + remapV3_5["SERVO10_REVERSED"] = QStringLiteral("RC10_REVERSED"); + remapV3_5["SERVO11_REVERSED"] = QStringLiteral("RC11_REVERSED"); + remapV3_5["SERVO12_REVERSED"] = QStringLiteral("RC12_REVERSED"); + remapV3_5["SERVO13_REVERSED"] = QStringLiteral("RC13_REVERSED"); + remapV3_5["SERVO14_REVERSED"] = QStringLiteral("RC14_REVERSED"); + _remapParamNameIntialized = true; } } @@ -139,7 +172,7 @@ void ArduCopterFirmwarePlugin::guidedModeLand(Vehicle* vehicle) void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle) { - if (!_armVehicle(vehicle)) { + if (!_armVehicleAndValidate(vehicle)) { qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to arm.")); return; } @@ -237,23 +270,13 @@ QString ArduCopterFirmwarePlugin::geoFenceRadiusParam(Vehicle* vehicle) bool ArduCopterFirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const { - if (!vehicle->isOfflineEditingVehicle() && vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("WP_YAW_BEHAVIOR"))) { - Fact* yawMode = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("WP_YAW_BEHAVIOR")); - return yawMode && yawMode->rawValue().toInt() != 0; + if (vehicle->isOfflineEditingVehicle()) { + return FirmwarePlugin::vehicleYawsToNextWaypointInMission(vehicle); + } else { + if (vehicle->multiRotor() && vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("WP_YAW_BEHAVIOR"))) { + Fact* yawMode = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("WP_YAW_BEHAVIOR")); + return yawMode && yawMode->rawValue().toInt() != 0; + } } return true; } - -void ArduCopterFirmwarePlugin::missionFlightSpeedInfo(Vehicle* vehicle, double& hoverSpeed, double& cruiseSpeed) -{ - QString hoverSpeedParam("WPNAV_SPEED"); - - // First pull settings defaults - FirmwarePlugin::missionFlightSpeedInfo(vehicle, hoverSpeed, cruiseSpeed); - - cruiseSpeed = 0; - if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, hoverSpeedParam)) { - Fact* speed = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, hoverSpeedParam); - hoverSpeed = speed->rawValue().toDouble() / 100; // cm/s -> m/s - } -} diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h index 5f35bfa15d53f2c92a040deeb2f510d4b923586b..345fdb232e35d043fff0161d3b861e6203254ef7 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h @@ -76,7 +76,6 @@ public: QString takeControlFlightMode(void) const override { return QString("Stablize"); } bool vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const final; QString autoDisarmParameter(Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral("DISARM_DELAY"); } - void missionFlightSpeedInfo(Vehicle* vehicle, double& hoverSpeed, double& cruiseSpeed) override; private: static bool _remapParamNameIntialized; diff --git a/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.cc index 996551e100d4825816fdc56e95b3c2f8630c9631..8b7953bf6039bbd1505ff411d0729a3a410046c9 100644 --- a/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.cc @@ -77,6 +77,39 @@ ArduPlaneFirmwarePlugin::ArduPlaneFirmwarePlugin(void) remapV3_8["SERVO13_FUNCTION"] = QStringLiteral("RC13_FUNCTION"); remapV3_8["SERVO14_FUNCTION"] = QStringLiteral("RC14_FUNCTION"); + remapV3_8["SERVO5_MIN"] = QStringLiteral("RC5_MIN"); + remapV3_8["SERVO6_MIN"] = QStringLiteral("RC6_MIN"); + remapV3_8["SERVO7_MIN"] = QStringLiteral("RC7_MIN"); + remapV3_8["SERVO8_MIN"] = QStringLiteral("RC8_MIN"); + remapV3_8["SERVO9_MIN"] = QStringLiteral("RC9_MIN"); + remapV3_8["SERVO10_MIN"] = QStringLiteral("RC10_MIN"); + remapV3_8["SERVO11_MIN"] = QStringLiteral("RC11_MIN"); + remapV3_8["SERVO12_MIN"] = QStringLiteral("RC12_MIN"); + remapV3_8["SERVO13_MIN"] = QStringLiteral("RC13_MIN"); + remapV3_8["SERVO14_MIN"] = QStringLiteral("RC14_MIN"); + + remapV3_8["SERVO5_MAX"] = QStringLiteral("RC5_MAX"); + remapV3_8["SERVO6_MAX"] = QStringLiteral("RC6_MAX"); + remapV3_8["SERVO7_MAX"] = QStringLiteral("RC7_MAX"); + remapV3_8["SERVO8_MAX"] = QStringLiteral("RC8_MAX"); + remapV3_8["SERVO9_MAX"] = QStringLiteral("RC9_MAX"); + remapV3_8["SERVO10_MAX"] = QStringLiteral("RC10_MAX"); + remapV3_8["SERVO11_MAX"] = QStringLiteral("RC11_MAX"); + remapV3_8["SERVO12_MAX"] = QStringLiteral("RC12_MAX"); + remapV3_8["SERVO13_MAX"] = QStringLiteral("RC13_MAX"); + remapV3_8["SERVO14_MAX"] = QStringLiteral("RC14_MAX"); + + remapV3_8["SERVO5_REVERSED"] = QStringLiteral("RC5_REVERSED"); + remapV3_8["SERVO6_REVERSED"] = QStringLiteral("RC6_REVERSED"); + remapV3_8["SERVO7_REVERSED"] = QStringLiteral("RC7_REVERSED"); + remapV3_8["SERVO8_REVERSED"] = QStringLiteral("RC8_REVERSED"); + remapV3_8["SERVO9_REVERSED"] = QStringLiteral("RC9_REVERSED"); + remapV3_8["SERVO10_REVERSED"] = QStringLiteral("RC10_REVERSED"); + remapV3_8["SERVO11_REVERSED"] = QStringLiteral("RC11_REVERSED"); + remapV3_8["SERVO12_REVERSED"] = QStringLiteral("RC12_REVERSED"); + remapV3_8["SERVO13_REVERSED"] = QStringLiteral("RC13_REVERSED"); + remapV3_8["SERVO14_REVERSED"] = QStringLiteral("RC14_REVERSED"); + _remapParamNameIntialized = true; } } diff --git a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc index bbb7ed961e1155518bd6d6a17c87d2f326a768f2..1e30340f13b01bc04be10f2bdd4d46248fa8a1ac 100644 --- a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc @@ -55,6 +55,39 @@ ArduRoverFirmwarePlugin::ArduRoverFirmwarePlugin(void) remapV3_2["SERVO13_FUNCTION"] = QStringLiteral("RC13_FUNCTION"); remapV3_2["SERVO14_FUNCTION"] = QStringLiteral("RC14_FUNCTION"); + remapV3_2["SERVO5_MIN"] = QStringLiteral("RC5_MIN"); + remapV3_2["SERVO6_MIN"] = QStringLiteral("RC6_MIN"); + remapV3_2["SERVO7_MIN"] = QStringLiteral("RC7_MIN"); + remapV3_2["SERVO8_MIN"] = QStringLiteral("RC8_MIN"); + remapV3_2["SERVO9_MIN"] = QStringLiteral("RC9_MIN"); + remapV3_2["SERVO10_MIN"] = QStringLiteral("RC10_MIN"); + remapV3_2["SERVO11_MIN"] = QStringLiteral("RC11_MIN"); + remapV3_2["SERVO12_MIN"] = QStringLiteral("RC12_MIN"); + remapV3_2["SERVO13_MIN"] = QStringLiteral("RC13_MIN"); + remapV3_2["SERVO14_MIN"] = QStringLiteral("RC14_MIN"); + + remapV3_2["SERVO5_MAX"] = QStringLiteral("RC5_MAX"); + remapV3_2["SERVO6_MAX"] = QStringLiteral("RC6_MAX"); + remapV3_2["SERVO7_MAX"] = QStringLiteral("RC7_MAX"); + remapV3_2["SERVO8_MAX"] = QStringLiteral("RC8_MAX"); + remapV3_2["SERVO9_MAX"] = QStringLiteral("RC9_MAX"); + remapV3_2["SERVO10_MAX"] = QStringLiteral("RC10_MAX"); + remapV3_2["SERVO11_MAX"] = QStringLiteral("RC11_MAX"); + remapV3_2["SERVO12_MAX"] = QStringLiteral("RC12_MAX"); + remapV3_2["SERVO13_MAX"] = QStringLiteral("RC13_MAX"); + remapV3_2["SERVO14_MAX"] = QStringLiteral("RC14_MAX"); + + remapV3_2["SERVO5_REVERSED"] = QStringLiteral("RC5_REVERSED"); + remapV3_2["SERVO6_REVERSED"] = QStringLiteral("RC6_REVERSED"); + remapV3_2["SERVO7_REVERSED"] = QStringLiteral("RC7_REVERSED"); + remapV3_2["SERVO8_REVERSED"] = QStringLiteral("RC8_REVERSED"); + remapV3_2["SERVO9_REVERSED"] = QStringLiteral("RC9_REVERSED"); + remapV3_2["SERVO10_REVERSED"] = QStringLiteral("RC10_REVERSED"); + remapV3_2["SERVO11_REVERSED"] = QStringLiteral("RC11_REVERSED"); + remapV3_2["SERVO12_REVERSED"] = QStringLiteral("RC12_REVERSED"); + remapV3_2["SERVO13_REVERSED"] = QStringLiteral("RC13_REVERSED"); + remapV3_2["SERVO14_REVERSED"] = QStringLiteral("RC14_REVERSED"); + _remapParamNameIntialized = true; } } diff --git a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc index 1dfa4fdf58659db0f3e219971a66838c88fa1303..992323af30c73bf4795e1f757247013c46537c0c 100644 --- a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc @@ -62,6 +62,41 @@ ArduSubFirmwarePlugin::ArduSubFirmwarePlugin(void) remapV3_5["SERVO13_FUNCTION"] = QStringLiteral("RC13_FUNCTION"); remapV3_5["SERVO14_FUNCTION"] = QStringLiteral("RC14_FUNCTION"); + remapV3_5["SERVO5_MIN"] = QStringLiteral("RC5_MIN"); + remapV3_5["SERVO6_MIN"] = QStringLiteral("RC6_MIN"); + remapV3_5["SERVO7_MIN"] = QStringLiteral("RC7_MIN"); + remapV3_5["SERVO8_MIN"] = QStringLiteral("RC8_MIN"); + remapV3_5["SERVO9_MIN"] = QStringLiteral("RC9_MIN"); + remapV3_5["SERVO10_MIN"] = QStringLiteral("RC10_MIN"); + remapV3_5["SERVO11_MIN"] = QStringLiteral("RC11_MIN"); + remapV3_5["SERVO12_MIN"] = QStringLiteral("RC12_MIN"); + remapV3_5["SERVO13_MIN"] = QStringLiteral("RC13_MIN"); + remapV3_5["SERVO14_MIN"] = QStringLiteral("RC14_MIN"); + + remapV3_5["SERVO5_MAX"] = QStringLiteral("RC5_MAX"); + remapV3_5["SERVO6_MAX"] = QStringLiteral("RC6_MAX"); + remapV3_5["SERVO7_MAX"] = QStringLiteral("RC7_MAX"); + remapV3_5["SERVO8_MAX"] = QStringLiteral("RC8_MAX"); + remapV3_5["SERVO9_MAX"] = QStringLiteral("RC9_MAX"); + remapV3_5["SERVO10_MAX"] = QStringLiteral("RC10_MAX"); + remapV3_5["SERVO11_MAX"] = QStringLiteral("RC11_MAX"); + remapV3_5["SERVO12_MAX"] = QStringLiteral("RC12_MAX"); + remapV3_5["SERVO13_MAX"] = QStringLiteral("RC13_MAX"); + remapV3_5["SERVO14_MAX"] = QStringLiteral("RC14_MAX"); + + remapV3_5["SERVO5_REVERSED"] = QStringLiteral("RC5_REVERSED"); + remapV3_5["SERVO6_REVERSED"] = QStringLiteral("RC6_REVERSED"); + remapV3_5["SERVO7_REVERSED"] = QStringLiteral("RC7_REVERSED"); + remapV3_5["SERVO8_REVERSED"] = QStringLiteral("RC8_REVERSED"); + remapV3_5["SERVO9_REVERSED"] = QStringLiteral("RC9_REVERSED"); + remapV3_5["SERVO10_REVERSED"] = QStringLiteral("RC10_REVERSED"); + remapV3_5["SERVO11_REVERSED"] = QStringLiteral("RC11_REVERSED"); + remapV3_5["SERVO12_REVERSED"] = QStringLiteral("RC12_REVERSED"); + remapV3_5["SERVO13_REVERSED"] = QStringLiteral("RC13_REVERSED"); + remapV3_5["SERVO14_REVERSED"] = QStringLiteral("RC14_REVERSED"); + + remapV3_5["FENCE_ALT_MIN"] = QStringLiteral("FENCE_DEPTH_MAX"); + _remapParamNameIntialized = true; } } @@ -114,7 +149,9 @@ const QVariantList& ArduSubFirmwarePlugin::toolBarIndicators(const Vehicle* vehi if(_toolBarIndicators.size() == 0) { _toolBarIndicators.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/MessageIndicator.qml"))); _toolBarIndicators.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/BatteryIndicator.qml"))); + _toolBarIndicators.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/JoystickIndicator.qml"))); _toolBarIndicators.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/ModeIndicator.qml"))); + _toolBarIndicators.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/ArmedIndicator.qml"))); } return _toolBarIndicators; } diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 9dc62344c62e8d7edd49cc8fb94ae4b81fef13c7..ecc9ff8bae4feae409f8ed4519e7362703be9e96 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -435,7 +435,7 @@ bool FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) return vehicle->multiRotor() ? false : true; } -bool FirmwarePlugin::_armVehicle(Vehicle* vehicle) +bool FirmwarePlugin::_armVehicleAndValidate(Vehicle* vehicle) { if (!vehicle->armed()) { vehicle->setArmed(true); @@ -466,12 +466,11 @@ QString FirmwarePlugin::autoDisarmParameter(Vehicle* vehicle) return QString(); } -void FirmwarePlugin::missionFlightSpeedInfo(Vehicle* vehicle, double& hoverSpeed, double& cruiseSpeed) +bool FirmwarePlugin::hasGimbal(Vehicle* vehicle, bool& rollSupported, bool& pitchSupported, bool& yawSupported) { Q_UNUSED(vehicle); - - // Best we can do is use settings - AppSettings* appSettings = qgcApp()->toolbox()->settingsManager()->appSettings(); - hoverSpeed = appSettings->offlineEditingHoverSpeed()->rawValue().toDouble(); - cruiseSpeed = appSettings->offlineEditingCruiseSpeed()->rawValue().toDouble(); + rollSupported = false; + pitchSupported = false; + yawSupported = false; + return false; } diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 5c6a6c6865847715668f167c55b6a2180306c124..0a94055056021ae55c1c335b9d9a37d54242a82f 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -62,7 +62,7 @@ public: /// value: remapParamNameMinorVersionRemapMap_t entry typedef QMap remapParamNameMajorVersionMap_t; - /// @return The AutoPilotPlugin associated with this firmware plugin. Must be overriden. + /// @return The AutoPilotPlugin associated with this firmware plugin. Must be overridden. virtual AutoPilotPlugin* autopilotPlugin(Vehicle* vehicle); /// Called when Vehicle is first created to perform any firmware specific setup. @@ -282,21 +282,23 @@ public: /// @param[out] cruiseAmps Current draw in amps during cruise virtual void batteryConsumptionData(Vehicle* vehicle, int& mAhBattery, double& hoverAmps, double& cruiseAmps) const; - /// Returns the default mission flight speeds. - /// @param[out] hoverSpeed Flight speed for vehicle flying in multi-rotor mode. 0 for none, or not available. - /// @param[out] cruiseSpeed Flight speed for vehicle flying in fixed wing forward flight mode. 0 for none, or not available. - virtual void missionFlightSpeedInfo(Vehicle* vehicle, double& hoverSpeed, double& cruiseSpeed); - - // Returns the parameter which control auto-dismar. Assume == 0 means no auto disarm + // Returns the parameter which control auto-disarm. Assume == 0 means no auto disarm virtual QString autoDisarmParameter(Vehicle* vehicle); + /// Used to determine whether a vehicle has a gimbal. + /// @param[out] rollSupported Gimbal supports roll + /// @param[out] pitchSupported Gimbal supports pitch + /// @param[out] yawSupported Gimbal supports yaw + /// @return true: vehicle has gimbal, false: gimbal support unknown + virtual bool hasGimbal(Vehicle* vehicle, bool& rollSupported, bool& pitchSupported, bool& yawSupported); + // FIXME: Hack workaround for non pluginize FollowMe support static const char* px4FollowMeFlightMode; protected: // Arms the vehicle, waiting for the arm state to change. // @return: true - vehicle armed, false - vehicle failed to arm - bool _armVehicle(Vehicle* vehicle); + bool _armVehicleAndValidate(Vehicle* vehicle); private: QVariantList _toolBarIndicatorList; diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index ce622359f3d2bf86e039efd72d9370a93700c9da..161f8b77a6f59bc522b5e3fd900dd03c4630af83 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -26,6 +26,13 @@ #include "px4_custom_mode.h" +PX4FirmwarePluginInstanceData::PX4FirmwarePluginInstanceData(QObject* parent) + : QObject(parent) + , versionNotified(false) +{ + +} + PX4FirmwarePlugin::PX4FirmwarePlugin(void) : _manualFlightMode(tr("Manual")) , _acroFlightMode(tr("Acro")) @@ -43,7 +50,6 @@ PX4FirmwarePlugin::PX4FirmwarePlugin(void) , _rtgsFlightMode(tr("Return to Groundstation")) , _followMeFlightMode(tr("Follow Me")) , _simpleFlightMode(tr("Simple")) - , _versionNotified(false) { qmlRegisterType ("QGroundControl.Controllers", 1, 0, "PX4AdvancedFlightModesController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "PX4SimpleFlightModesController"); @@ -229,9 +235,7 @@ bool PX4FirmwarePlugin::isCapable(const Vehicle *vehicle, FirmwareCapabilities c void PX4FirmwarePlugin::initializeVehicle(Vehicle* vehicle) { - Q_UNUSED(vehicle); - - // PX4 Flight Stack doesn't need to do any extra work + vehicle->setFirmwarePluginInstanceData(new PX4FirmwarePluginInstanceData); } bool PX4FirmwarePlugin::sendHomePositionToVehicle(void) @@ -268,6 +272,7 @@ QList PX4FirmwarePlugin::supportedMissionCommands(void) << MAV_CMD_DO_LAND_START << MAV_CMD_DO_MOUNT_CONFIGURE << MAV_CMD_DO_MOUNT_CONTROL + << MAV_CMD_SET_CAMERA_MODE << MAV_CMD_IMAGE_START_CAPTURE << MAV_CMD_IMAGE_STOP_CAPTURE << MAV_CMD_VIDEO_START_CAPTURE << MAV_CMD_VIDEO_STOP_CAPTURE << MAV_CMD_NAV_DELAY; @@ -352,6 +357,29 @@ void PX4FirmwarePlugin::guidedModeOrbit(Vehicle* vehicle, const QGeoCoordinate& centerCoord.isValid() ? centerCoord.altitude() : NAN); } +void PX4FirmwarePlugin::_mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle) +{ + Q_UNUSED(vehicleId); + Q_UNUSED(component); + Q_UNUSED(noReponseFromVehicle); + + Vehicle* vehicle = dynamic_cast(sender()); + if (!vehicle) { + qWarning() << "Dynamic cast failed!"; + return; + } + + if (command == MAV_CMD_NAV_TAKEOFF && result == MAV_RESULT_ACCEPTED) { + // Now that we are in takeoff mode we can arm the vehicle which will cause it to takeoff. + // We specifically don't retry arming if it fails. This way we don't fight with the user if + // They are trying to disarm. + disconnect(vehicle, &Vehicle::mavCommandResult, this, &PX4FirmwarePlugin::_mavCommandResult); + if (!vehicle->armed()) { + vehicle->setArmed(true); + } + } +} + void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle) { QString takeoffAltParam("MIS_TAKEOFF_ALT"); @@ -367,11 +395,7 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle) } Fact* takeoffAlt = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam); - if (!_armVehicle(vehicle)) { - qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to arm.")); - return; - } - + connect(vehicle, &Vehicle::mavCommandResult, this, &PX4FirmwarePlugin::_mavCommandResult); vehicle->sendMavCommand(vehicle->defaultComponentId(), MAV_CMD_NAV_TAKEOFF, true, // show error is fails @@ -436,7 +460,7 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu void PX4FirmwarePlugin::startMission(Vehicle* vehicle) { - if (!_armVehicle(vehicle)) { + if (!_armVehicleAndValidate(vehicle)) { qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to arm.")); return; } @@ -480,7 +504,8 @@ void PX4FirmwarePlugin::_handleAutopilotVersion(Vehicle* vehicle, mavlink_messag { Q_UNUSED(vehicle); - if (!_versionNotified) { + PX4FirmwarePluginInstanceData* instanceData = qobject_cast(vehicle->firmwarePluginInstanceData()); + if (!instanceData->versionNotified) { bool notifyUser = false; int supportedMajorVersion = 1; int supportedMinorVersion = 4; @@ -510,7 +535,7 @@ void PX4FirmwarePlugin::_handleAutopilotVersion(Vehicle* vehicle, mavlink_messag } if (notifyUser) { - _versionNotified = true; + instanceData->versionNotified = true; qgcApp()->showMessage(QString("QGroundControl supports PX4 Pro firmware Version %1.%2.%3 and above. You are using a version prior to that which will lead to unpredictable results. Please upgrade your firmware.").arg(supportedMajorVersion).arg(supportedMinorVersion).arg(supportedPatchVersion)); } } @@ -518,27 +543,13 @@ void PX4FirmwarePlugin::_handleAutopilotVersion(Vehicle* vehicle, mavlink_messag bool PX4FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const { - if (!vehicle->isOfflineEditingVehicle() && vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("MIS_YAWMODE"))) { - Fact* yawMode = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("MIS_YAWMODE")); - return yawMode && yawMode->rawValue().toInt() == 1; + if (vehicle->isOfflineEditingVehicle()) { + return FirmwarePlugin::vehicleYawsToNextWaypointInMission(vehicle); + } else { + if (vehicle->multiRotor() && vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("MIS_YAWMODE"))) { + Fact* yawMode = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("MIS_YAWMODE")); + return yawMode && yawMode->rawValue().toInt() == 1; + } } return true; } - -void PX4FirmwarePlugin::missionFlightSpeedInfo(Vehicle* vehicle, double& hoverSpeed, double& cruiseSpeed) -{ - QString hoverSpeedParam("MPC_XY_CRUISE"); - QString cruiseSpeedParam("FW_AIRSPD_TRIM"); - - // First pull settings defaults - FirmwarePlugin::missionFlightSpeedInfo(vehicle, hoverSpeed, cruiseSpeed); - - if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, hoverSpeedParam)) { - Fact* speed = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, hoverSpeedParam); - hoverSpeed = speed->rawValue().toDouble(); - } - if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, cruiseSpeedParam)) { - Fact* speed = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, cruiseSpeedParam); - cruiseSpeed = speed->rawValue().toDouble(); - } -} diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index 9e38ad582644c87b70994fb741321d6abe1aa502..bda3803a9848fce269f7ab5eb5e75f84f071bddf 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -68,7 +68,6 @@ public: QString brandImageOutdoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/PX4/BrandImage"); } bool vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const override; QString autoDisarmParameter (Vehicle* vehicle) override { Q_UNUSED(vehicle); return QStringLiteral("COM_DISARM_LAND"); } - void missionFlightSpeedInfo (Vehicle* vehicle, double& hoverSpeed, double& cruiseSpeed) override; protected: typedef struct { @@ -103,10 +102,24 @@ protected: QString _followMeFlightMode; QString _simpleFlightMode; +private slots: + void _mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle); + private: void _handleAutopilotVersion(Vehicle* vehicle, mavlink_message_t* message); - bool _versionNotified; ///< true: user notified over version issue + // Any instance data here must be global to all vehicles + // Vehicle specific data should go into PX4FirmwarePluginInstanceData +}; + +class PX4FirmwarePluginInstanceData : public QObject +{ + Q_OBJECT + +public: + PX4FirmwarePluginInstanceData(QObject* parent = NULL); + + bool versionNotified; ///< true: user notified over version issue }; #endif diff --git a/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml b/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml index 4da3065bd8cc04685af6f11605e5b98f04a2adda..d13163e97784881364176a7097952b273a77e317 100644 --- a/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml +++ b/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml @@ -3399,7 +3399,7 @@ if required for the gimbal (only in AUX output mode) Yaw feed forward - Feed forward weight for manual yaw control. 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + Feed forward weight for manual yaw control. 0 will give slow response and no overshot, 1 - fast response and big overshot. 0.0 1.0 2 @@ -3642,7 +3642,7 @@ if required for the gimbal (only in AUX output mode) Vertical velocity feed forward - Feed forward weight for altitude control in stabilized modes (ALTCTRL, POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + Feed forward weight for altitude control in stabilized modes (ALTCTRL, POSCTRL). 0 will give slow response and no overshot, 1 - fast response and big overshot. 0.0 1.0 2 @@ -3718,7 +3718,7 @@ if required for the gimbal (only in AUX output mode) Horizontal velocity feed forward - Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow response and no overshot, 1 - fast response and big overshot. 0.0 1.0 2 diff --git a/src/FirmwarePlugin/PX4/PX4ParameterMetaData.cc b/src/FirmwarePlugin/PX4/PX4ParameterMetaData.cc index a3c9e1e20a094da692504cdb6a17e980e817bc22..70de2c85b934f3295e9062cbf350a2ff7c3d7c6e 100644 --- a/src/FirmwarePlugin/PX4/PX4ParameterMetaData.cc +++ b/src/FirmwarePlugin/PX4/PX4ParameterMetaData.cc @@ -52,6 +52,7 @@ QVariant PX4ParameterMetaData::_stringToTypedVariant(const QString& string, Fact case FactMetaData::valueTypeFloat: convertTo = QMetaType::Float; break; + case FactMetaData::valueTypeElapsedTimeInSeconds: case FactMetaData::valueTypeDouble: convertTo = QVariant::Double; break; @@ -79,7 +80,7 @@ void PX4ParameterMetaData::loadParameterFactMetaDataFile(const QString& metaData return; } _parameterMetaDataLoaded = true; - + qCDebug(PX4ParameterMetaDataLog) << "Loading parameter meta data:" << metaDataFile; QFile xmlFile(metaDataFile); @@ -218,136 +219,132 @@ void PX4ParameterMetaData::loadParameterFactMetaDataFile(const QString& metaData } if (!badMetaData) { - if (elementName == "short_desc") { - Q_ASSERT(metaData); - QString text = xml.readElementText(); - text = text.replace("\n", " "); - qCDebug(PX4ParameterMetaDataLog) << "Short description:" << text; - metaData->setShortDescription(text); - - } else if (elementName == "long_desc") { - Q_ASSERT(metaData); - QString text = xml.readElementText(); - text = text.replace("\n", " "); - qCDebug(PX4ParameterMetaDataLog) << "Long description:" << text; - metaData->setLongDescription(text); - - } else if (elementName == "min") { - Q_ASSERT(metaData); - QString text = xml.readElementText(); - qCDebug(PX4ParameterMetaDataLog) << "Min:" << text; - - QVariant varMin; - if (metaData->convertAndValidateRaw(text, true /* convertOnly */, varMin, errorString)) { - metaData->setRawMin(varMin); - } else { - qCWarning(PX4ParameterMetaDataLog) << "Invalid min value, name:" << metaData->name() << " type:" << metaData->type() << " min:" << text << " error:" << errorString; - } - - } else if (elementName == "max") { - Q_ASSERT(metaData); - QString text = xml.readElementText(); - qCDebug(PX4ParameterMetaDataLog) << "Max:" << text; - - QVariant varMax; - if (metaData->convertAndValidateRaw(text, true /* convertOnly */, varMax, errorString)) { - metaData->setRawMax(varMax); - } else { - qCWarning(PX4ParameterMetaDataLog) << "Invalid max value, name:" << metaData->name() << " type:" << metaData->type() << " max:" << text << " error:" << errorString; - } - - } else if (elementName == "unit") { - Q_ASSERT(metaData); - QString text = xml.readElementText(); - qCDebug(PX4ParameterMetaDataLog) << "Unit:" << text; - metaData->setRawUnits(text); - - } else if (elementName == "decimal") { - Q_ASSERT(metaData); - QString text = xml.readElementText(); - qCDebug(PX4ParameterMetaDataLog) << "Decimal:" << text; - - bool convertOk; - QVariant varDecimals = QVariant(text).toUInt(&convertOk); - if (convertOk) { - metaData->setDecimalPlaces(varDecimals.toInt()); - } else { - qCWarning(PX4ParameterMetaDataLog) << "Invalid decimals value, name:" << metaData->name() << " type:" << metaData->type() << " decimals:" << text << " error: invalid number"; - } + if (metaData) { + if (elementName == "short_desc") { + QString text = xml.readElementText(); + text = text.replace("\n", " "); + qCDebug(PX4ParameterMetaDataLog) << "Short description:" << text; + metaData->setShortDescription(text); + + } else if (elementName == "long_desc") { + QString text = xml.readElementText(); + text = text.replace("\n", " "); + qCDebug(PX4ParameterMetaDataLog) << "Long description:" << text; + metaData->setLongDescription(text); + + } else if (elementName == "min") { + QString text = xml.readElementText(); + qCDebug(PX4ParameterMetaDataLog) << "Min:" << text; + + QVariant varMin; + if (metaData->convertAndValidateRaw(text, true /* convertOnly */, varMin, errorString)) { + metaData->setRawMin(varMin); + } else { + qCWarning(PX4ParameterMetaDataLog) << "Invalid min value, name:" << metaData->name() << " type:" << metaData->type() << " min:" << text << " error:" << errorString; + } - } else if (elementName == "reboot_required") { - Q_ASSERT(metaData); - QString text = xml.readElementText(); - qCDebug(PX4ParameterMetaDataLog) << "RebootRequired:" << text; - if (text.compare("true", Qt::CaseInsensitive) == 0) { - metaData->setRebootRequired(true); - } + } else if (elementName == "max") { + QString text = xml.readElementText(); + qCDebug(PX4ParameterMetaDataLog) << "Max:" << text; - } else if (elementName == "values") { - // doing nothing individual value will follow anyway. May be used for sanity checking. + QVariant varMax; + if (metaData->convertAndValidateRaw(text, true /* convertOnly */, varMax, errorString)) { + metaData->setRawMax(varMax); + } else { + qCWarning(PX4ParameterMetaDataLog) << "Invalid max value, name:" << metaData->name() << " type:" << metaData->type() << " max:" << text << " error:" << errorString; + } - } else if (elementName == "value") { - QString enumValueStr = xml.attributes().value("code").toString(); - QString enumString = xml.readElementText(); - qCDebug(PX4ParameterMetaDataLog) << "parameter value:" - << "value desc:" << enumString << "code:" << enumValueStr; + } else if (elementName == "unit") { + QString text = xml.readElementText(); + qCDebug(PX4ParameterMetaDataLog) << "Unit:" << text; + metaData->setRawUnits(text); - QVariant enumValue; - QString errorString; - if (metaData->convertAndValidateRaw(enumValueStr, false /* validate */, enumValue, errorString)) { - metaData->addEnumInfo(enumString, enumValue); - } else { - qCDebug(PX4ParameterMetaDataLog) << "Invalid enum value, name:" << metaData->name() - << " type:" << metaData->type() << " value:" << enumValueStr - << " error:" << errorString; - } - } else if (elementName == "increment") { - Q_ASSERT(metaData); - double increment; - bool ok; - QString text = xml.readElementText(); - increment = text.toDouble(&ok); - if (ok) { - metaData->setIncrement(increment); - } else { - qCWarning(PX4ParameterMetaDataLog) << "Invalid value for increment, name:" << metaData->name() << " increment:" << text; - } + } else if (elementName == "decimal") { + QString text = xml.readElementText(); + qCDebug(PX4ParameterMetaDataLog) << "Decimal:" << text; + + bool convertOk; + QVariant varDecimals = QVariant(text).toUInt(&convertOk); + if (convertOk) { + metaData->setDecimalPlaces(varDecimals.toInt()); + } else { + qCWarning(PX4ParameterMetaDataLog) << "Invalid decimals value, name:" << metaData->name() << " type:" << metaData->type() << " decimals:" << text << " error: invalid number"; + } - } else if (elementName == "boolean") { - QVariant enumValue; - metaData->convertAndValidateRaw(1, false /* validate */, enumValue, errorString); - metaData->addEnumInfo(tr("Enabled"), enumValue); - metaData->convertAndValidateRaw(0, false /* validate */, enumValue, errorString); - metaData->addEnumInfo(tr("Disabled"), enumValue); - - } else if (elementName == "bitmask") { - // doing nothing individual bits will follow anyway. May be used for sanity checking. - - } else if (elementName == "bit") { - bool ok = false; - unsigned char bit = xml.attributes().value("index").toString().toUInt(&ok); - if (ok) { - QString bitDescription = xml.readElementText(); + } else if (elementName == "reboot_required") { + QString text = xml.readElementText(); + qCDebug(PX4ParameterMetaDataLog) << "RebootRequired:" << text; + if (text.compare("true", Qt::CaseInsensitive) == 0) { + metaData->setRebootRequired(true); + } + + } else if (elementName == "values") { + // doing nothing individual value will follow anyway. May be used for sanity checking. + + } else if (elementName == "value") { + QString enumValueStr = xml.attributes().value("code").toString(); + QString enumString = xml.readElementText(); qCDebug(PX4ParameterMetaDataLog) << "parameter value:" - << "index:" << bit << "description:" << bitDescription; - - if (bit < 31) { - QVariant bitmaskRawValue = 1 << bit; - QVariant bitmaskValue; - QString errorString; - if (metaData->convertAndValidateRaw(bitmaskRawValue, true, bitmaskValue, errorString)) { - metaData->addBitmaskInfo(bitDescription, bitmaskValue); + << "value desc:" << enumString << "code:" << enumValueStr; + + QVariant enumValue; + QString errorString; + if (metaData->convertAndValidateRaw(enumValueStr, false /* validate */, enumValue, errorString)) { + metaData->addEnumInfo(enumString, enumValue); + } else { + qCDebug(PX4ParameterMetaDataLog) << "Invalid enum value, name:" << metaData->name() + << " type:" << metaData->type() << " value:" << enumValueStr + << " error:" << errorString; + } + } else if (elementName == "increment") { + double increment; + bool ok; + QString text = xml.readElementText(); + increment = text.toDouble(&ok); + if (ok) { + metaData->setIncrement(increment); + } else { + qCWarning(PX4ParameterMetaDataLog) << "Invalid value for increment, name:" << metaData->name() << " increment:" << text; + } + + } else if (elementName == "boolean") { + QVariant enumValue; + metaData->convertAndValidateRaw(1, false /* validate */, enumValue, errorString); + metaData->addEnumInfo(tr("Enabled"), enumValue); + metaData->convertAndValidateRaw(0, false /* validate */, enumValue, errorString); + metaData->addEnumInfo(tr("Disabled"), enumValue); + + } else if (elementName == "bitmask") { + // doing nothing individual bits will follow anyway. May be used for sanity checking. + + } else if (elementName == "bit") { + bool ok = false; + unsigned char bit = xml.attributes().value("index").toString().toUInt(&ok); + if (ok) { + QString bitDescription = xml.readElementText(); + qCDebug(PX4ParameterMetaDataLog) << "parameter value:" + << "index:" << bit << "description:" << bitDescription; + + if (bit < 31) { + QVariant bitmaskRawValue = 1 << bit; + QVariant bitmaskValue; + QString errorString; + if (metaData->convertAndValidateRaw(bitmaskRawValue, true, bitmaskValue, errorString)) { + metaData->addBitmaskInfo(bitDescription, bitmaskValue); + } else { + qCDebug(PX4ParameterMetaDataLog) << "Invalid bitmask value, name:" << metaData->name() + << " type:" << metaData->type() << " value:" << bitmaskValue + << " error:" << errorString; + } } else { - qCDebug(PX4ParameterMetaDataLog) << "Invalid bitmask value, name:" << metaData->name() - << " type:" << metaData->type() << " value:" << bitmaskValue - << " error:" << errorString; + qCWarning(PX4ParameterMetaDataLog) << "Invalid value for bitmask, bit:" << bit; } - } else { - qCWarning(PX4ParameterMetaDataLog) << "Invalid value for bitmask, bit:" << bit; } + } else { + qCDebug(PX4ParameterMetaDataLog) << "Unknown element in XML: " << elementName; } } else { - qCDebug(PX4ParameterMetaDataLog) << "Unknown element in XML: " << elementName; + qWarning() << "Internal error"; } } } @@ -358,12 +355,12 @@ void PX4ParameterMetaData::loadParameterFactMetaDataFile(const QString& metaData // Done loading this parameter, validate default value if (metaData->defaultValueAvailable()) { QVariant var; - + if (!metaData->convertAndValidateRaw(metaData->rawDefaultValue(), false /* convertOnly */, var, errorString)) { qCWarning(PX4ParameterMetaDataLog) << "Invalid default value, name:" << metaData->name() << " type:" << metaData->type() << " default:" << metaData->rawDefaultValue() << " error:" << errorString; } } - + // Reset for next parameter metaData = NULL; badMetaData = false; diff --git a/src/FlightDisplay/FlightDisplayView.qml b/src/FlightDisplay/FlightDisplayView.qml index b8c96f83f7d19573282d940f05ac10c731c8db54..b0cbf5fdc3dd04255b0ba4ea5fffa6befc3d2bae 100644 --- a/src/FlightDisplay/FlightDisplayView.qml +++ b/src/FlightDisplay/FlightDisplayView.qml @@ -167,11 +167,8 @@ QGCView { message: qsTr("Do you want to remove the mission from the vehicle?") function accept() { - _missionController.removeAllFromVehicle() - _geoFenceController.removeAllFromVehicle() - _rallyPointController.removeAllFromVehicle() + _planMasterController.removeAllFromVehicle() hideDialog() - } } } @@ -420,6 +417,12 @@ QGCView { action: _guidedController.actionStartMission, visible: _guidedController.showStartMission }, + { + title: _guidedController.continueMissionTitle, + text: _guidedController.continueMissionMessage, + action: _guidedController.actionContinueMission, + visible: _guidedController.showContinueMission + }, { title: _guidedController.resumeMissionTitle, text: _guidedController.resumeMissionMessage, @@ -485,11 +488,7 @@ QGCView { ] onClicked: { - //-- Dismiss any other dialog - rootLoader.sourceComponent = null - guidedActionConfirm.visible = false - guidedActionList.visible = false - altitudeSlider.visible = false + guidedActionsController.closeAll() var action = model[index].action if (action === -1) { if (index == 4) { @@ -509,6 +508,7 @@ QGCView { id: guidedActionsController missionController: _missionController confirmDialog: guidedActionConfirm + altitudeSlider: _altitudeSlider z: _flightVideoPipControl.z + 1 onShowStartMissionChanged: { @@ -528,6 +528,20 @@ QGCView { confirmAction(actionResumeMission) } } + + onShowLandAbortChanged: { + if (showLandAbort) { + confirmAction(actionLandAbort) + } + } + + /// Close all dialogs + function closeAll() { + rootLoader.sourceComponent = null + guidedActionConfirm.visible = false + guidedActionList.visible = false + altitudeSlider.visible = false + } } GuidedActionConfirm { @@ -545,7 +559,6 @@ QGCView { anchors.bottom: parent.bottom anchors.horizontalCenter: parent.horizontalCenter guidedController: _guidedController - altitudeSlider: _altitudeSlider } //-- Altitude slider diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index 943f6c3c5ead7a225202f5e3764ea47c1905a5fd..3e38ee151f56202779f93afea5c40ea0819ec0dd 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -60,8 +60,12 @@ FlightMap { // When the user pans the map we stop responding to vehicle coordinate updates until the panRecenterTimer fires onUserPannedChanged: { - _disableVehicleTracking = true - panRecenterTimer.start() + if (userPanned) { + console.log("user panned") + userPanned = false + _disableVehicleTracking = true + panRecenterTimer.restart() + } } function pointInRect(point, rect) { @@ -241,9 +245,9 @@ FlightMap { } } - // Camera points + // Camera trigger points MapItemView { - model: _missionController.cameraPoints + model: _activeVehicle ? _activeVehicle.cameraTriggerPoints : 0 delegate: CameraTriggerIndicator { coordinate: object.coordinate diff --git a/src/FlightDisplay/GuidedActionList.qml b/src/FlightDisplay/GuidedActionList.qml index 4e7d11727c39efcbf847f07a6cfd6727051a8af4..6ff749c8faf0dcfc085a33a865204ac7c9d35ffb 100644 --- a/src/FlightDisplay/GuidedActionList.qml +++ b/src/FlightDisplay/GuidedActionList.qml @@ -86,10 +86,6 @@ Rectangle { text: modelData.title onClicked: { - if (modelData.action === guidedController.actionChangeAlt) { - altitudeSlider.reset() - altitudeSlider.visible = true - } _root.visible = false guidedController.confirmAction(modelData.action) } diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml index e19ff89306c1420495eb5a0c27ad25184b3259fb..77b560e60d8a947f3503f170dd0ae98d002b473c 100644 --- a/src/FlightDisplay/GuidedActionsController.qml +++ b/src/FlightDisplay/GuidedActionsController.qml @@ -29,6 +29,7 @@ Item { property var missionController property var confirmDialog + property var altitudeSlider readonly property string emergencyStopTitle: qsTr("Emergency Stop") readonly property string armTitle: qsTr("Arm") @@ -48,12 +49,12 @@ Item { readonly property string armMessage: qsTr("Arm the vehicle.") readonly property string disarmMessage: qsTr("Disarm the vehicle") - readonly property string emergencyStopMessage: qsTr("WARNING: This still 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 air it will crash.") readonly property string takeoffMessage: qsTr("Takeoff from ground and hold position.") - readonly property string startMissionMessage: qsTr("Start the mission which is currently displayed above. If the vehicle is on the ground it will takeoff.") + 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.") - property string resumeMissionMessage: qsTr("Resume the mission which is displayed above. This will re-generate the mission from waypoint %1, takeoff and continue the mission.").arg(_resumeMissionIndex) - readonly property string resumeMissionReadyMessage: qsTr("Review the modified mission above. Confirm if you want to takeoff and begin mission.") + property string resumeMissionMessage: qsTr("Resume the current mission. This will re-generate the mission from waypoint %1, takeoff and continue the mission.").arg(_resumeMissionIndex) + readonly property string resumeMissionReadyMessage: qsTr("Review the modified mission. Confirm if you want to takeoff and begin mission.") readonly property string landMessage: qsTr("Land the vehicle at the current position.") readonly property string rtlMessage: qsTr("Return to the home position of the vehicle.") readonly property string changeAltMessage: qsTr("Change the altitude of the vehicle up or down.") @@ -87,12 +88,12 @@ Item { property bool showTakeoff: _activeVehicle && _activeVehicle.guidedModeSupported && !_vehicleFlying && !_activeVehicle.fixedWing property bool showLand: _activeVehicle && _activeVehicle.guidedModeSupported && _vehicleArmed && !_activeVehicle.fixedWing && !_vehicleInLandMode property bool showStartMission: _activeVehicle && _missionAvailable && !_missionActive && !_vehicleFlying - property bool showContinueMission: _activeVehicle && _missionAvailable && !_missionActive && _vehicleFlying - property bool showResumeMission: _activeVehicle && !_vehicleFlying && _missionAvailable && _resumeMissionIndex > 0 + property bool showContinueMission: _activeVehicle && _missionAvailable && !_missionActive && _vehicleFlying && (_currentMissionIndex < missionController.visualItems.count - 1) + property bool showResumeMission: _activeVehicle && !_vehicleFlying && _missionAvailable && _resumeMissionIndex > 0 && (_resumeMissionIndex < missionController.visualItems.count - 2) 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 + property bool showLandAbort: _activeVehicle && _vehicleFlying && _activeVehicle.fixedWing && _vehicleLanding property bool showGotoLocation: _activeVehicle && _activeVehicle.guidedMode && _vehicleFlying property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle @@ -101,15 +102,19 @@ Item { property bool _missionActive: _activeVehicle ? _vehicleArmed && (_vehicleInLandMode || _vehicleInRTLMode || _vehicleInMissionMode) : false property bool _vehicleArmed: _activeVehicle ? _activeVehicle.armed : false property bool _vehicleFlying: _activeVehicle ? _activeVehicle.flying : false + property bool _vehicleLanding: _activeVehicle ? _activeVehicle.landing : false property bool _vehiclePaused: false property bool _vehicleInMissionMode: false property bool _vehicleInRTLMode: false property bool _vehicleInLandMode: false + property int _currentMissionIndex: missionController.currentMissionIndex property int _resumeMissionIndex: missionController.resumeMissionIndex property bool _hideEmergenyStop: !QGroundControl.corePlugin.options.guidedBarShowEmergencyStop property bool _hideOrbit: !QGroundControl.corePlugin.options.guidedBarShowOrbit property var _actionData + on_CurrentMissionIndexChanged: console.log("_currentMissionIndex", _currentMissionIndex) + on_FlightModeChanged: { _vehiclePaused = _flightMode === _activeVehicle.pauseFlightMode _vehicleInRTLMode = _flightMode === _activeVehicle.rtlFlightMode @@ -119,6 +124,7 @@ Item { // Called when an action is about to be executed in order to confirm function confirmAction(actionCode, actionData) { + closeAll() confirmDialog.action = actionCode confirmDialog.actionData = actionData _actionData = actionData @@ -183,6 +189,8 @@ Item { confirmDialog.title = changeAltTitle confirmDialog.message = changeAltMessage confirmDialog.hideTrigger = Qt.binding(function() { return !showChangeAlt }) + altitudeSlider.reset() + altitudeSlider.visible = true break; case actionGoto: confirmDialog.title = gotoTitle diff --git a/src/FlightDisplay/VideoManager.cc b/src/FlightDisplay/VideoManager.cc index b6c5a83ea6c1de4ccd9a9636158c5c61b6b1a1eb..7188f1a34d2dccbf69d9df2e8382afb1acd17040 100644 --- a/src/FlightDisplay/VideoManager.cc +++ b/src/FlightDisplay/VideoManager.cc @@ -57,6 +57,9 @@ VideoManager::setToolbox(QGCToolbox *toolbox) _videoSettings = toolbox->settingsManager()->videoSettings(); QString videoSource = _videoSettings->videoSource()->rawValue().toString(); connect(_videoSettings->videoSource(), &Fact::rawValueChanged, this, &VideoManager::_videoSourceChanged); + connect(_videoSettings->udpPort(), &Fact::rawValueChanged, this, &VideoManager::_udpPortChanged); + connect(_videoSettings->rtspUrl(), &Fact::rawValueChanged, this, &VideoManager::_rtspUrlChanged); + #if defined(QGC_GST_STREAMING) #ifndef QGC_DISABLE_UVC @@ -96,6 +99,17 @@ void VideoManager::_videoSourceChanged(void) { emit hasVideoChanged(); emit isGStreamerChanged(); + _restartVideo(); +} + +void VideoManager::_udpPortChanged(void) +{ + _restartVideo(); +} + +void VideoManager::_rtspUrlChanged(void) +{ + _restartVideo(); } //----------------------------------------------------------------------------- @@ -170,6 +184,18 @@ void VideoManager::_updateTimer() #endif } +//----------------------------------------------------------------------------- +void VideoManager::_updateSettings() +{ + if(!_videoSettings || !_videoReceiver) + return; + + if (_videoSettings->videoSource()->rawValue().toString() == VideoSettings::videoSourceUDP) + _videoReceiver->setUri(QStringLiteral("udp://0.0.0.0:%1").arg(_videoSettings->udpPort()->rawValue().toInt())); + else + _videoReceiver->setUri(_videoSettings->rtspUrl()->rawValue().toString()); +} + //----------------------------------------------------------------------------- void VideoManager::_updateVideo() { @@ -182,12 +208,20 @@ void VideoManager::_updateVideo() _videoReceiver = new VideoReceiver(this); #if defined(QGC_GST_STREAMING) _videoReceiver->setVideoSink(_videoSurface->videoSink()); - QString videoSource = _videoSettings->videoSource()->rawValue().toString(); - if (_videoSettings->videoSource()->rawValue().toString() == VideoSettings::videoSourceUDP) - _videoReceiver->setUri(QStringLiteral("udp://0.0.0.0:%1").arg(_videoSettings->udpPort()->rawValue().toInt())); - else - _videoReceiver->setUri(_videoSettings->rtspUrl()->rawValue().toString()); + _updateSettings(); #endif _videoReceiver->start(); } } + +//----------------------------------------------------------------------------- +void VideoManager::_restartVideo() +{ + if(!_videoReceiver) + return; +#if defined(QGC_GST_STREAMING) + _videoReceiver->stop(); + _updateSettings(); + _videoReceiver->start(); +#endif +} diff --git a/src/FlightDisplay/VideoManager.h b/src/FlightDisplay/VideoManager.h index 162da00b0607519e7b1de2c29b4654ec0dac796a..0bade88feb9be2daaba6ffd379945c27508fa86c 100644 --- a/src/FlightDisplay/VideoManager.h +++ b/src/FlightDisplay/VideoManager.h @@ -69,10 +69,16 @@ signals: private slots: void _videoSourceChanged(void); + void _udpPortChanged(void); + void _rtspUrlChanged(void); + private: void _updateTimer (); + void _updateSettings (); void _updateVideo (); + void _restartVideo (); + VideoSurface* _videoSurface; VideoReceiver* _videoReceiver; diff --git a/src/FlightMap/Widgets/CenterMapDropButton.qml b/src/FlightMap/Widgets/CenterMapDropButton.qml index 8320fed2a95e5346eb94a40b0402366984d15644..e4f410ade5b19721bcb221bc1243a4e1e208a0de 100644 --- a/src/FlightMap/Widgets/CenterMapDropButton.qml +++ b/src/FlightMap/Widgets/CenterMapDropButton.qml @@ -200,11 +200,11 @@ DropButton { QGCButton { text: qsTr("Current Location") Layout.fillWidth: true - enabled: mainWindow.gcsPosition && mainWindow.gcsPosition.isValid && !followVehicleCheckBox.checked + enabled: map.gcsPosition ? map.gcsPosition.isValid && !followVehicleCheckBox.checked : false onClicked: { dropButton.hideDropDown() - map.center = mainWindow.gcsPosition + map.center = map.gcsPosition } } diff --git a/src/FlightMap/Widgets/InstrumentSwipeView.qml b/src/FlightMap/Widgets/InstrumentSwipeView.qml index a1eaf5f408f3a259ee5e7e47d4036292f2142661..814b0b45a70de1a83fd6317999bdb8fcb7255665 100644 --- a/src/FlightMap/Widgets/InstrumentSwipeView.qml +++ b/src/FlightMap/Widgets/InstrumentSwipeView.qml @@ -29,6 +29,7 @@ Item { function showPage(pageIndex) { pageRow.x = -(pageIndex * _pageWidth) + _currentPage = pageIndex } function showNextPage() { diff --git a/src/FlightMap/Widgets/ValuesWidgetController.cc b/src/FlightMap/Widgets/ValuesWidgetController.cc index 17d0cab29d4ada62f5ebc0f43f312044f41cc042..15a4927b4da818ad567ba38db43ce004b62b1a18 100644 --- a/src/FlightMap/Widgets/ValuesWidgetController.cc +++ b/src/FlightMap/Widgets/ValuesWidgetController.cc @@ -9,6 +9,8 @@ #include "ValuesWidgetController.h" +#include "QGCApplication.h" +#include "QGCCorePlugin.h" #include @@ -19,13 +21,14 @@ const char* ValuesWidgetController::_smallValuesKey = "small"; ValuesWidgetController::ValuesWidgetController(void) { QSettings settings; - QStringList largeDefaults; settings.beginGroup(_groupKey); - largeDefaults << "Vehicle.altitudeRelative" << "Vehicle.groundSpeed"; + QStringList largeDefaults, smallDefaults; + qgcApp()->toolbox()->corePlugin()->valuesWidgetDefaultSettings(largeDefaults, smallDefaults); + _largeValues = settings.value(_largeValuesKey, largeDefaults).toStringList(); - _smallValues = settings.value(_smallValuesKey, QStringList()).toStringList(); + _smallValues = settings.value(_smallValuesKey, smallDefaults).toStringList(); _altitudeProperties << "altitudeRelative" << "altitudeAMSL"; diff --git a/src/FlightMap/Widgets/VehicleHealthWidget.qml b/src/FlightMap/Widgets/VehicleHealthWidget.qml index 3e7a35c929ad72dbc19c64708d5dd0ea2eb6c27d..85eb212490a1fa1892ee298d1ac1870756f7d550 100644 --- a/src/FlightMap/Widgets/VehicleHealthWidget.qml +++ b/src/FlightMap/Widgets/VehicleHealthWidget.qml @@ -30,7 +30,7 @@ QGCFlickable { // Any time the unhealthy sensors list changes, switch to the health page onUnhealthySensorsChanged: { if (unhealthySensors.length != 0) { - showPage(1) + showPage(2) } } diff --git a/src/GPS/GPSManager.cc b/src/GPS/GPSManager.cc index 0c7c8406bdbfa5b76e03a15dd54637a99bd83524..0d1a28fb2e3ba00d53d65a6661af29c35bf31b5f 100644 --- a/src/GPS/GPSManager.cc +++ b/src/GPS/GPSManager.cc @@ -28,8 +28,6 @@ GPSManager::~GPSManager() void GPSManager::connectGPS(const QString& device) { - Q_ASSERT(_toolbox); - RTKSettings* rtkSettings = qgcApp()->toolbox()->settingsManager()->rtkSettings(); cleanup(); diff --git a/src/GPS/RTCM/RTCMMavlink.cc b/src/GPS/RTCM/RTCMMavlink.cc index b3ebf0744891de7543b99d031e0cef58d287a590..d8f6c92df4ad2dc5d15e96edba5fac513f753151 100644 --- a/src/GPS/RTCM/RTCMMavlink.cc +++ b/src/GPS/RTCM/RTCMMavlink.cc @@ -44,12 +44,12 @@ void RTCMMavlink::RTCMDataUpdate(QByteArray message) // We need to fragment static uint8_t sequenceId = 0; // Sequence id is used to indicate that the individual fragements belong to the same set - uint8_t fragmentId = 0; // Fragment id indicates the fragement within a set + uint8_t fragmentId = 0; // Fragment id indicates the fragment within a set int start = 0; while (start < message.size()) { int length = std::min(message.size() - start, maxMessageLength); - mavlinkRtcmData.flags = 1; // LSB set indicates messsage is fragmented + mavlinkRtcmData.flags = 1; // LSB set indicates message is fragmented mavlinkRtcmData.flags |= fragmentId++ << 1; // Next 2 bits are fragment id mavlinkRtcmData.flags |= sequenceId++ << 3; // Next 5 bits are sequence id mavlinkRtcmData.len = length; diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index 488429843392811cb3fa96acb593b2ae8a7db22d..2e98c3f8f746acdec38ad15d333087c4ad99c2fb 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -97,7 +97,7 @@ public: QString name(void) { return _name; } /* // Joystick index used by sdl library - // Settable because sdl library remaps indicies after certain events + // Settable because sdl library remaps indices after certain events virtual int index(void) = 0; virtual void setIndex(int index) = 0; */ diff --git a/src/JsonHelper.cc b/src/JsonHelper.cc index e6ee750e0f44769d45351262b46c9207a8457830..8d44bcb09c68b0e3791de3abb073976314b964cc 100644 --- a/src/JsonHelper.cc +++ b/src/JsonHelper.cc @@ -102,6 +102,10 @@ 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) { + // Null type signals a NaN on a double value + continue; + } if (jsonValue.type() != types[i]) { errorString = QObject::tr("Incorrect value type - key:type:expected %1:%2:%3").arg(valueKey).arg(_jsonValueTypeToString(jsonValue.type())).arg(_jsonValueTypeToString(types[i])); return false; @@ -342,3 +346,12 @@ void JsonHelper::savePolygon(QmlObjectListModel& list, QJsonArray& polygonArray) polygonArray.append(jsonValue); } } + +double JsonHelper::possibleNaNJsonValue(const QJsonValue& value) +{ + if (value.type() == QJsonValue::Null) { + return std::numeric_limits::quiet_NaN(); + } else { + return value.toDouble(); + } +} diff --git a/src/JsonHelper.h b/src/JsonHelper.h index dd6f7aa329e454fd767603471345d350780593eb..8f88dceab43ae08fd85e888edd44fc049c8826c1 100644 --- a/src/JsonHelper.h +++ b/src/JsonHelper.h @@ -34,20 +34,30 @@ public: /// jsonFileTypeKey - Required and checked to be equal to expectedFileType /// jsonVersionKey - Required and checked to be below supportedMajorVersion, supportedMinorVersion /// jsonGroundStationKey - Required and checked to be string type - /// @return false: validation failed - static bool validateQGCJsonFile(const QJsonObject& jsonObject, ///< root json object + /// @return false: validation failed, errorString set + static bool validateQGCJsonFile(const QJsonObject& jsonObject, ///< json object to validate const QString& expectedFileType, ///< correct file type for file int minSupportedVersion, ///< minimum supported version int maxSupportedVersion, ///< maximum supported major version int &version, ///< returned file version QString& errorString); ///< returned error string if validation fails - static bool validateRequiredKeys(const QJsonObject& jsonObject, const QStringList& keys, QString& errorString); - static bool validateKeyTypes(const QJsonObject& jsonObject, const QStringList& keys, const QList& types, QString& errorString); + /// Validates that the specified keys are in the object + /// @return false: validation failed, errorString set + static bool validateRequiredKeys(const QJsonObject& jsonObject, ///< json object to validate + const QStringList& keys, ///< keys which are required to be present + QString& errorString); ///< returned error string if validation fails + + /// Validates the types of specified keys are in the object + /// @return false: validation failed, errorString set + static bool validateKeyTypes(const QJsonObject& jsonObject, ///< json object to validate + const QStringList& keys, ///< keys to validate + const QList& types, ///< required type for each key, QJsonValue::Null specifies double with possible NaN + QString& errorString); ///< returned error string if validation fails typedef struct { const char* key; ///< json key name - QJsonValue::Type type; ///< type of key + QJsonValue::Type type; ///< required type for key, QJsonValue::Null specifies double with possible NaN bool required; ///< true: key must be present } KeyValidateInfo; @@ -96,6 +106,8 @@ public: static bool parseEnum(const QJsonObject& jsonObject, QStringList& enumStrings, QStringList& enumValues, QString& errorString); + /// Returns NaN if the value is null, or it not the double value + static double possibleNaNJsonValue(const QJsonValue& value); static const char* jsonVersionKey; static const char* jsonGroundStationKey; diff --git a/src/MissionManager/CameraSection.FactMetaData.json b/src/MissionManager/CameraSection.FactMetaData.json index c259465e981b4de892a137322dc8e6f584e6782d..cd95721f51c4849fd2b7edcf9ac3a5fc75692396 100644 --- a/src/MissionManager/CameraSection.FactMetaData.json +++ b/src/MissionManager/CameraSection.FactMetaData.json @@ -29,7 +29,7 @@ "name": "GimbalPitch", "shortDescription": "Gimbal pitch rotation.", "type": "double", - "units": "deg", + "units": "gimbal-degrees", "min": -90, "max": 0, "decimalPlaces": 0, @@ -40,9 +40,17 @@ "shortDescription": "Gimbal yaw rotation.", "type": "double", "units": "deg", - "min": 0.0, - "max": 360.0, + "min": -180.0, + "max": 180.0, "decimalPlaces": 0, "defaultValue": 0 +}, +{ + "name": "CameraMode", + "shortDescription": "Specify whether the camera should switch to photo or video mode", + "type": "uint32", + "enumStrings": "Take photos,Record video", + "enumValues": "0,1", + "defaultValue": 0 } ] diff --git a/src/MissionManager/CameraSection.cc b/src/MissionManager/CameraSection.cc index fc35be418e9cc1ed216dac2f31a545ce890b5e1f..131c2c18104f74963fbfa01fc1375cfd166f341d 100644 --- a/src/MissionManager/CameraSection.cc +++ b/src/MissionManager/CameraSection.cc @@ -9,14 +9,16 @@ #include "CameraSection.h" #include "SimpleMissionItem.h" +#include "FirmwarePlugin.h" QGC_LOGGING_CATEGORY(CameraSectionLog, "CameraSectionLog") -const char* CameraSection::_gimbalPitchName = "GimbalPitch"; -const char* CameraSection::_gimbalYawName = "GimbalYaw"; -const char* CameraSection::_cameraActionName = "CameraAction"; -const char* CameraSection::_cameraPhotoIntervalDistanceName = "CameraPhotoIntervalDistance"; -const char* CameraSection::_cameraPhotoIntervalTimeName = "CameraPhotoIntervalTime"; +const char* CameraSection::_gimbalPitchName = "GimbalPitch"; +const char* CameraSection::_gimbalYawName = "GimbalYaw"; +const char* CameraSection::_cameraActionName = "CameraAction"; +const char* CameraSection::_cameraPhotoIntervalDistanceName = "CameraPhotoIntervalDistance"; +const char* CameraSection::_cameraPhotoIntervalTimeName = "CameraPhotoIntervalTime"; +const char* CameraSection::_cameraModeName = "CameraMode"; QMap CameraSection::_metaDataMap; @@ -25,11 +27,13 @@ CameraSection::CameraSection(Vehicle* vehicle, QObject* parent) , _available(false) , _settingsSpecified(false) , _specifyGimbal(false) + , _specifyCameraMode(false) , _gimbalYawFact (0, _gimbalYawName, FactMetaData::valueTypeDouble) , _gimbalPitchFact (0, _gimbalPitchName, FactMetaData::valueTypeDouble) , _cameraActionFact (0, _cameraActionName, FactMetaData::valueTypeDouble) , _cameraPhotoIntervalDistanceFact (0, _cameraPhotoIntervalDistanceName, FactMetaData::valueTypeDouble) , _cameraPhotoIntervalTimeFact (0, _cameraPhotoIntervalTimeName, FactMetaData::valueTypeUint32) + , _cameraModeFact (0, _cameraModeName, FactMetaData::valueTypeUint32) , _dirty(false) { if (_metaDataMap.isEmpty()) { @@ -41,23 +45,29 @@ CameraSection::CameraSection(Vehicle* vehicle, QObject* parent) _cameraActionFact.setMetaData (_metaDataMap[_cameraActionName]); _cameraPhotoIntervalDistanceFact.setMetaData (_metaDataMap[_cameraPhotoIntervalDistanceName]); _cameraPhotoIntervalTimeFact.setMetaData (_metaDataMap[_cameraPhotoIntervalTimeName]); + _cameraModeFact.setMetaData (_metaDataMap[_cameraModeName]); _gimbalPitchFact.setRawValue (_gimbalPitchFact.rawDefaultValue()); _gimbalYawFact.setRawValue (_gimbalYawFact.rawDefaultValue()); _cameraActionFact.setRawValue (_cameraActionFact.rawDefaultValue()); _cameraPhotoIntervalDistanceFact.setRawValue (_cameraPhotoIntervalDistanceFact.rawDefaultValue()); _cameraPhotoIntervalTimeFact.setRawValue (_cameraPhotoIntervalTimeFact.rawDefaultValue()); + _cameraModeFact.setRawValue (_cameraModeFact.rawDefaultValue()); - connect(this, &CameraSection::specifyGimbalChanged, this, &CameraSection::_specifyGimbalChanged); - connect(&_cameraActionFact, &Fact::valueChanged, this, &CameraSection::_cameraActionChanged); + connect(this, &CameraSection::specifyGimbalChanged, this, &CameraSection::_specifyChanged); + connect(this, &CameraSection::specifyCameraModeChanged, this, &CameraSection::_specifyChanged); - connect(&_gimbalPitchFact, &Fact::valueChanged, this, &CameraSection::_setDirty); - connect(&_gimbalYawFact, &Fact::valueChanged, this, &CameraSection::_setDirty); - connect(&_cameraPhotoIntervalDistanceFact, &Fact::valueChanged, this, &CameraSection::_setDirty); - connect(&_cameraPhotoIntervalTimeFact, &Fact::valueChanged, this, &CameraSection::_setDirty); - connect(this, &CameraSection::specifyGimbalChanged, this, &CameraSection::_setDirty); + connect(&_cameraActionFact, &Fact::valueChanged, this, &CameraSection::_cameraActionChanged); - connect(&_gimbalYawFact, &Fact::valueChanged, this, &CameraSection::_updateSpecifiedGimbalYaw); + connect(&_gimbalPitchFact, &Fact::valueChanged, this, &CameraSection::_setDirty); + connect(&_gimbalYawFact, &Fact::valueChanged, this, &CameraSection::_setDirty); + connect(&_cameraPhotoIntervalDistanceFact, &Fact::valueChanged, this, &CameraSection::_setDirty); + connect(&_cameraPhotoIntervalTimeFact, &Fact::valueChanged, this, &CameraSection::_setDirty); + connect(&_cameraModeFact, &Fact::valueChanged, this, &CameraSection::_setDirty); + connect(this, &CameraSection::specifyGimbalChanged, this, &CameraSection::_setDirty); + connect(this, &CameraSection::specifyCameraModeChanged, this, &CameraSection::_setDirty); + + connect(&_gimbalYawFact, &Fact::valueChanged, this, &CameraSection::_updateSpecifiedGimbalYaw); } void CameraSection::setSpecifyGimbal(bool specifyGimbal) @@ -68,6 +78,14 @@ void CameraSection::setSpecifyGimbal(bool specifyGimbal) } } +void CameraSection::setSpecifyCameraMode(bool specifyCameraMode) +{ + if (specifyCameraMode != _specifyCameraMode) { + _specifyCameraMode = specifyCameraMode; + emit specifyCameraModeChanged(specifyCameraMode); + } +} + int CameraSection::itemCount(void) const { int itemCount = 0; @@ -75,6 +93,9 @@ int CameraSection::itemCount(void) const if (_specifyGimbal) { itemCount++; } + if (_specifyCameraMode) { + itemCount++; + } if (_cameraActionFact.rawValue().toInt() != CameraActionNone) { itemCount++; } @@ -121,7 +142,7 @@ void CameraSection::appendSectionItems(QList& items, QObject* miss 0, // Unlimited photo count -1, // Max resolution 0, 0, // param 4-5 not used - 0, // Camera ID + 0, // Camera ID, all cameras 0, // param 7 not used true, // autoContinue false, // isCurrentItem @@ -143,7 +164,7 @@ void CameraSection::appendSectionItems(QList& items, QObject* miss item = new MissionItem(nextSequenceNumber++, MAV_CMD_VIDEO_START_CAPTURE, MAV_FRAME_MISSION, - 0, // Camera ID + 0, // Camera ID, all cameras -1, // Max fps -1, // Max resolution 0, 0, 0, 0, // param 5-7 not used @@ -156,7 +177,7 @@ void CameraSection::appendSectionItems(QList& items, QObject* miss item = new MissionItem(nextSequenceNumber++, MAV_CMD_VIDEO_STOP_CAPTURE, MAV_FRAME_MISSION, - 0, // Camera ID + 0, // Camera ID, all cameras 0, 0, 0, 0, 0, 0, // param 2-7 not used true, // autoContinue false, // isCurrentItem @@ -176,7 +197,7 @@ void CameraSection::appendSectionItems(QList& items, QObject* miss item = new MissionItem(nextSequenceNumber++, MAV_CMD_IMAGE_STOP_CAPTURE, MAV_FRAME_MISSION, - 0, // camera id + 0, // camera id, all cameras 0, 0, 0, 0, 0, 0, // param 2-7 not used true, // autoContinue false, // isCurrentItem @@ -187,12 +208,26 @@ void CameraSection::appendSectionItems(QList& items, QObject* miss items.append(item); } } + + if (_specifyCameraMode) { + MissionItem* item = new MissionItem(nextSequenceNumber++, + MAV_CMD_SET_CAMERA_MODE, + MAV_FRAME_MISSION, + 0, // camera id, all cameras + _cameraModeFact.rawValue().toDouble(), + NAN, NAN, NAN, NAN, NAN, // param 3-7 unused + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); + } } bool CameraSection::scanForSection(QmlObjectListModel* visualItems, int scanIndex) { bool foundGimbal = false; bool foundCameraAction = false; + bool foundCameraMode = false; bool stopLooking = false; qCDebug(CameraSectionLog) << "CameraSection::scanForCameraSection" << visualItems->count() << scanIndex; @@ -234,8 +269,9 @@ bool CameraSection::scanForSection(QmlObjectListModel* visualItems, int scanInde cameraAction()->setRawValue(TakePhotosIntervalTime); cameraPhotoIntervalTime()->setRawValue(missionItem.param1()); visualItems->removeAt(scanIndex)->deleteLater(); + } else { + stopLooking = true; } - stopLooking = true; break; case MAV_CMD_DO_SET_CAM_TRIGG_DIST: @@ -277,8 +313,9 @@ bool CameraSection::scanForSection(QmlObjectListModel* visualItems, int scanInde foundCameraAction = true; cameraAction()->setRawValue(TakeVideo); visualItems->removeAt(scanIndex)->deleteLater(); + } else { + stopLooking = true; } - stopLooking = true; break; case MAV_CMD_VIDEO_STOP_CAPTURE: @@ -286,8 +323,21 @@ bool CameraSection::scanForSection(QmlObjectListModel* visualItems, int scanInde foundCameraAction = true; cameraAction()->setRawValue(StopTakingVideo); visualItems->removeAt(scanIndex)->deleteLater(); + } else { + stopLooking = true; + } + break; + + case MAV_CMD_SET_CAMERA_MODE: + // We specifically don't test param 5/6/7 since we don't have NaN persistence for those fields + if (!foundCameraMode && missionItem.param1() == 0 && (missionItem.param2() == 0 || missionItem.param2() == 1) && qIsNaN(missionItem.param3())) { + foundCameraMode = true; + setSpecifyCameraMode(true); + cameraMode()->setRawValue(missionItem.param2()); + visualItems->removeAt(scanIndex)->deleteLater(); + } else { + stopLooking = true; } - stopLooking = true; break; default: @@ -296,12 +346,12 @@ bool CameraSection::scanForSection(QmlObjectListModel* visualItems, int scanInde } } - qCDebug(CameraSectionLog) << foundGimbal << foundCameraAction; + qCDebug(CameraSectionLog) << foundGimbal << foundCameraAction << foundCameraMode; - _settingsSpecified = foundGimbal || foundCameraAction; + _settingsSpecified = foundGimbal || foundCameraAction || foundCameraMode; emit settingsSpecifiedChanged(_settingsSpecified); - return foundGimbal || foundCameraAction; + return _settingsSpecified; } void CameraSection::_setDirty(void) @@ -335,16 +385,15 @@ void CameraSection::_updateSpecifiedGimbalYaw(void) void CameraSection::_updateSettingsSpecified(void) { - bool newSettingsSpecified = _specifyGimbal || _cameraActionFact.rawValue().toInt() != CameraActionNone; + bool newSettingsSpecified = _specifyGimbal || _specifyCameraMode || _cameraActionFact.rawValue().toInt() != CameraActionNone; if (newSettingsSpecified != _settingsSpecified) { _settingsSpecified = newSettingsSpecified; emit settingsSpecifiedChanged(newSettingsSpecified); } } -void CameraSection::_specifyGimbalChanged(bool specifyGimbal) +void CameraSection::_specifyChanged(void) { - Q_UNUSED(specifyGimbal); _setDirtyAndUpdateItemCount(); _updateSettingsSpecified(); } @@ -354,3 +403,8 @@ void CameraSection::_cameraActionChanged(void) _setDirtyAndUpdateItemCount(); _updateSettingsSpecified(); } + +bool CameraSection::cameraModeSupported(void) const +{ + return _vehicle->firmwarePlugin()->supportedMissionCommands().contains(MAV_CMD_SET_CAMERA_MODE); +} diff --git a/src/MissionManager/CameraSection.h b/src/MissionManager/CameraSection.h index 9437955221c1eb4c7dd1d18595549d404d0e55f3..2e926c82e12ed689c4ca7bd6fadde08173e5ccc7 100644 --- a/src/MissionManager/CameraSection.h +++ b/src/MissionManager/CameraSection.h @@ -38,6 +38,9 @@ public: Q_PROPERTY(Fact* cameraAction READ cameraAction CONSTANT) Q_PROPERTY(Fact* cameraPhotoIntervalTime READ cameraPhotoIntervalTime CONSTANT) Q_PROPERTY(Fact* cameraPhotoIntervalDistance READ cameraPhotoIntervalDistance CONSTANT) + Q_PROPERTY(bool cameraModeSupported READ cameraModeSupported CONSTANT) ///< true: cameraMode is supported by this vehicle + Q_PROPERTY(bool specifyCameraMode READ specifyCameraMode WRITE setSpecifyCameraMode NOTIFY specifyCameraModeChanged) + Q_PROPERTY(Fact* cameraMode READ cameraMode CONSTANT) ///< MAV_CMD_SET_CAMERA_MODE.param2 bool specifyGimbal (void) const { return _specifyGimbal; } Fact* gimbalYaw (void) { return &_gimbalYawFact; } @@ -45,8 +48,12 @@ public: Fact* cameraAction (void) { return &_cameraActionFact; } Fact* cameraPhotoIntervalTime (void) { return &_cameraPhotoIntervalTimeFact; } Fact* cameraPhotoIntervalDistance (void) { return &_cameraPhotoIntervalDistanceFact; } + bool cameraModeSupported (void) const; + bool specifyCameraMode (void) const { return _specifyCameraMode; } + Fact* cameraMode (void) { return &_cameraModeFact; } - void setSpecifyGimbal (bool specifyGimbal); + void setSpecifyGimbal (bool specifyGimbal); + void setSpecifyCameraMode (bool specifyCameraMode); ///< @return The gimbal yaw specified by this item, NaN if not specified double specifiedGimbalYaw(void) const; @@ -63,13 +70,14 @@ public: signals: bool specifyGimbalChanged (bool specifyGimbal); + bool specifyCameraModeChanged (bool specifyCameraMode); void specifiedGimbalYawChanged (double gimbalYaw); private slots: void _setDirty(void); void _setDirtyAndUpdateItemCount(void); void _updateSpecifiedGimbalYaw(void); - void _specifyGimbalChanged(bool specifyGimbal); + void _specifyChanged(void); void _updateSettingsSpecified(void); void _cameraActionChanged(void); @@ -77,11 +85,13 @@ private: bool _available; bool _settingsSpecified; bool _specifyGimbal; + bool _specifyCameraMode; Fact _gimbalYawFact; Fact _gimbalPitchFact; Fact _cameraActionFact; Fact _cameraPhotoIntervalDistanceFact; Fact _cameraPhotoIntervalTimeFact; + Fact _cameraModeFact; bool _dirty; static QMap _metaDataMap; @@ -91,4 +101,5 @@ private: static const char* _cameraActionName; static const char* _cameraPhotoIntervalDistanceName; static const char* _cameraPhotoIntervalTimeName; + static const char* _cameraModeName; }; diff --git a/src/MissionManager/CameraSectionTest.cc b/src/MissionManager/CameraSectionTest.cc index 1660e5599dc0a8c123f49ac7601a45fa0cfe4b8e..4b1f9c66df5441d67dabe4e2d146ed17ca65eff3 100644 --- a/src/MissionManager/CameraSectionTest.cc +++ b/src/MissionManager/CameraSectionTest.cc @@ -30,6 +30,7 @@ void CameraSectionTest::init(void) rgCameraSignals[specifyGimbalChangedIndex] = SIGNAL(specifyGimbalChanged(bool)); rgCameraSignals[specifiedGimbalYawChangedIndex] = SIGNAL(specifiedGimbalYawChanged(double)); + rgCameraSignals[specifyCameraModeChangedIndex] = SIGNAL(specifyCameraModeChanged(bool)); _cameraSection = _simpleItem->cameraSection(); _createSpy(_cameraSection, &_spyCamera); @@ -58,6 +59,26 @@ void CameraSectionTest::init(void) _validStopTimeItem = new SimpleMissionItem(_offlineVehicle, MissionItem(1, MAV_CMD_IMAGE_STOP_CAPTURE, MAV_FRAME_MISSION, 0, 0, 0, 0, 0, 0, 0, true, false), this); + _validCameraPhotoModeItem = new SimpleMissionItem(_offlineVehicle, + MissionItem(0, // sequence number + MAV_CMD_SET_CAMERA_MODE, + MAV_FRAME_MISSION, + 0, // camera id = 0, all cameras + 0, // photo mode + NAN, NAN, NAN, NAN, NAN, // param 3-7 unused + true, // autocontinue + false), // isCurrentItem + this); + _validCameraVideoModeItem = new SimpleMissionItem(_offlineVehicle, + MissionItem(0, // sequence number + MAV_CMD_SET_CAMERA_MODE, + MAV_FRAME_MISSION, + 0, // camera id = 0, all cameras + 1, // video mode + NAN, NAN, NAN, NAN, NAN, // param 3-7 unused + true, // autocontinue + false), // isCurrentItem + this); } void CameraSectionTest::cleanup(void) @@ -85,74 +106,104 @@ void CameraSectionTest::_createSpy(CameraSection* cameraSection, MultiSignalSpy* void CameraSectionTest::_testDirty(void) { // Check for dirty not signalled if same value + _cameraSection->setSpecifyGimbal(_cameraSection->specifyGimbal()); QVERIFY(_spySection->checkNoSignals()); QCOMPARE(_cameraSection->dirty(), false); + + _cameraSection->setSpecifyCameraMode(_cameraSection->specifyCameraMode()); + QVERIFY(_spySection->checkNoSignals()); + QCOMPARE(_cameraSection->dirty(), false); + _cameraSection->gimbalPitch()->setRawValue(_cameraSection->gimbalPitch()->rawValue()); QVERIFY(_spySection->checkNoSignals()); QCOMPARE(_cameraSection->dirty(), false); + _cameraSection->gimbalYaw()->setRawValue(_cameraSection->gimbalPitch()->rawValue()); QVERIFY(_spySection->checkNoSignals()); QCOMPARE(_cameraSection->dirty(), false); + _cameraSection->cameraAction()->setRawValue(_cameraSection->cameraAction()->rawValue()); QVERIFY(_spySection->checkNoSignals()); QCOMPARE(_cameraSection->dirty(), false); + _cameraSection->cameraPhotoIntervalTime()->setRawValue(_cameraSection->cameraPhotoIntervalTime()->rawValue()); QVERIFY(_spySection->checkNoSignals()); QCOMPARE(_cameraSection->dirty(), false); + _cameraSection->cameraPhotoIntervalDistance()->setRawValue(_cameraSection->cameraPhotoIntervalDistance()->rawValue()); QVERIFY(_spySection->checkNoSignals()); QCOMPARE(_cameraSection->dirty(), false); // Check for no duplicate dirty signalling on change + _cameraSection->setSpecifyGimbal(!_cameraSection->specifyGimbal()); QVERIFY(_spySection->checkSignalByMask(dirtyChangedMask)); QCOMPARE(_spySection->pullBoolFromSignalIndex(dirtyChangedIndex), true); QCOMPARE(_cameraSection->dirty(), true); _spySection->clearAllSignals(); + _cameraSection->setSpecifyGimbal(!_cameraSection->specifyGimbal()); QVERIFY(_spySection->checkNoSignalByMask(dirtyChangedMask)); QCOMPARE(_cameraSection->dirty(), true); + _cameraSection->setDirty(false); _spySection->clearAllSignals(); - // Check that the dirty bit can be cleared - _cameraSection->setDirty(false); + _cameraSection->setSpecifyCameraMode(!_cameraSection->specifyCameraMode()); QVERIFY(_spySection->checkSignalByMask(dirtyChangedMask)); - QCOMPARE(_spySection->pullBoolFromSignalIndex(dirtyChangedIndex), false); - QCOMPARE(_cameraSection->dirty(), false); + QCOMPARE(_spySection->pullBoolFromSignalIndex(dirtyChangedIndex), true); + QCOMPARE(_cameraSection->dirty(), true); + _spySection->clearAllSignals(); + + _cameraSection->setSpecifyCameraMode(!_cameraSection->specifyCameraMode()); + QVERIFY(_spySection->checkNoSignalByMask(dirtyChangedMask)); + QCOMPARE(_cameraSection->dirty(), true); + _cameraSection->setDirty(false); _spySection->clearAllSignals(); // Check the remaining items that should set dirty bit + _cameraSection->gimbalPitch()->setRawValue(_cameraSection->gimbalPitch()->rawValue().toDouble() + 1); QVERIFY(_spySection->checkSignalByMask(dirtyChangedMask)); QCOMPARE(_spySection->pullBoolFromSignalIndex(dirtyChangedIndex), true); QCOMPARE(_cameraSection->dirty(), true); _cameraSection->setDirty(false); _spySection->clearAllSignals(); + _cameraSection->gimbalYaw()->setRawValue(_cameraSection->gimbalPitch()->rawValue().toDouble() + 1); QVERIFY(_spySection->checkSignalByMask(dirtyChangedMask)); QCOMPARE(_spySection->pullBoolFromSignalIndex(dirtyChangedIndex), true); QCOMPARE(_cameraSection->dirty(), true); _cameraSection->setDirty(false); _spySection->clearAllSignals(); + _cameraSection->cameraAction()->setRawValue(_cameraSection->cameraAction()->rawValue().toInt() + 1); QVERIFY(_spySection->checkSignalByMask(dirtyChangedMask)); QCOMPARE(_spySection->pullBoolFromSignalIndex(dirtyChangedIndex), true); QCOMPARE(_cameraSection->dirty(), true); _cameraSection->setDirty(false); _spySection->clearAllSignals(); + _cameraSection->cameraPhotoIntervalTime()->setRawValue(_cameraSection->cameraPhotoIntervalTime()->rawValue().toInt() + 1); QVERIFY(_spySection->checkSignalByMask(dirtyChangedMask)); QCOMPARE(_spySection->pullBoolFromSignalIndex(dirtyChangedIndex), true); QCOMPARE(_cameraSection->dirty(), true); _cameraSection->setDirty(false); _spySection->clearAllSignals(); + _cameraSection->cameraPhotoIntervalDistance()->setRawValue(_cameraSection->cameraPhotoIntervalDistance()->rawValue().toDouble() + 1); QVERIFY(_spySection->checkSignalByMask(dirtyChangedMask)); QCOMPARE(_spySection->pullBoolFromSignalIndex(dirtyChangedIndex), true); QCOMPARE(_cameraSection->dirty(), true); _cameraSection->setDirty(false); _spySection->clearAllSignals(); + + _cameraSection->cameraMode()->setRawValue(_cameraSection->cameraMode()->rawValue().toInt() == 0 ? 1 : 0); + QVERIFY(_spySection->checkSignalByMask(dirtyChangedMask)); + QCOMPARE(_spySection->pullBoolFromSignalIndex(dirtyChangedIndex), true); + QCOMPARE(_cameraSection->dirty(), true); + _cameraSection->setDirty(false); + _spySection->clearAllSignals(); } void CameraSectionTest::_testSettingsAvailable(void) @@ -162,7 +213,7 @@ void CameraSectionTest::_testSettingsAvailable(void) QCOMPARE(_cameraSection->specifyGimbal(), false); QCOMPARE(_cameraSection->settingsSpecified(), false); - // Check correct reaction to specifyGimbal on/off + // Check correct reaction to specify methods on/off _cameraSection->setSpecifyGimbal(true); QCOMPARE(_cameraSection->specifyGimbal(), true); @@ -184,6 +235,26 @@ void CameraSectionTest::_testSettingsAvailable(void) _spySection->clearAllSignals(); _spyCamera->clearAllSignals(); + _cameraSection->setSpecifyCameraMode(true); + QCOMPARE(_cameraSection->specifyCameraMode(), true); + QCOMPARE(_cameraSection->settingsSpecified(), true); + QVERIFY(_spyCamera->checkSignalByMask(specifyCameraModeChangedMask)); + QCOMPARE(_spyCamera->pullBoolFromSignalIndex(specifyCameraModeChangedIndex), true); + QVERIFY(_spySection->checkSignalByMask(settingsSpecifiedChangedMask)); + QCOMPARE(_spySection->pullBoolFromSignalIndex(settingsSpecifiedChangedIndex), true); + _spySection->clearAllSignals(); + _spyCamera->clearAllSignals(); + + _cameraSection->setSpecifyCameraMode(false); + QCOMPARE(_cameraSection->specifyCameraMode(), false); + QCOMPARE(_cameraSection->settingsSpecified(), false); + QVERIFY(_spyCamera->checkSignalByMask(specifyCameraModeChangedMask)); + QCOMPARE(_spyCamera->pullBoolFromSignalIndex(specifyCameraModeChangedIndex), false); + QVERIFY(_spySection->checkSignalByMask(settingsSpecifiedChangedMask)); + QCOMPARE(_spySection->pullBoolFromSignalIndex(settingsSpecifiedChangedIndex), false); + _spySection->clearAllSignals(); + _spyCamera->clearAllSignals(); + // Check correct reaction to cameraAction on/off _cameraSection->cameraAction()->setRawValue(CameraSection::TakePhotosIntervalTime); @@ -235,7 +306,7 @@ void CameraSectionTest::_testItemCount(void) // No settings specified to start QCOMPARE(_cameraSection->itemCount(), 0); - // Check specifyGimbal + // Check specify methods _cameraSection->setSpecifyGimbal(true); QCOMPARE(_cameraSection->itemCount(), 1); @@ -251,6 +322,34 @@ void CameraSectionTest::_testItemCount(void) _spySection->clearAllSignals(); _spyCamera->clearAllSignals(); + _cameraSection->setSpecifyCameraMode(true); + QCOMPARE(_cameraSection->itemCount(), 1); + QVERIFY(_spySection->checkSignalByMask(itemCountChangedMask)); + QCOMPARE(_spySection->pullIntFromSignalIndex(itemCountChangedIndex), 1); + _spySection->clearAllSignals(); + _spyCamera->clearAllSignals(); + + _cameraSection->setSpecifyCameraMode(false); + QCOMPARE(_cameraSection->itemCount(), 0); + QVERIFY(_spySection->checkSignalByMask(itemCountChangedMask)); + QCOMPARE(_spySection->pullIntFromSignalIndex(itemCountChangedIndex), 0); + _spySection->clearAllSignals(); + _spyCamera->clearAllSignals(); + + _cameraSection->setSpecifyGimbal(true); + _cameraSection->setSpecifyCameraMode(true); + QCOMPARE(_cameraSection->itemCount(), 2); + QVERIFY(_spySection->checkSignalsByMask(itemCountChangedMask)); + _spySection->clearAllSignals(); + _spyCamera->clearAllSignals(); + + _cameraSection->setSpecifyGimbal(false); + _cameraSection->setSpecifyCameraMode(false); + QCOMPARE(_cameraSection->itemCount(), 0); + QVERIFY(_spySection->checkSignalsByMask(itemCountChangedMask)); + _spySection->clearAllSignals(); + _spyCamera->clearAllSignals(); + // Check camera actions QList rgCameraActions; @@ -318,6 +417,28 @@ void CameraSectionTest::_testAppendSectionItems(void) rgMissionItems.clear(); seqNum = 0; + // Test specifyCameraMode + + _cameraSection->setSpecifyCameraMode(true); + _cameraSection->cameraMode()->setRawValue(0); + _cameraSection->appendSectionItems(rgMissionItems, this, seqNum); + QCOMPARE(rgMissionItems.count(), 1); + QCOMPARE(seqNum, 1); + _missionItemsEqual(*rgMissionItems[0], _validCameraPhotoModeItem->missionItem()); + _cameraSection->setSpecifyGimbal(false); + rgMissionItems.clear(); + seqNum = 0; + + _cameraSection->setSpecifyCameraMode(true); + _cameraSection->cameraMode()->setRawValue(1); + _cameraSection->appendSectionItems(rgMissionItems, this, seqNum); + QCOMPARE(rgMissionItems.count(), 1); + QCOMPARE(seqNum, 1); + _missionItemsEqual(*rgMissionItems[0], _validCameraVideoModeItem->missionItem()); + _cameraSection->setSpecifyCameraMode(false); + rgMissionItems.clear(); + seqNum = 0; + // Test camera actions _cameraSection->cameraAction()->setRawValue(CameraSection::TakePhotosIntervalTime); @@ -468,6 +589,62 @@ void CameraSectionTest::_testScanForGimbalSection(void) visualItems.clear(); } +void CameraSectionTest::_testScanForCameraModeSection(void) +{ + QCOMPARE(_cameraSection->available(), true); + + int scanIndex = 0; + QmlObjectListModel visualItems; + + _commonScanTest(_cameraSection); + + // Check for a scan success + + SimpleMissionItem* newValidCameraModeItem = new SimpleMissionItem(_offlineVehicle, this); + newValidCameraModeItem->missionItem() = _validCameraPhotoModeItem->missionItem(); + visualItems.append(newValidCameraModeItem); + scanIndex = 0; + QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), true); + QCOMPARE(visualItems.count(), 0); + QCOMPARE(_cameraSection->settingsSpecified(), true); + QCOMPARE(_cameraSection->specifyCameraMode(), true); + QCOMPARE(_cameraSection->cameraMode()->rawValue().toDouble(), _validCameraPhotoModeItem->missionItem().param2()); + _cameraSection->setSpecifyCameraMode(false); + visualItems.clear(); + scanIndex = 0; + + newValidCameraModeItem->missionItem() = _validCameraVideoModeItem->missionItem(); + visualItems.append(newValidCameraModeItem); + scanIndex = 0; + QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), true); + QCOMPARE(visualItems.count(), 0); + QCOMPARE(_cameraSection->settingsSpecified(), true); + QCOMPARE(_cameraSection->specifyCameraMode(), true); + QCOMPARE(_cameraSection->cameraMode()->rawValue().toDouble(), _validCameraVideoModeItem->missionItem().param2()); + _cameraSection->setSpecifyCameraMode(false); + visualItems.clear(); + scanIndex = 0; + +#if 0 + MAV_CMD_SET_CAMERA_MODE + Mission Param #1 Camera ID (0 for all cameras, 1 for first, 2 for second, etc.) + Mission Param #2 Camera mode (0: photo mode, 1: video mode) + Mission Param #3 Audio recording enabled (0: off 1: on) + Mission Param #4 Reserved (all remaining params) +#endif + + // Mode command but incorrect settings + + SimpleMissionItem invalidSimpleItem(_offlineVehicle, _validCameraPhotoModeItem->missionItem()); + invalidSimpleItem.missionItem().setParam3(0); // Audio is not supported + visualItems.append(&invalidSimpleItem); + QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false); + QCOMPARE(visualItems.count(), 1); + QCOMPARE(_cameraSection->specifyCameraMode(), false); + QCOMPARE(_cameraSection->settingsSpecified(), false); + visualItems.clear(); +} + void CameraSectionTest::_testScanForPhotoIntervalTimeSection(void) { QCOMPARE(_cameraSection->available(), true); diff --git a/src/MissionManager/CameraSectionTest.h b/src/MissionManager/CameraSectionTest.h index cf288964324ade0932667070e43c92d351f4d4b4..16cfbb96aaad35edcb6bca03087da54b53a50c74 100644 --- a/src/MissionManager/CameraSectionTest.h +++ b/src/MissionManager/CameraSectionTest.h @@ -35,6 +35,7 @@ private slots: void _testScanForStartVideoSection(void); void _testScanForStopVideoSection(void); void _testScanForStopImageSection(void); + void _testScanForCameraModeSection(void); void _testScanForFullSection(void); private: @@ -43,12 +44,14 @@ private: enum { specifyGimbalChangedIndex = 0, specifiedGimbalYawChangedIndex, + specifyCameraModeChangedIndex, maxSignalIndex, }; enum { specifyGimbalChangedMask = 1 << specifyGimbalChangedIndex, - specifiedGimbalYawChangedMask = 1 << specifiedGimbalYawChangedIndex + specifiedGimbalYawChangedMask = 1 << specifiedGimbalYawChangedIndex, + specifyCameraModeChangedMask = 1 << specifyCameraModeChangedIndex, }; static const size_t cCameraSignals = maxSignalIndex; @@ -64,4 +67,6 @@ private: SimpleMissionItem* _validStopVideoItem; SimpleMissionItem* _validStopDistanceItem; SimpleMissionItem* _validStopTimeItem; + SimpleMissionItem* _validCameraPhotoModeItem; + SimpleMissionItem* _validCameraVideoModeItem; }; diff --git a/src/MissionManager/FixedWingLandingComplexItem.cc b/src/MissionManager/FixedWingLandingComplexItem.cc index b5dcf4a84dcb445ddbfb279aaa7ec7aca62e3fad..753213ddb131e4f87a03b4bf50bc47375ec709ab 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.cc +++ b/src/MissionManager/FixedWingLandingComplexItem.cc @@ -90,6 +90,9 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObje connect(this, &FixedWingLandingComplexItem::loiterClockwiseChanged, this, &FixedWingLandingComplexItem::_setDirty); connect(this, &FixedWingLandingComplexItem::loiterAltitudeRelativeChanged, this, &FixedWingLandingComplexItem::_setDirty); connect(this, &FixedWingLandingComplexItem::landingAltitudeRelativeChanged, this, &FixedWingLandingComplexItem::_setDirty); + + connect(this, &FixedWingLandingComplexItem::loiterAltitudeRelativeChanged, this, &FixedWingLandingComplexItem::coordinateHasRelativeAltitudeChanged); + connect(this, &FixedWingLandingComplexItem::landingAltitudeRelativeChanged, this, &FixedWingLandingComplexItem::exitCoordinateHasRelativeAltitudeChanged); } int FixedWingLandingComplexItem::lastSequenceNumber(void) const @@ -409,6 +412,7 @@ void FixedWingLandingComplexItem::_recalcFromRadiusChange(void) double angleLoiterToTangent = qRadiansToDegrees(qAsin(radius/landToLoiterDistance)) * (_loiterClockwise ? -1 : 1); _loiterCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToLoiterDistance, heading + 180 + angleLoiterToTangent); + _loiterCoordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble()); _ignoreRecalcSignals = true; emit loiterCoordinateChanged(_loiterCoordinate); @@ -446,6 +450,7 @@ void FixedWingLandingComplexItem::_recalcFromHeadingAndDistanceChange(void) // Use those values to get the new loiter point which takes heading into acount _loiterCoordinate = _landingCoordinate.atDistanceAndAzimuth(loiterDistance, heading + 180 + loiterAzimuth); + _loiterCoordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble()); _ignoreRecalcSignals = true; emit loiterTangentCoordinateChanged(_loiterTangentCoordinate); @@ -509,11 +514,14 @@ void FixedWingLandingComplexItem::_recalcFromCoordinateChange(void) void FixedWingLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact(void) { _loiterCoordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble()); + emit loiterCoordinateChanged(_loiterCoordinate); + emit coordinateChanged(_loiterCoordinate); } void FixedWingLandingComplexItem::_updateLandingCoodinateAltitudeFromFact(void) { _landingCoordinate.setAltitude(_landingAltitudeFact.rawValue().toDouble()); + emit landingCoordinateChanged(_landingCoordinate); } void FixedWingLandingComplexItem::_setDirty(void) diff --git a/src/MissionManager/FixedWingLandingComplexItem.h b/src/MissionManager/FixedWingLandingComplexItem.h index 4fabf994b7483b36e422490ac04cf8d5d7cfb361..f723ad5831f1df8edb78b6ee78c6e62b71d63f33 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.h +++ b/src/MissionManager/FixedWingLandingComplexItem.h @@ -78,9 +78,9 @@ public: void appendMissionItems (QList& items, QObject* missionItemParent) final; void applyNewAltitude (double newAltitude) final; - bool coordinateHasRelativeAltitude (void) const final { return true; } - bool exitCoordinateHasRelativeAltitude (void) const final { return true; } - bool exitCoordinateSameAsEntry (void) const final { return true; } + bool coordinateHasRelativeAltitude (void) const final { return _loiterAltitudeRelative; } + bool exitCoordinateHasRelativeAltitude (void) const final { return _landingAltitudeRelative; } + bool exitCoordinateSameAsEntry (void) const final { return false; } void setDirty (bool dirty) final; void setCoordinate (const QGeoCoordinate& coordinate) final { setLoiterCoordinate(coordinate); } diff --git a/src/MissionManager/GeoFenceController.cc b/src/MissionManager/GeoFenceController.cc index dca83aa08cfefc6c1d88e511b3b8f6c3315ba3e3..cb2d6a3a11c2fa01826b7437d3beb2fb29183947 100644 --- a/src/MissionManager/GeoFenceController.cc +++ b/src/MissionManager/GeoFenceController.cc @@ -20,6 +20,7 @@ #include "JsonHelper.h" #include "QGCQGeoCoordinate.h" #include "AppSettings.h" +#include "PlanMasterController.h" #ifndef __mobile__ #include "MainWindow.h" @@ -34,13 +35,17 @@ QGC_LOGGING_CATEGORY(GeoFenceControllerLog, "GeoFenceControllerLog") const char* GeoFenceController::_jsonFileTypeValue = "GeoFence"; const char* GeoFenceController::_jsonBreachReturnKey = "breachReturn"; -GeoFenceController::GeoFenceController(QObject* parent) - : PlanElementController(parent) +GeoFenceController::GeoFenceController(PlanMasterController* masterController, QObject* parent) + : PlanElementController(masterController, parent) + , _geoFenceManager(_managerVehicle->geoFenceManager()) , _dirty(false) , _mapPolygon(this) + , _itemsRequested(false) { connect(_mapPolygon.qmlPathModel(), &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems); connect(_mapPolygon.qmlPathModel(), &QmlObjectListModel::dirtyChanged, this, &GeoFenceController::_polygonDirtyChanged); + + managerVehicleChanged(_managerVehicle); } GeoFenceController::~GeoFenceController() @@ -56,14 +61,6 @@ void GeoFenceController::start(bool editMode) _init(); } -void GeoFenceController::startStaticActiveVehicle(Vehicle* vehicle) -{ - qCDebug(GeoFenceControllerLog) << "startStaticActiveVehicle"; - - PlanElementController::startStaticActiveVehicle(vehicle); - _init(); -} - void GeoFenceController::_init(void) { @@ -89,28 +86,31 @@ void GeoFenceController::_signalAll(void) emit dirtyChanged(dirty()); } -void GeoFenceController::activeVehicleBeingRemoved(void) -{ - _activeVehicle->geoFenceManager()->disconnect(this); - _activeVehicle = NULL; -} - -void GeoFenceController::activeVehicleSet(Vehicle* vehicle) +void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle) { - _activeVehicle = vehicle; - GeoFenceManager* geoFenceManager = _activeVehicle->geoFenceManager(); - connect(geoFenceManager, &GeoFenceManager::breachReturnSupportedChanged, this, &GeoFenceController::breachReturnSupportedChanged); - connect(geoFenceManager, &GeoFenceManager::circleEnabledChanged, this, &GeoFenceController::circleEnabledChanged); - connect(geoFenceManager, &GeoFenceManager::circleRadiusFactChanged, this, &GeoFenceController::circleRadiusFactChanged); - connect(geoFenceManager, &GeoFenceManager::polygonEnabledChanged, this, &GeoFenceController::polygonEnabledChanged); - connect(geoFenceManager, &GeoFenceManager::polygonSupportedChanged, this, &GeoFenceController::polygonSupportedChanged); - connect(geoFenceManager, &GeoFenceManager::loadComplete, this, &GeoFenceController::_loadComplete); - connect(geoFenceManager, &GeoFenceManager::inProgressChanged, this, &GeoFenceController::syncInProgressChanged); + if (_managerVehicle) { + _geoFenceManager->disconnect(this); + _managerVehicle = NULL; + _geoFenceManager = NULL; + } - if (!geoFenceManager->inProgress()) { - _loadComplete(geoFenceManager->breachReturnPoint(), geoFenceManager->polygon()); + _managerVehicle = managerVehicle; + if (!_managerVehicle) { + qWarning() << "GeoFenceController::managerVehicleChanged managerVehicle=NULL"; + return; } + _geoFenceManager = _managerVehicle->geoFenceManager(); + connect(_geoFenceManager, &GeoFenceManager::breachReturnSupportedChanged, this, &GeoFenceController::breachReturnSupportedChanged); + connect(_geoFenceManager, &GeoFenceManager::circleEnabledChanged, this, &GeoFenceController::circleEnabledChanged); + connect(_geoFenceManager, &GeoFenceManager::circleRadiusFactChanged, this, &GeoFenceController::circleRadiusFactChanged); + connect(_geoFenceManager, &GeoFenceManager::polygonEnabledChanged, this, &GeoFenceController::polygonEnabledChanged); + connect(_geoFenceManager, &GeoFenceManager::polygonSupportedChanged, this, &GeoFenceController::polygonSupportedChanged); + connect(_geoFenceManager, &GeoFenceManager::loadComplete, this, &GeoFenceController::_managerLoadComplete); + connect(_geoFenceManager, &GeoFenceManager::sendComplete, this, &GeoFenceController::_managerSendComplete); + connect(_geoFenceManager, &GeoFenceManager::removeAllComplete, this, &GeoFenceController::_managerRemoveAllComplete); + connect(_geoFenceManager, &GeoFenceManager::inProgressChanged, this, &GeoFenceController::syncInProgressChanged); + _signalAll(); } @@ -156,29 +156,46 @@ void GeoFenceController::removeAll(void) _mapPolygon.clear(); } +void GeoFenceController::removeAllFromVehicle(void) +{ + if (_masterController->offline()) { + qCWarning(GeoFenceControllerLog) << "GeoFenceController::removeAllFromVehicle called while offline"; + } else if (syncInProgress()) { + qCWarning(GeoFenceControllerLog) << "GeoFenceController::removeAllFromVehicle called while syncInProgress"; + } else { + _geoFenceManager->removeAll(); + } +} + void GeoFenceController::loadFromVehicle(void) { - if (_activeVehicle->parameterManager()->parametersReady() && !syncInProgress()) { - _activeVehicle->geoFenceManager()->loadFromVehicle(); + if (_masterController->offline()) { + qCWarning(GeoFenceControllerLog) << "GeoFenceController::loadFromVehicle called while offline"; + } else if (syncInProgress()) { + qCWarning(GeoFenceControllerLog) << "GeoFenceController::loadFromVehicle called while syncInProgress"; } else { - qCWarning(GeoFenceControllerLog) << "GeoFenceController::loadFromVehicle call at wrong time" << _activeVehicle->parameterManager()->parametersReady() << syncInProgress(); + _itemsRequested = true; + _geoFenceManager->loadFromVehicle(); } } void GeoFenceController::sendToVehicle(void) { - if (_activeVehicle->parameterManager()->parametersReady() && !syncInProgress()) { - _activeVehicle->geoFenceManager()->sendToVehicle(_breachReturnPoint, _mapPolygon.pathModel()); + if (_masterController->offline()) { + qCWarning(GeoFenceControllerLog) << "GeoFenceController::sendToVehicle called while offline"; + } else if (syncInProgress()) { + qCWarning(GeoFenceControllerLog) << "GeoFenceController::sendToVehicle called while syncInProgress"; + } else { + qCDebug(GeoFenceControllerLog) << "GeoFenceController::sendToVehicle"; + _geoFenceManager->sendToVehicle(_breachReturnPoint, _mapPolygon.pathModel()); _mapPolygon.setDirty(false); setDirty(false); - } else { - qCWarning(GeoFenceControllerLog) << "GeoFenceController::loadFromVehicle call at wrong time" << _activeVehicle->parameterManager()->parametersReady() << syncInProgress(); } } bool GeoFenceController::syncInProgress(void) const { - return _activeVehicle->geoFenceManager()->inProgress(); + return _geoFenceManager->inProgress(); } bool GeoFenceController::dirty(void) const @@ -207,37 +224,37 @@ void GeoFenceController::_polygonDirtyChanged(bool dirty) bool GeoFenceController::breachReturnSupported(void) const { - return _activeVehicle->geoFenceManager()->breachReturnSupported(); + return _geoFenceManager->breachReturnSupported(); } bool GeoFenceController::circleEnabled(void) const { - return _activeVehicle->geoFenceManager()->circleEnabled(); + return _geoFenceManager->circleEnabled(); } Fact* GeoFenceController::circleRadiusFact(void) const { - return _activeVehicle->geoFenceManager()->circleRadiusFact(); + return _geoFenceManager->circleRadiusFact(); } bool GeoFenceController::polygonSupported(void) const { - return _activeVehicle->geoFenceManager()->polygonSupported(); + return _geoFenceManager->polygonSupported(); } bool GeoFenceController::polygonEnabled(void) const { - return _activeVehicle->geoFenceManager()->polygonEnabled(); + return _geoFenceManager->polygonEnabled(); } QVariantList GeoFenceController::params(void) const { - return _activeVehicle->geoFenceManager()->params(); + return _geoFenceManager->params(); } QStringList GeoFenceController::paramLabels(void) const { - return _activeVehicle->geoFenceManager()->paramLabels(); + return _geoFenceManager->paramLabels(); } void GeoFenceController::_setDirty(void) @@ -260,12 +277,34 @@ void GeoFenceController::_setReturnPointFromManager(QGeoCoordinate breachReturnP emit breachReturnPointChanged(_breachReturnPoint); } -void GeoFenceController::_loadComplete(const QGeoCoordinate& breachReturn, const QList& polygon) +void GeoFenceController::_managerLoadComplete(const QGeoCoordinate& breachReturn, const QList& polygon) { - _setReturnPointFromManager(breachReturn); - _setPolygonFromManager(polygon); - setDirty(false); - emit loadComplete(); + // Fly view always reloads on _loadComplete + // Plan view only reloads on _loadComplete if specifically requested + if (!_editMode || _itemsRequested) { + _setReturnPointFromManager(breachReturn); + _setPolygonFromManager(polygon); + setDirty(false); + _signalAll(); + emit loadComplete(); + } + _itemsRequested = false; +} + +void GeoFenceController::_managerSendComplete(bool error) +{ + // Fly view always reloads on manager sendComplete + if (!error && !_editMode) { + showPlanFromManagerVehicle(); + } +} + +void GeoFenceController::_managerRemoveAllComplete(bool error) +{ + if (!error) { + // Remove all from vehicle so we always update + showPlanFromManagerVehicle(); + } } bool GeoFenceController::containsItems(void) const @@ -278,7 +317,28 @@ void GeoFenceController::_updateContainsItems(void) emit containsItemsChanged(containsItems()); } -void GeoFenceController::removeAllFromVehicle(void) +bool GeoFenceController::showPlanFromManagerVehicle(void) { - _activeVehicle->geoFenceManager()->removeAll(); + qCDebug(GeoFenceControllerLog) << "showPlanFromManagerVehicle" << _editMode; + if (_masterController->offline()) { + qCWarning(GeoFenceControllerLog) << "GeoFenceController::showPlanFromManagerVehicle called while offline"; + return true; // stops further propagation of showPlanFromManagerVehicle due to error + } else { + _itemsRequested = true; + if (!_managerVehicle->initialPlanRequestComplete()) { + // The vehicle hasn't completed initial load, we can just wait for loadComplete to be signalled automatically + qCDebug(GeoFenceControllerLog) << "showPlanFromManagerVehicle: !initialPlanRequestComplete, wait for signal"; + return true; + } else if (syncInProgress()) { + // If the sync is already in progress, _loadComplete will be called automatically when it is done. So no need to do anything. + qCDebug(GeoFenceControllerLog) << "showPlanFromManagerVehicle: syncInProgress wait for signal"; + return true; + } else { + // Fake a _loadComplete with the current items + qCDebug(GeoFenceControllerLog) << "showPlanFromManagerVehicle: sync complete simulate signal"; + _itemsRequested = true; + _managerLoadComplete(_geoFenceManager->breachReturnPoint(), _geoFenceManager->polygon()); + return false; + } + } } diff --git a/src/MissionManager/GeoFenceController.h b/src/MissionManager/GeoFenceController.h index 61ada74d552ff5c871c58bd8d263cf7af7005661..7019cba3133a79e6a7f0c63c63c7b3492e990f6b 100644 --- a/src/MissionManager/GeoFenceController.h +++ b/src/MissionManager/GeoFenceController.h @@ -26,7 +26,7 @@ class GeoFenceController : public PlanElementController Q_OBJECT public: - GeoFenceController(QObject* parent = NULL); + GeoFenceController(PlanMasterController* masterController, QObject* parent = NULL); ~GeoFenceController(); Q_PROPERTY(QGCMapPolygon* mapPolygon READ mapPolygon CONSTANT) @@ -45,7 +45,6 @@ public: Q_INVOKABLE void removePolygon (void) { _mapPolygon.clear(); } void start (bool editMode) final; - void startStaticActiveVehicle (Vehicle* vehicle) final; void save (QJsonObject& json) final; bool load (const QJsonObject& json, QString& errorString) final; void loadFromVehicle (void) final; @@ -56,8 +55,8 @@ public: bool dirty (void) const final; void setDirty (bool dirty) final; bool containsItems (void) const final; - void activeVehicleBeingRemoved (void) final; - void activeVehicleSet (Vehicle* vehicle) final; + void managerVehicleChanged (Vehicle* managerVehicle) final; + bool showPlanFromManagerVehicle (void) final; bool circleEnabled (void) const; Fact* circleRadiusFact (void) const; @@ -89,16 +88,20 @@ private slots: void _setDirty(void); void _setPolygonFromManager(const QList& polygon); void _setReturnPointFromManager(QGeoCoordinate breachReturnPoint); - void _loadComplete(const QGeoCoordinate& breachReturn, const QList& polygon); + void _managerLoadComplete(const QGeoCoordinate& breachReturn, const QList& polygon); void _updateContainsItems(void); + void _managerSendComplete(bool error); + void _managerRemoveAllComplete(bool error); private: void _init(void); void _signalAll(void); - bool _dirty; - QGCMapPolygon _mapPolygon; - QGeoCoordinate _breachReturnPoint; + GeoFenceManager* _geoFenceManager; + bool _dirty; + QGCMapPolygon _mapPolygon; + QGeoCoordinate _breachReturnPoint; + bool _itemsRequested; static const char* _jsonFileTypeValue; static const char* _jsonBreachReturnKey; diff --git a/src/MissionManager/GeoFenceManager.cc b/src/MissionManager/GeoFenceManager.cc index a2e8f14bb058814f4e3a1686cc63ca3b81a8f2fc..357e3b768b335c2aa7f75bdfd2a76c74eadf1a59 100644 --- a/src/MissionManager/GeoFenceManager.cc +++ b/src/MissionManager/GeoFenceManager.cc @@ -40,8 +40,15 @@ void GeoFenceManager::loadFromVehicle(void) void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon) { + // No geofence support in unknown vehicle Q_UNUSED(breachReturn); Q_UNUSED(polygon); + emit sendComplete(false /* error */); +} + +void GeoFenceManager::removeAll(void) +{ // No geofence support in unknown vehicle + emit removeAllComplete(false /* error */); } diff --git a/src/MissionManager/GeoFenceManager.h b/src/MissionManager/GeoFenceManager.h index ffa82f0e365eb7108ac669c89ba0ce0454363ebf..a1ea66ac861f2f1d3118fb21be6aca636ab952d2 100644 --- a/src/MissionManager/GeoFenceManager.h +++ b/src/MissionManager/GeoFenceManager.h @@ -35,13 +35,16 @@ public: virtual bool inProgress(void) const { return false; } /// Load the current settings from the vehicle + /// Signals loadComplete when done virtual void loadFromVehicle(void); /// Send the current settings to the vehicle + /// Signals sendComplete when done virtual void sendToVehicle(const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon); - /// Remove all fence related items from vehicle (does not affect paramters) - virtual void removeAll(void) { } + /// Remove all fence related items from vehicle (does not affect parameters) + /// Signals removeAllComplete when done + virtual void removeAll(void); /// Returns true if this vehicle support polygon fence /// Signal: polygonSupportedChanged @@ -93,6 +96,8 @@ signals: void polygonSupportedChanged (bool polygonSupported); void polygonEnabledChanged (bool polygonEnabled); void breachReturnSupportedChanged (bool breachReturnSupported); + void removeAllComplete (bool error); + void sendComplete (bool error); protected: void _sendError(ErrorCode_t errorCode, const QString& errorMsg); diff --git a/src/MissionManager/MavCmdInfoCommon.json b/src/MissionManager/MavCmdInfoCommon.json index 8ba01705bb4d55a6087ec9b62de666abd26f2928..3534f354b3f5fed842542ff9e12ae4c770a80443 100644 --- a/src/MissionManager/MavCmdInfoCommon.json +++ b/src/MissionManager/MavCmdInfoCommon.json @@ -54,6 +54,7 @@ "label": "Heading", "units": "radians", "nanUnchanged": true, + "default": null, "decimalPlaces": 2 } }, @@ -75,6 +76,7 @@ "label": "Heading", "units": "radians", "nanUnchanged": true, + "default": null, "decimalPlaces": 2 } }, @@ -101,6 +103,7 @@ "label": "Heading", "units": "radians", "nanUnchanged": true, + "default": null, "decimalPlaces": 2 } }, @@ -157,6 +160,7 @@ "label": "Heading", "units": "radians", "nanUnchanged": true, + "default": null, "decimalPlaces": 2 } }, @@ -178,6 +182,7 @@ "label": "Heading", "units": "radians", "nanUnchanged": true, + "default": null, "decimalPlaces": 2 } }, @@ -316,6 +321,7 @@ "label": "Heading", "units": "deg", "nanUnchanged": true, + "default": null, "decimalPlaces": 2 } }, @@ -331,6 +337,7 @@ "label": "Heading", "units": "deg", "nanUnchanged": true, + "default": null, "decimalPlaces": 2 } }, @@ -960,6 +967,30 @@ { "id": 510, "rawName": "MAV_CMD_GET_MESSAGE_INTERVAL", "friendlyName": "Get message interval" }, { "id": 511, "rawName": "MAV_CMD_SET_MESSAGE_INTERVAL", "friendlyName": "Set message interval" }, { "id": 520, "rawName": "MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES", "friendlyName": "Get capabilities" }, + { + "id": 530, + "rawName": "MAV_CMD_SET_CAMERA_MODE", + "friendlyName": "Set camera modes" , + "description": "Set camera photo, video, audio modes.", + "category": "Camera", + "param1": { + "label": "Camera id", + "default": 0, + "decimalPlaces": 0 + }, + "param2": { + "label": "Mode", + "enumStrings": "Take photos,Record video", + "enumValues": "0,1", + "default": 0 + }, + "param3": { + "label": "Audio", + "enumStrings": "Recording disabled,Recording enabled", + "enumValues": "0,1", + "default": 0 + } + }, { "id": 2000, "rawName": "MAV_CMD_IMAGE_START_CAPTURE", diff --git a/src/MissionManager/MissionCommandUIInfo.cc b/src/MissionManager/MissionCommandUIInfo.cc index 8e9b1e3bb1260b60497c9b8b946ae84b59ee9808..ad63a6b1d77006ce0e45f9e87ea6ffe017f8e1ee 100644 --- a/src/MissionManager/MissionCommandUIInfo.cc +++ b/src/MissionManager/MissionCommandUIInfo.cc @@ -334,7 +334,7 @@ bool MissionCommandUIInfo::loadJsonInfo(const QJsonObject& jsonObject, bool requ // Validate key types QList types; - types << QJsonValue::Double << QJsonValue::Double << QJsonValue::String << QJsonValue::String << QJsonValue::String << QJsonValue::String << QJsonValue::Bool; + types << QJsonValue::Null << QJsonValue::Double << QJsonValue::String << QJsonValue::String << QJsonValue::String << QJsonValue::String << QJsonValue::Bool; if (!JsonHelper::validateKeyTypes(jsonObject, allParamKeys, types, internalError)) { errorString = _loadErrorString(internalError); return false; @@ -358,7 +358,15 @@ bool MissionCommandUIInfo::loadJsonInfo(const QJsonObject& jsonObject, bool requ paramInfo->_nanUnchanged = paramObject.value(_nanUnchangedJsonKey).toBool(false); if (paramObject.contains(_defaultJsonKey)) { - paramInfo->_defaultValue = paramObject.value(_defaultJsonKey).toDouble(0.0); + if (paramInfo->_nanUnchanged) { + paramInfo->_defaultValue = JsonHelper::possibleNaNJsonValue(paramObject[_defaultJsonKey]); + } else { + if (paramObject[_defaultJsonKey].type() == QJsonValue::Null) { + errorString = QString("Param %1 default value was null/NaN but NaN is not allowed"); + return false; + } + paramInfo->_defaultValue = paramObject.value(_defaultJsonKey).toDouble(0.0); + } } else { paramInfo->_defaultValue = paramInfo->_nanUnchanged ? std::numeric_limits::quiet_NaN() : 0; } diff --git a/src/MissionManager/MissionCommandUIInfo.h b/src/MissionManager/MissionCommandUIInfo.h index 4c1d4bfd144ffec9f8eee61e4a27a4e0c52d20d3..f2a66929308c4ec47cd8ff45aca2eaa8779c650e 100644 --- a/src/MissionManager/MissionCommandUIInfo.h +++ b/src/MissionManager/MissionCommandUIInfo.h @@ -24,7 +24,7 @@ class MissionCommandTreeTest; /// UI Information associated with a mission command (MAV_CMD) parameter /// -/// MissionCommandParamInfo is used to automatically generate editing ui for a parameter assocaited with a MAV_CMD. +/// MissionCommandParamInfo is used to automatically generate editing ui for a parameter associated with a MAV_CMD. /// /// The json format for a MissionCmdParamInfo object is: /// @@ -34,7 +34,7 @@ class MissionCommandTreeTest; /// default double 0.0/NaN Default value for param. If no default value specified and nanUnchanged == true, then defaultValue is NaN. /// decimalPlaces int 7 Number of decimal places to show for value /// enumStrings string Strings to show in combo box for selection -/// enumValues string Values assocaited with each enum string +/// enumValues string Values associated with each enum string /// nanUnchanged bool false True: value can be set to NaN to signal unchanged /// class MissionCmdParamInfo : public QObject { diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 496399d98141ec832b5f88cab4fe05b24cb88542..512d8a614737057abd7232cb14b98bad6f8e17a2 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -24,6 +24,7 @@ #include "AppSettings.h" #include "MissionSettingsItem.h" #include "QGCQGeoCoordinate.h" +#include "PlanMasterController.h" #ifndef __mobile__ #include "MainWindow.h" @@ -48,17 +49,20 @@ const char* MissionController::_jsonMavAutopilotKey = "MAV_AUTOPILOT"; const int MissionController::_missionFileVersion = 2; -MissionController::MissionController(QObject *parent) - : PlanElementController(parent) +MissionController::MissionController(PlanMasterController* masterController, QObject *parent) + : PlanElementController(masterController, parent) + , _missionManager(_managerVehicle->missionManager()) , _visualItems(NULL) , _settingsItem(NULL) , _firstItemsFromVehicle(false) - , _missionItemsRequested(false) + , _itemsRequested(false) , _surveyMissionItemName(tr("Survey")) , _fwLandingMissionItemName(tr("Fixed Wing Landing")) , _appSettings(qgcApp()->toolbox()->settingsManager()->appSettings()) + , _progressPct(0) { _resetMissionFlightStatus(); + managerVehicleChanged(_managerVehicle); } MissionController::~MissionController() @@ -75,9 +79,9 @@ void MissionController::_resetMissionFlightStatus(void) _missionFlightStatus.cruiseTime = 0.0; _missionFlightStatus.hoverDistance = 0.0; _missionFlightStatus.cruiseDistance = 0.0; - _missionFlightStatus.cruiseSpeed = _activeVehicle ? _activeVehicle->defaultCruiseSpeed() : std::numeric_limits::quiet_NaN(); - _missionFlightStatus.hoverSpeed = _activeVehicle ? _activeVehicle->defaultHoverSpeed() : std::numeric_limits::quiet_NaN(); - _missionFlightStatus.vehicleSpeed = _activeVehicle ? (_activeVehicle->multiRotor() || _activeVehicle->vtol() ? _missionFlightStatus.hoverSpeed : _missionFlightStatus.cruiseSpeed) : std::numeric_limits::quiet_NaN(); + _missionFlightStatus.cruiseSpeed = _controllerVehicle->defaultCruiseSpeed(); + _missionFlightStatus.hoverSpeed = _controllerVehicle->defaultHoverSpeed(); + _missionFlightStatus.vehicleSpeed = _controllerVehicle->multiRotor() || _managerVehicle->vtol() ? _missionFlightStatus.hoverSpeed : _missionFlightStatus.cruiseSpeed; _missionFlightStatus.vehicleYaw = 0.0; _missionFlightStatus.gimbalYaw = std::numeric_limits::quiet_NaN(); @@ -92,12 +96,10 @@ void MissionController::_resetMissionFlightStatus(void) _missionFlightStatus.batteryChangePoint = -1; _missionFlightStatus.batteriesRequired = -1; - if (_activeVehicle) { - _activeVehicle->firmwarePlugin()->batteryConsumptionData(_activeVehicle, _missionFlightStatus.mAhBattery, _missionFlightStatus.hoverAmps, _missionFlightStatus.cruiseAmps); - if (_missionFlightStatus.mAhBattery != 0) { - double batteryPercentRemainingAnnounce = qgcApp()->toolbox()->settingsManager()->appSettings()->batteryPercentRemainingAnnounce()->rawValue().toDouble(); - _missionFlightStatus.ampMinutesAvailable = (double)_missionFlightStatus.mAhBattery / 1000.0 * 60.0 * ((100.0 - batteryPercentRemainingAnnounce) / 100.0); - } + _controllerVehicle->firmwarePlugin()->batteryConsumptionData(_controllerVehicle, _missionFlightStatus.mAhBattery, _missionFlightStatus.hoverAmps, _missionFlightStatus.cruiseAmps); + if (_missionFlightStatus.mAhBattery != 0) { + double batteryPercentRemainingAnnounce = qgcApp()->toolbox()->settingsManager()->appSettings()->batteryPercentRemainingAnnounce()->rawValue().toDouble(); + _missionFlightStatus.ampMinutesAvailable = (double)_missionFlightStatus.mAhBattery / 1000.0 * 60.0 * ((100.0 - batteryPercentRemainingAnnounce) / 100.0); } emit missionDistanceChanged(_missionFlightStatus.totalDistance); @@ -120,19 +122,11 @@ void MissionController::start(bool editMode) _init(); } -void MissionController::startStaticActiveVehicle(Vehicle *vehicle) -{ - qCDebug(MissionControllerLog) << "startStaticActiveVehicle"; - - PlanElementController::startStaticActiveVehicle(vehicle); - _init(); -} - void MissionController::_init(void) { // We start with an empty mission _visualItems = new QmlObjectListModel(this); - _addMissionSettings(_activeVehicle, _visualItems, false /* addToCenter */); + _addMissionSettings(_visualItems, false /* addToCenter */); _initAllVisualItems(); } @@ -141,7 +135,9 @@ void MissionController::_newMissionItemsAvailableFromVehicle(bool removeAllReque { qCDebug(MissionControllerLog) << "_newMissionItemsAvailableFromVehicle"; - if (!_editMode || removeAllRequested || _missionItemsRequested || _visualItems->count() == 1) { + // Fly view always reloads on _loadComplete + // Plan view only reloads on _loadComplete if specifically requested + if (!_editMode || 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): @@ -150,13 +146,13 @@ void MissionController::_newMissionItemsAvailableFromVehicle(bool removeAllReque // - Remove all way requested from Fly view (clear mission on flight end) QmlObjectListModel* newControllerMissionItems = new QmlObjectListModel(this); - const QList& newMissionItems = _activeVehicle->missionManager()->missionItems(); + const QList& newMissionItems = _missionManager->missionItems(); qCDebug(MissionControllerLog) << "loading from vehicle: count"<< newMissionItems.count(); int i = 0; - if (_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle() && newMissionItems.count() != 0) { + if (_controllerVehicle->firmwarePlugin()->sendHomePositionToVehicle() && newMissionItems.count() != 0) { // First item is fake home position - _addMissionSettings(_activeVehicle, newControllerMissionItems, false /* addToCenter */); + _addMissionSettings(newControllerMissionItems, false /* addToCenter */); MissionSettingsItem* settingsItem = newControllerMissionItems->value(0); if (!settingsItem) { qWarning() << "First item is not settings item"; @@ -168,7 +164,7 @@ void MissionController::_newMissionItemsAvailableFromVehicle(bool removeAllReque for (; iappend(new SimpleMissionItem(_activeVehicle, *missionItem, this)); + newControllerMissionItems->append(new SimpleMissionItem(_controllerVehicle, *missionItem, this)); } _deinitAllVisualItems(); @@ -176,35 +172,49 @@ void MissionController::_newMissionItemsAvailableFromVehicle(bool removeAllReque _settingsItem = NULL; _visualItems = newControllerMissionItems; - if (!_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle() || _visualItems->count() == 0) { - _addMissionSettings(_activeVehicle, _visualItems, _editMode && _visualItems->count() > 0 /* addToCenter */); + if (!_controllerVehicle->firmwarePlugin()->sendHomePositionToVehicle() || _visualItems->count() == 0) { + _addMissionSettings(_visualItems, _editMode && _visualItems->count() > 0 /* addToCenter */); } - _missionItemsRequested = false; - if (_editMode) { - MissionController::_scanForAdditionalSettings(_visualItems, _activeVehicle); + MissionController::_scanForAdditionalSettings(_visualItems, _controllerVehicle); } _initAllVisualItems(); emit newItemsFromVehicle(); } + _itemsRequested = false; } void MissionController::loadFromVehicle(void) { - Vehicle* activeVehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle(); - - if (activeVehicle) { - _missionItemsRequested = true; - activeVehicle->missionManager()->requestMissionItems(); + if (_masterController->offline()) { + qCWarning(MissionControllerLog) << "MissionControllerLog::loadFromVehicle called while offline"; + } else if (syncInProgress()) { + qCWarning(MissionControllerLog) << "MissionControllerLog::loadFromVehicle called while syncInProgress"; + } else { + _itemsRequested = true; + _managerVehicle->missionManager()->loadFromVehicle(); } } void MissionController::sendToVehicle(void) { - sendItemsToVehicle(_activeVehicle, _visualItems); - _visualItems->setDirty(false); + if (_masterController->offline()) { + qCWarning(MissionControllerLog) << "MissionControllerLog::sendToVehicle called while offline"; + } else if (syncInProgress()) { + qCWarning(MissionControllerLog) << "MissionControllerLog::sendToVehicle called while syncInProgress"; + } else { + qCDebug(MissionControllerLog) << "MissionControllerLog::sendToVehicle"; + if (_visualItems->count() == 1) { + // This prevents us from sending a possibly bogus home position to the vehicle + QmlObjectListModel emptyModel; + sendItemsToVehicle(_managerVehicle, &emptyModel); + } else { + sendItemsToVehicle(_managerVehicle, _visualItems); + } + setDirty(false); + } } /// Converts from visual items to MissionItems @@ -212,6 +222,10 @@ void MissionController::sendToVehicle(void) /// @return true: Mission end action was added to end of list bool MissionController::_convertToMissionItems(QmlObjectListModel* visualMissionItems, QList& rgMissionItems, QObject* missionItemParent) { + if (visualMissionItems->count() == 0) { + return false; + } + bool endActionSet = false; int lastSeqNum = 0; @@ -265,13 +279,13 @@ int MissionController::_nextSequenceNumber(void) int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i) { int sequenceNumber = _nextSequenceNumber(); - SimpleMissionItem * newItem = new SimpleMissionItem(_activeVehicle, this); + SimpleMissionItem * newItem = new SimpleMissionItem(_controllerVehicle, this); newItem->setSequenceNumber(sequenceNumber); newItem->setCoordinate(coordinate); newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT); _initVisualItem(newItem); if (_visualItems->count() == 1) { - newItem->setCommand(_activeVehicle->vtol() ? MavlinkQmlSingleton::MAV_CMD_NAV_VTOL_TAKEOFF : MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF); + newItem->setCommand(_controllerVehicle->vtol() ? MavlinkQmlSingleton::MAV_CMD_NAV_VTOL_TAKEOFF : MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF); } newItem->setDefaultsForCommand(); if ((MAV_CMD)newItem->command() == MAV_CMD_NAV_WAYPOINT) { @@ -296,10 +310,29 @@ int MissionController::insertComplexMissionItem(QString itemName, QGeoCoordinate int sequenceNumber = _nextSequenceNumber(); if (itemName == _surveyMissionItemName) { - newItem = new SurveyMissionItem(_activeVehicle, _visualItems); + newItem = new SurveyMissionItem(_controllerVehicle, _visualItems); newItem->setCoordinate(mapCenterCoordinate); + // If the vehicle is known to have a gimbal then we automatically point the gimbal straight down if not already set + bool rollSupported = false; + bool pitchSupported = false; + bool yawSupported = false; + MissionSettingsItem* settingsItem = _visualItems->value(0); + CameraSection* cameraSection = settingsItem->cameraSection(); + // Set camera to photo mode (leave alone if user already specified) + if (cameraSection->cameraModeSupported() && !cameraSection->specifyCameraMode()) { + cameraSection->setSpecifyCameraMode(true); + cameraSection->cameraMode()->setRawValue(0); + } + // Point gimbal straight down + if (_controllerVehicle->firmwarePlugin()->hasGimbal(_controllerVehicle, rollSupported, pitchSupported, yawSupported) && pitchSupported) { + // If the user already specified a gimbal angle leave it alone + if (!cameraSection->specifyGimbal()) { + cameraSection->setSpecifyGimbal(true); + cameraSection->gimbalPitch()->setRawValue(-90.0); + } + } } else if (itemName == _fwLandingMissionItemName) { - newItem = new FixedWingLandingComplexItem(_activeVehicle, _visualItems); + newItem = new FixedWingLandingComplexItem(_controllerVehicle, _visualItems); } else { qWarning() << "Internal error: Unknown complex item:" << itemName; return sequenceNumber; @@ -316,13 +349,47 @@ int MissionController::insertComplexMissionItem(QString itemName, QGeoCoordinate void MissionController::removeMissionItem(int index) { + if (index <= 0 || index >= _visualItems->count()) { + qWarning() << "MissionController::removeMissionItem called with bad index - count:index" << _visualItems->count() << index; + return; + } + + bool surveyRemoved = _visualItems->value(index); VisualMissionItem* item = qobject_cast(_visualItems->removeAt(index)); _deinitVisualItem(item); item->deleteLater(); + if (surveyRemoved) { + // Determine if the mission still has another survey in it + bool foundSurvey = false; + for (int i=1; i<_visualItems->count(); i++) { + if (_visualItems->value(i)) { + foundSurvey = true; + break; + } + } + + // If there is no longer a survey item in the mission remove added commands + if (!foundSurvey) { + bool rollSupported = false; + bool pitchSupported = false; + bool yawSupported = false; + MissionSettingsItem* settingsItem = _visualItems->value(0); + CameraSection* cameraSection = settingsItem->cameraSection(); + if (_controllerVehicle->firmwarePlugin()->hasGimbal(_controllerVehicle, rollSupported, pitchSupported, yawSupported) && pitchSupported) { + if (cameraSection->specifyGimbal() && cameraSection->gimbalPitch()->rawValue().toDouble() == -90.0 && cameraSection->gimbalYaw()->rawValue().toDouble() == 0.0) { + cameraSection->setSpecifyGimbal(false); + } + } + if (cameraSection->cameraModeSupported() && cameraSection->specifyCameraMode() && cameraSection->cameraMode()->rawValue().toInt() == 0) { + cameraSection->setSpecifyCameraMode(false); + } + } + } + _recalcAll(); - _visualItems->setDirty(true); + setDirty(true); } void MissionController::removeAll(void) @@ -332,14 +399,14 @@ void MissionController::removeAll(void) _visualItems->deleteLater(); _settingsItem = NULL; _visualItems = new QmlObjectListModel(this); - _addMissionSettings(_activeVehicle, _visualItems, false /* addToCenter */); + _addMissionSettings(_visualItems, false /* addToCenter */); _initAllVisualItems(); - _visualItems->setDirty(true); + setDirty(true); _resetMissionFlightStatus(); } } -bool MissionController::_loadJsonMissionFileV1(Vehicle* vehicle, const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString) +bool MissionController::_loadJsonMissionFileV1(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString) { // Validate root object keys QList rootKeyInfoList = { @@ -364,7 +431,7 @@ bool MissionController::_loadJsonMissionFileV1(Vehicle* vehicle, const QJsonObje return false; } - SurveyMissionItem* item = new SurveyMissionItem(vehicle, visualItems); + SurveyMissionItem* item = new SurveyMissionItem(_controllerVehicle, visualItems); const QJsonObject itemObject = itemValue.toObject(); if (item->load(itemObject, itemObject["id"].toInt(), errorString)) { surveyItems.append(item); @@ -407,7 +474,7 @@ bool MissionController::_loadJsonMissionFileV1(Vehicle* vehicle, const QJsonObje } const QJsonObject itemObject = itemValue.toObject(); - SimpleMissionItem* item = new SimpleMissionItem(vehicle, visualItems); + SimpleMissionItem* item = new SimpleMissionItem(_controllerVehicle, 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; @@ -419,10 +486,10 @@ bool MissionController::_loadJsonMissionFileV1(Vehicle* vehicle, const QJsonObje } while (nextSimpleItemIndex < itemArray.count() || nextComplexItemIndex < surveyItems.count()); if (json.contains(_jsonPlannedHomePositionKey)) { - SimpleMissionItem* item = new SimpleMissionItem(vehicle, visualItems); + SimpleMissionItem* item = new SimpleMissionItem(_controllerVehicle, visualItems); if (item->load(json[_jsonPlannedHomePositionKey].toObject(), 0, errorString)) { - MissionSettingsItem* settingsItem = new MissionSettingsItem(vehicle, visualItems); + MissionSettingsItem* settingsItem = new MissionSettingsItem(_controllerVehicle, visualItems); settingsItem->setCoordinate(item->coordinate()); visualItems->insert(0, settingsItem); item->deleteLater(); @@ -430,13 +497,13 @@ bool MissionController::_loadJsonMissionFileV1(Vehicle* vehicle, const QJsonObje return false; } } else { - _addMissionSettings(vehicle, visualItems, true /* addToCenter */); + _addMissionSettings(visualItems, true /* addToCenter */); } return true; } -bool MissionController::_loadJsonMissionFileV2(Vehicle* vehicle, const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString) +bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString) { // Validate root object keys QList rootKeyInfoList = { @@ -454,13 +521,14 @@ bool MissionController::_loadJsonMissionFileV2(Vehicle* vehicle, const QJsonObje qCDebug(MissionControllerLog) << "MissionController::_loadJsonMissionFileV2 itemCount:" << json[_jsonItemsKey].toArray().count(); // Mission Settings - QGeoCoordinate homeCoordinate; AppSettings* appSettings = qgcApp()->toolbox()->settingsManager()->appSettings(); - if (!JsonHelper::loadGeoCoordinate(json[_jsonPlannedHomePositionKey], true /* altitudeRequired */, homeCoordinate, errorString)) { - return false; - } - if (json.contains(_jsonVehicleTypeKey) && vehicle->isOfflineEditingVehicle()) { - appSettings->offlineEditingVehicleType()->setRawValue(json[_jsonVehicleTypeKey].toDouble()); + + if (_masterController->offline()) { + // We only update if offline since if we are online we use the online vehicle settings + appSettings->offlineEditingFirmwareType()->setRawValue(AppSettings::offlineEditingFirmwareTypeFromFirmwareType((MAV_AUTOPILOT)json[_jsonFirmwareTypeKey].toInt())); + if (json.contains(_jsonVehicleTypeKey)) { + appSettings->offlineEditingVehicleType()->setRawValue(AppSettings::offlineEditingVehicleTypeFromVehicleType((MAV_TYPE)json[_jsonVehicleTypeKey].toInt())); + } } if (json.contains(_jsonCruiseSpeedKey)) { appSettings->offlineEditingCruiseSpeed()->setRawValue(json[_jsonCruiseSpeedKey].toDouble()); @@ -469,7 +537,11 @@ bool MissionController::_loadJsonMissionFileV2(Vehicle* vehicle, const QJsonObje appSettings->offlineEditingHoverSpeed()->setRawValue(json[_jsonHoverSpeedKey].toDouble()); } - MissionSettingsItem* settingsItem = new MissionSettingsItem(vehicle, visualItems); + QGeoCoordinate homeCoordinate; + if (!JsonHelper::loadGeoCoordinate(json[_jsonPlannedHomePositionKey], true /* altitudeRequired */, homeCoordinate, errorString)) { + return false; + } + MissionSettingsItem* settingsItem = new MissionSettingsItem(_controllerVehicle, visualItems); settingsItem->setCoordinate(homeCoordinate); visualItems->insert(0, settingsItem); qCDebug(MissionControllerLog) << "plannedHomePosition" << homeCoordinate; @@ -498,7 +570,7 @@ bool MissionController::_loadJsonMissionFileV2(Vehicle* vehicle, const QJsonObje QString itemType = itemObject[VisualMissionItem::jsonTypeKey].toString(); if (itemType == VisualMissionItem::jsonTypeSimpleItemValue) { - SimpleMissionItem* simpleItem = new SimpleMissionItem(vehicle, visualItems); + SimpleMissionItem* simpleItem = new SimpleMissionItem(_controllerVehicle, visualItems); if (simpleItem->load(itemObject, nextSequenceNumber, errorString)) { qCDebug(MissionControllerLog) << "Loading simple item: nextSequenceNumber:command" << nextSequenceNumber << simpleItem->command(); nextSequenceNumber = simpleItem->lastSequenceNumber() + 1; @@ -517,7 +589,7 @@ bool MissionController::_loadJsonMissionFileV2(Vehicle* vehicle, const QJsonObje if (complexItemType == SurveyMissionItem::jsonComplexItemTypeValue) { qCDebug(MissionControllerLog) << "Loading Survey: nextSequenceNumber" << nextSequenceNumber; - SurveyMissionItem* surveyItem = new SurveyMissionItem(vehicle, visualItems); + SurveyMissionItem* surveyItem = new SurveyMissionItem(_controllerVehicle, visualItems); if (!surveyItem->load(itemObject, nextSequenceNumber++, errorString)) { return false; } @@ -526,7 +598,7 @@ bool MissionController::_loadJsonMissionFileV2(Vehicle* vehicle, const QJsonObje visualItems->append(surveyItem); } else if (complexItemType == FixedWingLandingComplexItem::jsonComplexItemTypeValue) { qCDebug(MissionControllerLog) << "Loading Fixed Wing Landing Pattern: nextSequenceNumber" << nextSequenceNumber; - FixedWingLandingComplexItem* landingItem = new FixedWingLandingComplexItem(vehicle, visualItems); + FixedWingLandingComplexItem* landingItem = new FixedWingLandingComplexItem(_controllerVehicle, visualItems); if (!landingItem->load(itemObject, nextSequenceNumber++, errorString)) { return false; } @@ -535,7 +607,7 @@ bool MissionController::_loadJsonMissionFileV2(Vehicle* vehicle, const QJsonObje visualItems->append(landingItem); } else if (complexItemType == MissionSettingsItem::jsonComplexItemTypeValue) { qCDebug(MissionControllerLog) << "Loading Mission Settings: nextSequenceNumber" << nextSequenceNumber; - MissionSettingsItem* settingsItem = new MissionSettingsItem(vehicle, visualItems); + MissionSettingsItem* settingsItem = new MissionSettingsItem(_controllerVehicle, visualItems); if (!settingsItem->load(itemObject, nextSequenceNumber++, errorString)) { return false; } @@ -579,7 +651,6 @@ bool MissionController::_loadJsonMissionFileV2(Vehicle* vehicle, const QJsonObje return true; } -#if 0 bool MissionController::_loadItemsFromJson(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString) { // V1 file format has no file type key and version key is string. Convert to new format. @@ -588,26 +659,24 @@ bool MissionController::_loadItemsFromJson(const QJsonObject& json, QmlObjectLis } int fileVersion; - if (!JsonHelper::validateQGCJsonFile(json, - _jsonFileTypeValue, // expected file type - 1, // minimum supported version - 2, // maximum supported version - fileVersion, - errorString)) { - return false; - } + JsonHelper::validateQGCJsonFile(json, + _jsonFileTypeValue, // expected file type + 1, // minimum supported version + 2, // maximum supported version + fileVersion, + errorString); if (fileVersion == 1) { - return _loadJsonMissionFileV1(_activeVehicle, json, visualItems, errorString); + return _loadJsonMissionFileV1(json, visualItems, errorString); } else { - return _loadJsonMissionFileV2(_activeVehicle, json, visualItems, errorString); + return _loadJsonMissionFileV2(json, visualItems, errorString); } } -#endif -bool MissionController::_loadTextMissionFile(Vehicle* vehicle, QTextStream& stream, QmlObjectListModel* visualItems, QString& errorString) +bool MissionController::_loadTextMissionFile(QTextStream& stream, QmlObjectListModel* visualItems, QString& errorString) { - bool addPlannedHomePosition = false; + bool firstItem = true; + bool plannedHomePositionInFile = false; QString firstLine = stream.readLine(); const QStringList& version = firstLine.split(" "); @@ -617,19 +686,29 @@ bool MissionController::_loadTextMissionFile(Vehicle* vehicle, QTextStream& stre if (version[2] == "110") { // ArduPilot file, planned home position is already in position 0 versionOk = true; + plannedHomePositionInFile = true; } else if (version[2] == "120") { // Old QGC file, no planned home position versionOk = true; - addPlannedHomePosition = true; + plannedHomePositionInFile = false; } } if (versionOk) { + // Start with planned home in center + _addMissionSettings(visualItems, true /* addToCenter */); + MissionSettingsItem* settingsItem = visualItems->value(0); + while (!stream.atEnd()) { - SimpleMissionItem* item = new SimpleMissionItem(vehicle, visualItems); + SimpleMissionItem* item = new SimpleMissionItem(_controllerVehicle, visualItems); if (item->load(stream)) { - visualItems->append(item); + if (firstItem && plannedHomePositionInFile) { + settingsItem->setCoordinate(item->coordinate()); + } else { + visualItems->append(item); + } + firstItem = false; } else { errorString = QStringLiteral("The mission file is corrupted."); return false; @@ -640,9 +719,7 @@ bool MissionController::_loadTextMissionFile(Vehicle* vehicle, QTextStream& stre return false; } - if (addPlannedHomePosition || visualItems->count() == 0) { - _addMissionSettings(vehicle, visualItems, true /* addToCenter */); - + if (!plannedHomePositionInFile) { // 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)); @@ -655,38 +732,79 @@ bool MissionController::_loadTextMissionFile(Vehicle* vehicle, QTextStream& stre return true; } +void MissionController::_initLoadedVisualItems(QmlObjectListModel* loadedVisualItems) +{ + if (_visualItems) { + _deinitAllVisualItems(); + _visualItems->deleteLater(); + _settingsItem = NULL; + } + + _visualItems = loadedVisualItems; + + if (_visualItems->count() == 0) { + _addMissionSettings(_visualItems, true /* addToCenter */); + } + + MissionController::_scanForAdditionalSettings(_visualItems, _controllerVehicle); + + _initAllVisualItems(); +} + bool MissionController::load(const QJsonObject& json, QString& errorString) { QString errorStr; QString errorMessage = tr("Mission: %1"); - QmlObjectListModel* newVisualItems = new QmlObjectListModel(this); + QmlObjectListModel* loadedVisualItems = new QmlObjectListModel(this); - if (!_loadJsonMissionFileV2(_activeVehicle, json, newVisualItems, errorStr)) { + if (!_loadJsonMissionFileV2(json, loadedVisualItems, errorStr)) { errorString = errorMessage.arg(errorStr); return false; } + _initLoadedVisualItems(loadedVisualItems); - if (_visualItems) { - _deinitAllVisualItems(); - _visualItems->deleteLater(); - _settingsItem = NULL; - } + return true; +} - _visualItems = newVisualItems; +bool MissionController::loadJsonFile(QFile& file, QString& errorString) +{ + QString errorStr; + QString errorMessage = tr("Mission: %1"); + QJsonDocument jsonDoc; + QByteArray bytes = file.readAll(); - if (_visualItems->count() == 0) { - _addMissionSettings(_activeVehicle, _visualItems, true /* addToCenter */); + if (!JsonHelper::isJsonFile(bytes, jsonDoc, errorStr)) { + errorString = errorMessage.arg(errorStr); + return false; } - MissionController::_scanForAdditionalSettings(_visualItems, _activeVehicle); + QJsonObject json = jsonDoc.object(); + QmlObjectListModel* loadedVisualItems = new QmlObjectListModel(this); + if (!_loadItemsFromJson(json, loadedVisualItems, errorStr)) { + errorString = errorMessage.arg(errorStr); + return false; + } - _initAllVisualItems(); + _initLoadedVisualItems(loadedVisualItems); - if (!_activeVehicle->isOfflineEditingVehicle()) { - // Needs a sync to vehicle - setDirty(true); + return true; +} + +bool MissionController::loadTextFile(QFile& file, QString& errorString) +{ + QString errorStr; + QString errorMessage = tr("Mission: %1"); + QByteArray bytes = file.readAll(); + QTextStream stream(bytes); + + QmlObjectListModel* loadedVisualItems = new QmlObjectListModel(this); + if (!_loadTextMissionFile(stream, loadedVisualItems, errorStr)) { + errorString = errorMessage.arg(errorStr); + return false; } + _initLoadedVisualItems(loadedVisualItems); + return true; } @@ -704,10 +822,10 @@ void MissionController::save(QJsonObject& json) QJsonValue coordinateValue; JsonHelper::saveGeoCoordinate(settingsItem->coordinate(), true /* writeAltitude */, coordinateValue); json[_jsonPlannedHomePositionKey] = coordinateValue; - json[_jsonFirmwareTypeKey] = _activeVehicle->firmwareType(); - json[_jsonVehicleTypeKey] = _activeVehicle->vehicleType(); - json[_jsonCruiseSpeedKey] = _activeVehicle->defaultCruiseSpeed(); - json[_jsonHoverSpeedKey] = _activeVehicle->defaultHoverSpeed(); + json[_jsonFirmwareTypeKey] = _controllerVehicle->firmwareType(); + json[_jsonVehicleTypeKey] = _controllerVehicle->vehicleType(); + json[_jsonCruiseSpeedKey] = _controllerVehicle->defaultCruiseSpeed(); + json[_jsonHoverSpeedKey] = _controllerVehicle->defaultHoverSpeed(); // Save the visual items @@ -922,9 +1040,9 @@ void MissionController::_recalcMissionFlightStatus() // Look for speed changed double newSpeed = item->specifiedFlightSpeed(); if (!qIsNaN(newSpeed)) { - if (_activeVehicle->multiRotor()) { + if (_controllerVehicle->multiRotor()) { _missionFlightStatus.hoverSpeed = newSpeed; - } else if (_activeVehicle->vtol()) { + } else if (_controllerVehicle->vtol()) { if (vtolInHover) { _missionFlightStatus.hoverSpeed = newSpeed; } else { @@ -937,7 +1055,7 @@ void MissionController::_recalcMissionFlightStatus() } // Look for gimbal change - if (_activeVehicle->vehicleYawsToNextWaypointInMission()) { + if (_managerVehicle->vehicleYawsToNextWaypointInMission()) { // We current only support gimbal display in this mode double gimbalYaw = item->specifiedGimbalYaw(); if (!qIsNaN(gimbalYaw)) { @@ -958,7 +1076,7 @@ void MissionController::_recalcMissionFlightStatus() } // Update VTOL state - if (simpleItem && _activeVehicle->vtol()) { + if (simpleItem && _controllerVehicle->vtol()) { switch (simpleItem->command()) { case MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF: vtolInHover = false; @@ -1022,14 +1140,14 @@ void MissionController::_recalcMissionFlightStatus() // Calculate time/distance double hoverTime = distance / _missionFlightStatus.hoverSpeed; double cruiseTime = distance / _missionFlightStatus.cruiseSpeed; - if (_activeVehicle->vtol()) { + if (_controllerVehicle->vtol()) { if (vtolInHover) { _addHoverTime(hoverTime, distance, item->sequenceNumber()); } else { _addCruiseTime(cruiseTime, distance, item->sequenceNumber()); } } else { - if (_activeVehicle->multiRotor()) { + if (_controllerVehicle->multiRotor()) { _addHoverTime(hoverTime, distance, item->sequenceNumber()); } else { _addCruiseTime(cruiseTime, distance, item->sequenceNumber()); @@ -1044,14 +1162,14 @@ void MissionController::_recalcMissionFlightStatus() double hoverTime = distance / _missionFlightStatus.hoverSpeed; double cruiseTime = distance / _missionFlightStatus.cruiseSpeed; - if (_activeVehicle->vtol()) { + if (_controllerVehicle->vtol()) { if (vtolInHover) { _addHoverTime(hoverTime, distance, item->sequenceNumber()); } else { _addCruiseTime(cruiseTime, distance, item->sequenceNumber()); } } else { - if (_activeVehicle->multiRotor()) { + if (_controllerVehicle->multiRotor()) { _addHoverTime(hoverTime, distance, item->sequenceNumber()); } else { _addCruiseTime(cruiseTime, distance, item->sequenceNumber()); @@ -1175,8 +1293,8 @@ void MissionController::_initAllVisualItems(void) _settingsItem->setIsCurrentItem(true); } - if (!_editMode && _activeVehicle) { - _settingsItem->setCoordinate(_activeVehicle->homePosition()); + if (!_editMode && _controllerVehicle) { + _settingsItem->setCoordinate(_controllerVehicle->homePosition()); } connect(_settingsItem, &MissionSettingsItem::coordinateChanged, this, &MissionController::_recalcAll); @@ -1189,14 +1307,14 @@ void MissionController::_initAllVisualItems(void) _recalcAll(); - connect(_visualItems, &QmlObjectListModel::dirtyChanged, this, &MissionController::dirtyChanged); + connect(_visualItems, &QmlObjectListModel::dirtyChanged, this, &MissionController::_visualItemsDirtyChanged); connect(_visualItems, &QmlObjectListModel::countChanged, this, &MissionController::_updateContainsItems); emit visualItemsChanged(); emit containsItemsChanged(containsItems()); emit plannedHomePositionChanged(plannedHomePosition()); - _visualItems->setDirty(false); + setDirty(false); } void MissionController::_deinitAllVisualItems(void) @@ -1214,7 +1332,7 @@ void MissionController::_deinitAllVisualItems(void) void MissionController::_initVisualItem(VisualMissionItem* visualItem) { - _visualItems->setDirty(false); + setDirty(false); connect(visualItem, &VisualMissionItem::specifiesCoordinateChanged, this, &MissionController::_recalcWaypointLines); connect(visualItem, &VisualMissionItem::coordinateHasRelativeAltitudeChanged, this, &MissionController::_recalcWaypointLines); @@ -1253,61 +1371,44 @@ void MissionController::_itemCommandChanged(void) _recalcWaypointLines(); } -void MissionController::activeVehicleBeingRemoved(void) -{ - qCDebug(MissionControllerLog) << "MissionController::_activeVehicleBeingRemoved"; - - MissionManager* missionManager = _activeVehicle->missionManager(); - - disconnect(missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionController::_newMissionItemsAvailableFromVehicle); - disconnect(missionManager, &MissionManager::inProgressChanged, this, &MissionController::_inProgressChanged); - disconnect(missionManager, &MissionManager::currentIndexChanged, this, &MissionController::_currentMissionIndexChanged); - disconnect(missionManager, &MissionManager::lastCurrentIndexChanged, this, &MissionController::resumeMissionIndexChanged); - disconnect(missionManager, &MissionManager::resumeMissionReady, this, &MissionController::resumeMissionReady); - disconnect(missionManager, &MissionManager::cameraFeedback, this, &MissionController::_cameraFeedback); - disconnect(_activeVehicle, &Vehicle::homePositionChanged, this, &MissionController::_activeVehicleHomePositionChanged); - - // We always remove all items on vehicle change. This leaves a user model hole: - // If the user has unsaved changes in the Plan view they will lose them - removeAll(); - - _activeVehicle = NULL; -} - -void MissionController::activeVehicleSet(Vehicle* activeVehicle) +void MissionController::managerVehicleChanged(Vehicle* managerVehicle) { - _activeVehicle = activeVehicle; - - // We always remove all items on vehicle change. This leaves a user model hole: - // If the user has unsaved changes in the Plan view they will lose them - removeAll(); + if (_managerVehicle) { + _missionManager->disconnect(this); + _managerVehicle->disconnect(this); + _managerVehicle = NULL; + _missionManager = NULL; + } - MissionManager* missionManager = _activeVehicle->missionManager(); + _managerVehicle = managerVehicle; + if (!_managerVehicle) { + qWarning() << "MissionController::managerVehicleChanged managerVehicle=NULL"; + return; + } - connect(missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionController::_newMissionItemsAvailableFromVehicle); - connect(missionManager, &MissionManager::inProgressChanged, this, &MissionController::_inProgressChanged); - connect(missionManager, &MissionManager::currentIndexChanged, this, &MissionController::_currentMissionIndexChanged); - connect(missionManager, &MissionManager::lastCurrentIndexChanged, this, &MissionController::resumeMissionIndexChanged); - connect(missionManager, &MissionManager::resumeMissionReady, this, &MissionController::resumeMissionReady); - connect(missionManager, &MissionManager::cameraFeedback, this, &MissionController::_cameraFeedback); - connect(_activeVehicle, &Vehicle::homePositionChanged, this, &MissionController::_activeVehicleHomePositionChanged); - connect(_activeVehicle, &Vehicle::defaultCruiseSpeedChanged, this, &MissionController::_recalcMissionFlightStatus); - connect(_activeVehicle, &Vehicle::defaultHoverSpeedChanged, this, &MissionController::_recalcMissionFlightStatus); - connect(_activeVehicle, &Vehicle::vehicleTypeChanged, this, &MissionController::complexMissionItemNamesChanged); + _missionManager = _managerVehicle->missionManager(); + connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionController::_newMissionItemsAvailableFromVehicle); + connect(_missionManager, &MissionManager::sendComplete, this, &MissionController::_managerSendComplete); + connect(_missionManager, &MissionManager::removeAllComplete, this, &MissionController::_managerRemoveAllComplete); + connect(_missionManager, &MissionManager::inProgressChanged, this, &MissionController::_inProgressChanged); + connect(_missionManager, &MissionManager::progressPct, this, &MissionController::_progressPctChanged); + connect(_missionManager, &MissionManager::currentIndexChanged, this, &MissionController::_currentMissionIndexChanged); + connect(_missionManager, &MissionManager::lastCurrentIndexChanged, this, &MissionController::resumeMissionIndexChanged); + connect(_missionManager, &MissionManager::resumeMissionReady, this, &MissionController::resumeMissionReady); + connect(_managerVehicle, &Vehicle::homePositionChanged, this, &MissionController::_managerVehicleHomePositionChanged); + connect(_managerVehicle, &Vehicle::defaultCruiseSpeedChanged, this, &MissionController::_recalcMissionFlightStatus); + connect(_managerVehicle, &Vehicle::defaultHoverSpeedChanged, this, &MissionController::_recalcMissionFlightStatus); + connect(_managerVehicle, &Vehicle::vehicleTypeChanged, this, &MissionController::complexMissionItemNamesChanged); - if (_activeVehicle->parameterManager()->parametersReady() && !syncInProgress()) { - // We are switching between two previously existing vehicles. We have to manually ask for the items from the Vehicle. - // We don't request mission items for new vehicles since that will happen autamatically. - loadFromVehicle(); + if (!_masterController->offline()) { + _managerVehicleHomePositionChanged(_managerVehicle->homePosition()); } - _activeVehicleHomePositionChanged(_activeVehicle->homePosition()); - emit complexMissionItemNamesChanged(); emit resumeMissionIndexChanged(); } -void MissionController::_activeVehicleHomePositionChanged(const QGeoCoordinate& homePosition) +void MissionController::_managerVehicleHomePositionChanged(const QGeoCoordinate& homePosition) { if (_visualItems) { MissionSettingsItem* settingsItem = qobject_cast(_visualItems->get(0)); @@ -1316,6 +1417,10 @@ void MissionController::_activeVehicleHomePositionChanged(const QGeoCoordinate& } else { qWarning() << "First item is not MissionSettingsItem"; } + if (_visualItems->count() == 1) { + // Don't let this trip the dirty bit + _visualItems->setDirty(false); + } } } @@ -1372,9 +1477,9 @@ double MissionController::_normalizeLon(double lon) } /// Add the Mission Settings complex item to the front of the items -void MissionController::_addMissionSettings(Vehicle* vehicle, QmlObjectListModel* visualItems, bool addToCenter) +void MissionController::_addMissionSettings(QmlObjectListModel* visualItems, bool addToCenter) { - MissionSettingsItem* settingsItem = new MissionSettingsItem(vehicle, visualItems); + MissionSettingsItem* settingsItem = new MissionSettingsItem(_controllerVehicle, visualItems); visualItems->insert(0, settingsItem); @@ -1411,7 +1516,7 @@ void MissionController::_addMissionSettings(Vehicle* vehicle, QmlObjectListModel } } } else { - settingsItem->setCoordinate(vehicle->homePosition()); + settingsItem->setCoordinate(_controllerVehicle->homePosition()); } } @@ -1421,7 +1526,7 @@ int MissionController::resumeMissionIndex(void) const int resumeIndex = 0; if (!_editMode) { - resumeIndex = _activeVehicle->missionManager()->lastCurrentIndex() + (_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle() ? 0 : 1); + 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 resumeIndex--; @@ -1430,14 +1535,26 @@ int MissionController::resumeMissionIndex(void) const } } - qDebug() << "resumeIndex" << resumeIndex; return resumeIndex; } +int MissionController::currentMissionIndex(void) const +{ + if (_editMode) { + return -1; + } else { + int currentIndex = _missionManager->currentIndex(); + if (!_controllerVehicle->firmwarePlugin()->sendHomePositionToVehicle()) { + currentIndex++; + } + return currentIndex; + } +} + void MissionController::_currentMissionIndexChanged(int sequenceNumber) { if (!_editMode) { - if (!_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle()) { + if (!_controllerVehicle->firmwarePlugin()->sendHomePositionToVehicle()) { sequenceNumber++; } @@ -1445,12 +1562,13 @@ void MissionController::_currentMissionIndexChanged(int sequenceNumber) VisualMissionItem* item = qobject_cast(_visualItems->get(i)); item->setIsCurrentItem(item->sequenceNumber() == sequenceNumber); } + emit currentMissionIndexChanged(currentMissionIndex()); } } bool MissionController::syncInProgress(void) const { - return _activeVehicle ? _activeVehicle->missionManager()->inProgress() : false; + return _missionManager->inProgress(); } bool MissionController::dirty(void) const @@ -1507,8 +1625,14 @@ bool MissionController::containsItems(void) const void MissionController::removeAllFromVehicle(void) { - _missionItemsRequested = true; - _activeVehicle->missionManager()->removeAll(); + if (_masterController->offline()) { + qCWarning(MissionControllerLog) << "MissionControllerLog::removeAllFromVehicle called while offline"; + } else if (syncInProgress()) { + qCWarning(MissionControllerLog) << "MissionControllerLog::removeAllFromVehicle called while syncInProgress"; + } else { + _itemsRequested = true; + _missionManager->removeAll(); + } } QStringList MissionController::complexMissionItemNames(void) const @@ -1516,7 +1640,7 @@ QStringList MissionController::complexMissionItemNames(void) const QStringList complexItems; complexItems.append(_surveyMissionItemName); - if (_activeVehicle->fixedWing()) { + if (_controllerVehicle->fixedWing()) { complexItems.append(_fwLandingMissionItemName); } @@ -1525,10 +1649,10 @@ QStringList MissionController::complexMissionItemNames(void) const void MissionController::resumeMission(int resumeIndex) { - if (!_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle()) { + if (!_controllerVehicle->firmwarePlugin()->sendHomePositionToVehicle()) { resumeIndex--; } - _activeVehicle->missionManager()->generateResumeMission(resumeIndex); + _missionManager->generateResumeMission(resumeIndex); } QGeoCoordinate MissionController::plannedHomePosition(void) const @@ -1550,15 +1674,57 @@ void MissionController::applyDefaultMissionAltitude(void) } } -void MissionController::_cameraFeedback(QGeoCoordinate imageCoordinate, int index) +void MissionController::_progressPctChanged(double progressPct) { - Q_UNUSED(index); - if (!_editMode) { - _cameraPoints.append(new QGCQGeoCoordinate(imageCoordinate, this)); + if (!qFuzzyCompare(progressPct, _progressPct)) { + _progressPct = progressPct; + emit progressPctChanged(progressPct); } } -void MissionController::clearCameraPoints(void) +void MissionController::_visualItemsDirtyChanged(bool dirty) { - _cameraPoints.clearAndDeleteContents(); + // We could connect signal to signal and not need this but this is handy for setting a breakpoint on + emit dirtyChanged(dirty); +} + +bool MissionController::showPlanFromManagerVehicle (void) +{ + qCDebug(MissionControllerLog) << "showPlanFromManagerVehicle" << _editMode; + if (_masterController->offline()) { + qCWarning(MissionControllerLog) << "MissionController::showPlanFromManagerVehicle called while offline"; + return true; // stops further propagation of showPlanFromManagerVehicle due to error + } else { + if (!_managerVehicle->initialPlanRequestComplete()) { + // The vehicle hasn't completed initial load, we can just wait for newMissionItemsAvailable to be signalled automatically + qCDebug(MissionControllerLog) << "showPlanFromManagerVehicle: !initialPlanRequestComplete, wait for signal"; + return true; + } else if (syncInProgress()) { + // If the sync is already in progress, newMissionItemsAvailable will be signalled automatically when it is done. So no need to do anything. + qCDebug(MissionControllerLog) << "showPlanFromManagerVehicle: syncInProgress wait for signal"; + return true; + } else { + // Fake a _newMissionItemsAvailable with the current items + qCDebug(MissionControllerLog) << "showPlanFromManagerVehicle: sync complete simulate signal"; + _itemsRequested = true; + _newMissionItemsAvailableFromVehicle(false /* removeAllRequested */); + return false; + } + } +} + +void MissionController::_managerSendComplete(bool error) +{ + // Fly view always reloads on send complete + if (!error && !_editMode) { + showPlanFromManagerVehicle(); + } +} + +void MissionController::_managerRemoveAllComplete(bool error) +{ + if (!error) { + // Remove all from vehicle so we always update + showPlanFromManagerVehicle(); + } } diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index 4e734574050df61b5bb191de8e6d5264dcfb705b..b53a9de865c9d23a75edfb0c959b0910c3d2662c 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -24,17 +24,19 @@ class VisualMissionItem; class MissionItem; class MissionSettingsItem; class AppSettings; +class MissionManager; Q_DECLARE_LOGGING_CATEGORY(MissionControllerLog) typedef QPair VisualItemPair; typedef QHash CoordVectHashTable; + class MissionController : public PlanElementController { Q_OBJECT public: - MissionController(QObject* parent = NULL); + MissionController(PlanMasterController* masterController, QObject* parent = NULL); ~MissionController(); typedef struct { @@ -62,11 +64,13 @@ public: Q_PROPERTY(QmlObjectListModel* visualItems READ visualItems NOTIFY visualItemsChanged) Q_PROPERTY(QmlObjectListModel* waypointLines READ waypointLines NOTIFY waypointLinesChanged) - Q_PROPERTY(QmlObjectListModel* cameraPoints READ cameraPoints CONSTANT) Q_PROPERTY(QStringList complexMissionItemNames READ complexMissionItemNames NOTIFY complexMissionItemNamesChanged) Q_PROPERTY(QGeoCoordinate plannedHomePosition READ plannedHomePosition NOTIFY plannedHomePositionChanged) - Q_PROPERTY(int resumeMissionIndex READ resumeMissionIndex NOTIFY resumeMissionIndexChanged) + Q_PROPERTY(double progressPct READ progressPct NOTIFY progressPctChanged) + + Q_PROPERTY(int currentMissionIndex READ currentMissionIndex NOTIFY currentMissionIndexChanged) + Q_PROPERTY(int resumeMissionIndex READ resumeMissionIndex NOTIFY resumeMissionIndexChanged) ///< Returns the item index two which a mission should be resumed. -1 indicates resume mission not available. Q_PROPERTY(double missionDistance READ missionDistance NOTIFY missionDistanceChanged) Q_PROPERTY(double missionTime READ missionTime NOTIFY missionTimeChanged) @@ -101,36 +105,33 @@ public: /// Sends the mission items to the specified vehicle static void sendItemsToVehicle(Vehicle* vehicle, QmlObjectListModel* visualMissionItems); - Q_INVOKABLE void clearCameraPoints(void); + bool loadJsonFile(QFile& file, QString& errorString); + bool loadTextFile(QFile& file, QString& errorString); // Overrides from PlanElementController void start (bool editMode) final; - void startStaticActiveVehicle (Vehicle* vehicle) final; void save (QJsonObject& json) final; bool load (const QJsonObject& json, QString& errorString) final; void loadFromVehicle (void) final; void sendToVehicle (void) final; -#if 0 - void loadFromFile (const QString& filename) final; -#endif void removeAll (void) final; void removeAllFromVehicle (void) final; bool syncInProgress (void) const final; bool dirty (void) const final; void setDirty (bool dirty) final; bool containsItems (void) const final; - void activeVehicleBeingRemoved (void) final; - void activeVehicleSet (Vehicle* vehicle) final; + void managerVehicleChanged (Vehicle* managerVehicle) final; + bool showPlanFromManagerVehicle (void) final; // Property accessors QmlObjectListModel* visualItems (void) { return _visualItems; } QmlObjectListModel* waypointLines (void) { return &_waypointLines; } - QmlObjectListModel* cameraPoints (void) { return &_cameraPoints; } QStringList complexMissionItemNames (void) const; QGeoCoordinate plannedHomePosition (void) const; + double progressPct (void) const { return _progressPct; } - /// Returns the item index two which a mission should be resumed. -1 indicates resume mission not available. + int currentMissionIndex(void) const; int resumeMissionIndex(void) const; double missionDistance (void) const { return _missionFlightStatus.totalDistance; } @@ -161,17 +162,22 @@ signals: void batteryChangePointChanged(int batteryChangePoint); void batteriesRequiredChanged(int batteriesRequired); void plannedHomePositionChanged(QGeoCoordinate plannedHomePosition); + void progressPctChanged(double progressPct); + void currentMissionIndexChanged(int currentMissionIndex); private slots: void _newMissionItemsAvailableFromVehicle(bool removeAllRequested); void _itemCommandChanged(void); - void _activeVehicleHomePositionChanged(const QGeoCoordinate& homePosition); + void _managerVehicleHomePositionChanged(const QGeoCoordinate& homePosition); void _inProgressChanged(bool inProgress); void _currentMissionIndexChanged(int sequenceNumber); void _recalcWaypointLines(void); void _recalcMissionFlightStatus(void); void _updateContainsItems(void); - void _cameraFeedback(QGeoCoordinate imageCoordinate, int index); + void _progressPctChanged(double progressPct); + void _visualItemsDirtyChanged(bool dirty); + void _managerSendComplete(bool error); + void _managerRemoveAllComplete(bool error); private: void _init(void); @@ -188,11 +194,11 @@ private: bool _findPreviousAltitude(int newIndex, double* prevAltitude, MAV_FRAME* prevFrame); static double _normalizeLat(double lat); static double _normalizeLon(double lon); - static void _addMissionSettings(Vehicle* vehicle, QmlObjectListModel* visualItems, bool addToCenter); - static bool _loadJsonMissionFile(Vehicle* vehicle, const QByteArray& bytes, QmlObjectListModel* visualItems, QString& errorString); - static bool _loadJsonMissionFileV1(Vehicle* vehicle, const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString); - static bool _loadJsonMissionFileV2(Vehicle* vehicle, const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString); - static bool _loadTextMissionFile(Vehicle* vehicle, QTextStream& stream, QmlObjectListModel* visualItems, QString& errorString); + void _addMissionSettings(QmlObjectListModel* visualItems, bool addToCenter); + bool _loadJsonMissionFile(const QByteArray& bytes, QmlObjectListModel* visualItems, QString& errorString); + bool _loadJsonMissionFileV1(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString); + bool _loadJsonMissionFileV2(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString); + bool _loadTextMissionFile(QTextStream& stream, QmlObjectListModel* visualItems, QString& errorString); int _nextSequenceNumber(void); static void _scanForAdditionalSettings(QmlObjectListModel* visualItems, Vehicle* vehicle); static bool _convertToMissionItems(QmlObjectListModel* visualMissionItems, QList& rgMissionItems, QObject* missionItemParent); @@ -202,19 +208,21 @@ private: void _addCruiseTime(double cruiseTime, double cruiseDistance, int wayPointIndex); void _updateBatteryInfo(int waypointIndex); bool _loadItemsFromJson(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString); + void _initLoadedVisualItems(QmlObjectListModel* loadedVisualItems); private: + MissionManager* _missionManager; QmlObjectListModel* _visualItems; MissionSettingsItem* _settingsItem; QmlObjectListModel _waypointLines; - QmlObjectListModel _cameraPoints; CoordVectHashTable _linesTable; bool _firstItemsFromVehicle; - bool _missionItemsRequested; + bool _itemsRequested; MissionFlightStatus_t _missionFlightStatus; QString _surveyMissionItemName; QString _fwLandingMissionItemName; AppSettings* _appSettings; + double _progressPct; static const char* _settingsGroup; diff --git a/src/MissionManager/MissionControllerManagerTest.cc b/src/MissionManager/MissionControllerManagerTest.cc index ad0b719c8c0dd6cdb694692035749237dbdd0d26..22669624f64975d06918a33bde72759aeb127863 100644 --- a/src/MissionManager/MissionControllerManagerTest.cc +++ b/src/MissionManager/MissionControllerManagerTest.cc @@ -35,9 +35,10 @@ void MissionControllerManagerTest::_initForFirmwareType(MAV_AUTOPILOT firmwareTy _missionManager = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->missionManager(); QVERIFY(_missionManager); - _rgMissionManagerSignals[newMissionItemsAvailableSignalIndex] = SIGNAL(newMissionItemsAvailable(bool)); - _rgMissionManagerSignals[inProgressChangedSignalIndex] = SIGNAL(inProgressChanged(bool)); - _rgMissionManagerSignals[errorSignalIndex] = SIGNAL(error(int, const QString&)); + _rgMissionManagerSignals[newMissionItemsAvailableSignalIndex] = SIGNAL(newMissionItemsAvailable(bool)); + _rgMissionManagerSignals[sendCompleteSignalIndex] = SIGNAL(sendComplete(bool)); + _rgMissionManagerSignals[inProgressChangedSignalIndex] = SIGNAL(inProgressChanged(bool)); + _rgMissionManagerSignals[errorSignalIndex] = SIGNAL(error(int, const QString&)); _multiSpyMissionManager = new MultiSignalSpy(); Q_CHECK_PTR(_multiSpyMissionManager); diff --git a/src/MissionManager/MissionControllerManagerTest.h b/src/MissionManager/MissionControllerManagerTest.h index 1962805a9274788387dc3892665475b66a6aa13a..1d4ccaf5daad8e8bfe8d054049302419b33b1aa1 100644 --- a/src/MissionManager/MissionControllerManagerTest.h +++ b/src/MissionManager/MissionControllerManagerTest.h @@ -55,6 +55,7 @@ protected: typedef enum { newMissionItemsAvailableSignalIndex = 0, + sendCompleteSignalIndex, inProgressChangedSignalIndex, errorSignalIndex, maxSignalIndex @@ -62,6 +63,7 @@ protected: typedef enum { newMissionItemsAvailableSignalMask = 1 << newMissionItemsAvailableSignalIndex, + sendCompleteSignalMask = 1 << sendCompleteSignalIndex, inProgressChangedSignalMask = 1 << inProgressChangedSignalIndex, errorSignalMask = 1 << errorSignalIndex, } MissionManagerSignalMask_t; diff --git a/src/MissionManager/MissionControllerTest.cc b/src/MissionManager/MissionControllerTest.cc index d417708c1979df14ed3fdfd2f100621c4b2ce4b0..13f91037bff9a92dcde94c7cc2b9aac804d735cc 100644 --- a/src/MissionManager/MissionControllerTest.cc +++ b/src/MissionManager/MissionControllerTest.cc @@ -13,6 +13,9 @@ #include "MultiVehicleManager.h" #include "SimpleMissionItem.h" #include "MissionSettingsItem.h" +#include "QGCApplication.h" +#include "SettingsManager.h" +#include "AppSettings.h" MissionControllerTest::MissionControllerTest(void) : _multiSpyMissionController(NULL) @@ -24,8 +27,8 @@ MissionControllerTest::MissionControllerTest(void) void MissionControllerTest::cleanup(void) { - delete _missionController; - _missionController = NULL; + delete _masterController; + _masterController = NULL; delete _multiSpyMissionController; _multiSpyMissionController = NULL; @@ -38,8 +41,6 @@ void MissionControllerTest::cleanup(void) void MissionControllerTest::_initForFirmwareType(MAV_AUTOPILOT firmwareType) { - bool startController = false; - MissionControllerManagerTest::_initForFirmwareType(firmwareType); // VisualMissionItem signals @@ -49,19 +50,16 @@ void MissionControllerTest::_initForFirmwareType(MAV_AUTOPILOT firmwareType) _rgMissionControllerSignals[visualItemsChangedSignalIndex] = SIGNAL(visualItemsChanged()); _rgMissionControllerSignals[waypointLinesChangedSignalIndex] = SIGNAL(waypointLinesChanged()); - if (!_missionController) { - startController = true; - _missionController = new MissionController(); - Q_CHECK_PTR(_missionController); - } + // Master controller pulls offline vehicle info from settings + qgcApp()->toolbox()->settingsManager()->appSettings()->offlineEditingFirmwareType()->setRawValue(firmwareType); + _masterController = new PlanMasterController(this); + _missionController = _masterController->missionController(); _multiSpyMissionController = new MultiSignalSpy(); Q_CHECK_PTR(_multiSpyMissionController); QCOMPARE(_multiSpyMissionController->init(_missionController, _rgMissionControllerSignals, _cMissionControllerSignals), true); - if (startController) { - _missionController->start(true /* editMode */); - } + _masterController->start(true /* editMode */); // All signals should some through on start QCOMPARE(_multiSpyMissionController->checkOnlySignalsByMask(visualItemsChangedSignalMask | waypointLinesChangedSignalMask), true); @@ -162,6 +160,7 @@ void MissionControllerTest::_testAddWayppointPX4(void) _testAddWaypointWorker(MAV_AUTOPILOT_PX4); } +#if 0 void MissionControllerTest::_testOfflineToOnlineWorker(MAV_AUTOPILOT firmwareType) { // Start offline and add item @@ -191,6 +190,7 @@ void MissionControllerTest::_testOfflineToOnlinePX4(void) { _testOfflineToOnlineWorker(MAV_AUTOPILOT_PX4); } +#endif void MissionControllerTest::_setupVisualItemSignals(VisualMissionItem* visualItem) { diff --git a/src/MissionManager/MissionControllerTest.h b/src/MissionManager/MissionControllerTest.h index 2cc2559ad118c2504a00a385ea7436de29bd8c85..b92d9d9285df6c86acca26e6fb4effc2e7ab6091 100644 --- a/src/MissionManager/MissionControllerTest.h +++ b/src/MissionManager/MissionControllerTest.h @@ -16,6 +16,7 @@ #include "MissionManager.h" #include "MultiSignalSpy.h" #include "MissionControllerManagerTest.h" +#include "PlanMasterController.h" #include "MissionController.h" #include "SimpleMissionItem.h" @@ -38,14 +39,18 @@ private: void _testEmptyVehiclePX4(void); void _testAddWayppointAPM(void); void _testAddWayppointPX4(void); +#if 0 void _testOfflineToOnlineAPM(void); void _testOfflineToOnlinePX4(void); +#endif private: void _initForFirmwareType(MAV_AUTOPILOT firmwareType); void _testEmptyVehicleWorker(MAV_AUTOPILOT firmwareType); void _testAddWaypointWorker(MAV_AUTOPILOT firmwareType); +#if 0 void _testOfflineToOnlineWorker(MAV_AUTOPILOT firmwareType); +#endif void _setupVisualItemSignals(VisualMissionItem* visualItem); // MissiomItems signals @@ -81,7 +86,8 @@ private: static const size_t _cVisualItemSignals = visualItemMaxSignalIndex; const char* _rgVisualItemSignals[_cVisualItemSignals]; - MissionController* _missionController; + PlanMasterController* _masterController; + MissionController* _missionController; }; #endif diff --git a/src/MissionManager/MissionItem.cc b/src/MissionManager/MissionItem.cc index 80f9effb6263bb291b28bf5ae4b7943d7253b85b..c5bc12c030490bf4bb2d6f2c4708ed4a04a26891 100644 --- a/src/MissionManager/MissionItem.cc +++ b/src/MissionManager/MissionItem.cc @@ -260,6 +260,13 @@ bool MissionItem::load(const QJsonObject& json, int sequenceNumber, QString& err return false; } + for (int i=0; i<4; i++) { + if (rgParams[i].type() != QJsonValue::Double && rgParams[i].type() != QJsonValue::Null) { + errorString = tr("Param %1 incorrect type %2, must be double or null").arg(i+1).arg(rgParams[i].type()); + return false; + } + } + // Make sure to set these first since they can signal other changes setFrame((MAV_FRAME)v2Json[_jsonFrameKey].toInt()); setCommand((MAV_CMD)v2Json[_jsonCommandKey].toInt()); @@ -280,10 +287,10 @@ bool MissionItem::load(const QJsonObject& json, int sequenceNumber, QString& err setSequenceNumber(sequenceNumber); setAutoContinue(v2Json[_jsonAutoContinueKey].toBool()); - setParam1(rgParams[0].toDouble()); - setParam2(rgParams[1].toDouble()); - setParam3(rgParams[2].toDouble()); - setParam4(rgParams[3].toDouble()); + setParam1(JsonHelper::possibleNaNJsonValue(rgParams[0])); + setParam2(JsonHelper::possibleNaNJsonValue(rgParams[1])); + setParam3(JsonHelper::possibleNaNJsonValue(rgParams[2])); + setParam4(JsonHelper::possibleNaNJsonValue(rgParams[3])); return true; } diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index 358b1c7d2a0479308d413359723d990339838eeb..600bae0bcb3b7317df77d2d2cb4ae7d2b73fcc1f 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -16,6 +16,8 @@ #include "FirmwarePlugin.h" #include "MAVLinkProtocol.h" #include "QGCApplication.h" +#include "MissionCommandTree.h" +#include "MissionCommandUIInfo.h" QGC_LOGGING_CATEGORY(MissionManagerLog, "MissionManagerLog") @@ -48,12 +50,12 @@ void MissionManager::_writeMissionItemsWorker(void) { _lastMissionRequest = -1; - emit newMissionItemsAvailable(_missionItems.count() == 0); + emit progressPct(0); - qCDebug(MissionManagerLog) << "writeMissionItems count:" << _missionItems.count(); + qCDebug(MissionManagerLog) << "writeMissionItems count:" << _writeMissionItems.count(); // Prime write list - for (int i=0; i<_missionItems.count(); i++) { + for (int i=0; i<_writeMissionItems.count(); i++) { _itemIndicesToWrite << i; } @@ -80,15 +82,15 @@ void MissionManager::writeMissionItems(const QList& missionItems) return; } - bool skipFirstItem = !_vehicle->firmwarePlugin()->sendHomePositionToVehicle(); + _clearAndDeleteWriteMissionItems(); - _clearAndDeleteMissionItems(); + bool skipFirstItem = !_vehicle->firmwarePlugin()->sendHomePositionToVehicle(); int firstIndex = skipFirstItem ? 1 : 0; for (int i=firstIndex; isetIsCurrentItem(i == firstIndex); @@ -107,7 +109,7 @@ void MissionManager::writeMissionItems(const QList& missionItems) /// This begins the write sequence with the vehicle. This may be called during a retry. void MissionManager::_writeMissionCount(void) { - qCDebug(MissionManagerLog) << "_writeMissionCount count:_retryCount" << _missionItems.count() << _retryCount; + qCDebug(MissionManagerLog) << "_writeMissionCount count:_retryCount" << _writeMissionItems.count() << _retryCount; mavlink_message_t message; mavlink_mission_count_t missionCount; @@ -115,7 +117,7 @@ void MissionManager::_writeMissionCount(void) memset(&missionCount, 0, sizeof(missionCount)); missionCount.target_system = _vehicle->id(); missionCount.target_component = MAV_COMP_ID_MISSIONPLANNER; - missionCount.count = _missionItems.count(); + missionCount.count = _writeMissionItems.count(); _dedicatedLink = _vehicle->priorityLink(); mavlink_msg_mission_count_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), @@ -168,16 +170,16 @@ void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoC emit inProgressChanged(true); } -void MissionManager::requestMissionItems(void) +void MissionManager::loadFromVehicle(void) { if (_vehicle->isOfflineEditingVehicle()) { return; } - qCDebug(MissionManagerLog) << "requestMissionItems read sequence"; + qCDebug(MissionManagerLog) << "loadFromVehicle read sequence"; if (inProgress()) { - qCDebug(MissionManagerLog) << "requestMissionItems called while transaction in progress"; + qCDebug(MissionManagerLog) << "loadFromVehicle called while transaction in progress"; return; } @@ -339,7 +341,6 @@ void MissionManager::_readTransactionComplete(void) _vehicle->sendMessageOnLink(_dedicatedLink, message); _finishTransaction(true); - emit newMissionItemsAvailable(false); } void MissionManager::_handleMissionCount(const mavlink_message_t& message) @@ -410,11 +411,6 @@ void MissionManager::_requestNextMissionItem(void) void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool missionItemInt) { - - if (!_checkForExpectedAck(AckMissionItem)) { - return; - } - MAV_CMD command; MAV_FRAME frame; double param1; @@ -469,7 +465,24 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool m frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; } - qCDebug(MissionManagerLog) << "_handleMissionItem seq:command:current" << seq << command << isCurrentItem; + + bool ardupilotHomePositionUpdate = false; + if (!_checkForExpectedAck(AckMissionItem)) { + if (_vehicle->apmFirmware() && seq == 0) { + ardupilotHomePositionUpdate = true; + } else { + qCDebug(MissionManagerLog) << "_handleMissionItem dropping spurious item seq:command:current" << seq << command << isCurrentItem; + return; + } + } + + qCDebug(MissionManagerLog) << "_handleMissionItem seq:command:current:ardupilotHomePositionUpdate" << seq << command << isCurrentItem << ardupilotHomePositionUpdate; + + if (ardupilotHomePositionUpdate) { + QGeoCoordinate newHomePosition(param5, param6, param7); + _vehicle->_setHomePosition(newHomePosition); + return; + } if (_itemIndicesToRead.contains(seq)) { _itemIndicesToRead.removeOne(seq); @@ -500,6 +513,8 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool m _startAckTimeout(AckMissionItem); return; } + + emit progressPct((double)seq / (double)_missionItems.count()); _retryCount = 0; if (_itemIndicesToRead.count() == 0) { @@ -526,12 +541,14 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message, boo mavlink_msg_mission_request_decode(&message, &missionRequest); qCDebug(MissionManagerLog) << "_handleMissionRequest sequenceNumber" << missionRequest.seq; - if (missionRequest.seq > _missionItems.count() - 1) { - _sendError(RequestRangeError, QString("Vehicle requested item outside range, count:request %1:%2. Send to Vehicle failed.").arg(_missionItems.count()).arg(missionRequest.seq)); + if (missionRequest.seq > _writeMissionItems.count() - 1) { + _sendError(RequestRangeError, QString("Vehicle requested item outside range, count:request %1:%2. Send to Vehicle failed.").arg(_writeMissionItems.count()).arg(missionRequest.seq)); _finishTransaction(false); return; } + emit progressPct((double)missionRequest.seq / (double)_writeMissionItems.count()); + _lastMissionRequest = missionRequest.seq; if (!_itemIndicesToWrite.contains(missionRequest.seq)) { qCDebug(MissionManagerLog) << "_handleMissionRequest sequence number requested which has already been sent, sending again:" << missionRequest.seq; @@ -539,7 +556,7 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message, boo _itemIndicesToWrite.removeOne(missionRequest.seq); } - MissionItem* item = _missionItems[missionRequest.seq]; + MissionItem* item = _writeMissionItems[missionRequest.seq]; qCDebug(MissionManagerLog) << "_handleMissionRequest sequenceNumber:command" << missionRequest.seq << item->command(); mavlink_message_t messageOut; @@ -638,11 +655,11 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message) qCDebug(MissionManagerLog) << "_handleMissionAck write sequence complete"; _finishTransaction(true); } else { - _sendError(MissingRequestsError, QString("Vehicle did not request all items during write sequence, missed count %1. Vehicle only has partial list of mission items.").arg(_itemIndicesToWrite.count())); + _sendError(MissingRequestsError, QString("Vehicle did not request all items during write sequence, missed count %1.").arg(_itemIndicesToWrite.count())); _finishTransaction(false); } } else { - _sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle only has partial list of mission items.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); + _sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); _finishTransaction(false); } break; @@ -665,31 +682,6 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message) break; } } - -void MissionManager::_handleCameraFeedback(const mavlink_message_t& message) -{ - mavlink_camera_feedback_t feedback; - - mavlink_msg_camera_feedback_decode(&message, &feedback); - - QGeoCoordinate imageCoordinate((double)feedback.lat / qPow(10.0, 7.0), (double)feedback.lng / qPow(10.0, 7.0), feedback.alt_msl); - qCDebug(MissionManagerLog) << "_handleCameraFeedback coord:index" << imageCoordinate << feedback.img_idx; - emit cameraFeedback(imageCoordinate, feedback.img_idx); -} - -void MissionManager::_handleCameraImageCaptured(const mavlink_message_t& message) -{ - mavlink_camera_image_captured_t feedback; - - mavlink_msg_camera_image_captured_decode(&message, &feedback); - - QGeoCoordinate imageCoordinate((double)feedback.lat / qPow(10.0, 7.0), (double)feedback.lon / qPow(10.0, 7.0), feedback.alt); - qCDebug(MissionManagerLog) << "_handleCameraFeedback coord:index" << imageCoordinate << feedback.image_index << feedback.capture_result; - if (feedback.capture_result == 1) { - emit cameraFeedback(imageCoordinate, feedback.image_index); - } -} - /// Called when a new mavlink message for out vehicle is received void MissionManager::_mavlinkMessageReceived(const mavlink_message_t& message) { @@ -725,14 +717,6 @@ void MissionManager::_mavlinkMessageReceived(const mavlink_message_t& message) case MAVLINK_MSG_ID_MISSION_CURRENT: _handleMissionCurrent(message); break; - - case MAVLINK_MSG_ID_CAMERA_FEEDBACK: - _handleCameraFeedback(message); - break; - - case MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED: - _handleCameraImageCaptured(message); - break; } } @@ -764,14 +748,23 @@ QString MissionManager::_ackTypeToString(AckType_t ackType) QString MissionManager::_lastMissionReqestString(MAV_MISSION_RESULT result) { - if (_lastMissionRequest != -1 && _lastMissionRequest >= 0 && _lastMissionRequest < _missionItems.count()) { - MissionItem* item = _missionItems[_lastMissionRequest]; + if (_lastMissionRequest != -1 && _lastMissionRequest >= 0 && _lastMissionRequest < _writeMissionItems.count()) { + MissionItem* item = _writeMissionItems[_lastMissionRequest]; switch (result) { case MAV_MISSION_UNSUPPORTED_FRAME: return QString(". Frame: %1").arg(item->frame()); case MAV_MISSION_UNSUPPORTED: - return QString(". Command: %1").arg(item->command()); + { + const MissionCommandUIInfo* uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_vehicle, item->command()); + QString friendlyName; + QString rawName; + if (uiInfo) { + friendlyName = uiInfo->friendlyName(); + rawName = uiInfo->rawName(); + } + return QString(". Command: (%1, %2, %3)").arg(friendlyName).arg(rawName).arg(item->command()); + } case MAV_MISSION_INVALID_PARAM1: return QString(". Param1: %1").arg(item->param1()); case MAV_MISSION_INVALID_PARAM2: @@ -857,20 +850,49 @@ QString MissionManager::_missionResultToString(MAV_MISSION_RESULT result) void MissionManager::_finishTransaction(bool success) { - if (!success && _transactionInProgress == TransactionRead) { - // Read from vehicle failed, clear partial list - _clearAndDeleteMissionItems(); - emit newMissionItemsAvailable(false); - } + emit progressPct(1); _itemIndicesToRead.clear(); _itemIndicesToWrite.clear(); - if (_transactionInProgress != TransactionNone) { + // First thing we do is clear the transaction. This way inProgesss is off when we signal transaction complete. + TransactionType_t currentTransactionType = _transactionInProgress; + _transactionInProgress = TransactionNone; + if (currentTransactionType != TransactionNone) { _transactionInProgress = TransactionNone; + qDebug() << "inProgressChanged"; emit inProgressChanged(false); } + switch (currentTransactionType) { + case TransactionRead: + if (!success) { + // Read from vehicle failed, clear partial list + _clearAndDeleteMissionItems(); + } + emit newMissionItemsAvailable(false); + break; + case TransactionWrite: + if (success) { + // Write succeeded, update internal list to be current + _clearAndDeleteMissionItems(); + for (int i=0; i<_writeMissionItems.count(); i++) { + _missionItems.append(_writeMissionItems[i]); + } + _writeMissionItems.clear(); + } else { + // Write failed, throw out the write list + _clearAndDeleteWriteMissionItems(); + } + emit sendComplete(!success /* error */); + break; + case TransactionRemoveAll: + emit removeAllComplete(!success /* error */); + break; + default: + break; + } + if (_resumeMission) { _resumeMission = false; emit resumeMissionReady(); @@ -907,6 +929,8 @@ void MissionManager::_removeAllWorker(void) qCDebug(MissionManagerLog) << "_removeAllWorker"; + emit progressPct(0); + _dedicatedLink = _vehicle->priorityLink(); mavlink_msg_mission_clear_all_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), @@ -933,9 +957,8 @@ void MissionManager::removeAll(void) _lastCurrentIndex = -1; emit currentIndexChanged(-1); emit lastCurrentIndexChanged(-1); - emit newMissionItemsAvailable(true /* removeAllRequested */); - _transactionInProgress = TransactionClearAll; + _transactionInProgress = TransactionRemoveAll; _retryCount = 0; emit inProgressChanged(true); @@ -961,6 +984,8 @@ void MissionManager::generateResumeMission(int resumeIndex) } } + resumeIndex = qMin(resumeIndex, _missionItems.count() - 1); + int seqNum = 0; QList resumeMission; @@ -979,6 +1004,9 @@ void MissionManager::generateResumeMission(int resumeIndex) << MAV_CMD_IMAGE_STOP_CAPTURE << MAV_CMD_VIDEO_START_CAPTURE << MAV_CMD_VIDEO_STOP_CAPTURE; + if (_vehicle->fixedWing() && _vehicle->px4Firmware()) { + includedResumeCommands << MAV_CMD_NAV_TAKEOFF; + } bool addHomePosition = _vehicle->firmwarePlugin()->sendHomePositionToVehicle(); int setCurrentIndex = addHomePosition ? 1 : 0; @@ -1084,9 +1112,9 @@ void MissionManager::generateResumeMission(int resumeIndex) } // Send to vehicle - _clearAndDeleteMissionItems(); + _clearAndDeleteWriteMissionItems(); for (int i=0; ideleteLater(); + } + _writeMissionItems.clear(); +} diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h index b15ad967e9b06aa898ac15718c9b7734033b6498..5b258d1b12a8ec5e72a6d6bfb8fc4389b530c5a0 100644 --- a/src/MissionManager/MissionManager.h +++ b/src/MissionManager/MissionManager.h @@ -43,10 +43,13 @@ public: /// Last current mission item reported while in Mission flight mode int lastCurrentIndex(void) const { return _lastCurrentIndex; } - void requestMissionItems(void); + /// Load the mission items from the vehicle + /// Signals newMissionItemsAvailable when done + void loadFromVehicle(void); /// Writes the specified set of mission items to the vehicle /// @param missionItems Items to send to vehicle + /// Signals sendComplete when done void writeMissionItems(const QList& missionItems); /// Writes the specified set mission items to the vehicle as an ArduPilot guided mode mission item. @@ -55,6 +58,7 @@ public: void writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoCoord, bool altChangeOnly); /// Removes all mission items from vehicle + /// Signals removeAllComplete when done void removeAll(void); /// Generates a new mission which starts from the specified index. It will include all the CMD_DO items @@ -84,7 +88,9 @@ signals: void currentIndexChanged(int currentIndex); void lastCurrentIndexChanged(int lastCurrentIndex); void resumeMissionReady(void); - void cameraFeedback(QGeoCoordinate imageCoordinate, int index); + void progressPct(double progressPercentPct); + void removeAllComplete (bool error); + void sendComplete (bool error); private slots: void _mavlinkMessageReceived(const mavlink_message_t& message); @@ -104,7 +110,7 @@ private: TransactionNone, TransactionRead, TransactionWrite, - TransactionClearAll + TransactionRemoveAll } TransactionType_t; void _startAckTimeout(AckType_t ack); @@ -115,8 +121,6 @@ private: void _handleMissionRequest(const mavlink_message_t& message, bool missionItemInt); void _handleMissionAck(const mavlink_message_t& message); void _handleMissionCurrent(const mavlink_message_t& message); - void _handleCameraFeedback(const mavlink_message_t& message); - void _handleCameraImageCaptured(const mavlink_message_t& message); void _requestNextMissionItem(void); void _clearMissionItems(void); void _sendError(ErrorCode_t errorCode, const QString& errorMsg); @@ -127,6 +131,7 @@ private: void _writeMissionCount(void); void _writeMissionItemsWorker(void); void _clearAndDeleteMissionItems(void); + void _clearAndDeleteWriteMissionItems(void); QString _lastMissionReqestString(MAV_MISSION_RESULT result); void _removeAllWorker(void); @@ -146,7 +151,8 @@ private: QMutex _dataMutex; - QList _missionItems; + QList _missionItems; ///< Set of mission items on vehicle + QList _writeMissionItems; ///< Set of mission items currently being written to vehicle int _currentMissionIndex; int _lastCurrentIndex; }; diff --git a/src/MissionManager/MissionManagerTest.cc b/src/MissionManager/MissionManagerTest.cc index f6e94581c45a3454a8136dcc78af91cce2f8b3e4..4a2fdc7965ef59e98af36bd6ae3251f3811b9c78 100644 --- a/src/MissionManager/MissionManagerTest.cc +++ b/src/MissionManager/MissionManagerTest.cc @@ -60,9 +60,8 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f // writeMissionItems should emit these signals before returning: // inProgressChanged - // newMissionItemsAvailable QVERIFY(_missionManager->inProgress()); - QCOMPARE(_multiSpyMissionManager->checkSignalByMask(inProgressChangedSignalMask | newMissionItemsAvailableSignalMask), true); + QCOMPARE(_multiSpyMissionManager->checkSignalByMask(inProgressChangedSignalMask), true); _checkInProgressValues(true); _multiSpyMissionManager->clearAllSignals(); @@ -93,8 +92,9 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f // Wait for write sequence to complete. We should get: // inProgressChanged(false) signal - _multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime); - QCOMPARE(_multiSpyMissionManager->checkOnlySignalByMask(inProgressChangedSignalMask), true); + // sednComplete signal + _multiSpyMissionManager->waitForSignalByIndex(sendCompleteSignalIndex, _missionManagerSignalWaitTime); + QCOMPARE(_multiSpyMissionManager->checkSignalByMask(inProgressChangedSignalMask | sendCompleteSignalMask), true); // Validate inProgressChanged signal value _checkInProgressValues(false); @@ -120,7 +120,7 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode _mockLink->setMissionItemFailureMode(failureMode); // Read the items back from the vehicle - _missionManager->requestMissionItems(); + _missionManager->loadFromVehicle(); // requestMissionItems should emit inProgressChanged signal before returning so no need to wait for it QVERIFY(_missionManager->inProgress()); diff --git a/src/MissionManager/MissionSettingsItem.cc b/src/MissionManager/MissionSettingsItem.cc index 85d7bdc9445cc443ab5c59ffa767c21ceb43c0a7..bcf63aed9185bd8e42813d74124339370fc8a3fe 100644 --- a/src/MissionManager/MissionSettingsItem.cc +++ b/src/MissionManager/MissionSettingsItem.cc @@ -90,7 +90,7 @@ void MissionSettingsItem::save(QJsonArray& missionItems) appendMissionItems(items, this); - // First item show be planned home position, we are not reponsible for save/load + // First item show be planned home position, we are not responsible for save/load // Remaining items we just output as is for (int i=1; itoolbox()->multiVehicleManager()) - , _activeVehicle(_multiVehicleMgr->offlineEditingVehicle()) + , _masterController(masterController) + , _controllerVehicle(masterController->controllerVehicle()) + , _managerVehicle(masterController->managerVehicle()) , _editMode(false) { @@ -30,8 +34,7 @@ void PlanElementController::start(bool editMode) _editMode = editMode; } -void PlanElementController::startStaticActiveVehicle(Vehicle* vehicle) +void PlanElementController::managerVehicleChanged(Vehicle* managerVehicle) { - Q_UNUSED(vehicle); - _editMode = false; + _managerVehicle = managerVehicle; } diff --git a/src/MissionManager/PlanElementController.h b/src/MissionManager/PlanElementController.h index c74ef71e6ed86a85957ee534e09f1e9be148565b..090ea2a4617d967bfafd7aa00d17340ddebf33f3 100644 --- a/src/MissionManager/PlanElementController.h +++ b/src/MissionManager/PlanElementController.h @@ -15,6 +15,8 @@ #include "Vehicle.h" #include "MultiVehicleManager.h" +class PlanMasterController; + /// This is the abstract base clas for Plan Element controllers. /// Examples of plan elements are: missions (MissionController), geofence (GeoFenceController) class PlanElementController : public QObject @@ -22,50 +24,51 @@ class PlanElementController : public QObject Q_OBJECT public: - PlanElementController(QObject* parent = NULL); + PlanElementController(PlanMasterController* masterController, QObject* parent = NULL); ~PlanElementController(); - Q_PROPERTY(bool containsItems READ containsItems NOTIFY containsItemsChanged) ///< true: Elemement is non-empty - Q_PROPERTY(bool syncInProgress READ syncInProgress NOTIFY syncInProgressChanged) ///< true: information is currently being saved/sent, false: no active save/send in progress - Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) ///< true: unsaved/sent changes are present, false: no changes since last save/send - Q_PROPERTY(Vehicle* vehicle READ vehicle NOTIFY vehicleChanged) + Q_PROPERTY(bool containsItems READ containsItems NOTIFY containsItemsChanged) ///< true: Elemement is non-empty + Q_PROPERTY(bool syncInProgress READ syncInProgress NOTIFY syncInProgressChanged) ///< true: information is currently being saved/sent, false: no active save/send in progress + 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); - /// Starts the controller using a single static active vehicle. Will not track global active vehicle changes. - virtual void startStaticActiveVehicle(Vehicle* vehicle); - - virtual void save(QJsonObject& json) = 0; - virtual bool load(const QJsonObject& json, QString& errorString) = 0; - virtual void loadFromVehicle(void) = 0; - virtual void sendToVehicle(void) = 0; - virtual void removeAll(void) = 0; ///< Removes all from controller only, synce required to remove from vehicle - virtual void removeAllFromVehicle(void) = 0; ///< Removes all from vehicle and controller + virtual void save (QJsonObject& json) = 0; + virtual bool load (const QJsonObject& json, QString& errorString) = 0; + virtual void loadFromVehicle (void) = 0; + virtual void removeAll (void) = 0; ///< Removes all from controller only + virtual bool showPlanFromManagerVehicle (void) = 0; /// true: controller is waiting for the current load to complete virtual bool containsItems (void) const = 0; virtual bool syncInProgress (void) const = 0; virtual bool dirty (void) const = 0; virtual void setDirty (bool dirty) = 0; - /// Called when the current active vehicle is about to be removed. Derived classes should override to implement custom behavior. - virtual void activeVehicleBeingRemoved(void) = 0; + /// Sends the current plan element to the vehicle + /// Signals sendComplete when done + virtual void sendToVehicle(void) = 0; - /// Called when a new active vehicle has been set. Derived classes should override to implement custom behavior. - virtual void activeVehicleSet(Vehicle* activeVehicle) = 0; + /// Removes all from vehicle and controller + /// Signals removeAllComplete when done + virtual void removeAllFromVehicle(void) = 0; - Vehicle* vehicle(void) { return _activeVehicle; } + /// Called when a new manager vehicle has been set. + virtual void managerVehicleChanged(Vehicle* managerVehicle) = 0; signals: void containsItemsChanged (bool containsItems); void syncInProgressChanged (bool syncInProgress); void dirtyChanged (bool dirty); void vehicleChanged (Vehicle* vehicle); + void sendComplete (void); + void removeAllComplete (void); protected: - MultiVehicleManager* _multiVehicleMgr; - Vehicle* _activeVehicle; ///< Currently active vehicle, can be disconnected offline editing vehicle + PlanMasterController* _masterController; + Vehicle* _controllerVehicle; + Vehicle* _managerVehicle; bool _editMode; }; diff --git a/src/MissionManager/PlanMasterController.cc b/src/MissionManager/PlanMasterController.cc index bb2bd3eaf6b7d4002676901e5bf2e6d364eab260..b56ac71ae9f30c345d9274a69a4135d4d003aae4 100644 --- a/src/MissionManager/PlanMasterController.cc +++ b/src/MissionManager/PlanMasterController.cc @@ -10,12 +10,16 @@ #include "PlanMasterController.h" #include "QGCApplication.h" #include "MultiVehicleManager.h" +#include "SettingsManager.h" #include "AppSettings.h" #include "JsonHelper.h" +#include "MissionManager.h" #include #include +QGC_LOGGING_CATEGORY(PlanMasterControllerLog, "PlanMasterControllerLog") + const int PlanMasterController::_planFileVersion = 1; const char* PlanMasterController::_planFileType = "Plan"; const char* PlanMasterController::_jsonMissionObjectKey = "mission"; @@ -25,8 +29,18 @@ const char* PlanMasterController::_jsonRallyPointsObjectKey = "rallyPoints"; PlanMasterController::PlanMasterController(QObject* parent) : QObject(parent) , _multiVehicleMgr(qgcApp()->toolbox()->multiVehicleManager()) - , _activeVehicle(_multiVehicleMgr->offlineEditingVehicle()) + , _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) { connect(&_missionController, &MissionController::dirtyChanged, this, &PlanMasterController::dirtyChanged); connect(&_geoFenceController, &GeoFenceController::dirtyChanged, this, &PlanMasterController::dirtyChanged); @@ -60,52 +74,160 @@ void PlanMasterController::start(bool editMode) void PlanMasterController::startStaticActiveVehicle(Vehicle* vehicle) { _editMode = false; - _missionController.startStaticActiveVehicle(vehicle); - _geoFenceController.startStaticActiveVehicle(vehicle); - _rallyPointController.startStaticActiveVehicle(vehicle); _activeVehicleChanged(vehicle); } void PlanMasterController::_activeVehicleChanged(Vehicle* activeVehicle) { - if (_activeVehicle) { - _missionController.activeVehicleBeingRemoved(); - _geoFenceController.activeVehicleBeingRemoved(); - _rallyPointController.activeVehicleBeingRemoved(); - _activeVehicle = NULL; + if (_managerVehicle == activeVehicle) { + // We are already setup for this vehicle + return; } - if (activeVehicle) { - _activeVehicle = activeVehicle; + qCDebug(PlanMasterControllerLog) << "_activeVehicleChanged" << activeVehicle; + + bool newOffline = false; + if (activeVehicle == NULL) { + // Since there is no longer an active vehicle we use the offline controller vehicle as the manager vehicle + _managerVehicle = _controllerVehicle; + newOffline = true; } else { - _activeVehicle = _multiVehicleMgr->offlineEditingVehicle(); + newOffline = false; + _managerVehicle = activeVehicle; + + // Update controllerVehicle to the currently connected vehicle + AppSettings* appSettings = qgcApp()->toolbox()->settingsManager()->appSettings(); + appSettings->offlineEditingFirmwareType()->setRawValue(AppSettings::offlineEditingFirmwareTypeFromFirmwareType(_managerVehicle->firmwareType())); + appSettings->offlineEditingVehicleType()->setRawValue(AppSettings::offlineEditingVehicleTypeFromVehicleType(_managerVehicle->vehicleType())); + + // We use these signals to sequence upload and download to the multiple controller/managers + connect(_managerVehicle->missionManager(), &MissionManager::newMissionItemsAvailable, this, &PlanMasterController::_loadMissionComplete); + connect(_managerVehicle->geoFenceManager(), &GeoFenceManager::loadComplete, this, &PlanMasterController::_loadGeoFenceComplete); + connect(_managerVehicle->rallyPointManager(), &RallyPointManager::loadComplete, this, &PlanMasterController::_loadRallyPointsComplete); + connect(_managerVehicle->missionManager(), &MissionManager::sendComplete, this, &PlanMasterController::_sendMissionComplete); + connect(_managerVehicle->geoFenceManager(), &GeoFenceManager::sendComplete, this, &PlanMasterController::_sendGeoFenceComplete); + connect(_managerVehicle->rallyPointManager(), &RallyPointManager::sendComplete, this, &PlanMasterController::_sendRallyPointsComplete); + } + if (newOffline != _offline) { + _offline = newOffline; + emit offlineEditingChanged(newOffline); } - _missionController.activeVehicleSet(_activeVehicle); - _geoFenceController.activeVehicleSet(_activeVehicle); - _rallyPointController.activeVehicleSet(_activeVehicle); - - // Whenever vehicle changes we need to update syncInProgress - emit syncInProgressChanged(syncInProgress()); - emit vehicleChanged(_activeVehicle); + _missionController.managerVehicleChanged(_managerVehicle); + _geoFenceController.managerVehicleChanged(_managerVehicle); + _rallyPointController.managerVehicleChanged(_managerVehicle); + + if (_editMode) { + 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 + // - Otherwise leave the current plan alone + if (!containsItems()) { + qCDebug(PlanMasterControllerLog) << "_activeVehicleChanged: Plan view is empty so loading from manager"; + _showPlanFromManagerVehicle(); + } + } + } else { + if (offline()) { + // No more active vehicle, clear mission + qCDebug(PlanMasterControllerLog) << "_activeVehicleChanged: Fly view is offline clearing plan"; + removeAll(); + } else { + // Fly view has changed to a new active vehicle, update to show correct mission + qCDebug(PlanMasterControllerLog) << "_activeVehicleChanged: Fly view is online so loading from manager"; + _showPlanFromManagerVehicle(); + } + } } void PlanMasterController::loadFromVehicle(void) { - // FIXME: Hack implementation - _missionController.loadFromVehicle(); - _geoFenceController.loadFromVehicle(); - _rallyPointController.loadFromVehicle(); + if (offline()) { + qCWarning(PlanMasterControllerLog) << "PlanMasterController::loadFromVehicle called while offline"; + } else if (!_editMode) { + qCWarning(PlanMasterControllerLog) << "PlanMasterController::loadFromVehicle called from Fly view"; + } else if (syncInProgress()) { + qCWarning(PlanMasterControllerLog) << "PlanMasterController::loadFromVehicle called while syncInProgress"; + } else { + _loadGeoFence = true; + _syncInProgress = true; + _missionController.loadFromVehicle(); + setDirty(false); + } } -void PlanMasterController::sendToVehicle(void) + +void PlanMasterController::_loadMissionComplete(void) +{ + if (_editMode && _loadGeoFence) { + _loadGeoFence = false; + _loadRallyPoints = true; + _geoFenceController.loadFromVehicle(); + setDirty(false); + } +} + +void PlanMasterController::_loadGeoFenceComplete(void) +{ + if (_editMode && _loadRallyPoints) { + _loadRallyPoints = false; + _rallyPointController.loadFromVehicle(); + setDirty(false); + } +} + +void PlanMasterController::_loadRallyPointsComplete(void) +{ + if (_editMode) { + _syncInProgress = false; + emit syncInProgressChanged(false); + } +} + +void PlanMasterController::_sendMissionComplete(void) +{ + if (_editMode && _sendGeoFence) { + qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle start fence sendToVehicle"; + _sendGeoFence = false; + _sendRallyPoints = true; + _geoFenceController.sendToVehicle(); + setDirty(false); + } +} + +void PlanMasterController::_sendGeoFenceComplete(void) { - // FIXME: Hack implementation - _missionController.sendToVehicle(); - _geoFenceController.sendToVehicle(); - _rallyPointController.sendToVehicle(); + if (_editMode && _sendRallyPoints) { + qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle start rally sendToVehicle"; + _sendRallyPoints = false; + _rallyPointController.sendToVehicle(); + } } +void PlanMasterController::_sendRallyPointsComplete(void) +{ + if (_editMode && _syncInProgress) { + qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle rally point send complete"; + _syncInProgress = false; + emit syncInProgressChanged(false); + } +} + +void PlanMasterController::sendToVehicle(void) +{ + if (offline()) { + qCWarning(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle called while offline"; + } else if (!_editMode) { + qCWarning(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle called from Fly view"; + } else if (syncInProgress()) { + qCWarning(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle called while syncInProgress"; + } else { + qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle start mission sendToVehicle"; + _sendGeoFence = true; + _missionController.sendToVehicle(); + setDirty(false); + } +} void PlanMasterController::loadFromFile(const QString& filename) { @@ -123,38 +245,52 @@ void PlanMasterController::loadFromFile(const QString& filename) return; } - QJsonDocument jsonDoc; - QByteArray bytes = file.readAll(); - - if (!JsonHelper::isJsonFile(bytes, jsonDoc, errorString)) { - qgcApp()->showMessage(errorMessage.arg(errorString)); - return; - } - - int version; - QJsonObject json = jsonDoc.object(); - if (!JsonHelper::validateQGCJsonFile(json, _planFileType, _planFileVersion, _planFileVersion, version, errorString)) { - qgcApp()->showMessage(errorMessage.arg(errorString)); - return; - } - - QList rgKeyInfo = { - { _jsonMissionObjectKey, QJsonValue::Object, true }, - { _jsonGeoFenceObjectKey, QJsonValue::Object, true }, - { _jsonRallyPointsObjectKey, QJsonValue::Object, true }, - }; - if (!JsonHelper::validateKeys(json, rgKeyInfo, errorString)) { - qgcApp()->showMessage(errorMessage.arg(errorString)); - return; + QString fileExtension(".%1"); + if (filename.endsWith(fileExtension.arg(AppSettings::planFileExtension))) { + QJsonDocument jsonDoc; + QByteArray bytes = file.readAll(); + + if (!JsonHelper::isJsonFile(bytes, jsonDoc, errorString)) { + qgcApp()->showMessage(errorMessage.arg(errorString)); + return; + } + + int version; + QJsonObject json = jsonDoc.object(); + if (!JsonHelper::validateQGCJsonFile(json, _planFileType, _planFileVersion, _planFileVersion, version, errorString)) { + qgcApp()->showMessage(errorMessage.arg(errorString)); + return; + } + + QList rgKeyInfo = { + { _jsonMissionObjectKey, QJsonValue::Object, true }, + { _jsonGeoFenceObjectKey, QJsonValue::Object, true }, + { _jsonRallyPointsObjectKey, QJsonValue::Object, true }, + }; + if (!JsonHelper::validateKeys(json, rgKeyInfo, errorString)) { + qgcApp()->showMessage(errorMessage.arg(errorString)); + return; + } + + if (!_missionController.load(json[_jsonMissionObjectKey].toObject(), errorString) || + !_geoFenceController.load(json[_jsonGeoFenceObjectKey].toObject(), errorString) || + !_rallyPointController.load(json[_jsonRallyPointsObjectKey].toObject(), errorString)) { + qgcApp()->showMessage(errorMessage.arg(errorString)); + } + } else if (filename.endsWith(fileExtension.arg(AppSettings::missionFileExtension))) { + if (!_missionController.loadJsonFile(file, errorString)) { + qgcApp()->showMessage(errorMessage.arg(errorString)); + } + } else if (filename.endsWith(fileExtension.arg(AppSettings::waypointsFileExtension)) || + filename.endsWith(fileExtension.arg(QStringLiteral("txt")))) { + if (!_missionController.loadTextFile(file, errorString)) { + qgcApp()->showMessage(errorMessage.arg(errorString)); + } } - if (!_missionController.load(json[_jsonMissionObjectKey].toObject(), errorString) || - !_geoFenceController.load(json[_jsonGeoFenceObjectKey].toObject(), errorString) || - !_rallyPointController.load(json[_jsonRallyPointsObjectKey].toObject(), errorString)) { - qgcApp()->showMessage(errorMessage.arg(errorString)); - return; + if (!offline()) { + setDirty(true); } - setDirty(true); } void PlanMasterController::saveToFile(const QString& filename) @@ -190,22 +326,28 @@ void PlanMasterController::saveToFile(const QString& filename) file.write(saveDoc.toJson()); } - // If we are connected to a real vehicle, don't clear dirty bit on saving to file since vehicle is still out of date - if (_activeVehicle->isOfflineEditingVehicle()) { + // Only clear dirty bit if we are offline + if (offline()) { setDirty(false); } } void PlanMasterController::removeAll(void) { - + _missionController.removeAll(); + _geoFenceController.removeAll(); + _rallyPointController.removeAll(); } void PlanMasterController::removeAllFromVehicle(void) { - _missionController.removeAllFromVehicle(); - _geoFenceController.removeAllFromVehicle(); - _rallyPointController.removeAllFromVehicle(); + if (!offline()) { + _missionController.removeAllFromVehicle(); + _geoFenceController.removeAllFromVehicle(); + _rallyPointController.removeAllFromVehicle(); + } else { + qWarning() << "PlanMasterController::removeAllFromVehicle called while offline"; + } } bool PlanMasterController::containsItems(void) const @@ -213,11 +355,6 @@ bool PlanMasterController::containsItems(void) const return _missionController.containsItems() || _geoFenceController.containsItems() || _rallyPointController.containsItems(); } -bool PlanMasterController::syncInProgress(void) const -{ - return _missionController.syncInProgress() || _geoFenceController.syncInProgress() || _rallyPointController.syncInProgress(); -} - bool PlanMasterController::dirty(void) const { return _missionController.dirty() || _geoFenceController.dirty() || _rallyPointController.dirty(); @@ -239,9 +376,7 @@ QStringList PlanMasterController::loadNameFilters(void) const { QStringList filters; - filters << tr("Plan Files (*.%1)").arg(AppSettings::planFileExtension) << - tr("Mission Files (*.%1)").arg(AppSettings::missionFileExtension) << - tr("Waypoint Files (*.waypoints)") << + filters << tr("Supported types (*.%1 *.%2 *.%3 *.%4)").arg(AppSettings::planFileExtension).arg(AppSettings::missionFileExtension).arg(AppSettings::waypointsFileExtension).arg("txt") << tr("All Files (*.*)"); return filters; } @@ -264,3 +399,12 @@ void PlanMasterController::sendPlanToVehicle(Vehicle* vehicle, const QString& fi delete controller; } +void PlanMasterController::_showPlanFromManagerVehicle(void) +{ + // The crazy if structure is to handle the load propogating by itself through the system + if (!_missionController.showPlanFromManagerVehicle()) { + if (!_geoFenceController.showPlanFromManagerVehicle()) { + _rallyPointController.showPlanFromManagerVehicle(); + } + } +} diff --git a/src/MissionManager/PlanMasterController.h b/src/MissionManager/PlanMasterController.h index 6484b8356625dfa7a32fa64cff01d8e1f0c6bae5..82c6e3ded452c8dd14a785608ec8b45217f2a8fc 100644 --- a/src/MissionManager/PlanMasterController.h +++ b/src/MissionManager/PlanMasterController.h @@ -16,6 +16,9 @@ #include "RallyPointController.h" #include "Vehicle.h" #include "MultiVehicleManager.h" +#include "QGCLoggingCategory.h" + +Q_DECLARE_LOGGING_CATEGORY(PlanMasterControllerLog) /// Master controller for mission, fence, rally class PlanMasterController : public QObject @@ -30,13 +33,14 @@ public: Q_PROPERTY(GeoFenceController* geoFenceController READ geoFenceController CONSTANT) Q_PROPERTY(RallyPointController* rallyPointController READ rallyPointController CONSTANT) - Q_PROPERTY(bool containsItems READ containsItems NOTIFY containsItemsChanged) ///< true: Elemement is non-empty - Q_PROPERTY(bool syncInProgress READ syncInProgress NOTIFY syncInProgressChanged) ///< true: Information is currently being saved/sent, false: no active save/send in progress - Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) ///< true: Unsaved/sent changes are present, false: no changes since last save/send - Q_PROPERTY(Vehicle* vehicle READ vehicle NOTIFY vehicleChanged) - Q_PROPERTY(QString fileExtension READ fileExtension CONSTANT) ///< File extention 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(Vehicle* controllerVehicle MEMBER _controllerVehicle CONSTANT) + Q_PROPERTY(bool offline READ offline NOTIFY offlineEditingChanged) ///< true: controller is not connected to an active vehicle + Q_PROPERTY(bool containsItems READ containsItems NOTIFY containsItemsChanged) ///< true: Elemement is non-empty + Q_PROPERTY(bool syncInProgress READ syncInProgress NOTIFY syncInProgressChanged) ///< true: Information is currently being saved/sent, false: no active save/send in progress + Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) ///< true: Unsaved/sent changes are present, false: no changes since last save/send + Q_PROPERTY(QString fileExtension READ fileExtension CONSTANT) ///< 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 /// Should be called immediately upon Component.onCompleted. /// @param editMode true: controller being used in Plan view, false: controller being used in Fly view @@ -61,32 +65,50 @@ public: GeoFenceController* geoFenceController(void) { return &_geoFenceController; } RallyPointController* rallyPointController(void) { return &_rallyPointController; } + bool offline (void) const { return _offline; } bool containsItems (void) const; - bool syncInProgress (void) const; + bool syncInProgress (void) const { return _syncInProgress; } bool dirty (void) const; void setDirty (bool dirty); QString fileExtension (void) const; QStringList loadNameFilters (void) const; QStringList saveNameFilters (void) const; - Vehicle* vehicle(void) { return _activeVehicle; } + Vehicle* controllerVehicle(void) { return _controllerVehicle; } + Vehicle* managerVehicle(void) { return _managerVehicle; } signals: void containsItemsChanged (bool containsItems); void syncInProgressChanged (bool syncInProgress); void dirtyChanged (bool dirty); void vehicleChanged (Vehicle* vehicle); + void offlineEditingChanged (bool offlineEditing); private slots: void _activeVehicleChanged(Vehicle* activeVehicle); + void _loadMissionComplete(void); + void _loadGeoFenceComplete(void); + void _loadRallyPointsComplete(void); + void _sendMissionComplete(void); + void _sendGeoFenceComplete(void); + void _sendRallyPointsComplete(void); private: + void _showPlanFromManagerVehicle(void); + MultiVehicleManager* _multiVehicleMgr; - Vehicle* _activeVehicle; ///< Currently active vehicle, can be disconnected offline editing vehicle + Vehicle* _controllerVehicle; + Vehicle* _managerVehicle; bool _editMode; + bool _offline; MissionController _missionController; GeoFenceController _geoFenceController; RallyPointController _rallyPointController; + bool _loadGeoFence; + bool _loadRallyPoints; + bool _sendGeoFence; + bool _sendRallyPoints; + bool _syncInProgress; static const int _planFileVersion; static const char* _planFileType; diff --git a/src/MissionManager/PlanMasterControllerTest.cc b/src/MissionManager/PlanMasterControllerTest.cc new file mode 100644 index 0000000000000000000000000000000000000000..f52b5340083f1635a9c4ef4572ed9d14906de9ff --- /dev/null +++ b/src/MissionManager/PlanMasterControllerTest.cc @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * (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 "PlanMasterControllerTest.h" +#include "LinkManager.h" +#include "MultiVehicleManager.h" +#include "SimpleMissionItem.h" +#include "MissionSettingsItem.h" +#include "QGCApplication.h" +#include "SettingsManager.h" +#include "AppSettings.h" + +PlanMasterControllerTest::PlanMasterControllerTest(void) + : _masterController(NULL) +{ + +} + +void PlanMasterControllerTest::init(void) +{ + UnitTest::init(); + + _masterController = new PlanMasterController(this); + _masterController->start(true /* editMode */); +} + +void PlanMasterControllerTest::cleanup(void) +{ + delete _masterController; + _masterController = NULL; + + UnitTest::cleanup(); +} + +void PlanMasterControllerTest::_testMissionFileLoad(void) +{ + _masterController->loadFromFile(":/json/unittest/OldFileFormat.mission"); + QCOMPARE(_masterController->missionController()->visualItems()->count(), 7); +} + + +void PlanMasterControllerTest::_testMissionPlannerFileLoad(void) +{ + _masterController->loadFromFile(":/json/unittest/MissionPlanner.waypoints"); + QCOMPARE(_masterController->missionController()->visualItems()->count(), 6); +} diff --git a/src/MissionManager/PlanMasterControllerTest.h b/src/MissionManager/PlanMasterControllerTest.h new file mode 100644 index 0000000000000000000000000000000000000000..05f647adf1176891df69635ba0de52149578264d --- /dev/null +++ b/src/MissionManager/PlanMasterControllerTest.h @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * (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 "MockLink.h" +#include "MissionManager.h" +#include "MultiSignalSpy.h" +#include "MissionControllerManagerTest.h" +#include "PlanMasterController.h" +#include "MissionController.h" +#include "SimpleMissionItem.h" + +#include + +class PlanMasterControllerTest : public UnitTest +{ + Q_OBJECT + +public: + PlanMasterControllerTest(void); + +private slots: + void init(void) final; + void cleanup(void) final; + + void _testMissionFileLoad(void); + void _testMissionPlannerFileLoad(void); + +private: + PlanMasterController* _masterController; +}; diff --git a/src/MissionManager/RallyPointController.cc b/src/MissionManager/RallyPointController.cc index bfc447af37c63e7cf789dff271ea4e9864a012c8..c719e11f7ac0d8af22e54ed213665247e778056c 100644 --- a/src/MissionManager/RallyPointController.cc +++ b/src/MissionManager/RallyPointController.cc @@ -23,6 +23,7 @@ #include "QGroundControlQmlGlobal.h" #include "SettingsManager.h" #include "AppSettings.h" +#include "PlanMasterController.h" #ifndef __mobile__ #include "QGCQFileDialog.h" @@ -36,12 +37,16 @@ QGC_LOGGING_CATEGORY(RallyPointControllerLog, "RallyPointControllerLog") const char* RallyPointController::_jsonFileTypeValue = "RallyPoints"; const char* RallyPointController::_jsonPointsKey = "points"; -RallyPointController::RallyPointController(QObject* parent) - : PlanElementController(parent) +RallyPointController::RallyPointController(PlanMasterController* masterController, QObject* parent) + : PlanElementController(masterController, parent) + , _rallyPointManager(_managerVehicle->rallyPointManager()) , _dirty(false) , _currentRallyPoint(NULL) + , _itemsRequested(false) { connect(&_points, &QmlObjectListModel::countChanged, this, &RallyPointController::_updateContainsItems); + + managerVehicleChanged(_managerVehicle); } RallyPointController::~RallyPointController() @@ -49,23 +54,26 @@ RallyPointController::~RallyPointController() } -void RallyPointController::activeVehicleBeingRemoved(void) -{ - _activeVehicle->rallyPointManager()->disconnect(this); - _points.clearAndDeleteContents(); - _activeVehicle = NULL; -} - -void RallyPointController::activeVehicleSet(Vehicle* activeVehicle) +void RallyPointController::managerVehicleChanged(Vehicle* managerVehicle) { - _activeVehicle = activeVehicle; - RallyPointManager* rallyPointManager = _activeVehicle->rallyPointManager(); - connect(rallyPointManager, &RallyPointManager::loadComplete, this, &RallyPointController::_loadComplete); - connect(rallyPointManager, &RallyPointManager::inProgressChanged, this, &RallyPointController::syncInProgressChanged); + if (_managerVehicle) { + _rallyPointManager->disconnect(this); + _managerVehicle = NULL; + _rallyPointManager = NULL; + } - if (!rallyPointManager->inProgress()) { - _loadComplete(rallyPointManager->points()); + _managerVehicle = managerVehicle; + if (!_managerVehicle) { + qWarning() << "RallyPointController::managerVehicleChanged managerVehicle=NULL"; + return; } + + _rallyPointManager = _managerVehicle->rallyPointManager(); + connect(_rallyPointManager, &RallyPointManager::loadComplete, this, &RallyPointController::_managerLoadComplete); + connect(_rallyPointManager, &RallyPointManager::sendComplete, this, &RallyPointController::_managerSendComplete); + connect(_rallyPointManager, &RallyPointManager::removeAllComplete, this, &RallyPointController::_managerRemoveAllComplete); + connect(_rallyPointManager, &RallyPointManager::inProgressChanged, this, &RallyPointController::syncInProgressChanged); + emit rallyPointsSupportedChanged(rallyPointsSupported()); } @@ -119,32 +127,49 @@ void RallyPointController::removeAll(void) setCurrentRallyPoint(NULL); } +void RallyPointController::removeAllFromVehicle(void) +{ + if (_masterController->offline()) { + qCWarning(RallyPointControllerLog) << "RallyPointController::removeAllFromVehicle called while offline"; + } else if (syncInProgress()) { + qCWarning(RallyPointControllerLog) << "RallyPointController::removeAllFromVehicle called while syncInProgress"; + } else { + _rallyPointManager->removeAll(); + } +} + void RallyPointController::loadFromVehicle(void) { - if (_activeVehicle->parameterManager()->parametersReady() && !syncInProgress()) { - _activeVehicle->rallyPointManager()->loadFromVehicle(); + if (_masterController->offline()) { + qCWarning(RallyPointControllerLog) << "RallyPointController::loadFromVehicle called while offline"; + } else if (syncInProgress()) { + qCWarning(RallyPointControllerLog) << "RallyPointController::loadFromVehicle called while syncInProgress"; } else { - qCWarning(RallyPointControllerLog) << "RallyPointController::loadFromVehicle call at wrong time" << _activeVehicle->parameterManager()->parametersReady() << syncInProgress(); + _itemsRequested = true; + _rallyPointManager->loadFromVehicle(); } } void RallyPointController::sendToVehicle(void) { - if (!syncInProgress()) { + if (_masterController->offline()) { + qCWarning(RallyPointControllerLog) << "RallyPointController::sendToVehicle called while offline"; + } else if (syncInProgress()) { + qCWarning(RallyPointControllerLog) << "RallyPointController::sendToVehicle called while syncInProgress"; + } else { + qCDebug(RallyPointControllerLog) << "RallyPointController::sendToVehicle"; setDirty(false); QList rgPoints; for (int i=0; i<_points.count(); i++) { rgPoints.append(qobject_cast(_points[i])->coordinate()); } - _activeVehicle->rallyPointManager()->sendToVehicle(rgPoints); - } else { - qCWarning(RallyPointControllerLog) << "RallyPointController::loadFromVehicle call at wrong time" << _activeVehicle->parameterManager()->parametersReady() << syncInProgress(); + _rallyPointManager->sendToVehicle(rgPoints); } } bool RallyPointController::syncInProgress(void) const { - return _activeVehicle->rallyPointManager()->inProgress(); + return _rallyPointManager->inProgress(); } void RallyPointController::setDirty(bool dirty) @@ -157,20 +182,41 @@ void RallyPointController::setDirty(bool dirty) QString RallyPointController::editorQml(void) const { - return _activeVehicle->rallyPointManager()->editorQml(); + return _rallyPointManager->editorQml(); } -void RallyPointController::_loadComplete(const QList rgPoints) +void RallyPointController::_managerLoadComplete(const QList rgPoints) { - _points.clearAndDeleteContents(); - QObjectList pointList; - for (int i=0; irallyPointManager()->rallyPointsSupported(); + return _rallyPointManager->rallyPointsSupported(); } void RallyPointController::removePoint(QObject* rallyPoint) @@ -235,7 +281,27 @@ void RallyPointController::_updateContainsItems(void) emit containsItemsChanged(containsItems()); } -void RallyPointController::removeAllFromVehicle(void) +bool RallyPointController::showPlanFromManagerVehicle (void) { - _activeVehicle->rallyPointManager()->removeAll(); + qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle _editMode" << _editMode; + if (_masterController->offline()) { + qCWarning(RallyPointControllerLog) << "RallyPointController::showPlanFromManagerVehicle called while offline"; + return true; // stops further propagation of showPlanFromManagerVehicle due to error + } else { + if (!_managerVehicle->initialPlanRequestComplete()) { + // The vehicle hasn't completed initial load, we can just wait for loadComplete to be signalled automatically + qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle: !initialPlanRequestComplete, wait for signal"; + return true; + } else if (syncInProgress()) { + // If the sync is already in progress, _loadComplete will be called automatically when it is done. So no need to do anything. + qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle: syncInProgress wait for signal"; + return true; + } else { + // Fake a _loadComplete with the current items + qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle: sync complete simulate signal"; + _itemsRequested = true; + _managerLoadComplete(_rallyPointManager->points()); + return false; + } + } } diff --git a/src/MissionManager/RallyPointController.h b/src/MissionManager/RallyPointController.h index 6d0858c3491250aa843fe1e51c6689f9085f7cfb..7a2c945817ebe07d43ca7620d8c6439578a11534 100644 --- a/src/MissionManager/RallyPointController.h +++ b/src/MissionManager/RallyPointController.h @@ -26,7 +26,7 @@ class RallyPointController : public PlanElementController Q_OBJECT public: - RallyPointController(QObject* parent = NULL); + RallyPointController(PlanMasterController* masterController, QObject* parent = NULL); ~RallyPointController(); Q_PROPERTY(bool rallyPointsSupported READ rallyPointsSupported NOTIFY rallyPointsSupportedChanged) @@ -37,18 +37,18 @@ public: Q_INVOKABLE void addPoint(QGeoCoordinate point); Q_INVOKABLE void removePoint(QObject* rallyPoint); - void save (QJsonObject& json) final; - bool load (const QJsonObject& json, QString& errorString) final; - void loadFromVehicle (void) final; - void sendToVehicle (void) final; - void removeAll (void) final; - void removeAllFromVehicle (void) final; - bool syncInProgress (void) const final; - bool dirty (void) const final { return _dirty; } - void setDirty (bool dirty) final; - bool containsItems (void) const final; - void activeVehicleBeingRemoved (void) final; - void activeVehicleSet (Vehicle* vehicle) final; + void save (QJsonObject& json) final; + bool load (const QJsonObject& json, QString& errorString) final; + void loadFromVehicle (void) final; + void sendToVehicle (void) final; + void removeAll (void) final; + void removeAllFromVehicle (void) final; + bool syncInProgress (void) const final; + bool dirty (void) const final { return _dirty; } + void setDirty (bool dirty) final; + bool containsItems (void) const final; + void managerVehicleChanged (Vehicle* managerVehicle) final; + bool showPlanFromManagerVehicle (void) final; bool rallyPointsSupported (void) const; QmlObjectListModel* points (void) { return &_points; } @@ -63,14 +63,18 @@ signals: void loadComplete(void); private slots: - void _loadComplete(const QList rgPoints); + void _managerLoadComplete(const QList rgPoints); + void _managerSendComplete(bool error); + void _managerRemoveAllComplete(bool error); void _setFirstPointCurrent(void); void _updateContainsItems(void); private: + RallyPointManager* _rallyPointManager; bool _dirty; QmlObjectListModel _points; QObject* _currentRallyPoint; + bool _itemsRequested; static const char* _jsonFileTypeValue; static const char* _jsonPointsKey; diff --git a/src/MissionManager/RallyPointManager.cc b/src/MissionManager/RallyPointManager.cc index ba92da6a14bf18a6a63dabe71f76be0ef3b7b2dd..d101b4e890bb78fa30cc00b8aac8814b5c77ebe4 100644 --- a/src/MissionManager/RallyPointManager.cc +++ b/src/MissionManager/RallyPointManager.cc @@ -39,6 +39,13 @@ void RallyPointManager::loadFromVehicle(void) void RallyPointManager::sendToVehicle(const QList& rgPoints) { + // No support in generic vehicle Q_UNUSED(rgPoints); + emit sendComplete(false /* error */); +} + +void RallyPointManager::removeAll(void) +{ // No support in generic vehicle + emit removeAllComplete(false /* error */); } diff --git a/src/MissionManager/RallyPointManager.h b/src/MissionManager/RallyPointManager.h index ca391edaaba635598b231122e85973423fcc12ff..db46f18fe39cea75b0b9503959d20c621f50337d 100644 --- a/src/MissionManager/RallyPointManager.h +++ b/src/MissionManager/RallyPointManager.h @@ -33,12 +33,16 @@ public: virtual bool inProgress(void) const { return false; } /// Load the current settings from the vehicle + /// Signals loadComplete when done virtual void loadFromVehicle(void); /// Send the current settings to the vehicle + /// Signals sendComplete when done virtual void sendToVehicle(const QList& rgPoints); - virtual void removeAll(void) { }; + /// Remove all rally points from the vehicle + /// Signals removeAllCompleted when done + virtual void removeAll(void); virtual bool rallyPointsSupported(void) const { return false; } @@ -58,7 +62,9 @@ signals: void loadComplete (const QList rgPoints); void inProgressChanged (bool inProgress); void error (int errorCode, const QString& errorMsg); - + void removeAllComplete (bool error); + void sendComplete (bool error); + protected: void _sendError(ErrorCode_t errorCode, const QString& errorMsg); diff --git a/src/MissionManager/SectionTest.cc b/src/MissionManager/SectionTest.cc index cd95f96c0183f854b8aa9598b231f48ccae96455..ef7b4acc963fa650b5b8ea9f58131f55ef69fea3 100644 --- a/src/MissionManager/SectionTest.cc +++ b/src/MissionManager/SectionTest.cc @@ -56,16 +56,17 @@ void SectionTest::_createSpy(Section* section, MultiSignalSpy** sectionSpy) void SectionTest::_missionItemsEqual(MissionItem& item1, MissionItem& item2) { - QCOMPARE(item1.command(), item2.command()); - QCOMPARE(item1.frame(), item2.frame()); - QCOMPARE(item1.autoContinue(), item2.autoContinue()); - QCOMPARE(item1.param1(), item2.param1()); - QCOMPARE(item1.param2(), item2.param2()); - QCOMPARE(item1.param3(), item2.param3()); - QCOMPARE(item1.param4(), item2.param4()); - QCOMPARE(item1.param5(), item2.param5()); - QCOMPARE(item1.param6(), item2.param6()); - QCOMPARE(item1.param7(), item2.param7()); + QCOMPARE(item1.command(), item2.command()); + QCOMPARE(item1.frame(), item2.frame()); + QCOMPARE(item1.autoContinue(), item2.autoContinue()); + + QVERIFY(UnitTest::doubleNaNCompare(item1.param1(), item2.param1())); + QVERIFY(UnitTest::doubleNaNCompare(item1.param2(), item2.param2())); + QVERIFY(UnitTest::doubleNaNCompare(item1.param3(), item2.param3())); + QVERIFY(UnitTest::doubleNaNCompare(item1.param4(), item2.param4())); + QVERIFY(UnitTest::doubleNaNCompare(item1.param5(), item2.param5())); + QVERIFY(UnitTest::doubleNaNCompare(item1.param6(), item2.param6())); + QVERIFY(UnitTest::doubleNaNCompare(item1.param7(), item2.param7())); } void SectionTest::_commonScanTest(Section* section) diff --git a/src/MissionManager/SimpleMissionItem.cc b/src/MissionManager/SimpleMissionItem.cc index bc982faa94277826f66f9f957fff06fa028b6b7e..4372d5599c348613418f16cec87427337a8ba3a1 100644 --- a/src/MissionManager/SimpleMissionItem.cc +++ b/src/MissionManager/SimpleMissionItem.cc @@ -446,6 +446,17 @@ void SimpleMissionItem::_rebuildNaNFacts(void) const MissionCmdParamInfo* paramInfo = uiInfo->getParamInfo(i); if (paramInfo && paramInfo->nanUnchanged()) { + // Show hide Heading field on waypoint based on vehicle yaw to next waypoint setting. This needs to come from the actual vehicle if it exists + // and not _vehicle which is always offline. + Vehicle* firmwareVehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle(); + if (!firmwareVehicle) { + firmwareVehicle = _vehicle; + } + bool hideWaypointHeading = (command == MAV_CMD_NAV_WAYPOINT) && (i == 4) && firmwareVehicle->firmwarePlugin()->vehicleYawsToNextWaypointInMission(firmwareVehicle); + if (hideWaypointHeading) { + continue; + } + Fact* paramFact = rgParamFacts[i-1]; FactMetaData* paramMetaData = rgParamMetaData[i-1]; @@ -623,6 +634,7 @@ void SimpleMissionItem::setDefaultsForCommand(void) break; case MAV_CMD_NAV_LAND: + case MAV_CMD_NAV_VTOL_LAND: _missionItem.setParam7(0); break; default: diff --git a/src/MissionManager/SpeedSection.cc b/src/MissionManager/SpeedSection.cc index 5c45c31a7f95f4fb71487b7650a6f0b670d6902c..7979690ebb87834ecd6a8570f8ad4d4b9370d0fa 100644 --- a/src/MissionManager/SpeedSection.cc +++ b/src/MissionManager/SpeedSection.cc @@ -27,14 +27,11 @@ SpeedSection::SpeedSection(Vehicle* vehicle, QObject* parent) _metaDataMap = FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/SpeedSection.FactMetaData.json"), NULL /* metaDataParent */); } - double hoverSpeed, cruiseSpeed; double flightSpeed = 0; - - _vehicle->firmwarePlugin()->missionFlightSpeedInfo(_vehicle, hoverSpeed, cruiseSpeed); if (_vehicle->multiRotor()) { - flightSpeed = hoverSpeed; - } else if (_vehicle->fixedWing()) { - flightSpeed = cruiseSpeed; + flightSpeed = _vehicle->defaultHoverSpeed(); + } else { + flightSpeed = _vehicle->defaultCruiseSpeed(); } _metaDataMap[_flightSpeedName]->setRawDefaultValue(flightSpeed); diff --git a/src/MissionManager/SurveyMissionItem.cc b/src/MissionManager/SurveyMissionItem.cc index 54bab4591ef6874edf3247ca75377ff64124c917..52de59842287475d71d2e3eebc3b2386d284c4e1 100644 --- a/src/MissionManager/SurveyMissionItem.cc +++ b/src/MissionManager/SurveyMissionItem.cc @@ -437,7 +437,7 @@ void SurveyMissionItem::_convertTransectToGeo(const QList>& trans for (int j=0; j 1) { - qDebug() << "Reverse segments"; // We need to reverse the order of segments QList> rgReversedTransects; for (int i=_reflyTransectSegments.count() - 1; i>=0; i--) { @@ -476,7 +475,6 @@ void SurveyMissionItem::_optimizeReflySegments(void) _reflyTransectSegments = rgReversedTransects; } if (shortestIndex & 1) { - qDebug() << "Reverse points"; // We need to reverse the points within each segment for (int i=0; i<_reflyTransectSegments.count(); i++) { QList rgReversedCoords; @@ -497,6 +495,70 @@ void SurveyMissionItem::_appendGridPointsFromTransects(QList& 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); +} + void SurveyMissionItem::_generateGrid(void) { if (_ignoreRecalc) { @@ -515,17 +577,19 @@ void SurveyMissionItem::_generateGrid(void) QList polygonPoints; QList> transectSegments; - // Convert polygon to Qt coordinate system (y positive is down) + // Convert polygon to NED qCDebug(SurveyMissionItemLog) << "Convert polygon"; QGeoCoordinate tangentOrigin = _mapPolygon.path()[0].value(); for (int i=0; i<_mapPolygon.count(); i++) { double y, x, down; QGeoCoordinate vertex = _mapPolygon.pathModel().value(i)->coordinate(); convertGeoToNed(vertex, tangentOrigin, &y, &x, &down); - polygonPoints += QPointF(x, -y); + polygonPoints += QPointF(x, y); qCDebug(SurveyMissionItemLog) << vertex << polygonPoints.last().x() << polygonPoints.last().y(); } + polygonPoints = _convexPolygon(polygonPoints); + double coveredArea = 0.0; for (int i=0; i& lineList /// Adjust the line segments such that they are all going the same direction with respect to going from P1->P2 void SurveyMissionItem::_adjustLineDirection(const QList& lineList, QList& resultLines) { + qreal firstAngle = 0; for (int i=0; i 180.0) { + if (i == 0) { + firstAngle = line.angle(); + } + + if (qAbs(line.angle() - firstAngle) > 1.0) { adjustedLine.setP1(line.p2()); adjustedLine.setP2(line.p1()); } else { @@ -745,10 +814,10 @@ int SurveyMissionItem::_gridGenerator(const QList& polygonPoints, QLis // Rotate the bounding rect around it's center to generate the larger bounding rect QPolygonF boundPolygon; - boundPolygon << _rotatePoint(smallBoundRect.topLeft(), center, gridAngle); - boundPolygon << _rotatePoint(smallBoundRect.topRight(), center, gridAngle); - boundPolygon << _rotatePoint(smallBoundRect.bottomRight(), center, gridAngle); - boundPolygon << _rotatePoint(smallBoundRect.bottomLeft(), center, gridAngle); + boundPolygon << _rotatePoint(smallBoundRect.topLeft(), center, gridAngle); + boundPolygon << _rotatePoint(smallBoundRect.topRight(), center, gridAngle); + boundPolygon << _rotatePoint(smallBoundRect.bottomRight(), center, gridAngle); + boundPolygon << _rotatePoint(smallBoundRect.bottomLeft(), center, gridAngle); boundPolygon << boundPolygon[0]; QRectF largeBoundRect = boundPolygon.boundingRect(); qCDebug(SurveyMissionItemLog) << "Rotated bounding rect" << largeBoundRect.topLeft().x() << largeBoundRect.topLeft().y() << largeBoundRect.bottomRight().x() << largeBoundRect.bottomRight().y(); diff --git a/src/MissionManager/SurveyMissionItem.h b/src/MissionManager/SurveyMissionItem.h index 77ce7c73f5df2d50402daea0c7d1add2082b191f..3d50d3bbe1fe422c777fb5b3cad4ab2123a43cdb 100644 --- a/src/MissionManager/SurveyMissionItem.h +++ b/src/MissionManager/SurveyMissionItem.h @@ -199,6 +199,10 @@ private: bool _appendMissionItemsWorker(QList& items, QObject* missionItemParent, int& seqNum, bool hasRefly, bool buildRefly); void _optimizeReflySegments(void); 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); int _sequenceNumber; bool _dirty; diff --git a/src/MissionManager/UnitTest/MissionPlanner.waypoints b/src/MissionManager/UnitTest/MissionPlanner.waypoints new file mode 100644 index 0000000000000000000000000000000000000000..40c3175ee2eb5fd20700bcb83aa127b40c787ac1 --- /dev/null +++ b/src/MissionManager/UnitTest/MissionPlanner.waypoints @@ -0,0 +1,7 @@ +QGC WPL 110 +0 1 0 16 0 0 0 0 47.660459 -122.103167 5.210000 1 +1 0 3 16 0.000000 0.000000 0.000000 0.000000 47.661298 -122.103274 100.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 47.661030 -122.101858 100.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 47.659961 -122.101525 100.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 47.659195 -122.103370 100.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 47.660459 -122.105538 100.000000 1 diff --git a/src/MissionManager/UnitTest/OldFileFormat.mission b/src/MissionManager/UnitTest/OldFileFormat.mission new file mode 100644 index 0000000000000000000000000000000000000000..e2b1be92865d9269a30a64034d94a6d5f84701a1 --- /dev/null +++ b/src/MissionManager/UnitTest/OldFileFormat.mission @@ -0,0 +1,120 @@ +{ + "firmwareType": 3, + "groundStation": "QGroundControl", + "items": [ + { + "autoContinue": true, + "command": 22, + "coordinate": [ + 47.660137763851573, + -122.29411931627152, + 50 + ], + "doJumpId": 1, + "frame": 2, + "params": [ + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 18, + "coordinate": [ + 47.660316610000002, + -122.29423867, + 10 + ], + "doJumpId": 2, + "frame": 3, + "params": [ + 1, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 18, + "coordinate": [ + 47.660299945473923, + -122.29402946102036, + 20 + ], + "doJumpId": 3, + "frame": 3, + "params": [ + 3, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "coordinate": [ + 47.660239429999997, + -122.29371564, + 5 + ], + "doJumpId": 4, + "frame": 3, + "params": [ + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "coordinate": [ + 47.660368599999998, + -122.29426013, + 5 + ], + "doJumpId": 5, + "frame": 3, + "params": [ + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 21, + "coordinate": [ + 47.660153620000003, + -122.29410455999999, + 3 + ], + "doJumpId": 6, + "frame": 3, + "params": [ + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + } + ], + "plannedHomePosition": [ + 47.660137763851573, + -122.29411931627152, + 0 + ], + "version": 2 +} diff --git a/src/PlanView/CameraSection.qml b/src/PlanView/CameraSection.qml index 602f57718884c1a947fc48fe202dcdce3d38f6dc..0d20c39a6ec9e08ab9a68b4e128eb4fdc6001705 100644 --- a/src/PlanView/CameraSection.qml +++ b/src/PlanView/CameraSection.qml @@ -104,5 +104,25 @@ 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/FWLandingPatternEditor.qml b/src/PlanView/FWLandingPatternEditor.qml index d2b23d8a4516a3d9fa825b39bfe0a2e90931d2be..de2a84dadc90e94efc62d95780b2f51a8b99ff87 100644 --- a/src/PlanView/FWLandingPatternEditor.qml +++ b/src/PlanView/FWLandingPatternEditor.qml @@ -41,14 +41,6 @@ Rectangle { anchors.right: parent.right visible: missionItem.landingCoordSet - QGCLabel { - anchors.left: parent.left - anchors.right: parent.right - wrapMode: Text.WordWrap - font.pointSize: ScreenTools.smallFontPointSize - text: qsTr("WIP (NOT FOR REAL FLIGHT!)") - } - SectionHeader { text: qsTr("Loiter point") } diff --git a/src/PlanView/GeoFenceEditor.qml b/src/PlanView/GeoFenceEditor.qml index b34ec3e7feaaae74035e0182eb57674124548c1f..114f85ef348dd62b36fb17687bc1ea23689bc8e9 100644 --- a/src/PlanView/GeoFenceEditor.qml +++ b/src/PlanView/GeoFenceEditor.qml @@ -30,7 +30,7 @@ QGCFlickable { width: parent.width height: geoFenceItems.y + geoFenceItems.height + (_margin * 2) radius: _radius - color: qgcPal.buttonHighlight + color: qgcPal.missionItemEditor QGCLabel { id: geoFenceLabel diff --git a/src/PlanView/MissionItemEditor.qml b/src/PlanView/MissionItemEditor.qml index eaa8c8c64a60bad904d63e41853ccaf518221806..1fd1167f51a9f8c551c450ac03b6ec4b4eb5cba7 100644 --- a/src/PlanView/MissionItemEditor.qml +++ b/src/PlanView/MissionItemEditor.qml @@ -15,12 +15,12 @@ import QGroundControl.Palette 1.0 /// Mission item edit control Rectangle { id: _root - height: editorLoader.y + editorLoader.height + (_margin * 2) - color: _currentItem ? qgcPal.primaryButton : qgcPal.windowShade + height: editorLoader.y + (editorLoader.visible ? editorLoader.height : 0) + (_margin * 2) + color: _currentItem ? qgcPal.missionItemEditor : qgcPal.windowShade radius: _radius property var map ///< Map control - property var missionController + property var masterController property var missionItem ///< MissionItem associated with this editor property bool readOnly ///< true: read only view, false: full editing view property var rootQgcView @@ -30,11 +30,13 @@ Rectangle { signal insertWaypoint signal insertComplexItem(string complexItemName) + property var _masterController: masterController + property var _missionController: _masterController.missionController property bool _currentItem: missionItem.isCurrentItem property color _outerTextColor: _currentItem ? qgcPal.primaryButtonText : qgcPal.text property bool _noMissionItemsAdded: ListView.view.model.count === 1 property real _sectionSpacer: ScreenTools.defaultFontPixelWidth / 2 // spacing between section headings - property bool _singleComplexItem: missionController.complexMissionItemNames.length === 1 + property bool _singleComplexItem: _missionController.complexMissionItemNames.length === 1 readonly property real _editFieldWidth: Math.min(width - _margin * 2, ScreenTools.defaultFontPixelWidth * 12) readonly property real _margin: ScreenTools.defaultFontPixelWidth / 2 @@ -105,7 +107,7 @@ Rectangle { visible: !_singleComplexItem Instantiator { - model: missionController.complexMissionItemNames + model: _missionController.complexMissionItemNames onObjectAdded: patternMenu.insertItem(index, object) onObjectRemoved: patternMenu.removeItem(object) @@ -118,9 +120,9 @@ Rectangle { } MenuItem { - text: qsTr("Insert ") + missionController.complexMissionItemNames[0] + text: qsTr("Insert ") + _missionController.complexMissionItemNames[0] visible: _singleComplexItem - onTriggered: insertComplexItem(missionController.complexMissionItemNames[0]) + onTriggered: insertComplexItem(_missionController.complexMissionItemNames[0]) } MenuItem { @@ -196,14 +198,11 @@ Rectangle { anchors.topMargin: _margin anchors.left: parent.left anchors.top: commandPicker.bottom - height: item ? item.height : 0 source: missionItem.editorQml + visible: _currentItem - onLoaded: { - item.visible = Qt.binding(function() { return _currentItem; }) - } - - property real availableWidth: _root.width - (_margin * 2) ///< How wide the editor should be - property var editorRoot: _root + property var masterController: _masterController + property real availableWidth: _root.width - (_margin * 2) ///< How wide the editor should be + property var editorRoot: _root } } // Rectangle diff --git a/src/PlanView/MissionSettingsEditor.qml b/src/PlanView/MissionSettingsEditor.qml index 522957b9431386cd83c12a8679a1b063149db0d5..cde3ac00fab5f18cb115170ffb0d455aa035f81e 100644 --- a/src/PlanView/MissionSettingsEditor.qml +++ b/src/PlanView/MissionSettingsEditor.qml @@ -20,7 +20,9 @@ Rectangle { visible: missionItem.isCurrentItem radius: _radius - property var _missionVehicle: missionController.vehicle + property var _masterControler: masterController + property var _missionController: _masterControler.missionController + property var _missionVehicle: _masterControler.controllerVehicle property bool _vehicleHasHomePosition: _missionVehicle.homePosition.isValid property bool _offlineEditing: _missionVehicle.isOfflineEditingVehicle property bool _showOfflineVehicleCombos: _offlineEditing && _multipleFirmware && _noMissionItemsAdded @@ -32,7 +34,7 @@ Rectangle { property var _savePath: QGroundControl.settingsManager.appSettings.missionSavePath property var _fileExtension: QGroundControl.settingsManager.appSettings.missionFileExtension property var _appSettings: QGroundControl.settingsManager.appSettings - property bool _waypointsOnlyMode: QGroundControl.corePlugin.options.missionWaypointsOnly + property bool _waypointsOnlyMode: QGroundControl.corePlugin.options.missionWaypointsOnly readonly property string _firmwareLabel: qsTr("Firmware") readonly property string _vehicleLabel: qsTr("Vehicle") diff --git a/src/PlanView/PlanToolBar.qml b/src/PlanView/PlanToolBar.qml index fbfc757b4089a27cc551cb2e042a9ca20e9f76c5..fb05d33cdb02cd4c0ad728b626a1d354e288ad44 100644 --- a/src/PlanView/PlanToolBar.qml +++ b/src/PlanView/PlanToolBar.qml @@ -33,9 +33,9 @@ Rectangle { property bool missionDirty: _controllerValid ? planMasterController.missionController.dirty : false property bool _controllerValid: planMasterController != undefined - property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle - property var _controllerDirty: planMasterController ? planMasterController.dirty : false - property var _controllerSyncInProgress: planMasterController ? planMasterController.syncInProgress : false + property bool _controllerOffline: _controllerValid ? planMasterController.offline : true + property var _controllerDirty: _controllerValid ? planMasterController.dirty : false + property var _controllerSyncInProgress: _controllerValid ? planMasterController.syncInProgress : false property bool _statusValid: currentMissionItem != undefined property bool _missionValid: missionItems != undefined @@ -57,6 +57,8 @@ Rectangle { property real _missionTime: _missionValid ? missionTime : NaN property int _batteryChangePoint: _controllerValid ? planMasterController.missionController.batteryChangePoint : -1 property int _batteriesRequired: _controllerValid ? planMasterController.missionController.batteriesRequired : -1 + property real _controllerProgressPct: _controllerValid ? planMasterController.missionController.progressPct : 0 + property bool _syncInProgress: _controllerValid ? planMasterController.missionController.syncInProgress : false property string _distanceText: isNaN(_distance) ? "-.-" : QGroundControl.metersToAppSettingsDistanceUnits(_distance).toFixed(1) + " " + QGroundControl.appSettingsDistanceUnitsString property string _altDifferenceText: isNaN(_altDifference) ? "-.-" : QGroundControl.metersToAppSettingsDistanceUnits(_altDifference).toFixed(1) + " " + QGroundControl.appSettingsDistanceUnitsString @@ -110,20 +112,48 @@ Rectangle { } } - RowLayout { - anchors.top: parent.top - anchors.bottom: parent.bottom - spacing: _margins * 2 + // Progress bar + + on_ControllerProgressPctChanged: { + if (_controllerProgressPct === 1) { + resetProgressTimer.start() + } else if (_controllerProgressPct > 0) { + progressBar.visible = true + } + } + + Timer { + id: resetProgressTimer + interval: 1000 + onTriggered: 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 + } + + GridLayout { + anchors.top: parent.top + anchors.bottom: parent.bottom + anchors.leftMargin: _margins + anchors.rightMargin: _margins anchors.left: logoRow.right - anchors.leftMargin: _margins * 4 anchors.right: uploadButton.visible ? uploadButton.left : parent.right - anchors.rightMargin: _margins + columnSpacing: 0//_margins + columns: 3 GridLayout { anchors.verticalCenter: parent.verticalCenter columns: 8 rowSpacing: _rowSpacing columnSpacing: _labelToValueSpacing + Layout.alignment: Qt.AlignHCenter QGCLabel { text: qsTr("Selected Waypoint") @@ -178,6 +208,7 @@ Rectangle { columns: 5 rowSpacing: _rowSpacing columnSpacing: _labelToValueSpacing + Layout.alignment: Qt.AlignHCenter QGCLabel { text: qsTr("Total Mission") @@ -214,6 +245,7 @@ Rectangle { columns: 3 rowSpacing: _rowSpacing columnSpacing: _labelToValueSpacing + Layout.alignment: Qt.AlignHCenter QGCLabel { text: qsTr("Battery") @@ -245,8 +277,8 @@ Rectangle { anchors.right: parent.right anchors.verticalCenter: parent.verticalCenter text: _controllerDirty ? qsTr("Upload Required") : qsTr("Upload") - enabled: _activeVehicle && !_controllerSyncInProgress - visible: _activeVehicle + enabled: !_controllerSyncInProgress + visible: !_controllerOffline onClicked: planMasterController.upload() PropertyAnimation on opacity { diff --git a/src/PlanView/PlanView.qml b/src/PlanView/PlanView.qml index 5e3e41943afd8a12f4ee404abd951da24a3341e0..61ecad5e8e1ed2b0c87915a16fe70c83b40dfd84 100644 --- a/src/PlanView/PlanView.qml +++ b/src/PlanView/PlanView.qml @@ -138,7 +138,7 @@ QGCView { text: qsTr("Pause and Upload") onClicked: { _activeVehicle.flightMode = _activeVehicle.pauseFlightMode - _missionController.sendToVehicle() + _planMasterController.sendToVehicle() hideDialog() } } @@ -231,7 +231,8 @@ QGCView { id: fileDialog qgcView: _qgcView folder: QGroundControl.settingsManager.appSettings.missionSavePath - fileExtension: masterController.fileExtension + fileExtension: QGroundControl.settingsManager.appSettings.planFileExtension + fileExtension2: QGroundControl.settingsManager.appSettings.missionFileExtension onAcceptedForSave: { masterController.saveToFile(file) @@ -346,17 +347,18 @@ QGCView { // Add the mission item visuals to the map Repeater { - model: _missionController.visualItems + model: _editingLayer == _layerMission ? _missionController.visualItems : undefined delegate: MissionItemMapVisual { map: editorMap onClicked: setCurrentItem(sequenceNumber, false) + visible: _editingLayer == _layerMission } } // Add lines between waypoints MissionLineView { - model: _editingLayer == _layerMission ? _missionController.waypointLines : undefined + model: _editingLayer == _layerMission ? _missionController.waypointLines : undefined } // Add the vehicles to the map @@ -396,9 +398,9 @@ QGCView { color: qgcPal.window title: qsTr("Plan") z: QGroundControl.zOrderWidgets - showAlternateIcon: [ false, false, !_autoSync && masterController.dirty, false, false, false ] + showAlternateIcon: [ false, false, masterController.dirty, false, false, false ] rotateImage: [ false, false, masterController.syncInProgress, false, false, false ] - animateImage: [ false, false, !_autoSync && masterController.dirty, false, false, false ] + animateImage: [ false, false, masterController.dirty, false, false, false ] buttonEnabled: [ true, true, !masterController.syncInProgress, true, true, true ] buttonVisible: [ true, true, true, true, _showZoom, _showZoom ] maxHeight: mapScale.y - toolStrip.y @@ -557,7 +559,7 @@ QGCView { delegate: MissionItemEditor { map: editorMap - missionController: _missionController + masterController: _planMasterController missionItem: object width: parent.width readOnly: false @@ -653,7 +655,7 @@ QGCView { id: syncLoadFromFileOverwrite QGCViewMessage { id: syncLoadFromVehicleCheck - message: qsTr("You have unsaved/unsent changes. Loading a from a file will lose these changes. Are you sure you want to load from a file?") + message: qsTr("You have unsaved/unsent changes. Loading from a file will lose these changes. Are you sure you want to load from a file?") function accept() { hideDialog() masterController.loadFromSelectedFile() @@ -664,9 +666,14 @@ QGCView { Component { id: removeAllPromptDialog QGCViewMessage { - message: qsTr("Are you sure you want to remove all items? This will also remove all items from the vehicle.") + message: qsTr("Are you sure you want to remove all items? ") + + (_planMasterController.offline ? "" : qsTr("This will also remove all items from the vehicle.")) function accept() { - masterController.removeAllFromVehicle() + if (_planMasterController.offline) { + masterController.removeAll() + } else { + masterController.removeAllFromVehicle() + } hideDialog() } } @@ -734,7 +741,7 @@ QGCView { QGCButton { text: qsTr("Upload") Layout.fillWidth: true - enabled: _activeVehicle && !masterController.syncInProgress + enabled: !masterController.offline && !masterController.syncInProgress onClicked: { dropPanel.hide() masterController.upload() @@ -744,7 +751,7 @@ QGCView { QGCButton { text: qsTr("Download") Layout.fillWidth: true - enabled: _activeVehicle && !masterController.syncInProgress + enabled: !masterController.offline && !masterController.syncInProgress onClicked: { dropPanel.hide() if (masterController.dirty) { diff --git a/src/PlanView/RallyPointEditorHeader.qml b/src/PlanView/RallyPointEditorHeader.qml index 7f99828d3bc715de84a939cc1dc91133c6503f45..ba87f949fdde9f27239a488d9ac6cf65664a69ac 100644 --- a/src/PlanView/RallyPointEditorHeader.qml +++ b/src/PlanView/RallyPointEditorHeader.qml @@ -20,7 +20,7 @@ QGCFlickable { width: parent.width height: innerEditorRect.y + innerEditorRect.height + (_margin * 2) radius: _radius - color: qgcPal.buttonHighlight + color: qgcPal.missionItemEditor QGCLabel { id: editorLabel diff --git a/src/PlanView/RallyPointItemEditor.qml b/src/PlanView/RallyPointItemEditor.qml index b6b36ed07f547fa4efe43af7c6d1542bd2a9bfcd..775f322a65e46c8cefbabb1cc142ddc7caa8c488 100644 --- a/src/PlanView/RallyPointItemEditor.qml +++ b/src/PlanView/RallyPointItemEditor.qml @@ -11,7 +11,7 @@ import QGroundControl.Palette 1.0 Rectangle { id: root height: _currentItem ? valuesRect.y + valuesRect.height + (_margin * 2) : titleBar.y - titleBar.height + _margin - color: _currentItem ? qgcPal.buttonHighlight : qgcPal.windowShade + color: _currentItem ? qgcPal.missionItemEditor : qgcPal.windowShade radius: _radius property var rallyPoint ///< RallyPoint object associated with editor diff --git a/src/PlanView/RallyPointMapVisuals.qml b/src/PlanView/RallyPointMapVisuals.qml index 1f05dfc30fb50834a56bb71c0df467147eb4567d..f303349ad0396d461f08b4066273be8ea747f63c 100644 --- a/src/PlanView/RallyPointMapVisuals.qml +++ b/src/PlanView/RallyPointMapVisuals.qml @@ -40,6 +40,19 @@ Item { _rallyPointsComponent.destroy() } + Component { + id: dragAreaComponent + + MissionItemIndicatorDrag { + itemCoordinate: rallyPointObject.coordinate + visible: rallyPointObject == myRallyPointController.currentRallyPoint + + property var rallyPointObject + + onItemCoordinateChanged: rallyPointObject.coordinate = itemCoordinate + } + } + Component { id: rallyPointComponent @@ -72,17 +85,14 @@ Item { property var _visuals: [ ] Component.onCompleted: { - var rallyPoint = rallyPointComponent.createObject(map) - rallyPoint.coordinate = Qt.binding(function() { return object.coordinate }) - rallyPoint.rallyPointObject = Qt.binding(function() { return object }) - map.addMapItem(rallyPoint) - _visuals.push(rallyPoint) -/* - var dragArea = dragAreaComponent.createObject(map, { "itemIndicator": dragHandle, "itemCoordinate": object.coordinate }) - dragArea.polygonVertex = Qt.binding(function() { return index }) - _visuals.push(dragHandle) + var rallyPointIndicator = rallyPointComponent.createObject(map) + rallyPointIndicator.coordinate = Qt.binding(function() { return object.coordinate }) + rallyPointIndicator.rallyPointObject = Qt.binding(function() { return object }) + map.addMapItem(rallyPointIndicator) + _visuals.push(rallyPointIndicator) + + var dragArea = dragAreaComponent.createObject(map, { "itemIndicator": rallyPointIndicator, "rallyPointObject": object }) _visuals.push(dragArea) -*/ } Component.onDestruction: { diff --git a/src/PlanView/SimpleItemEditor.qml b/src/PlanView/SimpleItemEditor.qml index 97c3f3a3d18a56ed9caf14309c04ff86a0e9ae37..23106868791f0a2bfe6153d04a31d1625ae946fd 100644 --- a/src/PlanView/SimpleItemEditor.qml +++ b/src/PlanView/SimpleItemEditor.qml @@ -12,148 +12,130 @@ import QGroundControl.Palette 1.0 // Editor for Simple mission items Rectangle { - id: valuesRect - width: availableWidth - height: deferedload.status == Loader.Ready ? (visible ? deferedload.item.height : 0) : 0 - color: qgcPal.windowShadeDark - visible: missionItem.isCurrentItem - radius: _radius - - Loader { - id: deferedload - active: valuesRect.visible - asynchronous: true - anchors.margins: _margin - anchors.left: valuesRect.left - anchors.right: valuesRect.right - anchors.top: valuesRect.top - - sourceComponent: Component { - Item { - id: valuesItem - height: valuesColumn.height + (_margin * 2) - - Column { - id: valuesColumn - anchors.left: parent.left - anchors.right: parent.right - anchors.top: parent.top - spacing: _margin - - QGCLabel { - width: parent.width - wrapMode: Text.WordWrap - font.pointSize: ScreenTools.smallFontPointSize - text: missionItem.rawEdit ? - qsTr("Provides advanced access to all commands/parameters. Be very careful!") : - missionItem.commandDescription - } - - GridLayout { - anchors.left: parent.left - anchors.right: parent.right - columns: 2 - - Repeater { - model: missionItem.comboboxFacts - - QGCLabel { - text: object.name - visible: object.name != "" - Layout.column: 0 - Layout.row: index - } - } - - Repeater { - model: missionItem.comboboxFacts - - FactComboBox { - indexModel: false - model: object.enumStrings - fact: object - Layout.column: 1 - Layout.row: index - Layout.fillWidth: true - } - } - } - - GridLayout { - anchors.left: parent.left - anchors.right: parent.right - flow: GridLayout.TopToBottom - rows: missionItem.textFieldFacts.count + missionItem.nanFacts.count + (missionItem.speedSection.available ? 1 : 0) - columns: 2 - - Repeater { - model: missionItem.textFieldFacts - - QGCLabel { text: object.name } - } - - Repeater { - model: missionItem.nanFacts - - QGCCheckBox { - text: object.name - checked: !isNaN(object.rawValue) - onClicked: object.rawValue = checked ? 0 : NaN - } - } - - QGCCheckBox { - id: flightSpeedCheckbox - text: qsTr("Flight Speed") - checked: missionItem.speedSection.specifyFlightSpeed - onClicked: missionItem.speedSection.specifyFlightSpeed = checked - visible: missionItem.speedSection.available - } - - Repeater { - model: missionItem.textFieldFacts - - FactTextField { - showUnits: true - fact: object - Layout.fillWidth: true - } - } - - Repeater { - model: missionItem.nanFacts - - FactTextField { - showUnits: true - fact: object - Layout.fillWidth: true - enabled: !isNaN(object.rawValue) - } - } - - FactTextField { - fact: missionItem.speedSection.flightSpeed - Layout.fillWidth: true - enabled: flightSpeedCheckbox.checked - visible: missionItem.speedSection.available - } - } - - Repeater { - model: missionItem.checkboxFacts - - FactCheckBox { - text: object.name - fact: object - } - } - - CameraSection { - checked: missionItem.cameraSection.settingsSpecified - visible: missionItem.cameraSection.available - } - } // Column - } // Item - } // Component - } // Loader + width: availableWidth + height: valuesColumn.height + (_margin * 2) + color: qgcPal.windowShadeDark + radius: _radius + + Column { + id: valuesColumn + anchors.margins: _margin + anchors.left: parent.left + anchors.right: parent.right + anchors.top: parent.top + spacing: _margin + + QGCLabel { + width: parent.width + wrapMode: Text.WordWrap + font.pointSize: ScreenTools.smallFontPointSize + text: missionItem.rawEdit ? + qsTr("Provides advanced access to all commands/parameters. Be very careful!") : + missionItem.commandDescription + } + + GridLayout { + anchors.left: parent.left + anchors.right: parent.right + columns: 2 + + Repeater { + model: missionItem.comboboxFacts + + QGCLabel { + text: object.name + visible: object.name != "" + Layout.column: 0 + Layout.row: index + } + } + + Repeater { + model: missionItem.comboboxFacts + + FactComboBox { + indexModel: false + model: object.enumStrings + fact: object + Layout.column: 1 + Layout.row: index + Layout.fillWidth: true + } + } + } + + GridLayout { + anchors.left: parent.left + anchors.right: parent.right + flow: GridLayout.TopToBottom + rows: missionItem.textFieldFacts.count + missionItem.nanFacts.count + (missionItem.speedSection.available ? 1 : 0) + columns: 2 + + Repeater { + model: missionItem.textFieldFacts + + QGCLabel { text: object.name } + } + + Repeater { + model: missionItem.nanFacts + + QGCCheckBox { + text: object.name + checked: !isNaN(object.rawValue) + onClicked: object.rawValue = checked ? 0 : NaN + } + } + + QGCCheckBox { + id: flightSpeedCheckbox + text: qsTr("Flight Speed") + checked: missionItem.speedSection.specifyFlightSpeed + onClicked: missionItem.speedSection.specifyFlightSpeed = checked + visible: missionItem.speedSection.available + } + + Repeater { + model: missionItem.textFieldFacts + + FactTextField { + showUnits: true + fact: object + Layout.fillWidth: true + } + } + + Repeater { + model: missionItem.nanFacts + + FactTextField { + showUnits: true + fact: object + Layout.fillWidth: true + enabled: !isNaN(object.rawValue) + } + } + + FactTextField { + fact: missionItem.speedSection.flightSpeed + Layout.fillWidth: true + enabled: flightSpeedCheckbox.checked + visible: missionItem.speedSection.available + } + } + + Repeater { + model: missionItem.checkboxFacts + + FactCheckBox { + text: object.name + fact: object + } + } + + CameraSection { + checked: missionItem.cameraSection.settingsSpecified + visible: missionItem.cameraSection.available + } + } // Column } // Rectangle diff --git a/src/PlanView/SurveyItemEditor.qml b/src/PlanView/SurveyItemEditor.qml index 9e9043e76206190e4a08ea290500408fd30a0778..2fa223edce9f8b151913722be33c27131c318b59 100644 --- a/src/PlanView/SurveyItemEditor.qml +++ b/src/PlanView/SurveyItemEditor.qml @@ -1,6 +1,8 @@ import QtQuick 2.3 import QtQuick.Controls 1.2 +import QtQuick.Controls.Styles 1.4 import QtQuick.Dialogs 1.2 +import QtQuick.Extras 1.4 import QtQuick.Layouts 1.2 import QGroundControl 1.0 @@ -398,10 +400,40 @@ Rectangle { columns: 2 visible: gridHeader.checked - QGCLabel { text: qsTr("Angle") } + GridLayout { + anchors.left: parent.left + anchors.right: parent.right + columnSpacing: _margin + rowSpacing: _margin + columns: 3 + visible: gridHeader.checked + + QGCLabel { + id: angleText + text: qsTr("Angle") + } + + Item { Layout.fillWidth: true } + + property var activeVehicle: QGroundControl.multiVehicleManager.activeVehicle + ToolButton { + id: windRoseButton + anchors.verticalCenter: angleText.verticalCenter + iconSource: qgcPal.globalTheme === QGCPalette.Light ? "/res/wind-roseBlack.svg" : "/res/wind-rose.svg" + visible: _activeVehicle ? _activeVehicle.fixedWing : true + + onClicked: { + 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 + Layout.columnSpan: 1 } QGCLabel { text: qsTr("Turnaround dist") } @@ -418,11 +450,10 @@ Rectangle { } QGCLabel { - wrapMode: Text.WordWrap - font.pointSize: ScreenTools.smallFontPointSize - text: qsTr("Which value would you like to keep constant as you adjust other settings") + wrapMode: Text.WordWrap + text: qsTr("Select one:") Layout.preferredWidth: parent.width - Layout.columnSpan: 2 + Layout.columnSpan: 2 } QGCRadioButton { @@ -468,13 +499,49 @@ Rectangle { spacing: _margin visible: manualGridHeader.visible && manualGridHeader.checked + GridLayout { + anchors.left: parent.left + anchors.right: parent.right + columnSpacing: _margin + rowSpacing: _margin + columns: 4 + visible: gridHeader.checked + + QGCLabel { + id: manualAngleText + text: qsTr("Angle") + Layout.columnSpan: 1 + Layout.fillWidth: true + } + + property var activeVehicle: QGroundControl.multiVehicleManager.activeVehicle + ToolButton { + id: manualWindRoseButton + anchors.verticalCenter: manualAngleText.verticalCenter + Layout.columnSpan: 1 + iconSource: qgcPal.globalTheme === QGCPalette.Light ? "/res/wind-roseBlack.svg" : "/res/wind-rose.svg" + visible: _activeVehicle ? _activeVehicle.fixedWing : true + + onClicked: { + var cords = manualWindRoseButton.mapToItem(_root, 0, 0) + windRosePie.popup(cords.x + manualWindRoseButton.width / 2, cords.y + manualWindRoseButton.height / 2); + } + } + + FactTextField { + id: manualGridAngleText + fact: missionItem.gridAngle + Layout.columnSpan: 2 + } + } + FactTextFieldGrid { anchors.left: parent.left anchors.right: parent.right columnSpacing: ScreenTools.defaultFontPixelWidth rowSpacing: _margin - factList: [ missionItem.gridAngle, missionItem.gridSpacing, missionItem.gridAltitude, missionItem.turnaroundDist ] - factLabels: [ qsTr("Angle"), qsTr("Spacing"), qsTr("Altitude"), qsTr("Turnaround dist")] + factList: [ missionItem.gridSpacing, missionItem.gridAltitude, missionItem.turnaroundDist ] + factLabels: [ qsTr("Spacing"), qsTr("Altitude"), qsTr("Turnaround dist")] } QGCCheckBox { @@ -517,4 +584,112 @@ Rectangle { } } } + + 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 + } + 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 + } + + 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: 0 + + 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() + } + } + + 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; + + windRosePie.visible = true; + windRosePie.focus = true + } + + MouseArea { + id: mouseArea + anchors.fill: parent + acceptedButtons: Qt.LeftButton | Qt.RightButton + + onClicked: { + windRosePie.visible = false; + } + 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(); + } + } + } } diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index 4bb5ae7469894b67ba41c994baab432e78ddde56..d5a66d3675fbc813a30f849981d97e8992b54d21 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -51,6 +51,7 @@ #include "ScreenToolsController.h" #include "QFileDialogController.h" #include "RCChannelMonitorController.h" +#include "SyslinkComponentController.h" #include "AutoPilotPlugin.h" #include "VehicleComponent.h" #include "FirmwarePluginManager.h" @@ -176,7 +177,6 @@ QGCApplication::QGCApplication(int &argc, char* argv[], bool unitTesting) , _toolbox(NULL) , _bluetoothAvailable(false) { - Q_ASSERT(_app == NULL); _app = this; // This prevents usage of QQuickWidget to fail since it doesn't support native widget siblings @@ -193,11 +193,11 @@ QGCApplication::QGCApplication(int &argc, char* argv[], bool unitTesting) if (getuid() == 0) { QMessageBox msgBox; msgBox.setInformativeText(tr("You are running %1 as root. " - "You should not do this since it will cause other issues with %1. " - "%1 will now exit. " - "If you are having serial port issues on Ubuntu, execute the following commands to fix most issues:\n" - "sudo usermod -a -G dialout $USER\n" - "sudo apt-get remove modemmanager").arg(qgcApp()->applicationName())); + "You should not do this since it will cause other issues with %1. " + "%1 will now exit. " + "If you are having serial port issues on Ubuntu, execute the following commands to fix most issues:\n" + "sudo usermod -a -G dialout $USER\n" + "sudo apt-get remove modemmanager").arg(qgcApp()->applicationName())); msgBox.setStandardButtons(QMessageBox::Ok); msgBox.setDefaultButton(QMessageBox::Ok); msgBox.exec(); @@ -378,19 +378,20 @@ void QGCApplication::_initCommon(void) qmlRegisterUncreatableType ("QGroundControl.JoystickManager", 1, 0, "Joystick", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.QGCPositionManager", 1, 0, "QGCPositionManager", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.FlightMap", 1, 0, "QGCMapPolygon", "Reference only"); + qmlRegisterUncreatableType ("QGroundControl.Controllers", 1, 0, "MissionController", "Reference only"); + qmlRegisterUncreatableType ("QGroundControl.Controllers", 1, 0, "GeoFenceController", "Reference only"); + qmlRegisterUncreatableType("QGroundControl.Controllers", 1, 0, "RallyPointController", "Reference only"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "ParameterEditorController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "ESP8266ComponentController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "ScreenToolsController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "PlanElemementMasterController"); - qmlRegisterType ("QGroundControl.Controllers", 1, 0, "MissionController"); - qmlRegisterType ("QGroundControl.Controllers", 1, 0, "GeoFenceController"); - qmlRegisterType ("QGroundControl.Controllers", 1, 0, "RallyPointController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "ValuesWidgetController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "QFileDialogController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "RCChannelMonitorController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "JoystickConfigController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "LogDownloadController"); + qmlRegisterType ("QGroundControl.Controllers", 1, 0, "SyslinkComponentController"); #ifndef __mobile__ qmlRegisterType ("QGroundControl.Controllers", 1, 0, "ViewWidgetController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "CustomCommandWidgetController"); @@ -473,7 +474,6 @@ void QGCApplication::clearDeleteAllSettingsNextBoot(void) /// @brief Returns the QGCApplication object singleton. QGCApplication* qgcApp(void) { - Q_ASSERT(QGCApplication::_app); return QGCApplication::_app; } @@ -612,19 +612,19 @@ void QGCApplication::reportMissingParameter(int componentId, const QString& name /// Called when the delay timer fires to show the missing parameters warning void QGCApplication::_missingParamsDisplay(void) { - Q_ASSERT(_missingParams.count()); - - QString params; - foreach (const QString &name, _missingParams) { - if (params.isEmpty()) { - params += name; - } else { - params += QString(", %1").arg(name); + if (_missingParams.count()) { + QString params; + foreach (const QString &name, _missingParams) { + if (params.isEmpty()) { + params += name; + } else { + params += QString(", %1").arg(name); + } } - } - _missingParams.clear(); + _missingParams.clear(); - showMessage(QString("Parameters are missing from firmware. You may be running a version of firmware QGC does not work correctly with or your firmware has a bug in it. Missing params: %1").arg(params)); + showMessage(QString("Parameters are missing from firmware. You may be running a version of firmware QGC does not work correctly with or your firmware has a bug in it. Missing params: %1").arg(params)); + } } QObject* QGCApplication::_rootQmlObject() diff --git a/src/QGCApplication.h b/src/QGCApplication.h index 0e9afdabd0ade22ac299140cde0d1f6917d29917..5aea0aa4646bab32fdbd76ecfe348eabc063b38b 100644 --- a/src/QGCApplication.h +++ b/src/QGCApplication.h @@ -126,11 +126,11 @@ public: /// Although public should only be called by main. void _initCommon(void); - /// @brief Intialize the application for normal application boot. Or in other words we are not going to run + /// @brief Initialize the application for normal application boot. Or in other words we are not going to run /// unit tests. Although public should only be called by main. bool _initForNormalAppBoot(void); - /// @brief Intialize the application for normal application boot. Or in other words we are not going to run + /// @brief Initialize the application for normal application boot. Or in other words we are not going to run /// unit tests. Although public should only be called by main. bool _initForUnitTests(void); diff --git a/src/QGCFileDownload.cc b/src/QGCFileDownload.cc index be15da1ea07d01b5892c79936a50d2414a8368eb..9c3cdd0274ed073e7dd4df5b23403d656d25572c 100644 --- a/src/QGCFileDownload.cc +++ b/src/QGCFileDownload.cc @@ -110,19 +110,24 @@ void QGCFileDownload::_downloadFinished(void) // Download file location is in user attribute QString downloadFilename = reply->request().attribute(QNetworkRequest::User).toString(); - Q_ASSERT(!downloadFilename.isEmpty()); - - // Store downloaded file in download location - QFile file(downloadFilename); - if (!file.open(QIODevice::WriteOnly)) { - emit error(QString("Could not save downloaded file to %1. Error: %2").arg(downloadFilename).arg(file.errorString())); - return; - } - - file.write(reply->readAll()); - file.close(); - emit downloadFinished(_originalRemoteFile, downloadFilename); + if (!downloadFilename.isEmpty()) { + // Store downloaded file in download location + QFile file(downloadFilename); + if (!file.open(QIODevice::WriteOnly)) { + emit error(QString("Could not save downloaded file to %1. Error: %2").arg(downloadFilename).arg(file.errorString())); + return; + } + + file.write(reply->readAll()); + file.close(); + + emit downloadFinished(_originalRemoteFile, downloadFilename); + } else { + QString errorMsg = "Internal error"; + qWarning() << errorMsg; + emit error(errorMsg); + } } /// @brief Called when an error occurs during download diff --git a/src/QGCPalette.cc b/src/QGCPalette.cc index c0c2d76b5b9ba017eeb92ad9d5de258642d05bbd..dd55443472eb98cbd33563b329ecc62f80b78aec 100644 --- a/src/QGCPalette.cc +++ b/src/QGCPalette.cc @@ -12,6 +12,8 @@ /// @author Don Gagne #include "QGCPalette.h" +#include "QGCApplication.h" +#include "QGCCorePlugin.h" #include #include @@ -20,42 +22,16 @@ QList QGCPalette::_paletteObjects; QGCPalette::Theme QGCPalette::_theme = QGCPalette::Dark; -// Light Dark -// Disabled Enabled Disabled Enabled -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(warningText, "#cc0808", "#cc0808", "#f85761", "#f85761") -DECLARE_QGC_COLOR(button, "#ffffff", "#ffffff", "#707070", "#626270") -DECLARE_QGC_COLOR(buttonText, "#9d9d9d", "#000000", "#202020", "#ffffff") -DECLARE_QGC_COLOR(buttonHighlight, "#e4e4e4", "#946120", "#3a3a3a", "#fff291") -DECLARE_QGC_COLOR(buttonHighlightText, "#2c2c2c", "#ffffff", "#2c2c2c", "#000000") -DECLARE_QGC_COLOR(primaryButton, "#585858", "#8cb3be", "#585858", "#8cb3be") -DECLARE_QGC_COLOR(primaryButtonText, "#2c2c2c", "#000000", "#2c2c2c", "#000000") -DECLARE_QGC_COLOR(textField, "#ffffff", "#ffffff", "#585858", "#ffffff") -DECLARE_QGC_COLOR(textFieldText, "#dedede", "#000000", "#2c2c2c", "#000000") -DECLARE_QGC_COLOR(mapButton, "#585858", "#000000", "#585858", "#000000") -DECLARE_QGC_COLOR(mapButtonHighlight, "#585858", "#be781c", "#585858", "#be781c") -DECLARE_QGC_COLOR(colorGreen, "#009431", "#009431", "#00e04b", "#00e04b") -DECLARE_QGC_COLOR(colorOrange, "#b95604", "#b95604", "#de8500", "#de8500") -DECLARE_QGC_COLOR(colorRed, "#ed3939", "#ed3939", "#f32836", "#f32836") -DECLARE_QGC_COLOR(colorGrey, "#808080", "#808080", "#bfbfbf", "#bfbfbf") -DECLARE_QGC_COLOR(colorBlue, "#1a72ff", "#1a72ff", "#536dff", "#536dff") -DECLARE_QGC_COLOR(alertBackground, "#eecc44", "#eecc44", "#eecc44", "#eecc44") -DECLARE_QGC_COLOR(alertBorder, "#808080", "#808080", "#808080", "#808080") -DECLARE_QGC_COLOR(alertText, "#000000", "#000000", "#000000", "#000000") - -// Colors are not affecting by theming -DECLARE_QGC_COLOR(mapWidgetBorderLight, "#ffffff", "#ffffff", "#ffffff", "#ffffff") -DECLARE_QGC_COLOR(mapWidgetBorderDark, "#000000", "#000000", "#000000", "#000000") -DECLARE_QGC_COLOR(brandingPurple, "#4A2C6D", "#4A2C6D", "#4A2C6D", "#4A2C6D") -DECLARE_QGC_COLOR(brandingBlue, "#48D6FF", "#48D6FF", "#48D6FF", "#48D6FF") +QMap>> QGCPalette::_colorInfoMap; QGCPalette::QGCPalette(QObject* parent) : QObject(parent), _colorGroupEnabled(true) { + if (_colorInfoMap.isEmpty()) { + _buildMap(); + } + // We have to keep track of all QGCPalette objects in the system so we can signal theme change to all of them _paletteObjects += this; } @@ -63,8 +39,45 @@ QGCPalette::QGCPalette(QObject* parent) : QGCPalette::~QGCPalette() { bool fSuccess = _paletteObjects.removeOne(this); - Q_ASSERT(fSuccess); - Q_UNUSED(fSuccess); + if (!fSuccess) { + qWarning() << "Internal error"; + } +} + +void QGCPalette::_buildMap(void) +{ + // Light Dark + // Disabled Enabled Disabled Enabled + 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(warningText, "#cc0808", "#cc0808", "#f85761", "#f85761") + DECLARE_QGC_COLOR(button, "#ffffff", "#ffffff", "#707070", "#626270") + DECLARE_QGC_COLOR(buttonText, "#9d9d9d", "#000000", "#202020", "#ffffff") + DECLARE_QGC_COLOR(buttonHighlight, "#e4e4e4", "#946120", "#3a3a3a", "#fff291") + DECLARE_QGC_COLOR(buttonHighlightText, "#2c2c2c", "#ffffff", "#2c2c2c", "#000000") + DECLARE_QGC_COLOR(primaryButton, "#585858", "#8cb3be", "#585858", "#8cb3be") + DECLARE_QGC_COLOR(primaryButtonText, "#2c2c2c", "#000000", "#2c2c2c", "#000000") + DECLARE_QGC_COLOR(textField, "#ffffff", "#ffffff", "#585858", "#ffffff") + DECLARE_QGC_COLOR(textFieldText, "#dedede", "#000000", "#2c2c2c", "#000000") + DECLARE_QGC_COLOR(mapButton, "#585858", "#000000", "#585858", "#000000") + DECLARE_QGC_COLOR(mapButtonHighlight, "#585858", "#be781c", "#585858", "#be781c") + DECLARE_QGC_COLOR(colorGreen, "#009431", "#009431", "#00e04b", "#00e04b") + DECLARE_QGC_COLOR(colorOrange, "#b95604", "#b95604", "#de8500", "#de8500") + DECLARE_QGC_COLOR(colorRed, "#ed3939", "#ed3939", "#f32836", "#f32836") + DECLARE_QGC_COLOR(colorGrey, "#808080", "#808080", "#bfbfbf", "#bfbfbf") + DECLARE_QGC_COLOR(colorBlue, "#1a72ff", "#1a72ff", "#536dff", "#536dff") + DECLARE_QGC_COLOR(alertBackground, "#eecc44", "#eecc44", "#eecc44", "#eecc44") + DECLARE_QGC_COLOR(alertBorder, "#808080", "#808080", "#808080", "#808080") + DECLARE_QGC_COLOR(alertText, "#000000", "#000000", "#000000", "#000000") + DECLARE_QGC_COLOR(missionItemEditor, "#585858", "#8cb3be", "#585858", "#8cb3be") + + // Colors are not affecting by theming + DECLARE_QGC_COLOR(mapWidgetBorderLight, "#ffffff", "#ffffff", "#ffffff", "#ffffff") + DECLARE_QGC_COLOR(mapWidgetBorderDark, "#000000", "#000000", "#000000", "#000000") + DECLARE_QGC_COLOR(brandingPurple, "#4A2C6D", "#4A2C6D", "#4A2C6D", "#4A2C6D") + DECLARE_QGC_COLOR(brandingBlue, "#48D6FF", "#48D6FF", "#48D6FF", "#48D6FF") } void QGCPalette::setColorGroupEnabled(bool enabled) diff --git a/src/QGCPalette.h b/src/QGCPalette.h index ae28a9686bdaf49e143c519c181c6709e328e11e..1c47116b8db1f75186cd827ada587d18b926b84d 100644 --- a/src/QGCPalette.h +++ b/src/QGCPalette.h @@ -12,22 +12,25 @@ #include #include - -#define QGCColorThemes 2 -#define QGCColorGroups 2 - - -#define DECLARE_QGC_COLOR(name, lightEnabled, lightDisabled, darkEnabled, darkDisabled) \ - QColor QGCPalette::_##name[QGCColorThemes][QGCColorGroups] = { \ - { QColor(lightEnabled), QColor(lightDisabled) }, \ - { QColor(darkEnabled), QColor(darkDisabled) }, \ - }; +#include + +#define DECLARE_QGC_COLOR(name, lightDisabled, lightEnabled, darkDisabled, darkEnabled) \ + { \ + PaletteColorInfo_t colorInfo = { \ + { QColor(lightDisabled), QColor(lightEnabled) }, \ + { QColor(darkDisabled), QColor(darkEnabled) } \ + }; \ + qgcApp()->toolbox()->corePlugin()->paletteOverride(#name, colorInfo); \ + _colorInfoMap[Light][ColorGroupEnabled][QStringLiteral(#name)] = colorInfo[Light][ColorGroupEnabled]; \ + _colorInfoMap[Light][ColorGroupDisabled][QStringLiteral(#name)] = colorInfo[Light][ColorGroupDisabled]; \ + _colorInfoMap[Dark][ColorGroupEnabled][QStringLiteral(#name)] = colorInfo[Dark][ColorGroupEnabled]; \ + _colorInfoMap[Dark][ColorGroupDisabled][QStringLiteral(#name)] = colorInfo[Dark][ColorGroupDisabled]; \ + } #define DEFINE_QGC_COLOR(name, setName) \ Q_PROPERTY(QColor name READ name WRITE setName NOTIFY paletteChanged) \ - QColor name() const { return _##name[_theme][_colorGroupEnabled ? 1 : 0]; } \ - void setName(QColor& color) { _##name[_theme][_colorGroupEnabled ? 1 : 0] = color; _signalPaletteChangeToAll(); } \ - static QColor _##name[QGCColorThemes][QGCColorGroups]; + QColor name() const { return _colorInfoMap[_theme][_colorGroupEnabled ? ColorGroupEnabled : ColorGroupDisabled][QStringLiteral(#name)]; } \ + void setName(QColor& color) { _colorInfoMap[_theme][_colorGroupEnabled ? ColorGroupEnabled : ColorGroupDisabled][QStringLiteral(#name)] = color; _signalPaletteChangeToAll(); } /*! QGCPalette is used in QML ui to expose color properties for the QGC palette. There are two @@ -50,20 +53,24 @@ class QGCPalette : public QObject { Q_OBJECT + Q_ENUMS(Theme) public: - enum ColorGroup { - Disabled = 0, - Enabled + ColorGroupDisabled = 0, + ColorGroupEnabled, + cMaxColorGroup }; enum Theme { Light = 0, - Dark + Dark, + cMaxTheme }; + typedef QColor PaletteColorInfo_t[cMaxTheme][cMaxColorGroup]; + Q_PROPERTY(Theme globalTheme READ globalTheme WRITE setGlobalTheme NOTIFY paletteChanged) Q_PROPERTY(bool colorGroupEnabled READ colorGroupEnabled WRITE setColorGroupEnabled NOTIFY paletteChanged) @@ -94,27 +101,31 @@ public: DEFINE_QGC_COLOR(alertBackground, setAlertBackground) DEFINE_QGC_COLOR(alertBorder, setAlertBorder) DEFINE_QGC_COLOR(alertText, setAlertText) + DEFINE_QGC_COLOR(missionItemEditor, setMissionItemEditor) QGCPalette(QObject* parent = NULL); ~QGCPalette(); - bool colorGroupEnabled () const { return _colorGroupEnabled; } + bool colorGroupEnabled (void) const { return _colorGroupEnabled; } void setColorGroupEnabled (bool enabled); - static Theme globalTheme () { return _theme; } + static Theme globalTheme (void) { return _theme; } static void setGlobalTheme (Theme newTheme); signals: void paletteChanged (); private: - static void _signalPaletteChangeToAll (); - void _signalPaletteChanged (); - void _themeChanged (); + static void _buildMap (void); + static void _signalPaletteChangeToAll (void); + void _signalPaletteChanged (void); + void _themeChanged (void); static Theme _theme; ///< There is a single theme for all palettes bool _colorGroupEnabled; ///< Currently selected ColorGroup. true: enabled, false: disabled - static QList _paletteObjects; ///< List of all active QGCPalette objects + + static QMap>> _colorInfoMap; // theme -> colorGroup -> color name -> color + static QList _paletteObjects; ///< List of all active QGCPalette objects }; #endif diff --git a/src/QGCQFileDialog.cc b/src/QGCQFileDialog.cc index 0a4932fcb09020e1f6df42a6ea4b621d31365d95..21fa68b034290492e57cf3730a41f41d02e1db3f 100644 --- a/src/QGCQFileDialog.cc +++ b/src/QGCQFileDialog.cc @@ -126,7 +126,9 @@ QString QGCQFileDialog::getSaveFileName( defaultSuffixCopy = _getFirstExtensionInFilter(filter); } //-- If this is set to strict, we have to have a default extension - Q_ASSERT(defaultSuffixCopy.isEmpty() == false); + if (defaultSuffixCopy.isEmpty()) { + qWarning() << "Internal error"; + } //-- Forcefully append our desired extension result += "."; result += defaultSuffixCopy; @@ -197,9 +199,13 @@ void QGCQFileDialog::_validate(Options& options) Q_UNUSED(options) // You can't use QGCQFileDialog if QGCApplication is not created yet. - Q_ASSERT(qgcApp()); + if (!qgcApp()) { + qWarning() << "Internal error"; + return; + } - Q_ASSERT_X(QThread::currentThread() == qgcApp()->thread(), "Threading issue", "QGCQFileDialog can only be called from main thread"); - if (MainWindow::instance()) { + if (QThread::currentThread() != qgcApp()->thread()) { + qWarning() << "Threading issue: QGCQFileDialog can only be called from main thread"; + return; } } diff --git a/src/QmlControls/MavlinkQmlSingleton.h b/src/QmlControls/MavlinkQmlSingleton.h index 95268c0325c33e06f0f1be6a49c4212bded13616..fa064e6c43549f072fca7ffe32948311f32da777 100644 --- a/src/QmlControls/MavlinkQmlSingleton.h +++ b/src/QmlControls/MavlinkQmlSingleton.h @@ -97,7 +97,7 @@ public: MAV_CMD_VIDEO_STOP_CAPTURE=2501, /* Stop the current video capture |Reserved| Reserved| */ 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 overriden 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_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| */ diff --git a/src/QmlControls/ParameterEditorController.cc b/src/QmlControls/ParameterEditorController.cc index 66385dc9edbcdb17681f32c30146559fa7c7d2fe..9520b9eb9783753b6169195b1f52ea104132d274 100644 --- a/src/QmlControls/ParameterEditorController.cc +++ b/src/QmlControls/ParameterEditorController.cc @@ -89,8 +89,9 @@ QStringList ParameterEditorController::searchParametersForComponent(int componen void ParameterEditorController::clearRCToParam(void) { - Q_ASSERT(_uas); - _uas->unsetRCToParameterMap(); + if (_uas) { + _uas->unsetRCToParameterMap(); + } } void ParameterEditorController::saveToFile(const QString& filename) @@ -147,9 +148,10 @@ void ParameterEditorController::setRCToParam(const QString& paramName) #ifdef __mobile__ Q_UNUSED(paramName) #else - Q_ASSERT(_uas); - QGCMapRCToParamDialog * d = new QGCMapRCToParamDialog(paramName, _uas, qgcApp()->toolbox()->multiVehicleManager(), MainWindow::instance()); - d->exec(); + if (_uas) { + QGCMapRCToParamDialog * d = new QGCMapRCToParamDialog(paramName, _uas, qgcApp()->toolbox()->multiVehicleManager(), MainWindow::instance()); + d->exec(); + } #endif } diff --git a/src/QmlControls/ParameterEditorDialog.qml b/src/QmlControls/ParameterEditorDialog.qml index ccd7a6e27d6d531dd65a87443bd52f56d5392481..5ec4dbb79d42bf85394ff9714c28291af87f08ee 100644 --- a/src/QmlControls/ParameterEditorDialog.qml +++ b/src/QmlControls/ParameterEditorDialog.qml @@ -138,7 +138,9 @@ QGCViewDialog { } onCurrentIndexChanged: { - valueField.text = fact.enumValues[currentIndex] + if (currentIndex >=0 && currentIndex < model.length) { + valueField.text = fact.enumValues[currentIndex] + } } } diff --git a/src/QmlControls/QGCCheckBox.qml b/src/QmlControls/QGCCheckBox.qml index 424e244438ea79917e90e2b06d0512608a7b1329..735138d28ec87d8a44f310cc35b72e0217c94054 100644 --- a/src/QmlControls/QGCCheckBox.qml +++ b/src/QmlControls/QGCCheckBox.qml @@ -11,7 +11,7 @@ CheckBox { style: CheckBoxStyle { label: Item { implicitWidth: text.implicitWidth + 2 - implicitHeight: text.implicitHeight + implicitHeight: ScreenTools.implicitCheckBoxHeight baselineOffset: text.baselineOffset Rectangle { @@ -39,7 +39,7 @@ CheckBox { } // label indicator: Item { - implicitWidth: ScreenTools.implicitCheckBoxWidth + implicitWidth: ScreenTools.checkBoxIndicatorSize implicitHeight: implicitWidth Rectangle { diff --git a/src/QmlControls/QGCComboBox.qml b/src/QmlControls/QGCComboBox.qml index f703d6d64ce750a42c820c8686638ab11271ef23..32634b505c63d29c8ab967c757ff5ac066083027 100644 --- a/src/QmlControls/QGCComboBox.qml +++ b/src/QmlControls/QGCComboBox.qml @@ -210,7 +210,7 @@ Button { onObjectAdded: { // There is a bug in Instantiator which can cause objects to be added out of order from an index standpoint. - // If not handled correcty this will cause menu items to be added incorrectly due to the way Menu.insertItem works. + // If not handled correctly this will cause menu items to be added incorrectly due to the way Menu.insertItem works. //console.log("menu add", index, object.text) if (index === popup.__selectedIndex) { popup.selectedText = object["text"] diff --git a/src/QmlControls/QGCFileDialog.qml b/src/QmlControls/QGCFileDialog.qml index 64cb4aaa759520f6dd5ca41c3cecc0ea22bca3f0..5ef9432add7e6cf66fd192bb8041dc4ccbbfbe4c 100644 --- a/src/QmlControls/QGCFileDialog.qml +++ b/src/QmlControls/QGCFileDialog.qml @@ -18,6 +18,7 @@ Item { property string folder property var nameFilters property string fileExtension + property string fileExtension2 property string title property bool selectExisting property bool selectFolder @@ -90,7 +91,7 @@ Item { spacing: ScreenTools.defaultFontPixelHeight / 2 Repeater { - id: fileList; + id: fileList model: controller.getFiles(folder, fileExtension) QGCButton { @@ -105,9 +106,25 @@ Item { } } + Repeater { + id: fileList2 + model: fileExtension2 == "" ? [ ] : controller.getFiles(folder, fileExtension2) + + QGCButton { + anchors.left: parent.left + anchors.right: parent.right + text: modelData + + onClicked: { + hideDialog() + _root.acceptedForLoad(controller.fullyQualifiedFilename(folder, modelData, fileExtension2)) + } + } + } + QGCLabel { text: qsTr("No files") - visible: fileList.model.length == 0 + visible: fileList.model.length == 0 && fileList2.model.length == 0 } } } diff --git a/src/QmlControls/QGCRadioButton.qml b/src/QmlControls/QGCRadioButton.qml index 4eba07f153f1cb8bac9e8a0f8261bb8b59577cab..ccaaf0e4cb8813a14e1ac83a55f2f7c24387737f 100644 --- a/src/QmlControls/QGCRadioButton.qml +++ b/src/QmlControls/QGCRadioButton.qml @@ -6,16 +6,15 @@ import QGroundControl.Palette 1.0 import QGroundControl.ScreenTools 1.0 RadioButton { - property var color: qgcPal.text ///< Text color - property int textStyle: Text.Normal - property color textStyleColor: qgcPal.text - - QGCPalette { id: qgcPal; colorGroupEnabled: enabled } + property var color: qgcPal.text ///< Text color + property int textStyle: Text.Normal + property color textStyleColor: qgcPal.text + property var qgcPal: QGCPalette { colorGroupEnabled: enabled } style: RadioButtonStyle { label: Item { implicitWidth: text.implicitWidth + ScreenTools.defaultFontPixelWidth * 0.25 - implicitHeight: text.implicitHeight + implicitHeight: ScreenTools.implicitRadioButtonHeight baselineOffset: text.y + text.baselineOffset Rectangle { @@ -43,5 +42,24 @@ RadioButton { anchors.centerIn: parent } } + + indicator: Rectangle { + width: ScreenTools.radioButtonIndicatorSize + height: width + color: "white" + border.color: control.qgcPal.text + antialiasing: true + radius: height / 2 + + Rectangle { + anchors.centerIn: parent + width: Math.round(parent.width * 0.5) + height: width + antialiasing: true + radius: height / 2 + color: "black" + opacity: control.checked ? (control.enabled ? 1 : 0.5) : 0 + } + } } } diff --git a/src/QmlControls/QGCToolBarButton.qml b/src/QmlControls/QGCToolBarButton.qml index c73ec75b83b25b67cd8e0dae1a1083c2788684b7..cb2681fc66b9f7867e26181f1386374e19fe72fd 100644 --- a/src/QmlControls/QGCToolBarButton.qml +++ b/src/QmlControls/QGCToolBarButton.qml @@ -26,10 +26,10 @@ Item { property bool logo: false property ExclusiveGroup exclusiveGroup: null - readonly property real _topBottomMargins: ScreenTools.defaultFontPixelHeight / 2 - signal clicked() + readonly property real _topBottomMargins: ScreenTools.defaultFontPixelHeight / 2 + onExclusiveGroupChanged: { if (exclusiveGroup) { exclusiveGroup.bindCheckable(_root) @@ -41,7 +41,7 @@ Item { Rectangle { anchors.fill: parent visible: logo - color: "#4A2C6D" + color: qgcPal.brandingPurple } QGCColoredImage { diff --git a/src/QmlControls/QGroundControlQmlGlobal.cc b/src/QmlControls/QGroundControlQmlGlobal.cc index 4fbce6a3204ff968436c460394ae571e9b79bd0d..eb164e0a0225accf4e1f35bacc71b1b79898332a 100644 --- a/src/QmlControls/QGroundControlQmlGlobal.cc +++ b/src/QmlControls/QGroundControlQmlGlobal.cc @@ -26,7 +26,7 @@ const char* QGroundControlQmlGlobal::_flightMapZoomSettingsKey = QGroundControlQmlGlobal::QGroundControlQmlGlobal(QGCApplication* app, QGCToolbox* toolbox) : QGCTool(app, toolbox) - , _flightMapInitialZoom(14.7) // About 500 meter scale + , _flightMapInitialZoom(17.0) , _linkManager(NULL) , _multiVehicleManager(NULL) , _mapEngineManager(NULL) diff --git a/src/QmlControls/ScreenTools.qml b/src/QmlControls/ScreenTools.qml index 324b8fb8b75a6719d5d9fd4fe16be557e8fa1da7..fc13c3d41333867f90cf7e867c581e71a1d14cf5 100644 --- a/src/QmlControls/ScreenTools.qml +++ b/src/QmlControls/ScreenTools.qml @@ -64,13 +64,16 @@ Item { property real minTouchPixels: 0 ///< Minimum touch size in pixels // The implicit heights/widths for our custom control set - property real implicitButtonWidth: Math.round(defaultFontPixelWidth * (isMobile ? 7.0 : 5.0)) - property real implicitButtonHeight: Math.round(defaultFontPixelHeight * (isMobile ? 2.0 : 1.6)) - property real implicitCheckBoxWidth: Math.round(defaultFontPixelHeight * (isMobile ? 1.5 : 1.0)) - property real implicitTextFieldHeight: Math.round(defaultFontPixelHeight * (isMobile ? 2.0 : 1.6)) - property real implicitComboBoxHeight: Math.round(defaultFontPixelHeight * (isMobile ? 2.0 : 1.6)) - property real implicitComboBoxWidth: Math.round(defaultFontPixelWidth * (isMobile ? 7.0 : 5.0)) - property real implicitSliderHeight: isMobile ? Math.max(defaultFontPixelHeight, minTouchPixels) : defaultFontPixelHeight + property real implicitButtonWidth: Math.round(defaultFontPixelWidth * (isMobile ? 7.0 : 5.0)) + property real implicitButtonHeight: Math.round(defaultFontPixelHeight * (isMobile ? 2.0 : 1.6)) + property real implicitCheckBoxHeight: Math.round(defaultFontPixelHeight * (isMobile ? 2.0 : 1.0)) + property real implicitRadioButtonHeight: implicitCheckBoxHeight + property real implicitTextFieldHeight: Math.round(defaultFontPixelHeight * (isMobile ? 2.0 : 1.6)) + property real implicitComboBoxHeight: Math.round(defaultFontPixelHeight * (isMobile ? 2.0 : 1.6)) + property real implicitComboBoxWidth: Math.round(defaultFontPixelWidth * (isMobile ? 7.0 : 5.0)) + property real implicitSliderHeight: isMobile ? Math.max(defaultFontPixelHeight, minTouchPixels) : defaultFontPixelHeight + property real checkBoxIndicatorSize: Math.round(defaultFontPixelHeight * (isMobile ? 1.5 : 1.0)) + property real radioButtonIndicatorSize: checkBoxIndicatorSize readonly property string normalFontFamily: "opensans" readonly property string demiboldFontFamily: "opensans-demibold" diff --git a/src/Settings/App.SettingsGroup.json b/src/Settings/App.SettingsGroup.json index 9f66e7c877a606f21ce8989c0ff40d693eb5c941..30e9d4520c17330db6a263d3c5b354763c044376 100644 --- a/src/Settings/App.SettingsGroup.json +++ b/src/Settings/App.SettingsGroup.json @@ -5,7 +5,7 @@ "type": "uint32", "enumStrings": "ArduPilot,PX4 Pro,Mavlink Generic", "enumValues": "3,12,0", - "defaultValue": 3 + "defaultValue": 12 }, { "name": "OfflineEditingVehicleType", @@ -13,14 +13,14 @@ "type": "uint32", "enumStrings": "Fixed Wing,Multi-Rotor,VTOL,Rover,Sub", "enumValues": "1,2,19,10,12", - "defaultValue": 1 + "defaultValue": 2 }, { "name": "OfflineEditingCruiseSpeed", "shortDescription": "Offline editing cruise speed", "longDescription": "This value defines the cruising speed for forward flight vehicles for use in calculating mission duration when not connected to a vehicle.", "type": "double", - "defaultValue": 16.0, + "defaultValue": 15.0, "min": 1.0, "units": "m/s", "decimalPlaces": 2 @@ -30,7 +30,7 @@ "shortDescription": "Offline editing hover speed", "longDescription": "This value defines the cruising speed for multi-rotor vehicles for use in calculating mission duration when not connected to a vehicle.", "type": "double", - "defaultValue": 4.0, + "defaultValue": 5.0, "min": 1.0, "units": "m/s", "decimalPlaces": 2 @@ -116,13 +116,6 @@ "type": "bool", "defaultValue": false }, -{ - "name": "AutomaticMissionUpload", - "shortDescription": "Automatic mission upload", - "longDescription": "Automatically upload mission to vehicle after changes", - "type": "bool", - "defaultValue": true -}, { "name": "SavePath", "shortDescription": "Application save directory", diff --git a/src/Settings/AppSettings.cc b/src/Settings/AppSettings.cc index 80f616188a807497003fd2435a31dfd26fc35ea8..f5c0051311d363b263760e34207925832c8234ed 100644 --- a/src/Settings/AppSettings.cc +++ b/src/Settings/AppSettings.cc @@ -31,11 +31,11 @@ const char* AppSettings::indoorPaletteName = "StyleIs const char* AppSettings::showLargeCompassName = "ShowLargeCompass"; const char* AppSettings::savePathName = "SavePath"; const char* AppSettings::autoLoadMissionsName = "AutoLoadMissions"; -const char* AppSettings::automaticMissionUploadName = "AutomaticMissionUpload"; const char* AppSettings::parameterFileExtension = "params"; const char* AppSettings::planFileExtension = "plan"; const char* AppSettings::missionFileExtension = "mission"; +const char* AppSettings::waypointsFileExtension = "waypoints"; const char* AppSettings::fenceFileExtension = "fence"; const char* AppSettings::rallyPointFileExtension = "rally"; const char* AppSettings::telemetryFileExtension = "tlog"; @@ -61,7 +61,6 @@ AppSettings::AppSettings(QObject* parent) , _showLargeCompassFact(NULL) , _savePathFact(NULL) , _autoLoadMissionsFact(NULL) - , _automaticMissionUpload(NULL) { QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); qmlRegisterUncreatableType("QGroundControl.SettingsManager", 1, 0, "AppSettings", "Reference only"); @@ -279,12 +278,26 @@ Fact* AppSettings::autoLoadMissions(void) return _autoLoadMissionsFact; } -Fact* AppSettings::automaticMissionUpload(void) +MAV_AUTOPILOT AppSettings::offlineEditingFirmwareTypeFromFirmwareType(MAV_AUTOPILOT firmwareType) { - if (!_automaticMissionUpload) { - _automaticMissionUpload = _createSettingsFact(automaticMissionUploadName); + if (firmwareType != MAV_AUTOPILOT_PX4 && firmwareType != MAV_AUTOPILOT_ARDUPILOTMEGA) { + firmwareType = MAV_AUTOPILOT_GENERIC; } + return firmwareType; +} - return _automaticMissionUpload; +MAV_TYPE AppSettings::offlineEditingVehicleTypeFromVehicleType(MAV_TYPE vehicleType) +{ + if (QGCMAVLink::isRover(vehicleType)) { + return MAV_TYPE_GROUND_ROVER; + } else if (QGCMAVLink::isSub(vehicleType)) { + return MAV_TYPE_SUBMARINE; + } else if (QGCMAVLink::isVTOL(vehicleType)) { + return MAV_TYPE_VTOL_QUADROTOR; + } else if (QGCMAVLink::isFixedWing(vehicleType)) { + return MAV_TYPE_FIXED_WING; + } else { + return MAV_TYPE_QUADROTOR; + } } diff --git a/src/Settings/AppSettings.h b/src/Settings/AppSettings.h index bc98a51f05d4fb2427022ceb91303df79c690919..fbbfd587456550d5e0e89350744219aa41845f0a 100644 --- a/src/Settings/AppSettings.h +++ b/src/Settings/AppSettings.h @@ -11,6 +11,7 @@ #define AppSettings_H #include "SettingsGroup.h" +#include "QGCMAVLink.h" class AppSettings : public SettingsGroup { @@ -34,7 +35,6 @@ 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* automaticMissionUpload READ automaticMissionUpload CONSTANT) Q_PROPERTY(QString missionSavePath READ missionSavePath NOTIFY savePathsChanged) Q_PROPERTY(QString parameterSavePath READ parameterSavePath NOTIFY savePathsChanged) @@ -42,6 +42,7 @@ public: Q_PROPERTY(QString planFileExtension MEMBER planFileExtension CONSTANT) Q_PROPERTY(QString missionFileExtension MEMBER missionFileExtension CONSTANT) + Q_PROPERTY(QString waypointsFileExtension MEMBER waypointsFileExtension CONSTANT) Q_PROPERTY(QString parameterFileExtension MEMBER parameterFileExtension CONSTANT) Q_PROPERTY(QString telemetryFileExtension MEMBER telemetryFileExtension CONSTANT) @@ -60,12 +61,14 @@ public: Fact* showLargeCompass (void); Fact* savePath (void); Fact* autoLoadMissions (void); - Fact* automaticMissionUpload (void); QString missionSavePath (void); QString parameterSavePath (void); QString telemetrySavePath (void); + static MAV_AUTOPILOT offlineEditingFirmwareTypeFromFirmwareType(MAV_AUTOPILOT firmwareType); + static MAV_TYPE offlineEditingVehicleTypeFromVehicleType(MAV_TYPE vehicleType); + static const char* appSettingsGroupName; static const char* offlineEditingFirmwareTypeSettingsName; @@ -83,12 +86,12 @@ public: static const char* showLargeCompassName; static const char* savePathName; static const char* autoLoadMissionsName; - static const char* automaticMissionUploadName; // Application wide file extensions static const char* parameterFileExtension; static const char* planFileExtension; static const char* missionFileExtension; + static const char* waypointsFileExtension; static const char* fenceFileExtension; static const char* rallyPointFileExtension; static const char* telemetryFileExtension; @@ -121,7 +124,6 @@ private: SettingsFact* _showLargeCompassFact; SettingsFact* _savePathFact; SettingsFact* _autoLoadMissionsFact; - SettingsFact* _automaticMissionUpload; }; #endif diff --git a/src/Settings/Video.SettingsGroup.json b/src/Settings/Video.SettingsGroup.json index 260a872a64c7ba927a663936adcad5c1e274106e..b3f39e27db017e6f0981e07dea5a52af14e55a10 100644 --- a/src/Settings/Video.SettingsGroup.json +++ b/src/Settings/Video.SettingsGroup.json @@ -39,7 +39,7 @@ { "name": "VideoGridLines", "shortDescription": "Video Grid Lines", - "longDescription": "Displays a grid overlayed over the video view.", + "longDescription": "Displays a grid overlaid over the video view.", "type": "uint32", "enumStrings": "Hide,Show", "enumValues": "1,0", diff --git a/src/Vehicle/MAVLinkLogManager.cc b/src/Vehicle/MAVLinkLogManager.cc index ca56c1ff1b50a3ff4a6abdecbdecd520976f30ac..efeca2c8acd1609b4e373138aab57dd5b807eae7 100644 --- a/src/Vehicle/MAVLinkLogManager.cc +++ b/src/Vehicle/MAVLinkLogManager.cc @@ -150,10 +150,10 @@ bool MAVLinkLogProcessor::create(MAVLinkLogManager* manager, const QString path, uint8_t id) { _fileName.sprintf("%s/%03d-%s%s", - path.toLatin1().data(), - id, - QDateTime::currentDateTime().toString("yyyy-MM-dd-hh-mm-ss-zzz").toLatin1().data(), - kUlogExtension); + path.toLatin1().data(), + id, + QDateTime::currentDateTime().toString("yyyy-MM-dd-hh-mm-ss-zzz").toLatin1().data(), + kUlogExtension); _fd = fopen(_fileName.toLatin1().data(), "wb"); if(_fd) { _record = new MAVLinkLogFiles(manager, _fileName, true); @@ -264,7 +264,7 @@ MAVLinkLogProcessor::processStreamData(uint16_t sequence, uint8_t first_message, if(num_drops > 0) { _writeUlogMessage(_ulogMessage); _ulogMessage.clear(); - //-- If no usefull information in this message. Drop it. + //-- If no useful information in this message. Drop it. if(first_message == 255) { break; } @@ -498,17 +498,20 @@ MAVLinkLogManager::uploadLog() } for(int i = 0; i < _logFiles.count(); i++) { _currentLogfile = qobject_cast(_logFiles.get(i)); - Q_ASSERT(_currentLogfile); - if(_currentLogfile->selected()) { - _currentLogfile->setSelected(false); - if(!_currentLogfile->uploaded() && !_emailAddress.isEmpty() && !_uploadURL.isEmpty()) { - _currentLogfile->setUploading(true); - _currentLogfile->setProgress(0.0); - QString filePath = _makeFilename(_currentLogfile->name()); - _sendLog(filePath); - emit uploadingChanged(); - return; + if (_currentLogfile) { + if(_currentLogfile->selected()) { + _currentLogfile->setSelected(false); + if(!_currentLogfile->uploaded() && !_emailAddress.isEmpty() && !_uploadURL.isEmpty()) { + _currentLogfile->setUploading(true); + _currentLogfile->setProgress(0.0); + QString filePath = _makeFilename(_currentLogfile->name()); + _sendLog(filePath); + emit uploadingChanged(); + return; + } } + } else { + qWarning() << "Internal error"; } } _currentLogfile = NULL; @@ -541,9 +544,12 @@ MAVLinkLogManager::_getFirstSelected() { for(int i = 0; i < _logFiles.count(); i++) { MAVLinkLogFiles* f = qobject_cast(_logFiles.get(i)); - Q_ASSERT(f); - if(f->selected()) { - return i; + if (f) { + if(f->selected()) { + return i; + } + } else { + qWarning() << "Internal error"; } } return -1; @@ -590,9 +596,12 @@ MAVLinkLogManager::cancelUpload() { for(int i = 0; i < _logFiles.count(); i++) { MAVLinkLogFiles* pLogFile = qobject_cast(_logFiles.get(i)); - Q_ASSERT(pLogFile); - if(pLogFile->selected() && pLogFile != _currentLogfile) { - pLogFile->setSelected(false); + if (pLogFile) { + if(pLogFile->selected() && pLogFile != _currentLogfile) { + pLogFile->setSelected(false); + } + } else { + qWarning() << "Internal error"; } } if(_currentLogfile) { diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index 23eb01f2bdc266065d2f778f269ed1829a2c5ec6..2316d6ed93d97058a215ae5197540867e0c9f030 100644 --- a/src/Vehicle/MultiVehicleManager.cc +++ b/src/Vehicle/MultiVehicleManager.cc @@ -16,6 +16,8 @@ #include "QGroundControlQmlGlobal.h" #include "ParameterManager.h" #include "SettingsManager.h" +#include "QGCCorePlugin.h" +#include "QGCOptions.h" #if defined (__ios__) || defined(__android__) #include "MobileScreenMgr.h" @@ -72,6 +74,9 @@ void MultiVehicleManager::setToolbox(QGCToolbox *toolbox) void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleMavlinkVersion, int vehicleFirmwareType, int vehicleType) { + if (_vehicles.count() > 0 && !qgcApp()->toolbox()->corePlugin()->options()->multiVehicleEnabled()) { + return; + } if (_ignoreVehicleIds.contains(vehicleId) || getVehicleById(vehicleId) || vehicleId == 0) { return; } diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index a4eb9e205fd4038f52f067875519a915289eafe2..d961c08eb2d178d7709ef27dc56d2e74c1d52083 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -29,6 +29,7 @@ #include "MissionCommandTree.h" #include "QGroundControlQmlGlobal.h" #include "SettingsManager.h" +#include "QGCQGeoCoordinate.h" QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog") @@ -51,6 +52,7 @@ const char* Vehicle::_climbRateFactName = "climbRate"; const char* Vehicle::_altitudeRelativeFactName = "altitudeRelative"; const char* Vehicle::_altitudeAMSLFactName = "altitudeAMSL"; const char* Vehicle::_flightDistanceFactName = "flightDistance"; +const char* Vehicle::_flightTimeFactName = "flightTime"; const char* Vehicle::_gpsFactGroupName = "gps"; const char* Vehicle::_batteryFactGroupName = "battery"; @@ -93,6 +95,7 @@ Vehicle::Vehicle(LinkInterface* link, , _rcRSSIstore(255) , _autoDisconnect(false) , _flying(false) + , _landing(false) , _onboardControlSensorsPresent(0) , _onboardControlSensorsEnabled(0) , _onboardControlSensorsHealth(0) @@ -112,6 +115,7 @@ Vehicle::Vehicle(LinkInterface* link, , _supportsMissionItemInt(false) , _connectionLost(false) , _connectionLostEnabled(true) + , _initialPlanRequestComplete(false) , _missionManager(NULL) , _missionManagerInitialRequestSent(false) , _geoFenceManager(NULL) @@ -147,6 +151,7 @@ Vehicle::Vehicle(LinkInterface* link, , _altitudeRelativeFact (0, _altitudeRelativeFactName, FactMetaData::valueTypeDouble) , _altitudeAMSLFact (0, _altitudeAMSLFactName, FactMetaData::valueTypeDouble) , _flightDistanceFact (0, _flightDistanceFactName, FactMetaData::valueTypeDouble) + , _flightTimeFact (0, _flightTimeFactName, FactMetaData::valueTypeElapsedTimeInSeconds) , _gpsFactGroup(this) , _batteryFactGroup(this) , _windFactGroup(this) @@ -160,7 +165,6 @@ Vehicle::Vehicle(LinkInterface* link, _mavlink = qgcApp()->toolbox()->mavlinkProtocol(); connect(_mavlink, &MAVLinkProtocol::messageReceived, this, &Vehicle::_mavlinkMessageReceived); - connect(_mavlink, &MAVLinkProtocol::radioStatusChanged, this, &Vehicle::_telemetryChanged); connect(this, &Vehicle::_sendMessageOnLinkOnThread, this, &Vehicle::_sendMessageOnLink, Qt::QueuedConnection); connect(this, &Vehicle::flightModeChanged, this, &Vehicle::_handleFlightModeChanged); @@ -252,6 +256,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _rcRSSIstore(255) , _autoDisconnect(false) , _flying(false) + , _landing(false) , _onboardControlSensorsPresent(0) , _onboardControlSensorsEnabled(0) , _onboardControlSensorsHealth(0) @@ -264,6 +269,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _supportsMissionItemInt(false) , _connectionLost(false) , _connectionLostEnabled(true) + , _initialPlanRequestComplete(false) , _missionManager(NULL) , _missionManagerInitialRequestSent(false) , _geoFenceManager(NULL) @@ -298,6 +304,8 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _climbRateFact (0, _climbRateFactName, FactMetaData::valueTypeDouble) , _altitudeRelativeFact (0, _altitudeRelativeFactName, FactMetaData::valueTypeDouble) , _altitudeAMSLFact (0, _altitudeAMSLFactName, FactMetaData::valueTypeDouble) + , _flightDistanceFact (0, _flightDistanceFactName, FactMetaData::valueTypeDouble) + , _flightTimeFact (0, _flightTimeFactName, FactMetaData::valueTypeElapsedTimeInSeconds) , _gpsFactGroup(this) , _batteryFactGroup(this) , _windFactGroup(this) @@ -313,7 +321,11 @@ void Vehicle::_commonInit(void) _missionManager = new MissionManager(this); connect(_missionManager, &MissionManager::error, this, &Vehicle::_missionManagerError); - connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_newMissionItemsAvailable); + connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_missionLoadComplete); + connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_clearCameraTriggerPoints); + connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_clearTrajectoryPoints); + connect(_missionManager, &MissionManager::sendComplete, this, &Vehicle::_clearCameraTriggerPoints); + connect(_missionManager, &MissionManager::sendComplete, this, &Vehicle::_clearTrajectoryPoints); _parameterManager = new ParameterManager(this); connect(_parameterManager, &ParameterManager::parametersReadyChanged, this, &Vehicle::_parametersReady); @@ -321,10 +333,11 @@ void Vehicle::_commonInit(void) // GeoFenceManager needs to access ParameterManager so make sure to create after _geoFenceManager = _firmwarePlugin->newGeoFenceManager(this); connect(_geoFenceManager, &GeoFenceManager::error, this, &Vehicle::_geoFenceManagerError); - connect(_geoFenceManager, &GeoFenceManager::loadComplete, this, &Vehicle::_newGeoFenceAvailable); + connect(_geoFenceManager, &GeoFenceManager::loadComplete, this, &Vehicle::_geoFenceLoadComplete); _rallyPointManager = _firmwarePlugin->newRallyPointManager(this); - connect(_rallyPointManager, &RallyPointManager::error, this, &Vehicle::_rallyPointManagerError); + connect(_rallyPointManager, &RallyPointManager::error, this, &Vehicle::_rallyPointManagerError); + connect(_rallyPointManager, &RallyPointManager::loadComplete, this, &Vehicle::_rallyPointLoadComplete); // Offline editing vehicle tracks settings changes for offline editing settings connect(_settingsManager->appSettings()->offlineEditingFirmwareType(), &Fact::rawValueChanged, this, &Vehicle::_offlineFirmwareTypeSettingChanged); @@ -343,6 +356,7 @@ void Vehicle::_commonInit(void) _addFact(&_altitudeRelativeFact, _altitudeRelativeFactName); _addFact(&_altitudeAMSLFact, _altitudeAMSLFactName); _addFact(&_flightDistanceFact, _flightDistanceFactName); + _addFact(&_flightTimeFact, _flightTimeFactName); _addFactGroup(&_gpsFactGroup, _gpsFactGroupName); _addFactGroup(&_batteryFactGroup, _batteryFactGroupName); @@ -351,6 +365,7 @@ void Vehicle::_commonInit(void) _addFactGroup(&_temperatureFactGroup, _temperatureFactGroupName); _flightDistanceFact.setRawValue(0); + _flightTimeFact.setRawValue(0); } Vehicle::~Vehicle() @@ -429,37 +444,6 @@ void Vehicle::resetCounters() _heardFrom = false; } -void Vehicle::_telemetryChanged(LinkInterface*, unsigned rxerrors, unsigned fixed, int rssi, int remrssi, unsigned txbuf, unsigned noise, unsigned remnoise) -{ - if(_telemetryLRSSI != rssi) { - _telemetryLRSSI = rssi; - emit telemetryLRSSIChanged(_telemetryLRSSI); - } - if(_telemetryRRSSI != remrssi) { - _telemetryRRSSI = remrssi; - emit telemetryRRSSIChanged(_telemetryRRSSI); - } - if(_telemetryRXErrors != rxerrors) { - _telemetryRXErrors = rxerrors; - emit telemetryRXErrorsChanged(_telemetryRXErrors); - } - if(_telemetryFixed != fixed) { - _telemetryFixed = fixed; - emit telemetryFixedChanged(_telemetryFixed); - } - if(_telemetryTXBuffer != txbuf) { - _telemetryTXBuffer = txbuf; - emit telemetryTXBufferChanged(_telemetryTXBuffer); - } - if(_telemetryLNoise != noise) { - _telemetryLNoise = noise; - emit telemetryLNoiseChanged(_telemetryLNoise); - } - if(_telemetryRNoise != remnoise) { - _telemetryRNoise = remnoise; - emit telemetryRNoiseChanged(_telemetryRNoise); - } -} void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message) { @@ -513,6 +497,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_HEARTBEAT: _handleHeartbeat(message); break; + case MAVLINK_MSG_ID_RADIO_STATUS: + _handleRadioStatus(message); + break; case MAVLINK_MSG_ID_RC_CHANNELS: _handleRCChannels(message); break; @@ -581,6 +568,13 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes break; case MAVLINK_MSG_ID_SCALED_PRESSURE3: _handleScaledPressure3(message); + break; + case MAVLINK_MSG_ID_CAMERA_FEEDBACK: + _handleCameraFeedback(message); + break; + + case MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED: + _handleCameraImageCaptured(message); break; case MAVLINK_MSG_ID_SERIAL_CONTROL: @@ -602,6 +596,31 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes _uas->receiveMessage(message); } + +void Vehicle::_handleCameraFeedback(const mavlink_message_t& message) +{ + mavlink_camera_feedback_t feedback; + + mavlink_msg_camera_feedback_decode(&message, &feedback); + + QGeoCoordinate imageCoordinate((double)feedback.lat / qPow(10.0, 7.0), (double)feedback.lng / qPow(10.0, 7.0), feedback.alt_msl); + qCDebug(VehicleLog) << "_handleCameraFeedback coord:index" << imageCoordinate << feedback.img_idx; + _cameraTriggerPoints.append(new QGCQGeoCoordinate(imageCoordinate, this)); +} + +void Vehicle::_handleCameraImageCaptured(const mavlink_message_t& message) +{ + mavlink_camera_image_captured_t feedback; + + mavlink_msg_camera_image_captured_decode(&message, &feedback); + + QGeoCoordinate imageCoordinate((double)feedback.lat / qPow(10.0, 7.0), (double)feedback.lon / qPow(10.0, 7.0), feedback.alt); + qCDebug(VehicleLog) << "_handleCameraFeedback coord:index" << imageCoordinate << feedback.image_index << feedback.capture_result; + if (feedback.capture_result == 1) { + _cameraTriggerPoints.append(new QGCQGeoCoordinate(imageCoordinate, this)); + } +} + void Vehicle::_handleVfrHud(mavlink_message_t& message) { mavlink_vfr_hud_t vfrHud; @@ -666,6 +685,16 @@ void Vehicle::_handleAltitude(mavlink_message_t& message) } } +void Vehicle::_setCapabilities(uint64_t capabilityBits) +{ + if (capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_INT) { + _supportsMissionItemInt = true; + } + _vehicleCapabilitiesKnown = true; + + qCDebug(VehicleLog) << QString("Vehicle %1 MISSION_ITEM_INT").arg(_supportsMissionItemInt ? QStringLiteral("supports") : QStringLiteral("does not support")); +} + void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& message) { Q_UNUSED(link); @@ -700,12 +729,8 @@ void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& me emit gitHashChanged(_gitHash); } - if (autopilotVersion.capabilities & MAV_PROTOCOL_CAPABILITY_MISSION_INT) { - qCDebug(VehicleLog) << "Vehicle supports MISSION_ITEM_INT"; - _supportsMissionItemInt = true; - _vehicleCapabilitiesKnown = true; - _startMissionRequest(); - } + _setCapabilities(autopilotVersion.capabilities); + _startPlanRequest(); } void Vehicle::_handleHilActuatorControls(mavlink_message_t &message) @@ -742,7 +767,8 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message) if (ack.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES && ack.result != MAV_RESULT_ACCEPTED) { // We aren't going to get a response back for capabilities, so stop waiting for it before we ask for mission items qCDebug(VehicleLog) << "Vehicle failed to responded to MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES with error. Starting mission request."; - _startMissionRequest(); + _setCapabilities(0); + _startPlanRequest(); } if (_mavCommandQueue.count() && ack.command == _mavCommandQueue[0].command) { @@ -784,14 +810,21 @@ void Vehicle::_handleExtendedSysState(mavlink_message_t& message) mavlink_msg_extended_sys_state_decode(&message, &extendedState); switch (extendedState.landed_state) { - case MAV_LANDED_STATE_UNDEFINED: - break; case MAV_LANDED_STATE_ON_GROUND: - setFlying(false); + _setFlying(false); + _setLanding(false); break; + case MAV_LANDED_STATE_TAKEOFF: case MAV_LANDED_STATE_IN_AIR: - setFlying(true); - return; + _setFlying(true); + _setLanding(false); + break; + case MAV_LANDED_STATE_LANDING: + _setFlying(true); + _setLanding(true); + break; + default: + break; } } @@ -896,6 +929,14 @@ void Vehicle::_handleBatteryStatus(mavlink_message_t& message) _batteryFactGroup.cellCount()->setRawValue(cellCount); } +void Vehicle::_setHomePosition(QGeoCoordinate& homeCoord) +{ + if (homeCoord != _homePosition) { + _homePosition = homeCoord; + emit homePositionChanged(_homePosition); + } +} + void Vehicle::_handleHomePosition(mavlink_message_t& message) { mavlink_home_position_t homePos; @@ -905,10 +946,7 @@ void Vehicle::_handleHomePosition(mavlink_message_t& message) QGeoCoordinate newHomePosition (homePos.latitude / 10000000.0, homePos.longitude / 10000000.0, homePos.altitude / 1000.0); - if (newHomePosition != _homePosition) { - _homePosition = newHomePosition; - emit homePositionChanged(_homePosition); - } + _setHomePosition(newHomePosition); } void Vehicle::_handleHeartbeat(mavlink_message_t& message) @@ -930,15 +968,82 @@ void Vehicle::_handleHeartbeat(mavlink_message_t& message) // We are transitioning to the armed state, begin tracking trajectory points for the map if (_armed) { _mapTrajectoryStart(); + _clearCameraTriggerPoints(); } else { _mapTrajectoryStop(); } } if (heartbeat.base_mode != _base_mode || heartbeat.custom_mode != _custom_mode) { + QString previousFlightMode; + if (_base_mode != 0 || _custom_mode != 0){ + // Vehicle is initialized with _base_mode=0 and _custom_mode=0. Don't pass this to flightMode() since it will complain about + // bad modes while unit testing. + previousFlightMode = flightMode(); + } _base_mode = heartbeat.base_mode; _custom_mode = heartbeat.custom_mode; - emit flightModeChanged(flightMode()); + if (previousFlightMode != flightMode()) { + emit flightModeChanged(flightMode()); + } + } +} + +void Vehicle::_handleRadioStatus(mavlink_message_t& message) +{ + //-- Process telemetry status message + mavlink_radio_status_t rstatus; + mavlink_msg_radio_status_decode(&message, &rstatus); + int rssi = rstatus.rssi; + int remrssi = rstatus.remrssi; + int lnoise = (int)(int8_t)rstatus.noise; + int rnoise = (int)(int8_t)rstatus.remnoise; + //-- 3DR Si1k radio needs rssi fields to be converted to dBm + if (message.sysid == '3' && message.compid == 'D') { + /* Per the Si1K datasheet figure 23.25 and SI AN474 code + * samples the relationship between the RSSI register + * and received power is as follows: + * + * 10 + * inputPower = rssi * ------ 127 + * 19 + * + * Additionally limit to the only realistic range [-120,0] dBm + */ + rssi = qMin(qMax(qRound(static_cast(rssi) / 1.9 - 127.0), - 120), 0); + remrssi = qMin(qMax(qRound(static_cast(remrssi) / 1.9 - 127.0), - 120), 0); + } else { + rssi = (int)(int8_t)rstatus.rssi; + remrssi = (int)(int8_t)rstatus.remrssi; + } + //-- Check for changes + if(_telemetryLRSSI != rssi) { + _telemetryLRSSI = rssi; + emit telemetryLRSSIChanged(_telemetryLRSSI); + } + if(_telemetryRRSSI != remrssi) { + _telemetryRRSSI = remrssi; + emit telemetryRRSSIChanged(_telemetryRRSSI); + } + if(_telemetryRXErrors != rstatus.rxerrors) { + _telemetryRXErrors = rstatus.rxerrors; + emit telemetryRXErrorsChanged(_telemetryRXErrors); + } + if(_telemetryFixed != rstatus.fixed) { + _telemetryFixed = rstatus.fixed; + emit telemetryFixedChanged(_telemetryFixed); + } + if(_telemetryTXBuffer != rstatus.txbuf) { + _telemetryTXBuffer = rstatus.txbuf; + emit telemetryTXBufferChanged(_telemetryTXBuffer); + } + if(_telemetryLNoise != lnoise) { + _telemetryLNoise = lnoise; + emit telemetryLNoiseChanged(_telemetryLNoise); + } + if(_telemetryRNoise != rnoise) { + _telemetryRNoise = rnoise; + emit telemetryRNoiseChanged(_telemetryRNoise); } } @@ -1262,7 +1367,6 @@ void Vehicle::_handleTextMessage(int newCount) } UASMessageHandler* pMh = qgcApp()->toolbox()->uasMessageHandler(); - Q_ASSERT(pMh); MessageType_t type = newCount ? _currentMessageType : MessageNone; int errorCount = _currentErrorCount; int warnCount = _currentWarningCount; @@ -1349,7 +1453,7 @@ void Vehicle::_loadSettings(void) // Joystick enabled is a global setting so first make sure there are any joysticks connected if (qgcApp()->toolbox()->joystickManager()->joysticks().count()) { - _joystickEnabled = settings.value(_joystickEnabledSettingsKey, false).toBool(); + setJoystickEnabled(settings.value(_joystickEnabledSettingsKey, false).toBool()); } } @@ -1596,23 +1700,38 @@ void Vehicle::_rallyPointManagerError(int errorCode, const QString& errorMsg) void Vehicle::_addNewMapTrajectoryPoint(void) { if (_mapTrajectoryHaveFirstCoordinate) { - // Keep three minutes of trajectory + // Keep three minutes of trajectory on mobile due to perf impact of lines +#ifdef __mobile__ if (_mapTrajectoryList.count() * _mapTrajectoryMsecsBetweenPoints > 3 * 1000 * 60) { _mapTrajectoryList.removeAt(0)->deleteLater(); } +#endif _mapTrajectoryList.append(new CoordinateVector(_mapTrajectoryLastCoordinate, _coordinate, this)); _flightDistanceFact.setRawValue(_flightDistanceFact.rawValue().toDouble() + _mapTrajectoryLastCoordinate.distanceTo(_coordinate)); } _mapTrajectoryHaveFirstCoordinate = true; _mapTrajectoryLastCoordinate = _coordinate; + _flightTimeFact.setRawValue((double)_flightTimer.elapsed() / 1000.0); +} + +void Vehicle::_clearTrajectoryPoints(void) +{ + _mapTrajectoryList.clearAndDeleteContents(); +} + +void Vehicle::_clearCameraTriggerPoints(void) +{ + _cameraTriggerPoints.clearAndDeleteContents(); } void Vehicle::_mapTrajectoryStart(void) { _mapTrajectoryHaveFirstCoordinate = false; - _mapTrajectoryList.clear(); + _clearTrajectoryPoints(); _mapTrajectoryTimer.start(); + _flightTimer.start(); _flightDistanceFact.setRawValue(0); + _flightTimeFact.setRawValue(0); } void Vehicle::_mapTrajectoryStop() @@ -1620,10 +1739,14 @@ void Vehicle::_mapTrajectoryStop() _mapTrajectoryTimer.stop(); } -void Vehicle::_startMissionRequest(void) +void Vehicle::_startPlanRequest(void) { - if (!_missionManagerInitialRequestSent && _parameterManager->parametersReady() && _vehicleCapabilitiesKnown) { - qCDebug(VehicleLog) << "_startMissionRequest"; + if (_missionManagerInitialRequestSent) { + return; + } + + if (_parameterManager->parametersReady() && _vehicleCapabilitiesKnown) { + qCDebug(VehicleLog) << "_startPlanRequest"; _missionManagerInitialRequestSent = true; if (_settingsManager->appSettings()->autoLoadMissions()->rawValue().toBool()) { QString missionAutoLoadDirPath = _settingsManager->appSettings()->missionSavePath(); @@ -1636,21 +1759,45 @@ void Vehicle::_startMissionRequest(void) } } } - _missionManager->requestMissionItems(); + _missionManager->loadFromVehicle(); } else { if (!_parameterManager->parametersReady()) { - qCDebug(VehicleLog) << "Delaying _startMissionRequest due to parameters not ready"; + qCDebug(VehicleLog) << "Delaying _startPlanRequest due to parameters not ready"; } else if (!_vehicleCapabilitiesKnown) { - qCDebug(VehicleLog) << "Delaying _startMissionRequest due to vehicle capabilities not know"; + qCDebug(VehicleLog) << "Delaying _startPlanRequest due to vehicle capabilities not known"; } } } +void Vehicle::_missionLoadComplete(void) +{ + // After the initial mission request completes we ask for the geofence + if (!_geoFenceManagerInitialRequestSent) { + _geoFenceManagerInitialRequestSent = true; + _geoFenceManager->loadFromVehicle(); + } +} + +void Vehicle::_geoFenceLoadComplete(void) +{ + // After geofence request completes we ask for the rally points + if (!_rallyPointManagerInitialRequestSent) { + _rallyPointManagerInitialRequestSent = true; + _rallyPointManager->loadFromVehicle(); + } +} + + +void Vehicle::_rallyPointLoadComplete(void) +{ + _initialPlanRequestComplete = true; +} + void Vehicle::_parametersReady(bool parametersReady) { if (parametersReady) { _setupAutoDisarmSignalling(); - _startMissionRequest(); + _startPlanRequest(); setJoystickEnabled(_joystickEnabled); } } @@ -1864,12 +2011,7 @@ void Vehicle::_announceArmedChanged(bool armed) _say(QString("%1 %2").arg(_vehicleIdSpeech()).arg(armed ? QStringLiteral("armed") : QStringLiteral("disarmed"))); } -void Vehicle::clearTrajectoryPoints(void) -{ - _mapTrajectoryList.clearAndDeleteContents(); -} - -void Vehicle::setFlying(bool flying) +void Vehicle::_setFlying(bool flying) { if (armed() && _flying != flying) { _flying = flying; @@ -1877,6 +2019,14 @@ void Vehicle::setFlying(bool flying) } } +void Vehicle::_setLanding(bool landing) +{ + if (armed() && _landing != landing) { + _landing = landing; + emit landingChanged(landing); + } +} + bool Vehicle::guidedModeSupported(void) const { return _firmwarePlugin->isCapable(this, FirmwarePlugin::GuidedModeCapability); @@ -1931,6 +2081,14 @@ void Vehicle::guidedModeGotoLocation(const QGeoCoordinate& gotoCoord) qgcApp()->showMessage(guided_mode_not_supported_by_vehicle); return; } + if (!coordinate().isValid()) { + return; + } + double maxDistance = 1000.0; + if (coordinate().distanceTo(gotoCoord) > maxDistance) { + qgcApp()->showMessage(QString("New location is too far. Must be less than %1 %2").arg(qRound(FactMetaData::metersToAppSettingsDistanceUnits(maxDistance).toDouble())).arg(FactMetaData::appSettingsDistanceUnitsString())); + return; + } _firmwarePlugin->guidedModeGotoLocation(this, gotoCoord); } @@ -2040,7 +2198,8 @@ void Vehicle::_sendMavCommandAgain(void) if (_mavCommandRetryCount++ > _mavCommandMaxRetryCount) { if (queuedCommand.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) { // We aren't going to get a response back for capabilities, so stop waiting for it before we ask for mission items - _startMissionRequest(); + _setCapabilities(0); + _startPlanRequest(); } emit mavCommandResult(_id, queuedCommand.component, queuedCommand.command, MAV_RESULT_FAILED, true /* noResponsefromVehicle */); @@ -2073,7 +2232,7 @@ void Vehicle::_sendMavCommandAgain(void) } } } - qDebug() << "Vehicle::_sendMavCommandAgain retrying command:_mavCommandRetryCount" << queuedCommand.command << _mavCommandRetryCount; + qCDebug(VehicleLog) << "Vehicle::_sendMavCommandAgain retrying command:_mavCommandRetryCount" << queuedCommand.command << _mavCommandRetryCount; } _mavCommandAckTimer.start(); @@ -2175,24 +2334,6 @@ void Vehicle::motorTest(int motor, int percent, int timeoutSecs) } #endif -void Vehicle::_newMissionItemsAvailable(void) -{ - // After the initial mission request completes we ask for the geofence - if (!_geoFenceManagerInitialRequestSent) { - _geoFenceManagerInitialRequestSent = true; - _geoFenceManager->loadFromVehicle(); - } -} - -void Vehicle::_newGeoFenceAvailable(void) -{ - // After geofence request completes we ask for the rally points - if (!_rallyPointManagerInitialRequestSent) { - _rallyPointManagerInitialRequestSent = true; - _rallyPointManager->loadFromVehicle(); - } -} - QString Vehicle::brandImageIndoor(void) const { return _firmwarePlugin->brandImageIndoor(this); @@ -2261,7 +2402,7 @@ void Vehicle::setOfflineEditingDefaultComponentId(int defaultComponentId) void Vehicle::triggerCamera(void) { - sendMavCommand(FactSystem::defaultComponentId, + sendMavCommand(_defaultComponentId, MAV_CMD_DO_DIGICAM_CONTROL, true, // show errors 0.0, 0.0, 0.0, 0.0, // param 1-4 unused diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 4b9d4b2fb781503a58c5ecef18887cf53d022481..a1c1ca943f4e097e37476a80f602be89ec04ba9b 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -246,6 +246,7 @@ public: 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) + Q_PROPERTY(QmlObjectListModel* cameraTriggerPoints READ cameraTriggerPoints CONSTANT) Q_PROPERTY(float latitude READ latitude NOTIFY coordinateChanged) Q_PROPERTY(float longitude READ longitude NOTIFY coordinateChanged) Q_PROPERTY(bool messageTypeNone READ messageTypeNone NOTIFY messageTypeChanged) @@ -307,16 +308,19 @@ public: Q_PROPERTY(unsigned int telemetryRXErrors READ telemetryRXErrors NOTIFY telemetryRXErrorsChanged) Q_PROPERTY(unsigned int telemetryFixed READ telemetryFixed NOTIFY telemetryFixedChanged) Q_PROPERTY(unsigned int telemetryTXBuffer READ telemetryTXBuffer NOTIFY telemetryTXBufferChanged) - Q_PROPERTY(unsigned int telemetryLNoise READ telemetryLNoise NOTIFY telemetryLNoiseChanged) - Q_PROPERTY(unsigned int telemetryRNoise READ telemetryRNoise NOTIFY telemetryRNoiseChanged) + Q_PROPERTY(int telemetryLNoise READ telemetryLNoise NOTIFY telemetryLNoiseChanged) + Q_PROPERTY(int telemetryRNoise READ telemetryRNoise NOTIFY telemetryRNoiseChanged) Q_PROPERTY(QVariantList toolBarIndicators READ toolBarIndicators CONSTANT) Q_PROPERTY(QVariantList cameraList READ cameraList CONSTANT) /// true: Vehicle is flying, false: Vehicle is on ground - Q_PROPERTY(bool flying READ flying WRITE setFlying NOTIFY flyingChanged) + Q_PROPERTY(bool flying READ flying NOTIFY flyingChanged) + + /// true: Vehicle is flying, false: Vehicle is on ground + Q_PROPERTY(bool landing READ landing NOTIFY landingChanged) /// true: Vehicle is in Guided mode and can respond to guided commands, false: vehicle cannot respond to direct control commands - Q_PROPERTY(bool guidedMode READ guidedMode WRITE setGuidedMode NOTIFY guidedModeChanged) + Q_PROPERTY(bool guidedMode READ guidedMode WRITE setGuidedMode NOTIFY guidedModeChanged) /// true: Guided mode commands are supported by this vehicle Q_PROPERTY(bool guidedModeSupported READ guidedModeSupported CONSTANT) @@ -371,8 +375,6 @@ public: Q_INVOKABLE void virtualTabletJoystickValue(double roll, double pitch, double yaw, double thrust); Q_INVOKABLE void disconnectInactiveVehicle(void); - Q_INVOKABLE void clearTrajectoryPoints(void); - /// Command vehicle to return to launch Q_INVOKABLE void guidedModeRTL(void); @@ -464,7 +466,7 @@ public: MAV_TYPE vehicleType(void) const { return _vehicleType; } Q_INVOKABLE QString vehicleTypeName(void) const; - /// Returns the highest quality link available to the Vehicle. If you need to hold a refernce to this link use + /// Returns the highest quality link available to the Vehicle. If you need to hold a reference to this link use /// LinkManager::sharedLinkInterfaceForGet to get QSharedPointer for link. LinkInterface* priorityLink(void) { return _priorityLink.data(); } @@ -517,13 +519,13 @@ public: bool supportsCalibratePressure(void) const; bool supportsMotorInterference(void) const; - void setFlying(bool flying); void setGuidedMode(bool guidedMode); QString prearmError(void) const { return _prearmError; } void setPrearmError(const QString& prearmError); QmlObjectListModel* trajectoryPoints(void) { return &_mapTrajectoryList; } + QmlObjectListModel* cameraTriggerPoints(void) { return &_cameraTriggerPoints; } int flowImageIndex() { return _flowImageIndex; } @@ -566,6 +568,7 @@ public: uint messagesSent () { return _messagesSent; } uint messagesLost () { return _messagesLost; } bool flying () const { return _flying; } + bool landing () const { return _landing; } bool guidedMode () const; uint8_t baseMode () const { return _base_mode; } uint32_t customMode () const { return _custom_mode; } @@ -587,8 +590,8 @@ public: unsigned int telemetryRXErrors () { return _telemetryRXErrors; } unsigned int telemetryFixed () { return _telemetryFixed; } unsigned int telemetryTXBuffer () { return _telemetryTXBuffer; } - unsigned int telemetryLNoise () { return _telemetryLNoise; } - unsigned int telemetryRNoise () { return _telemetryRNoise; } + int telemetryLNoise () { return _telemetryLNoise; } + int telemetryRNoise () { return _telemetryRNoise; } bool autoDisarm (); Fact* roll (void) { return &_rollFact; } @@ -621,6 +624,7 @@ public: /// @param component Component to send to /// @param command MAV_CMD to send /// @param showError true: Display error to user if command failed, false: no error shown + /// Signals: mavCommandResult on success or failure void sendMavCommand(int component, MAV_CMD command, bool showError, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f); int firmwareMajorVersion(void) const { return _firmwareMajorVersion; } @@ -669,6 +673,14 @@ public: /// @true: When flying a mission the vehicle is always facing towards the next waypoint bool vehicleYawsToNextWaypointInMission(void) const; + /// The vehicle is responsible for making the initial request for the Plan. + /// @return: true: initial request is complete, false: initial request is still in progress; + bool initialPlanRequestComplete(void) const { return _initialPlanRequestComplete; } + + void _setFlying(bool flying); + void _setLanding(bool landing); + void _setHomePosition(QGeoCoordinate& homeCoord); + signals: void allLinksInactive(Vehicle* vehicle); void coordinateChanged(QGeoCoordinate coordinate); @@ -686,6 +698,7 @@ signals: void connectionLostEnabledChanged(bool connectionLostEnabled); void autoDisconnectChanged(bool autoDisconnectChanged); void flyingChanged(bool flying); + void landingChanged(bool landing); void guidedModeChanged(bool guidedMode); void prearmErrorChanged(const QString& prearmError); void soloFirmwareChanged(bool soloFirmware); @@ -717,8 +730,8 @@ signals: void telemetryRXErrorsChanged (unsigned int value); void telemetryFixedChanged (unsigned int value); void telemetryTXBufferChanged (unsigned int value); - void telemetryLNoiseChanged (unsigned int value); - void telemetryRNoiseChanged (unsigned int value); + void telemetryLNoiseChanged (int value); + void telemetryRNoiseChanged (int value); void autoDisarmChanged (void); void firmwareMajorVersionChanged(int major); @@ -757,7 +770,6 @@ signals: private slots: void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message); - void _telemetryChanged(LinkInterface* link, unsigned rxerrors, unsigned fixed, int rssi, int remrssi, unsigned txbuf, unsigned noise, unsigned remnoise); void _linkInactiveOrDeleted(LinkInterface* link); void _sendMessageOnLink(LinkInterface* link, mavlink_message_t message); void _sendMessageMultipleNext(void); @@ -781,11 +793,13 @@ private slots: void _imageReady (UASInterface* uas); void _connectionLostTimeout(void); void _prearmErrorTimeout(void); - void _newMissionItemsAvailable(void); - void _newGeoFenceAvailable(void); + void _missionLoadComplete(void); + void _geoFenceLoadComplete(void); + void _rallyPointLoadComplete(void); void _sendMavCommandAgain(void); - void _activeJoystickChanged(void); + void _clearTrajectoryPoints(void); + void _clearCameraTriggerPoints(void); private: bool _containsLink(LinkInterface* link); @@ -795,6 +809,7 @@ private: void _startJoystick(bool start); void _handleHomePosition(mavlink_message_t& message); void _handleHeartbeat(mavlink_message_t& message); + void _handleRadioStatus(mavlink_message_t& message); void _handleRCChannels(mavlink_message_t& message); void _handleRCChannelsRaw(mavlink_message_t& message); void _handleBatteryStatus(mavlink_message_t& message); @@ -813,6 +828,8 @@ private: void _handleScaledPressure(mavlink_message_t& message); void _handleScaledPressure2(mavlink_message_t& message); void _handleScaledPressure3(mavlink_message_t& message); + void _handleCameraFeedback(const mavlink_message_t& message); + void _handleCameraImageCaptured(const mavlink_message_t& message); void _missionManagerError(int errorCode, const QString& errorMsg); void _geoFenceManagerError(int errorCode, const QString& errorMsg); void _rallyPointManagerError(int errorCode, const QString& errorMsg); @@ -827,8 +844,9 @@ private: void _sendNextQueuedMavCommand(void); void _updatePriorityLink(void); void _commonInit(void); - void _startMissionRequest(void); + void _startPlanRequest(void); void _setupAutoDisarmSignalling(void); + void _setCapabilities(uint64_t capabilityBits); int _id; ///< Mavlink system id int _defaultComponentId; @@ -868,6 +886,7 @@ private: double _rcRSSIstore; bool _autoDisconnect; ///< true: Automatically disconnect vehicle when last connection goes away or lost heartbeat bool _flying; + bool _landing; uint32_t _onboardControlSensorsPresent; uint32_t _onboardControlSensorsEnabled; uint32_t _onboardControlSensorsHealth; @@ -881,8 +900,8 @@ private: uint32_t _telemetryRXErrors; uint32_t _telemetryFixed; uint32_t _telemetryTXBuffer; - uint32_t _telemetryLNoise; - uint32_t _telemetryRNoise; + int _telemetryLNoise; + int _telemetryRNoise; bool _vehicleCapabilitiesKnown; bool _supportsMissionItemInt; @@ -909,6 +928,8 @@ private: static const int _connectionLostTimeoutMSecs = 3500; // Signal connection lost after 3.5 seconds of missed heartbeat QTimer _connectionLostTimer; + bool _initialPlanRequestComplete; + MissionManager* _missionManager; bool _missionManagerInitialRequestSent; @@ -938,12 +959,15 @@ private: QTimer _sendMultipleTimer; int _nextSendMessageMultipleIndex; + QTime _flightTimer; QTimer _mapTrajectoryTimer; QmlObjectListModel _mapTrajectoryList; QGeoCoordinate _mapTrajectoryLastCoordinate; bool _mapTrajectoryHaveFirstCoordinate; static const int _mapTrajectoryMsecsBetweenPoints = 1000; + QmlObjectListModel _cameraTriggerPoints; + // Toolbox references FirmwarePluginManager* _firmwarePluginManager; JoystickManager* _joystickManager; @@ -981,6 +1005,7 @@ private: Fact _altitudeRelativeFact; Fact _altitudeAMSLFact; Fact _flightDistanceFact; + Fact _flightTimeFact; VehicleGPSFactGroup _gpsFactGroup; VehicleBatteryFactGroup _batteryFactGroup; @@ -997,6 +1022,7 @@ private: static const char* _altitudeRelativeFactName; static const char* _altitudeAMSLFactName; static const char* _flightDistanceFactName; + static const char* _flightTimeFactName; static const char* _gpsFactGroupName; static const char* _batteryFactGroupName; diff --git a/src/Vehicle/VehicleFact.json b/src/Vehicle/VehicleFact.json index abef3dcd07df15639f551ebdc824b89b98db4408..400fec14a34e6c0382928a290cb574548c04900a 100644 --- a/src/Vehicle/VehicleFact.json +++ b/src/Vehicle/VehicleFact.json @@ -61,5 +61,11 @@ "type": "double", "decimalPlaces": 1, "units": "m" +}, +{ + "name": "flightTime", + "shortDescription": "Flight Time", + "type": "elapsedSeconds", + "decimalPlaces": 1 } ] diff --git a/src/VehicleSetup/Bootloader.cc b/src/VehicleSetup/Bootloader.cc index a023ab140fb547fb7eea91538f710c7a7b1b3f4c..0dfc3ee94b4a2f8630957846b88d84bf3f65f357 100644 --- a/src/VehicleSetup/Bootloader.cc +++ b/src/VehicleSetup/Bootloader.cc @@ -96,6 +96,9 @@ bool Bootloader::_getCommandResponse(QextSerialPort* port, int responseTimeout) if (response[0] != PROTO_INSYNC) { _errorString = tr("Invalid sync response: 0x%1 0x%2").arg(response[0], 2, 16, QLatin1Char('0')).arg(response[1], 2, 16, QLatin1Char('0')); return false; + } else if (response[0] == PROTO_INSYNC && response[1] == PROTO_BAD_SILICON_REV) { + _errorString = tr("This board is using a microcontroller with faulty silicon and an incorrect configuration and should be put out of service."); + return false; } else if (response[1] != PROTO_OK) { QString responseCode = tr("Unknown response code"); if (response[1] == PROTO_FAILED) { diff --git a/src/VehicleSetup/Bootloader.h b/src/VehicleSetup/Bootloader.h index 268132483a720f95d148e8856ee4ba2c16606147..031ab69958f844bad1b491bc9c676c274ff347e9 100644 --- a/src/VehicleSetup/Bootloader.h +++ b/src/VehicleSetup/Bootloader.h @@ -64,6 +64,8 @@ public: static const int boardIDPX4FMUV1 = 5; ///< PX4 V1 board, as from USB PID static const int boardIDPX4FMUV2 = 9; ///< PX4 V2 board, as from USB PID static const int boardIDPX4FMUV4 = 11; ///< PX4 V4 board, as from USB PID + static const int boardIDPX4FMUV4PRO = 13; ///< PX4 V4PRO board, as from USB PID + static const int boardIDPX4FMUV5 = 50; ///< PX4 V5 board, as from USB PID static const int boardIDPX4Flow = 6; ///< PX4 Flow board, as from USB PID static const int boardIDAeroCore = 98; ///< Gumstix AeroCore board, as from USB PID static const int boardIDAUAVX2_1 = 33; ///< AUAV X2.1 board, as from USB PID @@ -71,7 +73,7 @@ public: static const int boardIDMINDPXFMUV2 = 88; ///< MindPX V2 board, as from USB PID static const int boardIDTAPV1 = 64; ///< TAP V1 board, as from USB PID static const int boardIDASCV1 = 65; ///< ASC V1 board, as from USB PID - + static const int boardIDCrazyflie2 = 12; ///< Crazyflie 2.0 board, as from USB PID signals: /// @brief Signals progress indicator for long running bootloader utility routines void updateProgress(int curr, int total); @@ -98,6 +100,7 @@ private: enum { // protocol bytes PROTO_INSYNC = 0x12, ///< 'in sync' byte sent before status + PROTO_BAD_SILICON_REV = 0x14, ///< device is using silicon not suitable for the target the bootloader was used for PROTO_EOC = 0x20, ///< end of command // Reply bytes diff --git a/src/VehicleSetup/FirmwareUpgradeController.cc b/src/VehicleSetup/FirmwareUpgradeController.cc index 0585302d9605e8c304dbb06e9ffae41ed4b146b4..ea3ebb07199c8b792a39487eaad9348028c96676 100644 --- a/src/VehicleSetup/FirmwareUpgradeController.cc +++ b/src/VehicleSetup/FirmwareUpgradeController.cc @@ -190,6 +190,22 @@ void FirmwareUpgradeController::_initFirmwareHash() return; } + //////////////////////////////////// PX4FMUV5 firmwares ////////////////////////////////////////////////// + FirmwareToUrlElement_t rgPX4FMV5FirmwareArray[] = { + { AutoPilotStackPX4, StableFirmware, DefaultVehicleFirmware, "http://px4-travis.s3.amazonaws.com/Firmware/stable/px4fmu-v5_default.px4"}, + { AutoPilotStackPX4, BetaFirmware, DefaultVehicleFirmware, "http://px4-travis.s3.amazonaws.com/Firmware/beta/px4fmu-v5_default.px4"}, + { AutoPilotStackPX4, DeveloperFirmware, DefaultVehicleFirmware, "http://px4-travis.s3.amazonaws.com/Firmware/master/px4fmu-v5_default.px4"}, + { SingleFirmwareMode,StableFirmware, DefaultVehicleFirmware, _singleFirmwareURL}, + }; + + //////////////////////////////////// PX4FMUV4PRO firmwares ////////////////////////////////////////////////// + FirmwareToUrlElement_t rgPX4FMV4PROFirmwareArray[] = { + { AutoPilotStackPX4, StableFirmware, DefaultVehicleFirmware, "http://px4-travis.s3.amazonaws.com/Firmware/stable/px4fmu-v4pro_default.px4"}, + { AutoPilotStackPX4, BetaFirmware, DefaultVehicleFirmware, "http://px4-travis.s3.amazonaws.com/Firmware/beta/px4fmu-v4pro_default.px4"}, + { AutoPilotStackPX4, DeveloperFirmware, DefaultVehicleFirmware, "http://px4-travis.s3.amazonaws.com/Firmware/master/px4fmu-v4pro_default.px4"}, + { SingleFirmwareMode,StableFirmware, DefaultVehicleFirmware, _singleFirmwareURL}, + }; + //////////////////////////////////// PX4FMUV4 firmwares ////////////////////////////////////////////////// FirmwareToUrlElement_t rgPX4FMV4FirmwareArray[] = { { AutoPilotStackPX4, StableFirmware, DefaultVehicleFirmware, "http://px4-travis.s3.amazonaws.com/Firmware/stable/px4fmu-v4_default.px4"}, @@ -319,6 +335,14 @@ void FirmwareUpgradeController::_initFirmwareHash() { AutoPilotStackPX4, BetaFirmware, DefaultVehicleFirmware, "http://px4-travis.s3.amazonaws.com/Firmware/beta/asc-v1_default.px4"}, { AutoPilotStackPX4, DeveloperFirmware, DefaultVehicleFirmware, "http://px4-travis.s3.amazonaws.com/Firmware/master/asc-v1_default.px4"}, }; + + //////////////////////////////////// Crazyflie 2.0 firmwares ////////////////////////////////////////////////// + FirmwareToUrlElement_t rgCrazyflie2FirmwareArray[] = { + { AutoPilotStackPX4, StableFirmware, DefaultVehicleFirmware, "http://px4-travis.s3.amazonaws.com/Firmware/stable/crazyflie_default.px4"}, + { AutoPilotStackPX4, BetaFirmware, DefaultVehicleFirmware, "http://px4-travis.s3.amazonaws.com/Firmware/beta/crazyflie_default.px4"}, + { AutoPilotStackPX4, DeveloperFirmware, DefaultVehicleFirmware, "http://px4-travis.s3.amazonaws.com/Firmware/master/crazyflie_default.px4"}, + }; + /////////////////////////////// px4flow firmwares /////////////////////////////////////// FirmwareToUrlElement_t rgPX4FLowFirmwareArray[] = { { PX4Flow, StableFirmware, DefaultVehicleFirmware, "http://px4-travis.s3.amazonaws.com/Flow/master/px4flow.px4" }, @@ -330,7 +354,19 @@ void FirmwareUpgradeController::_initFirmwareHash() }; // populate hashes now - int size = sizeof(rgPX4FMV4FirmwareArray)/sizeof(rgPX4FMV4FirmwareArray[0]); + int size = sizeof(rgPX4FMV5FirmwareArray)/sizeof(rgPX4FMV5FirmwareArray[0]); + for (int i = 0; i < size; i++) { + const FirmwareToUrlElement_t& element = rgPX4FMV5FirmwareArray[i]; + _rgPX4FMUV5Firmware.insert(FirmwareIdentifier(element.stackType, element.firmwareType, element.vehicleType), element.url); + } + + size = sizeof(rgPX4FMV4PROFirmwareArray)/sizeof(rgPX4FMV4PROFirmwareArray[0]); + for (int i = 0; i < size; i++) { + const FirmwareToUrlElement_t& element = rgPX4FMV4PROFirmwareArray[i]; + _rgPX4FMUV4PROFirmware.insert(FirmwareIdentifier(element.stackType, element.firmwareType, element.vehicleType), element.url); + } + + size = sizeof(rgPX4FMV4FirmwareArray)/sizeof(rgPX4FMV4FirmwareArray[0]); for (int i = 0; i < size; i++) { const FirmwareToUrlElement_t& element = rgPX4FMV4FirmwareArray[i]; _rgPX4FMUV4Firmware.insert(FirmwareIdentifier(element.stackType, element.firmwareType, element.vehicleType), element.url); @@ -378,6 +414,12 @@ void FirmwareUpgradeController::_initFirmwareHash() _rgASCV1Firmware.insert(FirmwareIdentifier(element.stackType, element.firmwareType, element.vehicleType), element.url); } + size = sizeof(rgCrazyflie2FirmwareArray)/sizeof(rgCrazyflie2FirmwareArray[0]); + for (int i = 0; i < size; i++) { + const FirmwareToUrlElement_t& element = rgCrazyflie2FirmwareArray[i]; + _rgCrazyflie2Firmware.insert(FirmwareIdentifier(element.stackType, element.firmwareType, element.vehicleType), element.url); + } + size = sizeof(rgPX4FLowFirmwareArray)/sizeof(rgPX4FLowFirmwareArray[0]); for (int i = 0; i < size; i++) { const FirmwareToUrlElement_t& element = rgPX4FLowFirmwareArray[i]; @@ -415,6 +457,10 @@ QHash* FirmwareUpgradeCo return &_rgPX4FMUV2Firmware; case Bootloader::boardIDPX4FMUV4: return &_rgPX4FMUV4Firmware; + case Bootloader::boardIDPX4FMUV4PRO: + return &_rgPX4FMUV4PROFirmware; + case Bootloader::boardIDPX4FMUV5: + return &_rgPX4FMUV5Firmware; case Bootloader::boardIDAeroCore: return &_rgAeroCoreFirmware; case Bootloader::boardIDAUAVX2_1: @@ -425,6 +471,8 @@ QHash* FirmwareUpgradeCo return &_rgTAPV1Firmware; case Bootloader::boardIDASCV1: return &_rgASCV1Firmware; + case Bootloader::boardIDCrazyflie2: + return &_rgCrazyflie2Firmware; case Bootloader::boardID3DRRadio: return &_rg3DRRadioFirmware; default: diff --git a/src/VehicleSetup/FirmwareUpgradeController.h b/src/VehicleSetup/FirmwareUpgradeController.h index 6ce335d6caa2a0eb561e34b161a111bf05322dd8..be4513ad4219dc85513104db4a32bf2a558c124d 100644 --- a/src/VehicleSetup/FirmwareUpgradeController.h +++ b/src/VehicleSetup/FirmwareUpgradeController.h @@ -201,6 +201,8 @@ private: QString _portDescription; // firmware hashes + QHash _rgPX4FMUV5Firmware; + QHash _rgPX4FMUV4PROFirmware; QHash _rgPX4FMUV4Firmware; QHash _rgPX4FMUV2Firmware; QHash _rgAeroCoreFirmware; @@ -209,6 +211,7 @@ private: QHash _rgMindPXFMUV2Firmware; QHash _rgTAPV1Firmware; QHash _rgASCV1Firmware; + QHash _rgCrazyflie2Firmware; QHash _rgPX4FLowFirmware; QHash _rg3DRRadioFirmware; diff --git a/src/VehicleSetup/VehicleComponent.cc b/src/VehicleSetup/VehicleComponent.cc index 3bfc3c3e5a60c793763282769ac736124c36768b..3afc798ace843aa43f4ed8909e2db2cd4960dd67 100644 --- a/src/VehicleSetup/VehicleComponent.cc +++ b/src/VehicleSetup/VehicleComponent.cc @@ -20,8 +20,9 @@ VehicleComponent::VehicleComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, _vehicle(vehicle), _autopilot(autopilot) { - Q_ASSERT(vehicle); - Q_ASSERT(autopilot); + if (!vehicle || !autopilot) { + qWarning() << "Internal error"; + } } VehicleComponent::~VehicleComponent() @@ -31,19 +32,23 @@ VehicleComponent::~VehicleComponent() void VehicleComponent::addSummaryQmlComponent(QQmlContext* context, QQuickItem* parent) { - Q_ASSERT(context); - - // FIXME: We own this object now, need to delete somewhere - QQmlComponent component(context->engine(), QUrl::fromUserInput("qrc:/qml/VehicleComponentSummaryButton.qml")); - if (component.status() == QQmlComponent::Error) { - qDebug() << component.errors(); - Q_ASSERT(false); + if (context) { + // FIXME: We own this object now, need to delete somewhere + QQmlComponent component(context->engine(), QUrl::fromUserInput("qrc:/qml/VehicleComponentSummaryButton.qml")); + if (component.status() == QQmlComponent::Error) { + qWarning() << component.errors(); + } else { + QQuickItem* item = qobject_cast(component.create(context)); + if (item) { + item->setParentItem(parent); + item->setProperty("vehicleComponent", QVariant::fromValue(this)); + } else { + qWarning() << "Internal error"; + } + } + } else { + qWarning() << "Internal error"; } - - QQuickItem* item = qobject_cast(component.create(context)); - Q_ASSERT(item); - item->setParentItem(parent); - item->setProperty("vehicleComponent", QVariant::fromValue(this)); } void VehicleComponent::setupTriggerSignals(void) diff --git a/src/ViewWidgets/ViewWidgetController.cc b/src/ViewWidgets/ViewWidgetController.cc index c9651908b9a7252d92464e5268980d697a0a64a3..3a3ba13d53742daa7c4de97f132da1c52060ef49 100644 --- a/src/ViewWidgets/ViewWidgetController.cc +++ b/src/ViewWidgets/ViewWidgetController.cc @@ -33,11 +33,11 @@ void ViewWidgetController::_vehicleAvailable(bool available) _uas = vehicle->uas(); _autopilot = vehicle->autopilotPlugin(); - Q_ASSERT(_autopilot); emit pluginConnected(QVariant::fromValue(_autopilot)); } } -Q_INVOKABLE void ViewWidgetController::checkForVehicle(void) + +void ViewWidgetController::checkForVehicle(void) { _vehicleAvailable(qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()); } diff --git a/src/api/QGCCorePlugin.cc b/src/api/QGCCorePlugin.cc index 1f9bf4e92f6a34c22b257d17ce197c17afb6a50b..909fa3f9bc66da56fc85f79a412f216ae495eff2 100644 --- a/src/api/QGCCorePlugin.cc +++ b/src/api/QGCCorePlugin.cc @@ -184,3 +184,23 @@ void QGCCorePlugin::setShowAdvancedUI(bool show) emit showAdvancedUIChanged(show); } } + +void QGCCorePlugin::paletteOverride(QString colorName, QGCPalette::PaletteColorInfo_t& colorInfo) +{ + Q_UNUSED(colorName); + Q_UNUSED(colorInfo); +} + +QString QGCCorePlugin::showAdvancedUIMessage(void) const +{ + return tr("WARNING: You are about to enter Advanced Mode. " + "If used incorrectly, this may cause your vehicle to malfunction thus voiding your warranty. " + "You should do so only if instructed by customer support. " + "Are you sure you want to enable Advanced Mode?"); +} + +void QGCCorePlugin::valuesWidgetDefaultSettings(QStringList& largeValues, QStringList& smallValues) +{ + Q_UNUSED(smallValues); + largeValues << "Vehicle.altitudeRelative" << "Vehicle.groundSpeed" << "Vehicle.flightTime"; +} diff --git a/src/api/QGCCorePlugin.h b/src/api/QGCCorePlugin.h index acb5b4f528964a61f4f20d90c4f565cb7596fcf3..567e8512c235fc788b8d780c32251514f0424224 100644 --- a/src/api/QGCCorePlugin.h +++ b/src/api/QGCCorePlugin.h @@ -10,6 +10,7 @@ #pragma once #include "QGCToolbox.h" +#include "QGCPalette.h" #include #include @@ -47,25 +48,25 @@ public: /// The list of settings under the Settings Menu /// @return A list of QGCSettings - virtual QVariantList& settingsPages (); + virtual QVariantList& settingsPages(void); /// The default settings panel to show /// @return The settings index - virtual int defaultSettings (); + virtual int defaultSettings(void); /// Global options /// @return An instance of QGCOptions - virtual QGCOptions* options (); + virtual QGCOptions* options(void); /// Allows the core plugin to override the visibility for a settings group /// @param name - Setting group name /// @return true: Show settings ui, false: Hide settings ui - virtual bool overrideSettingsGroupVisibility (QString name); + virtual bool overrideSettingsGroupVisibility(QString name); /// Allows the core plugin to override the setting meta data before the setting fact is created. /// @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(FactMetaData& metaData); /// Return the resource file which contains the brand image for for Indoor theme. virtual QString brandImageIndoor(void) const { return QString(); } @@ -74,11 +75,16 @@ public: virtual QString brandImageOutdoor(void) const { return QString(); } /// @return The message to show to the user when they a re prompted to confirm turning on advanced ui. - virtual QString showAdvancedUIMessage(void) const { return tr("WARNING: You are about to enter Advanced Mode. This may expose features which may cause your vehicle to malfunction. " - "You should do so only if instructed by customer support. Are you sure you want to enable Advanced Mode?"); } + virtual QString showAdvancedUIMessage(void) const; - /// @return An instance of an alternate postion source (or NULL if not available) - virtual QGeoPositionInfoSource* createPositionSource (QObject* parent) { Q_UNUSED(parent); return NULL; } + /// @return An instance of an alternate position source (or NULL if not available) + virtual QGeoPositionInfoSource* createPositionSource(QObject* parent) { Q_UNUSED(parent); return NULL; } + + /// Allows a plugin to override the specified color name from the palette + virtual void paletteOverride(QString colorName, QGCPalette::PaletteColorInfo_t& colorInfo); + + /// Allows the plugin the override the default settings for the Values Widget large and small values + virtual void valuesWidgetDefaultSettings(QStringList& largeValues, QStringList& smallValues); bool showTouchAreas(void) const { return _showTouchAreas; } bool showAdvancedUI(void) const { return _showAdvancedUI; } diff --git a/src/api/QGCOptions.h b/src/api/QGCOptions.h index de9940e210726bc1908624e6d65952a3ef1bec2e..b79fd9f344acdabd00254c1567e8158da068c901 100644 --- a/src/api/QGCOptions.h +++ b/src/api/QGCOptions.h @@ -41,6 +41,7 @@ public: Q_PROPERTY(bool guidedBarShowEmergencyStop READ guidedBarShowEmergencyStop NOTIFY guidedBarShowEmergencyStopChanged) Q_PROPERTY(bool guidedBarShowOrbit READ guidedBarShowOrbit NOTIFY guidedBarShowOrbitChanged) Q_PROPERTY(bool missionWaypointsOnly READ missionWaypointsOnly NOTIFY missionWaypointsOnlyChanged) + Q_PROPERTY(bool multiVehicleEnabled READ multiVehicleEnabled NOTIFY multiVehicleEnabledChanged) /// 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. @@ -68,13 +69,11 @@ public: virtual bool showSensorCalibrationAirspeed () const { return true; } virtual bool wifiReliableForCalibration () const { return false; } virtual bool sensorsHaveFixedOrientation () const { return false; } - virtual bool showFirmwareUpgrade () const { return true; } - virtual bool guidedBarShowEmergencyStop () const { return true; } 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 /// If returned QString in non-empty it means that firmware upgrade will run in a mode which only /// supports downloading a single firmware file from the URL. It also supports custom install through @@ -91,6 +90,7 @@ signals: void guidedBarShowEmergencyStopChanged (bool show); void guidedBarShowOrbitChanged (bool show); void missionWaypointsOnlyChanged (bool missionWaypointsOnly); + void multiVehicleEnabledChanged (bool multiVehicleEnabled); private: CustomInstrumentWidget* _defaultInstrumentWidget; diff --git a/src/comm/LinkInterface.cc b/src/comm/LinkInterface.cc index 9361fb2b77faf1421d56d9db874a19e6d4d61aa7..f995fd10482bd51ff7010b06d8eb2697e65a1d51 100644 --- a/src/comm/LinkInterface.cc +++ b/src/comm/LinkInterface.cc @@ -152,7 +152,9 @@ qint64 LinkInterface::_getCurrentDataRate(int index, const qint64 dataWriteTimes /// Sets the mavlink channel to use for this link void LinkInterface::_setMavlinkChannel(uint8_t channel) { - Q_ASSERT(!_mavlinkChannelSet); + if (_mavlinkChannelSet) { + qWarning() << "Mavlink channel set multiple times"; + } _mavlinkChannelSet = true; _mavlinkChannel = channel; } diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index ff5350a53dfcd5c95cd029532d82e7865912d710..f08b83f9b10fd9af294a0a705913790817e44cd9 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -26,7 +26,7 @@ #endif #ifndef __mobile__ - #include "GPSManager.h" +#include "GPSManager.h" #endif QGC_LOGGING_CATEGORY(LinkManagerLog, "LinkManagerLog") @@ -69,10 +69,10 @@ LinkManager::~LinkManager() void LinkManager::setToolbox(QGCToolbox *toolbox) { - QGCTool::setToolbox(toolbox); + QGCTool::setToolbox(toolbox); - _autoConnectSettings = toolbox->settingsManager()->autoConnectSettings(); - _mavlinkProtocol = _toolbox->mavlinkProtocol(); + _autoConnectSettings = toolbox->settingsManager()->autoConnectSettings(); + _mavlinkProtocol = _toolbox->mavlinkProtocol(); connect(&_portListTimer, &QTimer::timeout, this, &LinkManager::_updateAutoConnectLinks); _portListTimer.start(_autoconnectUpdateTimerMSecs); // timeout must be long enough to get past bootloader on second pass @@ -99,45 +99,45 @@ LinkInterface* LinkManager::createConnectedLink(SharedLinkConfigurationPointer& LinkInterface* pLink = NULL; switch(config->type()) { #ifndef NO_SERIAL_LINK - case LinkConfiguration::TypeSerial: - { - SerialConfiguration* serialConfig = dynamic_cast(config.data()); - if (serialConfig) { - pLink = new SerialLink(config); - if (serialConfig->usbDirect()) { - _activeLinkCheckList.append((SerialLink*)pLink); - if (!_activeLinkCheckTimer.isActive()) { - _activeLinkCheckTimer.start(); - } + case LinkConfiguration::TypeSerial: + { + SerialConfiguration* serialConfig = dynamic_cast(config.data()); + if (serialConfig) { + pLink = new SerialLink(config); + if (serialConfig->usbDirect()) { + _activeLinkCheckList.append((SerialLink*)pLink); + if (!_activeLinkCheckTimer.isActive()) { + _activeLinkCheckTimer.start(); } } } + } break; #endif - case LinkConfiguration::TypeUdp: - pLink = new UDPLink(config); - break; - case LinkConfiguration::TypeTcp: - pLink = new TCPLink(config); - break; + case LinkConfiguration::TypeUdp: + pLink = new UDPLink(config); + break; + case LinkConfiguration::TypeTcp: + pLink = new TCPLink(config); + break; #ifdef QGC_ENABLE_BLUETOOTH - case LinkConfiguration::TypeBluetooth: - pLink = new BluetoothLink(config); - break; + case LinkConfiguration::TypeBluetooth: + pLink = new BluetoothLink(config); + break; #endif #ifndef __mobile__ - case LinkConfiguration::TypeLogReplay: - pLink = new LogReplayLink(config); - break; + case LinkConfiguration::TypeLogReplay: + pLink = new LogReplayLink(config); + break; #endif #ifdef QT_DEBUG - case LinkConfiguration::TypeMock: - pLink = new MockLink(config); - break; + case LinkConfiguration::TypeMock: + pLink = new MockLink(config); + break; #endif - case LinkConfiguration::TypeLast: - default: - break; + case LinkConfiguration::TypeLast: + default: + break; } if (pLink) { @@ -150,11 +150,15 @@ LinkInterface* LinkManager::createConnectedLink(SharedLinkConfigurationPointer& LinkInterface* LinkManager::createConnectedLink(const QString& name) { - Q_ASSERT(name.isEmpty() == false); - for(int i = 0; i < _sharedConfigurations.count(); i++) { - SharedLinkConfigurationPointer& conf = _sharedConfigurations[i]; - if (conf->name() == name) - return createConnectedLink(conf); + if (name.isEmpty()) { + qWarning() << "Internal error"; + } else { + for(int i = 0; i < _sharedConfigurations.count(); i++) { + SharedLinkConfigurationPointer& conf = _sharedConfigurations[i]; + if (conf->name() == name) { + return createConnectedLink(conf); + } + } } return NULL; } @@ -171,25 +175,12 @@ void LinkManager::_addLink(LinkInterface* link) } if (!containsLink(link)) { - bool channelSet = false; - - // Find a mavlink channel to use for this link, Channel 0 is reserved for internal use. - for (int i=1; i<32; i++) { - if (!(_mavlinkChannelsUsedBitMask & 1 << i)) { - mavlink_reset_channel_status(i); - link->_setMavlinkChannel(i); - // Start the channel on Mav 1 protocol - mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(i); - mavlinkStatus->flags = mavlink_get_channel_status(i)->flags | MAVLINK_STATUS_FLAG_OUT_MAVLINK1; - qDebug() << "LinkManager mavlinkStatus:channel:flags" << mavlinkStatus << i << mavlinkStatus->flags; - _mavlinkChannelsUsedBitMask |= 1 << i; - channelSet = true; - break; - } - } - - if (!channelSet) { + int mavlinkChannel = _reserveMavlinkChannel(); + if (mavlinkChannel != 0) { + link->_setMavlinkChannel(mavlinkChannel); + } else { qWarning() << "Ran out of mavlink channels"; + return; } _sharedLinks.append(SharedLinkInterfacePointer(link)); @@ -219,13 +210,15 @@ void LinkManager::disconnectAll(void) bool LinkManager::connectLink(LinkInterface* link) { - Q_ASSERT(link); - - if (_connectionsSuspendedMsg()) { + if (link) { + if (_connectionsSuspendedMsg()) { + return false; + } + return link->_connect(); + } else { + qWarning() << "Internal error"; return false; } - - return link->_connect(); } void LinkManager::disconnectLink(LinkInterface* link) @@ -260,7 +253,7 @@ void LinkManager::_deleteLink(LinkInterface* link) } // Free up the mavlink channel associated with this link - _mavlinkChannelsUsedBitMask &= ~(1 << link->mavlinkChannel()); + _freeMavlinkChannel(link->mavlinkChannel()); for (int i=0; i<_sharedLinks.count(); i++) { if (_sharedLinks[i].data() == link) { @@ -301,7 +294,6 @@ void LinkManager::setConnectionsSuspended(QString reason) { _connectionsSuspended = true; _connectionsSuspendedReason = reason; - Q_ASSERT(!reason.isEmpty()); } void LinkManager::_linkConnected(void) @@ -372,34 +364,34 @@ void LinkManager::loadLinkConfigurationList() bool autoConnect = settings.value(root + "/auto").toBool(); switch((LinkConfiguration::LinkType)type) { #ifndef NO_SERIAL_LINK - case LinkConfiguration::TypeSerial: - pLink = (LinkConfiguration*)new SerialConfiguration(name); - break; + case LinkConfiguration::TypeSerial: + pLink = (LinkConfiguration*)new SerialConfiguration(name); + break; #endif - case LinkConfiguration::TypeUdp: - pLink = (LinkConfiguration*)new UDPConfiguration(name); - break; - case LinkConfiguration::TypeTcp: - pLink = (LinkConfiguration*)new TCPConfiguration(name); - break; + case LinkConfiguration::TypeUdp: + pLink = (LinkConfiguration*)new UDPConfiguration(name); + break; + case LinkConfiguration::TypeTcp: + pLink = (LinkConfiguration*)new TCPConfiguration(name); + break; #ifdef QGC_ENABLE_BLUETOOTH - case LinkConfiguration::TypeBluetooth: - pLink = (LinkConfiguration*)new BluetoothConfiguration(name); - break; + case LinkConfiguration::TypeBluetooth: + pLink = (LinkConfiguration*)new BluetoothConfiguration(name); + break; #endif #ifndef __mobile__ - case LinkConfiguration::TypeLogReplay: - pLink = (LinkConfiguration*)new LogReplayLinkConfiguration(name); - break; + case LinkConfiguration::TypeLogReplay: + pLink = (LinkConfiguration*)new LogReplayLinkConfiguration(name); + break; #endif #ifdef QT_DEBUG - case LinkConfiguration::TypeMock: - pLink = (LinkConfiguration*)new MockConfiguration(name); - break; + case LinkConfiguration::TypeMock: + pLink = (LinkConfiguration*)new MockConfiguration(name); + break; #endif - default: - case LinkConfiguration::TypeLast: - break; + default: + case LinkConfiguration::TypeLast: + break; } if(pLink) { //-- Have the instance load its own values @@ -469,7 +461,7 @@ void LinkManager::_updateAutoConnectLinks(void) qCDebug(LinkManagerLog) << "New auto-connect UDP port added"; UDPConfiguration* udpConfig = new UDPConfiguration(_defaultUPDLinkName); udpConfig->setLocalPort(QGC_UDP_LOCAL_PORT); - udpConfig->setDynamic(true); + udpConfig->setDynamic(true); SharedLinkConfigurationPointer config = addConfiguration(udpConfig); createConnectedLink(config); emit linkConfigurationsChanged(); @@ -647,7 +639,9 @@ QStringList LinkManager::linkTypeStrings(void) const #ifndef __mobile__ list += "Log Replay"; #endif - Q_ASSERT(list.size() == (int)LinkConfiguration::TypeLast); + if (list.size() != (int)LinkConfiguration::TypeLast) { + qWarning() << "Internal error"; + } } return list; } @@ -697,24 +691,29 @@ QStringList LinkManager::serialBaudRates(void) bool LinkManager::endConfigurationEditing(LinkConfiguration* config, LinkConfiguration* editedConfig) { - Q_ASSERT(config != NULL); - Q_ASSERT(editedConfig != NULL); - _fixUnnamed(editedConfig); - config->copyFrom(editedConfig); - saveLinkConfigurationList(); - // Tell link about changes (if any) - config->updateSettings(); - // Discard temporary duplicate - delete editedConfig; + if (config && editedConfig) { + _fixUnnamed(editedConfig); + config->copyFrom(editedConfig); + saveLinkConfigurationList(); + // Tell link about changes (if any) + config->updateSettings(); + // Discard temporary duplicate + delete editedConfig; + } else { + qWarning() << "Internal error"; + } return true; } bool LinkManager::endCreateConfiguration(LinkConfiguration* config) { - Q_ASSERT(config != NULL); - _fixUnnamed(config); - addConfiguration(config); - saveLinkConfigurationList(); + if (config) { + _fixUnnamed(config); + addConfiguration(config); + saveLinkConfigurationList(); + } else { + qWarning() << "Internal error"; + } return true; } @@ -729,21 +728,25 @@ LinkConfiguration* LinkManager::createConfiguration(int type, const QString& nam LinkConfiguration* LinkManager::startConfigurationEditing(LinkConfiguration* config) { - Q_ASSERT(config != NULL); + if (config) { #ifndef NO_SERIAL_LINK - if(config->type() == LinkConfiguration::TypeSerial) - _updateSerialPorts(); + if(config->type() == LinkConfiguration::TypeSerial) + _updateSerialPorts(); #endif - return LinkConfiguration::duplicateSettings(config); + return LinkConfiguration::duplicateSettings(config); + } else { + qWarning() << "Internal error"; + return NULL; + } } void LinkManager::_fixUnnamed(LinkConfiguration* config) { - Q_ASSERT(config != NULL); - //-- Check for "Unnamed" - if (config->name() == "Unnamed") { - switch(config->type()) { + if (config) { + //-- Check for "Unnamed" + if (config->name() == "Unnamed") { + switch(config->type()) { #ifndef NO_SERIAL_LINK case LinkConfiguration::TypeSerial: { QString tname = dynamic_cast(config)->portName(); @@ -755,61 +758,67 @@ void LinkManager::_fixUnnamed(LinkConfiguration* config) #endif config->setName(QString("Serial Device on %1").arg(tname)); break; - } + } #endif case LinkConfiguration::TypeUdp: config->setName( - QString("UDP Link on Port %1").arg(dynamic_cast(config)->localPort())); + QString("UDP Link on Port %1").arg(dynamic_cast(config)->localPort())); break; case LinkConfiguration::TypeTcp: { - TCPConfiguration* tconfig = dynamic_cast(config); - if(tconfig) { - config->setName( - QString("TCP Link %1:%2").arg(tconfig->address().toString()).arg((int)tconfig->port())); - } + TCPConfiguration* tconfig = dynamic_cast(config); + if(tconfig) { + config->setName( + QString("TCP Link %1:%2").arg(tconfig->address().toString()).arg((int)tconfig->port())); } + } break; #ifdef QGC_ENABLE_BLUETOOTH case LinkConfiguration::TypeBluetooth: { - BluetoothConfiguration* tconfig = dynamic_cast(config); - if(tconfig) { - config->setName(QString("%1 (Bluetooth Device)").arg(tconfig->device().name)); - } + BluetoothConfiguration* tconfig = dynamic_cast(config); + if(tconfig) { + config->setName(QString("%1 (Bluetooth Device)").arg(tconfig->device().name)); } + } break; #endif #ifndef __mobile__ case LinkConfiguration::TypeLogReplay: { - LogReplayLinkConfiguration* tconfig = dynamic_cast(config); - if(tconfig) { - config->setName(QString("Log Replay %1").arg(tconfig->logFilenameShort())); - } + LogReplayLinkConfiguration* tconfig = dynamic_cast(config); + if(tconfig) { + config->setName(QString("Log Replay %1").arg(tconfig->logFilenameShort())); } + } break; #endif #ifdef QT_DEBUG case LinkConfiguration::TypeMock: config->setName( - QString("Mock Link")); + QString("Mock Link")); break; #endif case LinkConfiguration::TypeLast: default: break; + } } + } else { + qWarning() << "Internal error"; } } void LinkManager::removeConfiguration(LinkConfiguration* config) { - Q_ASSERT(config != NULL); - LinkInterface* iface = config->link(); - if(iface) { - disconnectLink(iface); - } + if (config) { + LinkInterface* iface = config->link(); + if(iface) { + disconnectLink(iface); + } - _removeConfiguration(config); - saveLinkConfigurationList(); + _removeConfiguration(config); + saveLinkConfigurationList(); + } else { + qWarning() << "Internal error"; + } } bool LinkManager::isAutoconnectLink(LinkInterface* link) @@ -925,3 +934,25 @@ void LinkManager::startAutoConnectedLinks(void) createConnectedLink(conf); } } + +int LinkManager::_reserveMavlinkChannel(void) +{ + // Find a mavlink channel to use for this link, Channel 0 is reserved for internal use. + for (int mavlinkChannel=1; mavlinkChannel<32; mavlinkChannel++) { + if (!(_mavlinkChannelsUsedBitMask & 1 << mavlinkChannel)) { + mavlink_reset_channel_status(mavlinkChannel); + // Start the channel on Mav 1 protocol + mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(mavlinkChannel); + mavlinkStatus->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + _mavlinkChannelsUsedBitMask |= 1 << mavlinkChannel; + return mavlinkChannel; + } + } + + return 0; // All channels reserved +} + +void LinkManager::_freeMavlinkChannel(int channel) +{ + _mavlinkChannelsUsedBitMask &= ~(1 << channel); +} diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index 7a44cb0eb1602e33b9026a27b845d13f9fc3d12e..fc2a9c74e879daea42a5aa730f626b3e3e3b3138 100644 --- a/src/comm/LinkManager.h +++ b/src/comm/LinkManager.h @@ -153,6 +153,13 @@ public: void startAutoConnectedLinks(void); + /// Reserves a mavlink channel for use + /// @return Mavlink channel index, 0 for no channels available + int _reserveMavlinkChannel(void); + + /// Free the specified mavlink channel for re-use + void _freeMavlinkChannel(int channel); + static const char* settingsGroup; signals: diff --git a/src/comm/LogReplayLink.cc b/src/comm/LogReplayLink.cc index 1fc8e73e16145114d7365c4678b9dc831b3f5153..8bbe32dd078c487911ff2f39a47e6beff965b034 100644 --- a/src/comm/LogReplayLink.cc +++ b/src/comm/LogReplayLink.cc @@ -35,8 +35,11 @@ void LogReplayLinkConfiguration::copyFrom(LinkConfiguration *source) { LinkConfiguration::copyFrom(source); LogReplayLinkConfiguration* ssource = dynamic_cast(source); - Q_ASSERT(ssource != NULL); - _logFilename = ssource->logFilename(); + if (ssource) { + _logFilename = ssource->logFilename(); + } else { + qWarning() << "Internal error"; + } } void LogReplayLinkConfiguration::saveSettings(QSettings& settings, const QString& root) @@ -70,7 +73,9 @@ LogReplayLink::LogReplayLink(SharedLinkConfigurationPointer& config) , _connected(false) , _replayAccelerationFactor(1.0f) { - Q_ASSERT(_logReplayConfig); + if (!_logReplayConfig) { + qWarning() << "Internal error"; + } _readTickTimer.moveToThread(this); diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index faab603bc703aeeae515ae6b1808bc7334f6a93a..77b240b6222f49a8c24b54fa19d3986df6723167 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -208,39 +208,6 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) link->setDecodedFirstMavlinkPacket(true); } - if(message.msgid == MAVLINK_MSG_ID_RADIO_STATUS) - { - // process telemetry status message - mavlink_radio_status_t rstatus; - mavlink_msg_radio_status_decode(&message, &rstatus); - int rssi = rstatus.rssi; - int remrssi = rstatus.remrssi; - int noise = rstatus.noise; - int remnoise = rstatus.remnoise; - // 3DR Si1k radio needs rssi fields to be converted to dBm - if (message.sysid == '3' && message.compid == 'D') { - /* Per the Si1K datasheet figure 23.25 and SI AN474 code - * samples the relationship between the RSSI register - * and received power is as follows: - * - * 10 - * inputPower = rssi * ------ 127 - * 19 - * - * Additionally limit to the only realistic range [-120,0] dBm - */ - rssi = qMin(qMax(qRound(static_cast(rssi) / 1.9 - 127.0), - 120), 0); - remrssi = qMin(qMax(qRound(static_cast(remrssi) / 1.9 - 127.0), - 120), 0); - } else { - rssi = (int8_t) rstatus.rssi; - remrssi = (int8_t) rstatus.remrssi; - noise = (int8_t) rstatus.noise; - remnoise = (int8_t) rstatus.remnoise; - } - emit radioStatusChanged(link, rstatus.rxerrors, rstatus.fixed, rssi, remrssi, - rstatus.txbuf, noise, remnoise); - } - #ifndef __mobile__ // Log data diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index b3cd24ff6d4720a184c8f62e6d2ce1437d4c4898..93d14200371b4cb5a60ad50a466eed93fe722e63 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -49,6 +49,7 @@ MockLink::MockLink(SharedLinkConfigurationPointer& config) , _missionItemHandler(this, qgcApp()->toolbox()->mavlinkProtocol()) , _name("MockLink") , _connected(false) + , _mavlinkChannel(0) , _vehicleSystemId(_nextVehicleSystemId++) , _vehicleComponentId(200) , _inNSH(false) @@ -100,6 +101,14 @@ bool MockLink::_connect(void) { if (!_connected) { _connected = true; + _mavlinkChannel = qgcApp()->toolbox()->linkManager()->_reserveMavlinkChannel(); + if (_mavlinkChannel == 0) { + qWarning() << "No mavlink channels available"; + return false; + } + // MockLinks use Mavlink 2.0 + mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(_mavlinkChannel); + mavlinkStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; start(); emit connected(); } @@ -110,6 +119,9 @@ bool MockLink::_connect(void) void MockLink::_disconnect(void) { if (_connected) { + if (_mavlinkChannel != 0) { + qgcApp()->toolbox()->linkManager()->_freeMavlinkChannel(_mavlinkChannel); + } _connected = false; quit(); wait(); @@ -261,7 +273,7 @@ void MockLink::_sendHeartBeat(void) mavlink_msg_heartbeat_pack_chan(_vehicleSystemId, _vehicleComponentId, - mavlinkChannel(), + _mavlinkChannel, &msg, _vehicleType, // MAV_TYPE _firmwareType, // MAV_AUTOPILOT @@ -278,7 +290,7 @@ void MockLink::_sendVibration(void) mavlink_msg_vibration_pack_chan(_vehicleSystemId, _vehicleComponentId, - mavlinkChannel(), + _mavlinkChannel, &msg, 0, // time_usec 50.5, // vibration_x, @@ -347,7 +359,7 @@ void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes) for (qint64 i=0; iseverity, status->msg); @@ -1113,7 +1126,7 @@ void MockLink::_sendRCChannels(void) mavlink_msg_rc_channels_pack_chan(_vehicleSystemId, _vehicleComponentId, - mavlinkChannel(), + _mavlinkChannel, &msg, 0, // time_boot_ms 8, // chancount @@ -1155,7 +1168,7 @@ void MockLink::_handlePreFlightCalibration(const mavlink_command_long_t& request mavlink_message_t msg; mavlink_msg_statustext_pack_chan(_vehicleSystemId, _vehicleComponentId, - mavlinkChannel(), + _mavlinkChannel, &msg, MAV_SEVERITY_INFO, pCalMessage); @@ -1176,7 +1189,7 @@ void MockLink::_handleLogRequestList(const mavlink_message_t& msg) mavlink_message_t responseMsg; mavlink_msg_log_entry_pack_chan(_vehicleSystemId, _vehicleComponentId, - mavlinkChannel(), + _mavlinkChannel, &responseMsg, _logDownloadLogId, // log id 1, // num_logs @@ -1232,7 +1245,7 @@ void MockLink::_logDownloadWorker(void) mavlink_message_t responseMsg; mavlink_msg_log_data_pack_chan(_vehicleSystemId, _vehicleComponentId, - mavlinkChannel(), + _mavlinkChannel, &responseMsg, _logDownloadLogId, _logDownloadCurrentOffset, diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 15e8a5eebdff9b9e37ced799ab0185bf93c24379..90393507a18eac0d1667e461221b381c8a9ab970 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -141,7 +141,7 @@ public: /// Reset the state of the MissionItemHandler to no items, no transactions in progress. void resetMissionItemHandler(void) { _missionItemHandler.reset(); } - /// Returns the filename for the simulated log file. Onyl available after a download is requested. + /// Returns the filename for the simulated log file. Only available after a download is requested. QString logDownloadFile(void) { return _logDownloadFilename; } static MockLink* startPX4MockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); @@ -199,6 +199,7 @@ private: QString _name; bool _connected; + int _mavlinkChannel; uint8_t _vehicleSystemId; uint8_t _vehicleComponentId; diff --git a/src/comm/QGCSerialPortInfo.cc b/src/comm/QGCSerialPortInfo.cc index 5e50529b143297c1b99760f793020889fa23a238..1f07da7d2c152043b9ae16aef6f05584d804bfc0 100644 --- a/src/comm/QGCSerialPortInfo.cc +++ b/src/comm/QGCSerialPortInfo.cc @@ -204,7 +204,6 @@ bool QGCSerialPortInfo::getBoardInfo(QGCSerialPortInfo::BoardType_t& boardType, if (boardType == BoardTypeUnknown) { // Fall back to port name matching which could lead to incorrect board mapping. But in some cases the // vendor and product id do not come through correctly so this is used as a last chance detection method. - for (int i=0; i<_boardFallbackList.count(); i++) { const BoardFallback_t& boardFallback = _boardFallbackList[i]; diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc index 87b4247eb64c5a29be08a73109dc4bb175cf6181..247e7efa29913fc8aa726b76386aaad02b47faa2 100644 --- a/src/comm/QGCXPlaneLink.cc +++ b/src/comm/QGCXPlaneLink.cc @@ -202,7 +202,7 @@ void QGCXPlaneLink::run() struct iset_struct { char b[5]; - int index; // (0->20 in the lsit below) + int index; // (0->20 in the list below) char str_ipad_them[16]; char str_port_them[6]; char padding[2]; diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index 9e600db18d1256be835e7a0af4e8dba02d3b96f7..4719c33e940466380ca528d3f94004f408e17519 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -38,10 +38,13 @@ SerialLink::SerialLink(SharedLinkConfigurationPointer& config) , _reqReset(false) , _serialConfig(qobject_cast(config.data())) { - Q_ASSERT(_serialConfig); + if (!_serialConfig) { + qWarning() << "Internal error"; + return; + } qCDebug(SerialLinkLog) << "Create SerialLink " << _serialConfig->portName() << _serialConfig->baud() << _serialConfig->flowControl() - << _serialConfig->parity() << _serialConfig->dataBits() << _serialConfig->stopBits(); + << _serialConfig->parity() << _serialConfig->dataBits() << _serialConfig->stopBits(); qCDebug(SerialLinkLog) << "portName: " << _serialConfig->portName(); } @@ -76,7 +79,7 @@ bool SerialLink::_isBootloader() info.description().toLower().contains("px4 fmu v1.6"))) { qCDebug(SerialLinkLog) << "BOOTLOADER FOUND"; return true; - } + } } // Not found return false; @@ -229,7 +232,7 @@ bool SerialLink::_hardwareConnect(QSerialPort::SerialPortError& error, QString& emit connected(); qCDebug(SerialLinkLog) << "Connection SeriaLink: " << "with settings" << _serialConfig->portName() - << _serialConfig->baud() << _serialConfig->dataBits() << _serialConfig->parity() << _serialConfig->stopBits(); + << _serialConfig->baud() << _serialConfig->dataBits() << _serialConfig->parity() << _serialConfig->stopBits(); return true; // successful connection } @@ -299,34 +302,34 @@ qint64 SerialLink::getConnectionSpeed() const qint64 dataRate; switch (baudRate) { - case QSerialPort::Baud1200: - dataRate = 1200; - break; - case QSerialPort::Baud2400: - dataRate = 2400; - break; - case QSerialPort::Baud4800: - dataRate = 4800; - break; - case QSerialPort::Baud9600: - dataRate = 9600; - break; - case QSerialPort::Baud19200: - dataRate = 19200; - break; - case QSerialPort::Baud38400: - dataRate = 38400; - break; - case QSerialPort::Baud57600: - dataRate = 57600; - break; - case QSerialPort::Baud115200: - dataRate = 115200; - break; - // Otherwise do nothing. - default: - dataRate = -1; - break; + case QSerialPort::Baud1200: + dataRate = 1200; + break; + case QSerialPort::Baud2400: + dataRate = 2400; + break; + case QSerialPort::Baud4800: + dataRate = 4800; + break; + case QSerialPort::Baud9600: + dataRate = 9600; + break; + case QSerialPort::Baud19200: + dataRate = 19200; + break; + case QSerialPort::Baud38400: + dataRate = 38400; + break; + case QSerialPort::Baud57600: + dataRate = 57600; + break; + case QSerialPort::Baud115200: + dataRate = 115200; + break; + // Otherwise do nothing. + default: + dataRate = -1; + break; } return dataRate; } @@ -378,15 +381,18 @@ void SerialConfiguration::copyFrom(LinkConfiguration *source) { LinkConfiguration::copyFrom(source); SerialConfiguration* ssource = dynamic_cast(source); - Q_ASSERT(ssource != NULL); - _baud = ssource->baud(); - _flowControl = ssource->flowControl(); - _parity = ssource->parity(); - _dataBits = ssource->dataBits(); - _stopBits = ssource->stopBits(); - _portName = ssource->portName(); - _portDisplayName = ssource->portDisplayName(); - _usbDirect = ssource->_usbDirect; + if (ssource) { + _baud = ssource->baud(); + _flowControl = ssource->flowControl(); + _parity = ssource->parity(); + _dataBits = ssource->dataBits(); + _stopBits = ssource->stopBits(); + _portName = ssource->portName(); + _portDisplayName = ssource->portDisplayName(); + _usbDirect = ssource->_usbDirect; + } else { + qWarning() << "Internal error"; + } } void SerialConfiguration::updateSettings() diff --git a/src/comm/UDPLink.cc b/src/comm/UDPLink.cc index a286a078a05022a00af20213ff8df687cb0370f9..a0810ba566b76994777a8cd12895cbfd78255963 100644 --- a/src/comm/UDPLink.cc +++ b/src/comm/UDPLink.cc @@ -67,15 +67,17 @@ static QString get_ip_address(const QString& address) UDPLink::UDPLink(SharedLinkConfigurationPointer& config) : LinkInterface(config) -#if defined(QGC_ZEROCONF_ENABLED) + #if defined(QGC_ZEROCONF_ENABLED) , _dnssServiceRef(NULL) -#endif + #endif , _running(false) , _socket(NULL) , _udpConfig(qobject_cast(config.data())) , _connectState(false) { - Q_ASSERT(_udpConfig); + if (!_udpConfig) { + qWarning() << "Internal error"; + } moveToThread(this); } @@ -292,14 +294,14 @@ void UDPLink::_registerZeroconf(uint16_t port, const std::string ®Type) { #if defined(QGC_ZEROCONF_ENABLED) DNSServiceErrorType result = DNSServiceRegister(&_dnssServiceRef, 0, 0, 0, - regType.c_str(), - NULL, - NULL, - htons(port), - 0, - NULL, - NULL, - NULL); + regType.c_str(), + NULL, + NULL, + htons(port), + 0, + NULL, + NULL, + NULL); if (result != kDNSServiceErr_NoError) { emit communicationError("UDP Link Error", "Error registering Zeroconf"); @@ -315,10 +317,10 @@ void UDPLink::_deregisterZeroconf() { #if defined(QGC_ZEROCONF_ENABLED) if (_dnssServiceRef) - { - DNSServiceRefDeallocate(_dnssServiceRef); - _dnssServiceRef = NULL; - } + { + DNSServiceRefDeallocate(_dnssServiceRef); + _dnssServiceRef = NULL; + } #endif } @@ -347,15 +349,18 @@ void UDPConfiguration::copyFrom(LinkConfiguration *source) { LinkConfiguration::copyFrom(source); UDPConfiguration* usource = dynamic_cast(source); - Q_ASSERT(usource != NULL); - _localPort = usource->localPort(); - _hosts.clear(); - QString host; - int port; - if(usource->firstHost(host, port)) { - do { - addHost(host, port); - } while(usource->nextHost(host, port)); + if (usource) { + _localPort = usource->localPort(); + _hosts.clear(); + QString host; + int port; + if(usource->firstHost(host, port)) { + do { + addHost(host, port); + } while(usource->nextHost(host, port)); + } + } else { + qWarning() << "Internal error"; } } diff --git a/src/comm/USBBoardInfo.json b/src/comm/USBBoardInfo.json index 11465cbbb4e34f4275aed615f56ca2900d45d0a5..87f738cf498c3f42e669fe0917e69ea4cfa0853c 100644 --- a/src/comm/USBBoardInfo.json +++ b/src/comm/USBBoardInfo.json @@ -9,12 +9,15 @@ { "vendorID": 9900, "productID": 16, "boardClass": "Pixhawk", "name": "PX4 FMU V1" }, { "vendorID": 9900, "productID": 17, "boardClass": "Pixhawk", "name": "PX4 FMU V2" }, { "vendorID": 9900, "productID": 18, "boardClass": "Pixhawk", "name": "PX4 FMU V4" }, + { "vendorID": 9900, "productID": 19, "boardClass": "Pixhawk", "name": "PX4 FMU V4 PRO" }, { "vendorID": 9900, "productID": 22, "boardClass": "Pixhawk", "name": "PX4 FMU V2", "comment": "Bootloader on older Pixhawk V2 boards" }, { "vendorID": 9900, "productID": 4097, "boardClass": "Pixhawk", "name": "AeroCore" }, { "vendorID": 9900, "productID": 33, "boardClass": "Pixhawk", "name": "AUAV X2.1 FMU V2" }, { "vendorID": 9900, "productID": 48, "boardClass": "Pixhawk", "name": "MindPX FMU V2" }, + { "vendorID": 9900, "productID": 50, "boardClass": "Pixhawk", "name": "PX4 FMU V5" }, { "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": 21, "boardClass": "PX4 Flow", "name": "PX4 Flow" }, @@ -30,6 +33,10 @@ ], "boardFallback": [ + { "regExp": "^PX4 FMU v5.x$", "boardClass": "Pixhawk" }, + { "regExp": "^PX4 BL FMU v5.x$","boardClass": "Pixhawk" }, + { "regExp": "^PX4 FMU v4.x PRO$", "boardClass": "Pixhawk" }, + { "regExp": "^PX4 BL FMU v4.x PRO$","boardClass": "Pixhawk" }, { "regExp": "^PX4 FMU v4.x$", "boardClass": "Pixhawk" }, { "regExp": "^PX4 BL FMU v4.x$", "boardClass": "Pixhawk" }, { "regExp": "^PX4 FMU v2.x$", "boardClass": "Pixhawk" }, @@ -43,6 +50,8 @@ { "regExp": "^PX4 ASC v1.x$", "boardClass": "Pixhawk" }, { "regExp": "^PX4 BL ASC v1.x$", "boardClass": "Pixhawk" }, { "regExp": "^PX4 FMU", "boardClass": "Pixhawk" }, + { "regExp": "^PX4 Crazyflie v2.0", "boardClass": "Pixhawk" }, + { "regExp": "^Crazyflie BL", "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 161a02dfd903f5388ba16e39da98103b21fd6fa8..1352b175cf4c715ff0cf9c9c7b7f0ff3692d57af 100644 --- a/src/qgcunittest/UnitTest.cc +++ b/src/qgcunittest/UnitTest.cc @@ -504,3 +504,12 @@ bool UnitTest::fileCompare(const QString& file1, const QString& file2) return true; } + +bool UnitTest::doubleNaNCompare(double value1, double value2) +{ + if (qIsNaN(value1) && qIsNaN(value2)) { + return true; + } else { + return qFuzzyCompare(value1, value2); + } +} diff --git a/src/qgcunittest/UnitTest.h b/src/qgcunittest/UnitTest.h index fc06f287db325d505a6ff46072971c004b33b739..fa1d164f175ebeda594d3c909d5a4326686634ef 100644 --- a/src/qgcunittest/UnitTest.h +++ b/src/qgcunittest/UnitTest.h @@ -92,17 +92,21 @@ public: /// @return true: files are alike, false: files differ static bool fileCompare(const QString& file1, const QString& file2); + /// Fuzzy compare on two doubles, where NaN is a possible value + /// @return true: equal + static bool doubleNaNCompare(double value1, double value2); + protected slots: // These are all pure virtuals to force the derived class to implement each one and in turn // call the UnitTest private implementation. /// @brief Called before each test. - /// Make sure to call _init first in your derived class. + /// Make sure to call UnitTest::init first in your derived class. virtual void init(void); /// @brief Called after each test. - /// Make sure to call _cleanup first in your derived class. + /// Make sure to call UnitTest::cleanup last in your derived class. virtual void cleanup(void); protected: diff --git a/src/qgcunittest/UnitTestList.cc b/src/qgcunittest/UnitTestList.cc index d529c9acaf56bc00016748cf262b4f7b98c12db6..3fc648abf29f5575033b31927e81f0abc7a4d085 100644 --- a/src/qgcunittest/UnitTestList.cc +++ b/src/qgcunittest/UnitTestList.cc @@ -35,6 +35,7 @@ #include "VisualMissionItemTest.h" #include "CameraSectionTest.h" #include "SpeedSectionTest.h" +#include "PlanMasterControllerTest.h" UT_REGISTER_TEST(FactSystemTestGeneric) UT_REGISTER_TEST(FactSystemTestPX4) @@ -56,6 +57,7 @@ UT_REGISTER_TEST(SendMavCommandTest) UT_REGISTER_TEST(SurveyMissionItemTest) UT_REGISTER_TEST(CameraSectionTest) UT_REGISTER_TEST(SpeedSectionTest) +UT_REGISTER_TEST(PlanMasterControllerTest) // List of unit test which are currently disabled. // If disabling a new test, include reason in comment. @@ -66,5 +68,5 @@ UT_REGISTER_TEST(SpeedSectionTest) // Onboard file support has been removed until it can be make to work correctly //UT_REGISTER_TEST(FileManagerTest) -// Needs to be update for latest changes +// Needs to be update for latest updates //UT_REGISTER_TEST(MavlinkLogTest) diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 67621b33b234d9757f3e593cfb4407ef889e45d8..87cef32ba6d58cd364de6be41bd0f5b2bd2db328 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -92,10 +92,7 @@ static MainWindow* _instance = NULL; ///< @brief MainWindow singleton MainWindow* MainWindow::_create() { - Q_ASSERT(_instance == NULL); new MainWindow(); - // _instance is set in constructor - Q_ASSERT(_instance); return _instance; } @@ -118,7 +115,6 @@ MainWindow::MainWindow() , _mainQmlWidgetHolder(NULL) , _forceClose(false) { - Q_ASSERT(_instance == NULL); _instance = this; //-- Load fonts @@ -337,7 +333,7 @@ void MainWindow::_showDockWidget(const QString& name, bool show) // Create the inner widget if we need to if (!_mapName2DockWidget.contains(name)) { if(!_createInnerDockWidget(name)) { - qWarning() << "Trying to load non existing widget:" << name; + qWarning() << "Trying to load non existent widget:" << name; return; } } diff --git a/src/ui/QGCMAVLinkLogPlayer.cc b/src/ui/QGCMAVLinkLogPlayer.cc index cdfd274d84330ea1421091d85ff3fac5a1082f91..32ecda2516505a11207ff4925805b7e51d33deff 100644 --- a/src/ui/QGCMAVLinkLogPlayer.cc +++ b/src/ui/QGCMAVLinkLogPlayer.cc @@ -9,6 +9,8 @@ #include "QGC.h" #include "ui_QGCMAVLinkLogPlayer.h" #include "QGCApplication.h" +#include "SettingsManager.h" +#include "AppSettings.h" #include "LinkManager.h" #include "QGCQFileDialog.h" #include "QGCMessageBox.h" @@ -22,10 +24,10 @@ QGCMAVLinkLogPlayer::QGCMAVLinkLogPlayer(QWidget *parent) : _ui->horizontalLayout->setAlignment(Qt::AlignTop); // Setup buttons - connect(_ui->selectFileButton, &QPushButton::clicked, this, &QGCMAVLinkLogPlayer::_selectLogFileForPlayback); - connect(_ui->playButton, &QPushButton::clicked, this, &QGCMAVLinkLogPlayer::_playPauseToggle); - connect(_ui->positionSlider, &QSlider::valueChanged, this, &QGCMAVLinkLogPlayer::_setPlayheadFromSlider); - connect(_ui->positionSlider, &QSlider::sliderPressed, this, &QGCMAVLinkLogPlayer::_pause); + connect(_ui->selectFileButton, &QPushButton::clicked, this, &QGCMAVLinkLogPlayer::_selectLogFileForPlayback); + connect(_ui->playButton, &QPushButton::clicked, this, &QGCMAVLinkLogPlayer::_playPauseToggle); + connect(_ui->positionSlider, &QSlider::valueChanged, this, &QGCMAVLinkLogPlayer::_setPlayheadFromSlider); + connect(_ui->positionSlider, &QSlider::sliderPressed, this, &QGCMAVLinkLogPlayer::_pause); #if 0 // Speed slider is removed from 3.0 release. Too broken to fix. @@ -71,8 +73,8 @@ void QGCMAVLinkLogPlayer::_selectLogFileForPlayback(void) QString logFilename = QGCQFileDialog::getOpenFileName( this, - tr("Load MAVLink Log File"), - QStandardPaths::writableLocation(QStandardPaths::DocumentsLocation), + tr("Load Telemetry Log File"), + qgcApp()->toolbox()->settingsManager()->appSettings()->telemetrySavePath(), tr("MAVLink Log Files (*.tlog);;All Files (*)")); if (logFilename.isEmpty()) { @@ -138,6 +140,7 @@ void QGCMAVLinkLogPlayer::_playbackStarted(void) _enablePlaybackControls(true); _ui->playButton->setChecked(true); _ui->playButton->setIcon(QIcon(":/res/Pause")); + _ui->positionSlider->setEnabled(false); } /// Signalled from LogReplayLink when replay is paused @@ -145,6 +148,7 @@ void QGCMAVLinkLogPlayer::_playbackPaused(void) { _ui->playButton->setIcon(QIcon(":/res/Play")); _ui->playButton->setChecked(false); + _ui->positionSlider->setEnabled(true); } void QGCMAVLinkLogPlayer::_playbackPercentCompleteChanged(int percentComplete) diff --git a/src/ui/linechart/LinechartPlot.cc b/src/ui/linechart/LinechartPlot.cc index 61d14b870fa9f3ecf36607efe7b3621430b81040..9399e2ba379bbebd6c8376eb46c5c237151df512 100644 --- a/src/ui/linechart/LinechartPlot.cc +++ b/src/ui/linechart/LinechartPlot.cc @@ -557,7 +557,7 @@ quint64 LinechartPlot::getMaxTime() * @brief Get the plot interval * The plot interval is the time interval which is displayed on the plot * - * @return The plot inteval in milliseconds + * @return The plot interval in milliseconds * @see setPlotInterval() * @see getDataInterval() To get the interval for which data is available **/ diff --git a/src/ui/preferences/GeneralSettings.qml b/src/ui/preferences/GeneralSettings.qml index 0f1a160e6c3ec22fe1d17f1e842ace12d30336e7..810b3d31669414cae142ab7ba720f3943858b7e7 100644 --- a/src/ui/preferences/GeneralSettings.qml +++ b/src/ui/preferences/GeneralSettings.qml @@ -112,7 +112,7 @@ QGCView { } //----------------------------------------------------------------- - //-- Miscellanous + //-- Miscellaneous Item { width: _qgcView.width * 0.8 height: miscLabel.height @@ -425,7 +425,7 @@ QGCView { } QGCLabel { - text: qsTr("Minumum observation duration:") + text: qsTr("Minimum observation duration:") } FactTextField { fact: QGroundControl.settingsManager.rtkSettings.surveyInMinObservationDuration @@ -495,7 +495,7 @@ QGCView { visible: QGroundControl.settingsManager.videoSettings.visible QGCLabel { id: videoLabel - text: qsTr("Video (Requires Restart)") + text: qsTr("Video") font.family: ScreenTools.demiboldFontFamily } } diff --git a/src/ui/toolbar/ArmedIndicator.qml b/src/ui/toolbar/ArmedIndicator.qml index 9428825e5c9d6256c097f83dae3110b98fcdb88c..3b549dd7334c5d0f585817632b1b5fde11b8002b 100644 --- a/src/ui/toolbar/ArmedIndicator.qml +++ b/src/ui/toolbar/ArmedIndicator.qml @@ -26,11 +26,8 @@ QGCLabel { text: _armed ? qsTr("Armed") : qsTr("Disarmed") font.pointSize: ScreenTools.mediumFontPointSize color: qgcPal.buttonText - visible: !_autoDisarm || _fixedWing property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle - property bool _autoDisarm: _activeVehicle ? _activeVehicle.autoDisarm : false - property bool _fixedWing: _activeVehicle ? _activeVehicle.fixedWing : false property bool _armed: _activeVehicle ? _activeVehicle.armed : false QGCPalette { id: qgcPal } diff --git a/src/ui/toolbar/Images/Joystick.png b/src/ui/toolbar/Images/Joystick.png new file mode 100644 index 0000000000000000000000000000000000000000..bbaa8350d08ac3172e5dca97689c8b3dcf4fc5df Binary files /dev/null and b/src/ui/toolbar/Images/Joystick.png differ diff --git a/src/ui/toolbar/JoystickIndicator.qml b/src/ui/toolbar/JoystickIndicator.qml new file mode 100644 index 0000000000000000000000000000000000000000..ecfa8d95dfcbf61f58f840595238efa0a56cf2fc --- /dev/null +++ b/src/ui/toolbar/JoystickIndicator.qml @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * (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.5 +import QtQuick.Controls 1.2 +import QtQuick.Layouts 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 + +// Joystick Indicator +Item { + width: joystickRow.width * 1.1 + anchors.top: parent.top + anchors.bottom: parent.bottom + visible: activeVehicle ? activeVehicle.sub : false + + + Component { + id: joystickInfo + + Rectangle { + width: joystickCol.width + ScreenTools.defaultFontPixelWidth * 3 + height: joystickCol.height + ScreenTools.defaultFontPixelHeight * 2 + radius: ScreenTools.defaultFontPixelHeight * 0.5 + color: qgcPal.window + border.color: qgcPal.text + + Column { + id: joystickCol + spacing: ScreenTools.defaultFontPixelHeight * 0.5 + width: Math.max(joystickGrid.width, joystickLabel.width) + anchors.margins: ScreenTools.defaultFontPixelHeight + anchors.centerIn: parent + + QGCLabel { + id: joystickLabel + text: qsTr("Joystick Status") + font.family: ScreenTools.demiboldFontFamily + anchors.horizontalCenter: parent.horizontalCenter + } + + GridLayout { + id: joystickGrid + anchors.margins: ScreenTools.defaultFontPixelHeight + columnSpacing: ScreenTools.defaultFontPixelWidth + columns: 2 + anchors.horizontalCenter: parent.horizontalCenter + + QGCLabel { text: qsTr("Connected:") } + QGCLabel { + text: joystickManager.activeJoystick ? "Yes" : "No" + color: joystickManager.activeJoystick ? qgcPal.buttonText : "red" + } + QGCLabel { text: qsTr("Enabled:") } + QGCLabel { + text: activeVehicle && activeVehicle.joystickEnabled ? "Yes" : "No" + color: activeVehicle && activeVehicle.joystickEnabled ? qgcPal.buttonText : "red" + } + } + } + + Component.onCompleted: { + var pos = mapFromItem(toolBar, centerX - (width / 2), toolBar.height) + x = pos.x + y = pos.y + ScreenTools.defaultFontPixelHeight + } + } + } + + Row { + id: joystickRow + anchors.top: parent.top + anchors.bottom: parent.bottom + spacing: ScreenTools.defaultFontPixelWidth + + QGCColoredImage { + width: height + anchors.top: parent.top + anchors.bottom: parent.bottom + sourceSize.height: height + source: "/qmlimages/Joystick.png" + fillMode: Image.PreserveAspectFit + color: activeVehicle && activeVehicle.joystickEnabled && joystickManager.activeJoystick ? qgcPal.buttonText : "red" + } + } + + MouseArea { + anchors.fill: parent + onClicked: mainWindow.showPopUp(joystickInfo, mapToItem(toolBar, x, y).x + (width / 2)) + } +} diff --git a/src/ui/toolbar/MainToolBar.qml b/src/ui/toolbar/MainToolBar.qml index 03101ddca1859dc5def5bd3319e7d4d785d4b28e..aba0d72c253786488bb1e6bb095d050a294810eb 100644 --- a/src/ui/toolbar/MainToolBar.qml +++ b/src/ui/toolbar/MainToolBar.qml @@ -129,7 +129,7 @@ Rectangle { anchors.bottom: parent.bottom exclusiveGroup: mainActionGroup source: "/qmlimages/Analyze.svg" - visible: !ScreenTools.isMobile + visible: !ScreenTools.isMobile && QGroundControl.corePlugin.showAdvancedUI onClicked: toolBar.showAnalyzeView() } @@ -211,5 +211,4 @@ Rectangle { width: _activeVehicle ? _activeVehicle.parameterManager.loadProgress * parent.width : 0 color: qgcPal.colorGreen } - } diff --git a/src/ui/toolbar/RCRSSIIndicator.qml b/src/ui/toolbar/RCRSSIIndicator.qml index 0ec602c3f4f1993e447deb25c2e7b1149207c691..baf04b172e0d9009237c76614c81b09aa67ac795 100644 --- a/src/ui/toolbar/RCRSSIIndicator.qml +++ b/src/ui/toolbar/RCRSSIIndicator.qml @@ -19,7 +19,7 @@ import QGroundControl.ScreenTools 1.0 import QGroundControl.Palette 1.0 //------------------------------------------------------------------------- -//-- GPS Indicator +//-- RC RSSI Indicator Item { width: rssiRow.width * 1.1 anchors.top: parent.top diff --git a/src/ui/toolbar/TelemetryRSSIIndicator.qml b/src/ui/toolbar/TelemetryRSSIIndicator.qml index 89e7f5946d028307459c13f2e8e3de578156ef15..3de4c8a97d5310feab4a08a10cf286ec3c0a6a6c 100644 --- a/src/ui/toolbar/TelemetryRSSIIndicator.qml +++ b/src/ui/toolbar/TelemetryRSSIIndicator.qml @@ -20,16 +20,14 @@ import QGroundControl.Palette 1.0 //------------------------------------------------------------------------- //-- Telemetry RSSI -QGCColoredImage { - anchors.top: parent.top - anchors.bottom: parent.bottom - sourceSize.height: height - source: "/qmlimages/TelemRSSI.svg" - fillMode: Image.PreserveAspectFit - color: qgcPal.buttonText - visible: _activeVehicle ? (_activeVehicle.telemetryLRSSI < 0) : false +Item { + anchors.top: parent.top + anchors.bottom: parent.bottom + width: _hasTelemetry ? telemIcon.width * 1.1 : 0 + visible: _hasTelemetry - property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle + property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle + property bool _hasTelemetry: _activeVehicle ? _activeVehicle.telemetryLRSSI !== 0 : false Component { id: telemRSSIInfo @@ -58,9 +56,9 @@ QGCColoredImage { columns: 2 anchors.horizontalCenter: parent.horizontalCenter QGCLabel { text: qsTr("Local RSSI:") } - QGCLabel { text: _activeVehicle.telemetryLRSSI + " dBm" } + QGCLabel { text: _activeVehicle.telemetryLRSSI + " dBm"} QGCLabel { text: qsTr("Remote RSSI:") } - QGCLabel { text: _activeVehicle.telemetryRRSSI + " dBm" } + QGCLabel { text: _activeVehicle.telemetryRRSSI + " dBm"} QGCLabel { text: qsTr("RX Errors:") } QGCLabel { text: _activeVehicle.telemetryRXErrors } QGCLabel { text: qsTr("Errors Fixed:") } @@ -80,6 +78,16 @@ QGCColoredImage { } } } + QGCColoredImage { + id: telemIcon + anchors.top: parent.top + anchors.bottom: parent.bottom + width: height + sourceSize.height: height + source: "/qmlimages/TelemRSSI.svg" + fillMode: Image.PreserveAspectFit + color: qgcPal.buttonText + } MouseArea { anchors.fill: parent onClicked: {