From c65f8ecadafeaf878dd0376f37185b0a746577da Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Thu, 1 Oct 2020 10:49:05 +0200 Subject: [PATCH] wima controller mod, appimage updated --- qgroundcontrol.pro | 2 - src/FlightDisplay/FlightDisplayViewMap.qml | 73 +- src/MissionManager/MissionItem.cc | 760 +++++++++---------- src/QmlControls/QmlObjectListModel.cc | 6 +- src/QmlControls/QmlObjectListModel.h | 4 +- src/Wima/CircularSurvey.cc | 7 +- src/Wima/Geometry/WimaMeasurementArea.cc | 9 + src/Wima/Geometry/WimaMeasurementArea.h | 2 + src/Wima/Geometry/WimaMeasurementAreaData.cc | 4 +- src/Wima/RoutingThread.cpp | 4 +- src/Wima/RoutingThread.h | 3 +- src/Wima/Snake/NemoInterface.cpp | 13 +- src/Wima/Snake/NemoInterface.h | 2 - src/Wima/Snake/SnakeTileLocal.h | 2 +- src/Wima/Snake/SnakeTilesLocal.h | 6 +- src/Wima/Snake/snake.h | 62 +- src/Wima/WimaController.cc | 293 +++++-- src/Wima/WimaController.h | 11 +- src/Wima/WimaPlanData.cc | 8 + src/Wima/WimaPlaner.cc | 19 +- src/WimaView/ProgressIndicator.qml | 6 +- 21 files changed, 752 insertions(+), 544 deletions(-) diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index d0e75dde4..49544aa75 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -448,7 +448,6 @@ HEADERS += \ src/Wima/Snake/QNemoHeartbeat.h \ src/Wima/Snake/QNemoProgress.h \ src/Wima/Snake/QNemoProgress.h \ - src/Wima/Snake/SnakeThread.h \ src/Wima/Snake/SnakeTile.h \ src/Wima/Snake/SnakeTileLocal.h \ src/Wima/Snake/SnakeTiles.h \ @@ -513,7 +512,6 @@ SOURCES += \ src/Wima/Geometry/GeoPoint3D.cpp \ src/Wima/Snake/NemoInterface.cpp \ src/Wima/Snake/QNemoProgress.cc \ - src/Wima/Snake/SnakeThread.cc \ src/Wima/Snake/SnakeTile.cpp \ src/Wima/WaypointManager/AreaInterface.cpp \ src/Wima/WaypointManager/DefaultManager.cpp \ diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index 24ee217d0..023f953d3 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -241,43 +241,66 @@ FlightMap { color: "green" } - // Add Snake tile center points to the map - MapItemView { - property bool _enable: wimaController.enableWimaController.value - && wimaController.enableSnake.value - model: _enable ? wimaController.snakeTileCenterPoints : 0 - - delegate: ProgressIndicator{ - coordinate: modelData - currentValue: getProgress() - width: 10 - height: 10 - z: 1 - - function getProgress() { - var progress = wimaController.nemoProgress[index] - if (progress < 0) - progress = 0 - if (progress > 100) - progress = 100 - return progress - } - } - } +// // Add Snake tile center points to the map +// MapItemView { +// id:progressView +// property bool _enable: wimaController.enableWimaController.value +// && wimaController.enableSnake.value +// property bool valid: wimaController.snakeTileCenterPoints.length +// === wimaController.nemoProgress.length +// model: _enable ? wimaController.snakeTileCenterPoints : 0 + +// delegate: ProgressIndicator{ +// coordinate: modelData +// currentValue: getProgress() +// z: 1 + +// function getProgress() { +// var progress = 0 +// if (progressView.valid){ +// progress = wimaController.nemoProgress[index] +// } +// if (progress < 0) +// progress = 0 +// if (progress > 100) +// progress = 100 +// return progress +// } +// } +// } // Add Snake tiles to the map MapItemView { + id: tileView property bool _enable: wimaController.enableWimaController.value && wimaController.enableSnake.value + property bool valid: wimaController.snakeTileCenterPoints.length + === wimaController.nemoProgress.length model: _enable ? wimaController.snakeTiles : 0 delegate: MapPolygon{ path: object.path; border.color: "black" border.width: 1 - color: "transparent" - opacity: 1 + color: getColor() + opacity: 0.6 z: 2 + + function getColor() { + var progress = 0 + if (tileView.valid){ + progress = wimaController.nemoProgress[index] + } + if (progress < 25) + return "transparent" + if (progress < 50) + return "orange" + if (progress < 75) + return "yellow" + if (progress < 100) + return "greenyellow" + return "limegreen" + } } } diff --git a/src/MissionManager/MissionItem.cc b/src/MissionManager/MissionItem.cc index f495f6a84..a924fa2b2 100644 --- a/src/MissionManager/MissionItem.cc +++ b/src/MissionManager/MissionItem.cc @@ -7,476 +7,460 @@ * ****************************************************************************/ - -#include #include +#include -#include "MissionItem.h" #include "FirmwarePluginManager.h" -#include "QGCApplication.h" #include "JsonHelper.h" +#include "MissionItem.h" +#include "QGCApplication.h" #include "VisualMissionItem.h" -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"; +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"; - -MissionItem::MissionItem(QObject* parent) - : QObject(parent) - , _sequenceNumber(0) - , _doJumpId(-1) - , _isCurrentItem(false) - , _autoContinueFact (0, "AutoContinue", FactMetaData::valueTypeUint32) - , _commandFact (0, "", FactMetaData::valueTypeUint32) - , _frameFact (0, "", FactMetaData::valueTypeUint32) - , _param1Fact (0, "Param1:", FactMetaData::valueTypeDouble) - , _param2Fact (0, "Param2:", FactMetaData::valueTypeDouble) - , _param3Fact (0, "Param3:", FactMetaData::valueTypeDouble) - , _param4Fact (0, "Param4:", FactMetaData::valueTypeDouble) - , _param5Fact (0, "Latitude:", FactMetaData::valueTypeDouble) - , _param6Fact (0, "Longitude:", FactMetaData::valueTypeDouble) - , _param7Fact (0, "Altitude:", FactMetaData::valueTypeDouble) -{ - // Need a good command and frame before we start passing signals around - _commandFact.setRawValue(MAV_CMD_NAV_WAYPOINT); - _frameFact.setRawValue(MAV_FRAME_GLOBAL_RELATIVE_ALT); - - setAutoContinue(true); - - connect(&_param1Fact, &Fact::rawValueChanged, this, &MissionItem::_param1Changed); - connect(&_param2Fact, &Fact::rawValueChanged, this, &MissionItem::_param2Changed); - connect(&_param3Fact, &Fact::rawValueChanged, this, &MissionItem::_param3Changed); -} - -MissionItem::MissionItem(int sequenceNumber, - MAV_CMD command, - MAV_FRAME frame, - double param1, - double param2, - double param3, - double param4, - double param5, - double param6, - double param7, - bool autoContinue, - bool isCurrentItem, - QObject* parent) - : QObject(parent) - , _sequenceNumber(sequenceNumber) - , _doJumpId(-1) - , _isCurrentItem(isCurrentItem) - , _commandFact (0, "", FactMetaData::valueTypeUint32) - , _frameFact (0, "", FactMetaData::valueTypeUint32) - , _param1Fact (0, "Param1:", FactMetaData::valueTypeDouble) - , _param2Fact (0, "Param2:", FactMetaData::valueTypeDouble) - , _param3Fact (0, "Param3:", FactMetaData::valueTypeDouble) - , _param4Fact (0, "Param4:", FactMetaData::valueTypeDouble) - , _param5Fact (0, "Lat/X:", FactMetaData::valueTypeDouble) - , _param6Fact (0, "Lon/Y:", FactMetaData::valueTypeDouble) - , _param7Fact (0, "Alt/Z:", FactMetaData::valueTypeDouble) -{ - // Need a good command and frame before we start passing signals around - _commandFact.setRawValue(MAV_CMD_NAV_WAYPOINT); - _frameFact.setRawValue(MAV_FRAME_GLOBAL_RELATIVE_ALT); - - setCommand(command); - setFrame(frame); - setAutoContinue(autoContinue); - - _param1Fact.setRawValue(param1); - _param2Fact.setRawValue(param2); - _param3Fact.setRawValue(param3); - _param4Fact.setRawValue(param4); - _param5Fact.setRawValue(param5); - _param6Fact.setRawValue(param6); - _param7Fact.setRawValue(param7); - - connect(&_param2Fact, &Fact::rawValueChanged, this, &MissionItem::_param2Changed); - connect(&_param3Fact, &Fact::rawValueChanged, this, &MissionItem::_param3Changed); +const char *MissionItem::_jsonParam1Key = "param1"; +const char *MissionItem::_jsonParam2Key = "param2"; +const char *MissionItem::_jsonParam3Key = "param3"; +const char *MissionItem::_jsonParam4Key = "param4"; + +MissionItem::MissionItem(QObject *parent) + : QObject(parent), + _autoContinueFact(0, "AutoContinue", FactMetaData::valueTypeUint32), + _commandFact(0, "", FactMetaData::valueTypeUint32), + _frameFact(0, "", FactMetaData::valueTypeUint32), + _param1Fact(0, "Param1:", FactMetaData::valueTypeDouble), + _param2Fact(0, "Param2:", FactMetaData::valueTypeDouble), + _param3Fact(0, "Param3:", FactMetaData::valueTypeDouble), + _param4Fact(0, "Param4:", FactMetaData::valueTypeDouble), + _param5Fact(0, "Latitude:", FactMetaData::valueTypeDouble), + _param6Fact(0, "Longitude:", FactMetaData::valueTypeDouble), + _param7Fact(0, "Altitude:", FactMetaData::valueTypeDouble), + _sequenceNumber(0), _doJumpId(-1), _isCurrentItem(false) { + // Need a good command and frame before we start passing signals around + _commandFact.setRawValue(MAV_CMD_NAV_WAYPOINT); + _frameFact.setRawValue(MAV_FRAME_GLOBAL_RELATIVE_ALT); + + setAutoContinue(true); + + connect(&_param1Fact, &Fact::rawValueChanged, this, + &MissionItem::_param1Changed); + connect(&_param2Fact, &Fact::rawValueChanged, this, + &MissionItem::_param2Changed); + connect(&_param3Fact, &Fact::rawValueChanged, this, + &MissionItem::_param3Changed); } -MissionItem::MissionItem(const MissionItem& other, QObject* parent) - : QObject(parent) - , _sequenceNumber(0) - , _doJumpId(-1) - , _isCurrentItem(false) - , _commandFact (0, "", FactMetaData::valueTypeUint32) - , _frameFact (0, "", FactMetaData::valueTypeUint32) - , _param1Fact (0, "Param1:", FactMetaData::valueTypeDouble) - , _param2Fact (0, "Param2:", FactMetaData::valueTypeDouble) - , _param3Fact (0, "Param3:", FactMetaData::valueTypeDouble) - , _param4Fact (0, "Param4:", FactMetaData::valueTypeDouble) - , _param5Fact (0, "Lat/X:", FactMetaData::valueTypeDouble) - , _param6Fact (0, "Lon/Y:", FactMetaData::valueTypeDouble) - , _param7Fact (0, "Alt/Z:", FactMetaData::valueTypeDouble) -{ - // Need a good command and frame before we start passing signals around - _commandFact.setRawValue(MAV_CMD_NAV_WAYPOINT); - _frameFact.setRawValue(MAV_FRAME_GLOBAL_RELATIVE_ALT); - - *this = other; - - connect(&_param2Fact, &Fact::rawValueChanged, this, &MissionItem::_param2Changed); - connect(&_param3Fact, &Fact::rawValueChanged, this, &MissionItem::_param3Changed); +MissionItem::MissionItem(int sequenceNumber, MAV_CMD command, MAV_FRAME frame, + double param1, double param2, double param3, + double param4, double param5, double param6, + double param7, bool autoContinue, bool isCurrentItem, + QObject *parent) + : QObject(parent), _commandFact(0, "", FactMetaData::valueTypeUint32), + _frameFact(0, "", FactMetaData::valueTypeUint32), + _param1Fact(0, "Param1:", FactMetaData::valueTypeDouble), + _param2Fact(0, "Param2:", FactMetaData::valueTypeDouble), + _param3Fact(0, "Param3:", FactMetaData::valueTypeDouble), + _param4Fact(0, "Param4:", FactMetaData::valueTypeDouble), + _param5Fact(0, "Lat/X:", FactMetaData::valueTypeDouble), + _param6Fact(0, "Lon/Y:", FactMetaData::valueTypeDouble), + _param7Fact(0, "Alt/Z:", FactMetaData::valueTypeDouble), + _sequenceNumber(sequenceNumber), _doJumpId(-1), + _isCurrentItem(isCurrentItem) { + // Need a good command and frame before we start passing signals around + _commandFact.setRawValue(MAV_CMD_NAV_WAYPOINT); + _frameFact.setRawValue(MAV_FRAME_GLOBAL_RELATIVE_ALT); + + setCommand(command); + setFrame(frame); + setAutoContinue(autoContinue); + + _param1Fact.setRawValue(param1); + _param2Fact.setRawValue(param2); + _param3Fact.setRawValue(param3); + _param4Fact.setRawValue(param4); + _param5Fact.setRawValue(param5); + _param6Fact.setRawValue(param6); + _param7Fact.setRawValue(param7); + + connect(&_param2Fact, &Fact::rawValueChanged, this, + &MissionItem::_param2Changed); + connect(&_param3Fact, &Fact::rawValueChanged, this, + &MissionItem::_param3Changed); } -const MissionItem& MissionItem::operator=(const MissionItem& other) -{ - _doJumpId = other._doJumpId; - - setCommand(other.command()); - setFrame(other.frame()); - setSequenceNumber(other._sequenceNumber); - setAutoContinue(other.autoContinue()); - setIsCurrentItem(other._isCurrentItem); - - _param1Fact.setRawValue(other._param1Fact.rawValue()); - _param2Fact.setRawValue(other._param2Fact.rawValue()); - _param3Fact.setRawValue(other._param3Fact.rawValue()); - _param4Fact.setRawValue(other._param4Fact.rawValue()); - _param5Fact.setRawValue(other._param5Fact.rawValue()); - _param6Fact.setRawValue(other._param6Fact.rawValue()); - _param7Fact.setRawValue(other._param7Fact.rawValue()); - - return *this; +MissionItem::MissionItem(const MissionItem &other, QObject *parent) + : QObject(parent), _commandFact(0, "", FactMetaData::valueTypeUint32), + _frameFact(0, "", FactMetaData::valueTypeUint32), + _param1Fact(0, "Param1:", FactMetaData::valueTypeDouble), + _param2Fact(0, "Param2:", FactMetaData::valueTypeDouble), + _param3Fact(0, "Param3:", FactMetaData::valueTypeDouble), + _param4Fact(0, "Param4:", FactMetaData::valueTypeDouble), + _param5Fact(0, "Lat/X:", FactMetaData::valueTypeDouble), + _param6Fact(0, "Lon/Y:", FactMetaData::valueTypeDouble), + _param7Fact(0, "Alt/Z:", FactMetaData::valueTypeDouble), + _sequenceNumber(0), _doJumpId(-1), _isCurrentItem(false) { + // Need a good command and frame before we start passing signals around + _commandFact.setRawValue(MAV_CMD_NAV_WAYPOINT); + _frameFact.setRawValue(MAV_FRAME_GLOBAL_RELATIVE_ALT); + + *this = other; + + connect(&_param2Fact, &Fact::rawValueChanged, this, + &MissionItem::_param2Changed); + connect(&_param3Fact, &Fact::rawValueChanged, this, + &MissionItem::_param3Changed); } -MissionItem::~MissionItem() -{ +const MissionItem &MissionItem::operator=(const MissionItem &other) { + _doJumpId = other._doJumpId; -} + setCommand(other.command()); + setFrame(other.frame()); + setSequenceNumber(other._sequenceNumber); + setAutoContinue(other.autoContinue()); + setIsCurrentItem(other._isCurrentItem); -void MissionItem::save(QJsonObject& json) const -{ - json[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeSimpleItemValue; - json[_jsonFrameKey] = frame(); - json[_jsonCommandKey] = command(); - json[_jsonAutoContinueKey] = autoContinue(); - json[_jsonDoJumpIdKey] = _sequenceNumber; + _param1Fact.setRawValue(other._param1Fact.rawValue()); + _param2Fact.setRawValue(other._param2Fact.rawValue()); + _param3Fact.setRawValue(other._param3Fact.rawValue()); + _param4Fact.setRawValue(other._param4Fact.rawValue()); + _param5Fact.setRawValue(other._param5Fact.rawValue()); + _param6Fact.setRawValue(other._param6Fact.rawValue()); + _param7Fact.setRawValue(other._param7Fact.rawValue()); - QJsonArray rgParams = { param1(), param2(), param3(), param4(), param5(), param6(), param7() }; - json[_jsonParamsKey] = rgParams; + return *this; } -bool MissionItem::load(QTextStream &loadStream) -{ - const QStringList &wpParams = loadStream.readLine().split("\t"); - if (wpParams.size() == 12) { - setCommand((MAV_CMD)wpParams[3].toInt()); // Has to be first since it triggers defaults to be set, which are then override by below set calls - setSequenceNumber(wpParams[0].toInt()); - setIsCurrentItem(wpParams[1].toInt() == 1 ? true : false); - setFrame((MAV_FRAME)wpParams[2].toInt()); - setParam1(wpParams[4].toDouble()); - setParam2(wpParams[5].toDouble()); - setParam3(wpParams[6].toDouble()); - setParam4(wpParams[7].toDouble()); - setParam5(wpParams[8].toDouble()); - setParam6(wpParams[9].toDouble()); - setParam7(wpParams[10].toDouble()); - setAutoContinue(wpParams[11].toInt() == 1 ? true : false); - return true; - } +MissionItem::~MissionItem() {} - return false; +void MissionItem::save(QJsonObject &json) const { + json[VisualMissionItem::jsonTypeKey] = + VisualMissionItem::jsonTypeSimpleItemValue; + json[_jsonFrameKey] = frame(); + json[_jsonCommandKey] = command(); + json[_jsonAutoContinueKey] = autoContinue(); + json[_jsonDoJumpIdKey] = _sequenceNumber; + + QJsonArray rgParams = {param1(), param2(), param3(), param4(), + param5(), param6(), param7()}; + json[_jsonParamsKey] = rgParams; } -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; - } +bool MissionItem::load(QTextStream &loadStream) { + const QStringList &wpParams = loadStream.readLine().split("\t"); + if (wpParams.size() == 12) { + setCommand( + (MAV_CMD)wpParams[3] + .toInt()); // Has to be first since it triggers defaults to be set, + // which are then override by below set calls + setSequenceNumber(wpParams[0].toInt()); + setIsCurrentItem(wpParams[1].toInt() == 1 ? true : false); + setFrame((MAV_FRAME)wpParams[2].toInt()); + setParam1(wpParams[4].toDouble()); + setParam2(wpParams[5].toDouble()); + setParam3(wpParams[6].toDouble()); + setParam4(wpParams[7].toDouble()); + setParam5(wpParams[8].toDouble()); + setParam6(wpParams[9].toDouble()); + setParam7(wpParams[10].toDouble()); + setAutoContinue(wpParams[11].toInt() == 1 ? true : false); + return true; + } - if (v2Json[VisualMissionItem::jsonTypeKey].toString() == QStringLiteral("missionItem")) { - v2Json[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeSimpleItemValue; - } + return false; +} - 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); +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::_convertJsonV2ToV3(QJsonObject& json, QString& errorString) -{ - // V2 format: param 5/6/7 stored in GeoCoordinate - // V3 format: param 5/6/7 stored in params array - - if (!json.contains(_jsonCoordinateKey)) { - // Already V3 format - return true; - } - - QList keyInfoList = { - { _jsonCoordinateKey, QJsonValue::Array, true }, - }; - if (!JsonHelper::validateKeys(json, keyInfoList, errorString)) { - return false; - } - - QGeoCoordinate coordinate; - if (!JsonHelper::loadGeoCoordinate(json[_jsonCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) { - return false; - } - - QJsonArray rgParam = json[_jsonParamsKey].toArray(); - rgParam.append(coordinate.latitude()); - rgParam.append(coordinate.longitude()); - rgParam.append(coordinate.altitude()); - json[_jsonParamsKey] = rgParam; - - json.remove(_jsonCoordinateKey); +bool MissionItem::_convertJsonV2ToV3(QJsonObject &json, QString &errorString) { + // V2 format: param 5/6/7 stored in GeoCoordinate + // V3 format: param 5/6/7 stored in params array + if (!json.contains(_jsonCoordinateKey)) { + // Already V3 format return true; -} - -bool MissionItem::load(const QJsonObject& json, int sequenceNumber, QString& errorString) -{ - QJsonObject convertedJson; - if (!_convertJsonV1ToV2(json, convertedJson, errorString)) { - return false; - } - if (!_convertJsonV2ToV3(convertedJson, 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 }, - { _jsonDoJumpIdKey, QJsonValue::Double, false }, - }; - if (!JsonHelper::validateKeys(convertedJson, keyInfoList, errorString)) { - return false; - } + QList keyInfoList = { + {_jsonCoordinateKey, QJsonValue::Array, true}, + }; + if (!JsonHelper::validateKeys(json, keyInfoList, errorString)) { + return false; + } - if (convertedJson[VisualMissionItem::jsonTypeKey] != VisualMissionItem::jsonTypeSimpleItemValue) { - errorString = tr("Type found: %1 must be: %2").arg(convertedJson[VisualMissionItem::jsonTypeKey].toString()).arg(VisualMissionItem::jsonTypeSimpleItemValue); - return false; - } + QGeoCoordinate coordinate; + if (!JsonHelper::loadGeoCoordinate(json[_jsonCoordinateKey], + true /* altitudeRequired */, coordinate, + errorString)) { + return false; + } - QJsonArray rgParams = convertedJson[_jsonParamsKey].toArray(); - if (rgParams.count() != 7) { - errorString = tr("%1 key must contains 7 values").arg(_jsonParamsKey); - return false; - } + QJsonArray rgParam = json[_jsonParamsKey].toArray(); + rgParam.append(coordinate.latitude()); + rgParam.append(coordinate.longitude()); + rgParam.append(coordinate.altitude()); + json[_jsonParamsKey] = rgParam; - for (int i=0; i<4; i++) { - if (rgParams[i].type() != QJsonValue::Double && rgParams[i].type() != QJsonValue::Null) { - errorString = tr("Param %1 incorrect type %2, must be double or null").arg(i+1).arg(rgParams[i].type()); - return false; - } - } + json.remove(_jsonCoordinateKey); - // Make sure to set these first since they can signal other changes - setCommand((MAV_CMD)convertedJson[_jsonCommandKey].toInt()); - setFrame((MAV_FRAME)convertedJson[_jsonFrameKey].toInt()); + return true; +} - _doJumpId = -1; - if (convertedJson.contains(_jsonDoJumpIdKey)) { - _doJumpId = convertedJson[_jsonDoJumpIdKey].toInt(); - } - setIsCurrentItem(false); - setSequenceNumber(sequenceNumber); - setAutoContinue(convertedJson[_jsonAutoContinueKey].toBool()); - - setParam1(JsonHelper::possibleNaNJsonValue(rgParams[0])); - setParam2(JsonHelper::possibleNaNJsonValue(rgParams[1])); - setParam3(JsonHelper::possibleNaNJsonValue(rgParams[2])); - setParam4(JsonHelper::possibleNaNJsonValue(rgParams[3])); - setParam5(JsonHelper::possibleNaNJsonValue(rgParams[4])); - setParam6(JsonHelper::possibleNaNJsonValue(rgParams[5])); - setParam7(JsonHelper::possibleNaNJsonValue(rgParams[6])); +bool MissionItem::load(const QJsonObject &json, int sequenceNumber, + QString &errorString) { + QJsonObject convertedJson; + if (!_convertJsonV1ToV2(json, convertedJson, errorString)) { + return false; + } + if (!_convertJsonV2ToV3(convertedJson, 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}, + {_jsonDoJumpIdKey, QJsonValue::Double, false}, + }; + if (!JsonHelper::validateKeys(convertedJson, keyInfoList, errorString)) { + return false; + } + + if (convertedJson[VisualMissionItem::jsonTypeKey] != + VisualMissionItem::jsonTypeSimpleItemValue) { + errorString = + tr("Type found: %1 must be: %2") + .arg(convertedJson[VisualMissionItem::jsonTypeKey].toString()) + .arg(VisualMissionItem::jsonTypeSimpleItemValue); + return false; + } - return true; + QJsonArray rgParams = convertedJson[_jsonParamsKey].toArray(); + if (rgParams.count() != 7) { + errorString = tr("%1 key must contains 7 values").arg(_jsonParamsKey); + return false; + } + + for (int i = 0; i < 4; i++) { + if (rgParams[i].type() != QJsonValue::Double && + rgParams[i].type() != QJsonValue::Null) { + errorString = tr("Param %1 incorrect type %2, must be double or null") + .arg(i + 1) + .arg(rgParams[i].type()); + return false; + } + } + + // Make sure to set these first since they can signal other changes + setCommand((MAV_CMD)convertedJson[_jsonCommandKey].toInt()); + setFrame((MAV_FRAME)convertedJson[_jsonFrameKey].toInt()); + + _doJumpId = -1; + if (convertedJson.contains(_jsonDoJumpIdKey)) { + _doJumpId = convertedJson[_jsonDoJumpIdKey].toInt(); + } + setIsCurrentItem(false); + setSequenceNumber(sequenceNumber); + setAutoContinue(convertedJson[_jsonAutoContinueKey].toBool()); + + setParam1(JsonHelper::possibleNaNJsonValue(rgParams[0])); + setParam2(JsonHelper::possibleNaNJsonValue(rgParams[1])); + setParam3(JsonHelper::possibleNaNJsonValue(rgParams[2])); + setParam4(JsonHelper::possibleNaNJsonValue(rgParams[3])); + setParam5(JsonHelper::possibleNaNJsonValue(rgParams[4])); + setParam6(JsonHelper::possibleNaNJsonValue(rgParams[5])); + setParam7(JsonHelper::possibleNaNJsonValue(rgParams[6])); + + return true; } - -void MissionItem::setSequenceNumber(int sequenceNumber) -{ - if (_sequenceNumber != sequenceNumber) { - _sequenceNumber = sequenceNumber; - emit sequenceNumberChanged(_sequenceNumber); - } +void MissionItem::setSequenceNumber(int sequenceNumber) { + if (_sequenceNumber != sequenceNumber) { + _sequenceNumber = sequenceNumber; + emit sequenceNumberChanged(_sequenceNumber); + } } -void MissionItem::setCommand(MAV_CMD command) -{ - if ((MAV_CMD)this->command() != command) { - _commandFact.setRawValue(command); - } +void MissionItem::setCommand(MAV_CMD command) { + if ((MAV_CMD)this->command() != command) { + _commandFact.setRawValue(command); + } } -void MissionItem::setFrame(MAV_FRAME frame) -{ - if (this->frame() != frame) { - _frameFact.setRawValue(frame); - } +void MissionItem::setFrame(MAV_FRAME frame) { + if (this->frame() != frame) { + _frameFact.setRawValue(frame); + } } -void MissionItem::setAutoContinue(bool autoContinue) -{ - if (this->autoContinue() != autoContinue) { - _autoContinueFact.setRawValue(autoContinue); - } +void MissionItem::setAutoContinue(bool autoContinue) { + if (this->autoContinue() != autoContinue) { + _autoContinueFact.setRawValue(autoContinue); + } } -void MissionItem::setIsCurrentItem(bool isCurrentItem) -{ - if (_isCurrentItem != isCurrentItem) { - _isCurrentItem = isCurrentItem; - emit isCurrentItemChanged(isCurrentItem); - } +void MissionItem::setIsCurrentItem(bool isCurrentItem) { + if (_isCurrentItem != isCurrentItem) { + _isCurrentItem = isCurrentItem; + emit isCurrentItemChanged(isCurrentItem); + } } -void MissionItem::setParam1(double param) -{ - if (param1() != param) { - _param1Fact.setRawValue(param); - } +void MissionItem::setParam1(double param) { + if (param1() != param) { + _param1Fact.setRawValue(param); + } } -void MissionItem::setParam2(double param) -{ - if (param2() != param) { - _param2Fact.setRawValue(param); - } +void MissionItem::setParam2(double param) { + if (param2() != param) { + _param2Fact.setRawValue(param); + } } -void MissionItem::setParam3(double param) -{ - if (param3() != param) { - _param3Fact.setRawValue(param); - } +void MissionItem::setParam3(double param) { + if (param3() != param) { + _param3Fact.setRawValue(param); + } } -void MissionItem::setParam4(double param) -{ - if (param4() != param) { - _param4Fact.setRawValue(param); - } +void MissionItem::setParam4(double param) { + if (param4() != param) { + _param4Fact.setRawValue(param); + } } -void MissionItem::setParam5(double param) -{ - if (param5() != param) { - _param5Fact.setRawValue(param); - } +void MissionItem::setParam5(double param) { + if (param5() != param) { + _param5Fact.setRawValue(param); + } } -void MissionItem::setParam6(double param) -{ - if (param6() != param) { - _param6Fact.setRawValue(param); - } +void MissionItem::setParam6(double param) { + if (param6() != param) { + _param6Fact.setRawValue(param); + } } -void MissionItem::setParam7(double param) -{ - if (param7() != param) { - _param7Fact.setRawValue(param); - } +void MissionItem::setParam7(double param) { + if (param7() != param) { + _param7Fact.setRawValue(param); + } } -QGeoCoordinate MissionItem::coordinate(void) const -{ - if(!std::isfinite(param5()) || !std::isfinite(param6())) { - //-- If either of these are NAN, return an invalid (QGeoCoordinate::isValid() == false) coordinate - return QGeoCoordinate(); - } - return QGeoCoordinate(param5(), param6(), param7()); +QGeoCoordinate MissionItem::coordinate(void) const { + if (!std::isfinite(param5()) || !std::isfinite(param6())) { + //-- If either of these are NAN, return an invalid + //(QGeoCoordinate::isValid() == false) coordinate + return QGeoCoordinate(); + } + return QGeoCoordinate(param5(), param6(), param7()); } -double MissionItem::specifiedFlightSpeed(void) const -{ - double flightSpeed = std::numeric_limits::quiet_NaN(); +double MissionItem::specifiedFlightSpeed(void) const { + double flightSpeed = std::numeric_limits::quiet_NaN(); - if (_commandFact.rawValue().toInt() == MAV_CMD_DO_CHANGE_SPEED && _param2Fact.rawValue().toDouble() > 0) { - flightSpeed = _param2Fact.rawValue().toDouble(); - } + if (_commandFact.rawValue().toInt() == MAV_CMD_DO_CHANGE_SPEED && + _param2Fact.rawValue().toDouble() > 0) { + flightSpeed = _param2Fact.rawValue().toDouble(); + } - return flightSpeed; + return flightSpeed; } -double MissionItem::specifiedGimbalYaw(void) const -{ - double gimbalYaw = std::numeric_limits::quiet_NaN(); +double MissionItem::specifiedGimbalYaw(void) const { + double gimbalYaw = std::numeric_limits::quiet_NaN(); - if (_commandFact.rawValue().toInt() == MAV_CMD_DO_MOUNT_CONTROL && _param7Fact.rawValue().toInt() == MAV_MOUNT_MODE_MAVLINK_TARGETING) { - gimbalYaw = _param3Fact.rawValue().toDouble(); - } + if (_commandFact.rawValue().toInt() == MAV_CMD_DO_MOUNT_CONTROL && + _param7Fact.rawValue().toInt() == MAV_MOUNT_MODE_MAVLINK_TARGETING) { + gimbalYaw = _param3Fact.rawValue().toDouble(); + } - return gimbalYaw; + return gimbalYaw; } -double MissionItem::specifiedGimbalPitch(void) const -{ - double gimbalPitch = std::numeric_limits::quiet_NaN(); +double MissionItem::specifiedGimbalPitch(void) const { + double gimbalPitch = std::numeric_limits::quiet_NaN(); - if (_commandFact.rawValue().toInt() == MAV_CMD_DO_MOUNT_CONTROL && _param7Fact.rawValue().toInt() == MAV_MOUNT_MODE_MAVLINK_TARGETING) { - gimbalPitch = _param1Fact.rawValue().toDouble(); - } + if (_commandFact.rawValue().toInt() == MAV_CMD_DO_MOUNT_CONTROL && + _param7Fact.rawValue().toInt() == MAV_MOUNT_MODE_MAVLINK_TARGETING) { + gimbalPitch = _param1Fact.rawValue().toDouble(); + } - return gimbalPitch; + return gimbalPitch; } -void MissionItem::_param1Changed(QVariant value) -{ - Q_UNUSED(value); +void MissionItem::_param1Changed(QVariant value) { + Q_UNUSED(value); - double gimbalPitch = specifiedGimbalPitch(); - if (!qIsNaN(gimbalPitch)) { - emit specifiedGimbalPitchChanged(gimbalPitch); - } + double gimbalPitch = specifiedGimbalPitch(); + if (!qIsNaN(gimbalPitch)) { + emit specifiedGimbalPitchChanged(gimbalPitch); + } } -void MissionItem::_param2Changed(QVariant value) -{ - Q_UNUSED(value); +void MissionItem::_param2Changed(QVariant value) { + Q_UNUSED(value); - double flightSpeed = specifiedFlightSpeed(); - if (!qIsNaN(flightSpeed)) { - emit specifiedFlightSpeedChanged(flightSpeed); - } + double flightSpeed = specifiedFlightSpeed(); + if (!qIsNaN(flightSpeed)) { + emit specifiedFlightSpeedChanged(flightSpeed); + } } -void MissionItem::_param3Changed(QVariant value) -{ - Q_UNUSED(value); +void MissionItem::_param3Changed(QVariant value) { + Q_UNUSED(value); - double gimbalYaw = specifiedGimbalYaw(); - if (!qIsNaN(gimbalYaw)) { - emit specifiedGimbalYawChanged(gimbalYaw); - } + double gimbalYaw = specifiedGimbalYaw(); + if (!qIsNaN(gimbalYaw)) { + emit specifiedGimbalYawChanged(gimbalYaw); + } } diff --git a/src/QmlControls/QmlObjectListModel.cc b/src/QmlControls/QmlObjectListModel.cc index 1df531a99..e5ba428b8 100644 --- a/src/QmlControls/QmlObjectListModel.cc +++ b/src/QmlControls/QmlObjectListModel.cc @@ -121,9 +121,9 @@ const QObject *QmlObjectListModel::operator[](int index) const { return _objectList[index]; } -bool QmlObjectListModel::operator==(const QmlObjectListModel &other) { +bool QmlObjectListModel::operator==(const QmlObjectListModel &other) const { if (this->count() == other.count()) { - for (std::size_t i = 0; i < this->count(); ++i) { + for (int i = 0; i < this->count(); ++i) { if (this->get(i) != other.get(i)) { return false; } @@ -134,7 +134,7 @@ bool QmlObjectListModel::operator==(const QmlObjectListModel &other) { } } -bool QmlObjectListModel::operator==(const QmlObjectListModel &other) { +bool QmlObjectListModel::operator!=(const QmlObjectListModel &other) const { return !this->operator==(other); } diff --git a/src/QmlControls/QmlObjectListModel.h b/src/QmlControls/QmlObjectListModel.h index 5c7060d24..8f45fa09e 100644 --- a/src/QmlControls/QmlObjectListModel.h +++ b/src/QmlControls/QmlObjectListModel.h @@ -51,8 +51,8 @@ public: } QList *objectList() { return &_objectList; } - bool operator==(const QmlObjectListModel &other); - bool operator!=(const QmlObjectListModel &other); + bool operator==(const QmlObjectListModel &other) const; + bool operator!=(const QmlObjectListModel &other) const; /// Calls deleteLater on all items and this itself. void deleteListAndContents(); diff --git a/src/Wima/CircularSurvey.cc b/src/Wima/CircularSurvey.cc index 87beb9518..c45be2c66 100644 --- a/src/Wima/CircularSurvey.cc +++ b/src/Wima/CircularSurvey.cc @@ -359,7 +359,12 @@ void CircularSurvey::_rebuildTransectsPhase1(void) { // Store raw transects. const auto &transectsENU = this->_workerOutput->transects; const auto &ori = this->_referencePoint; - for (auto &t : transectsENU) { + std::size_t startIdx = 0; + if (transectsENU.size() > 0 && transectsENU.front().size() == 1) { + startIdx = 1; + } + for (std::size_t i = startIdx; i < transectsENU.size(); ++i) { + const auto &t = transectsENU[i]; QList trGeo; for (auto &v : t) { QGeoCoordinate c; diff --git a/src/Wima/Geometry/WimaMeasurementArea.cc b/src/Wima/Geometry/WimaMeasurementArea.cc index 6ff14fd93..3ec518ea6 100644 --- a/src/Wima/Geometry/WimaMeasurementArea.cc +++ b/src/Wima/Geometry/WimaMeasurementArea.cc @@ -28,6 +28,15 @@ TileData &TileData::operator=(const TileData &other) { return *this; } +bool TileData::operator==(const TileData &other) const { + return this->tiles == other.tiles && + this->tileCenterPoints == other.tileCenterPoints; +} + +bool TileData::operator!=(const TileData &other) const { + return !this->operator==(other); +} + void TileData::clear() { this->tiles.clearAndDeleteContents(); this->tileCenterPoints.clear(); diff --git a/src/Wima/Geometry/WimaMeasurementArea.h b/src/Wima/Geometry/WimaMeasurementArea.h index cac4482a9..2a82baed7 100644 --- a/src/Wima/Geometry/WimaMeasurementArea.h +++ b/src/Wima/Geometry/WimaMeasurementArea.h @@ -17,6 +17,8 @@ public: ~TileData(); TileData &operator=(const TileData &other); + bool operator==(const TileData &other) const; + bool operator!=(const TileData &other) const; void clear(); std::size_t size() const; diff --git a/src/Wima/Geometry/WimaMeasurementAreaData.cc b/src/Wima/Geometry/WimaMeasurementAreaData.cc index 0fc241f54..879b0fde4 100644 --- a/src/Wima/Geometry/WimaMeasurementAreaData.cc +++ b/src/Wima/Geometry/WimaMeasurementAreaData.cc @@ -61,10 +61,10 @@ QVariantList &WimaMeasurementAreaData::tileCenterPoints() { } const TileData &WimaMeasurementAreaData::tileData() const { - return this->tileData(); + return this->_tileData; } -TileData &WimaMeasurementAreaData::tileData() { return this->tileData(); } +TileData &WimaMeasurementAreaData::tileData() { return this->_tileData; } const QVector &WimaMeasurementAreaData::progress() const { return this->_progress; diff --git a/src/Wima/RoutingThread.cpp b/src/Wima/RoutingThread.cpp index 4c25fed3a..8461ea773 100644 --- a/src/Wima/RoutingThread.cpp +++ b/src/Wima/RoutingThread.cpp @@ -21,7 +21,7 @@ RoutingThread::~RoutingThread() { this->wait(); } -bool RoutingThread::calculating() { return this->_calculating; } +bool RoutingThread::calculating() const { return this->_calculating; } void RoutingThread::route(const snake::BoostPolygon &safeArea, const RoutingThread::Generator &generator) { @@ -107,7 +107,7 @@ void RoutingThread::run() { } // end calculation #ifdef DEBUG_CIRCULAR_SURVEY else { - qWarning() << "RoutingWorker::run(): preconditions failed."; + qWarning() << "RoutingWorker::run(): generator() failed."; } #endif #ifdef SHOW_CIRCULAR_SURVEY_TIME diff --git a/src/Wima/RoutingThread.h b/src/Wima/RoutingThread.h index 963941c92..b725a1805 100644 --- a/src/Wima/RoutingThread.h +++ b/src/Wima/RoutingThread.h @@ -27,11 +27,12 @@ class RoutingThread : public QThread { public: using PtrRoutingData = QSharedPointer; using Generator = std::function; + using Consumer = std::function; RoutingThread(QObject *parent = nullptr); ~RoutingThread() override; - bool calculating(); + bool calculating() const; public slots: void route(const snake::BoostPolygon &safeArea, const Generator &generator); diff --git a/src/Wima/Snake/NemoInterface.cpp b/src/Wima/Snake/NemoInterface.cpp index 2aa69d4e2..199b194f5 100644 --- a/src/Wima/Snake/NemoInterface.cpp +++ b/src/Wima/Snake/NemoInterface.cpp @@ -1,4 +1,5 @@ #include "NemoInterface.h" +#include "SnakeTilesLocal.h" #include "QGCApplication.h" #include "QGCToolbox.h" @@ -135,12 +136,12 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) { const auto &origin = this->ENUOrigin; this->tilesENU.polygons().clear(); bool error = false; - for (std::size_t i = 0; i < tileData.tiles.count(); ++i) { - *obj = tileData.tiles.get(i); - *tile = qobject_cast(obj); + for (int i = 0; i < tileData.tiles.count(); ++i) { + obj = tileData.tiles.get(i); + tile = qobject_cast(obj); if (tile != nullptr) { - snake::BoostPolygon tileENU; - snake::areaToEnu(origin, tile->coordinateList(), tileENU); + SnakeTileLocal tileENU; + snake::areaToEnu(origin, tile->coordinateList(), tileENU.path()); this->tilesENU.polygons().push_back(std::move(tileENU)); } else { qWarning() << "NemoInterface::Impl::setTileData(): nullptr."; @@ -162,7 +163,7 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) { } bool NemoInterface::Impl::hasTileData(const TileData &tileData) const { - return this->tileData = tileData; + return this->tileData == tileData; } NemoInterface::NemoStatus NemoInterface::Impl::status() { diff --git a/src/Wima/Snake/NemoInterface.h b/src/Wima/Snake/NemoInterface.h index 5a23fdff7..f7468b4fe 100644 --- a/src/Wima/Snake/NemoInterface.h +++ b/src/Wima/Snake/NemoInterface.h @@ -3,8 +3,6 @@ #include #include -#include "SnakeTilesLocal.h" - #include class TileData; diff --git a/src/Wima/Snake/SnakeTileLocal.h b/src/Wima/Snake/SnakeTileLocal.h index c1bcd2bb6..fa323fa14 100644 --- a/src/Wima/Snake/SnakeTileLocal.h +++ b/src/Wima/Snake/SnakeTileLocal.h @@ -1,4 +1,4 @@ #pragma once #include "Wima/Geometry/GenericPolygon.h" -using SnakeTileLocal = GenericPolygon<>; +using SnakeTileLocal = GenericPolygon; diff --git a/src/Wima/Snake/SnakeTilesLocal.h b/src/Wima/Snake/SnakeTilesLocal.h index 54c8225d6..f7ccdc7d2 100644 --- a/src/Wima/Snake/SnakeTilesLocal.h +++ b/src/Wima/Snake/SnakeTilesLocal.h @@ -1,6 +1,6 @@ #pragma once -#include "Wima/Snake/SnakeTileLocal.h" #include "Wima/Geometry/GenericPolygonArray.h" - -typedef GenericPolygonArray SnakeTilesLocal; +#include "Wima/Snake/SnakeTileLocal.h" +#include +typedef GenericPolygonArray SnakeTilesLocal; diff --git a/src/Wima/Snake/snake.h b/src/Wima/Snake/snake.h index 34279a896..c21c6ee4b 100644 --- a/src/Wima/Snake/snake.h +++ b/src/Wima/Snake/snake.h @@ -92,10 +92,10 @@ struct BoundingBox { BoostPolygon corners; }; -template -void toENU(const GeoPoint &origin, const GeoPoint &in, BoostPoint &out) { - GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), - GeographicLib::Constants::WGS84_f()); +template +void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, BoostPoint &out) { + static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), + GeographicLib::Constants::WGS84_f()); GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(), origin.altitude(), earth); @@ -108,10 +108,26 @@ void toENU(const GeoPoint &origin, const GeoPoint &in, BoostPoint &out) { (void)z; } +template +void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, Point &out) { + static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), + GeographicLib::Constants::WGS84_f()); + GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(), + origin.altitude(), earth); + + double x = 0, y = 0, z = 0; + auto alt = in.altitude(); + alt = std::isnan(alt) ? 0 : alt; + proj.Forward(in.latitude(), in.longitude(), alt, x, y, z); + out.setX(x); + out.setY(y); + (void)z; +} + template void fromENU(const GeoPoint &origin, const BoostPoint &in, GeoPoint &out) { - GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), - GeographicLib::Constants::WGS84_f()); + static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), + GeographicLib::Constants::WGS84_f()); GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(), origin.altitude(), earth); @@ -122,10 +138,18 @@ void fromENU(const GeoPoint &origin, const BoostPoint &in, GeoPoint &out) { out.setAltitude(alt); } -template class Container> -void areaToEnu(const GeoPoint &origin, const Container &in, - BoostPolygon &out) { - for (auto vertex : in) { +template +void areaToEnu(const GeoPoint &origin, const Container1 &in, Container2 &out) { + for (auto &vertex : in) { + typename Container2::value_type p; + toENU(origin, vertex, p); + out.push_back(p); + } +} + +template +void areaToEnu(const GeoPoint &origin, const Container &in, BoostPolygon &out) { + for (auto &vertex : in) { BoostPoint p; toENU(origin, vertex, p); out.outer().push_back(p); @@ -133,11 +157,21 @@ void areaToEnu(const GeoPoint &origin, const Container &in, bg::correct(out); } -template class Container> +template +void areaFromEnu(const GeoPoint &origin, Container1 &in, + const Container2 &out) { + for (auto &vertex : in) { + typename Container2::value_type p; + fromENU(origin, vertex, p); + out.push_back(p); + } +} + +template void areaFromEnu(const GeoPoint &origin, BoostPolygon &in, - const Container &out) { - for (auto vertex : in.outer()) { - GeoPoint p; + const Container &out) { + for (auto &vertex : in.outer()) { + typename Container::value_type p; fromENU(origin, vertex, p); out.push_back(p); } diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 3c0f4c468..d1a8c35b2 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -6,14 +6,20 @@ #include "MissionSettingsItem.h" #include "PlanMasterController.h" #include "QGCApplication.h" +#include "SettingsManager.h" #include "SimpleMissionItem.h" +#include "WimaSettings.h" + #include "Snake/QNemoHeartbeat.h" #include "Snake/QNemoProgress.h" +#include "Snake/SnakeTile.h" #include "QVector3D" #include -#include + +#define CLIPPER_SCALE 10000 +#include "clipper/clipper.hpp" #include @@ -48,7 +54,7 @@ WimaController::StatusMap WimaController::_nemoStatusMap{ WimaController::WimaController(QObject *parent) : QObject(parent), _joinedArea(), _measurementArea(), _serviceArea(), - _corridor(), _localPlanDataValid(false), + _corridor(), _planDataValid(false), _areaInterface(&_measurementArea, &_serviceArea, &_corridor, &_joinedArea), _WMSettings(), _defaultWM(_WMSettings, _areaInterface), @@ -141,6 +147,12 @@ WimaController::WimaController(QObject *parent) connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_switchToSnakeWaypointManager); _switchToSnakeWaypointManager(_enableSnake.rawValue()); + + // Routing + connect(&_routingThread, &RoutingThread::result, this, + &WimaController::_storeRoute); + connect(&_routingThread, &RoutingThread::calculatingChanged, this, + &WimaController::snakeCalcInProgressChanged); } PlanMasterController *WimaController::masterController() { @@ -192,7 +204,7 @@ Fact *WimaController::arrivalReturnSpeed() { return &_arrivalReturnSpeed; } Fact *WimaController::altitude() { return &_altitude; } QmlObjectListModel *WimaController::snakeTiles() { - return &this->_measurementArea.tiles(); + return this->_measurementArea.tiles(); } QVariantList WimaController::snakeTileCenterPoints() { @@ -222,8 +234,7 @@ QString WimaController::nemoStatusString() const { } bool WimaController::snakeCalcInProgress() const { - qWarning() << "using snakeCalcInProgress dummy"; - return true; + return _routingThread.calculating(); } void WimaController::setMasterController(PlanMasterController *masterC) { @@ -393,7 +404,7 @@ bool WimaController::setWimaPlanData(QSharedPointer planData) { emit snakeTilesChanged(); emit nemoProgressChanged(); - _localPlanDataValid = false; + _planDataValid = false; // extract list with WimaAreas QList areaList = planData->areaList(); @@ -452,20 +463,6 @@ bool WimaController::setWimaPlanData(QSharedPointer planData) { emit visualItemsChanged(); emit snakeTilesChanged(); - // Publish tiles if necessary. - if (!this->_nemoInterface.hasTileData(this->_measurementArea.tileData())) { - this->_nemoInterface.publishTileData(this->_measurementArea.tileData()); - this->_measurementArea.progress() = - QVector(this->_measurementArea.tiles()->count, 0); - emit nemoProgressChanged(); - } else if (this->_enableSnake.rawValue().toBool()) { - const auto progess = this->_nemoInterface.progress(); - if (progess.size() == this->_measurementArea.tiles()->count()) { - this->_measurementArea.progress() = std::move(progress); - emit nemoProgressChanged(); - } - } - // Copy raw transects. this->_rawTransects = planData->transects(); @@ -477,9 +474,9 @@ bool WimaController::setWimaPlanData(QSharedPointer planData) { } for (auto *item : tempMissionItems) { + _snakeWM.push_back(item->coordinate()); _defaultWM.push_back(item->coordinate()); } - // // extract mission items _WMSettings.setHomePosition(QGeoCoordinate( _serviceArea.depot().latitude(), _serviceArea.depot().longitude(), 0)); // auto tempMissionItems = planData->missionItems(); @@ -487,15 +484,37 @@ bool WimaController::setWimaPlanData(QSharedPointer planData) { Q_ASSERT(false); return false; } - // if (tempMissionItems.size() < 1) { - emit missionItemsChanged(); - emit currentMissionItemsChanged(); - emit waypointPathChanged(); - emit currentWaypointPathChanged(); + if (!_snakeWM.reset()) { + Q_ASSERT(false); + return false; + } + + if (_currentWM == &_defaultWM || _currentWM == &_snakeWM) { + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit waypointPathChanged(); + emit currentWaypointPathChanged(); + } + + // Publish tiles, set progress if necessary. + if (!this->_nemoInterface.hasTileData(this->_measurementArea.tileData())) { + if (_enableSnake.rawValue().toBool()) { + this->_nemoInterface.publishTileData(this->_measurementArea.tileData()); + } + this->_measurementArea.progress() = + QVector(this->_measurementArea.tiles()->count(), 0); + emit nemoProgressChanged(); - qWarning("WimaController: add snake update here lkjdfl!"); + } else if (this->_enableSnake.rawValue().toBool()) { + const auto progress = this->_nemoInterface.progress(); + if (progress.size() == this->_measurementArea.tiles()->count()) { + this->_measurementArea.progress() = std::move(progress); + emit nemoProgressChanged(); + this->_updateRoute(); + } + } - _localPlanDataValid = true; + _planDataValid = true; return true; } @@ -701,7 +720,7 @@ void WimaController::_setPhaseDuration(double duration) { } bool WimaController::_SRTLPrecondition(QString &errorString) { - if (!_localPlanDataValid) { + if (!_planDataValid) { errorString.append(tr("No WiMA data available. Please define at least a " "measurement and a service area.")); return false; @@ -772,27 +791,61 @@ void WimaController::_initSmartRTL() { } } -void WimaController::_threadFinishedHandler() { - qWarning() << "update here as well .jisddf"; +void WimaController::_storeRoute(RoutingThread::PtrRoutingData data) { +#ifdef SNAKE_SHOW_TIME auto start = std::chrono::high_resolution_clock::now(); #endif // Copy waypoints to waypoint manager. _snakeWM.clear(); - auto waypoints = 1; - if (waypoints.size() < 1) { - return; - } - for (auto &vertex : waypoints) { - _snakeWM.push_back(vertex); - } - // Do update. - this->_snakeWM.update(); // this can take a while (ca. 200ms) + if (data->route.size() > 0) { + // Store route. + const auto &transectsENU = data->transects; + const auto &transectsInfo = data->transectsInfo; + const auto &route = data->route; + // Find index of first waypoint. + std::size_t idxFirst = 0; + const auto &infoFirst = transectsInfo.front(); + const auto &firstTransect = transectsENU[infoFirst.index]; + const auto &firstWaypoint = + infoFirst.reversed ? firstTransect.back() : firstTransect.front(); + double th = 0.001; + for (std::size_t i = 0; i < route.size(); ++i) { + auto dist = bg::distance(route[i], firstWaypoint); + if (dist < th) { + idxFirst = i; + break; + } + } + // Find index of last waypoint. + std::size_t idxLast = route.size() - 1; + const auto &infoLast = transectsInfo.back(); + const auto &lastTransect = transectsENU[infoLast.index]; + const auto &lastWaypoint = + infoLast.reversed ? lastTransect.front() : lastTransect.back(); + for (long i = route.size() - 1; i >= 0; --i) { + auto dist = bg::distance(route[i], lastWaypoint); + if (dist < th) { + idxLast = i; + break; + } + } + // Convert to geo coordinates and append to waypoint manager. + const auto &ori = this->_origin; + for (std::size_t i = idxFirst; i <= idxLast; ++i) { + auto &vertex = route[i]; + QGeoCoordinate c; + snake::fromENU(ori, vertex, c); + _snakeWM.push_back(c); + } + // Do update. + this->_snakeWM.update(); // this can take a while (ca. 200ms) - emit missionItemsChanged(); - emit currentMissionItemsChanged(); - emit currentWaypointPathChanged(); - emit waypointPathChanged(); + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit currentWaypointPathChanged(); + emit waypointPathChanged(); + } #ifdef SNAKE_SHOW_TIME auto end = std::chrono::high_resolution_clock::now(); std::cout << "WimaController::_threadFinishedHandler(): waypoints: " @@ -813,10 +866,11 @@ void WimaController::_switchToSnakeWaypointManager(QVariant variant) { void WimaController::_progressChangedHandler() { const auto progress = this->_nemoInterface.progress(); - if (this->_measurementArea.tiles().count() == progress.size()) { + if (this->_measurementArea.tiles()->count() == progress.size()) { this->_measurementArea.progress() = std::move(progress); emit nemoProgressChanged(); - } else if (_localPlanDataValid) { + this->_updateRoute(); + } else if (_planDataValid) { this->_nemoInterface.publishTileData(this->_measurementArea.tileData()); } } @@ -824,43 +878,134 @@ void WimaController::_progressChangedHandler() { void WimaController::_enableSnakeChangedHandler() { if (this->_enableSnake.rawValue().toBool()) { qDebug() << "WimaController: enabling snake."; + _switchWaypointManager(_snakeWM); this->_nemoInterface.start(); + if (_planDataValid) { + if (!this->_nemoInterface.hasTileData( + this->_measurementArea.tileData())) { + this->_nemoInterface.publishTileData(this->_measurementArea.tileData()); + } + } } else { qDebug() << "WimaController: disabling snake."; + _switchWaypointManager(_defaultWM); this->_nemoInterface.stop(); } } void WimaController::_updateRoute() { - if (_localPlanDataValid && this->_joinedArea.coordinateList().size() > 0) { - auto safeArea = this->_joinedArea.coordinateList(); - for (auto &v : safeArea) { - v.setAltitude(0); - } - const auto &origin = safeArea.front(); - snake::BoostPolygon safeAreaENU; - snake::areaToEnu(origin, safeArea, safeAreaENU); - const auto &depot = this->_serviceArea.depot(); - const auto &geoTransects = this->_rawTransects; - const auto progess = this->_nemoInterface.progress(); - auto pTiles = std::make_shared>>(); - - auto generator = [origin, depot, - geoTransects](snake::Transects &transects) -> bool { - snake::BoostPolygon depotENU; - snake::toENU(origin, depot, depotENU); - transects.push_back(snake::BoostLineString{depotENU}); - for (auto &geoTransect : geoTransects) { - snake::BoostLineString t; - for (auto &geoVertex : geoTransect) { - snake::BoostPoint v; - snake::toENU(origin, geoVertex, v); - t.push_back(v); + // precondtion + if (_planDataValid && this->_joinedArea.coordinateList().size() > 0) { + const auto progress = this->_nemoInterface.progress(); + const auto *tiles = this->_measurementArea.tiles(); + // precondtion + if (tiles->count() > 0 && progress.size() == tiles->count() && + this->_rawTransects.size() > 0 && + this->_joinedArea.coordinateList().size() > 0) { + // Fetch tiles and convert to ENU. + this->_origin = this->_joinedArea.coordinateList().first(); + this->_origin.setAltitude(0); + const auto &origin = this->_origin; + auto pTiles = std::make_shared>(); + std::size_t numTiles = 0; + for (int i = 0; i < progress.size(); ++i) { + if (progress[i] == 100) { + ++numTiles; + const auto *obj = tiles->get(i); + const auto *tile = qobject_cast(obj); + if (tile != nullptr) { + snake::BoostPolygon t; + snake::areaToEnu(origin, tile->coordinateList(), t); + pTiles->push_back(std::move(t)); + } else { + return; + } } - transects.push_back(t); } - return true; - }; - this->_routingThread.route(safeAreaENU, generator); + if (numTiles > 0) { + // Fetch safe area and convert to ENU. + auto safeArea = this->_joinedArea.coordinateList(); + for (auto &v : safeArea) { + v.setAltitude(0); + } + snake::BoostPolygon safeAreaENU; + snake::areaToEnu(origin, safeArea, safeAreaENU); + const auto &depot = this->_serviceArea.depot(); + snake::BoostPoint depotENU; + snake::toENU(origin, depot, depotENU); + // Fetch geo transects and convert to ENU. + const auto &geoTransects = this->_rawTransects; + auto pUnclippedTransects = std::make_shared(); + for (auto &geoTransect : geoTransects) { + snake::BoostLineString t; + for (auto &geoVertex : geoTransect) { + snake::BoostPoint v; + snake::toENU(origin, geoVertex, v); + t.push_back(v); + } + pUnclippedTransects->push_back(t); + } + std::size_t minLength = 1; // meter + qWarning() << "using minLength dummy lkjfdslooij"; + auto generator = [depotENU, pUnclippedTransects, pTiles, + minLength](snake::Transects &transects) -> bool { + // Convert transects to clipper path. + vector transectsClipper; + for (const auto &t : *pUnclippedTransects) { + ClipperLib::Path path; + for (const auto &v : t) { + ClipperLib::IntPoint c{ + static_cast(v.get<0>() * CLIPPER_SCALE), + static_cast(v.get<1>() * CLIPPER_SCALE)}; + path.push_back(c); + } + transectsClipper.push_back(std::move(path)); + } + // Add transects to clipper. + ClipperLib::Clipper clipper; + clipper.AddPaths(transectsClipper, ClipperLib::ptSubject, false); + // Add tiles to clipper. + vector tilesClipper; + for (const auto &t : *pTiles) { + ClipperLib::Path path; + for (const auto &v : t.outer()) { + path.push_back(ClipperLib::IntPoint{ + static_cast(v.get<0>() * CLIPPER_SCALE), + static_cast(v.get<1>() * CLIPPER_SCALE)}); + } + tilesClipper.push_back(std::move(path)); + } + + clipper.AddPaths(tilesClipper, ClipperLib::ptClip, true); + // Clip transects. + ClipperLib::PolyTree clippedTransecs; + clipper.Execute(ClipperLib::ctDifference, clippedTransecs, + ClipperLib::pftNonZero, ClipperLib::pftNonZero); + // Extract transects from PolyTree and convert them to + // BoostLineString + transects.push_back(snake::BoostLineString{depotENU}); + for (const auto &child : clippedTransecs.Childs) { + snake::BoostLineString transect; + for (const auto &v : child->Contour) { + snake::BoostPoint c{static_cast(v.X) / CLIPPER_SCALE, + static_cast(v.Y) / CLIPPER_SCALE}; + transect.push_back(c); + } + + if (bg::length(transect) >= minLength) { + transects.push_back(std::move(transect)); + } + } + if (transects.size() > 1) { + return true; + } else { + return false; + } + }; // generator + this->_routingThread.route(safeAreaENU, generator); + } else { + this->_storeRoute(RoutingThread::PtrRoutingData(new RoutingData())); + } + } } } diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index 57d49e076..f3714022c 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -6,12 +6,8 @@ #include "QGCMapPolygon.h" #include "QmlObjectListModel.h" -//#include "Geometry/WimaArea.h" -//#include "Geometry/WimaCorridor.h" #include "Geometry/WimaCorridorData.h" -//#include "Geometry/WimaMeasurementArea.h" #include "Geometry/WimaMeasurementAreaData.h" -//#include "Geometry/WimaServiceArea.h" #include "Geometry/WimaServiceAreaData.h" #include "WimaPlanData.h" @@ -33,8 +29,6 @@ typedef std::unique_ptr JsonDocUPtr; class WimaController : public QObject { Q_OBJECT - enum FileType { WimaFile, PlanFile }; - public: WimaController(QObject *parent = nullptr); @@ -213,7 +207,7 @@ private slots: void _initSmartRTL(); void _smartRTLCleanUp(bool flying); // Snake. - void _threadFinishedHandler(); + void _storeRoute(RoutingThread::PtrRoutingData data); void _switchWaypointManager(WaypointManager::ManagerBase &manager); void _switchToSnakeWaypointManager(QVariant variant); void _progressChangedHandler(); @@ -234,7 +228,7 @@ private: WimaMeasurementAreaData _measurementArea; // measurement area WimaServiceAreaData _serviceArea; // area for supplying WimaCorridorData _corridor; // corridor connecting opArea and serArea - bool _localPlanDataValid; + bool _planDataValid; // Waypoint Managers. WaypointManager::AreaInterface _areaInterface; @@ -283,6 +277,7 @@ private: using StatusMap = std::map; static StatusMap _nemoStatusMap; RoutingThread _routingThread; + QGeoCoordinate _origin; // Periodic tasks. QTimer _eventTimer; diff --git a/src/Wima/WimaPlanData.cc b/src/Wima/WimaPlanData.cc index e1ea35911..6f597f329 100644 --- a/src/Wima/WimaPlanData.cc +++ b/src/Wima/WimaPlanData.cc @@ -89,6 +89,10 @@ void WimaPlanData::append(const WimaMeasurementAreaData &areaData) { } } +void WimaPlanData::setTransects(const QList> &transects) { + _transects = transects; +} + void WimaPlanData::append(const QList &missionItems) { for (auto *item : missionItems) { item->setParent(this); @@ -110,6 +114,10 @@ const QList &WimaPlanData::areaList() const { return _areaList; } +const QList> &WimaPlanData::transects() const { + return _transects; +} + const QList &WimaPlanData::missionItems() const { return _missionItems; } diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc index 7a03198eb..12d8874da 100644 --- a/src/Wima/WimaPlaner.cc +++ b/src/Wima/WimaPlaner.cc @@ -668,9 +668,13 @@ void WimaPlaner::synchronize() { if (_wimaBridge != nullptr) { if (readyForSynchronization()) { auto planData = toPlanData(); - (void)_wimaBridge->setWimaPlanData(planData); - this->_synchronized = true; - emit synchronizedChanged(); + if (planData) { + (void)_wimaBridge->setWimaPlanData(planData); + this->_synchronized = true; + emit synchronizedChanged(); + } else { + qWarning("WimaPlaner::uploadToContainer(): error creating plan data."); + } } } else { qWarning("WimaPlaner::uploadToContainer(): no container assigned."); @@ -741,17 +745,18 @@ QSharedPointer WimaPlaner::toPlanData() { planData->append(WimaJoinedAreaData(_joinedArea)); // convert mission items to mavlink commands - QList missionItems; if (_missionController && _missionController->visualItems()) { int surveyIndex = _missionController->visualItems()->indexOf(_TSComplexItem); if (surveyIndex > 0) { + QList missionItems; _TSComplexItem->appendMissionItems(missionItems, nullptr); + planData->append(missionItems); + planData->setTransects(this->_TSComplexItem->rawTransects()); + return planData; } } - // store mavlink commands - planData->append(missionItems); - return planData; + return QSharedPointer(); } #ifndef NDEBUG diff --git a/src/WimaView/ProgressIndicator.qml b/src/WimaView/ProgressIndicator.qml index 3b6d39495..99aa51343 100644 --- a/src/WimaView/ProgressIndicator.qml +++ b/src/WimaView/ProgressIndicator.qml @@ -5,8 +5,8 @@ import QtLocation 5.3 MapQuickItem { id: root - width: 20 - height: 20 + width: 15 + height: 15 anchorPoint.x: width/2 anchorPoint.y: height/2 @@ -89,7 +89,7 @@ MapQuickItem { // From 0 to angle ctx.beginPath(); - ctx.lineWidth = 3; + ctx.lineWidth = 1; ctx.strokeStyle = root.secondaryColor; ctx.arc(root.centerWidth, root.centerHeight, -- 2.22.0