Merge pull request #4599 from DonLakeFlyer/FWLandingPattern

Complex Item: Fixed Wing Landing Pattern

qgroundcontrol.qrc

@@ -56,6 +56,7 @@
         <file alias="QGroundControl/Controls/FactSliderPanel.qml">src/QmlControls/FactSliderPanel.qml</file>
         <file alias="QGroundControl/Controls/FlightModeDropdown.qml">src/QmlControls/FlightModeDropdown.qml</file>
         <file alias="QGroundControl/Controls/FlightModeMenu.qml">src/QmlControls/FlightModeMenu.qml</file>
+        <file alias="QGroundControl/Controls/FWLandingPatternMapVisual.qml">src/MissionEditor/FWLandingPatternMapVisual.qml</file>
         <file alias="QGroundControl/Controls/GuidedBar.qml">src/QmlControls/GuidedBar.qml</file>
         <file alias="QGroundControl/Controls/IndicatorButton.qml">src/QmlControls/IndicatorButton.qml</file>
         <file alias="QGroundControl/Controls/JoystickThumbPad.qml">src/QmlControls/JoystickThumbPad.qml</file>

src/MissionEditor/FWLandingPatternEditor.qml

@@ -35,7 +35,21 @@ Rectangle {
 
     Column {
         id:                 editorColumn
+        anchors.margins:    _margin
+        anchors.left:       parent.left
+        anchors.right:      parent.right
 
-        QGCLabel { text: "WIP" }
+        QGCLabel { text: "WIP (NOT FOR REAL FLIGHT!)" }
+
+        FactTextFieldGrid {
+            anchors.left:   parent.left
+            anchors.right:  parent.right
+            factList:       missionItem.textFieldFacts
+        }
+
+        FactCheckBox {
+            text:   missionItem.loiterClockwise.name
+            fact:   missionItem.loiterClockwise
+        }
     }
 }

src/MissionEditor/FWLandingPatternMapVisual.qml

@@ -0,0 +1,64 @@
+/****************************************************************************
+ *
+ *   (c) 2009-2016 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.2
+import QtQuick.Controls 1.2
+import QtLocation       5.3
+import QtPositioning    5.2
+
+import QGroundControl.ScreenTools   1.0
+import QGroundControl.Palette       1.0
+import QGroundControl.Controls      1.0
+
+/// Fixed Wing Landing Pattern map visuals
+Item {
+    property var map    ///< Map control to place item in
+
+    property var _loiterPoint
+    property var _flightPath
+
+    Component.onCompleted: {
+        _flightPath = flightPathComponent.createObject(map)
+        _loiterPoint = loiterComponent.createObject(map)
+        map.addMapItem(_flightPath)
+        map.addMapItem(_loiterPoint)
+    }
+
+    Component.onDestruction: {
+        _loiterPoint.destroy()
+        _flightPath.destroy()
+    }
+
+    // Flight path
+    Component {
+        id: flightPathComponent
+
+        MapPolyline {
+            line.color: "white"
+            line.width: 2
+            path:       [ object.loiterCoordinate, object.exitCoordinate ]
+        }
+    }
+
+    // Loiter point
+    Component {
+        id: loiterComponent
+
+        MapQuickItem {
+            anchorPoint.x:  sourceItem.width  / 2
+            anchorPoint.y:  sourceItem.height / 2
+            coordinate:     object.loiterCoordinate
+
+            sourceItem:
+                MissionItemIndexLabel {
+                    label:      "L"
+                }
+        }
+    }
+}

src/MissionManager/FWLandingPattern.FactMetaData.json

@@ -1,2 +1,41 @@
 [
+{
+    "name":             "Landing distance",
+    "shortDescription": "Distance between landing and loiter points.",
+    "type":             "double",
+    "units":            "m",
+    "decimalPlaces":    1,
+    "defaultValue":     100.0
+},
+{
+    "name":             "Landing heading",
+    "shortDescription": "Heading from land point to loiter point.",
+    "type":             "double",
+    "units":            "deg",
+    "decimalPlaces":    0,
+    "defaultValue":     0.0
+},
+{
+    "name":             "Loiter altitude",
+    "shortDescription": "Altitude to loiter prior to landing.",
+    "type":             "double",
+    "units":            "m",
+    "decimalPlaces":    1,
+    "defaultValue":     40.0
+},
+{
+    "name":             "Loiter radius",
+    "shortDescription": "Loiter radius.",
+    "type":             "double",
+    "decimalPlaces":    1,
+    "min":              0.1,
+    "units":            "m",
+    "defaultValue":     75.0
+},
+{
+    "name":             "Clockwise loiter",
+    "shortDescription": "If true, loiter will be clockwise. False, loiter will be counter-clockwise.",
+    "type":             "bool",
+    "defaultValue":     true
+}
 ]

src/MissionManager/FixedWingLandingComplexItem.cc

@@ -19,23 +19,57 @@ QGC_LOGGING_CATEGORY(FixedWingLandingComplexItemLog, "FixedWingLandingComplexIte
 
 const char* FixedWingLandingComplexItem::jsonComplexItemTypeValue = "fwLandingPattern";
 
+const char* FixedWingLandingComplexItem::_loiterToLandDistanceName =    "Landing distance";
+const char* FixedWingLandingComplexItem::_landingHeadingName =          "Landing heading";
+const char* FixedWingLandingComplexItem::_loiterAltitudeName =          "Loiter altitude";
+const char* FixedWingLandingComplexItem::_loiterRadiusName =            "Loiter radius";
+const char* FixedWingLandingComplexItem::_loiterClockwiseName =         "Clockwise loiter";
+
 QMap<QString, FactMetaData*> FixedWingLandingComplexItem::_metaDataMap;
 
-FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObject* parent)
+FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QGeoCoordinate mapClickCoordinate, QObject* parent)
     : ComplexMissionItem(vehicle, parent)
     , _sequenceNumber(0)
     , _dirty(false)
+    , _loiterToLandDistanceFact (0, _loiterToLandDistanceName,  FactMetaData::valueTypeDouble)
+    , _loiterAltitudeFact       (0, _loiterAltitudeName,        FactMetaData::valueTypeDouble)
+    , _loiterRadiusFact         (0, _loiterRadiusName,          FactMetaData::valueTypeDouble)
+    , _loiterClockwiseFact      (0, _loiterClockwiseName,       FactMetaData::valueTypeBool)
+    , _landingHeadingFact       (0, _landingHeadingName,        FactMetaData::valueTypeDouble)
 {
     _editorQml = "qrc:/qml/FWLandingPatternEditor.qml";
 
     if (_metaDataMap.isEmpty()) {
         _metaDataMap = FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/FWLandingPattern.FactMetaData.json"), NULL /* metaDataParent */);
     }
