Merge pull request #4709 from DonLakeFlyer/FWLandingPersistence

Support Fixed Wing Landing Pattern file persistence

src/MissionManager/FixedWingLandingComplexItem.cc

@@ -25,6 +25,13 @@ const char* FixedWingLandingComplexItem::_loiterAltitudeName =          "Loiter
 const char* FixedWingLandingComplexItem::_loiterRadiusName =            "Loiter radius";
 const char* FixedWingLandingComplexItem::_landingAltitudeName =         "Landing altitude";
 
+const char* FixedWingLandingComplexItem::_jsonLoiterCoordinateKey =         "loiterCoordinate";
+const char* FixedWingLandingComplexItem::_jsonLoiterRadiusKey =             "loiterRadius";
+const char* FixedWingLandingComplexItem::_jsonLoiterClockwiseKey =          "loiterClockwise";
+const char* FixedWingLandingComplexItem::_jsonLoiterAltitudeRelativeKey =   "loiterAltitudeRelative";
+const char* FixedWingLandingComplexItem::_jsonLandingCoordinateKey =        "landCoordinate";
+const char* FixedWingLandingComplexItem::_jsonLandingAltitudeRelativeKey =  "landAltitudeRelative";
+
 QMap<QString, FactMetaData*> FixedWingLandingComplexItem::_metaDataMap;
 
 FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObject* parent)
@@ -60,6 +67,8 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObje
     _landingHeadingFact.setRawValue         (_landingHeadingFact.rawDefaultValue());
     _landingAltitudeFact.setRawValue        (_landingAltitudeFact.rawDefaultValue());
 
+    connect(&_loiterAltitudeFact,       &Fact::valueChanged,                                    this, &FixedWingLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact);
+    connect(&_landingAltitudeFact,       &Fact::valueChanged,                                   this, &FixedWingLandingComplexItem::_updateLandingCoodinateAltitudeFromFact);
     connect(&_loiterToLandDistanceFact, &Fact::valueChanged,                                    this, &FixedWingLandingComplexItem::_recalcLoiterCoordFromFacts);
     connect(&_landingHeadingFact,       &Fact::valueChanged,                                    this, &FixedWingLandingComplexItem::_recalcLoiterCoordFromFacts);
     connect(this,                       &FixedWingLandingComplexItem::loiterCoordinateChanged,  this, &FixedWingLandingComplexItem::_recalcFactsFromCoords);
@@ -86,7 +95,23 @@ void FixedWingLandingComplexItem::save(QJsonObject& saveObject) const
     saveObject[VisualMissionItem::jsonTypeKey] =                VisualMissionItem::jsonTypeComplexItemValue;
     saveObject[ComplexMissionItem::jsonComplexItemTypeKey] =    jsonComplexItemTypeValue;
 
-    // FIXME: Need real implementation
+    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[_jsonLoiterClockwiseKey] =           _loiterClockwise;
+    saveObject[_jsonLoiterAltitudeRelativeKey] =    _loiterAltitudeRelative;
+    saveObject[_jsonLandingAltitudeRelativeKey] =   _landingAltitudeRelative;
 }
 
 void FixedWingLandingComplexItem::setSequenceNumber(int sequenceNumber)
@@ -100,32 +125,57 @@ void FixedWingLandingComplexItem::setSequenceNumber(int sequenceNumber)
 
 bool FixedWingLandingComplexItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString)
 {
-    // FIXME: Need real implementation
-    Q_UNUSED(complexObject);
-    Q_UNUSED(sequenceNumber);
+    QList<JsonHelper::KeyValidateInfo> 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 },
+        { _jsonLoiterAltitudeRelativeKey,               QJsonValue::Bool,   true },
+        { _jsonLandingCoordinateKey,                    QJsonValue::Array,  true },
+        { _jsonLandingAltitudeRelativeKey,              QJsonValue::Bool,   true },
+    };
+    if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) {
+        return false;
+    }
 
-    errorString = "NYI";
-    return false;
-}
+    QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString();
+    QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString();
+    if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonComplexItemTypeValue) {
+        errorString = tr("QGroundControl does not support loading this complex mission item type: %1:2").arg(itemType).arg(complexType);
+        return false;
+    }
 
