diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 9309e9ef4344a1301a92297b951faff8e4de74bb..9cf212f4194316a20ba4029c7546ef0d96ece377 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -618,6 +618,7 @@ HEADERS += \ src/MissionManager/TakeoffMissionItem.h \ src/MissionManager/TransectStyleComplexItem.h \ src/MissionManager/VisualMissionItem.h \ + src/MissionManager/VTOLLandingComplexItem.h \ src/PositionManager/PositionManager.h \ src/PositionManager/SimulatedPosition.h \ src/Geo/QGCGeo.h \ @@ -823,6 +824,7 @@ SOURCES += \ src/MissionManager/TakeoffMissionItem.cc \ src/MissionManager/TransectStyleComplexItem.cc \ src/MissionManager/VisualMissionItem.cc \ + src/MissionManager/VTOLLandingComplexItem.cc \ src/PositionManager/PositionManager.cpp \ src/PositionManager/SimulatedPosition.cc \ src/Geo/QGCGeo.cc \ diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index f363815097e8d31a601238354d82143a967a3c0f..e0ef70e51d10921e87b330b523fd1e45c099ecff 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -242,6 +242,8 @@ src/FlightMap/Widgets/VibrationPageWidget.qml src/FlightMap/Widgets/VideoPageWidget.qml src/FlightDisplay/VirtualJoystick.qml + src/PlanView/VTOLLandingPatternMapVisual.qml + src/PlanView/VTOLLandingPatternEditor.qml src/Settings/ADSBVehicleManager.SettingsGroup.json @@ -291,6 +293,7 @@ src/Vehicle/VibrationFact.json src/Vehicle/WindFact.json src/Settings/Video.SettingsGroup.json + src/MissionManager/VTOLLandingPattern.FactMetaData.json src/comm/APMArduSubMockLink.params diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 6ac56aede714e5669485563fc4c252ca658d1711..f85d0c9e0b039191a127b84f2c80f41dcfc201e5 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -7,7 +7,6 @@ * ****************************************************************************/ - #include "MissionCommandUIInfo.h" #include "MissionController.h" #include "MultiVehicleManager.h" @@ -18,6 +17,7 @@ #include "SimpleMissionItem.h" #include "SurveyComplexItem.h" #include "FixedWingLandingComplexItem.h" +#include "VTOLLandingComplexItem.h" #include "StructureScanComplexItem.h" #include "CorridorScanComplexItem.h" #include "JsonHelper.h" @@ -55,6 +55,7 @@ const int MissionController::_missionFileVersion = 2; const QString MissionController::patternSurveyName (QT_TRANSLATE_NOOP("MissionController", "Survey")); const QString MissionController::patternFWLandingName (QT_TRANSLATE_NOOP("MissionController", "Fixed Wing Landing")); +const QString MissionController::patternVTOLLandingName (QT_TRANSLATE_NOOP("MissionController", "VTOL Landing")); const QString MissionController::patternStructureScanName (QT_TRANSLATE_NOOP("MissionController", "Structure Scan")); const QString MissionController::patternCorridorScanName (QT_TRANSLATE_NOOP("MissionController", "Corridor Scan")); @@ -387,6 +388,9 @@ VisualMissionItem* MissionController::insertLandItem(QGeoCoordinate coordinate, if (_managerVehicle->fixedWing()) { FixedWingLandingComplexItem* fwLanding = qobject_cast(insertComplexMissionItem(MissionController::patternFWLandingName, coordinate, visualItemIndex, makeCurrentItem)); return fwLanding; + } else if (_managerVehicle->vtol()) { + VTOLLandingComplexItem* vtolLanding = qobject_cast(insertComplexMissionItem(MissionController::patternVTOLLandingName, coordinate, visualItemIndex, makeCurrentItem)); + return vtolLanding; } else { return _insertSimpleMissionItemWorker(coordinate, _managerVehicle->vtol() ? MAV_CMD_NAV_VTOL_LAND : MAV_CMD_NAV_RETURN_TO_LAUNCH, visualItemIndex, makeCurrentItem); } @@ -425,6 +429,8 @@ VisualMissionItem* MissionController::insertComplexMissionItem(QString itemName, newItem->setCoordinate(mapCenterCoordinate); } else if (itemName == patternFWLandingName) { newItem = new FixedWingLandingComplexItem(_masterController, _flyView, _visualItems /* parent */); + } else if (itemName == patternVTOLLandingName) { + newItem = new VTOLLandingComplexItem(_masterController, _flyView, _visualItems /* parent */); } else if (itemName == patternStructureScanName) { newItem = new StructureScanComplexItem(_masterController, _flyView, QString() /* kmlFile */, _visualItems /* parent */); } else if (itemName == patternCorridorScanName) { @@ -791,6 +797,15 @@ bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjec nextSequenceNumber = landingItem->lastSequenceNumber() + 1; qCDebug(MissionControllerLog) << "FW Landing Pattern load complete: nextSequenceNumber" << nextSequenceNumber; visualItems->append(landingItem); + } else if (complexItemType == VTOLLandingComplexItem::jsonComplexItemTypeValue) { + qCDebug(MissionControllerLog) << "Loading VTOL Landing Pattern: nextSequenceNumber" << nextSequenceNumber; + VTOLLandingComplexItem* landingItem = new VTOLLandingComplexItem(_masterController, _flyView, visualItems); + if (!landingItem->load(itemObject, nextSequenceNumber++, errorString)) { + return false; + } + nextSequenceNumber = landingItem->lastSequenceNumber() + 1; + qCDebug(MissionControllerLog) << "VTOL Landing Pattern load complete: nextSequenceNumber" << nextSequenceNumber; + visualItems->append(landingItem); } else if (complexItemType == StructureScanComplexItem::jsonComplexItemTypeValue) { qCDebug(MissionControllerLog) << "Loading Structure Scan: nextSequenceNumber" << nextSequenceNumber; StructureScanComplexItem* structureItem = new StructureScanComplexItem(_masterController, _flyView, QString() /* kmlFile */, visualItems); diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index c85e726b0079f860ee4bb93b6d0eeba11ba3a85d..0f18af80690b49e3db71cc2f9c30fa421ddd96ca 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -233,8 +233,9 @@ public: bool isEmpty (void) const; // These are the names shown in the UI for the pattern items. They are public so custom builds can remove the ones - // they don't want through the QGCCorePlugin:: + // they don't want through the QGCCorePlugin. static const QString patternFWLandingName; + static const QString patternVTOLLandingName; static const QString patternStructureScanName; static const QString patternCorridorScanName; static const QString patternSurveyName; diff --git a/src/MissionManager/TakeoffMissionItem.cc b/src/MissionManager/TakeoffMissionItem.cc index e6bfc703066f948354245a8359603b0970ddaf5b..5dfffa646ad4c186b7d54853e5c4eef99615e74a 100644 --- a/src/MissionManager/TakeoffMissionItem.cc +++ b/src/MissionManager/TakeoffMissionItem.cc @@ -175,16 +175,18 @@ void TakeoffMissionItem::setLaunchCoordinate(const QGeoCoordinate& launchCoordin if (_launchTakeoffAtSameLocation) { takeoffCoordinate = launchCoordinate; } else { - double altitude = this->altitude()->rawValue().toDouble(); - double distance = 0.0; - - if (coordinateHasRelativeAltitude()) { - // Offset for fixed wing climb out of 30 degrees - if (altitude != 0.0) { - distance = altitude / tan(qDegreesToRadians(30.0)); + double distance = 30.0; // Default distance is VTOL transition to takeoff point distance + if (_controllerVehicle->fixedWing()) { + double altitude = this->altitude()->rawValue().toDouble(); + + if (coordinateHasRelativeAltitude()) { + // Offset for fixed wing climb out of 30 degrees + if (altitude != 0.0) { + distance = altitude / tan(qDegreesToRadians(30.0)); + } + } else { + distance = altitude * 1.5; } - } else { - distance = altitude * 1.5; } takeoffCoordinate = launchCoordinate.atDistanceAndAzimuth(distance, 0); } diff --git a/src/MissionManager/VTOLLandingComplexItem.cc b/src/MissionManager/VTOLLandingComplexItem.cc new file mode 100644 index 0000000000000000000000000000000000000000..174bbb46fb90199ca229d6ea8e155be8300e5c43 --- /dev/null +++ b/src/MissionManager/VTOLLandingComplexItem.cc @@ -0,0 +1,661 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "VTOLLandingComplexItem.h" +#include "JsonHelper.h" +#include "MissionController.h" +#include "QGCGeo.h" +#include "SimpleMissionItem.h" +#include "PlanMasterController.h" + +#include + +QGC_LOGGING_CATEGORY(VTOLLandingComplexItemLog, "VTOLLandingComplexItemLog") + +const char* VTOLLandingComplexItem::settingsGroup = "VTOLLanding"; +const char* VTOLLandingComplexItem::jsonComplexItemTypeValue = "vtolLandingPattern"; + +const char* VTOLLandingComplexItem::loiterToLandDistanceName = "LandingDistance"; +const char* VTOLLandingComplexItem::landingHeadingName = "LandingHeading"; +const char* VTOLLandingComplexItem::loiterAltitudeName = "LoiterAltitude"; +const char* VTOLLandingComplexItem::loiterRadiusName = "LoiterRadius"; +const char* VTOLLandingComplexItem::landingAltitudeName = "LandingAltitude"; +const char* VTOLLandingComplexItem::stopTakingPhotosName = "StopTakingPhotos"; +const char* VTOLLandingComplexItem::stopTakingVideoName = "StopTakingVideo"; + +const char* VTOLLandingComplexItem::_jsonLoiterCoordinateKey = "loiterCoordinate"; +const char* VTOLLandingComplexItem::_jsonLoiterRadiusKey = "loiterRadius"; +const char* VTOLLandingComplexItem::_jsonLoiterClockwiseKey = "loiterClockwise"; +const char* VTOLLandingComplexItem::_jsonLandingCoordinateKey = "landCoordinate"; +const char* VTOLLandingComplexItem::_jsonAltitudesAreRelativeKey = "altitudesAreRelative"; +const char* VTOLLandingComplexItem::_jsonStopTakingPhotosKey = "stopTakingPhotos"; +const char* VTOLLandingComplexItem::_jsonStopTakingVideoKey = "stopVideoPhotos"; + +VTOLLandingComplexItem::VTOLLandingComplexItem(PlanMasterController* masterController, bool flyView, QObject* parent) + : ComplexMissionItem (masterController, flyView, parent) + , _sequenceNumber (0) + , _dirty (false) + , _landingCoordSet (false) + , _ignoreRecalcSignals (false) + , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/VTOLLandingPattern.FactMetaData.json"), this)) + , _landingDistanceFact (settingsGroup, _metaDataMap[loiterToLandDistanceName]) + , _loiterAltitudeFact (settingsGroup, _metaDataMap[loiterAltitudeName]) + , _loiterRadiusFact (settingsGroup, _metaDataMap[loiterRadiusName]) + , _landingHeadingFact (settingsGroup, _metaDataMap[landingHeadingName]) + , _landingAltitudeFact (settingsGroup, _metaDataMap[landingAltitudeName]) + , _stopTakingPhotosFact (settingsGroup, _metaDataMap[stopTakingPhotosName]) + , _stopTakingVideoFact (settingsGroup, _metaDataMap[stopTakingVideoName]) + , _loiterClockwise (true) + , _altitudesAreRelative (true) +{ + _editorQml = "qrc:/qml/VTOLLandingPatternEditor.qml"; + _isIncomplete = false; + + QGeoCoordinate homePositionCoordinate = masterController->missionController()->plannedHomePosition(); + if (homePositionCoordinate.isValid()) { + setLandingCoordinate(homePositionCoordinate); + } + + connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact); + connect(&_landingAltitudeFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_updateLandingCoodinateAltitudeFromFact); + + connect(&_landingDistanceFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_recalcFromHeadingAndDistanceChange); + connect(&_landingHeadingFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_recalcFromHeadingAndDistanceChange); + + connect(&_loiterRadiusFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_recalcFromRadiusChange); + connect(this, &VTOLLandingComplexItem::loiterClockwiseChanged, this, &VTOLLandingComplexItem::_recalcFromRadiusChange); + + connect(this, &VTOLLandingComplexItem::loiterCoordinateChanged, this, &VTOLLandingComplexItem::_recalcFromCoordinateChange); + connect(this, &VTOLLandingComplexItem::landingCoordinateChanged, this, &VTOLLandingComplexItem::_recalcFromCoordinateChange); + + connect(&_stopTakingPhotosFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_signalLastSequenceNumberChanged); + connect(&_stopTakingVideoFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_signalLastSequenceNumberChanged); + + connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_setDirty); + connect(&_landingAltitudeFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_setDirty); + connect(&_landingDistanceFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_setDirty); + connect(&_landingHeadingFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_setDirty); + connect(&_loiterRadiusFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_setDirty); + connect(&_stopTakingPhotosFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_setDirty); + connect(&_stopTakingVideoFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_setDirty); + connect(this, &VTOLLandingComplexItem::loiterCoordinateChanged, this, &VTOLLandingComplexItem::_setDirty); + connect(this, &VTOLLandingComplexItem::landingCoordinateChanged, this, &VTOLLandingComplexItem::_setDirty); + connect(this, &VTOLLandingComplexItem::loiterClockwiseChanged, this, &VTOLLandingComplexItem::_setDirty); + connect(this, &VTOLLandingComplexItem::altitudesAreRelativeChanged, this, &VTOLLandingComplexItem::_setDirty); + + connect(this, &VTOLLandingComplexItem::altitudesAreRelativeChanged, this, &VTOLLandingComplexItem::coordinateHasRelativeAltitudeChanged); + connect(this, &VTOLLandingComplexItem::altitudesAreRelativeChanged, this, &VTOLLandingComplexItem::exitCoordinateHasRelativeAltitudeChanged); + + connect(this, &VTOLLandingComplexItem::landingCoordSetChanged, this, &VTOLLandingComplexItem::readyForSaveStateChanged); + connect(this, &VTOLLandingComplexItem::wizardModeChanged, this, &VTOLLandingComplexItem::readyForSaveStateChanged); + + if (_masterController->controllerVehicle()->apmFirmware()) { + // ArduPilot does not support camera commands + _stopTakingVideoFact.setRawValue(false); + _stopTakingPhotosFact.setRawValue(false); + } + + _recalcFromHeadingAndDistanceChange(); + + setDirty(false); +} + +int VTOLLandingComplexItem::lastSequenceNumber(void) const +{ + // Fixed items are: + // land start, loiter, land + // Optional items are: + // stop photos/video + return _sequenceNumber + 2 + (_stopTakingPhotosFact.rawValue().toBool() ? 2 : 0) + (_stopTakingVideoFact.rawValue().toBool() ? 1 : 0); +} + +void VTOLLandingComplexItem::setDirty(bool dirty) +{ + if (_dirty != dirty) { + _dirty = dirty; + emit dirtyChanged(_dirty); + } +} + +void VTOLLandingComplexItem::save(QJsonArray& missionItems) +{ + QJsonObject saveObject; + + saveObject[JsonHelper::jsonVersionKey] = 1; + saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; + saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; + + QGeoCoordinate coordinate; + QJsonValue jsonCoordinate; + + coordinate = _loiterCoordinate; + coordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble()); + JsonHelper::saveGeoCoordinate(coordinate, true /* writeAltitude */, jsonCoordinate); + saveObject[_jsonLoiterCoordinateKey] = jsonCoordinate; + + coordinate = _landingCoordinate; + coordinate.setAltitude(_landingAltitudeFact.rawValue().toDouble()); + JsonHelper::saveGeoCoordinate(coordinate, true /* writeAltitude */, jsonCoordinate); + saveObject[_jsonLandingCoordinateKey] = jsonCoordinate; + + saveObject[_jsonLoiterRadiusKey] = _loiterRadiusFact.rawValue().toDouble(); + saveObject[_jsonStopTakingPhotosKey] = _stopTakingPhotosFact.rawValue().toBool(); + saveObject[_jsonStopTakingVideoKey] = _stopTakingVideoFact.rawValue().toBool(); + saveObject[_jsonLoiterClockwiseKey] = _loiterClockwise; + saveObject[_jsonAltitudesAreRelativeKey] = _altitudesAreRelative; + + missionItems.append(saveObject); +} + +void VTOLLandingComplexItem::setSequenceNumber(int sequenceNumber) +{ + if (_sequenceNumber != sequenceNumber) { + _sequenceNumber = sequenceNumber; + emit sequenceNumberChanged(sequenceNumber); + emit lastSequenceNumberChanged(lastSequenceNumber()); + } +} + +bool VTOLLandingComplexItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString) +{ + QList keyInfoList = { + { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, + { VisualMissionItem::jsonTypeKey, QJsonValue::String, true }, + { ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true }, + { _jsonLoiterCoordinateKey, QJsonValue::Array, true }, + { _jsonLoiterRadiusKey, QJsonValue::Double, true }, + { _jsonLoiterClockwiseKey, QJsonValue::Bool, true }, + { _jsonLandingCoordinateKey, QJsonValue::Array, true }, + { _jsonStopTakingPhotosKey, QJsonValue::Bool, false }, + { _jsonStopTakingVideoKey, QJsonValue::Bool, false }, + }; + if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) { + return false; + } + + QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString(); + QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString(); + if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonComplexItemTypeValue) { + errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType); + return false; + } + + setSequenceNumber(sequenceNumber); + + _ignoreRecalcSignals = true; + + int version = complexObject[JsonHelper::jsonVersionKey].toInt(); + if (version == 1) { + QList v2KeyInfoList = { + { _jsonAltitudesAreRelativeKey, QJsonValue::Bool, true }, + }; + if (!JsonHelper::validateKeys(complexObject, v2KeyInfoList, errorString)) { + _ignoreRecalcSignals = false; + return false; + } + + _altitudesAreRelative = complexObject[_jsonAltitudesAreRelativeKey].toBool(); + } else { + errorString = tr("%1 complex item version %2 not supported").arg(jsonComplexItemTypeValue).arg(version); + _ignoreRecalcSignals = false; + return false; + } + + QGeoCoordinate coordinate; + if (!JsonHelper::loadGeoCoordinate(complexObject[_jsonLoiterCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) { + return false; + } + _loiterCoordinate = coordinate; + _loiterAltitudeFact.setRawValue(coordinate.altitude()); + + if (!JsonHelper::loadGeoCoordinate(complexObject[_jsonLandingCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) { + return false; + } + _landingCoordinate = coordinate; + _landingAltitudeFact.setRawValue(coordinate.altitude()); + + _loiterRadiusFact.setRawValue(complexObject[_jsonLoiterRadiusKey].toDouble()); + _loiterClockwise = complexObject[_jsonLoiterClockwiseKey].toBool(); + + if (complexObject.contains(_jsonStopTakingPhotosKey)) { + _stopTakingPhotosFact.setRawValue(complexObject[_jsonStopTakingPhotosKey].toBool()); + } else { + _stopTakingPhotosFact.setRawValue(false); + } + if (complexObject.contains(_jsonStopTakingVideoKey)) { + _stopTakingVideoFact.setRawValue(complexObject[_jsonStopTakingVideoKey].toBool()); + } else { + _stopTakingVideoFact.setRawValue(false); + } + + _landingCoordSet = true; + + _ignoreRecalcSignals = false; + + _recalcFromCoordinateChange(); + emit coordinateChanged(this->coordinate()); // This will kick off terrain query + + return true; +} + +double VTOLLandingComplexItem::greatestDistanceTo(const QGeoCoordinate &other) const +{ + return qMax(_loiterCoordinate.distanceTo(other),_landingCoordinate.distanceTo(other)); +} + +bool VTOLLandingComplexItem::specifiesCoordinate(void) const +{ + return true; +} + +MissionItem* VTOLLandingComplexItem::createDoLandStartItem(int seqNum, QObject* parent) +{ + return new MissionItem(seqNum, // sequence number + MAV_CMD_DO_LAND_START, // MAV_CMD + MAV_FRAME_MISSION, // MAV_FRAME + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // param 1-7 + true, // autoContinue + false, // isCurrentItem + parent); +} + +MissionItem* VTOLLandingComplexItem::createLoiterToAltItem(int seqNum, bool altRel, double loiterRadius, double lat, double lon, double alt, QObject* parent) +{ + return new MissionItem(seqNum, + MAV_CMD_NAV_LOITER_TO_ALT, + altRel ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, + 1.0, // Heading required = true + loiterRadius, // Loiter radius + 0.0, // param 3 - unused + 1.0, // Exit crosstrack - tangent of loiter to land point + lat, lon, alt, + true, // autoContinue + false, // isCurrentItem + parent); +} + +MissionItem* VTOLLandingComplexItem::createLandItem(int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent) +{ + return new MissionItem(seqNum, + MAV_CMD_NAV_VTOL_LAND, + altRel ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, + 0.0, 0.0, 0.0, 0.0, + lat, lon, alt, + true, // autoContinue + false, // isCurrentItem + parent); + +} + +void VTOLLandingComplexItem::appendMissionItems(QList& items, QObject* missionItemParent) +{ + int seqNum = _sequenceNumber; + + // IMPORTANT NOTE: Any changes here must also be taken into account in scanForItem + + MissionItem* item = createDoLandStartItem(seqNum++, missionItemParent); + items.append(item); + + + if (_stopTakingPhotosFact.rawValue().toBool()) { + CameraSection::appendStopTakingPhotos(items, seqNum, missionItemParent); + } + + if (_stopTakingVideoFact.rawValue().toBool()) { + CameraSection::appendStopTakingVideo(items, seqNum, missionItemParent); + } + + double loiterRadius = _loiterRadiusFact.rawValue().toDouble() * (_loiterClockwise ? 1.0 : -1.0); + item = createLoiterToAltItem(seqNum++, + _altitudesAreRelative, + loiterRadius, + _loiterCoordinate.latitude(), _loiterCoordinate.longitude(), _loiterAltitudeFact.rawValue().toDouble(), + missionItemParent); + items.append(item); + + item = createLandItem(seqNum++, + _altitudesAreRelative, + _landingCoordinate.latitude(), _landingCoordinate.longitude(), _landingAltitudeFact.rawValue().toDouble(), + missionItemParent); + items.append(item); +} + +bool VTOLLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController) +{ + qCDebug(VTOLLandingComplexItemLog) << "VTOLLandingComplexItem::scanForItem count" << visualItems->count(); + + if (visualItems->count() < 3) { + return false; + } + + // A valid fixed wing landing pattern is comprised of the follow commands in this order at the end of the item list: + // MAV_CMD_DO_LAND_START - required + // Stop taking photos sequence - optional + // Stop taking video sequence - optional + // MAV_CMD_NAV_LOITER_TO_ALT - required + // MAV_CMD_NAV_LAND - required + + // Start looking for the commands in reverse order from the end of the list + + int scanIndex = visualItems->count() - 1; + + if (scanIndex < 0 || scanIndex > visualItems->count() - 1) { + return false; + } + SimpleMissionItem* item = visualItems->value(scanIndex--); + if (!item) { + return false; + } + MissionItem& missionItemLand = item->missionItem(); + if (missionItemLand.command() != MAV_CMD_NAV_LAND || + !(missionItemLand.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT || missionItemLand.frame() == MAV_FRAME_GLOBAL) || + missionItemLand.param1() != 0 || missionItemLand.param2() != 0 || missionItemLand.param3() != 0 || missionItemLand.param4() != 0) { + return false; + } + MAV_FRAME landPointFrame = missionItemLand.frame(); + + if (scanIndex < 0 || scanIndex > visualItems->count() - 1) { + return false; + } + item = visualItems->value(scanIndex); + if (!item) { + return false; + } + MissionItem& missionItemLoiter = item->missionItem(); + if (missionItemLoiter.command() != MAV_CMD_NAV_LOITER_TO_ALT || + missionItemLoiter.frame() != landPointFrame || + missionItemLoiter.param1() != 1.0 || missionItemLoiter.param3() != 0 || missionItemLoiter.param4() != 1.0) { + return false; + } + + scanIndex -= CameraSection::stopTakingVideoCommandCount(); + bool stopTakingVideo = CameraSection::scanStopTakingVideo(visualItems, scanIndex, false /* removeScannedItems */); + if (!stopTakingVideo) { + scanIndex += CameraSection::stopTakingVideoCommandCount(); + } + + scanIndex -= CameraSection::stopTakingPhotosCommandCount(); + bool stopTakingPhotos = CameraSection::scanStopTakingPhotos(visualItems, scanIndex, false /* removeScannedItems */); + if (!stopTakingPhotos) { + scanIndex += CameraSection::stopTakingPhotosCommandCount(); + } + + scanIndex--; + if (scanIndex < 0 || scanIndex > visualItems->count() - 1) { + return false; + } + item = visualItems->value(scanIndex); + if (!item) { + return false; + } + MissionItem& missionItemDoLandStart = item->missionItem(); + if (missionItemDoLandStart.command() != MAV_CMD_DO_LAND_START || + missionItemDoLandStart.frame() != MAV_FRAME_MISSION || + missionItemDoLandStart.param1() != 0 || missionItemDoLandStart.param2() != 0 || missionItemDoLandStart.param3() != 0 || missionItemDoLandStart.param4() != 0|| missionItemDoLandStart.param5() != 0|| missionItemDoLandStart.param6() != 0) { + return false; + } + + // We made it this far so we do have a Fixed Wing Landing Pattern item at the end of the mission. + // Since we have scanned it we need to remove the items for it fromt the list + int deleteCount = 3; + if (stopTakingPhotos) { + deleteCount += CameraSection::stopTakingPhotosCommandCount(); + } + if (stopTakingVideo) { + deleteCount += CameraSection::stopTakingVideoCommandCount(); + } + int firstItem = visualItems->count() - deleteCount; + while (deleteCount--) { + visualItems->removeAt(firstItem)->deleteLater(); + } + + // Now stuff all the scanned information into the item + + VTOLLandingComplexItem* complexItem = new VTOLLandingComplexItem(masterController, flyView, visualItems); + + complexItem->_ignoreRecalcSignals = true; + + complexItem->_altitudesAreRelative = landPointFrame == MAV_FRAME_GLOBAL_RELATIVE_ALT; + complexItem->_loiterRadiusFact.setRawValue(qAbs(missionItemLoiter.param2())); + complexItem->_loiterClockwise = missionItemLoiter.param2() > 0; + complexItem->setLoiterCoordinate(QGeoCoordinate(missionItemLoiter.param5(), missionItemLoiter.param6())); + complexItem->_loiterAltitudeFact.setRawValue(missionItemLoiter.param7()); + + complexItem->_landingCoordinate.setLatitude(missionItemLand.param5()); + complexItem->_landingCoordinate.setLongitude(missionItemLand.param6()); + complexItem->_landingAltitudeFact.setRawValue(missionItemLand.param7()); + + complexItem->_stopTakingPhotosFact.setRawValue(stopTakingPhotos); + complexItem->_stopTakingVideoFact.setRawValue(stopTakingVideo); + + complexItem->_landingCoordSet = true; + + complexItem->_ignoreRecalcSignals = false; + complexItem->_recalcFromCoordinateChange(); + complexItem->setDirty(false); + + visualItems->append(complexItem); + + return true; +} + +double VTOLLandingComplexItem::complexDistance(void) const +{ + return _loiterCoordinate.distanceTo(_landingCoordinate); +} + +void VTOLLandingComplexItem::setLandingCoordinate(const QGeoCoordinate& coordinate) +{ + if (coordinate != _landingCoordinate) { + _landingCoordinate = coordinate; + if (_landingCoordSet) { + emit exitCoordinateChanged(coordinate); + emit landingCoordinateChanged(coordinate); + } else { + _ignoreRecalcSignals = true; + emit exitCoordinateChanged(coordinate); + emit landingCoordinateChanged(coordinate); + _ignoreRecalcSignals = false; + _landingCoordSet = true; + _recalcFromHeadingAndDistanceChange(); + emit landingCoordSetChanged(true); + } + } +} + +void VTOLLandingComplexItem::setLoiterCoordinate(const QGeoCoordinate& coordinate) +{ + if (coordinate != _loiterCoordinate) { + _loiterCoordinate = coordinate; + emit coordinateChanged(coordinate); + emit loiterCoordinateChanged(coordinate); + } +} + +double VTOLLandingComplexItem::_mathematicAngleToHeading(double angle) +{ + double heading = (angle - 90) * -1; + if (heading < 0) { + heading += 360; + } + + return heading; +} + +double VTOLLandingComplexItem::_headingToMathematicAngle(double heading) +{ + return heading - 90 * -1; +} + +void VTOLLandingComplexItem::_recalcFromRadiusChange(void) +{ + // Fixed: + // land + // loiter tangent + // distance + // radius + // heading + // Adjusted: + // loiter + + if (!_ignoreRecalcSignals) { + // These are our known values + double radius = _loiterRadiusFact.rawValue().toDouble(); + double landToTangentDistance = _landingDistanceFact.rawValue().toDouble(); + double heading = _landingHeadingFact.rawValue().toDouble(); + + double landToLoiterDistance = _landingCoordinate.distanceTo(_loiterCoordinate); + if (landToLoiterDistance < radius) { + // Degnenerate case: Move tangent to loiter point + _loiterTangentCoordinate = _loiterCoordinate; + + double heading = _landingCoordinate.azimuthTo(_loiterTangentCoordinate); + + _ignoreRecalcSignals = true; + _landingHeadingFact.setRawValue(heading); + emit loiterTangentCoordinateChanged(_loiterTangentCoordinate); + _ignoreRecalcSignals = false; + } else { + double landToLoiterDistance = qSqrt(qPow(radius, 2) + qPow(landToTangentDistance, 2)); + 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); + emit coordinateChanged(_loiterCoordinate); + _ignoreRecalcSignals = false; + } + } +} + +void VTOLLandingComplexItem::_recalcFromHeadingAndDistanceChange(void) +{ + // Fixed: + // land + // heading + // distance + // radius + // Adjusted: + // loiter + // loiter tangent + // glide slope + + if (!_ignoreRecalcSignals && _landingCoordSet) { + // These are our known values + double radius = _loiterRadiusFact.rawValue().toDouble(); + double landToTangentDistance = _landingDistanceFact.rawValue().toDouble(); + double heading = _landingHeadingFact.rawValue().toDouble(); + + // Calculate loiter tangent coordinate + _loiterTangentCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, heading + 180); + + // Calculate the distance and angle to the loiter coordinate + QGeoCoordinate tangent = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, 0); + QGeoCoordinate loiter = tangent.atDistanceAndAzimuth(radius, 90); + double loiterDistance = _landingCoordinate.distanceTo(loiter); + double loiterAzimuth = _landingCoordinate.azimuthTo(loiter) * (_loiterClockwise ? -1 : 1); + + // 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); + emit loiterCoordinateChanged(_loiterCoordinate); + emit coordinateChanged(_loiterCoordinate); + _ignoreRecalcSignals = false; + } +} + +QPointF VTOLLandingComplexItem::_rotatePoint(const QPointF& point, const QPointF& origin, double angle) +{ + QPointF rotated; + double radians = (M_PI / 180.0) * angle; + + rotated.setX(((point.x() - origin.x()) * cos(radians)) - ((point.y() - origin.y()) * sin(radians)) + origin.x()); + rotated.setY(((point.x() - origin.x()) * sin(radians)) + ((point.y() - origin.y()) * cos(radians)) + origin.y()); + + return rotated; +} + +void VTOLLandingComplexItem::_recalcFromCoordinateChange(void) +{ + // Fixed: + // land + // loiter + // radius + // Adjusted: + // loiter tangent + // heading + // distance + // glide slope + + if (!_ignoreRecalcSignals && _landingCoordSet) { + // These are our known values + double radius = _loiterRadiusFact.rawValue().toDouble(); + double landToLoiterDistance = _landingCoordinate.distanceTo(_loiterCoordinate); + double landToLoiterHeading = _landingCoordinate.azimuthTo(_loiterCoordinate); + + double landToTangentDistance; + if (landToLoiterDistance < radius) { + // Degenerate case, set tangent to loiter coordinate + _loiterTangentCoordinate = _loiterCoordinate; + landToTangentDistance = _landingCoordinate.distanceTo(_loiterTangentCoordinate); + } else { + double loiterToTangentAngle = qRadiansToDegrees(qAsin(radius/landToLoiterDistance)) * (_loiterClockwise ? 1 : -1); + landToTangentDistance = qSqrt(qPow(landToLoiterDistance, 2) - qPow(radius, 2)); + + _loiterTangentCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, landToLoiterHeading + loiterToTangentAngle); + + } + + double heading = _loiterTangentCoordinate.azimuthTo(_landingCoordinate); + + _ignoreRecalcSignals = true; + _landingHeadingFact.setRawValue(heading); + _landingDistanceFact.setRawValue(landToTangentDistance); + emit loiterTangentCoordinateChanged(_loiterTangentCoordinate); + _ignoreRecalcSignals = false; + } +} + +void VTOLLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact(void) +{ + _loiterCoordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble()); + emit loiterCoordinateChanged(_loiterCoordinate); + emit coordinateChanged(_loiterCoordinate); +} + +void VTOLLandingComplexItem::_updateLandingCoodinateAltitudeFromFact(void) +{ + _landingCoordinate.setAltitude(_landingAltitudeFact.rawValue().toDouble()); + emit landingCoordinateChanged(_landingCoordinate); +} + +void VTOLLandingComplexItem::_setDirty(void) +{ + setDirty(true); +} + +void VTOLLandingComplexItem::applyNewAltitude(double newAltitude) +{ + _loiterAltitudeFact.setRawValue(newAltitude); +} + +void VTOLLandingComplexItem::_signalLastSequenceNumberChanged(void) +{ + emit lastSequenceNumberChanged(lastSequenceNumber()); +} + +VTOLLandingComplexItem::ReadyForSaveState VTOLLandingComplexItem::readyForSaveState(void) const +{ + return _landingCoordSet && !_wizardMode ? ReadyForSave : NotReadyForSaveData; +} diff --git a/src/MissionManager/VTOLLandingComplexItem.h b/src/MissionManager/VTOLLandingComplexItem.h new file mode 100644 index 0000000000000000000000000000000000000000..b925acb91c59bf270de5a414c9e46a42a6019b41 --- /dev/null +++ b/src/MissionManager/VTOLLandingComplexItem.h @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * (c) 2009-2020 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 "ComplexMissionItem.h" +#include "MissionItem.h" +#include "Fact.h" +#include "QGCLoggingCategory.h" + +Q_DECLARE_LOGGING_CATEGORY(VTOLLandingComplexItemLog) + +class VTOLLandingPatternTest; +class PlanMasterController; + +class VTOLLandingComplexItem : public ComplexMissionItem +{ + Q_OBJECT + +public: + VTOLLandingComplexItem(PlanMasterController* masterController, bool flyView, QObject* parent); + + Q_PROPERTY(Fact* loiterAltitude READ loiterAltitude CONSTANT) + Q_PROPERTY(Fact* loiterRadius READ loiterRadius CONSTANT) + Q_PROPERTY(Fact* landingAltitude READ landingAltitude CONSTANT) + Q_PROPERTY(Fact* landingHeading READ landingHeading CONSTANT) + Q_PROPERTY(Fact* landingDistance READ landingDistance CONSTANT) + Q_PROPERTY(bool loiterClockwise MEMBER _loiterClockwise NOTIFY loiterClockwiseChanged) + Q_PROPERTY(bool altitudesAreRelative MEMBER _altitudesAreRelative NOTIFY altitudesAreRelativeChanged) + Q_PROPERTY(Fact* stopTakingPhotos READ stopTakingPhotos CONSTANT) + Q_PROPERTY(Fact* stopTakingVideo READ stopTakingVideo CONSTANT) + Q_PROPERTY(QGeoCoordinate loiterCoordinate READ loiterCoordinate WRITE setLoiterCoordinate NOTIFY loiterCoordinateChanged) + Q_PROPERTY(QGeoCoordinate loiterTangentCoordinate READ loiterTangentCoordinate NOTIFY loiterTangentCoordinateChanged) + Q_PROPERTY(QGeoCoordinate landingCoordinate READ landingCoordinate WRITE setLandingCoordinate NOTIFY landingCoordinateChanged) + Q_PROPERTY(bool landingCoordSet MEMBER _landingCoordSet NOTIFY landingCoordSetChanged) + + Fact* loiterAltitude (void) { return &_loiterAltitudeFact; } + Fact* loiterRadius (void) { return &_loiterRadiusFact; } + Fact* landingAltitude (void) { return &_landingAltitudeFact; } + Fact* landingDistance (void) { return &_landingDistanceFact; } + Fact* landingHeading (void) { return &_landingHeadingFact; } + Fact* stopTakingPhotos (void) { return &_stopTakingPhotosFact; } + Fact* stopTakingVideo (void) { return &_stopTakingVideoFact; } + QGeoCoordinate landingCoordinate (void) const { return _landingCoordinate; } + QGeoCoordinate loiterCoordinate (void) const { return _loiterCoordinate; } + QGeoCoordinate loiterTangentCoordinate (void) const { return _loiterTangentCoordinate; } + + void setLandingCoordinate (const QGeoCoordinate& coordinate); + void setLoiterCoordinate (const QGeoCoordinate& coordinate); + + /// Scans the loaded items for a landing pattern complex item + static bool scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController); + + static MissionItem* createDoLandStartItem (int seqNum, QObject* parent); + static MissionItem* createLoiterToAltItem (int seqNum, bool altRel, double loiterRaidus, double lat, double lon, double alt, QObject* parent); + static MissionItem* createLandItem (int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent); + + // Overrides from ComplexMissionItem + double complexDistance (void) const final; + int lastSequenceNumber (void) const final; + bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final; + double greatestDistanceTo (const QGeoCoordinate &other) const final; + QString mapVisualQML (void) const final { return QStringLiteral("VTOLLandingPatternMapVisual.qml"); } + + // Overrides from VisualMissionItem + bool dirty (void) const final { return _dirty; } + bool isSimpleItem (void) const final { return false; } + bool isStandaloneCoordinate (void) const final { return false; } + bool specifiesCoordinate (void) const final; + bool specifiesAltitudeOnly (void) const final { return false; } + QString commandDescription (void) const final { return "Landing Pattern"; } + QString commandName (void) const final { return "Landing Pattern"; } + QString abbreviation (void) const final { return "L"; } + QGeoCoordinate coordinate (void) const final { return _loiterCoordinate; } + QGeoCoordinate exitCoordinate (void) const final { return _landingCoordinate; } + int sequenceNumber (void) const final { return _sequenceNumber; } + double specifiedFlightSpeed (void) final { return std::numeric_limits::quiet_NaN(); } + double specifiedGimbalYaw (void) final { return std::numeric_limits::quiet_NaN(); } + double specifiedGimbalPitch (void) final { return std::numeric_limits::quiet_NaN(); } + void appendMissionItems (QList& items, QObject* missionItemParent) final; + void applyNewAltitude (double newAltitude) final; + double additionalTimeDelay (void) const final { return 0; } + ReadyForSaveState readyForSaveState (void) const final; + + bool coordinateHasRelativeAltitude (void) const final { return _altitudesAreRelative; } + bool exitCoordinateHasRelativeAltitude (void) const final { return _altitudesAreRelative; } + bool exitCoordinateSameAsEntry (void) const final { return false; } + + void setDirty (bool dirty) final; + void setCoordinate (const QGeoCoordinate& coordinate) final { setLoiterCoordinate(coordinate); } + void setSequenceNumber (int sequenceNumber) final; + void save (QJsonArray& missionItems) final; + + static const char* jsonComplexItemTypeValue; + + static const char* settingsGroup; + static const char* loiterToLandDistanceName; + static const char* loiterAltitudeName; + static const char* loiterRadiusName; + static const char* landingHeadingName; + static const char* landingAltitudeName; + static const char* stopTakingPhotosName; + static const char* stopTakingVideoName; + +signals: + void loiterCoordinateChanged (QGeoCoordinate coordinate); + void loiterTangentCoordinateChanged (QGeoCoordinate coordinate); + void landingCoordinateChanged (QGeoCoordinate coordinate); + void landingCoordSetChanged (bool landingCoordSet); + void loiterClockwiseChanged (bool loiterClockwise); + void altitudesAreRelativeChanged (bool altitudesAreRelative); + +private slots: + void _recalcFromHeadingAndDistanceChange (void); + void _recalcFromCoordinateChange (void); + void _recalcFromRadiusChange (void); + void _updateLoiterCoodinateAltitudeFromFact (void); + void _updateLandingCoodinateAltitudeFromFact (void); + double _mathematicAngleToHeading (double angle); + double _headingToMathematicAngle (double heading); + void _setDirty (void); + void _signalLastSequenceNumberChanged (void); + +private: + QPointF _rotatePoint (const QPointF& point, const QPointF& origin, double angle); + + int _sequenceNumber; + bool _dirty; + QGeoCoordinate _loiterCoordinate; + QGeoCoordinate _loiterTangentCoordinate; + QGeoCoordinate _landingCoordinate; + bool _landingCoordSet; + bool _ignoreRecalcSignals; + + QMap _metaDataMap; + + Fact _landingDistanceFact; + Fact _loiterAltitudeFact; + Fact _loiterRadiusFact; + Fact _landingHeadingFact; + Fact _landingAltitudeFact; + Fact _stopTakingPhotosFact; + Fact _stopTakingVideoFact; + + bool _loiterClockwise; + bool _altitudesAreRelative; + + static const char* _jsonLoiterCoordinateKey; + static const char* _jsonLoiterRadiusKey; + static const char* _jsonLoiterClockwiseKey; + static const char* _jsonLandingCoordinateKey; + static const char* _jsonAltitudesAreRelativeKey; + static const char* _jsonStopTakingPhotosKey; + static const char* _jsonStopTakingVideoKey; + + friend VTOLLandingPatternTest; +}; diff --git a/src/MissionManager/VTOLLandingPattern.FactMetaData.json b/src/MissionManager/VTOLLandingPattern.FactMetaData.json new file mode 100644 index 0000000000000000000000000000000000000000..2ccd0a5d83adb41afdbb6cc2569a9c00f9e03d25 --- /dev/null +++ b/src/MissionManager/VTOLLandingPattern.FactMetaData.json @@ -0,0 +1,58 @@ +[ +{ + "name": "LandingDistance", + "shortDescription": "Distance between landing and loiter points.", + "type": "double", + "units": "m", + "min": 10, + "decimalPlaces": 1, + "defaultValue": 30.0 +}, +{ + "name": "LandingHeading", + "shortDescription": "Heading from loiter point to land point.", + "type": "double", + "units": "deg", + "min": 0.0, + "max": 360.0, + "decimalPlaces": 0, + "defaultValue": 270.0 +}, +{ + "name": "LoiterAltitude", + "shortDescription": "Aircraft will proceed to the loiter point and loiter downwards until it reaches this approach altitude. Once altitude is reached the aircraft will fly to land point at current altitude.", + "type": "double", + "units": "m", + "decimalPlaces": 1, + "defaultValue": 40.0 +}, +{ + "name": "LoiterRadius", + "shortDescription": "Loiter radius.", + "type": "double", + "decimalPlaces": 1, + "min": 1, + "units": "m", + "defaultValue": 75.0 +}, +{ + "name": "LandingAltitude", + "shortDescription": "Altitude for landing point on ground.", + "type": "double", + "units": "m", + "decimalPlaces": 1, + "defaultValue": 0.0 +}, +{ + "name": "StopTakingPhotos", + "shortDescription": "Stop taking photos", + "type": "bool", + "defaultValue": true +}, +{ + "name": "StopTakingVideo", + "shortDescription": "Stop taking video", + "type": "bool", + "defaultValue": true +} +] diff --git a/src/PlanView/FWLandingPatternMapVisual.qml b/src/PlanView/FWLandingPatternMapVisual.qml index 69a1ffd2f4a4b2edbc1ca0cfc8cc92ae1a031d3e..277c2ee0bdd283c4e50f8801eab6d0ae87b73ff1 100644 --- a/src/PlanView/FWLandingPatternMapVisual.qml +++ b/src/PlanView/FWLandingPatternMapVisual.qml @@ -274,7 +274,7 @@ Item { sourceItem: MissionItemIndexLabel { - index: _missionItem.sequenceNumber + index: _missionItem.lastSequenceNumber checked: _missionItem.isCurrentItem onClicked: _root.clicked(_missionItem.sequenceNumber) diff --git a/src/PlanView/VTOLLandingPatternEditor.qml b/src/PlanView/VTOLLandingPatternEditor.qml new file mode 100644 index 0000000000000000000000000000000000000000..84a8a83e059bfcba65194b8d06eb1d2f1157ffbf --- /dev/null +++ b/src/PlanView/VTOLLandingPatternEditor.qml @@ -0,0 +1,296 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.3 +import QtQuick.Controls 1.2 +import QtQuick.Dialogs 1.2 +import QtQuick.Layouts 1.2 + +import QGroundControl 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Vehicle 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.FactSystem 1.0 +import QGroundControl.FactControls 1.0 +import QGroundControl.Palette 1.0 + +// Editor for Fixed Wing Landing Pattern complex mission item +Rectangle { + id: _root + height: visible ? ((editorColumn.visible ? editorColumn.height : editorColumnNeedLandingPoint.height) + (_margin * 2)) : 0 + width: availableWidth + color: qgcPal.windowShadeDark + radius: _radius + + // The following properties must be available up the hierarchy chain + //property real availableWidth ///< Width for control + //property var missionItem ///< Mission Item for editor + + property var _masterControler: masterController + property var _missionController: _masterControler.missionController + property var _missionVehicle: _masterControler.controllerVehicle + property real _margin: ScreenTools.defaultFontPixelWidth / 2 + property real _spacer: ScreenTools.defaultFontPixelWidth / 2 + property string _setToVehicleHeadingStr: qsTr("Set to vehicle heading") + property string _setToVehicleLocationStr: qsTr("Set to vehicle location") + property bool _showCameraSection: !_missionVehicle.apmFirmware + property int _altitudeMode: missionItem.altitudesAreRelative ? QGroundControl.AltitudeModeRelative : QGroundControl.AltitudeModeAbsolute + + + Column { + id: editorColumn + anchors.margins: _margin + anchors.left: parent.left + anchors.right: parent.right + spacing: _margin + visible: !editorColumnNeedLandingPoint.visible + + SectionHeader { + id: loiterPointSection + anchors.left: parent.left + anchors.right: parent.right + text: qsTr("Loiter point") + } + + Column { + anchors.left: parent.left + anchors.right: parent.right + spacing: _margin + visible: loiterPointSection.checked + + Item { width: 1; height: _spacer } + + GridLayout { + anchors.left: parent.left + anchors.right: parent.right + columns: 2 + + QGCLabel { text: qsTr("Altitude") } + + AltitudeFactTextField { + Layout.fillWidth: true + fact: missionItem.loiterAltitude + altitudeMode: _altitudeMode + } + + QGCLabel { text: qsTr("Radius") } + + FactTextField { + Layout.fillWidth: true + fact: missionItem.loiterRadius + } + } + + Item { width: 1; height: _spacer } + + QGCCheckBox { + text: qsTr("Loiter clockwise") + checked: missionItem.loiterClockwise + onClicked: missionItem.loiterClockwise = checked + } + + QGCButton { + text: _setToVehicleHeadingStr + visible: activeVehicle + onClicked: missionItem.landingHeading.rawValue = activeVehicle.heading.rawValue + } + } + + SectionHeader { + id: landingPointSection + anchors.left: parent.left + anchors.right: parent.right + text: qsTr("Landing point") + } + + Column { + anchors.left: parent.left + anchors.right: parent.right + spacing: _margin + visible: landingPointSection.checked + + Item { width: 1; height: _spacer } + + GridLayout { + anchors.left: parent.left + anchors.right: parent.right + columns: 2 + + QGCLabel { text: qsTr("Heading") } + + FactTextField { + Layout.fillWidth: true + fact: missionItem.landingHeading + } + + QGCLabel { text: qsTr("Altitude") } + + AltitudeFactTextField { + Layout.fillWidth: true + fact: missionItem.landingAltitude + altitudeMode: _altitudeMode + } + + QGCLabel { text: qsTr("Landing Dist") } + + FactTextField { + fact: missionItem.landingDistance + Layout.fillWidth: true + } + + QGCButton { + text: _setToVehicleLocationStr + visible: activeVehicle + Layout.columnSpan: 2 + onClicked: missionItem.landingCoordinate = activeVehicle.coordinate + } + } + } + + Item { width: 1; height: _spacer } + + QGCCheckBox { + anchors.right: parent.right + text: qsTr("Altitudes relative to launch") + checked: missionItem.altitudesAreRelative + visible: QGroundControl.corePlugin.options.showMissionAbsoluteAltitude || !missionItem.altitudesAreRelative + onClicked: missionItem.altitudesAreRelative = checked + } + + SectionHeader { + id: cameraSection + anchors.left: parent.left + anchors.right: parent.right + text: qsTr("Camera") + visible: _showCameraSection + } + + Column { + anchors.left: parent.left + anchors.right: parent.right + spacing: _margin + visible: _showCameraSection && cameraSection.checked + + Item { width: 1; height: _spacer } + + FactCheckBox { + text: _stopTakingPhotos.shortDescription + fact: _stopTakingPhotos + + property Fact _stopTakingPhotos: missionItem.stopTakingPhotos + } + + FactCheckBox { + text: _stopTakingVideo.shortDescription + fact: _stopTakingVideo + + property Fact _stopTakingVideo: missionItem.stopTakingVideo + } + } + + Column { + anchors.left: parent.left + anchors.right: parent.right + spacing: 0 + + QGCLabel { + anchors.left: parent.left + anchors.right: parent.right + wrapMode: Text.WordWrap + color: qgcPal.warningText + font.pointSize: ScreenTools.smallFontPointSize + text: qsTr("* Actual flight path will vary.") + } + + QGCLabel { + anchors.left: parent.left + anchors.right: parent.right + wrapMode: Text.WordWrap + color: qgcPal.warningText + font.pointSize: ScreenTools.smallFontPointSize + text: qsTr("* Avoid tailwind from loiter to land.") + } + } + } + + Column { + id: editorColumnNeedLandingPoint + anchors.margins: _margin + anchors.top: parent.top + anchors.left: parent.left + anchors.right: parent.right + visible: !missionItem.landingCoordSet || missionItem.wizardMode + spacing: ScreenTools.defaultFontPixelHeight + + Column { + id: landingCoordColumn + anchors.left: parent.left + anchors.right: parent.right + spacing: ScreenTools.defaultFontPixelHeight + visible: !missionItem.landingCoordSet + + QGCLabel { + anchors.left: parent.left + anchors.right: parent.right + wrapMode: Text.WordWrap + horizontalAlignment: Text.AlignHCenter + text: qsTr("Click in map to set landing point.") + } + + QGCLabel { + anchors.left: parent.left + anchors.right: parent.right + horizontalAlignment: Text.AlignHCenter + text: qsTr("- or -") + visible: activeVehicle + } + + QGCButton { + anchors.horizontalCenter: parent.horizontalCenter + text: _setToVehicleLocationStr + visible: activeVehicle + + onClicked: { + missionItem.landingCoordinate = activeVehicle.coordinate + missionItem.landingHeading.rawValue = activeVehicle.heading.rawValue + } + } + } + + ColumnLayout { + anchors.left: parent.left + anchors.right: parent.right + spacing: ScreenTools.defaultFontPixelHeight + visible: !landingCoordColumn.visible + + onVisibleChanged: { + if (visible) { + console.log(missionItem.landingDistance.rawValue) + } + } + + QGCLabel { + Layout.fillWidth: true + wrapMode: Text.WordWrap + text: qsTr("Drag the loiter point to adjust landing direction for wind and obstacles as well as distance to land point.") + } + + QGCButton { + text: qsTr("Done") + Layout.fillWidth: true + onClicked: { + missionItem.wizardMode = false + missionItem.landingDragAngleOnly = false + // Trial of no auto select next item + //editorRoot.selectNextNotReadyItem() + } + } + } + } +} diff --git a/src/PlanView/VTOLLandingPatternMapVisual.qml b/src/PlanView/VTOLLandingPatternMapVisual.qml new file mode 100644 index 0000000000000000000000000000000000000000..2c89df92316e182a09a7ab2c91a5e3d952c3d70d --- /dev/null +++ b/src/PlanView/VTOLLandingPatternMapVisual.qml @@ -0,0 +1,258 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.3 +import QtQuick.Controls 1.2 +import QtLocation 5.3 +import QtPositioning 5.3 +import QtQuick.Layouts 1.11 + +import QGroundControl 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Palette 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.FlightMap 1.0 + +Item { + id: _root + + property var map ///< Map control to place item in + + signal clicked(int sequenceNumber) + + readonly property real _landingWidthMeters: 15 + readonly property real _landingLengthMeters: 100 + + property var _missionItem: object + property var _mouseArea + property var _dragAreas: [ ] + property var _flightPath + property real _landingAreaBearing: _missionItem.landingCoordinate.azimuthTo(_missionItem.loiterTangentCoordinate) + property var _loiterPointObject + property var _landingPointObject + + function hideItemVisuals() { + objMgr.destroyObjects() + } + + function showItemVisuals() { + if (objMgr.rgDynamicObjects.length === 0) { + _loiterPointObject = objMgr.createObject(loiterPointComponent, map, true /* parentObjectIsMap */) + _landingPointObject = objMgr.createObject(landingPointComponent, map, true /* parentObjectIsMap */) + + var rgComponents = [ flightPathComponent, loiterRadiusComponent ] + objMgr.createObjects(rgComponents, map, true /* parentObjectIsMap */) + } + } + + function hideMouseArea() { + if (_mouseArea) { + _mouseArea.destroy() + _mouseArea = undefined + } + } + + function showMouseArea() { + if (!_mouseArea) { + _mouseArea = mouseAreaComponent.createObject(map) + map.addMapItem(_mouseArea) + } + } + + function hideDragAreas() { + for (var i=0; i<_dragAreas.length; i++) { + _dragAreas[i].destroy() + } + _dragAreas = [ ] + } + + function showDragAreas() { + if (_dragAreas.length === 0) { + _dragAreas.push(loiterDragAreaComponent.createObject(map)) + _dragAreas.push(landDragAreaComponent.createObject(map)) + } + } + + function _setFlightPath() { + _flightPath = [ _missionItem.loiterTangentCoordinate, _missionItem.landingCoordinate ] + } + + QGCDynamicObjectManager { + id: objMgr + } + + Component.onCompleted: { + if (_missionItem.landingCoordSet) { + showItemVisuals() + if (!_missionItem.flyView && _missionItem.isCurrentItem) { + showDragAreas() + } + _setFlightPath() + } else if (!_missionItem.flyView && _missionItem.isCurrentItem) { + showMouseArea() + } + } + + Component.onDestruction: { + hideDragAreas() + hideMouseArea() + hideItemVisuals() + } + + Connections { + target: _missionItem + + onIsCurrentItemChanged: { + if (_missionItem.flyView) { + return + } + if (_missionItem.isCurrentItem) { + if (_missionItem.landingCoordSet) { + showDragAreas() + } else { + showMouseArea() + } + } else { + hideMouseArea() + hideDragAreas() + } + } + + onLandingCoordSetChanged: { + if (_missionItem.flyView) { + return + } + if (_missionItem.landingCoordSet) { + hideMouseArea() + showItemVisuals() + showDragAreas() + _setFlightPath() + } else if (_missionItem.isCurrentItem) { + hideDragAreas() + showMouseArea() + } + } + + onLandingCoordinateChanged: _setFlightPath() + onLoiterTangentCoordinateChanged: _setFlightPath() + } + + // Mouse area to capture landing point coordindate + Component { + id: mouseAreaComponent + + MouseArea { + anchors.fill: map + z: QGroundControl.zOrderMapItems + 1 // Over item indicators + + readonly property int _decimalPlaces: 8 + + onClicked: { + var coordinate = map.toCoordinate(Qt.point(mouse.x, mouse.y), false /* clipToViewPort */) + coordinate.latitude = coordinate.latitude.toFixed(_decimalPlaces) + coordinate.longitude = coordinate.longitude.toFixed(_decimalPlaces) + coordinate.altitude = coordinate.altitude.toFixed(_decimalPlaces) + _missionItem.landingCoordinate = coordinate + } + } + } + + // Control which is used to drag the loiter point + Component { + id: loiterDragAreaComponent + + MissionItemIndicatorDrag { + mapControl: _root.map + itemIndicator: _loiterPointObject + itemCoordinate: _missionItem.loiterCoordinate + + onItemCoordinateChanged: _missionItem.loiterCoordinate = itemCoordinate + } + } + + // Control which is used to drag the landing point + Component { + id: landDragAreaComponent + + MissionItemIndicatorDrag { + mapControl: _root.map + itemIndicator: _landingPointObject + itemCoordinate: _missionItem.landingCoordinate + + onItemCoordinateChanged: _missionItem.landingCoordinate = itemCoordinate + } + } + + // Flight path + Component { + id: flightPathComponent + + MapPolyline { + z: QGroundControl.zOrderMapItems - 1 // Under item indicators + line.color: "#be781c" + line.width: 2 + path: _flightPath + } + } + + // Loiter point + Component { + id: loiterPointComponent + + MapQuickItem { + anchorPoint.x: sourceItem.anchorPointX + anchorPoint.y: sourceItem.anchorPointY + z: QGroundControl.zOrderMapItems + coordinate: _missionItem.loiterCoordinate + + sourceItem: + MissionItemIndexLabel { + index: _missionItem.sequenceNumber + label: qsTr("Loiter") + checked: _missionItem.isCurrentItem + + onClicked: _root.clicked(_missionItem.sequenceNumber) + } + } + } + + // Landing point + Component { + id: landingPointComponent + + MapQuickItem { + anchorPoint.x: sourceItem.anchorPointX + anchorPoint.y: sourceItem.anchorPointY + z: QGroundControl.zOrderMapItems + coordinate: _missionItem.landingCoordinate + + sourceItem: + MissionItemIndexLabel { + index: _missionItem.lastSequenceNumber + label: qsTr("Land") + checked: _missionItem.isCurrentItem + + onClicked: _root.clicked(_missionItem.sequenceNumber) + } + } + } + + Component { + id: loiterRadiusComponent + + MapCircle { + z: QGroundControl.zOrderMapItems + center: _missionItem.loiterCoordinate + radius: _missionItem.loiterRadius.rawValue + border.width: 2 + border.color: "green" + color: "transparent" + } + } +}