+
+    _loiterToLandDistanceFact.setMetaData   (_metaDataMap[_loiterToLandDistanceName]);
+    _loiterAltitudeFact.setMetaData         (_metaDataMap[_loiterAltitudeName]);
+    _loiterRadiusFact.setMetaData           (_metaDataMap[_loiterRadiusName]);
+    _loiterClockwiseFact.setMetaData        (_metaDataMap[_loiterClockwiseName]);
+    _landingHeadingFact.setMetaData         (_metaDataMap[_landingHeadingName]);
+
+    _loiterToLandDistanceFact.setRawValue   (_loiterToLandDistanceFact.rawDefaultValue());
+    _loiterAltitudeFact.setRawValue         (_loiterAltitudeFact.rawDefaultValue());
+    _loiterRadiusFact.setRawValue           (_loiterRadiusFact.rawDefaultValue());
+    _loiterClockwiseFact.setRawValue        (_loiterClockwiseFact.rawDefaultValue());
+    _landingHeadingFact.setRawValue         (_landingHeadingFact.rawDefaultValue());
+
+    connect(&_loiterToLandDistanceFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_recalcLoiterPosition);
+    connect(&_landingHeadingFact,       &Fact::valueChanged, this, &FixedWingLandingComplexItem::_recalcLoiterPosition);
+
+    _textFieldFacts << QVariant::fromValue(&_loiterToLandDistanceFact) << QVariant::fromValue(&_loiterAltitudeFact) << QVariant::fromValue(&_loiterRadiusFact) << QVariant::fromValue(&_landingHeadingFact);
+
+    // Exit coordinate is our land point
+    _exitCoordinate = mapClickCoordinate;
+
+    _recalcLoiterPosition();
 }
 
 int FixedWingLandingComplexItem::lastSequenceNumber(void) const
 {
-    return _sequenceNumber;
+    // land start, loiter, land
+    return _sequenceNumber + 2;
 }
 
 void FixedWingLandingComplexItem::setCoordinate(const QGeoCoordinate& coordinate)
