Commit 2b1fc9de authored by DoinLakeFlyer's avatar DoinLakeFlyer

parent 4a049d4f
......@@ -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 \
......
......@@ -242,6 +242,8 @@
<file alias="VibrationPageWidget.qml">src/FlightMap/Widgets/VibrationPageWidget.qml</file>
<file alias="VideoPageWidget.qml">src/FlightMap/Widgets/VideoPageWidget.qml</file>
<file alias="VirtualJoystick.qml">src/FlightDisplay/VirtualJoystick.qml</file>
<file alias="QGroundControl/Controls/VTOLLandingPatternMapVisual.qml">src/PlanView/VTOLLandingPatternMapVisual.qml</file>
<file alias="VTOLLandingPatternEditor.qml">src/PlanView/VTOLLandingPatternEditor.qml</file>
</qresource>
<qresource prefix="/json">
<file alias="ADSBVehicleManager.SettingsGroup.json">src/Settings/ADSBVehicleManager.SettingsGroup.json</file>
......@@ -291,6 +293,7 @@
<file alias="Vehicle/VibrationFact.json">src/Vehicle/VibrationFact.json</file>
<file alias="Vehicle/WindFact.json">src/Vehicle/WindFact.json</file>
<file alias="Video.SettingsGroup.json">src/Settings/Video.SettingsGroup.json</file>
<file alias="VTOLLandingPattern.FactMetaData.json">src/MissionManager/VTOLLandingPattern.FactMetaData.json</file>
</qresource>
<qresource prefix="/MockLink">
<file alias="APMArduSubMockLink.params">src/comm/APMArduSubMockLink.params</file>
......
......@@ -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<FixedWingLandingComplexItem*>(insertComplexMissionItem(MissionController::patternFWLandingName, coordinate, visualItemIndex, makeCurrentItem));
return fwLanding;
} else if (_managerVehicle->vtol()) {
VTOLLandingComplexItem* vtolLanding = qobject_cast<VTOLLandingComplexItem*>(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);
......
......@@ -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;
......
......@@ -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);
}
......
This diff is collapsed.
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* 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<double>::quiet_NaN(); }
double specifiedGimbalYaw (void) final { return std::numeric_limits<double>::quiet_NaN(); }
double specifiedGimbalPitch (void) final { return std::numeric_limits<double>::quiet_NaN(); }
void appendMissionItems (QList<MissionItem*>& 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<QString, FactMetaData*> _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;
};
[
{
"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
}
]
......@@ -274,7 +274,7 @@ Item {
sourceItem:
MissionItemIndexLabel {
index: _missionItem.sequenceNumber
index: _missionItem.lastSequenceNumber
checked: _missionItem.isCurrentItem
onClicked: _root.clicked(_missionItem.sequenceNumber)
......
This diff is collapsed.
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* 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"
}
}
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment