diff --git a/.appveyor.yml b/.appveyor.yml index 57050f41ceb2a3b58c59bd36502e178673aab38d..8440d7a5a3d3cc0b4be873c9056a49030620bab7 100644 --- a/.appveyor.yml +++ b/.appveyor.yml @@ -50,23 +50,11 @@ build_script: test_script: - if "%CONFIG%" EQU "debug" ( %SHADOW_BUILD_DIR%\debug\qgroundcontrol --unittest ) -for: -- - branches: - only: - - master - artifacts: +artifacts: - path: QGroundControl-installer.exe name: qgcinstaller -- - branches: - only: - - Stable_V3.5 - artifacts: - - path: QGroundControl-installer.exe - name: qgcinstaller - - path: symbols\**\*.*_ - name: symbols + - path: build_windows_install\release\qgroundcontrol.pdb + name: pdb deploy: # deploy continuous builds to s3 @@ -101,19 +89,19 @@ deploy: # appveyor_repo_tag: false # deploy release symbols to s3 - - provider: S3 - access_key_id: - secure: IGAojLMqokL+76DbdulmWDA3MTsxEBBi3ReVVSqTy9c= - secret_access_key: - secure: RiYqaR+3T2PMNz2j5ur8LCA6H/Zfd4jTX33CZE5iBxm+zaz4QLs25p0B7prpaoNN - bucket: qgroundcontrol - region: us-west-2 - set_public: true - folder: releasesyms - artifact: symbols - on: - CONFIG: installer - appveyor_repo_tag: true +# - provider: S3 +# access_key_id: +# secure: IGAojLMqokL+76DbdulmWDA3MTsxEBBi3ReVVSqTy9c= +# secret_access_key: +# secure: RiYqaR+3T2PMNz2j5ur8LCA6H/Zfd4jTX33CZE5iBxm+zaz4QLs25p0B7prpaoNN +# bucket: qgroundcontrol +# region: us-west-2 +# set_public: true +# folder: releasesyms +# artifact: symbols +# on: +# CONFIG: installer +# appveyor_repo_tag: true # deploy tagged releases to Github releases - provider: GitHub @@ -155,3 +143,18 @@ deploy: on: CONFIG: installer appveyor_repo_tag: true + +# deploy pdb for tagged releases to s3 latest folder + - provider: S3 + access_key_id: + secure: IGAojLMqokL+76DbdulmWDA3MTsxEBBi3ReVVSqTy9c= + secret_access_key: + secure: RiYqaR+3T2PMNz2j5ur8LCA6H/Zfd4jTX33CZE5iBxm+zaz4QLs25p0B7prpaoNN + bucket: qgroundcontrol + region: us-west-2 + set_public: true + folder: latest + artifact: pdb + on: + CONFIG: installer + appveyor_repo_tag: true diff --git a/ChangeLog.md b/ChangeLog.md index acda03b96ce76e61e99f61c66cc33fc4742fcbd5..b76079bbc799135c4e2220713104c2166a9c4467 100644 --- a/ChangeLog.md +++ b/ChangeLog.md @@ -6,6 +6,7 @@ Note: This file only contains high level features or important fixes. ### 3.6.0 - Daily Build +* Plan/Pattern: Support named presets to simplify commonly used settings setup. Currently only supported by Survey. * ArduCopter: Handle 3.7 parameter name change from CH#_OPT to RC#_OPTION. * Improved support for flashing/connecting to ChibiOS bootloaders boards. * Making the camera API available to all firmwares, not just PX4. @@ -13,6 +14,7 @@ Note: This file only contains high level features or important fixes. * Major rewrite and bug fix pass through Structure Scan. Previous version had such bad problems that it can no longer be supported. Plans with Structure Scan will need to be recreated. New QGC will not load old Structure Scan plans. ### 3.5.1 - Not yet released +* Add ArduPilot CubeBlack Service Bulletin check * Fix visibility of PX4/ArduPilot logo in toolbar * Fix tile set count but in OfflineMaps which would cause image and elevation tile set to have incorrect counts and be incorrectly marked as download incomplete. diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index a07d060fdfc762fc0dd3d12bc568a6b87be73bb5..40a41ca643983a3cf6ea431fe1ee388c378fa2fa 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -210,6 +210,7 @@ src/Settings/APMMavlinkStreamRate.SettingsGroup.json + src/MissionManager/BreachReturn.FactMetaData.json src/MissionManager/CameraCalc.FactMetaData.json src/MissionManager/CameraSpec.FactMetaData.json src/MissionManager/CorridorScan.SettingsGroup.json diff --git a/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc b/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc index 22543319485a3391bfc5b5361c7b4a3df55181fb..abc156e12726412c5ccdcdf9ad2c8cb1a187b4d7 100644 --- a/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc @@ -29,6 +29,11 @@ #include "APMSubFrameComponent.h" #include "ESP8266Component.h" #include "APMHeliComponent.h" +#include "QGCApplication.h" + +#if !defined(NO_SERIAL_LINK) && !defined(__android__) +#include +#endif /// This is the AutoPilotPlugin implementatin for the MAV_AUTOPILOT_ARDUPILOT type. APMAutoPilotPlugin::APMAutoPilotPlugin(Vehicle* vehicle, QObject* parent) @@ -50,6 +55,10 @@ APMAutoPilotPlugin::APMAutoPilotPlugin(Vehicle* vehicle, QObject* parent) , _heliComponent (NULL) { APMAirframeLoader::loadAirframeFactMetaData(); + +#if !defined(NO_SERIAL_LINK) && !defined(__android__) + connect(vehicle->parameterManager(), &ParameterManager::parametersReadyChanged, this, &APMAutoPilotPlugin::_checkForBadCubeBlack); +#endif } APMAutoPilotPlugin::~APMAutoPilotPlugin() @@ -170,3 +179,34 @@ QString APMAutoPilotPlugin::prerequisiteSetup(VehicleComponent* component) const return QString(); } + +#if !defined(NO_SERIAL_LINK) && !defined(__android__) +/// The following code is executed when the Vehicle is parameter ready. It checks for the service bulletin against Cube Blacks. +void APMAutoPilotPlugin::_checkForBadCubeBlack(void) +{ + bool cubeBlackFound = false; + for (const QVariant& varLink: _vehicle->links()) { + SerialLink* serialLink = varLink.value(); + if (serialLink && QSerialPortInfo(*serialLink->_hackAccessToPort()).description().contains(QStringLiteral("CubeBlack"))) { + cubeBlackFound = true; + } + + } + if (!cubeBlackFound) { + return; + } + + ParameterManager* paramMgr = _vehicle->parameterManager(); + + QString paramAcc3("INS_ACC3_ID"); + QString paramGyr3("INS_GYR3_ID"); + QString paramEnableMask("INS_ENABLE_MASK"); + + if (paramMgr->parameterExists(-1, paramAcc3) && paramMgr->getParameter(-1, paramAcc3)->rawValue().toInt() == 0 && + paramMgr->parameterExists(-1, paramGyr3) && paramMgr->getParameter(-1, paramGyr3)->rawValue().toInt() == 0 && + paramMgr->parameterExists(-1, paramEnableMask) && paramMgr->getParameter(-1, paramEnableMask)->rawValue().toInt() >= 7) { + qgcApp()->showMessage(tr("WARNING: The flight board you are using has a critical service bulletin against ti which advise against flying. https://discuss.cubepilot.org/t/sb-0000002-critical-service-bulletin-for-cubes-purchased-between-january-2019-to-present-do-not-fly/406")); + + } +} +#endif diff --git a/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.h b/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.h index 09bb7c24f2480eafadae16c93fc134ef3a6b007a..5c6a5acc4cbaecfd23c19025dc6eb60d545d55d3 100644 --- a/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.h +++ b/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.h @@ -59,6 +59,11 @@ protected: ESP8266Component* _esp8266Component; APMHeliComponent* _heliComponent; +#if !defined(NO_SERIAL_LINK) && !defined(__android__) +private slots: + void _checkForBadCubeBlack(void); +#endif + private: QVariantList _components; }; diff --git a/src/AutoPilotPlugins/APM/APMPowerComponent.qml b/src/AutoPilotPlugins/APM/APMPowerComponent.qml index 43b5003968722006ed930b73d059592042cec9b8..355535e6bd6dcd979f9a260713dcaaf88d00a759 100644 --- a/src/AutoPilotPlugins/APM/APMPowerComponent.qml +++ b/src/AutoPilotPlugins/APM/APMPowerComponent.qml @@ -44,6 +44,8 @@ SetupPage { property bool _batt2ParamsAvailable: controller.parameterExists(-1, "BATT2_CAPACITY") property bool _showBatt1Reboot: _batt1MonitorEnabled && !_batt1ParamsAvailable property bool _showBatt2Reboot: _batt2MonitorEnabled && !_batt2ParamsAvailable + property bool _escCalibrationAvailable: controller.parameterExists(-1, "ESC_CALIBRATION") + property Fact _escCalibration: controller.getParameterFact(-1, "ESC_CALIBRATION", false /* reportMissing */) property string _restartRequired: qsTr("Requires vehicle reboot") @@ -216,6 +218,61 @@ SetupPage { } } } + + Column { + spacing: _margins / 2 + visible: _escCalibrationAvailable + + QGCLabel { + text: qsTr("ESC Calibration") + font.family: ScreenTools.demiboldFontFamily + } + + Rectangle { + width: escCalibrationHolder.x + escCalibrationHolder.width + _margins + height: escCalibrationHolder.y + escCalibrationHolder.height + _margins + color: ggcPal.windowShade + + Column { + id: escCalibrationHolder + x: _margins + y: _margins + spacing: _margins + + Column { + spacing: _margins + + QGCLabel { + text: qsTr("WARNING: Remove props prior to calibration!") + color: qgcPal.warningText + } + + Row { + spacing: _margins + + QGCButton { + text: qsTr("Calibrate") + enabled: _escCalibration.rawValue === 0 + onClicked: _escCalibration.rawValue = 3 + } + + Column { + enabled: _escCalibration.rawValue === 3 + QGCLabel { text: _escCalibration.rawValue === 3 ? qsTr("Now perform these steps:") : qsTr("Click Calibrate to start, then:") } + QGCLabel { text: qsTr("- Disconnect USB and battery so flight controller powers down") } + QGCLabel { text: qsTr("- Connect the battery") } + QGCLabel { text: qsTr("- The arming tone will be played (if the vehicle has a buzzer attached)") } + QGCLabel { text: qsTr("- If using a flight controller with a safety button press it until it displays solid red") } + QGCLabel { text: qsTr("- You will hear a musical tone then two beeps") } + QGCLabel { text: qsTr("- A few seconds later you should hear a number of beeps (one for each battery cell you’re using)") } + QGCLabel { text: qsTr("- And finally a single long beep indicating the end points have been set and the ESC is calibrated") } + QGCLabel { text: qsTr("- Disconnect the battery and power up again normally") } + } + } + } + } + } + } } // Flow } // Component - powerPageComponent diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 76fcda2bbae8b3c754c5a6f0715fbed5d25d19ee..ffa0cb7469f86806cd1eb677a77776cd321a3e2c 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -897,9 +897,9 @@ void APMFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoord vehicle->missionManager()->writeArduPilotGuidedMissionItem(coordWithAltitude, false /* altChangeOnly */); } -void APMFirmwarePlugin::guidedModeRTL(Vehicle* vehicle) +void APMFirmwarePlugin::guidedModeRTL(Vehicle* vehicle, bool smartRTL) { - _setFlightModeAndValidate(vehicle, rtlFlightMode()); + _setFlightModeAndValidate(vehicle, smartRTL ? smartRTLFlightMode() : rtlFlightMode()); } void APMFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange) diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index ddf754ed54ef60e4a2edc956c5a3e53b0a6ca31c..b7cbfbb94ad7cf08732fc4b158f62fbd4548b749 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -86,9 +86,10 @@ public: bool isGuidedMode (const Vehicle* vehicle) const override; QString gotoFlightMode (void) const override { return QStringLiteral("Guided"); } QString rtlFlightMode (void) const override { return QString("RTL"); } + QString smartRTLFlightMode (void) const override { return QString("Smart RTL"); } QString missionFlightMode (void) const override { return QString("Auto"); } void pauseVehicle (Vehicle* vehicle) override; - void guidedModeRTL (Vehicle* vehicle) override; + void guidedModeRTL (Vehicle* vehicle, bool smartRTL) override; void guidedModeChangeAltitude (Vehicle* vehicle, double altitudeChange) override; bool adjustIncomingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message) override; void adjustOutgoingMavlinkMessage (Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message) override; diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index 93d653b9ce7171bc7fbecfcd4f5106725c022fc6..dce148791d7e5bf2de149e0914b815dafd15a581 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -23,26 +23,25 @@ APMCopterMode::APMCopterMode(uint32_t mode, bool settable) : APMCustomMode(mode, settable) { QMap enumToString; - enumToString.insert(STABILIZE, "Stabilize"); - enumToString.insert(ACRO, "Acro"); - enumToString.insert(ALT_HOLD, "Altitude Hold"); - enumToString.insert(AUTO, "Auto"); - enumToString.insert(GUIDED, "Guided"); - enumToString.insert(LOITER, "Loiter"); - enumToString.insert(RTL, "RTL"); - enumToString.insert(CIRCLE, "Circle"); - enumToString.insert(LAND, "Land"); - enumToString.insert(DRIFT, "Drift"); - enumToString.insert(SPORT, "Sport"); - enumToString.insert(FLIP, "Flip"); - enumToString.insert(AUTOTUNE, "Autotune"); - enumToString.insert(POS_HOLD, "Position Hold"); - enumToString.insert(BRAKE, "Brake"); - enumToString.insert(THROW, "Throw"); - enumToString.insert(AVOID_ADSB,"Avoid ADSB"); - enumToString.insert(GUIDED_NOGPS,"Guided No GPS"); - enumToString.insert(SAFE_RTL,"Smart RTL"); - + enumToString.insert(STABILIZE, "Stabilize"); + enumToString.insert(ACRO, "Acro"); + enumToString.insert(ALT_HOLD, "Altitude Hold"); + enumToString.insert(AUTO, "Auto"); + enumToString.insert(GUIDED, "Guided"); + enumToString.insert(LOITER, "Loiter"); + enumToString.insert(RTL, "RTL"); + enumToString.insert(CIRCLE, "Circle"); + enumToString.insert(LAND, "Land"); + enumToString.insert(DRIFT, "Drift"); + enumToString.insert(SPORT, "Sport"); + enumToString.insert(FLIP, "Flip"); + enumToString.insert(AUTOTUNE, "Autotune"); + enumToString.insert(POS_HOLD, "Position Hold"); + enumToString.insert(BRAKE, "Brake"); + enumToString.insert(THROW, "Throw"); + enumToString.insert(AVOID_ADSB, "Avoid ADSB"); + enumToString.insert(GUIDED_NOGPS, "Guided No GPS"); + enumToString.insert(SAFE_RTL, "Smart RTL"); setEnumToStringMapping(enumToString); } diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h index 6bdc9f61910d1cb00d92052da1d72fc9e5f777b0..563baf7b68fd7db0c300ecfbf24c94257cd574a2 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h @@ -65,8 +65,9 @@ public: QString pauseFlightMode (void) const override { return QString("Brake"); } QString landFlightMode (void) const override { return QString("Land"); } QString takeControlFlightMode (void) const override { return QString("Loiter"); } - bool vehicleYawsToNextWaypointInMission (const Vehicle* vehicle) const final; - QString autoDisarmParameter (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral("DISARM_DELAY"); } + bool vehicleYawsToNextWaypointInMission (const Vehicle* vehicle) const override; + QString autoDisarmParameter (Vehicle* vehicle) override { Q_UNUSED(vehicle); return QStringLiteral("DISARM_DELAY"); } + bool supportsSmartRTL (void) const override { return true; } private: static bool _remapParamNameIntialized; diff --git a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h index 64d61334f5963a3fc5a062968adeb168642e30d0..c27e26410814f40b9943745614f01353ba8eaba7 100644 --- a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h @@ -49,8 +49,9 @@ public: QString pauseFlightMode (void) const override { return QString("Hold"); } void guidedModeChangeAltitude (Vehicle* vehicle, double altitudeChange) final; int remapParamNameHigestMinorVersionNumber (int majorVersionNumber) const final; - const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; } - bool supportsNegativeThrust(void) final; + const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; } + bool supportsNegativeThrust (void) final; + bool supportsSmartRTL (void) const override { return true; } private: static bool _remapParamNameIntialized; diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 0ab4af0a62ffb1b766ab73da29c4cb0bcfb8f5c7..f4cbcc688b64fb6380a36f134a737e26338b1a55 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -240,10 +240,11 @@ void FirmwarePlugin::pauseVehicle(Vehicle* vehicle) qgcApp()->showMessage(guided_mode_not_supported_by_vehicle); } -void FirmwarePlugin::guidedModeRTL(Vehicle* vehicle) +void FirmwarePlugin::guidedModeRTL(Vehicle* vehicle, bool smartRTL) { // Not supported by generic vehicle Q_UNUSED(vehicle); + Q_UNUSED(smartRTL); qgcApp()->showMessage(guided_mode_not_supported_by_vehicle); } diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index df1ebbab7427b548075339d0532cfc22deca5ff3..3fb7ca8ad40241d8c2ae1992cc234af1e7441b71 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -105,6 +105,11 @@ public: /// Returns the flight mode for RTL virtual QString rtlFlightMode(void) const { return QString(); } + /// Returns the flight mode for Smart RTL + virtual QString smartRTLFlightMode(void) const { return QString(); } + + virtual bool supportsSmartRTL(void) const { return false; } + /// Returns the flight mode for Land virtual QString landFlightMode(void) const { return QString(); } @@ -125,7 +130,7 @@ public: virtual void pauseVehicle(Vehicle* vehicle); /// Command vehicle to return to launch - virtual void guidedModeRTL(Vehicle* vehicle); + virtual void guidedModeRTL(Vehicle* vehicle, bool smartRTL); /// Command vehicle to land at current location virtual void guidedModeLand(Vehicle* vehicle); diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 5373488a22f00b9020e3fe6b5d0cf61204b81649..63b7054fff104f56ed5a8077e74866f3f9cf52f2 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -351,8 +351,9 @@ void PX4FirmwarePlugin::pauseVehicle(Vehicle* vehicle) NAN); } -void PX4FirmwarePlugin::guidedModeRTL(Vehicle* vehicle) +void PX4FirmwarePlugin::guidedModeRTL(Vehicle* vehicle, bool smartRTL) { + Q_UNUSED(smartRTL); _setFlightModeAndValidate(vehicle, _rtlFlightMode); } diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index edee88893d0ae7788c5f846f6620aacd41cbe152..3e7f794ce770d707fc138e1d2e9ab01c8b83f443 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -44,7 +44,7 @@ public: QString takeControlFlightMode (void) const override { return _manualFlightMode; } QString gotoFlightMode (void) const override { return _holdFlightMode; } void pauseVehicle (Vehicle* vehicle) override; - void guidedModeRTL (Vehicle* vehicle) override; + void guidedModeRTL (Vehicle* vehicle, bool smartRTL) override; void guidedModeLand (Vehicle* vehicle) override; void guidedModeTakeoff (Vehicle* vehicle, double takeoffAltRel) override; void guidedModeGotoLocation (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) override; diff --git a/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml b/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml index 7548962400c15ab77d18a3beb208854ef66e9e7d..48dd0f1a0cb0ed5716f50128753b66158b4c76ab 100644 --- a/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml +++ b/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml @@ -1083,7 +1083,7 @@ This parameter controls the time constant of the decay Integer bitmask controlling data fusion and aiding methods - Set bits in the following positions to enable: 0 : Set to true to use GPS data if available 1 : Set to true to use optical flow data if available 2 : Set to true to inhibit IMU bias estimation 3 : Set to true to enable vision position fusion 4 : Set to true to enable vision yaw fusion. Cannot be used if bit position 7 is true. 5 : Set to true to enable multi-rotor drag specific force fusion 6 : set to true if the EV observations are in a non NED reference frame and need to be rotated before being used 7 : Set to true to enable GPS yaw fusion. Cannot be used if bit position 4 is true. + Set bits in the following positions to enable: 0 : Set to true to use GPS data if available 1 : Set to true to use optical flow data if available 2 : Set to true to inhibit IMU delta velocity bias estimation 3 : Set to true to enable vision position fusion 4 : Set to true to enable vision yaw fusion. Cannot be used if bit position 7 is true. 5 : Set to true to enable multi-rotor drag specific force fusion 6 : set to true if the EV observations are in a non NED reference frame and need to be rotated before being used 7 : Set to true to enable GPS yaw fusion. Cannot be used if bit position 4 is true. 0 255 true @@ -4471,15 +4471,6 @@ default 1.5 turns per second 2 0.01 - - Max yaw rate in auto mode - Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation. - 0.0 - 360.0 - deg/s - 1 - 5 - Yaw P gain Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad. @@ -4496,6 +4487,15 @@ default 1.5 turns per second 2 0.01 + + Max yaw rate in auto mode + Limit the rate of change of the yaw setpoint in autonomous mode to avoid large control output and mixer saturation. + 0.0 + 360.0 + deg/s + 1 + 5 + diff --git a/src/FlightDisplay/GuidedActionConfirm.qml b/src/FlightDisplay/GuidedActionConfirm.qml index 177e1944ad82a013cb151b143173a44f30027e75..dcb74cae493e09141d792629e2a4f692a175b25d 100644 --- a/src/FlightDisplay/GuidedActionConfirm.qml +++ b/src/FlightDisplay/GuidedActionConfirm.qml @@ -35,6 +35,8 @@ Rectangle { property var actionData property bool hideTrigger: false property var mapIndicator + property alias optionText: optionCheckBox.text + property alias optionChecked: optionCheckBox.checked property real _margins: ScreenTools.defaultFontPixelWidth property bool _emergencyAction: action === guidedController.actionEmergencyStop @@ -101,6 +103,13 @@ Rectangle { wrapMode: Text.WordWrap } + QGCCheckBox { + id: optionCheckBox + anchors.horizontalCenter: parent.horizontalCenter + text: "" + visible: text !== "" + } + // Action confirmation control SliderSwitch { id: slider @@ -115,7 +124,7 @@ Rectangle { altitudeSlider.visible = false } hideTrigger = false - guidedController.executeAction(_root.action, _root.actionData, altitudeChange) + guidedController.executeAction(_root.action, _root.actionData, altitudeChange, _root.optionChecked) if (mapIndicator) { mapIndicator.actionConfirmed() mapIndicator = undefined diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml index 095a934977089bcfd2da73036ef4c3e5df1be542..e9afe1a8b88ff3b576e05ed0f5f5f14d8f16daf8 100644 --- a/src/FlightDisplay/GuidedActionsController.qml +++ b/src/FlightDisplay/GuidedActionsController.qml @@ -208,7 +208,7 @@ Item { on_FlightModeChanged: { _vehiclePaused = activeVehicle ? _flightMode === activeVehicle.pauseFlightMode : false - _vehicleInRTLMode = activeVehicle ? _flightMode === activeVehicle.rtlFlightMode : false + _vehicleInRTLMode = activeVehicle ? _flightMode === activeVehicle.rtlFlightMode || _flightMode === activeVehicle.smartRTLFlightMode : false _vehicleInLandMode = activeVehicle ? _flightMode === activeVehicle.landFlightMode : false _vehicleInMissionMode = activeVehicle ? _flightMode === activeVehicle.missionFlightMode : false // Must be last to get correct signalling for showStartMission popups } @@ -221,6 +221,7 @@ Item { confirmDialog.actionData = actionData confirmDialog.hideTrigger = true confirmDialog.mapIndicator = mapIndicator + confirmDialog.optionText = "" _actionData = actionData switch (actionCode) { case actionArm: @@ -284,6 +285,10 @@ Item { case actionRTL: confirmDialog.title = rtlTitle confirmDialog.message = rtlMessage + if (activeVehicle.supportsSmartRTL) { + confirmDialog.optionText = qsTr("Smart RTL") + confirmDialog.optionChecked = false + } confirmDialog.hideTrigger = Qt.binding(function() { return !showRTL }) break; case actionChangeAlt: @@ -344,12 +349,12 @@ Item { } // Executes the specified action - function executeAction(actionCode, actionData, actionAltitudeChange) { + function executeAction(actionCode, actionData, actionAltitudeChange, optionChecked) { var i; var rgVehicle; switch (actionCode) { case actionRTL: - activeVehicle.guidedModeRTL() + activeVehicle.guidedModeRTL(optionChecked) break case actionLand: activeVehicle.guidedModeLand() diff --git a/src/MissionManager/BreachReturn.FactMetaData.json b/src/MissionManager/BreachReturn.FactMetaData.json new file mode 100644 index 0000000000000000000000000000000000000000..fa7536ee26e79f2e7ea61e492407ff1c8a87f839 --- /dev/null +++ b/src/MissionManager/BreachReturn.FactMetaData.json @@ -0,0 +1,21 @@ +[ +{ + "name": "Latitude", + "shortDescription": "Latitude of breach return point position", + "type": "double", + "decimalPlaces": 7 +}, +{ + "name": "Longitude", + "shortDescription": "Longitude of breach return point position", + "type": "double", + "decimalPlaces": 7 +}, +{ + "name": "Altitude", + "shortDescription": "Altitude of breach return point position (Rel)", + "type": "double", + "decimalPlaces": 2, + "units": "m" +} +] diff --git a/src/MissionManager/CameraCalc.cc b/src/MissionManager/CameraCalc.cc index c775c1b76f9733b486b2469b95fa21dc6b2557a7..f937972c46408b2a4669f04178293c6095cf19d9 100644 --- a/src/MissionManager/CameraCalc.cc +++ b/src/MissionManager/CameraCalc.cc @@ -95,7 +95,7 @@ void CameraCalc::_cameraNameChanged(void) // Validate known camera name bool foundKnownCamera = false; - CameraMetaData* cameraMetaData = NULL; + CameraMetaData* cameraMetaData = nullptr; if (!isManualCamera() && !isCustomCamera()) { for (int cameraIndex=0; cameraIndex<_knownCameraList.count(); cameraIndex++) { cameraMetaData = _knownCameraList[cameraIndex].value(); @@ -201,11 +201,12 @@ void CameraCalc::save(QJsonObject& json) const } } -bool CameraCalc::load(const QJsonObject& json, QString& errorString) +bool CameraCalc::load(const QJsonObject& json, bool forPresets, bool cameraSpecInPreset, QString& errorString) { + qDebug() << "CameraCalc::load" << forPresets << cameraSpecInPreset; QJsonObject v1Json = json; - if (!v1Json.contains(JsonHelper::jsonVersionKey)) { + if (!json.contains(JsonHelper::jsonVersionKey)) { // Version 0 file. Differences from Version 1 for conversion: // JsonHelper::jsonVersionKey not stored // _jsonCameraSpecTypeKey stores CameraSpecType @@ -231,18 +232,13 @@ bool CameraCalc::load(const QJsonObject& json, QString& errorString) { adjustedFootprintSideName, QJsonValue::Double, true }, { adjustedFootprintFrontalName, QJsonValue::Double, true }, { distanceToSurfaceName, QJsonValue::Double, true }, - { distanceToSurfaceRelativeName, QJsonValue::Bool, true }, + { distanceToSurfaceRelativeName, QJsonValue::Bool, true }, }; if (!JsonHelper::validateKeys(v1Json, keyInfoList1, errorString)) { return false; } - _disableRecalc = true; - - _cameraNameFact.setRawValue (v1Json[cameraNameName].toString()); - _adjustedFootprintSideFact.setRawValue (v1Json[adjustedFootprintSideName].toDouble()); - _adjustedFootprintFrontalFact.setRawValue (v1Json[adjustedFootprintFrontalName].toDouble()); - _distanceToSurfaceFact.setRawValue (v1Json[distanceToSurfaceName].toDouble()); + _disableRecalc = !forPresets; _distanceToSurfaceRelative = v1Json[distanceToSurfaceRelativeName].toBool(); @@ -259,13 +255,40 @@ bool CameraCalc::load(const QJsonObject& json, QString& errorString) } _valueSetIsDistanceFact.setRawValue (v1Json[valueSetIsDistanceName].toBool()); - _imageDensityFact.setRawValue (v1Json[imageDensityName].toDouble()); _frontalOverlapFact.setRawValue (v1Json[frontalOverlapName].toDouble()); _sideOverlapFact.setRawValue (v1Json[sideOverlapName].toDouble()); + } - if (!CameraSpec::load(v1Json, errorString)) { - _disableRecalc = false; - return false; + if (forPresets) { + if (isManualCamera()) { + _distanceToSurfaceFact.setRawValue(v1Json[distanceToSurfaceName].toDouble()); + } else { + if (_valueSetIsDistanceFact.rawValue().toBool()) { + _distanceToSurfaceFact.setRawValue(v1Json[distanceToSurfaceName].toDouble()); + } else { + _imageDensityFact.setRawValue(v1Json[imageDensityName].toDouble()); + } + + if (cameraSpecInPreset) { + _cameraNameFact.setRawValue(v1Json[cameraNameName].toString()); + if (!CameraSpec::load(v1Json, errorString)) { + _disableRecalc = false; + return false; + } + } + } + } else { + _cameraNameFact.setRawValue (v1Json[cameraNameName].toString()); + _adjustedFootprintSideFact.setRawValue (v1Json[adjustedFootprintSideName].toDouble()); + _adjustedFootprintFrontalFact.setRawValue (v1Json[adjustedFootprintFrontalName].toDouble()); + _distanceToSurfaceFact.setRawValue (v1Json[distanceToSurfaceName].toDouble()); + if (!isManualCamera()) { + _imageDensityFact.setRawValue(v1Json[imageDensityName].toDouble()); + + if (!CameraSpec::load(v1Json, errorString)) { + _disableRecalc = false; + return false; + } } } diff --git a/src/MissionManager/CameraCalc.h b/src/MissionManager/CameraCalc.h index 3d88b5902a35b8e9275b56b7c9e30caff7a8ec76..95eeb1195c0de0d9703c9ce6816bc8c001fead7b 100644 --- a/src/MissionManager/CameraCalc.h +++ b/src/MissionManager/CameraCalc.h @@ -70,7 +70,7 @@ public: void setDistanceToSurfaceRelative (bool distanceToSurfaceRelative); void save(QJsonObject& json) const; - bool load(const QJsonObject& json, QString& errorString); + bool load(const QJsonObject& json, bool forPresets, bool cameraSpecInPreset, QString& errorString); static const char* cameraNameName; static const char* valueSetIsDistanceName; diff --git a/src/MissionManager/ComplexMissionItem.cc b/src/MissionManager/ComplexMissionItem.cc index 44af71895fa2eb43d483f72186a3baab7fb606ab..9c48b54e620b4b188e9652c01a816df2240f1a51 100644 --- a/src/MissionManager/ComplexMissionItem.cc +++ b/src/MissionManager/ComplexMissionItem.cc @@ -8,11 +8,21 @@ ****************************************************************************/ #include "ComplexMissionItem.h" +#include "QGCApplication.h" + +#include const char* ComplexMissionItem::jsonComplexItemTypeKey = "complexItemType"; +const char* ComplexMissionItem::_presetSettingsKey = "_presets"; +const char* ComplexMissionItem::_presetNameKey = "complexItemPresetName"; +const char* ComplexMissionItem::_saveCameraInPresetKey = "complexItemCameraSavedInPreset"; +const char* ComplexMissionItem::_builtInPresetKey = "complexItemBuiltInPreset"; + ComplexMissionItem::ComplexMissionItem(Vehicle* vehicle, bool flyView, QObject* parent) - : VisualMissionItem(vehicle, flyView, parent) + : VisualMissionItem (vehicle, flyView, parent) + , _cameraInPreset (true) + , _builtInPreset (false) { } @@ -21,5 +31,114 @@ const ComplexMissionItem& ComplexMissionItem::operator=(const ComplexMissionItem { VisualMissionItem::operator=(other); + _currentPreset = other._currentPreset; + _cameraInPreset = other._cameraInPreset; + _builtInPreset = other._builtInPreset; + return *this; } + +QStringList ComplexMissionItem::presetNames(void) +{ + QStringList names; + + QSettings settings; + + settings.beginGroup(presetsSettingsGroup()); + settings.beginGroup(_presetSettingsKey); + return settings.childKeys(); +} + +void ComplexMissionItem::loadPreset(const QString& name) +{ + Q_UNUSED(name); + qgcApp()->showMessage(tr("This Pattern does not support Presets.")); +} + +void ComplexMissionItem::savePreset(const QString& name) +{ + Q_UNUSED(name); + qgcApp()->showMessage(tr("This Pattern does not support Presets.")); +} + +void ComplexMissionItem::clearCurrentPreset(void) +{ + _currentPreset.clear(); + emit currentPresetChanged(_currentPreset); +} + +void ComplexMissionItem::deleteCurrentPreset(void) +{ + qDebug() << "deleteCurrentPreset" << _currentPreset; + if (!_currentPreset.isEmpty()) { + QSettings settings; + + settings.beginGroup(presetsSettingsGroup()); + settings.beginGroup(_presetSettingsKey); + settings.remove(_currentPreset); + emit presetNamesChanged(); + + clearCurrentPreset(); + } +} + +void ComplexMissionItem::_savePresetJson(const QString& name, QJsonObject& presetObject) +{ + presetObject[_presetNameKey] = name; + + QSettings settings; + settings.beginGroup(presetsSettingsGroup()); + settings.beginGroup(_presetSettingsKey); + settings.setValue(name, QJsonDocument(presetObject).toBinaryData()); + emit presetNamesChanged(); + + _currentPreset = name; + emit currentPresetChanged(name); +} + +QJsonObject ComplexMissionItem::_loadPresetJson(const QString& name) +{ + QSettings settings; + settings.beginGroup(presetsSettingsGroup()); + settings.beginGroup(_presetSettingsKey); + return QJsonDocument::fromBinaryData(settings.value(name).toByteArray()).object(); +} + +void ComplexMissionItem::_saveItem(QJsonObject& saveObject) +{ + qDebug() << "_saveItem" << _cameraInPreset; + saveObject[_presetNameKey] = _currentPreset; + saveObject[_saveCameraInPresetKey] = _cameraInPreset; + saveObject[_builtInPresetKey] = _builtInPreset; +} + +void ComplexMissionItem::_loadItem(const QJsonObject& saveObject) +{ + _currentPreset = saveObject[_presetNameKey].toString(); + _cameraInPreset = saveObject[_saveCameraInPresetKey].toBool(false); + _builtInPreset = saveObject[_builtInPresetKey].toBool(false); + + if (!presetNames().contains(_currentPreset)) { + _currentPreset.clear(); + } + + emit cameraInPresetChanged (_cameraInPreset); + emit currentPresetChanged (_currentPreset); + emit builtInPresetChanged (_builtInPreset); +} + +void ComplexMissionItem::setCameraInPreset(bool cameraInPreset) +{ + if (cameraInPreset != _cameraInPreset) { + _cameraInPreset = cameraInPreset; + emit cameraInPresetChanged(_cameraInPreset); + } +} + +void ComplexMissionItem::setBuiltInPreset(bool builtInPreset) +{ + if (builtInPreset != _builtInPreset) { + _builtInPreset = builtInPreset; + emit builtInPresetChanged(_builtInPreset); + } +} diff --git a/src/MissionManager/ComplexMissionItem.h b/src/MissionManager/ComplexMissionItem.h index 71046d1d47d2508047f61a4927b846d723c2bb8d..111ffbb7101fd33336520483d696a169e4cc05ea 100644 --- a/src/MissionManager/ComplexMissionItem.h +++ b/src/MissionManager/ComplexMissionItem.h @@ -13,6 +13,8 @@ #include "VisualMissionItem.h" #include "QGCGeo.h" +#include + class ComplexMissionItem : public VisualMissionItem { Q_OBJECT @@ -22,7 +24,12 @@ public: const ComplexMissionItem& operator=(const ComplexMissionItem& other); - Q_PROPERTY(double complexDistance READ complexDistance NOTIFY complexDistanceChanged) + Q_PROPERTY(double complexDistance READ complexDistance NOTIFY complexDistanceChanged) + Q_PROPERTY(bool presetsSupported READ presetsSupported CONSTANT) + Q_PROPERTY(QStringList presetNames READ presetNames NOTIFY presetNamesChanged) + Q_PROPERTY(QString currentPreset READ currentPreset NOTIFY currentPresetChanged) + Q_PROPERTY(bool cameraInPreset READ cameraInPreset WRITE setCameraInPreset NOTIFY cameraInPresetChanged) + Q_PROPERTY(bool builtInPreset READ builtInPreset WRITE setBuiltInPreset NOTIFY builtInPresetChanged) /// @return The distance covered the complex mission item in meters. /// Signals complexDistanceChanged @@ -35,12 +42,38 @@ public: /// @return true: load success, false: load failed, errorString set virtual bool load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString) = 0; + /// Loads the specified preset into the complex item. + /// @param name Preset name. + Q_INVOKABLE virtual void loadPreset(const QString& name); + + /// Saves the current state of the complex item as the named preset. + /// @param name User visible name for preset. Will replace existing preset if already exists. + Q_INVOKABLE virtual void savePreset(const QString& name); + + Q_INVOKABLE void clearCurrentPreset(void); + Q_INVOKABLE void deleteCurrentPreset(void); + /// Get the point of complex mission item furthest away from a coordinate /// @param other QGeoCoordinate to which distance is calculated /// @return the greatest distance from any point of the complex item to some coordinate /// Signals greatestDistanceToChanged virtual double greatestDistanceTo(const QGeoCoordinate &other) const = 0; + /// Returns the list of currently saved presets for this complex item type. + /// @param name User visible name for preset. Will replace existing preset if already exists. + virtual QStringList presetNames(void); + + /// Returns the name of the settings group for presets. + /// Empty string signals no support for presets. + virtual QString presetsSettingsGroup(void) { return QString(); } + + bool presetsSupported (void) { return !presetsSettingsGroup().isEmpty(); } + QString currentPreset (void) const { return _currentPreset; } + bool cameraInPreset (void) const { return _cameraInPreset; } + bool builtInPreset (void) const { return _builtInPreset; } + void setCameraInPreset (bool cameraInPreset); + void setBuiltInPreset (bool builtInPreset); + /// This mission item attribute specifies the type of the complex item. static const char* jsonComplexItemTypeKey; @@ -48,6 +81,29 @@ signals: void complexDistanceChanged (void); void boundingCubeChanged (void); void greatestDistanceToChanged (void); + void presetNamesChanged (void); + void currentPresetChanged (QString currentPreset); + void cameraInPresetChanged (bool cameraInPreset); + void builtInPresetChanged (bool builtInPreset); + +protected: + void _saveItem (QJsonObject& saveObject); + void _loadItem (const QJsonObject& saveObject); + void _savePresetJson (const QString& name, QJsonObject& presetObject); + QJsonObject _loadPresetJson (const QString& name); + + + QMap _metaDataMap; + + QString _currentPreset; + SettingsFact _saveCameraInPresetFact; + bool _cameraInPreset; + bool _builtInPreset; + + static const char* _presetSettingsKey; + static const char* _presetNameKey; + static const char* _saveCameraInPresetKey; + static const char* _builtInPresetKey; }; #endif diff --git a/src/MissionManager/CorridorScanComplexItem.cc b/src/MissionManager/CorridorScanComplexItem.cc index 2d165d16e1af03a5a6447055dd9f9412a8e3b325..f253f8d8e74efc46c3bc127edb25e3435e39202c 100644 --- a/src/MissionManager/CorridorScanComplexItem.cc +++ b/src/MissionManager/CorridorScanComplexItem.cc @@ -62,7 +62,7 @@ void CorridorScanComplexItem::save(QJsonArray& planItems) { QJsonObject saveObject; - _save(saveObject); + TransectStyleComplexItem::_save(saveObject); saveObject[JsonHelper::jsonVersionKey] = 2; saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; @@ -115,7 +115,7 @@ bool CorridorScanComplexItem::load(const QJsonObject& complexObject, int sequenc setSequenceNumber(sequenceNumber); - if (!_load(complexObject, errorString)) { + if (!_load(complexObject, false /* forPresets */, errorString)) { _ignoreRecalc = false; return false; } diff --git a/src/MissionManager/GeoFenceController.cc b/src/MissionManager/GeoFenceController.cc index 9e9e23e6f9be45136e6445d40bff926b12d52850..64e89a5ccffa70b2bbafc4295056e2873a8faf2b 100644 --- a/src/MissionManager/GeoFenceController.cc +++ b/src/MissionManager/GeoFenceController.cc @@ -21,30 +21,51 @@ #include "QGCQGeoCoordinate.h" #include "AppSettings.h" #include "PlanMasterController.h" +#include "SettingsManager.h" +#include "AppSettings.h" #include #include QGC_LOGGING_CATEGORY(GeoFenceControllerLog, "GeoFenceControllerLog") +QMap GeoFenceController::_metaDataMap; + const char* GeoFenceController::_jsonFileTypeValue = "GeoFence"; const char* GeoFenceController::_jsonBreachReturnKey = "breachReturn"; const char* GeoFenceController::_jsonPolygonsKey = "polygons"; const char* GeoFenceController::_jsonCirclesKey = "circles"; +const char* GeoFenceController::_breachReturnAltitudeFactName = "Altitude"; + const char* GeoFenceController::_px4ParamCircularFence = "GF_MAX_HOR_DIST"; GeoFenceController::GeoFenceController(PlanMasterController* masterController, QObject* parent) - : PlanElementController (masterController, parent) - , _geoFenceManager (_managerVehicle->geoFenceManager()) - , _dirty (false) + : PlanElementController (masterController, parent) + , _geoFenceManager (_managerVehicle->geoFenceManager()) + , _dirty (false) + , _breachReturnAltitudeFact (0, _breachReturnAltitudeFactName, FactMetaData::valueTypeDouble) + , _breachReturnDefaultAltitude (qgcApp()->toolbox()->settingsManager()->appSettings()->defaultMissionItemAltitude()->rawValue().toDouble()) + , _itemsRequested (false) - , _px4ParamCircularFenceFact(NULL) + , _px4ParamCircularFenceFact(nullptr) { + if (_metaDataMap.isEmpty()) { + _metaDataMap = FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/BreachReturn.FactMetaData.json"), nullptr /* metaDataParent */); + } + + _breachReturnAltitudeFact.setMetaData(_metaDataMap[_breachReturnAltitudeFactName]); + _breachReturnAltitudeFact.setRawValue(_breachReturnDefaultAltitude); + connect(&_polygons, &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems); connect(&_circles, &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems); managerVehicleChanged(_managerVehicle); + + connect(this, &GeoFenceController::breachReturnPointChanged, this, &GeoFenceController::_setDirty); + connect(&_breachReturnAltitudeFact, &Fact::rawValueChanged, this, &GeoFenceController::_setDirty); + connect(&_polygons, &QmlObjectListModel::dirtyChanged, this, &GeoFenceController::_setDirty); + connect(&_circles, &QmlObjectListModel::dirtyChanged, this, &GeoFenceController::_setDirty); } GeoFenceController::~GeoFenceController() @@ -74,26 +95,19 @@ void GeoFenceController::setBreachReturnPoint(const QGeoCoordinate& breachReturn } } -void GeoFenceController::_signalAll(void) -{ - emit breachReturnPointChanged(breachReturnPoint()); - emit dirtyChanged(dirty()); - emit supportedChanged(supported()); -} - void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle) { if (_managerVehicle) { _geoFenceManager->disconnect(this); _managerVehicle->disconnect(this); _managerVehicle->parameterManager()->disconnect(this); - _managerVehicle = NULL; - _geoFenceManager = NULL; + _managerVehicle = nullptr; + _geoFenceManager = nullptr; } _managerVehicle = managerVehicle; if (!_managerVehicle) { - qWarning() << "GeoFenceController::managerVehicleChanged managerVehicle=NULL"; + qWarning() << "GeoFenceController::managerVehicleChanged managerVehicle=nullptr"; return; } @@ -109,7 +123,6 @@ void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle) _parametersReady(); emit supportedChanged(supported()); - _signalAll(); } bool GeoFenceController::load(const QJsonObject& json, QString& errorString) @@ -128,6 +141,7 @@ bool GeoFenceController::load(const QJsonObject& json, QString& errorString) { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, { _jsonCirclesKey, QJsonValue::Array, true }, { _jsonPolygonsKey, QJsonValue::Array, true }, + { _jsonBreachReturnKey, QJsonValue::Array, false }, }; if (!JsonHelper::validateKeys(json, keyInfoList, errorString)) { return false; @@ -139,7 +153,7 @@ bool GeoFenceController::load(const QJsonObject& json, QString& errorString) } QJsonArray jsonPolygonArray = json[_jsonPolygonsKey].toArray(); - for (const QJsonValue& jsonPolygonValue: jsonPolygonArray) { + for (const QJsonValue jsonPolygonValue: jsonPolygonArray) { if (jsonPolygonValue.type() != QJsonValue::Object) { errorString = tr("GeoFence polygon not stored as object"); return false; @@ -153,7 +167,7 @@ bool GeoFenceController::load(const QJsonObject& json, QString& errorString) } QJsonArray jsonCircleArray = json[_jsonCirclesKey].toArray(); - for (const QJsonValue& jsonCircleValue: jsonCircleArray) { + for (const QJsonValue jsonCircleValue: jsonCircleArray) { if (jsonCircleValue.type() != QJsonValue::Object) { errorString = tr("GeoFence circle not stored as object"); return false; @@ -166,8 +180,18 @@ bool GeoFenceController::load(const QJsonObject& json, QString& errorString) _circles.append(fenceCircle); } + if (json.contains(_jsonBreachReturnKey)) { + if (!JsonHelper::loadGeoCoordinate(json[_jsonBreachReturnKey], true /* altitudeRequred */, _breachReturnPoint, errorString)) { + return false; + } + _breachReturnAltitudeFact.setRawValue(_breachReturnPoint.altitude()); + } else { + _breachReturnPoint = QGeoCoordinate(); + _breachReturnAltitudeFact.setRawValue(_breachReturnDefaultAltitude); + } + emit breachReturnPointChanged(_breachReturnPoint); + setDirty(false); - _signalAll(); return true; } @@ -193,6 +217,14 @@ void GeoFenceController::save(QJsonObject& json) jsonCircleArray.append(jsonCircle); } json[_jsonCirclesKey] = jsonCircleArray; + + if (_breachReturnPoint.isValid()) { + QJsonValue jsonCoordinate; + + _breachReturnPoint.setAltitude(_breachReturnAltitudeFact.rawValue().toDouble()); + JsonHelper::saveGeoCoordinate(_breachReturnPoint, true /* writeAltitude */, jsonCoordinate); + json[_jsonBreachReturnKey] = jsonCoordinate; + } } void GeoFenceController::removeAll(void) @@ -300,6 +332,11 @@ void GeoFenceController::_setReturnPointFromManager(QGeoCoordinate breachReturnP { _breachReturnPoint = breachReturnPoint; emit breachReturnPointChanged(_breachReturnPoint); + if (_breachReturnPoint.isValid()) { + _breachReturnAltitudeFact.setRawValue(_breachReturnPoint.altitude()); + } else { + _breachReturnAltitudeFact.setRawValue(_breachReturnDefaultAltitude); + } } void GeoFenceController::_managerLoadComplete(void) @@ -310,7 +347,6 @@ void GeoFenceController::_managerLoadComplete(void) _setReturnPointFromManager(_geoFenceManager->breachReturnPoint()); _setFenceFromManager(_geoFenceManager->polygons(), _geoFenceManager->circles()); setDirty(false); - _signalAll(); emit loadComplete(); } _itemsRequested = false; @@ -471,7 +507,7 @@ void GeoFenceController::_parametersReady(void) { if (_px4ParamCircularFenceFact) { _px4ParamCircularFenceFact->disconnect(this); - _px4ParamCircularFenceFact = NULL; + _px4ParamCircularFenceFact = nullptr; } if (_managerVehicle->isOfflineEditingVehicle() || !_managerVehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _px4ParamCircularFence)) { diff --git a/src/MissionManager/GeoFenceController.h b/src/MissionManager/GeoFenceController.h index 9412fc00530c0ca81c0408bf10af327ec5e59502..65c098d6916272fd8639ee41c7d669987b0f25d6 100644 --- a/src/MissionManager/GeoFenceController.h +++ b/src/MissionManager/GeoFenceController.h @@ -27,12 +27,13 @@ class GeoFenceController : public PlanElementController Q_OBJECT public: - GeoFenceController(PlanMasterController* masterController, QObject* parent = NULL); + GeoFenceController(PlanMasterController* masterController, QObject* parent = nullptr); ~GeoFenceController(); - Q_PROPERTY(QmlObjectListModel* polygons READ polygons CONSTANT) - Q_PROPERTY(QmlObjectListModel* circles READ circles CONSTANT) - Q_PROPERTY(QGeoCoordinate breachReturnPoint READ breachReturnPoint WRITE setBreachReturnPoint NOTIFY breachReturnPointChanged) + Q_PROPERTY(QmlObjectListModel* polygons READ polygons CONSTANT) + Q_PROPERTY(QmlObjectListModel* circles READ circles CONSTANT) + Q_PROPERTY(QGeoCoordinate breachReturnPoint READ breachReturnPoint WRITE setBreachReturnPoint NOTIFY breachReturnPointChanged) + Q_PROPERTY(Fact* breachReturnAltitude READ breachReturnAltitude CONSTANT) // Hack to expose PX4 circular fence controlled by GF_MAX_HOR_DIST Q_PROPERTY(double paramCircularFence READ paramCircularFence NOTIFY paramCircularFenceChanged) @@ -58,7 +59,8 @@ public: /// Clears the interactive bit from all fence items Q_INVOKABLE void clearAllInteractive(void); - double paramCircularFence(void); + double paramCircularFence (void); + Fact* breachReturnAltitude(void) { return &_breachReturnAltitudeFact; } // Overrides from PlanElementController bool supported (void) const final; @@ -101,16 +103,19 @@ private slots: private: void _init(void); - void _signalAll(void); GeoFenceManager* _geoFenceManager; bool _dirty; QmlObjectListModel _polygons; QmlObjectListModel _circles; QGeoCoordinate _breachReturnPoint; + Fact _breachReturnAltitudeFact; + double _breachReturnDefaultAltitude; bool _itemsRequested; Fact* _px4ParamCircularFenceFact; + static QMap _metaDataMap; + static const char* _px4ParamCircularFence; static const int _jsonCurrentVersion = 2; @@ -119,6 +124,8 @@ private: static const char* _jsonBreachReturnKey; static const char* _jsonPolygonsKey; static const char* _jsonCirclesKey; + + static const char* _breachReturnAltitudeFactName; }; #endif diff --git a/src/MissionManager/GeoFenceManager.cc b/src/MissionManager/GeoFenceManager.cc index 05ce7ae5473d02ff67cdcfedb656b3d339da8960..7702e7ad717af053e72747af6b9052651593c085 100644 --- a/src/MissionManager/GeoFenceManager.cc +++ b/src/MissionManager/GeoFenceManager.cc @@ -51,8 +51,6 @@ void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, QmlObjectListModel& polygons, QmlObjectListModel& circles) { - Q_UNUSED(breachReturn); - QList fenceItems; _sendPolygons.clear(); @@ -64,6 +62,7 @@ void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, for (int i=0; i(i)); } + _breachReturnPoint = breachReturn; for (int i=0; i<_sendPolygons.count(); i++) { const QGCFencePolygon& polygon = _sendPolygons[i]; @@ -103,12 +102,30 @@ void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, fenceItems.append(item); } + if (_breachReturnPoint.isValid()) { + MissionItem* item = new MissionItem(0, + MAV_CMD_NAV_FENCE_RETURN_POINT, + MAV_FRAME_GLOBAL_RELATIVE_ALT, + 0, 0, 0, 0, // param 1-4 unused + breachReturn.latitude(), + breachReturn.longitude(), + breachReturn.altitude(), + false, // autocontinue + false, // isCurrentItem + this); // parent + fenceItems.append(item); + } + // Plan manager takes control of MissionItems, so no need to delete _planManager.writeMissionItems(fenceItems); } void GeoFenceManager::removeAll(void) { + _polygons.clear(); + _circles.clear(); + _breachReturnPoint = QGeoCoordinate(); + _planManager.removeAll(); } @@ -117,6 +134,7 @@ void GeoFenceManager::_sendComplete(bool error) if (error) { _polygons.clear(); _circles.clear(); + _breachReturnPoint = QGeoCoordinate(); } else { _polygons = _sendPolygons; _circles = _sendCircles; @@ -174,6 +192,8 @@ void GeoFenceManager::_planManagerLoadComplete(bool removeAllRequested) } QGCFenceCircle circle(QGeoCoordinate(item->param5(), item->param6()), item->param1(), command == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION /* inclusion */); _circles.append(circle); + } else if (command == MAV_CMD_NAV_FENCE_RETURN_POINT) { + _breachReturnPoint = QGeoCoordinate(item->param5(), item->param6(), item->param7()); } else { emit error(UnsupportedCommand, tr("GeoFence load: Unsupported command %1").arg(item->command())); break; @@ -183,6 +203,7 @@ void GeoFenceManager::_planManagerLoadComplete(bool removeAllRequested) if (loadFailed) { _polygons.clear(); _circles.clear(); + _breachReturnPoint = QGeoCoordinate(); } emit loadComplete(); diff --git a/src/MissionManager/RallyPointManager.cc b/src/MissionManager/RallyPointManager.cc index 0db0f7342c1a3633c315fa7c3c7dac869765b281..6230cff8c7d29a7b32a83611220c5430f78fb2e6 100644 --- a/src/MissionManager/RallyPointManager.cc +++ b/src/MissionManager/RallyPointManager.cc @@ -72,6 +72,7 @@ void RallyPointManager::sendToVehicle(const QList& rgPoints) void RallyPointManager::removeAll(void) { + _rgPoints.clear(); _planManager.removeAll(); } diff --git a/src/MissionManager/StructureScanComplexItem.cc b/src/MissionManager/StructureScanComplexItem.cc index 8eae2d8ff6b988b0a9e198c0188f24b99a43049f..8f4eb958345e94f431e47afbd9772dacdb270a6b 100644 --- a/src/MissionManager/StructureScanComplexItem.cc +++ b/src/MissionManager/StructureScanComplexItem.cc @@ -83,6 +83,9 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, bool flyVie connect(&_cameraCalc, &CameraCalc::isManualCameraChanged, this, &StructureScanComplexItem::_updateGimbalPitch); + connect(&_layersFact, &Fact::valueChanged, this, &StructureScanComplexItem::_recalcScanDistance); + connect(&_flightPolygon, &QGCMapPolygon::pathChanged, this, &StructureScanComplexItem::_recalcScanDistance); + _recalcLayerInfo(); if (!kmlOrShpFile.isEmpty()) { @@ -93,14 +96,6 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, bool flyVie setDirty(false); } -void StructureScanComplexItem::_setScanDistance(double scanDistance) -{ - if (!qFuzzyCompare(_scanDistance, scanDistance)) { - _scanDistance = scanDistance; - emit complexDistanceChanged(); - } -} - void StructureScanComplexItem::_setCameraShots(int cameraShots) { if (_cameraShots != cameraShots) { @@ -218,7 +213,7 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen setSequenceNumber(sequenceNumber); // Load CameraCalc first since it will trigger camera name change which will trounce gimbal angles - if (!_cameraCalc.load(complexObject[_jsonCameraCalcKey].toObject(), errorString)) { + if (!_cameraCalc.load(complexObject[_jsonCameraCalcKey].toObject(), false /* forPresets */, false /* cameraSpecInPreset */, errorString)) { return false; } @@ -519,6 +514,7 @@ void StructureScanComplexItem::_rebuildFlightPolygon(void) } else { _entryVertex = savedEntryVertex; } + emit coordinateChanged(coordinate()); emit exitCoordinateChanged(exitCoordinate()); } @@ -598,3 +594,26 @@ void StructureScanComplexItem::_signalTopBottomAltChanged(void) emit topFlightAltChanged(); emit bottomFlightAltChanged(); } + +void StructureScanComplexItem::_recalcScanDistance() +{ + double scanDistance = 0; + QList vertices = _flightPolygon.coordinateList(); + for (int i=0; i keyInfoList = { { VisualMissionItem::jsonTypeKey, QJsonValue::String, true }, @@ -197,16 +218,18 @@ bool SurveyComplexItem::_loadV4V5(const QJsonObject& complexObject, int sequence return false; } - _ignoreRecalc = true; + _ignoreRecalc = !forPresets; - setSequenceNumber(sequenceNumber); + if (!forPresets) { + setSequenceNumber(sequenceNumber); - if (!_surveyAreaPolygon.loadFromJson(complexObject, true /* required */, errorString)) { - _surveyAreaPolygon.clear(); - return false; + if (!_surveyAreaPolygon.loadFromJson(complexObject, true /* required */, errorString)) { + _surveyAreaPolygon.clear(); + return false; + } } - if (!_load(complexObject, errorString)) { + if (!TransectStyleComplexItem::_load(complexObject, forPresets, errorString)) { _ignoreRecalc = false; return false; } @@ -214,7 +237,7 @@ bool SurveyComplexItem::_loadV4V5(const QJsonObject& complexObject, int sequence _gridAngleFact.setRawValue (complexObject[_jsonGridAngleKey].toDouble()); _flyAlternateTransectsFact.setRawValue (complexObject[_jsonFlyAlternateTransectsKey].toBool(false)); - if(version == 5) { + if (version == 5) { _splitConcavePolygonsFact.setRawValue (complexObject[_jsonSplitConcavePolygonsKey].toBool(true)); } @@ -282,7 +305,7 @@ bool SurveyComplexItem::_loadV3(const QJsonObject& complexObject, int sequenceNu _turnAroundDistanceFact.setRawValue (gridObject[_jsonV3TurnaroundDistKey].toDouble()); if (gridObject.contains(_jsonEntryPointKey)) { - _entryPoint = gridObject[_jsonEntryPointKey].toDouble(); + _entryPoint = gridObject[_jsonEntryPointKey].toInt(); } else { _entryPoint = EntryLocationTopRight; } diff --git a/src/MissionManager/SurveyComplexItem.h b/src/MissionManager/SurveyComplexItem.h index eda8ef30b5956fa63592b1f51710616bbcb70fa2..6d1348fea700af9909eb678e908daaa1b3e81fc1 100644 --- a/src/MissionManager/SurveyComplexItem.h +++ b/src/MissionManager/SurveyComplexItem.h @@ -39,6 +39,9 @@ public: // Overrides from ComplexMissionItem bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final; QString mapVisualQML (void) const final { return QStringLiteral("SurveyMapVisual.qml"); } + QString presetsSettingsGroup(void) { return settingsGroup; } + void savePreset (const QString& name); + void loadPreset (const QString& name); // Overrides from TransectStyleComplexItem void save (QJsonArray& planItems) final; @@ -114,7 +117,8 @@ private: double _turnaroundDistance(void) const; bool _hoverAndCaptureEnabled(void) const; bool _loadV3(const QJsonObject& complexObject, int sequenceNumber, QString& errorString); - bool _loadV4V5(const QJsonObject& complexObject, int sequenceNumber, QString& errorString, int version); + bool _loadV4V5(const QJsonObject& complexObject, int sequenceNumber, QString& errorString, int version, bool forPresets); + void _saveWorker(QJsonObject& complexObject); void _rebuildTransectsPhase1Worker(bool refly); void _rebuildTransectsPhase1WorkerSinglePolygon(bool refly); void _rebuildTransectsPhase1WorkerSplitPolygons(bool refly); diff --git a/src/MissionManager/TransectStyleComplexItem.cc b/src/MissionManager/TransectStyleComplexItem.cc index 156818403f047e31ea17807fb6b567ca7f8877f7..3941016ba9888a25b09b7605d90c356228b38e34 100644 --- a/src/MissionManager/TransectStyleComplexItem.cc +++ b/src/MissionManager/TransectStyleComplexItem.cc @@ -132,6 +132,8 @@ void TransectStyleComplexItem::setDirty(bool dirty) void TransectStyleComplexItem::_save(QJsonObject& complexObject) { + ComplexMissionItem::_saveItem(complexObject); + QJsonObject innerObject; innerObject[JsonHelper::jsonVersionKey] = 1; @@ -183,8 +185,9 @@ void TransectStyleComplexItem::setSequenceNumber(int sequenceNumber) } } -bool TransectStyleComplexItem::_load(const QJsonObject& complexObject, QString& errorString) +bool TransectStyleComplexItem::_load(const QJsonObject& complexObject, bool forPresets, QString& errorString) { + ComplexMissionItem::_loadItem(complexObject); QList keyInfoList = { { _jsonTransectStyleComplexItemKey, QJsonValue::Object, true }, @@ -211,8 +214,8 @@ bool TransectStyleComplexItem::_load(const QJsonObject& complexObject, QString& { hoverAndCaptureName, QJsonValue::Bool, true }, { refly90DegreesName, QJsonValue::Bool, true }, { _jsonCameraCalcKey, QJsonValue::Object, true }, - { _jsonVisualTransectPointsKey, QJsonValue::Array, true }, - { _jsonItemsKey, QJsonValue::Array, true }, + { _jsonVisualTransectPointsKey, QJsonValue::Array, !forPresets }, + { _jsonItemsKey, QJsonValue::Array, !forPresets }, { _jsonFollowTerrainKey, QJsonValue::Bool, true }, { _jsonCameraShotsKey, QJsonValue::Double, false }, // Not required since it was missing from initial implementation }; @@ -220,28 +223,30 @@ bool TransectStyleComplexItem::_load(const QJsonObject& complexObject, QString& return false; } - // Load visual transect points - if (!JsonHelper::loadGeoCoordinateArray(innerObject[_jsonVisualTransectPointsKey], false /* altitudeRequired */, _visualTransectPoints, errorString)) { - return false; - } - _coordinate = _visualTransectPoints.count() ? _visualTransectPoints.first().value() : QGeoCoordinate(); - _exitCoordinate = _visualTransectPoints.count() ? _visualTransectPoints.last().value() : QGeoCoordinate(); - - // Load generated mission items - _loadedMissionItemsParent = new QObject(this); - QJsonArray missionItemsJsonArray = innerObject[_jsonItemsKey].toArray(); - for (const QJsonValue& missionItemJson: missionItemsJsonArray) { - MissionItem* missionItem = new MissionItem(_loadedMissionItemsParent); - if (!missionItem->load(missionItemJson.toObject(), 0 /* sequenceNumber */, errorString)) { - _loadedMissionItemsParent->deleteLater(); - _loadedMissionItemsParent = NULL; + if (!forPresets) { + // Load visual transect points + if (!JsonHelper::loadGeoCoordinateArray(innerObject[_jsonVisualTransectPointsKey], false /* altitudeRequired */, _visualTransectPoints, errorString)) { return false; } - _loadedMissionItems.append(missionItem); + _coordinate = _visualTransectPoints.count() ? _visualTransectPoints.first().value() : QGeoCoordinate(); + _exitCoordinate = _visualTransectPoints.count() ? _visualTransectPoints.last().value() : QGeoCoordinate(); + + // Load generated mission items + _loadedMissionItemsParent = new QObject(this); + QJsonArray missionItemsJsonArray = innerObject[_jsonItemsKey].toArray(); + for (const QJsonValue missionItemJson: missionItemsJsonArray) { + MissionItem* missionItem = new MissionItem(_loadedMissionItemsParent); + if (!missionItem->load(missionItemJson.toObject(), 0 /* sequenceNumber */, errorString)) { + _loadedMissionItemsParent->deleteLater(); + _loadedMissionItemsParent = nullptr; + return false; + } + _loadedMissionItems.append(missionItem); + } } // Load CameraCalc data - if (!_cameraCalc.load(innerObject[_jsonCameraCalcKey].toObject(), errorString)) { + if (!_cameraCalc.load(innerObject[_jsonCameraCalcKey].toObject(), forPresets, cameraInPreset(), errorString)) { return false; } @@ -435,7 +440,7 @@ void TransectStyleComplexItem::_reallyQueryTransectsPathHeightInfo(void) // Let the signal fall on the floor disconnect(_terrainPolyPathQuery, &TerrainPolyPathQuery::terrainDataReceived, this, &TransectStyleComplexItem::_polyPathTerrainData); #endif - _terrainPolyPathQuery = NULL; + _terrainPolyPathQuery = nullptr; } // Append all transects into a single PolyPath query @@ -479,7 +484,7 @@ void TransectStyleComplexItem::_polyPathTerrainData(bool success, const QList 0 + Layout.fillWidth: true + columns: 3 + flow: GridLayout.TopToBottom + visible: polygonSection.checked && myGeoFenceController.polygons.count > 0 QGCLabel { text: qsTr("Inclusion") @@ -224,11 +222,11 @@ QGCFlickable { } GridLayout { - anchors.left: parent.left - anchors.right: parent.right - columns: 4 - flow: GridLayout.TopToBottom - visible: polygonSection.checked && myGeoFenceController.circles.count > 0 + anchors.left: parent.left + anchors.right: parent.right + columns: 4 + flow: GridLayout.TopToBottom + visible: polygonSection.checked && myGeoFenceController.circles.count > 0 QGCLabel { text: qsTr("Inclusion") @@ -302,6 +300,46 @@ QGCFlickable { } } } // GridLayout + + SectionHeader { + id: breachReturnSection + text: qsTr("Breach Return Point") + } + + QGCButton { + text: qsTr("Add Breach Return Point") + visible: breachReturnSection.visible && !myGeoFenceController.breachReturnPoint.isValid + anchors.left: parent.left + anchors.right: parent.right + + onClicked: myGeoFenceController.breachReturnPoint = flightMap.center + } + + QGCButton { + text: qsTr("Remove Breach Return Point") + visible: breachReturnSection.visible && myGeoFenceController.breachReturnPoint.isValid + anchors.left: parent.left + anchors.right: parent.right + + onClicked: myGeoFenceController.breachReturnPoint = QtPositioning.coordinate() + } + + ColumnLayout { + anchors.left: parent.left + anchors.right: parent.right + spacing: _margin + visible: breachReturnSection.visible && myGeoFenceController.breachReturnPoint.isValid + + QGCLabel { + text: qsTr("Altitude") + } + + AltitudeFactTextField { + fact: myGeoFenceController.breachReturnAltitude + altitudeMode: QGroundControl.AltitudeModeRelative + } + } + } } } diff --git a/src/PlanView/GeoFenceMapVisuals.qml b/src/PlanView/GeoFenceMapVisuals.qml index 371a64a4ca546f00aa288da2e7d77c367918e4cc..18bc19b37bacd8639d7cf676090e55fcd929bf70 100644 --- a/src/PlanView/GeoFenceMapVisuals.qml +++ b/src/PlanView/GeoFenceMapVisuals.qml @@ -28,8 +28,8 @@ Item { property bool planView: false ///< true: visuals showing in plan view property var homePosition - //property var _breachReturnPointComponent - //property var _mouseAreaComponent + property var _breachReturnPointComponent + property var _breachReturnDragComponent property var _paramCircleFenceComponent property var _polygons: myGeoFenceController.polygons property var _circles: myGeoFenceController.circles @@ -75,30 +75,19 @@ Item { } Component.onCompleted: { - //_breachReturnPointComponent = breachReturnPointComponent.createObject(map) - //map.addMapItem(_breachReturnPointComponent) - //_mouseAreaComponent = mouseAreaComponent.createObject(map) + _breachReturnPointComponent = breachReturnPointComponent.createObject(map) + map.addMapItem(_breachReturnPointComponent) + _breachReturnDragComponent = breachReturnDragComponent.createObject(map, { "itemIndicator": _breachReturnPointComponent }) _paramCircleFenceComponent = paramCircleFenceComponent.createObject(map) map.addMapItem(_paramCircleFenceComponent) } Component.onDestruction: { - //_breachReturnPointComponent.destroy() - //_mouseAreaComponent.destroy() + _breachReturnPointComponent.destroy() + _breachReturnDragComponent.destroy() _paramCircleFenceComponent.destroy() } - // Mouse area to capture breach return point coordinate - Component { - id: mouseAreaComponent - - MouseArea { - anchors.fill: map - visible: interactive - onClicked: myGeoFenceController.breachReturnPoint = map.toCoordinate(Qt.point(mouse.x, mouse.y), false /* clipToViewPort */) - } - } - Instantiator { model: _polygons @@ -144,6 +133,19 @@ Item { } } + Component { + id: breachReturnDragComponent + + MissionItemIndicatorDrag { + mapControl: map + itemCoordinate: myGeoFenceController.breachReturnPoint + //visible: itemCoordinate.isValid + + onItemCoordinateChanged: myGeoFenceController.breachReturnPoint = itemCoordinate + } + } + + // Breach return point Component { id: breachReturnPointComponent @@ -155,7 +157,8 @@ Item { coordinate: myGeoFenceController.breachReturnPoint sourceItem: MissionItemIndexLabel { - label: "B" + label: qsTr("B", "Breach Return Point item indicator") + checked: true } } } diff --git a/src/PlanView/PlanView.qml b/src/PlanView/PlanView.qml index 068cbb71f2aa7d0796e1be555bfb0c6504bde1ee..0130561bb7fe37c9d9373329869ad0428aa24e10 100644 --- a/src/PlanView/PlanView.qml +++ b/src/PlanView/PlanView.qml @@ -43,7 +43,7 @@ Item { readonly property var _defaultVehicleCoordinate: QtPositioning.coordinate(37.803784, -122.462276) readonly property bool _waypointsOnlyMode: QGroundControl.corePlugin.options.missionWaypointsOnly - property bool _airspaceEnabled: QGroundControl.airmapSupported ? (QGroundControl.settingsManager.airMapSettings.enableAirMap.rawValue && QGroundControl.airspaceManager.connected): false + property bool _airspaceEnabled: QGroundControl.airmapSupported ? (QGroundControl.settingsManager.airMapSettings.enableAirMap.rawValue && QGroundControl.airspaceManager.connected): false property var _missionController: _planMasterController.missionController property var _geoFenceController: _planMasterController.geoFenceController property var _rallyPointController: _planMasterController.rallyPointController @@ -534,91 +534,91 @@ Item { //----------------------------------------------------------- // Left tool strip - ToolStrip { - id: toolStrip + ToolStrip { + id: toolStrip anchors.leftMargin: ScreenTools.defaultFontPixelWidth * 2 - anchors.left: parent.left + anchors.left: parent.left anchors.topMargin: _toolButtonTopMargin + anchors.leftMargin - anchors.top: parent.top - z: QGroundControl.zOrderWidgets + anchors.top: parent.top + z: QGroundControl.zOrderWidgets showAlternateIcon: [ false, _planMasterController.dirty, false, false, false, false, false, false ] rotateImage: [ false, _planMasterController.syncInProgress, false, false, false, false, false, false ] animateImage: [ false, _planMasterController.dirty, false, false, false, false, false, false ] buttonEnabled: [ true, !_planMasterController.syncInProgress, true, true, true, true, true, true ] buttonVisible: [ true, true, true, _waypointsOnlyMode, true, true, _showZoom, _showZoom ] - maxHeight: mapScale.y - toolStrip.y + maxHeight: mapScale.y - toolStrip.y - property bool _showZoom: !ScreenTools.isMobile + property bool _showZoom: !ScreenTools.isMobile - model: [ - { + model: [ + { name: qsTr("Fly"), iconSource: "/qmlimages/PaperPlane.svg", }, { - name: qsTr("File"), - iconSource: "/qmlimages/MapSync.svg", - alternateIconSource: "/qmlimages/MapSyncChanged.svg", - dropPanelComponent: syncDropPanel - }, - { - name: qsTr("Waypoint"), - iconSource: "/qmlimages/MapAddMission.svg", - toggle: true - }, - { - name: qsTr("ROI"), - iconSource: "/qmlimages/MapAddMission.svg", - toggle: true - }, - { - name: _singleComplexItem ? _missionController.complexMissionItemNames[0] : qsTr("Pattern"), - iconSource: "/qmlimages/MapDrawShape.svg", - dropPanelComponent: _singleComplexItem ? undefined : patternDropPanel - }, - { - name: qsTr("Center"), - iconSource: "/qmlimages/MapCenter.svg", - dropPanelComponent: centerMapDropPanel - }, - { - name: qsTr("In"), - iconSource: "/qmlimages/ZoomPlus.svg" - }, - { - name: qsTr("Out"), - iconSource: "/qmlimages/ZoomMinus.svg" - } - ] + name: qsTr("File"), + iconSource: "/qmlimages/MapSync.svg", + alternateIconSource: "/qmlimages/MapSyncChanged.svg", + dropPanelComponent: syncDropPanel + }, + { + name: qsTr("Waypoint"), + iconSource: "/qmlimages/MapAddMission.svg", + toggle: true + }, + { + name: qsTr("ROI"), + iconSource: "/qmlimages/MapAddMission.svg", + toggle: true + }, + { + name: _singleComplexItem ? _missionController.complexMissionItemNames[0] : qsTr("Pattern"), + iconSource: "/qmlimages/MapDrawShape.svg", + dropPanelComponent: _singleComplexItem ? undefined : patternDropPanel + }, + { + name: qsTr("Center"), + iconSource: "/qmlimages/MapCenter.svg", + dropPanelComponent: centerMapDropPanel + }, + { + name: qsTr("In"), + iconSource: "/qmlimages/ZoomPlus.svg" + }, + { + name: qsTr("Out"), + iconSource: "/qmlimages/ZoomMinus.svg" + } + ] - onClicked: { - switch (index) { + onClicked: { + switch (index) { case 0: mainWindow.showFlyView() break; case 2: - _addWaypointOnClick = checked - _addROIOnClick = false - break + _addWaypointOnClick = checked + _addROIOnClick = false + break case 3: - _addROIOnClick = checked - _addWaypointOnClick = false - break + _addROIOnClick = checked + _addWaypointOnClick = false + break case 4: - if (_singleComplexItem) { - addComplexItem(_missionController.complexMissionItemNames[0]) - } - break - case 6: + if (_singleComplexItem) { + addComplexItem(_missionController.complexMissionItemNames[0]) + } + break + case 6: editorMap.zoomLevel += 0.5 break case 7: - editorMap.zoomLevel -= 0.5 - break + editorMap.zoomLevel -= 0.5 + break + } } } - } //----------------------------------------------------------- // Right pane for mission editing controls @@ -800,7 +800,7 @@ Item { //-- List Elements delegate: MissionItemEditor { map: editorMap - masterController: _planMasterController + masterController: _planMasterController missionItem: object width: parent.width readOnly: false @@ -1007,7 +1007,7 @@ Item { QGCButton { text: qsTr("New...") Layout.fillWidth: true - enabled: _visualItems.count > 1 + enabled: _planMasterController.containsItems onClicked: { dropPanel.hide() mainWindow.showDialog(removeAllPromptDialog, qsTr("New Plan"), mainWindow.showDialogDefaultWidth, StandardButton.Yes | StandardButton.No) @@ -1045,7 +1045,7 @@ Item { QGCButton { text: qsTr("Save As...") Layout.fillWidth: true - enabled: !_planMasterController.syncInProgress && _visualItems.count > 1 + enabled: !_planMasterController.syncInProgress && _planMasterController.containsItems onClicked: { dropPanel.hide() _planMasterController.saveToSelectedFile() @@ -1053,7 +1053,7 @@ Item { } QGCButton { - text: qsTr("Save Mission Waypoints As KML...") + text: qsTr("Save Mission Waypoints As KML...") Layout.columnSpan: 2 enabled: !_planMasterController.syncInProgress && _visualItems.count > 1 onClicked: { @@ -1080,7 +1080,7 @@ Item { QGCButton { text: qsTr("Upload") Layout.fillWidth: true - enabled: !_planMasterController.offline && !_planMasterController.syncInProgress && _visualItems.count > 1 + enabled: !_planMasterController.offline && !_planMasterController.syncInProgress && _planMasterController.containsItems visible: !QGroundControl.corePlugin.options.disableVehicleConnection onClicked: { dropPanel.hide() diff --git a/src/PlanView/SurveyItemEditor.qml b/src/PlanView/SurveyItemEditor.qml index 2b183a7080c9501e564a1d0aa9a9a41c7b78b498..ffb4fd7d8980c52bc4e621a0b5c2335c875cae71 100644 --- a/src/PlanView/SurveyItemEditor.qml +++ b/src/PlanView/SurveyItemEditor.qml @@ -1,6 +1,5 @@ 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 @@ -29,6 +28,8 @@ Rectangle { property real _fieldWidth: ScreenTools.defaultFontPixelWidth * 10.5 property var _vehicle: QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle : QGroundControl.multiVehicleManager.offlineEditingVehicle property real _cameraMinTriggerInterval: missionItem.cameraCalc.minTriggerInterval.rawValue + property string _currentPreset: missionItem.currentPreset + property bool _usingPreset: _currentPreset !== "" function polygonCaptureStarted() { missionItem.clearPolygon() @@ -66,6 +67,97 @@ Rectangle { visible: missionItem.cameraShots > 0 && _cameraMinTriggerInterval !== 0 && _cameraMinTriggerInterval > missionItem.timeBetweenShots } + QGCLabel { + text: qsTr("Presets") + } + + QGCComboBox { + id: presetCombo + anchors.left: parent.left + anchors.right: parent.right + model: _presetList + + property var _presetList: [] + + readonly property int _indexCustom: 0 + readonly property int _indexCreate: 1 + readonly property int _indexDelete: 2 + readonly property int _indexLabel: 3 + readonly property int _indexFirstPreset: 4 + + Component.onCompleted: _updateList() + + onActivated: { + if (index == _indexCustom) { + missionItem.clearCurrentPreset() + } else if (index == _indexCreate) { + rootQgcView.showDialog(savePresetDialog, qsTr("Save Preset"), rootQgcView.showDialogDefaultWidth, StandardButton.Save | StandardButton.Cancel) + } else if (index == _indexDelete) { + if (missionItem.builtInPreset) { + rootQgcView.showMessage(qsTr("Delete Preset"), qsTr("This preset cannot be deleted."), StandardButton.Ok) + } else { + missionItem.deleteCurrentPreset() + } + } else if (index >= _indexFirstPreset) { + missionItem.loadPreset(textAt(index)) + } else { + _selectCurrentPreset() + } + } + + Connections { + target: missionItem + + onPresetNamesChanged: presetCombo._updateList() + onCurrentPresetChanged: presetCombo._selectCurrentPreset() + } + + // There is some major strangeness going on with programatically changing the index of a combo box in this scenario. + // If you just set currentIndex directly it will just change back 1o -1 magically. Has something to do with resetting + // model on the fly I think. But not sure. To work around I delay the currentIndex changes to let things unwind. + Timer { + id: delayedIndexChangeTimer + interval: 10 + + property int newIndex + + onTriggered: presetCombo.currentIndex = newIndex + + } + + function delayedIndexChange(index) { + delayedIndexChangeTimer.newIndex = index + delayedIndexChangeTimer.start() + } + + function _updateList() { + _presetList = [] + _presetList.push(qsTr("Custom (specify all settings)")) + _presetList.push(qsTr("Save Settings As Preset")) + _presetList.push(qsTr("Delete Current Preset")) + if (missionItem.presetNames.length !== 0) { + _presetList.push(qsTr("Presets:")) + } + + for (var i=0; igotoFlightMode(); } -void Vehicle::guidedModeRTL(void) +void Vehicle::guidedModeRTL(bool smartRTL) { if (!guidedModeSupported()) { qgcApp()->showMessage(guided_mode_not_supported_by_vehicle); return; } - _firmwarePlugin->guidedModeRTL(this); + _firmwarePlugin->guidedModeRTL(this, smartRTL); } void Vehicle::guidedModeLand(void) @@ -3602,6 +3602,16 @@ QString Vehicle::rtlFlightMode(void) const return _firmwarePlugin->rtlFlightMode(); } +QString Vehicle::smartRTLFlightMode(void) const +{ + return _firmwarePlugin->smartRTLFlightMode(); +} + +bool Vehicle::supportsSmartRTL(void) const +{ + return _firmwarePlugin->supportsSmartRTL(); +} + QString Vehicle::landFlightMode(void) const { return _firmwarePlugin->landFlightMode(); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index c3706980dc5e946176b8991e29f30813b7aa48d0..a71e6a552d8c04110ae33b25e595bd39e95c60a8 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -600,6 +600,8 @@ public: Q_PROPERTY(QString missionFlightMode READ missionFlightMode CONSTANT) Q_PROPERTY(QString pauseFlightMode READ pauseFlightMode CONSTANT) Q_PROPERTY(QString rtlFlightMode READ rtlFlightMode CONSTANT) + Q_PROPERTY(QString smartRTLFlightMode READ smartRTLFlightMode CONSTANT) + Q_PROPERTY(bool supportsSmartRTL READ supportsSmartRTL CONSTANT) Q_PROPERTY(QString landFlightMode READ landFlightMode CONSTANT) Q_PROPERTY(QString takeControlFlightMode READ takeControlFlightMode CONSTANT) Q_PROPERTY(QString firmwareTypeString READ firmwareTypeString NOTIFY firmwareTypeChanged) @@ -698,7 +700,7 @@ public: Q_INVOKABLE void disconnectInactiveVehicle(void); /// Command vehicle to return to launch - Q_INVOKABLE void guidedModeRTL(void); + Q_INVOKABLE void guidedModeRTL(bool smartRTL); /// Command vehicle to land at current location Q_INVOKABLE void guidedModeLand(void); @@ -919,6 +921,8 @@ public: QString missionFlightMode () const; QString pauseFlightMode () const; QString rtlFlightMode () const; + QString smartRTLFlightMode () const; + bool supportsSmartRTL () const; QString landFlightMode () const; QString takeControlFlightMode () const; double defaultCruiseSpeed () const { return _defaultCruiseSpeed; } diff --git a/src/comm/SerialLink.h b/src/comm/SerialLink.h index 3d02197c93c3776322c69932561a59043c0fa78d..5dd0074a1d6472a86bf50a41f7c0761d8f58f04d 100644 --- a/src/comm/SerialLink.h +++ b/src/comm/SerialLink.h @@ -145,6 +145,9 @@ public: bool connect(void); bool disconnect(void); + /// Don't even think of calling this method! + QSerialPort* _hackAccessToPort(void) { return _port; } + private slots: /** * @brief Write a number of bytes to the interface.