@@ -117,54 +151,46 @@ bool FixedWingLandingComplexItem::specifiesCoordinate(void) const
 
 QmlObjectListModel* FixedWingLandingComplexItem::getMissionItems(void) const
 {
-    // FIXME: Need real implementation
     QmlObjectListModel* pMissionItems = new QmlObjectListModel;
 
-#if 0
     int seqNum = _sequenceNumber;
-    for (int i=0; i<_gridPoints.count(); i++) {
-        QGeoCoordinate coord = _gridPoints[i].value<QGeoCoordinate>();
-        double altitude = _gridAltitudeFact.rawValue().toDouble();
 
     MissionItem* item = new MissionItem(seqNum++,                           // sequence number
-                                            MAV_CMD_NAV_WAYPOINT,           // MAV_CMD
+                                        MAV_CMD_DO_LAND_START,              // MAV_CMD
-                                            _gridAltitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL,  // MAV_FRAME
+                                        MAV_FRAME_GLOBAL_RELATIVE_ALT,      // MAV_FRAME
-                                            0.0, 0.0, 0.0, 0.0,             // param 1-4
+                                        0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,  // param 1-7
-                                            coord.latitude(),
-                                            coord.longitude(),
-                                            altitude,
                                         true,                               // autoContinue
                                         false,                              // isCurrentItem
                                         pMissionItems);                     // parent - allow delete on pMissionItems to delete everthing
     pMissionItems->append(item);
 
-        if (_cameraTrigger && i == 0) {
+    float loiterRadius = _loiterRadiusFact.rawValue().toDouble() * (_loiterClockwiseFact.rawValue().toBool() ? 1.0 : -1.0);
-            // Turn on camera
+    item = new MissionItem(seqNum++,
-            MissionItem* item = new MissionItem(seqNum++,                       // sequence number
+                           MAV_CMD_NAV_LOITER_TO_ALT,
-                                                MAV_CMD_DO_SET_CAM_TRIGG_DIST,  // MAV_CMD
+                           MAV_FRAME_GLOBAL_RELATIVE_ALT,
-                                                MAV_FRAME_MISSION,              // MAV_FRAME
+                           1.0,                             // Heading required = true
-                                                _cameraTriggerDistanceFact.rawValue().toDouble(),   // trigger distance
+                           loiterRadius,                    // Loiter radius
-                                                0.0, 0.0, 0.0, 0.0, 0.0, 0.0,   // param 2-7
+                           0.0,                             // param 3 - unused
+                           0.0,                             // Exit crosstrack - center of waypoint
+                           _loiterCoordinate.latitude(),
+                           _loiterCoordinate.longitude(),
+                           _loiterCoordinate.altitude(),
                            true,                            // autoContinue
                            false,                           // isCurrentItem
                            pMissionItems);                  // parent - allow delete on pMissionItems to delete everthing
     pMissionItems->append(item);
-        }
-    }
 
-    if (_cameraTrigger) {
+    item = new MissionItem(seqNum++,
-        // Turn off camera
+                           MAV_CMD_NAV_LAND,
-        MissionItem* item = new MissionItem(seqNum++,                       // sequence number
+                           MAV_FRAME_GLOBAL_RELATIVE_ALT,
-                                            MAV_CMD_DO_SET_CAM_TRIGG_DIST,  // MAV_CMD
+                           0.0, 0.0, 0.0, 0.0,                 // param 1-4
-                                            MAV_FRAME_MISSION,              // MAV_FRAME
+                           _exitCoordinate.latitude(),
-                                            0.0,                            // trigger distance
+                           _exitCoordinate.longitude(),
-                                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0,   // param 2-7
+                           0.0,                                // altitude
                            true,                               // autoContinue
                            false,                              // isCurrentItem
                            pMissionItems);                     // parent - allow delete on pMissionItems to delete everthing
     pMissionItems->append(item);
-    }
-#endif
 
     return pMissionItems;
 }
@@ -180,3 +206,32 @@ void FixedWingLandingComplexItem::setCruiseSpeed(double cruiseSpeed)
     // FIXME: Need real implementation
     Q_UNUSED(cruiseSpeed);
 }
+
+void FixedWingLandingComplexItem::_recalcLoiterPosition(void)
+{
+    double          north, east, down;
+    QGeoCoordinate  tangentOrigin = _exitCoordinate;
+
+    convertGeoToNed(_exitCoordinate, tangentOrigin, &north, &east, &down);
+
+    QPointF originPoint(east, north);
+    north += _loiterToLandDistanceFact.rawValue().toDouble();
+    QPointF loiterPoint(east, north);
+    QPointF rotatedLoiterPoint = _rotatePoint(loiterPoint, originPoint, _landingHeadingFact.rawValue().toDouble());
+
+    convertNedToGeo(rotatedLoiterPoint.y(), rotatedLoiterPoint.x(), down, tangentOrigin, &_loiterCoordinate);
+
+    emit loiterCoordinateChanged(_loiterCoordinate);
+    setCoordinate(_loiterCoordinate);
+}
+
+QPointF FixedWingLandingComplexItem::_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;
+}