-double FixedWingLandingComplexItem::greatestDistanceTo(const QGeoCoordinate &other) const
-{
-    // FIXME: Need real implementation
-    Q_UNUSED(other);
+    setSequenceNumber(sequenceNumber);
 
-    double greatestDistance = 0.0;
+    QGeoCoordinate coordinate;
+    if (!JsonHelper::loadGeoCoordinate(complexObject[_jsonLoiterCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) {
+        return false;
+    }
+    _loiterCoordinate = coordinate;
+    _loiterAltitudeFact.setRawValue(coordinate.altitude());
 
-#if 0
-    for (int i=0; i<_gridPoints.count(); i++) {
-        QGeoCoordinate currentCoord = _gridPoints[i].value<QGeoCoordinate>();
-        double distance = currentCoord.distanceTo(other);
-        if (distance > greatestDistance) {
-            greatestDistance = distance;
-        }
+    if (!JsonHelper::loadGeoCoordinate(complexObject[_jsonLandingCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) {
+        return false;
     }
-#endif
+    _landingCoordinate = coordinate;
+    _landingAltitudeFact.setRawValue(coordinate.altitude());
+
+    _loiterRadiusFact.setRawValue(complexObject[_jsonLoiterRadiusKey].toDouble());
+    _loiterClockwise  = complexObject[_jsonLoiterClockwiseKey].toBool();
+    _loiterAltitudeRelative = complexObject[_jsonLoiterAltitudeRelativeKey].toBool();
+    _landingAltitudeRelative = complexObject[_jsonLandingAltitudeRelativeKey].toBool();
 
-    return greatestDistance;
+    _landingCoordSet = true;
+    _recalcFactsFromCoords();
+
+    return true;
+}
+
+double FixedWingLandingComplexItem::greatestDistanceTo(const QGeoCoordinate &other) const
+{
+    return qMax(_loiterCoordinate.distanceTo(other),_landingCoordinate.distanceTo(other));
 }
 
 bool FixedWingLandingComplexItem::specifiesCoordinate(void) const
@@ -181,13 +231,12 @@ QmlObjectListModel* FixedWingLandingComplexItem::getMissionItems(void) const
 
 double FixedWingLandingComplexItem::complexDistance(void) const
 {
-    // FIXME: Need real implementation
-    return 0;
+    return _loiterCoordinate.distanceTo(_landingCoordinate);
 }
 
 void FixedWingLandingComplexItem::setCruiseSpeed(double cruiseSpeed)
 {
-    // FIXME: Need real implementation
+    // We don't care about cruise speed
     Q_UNUSED(cruiseSpeed);
 }
 
@@ -296,3 +345,13 @@ void FixedWingLandingComplexItem::_recalcFactsFromCoords(void)
         _ignoreRecalcSignals = false;
     }
 }
+
+void FixedWingLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact(void)
+{
+    _loiterCoordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble());
+}
+
+void FixedWingLandingComplexItem::_updateLandingCoodinateAltitudeFromFact(void)
+{
+    _landingCoordinate.setAltitude(_landingAltitudeFact.rawValue().toDouble());
+}

src/MissionManager/FixedWingLandingComplexItem.h

@@ -93,6 +93,8 @@ signals:
 private slots:
     void _recalcLoiterCoordFromFacts(void);
     void _recalcFactsFromCoords(void);
+    void _updateLoiterCoodinateAltitudeFromFact(void);
+    void _updateLandingCoodinateAltitudeFromFact(void);
 
 private:
     QPointF _rotatePoint(const QPointF& point, const QPointF& origin, double angle);
@@ -121,6 +123,13 @@ private:
     static const char* _loiterRadiusName;
     static const char* _landingHeadingName;
     static const char* _landingAltitudeName;
+
+    static const char* _jsonLoiterCoordinateKey;
+    static const char* _jsonLoiterRadiusKey;
+    static const char* _jsonLoiterClockwiseKey;
+    static const char* _jsonLoiterAltitudeRelativeKey;
+    static const char* _jsonLandingCoordinateKey;
+    static const char* _jsonLandingAltitudeRelativeKey;
 };
 
 #endif

src/MissionManager/MissionController.cc

@@ -488,6 +488,16 @@ bool MissionController::_loadJsonMissionFileV2(Vehicle* vehicle, const QJsonObje
                 qCDebug(MissionControllerLog) << "Survey load complete: nextSequenceNumber" << nextSequenceNumber;
                 visualItems->append(surveyItem);
                 complexItems->append(surveyItem);
+            } else if (complexItemType == FixedWingLandingComplexItem::jsonComplexItemTypeValue) {
+                    qCDebug(MissionControllerLog) << "Loading Fixed Wing Landing Pattern: nextSequenceNumber" << nextSequenceNumber;
+                    FixedWingLandingComplexItem* landingItem = new FixedWingLandingComplexItem(vehicle, visualItems);
+                    if (!landingItem->load(itemObject, nextSequenceNumber++, errorString)) {
+                        return false;
+                    }
+                    nextSequenceNumber = landingItem->lastSequenceNumber() + 1;
+                    qCDebug(MissionControllerLog) << "FW Landing Pattern load complete: nextSequenceNumber" << nextSequenceNumber;
+                    visualItems->append(landingItem);
+                    complexItems->append(landingItem);
             } else {
                 errorString = tr("Unsupported complex item type: %1").arg(complexItemType);
             }