From 14474c94a75d2d0a265292c4b2aa96c376ad502a Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sat, 4 Mar 2017 19:28:58 -0800 Subject: [PATCH] Support Fixed Wing Landing Pattern file persistence --- .../FixedWingLandingComplexItem.cc | 107 ++++++++++++++---- .../FixedWingLandingComplexItem.h | 9 ++ src/MissionManager/MissionController.cc | 10 ++ 3 files changed, 102 insertions(+), 24 deletions(-) diff --git a/src/MissionManager/FixedWingLandingComplexItem.cc b/src/MissionManager/FixedWingLandingComplexItem.cc index 0ef549d8a..68b08916a 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.cc +++ b/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 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 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(); - 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()); +} diff --git a/src/MissionManager/FixedWingLandingComplexItem.h b/src/MissionManager/FixedWingLandingComplexItem.h index a9be9ccfc..e20532e9d 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.h +++ b/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 diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 64ec387b1..bcf3377a2 100644 --- a/src/MissionManager/MissionController.cc +++ b/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); } -- 2.22.0