From b15eba775cbf7f6fc80bd52a09d43874fc8ab811 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 2 Jan 2017 10:18:01 -0800 Subject: [PATCH] New V2 mission file format --- src/JsonHelper.cc | 72 ++-- src/JsonHelper.h | 12 +- src/MissionManager/ComplexMissionItem.cc | 3 +- src/MissionManager/ComplexMissionItem.h | 6 +- src/MissionManager/GeoFenceController.cc | 30 +- src/MissionManager/MissionController.cc | 208 ++++++++--- src/MissionManager/MissionController.h | 13 +- src/MissionManager/MissionItem.cc | 125 +++++-- src/MissionManager/MissionItem.h | 21 +- src/MissionManager/MissionItemTest.cc | 406 ++++----------------- src/MissionManager/MissionItemTest.h | 5 +- src/MissionManager/RallyPointController.cc | 11 +- src/MissionManager/SimpleMissionItem.cc | 18 +- src/MissionManager/SimpleMissionItem.h | 2 +- src/MissionManager/SurveyMissionItem.cc | 147 ++++---- src/MissionManager/SurveyMissionItem.h | 8 +- src/MissionManager/VisualMissionItem.cc | 4 + src/MissionManager/VisualMissionItem.h | 4 + 18 files changed, 540 insertions(+), 555 deletions(-) diff --git a/src/JsonHelper.cc b/src/JsonHelper.cc index b15220ca9..e29a94a89 100644 --- a/src/JsonHelper.cc +++ b/src/JsonHelper.cc @@ -97,10 +97,12 @@ void JsonHelper::saveGeoCoordinate(const QGeoCoordinate& coordinate, bool JsonHelper::validateKeyTypes(const QJsonObject& jsonObject, const QStringList& keys, const QList& types, QString& errorString) { - for (int i=0; i supportedMajorVersion || (fileMajorVersion == supportedMajorVersion && fileMinorVersion > supportedMinorVersion)) { - errorString = QObject::tr("File version (%1.%2) is larger than current supported version (%3.%4)").arg(fileMajorVersion).arg(fileMinorVersion).arg(supportedMajorVersion).arg(supportedMinorVersion); + if (version > maxSupportedVersion) { + errorString = QObject::tr("File version %1 is newer than current supported version %2").arg(version).arg(maxSupportedVersion); return false; } @@ -284,3 +284,27 @@ bool JsonHelper::validateKeys(const QJsonObject& jsonObject, const QList typeList; - keyList << jsonSimpleItemsKey << _jsonVersionKey << _jsonGroundStationKey << _jsonMavAutopilotKey << _jsonComplexItemsKey << _jsonPlannedHomePositionKey; - typeList << QJsonValue::Array << QJsonValue::String << QJsonValue::String << QJsonValue::Double << QJsonValue::Array << QJsonValue::Object; - if (!JsonHelper::validateKeyTypes(json, keyList, typeList, errorString)) { - return false; - } -#endif - - // Version check - if (json[JsonHelper::jsonVersionKey].toString() != "1.0") { - errorString = QStringLiteral("QGroundControl does not support this file version"); + int fileVersion; + if (!JsonHelper::validateQGCJsonFile(json, + _jsonFileTypeValue, // expected file type + 1, // minimum supported version + 1, // maximum supported version + fileVersion, + errorString)) { return false; } @@ -269,7 +255,7 @@ void GeoFenceController::saveToFile(const QString& filename) QJsonObject fenceFileObject; // top level json object fenceFileObject[JsonHelper::jsonFileTypeKey] = _jsonFileTypeValue; - fenceFileObject[JsonHelper::jsonVersionKey] = QStringLiteral("1.0"); + fenceFileObject[JsonHelper::jsonVersionKey] = 1; fenceFileObject[JsonHelper::jsonGroundStationKey] = JsonHelper::jsonGroundStationValue; QStringList paramNames; diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 03bf70e4e..760bbc248 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -27,12 +27,19 @@ QGC_LOGGING_CATEGORY(MissionControllerLog, "MissionControllerLog") -const char* MissionController::jsonSimpleItemsKey = "items"; const char* MissionController::_settingsGroup = "MissionController"; -const char* MissionController::_jsonMavAutopilotKey = "MAV_AUTOPILOT"; -const char* MissionController::_jsonComplexItemsKey = "complexItems"; +const char* MissionController::_jsonFileTypeValue = "Mission"; +const char* MissionController::_jsonItemsKey = "items"; const char* MissionController::_jsonPlannedHomePositionKey = "plannedHomePosition"; +const char* MissionController::_jsonFirmwareTypeKey = "firmwareType"; +const char* MissionController::_jsonParamsKey = "params"; + +// Deprecated V1 format keys +const char* MissionController::_jsonComplexItemsKey = "complexItems"; +const char* MissionController::_jsonMavAutopilotKey = "MAV_AUTOPILOT"; + +const int MissionController::_missionFileVersion = 2; MissionController::MissionController(QObject *parent) : PlanElementController(parent) @@ -252,28 +259,40 @@ bool MissionController::_loadJsonMissionFile(const QByteArray& bytes, QmlObjectL errorString = jsonParseError.errorString(); return false; } - QJsonObject json = jsonDoc.object(); - // Check for required keys - QStringList requiredKeys; - requiredKeys << JsonHelper::jsonVersionKey << _jsonPlannedHomePositionKey; - if (!JsonHelper::validateRequiredKeys(json, requiredKeys, errorString)) { - return false; + // V1 file format has no file type key and version key is string. Convert to new format. + if (!json.contains(JsonHelper::jsonFileTypeKey)) { + json[JsonHelper::jsonFileTypeKey] = _jsonFileTypeValue; } - // Validate base key types - QStringList keyList; - QList typeList; - keyList << jsonSimpleItemsKey << JsonHelper::jsonVersionKey << JsonHelper::jsonGroundStationKey << _jsonMavAutopilotKey << _jsonComplexItemsKey << _jsonPlannedHomePositionKey; - typeList << QJsonValue::Array << QJsonValue::String << QJsonValue::String << QJsonValue::Double << QJsonValue::Array << QJsonValue::Object; - if (!JsonHelper::validateKeyTypes(json, keyList, typeList, errorString)) { + int fileVersion; + if (!JsonHelper::validateQGCJsonFile(json, + _jsonFileTypeValue, // expected file type + 1, // minimum supported version + 2, // maximum supported version + fileVersion, + errorString)) { return false; } - // Version check - if (json[JsonHelper::jsonVersionKey].toString() != "1.0") { - errorString = QStringLiteral("QGroundControl does not support this file version"); + if (fileVersion == 1) { + return _loadJsonMissionFileV1(json, visualItems, complexItems, errorString); + } else { + return _loadJsonMissionFileV2(json, visualItems, complexItems, errorString); + } +} + +bool MissionController::_loadJsonMissionFileV1(const QJsonObject& json, QmlObjectListModel* visualItems, QmlObjectListModel* complexItems, QString& errorString) +{ + // Validate root object keys + QList rootKeyInfoList = { + { _jsonPlannedHomePositionKey, QJsonValue::Object, true }, + { _jsonItemsKey, QJsonValue::Array, true }, + { _jsonMavAutopilotKey, QJsonValue::Double, true }, + { _jsonComplexItemsKey, QJsonValue::Array, true }, + }; + if (!JsonHelper::validateKeys(json, rootKeyInfoList, errorString)) { return false; } @@ -289,8 +308,8 @@ bool MissionController::_loadJsonMissionFile(const QByteArray& bytes, QmlObjectL } SurveyMissionItem* item = new SurveyMissionItem(_activeVehicle, this); - if (item->load(itemValue.toObject(), errorString)) { - qCDebug(MissionControllerLog) << "Json load: complex item start:stop" << item->sequenceNumber() << item->lastSequenceNumber(); + const QJsonObject itemObject = itemValue.toObject(); + if (item->load(itemObject, itemObject["id"].toInt(), errorString)) { complexItems->append(item); } else { return false; @@ -302,7 +321,7 @@ bool MissionController::_loadJsonMissionFile(const QByteArray& bytes, QmlObjectL int nextSimpleItemIndex= 0; int nextComplexItemIndex= 0; int nextSequenceNumber = 1; // Start with 1 since home is in 0 - QJsonArray itemArray(json[jsonSimpleItemsKey].toArray()); + QJsonArray itemArray(json[_jsonItemsKey].toArray()); qCDebug(MissionControllerLog) << "Json load: simple item loop start simpleItemCount:ComplexItemCount" << itemArray.count() << complexItems->count(); do { @@ -330,8 +349,9 @@ bool MissionController::_loadJsonMissionFile(const QByteArray& bytes, QmlObjectL return false; } + const QJsonObject itemObject = itemValue.toObject(); SimpleMissionItem* item = new SimpleMissionItem(_activeVehicle, this); - if (item->load(itemValue.toObject(), errorString)) { + if (item->load(itemObject, itemObject["id"].toInt(), errorString)) { qCDebug(MissionControllerLog) << "Json load: adding simple item expectedSequence:actualSequence" << nextSequenceNumber << item->sequenceNumber(); visualItems->append(item); } else { @@ -345,7 +365,7 @@ bool MissionController::_loadJsonMissionFile(const QByteArray& bytes, QmlObjectL if (json.contains(_jsonPlannedHomePositionKey)) { SimpleMissionItem* item = new SimpleMissionItem(_activeVehicle, this); - if (item->load(json[_jsonPlannedHomePositionKey].toObject(), errorString)) { + if (item->load(json[_jsonPlannedHomePositionKey].toObject(), 0, errorString)) { visualItems->insert(0, item); } else { return false; @@ -357,6 +377,117 @@ bool MissionController::_loadJsonMissionFile(const QByteArray& bytes, QmlObjectL return true; } +bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjectListModel* visualItems, QmlObjectListModel* complexItems, QString& errorString) +{ + // Validate root object keys + QList rootKeyInfoList = { + { _jsonPlannedHomePositionKey, QJsonValue::Array, true }, + { _jsonItemsKey, QJsonValue::Array, true }, + { _jsonFirmwareTypeKey, QJsonValue::Double, true }, + }; + if (!JsonHelper::validateKeys(json, rootKeyInfoList, errorString)) { + return false; + } + + qCDebug(MissionControllerLog) << "MissionController::_loadJsonMissionFileV2 itemCount:" << json[_jsonItemsKey].toArray().count(); + + // Planned home position + QGeoCoordinate homeCoordinate; + if (!JsonHelper::loadGeoCoordinate(json[_jsonPlannedHomePositionKey], true /* altitudeRequired */, homeCoordinate, errorString)) { + return false; + } + SimpleMissionItem* homeItem = new SimpleMissionItem(_activeVehicle, this); + homeItem->setCoordinate(homeCoordinate); + visualItems->insert(0, homeItem); + qCDebug(MissionControllerLog) << "plannedHomePosition" << homeCoordinate; + + // Read mission items + + int nextSequenceNumber = 1; // Start with 1 since home is in 0 + const QJsonArray rgMissionItems(json[_jsonItemsKey].toArray()); + for (int i=0; i itemKeyInfoList = { + { VisualMissionItem::jsonTypeKey, QJsonValue::String, true }, + }; + if (!JsonHelper::validateKeys(itemObject, itemKeyInfoList, errorString)) { + return false; + } + QString itemType = itemObject[VisualMissionItem::jsonTypeKey].toString(); + + if (itemType == VisualMissionItem::jsonTypeSimpleItemValue) { + qCDebug(MissionControllerLog) << "Loading MISSION_ITEM: nextSequenceNumber" << nextSequenceNumber; + SimpleMissionItem* simpleItem = new SimpleMissionItem(_activeVehicle, this); + if (simpleItem->load(itemObject, nextSequenceNumber++, errorString)) { + visualItems->append(simpleItem); + } else { + return false; + } + } else if (itemType == VisualMissionItem::jsonTypeComplexItemValue) { + QList complexItemKeyInfoList = { + { ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true }, + }; + if (!JsonHelper::validateKeys(itemObject, complexItemKeyInfoList, errorString)) { + return false; + } + QString complexItemType = itemObject[ComplexMissionItem::jsonComplexItemTypeKey].toString(); + + if (complexItemType == SurveyMissionItem::jsonComplexItemTypeValue) { + qCDebug(MissionControllerLog) << "Loading Survey: nextSequenceNumber" << nextSequenceNumber; + SurveyMissionItem* surveyItem = new SurveyMissionItem(_activeVehicle, this); + if (!surveyItem->load(itemObject, nextSequenceNumber++, errorString)) { + return false; + } + nextSequenceNumber = surveyItem->lastSequenceNumber() + 1; + qCDebug(MissionControllerLog) << "Survey load complete: nextSequenceNumber" << nextSequenceNumber; + visualItems->append(surveyItem); + complexItems->append(surveyItem); + } else { + errorString = tr("Unsupported complex item type: %1").arg(complexItemType); + } + } else { + errorString = tr("Unknown item type: %1").arg(itemType); + return false; + } + } + + // Fix up the DO_JUMP commands jump sequence number by finding the item with the matching doJumpId + for (int i=0; icount(); i++) { + if (visualItems->value(i)->isSimpleItem()) { + SimpleMissionItem* doJumpItem = visualItems->value(i); + if (doJumpItem->command() == MAV_CMD_DO_JUMP) { + bool found = false; + int findDoJumpId = doJumpItem->missionItem().param1(); + for (int j=0; jcount(); j++) { + if (visualItems->value(j)->isSimpleItem()) { + SimpleMissionItem* targetItem = visualItems->value(j); + if (targetItem->missionItem().doJumpId() == findDoJumpId) { + doJumpItem->missionItem().setParam1(targetItem->sequenceNumber()); + found = true; + break; + } + } + } + if (!found) { + errorString = tr("Could not find doJumpId: %1").arg(findDoJumpId); + return false; + } + } + } + } + + return true; +} + bool MissionController::_loadTextMissionFile(QTextStream& stream, QmlObjectListModel* visualItems, QString& errorString) { bool addPlannedHomePosition = false; @@ -498,10 +629,8 @@ void MissionController::saveToFile(const QString& filename) qgcApp()->showMessage(file.errorString()); } else { QJsonObject missionFileObject; // top level json object - QJsonArray simpleItemsObject; - QJsonArray complexItemsObject; - missionFileObject[JsonHelper::jsonVersionKey] = "1.0"; + missionFileObject[JsonHelper::jsonVersionKey] = _missionFileVersion; missionFileObject[JsonHelper::jsonGroundStationKey] = JsonHelper::jsonGroundStationValue; MAV_AUTOPILOT firmwareType = MAV_AUTOPILOT_GENERIC; @@ -514,35 +643,28 @@ void MissionController::saveToFile(const QString& filename) QSettings settings; firmwareType = (MAV_AUTOPILOT)settings.value("OfflineEditingFirmwareType", MAV_AUTOPILOT_ARDUPILOTMEGA).toInt(); } - missionFileObject[_jsonMavAutopilotKey] = firmwareType; + missionFileObject[_jsonFirmwareTypeKey] = firmwareType; // Save planned home position - QJsonObject homePositionObject; SimpleMissionItem* homeItem = qobject_cast(_visualItems->get(0)); - if (homeItem) { - homeItem->missionItem().save(homePositionObject); - } else { + if (!homeItem) { qgcApp()->showMessage(QStringLiteral("Internal error: VisualMissionItem at index 0 not SimpleMissionItem")); return; } - missionFileObject[_jsonPlannedHomePositionKey] = homePositionObject; + QJsonValue coordinateValue; + JsonHelper::saveGeoCoordinate(homeItem->coordinate(), true /* writeAltitude */, coordinateValue); + missionFileObject[_jsonPlannedHomePositionKey] = coordinateValue; // Save the visual items + QJsonArray rgMissionItems; for (int i=1; i<_visualItems->count(); i++) { QJsonObject itemObject; VisualMissionItem* visualItem = qobject_cast(_visualItems->get(i)); visualItem->save(itemObject); - - if (visualItem->isSimpleItem()) { - simpleItemsObject.append(itemObject); - } else { - complexItemsObject.append(itemObject); - } + rgMissionItems.append(itemObject); } - - missionFileObject[jsonSimpleItemsKey] = simpleItemsObject; - missionFileObject[_jsonComplexItemsKey] = complexItemsObject; + missionFileObject[_jsonItemsKey] = rgMissionItems; QJsonDocument saveDoc(missionFileObject); file.write(saveDoc.toJson()); @@ -657,7 +779,7 @@ void MissionController::_recalcWaypointLines(void) // Create a new segment and wire update notifiers auto linevect = new CoordinateVector(lastCoordinateItem->isSimpleItem() ? lastCoordinateItem->coordinate() : lastCoordinateItem->exitCoordinate(), item->coordinate(), this); auto originNotifier = lastCoordinateItem->isSimpleItem() ? &VisualMissionItem::coordinateChanged : &VisualMissionItem::exitCoordinateChanged, - endNotifier = &VisualMissionItem::coordinateChanged; + endNotifier = &VisualMissionItem::coordinateChanged; // Use signals/slots to update the coordinate endpoints connect(lastCoordinateItem, originNotifier, linevect, &CoordinateVector::setCoordinate1); connect(item, endNotifier, linevect, &CoordinateVector::setCoordinate2); @@ -883,7 +1005,7 @@ void MissionController::_recalcSequence(void) ComplexMissionItem* complexItem = qobject_cast(item); if (complexItem) { - sequenceNumber = complexItem->lastSequenceNumber() + 1; + sequenceNumber = complexItem->lastSequenceNumber() + 1; } else { qWarning() << "isSimpleItem == false, yet not ComplexMissionItem"; } diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index fc1f288d3..42e495e48 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -88,7 +88,6 @@ public: void setCruiseDistance (double cruiseDistance ); void setHoverDistance (double hoverDistance ); - static const char* jsonSimpleItemsKey; ///< Key for simple items in a json file signals: void plannedHomePositionChanged(QGeoCoordinate plannedHomePosition); @@ -129,6 +128,8 @@ private: double _normalizeLat(double lat); double _normalizeLon(double lon); bool _loadJsonMissionFile(const QByteArray& bytes, QmlObjectListModel* visualItems, QmlObjectListModel* complexItems, QString& errorString); + bool _loadJsonMissionFileV1(const QJsonObject& json, QmlObjectListModel* visualItems, QmlObjectListModel* complexItems, QString& errorString); + bool _loadJsonMissionFileV2(const QJsonObject& json, QmlObjectListModel* visualItems, QmlObjectListModel* complexItems, QString& errorString); bool _loadTextMissionFile(QTextStream& stream, QmlObjectListModel* visualItems, QString& errorString); int _nextSequenceNumber(void); @@ -150,9 +151,17 @@ private: double _hoverDistance; static const char* _settingsGroup; + static const char* _jsonFileTypeValue; + static const char* _jsonFirmwareTypeKey; + static const char* _jsonItemsKey; + static const char* _jsonPlannedHomePositionKey; + static const char* _jsonParamsKey; + + // Deprecated V1 format keys static const char* _jsonMavAutopilotKey; static const char* _jsonComplexItemsKey; - static const char* _jsonPlannedHomePositionKey; + + static const int _missionFileVersion; }; #endif diff --git a/src/MissionManager/MissionItem.cc b/src/MissionManager/MissionItem.cc index 8b7df76ce..0a50253a9 100644 --- a/src/MissionManager/MissionItem.cc +++ b/src/MissionManager/MissionItem.cc @@ -15,22 +15,25 @@ #include "FirmwarePluginManager.h" #include "QGCApplication.h" #include "JsonHelper.h" +#include "VisualMissionItem.h" -const char* MissionItem::_itemType = "missionItem"; -const char* MissionItem::_jsonTypeKey = "type"; -const char* MissionItem::_jsonIdKey = "id"; const char* MissionItem::_jsonFrameKey = "frame"; const char* MissionItem::_jsonCommandKey = "command"; +const char* MissionItem::_jsonAutoContinueKey = "autoContinue"; +const char* MissionItem::_jsonCoordinateKey = "coordinate"; +const char* MissionItem::_jsonParamsKey = "params"; +const char* MissionItem::_jsonDoJumpIdKey = "doJumpId"; + +// Deprecated V1 format keys const char* MissionItem::_jsonParam1Key = "param1"; const char* MissionItem::_jsonParam2Key = "param2"; const char* MissionItem::_jsonParam3Key = "param3"; const char* MissionItem::_jsonParam4Key = "param4"; -const char* MissionItem::_jsonAutoContinueKey = "autoContinue"; -const char* MissionItem::_jsonCoordinateKey = "coordinate"; MissionItem::MissionItem(QObject* parent) : QObject(parent) , _sequenceNumber(0) + , _doJumpId(-1) , _isCurrentItem(false) , _autoContinueFact (0, "AutoContinue", FactMetaData::valueTypeUint32) , _commandFact (0, "", FactMetaData::valueTypeUint32) @@ -65,6 +68,7 @@ MissionItem::MissionItem(int sequenceNumber, QObject* parent) : QObject(parent) , _sequenceNumber(sequenceNumber) + , _doJumpId(-1) , _isCurrentItem(isCurrentItem) , _commandFact (0, "", FactMetaData::valueTypeUint32) , _frameFact (0, "", FactMetaData::valueTypeUint32) @@ -96,6 +100,7 @@ MissionItem::MissionItem(int sequenceNumber, MissionItem::MissionItem(const MissionItem& other, QObject* parent) : QObject(parent) , _sequenceNumber(0) + , _doJumpId(-1) , _isCurrentItem(false) , _commandFact (0, "", FactMetaData::valueTypeUint32) , _frameFact (0, "", FactMetaData::valueTypeUint32) @@ -116,6 +121,8 @@ MissionItem::MissionItem(const MissionItem& other, QObject* parent) const MissionItem& MissionItem::operator=(const MissionItem& other) { + _doJumpId = other._doJumpId; + setCommand(other.command()); setFrame(other.frame()); setSequenceNumber(other._sequenceNumber); @@ -138,19 +145,18 @@ MissionItem::~MissionItem() void MissionItem::save(QJsonObject& json) const { - json[_jsonTypeKey] = _itemType; - json[_jsonIdKey] = sequenceNumber(); + json[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeSimpleItemValue; json[_jsonFrameKey] = frame(); json[_jsonCommandKey] = command(); - json[_jsonParam1Key] = param1(); - json[_jsonParam2Key] = param2(); - json[_jsonParam3Key] = param3(); - json[_jsonParam4Key] = param4(); json[_jsonAutoContinueKey] = autoContinue(); + json[_jsonDoJumpIdKey] = _sequenceNumber; + + QJsonArray rgParams = { param1(), param2(), param3(), param4() }; + json[_jsonParamsKey] = rgParams; - QJsonArray coordinateArray; - coordinateArray << param5() << param6() << param7(); - json[_jsonCoordinateKey] = coordinateArray; + QJsonValue coordinateValue; + JsonHelper::saveGeoCoordinate(QGeoCoordinate(param5(), param6(), param7()), true /* writeAltitude */, coordinateValue); + json[_jsonCoordinateKey] = coordinateValue; } bool MissionItem::load(QTextStream &loadStream) @@ -175,41 +181,98 @@ bool MissionItem::load(QTextStream &loadStream) return false; } -bool MissionItem::load(const QJsonObject& json, QString& errorString) +bool MissionItem::_convertJsonV1ToV2(const QJsonObject& json, QJsonObject& v2Json, QString& errorString) +{ + // V1 format type = "missionItem", V2 format type = "MissionItem" + // V1 format has params in separate param[1-n] keys + // V2 format has params in params array + v2Json = json; + + if (json.contains(_jsonParamsKey)) { + // Already V2 format + return true; + } + + QList keyInfoList = { + { VisualMissionItem::jsonTypeKey, QJsonValue::String, true }, + { _jsonParam1Key, QJsonValue::Double, true }, + { _jsonParam2Key, QJsonValue::Double, true }, + { _jsonParam3Key, QJsonValue::Double, true }, + { _jsonParam4Key, QJsonValue::Double, true }, + }; + if (!JsonHelper::validateKeys(json, keyInfoList, errorString)) { + return false; + } + + if (v2Json[VisualMissionItem::jsonTypeKey].toString() == QStringLiteral("missionItem")) { + v2Json[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeSimpleItemValue; + } + + QJsonArray rgParams = { json[_jsonParam1Key].toDouble(), json[_jsonParam2Key].toDouble(), json[_jsonParam3Key].toDouble(), json[_jsonParam4Key].toDouble() }; + v2Json[_jsonParamsKey] = rgParams; + v2Json.remove(_jsonParam1Key); + v2Json.remove(_jsonParam2Key); + v2Json.remove(_jsonParam3Key); + v2Json.remove(_jsonParam4Key); + + return true; +} + +bool MissionItem::load(const QJsonObject& json, int sequenceNumber, QString& errorString) { - QStringList requiredKeys; + QJsonObject v2Json; + if (!_convertJsonV1ToV2(json, v2Json, errorString)) { + return false; + } + + QList keyInfoList = { + { VisualMissionItem::jsonTypeKey, QJsonValue::String, true }, + { _jsonFrameKey, QJsonValue::Double, true }, + { _jsonCommandKey, QJsonValue::Double, true }, + { _jsonParamsKey, QJsonValue::Array, true }, + { _jsonAutoContinueKey, QJsonValue::Bool, true }, + { _jsonCoordinateKey, QJsonValue::Array, true }, + { _jsonDoJumpIdKey, QJsonValue::Double, false }, + }; + if (!JsonHelper::validateKeys(v2Json, keyInfoList, errorString)) { + return false; + } - requiredKeys << _jsonTypeKey << _jsonIdKey << _jsonFrameKey << _jsonCommandKey << - _jsonParam1Key << _jsonParam2Key << _jsonParam3Key << _jsonParam4Key << - _jsonAutoContinueKey << _jsonCoordinateKey; - if (!JsonHelper::validateRequiredKeys(json, requiredKeys, errorString)) { + if (v2Json[VisualMissionItem::jsonTypeKey] != VisualMissionItem::jsonTypeSimpleItemValue) { + errorString = tr("Type found: %1 must be: %2").arg(v2Json[VisualMissionItem::jsonTypeKey].toString()).arg(VisualMissionItem::jsonTypeSimpleItemValue); return false; } - if (json[_jsonTypeKey] != _itemType) { - errorString = QString("type found: %1 must be: %2").arg(json[_jsonTypeKey].toString()).arg(_itemType); + QJsonArray rgParams = v2Json[_jsonParamsKey].toArray(); + if (rgParams.count() != 4) { + errorString = tr("%1 key must contains 4 values").arg(_jsonParamsKey); return false; } // Make sure to set these first since they can signal other changes - setFrame((MAV_FRAME)json[_jsonFrameKey].toInt()); - setCommand((MAV_CMD)json[_jsonCommandKey].toInt()); + setFrame((MAV_FRAME)v2Json[_jsonFrameKey].toInt()); + setCommand((MAV_CMD)v2Json[_jsonCommandKey].toInt()); QGeoCoordinate coordinate; - if (!JsonHelper::loadGeoCoordinate(json[_jsonCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) { + if (!JsonHelper::loadGeoCoordinate(v2Json[_jsonCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) { return false; } setParam5(coordinate.latitude()); setParam6(coordinate.longitude()); setParam7(coordinate.altitude()); + _doJumpId = -1; + if (v2Json.contains(_jsonDoJumpIdKey)) { + _doJumpId = v2Json[_jsonDoJumpIdKey].toInt(); + } setIsCurrentItem(false); - setSequenceNumber(json[_jsonIdKey].toInt()); - setParam1(json[_jsonParam1Key].toDouble()); - setParam2(json[_jsonParam2Key].toDouble()); - setParam3(json[_jsonParam3Key].toDouble()); - setParam4(json[_jsonParam4Key].toDouble()); - setAutoContinue(json[_jsonAutoContinueKey].toBool()); + setSequenceNumber(sequenceNumber); + setAutoContinue(v2Json[_jsonAutoContinueKey].toBool()); + + setParam1(rgParams[0].toDouble()); + setParam2(rgParams[1].toDouble()); + setParam3(rgParams[2].toDouble()); + setParam4(rgParams[3].toDouble()); return true; } diff --git a/src/MissionManager/MissionItem.h b/src/MissionManager/MissionItem.h index ee52892ae..5d4b0f2dc 100644 --- a/src/MissionManager/MissionItem.h +++ b/src/MissionManager/MissionItem.h @@ -74,6 +74,7 @@ public: double param6 (void) const { return _param6Fact.rawValue().toDouble(); } double param7 (void) const { return _param7Fact.rawValue().toDouble(); } QGeoCoordinate coordinate (void) const; + int doJumpId (void) const { return _doJumpId; } void setCommand (MAV_CMD command); void setSequenceNumber (int sequenceNumber); @@ -91,7 +92,7 @@ public: void save(QJsonObject& json) const; bool load(QTextStream &loadStream); - bool load(const QJsonObject& json, QString& errorString); + bool load(const QJsonObject& json, int sequenceNumber, QString& errorString); bool relativeAltitude(void) const { return frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT; } @@ -100,8 +101,11 @@ signals: void sequenceNumberChanged (int sequenceNumber); private: - int _sequenceNumber; - bool _isCurrentItem; + bool _convertJsonV1ToV2(const QJsonObject& json, QJsonObject& v2Json, QString& errorString); + + int _sequenceNumber; + int _doJumpId; + bool _isCurrentItem; Fact _autoContinueFact; Fact _commandFact; @@ -115,17 +119,18 @@ private: Fact _param7Fact; // Keys for Json save - static const char* _itemType; - static const char* _jsonTypeKey; - static const char* _jsonIdKey; static const char* _jsonFrameKey; static const char* _jsonCommandKey; + static const char* _jsonAutoContinueKey; + static const char* _jsonCoordinateKey; + static const char* _jsonParamsKey; + static const char* _jsonDoJumpIdKey; + + // Deprecated V1 format keys static const char* _jsonParam1Key; static const char* _jsonParam2Key; static const char* _jsonParam3Key; static const char* _jsonParam4Key; - static const char* _jsonAutoContinueKey; - static const char* _jsonCoordinateKey; friend class SurveyMissionItem; friend class SimpleMissionItem; diff --git a/src/MissionManager/MissionItemTest.cc b/src/MissionManager/MissionItemTest.cc index a408bf911..bb7d95e52 100644 --- a/src/MissionManager/MissionItemTest.cc +++ b/src/MissionManager/MissionItemTest.cc @@ -226,7 +226,7 @@ void MissionItemTest::_testFactSignals(void) void MissionItemTest::_checkExpectedMissionItem(const MissionItem& missionItem) { - QCOMPARE(missionItem.sequenceNumber(), 10); + QCOMPARE(missionItem.sequenceNumber(), _seq); QCOMPARE(missionItem.isCurrentItem(), false); QCOMPARE(missionItem.frame(), (MAV_FRAME)3); QCOMPARE(missionItem.command(), (MAV_CMD)80); @@ -264,33 +264,71 @@ void MissionItemTest::_testSimpleLoadFromStream(void) _checkExpectedMissionItem(simpleMissionItem.missionItem()); } -void MissionItemTest::_testLoadFromJson(void) +void MissionItemTest::_testLoadFromJsonV1(void) { MissionItem missionItem; QString errorString; QJsonArray coordinateArray; coordinateArray << -10.0 << -20.0 <<-30.0; QJsonObject jsonObject; - jsonObject.insert("autoContinue", true); - jsonObject.insert("command", 80); - jsonObject.insert("frame", 3); - jsonObject.insert("id", 10); - jsonObject.insert("param1", 10); - jsonObject.insert("param2", 20); - jsonObject.insert("param3", 30); - jsonObject.insert("param4", 40); - jsonObject.insert("type", "missionItem"); - jsonObject.insert("coordinate", coordinateArray); + jsonObject.insert(MissionItem::_jsonAutoContinueKey, true); + jsonObject.insert(MissionItem::_jsonCommandKey, 80); + jsonObject.insert(MissionItem::_jsonFrameKey, 3); + jsonObject.insert(MissionItem::_jsonParam1Key, 10); + jsonObject.insert(MissionItem::_jsonParam2Key, 20); + jsonObject.insert(MissionItem::_jsonParam3Key, 30); + jsonObject.insert(MissionItem::_jsonParam4Key, 40); + jsonObject.insert(VisualMissionItem::jsonTypeKey, VisualMissionItem::jsonTypeSimpleItemValue); + jsonObject.insert(MissionItem::_jsonCoordinateKey, coordinateArray); + // We only need to test the differences between V1 and V2 + + QStringList removeKeys; + removeKeys << MissionItem::_jsonParam1Key << MissionItem::_jsonParam2Key << MissionItem::_jsonParam3Key << MissionItem::_jsonParam4Key; + foreach (const QString& removeKey, removeKeys) { + QJsonObject badObject = jsonObject; + badObject.remove(removeKey); + QCOMPARE(missionItem.load(badObject, _seq, errorString), false); + QVERIFY(!errorString.isEmpty()); + qDebug() << errorString; + } + + // Test good load + + QVERIFY(missionItem.load(jsonObject, _seq, errorString)); + _checkExpectedMissionItem(missionItem); +} + +void MissionItemTest::_testLoadFromJsonV2(void) +{ + MissionItem missionItem; + QString errorString; + QJsonArray coordinateArray; + coordinateArray << -10.0 << -20.0 <<-30.0; + QJsonObject jsonObject; + jsonObject.insert(MissionItem::_jsonAutoContinueKey, true); + jsonObject.insert(MissionItem::_jsonCommandKey, 80); + jsonObject.insert(MissionItem::_jsonFrameKey, 3); + jsonObject.insert(VisualMissionItem::jsonTypeKey, VisualMissionItem::jsonTypeSimpleItemValue); + jsonObject.insert(MissionItem::_jsonCoordinateKey, coordinateArray); + + QJsonArray rgParams = { 10, 20, 30, 40 }; + jsonObject.insert(MissionItem::_jsonParamsKey, rgParams); + // Test missing key detection QStringList removeKeys; - removeKeys << "autoContinue" << "command" << "frame" << "id" << "param1" << "param2" << "param3" << "param4" << "type" << "coordinate"; + removeKeys << MissionItem::_jsonAutoContinueKey << + MissionItem::_jsonCommandKey << + MissionItem::_jsonFrameKey << + MissionItem::_jsonParamsKey << + VisualMissionItem::jsonTypeKey << + MissionItem::_jsonCoordinateKey; foreach(const QString& removeKey, removeKeys) { QJsonObject badObject = jsonObject; badObject.remove(removeKey); - QCOMPARE(missionItem.load(badObject, errorString), false); + QCOMPARE(missionItem.load(badObject, _seq, errorString), false); QVERIFY(!errorString.isEmpty()); qDebug() << errorString; } @@ -300,7 +338,7 @@ void MissionItemTest::_testLoadFromJson(void) QJsonObject badObject = jsonObject; badObject.remove("coordinate"); badObject["coordinate"] = 10; - QCOMPARE(missionItem.load(badObject, errorString), false); + QCOMPARE(missionItem.load(badObject, _seq, errorString), false); QVERIFY(!errorString.isEmpty()); qDebug() << errorString; @@ -309,7 +347,7 @@ void MissionItemTest::_testLoadFromJson(void) badObject = jsonObject; badObject.remove("coordinate"); badObject["coordinate"] = badCoordinateArray; - QCOMPARE(missionItem.load(badObject, errorString), false); + QCOMPARE(missionItem.load(badObject, _seq, errorString), false); QVERIFY(!errorString.isEmpty()); qDebug() << errorString; @@ -318,7 +356,7 @@ void MissionItemTest::_testLoadFromJson(void) badObject = jsonObject; badObject.remove("coordinate"); badObject["coordinate"] = badCoordinateArray_second; - QCOMPARE(missionItem.load(badObject, errorString), false); + QCOMPARE(missionItem.load(badObject, _seq, errorString), false); QVERIFY(!errorString.isEmpty()); qDebug() << errorString; @@ -329,7 +367,7 @@ void MissionItemTest::_testLoadFromJson(void) badObject = jsonObject; badObject.remove("coordinate"); badObject["coordinate"] = badCoordinateArray_third; - QCOMPARE(missionItem.load(badObject, errorString), false); + QCOMPARE(missionItem.load(badObject, _seq, errorString), false); QVERIFY(!errorString.isEmpty()); qDebug() << errorString; @@ -338,13 +376,17 @@ void MissionItemTest::_testLoadFromJson(void) badObject = jsonObject; badObject.remove("type"); badObject["type"] = "foo"; - QCOMPARE(missionItem.load(badObject, errorString), false); + QCOMPARE(missionItem.load(badObject, _seq, errorString), false); QVERIFY(!errorString.isEmpty()); qDebug() << errorString; // Test good load - QVERIFY(missionItem.load(jsonObject, errorString)); + bool result = missionItem.load(jsonObject, _seq, errorString); + if (!result) { + qDebug() << errorString; + QVERIFY(result); + } _checkExpectedMissionItem(missionItem); } @@ -356,21 +398,19 @@ void MissionItemTest::_testSimpleLoadFromJson(void) SimpleMissionItem simpleMissionItem(NULL); QString errorString; QJsonArray coordinateArray; - coordinateArray << -10.0 << -20.0 << -30.0; QJsonObject jsonObject; - jsonObject.insert("autoContinue", true); - jsonObject.insert("command", 80); - jsonObject.insert("frame", 3); - jsonObject.insert("id", 10); - jsonObject.insert("param1", 10); - jsonObject.insert("param2", 20); - jsonObject.insert("param3", 30); - jsonObject.insert("param4", 40); - jsonObject.insert("type", "missionItem"); - jsonObject.insert("coordinate", coordinateArray); - - - QVERIFY(simpleMissionItem.load(jsonObject, errorString)); + + coordinateArray << -10.0 << -20.0 <<-30.0; + jsonObject.insert(MissionItem::_jsonAutoContinueKey, true); + jsonObject.insert(MissionItem::_jsonCommandKey, 80); + jsonObject.insert(MissionItem::_jsonFrameKey, 3); + jsonObject.insert(VisualMissionItem::jsonTypeKey, VisualMissionItem::jsonTypeSimpleItemValue); + jsonObject.insert(MissionItem::_jsonCoordinateKey, coordinateArray); + + QJsonArray rgParams = { 10, 20, 30, 40 }; + jsonObject.insert(MissionItem::_jsonParamsKey, rgParams); + + QVERIFY(simpleMissionItem.load(jsonObject, _seq, errorString)); _checkExpectedMissionItem(simpleMissionItem.missionItem()); } @@ -378,7 +418,7 @@ void MissionItemTest::_testSaveToJson(void) { MissionItem missionItem; - missionItem.setSequenceNumber(10); + missionItem.setSequenceNumber(_seq); missionItem.setIsCurrentItem(true); missionItem.setFrame((MAV_FRAME)3); missionItem.setCommand((MAV_CMD)80); @@ -395,9 +435,9 @@ void MissionItemTest::_testSaveToJson(void) QJsonObject jsonObject; QString errorString; missionItem.save(jsonObject); - QVERIFY(missionItem.load(jsonObject, errorString)); + QVERIFY(missionItem.load(jsonObject, _seq, errorString)); - QCOMPARE(missionItem.sequenceNumber(), 10); + QCOMPARE(missionItem.sequenceNumber(), _seq); QCOMPARE(missionItem.isCurrentItem(), false); QCOMPARE(missionItem.frame(), (MAV_FRAME)3); QCOMPARE(missionItem.command(), (MAV_CMD)80); @@ -410,295 +450,3 @@ void MissionItemTest::_testSaveToJson(void) QCOMPARE(missionItem.param7(), -30.1234567); QCOMPARE(missionItem.autoContinue(), true); } - -#if 0 -void MissionItemTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode) -{ - _mockLink->setMissionItemFailureMode(failureMode); - - // Setup our test case data - QList missionItems; - - // Editor has a home position item on the front, so we do the same - MissionItem* homeItem = new MissionItem(NULL /* Vehicle */, this); - homeItem->setCommand(MAV_CMD_NAV_WAYPOINT); - homeItem->setCoordinate(QGeoCoordinate(47.3769, 8.549444, 0)); - homeItem->setSequenceNumber(0); - missionItems.append(homeItem); - - for (size_t i=0; i<_cTestCases; i++) { - const TestCase_t* testCase = &_rgTestCases[i]; - - MissionItem* missionItem = new MissionItem(this); - - QTextStream loadStream(testCase->itemStream, QIODevice::ReadOnly); - QVERIFY(missionItem->load(loadStream)); - - // Mission Manager expects to get 1-base sequence numbers for write - missionItem->setSequenceNumber(missionItem->sequenceNumber() + 1); - - missionItems.append(missionItem); - } - - // Send the items to the vehicle - _missionManager->writeMissionItems(missionItems); - - // writeMissionItems should emit these signals before returning: - // inProgressChanged - // newMissionItemsAvailable - QVERIFY(_missionManager->inProgress()); - QCOMPARE(_multiSpyMissionManager->checkSignalByMask(inProgressChangedSignalMask | newMissionItemsAvailableSignalMask), true); - _checkInProgressValues(true); - - _multiSpyMissionManager->clearAllSignals(); - - if (failureMode == MockLinkMissionItemHandler::FailNone) { - // This should be clean run - - // Wait for write sequence to complete. We should get: - // inProgressChanged(false) signal - _multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime); - QCOMPARE(_multiSpyMissionManager->checkOnlySignalByMask(inProgressChangedSignalMask), true); - - // Validate inProgressChanged signal value - _checkInProgressValues(false); - - // Validate item count in mission manager - - int expectedCount = (int)_cTestCases; - if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { - // Home position at position 0 comes from vehicle - expectedCount++; - } - - QCOMPARE(_missionManager->missionItems().count(), expectedCount); - } else { - // This should be a failed run - - setExpectedMessageBox(QMessageBox::Ok); - - // Wait for write sequence to complete. We should get: - // inProgressChanged(false) signal - // error(errorCode, QString) signal - _multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime); - QCOMPARE(_multiSpyMissionManager->checkSignalByMask(inProgressChangedSignalMask | errorSignalMask), true); - - // Validate inProgressChanged signal value - _checkInProgressValues(false); - - // Validate error signal values - QSignalSpy* spy = _multiSpyMissionManager->getSpyByIndex(errorSignalIndex); - QList signalArgs = spy->takeFirst(); - QCOMPARE(signalArgs.count(), 2); - qDebug() << signalArgs[1].toString(); - - checkExpectedMessageBox(); - } - - _multiSpyMissionManager->clearAllSignals(); -} - -void MissionItemTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode) -{ - _writeItems(MockLinkMissionItemHandler::FailNone); - - _mockLink->setMissionItemFailureMode(failureMode); - - // Read the items back from the vehicle - _missionManager->requestMissionItems(); - - // requestMissionItems should emit inProgressChanged signal before returning so no need to wait for it - QVERIFY(_missionManager->inProgress()); - QCOMPARE(_multiSpyMissionManager->checkOnlySignalByMask(inProgressChangedSignalMask), true); - _checkInProgressValues(true); - - _multiSpyMissionManager->clearAllSignals(); - - if (failureMode == MockLinkMissionItemHandler::FailNone) { - // This should be clean run - - // Now wait for read sequence to complete. We should get: - // inProgressChanged(false) signal to signal completion - // newMissionItemsAvailable signal - _multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime); - QCOMPARE(_multiSpyMissionManager->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true); - _checkInProgressValues(false); - - } else { - // This should be a failed run - - setExpectedMessageBox(QMessageBox::Ok); - - // Wait for read sequence to complete. We should get: - // inProgressChanged(false) signal to signal completion - // error(errorCode, QString) signal - // newMissionItemsAvailable signal - _multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime); - QCOMPARE(_multiSpyMissionManager->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask | errorSignalMask), true); - - // Validate inProgressChanged signal value - _checkInProgressValues(false); - - // Validate error signal values - QSignalSpy* spy = _multiSpyMissionManager->getSpyByIndex(errorSignalIndex); - QList signalArgs = spy->takeFirst(); - QCOMPARE(signalArgs.count(), 2); - qDebug() << signalArgs[1].toString(); - - checkExpectedMessageBox(); - } - - _multiSpyMissionManager->clearAllSignals(); - - // Validate returned items - - size_t cMissionItemsExpected; - - if (failureMode == MockLinkMissionItemHandler::FailNone) { - cMissionItemsExpected = (int)_cTestCases; - if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { - // Home position at position 0 comes from vehicle - cMissionItemsExpected++; - } - } else { - cMissionItemsExpected = 0; - } - - QCOMPARE(_missionManager->missionItems().count(), (int)cMissionItemsExpected); - - size_t firstActualItem = 0; - if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { - // First item is home position, don't validate it - firstActualItem++; - } - - int testCaseIndex = 0; - for (size_t actualItemIndex=firstActualItem; actualItemIndexexpectedItem.sequenceNumber; - if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { - // Account for home position in first item - expectedSequenceNumber++; - } - - MissionItem* actual = _missionManager->missionItems()[actualItemIndex]; - - qDebug() << "Test case" << testCaseIndex; - QCOMPARE(actual->sequenceNumber(), expectedSequenceNumber); - QCOMPARE(actual->coordinate().latitude(), testCase->expectedItem.coordinate.latitude()); - QCOMPARE(actual->coordinate().longitude(), testCase->expectedItem.coordinate.longitude()); - QCOMPARE(actual->coordinate().altitude(), testCase->expectedItem.coordinate.altitude()); - QCOMPARE((int)actual->command(), (int)testCase->expectedItem.command); - QCOMPARE(actual->param1(), testCase->expectedItem.param1); - QCOMPARE(actual->param2(), testCase->expectedItem.param2); - QCOMPARE(actual->param3(), testCase->expectedItem.param3); - QCOMPARE(actual->param4(), testCase->expectedItem.param4); - QCOMPARE(actual->autoContinue(), testCase->expectedItem.autocontinue); - QCOMPARE(actual->frame(), testCase->expectedItem.frame); - - testCaseIndex++; - } - -} - -void MissionItemTest::_testWriteFailureHandlingWorker(void) -{ - /* - /// Called to send a MISSION_ACK message while the MissionManager is in idle state - void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType) { _missionItemHandler.sendUnexpectedMissionAck(ackType); } - - /// Called to send a MISSION_ITEM message while the MissionManager is in idle state - void sendUnexpectedMissionItem(void) { _missionItemHandler.sendUnexpectedMissionItem(); } - - /// Called to send a MISSION_REQUEST message while the MissionManager is in idle state - void sendUnexpectedMissionRequest(void) { _missionItemHandler.sendUnexpectedMissionRequest(); } - */ - - typedef struct { - const char* failureText; - MockLinkMissionItemHandler::FailureMode_t failureMode; - } TestCase_t; - - static const TestCase_t rgTestCases[] = { - { "No Failure", MockLinkMissionItemHandler::FailNone }, - { "FailWriteRequest0NoResponse", MockLinkMissionItemHandler::FailWriteRequest0NoResponse }, - { "FailWriteRequest1NoResponse", MockLinkMissionItemHandler::FailWriteRequest1NoResponse }, - { "FailWriteRequest0IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest0IncorrectSequence }, - { "FailWriteRequest1IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest1IncorrectSequence }, - { "FailWriteRequest0ErrorAck", MockLinkMissionItemHandler::FailWriteRequest0ErrorAck }, - { "FailWriteRequest1ErrorAck", MockLinkMissionItemHandler::FailWriteRequest1ErrorAck }, - { "FailWriteFinalAckNoResponse", MockLinkMissionItemHandler::FailWriteFinalAckNoResponse }, - { "FailWriteFinalAckErrorAck", MockLinkMissionItemHandler::FailWriteFinalAckErrorAck }, - { "FailWriteFinalAckMissingRequests", MockLinkMissionItemHandler::FailWriteFinalAckMissingRequests }, - }; - - for (size_t i=0; iresetMissionItemHandler(); - } -} - -void MissionItemTest::_testReadFailureHandlingWorker(void) -{ - /* - /// Called to send a MISSION_ACK message while the MissionManager is in idle state - void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType) { _missionItemHandler.sendUnexpectedMissionAck(ackType); } - - /// Called to send a MISSION_ITEM message while the MissionManager is in idle state - void sendUnexpectedMissionItem(void) { _missionItemHandler.sendUnexpectedMissionItem(); } - - /// Called to send a MISSION_REQUEST message while the MissionManager is in idle state - void sendUnexpectedMissionRequest(void) { _missionItemHandler.sendUnexpectedMissionRequest(); } - */ - - typedef struct { - const char* failureText; - MockLinkMissionItemHandler::FailureMode_t failureMode; - } TestCase_t; - - static const TestCase_t rgTestCases[] = { - { "No Failure", MockLinkMissionItemHandler::FailNone }, - { "FailReadRequestListNoResponse", MockLinkMissionItemHandler::FailReadRequestListNoResponse }, - { "FailReadRequest0NoResponse", MockLinkMissionItemHandler::FailReadRequest0NoResponse }, - { "FailReadRequest1NoResponse", MockLinkMissionItemHandler::FailReadRequest1NoResponse }, - { "FailReadRequest0IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest0IncorrectSequence }, - { "FailReadRequest1IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest1IncorrectSequence }, - { "FailReadRequest0ErrorAck", MockLinkMissionItemHandler::FailReadRequest0ErrorAck }, - { "FailReadRequest1ErrorAck", MockLinkMissionItemHandler::FailReadRequest1ErrorAck }, - }; - - for (size_t i=0; iresetMissionItemHandler(); - _multiSpyMissionManager->clearAllSignals(); - } -} - -void MissionItemTest::_testWriteFailureHandlingAPM(void) -{ - _initForFirmwareType(MAV_AUTOPILOT_ARDUPILOTMEGA); - _testWriteFailureHandlingWorker(); -} - -void MissionItemTest::_testReadFailureHandlingAPM(void) -{ - _initForFirmwareType(MAV_AUTOPILOT_ARDUPILOTMEGA); - _testReadFailureHandlingWorker(); -} - - -void MissionItemTest::_testWriteFailureHandlingPX4(void) -{ - _initForFirmwareType(MAV_AUTOPILOT_PX4); - _testWriteFailureHandlingWorker(); -} - -void MissionItemTest::_testReadFailureHandlingPX4(void) -{ - _initForFirmwareType(MAV_AUTOPILOT_PX4); - _testReadFailureHandlingWorker(); -} -#endif diff --git a/src/MissionManager/MissionItemTest.h b/src/MissionManager/MissionItemTest.h index 857235606..4dd1f6b35 100644 --- a/src/MissionManager/MissionItemTest.h +++ b/src/MissionManager/MissionItemTest.h @@ -29,12 +29,15 @@ private slots: void _testFactSignals(void); void _testLoadFromStream(void); void _testSimpleLoadFromStream(void); - void _testLoadFromJson(void); + void _testLoadFromJsonV1(void); + void _testLoadFromJsonV2(void); void _testSimpleLoadFromJson(void); void _testSaveToJson(void); private: void _checkExpectedMissionItem(const MissionItem& missionItem); + + int _seq = 10; }; #endif diff --git a/src/MissionManager/RallyPointController.cc b/src/MissionManager/RallyPointController.cc index 0b3bfeb01..3a653e776 100644 --- a/src/MissionManager/RallyPointController.cc +++ b/src/MissionManager/RallyPointController.cc @@ -75,8 +75,13 @@ bool RallyPointController::_loadJsonFile(QJsonDocument& jsonDoc, QString& errorS { QJsonObject json = jsonDoc.object(); - int fileMajorVersion, fileMinorVersion; - if (!JsonHelper::validateQGCJsonFile(json, _jsonFileTypeValue, 1 /* supportedMajorVersion */, 0 /* supportedMinorVersion */, fileMajorVersion, fileMinorVersion, errorString)) { + int fileVersion; + if (!JsonHelper::validateQGCJsonFile(json, + _jsonFileTypeValue, // expected file type + 1, // minimum supported version + 1, // maximum supported version + fileVersion, + errorString)) { return false; } @@ -166,7 +171,7 @@ void RallyPointController::saveToFile(const QString& filename) QJsonObject jsonObject; jsonObject[JsonHelper::jsonFileTypeKey] = _jsonFileTypeValue; - jsonObject[JsonHelper::jsonVersionKey] = QStringLiteral("1.0"); + jsonObject[JsonHelper::jsonVersionKey] = 1; jsonObject[JsonHelper::jsonGroundStationKey] = JsonHelper::jsonGroundStationValue; QJsonArray rgPoints; diff --git a/src/MissionManager/SimpleMissionItem.cc b/src/MissionManager/SimpleMissionItem.cc index 35614a1eb..22ecd6252 100644 --- a/src/MissionManager/SimpleMissionItem.cc +++ b/src/MissionManager/SimpleMissionItem.cc @@ -18,14 +18,14 @@ #include "MissionCommandTree.h" #include "MissionCommandUIInfo.h" -const double SimpleMissionItem::defaultAltitude = 50.0; +const double SimpleMissionItem::defaultAltitude = 50.0; -FactMetaData* SimpleMissionItem::_altitudeMetaData = NULL; -FactMetaData* SimpleMissionItem::_commandMetaData = NULL; -FactMetaData* SimpleMissionItem::_defaultParamMetaData = NULL; -FactMetaData* SimpleMissionItem::_frameMetaData = NULL; -FactMetaData* SimpleMissionItem::_latitudeMetaData = NULL; -FactMetaData* SimpleMissionItem::_longitudeMetaData = NULL; +FactMetaData* SimpleMissionItem::_altitudeMetaData = NULL; +FactMetaData* SimpleMissionItem::_commandMetaData = NULL; +FactMetaData* SimpleMissionItem::_defaultParamMetaData = NULL; +FactMetaData* SimpleMissionItem::_frameMetaData = NULL; +FactMetaData* SimpleMissionItem::_latitudeMetaData = NULL; +FactMetaData* SimpleMissionItem::_longitudeMetaData = NULL; struct EnumInfo_s { const char * label; @@ -248,9 +248,9 @@ bool SimpleMissionItem::load(QTextStream &loadStream) return _missionItem.load(loadStream); } -bool SimpleMissionItem::load(const QJsonObject& json, QString& errorString) +bool SimpleMissionItem::load(const QJsonObject& json, int sequenceNumber, QString& errorString) { - return _missionItem.load(json, errorString); + return _missionItem.load(json, sequenceNumber, errorString); } bool SimpleMissionItem::isStandaloneCoordinate(void) const diff --git a/src/MissionManager/SimpleMissionItem.h b/src/MissionManager/SimpleMissionItem.h index a079478b2..9d3650c0c 100644 --- a/src/MissionManager/SimpleMissionItem.h +++ b/src/MissionManager/SimpleMissionItem.h @@ -71,7 +71,7 @@ public: void setDistance (double distance); bool load(QTextStream &loadStream); - bool load(const QJsonObject& json, QString& errorString); + bool load(const QJsonObject& json, int sequenceNumber, QString& errorString); bool relativeAltitude(void) { return _missionItem.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT; } diff --git a/src/MissionManager/SurveyMissionItem.cc b/src/MissionManager/SurveyMissionItem.cc index aae975f13..fd10f9aeb 100644 --- a/src/MissionManager/SurveyMissionItem.cc +++ b/src/MissionManager/SurveyMissionItem.cc @@ -17,9 +17,9 @@ QGC_LOGGING_CATEGORY(SurveyMissionItemLog, "SurveyMissionItemLog") -const char* SurveyMissionItem::_jsonTypeKey = "type"; +const char* SurveyMissionItem::jsonComplexItemTypeValue = "survey"; + const char* SurveyMissionItem::_jsonPolygonObjectKey = "polygon"; -const char* SurveyMissionItem::_jsonIdKey = "id"; const char* SurveyMissionItem::_jsonGridObjectKey = "grid"; const char* SurveyMissionItem::_jsonGridAltitudeKey = "altitude"; const char* SurveyMissionItem::_jsonGridAltitudeRelativeKey = "relativeAltitude"; @@ -56,8 +56,6 @@ const char* SurveyMissionItem::_cameraResolutionWidthFactName = "Camera reso const char* SurveyMissionItem::_cameraResolutionHeightFactName = "Camera resolution height"; const char* SurveyMissionItem::_cameraFocalLengthFactName = "Focal length"; -const char* SurveyMissionItem::_complexType = "survey"; - QMap SurveyMissionItem::_metaDataMap; SurveyMissionItem::SurveyMissionItem(Vehicle* vehicle, QObject* parent) @@ -239,12 +237,12 @@ void SurveyMissionItem::setDirty(bool dirty) void SurveyMissionItem::save(QJsonObject& saveObject) const { - saveObject[JsonHelper::jsonVersionKey] = 2; - saveObject[_jsonTypeKey] = _complexType; - saveObject[_jsonIdKey] = sequenceNumber(); - saveObject[_jsonCameraTriggerKey] = _cameraTrigger; - saveObject[_jsonManualGridKey] = _manualGrid; - saveObject[_jsonFixedValueIsAltitudeKey] = _fixedValueIsAltitude; + saveObject[JsonHelper::jsonVersionKey] = 3; + saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; + saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; + saveObject[_jsonCameraTriggerKey] = _cameraTrigger; + saveObject[_jsonManualGridKey] = _manualGrid; + saveObject[_jsonFixedValueIsAltitudeKey] = _fixedValueIsAltitude; if (_cameraTrigger) { saveObject[_jsonCameraTriggerDistanceKey] = _cameraTriggerDistanceFact.rawValue().toDouble(); @@ -306,98 +304,107 @@ void SurveyMissionItem::_clear(void) } -bool SurveyMissionItem::load(const QJsonObject& complexObject, QString& errorString) +bool SurveyMissionItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString) { - struct jsonKeyInfo_s { - const char* key; - QJsonValue::Type type; - bool required; - }; + QJsonObject v2Object = complexObject; - QList mainKeyInfoList = { - { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, - { _jsonTypeKey, QJsonValue::String, true }, - { _jsonPolygonObjectKey, QJsonValue::Array, true }, - { _jsonIdKey, QJsonValue::Double, true }, - { _jsonGridObjectKey, QJsonValue::Object, true }, - { _jsonCameraObjectKey, QJsonValue::Object, false }, - { _jsonCameraTriggerKey, QJsonValue::Bool, true }, - { _jsonCameraTriggerDistanceKey, QJsonValue::Double, false }, - { _jsonManualGridKey, QJsonValue::Bool, true }, - { _jsonFixedValueIsAltitudeKey, QJsonValue::Bool, true }, + // We need to pull version first to determine what validation/conversion needs to be performed. + QList versionKeyInfoList = { + { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, }; - - QList gridKeyInfoList = { - { _jsonGridAltitudeKey, QJsonValue::Double, true }, - { _jsonGridAltitudeRelativeKey, QJsonValue::Bool, true }, - { _jsonGridAngleKey, QJsonValue::Double, true }, - { _jsonGridSpacingKey, QJsonValue::Double, true }, - { _jsonTurnaroundDistKey, QJsonValue::Double, true }, - }; - - QList cameraKeyInfoList = { - { _jsonGroundResolutionKey, QJsonValue::Double, true }, - { _jsonFrontalOverlapKey, QJsonValue::Double, true }, - { _jsonSideOverlapKey, QJsonValue::Double, true }, - { _jsonCameraSensorWidthKey, QJsonValue::Double, true }, - { _jsonCameraSensorHeightKey, QJsonValue::Double, true }, - { _jsonCameraResolutionWidthKey, QJsonValue::Double, true }, - { _jsonCameraResolutionHeightKey, QJsonValue::Double, true }, - { _jsonCameraFocalLengthKey, QJsonValue::Double, true }, - { _jsonCameraNameKey, QJsonValue::String, true }, - { _jsonCameraOrientationLandscapeKey, QJsonValue::Bool, true }, - }; - - if (!JsonHelper::validateKeys(complexObject, mainKeyInfoList, errorString)) { + if (!JsonHelper::validateKeys(v2Object, versionKeyInfoList, errorString)) { return false; } - if (!JsonHelper::validateKeys(complexObject[_jsonGridObjectKey].toObject(), gridKeyInfoList, errorString)) { + + int version = v2Object[JsonHelper::jsonVersionKey].toInt(); + if (version != 2 && version != 3) { + errorString = tr("QGroundControl does not support this version of survey items"); return false; } + if (version == 2) { + // Convert to v3 + if (v2Object.contains(VisualMissionItem::jsonTypeKey) && v2Object[VisualMissionItem::jsonTypeKey].toString() == QStringLiteral("survey")) { + v2Object[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; + v2Object[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; + } + } - // Version check - if (complexObject[JsonHelper::jsonVersionKey].toInt() != 2) { - errorString = tr("QGroundControl does not support this version of survey items"); + QList mainKeyInfoList = { + { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, + { VisualMissionItem::jsonTypeKey, QJsonValue::String, true }, + { ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true }, + { _jsonPolygonObjectKey, QJsonValue::Array, true }, + { _jsonGridObjectKey, QJsonValue::Object, true }, + { _jsonCameraObjectKey, QJsonValue::Object, false }, + { _jsonCameraTriggerKey, QJsonValue::Bool, true }, + { _jsonCameraTriggerDistanceKey, QJsonValue::Double, false }, + { _jsonManualGridKey, QJsonValue::Bool, true }, + { _jsonFixedValueIsAltitudeKey, QJsonValue::Bool, true }, + }; + if (!JsonHelper::validateKeys(v2Object, mainKeyInfoList, errorString)) { return false; } - QString complexType = complexObject[_jsonTypeKey].toString(); - if (complexType != _complexType) { - errorString = tr("QGroundControl does not support loading this complex mission item type: %1").arg(complexType); + + QString itemType = v2Object[VisualMissionItem::jsonTypeKey].toString(); + QString complexType = v2Object[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; } _clear(); - setSequenceNumber(complexObject[_jsonIdKey].toInt()); - - _manualGrid = complexObject[_jsonManualGridKey].toBool(true); - _cameraTrigger = complexObject[_jsonCameraTriggerKey].toBool(false); - _fixedValueIsAltitude = complexObject[_jsonFixedValueIsAltitudeKey].toBool(true); - _gridAltitudeRelative = complexObject[_jsonGridAltitudeRelativeKey].toBool(true); + setSequenceNumber(sequenceNumber); - QJsonObject gridObject = complexObject[_jsonGridObjectKey].toObject(); + _manualGrid = v2Object[_jsonManualGridKey].toBool(true); + _cameraTrigger = v2Object[_jsonCameraTriggerKey].toBool(false); + _fixedValueIsAltitude = v2Object[_jsonFixedValueIsAltitudeKey].toBool(true); + _gridAltitudeRelative = v2Object[_jsonGridAltitudeRelativeKey].toBool(true); + QList gridKeyInfoList = { + { _jsonGridAltitudeKey, QJsonValue::Double, true }, + { _jsonGridAltitudeRelativeKey, QJsonValue::Bool, true }, + { _jsonGridAngleKey, QJsonValue::Double, true }, + { _jsonGridSpacingKey, QJsonValue::Double, true }, + { _jsonTurnaroundDistKey, QJsonValue::Double, true }, + }; + QJsonObject gridObject = v2Object[_jsonGridObjectKey].toObject(); + if (!JsonHelper::validateKeys(gridObject, gridKeyInfoList, errorString)) { + return false; + } _gridAltitudeFact.setRawValue (gridObject[_jsonGridAltitudeKey].toDouble()); _gridAngleFact.setRawValue (gridObject[_jsonGridAngleKey].toDouble()); _gridSpacingFact.setRawValue (gridObject[_jsonGridSpacingKey].toDouble()); _turnaroundDistFact.setRawValue (gridObject[_jsonTurnaroundDistKey].toDouble()); if (_cameraTrigger) { - if (!complexObject.contains(_jsonCameraTriggerDistanceKey)) { + if (!v2Object.contains(_jsonCameraTriggerDistanceKey)) { errorString = tr("%1 but %2 is missing").arg("cameraTrigger = true").arg("cameraTriggerDistance"); return false; } - _cameraTriggerDistanceFact.setRawValue(complexObject[_jsonCameraTriggerDistanceKey].toDouble()); + _cameraTriggerDistanceFact.setRawValue(v2Object[_jsonCameraTriggerDistanceKey].toDouble()); } if (!_manualGrid) { - if (!complexObject.contains(_jsonCameraObjectKey)) { + if (!v2Object.contains(_jsonCameraObjectKey)) { errorString = tr("%1 but %2 object is missing").arg("manualGrid = false").arg("camera"); return false; } - QJsonObject cameraObject = complexObject[_jsonCameraObjectKey].toObject(); - + QJsonObject cameraObject = v2Object[_jsonCameraObjectKey].toObject(); + + QList cameraKeyInfoList = { + { _jsonGroundResolutionKey, QJsonValue::Double, true }, + { _jsonFrontalOverlapKey, QJsonValue::Double, true }, + { _jsonSideOverlapKey, QJsonValue::Double, true }, + { _jsonCameraSensorWidthKey, QJsonValue::Double, true }, + { _jsonCameraSensorHeightKey, QJsonValue::Double, true }, + { _jsonCameraResolutionWidthKey, QJsonValue::Double, true }, + { _jsonCameraResolutionHeightKey, QJsonValue::Double, true }, + { _jsonCameraFocalLengthKey, QJsonValue::Double, true }, + { _jsonCameraNameKey, QJsonValue::String, true }, + { _jsonCameraOrientationLandscapeKey, QJsonValue::Bool, true }, + }; if (!JsonHelper::validateKeys(cameraObject, cameraKeyInfoList, errorString)) { return false; } @@ -416,7 +423,7 @@ bool SurveyMissionItem::load(const QJsonObject& complexObject, QString& errorStr } // Polygon shape - QJsonArray polygonArray(complexObject[_jsonPolygonObjectKey].toArray()); + QJsonArray polygonArray(v2Object[_jsonPolygonObjectKey].toArray()); for (int i=0; i _metaDataMap; - static const char* _jsonTypeKey; static const char* _jsonPolygonObjectKey; - static const char* _jsonIdKey; static const char* _jsonGridObjectKey; static const char* _jsonGridAltitudeKey; static const char* _jsonGridAltitudeRelativeKey; @@ -212,8 +212,6 @@ private: static const char* _cameraResolutionWidthFactName; static const char* _cameraResolutionHeightFactName; static const char* _cameraFocalLengthFactName; - - static const char* _complexType; }; #endif diff --git a/src/MissionManager/VisualMissionItem.cc b/src/MissionManager/VisualMissionItem.cc index e0b8faf16..db8b1ba78 100644 --- a/src/MissionManager/VisualMissionItem.cc +++ b/src/MissionManager/VisualMissionItem.cc @@ -16,6 +16,10 @@ #include "QGCApplication.h" #include "JsonHelper.h" +const char* VisualMissionItem::jsonTypeKey = "type"; +const char* VisualMissionItem::jsonTypeSimpleItemValue = "SimpleItem"; +const char* VisualMissionItem::jsonTypeComplexItemValue = "ComplexItem"; + VisualMissionItem::VisualMissionItem(Vehicle* vehicle, QObject* parent) : QObject(parent) , _vehicle(vehicle) diff --git a/src/MissionManager/VisualMissionItem.h b/src/MissionManager/VisualMissionItem.h index 6e1fa8daf..32bc65aad 100644 --- a/src/MissionManager/VisualMissionItem.h +++ b/src/MissionManager/VisualMissionItem.h @@ -123,6 +123,10 @@ public: /// @param saveObject Save the item to this json object virtual void save(QJsonObject& saveObject) const = 0; + static const char* jsonTypeKey; ///< Json file attribute which specifies the item type + static const char* jsonTypeSimpleItemValue; ///< Item type is MISSION_ITEM + static const char* jsonTypeComplexItemValue; ///< Item type is Complex Item + signals: void altDifferenceChanged (double altDifference); void altPercentChanged (double altPercent); -- 2.22.0