src/MissionManager/FixedWingLandingComplexItem.h

@@ -22,7 +22,13 @@ class FixedWingLandingComplexItem : public ComplexMissionItem
     Q_OBJECT
 
 public:
-    FixedWingLandingComplexItem(Vehicle* vehicle, QObject* parent = NULL);
+    FixedWingLandingComplexItem(Vehicle* vehicle, QGeoCoordinate mapClickCoordinate, QObject* parent = NULL);
+
+    Q_PROPERTY(QVariantList     textFieldFacts      MEMBER  _textFieldFacts     CONSTANT)
+    Q_PROPERTY(Fact*            loiterClockwise     READ    loiterClockwise     CONSTANT)
+    Q_PROPERTY(QGeoCoordinate   loiterCoordinate    MEMBER  _loiterCoordinate   NOTIFY loiterCoordinateChanged)
+
+    Fact* loiterClockwise(void) { return &_loiterClockwiseFact; }
 
     // Overrides from ComplexMissionItem
 
@@ -32,7 +38,7 @@ public:
     bool                load                (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final;
     double              greatestDistanceTo  (const QGeoCoordinate &other) const final;
     void                setCruiseSpeed      (double cruiseSpeed) final;
-    QString             mapVisualQML        (void) const final { return QString(); }
+    QString             mapVisualQML        (void) const final { return QStringLiteral("FWLandingPatternMapVisual.qml"); }
 
     // Overrides from VisualMissionItem
 
@@ -60,18 +66,36 @@ public:
     static const char* jsonComplexItemTypeValue;
 
 signals:
+    void loiterCoordinateChanged(QGeoCoordinate coordinate);
 
 private slots:
+    void _recalcLoiterPosition(void);
 
 private:
     void _setExitCoordinate(const QGeoCoordinate& coordinate);
+    QPointF _rotatePoint(const QPointF& point, const QPointF& origin, double angle);
 
     int             _sequenceNumber;
     bool            _dirty;
     QGeoCoordinate  _coordinate;
     QGeoCoordinate  _exitCoordinate;
+    QGeoCoordinate  _loiterCoordinate;
+
+    Fact            _loiterToLandDistanceFact;
+    Fact            _loiterAltitudeFact;
+    Fact            _loiterRadiusFact;
+    Fact            _loiterClockwiseFact;
+    Fact            _landingHeadingFact;
 
     static QMap<QString, FactMetaData*> _metaDataMap;
+
+    QVariantList _textFieldFacts;
+
+    static const char* _loiterToLandDistanceName;
+    static const char* _loiterAltitudeName;
+    static const char* _loiterRadiusName;
+    static const char* _loiterClockwiseName;
+    static const char* _landingHeadingName;
 };
 
 #endif

src/MissionManager/MissionController.cc

@@ -226,14 +226,14 @@ int MissionController::insertComplexMissionItem(QString itemName, QGeoCoordinate
     int sequenceNumber = _nextSequenceNumber();
     if (itemName == _surveyMissionItemName) {
         newItem = new SurveyMissionItem(_activeVehicle, _visualItems);
+        newItem->setCoordinate(mapCenterCoordinate);
     } else if (itemName == _fwLandingMissionItemName) {
-        newItem = new FixedWingLandingComplexItem(_activeVehicle, _visualItems);
+        newItem = new FixedWingLandingComplexItem(_activeVehicle, mapCenterCoordinate, _visualItems);
     } else {
         qWarning() << "Internal error: Unknown complex item:" << itemName;
         return sequenceNumber;
     }
     newItem->setSequenceNumber(sequenceNumber);
-    newItem->setCoordinate(mapCenterCoordinate);
     _initVisualItem(newItem);
 
     _visualItems->insert(i, newItem);