diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 10bd5e234ccaaab73df8fbea107e19b850c834f1..cb5825b276a4041092cb0702efdec7fbf97baafc 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -509,7 +509,11 @@ HEADERS += \ src/MissionManager/MissionManager.h \ src/MissionManager/MissionSettingsItem.h \ src/MissionManager/PlanElementController.h \ + src/MissionManager/PlanManager.h \ src/MissionManager/PlanMasterController.h \ + src/MissionManager/QGCFenceCircle.h \ + src/MissionManager/QGCFencePolygon.h \ + src/MissionManager/QGCMapCircle.h \ src/MissionManager/QGCMapPolygon.h \ src/MissionManager/RallyPoint.h \ src/MissionManager/RallyPointController.h \ @@ -689,7 +693,11 @@ SOURCES += \ src/MissionManager/MissionManager.cc \ src/MissionManager/MissionSettingsItem.cc \ src/MissionManager/PlanElementController.cc \ + src/MissionManager/PlanManager.cc \ src/MissionManager/PlanMasterController.cc \ + src/MissionManager/QGCFenceCircle.cc \ + src/MissionManager/QGCFencePolygon.cc \ + src/MissionManager/QGCMapCircle.cc \ src/MissionManager/QGCMapPolygon.cc \ src/MissionManager/RallyPoint.cc \ src/MissionManager/RallyPointController.cc \ @@ -906,9 +914,7 @@ APMFirmwarePlugin { src/AutoPilotPlugins/APM/APMSensorsComponentController.h \ src/AutoPilotPlugins/APM/APMTuningComponent.h \ src/FirmwarePlugin/APM/APMFirmwarePlugin.h \ - src/FirmwarePlugin/APM/APMGeoFenceManager.h \ src/FirmwarePlugin/APM/APMParameterMetaData.h \ - src/FirmwarePlugin/APM/APMRallyPointManager.h \ src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h \ src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h \ src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h \ @@ -933,9 +939,7 @@ APMFirmwarePlugin { src/AutoPilotPlugins/APM/APMSensorsComponentController.cc \ src/AutoPilotPlugins/APM/APMTuningComponent.cc \ src/FirmwarePlugin/APM/APMFirmwarePlugin.cc \ - src/FirmwarePlugin/APM/APMGeoFenceManager.cc \ src/FirmwarePlugin/APM/APMParameterMetaData.cc \ - src/FirmwarePlugin/APM/APMRallyPointManager.cc \ src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc \ src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.cc \ src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc \ @@ -974,7 +978,6 @@ PX4FirmwarePlugin { src/AutoPilotPlugins/PX4/SensorsComponent.h \ src/AutoPilotPlugins/PX4/SensorsComponentController.h \ src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h \ - src/FirmwarePlugin/PX4/PX4GeoFenceManager.h \ src/FirmwarePlugin/PX4/PX4ParameterMetaData.h \ SOURCES += \ @@ -995,7 +998,6 @@ PX4FirmwarePlugin { src/AutoPilotPlugins/PX4/SensorsComponent.cc \ src/AutoPilotPlugins/PX4/SensorsComponentController.cc \ src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc \ - src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc \ src/FirmwarePlugin/PX4/PX4ParameterMetaData.cc \ } diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index 7c2fcf20375c7a3fa50152a89f6ecc1c0a5b710c..974c2eb57f2e0f4ade902d293d8db77999f10fe5 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -90,6 +90,7 @@ src/QmlControls/QGCLabel.qml src/QmlControls/QGCListView.qml src/QmlControls/QGCMapLabel.qml + src/MissionManager/QGCMapCircleVisuals.qml src/MissionManager/QGCMapPolygonVisuals.qml src/QmlControls/QGCMouseArea.qml src/QmlControls/QGCMovableItem.qml @@ -199,6 +200,7 @@ src/Settings/AutoConnect.SettingsGroup.json src/Settings/FlightMap.SettingsGroup.json src/Settings/Guided.SettingsGroup.json + src/MissionManager/QGCMapCircle.Facts.json src/Settings/RTK.SettingsGroup.json src/MissionManager/Survey.SettingsGroup.json src/Settings/Units.SettingsGroup.json diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 19faca6667f2154f8249ecde20c2c720b2bda0d1..aa27e876b13d59e85ec7fc9ce35cfa0bbd5c55ac 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -17,8 +17,6 @@ #include "FirmwarePlugin.h" #include "QGCLoggingCategory.h" #include "APMParameterMetaData.h" -#include "APMGeoFenceManager.h" -#include "APMRallyPointManager.h" #include @@ -93,8 +91,6 @@ public: QString internalParameterMetaDataFile (Vehicle* vehicle) final; void getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) final { APMParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); } QObject* loadParameterMetaData (const QString& metaDataFile) final; - GeoFenceManager* newGeoFenceManager (Vehicle* vehicle) final { return new APMGeoFenceManager(vehicle); } - RallyPointManager* newRallyPointManager (Vehicle* vehicle) final { return new APMRallyPointManager(vehicle); } QString brandImageIndoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); } QString brandImageOutdoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); } diff --git a/src/FirmwarePlugin/APM/APMGeoFenceManager.cc b/src/FirmwarePlugin/APM/APMGeoFenceManager.cc deleted file mode 100644 index 5fd028903fce54c1c597f69b14401ace6ee68cf7..0000000000000000000000000000000000000000 --- a/src/FirmwarePlugin/APM/APMGeoFenceManager.cc +++ /dev/null @@ -1,337 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2016 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - -#include "APMGeoFenceManager.h" -#include "Vehicle.h" -#include "FirmwarePlugin.h" -#include "MAVLinkProtocol.h" -#include "QGCApplication.h" -#include "ParameterManager.h" -#include "QmlObjectListModel.h" -#include "QGCQGeoCoordinate.h" - -const char* APMGeoFenceManager::_fenceTotalParam = "FENCE_TOTAL"; -const char* APMGeoFenceManager::_fenceActionParam = "FENCE_ACTION"; -const char* APMGeoFenceManager::_fenceEnableParam = "FENCE_ENABLE"; - -APMGeoFenceManager::APMGeoFenceManager(Vehicle* vehicle) - : GeoFenceManager(vehicle) - , _fenceSupported(false) - , _circleEnabled(false) - , _polygonSupported(false) - , _polygonEnabled(false) - , _breachReturnSupported(vehicle->fixedWing()) - , _firstParamLoadComplete(false) - , _readTransactionInProgress(false) - , _writeTransactionInProgress(false) - , _fenceTypeFact(NULL) - , _fenceEnableFact(NULL) - , _circleRadiusFact(NULL) -{ - connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &APMGeoFenceManager::_mavlinkMessageReceived); - connect(_vehicle->parameterManager(), &ParameterManager::parametersReadyChanged, this, &APMGeoFenceManager::_parametersReady); - - if (_vehicle->parameterManager()->parametersReady()) { - _parametersReady(); - } -} - -APMGeoFenceManager::~APMGeoFenceManager() -{ - -} - -void APMGeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon) -{ - if (_vehicle->isOfflineEditingVehicle()) { - return; - } - - if (_readTransactionInProgress) { - _sendError(InternalError, QStringLiteral("Geo-Fence write attempted while read in progress.")); - return; - } - - if (!_fenceSupported) { - return; - } - - // Validate - int validatedPolygonCount = 0; - if (polygon.count() >= 3) { - validatedPolygonCount = polygon.count(); - } - if (polygon.count() > std::numeric_limits::max()) { - _sendError(TooManyPoints, QStringLiteral("Geo-Fence polygon has too many points: %1.").arg(_polygon.count())); - validatedPolygonCount = 0; - } - - _breachReturnPoint = breachReturn; - _polygon.clear(); - if (validatedPolygonCount) { - for (int i=0; i(i)->coordinate()); - } - } - - // Total point count, +1 polygon close in last index, +1 for breach in index 0 - _cWriteFencePoints = validatedPolygonCount ? validatedPolygonCount + 1 + 1 : 0; - qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::sendToVehicle validatedPolygonCount:_cWriteFencePoints" << validatedPolygonCount << _cWriteFencePoints; - _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceTotalParam)->setRawValue(_cWriteFencePoints); - - // FIXME: No validation of correct fence received - for (uint8_t index=0; index<_cWriteFencePoints; index++) { - _sendFencePoint(index); - } - - emit sendComplete(false /* error */); -} - -void APMGeoFenceManager::loadFromVehicle(void) -{ - if (_vehicle->isOfflineEditingVehicle() || _readTransactionInProgress) { - return; - } - - _breachReturnPoint = QGeoCoordinate(); - _polygon.clear(); - - if (!_fenceSupported) { - emit loadComplete(_breachReturnPoint, _polygon); - return; - } - - // Point 0: Breach return point (always sent, but supported by ArduPlane only) - // Point [1,N]: Polygon points - // Point N+1: Close polygon point (same as point 1) - int cFencePoints = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceTotalParam)->rawValue().toInt(); - int minFencePoints = 5; - qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::loadFromVehicle" << cFencePoints; - if (cFencePoints == 0) { - // No fence - emit loadComplete(_breachReturnPoint, _polygon); - return; - } - if (cFencePoints < 0 || (cFencePoints > 0 && cFencePoints < minFencePoints)) { - _sendError(TooFewPoints, QStringLiteral("Geo-Fence information from Vehicle has too few points: %1").arg(cFencePoints)); - return; - } - if (cFencePoints > std::numeric_limits::max()) { - _sendError(TooManyPoints, QStringLiteral("Geo-Fence information from Vehicle has too many points: %1").arg(cFencePoints)); - return; - } - - _readTransactionInProgress = true; - _cReadFencePoints = cFencePoints; - _currentFencePoint = 0; - - _requestFencePoint(_currentFencePoint); -} - -/// Called when a new mavlink message for our vehicle is received -void APMGeoFenceManager::_mavlinkMessageReceived(const mavlink_message_t& message) -{ - if (message.msgid == MAVLINK_MSG_ID_FENCE_POINT) { - mavlink_fence_point_t fencePoint; - - mavlink_msg_fence_point_decode(&message, &fencePoint); - qCDebug(GeoFenceManagerLog) << "From vehicle fence_point: idx:lat:lng" << fencePoint.idx << fencePoint.lat << fencePoint.lng; - - if (fencePoint.idx != _currentFencePoint) { - // FIXME: Protocol out of whack - _readTransactionInProgress = false; - emit inProgressChanged(inProgress()); - qCWarning(GeoFenceManagerLog) << "Indices out of sync" << fencePoint.idx << _currentFencePoint; - return; - } - - if (fencePoint.idx == 0) { - _breachReturnPoint = QGeoCoordinate(fencePoint.lat, fencePoint.lng); - qCDebug(GeoFenceManagerLog) << "From vehicle: breach return point" << _breachReturnPoint; - _requestFencePoint(++_currentFencePoint); - } else if (fencePoint.idx < _cReadFencePoints - 1) { - QGeoCoordinate polyCoord(fencePoint.lat, fencePoint.lng); - _polygon.append(polyCoord); - qCDebug(GeoFenceManagerLog) << "From vehicle: polygon point" << fencePoint.idx << polyCoord; - if (fencePoint.idx < _cReadFencePoints - 2) { - // Still more points to request - _requestFencePoint(++_currentFencePoint); - } else { - // We've finished collecting fence points - qCDebug(GeoFenceManagerLog) << "Fence point load complete"; - _readTransactionInProgress = false; - emit loadComplete(_breachReturnPoint, _polygon); - emit inProgressChanged(inProgress()); - } - } - } -} - -void APMGeoFenceManager::_requestFencePoint(uint8_t pointIndex) -{ - mavlink_message_t msg; - MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); - - qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::_requestFencePoint" << pointIndex; - mavlink_msg_fence_fetch_point_pack_chan(mavlink->getSystemId(), - mavlink->getComponentId(), - _vehicle->priorityLink()->mavlinkChannel(), - &msg, - _vehicle->id(), - _vehicle->defaultComponentId(), - pointIndex); - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); -} - -void APMGeoFenceManager::_sendFencePoint(uint8_t pointIndex) -{ - mavlink_message_t msg; - MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); - - QGeoCoordinate fenceCoord; - if (pointIndex == 0) { - fenceCoord = breachReturnPoint(); - } else if (pointIndex == _cWriteFencePoints - 1) { - // Polygon close point - fenceCoord = _polygon[0]; - } else { - // Polygon point - fenceCoord = _polygon[pointIndex - 1]; - } - - // Total point count, +1 polygon close in last index, +1 for breach in index 0 - uint8_t totalPointCount = _polygon.count() + 1 + 1; - - mavlink_msg_fence_point_pack_chan(mavlink->getSystemId(), - mavlink->getComponentId(), - _vehicle->priorityLink()->mavlinkChannel(), - &msg, - _vehicle->id(), - _vehicle->defaultComponentId(), - pointIndex, // Index of point to set - totalPointCount, - fenceCoord.latitude(), - fenceCoord.longitude()); - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); -} - -bool APMGeoFenceManager::inProgress(void) const -{ - return _readTransactionInProgress || _writeTransactionInProgress; -} - -void APMGeoFenceManager::_setCircleEnabled(bool circleEnabled) -{ - if (circleEnabled != _circleEnabled) { - _circleEnabled = circleEnabled; - emit circleEnabledChanged(circleEnabled); - } -} - -void APMGeoFenceManager::_setPolygonEnabled(bool polygonEnabled) -{ - if (polygonEnabled != _polygonEnabled) { - _polygonEnabled = polygonEnabled; - emit polygonEnabledChanged(polygonEnabled); - } -} - -void APMGeoFenceManager::_updateEnabledFlags(void) -{ - bool fenceEnabled = false; - - if (_fenceSupported) { - if (_fenceEnableFact) { - fenceEnabled = _fenceEnableFact->rawValue().toBool(); - } else { - fenceEnabled = true; - } - - bool newCircleEnabled = fenceEnabled && _fenceTypeFact && (_fenceTypeFact->rawValue().toInt() & 2); - _setCircleEnabled(newCircleEnabled); - - bool newPolygonEnabled = _fenceSupported && fenceEnabled && - ((_vehicle->multiRotor() && _fenceTypeFact && (_fenceTypeFact->rawValue().toInt() & 4)) || - _vehicle->fixedWing()); - _setPolygonEnabled(newPolygonEnabled); - } else { - _setCircleEnabled(false); - _setPolygonEnabled(false); - } -} - -void APMGeoFenceManager::_parametersReady(void) -{ - if (!_firstParamLoadComplete) { - _firstParamLoadComplete = true; - - _fenceSupported = _vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _fenceTotalParam) && - _vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _fenceActionParam) && - !qgcApp()->runningUnitTests(); - - if (_fenceSupported) { - QStringList paramNames; - QStringList paramLabels; - - _polygonSupported = true; - - if (_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _fenceEnableParam)) { - _fenceEnableFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceEnableParam); - connect(_fenceEnableFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_updateEnabledFlags); - } - - if (_vehicle->multiRotor()) { - _breachReturnSupported = false; - _fenceTypeFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("FENCE_TYPE")); - connect(_fenceTypeFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_updateEnabledFlags); - - _circleRadiusFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("FENCE_RADIUS")); - - paramNames << QStringLiteral("FENCE_ENABLE") << QStringLiteral("FENCE_TYPE") << QStringLiteral("FENCE_ACTION") << QStringLiteral("FENCE_ALT_MAX") - << QStringLiteral("FENCE_RADIUS") << QStringLiteral("FENCE_MARGIN"); - paramLabels << QStringLiteral("Enabled:") << QStringLiteral("Type:") << QStringLiteral("Breach Action:") << QStringLiteral("Max Altitude:") - << QStringLiteral("Radius:") << QStringLiteral("Margin:"); - } if (_vehicle->fixedWing()) { - _breachReturnSupported = true; - paramNames << QStringLiteral("FENCE_ACTION") << QStringLiteral("FENCE_MINALT") << QStringLiteral("FENCE_MAXALT") << QStringLiteral("FENCE_RETALT") - << QStringLiteral("FENCE_AUTOENABLE") << QStringLiteral("FENCE_RET_RALLY"); - paramLabels << QStringLiteral("Breach Action:") << QStringLiteral("Min Altitude:") << QStringLiteral("Max Altitude:") << QStringLiteral("Return Altitude:") - << QStringLiteral("Auto-Enable:") << QStringLiteral("Return to Rally:"); - } - - _params.clear(); - _paramLabels.clear(); - for (int i=0; iparameterManager()->parameterExists(FactSystem::defaultComponentId, paramName)) { - Fact* paramFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, paramName); - _params << QVariant::fromValue(paramFact); - _paramLabels << paramLabels[i]; - } - } - emit paramsChanged(_params); - emit paramLabelsChanged(_paramLabels); - - emit breachReturnSupportedChanged(_breachReturnSupported); - emit polygonSupportedChanged(_polygonSupported); - } - - _updateEnabledFlags(); - } -} - -void APMGeoFenceManager::removeAll(void) -{ - qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::removeAll"; - - QmlObjectListModel emptyPolygon; - - sendToVehicle(_breachReturnPoint, emptyPolygon); - emit removeAllComplete(false /* error */); -} diff --git a/src/FirmwarePlugin/APM/APMGeoFenceManager.h b/src/FirmwarePlugin/APM/APMGeoFenceManager.h deleted file mode 100644 index a092e412f8e5737135c44e9249af460b74c17d1c..0000000000000000000000000000000000000000 --- a/src/FirmwarePlugin/APM/APMGeoFenceManager.h +++ /dev/null @@ -1,78 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2016 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - -#ifndef APMGeoFenceManager_H -#define APMGeoFenceManager_H - -#include "GeoFenceManager.h" -#include "QGCMAVLink.h" -#include "FactSystem.h" - -class QmlObjectListModel; - -class APMGeoFenceManager : public GeoFenceManager -{ - Q_OBJECT - -public: - APMGeoFenceManager(Vehicle* vehicle); - ~APMGeoFenceManager(); - - // Overrides from GeoFenceManager - bool inProgress (void) const final; - void loadFromVehicle (void) final; - void sendToVehicle (const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon) final; - bool polygonSupported (void) const final { return _polygonSupported; } - bool polygonEnabled (void) const final { return _polygonEnabled; } - bool breachReturnSupported (void) const final { return _breachReturnSupported; } - bool circleEnabled (void) const { return _circleEnabled; } - Fact* circleRadiusFact (void) const { return _circleRadiusFact; } - QVariantList params (void) const final { return _params; } - QStringList paramLabels (void) const final { return _paramLabels; } - void removeAll (void) final; - -private slots: - void _mavlinkMessageReceived (const mavlink_message_t& message); - void _parametersReady (void); - -private: - void _requestFencePoint (uint8_t pointIndex); - void _sendFencePoint (uint8_t pointIndex); - void _updateEnabledFlags(void); - void _setCircleEnabled (bool circleEnabled); - void _setPolygonEnabled (bool polygonEnabled); - -private: - bool _fenceSupported; - bool _circleEnabled; - bool _polygonSupported; - bool _polygonEnabled; - bool _breachReturnSupported; - bool _firstParamLoadComplete; - - QVariantList _params; - QStringList _paramLabels; - - bool _readTransactionInProgress; - bool _writeTransactionInProgress; - - uint8_t _cReadFencePoints; - uint8_t _cWriteFencePoints; - uint8_t _currentFencePoint; - - Fact* _fenceTypeFact; - Fact* _fenceEnableFact; - Fact* _circleRadiusFact; - - static const char* _fenceTotalParam; - static const char* _fenceActionParam; - static const char* _fenceEnableParam; -}; - -#endif diff --git a/src/FirmwarePlugin/APM/APMRallyPointManager.cc b/src/FirmwarePlugin/APM/APMRallyPointManager.cc deleted file mode 100644 index 86011d98ec54e9633e096b6bfff050fc3338a7c8..0000000000000000000000000000000000000000 --- a/src/FirmwarePlugin/APM/APMRallyPointManager.cc +++ /dev/null @@ -1,166 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2016 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - -#include "APMRallyPointManager.h" -#include "Vehicle.h" -#include "FirmwarePlugin.h" -#include "MAVLinkProtocol.h" -#include "QGCApplication.h" -#include "ParameterManager.h" - -const char* APMRallyPointManager::_rallyTotalParam = "RALLY_TOTAL"; - -APMRallyPointManager::APMRallyPointManager(Vehicle* vehicle) - : RallyPointManager(vehicle) - , _readTransactionInProgress(false) - , _writeTransactionInProgress(false) -{ - connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &APMRallyPointManager::_mavlinkMessageReceived); -} - -APMRallyPointManager::~APMRallyPointManager() -{ - -} - -void APMRallyPointManager::sendToVehicle(const QList& rgPoints) -{ - if (_vehicle->isOfflineEditingVehicle() || !rallyPointsSupported()) { - return; - } - - if (_readTransactionInProgress) { - _sendError(InternalError, QStringLiteral("Rally Point write attempted while read in progress.")); - return; - } - - _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _rallyTotalParam)->setRawValue(rgPoints.count()); - - // FIXME: No validation of correct point received - _rgPoints = rgPoints; - for (uint8_t index=0; index<_rgPoints.count(); index++) { - _sendRallyPoint(index); - } - - emit sendComplete(false /* error */); -} - -void APMRallyPointManager::loadFromVehicle(void) -{ - if (_vehicle->isOfflineEditingVehicle() || _readTransactionInProgress) { - return; - } - - _rgPoints.clear(); - - if (!rallyPointsSupported()) { - emit loadComplete(QList()); - return; - } - - _cReadRallyPoints = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _rallyTotalParam)->rawValue().toInt(); - qCDebug(RallyPointManagerLog) << "APMRallyPointManager::loadFromVehicle - point count" << _cReadRallyPoints; - if (_cReadRallyPoints == 0) { - emit loadComplete(_rgPoints); - return; - } - - _currentRallyPoint = 0; - _readTransactionInProgress = true; - _requestRallyPoint(_currentRallyPoint); -} - -/// Called when a new mavlink message for out vehicle is received -void APMRallyPointManager::_mavlinkMessageReceived(const mavlink_message_t& message) -{ - if (message.msgid == MAVLINK_MSG_ID_RALLY_POINT) { - mavlink_rally_point_t rallyPoint; - - mavlink_msg_rally_point_decode(&message, &rallyPoint); - qCDebug(RallyPointManagerLog) << "From vehicle rally_point: idx:lat:lng:alt" << rallyPoint.idx << rallyPoint.lat << rallyPoint.lng << rallyPoint.alt; - - if (rallyPoint.idx != _currentRallyPoint) { - // FIXME: Protocol out of whack - _readTransactionInProgress = false; - emit inProgressChanged(inProgress()); - qCWarning(RallyPointManagerLog) << "Indices out of sync" << rallyPoint.idx << _currentRallyPoint; - return; - } - - QGeoCoordinate point((float)rallyPoint.lat / 1e7, (float)rallyPoint.lng / 1e7, rallyPoint.alt); - _rgPoints.append(point); - if (rallyPoint.idx < _cReadRallyPoints - 1) { - // Still more points to request - _requestRallyPoint(++_currentRallyPoint); - } else { - // We've finished collecting rally points - qCDebug(RallyPointManagerLog) << "Rally point load complete"; - _readTransactionInProgress = false; - emit loadComplete(_rgPoints); - emit inProgressChanged(inProgress()); - } - } -} - -void APMRallyPointManager::_requestRallyPoint(uint8_t pointIndex) -{ - mavlink_message_t msg; - MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); - - qCDebug(RallyPointManagerLog) << "APMRallyPointManager::_requestRallyPoint" << pointIndex; - mavlink_msg_rally_fetch_point_pack_chan(mavlink->getSystemId(), - mavlink->getComponentId(), - _vehicle->priorityLink()->mavlinkChannel(), - &msg, - _vehicle->id(), - _vehicle->defaultComponentId(), - pointIndex); - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); -} - -void APMRallyPointManager::_sendRallyPoint(uint8_t pointIndex) -{ - mavlink_message_t msg; - MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); - - QGeoCoordinate point = _rgPoints[pointIndex]; - mavlink_msg_rally_point_pack_chan(mavlink->getSystemId(), - mavlink->getComponentId(), - _vehicle->priorityLink()->mavlinkChannel(), - &msg, - _vehicle->id(), - _vehicle->defaultComponentId(), - pointIndex, - _rgPoints.count(), - point.latitude() * 1e7, - point.longitude() * 1e7, - point.altitude(), - 0, 0, 0); // break_alt, land_dir, flags - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); -} - -bool APMRallyPointManager::inProgress(void) const -{ - return _readTransactionInProgress || _writeTransactionInProgress; -} - -bool APMRallyPointManager::rallyPointsSupported(void) const -{ - return _vehicle->parameterManager()->parameterExists(_vehicle->defaultComponentId(), _rallyTotalParam); -} - -void APMRallyPointManager::removeAll(void) -{ - qCDebug(RallyPointManagerLog) << "APMRallyPointManager::removeAll"; - - QList noRallyPoints; - - sendToVehicle(noRallyPoints); - emit removeAllComplete(false /* error */); -} diff --git a/src/FirmwarePlugin/APM/APMRallyPointManager.h b/src/FirmwarePlugin/APM/APMRallyPointManager.h deleted file mode 100644 index 74fc7012c02bb07fbae1c19eac576177d48d2e01..0000000000000000000000000000000000000000 --- a/src/FirmwarePlugin/APM/APMRallyPointManager.h +++ /dev/null @@ -1,51 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2016 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - -#ifndef APMRallyPointManager_H -#define APMRallyPointManager_H - -#include "RallyPointManager.h" -#include "QGCMAVLink.h" - -class APMRallyPointManager : public RallyPointManager -{ - Q_OBJECT - -public: - APMRallyPointManager(Vehicle* vehicle); - ~APMRallyPointManager(); - - // Overrides from RallyPointManager - bool inProgress (void) const final; - void loadFromVehicle (void) final; - void sendToVehicle (const QList& rgPoints) final; - bool rallyPointsSupported (void) const final; - - void removeAll (void); - - QString editorQml(void) const final { return QStringLiteral("qrc:/FirmwarePlugin/APM/APMRallyPointEditor.qml"); } - -private slots: - void _mavlinkMessageReceived(const mavlink_message_t& message); - -private: - void _requestRallyPoint(uint8_t pointIndex); - void _sendRallyPoint(uint8_t pointIndex); - -private: - bool _readTransactionInProgress; - bool _writeTransactionInProgress; - - uint8_t _cReadRallyPoints; - uint8_t _currentRallyPoint; - - static const char* _rallyTotalParam; -}; - -#endif diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index c8752af7630ab03ff21d04f420a09ebae620086f..9a8fb896d3bcfe994081c252631945a64b06a273 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -278,12 +278,6 @@ bool ArduCopterFirmwarePlugin::multiRotorXConfig(Vehicle* vehicle) return vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, "FRAME")->rawValue().toInt() != 0; } -QString ArduCopterFirmwarePlugin::geoFenceRadiusParam(Vehicle* vehicle) -{ - Q_UNUSED(vehicle); - return QStringLiteral("FENCE_RADIUS"); -} - bool ArduCopterFirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const { if (vehicle->isOfflineEditingVehicle()) { diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h index 89e164657a9f50d0b545aab33e15dcd21555ce58..e21d4f6d09a051080b9f9d507a44daa644623a14 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h @@ -67,7 +67,6 @@ public: int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final; bool multiRotorCoaxialMotors (Vehicle* vehicle) final; bool multiRotorXConfig (Vehicle* vehicle) final; - QString geoFenceRadiusParam (Vehicle* vehicle) final; QString offlineEditingParamFile (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Copter.OfflineEditing.params"); } QString pauseFlightMode (void) const override { return QString("Brake"); } QString missionFlightMode (void) const override { return QString("Auto"); } diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 0aae9367fc4a99ed278206da3a5f1c6aa97767eb..1df9f74fd60d0233457b9ecb635e86a6d7168f07 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -235,15 +235,6 @@ public: /// @return true: X confiuration, false: Plus configuration virtual bool multiRotorXConfig(Vehicle* vehicle) { Q_UNUSED(vehicle); return false; } - /// Returns a newly created geofence manager for this vehicle. - virtual GeoFenceManager* newGeoFenceManager(Vehicle* vehicle) { return new GeoFenceManager(vehicle); } - - /// Returns the parameter which holds the fence circle radius if supported. - virtual QString geoFenceRadiusParam(Vehicle* vehicle) { Q_UNUSED(vehicle); return QString(); } - - /// Returns a newly created rally point manager for this vehicle. - virtual RallyPointManager* newRallyPointManager(Vehicle* vehicle) { return new RallyPointManager(vehicle); } - /// Return the resource file which contains the set of params loaded for offline editing. virtual QString offlineEditingParamFile(Vehicle* vehicle) { Q_UNUSED(vehicle); return QString(); } diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index bda3803a9848fce269f7ab5eb5e75f84f071bddf..8e4a8de55f02039deb4a42b959b286948e61a7f9 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -17,7 +17,6 @@ #include "FirmwarePlugin.h" #include "ParameterManager.h" #include "PX4ParameterMetaData.h" -#include "PX4GeoFenceManager.h" class PX4FirmwarePlugin : public FirmwarePlugin { @@ -62,7 +61,6 @@ public: void getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) override { PX4ParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); } QObject* loadParameterMetaData (const QString& metaDataFile) final; bool adjustIncomingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message) override; - GeoFenceManager* newGeoFenceManager (Vehicle* vehicle) override { return new PX4GeoFenceManager(vehicle); } QString offlineEditingParamFile(Vehicle* vehicle) override { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/PX4/PX4.OfflineEditing.params"); } QString brandImageIndoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/PX4/BrandImage"); } QString brandImageOutdoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/PX4/BrandImage"); } diff --git a/src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc b/src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc deleted file mode 100644 index fec0bfd2fb52e999b54cc73b25740af0fcaf0a80..0000000000000000000000000000000000000000 --- a/src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc +++ /dev/null @@ -1,60 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2016 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - -#include "PX4GeoFenceManager.h" -#include "Vehicle.h" -#include "FirmwarePlugin.h" -#include "ParameterManager.h" - -PX4GeoFenceManager::PX4GeoFenceManager(Vehicle* vehicle) - : GeoFenceManager(vehicle) - , _firstParamLoadComplete(false) - , _circleRadiusFact(NULL) -{ - connect(_vehicle->parameterManager(), &ParameterManager::parametersReadyChanged, this, &PX4GeoFenceManager::_parametersReady); - - if (_vehicle->parameterManager()->parametersReady()) { - _parametersReady(); - } -} - -PX4GeoFenceManager::~PX4GeoFenceManager() -{ - -} - -void PX4GeoFenceManager::_parametersReady(void) -{ - if (!_firstParamLoadComplete) { - _firstParamLoadComplete = true; - - _circleRadiusFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("GF_MAX_HOR_DIST")); - emit circleRadiusFactChanged(_circleRadiusFact); - - QStringList paramNames; - QStringList paramLabels; - - paramNames << QStringLiteral("GF_ACTION") << QStringLiteral("GF_MAX_HOR_DIST") << QStringLiteral("GF_MAX_VER_DIST"); - paramLabels << QStringLiteral("Breach Action:") << QStringLiteral("Radius:") << QStringLiteral("Max Altitude:"); - - _params.clear(); - _paramLabels.clear(); - for (int i=0; iparameterManager()->parameterExists(FactSystem::defaultComponentId, paramName)) { - Fact* paramFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, paramName); - _params << QVariant::fromValue(paramFact); - _paramLabels << paramLabels[i]; - } - } - - emit paramsChanged(_params); - emit paramLabelsChanged(_paramLabels); - } -} diff --git a/src/FirmwarePlugin/PX4/PX4GeoFenceManager.h b/src/FirmwarePlugin/PX4/PX4GeoFenceManager.h deleted file mode 100644 index 7a87f2cbf234ed348ad342c4c6b1b3cc25de3361..0000000000000000000000000000000000000000 --- a/src/FirmwarePlugin/PX4/PX4GeoFenceManager.h +++ /dev/null @@ -1,41 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2016 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - -#ifndef PX4GeoFenceManager_H -#define PX4GeoFenceManager_H - -#include "GeoFenceManager.h" -#include "QGCMAVLink.h" -#include "FactSystem.h" - -class PX4GeoFenceManager : public GeoFenceManager -{ - Q_OBJECT - -public: - PX4GeoFenceManager(Vehicle* vehicle); - ~PX4GeoFenceManager(); - - // Overrides from GeoFenceManager - bool circleEnabled (void) const { return true; } - Fact* circleRadiusFact (void) const { return _circleRadiusFact; } - QVariantList params (void) const final { return _params; } - QStringList paramLabels (void) const final { return _paramLabels; } - -private slots: - void _parametersReady(void); - -private: - bool _firstParamLoadComplete; - Fact* _circleRadiusFact; - QVariantList _params; - QStringList _paramLabels; -}; - -#endif diff --git a/src/FlightMap/qmldir b/src/FlightMap/qmldir index 9afe7d25f264f96ba86619235822be5661b9a941..bf11b6fc816e8ca4cddb00251aeac48ce530abbc 100644 --- a/src/FlightMap/qmldir +++ b/src/FlightMap/qmldir @@ -29,6 +29,3 @@ MissionItemView 1.0 MissionItemView.qml MissionLineView 1.0 MissionLineView.qml PolygonEditor 1.0 PolygonEditor.qml VehicleMapItem 1.0 VehicleMapItem.qml - -# Editor controls -QGCMapPolygonControls 1.0 QGCMapPolygonControls.qml diff --git a/src/MissionManager/GeoFenceController.cc b/src/MissionManager/GeoFenceController.cc index b8fca7ec94726ef58b8ebf1d9b610b8d564b2326..64980ba51b7d02ae0d6b3455edffd2c8c5d341dd 100644 --- a/src/MissionManager/GeoFenceController.cc +++ b/src/MissionManager/GeoFenceController.cc @@ -39,11 +39,10 @@ GeoFenceController::GeoFenceController(PlanMasterController* masterController, Q : PlanElementController(masterController, parent) , _geoFenceManager(_managerVehicle->geoFenceManager()) , _dirty(false) - , _mapPolygon(this) , _itemsRequested(false) { - connect(_mapPolygon.qmlPathModel(), &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems); - connect(_mapPolygon.qmlPathModel(), &QmlObjectListModel::dirtyChanged, this, &GeoFenceController::_polygonDirtyChanged); + connect(&_polygons, &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems); + connect(&_circles, &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems); managerVehicleChanged(_managerVehicle); } @@ -77,21 +76,16 @@ void GeoFenceController::setBreachReturnPoint(const QGeoCoordinate& breachReturn void GeoFenceController::_signalAll(void) { - emit breachReturnSupportedChanged(breachReturnSupported()); emit breachReturnPointChanged(breachReturnPoint()); - emit circleEnabledChanged(circleEnabled()); - emit circleRadiusFactChanged(circleRadiusFact()); - emit polygonEnabledChanged(polygonEnabled()); - emit polygonSupportedChanged(polygonSupported()); emit dirtyChanged(dirty()); - emit paramsChanged(params()); - emit paramLabelsChanged(paramLabels()); + emit supportedChanged(supported()); } void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle) { if (_managerVehicle) { _geoFenceManager->disconnect(this); + _managerVehicle->disconnect(this); _managerVehicle = NULL; _geoFenceManager = NULL; } @@ -103,21 +97,23 @@ void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle) } _geoFenceManager = _managerVehicle->geoFenceManager(); - connect(_geoFenceManager, &GeoFenceManager::breachReturnSupportedChanged, this, &GeoFenceController::breachReturnSupportedChanged); - connect(_geoFenceManager, &GeoFenceManager::circleEnabledChanged, this, &GeoFenceController::circleEnabledChanged); - connect(_geoFenceManager, &GeoFenceManager::circleRadiusFactChanged, this, &GeoFenceController::circleRadiusFactChanged); - connect(_geoFenceManager, &GeoFenceManager::polygonEnabledChanged, this, &GeoFenceController::polygonEnabledChanged); - connect(_geoFenceManager, &GeoFenceManager::polygonSupportedChanged, this, &GeoFenceController::polygonSupportedChanged); connect(_geoFenceManager, &GeoFenceManager::loadComplete, this, &GeoFenceController::_managerLoadComplete); connect(_geoFenceManager, &GeoFenceManager::sendComplete, this, &GeoFenceController::_managerSendComplete); connect(_geoFenceManager, &GeoFenceManager::removeAllComplete, this, &GeoFenceController::_managerRemoveAllComplete); connect(_geoFenceManager, &GeoFenceManager::inProgressChanged, this, &GeoFenceController::syncInProgressChanged); + connect(_managerVehicle, &Vehicle::capabilityBitsChanged, this, &RallyPointController::supportedChanged); + + emit supportedChanged(supported()); _signalAll(); } bool GeoFenceController::load(const QJsonObject& json, QString& errorString) { + Q_UNUSED(json); + Q_UNUSED(errorString); + +#if 0 QString errorStr; QString errorMessage = tr("GeoFence: %1"); @@ -135,12 +131,15 @@ bool GeoFenceController::load(const QJsonObject& json, QString& errorString) setDirty(false); _signalAll(); +#endif return true; } void GeoFenceController::save(QJsonObject& json) { + Q_UNUSED(json); +#if 0 json[JsonHelper::jsonVersionKey] = 1; if (_breachReturnPoint.isValid()) { @@ -150,12 +149,14 @@ void GeoFenceController::save(QJsonObject& json) } _mapPolygon.saveToJson(json); +#endif } void GeoFenceController::removeAll(void) -{ +{ setBreachReturnPoint(QGeoCoordinate()); - _mapPolygon.clear(); + _polygons.clearAndDeleteContents(); + _circles.clearAndDeleteContents(); } void GeoFenceController::removeAllFromVehicle(void) @@ -189,8 +190,7 @@ void GeoFenceController::sendToVehicle(void) qCWarning(GeoFenceControllerLog) << "GeoFenceController::sendToVehicle called while syncInProgress"; } else { qCDebug(GeoFenceControllerLog) << "GeoFenceController::sendToVehicle"; - _geoFenceManager->sendToVehicle(_breachReturnPoint, _mapPolygon.pathModel()); - _mapPolygon.setDirty(false); + _geoFenceManager->sendToVehicle(_breachReturnPoint, _polygons, _circles); setDirty(false); } } @@ -211,7 +211,14 @@ void GeoFenceController::setDirty(bool dirty) if (dirty != _dirty) { _dirty = dirty; if (!dirty) { - _mapPolygon.setDirty(dirty); + for (int i=0; i<_polygons.count(); i++) { + QGCFencePolygon* polygon = _polygons.value(i); + polygon->setDirty(false); + } + for (int i=0; i<_circles.count(); i++) { + QGCFenceCircle* circle = _circles.value(i); + circle->setDirty(false); + } } emit dirtyChanged(dirty); } @@ -224,53 +231,26 @@ void GeoFenceController::_polygonDirtyChanged(bool dirty) } } -bool GeoFenceController::breachReturnSupported(void) const -{ - return _geoFenceManager->breachReturnSupported(); -} - -bool GeoFenceController::circleEnabled(void) const -{ - return _geoFenceManager->circleEnabled(); -} - -Fact* GeoFenceController::circleRadiusFact(void) const -{ - return _geoFenceManager->circleRadiusFact(); -} - -bool GeoFenceController::polygonSupported(void) const -{ - return _geoFenceManager->polygonSupported(); -} - -bool GeoFenceController::polygonEnabled(void) const -{ - return _geoFenceManager->polygonEnabled(); -} - -QVariantList GeoFenceController::params(void) const -{ - return _geoFenceManager->params(); -} - -QStringList GeoFenceController::paramLabels(void) const -{ - return _geoFenceManager->paramLabels(); -} - void GeoFenceController::_setDirty(void) { setDirty(true); } -void GeoFenceController::_setPolygonFromManager(const QList& polygon) +void GeoFenceController::_setFenceFromManager(const QList& polygons, + const QList& circles) { - _mapPolygon.clear(); - for (int i=0; i& polygon) +void GeoFenceController::_managerLoadComplete(void) { // Fly view always reloads on _loadComplete // Plan view only reloads on _loadComplete if specifically requested if (!_editMode || _itemsRequested) { - _setReturnPointFromManager(breachReturn); - _setPolygonFromManager(polygon); + _setReturnPointFromManager(_geoFenceManager->breachReturnPoint()); + _setFenceFromManager(_geoFenceManager->polygons(), _geoFenceManager->circles()); setDirty(false); _signalAll(); emit loadComplete(); @@ -311,7 +291,7 @@ void GeoFenceController::_managerRemoveAllComplete(bool error) bool GeoFenceController::containsItems(void) const { - return _mapPolygon.count() > 2; + return _polygons.count() > 0 || _circles.count() > 0; } void GeoFenceController::_updateContainsItems(void) @@ -339,8 +319,97 @@ bool GeoFenceController::showPlanFromManagerVehicle(void) // Fake a _loadComplete with the current items qCDebug(GeoFenceControllerLog) << "showPlanFromManagerVehicle: sync complete simulate signal"; _itemsRequested = true; - _managerLoadComplete(_geoFenceManager->breachReturnPoint(), _geoFenceManager->polygon()); + _managerLoadComplete(); return false; } } } + +void GeoFenceController::addInclusionPolygon(QGeoCoordinate topLeft, QGeoCoordinate bottomRight) +{ + QGeoCoordinate topRight(topLeft.latitude(), bottomRight.longitude()); + QGeoCoordinate bottomLeft(bottomRight.latitude(), topLeft.longitude()); + + double halfWidthMeters = topLeft.distanceTo(topRight) / 2.0; + double halfHeightMeters = topLeft.distanceTo(bottomLeft) / 2.0; + + QGeoCoordinate centerLeftEdge = topLeft.atDistanceAndAzimuth(halfHeightMeters, 180); + QGeoCoordinate centerTopEdge = topLeft.atDistanceAndAzimuth(halfWidthMeters, 90); + QGeoCoordinate center(centerLeftEdge.latitude(), centerTopEdge.longitude()); + + // Initial polygon is inset to take 3/4s of viewport with max width/height of 3000 meters + halfWidthMeters = qMin(halfWidthMeters * 0.75, 1500.0); + halfHeightMeters = qMin(halfHeightMeters * 0.75, 1500.0); + + // Initial polygon has max width and height of 3000 meters + topLeft = center.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 0); + topRight = center.atDistanceAndAzimuth(halfWidthMeters, 90).atDistanceAndAzimuth(halfHeightMeters, 0); + bottomLeft = center.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 180); + bottomRight = center.atDistanceAndAzimuth(halfWidthMeters, 90).atDistanceAndAzimuth(halfHeightMeters, 180); + + QGCFencePolygon* polygon = new QGCFencePolygon(true /* inclusion */, this); + polygon->appendVertex(topLeft); + polygon->appendVertex(topRight); + polygon->appendVertex(bottomRight); + polygon->appendVertex(bottomLeft); + _polygons.append(polygon); + + clearAllInteractive(); + polygon->setInteractive(true); +} + +void GeoFenceController::addInclusionCircle(QGeoCoordinate topLeft, QGeoCoordinate bottomRight) +{ + QGeoCoordinate topRight(topLeft.latitude(), bottomRight.longitude()); + QGeoCoordinate bottomLeft(bottomRight.latitude(), topLeft.longitude()); + + // Initial radius is inset to take 3/4s of viewport and max of 1500 meters + double halfWidthMeters = topLeft.distanceTo(topRight) / 2.0; + double halfHeightMeters = topLeft.distanceTo(bottomLeft) / 2.0; + double radius = qMin(qMin(halfWidthMeters, halfHeightMeters) * 0.75, 1500.0); + + QGeoCoordinate centerLeftEdge = topLeft.atDistanceAndAzimuth(halfHeightMeters, 180); + QGeoCoordinate centerTopEdge = topLeft.atDistanceAndAzimuth(halfWidthMeters, 90); + QGeoCoordinate center(centerLeftEdge.latitude(), centerTopEdge.longitude()); + + QGCFenceCircle* circle = new QGCFenceCircle(center, radius, true /* inclusion */, this); + _circles.append(circle); + + clearAllInteractive(); + circle->setInteractive(true); +} + +void GeoFenceController::deletePolygon(int index) +{ + if (index < 0 || index > _polygons.count() - 1) { + return; + } + + QGCFencePolygon* polygon = qobject_cast(_polygons.removeAt(index)); + polygon->deleteLater(); +} + +void GeoFenceController::deleteCircle(int index) +{ + if (index < 0 || index > _circles.count() - 1) { + return; + } + + QGCFenceCircle* circle = qobject_cast(_circles.removeAt(index)); + circle->deleteLater(); +} + +void GeoFenceController::clearAllInteractive(void) +{ + for (int i=0; i<_polygons.count(); i++) { + _polygons.value(i)->setInteractive(false); + } + for (int i=0; i<_circles.count(); i++) { + _circles.value(i)->setInteractive(false); + } +} + +bool GeoFenceController::supported(void) const +{ + return (_managerVehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE) && (_managerVehicle->maxProtoVersion() >= 200); +} diff --git a/src/MissionManager/GeoFenceController.h b/src/MissionManager/GeoFenceController.h index 7019cba3133a79e6a7f0c63c63c7b3492e990f6b..dd13cec9256fc0493932468321570935d343fbf4 100644 --- a/src/MissionManager/GeoFenceController.h +++ b/src/MissionManager/GeoFenceController.h @@ -12,7 +12,8 @@ #include "PlanElementController.h" #include "GeoFenceManager.h" -#include "QGCMapPolygon.h" +#include "QGCFencePolygon.h" +#include "QGCFenceCircle.h" #include "Vehicle.h" #include "MultiVehicleManager.h" #include "QGCLoggingCategory.h" @@ -29,21 +30,32 @@ public: GeoFenceController(PlanMasterController* masterController, QObject* parent = NULL); ~GeoFenceController(); - Q_PROPERTY(QGCMapPolygon* mapPolygon READ mapPolygon CONSTANT) - Q_PROPERTY(QGeoCoordinate breachReturnPoint READ breachReturnPoint WRITE setBreachReturnPoint NOTIFY breachReturnPointChanged) + Q_PROPERTY(QmlObjectListModel* polygons READ polygons CONSTANT) + Q_PROPERTY(QmlObjectListModel* circles READ circles CONSTANT) + Q_PROPERTY(QGeoCoordinate breachReturnPoint READ breachReturnPoint WRITE setBreachReturnPoint NOTIFY breachReturnPointChanged) - // The following properties are reflections of properties from GeoFenceManager - Q_PROPERTY(bool circleEnabled READ circleEnabled NOTIFY circleEnabledChanged) - Q_PROPERTY(Fact* circleRadiusFact READ circleRadiusFact NOTIFY circleRadiusFactChanged) - Q_PROPERTY(bool polygonSupported READ polygonSupported NOTIFY polygonSupportedChanged) - Q_PROPERTY(bool polygonEnabled READ polygonEnabled NOTIFY polygonEnabledChanged) - Q_PROPERTY(bool breachReturnSupported READ breachReturnSupported NOTIFY breachReturnSupportedChanged) - Q_PROPERTY(QVariantList params READ params NOTIFY paramsChanged) - Q_PROPERTY(QStringList paramLabels READ paramLabels NOTIFY paramLabelsChanged) + /// Add a new inclusion polygon to the fence + /// @param topLeft - Top left coordinate or map viewport + /// @param topLeft - Bottom right left coordinate or map viewport + Q_INVOKABLE void addInclusionPolygon(QGeoCoordinate topLeft, QGeoCoordinate bottomRight); - Q_INVOKABLE void addPolygon (void) { emit addInitialFencePolygon(); } - Q_INVOKABLE void removePolygon (void) { _mapPolygon.clear(); } + /// Add a new inclusion circle to the fence + /// @param topLeft - Top left coordinate or map viewport + /// @param topLeft - Bottom right left coordinate or map viewport + Q_INVOKABLE void addInclusionCircle(QGeoCoordinate topLeft, QGeoCoordinate bottomRight); + /// Deletes the specified polygon from the polygon list + /// @param index Index of poygon to delete + Q_INVOKABLE void deletePolygon(int index); + + /// Deletes the specified circle from the circle list + /// @param index Index of circle to delete + Q_INVOKABLE void deleteCircle(int index); + + /// Clears the interactive bit from all fence items + Q_INVOKABLE void clearAllInteractive(void); + + bool supported (void) const final; void start (bool editMode) final; void save (QJsonObject& json) final; bool load (const QJsonObject& json, QString& errorString) final; @@ -58,15 +70,9 @@ public: void managerVehicleChanged (Vehicle* managerVehicle) final; bool showPlanFromManagerVehicle (void) final; - bool circleEnabled (void) const; - Fact* circleRadiusFact (void) const; - bool polygonSupported (void) const; - bool polygonEnabled (void) const; - bool breachReturnSupported (void) const; - QVariantList params (void) const; - QStringList paramLabels (void) const; - QGCMapPolygon* mapPolygon (void) { return &_mapPolygon; } - QGeoCoordinate breachReturnPoint (void) const { return _breachReturnPoint; } + QmlObjectListModel* polygons (void) { return &_polygons; } + QmlObjectListModel* circles (void) { return &_circles; } + QGeoCoordinate breachReturnPoint (void) const { return _breachReturnPoint; } void setBreachReturnPoint(const QGeoCoordinate& breachReturnPoint); @@ -74,21 +80,14 @@ signals: void breachReturnPointChanged (QGeoCoordinate breachReturnPoint); void editorQmlChanged (QString editorQml); void loadComplete (void); - void addInitialFencePolygon (void); - void circleEnabledChanged (bool circleEnabled); - void circleRadiusFactChanged (Fact* circleRadiusFact); - void polygonSupportedChanged (bool polygonSupported); - void polygonEnabledChanged (bool polygonEnabled); - void breachReturnSupportedChanged (bool breachReturnSupported); - void paramsChanged (QVariantList params); - void paramLabelsChanged (QStringList paramLabels); private slots: void _polygonDirtyChanged(bool dirty); void _setDirty(void); - void _setPolygonFromManager(const QList& polygon); + void _setFenceFromManager(const QList& polygons, + const QList& circles); void _setReturnPointFromManager(QGeoCoordinate breachReturnPoint); - void _managerLoadComplete(const QGeoCoordinate& breachReturn, const QList& polygon); + void _managerLoadComplete(void); void _updateContainsItems(void); void _managerSendComplete(bool error); void _managerRemoveAllComplete(bool error); @@ -99,7 +98,8 @@ private: GeoFenceManager* _geoFenceManager; bool _dirty; - QGCMapPolygon _mapPolygon; + QmlObjectListModel _polygons; + QmlObjectListModel _circles; QGeoCoordinate _breachReturnPoint; bool _itemsRequested; diff --git a/src/MissionManager/GeoFenceManager.cc b/src/MissionManager/GeoFenceManager.cc index 357e3b768b335c2aa7f75bdfd2a76c74eadf1a59..448be9eb03b4d79462a1eec083b950d2a7e40f46 100644 --- a/src/MissionManager/GeoFenceManager.cc +++ b/src/MissionManager/GeoFenceManager.cc @@ -10,14 +10,22 @@ #include "GeoFenceManager.h" #include "Vehicle.h" #include "QmlObjectListModel.h" +#include "ParameterManager.h" +#include "QGCMapPolygon.h" +#include "QGCMapCircle.h" QGC_LOGGING_CATEGORY(GeoFenceManagerLog, "GeoFenceManagerLog") GeoFenceManager::GeoFenceManager(Vehicle* vehicle) - : QObject(vehicle) - , _vehicle(vehicle) + : _vehicle (vehicle) + , _planManager (vehicle, MAV_MISSION_TYPE_FENCE) + , _firstParamLoadComplete (false) { - + connect(&_planManager, &PlanManager::inProgressChanged, this, &GeoFenceManager::inProgressChanged); + connect(&_planManager, &PlanManager::error, this, &GeoFenceManager::error); + connect(&_planManager, &PlanManager::removeAllComplete, this, &GeoFenceManager::removeAllComplete); + connect(&_planManager, &PlanManager::sendComplete, this, &GeoFenceManager::_sendComplete); + connect(&_planManager, &PlanManager::newMissionItemsAvailable, this, &GeoFenceManager::_planManagerLoadComplete); } GeoFenceManager::~GeoFenceManager() @@ -25,30 +33,161 @@ GeoFenceManager::~GeoFenceManager() } -void GeoFenceManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg) +bool GeoFenceManager::inProgress(void) const { - qCDebug(GeoFenceManagerLog) << "Sending error" << errorCode << errorMsg; - - emit error(errorCode, errorMsg); + return _planManager.inProgress(); } void GeoFenceManager::loadFromVehicle(void) { - // No geofence support in unknown vehicle - emit loadComplete(QGeoCoordinate(), QList()); + _planManager.loadFromVehicle(); } -void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon) +void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, + QmlObjectListModel& polygons, + QmlObjectListModel& circles) { - // No geofence support in unknown vehicle Q_UNUSED(breachReturn); - Q_UNUSED(polygon); - emit sendComplete(false /* error */); -} + QList fenceItems; + + _sendPolygons.clear(); + _sendCircles.clear(); + + for (int i=0; i(i)); + } + for (int i=0; i(i)); + } + + for (int i=0; i<_sendPolygons.count(); i++) { + const QGCFencePolygon& polygon = _sendPolygons[i]; + + for (int j=0; j(); + + MissionItem* item = new MissionItem(0, + polygon.inclusion() ? MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION : MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, + MAV_FRAME_GLOBAL, + polygon.count(), // vertex count + 0, 0, 0, // param 2-4 unused + vertex.latitude(), + vertex.longitude(), + 0, // param 7 unused + false, // autocontinue + false, // isCurrentItem + this); // parent + fenceItems.append(item); + } + } + + for (int i=0; i<_sendCircles.count(); i++) { + QGCFenceCircle& circle = _sendCircles[i]; + + MissionItem* item = new MissionItem(0, + circle.inclusion() ? MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION : MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION, + MAV_FRAME_GLOBAL, + circle.radius()->rawValue().toDouble(), + 0, 0, 0, // param 2-4 unused + circle.center().latitude(), + circle.center().longitude(), + 0, // param 7 unused + false, // autocontinue + false, // isCurrentItem + this); // parent + fenceItems.append(item); + } + + _planManager.writeMissionItems(fenceItems); + + for (int i=0; ideleteLater(); + } +} void GeoFenceManager::removeAll(void) { - // No geofence support in unknown vehicle - emit removeAllComplete(false /* error */); + _planManager.removeAll(); +} + +void GeoFenceManager::_sendComplete(bool error) +{ + if (error) { + _polygons.clear(); + _circles.clear(); + } else { + _polygons = _sendPolygons; + _circles = _sendCircles; + } + _sendPolygons.clear(); + _sendCircles.clear(); + emit sendComplete(error); +} + +void GeoFenceManager::_planManagerLoadComplete(bool removeAllRequested) +{ + bool loadFailed = false; + + Q_UNUSED(removeAllRequested); + + _polygons.clear(); + _circles.clear(); + + MAV_CMD expectedCommand = (MAV_CMD)0; + int expectedVertexCount = 0; + QGCFencePolygon nextPolygon(true /* inclusion */); + const QList& fenceItems = _planManager.missionItems(); + + for (int i=0; icommand(); + + if (command == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION || command == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) { + if (nextPolygon.count() == 0) { + // Starting a new polygon + expectedVertexCount = item->param1(); + expectedCommand = command; + } else if (expectedVertexCount != item->param1()){ + // In the middle of a polygon, but count suddenly changed + emit error(BadPolygonItemFormat, tr("GeoFence load: Vertex count change mid-polygon - actual:expected").arg(item->param1()).arg(expectedVertexCount)); + break; + } if (expectedCommand != command) { + // Command changed before last polygon was completely loaded + emit error(BadPolygonItemFormat, tr("GeoFence load: Polygon type changed before last load complete - actual:expected").arg(command).arg(expectedCommand)); + break; + } + nextPolygon.appendVertex(QGeoCoordinate(item->param5(), item->param6())); + if (nextPolygon.count() == expectedVertexCount) { + // Polygon is complete + nextPolygon.setInclusion(command == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION); + _polygons.append(nextPolygon); + nextPolygon.clear(); + } + } else if (command == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION || command == MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION) { + if (nextPolygon.count() != 0) { + // Incomplete polygon + emit error(IncompletePolygonLoad, tr("GeoFence load: Incomplete polygon loaded")); + break; + } + QGCFenceCircle circle(QGeoCoordinate(item->param5(), item->param6()), item->param1(), command == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION /* inclusion */); + _circles.append(circle); + } else { + emit error(UnsupportedCommand, tr("GeoFence load: Unsupported command %1").arg(item->command())); + break; + } + } + + if (loadFailed) { + _polygons.clear(); + _circles.clear(); + } + + emit loadComplete(); +} + +bool GeoFenceManager::supported(void) const +{ + return (_vehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE) && (_vehicle->maxProtoVersion() >= 200); } diff --git a/src/MissionManager/GeoFenceManager.h b/src/MissionManager/GeoFenceManager.h index a1ea66ac861f2f1d3118fb21be6aca636ab952d2..9c7c4e41fd31cdf3d415e6932da8041a7a9e6054 100644 --- a/src/MissionManager/GeoFenceManager.h +++ b/src/MissionManager/GeoFenceManager.h @@ -15,9 +15,13 @@ #include "QGCLoggingCategory.h" #include "FactSystem.h" +#include "PlanManager.h" +#include "QGCFencePolygon.h" +#include "QGCFenceCircle.h" class Vehicle; class QmlObjectListModel; +class PlanManager; Q_DECLARE_LOGGING_CATEGORY(GeoFenceManagerLog) @@ -31,80 +35,67 @@ public: GeoFenceManager(Vehicle* vehicle); ~GeoFenceManager(); + /// Returns true if GeoFence is supported by this vehicle + virtual bool supported(void) const; + /// Returns true if the manager is currently communicating with the vehicle - virtual bool inProgress(void) const { return false; } + virtual bool inProgress(void) const; /// Load the current settings from the vehicle /// Signals loadComplete when done virtual void loadFromVehicle(void); - /// Send the current settings to the vehicle + /// Send the geofence settings to the vehicle /// Signals sendComplete when done - virtual void sendToVehicle(const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon); + virtual void sendToVehicle(const QGeoCoordinate& breachReturn, ///< Breach return point + QmlObjectListModel& polygons, ///< List of QGCFencePolygons + QmlObjectListModel& circles); ///< List of QGCFenceCircles /// Remove all fence related items from vehicle (does not affect parameters) /// Signals removeAllComplete when done virtual void removeAll(void); - /// Returns true if this vehicle support polygon fence - /// Signal: polygonSupportedChanged - virtual bool polygonSupported(void) const { return false; } - /// Returns true if polygon fence is currently enabled on this vehicle /// Signal: polygonEnabledChanged - virtual bool polygonEnabled(void) const { return false; } - - /// Returns true if breach return is supported by this vehicle - /// Signal: breachReturnSupportedChanged - virtual bool breachReturnSupported(void) const { return false; } - - /// Returns a list of parameter facts that relate to geofence support for the vehicle - /// Signal: paramsChanged - virtual QVariantList params(void) const { return QVariantList(); } + virtual bool polygonEnabled(void) const { return true; } - /// Returns the user visible labels for the paremeters returned by params method - /// Signal: paramLabelsChanged - virtual QStringList paramLabels(void) const { return QStringList(); } - - /// Returns true if circular fence is currently enabled on vehicle - /// Signal: circleEnabledChanged - virtual bool circleEnabled(void) const { return false; } - - /// Returns the fact which controls the fence circle radius. NULL if not supported - /// Signal: circleRadiusFactChanged - virtual Fact* circleRadiusFact(void) const { return NULL; } - - QList polygon (void) const { return _polygon; } - QGeoCoordinate breachReturnPoint (void) const { return _breachReturnPoint; } + const QList& polygons(void) { return _polygons; } + const QList& circles(void) { return _circles; } + const QGeoCoordinate& breachReturnPoint(void) const { return _breachReturnPoint; } /// Error codes returned in error signal typedef enum { InternalError, - TooFewPoints, ///< Too few points for valid geofence - TooManyPoints, ///< Too many points for valid geofence + PolygonTooFewPoints, ///< Too few points for valid fence polygon + PolygonTooManyPoints, ///< Too many points for valid fence polygon + IncompletePolygonLoad, ///< Incomplete polygon loaded + UnsupportedCommand, ///< Usupported command in mission type + BadPolygonItemFormat, ///< Error re-creating polygons from mission items InvalidCircleRadius, } ErrorCode_t; signals: - void loadComplete (const QGeoCoordinate& breachReturn, const QList& polygon); + void loadComplete (void); void inProgressChanged (bool inProgress); void error (int errorCode, const QString& errorMsg); - void paramsChanged (QVariantList params); - void paramLabelsChanged (QStringList paramLabels); - void circleEnabledChanged (bool circleEnabled); - void circleRadiusFactChanged (Fact* circleRadiusFact); - void polygonSupportedChanged (bool polygonSupported); - void polygonEnabledChanged (bool polygonEnabled); - void breachReturnSupportedChanged (bool breachReturnSupported); void removeAllComplete (bool error); void sendComplete (bool error); -protected: +private slots: + void _sendComplete (bool error); + void _planManagerLoadComplete (bool removeAllRequested); + +private: void _sendError(ErrorCode_t errorCode, const QString& errorMsg); Vehicle* _vehicle; - QList _polygon; + PlanManager _planManager; + QList _polygons; + QList _circles; QGeoCoordinate _breachReturnPoint; + bool _firstParamLoadComplete; + QList _sendPolygons; + QList _sendCircles; }; #endif diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index daba4ffb6b0218ae4dc4e154a8d644c6c67f3ac0..54362623dd4bb502edcb157456f98d03292d8876 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -110,6 +110,7 @@ public: bool loadTextFile(QFile& file, QString& errorString); // Overrides from PlanElementController + bool supported (void) const final { return true; }; void start (bool editMode) final; void save (QJsonObject& json) final; bool load (const QJsonObject& json, QString& errorString) final; diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index 5b3372f8aaf0de6235ebb68bed3c207edd7e1765..1a2d0115608b8b85a2a8ae9d476d835279c243ba 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -22,110 +22,15 @@ QGC_LOGGING_CATEGORY(MissionManagerLog, "MissionManagerLog") MissionManager::MissionManager(Vehicle* vehicle) - : _vehicle(vehicle) - , _dedicatedLink(NULL) - , _ackTimeoutTimer(NULL) - , _expectedAck(AckNone) - , _transactionInProgress(TransactionNone) - , _resumeMission(false) - , _lastMissionRequest(-1) - , _currentMissionIndex(-1) - , _lastCurrentIndex(-1) -{ - connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &MissionManager::_mavlinkMessageReceived); - - _ackTimeoutTimer = new QTimer(this); - _ackTimeoutTimer->setSingleShot(true); - _ackTimeoutTimer->setInterval(_ackTimeoutMilliseconds); - - connect(_ackTimeoutTimer, &QTimer::timeout, this, &MissionManager::_ackTimeout); -} - -MissionManager::~MissionManager() + : PlanManager(vehicle, MAV_MISSION_TYPE_MISSION) { } -void MissionManager::_writeMissionItemsWorker(void) -{ - _lastMissionRequest = -1; - - emit progressPct(0); - - qCDebug(MissionManagerLog) << "writeMissionItems count:" << _writeMissionItems.count(); - - // Prime write list - _itemIndicesToWrite.clear(); - for (int i=0; i<_writeMissionItems.count(); i++) { - _itemIndicesToWrite << i; - } - - _transactionInProgress = TransactionWrite; - _retryCount = 0; - emit inProgressChanged(true); - _writeMissionCount(); -} - - -void MissionManager::writeMissionItems(const QList& missionItems) -{ - if (_vehicle->isOfflineEditingVehicle()) { - return; - } - - if (inProgress()) { - qCDebug(MissionManagerLog) << "writeMissionItems called while transaction in progress"; - return; - } - - _clearAndDeleteWriteMissionItems(); - - bool skipFirstItem = !_vehicle->firmwarePlugin()->sendHomePositionToVehicle(); - - int firstIndex = skipFirstItem ? 1 : 0; - - for (int i=firstIndex; isetIsCurrentItem(i == firstIndex); - - if (skipFirstItem) { - // Home is in sequence 0, remainder of items start at sequence 1 - item->setSequenceNumber(item->sequenceNumber() - 1); - if (item->command() == MAV_CMD_DO_JUMP) { - item->setParam1((int)item->param1() - 1); - } - } - } - - _writeMissionItemsWorker(); -} - -/// This begins the write sequence with the vehicle. This may be called during a retry. -void MissionManager::_writeMissionCount(void) +MissionManager::~MissionManager() { - qCDebug(MissionManagerLog) << "_writeMissionCount count:_retryCount" << _writeMissionItems.count() << _retryCount; - - mavlink_message_t message; - mavlink_mission_count_t missionCount; - - memset(&missionCount, 0, sizeof(missionCount)); - missionCount.target_system = _vehicle->id(); - missionCount.target_component = MAV_COMP_ID_MISSIONPLANNER; - missionCount.count = _writeMissionItems.count(); - - _dedicatedLink = _vehicle->priorityLink(); - mavlink_msg_mission_count_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), - qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), - _dedicatedLink->mavlinkChannel(), - &message, - &missionCount); - _vehicle->sendMessageOnLink(_dedicatedLink, message); - _startAckTimeout(AckMissionRequest); } - void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoCoord, bool altChangeOnly) { if (inProgress()) { @@ -166,809 +71,6 @@ void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoC emit inProgressChanged(true); } -void MissionManager::loadFromVehicle(void) -{ - if (_vehicle->isOfflineEditingVehicle()) { - return; - } - - qCDebug(MissionManagerLog) << "loadFromVehicle read sequence"; - - if (inProgress()) { - qCDebug(MissionManagerLog) << "loadFromVehicle called while transaction in progress"; - return; - } - - _retryCount = 0; - _transactionInProgress = TransactionRead; - emit inProgressChanged(true); - _requestList(); -} - -/// Internal call to request list of mission items. May be called during a retry sequence. -void MissionManager::_requestList(void) -{ - qCDebug(MissionManagerLog) << "_requestList retry count" << _retryCount; - - mavlink_message_t message; - mavlink_mission_request_list_t request; - - memset(&request, 0, sizeof(request)); - - _itemIndicesToRead.clear(); - _clearMissionItems(); - - request.target_system = _vehicle->id(); - request.target_component = MAV_COMP_ID_MISSIONPLANNER; - - _dedicatedLink = _vehicle->priorityLink(); - mavlink_msg_mission_request_list_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), - qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), - _dedicatedLink->mavlinkChannel(), - &message, - &request); - - _vehicle->sendMessageOnLink(_dedicatedLink, message); - _startAckTimeout(AckMissionCount); -} - -void MissionManager::_ackTimeout(void) -{ - if (_expectedAck == AckNone) { - return; - } - - switch (_expectedAck) { - case AckNone: - qCWarning(MissionManagerLog) << "_ackTimeout timeout with AckNone"; - _sendError(InternalError, "Internal error occurred during Mission Item communication: _ackTimeOut:_expectedAck == AckNone"); - break; - case AckMissionCount: - // MISSION_COUNT message expected - if (_retryCount > _maxRetryCount) { - _sendError(VehicleError, QStringLiteral("Mission request list failed, maximum retries exceeded.")); - _finishTransaction(false); - } else { - _retryCount++; - qCDebug(MissionManagerLog) << "Retrying REQUEST_LIST retry Count" << _retryCount; - _requestList(); - } - break; - case AckMissionItem: - // MISSION_ITEM expected - if (_retryCount > _maxRetryCount) { - _sendError(VehicleError, QStringLiteral("Mission read failed, maximum retries exceeded.")); - _finishTransaction(false); - } else { - _retryCount++; - qCDebug(MissionManagerLog) << "Retrying MISSION_REQUEST retry Count" << _retryCount; - _requestNextMissionItem(); - } - break; - case AckMissionRequest: - // MISSION_REQUEST is expected, or MISSION_ACK to end sequence - if (_itemIndicesToWrite.count() == 0) { - // Vehicle did not send final MISSION_ACK at end of sequence - _sendError(VehicleError, QStringLiteral("Mission write failed, vehicle failed to send final ack.")); - _finishTransaction(false); - } else if (_itemIndicesToWrite[0] == 0) { - // Vehicle did not respond to MISSION_COUNT, try again - if (_retryCount > _maxRetryCount) { - _sendError(VehicleError, QStringLiteral("Mission write mission count failed, maximum retries exceeded.")); - _finishTransaction(false); - } else { - _retryCount++; - qCDebug(MissionManagerLog) << "Retrying MISSION_COUNT retry Count" << _retryCount; - _writeMissionCount(); - } - } else { - // Vehicle did not request all items from ground station - _sendError(AckTimeoutError, QString("Vehicle did not request all items from ground station: %1").arg(_ackTypeToString(_expectedAck))); - _expectedAck = AckNone; - _finishTransaction(false); - } - break; - case AckMissionClearAll: - // MISSION_ACK expected - if (_retryCount > _maxRetryCount) { - _sendError(VehicleError, QStringLiteral("Mission remove all, maximum retries exceeded.")); - _finishTransaction(false); - } else { - _retryCount++; - qCDebug(MissionManagerLog) << "Retrying MISSION_CLEAR_ALL retry Count" << _retryCount; - _removeAllWorker(); - } - break; - case AckGuidedItem: - // MISSION_REQUEST is expected, or MISSION_ACK to end sequence - default: - _sendError(AckTimeoutError, QString("Vehicle did not respond to mission item communication: %1").arg(_ackTypeToString(_expectedAck))); - _expectedAck = AckNone; - _finishTransaction(false); - } -} - -void MissionManager::_startAckTimeout(AckType_t ack) -{ - _expectedAck = ack; - _ackTimeoutTimer->start(); -} - -/// Checks the received ack against the expected ack. If they match the ack timeout timer will be stopped. -/// @return true: received ack matches expected ack -bool MissionManager::_checkForExpectedAck(AckType_t receivedAck) -{ - if (receivedAck == _expectedAck) { - _expectedAck = AckNone; - _ackTimeoutTimer->stop(); - return true; - } else { - if (_expectedAck == AckNone) { - // Don't worry about unexpected mission commands, just ignore them; ArduPilot updates home position using - // spurious MISSION_ITEMs. - } else { - // We just warn in this case, this could be crap left over from a previous transaction or the vehicle going bonkers. - // Whatever it is we let the ack timeout handle any error output to the user. - qCDebug(MissionManagerLog) << QString("Out of sequence ack expected:received %1:%2").arg(_ackTypeToString(_expectedAck)).arg(_ackTypeToString(receivedAck)); - } - return false; - } -} - -void MissionManager::_readTransactionComplete(void) -{ - qCDebug(MissionManagerLog) << "_readTransactionComplete read sequence complete"; - - mavlink_message_t message; - mavlink_mission_ack_t missionAck; - - memset(&missionAck, 0, sizeof(missionAck)); - - missionAck.target_system = _vehicle->id(); - missionAck.target_component = MAV_COMP_ID_MISSIONPLANNER; - missionAck.type = MAV_MISSION_ACCEPTED; - - mavlink_msg_mission_ack_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), - qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), - _dedicatedLink->mavlinkChannel(), - &message, - &missionAck); - - _vehicle->sendMessageOnLink(_dedicatedLink, message); - - _finishTransaction(true); -} - -void MissionManager::_handleMissionCount(const mavlink_message_t& message) -{ - mavlink_mission_count_t missionCount; - - if (!_checkForExpectedAck(AckMissionCount)) { - return; - } - - _retryCount = 0; - - mavlink_msg_mission_count_decode(&message, &missionCount); - qCDebug(MissionManagerLog) << "_handleMissionCount count:" << missionCount.count; - - if (missionCount.count == 0) { - _readTransactionComplete(); - } else { - // Prime read list - for (int i=0; isupportsMissionItemInt()) { - mavlink_mission_request_int_t missionRequest; - - memset(&missionRequest, 0, sizeof(missionRequest)); - missionRequest.target_system = _vehicle->id(); - missionRequest.target_component = MAV_COMP_ID_MISSIONPLANNER; - missionRequest.seq = _itemIndicesToRead[0]; - - mavlink_msg_mission_request_int_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), - qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), - _dedicatedLink->mavlinkChannel(), - &message, - &missionRequest); - } else { - mavlink_mission_request_t missionRequest; - - memset(&missionRequest, 0, sizeof(missionRequest)); - missionRequest.target_system = _vehicle->id(); - missionRequest.target_component = MAV_COMP_ID_MISSIONPLANNER; - missionRequest.seq = _itemIndicesToRead[0]; - - mavlink_msg_mission_request_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), - qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), - _dedicatedLink->mavlinkChannel(), - &message, - &missionRequest); - } - - _vehicle->sendMessageOnLink(_dedicatedLink, message); - _startAckTimeout(AckMissionItem); -} - -void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool missionItemInt) -{ - MAV_CMD command; - MAV_FRAME frame; - double param1; - double param2; - double param3; - double param4; - double param5; - double param6; - double param7; - bool autoContinue; - bool isCurrentItem; - int seq; - - if (missionItemInt) { - mavlink_mission_item_int_t missionItem; - mavlink_msg_mission_item_int_decode(&message, &missionItem); - - command = (MAV_CMD)missionItem.command, - frame = (MAV_FRAME)missionItem.frame, - param1 = missionItem.param1; - param2 = missionItem.param2; - param3 = missionItem.param3; - param4 = missionItem.param4; - param5 = (double)missionItem.x / qPow(10.0, 7.0); - param6 = (double)missionItem.y / qPow(10.0, 7.0); - param7 = (double)missionItem.z; - autoContinue = missionItem.autocontinue; - isCurrentItem = missionItem.current; - seq = missionItem.seq; - } else { - mavlink_mission_item_t missionItem; - mavlink_msg_mission_item_decode(&message, &missionItem); - - command = (MAV_CMD)missionItem.command, - frame = (MAV_FRAME)missionItem.frame, - param1 = missionItem.param1; - param2 = missionItem.param2; - param3 = missionItem.param3; - param4 = missionItem.param4; - param5 = missionItem.x; - param6 = missionItem.y; - param7 = missionItem.z; - autoContinue = missionItem.autocontinue; - isCurrentItem = missionItem.current; - seq = missionItem.seq; - } - - // We don't support editing ALT_INT frames so change on the way in. - if (frame == MAV_FRAME_GLOBAL_INT) { - frame = MAV_FRAME_GLOBAL; - } else if (frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { - frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - } - - - bool ardupilotHomePositionUpdate = false; - if (!_checkForExpectedAck(AckMissionItem)) { - if (_vehicle->apmFirmware() && seq == 0) { - ardupilotHomePositionUpdate = true; - } else { - qCDebug(MissionManagerLog) << "_handleMissionItem dropping spurious item seq:command:current" << seq << command << isCurrentItem; - return; - } - } - - qCDebug(MissionManagerLog) << "_handleMissionItem seq:command:current:ardupilotHomePositionUpdate" << seq << command << isCurrentItem << ardupilotHomePositionUpdate; - - if (ardupilotHomePositionUpdate) { - QGeoCoordinate newHomePosition(param5, param6, param7); - _vehicle->_setHomePosition(newHomePosition); - return; - } - - if (_itemIndicesToRead.contains(seq)) { - _itemIndicesToRead.removeOne(seq); - - MissionItem* item = new MissionItem(seq, - command, - frame, - param1, - param2, - param3, - param4, - param5, - param6, - param7, - autoContinue, - isCurrentItem, - this); - - if (item->command() == MAV_CMD_DO_JUMP && !_vehicle->firmwarePlugin()->sendHomePositionToVehicle()) { - // Home is in position 0 - item->setParam1((int)item->param1() + 1); - } - - _missionItems.append(item); - } else { - qCDebug(MissionManagerLog) << "_handleMissionItem mission item received item index which was not requested, disregrarding:" << seq; - // We have to put the ack timeout back since it was removed above - _startAckTimeout(AckMissionItem); - return; - } - - emit progressPct((double)seq / (double)_missionItems.count()); - - _retryCount = 0; - if (_itemIndicesToRead.count() == 0) { - _readTransactionComplete(); - } else { - _requestNextMissionItem(); - } -} - -void MissionManager::_clearMissionItems(void) -{ - _itemIndicesToRead.clear(); - _clearAndDeleteMissionItems(); -} - -void MissionManager::_handleMissionRequest(const mavlink_message_t& message, bool missionItemInt) -{ - mavlink_mission_request_t missionRequest; - - if (!_checkForExpectedAck(AckMissionRequest)) { - return; - } - - mavlink_msg_mission_request_decode(&message, &missionRequest); - qCDebug(MissionManagerLog) << "_handleMissionRequest sequenceNumber" << missionRequest.seq; - - if (missionRequest.seq > _writeMissionItems.count() - 1) { - _sendError(RequestRangeError, QString("Vehicle requested item outside range, count:request %1:%2. Send to Vehicle failed.").arg(_writeMissionItems.count()).arg(missionRequest.seq)); - _finishTransaction(false); - return; - } - - emit progressPct((double)missionRequest.seq / (double)_writeMissionItems.count()); - - _lastMissionRequest = missionRequest.seq; - if (!_itemIndicesToWrite.contains(missionRequest.seq)) { - qCDebug(MissionManagerLog) << "_handleMissionRequest sequence number requested which has already been sent, sending again:" << missionRequest.seq; - } else { - _itemIndicesToWrite.removeOne(missionRequest.seq); - } - - MissionItem* item = _writeMissionItems[missionRequest.seq]; - qCDebug(MissionManagerLog) << "_handleMissionRequest sequenceNumber:command" << missionRequest.seq << item->command(); - - mavlink_message_t messageOut; - if (missionItemInt) { - mavlink_mission_item_int_t missionItem; - - memset(&missionItem, 0, sizeof(missionItem)); - missionItem.target_system = _vehicle->id(); - missionItem.target_component = MAV_COMP_ID_MISSIONPLANNER; - missionItem.seq = missionRequest.seq; - missionItem.command = item->command(); - missionItem.param1 = item->param1(); - missionItem.param2 = item->param2(); - missionItem.param3 = item->param3(); - missionItem.param4 = item->param4(); - missionItem.x = item->param5() * qPow(10.0, 7.0); - missionItem.y = item->param6() * qPow(10.0, 7.0); - missionItem.z = item->param7(); - missionItem.frame = item->frame(); - missionItem.current = missionRequest.seq == 0; - missionItem.autocontinue = item->autoContinue(); - - mavlink_msg_mission_item_int_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), - qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), - _dedicatedLink->mavlinkChannel(), - &messageOut, - &missionItem); - } else { - mavlink_mission_item_t missionItem; - - memset(&missionItem, 0, sizeof(missionItem)); - missionItem.target_system = _vehicle->id(); - missionItem.target_component = MAV_COMP_ID_MISSIONPLANNER; - missionItem.seq = missionRequest.seq; - missionItem.command = item->command(); - missionItem.param1 = item->param1(); - missionItem.param2 = item->param2(); - missionItem.param3 = item->param3(); - missionItem.param4 = item->param4(); - missionItem.x = item->param5(); - missionItem.y = item->param6(); - missionItem.z = item->param7(); - missionItem.frame = item->frame(); - missionItem.current = missionRequest.seq == 0; - missionItem.autocontinue = item->autoContinue(); - - mavlink_msg_mission_item_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), - qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), - _dedicatedLink->mavlinkChannel(), - &messageOut, - &missionItem); - } - - _vehicle->sendMessageOnLink(_dedicatedLink, messageOut); - _startAckTimeout(AckMissionRequest); -} - -void MissionManager::_handleMissionAck(const mavlink_message_t& message) -{ - mavlink_mission_ack_t missionAck; - - // Save the retry ack before calling _checkForExpectedAck since we'll need it to determine what - // type of a protocol sequence we are in. - AckType_t savedExpectedAck = _expectedAck; - - // We can get a MISSION_ACK with an error at any time, so if the Acks don't match it is not - // a protocol sequence error. Call _checkForExpectedAck with _retryAck so it will succeed no - // matter what. - if (!_checkForExpectedAck(_expectedAck)) { - return; - } - - mavlink_msg_mission_ack_decode(&message, &missionAck); - - qCDebug(MissionManagerLog) << "_handleMissionAck type:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type); - - switch (savedExpectedAck) { - case AckNone: - // State machine is idle. Vehicle is confused. - _sendError(VehicleError, QString("Vehicle sent unexpected MISSION_ACK message, error: %1").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); - break; - case AckMissionCount: - // MISSION_COUNT message expected - _sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); - _finishTransaction(false); - break; - case AckMissionItem: - // MISSION_ITEM expected - _sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); - _finishTransaction(false); - break; - case AckMissionRequest: - // MISSION_REQUEST is expected, or MISSION_ACK to end sequence - if (missionAck.type == MAV_MISSION_ACCEPTED) { - if (_itemIndicesToWrite.count() == 0) { - qCDebug(MissionManagerLog) << "_handleMissionAck write sequence complete"; - _finishTransaction(true); - } else { - _sendError(MissingRequestsError, QString("Vehicle did not request all items during write sequence, missed count %1.").arg(_itemIndicesToWrite.count())); - _finishTransaction(false); - } - } else { - _sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); - _finishTransaction(false); - } - break; - case AckMissionClearAll: - // MISSION_ACK expected - if (missionAck.type != MAV_MISSION_ACCEPTED) { - _sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle remove all failed.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); - } - _finishTransaction(missionAck.type == MAV_MISSION_ACCEPTED); - break; - case AckGuidedItem: - // MISSION_REQUEST is expected, or MISSION_ACK to end sequence - if (missionAck.type == MAV_MISSION_ACCEPTED) { - qCDebug(MissionManagerLog) << "_handleMissionAck guided mode item accepted"; - _finishTransaction(true); - } else { - _sendError(VehicleError, QString("Vehicle returned error: %1. %2Vehicle did not accept guided item.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); - _finishTransaction(false); - } - break; - } -} -/// Called when a new mavlink message for out vehicle is received -void MissionManager::_mavlinkMessageReceived(const mavlink_message_t& message) -{ - switch (message.msgid) { - case MAVLINK_MSG_ID_MISSION_COUNT: - _handleMissionCount(message); - break; - - case MAVLINK_MSG_ID_MISSION_ITEM: - _handleMissionItem(message, false /* missionItemInt */); - break; - - case MAVLINK_MSG_ID_MISSION_ITEM_INT: - _handleMissionItem(message, true /* missionItemInt */); - break; - - case MAVLINK_MSG_ID_MISSION_REQUEST: - _handleMissionRequest(message, false /* missionItemInt */); - break; - - case MAVLINK_MSG_ID_MISSION_REQUEST_INT: - _handleMissionRequest(message, true /* missionItemInt */); - break; - - case MAVLINK_MSG_ID_MISSION_ACK: - _handleMissionAck(message); - break; - - case MAVLINK_MSG_ID_MISSION_ITEM_REACHED: - // FIXME: NYI - break; - - case MAVLINK_MSG_ID_MISSION_CURRENT: - _handleMissionCurrent(message); - break; - } -} - -void MissionManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg) -{ - qCDebug(MissionManagerLog) << "Sending error" << errorCode << errorMsg; - - emit error(errorCode, errorMsg); -} - -QString MissionManager::_ackTypeToString(AckType_t ackType) -{ - switch (ackType) { - case AckNone: - return QString("No Ack"); - case AckMissionCount: - return QString("MISSION_COUNT"); - case AckMissionItem: - return QString("MISSION_ITEM"); - case AckMissionRequest: - return QString("MISSION_REQUEST"); - case AckGuidedItem: - return QString("Guided Mode Item"); - default: - qWarning(MissionManagerLog) << "Fell off end of switch statement"; - return QString("QGC Internal Error"); - } -} - -QString MissionManager::_lastMissionReqestString(MAV_MISSION_RESULT result) -{ - if (_lastMissionRequest != -1 && _lastMissionRequest >= 0 && _lastMissionRequest < _writeMissionItems.count()) { - MissionItem* item = _writeMissionItems[_lastMissionRequest]; - - switch (result) { - case MAV_MISSION_UNSUPPORTED_FRAME: - return QString(". Frame: %1").arg(item->frame()); - case MAV_MISSION_UNSUPPORTED: - { - const MissionCommandUIInfo* uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_vehicle, item->command()); - QString friendlyName; - QString rawName; - if (uiInfo) { - friendlyName = uiInfo->friendlyName(); - rawName = uiInfo->rawName(); - } - return QString(". Command: (%1, %2, %3)").arg(friendlyName).arg(rawName).arg(item->command()); - } - case MAV_MISSION_INVALID_PARAM1: - return QString(". Param1: %1").arg(item->param1()); - case MAV_MISSION_INVALID_PARAM2: - return QString(". Param2: %1").arg(item->param2()); - case MAV_MISSION_INVALID_PARAM3: - return QString(". Param3: %1").arg(item->param3()); - case MAV_MISSION_INVALID_PARAM4: - return QString(". Param4: %1").arg(item->param4()); - case MAV_MISSION_INVALID_PARAM5_X: - return QString(". Param5: %1").arg(item->param5()); - case MAV_MISSION_INVALID_PARAM6_Y: - return QString(". Param6: %1").arg(item->param6()); - case MAV_MISSION_INVALID_PARAM7: - return QString(". Param7: %1").arg(item->param7()); - case MAV_MISSION_INVALID_SEQUENCE: - return QString(". Sequence: %1").arg(item->sequenceNumber()); - default: - break; - } - } - - return QString(); -} - -QString MissionManager::_missionResultToString(MAV_MISSION_RESULT result) -{ - QString resultString; - QString lastRequestString = _lastMissionReqestString(result); - - switch (result) { - case MAV_MISSION_ACCEPTED: - resultString = QString("Mission accepted (MAV_MISSION_ACCEPTED)"); - break; - case MAV_MISSION_ERROR: - resultString = QString("Unspecified error (MAV_MISSION_ERROR)"); - break; - case MAV_MISSION_UNSUPPORTED_FRAME: - resultString = QString("Coordinate frame is not supported (MAV_MISSION_UNSUPPORTED_FRAME)"); - break; - case MAV_MISSION_UNSUPPORTED: - resultString = QString("Command is not supported (MAV_MISSION_UNSUPPORTED)"); - break; - case MAV_MISSION_NO_SPACE: - resultString = QString("Mission item exceeds storage space (MAV_MISSION_NO_SPACE)"); - break; - case MAV_MISSION_INVALID: - resultString = QString("One of the parameters has an invalid value (MAV_MISSION_INVALID)"); - break; - case MAV_MISSION_INVALID_PARAM1: - resultString = QString("Param1 has an invalid value (MAV_MISSION_INVALID_PARAM1)"); - break; - case MAV_MISSION_INVALID_PARAM2: - resultString = QString("Param2 has an invalid value (MAV_MISSION_INVALID_PARAM2)"); - break; - case MAV_MISSION_INVALID_PARAM3: - resultString = QString("Param3 has an invalid value (MAV_MISSION_INVALID_PARAM3)"); - break; - case MAV_MISSION_INVALID_PARAM4: - resultString = QString("Param4 has an invalid value (MAV_MISSION_INVALID_PARAM4)"); - break; - case MAV_MISSION_INVALID_PARAM5_X: - resultString = QString("X/Param5 has an invalid value (MAV_MISSION_INVALID_PARAM5_X)"); - break; - case MAV_MISSION_INVALID_PARAM6_Y: - resultString = QString("Y/Param6 has an invalid value (MAV_MISSION_INVALID_PARAM6_Y)"); - break; - case MAV_MISSION_INVALID_PARAM7: - resultString = QString("Param7 has an invalid value (MAV_MISSION_INVALID_PARAM7)"); - break; - case MAV_MISSION_INVALID_SEQUENCE: - resultString = QString("Received mission item out of sequence (MAV_MISSION_INVALID_SEQUENCE)"); - break; - case MAV_MISSION_DENIED: - resultString = QString("Not accepting any mission commands (MAV_MISSION_DENIED)"); - break; - default: - qWarning(MissionManagerLog) << "Fell off end of switch statement"; - resultString = QString("QGC Internal Error"); - } - - return resultString + lastRequestString; -} - -void MissionManager::_finishTransaction(bool success) -{ - emit progressPct(1); - - _itemIndicesToRead.clear(); - _itemIndicesToWrite.clear(); - - // First thing we do is clear the transaction. This way inProgesss is off when we signal transaction complete. - TransactionType_t currentTransactionType = _transactionInProgress; - _transactionInProgress = TransactionNone; - if (currentTransactionType != TransactionNone) { - _transactionInProgress = TransactionNone; - qDebug() << "inProgressChanged"; - emit inProgressChanged(false); - } - - switch (currentTransactionType) { - case TransactionRead: - if (!success) { - // Read from vehicle failed, clear partial list - _clearAndDeleteMissionItems(); - } - emit newMissionItemsAvailable(false); - break; - case TransactionWrite: - if (success) { - // Write succeeded, update internal list to be current - _currentMissionIndex = -1; - _lastCurrentIndex = -1; - emit currentIndexChanged(-1); - emit lastCurrentIndexChanged(-1); - _clearAndDeleteMissionItems(); - for (int i=0; i<_writeMissionItems.count(); i++) { - _missionItems.append(_writeMissionItems[i]); - } - _writeMissionItems.clear(); - } else { - // Write failed, throw out the write list - _clearAndDeleteWriteMissionItems(); - } - emit sendComplete(!success /* error */); - break; - case TransactionRemoveAll: - emit removeAllComplete(!success /* error */); - break; - default: - break; - } - - if (_resumeMission) { - _resumeMission = false; - if (success) { - emit resumeMissionReady(); - } else { - emit resumeMissionUploadFail(); - } - } -} - -bool MissionManager::inProgress(void) -{ - return _transactionInProgress != TransactionNone; -} - -void MissionManager::_handleMissionCurrent(const mavlink_message_t& message) -{ - mavlink_mission_current_t missionCurrent; - - mavlink_msg_mission_current_decode(&message, &missionCurrent); - - if (missionCurrent.seq != _currentMissionIndex) { - qCDebug(MissionManagerLog) << "_handleMissionCurrent currentIndex:" << missionCurrent.seq; - _currentMissionIndex = missionCurrent.seq; - emit currentIndexChanged(_currentMissionIndex); - } - - if (_vehicle->flightMode() == _vehicle->missionFlightMode() && _currentMissionIndex != _lastCurrentIndex) { - qCDebug(MissionManagerLog) << "_handleMissionCurrent lastCurrentIndex:" << _currentMissionIndex; - _lastCurrentIndex = _currentMissionIndex; - emit lastCurrentIndexChanged(_lastCurrentIndex); - } -} - -void MissionManager::_removeAllWorker(void) -{ - mavlink_message_t message; - - qCDebug(MissionManagerLog) << "_removeAllWorker"; - - emit progressPct(0); - - _dedicatedLink = _vehicle->priorityLink(); - mavlink_msg_mission_clear_all_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), - qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), - _dedicatedLink->mavlinkChannel(), - &message, - _vehicle->id(), - MAV_COMP_ID_MISSIONPLANNER, - MAV_MISSION_TYPE_MISSION); - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), message); - _startAckTimeout(AckMissionClearAll); -} - -void MissionManager::removeAll(void) -{ - if (inProgress()) { - return; - } - - qCDebug(MissionManagerLog) << "removeAll"; - - _clearAndDeleteMissionItems(); - - _currentMissionIndex = -1; - _lastCurrentIndex = -1; - emit currentIndexChanged(-1); - emit lastCurrentIndexChanged(-1); - - _transactionInProgress = TransactionRemoveAll; - _retryCount = 0; - emit inProgressChanged(true); - - _removeAllWorker(); -} - void MissionManager::generateResumeMission(int resumeIndex) { if (_vehicle->isOfflineEditingVehicle()) { @@ -1129,20 +231,3 @@ void MissionManager::generateResumeMission(int resumeIndex) resumeMission[i]->deleteLater(); } } - -void MissionManager::_clearAndDeleteMissionItems(void) -{ - for (int i=0; i<_missionItems.count(); i++) { - _missionItems[i]->deleteLater(); - } - _missionItems.clear(); -} - - -void MissionManager::_clearAndDeleteWriteMissionItems(void) -{ - for (int i=0; i<_writeMissionItems.count(); i++) { - _writeMissionItems[i]->deleteLater(); - } - _writeMissionItems.clear(); -} diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h index 6c6a792a7c7433bbe2daf525d1c447d28734a4ed..bf6936c6aba9fefc7b5c41e964091f25be46060c 100644 --- a/src/MissionManager/MissionManager.h +++ b/src/MissionManager/MissionManager.h @@ -11,22 +11,11 @@ #ifndef MissionManager_H #define MissionManager_H -#include -#include -#include -#include -#include - -#include "MissionItem.h" -#include "QGCMAVLink.h" -#include "QGCLoggingCategory.h" -#include "LinkInterface.h" - -class Vehicle; +#include "PlanManager.h" Q_DECLARE_LOGGING_CATEGORY(MissionManagerLog) -class MissionManager : public QObject +class MissionManager : public PlanManager { Q_OBJECT @@ -34,128 +23,14 @@ public: MissionManager(Vehicle* vehicle); ~MissionManager(); - bool inProgress(void); - const QList& missionItems(void) { return _missionItems; } - - /// Current mission item as reported by MISSION_CURRENT - int currentIndex(void) const { return _currentMissionIndex; } - - /// Last current mission item reported while in Mission flight mode - int lastCurrentIndex(void) const { return _lastCurrentIndex; } - - /// Load the mission items from the vehicle - /// Signals newMissionItemsAvailable when done - void loadFromVehicle(void); - - /// Writes the specified set of mission items to the vehicle - /// @param missionItems Items to send to vehicle - /// Signals sendComplete when done - void writeMissionItems(const QList& missionItems); - /// Writes the specified set mission items to the vehicle as an ArduPilot guided mode mission item. /// @param gotoCoord Coordinate to move to /// @param altChangeOnly true: only altitude change, false: lat/lon/alt change void writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoCoord, bool altChangeOnly); - /// Removes all mission items from vehicle - /// Signals removeAllComplete when done - void removeAll(void); - /// Generates a new mission which starts from the specified index. It will include all the CMD_DO items /// from mission start to resumeIndex in the generate mission. void generateResumeMission(int resumeIndex); - - /// Error codes returned in error signal - typedef enum { - InternalError, - AckTimeoutError, ///< Timed out waiting for response from vehicle - ProtocolOrderError, ///< Incorrect protocol sequence from vehicle - RequestRangeError, ///< Vehicle requested item out of range - ItemMismatchError, ///< Vehicle returned item with seq # different than requested - VehicleError, ///< Vehicle returned error - MissingRequestsError, ///< Vehicle did not request all items during write sequence - MaxRetryExceeded, ///< Retry failed - } ErrorCode_t; - - // These values are public so the unit test can set appropriate signal wait times - static const int _ackTimeoutMilliseconds = 1000; - static const int _maxRetryCount = 5; - -signals: - void newMissionItemsAvailable(bool removeAllRequested); - void inProgressChanged(bool inProgress); - void error(int errorCode, const QString& errorMsg); - void currentIndexChanged(int currentIndex); - void lastCurrentIndexChanged(int lastCurrentIndex); - void resumeMissionReady(void); - void resumeMissionUploadFail(void); - void progressPct(double progressPercentPct); - void removeAllComplete (bool error); - void sendComplete (bool error); - -private slots: - void _mavlinkMessageReceived(const mavlink_message_t& message); - void _ackTimeout(void); - -private: - typedef enum { - AckNone, ///< State machine is idle - AckMissionCount, ///< MISSION_COUNT message expected - AckMissionItem, ///< MISSION_ITEM expected - AckMissionRequest, ///< MISSION_REQUEST is expected, or MISSION_ACK to end sequence - AckMissionClearAll, ///< MISSION_CLEAR_ALL sent, MISSION_ACK is expected - AckGuidedItem, ///< MISSION_ACK expected in response to ArduPilot guided mode single item send - } AckType_t; - - typedef enum { - TransactionNone, - TransactionRead, - TransactionWrite, - TransactionRemoveAll - } TransactionType_t; - - void _startAckTimeout(AckType_t ack); - bool _checkForExpectedAck(AckType_t receivedAck); - void _readTransactionComplete(void); - void _handleMissionCount(const mavlink_message_t& message); - void _handleMissionItem(const mavlink_message_t& message, bool missionItemInt); - void _handleMissionRequest(const mavlink_message_t& message, bool missionItemInt); - void _handleMissionAck(const mavlink_message_t& message); - void _handleMissionCurrent(const mavlink_message_t& message); - void _requestNextMissionItem(void); - void _clearMissionItems(void); - void _sendError(ErrorCode_t errorCode, const QString& errorMsg); - QString _ackTypeToString(AckType_t ackType); - QString _missionResultToString(MAV_MISSION_RESULT result); - void _finishTransaction(bool success); - void _requestList(void); - void _writeMissionCount(void); - void _writeMissionItemsWorker(void); - void _clearAndDeleteMissionItems(void); - void _clearAndDeleteWriteMissionItems(void); - QString _lastMissionReqestString(MAV_MISSION_RESULT result); - void _removeAllWorker(void); - -private: - Vehicle* _vehicle; - LinkInterface* _dedicatedLink; - - QTimer* _ackTimeoutTimer; - AckType_t _expectedAck; - int _retryCount; - - TransactionType_t _transactionInProgress; - bool _resumeMission; - QList _itemIndicesToWrite; ///< List of mission items which still need to be written to vehicle - QList _itemIndicesToRead; ///< List of mission items which still need to be requested from vehicle - int _lastMissionRequest; ///< Index of item last requested by MISSION_REQUEST - - QMutex _dataMutex; - - QList _missionItems; ///< Set of mission items on vehicle - QList _writeMissionItems; ///< Set of mission items currently being written to vehicle - int _currentMissionIndex; - int _lastCurrentIndex; }; #endif diff --git a/src/MissionManager/PlanElementController.h b/src/MissionManager/PlanElementController.h index 090ea2a4617d967bfafd7aa00d17340ddebf33f3..ffe29497831e3633b304b467f9985f9f271866b6 100644 --- a/src/MissionManager/PlanElementController.h +++ b/src/MissionManager/PlanElementController.h @@ -27,6 +27,7 @@ public: PlanElementController(PlanMasterController* masterController, QObject* parent = NULL); ~PlanElementController(); + Q_PROPERTY(bool supported READ supported NOTIFY supportedChanged) ///< true: Element is supported by Vehicle Q_PROPERTY(bool containsItems READ containsItems NOTIFY containsItemsChanged) ///< true: Elemement is non-empty Q_PROPERTY(bool syncInProgress READ syncInProgress NOTIFY syncInProgressChanged) ///< true: information is currently being saved/sent, false: no active save/send in progress Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) ///< true: unsaved/sent changes are present, false: no changes since last save/send @@ -41,10 +42,11 @@ public: virtual void removeAll (void) = 0; ///< Removes all from controller only virtual bool showPlanFromManagerVehicle (void) = 0; /// true: controller is waiting for the current load to complete - virtual bool containsItems (void) const = 0; - virtual bool syncInProgress (void) const = 0; - virtual bool dirty (void) const = 0; - virtual void setDirty (bool dirty) = 0; + virtual bool supported (void) const = 0; + virtual bool containsItems (void) const = 0; + virtual bool syncInProgress (void) const = 0; + virtual bool dirty (void) const = 0; + virtual void setDirty (bool dirty) = 0; /// Sends the current plan element to the vehicle /// Signals sendComplete when done @@ -58,6 +60,7 @@ public: virtual void managerVehicleChanged(Vehicle* managerVehicle) = 0; signals: + void supportedChanged (bool supported); void containsItemsChanged (bool containsItems); void syncInProgressChanged (bool syncInProgress); void dirtyChanged (bool dirty); diff --git a/src/MissionManager/PlanManager.cc b/src/MissionManager/PlanManager.cc new file mode 100644 index 0000000000000000000000000000000000000000..b132c5228e97d0e3eb89fac46901374177edec89 --- /dev/null +++ b/src/MissionManager/PlanManager.cc @@ -0,0 +1,970 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + + +/// @file +/// @author Don Gagne + +#include "PlanManager.h" +#include "Vehicle.h" +#include "FirmwarePlugin.h" +#include "MAVLinkProtocol.h" +#include "QGCApplication.h" +#include "MissionCommandTree.h" +#include "MissionCommandUIInfo.h" + +QGC_LOGGING_CATEGORY(PlanManagerLog, "PlanManagerLog") + +PlanManager::PlanManager(Vehicle* vehicle, MAV_MISSION_TYPE planType) + : _vehicle(vehicle) + , _planType(planType) + , _dedicatedLink(NULL) + , _ackTimeoutTimer(NULL) + , _expectedAck(AckNone) + , _transactionInProgress(TransactionNone) + , _resumeMission(false) + , _lastMissionRequest(-1) + , _currentMissionIndex(-1) + , _lastCurrentIndex(-1) +{ + _ackTimeoutTimer = new QTimer(this); + _ackTimeoutTimer->setSingleShot(true); + _ackTimeoutTimer->setInterval(_ackTimeoutMilliseconds); + + connect(_ackTimeoutTimer, &QTimer::timeout, this, &PlanManager::_ackTimeout); +} + +PlanManager::~PlanManager() +{ + +} + +void PlanManager::_writeMissionItemsWorker(void) +{ + _lastMissionRequest = -1; + + emit progressPct(0); + + qCDebug(PlanManagerLog) << QStringLiteral("writeMissionItems %1 count:").arg(_planTypeString()) << _writeMissionItems.count(); + + // Prime write list + _itemIndicesToWrite.clear(); + for (int i=0; i<_writeMissionItems.count(); i++) { + _itemIndicesToWrite << i; + } + + _transactionInProgress = TransactionWrite; + _retryCount = 0; + emit inProgressChanged(true); + _connectToMavlink(); + _writeMissionCount(); +} + + +void PlanManager::writeMissionItems(const QList& missionItems) +{ + if (_vehicle->isOfflineEditingVehicle()) { + return; + } + + if (inProgress()) { + qCDebug(PlanManagerLog) << QStringLiteral("writeMissionItems %1 called while transaction in progress").arg(_planTypeString()); + return; + } + + _clearAndDeleteWriteMissionItems(); + + bool skipFirstItem = _planType == MAV_MISSION_TYPE_MISSION && !_vehicle->firmwarePlugin()->sendHomePositionToVehicle(); + + int firstIndex = skipFirstItem ? 1 : 0; + + for (int i=firstIndex; isetIsCurrentItem(i == firstIndex); + + if (skipFirstItem) { + // Home is in sequence 0, remainder of items start at sequence 1 + item->setSequenceNumber(item->sequenceNumber() - 1); + if (item->command() == MAV_CMD_DO_JUMP) { + item->setParam1((int)item->param1() - 1); + } + } + } + + _writeMissionItemsWorker(); +} + +/// This begins the write sequence with the vehicle. This may be called during a retry. +void PlanManager::_writeMissionCount(void) +{ + qCDebug(PlanManagerLog) << QStringLiteral("_writeMissionCount %1 count:_retryCount").arg(_planTypeString()) << _writeMissionItems.count() << _retryCount; + + mavlink_message_t message; + + _dedicatedLink = _vehicle->priorityLink(); + mavlink_msg_mission_count_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _dedicatedLink->mavlinkChannel(), + &message, + _vehicle->id(), + MAV_COMP_ID_MISSIONPLANNER, + _writeMissionItems.count(), + _planType); + + _vehicle->sendMessageOnLink(_dedicatedLink, message); + _startAckTimeout(AckMissionRequest); +} + +void PlanManager::loadFromVehicle(void) +{ + if (_vehicle->isOfflineEditingVehicle()) { + return; + } + + qCDebug(PlanManagerLog) << QStringLiteral("loadFromVehicle %1 read sequence").arg(_planTypeString()); + + if (inProgress()) { + qCDebug(PlanManagerLog) << QStringLiteral("loadFromVehicle %1 called while transaction in progress").arg(_planTypeString()); + return; + } + + _retryCount = 0; + _transactionInProgress = TransactionRead; + emit inProgressChanged(true); + _connectToMavlink(); + _requestList(); +} + +/// Internal call to request list of mission items. May be called during a retry sequence. +void PlanManager::_requestList(void) +{ + qCDebug(PlanManagerLog) << QStringLiteral("_requestList %1 _planType:_retryCount").arg(_planTypeString()) << _planType << _retryCount; + + mavlink_message_t message; + + _itemIndicesToRead.clear(); + _clearMissionItems(); + + _dedicatedLink = _vehicle->priorityLink(); + mavlink_msg_mission_request_list_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _dedicatedLink->mavlinkChannel(), + &message, + _vehicle->id(), + MAV_COMP_ID_MISSIONPLANNER, + _planType); + + _vehicle->sendMessageOnLink(_dedicatedLink, message); + _startAckTimeout(AckMissionCount); +} + +void PlanManager::_ackTimeout(void) +{ + if (_expectedAck == AckNone) { + return; + } + + switch (_expectedAck) { + case AckNone: + qCWarning(PlanManagerLog) << QStringLiteral("_ackTimeout %1 timeout with AckNone").arg(_planTypeString()); + _sendError(InternalError, "Internal error occurred during Mission Item communication: _ackTimeOut:_expectedAck == AckNone"); + break; + case AckMissionCount: + // MISSION_COUNT message expected + if (_retryCount > _maxRetryCount) { + _sendError(VehicleError, QStringLiteral("Mission request list failed, maximum retries exceeded.")); + _finishTransaction(false); + } else { + _retryCount++; + qCDebug(PlanManagerLog) << QStringLiteral("Retrying %1 REQUEST_LIST retry Count").arg(_planTypeString()) << _retryCount; + _requestList(); + } + break; + case AckMissionItem: + // MISSION_ITEM expected + if (_retryCount > _maxRetryCount) { + _sendError(VehicleError, QStringLiteral("Mission read failed, maximum retries exceeded.")); + _finishTransaction(false); + } else { + _retryCount++; + qCDebug(PlanManagerLog) << QStringLiteral("Retrying %1 MISSION_REQUEST retry Count").arg(_planTypeString()) << _retryCount; + _requestNextMissionItem(); + } + break; + case AckMissionRequest: + // MISSION_REQUEST is expected, or MISSION_ACK to end sequence + if (_itemIndicesToWrite.count() == 0) { + // Vehicle did not send final MISSION_ACK at end of sequence + _sendError(VehicleError, QStringLiteral("Mission write failed, vehicle failed to send final ack.")); + _finishTransaction(false); + } else if (_itemIndicesToWrite[0] == 0) { + // Vehicle did not respond to MISSION_COUNT, try again + if (_retryCount > _maxRetryCount) { + _sendError(VehicleError, QStringLiteral("Mission write mission count failed, maximum retries exceeded.")); + _finishTransaction(false); + } else { + _retryCount++; + qCDebug(PlanManagerLog) << QStringLiteral("Retrying %1 MISSION_COUNT retry Count").arg(_planTypeString()) << _retryCount; + _writeMissionCount(); + } + } else { + // Vehicle did not request all items from ground station + _sendError(AckTimeoutError, QString("Vehicle did not request all items from ground station: %1").arg(_ackTypeToString(_expectedAck))); + _expectedAck = AckNone; + _finishTransaction(false); + } + break; + case AckMissionClearAll: + // MISSION_ACK expected + if (_retryCount > _maxRetryCount) { + _sendError(VehicleError, QStringLiteral("Mission remove all, maximum retries exceeded.")); + _finishTransaction(false); + } else { + _retryCount++; + qCDebug(PlanManagerLog) << QStringLiteral("Retrying %1 MISSION_CLEAR_ALL retry Count").arg(_planTypeString()) << _retryCount; + _removeAllWorker(); + } + break; + case AckGuidedItem: + // MISSION_REQUEST is expected, or MISSION_ACK to end sequence + default: + _sendError(AckTimeoutError, QString("Vehicle did not respond to mission item communication: %1").arg(_ackTypeToString(_expectedAck))); + _expectedAck = AckNone; + _finishTransaction(false); + } +} + +void PlanManager::_startAckTimeout(AckType_t ack) +{ + _expectedAck = ack; + _ackTimeoutTimer->start(); +} + +/// Checks the received ack against the expected ack. If they match the ack timeout timer will be stopped. +/// @return true: received ack matches expected ack +bool PlanManager::_checkForExpectedAck(AckType_t receivedAck) +{ + if (receivedAck == _expectedAck) { + _expectedAck = AckNone; + _ackTimeoutTimer->stop(); + return true; + } else { + if (_expectedAck == AckNone) { + // Don't worry about unexpected mission commands, just ignore them; ArduPilot updates home position using + // spurious MISSION_ITEMs. + } else { + // We just warn in this case, this could be crap left over from a previous transaction or the vehicle going bonkers. + // Whatever it is we let the ack timeout handle any error output to the user. + qCDebug(PlanManagerLog) << QString("Out of sequence ack expected:received %1:%2 %1").arg(_ackTypeToString(_expectedAck)).arg(_ackTypeToString(receivedAck)).arg(_planTypeString()); + } + return false; + } +} + +void PlanManager::_readTransactionComplete(void) +{ + qCDebug(PlanManagerLog) << "_readTransactionComplete read sequence complete"; + + mavlink_message_t message; + + mavlink_msg_mission_ack_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _dedicatedLink->mavlinkChannel(), + &message, + _vehicle->id(), + MAV_COMP_ID_MISSIONPLANNER, + MAV_MISSION_ACCEPTED, + _planType); + + _vehicle->sendMessageOnLink(_dedicatedLink, message); + + _finishTransaction(true); +} + +void PlanManager::_handleMissionCount(const mavlink_message_t& message) +{ + mavlink_mission_count_t missionCount; + + mavlink_msg_mission_count_decode(&message, &missionCount); + + if (missionCount.mission_type != _planType) { + // if there was a previous transaction with a different mission_type, it can happen that we receive + // a stale message here, for example when the MAV ran into a timeout and sent a message twice + qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionCount %1 Incorrect mission_type received expected:actual").arg(_planTypeString()) << _planType << missionCount.mission_type; + return; + } + + if (!_checkForExpectedAck(AckMissionCount)) { + return; + } + + qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionCount %1 count:").arg(_planTypeString()) << missionCount.count; + + _retryCount = 0; + + if (missionCount.count == 0) { + _readTransactionComplete(); + } else { + // Prime read list + for (int i=0; icapabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_INT) { + mavlink_msg_mission_request_int_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _dedicatedLink->mavlinkChannel(), + &message, + _vehicle->id(), + MAV_COMP_ID_MISSIONPLANNER, + _itemIndicesToRead[0], + _planType); + } else { + mavlink_msg_mission_request_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _dedicatedLink->mavlinkChannel(), + &message, + _vehicle->id(), + MAV_COMP_ID_MISSIONPLANNER, + _itemIndicesToRead[0], + _planType); + } + + _vehicle->sendMessageOnLink(_dedicatedLink, message); + _startAckTimeout(AckMissionItem); +} + +void PlanManager::_handleMissionItem(const mavlink_message_t& message, bool missionItemInt) +{ + MAV_CMD command; + MAV_FRAME frame; + double param1; + double param2; + double param3; + double param4; + double param5; + double param6; + double param7; + bool autoContinue; + bool isCurrentItem; + int seq; + + if (missionItemInt) { + mavlink_mission_item_int_t missionItem; + mavlink_msg_mission_item_int_decode(&message, &missionItem); + + command = (MAV_CMD)missionItem.command, + frame = (MAV_FRAME)missionItem.frame, + param1 = missionItem.param1; + param2 = missionItem.param2; + param3 = missionItem.param3; + param4 = missionItem.param4; + param5 = (double)missionItem.x / qPow(10.0, 7.0); + param6 = (double)missionItem.y / qPow(10.0, 7.0); + param7 = (double)missionItem.z; + autoContinue = missionItem.autocontinue; + isCurrentItem = missionItem.current; + seq = missionItem.seq; + } else { + mavlink_mission_item_t missionItem; + mavlink_msg_mission_item_decode(&message, &missionItem); + + command = (MAV_CMD)missionItem.command, + frame = (MAV_FRAME)missionItem.frame, + param1 = missionItem.param1; + param2 = missionItem.param2; + param3 = missionItem.param3; + param4 = missionItem.param4; + param5 = missionItem.x; + param6 = missionItem.y; + param7 = missionItem.z; + autoContinue = missionItem.autocontinue; + isCurrentItem = missionItem.current; + seq = missionItem.seq; + } + + // We don't support editing ALT_INT frames so change on the way in. + if (frame == MAV_FRAME_GLOBAL_INT) { + frame = MAV_FRAME_GLOBAL; + } else if (frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { + frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + } + + + bool ardupilotHomePositionUpdate = false; + if (!_checkForExpectedAck(AckMissionItem)) { + if (_vehicle->apmFirmware() && seq == 0 && _planType == MAV_MISSION_TYPE_MISSION) { + ardupilotHomePositionUpdate = true; + } else { + qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionItem %1 dropping spurious item seq:command:current").arg(_planTypeString()) << seq << command << isCurrentItem; + return; + } + } + + qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionItem %1 seq:command:current:ardupilotHomePositionUpdate").arg(_planTypeString()) << seq << command << isCurrentItem << ardupilotHomePositionUpdate; + + if (ardupilotHomePositionUpdate) { + QGeoCoordinate newHomePosition(param5, param6, param7); + _vehicle->_setHomePosition(newHomePosition); + return; + } + + if (_itemIndicesToRead.contains(seq)) { + _itemIndicesToRead.removeOne(seq); + + MissionItem* item = new MissionItem(seq, + command, + frame, + param1, + param2, + param3, + param4, + param5, + param6, + param7, + autoContinue, + isCurrentItem, + this); + + if (item->command() == MAV_CMD_DO_JUMP && !_vehicle->firmwarePlugin()->sendHomePositionToVehicle()) { + // Home is in position 0 + item->setParam1((int)item->param1() + 1); + } + + _missionItems.append(item); + } else { + qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionItem %1 mission item received item index which was not requested, disregrarding:").arg(_planTypeString()) << seq; + // We have to put the ack timeout back since it was removed above + _startAckTimeout(AckMissionItem); + return; + } + + emit progressPct((double)seq / (double)_missionItems.count()); + + _retryCount = 0; + if (_itemIndicesToRead.count() == 0) { + _readTransactionComplete(); + } else { + _requestNextMissionItem(); + } +} + +void PlanManager::_clearMissionItems(void) +{ + _itemIndicesToRead.clear(); + _clearAndDeleteMissionItems(); +} + +void PlanManager::_handleMissionRequest(const mavlink_message_t& message, bool missionItemInt) +{ + mavlink_mission_request_t missionRequest; + + mavlink_msg_mission_request_decode(&message, &missionRequest); + + if (missionRequest.mission_type != _planType) { + // if there was a previous transaction with a different mission_type, it can happen that we receive + // a stale message here, for example when the MAV ran into a timeout and sent a message twice + qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 Incorrect mission_type received expected:actual").arg(_planTypeString()) << _planType << missionRequest.mission_type; + return; + } + + if (!_checkForExpectedAck(AckMissionRequest)) { + return; + } + + qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 sequenceNumber").arg(_planTypeString()) << missionRequest.seq; + + if (missionRequest.seq > _writeMissionItems.count() - 1) { + _sendError(RequestRangeError, QString("Vehicle requested item outside range, count:request %1:%2. Send to Vehicle failed.").arg(_writeMissionItems.count()).arg(missionRequest.seq)); + _finishTransaction(false); + return; + } + + emit progressPct((double)missionRequest.seq / (double)_writeMissionItems.count()); + + _lastMissionRequest = missionRequest.seq; + if (!_itemIndicesToWrite.contains(missionRequest.seq)) { + qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 sequence number requested which has already been sent, sending again:").arg(_planTypeString()) << missionRequest.seq; + } else { + _itemIndicesToWrite.removeOne(missionRequest.seq); + } + + MissionItem* item = _writeMissionItems[missionRequest.seq]; + qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 sequenceNumber:command").arg(_planTypeString()) << missionRequest.seq << item->command(); + + mavlink_message_t messageOut; + if (missionItemInt) { + mavlink_msg_mission_item_int_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _dedicatedLink->mavlinkChannel(), + &messageOut, + _vehicle->id(), + MAV_COMP_ID_MISSIONPLANNER, + missionRequest.seq, + item->frame(), + item->command(), + missionRequest.seq == 0, + item->autoContinue(), + item->param1(), + item->param2(), + item->param3(), + item->param4(), + item->param5() * qPow(10.0, 7.0), + item->param6() * qPow(10.0, 7.0), + item->param7(), + _planType); + } else { + mavlink_msg_mission_item_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _dedicatedLink->mavlinkChannel(), + &messageOut, + _vehicle->id(), + MAV_COMP_ID_MISSIONPLANNER, + missionRequest.seq, + item->frame(), + item->command(), + missionRequest.seq == 0, + item->autoContinue(), + item->param1(), + item->param2(), + item->param3(), + item->param4(), + item->param5(), + item->param6(), + item->param7(), + _planType); + } + + _vehicle->sendMessageOnLink(_dedicatedLink, messageOut); + _startAckTimeout(AckMissionRequest); +} + +void PlanManager::_handleMissionAck(const mavlink_message_t& message) +{ + mavlink_mission_ack_t missionAck; + + mavlink_msg_mission_ack_decode(&message, &missionAck); + if (missionAck.mission_type != _planType) { + // if there was a previous transaction with a different mission_type, it can happen that we receive + // a stale message here, for example when the MAV ran into a timeout and sent a message twice + qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionAck %1 Incorrect mission_type received expected:actual").arg(_planTypeString()) << _planType << missionAck.mission_type; + return; + } + + // Save the retry ack before calling _checkForExpectedAck since we'll need it to determine what + // type of a protocol sequence we are in. + AckType_t savedExpectedAck = _expectedAck; + + // We can get a MISSION_ACK with an error at any time, so if the Acks don't match it is not + // a protocol sequence error. Call _checkForExpectedAck with _retryAck so it will succeed no + // matter what. + if (!_checkForExpectedAck(_expectedAck)) { + return; + } + + qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionAck %1 type:").arg(_planTypeString()) << _missionResultToString((MAV_MISSION_RESULT)missionAck.type); + + switch (savedExpectedAck) { + case AckNone: + // State machine is idle. Vehicle is confused. + _sendError(VehicleError, QString("Vehicle sent unexpected MISSION_ACK message, error: %1").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); + break; + case AckMissionCount: + // MISSION_COUNT message expected + _sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); + _finishTransaction(false); + break; + case AckMissionItem: + // MISSION_ITEM expected + _sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); + _finishTransaction(false); + break; + case AckMissionRequest: + // MISSION_REQUEST is expected, or MISSION_ACK to end sequence + if (missionAck.type == MAV_MISSION_ACCEPTED) { + if (_itemIndicesToWrite.count() == 0) { + qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionAck write sequence complete").arg(_planTypeString()); + _finishTransaction(true); + } else { + _sendError(MissingRequestsError, QString("Vehicle did not request all items during write sequence, missed count %1.").arg(_itemIndicesToWrite.count())); + _finishTransaction(false); + } + } else { + _sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); + _finishTransaction(false); + } + break; + case AckMissionClearAll: + // MISSION_ACK expected + if (missionAck.type != MAV_MISSION_ACCEPTED) { + _sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle remove all failed.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); + } + _finishTransaction(missionAck.type == MAV_MISSION_ACCEPTED); + break; + case AckGuidedItem: + // MISSION_REQUEST is expected, or MISSION_ACK to end sequence + if (missionAck.type == MAV_MISSION_ACCEPTED) { + qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionAck %1 guided mode item accepted").arg(_planTypeString()); + _finishTransaction(true); + } else { + _sendError(VehicleError, QString("Vehicle returned error: %1. %2Vehicle did not accept guided item.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); + _finishTransaction(false); + } + break; + } +} +/// Called when a new mavlink message for out vehicle is received +void PlanManager::_mavlinkMessageReceived(const mavlink_message_t& message) +{ + switch (message.msgid) { + case MAVLINK_MSG_ID_MISSION_COUNT: + _handleMissionCount(message); + break; + + case MAVLINK_MSG_ID_MISSION_ITEM: + _handleMissionItem(message, false /* missionItemInt */); + break; + + case MAVLINK_MSG_ID_MISSION_ITEM_INT: + _handleMissionItem(message, true /* missionItemInt */); + break; + + case MAVLINK_MSG_ID_MISSION_REQUEST: + _handleMissionRequest(message, false /* missionItemInt */); + break; + + case MAVLINK_MSG_ID_MISSION_REQUEST_INT: + _handleMissionRequest(message, true /* missionItemInt */); + break; + + case MAVLINK_MSG_ID_MISSION_ACK: + _handleMissionAck(message); + break; + + case MAVLINK_MSG_ID_MISSION_ITEM_REACHED: + // FIXME: NYI + break; + + case MAVLINK_MSG_ID_MISSION_CURRENT: + _handleMissionCurrent(message); + break; + } +} + +void PlanManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg) +{ + qCDebug(PlanManagerLog) << QStringLiteral("Sending %1 error").arg(_planTypeString()) << errorCode << errorMsg; + + emit error(errorCode, errorMsg); +} + +QString PlanManager::_ackTypeToString(AckType_t ackType) +{ + switch (ackType) { + case AckNone: + return QString("No Ack"); + case AckMissionCount: + return QString("MISSION_COUNT"); + case AckMissionItem: + return QString("MISSION_ITEM"); + case AckMissionRequest: + return QString("MISSION_REQUEST"); + case AckGuidedItem: + return QString("Guided Mode Item"); + default: + qWarning(PlanManagerLog) << QStringLiteral("Fell off end of switch statement %1").arg(_planTypeString()); + return QString("QGC Internal Error"); + } +} + +QString PlanManager::_lastMissionReqestString(MAV_MISSION_RESULT result) +{ + if (_lastMissionRequest != -1 && _lastMissionRequest >= 0 && _lastMissionRequest < _writeMissionItems.count()) { + MissionItem* item = _writeMissionItems[_lastMissionRequest]; + + switch (result) { + case MAV_MISSION_UNSUPPORTED_FRAME: + return QString(". Frame: %1").arg(item->frame()); + case MAV_MISSION_UNSUPPORTED: + { + const MissionCommandUIInfo* uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_vehicle, item->command()); + QString friendlyName; + QString rawName; + if (uiInfo) { + friendlyName = uiInfo->friendlyName(); + rawName = uiInfo->rawName(); + } + return QString(". Command: (%1, %2, %3)").arg(friendlyName).arg(rawName).arg(item->command()); + } + case MAV_MISSION_INVALID_PARAM1: + return QString(". Param1: %1").arg(item->param1()); + case MAV_MISSION_INVALID_PARAM2: + return QString(". Param2: %1").arg(item->param2()); + case MAV_MISSION_INVALID_PARAM3: + return QString(". Param3: %1").arg(item->param3()); + case MAV_MISSION_INVALID_PARAM4: + return QString(". Param4: %1").arg(item->param4()); + case MAV_MISSION_INVALID_PARAM5_X: + return QString(". Param5: %1").arg(item->param5()); + case MAV_MISSION_INVALID_PARAM6_Y: + return QString(". Param6: %1").arg(item->param6()); + case MAV_MISSION_INVALID_PARAM7: + return QString(". Param7: %1").arg(item->param7()); + case MAV_MISSION_INVALID_SEQUENCE: + return QString(". Sequence: %1").arg(item->sequenceNumber()); + default: + break; + } + } + + return QString(); +} + +QString PlanManager::_missionResultToString(MAV_MISSION_RESULT result) +{ + QString resultString; + QString lastRequestString = _lastMissionReqestString(result); + + switch (result) { + case MAV_MISSION_ACCEPTED: + resultString = QString("Mission accepted (MAV_MISSION_ACCEPTED)"); + break; + case MAV_MISSION_ERROR: + resultString = QString("Unspecified error (MAV_MISSION_ERROR)"); + break; + case MAV_MISSION_UNSUPPORTED_FRAME: + resultString = QString("Coordinate frame is not supported (MAV_MISSION_UNSUPPORTED_FRAME)"); + break; + case MAV_MISSION_UNSUPPORTED: + resultString = QString("Command is not supported (MAV_MISSION_UNSUPPORTED)"); + break; + case MAV_MISSION_NO_SPACE: + resultString = QString("Mission item exceeds storage space (MAV_MISSION_NO_SPACE)"); + break; + case MAV_MISSION_INVALID: + resultString = QString("One of the parameters has an invalid value (MAV_MISSION_INVALID)"); + break; + case MAV_MISSION_INVALID_PARAM1: + resultString = QString("Param1 has an invalid value (MAV_MISSION_INVALID_PARAM1)"); + break; + case MAV_MISSION_INVALID_PARAM2: + resultString = QString("Param2 has an invalid value (MAV_MISSION_INVALID_PARAM2)"); + break; + case MAV_MISSION_INVALID_PARAM3: + resultString = QString("Param3 has an invalid value (MAV_MISSION_INVALID_PARAM3)"); + break; + case MAV_MISSION_INVALID_PARAM4: + resultString = QString("Param4 has an invalid value (MAV_MISSION_INVALID_PARAM4)"); + break; + case MAV_MISSION_INVALID_PARAM5_X: + resultString = QString("X/Param5 has an invalid value (MAV_MISSION_INVALID_PARAM5_X)"); + break; + case MAV_MISSION_INVALID_PARAM6_Y: + resultString = QString("Y/Param6 has an invalid value (MAV_MISSION_INVALID_PARAM6_Y)"); + break; + case MAV_MISSION_INVALID_PARAM7: + resultString = QString("Param7 has an invalid value (MAV_MISSION_INVALID_PARAM7)"); + break; + case MAV_MISSION_INVALID_SEQUENCE: + resultString = QString("Received mission item out of sequence (MAV_MISSION_INVALID_SEQUENCE)"); + break; + case MAV_MISSION_DENIED: + resultString = QString("Not accepting any mission commands (MAV_MISSION_DENIED)"); + break; + default: + qWarning(PlanManagerLog) << QStringLiteral("Fell off end of switch statement %1").arg(_planTypeString()); + resultString = QString("QGC Internal Error"); + } + + return resultString + lastRequestString; +} + +void PlanManager::_finishTransaction(bool success) +{ + emit progressPct(1); + _disconnectFromMavlink(); + + _itemIndicesToRead.clear(); + _itemIndicesToWrite.clear(); + + // First thing we do is clear the transaction. This way inProgesss is off when we signal transaction complete. + TransactionType_t currentTransactionType = _transactionInProgress; + _transactionInProgress = TransactionNone; + if (currentTransactionType != TransactionNone) { + _transactionInProgress = TransactionNone; + qCDebug(PlanManagerLog) << QStringLiteral("inProgressChanged %1").arg(_planTypeString()); + emit inProgressChanged(false); + } + + switch (currentTransactionType) { + case TransactionRead: + if (!success) { + // Read from vehicle failed, clear partial list + _clearAndDeleteMissionItems(); + } + emit newMissionItemsAvailable(false); + break; + case TransactionWrite: + if (success) { + // Write succeeded, update internal list to be current + _currentMissionIndex = -1; + _lastCurrentIndex = -1; + emit currentIndexChanged(-1); + emit lastCurrentIndexChanged(-1); + _clearAndDeleteMissionItems(); + for (int i=0; i<_writeMissionItems.count(); i++) { + _missionItems.append(_writeMissionItems[i]); + } + _writeMissionItems.clear(); + } else { + // Write failed, throw out the write list + _clearAndDeleteWriteMissionItems(); + } + emit sendComplete(!success /* error */); + break; + case TransactionRemoveAll: + emit removeAllComplete(!success /* error */); + break; + default: + break; + } + + if (_resumeMission) { + _resumeMission = false; + if (success) { + emit resumeMissionReady(); + } else { + emit resumeMissionUploadFail(); + } + } +} + +bool PlanManager::inProgress(void) const +{ + return _transactionInProgress != TransactionNone; +} + +void PlanManager::_handleMissionCurrent(const mavlink_message_t& message) +{ + mavlink_mission_current_t missionCurrent; + + mavlink_msg_mission_current_decode(&message, &missionCurrent); + + if (missionCurrent.seq != _currentMissionIndex) { + qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionCurrent %1 currentIndex:").arg(_planTypeString()) << missionCurrent.seq; + _currentMissionIndex = missionCurrent.seq; + emit currentIndexChanged(_currentMissionIndex); + } + + if (_vehicle->flightMode() == _vehicle->missionFlightMode() && _currentMissionIndex != _lastCurrentIndex) { + qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionCurrent %1 lastCurrentIndex:").arg(_planTypeString()) << _currentMissionIndex; + _lastCurrentIndex = _currentMissionIndex; + emit lastCurrentIndexChanged(_lastCurrentIndex); + } +} + +void PlanManager::_removeAllWorker(void) +{ + mavlink_message_t message; + + qCDebug(PlanManagerLog) << "_removeAllWorker"; + + emit progressPct(0); + + _connectToMavlink(); + _dedicatedLink = _vehicle->priorityLink(); + mavlink_msg_mission_clear_all_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _dedicatedLink->mavlinkChannel(), + &message, + _vehicle->id(), + MAV_COMP_ID_MISSIONPLANNER, + _planType); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), message); + _startAckTimeout(AckMissionClearAll); +} + +void PlanManager::removeAll(void) +{ + if (inProgress()) { + return; + } + + qCDebug(PlanManagerLog) << QStringLiteral("removeAll").arg(_planTypeString()); + + _clearAndDeleteMissionItems(); + + _currentMissionIndex = -1; + _lastCurrentIndex = -1; + emit currentIndexChanged(-1); + emit lastCurrentIndexChanged(-1); + + _transactionInProgress = TransactionRemoveAll; + _retryCount = 0; + emit inProgressChanged(true); + + _removeAllWorker(); +} + +void PlanManager::_clearAndDeleteMissionItems(void) +{ + for (int i=0; i<_missionItems.count(); i++) { + _missionItems[i]->deleteLater(); + } + _missionItems.clear(); +} + + +void PlanManager::_clearAndDeleteWriteMissionItems(void) +{ + for (int i=0; i<_writeMissionItems.count(); i++) { + _writeMissionItems[i]->deleteLater(); + } + _writeMissionItems.clear(); +} + +void PlanManager::_connectToMavlink(void) +{ + connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &PlanManager::_mavlinkMessageReceived); +} + +void PlanManager::_disconnectFromMavlink(void) +{ + disconnect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &PlanManager::_mavlinkMessageReceived); +} + +QString PlanManager::_planTypeString(void) +{ + switch (_planType) { + case MAV_MISSION_TYPE_MISSION: + return QStringLiteral("T:Mission"); + case MAV_MISSION_TYPE_FENCE: + return QStringLiteral("T:GeoFence"); + case MAV_MISSION_TYPE_RALLY: + return QStringLiteral("T:Rally"); + default: + qWarning() << "Unknown plan type" << _planType; + return QStringLiteral("T:Unknown"); + } +} diff --git a/src/MissionManager/PlanManager.h b/src/MissionManager/PlanManager.h new file mode 100644 index 0000000000000000000000000000000000000000..43f696c14ed9f7f6a8d5834c5cb311070a89e747 --- /dev/null +++ b/src/MissionManager/PlanManager.h @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#ifndef PlanManager_H +#define PlanManager_H + +#include +#include +#include + +#include "MissionItem.h" +#include "QGCMAVLink.h" +#include "QGCLoggingCategory.h" +#include "LinkInterface.h" + +class Vehicle; + +Q_DECLARE_LOGGING_CATEGORY(PlanManagerLog) + +/// The PlanManager class is the base class for the Mission, GeoFence and Rally Point managers. All of which use the +/// new mavlink v2 mission protocol. +class PlanManager : public QObject +{ + Q_OBJECT + +public: + PlanManager(Vehicle* vehicle, MAV_MISSION_TYPE planType); + ~PlanManager(); + + bool inProgress(void) const; + const QList& missionItems(void) { return _missionItems; } + + /// Current mission item as reported by MISSION_CURRENT + int currentIndex(void) const { return _currentMissionIndex; } + + /// Last current mission item reported while in Mission flight mode + int lastCurrentIndex(void) const { return _lastCurrentIndex; } + + /// Load the mission items from the vehicle + /// Signals newMissionItemsAvailable when done + void loadFromVehicle(void); + + /// Writes the specified set of mission items to the vehicle + /// @param missionItems Items to send to vehicle + /// Signals sendComplete when done + void writeMissionItems(const QList& missionItems); + + /// Removes all mission items from vehicle + /// Signals removeAllComplete when done + void removeAll(void); + + /// Error codes returned in error signal + typedef enum { + InternalError, + AckTimeoutError, ///< Timed out waiting for response from vehicle + ProtocolOrderError, ///< Incorrect protocol sequence from vehicle + RequestRangeError, ///< Vehicle requested item out of range + ItemMismatchError, ///< Vehicle returned item with seq # different than requested + VehicleError, ///< Vehicle returned error + MissingRequestsError, ///< Vehicle did not request all items during write sequence + MaxRetryExceeded, ///< Retry failed + MissionTypeMismatch, ///< MAV_MISSION_TYPE does not match _planType + } ErrorCode_t; + + // These values are public so the unit test can set appropriate signal wait times + static const int _ackTimeoutMilliseconds = 1000; + static const int _maxRetryCount = 5; + +signals: + void newMissionItemsAvailable (bool removeAllRequested); + void inProgressChanged (bool inProgress); + void error (int errorCode, const QString& errorMsg); + void currentIndexChanged (int currentIndex); + void lastCurrentIndexChanged (int lastCurrentIndex); + void progressPct (double progressPercentPct); + void removeAllComplete (bool error); + void sendComplete (bool error); + void resumeMissionReady (void); + void resumeMissionUploadFail (void); + +private slots: + void _mavlinkMessageReceived(const mavlink_message_t& message); + void _ackTimeout(void); + +protected: + typedef enum { + AckNone, ///< State machine is idle + AckMissionCount, ///< MISSION_COUNT message expected + AckMissionItem, ///< MISSION_ITEM expected + AckMissionRequest, ///< MISSION_REQUEST is expected, or MISSION_ACK to end sequence + AckMissionClearAll, ///< MISSION_CLEAR_ALL sent, MISSION_ACK is expected + AckGuidedItem, ///< MISSION_ACK expected in response to ArduPilot guided mode single item send + } AckType_t; + + typedef enum { + TransactionNone, + TransactionRead, + TransactionWrite, + TransactionRemoveAll + } TransactionType_t; + + void _startAckTimeout(AckType_t ack); + bool _checkForExpectedAck(AckType_t receivedAck); + void _readTransactionComplete(void); + void _handleMissionCount(const mavlink_message_t& message); + void _handleMissionItem(const mavlink_message_t& message, bool missionItemInt); + void _handleMissionRequest(const mavlink_message_t& message, bool missionItemInt); + void _handleMissionAck(const mavlink_message_t& message); + void _handleMissionCurrent(const mavlink_message_t& message); + void _requestNextMissionItem(void); + void _clearMissionItems(void); + void _sendError(ErrorCode_t errorCode, const QString& errorMsg); + QString _ackTypeToString(AckType_t ackType); + QString _missionResultToString(MAV_MISSION_RESULT result); + void _finishTransaction(bool success); + void _requestList(void); + void _writeMissionCount(void); + void _writeMissionItemsWorker(void); + void _clearAndDeleteMissionItems(void); + void _clearAndDeleteWriteMissionItems(void); + QString _lastMissionReqestString(MAV_MISSION_RESULT result); + void _removeAllWorker(void); + void _connectToMavlink(void); + void _disconnectFromMavlink(void); + QString _planTypeString(void); + +protected: + Vehicle* _vehicle; + MAV_MISSION_TYPE _planType; + LinkInterface* _dedicatedLink; + + QTimer* _ackTimeoutTimer; + AckType_t _expectedAck; + int _retryCount; + + TransactionType_t _transactionInProgress; + bool _resumeMission; + QList _itemIndicesToWrite; ///< List of mission items which still need to be written to vehicle + QList _itemIndicesToRead; ///< List of mission items which still need to be requested from vehicle + int _lastMissionRequest; ///< Index of item last requested by MISSION_REQUEST + + QList _missionItems; ///< Set of mission items on vehicle + QList _writeMissionItems; ///< Set of mission items currently being written to vehicle + int _currentMissionIndex; + int _lastCurrentIndex; +}; + +#endif diff --git a/src/MissionManager/PlanMasterController.cc b/src/MissionManager/PlanMasterController.cc index 797bd59cc6d4e16926f1b976fd0e0a387f2ea02f..6b9e453f6f7ddf619733f5d895b2e51e79b4a6ee 100644 --- a/src/MissionManager/PlanMasterController.cc +++ b/src/MissionManager/PlanMasterController.cc @@ -177,8 +177,14 @@ void PlanMasterController::_loadMissionComplete(void) if (_editMode && _loadGeoFence) { _loadGeoFence = false; _loadRallyPoints = true; - qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadMissionComplete _geoFenceController.loadFromVehicle"; - _geoFenceController.loadFromVehicle(); + if (_geoFenceController.supported()) { + qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadMissionComplete _geoFenceController.loadFromVehicle"; + _geoFenceController.loadFromVehicle(); + } else { + qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadMissionComplete GeoFence not supported skipping"; + _geoFenceController.removeAll(); + _loadGeoFenceComplete(); + } setDirty(false); } } @@ -187,8 +193,14 @@ void PlanMasterController::_loadGeoFenceComplete(void) { if (_editMode && _loadRallyPoints) { _loadRallyPoints = false; - qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadGeoFenceComplete _rallyPointController.loadFromVehicle"; - _rallyPointController.loadFromVehicle(); + if (_rallyPointController.supported()) { + qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadGeoFenceComplete _rallyPointController.loadFromVehicle"; + _rallyPointController.loadFromVehicle(); + } else { + qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadMissionComplete Rally Points not supported skipping"; + _rallyPointController.removeAll(); + _loadRallyPointsComplete(); + } setDirty(false); } } @@ -204,10 +216,15 @@ void PlanMasterController::_loadRallyPointsComplete(void) void PlanMasterController::_sendMissionComplete(void) { if (_sendGeoFence) { - qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle start fence sendToVehicle"; _sendGeoFence = false; _sendRallyPoints = true; - _geoFenceController.sendToVehicle(); + if (_geoFenceController.supported()) { + qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle start GeoFence sendToVehicle"; + _geoFenceController.sendToVehicle(); + } else { + qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle GeoFence not supported skipping"; + _sendGeoFenceComplete(); + } setDirty(false); } } @@ -215,16 +232,21 @@ void PlanMasterController::_sendMissionComplete(void) void PlanMasterController::_sendGeoFenceComplete(void) { if (_sendRallyPoints) { - qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle start rally sendToVehicle"; _sendRallyPoints = false; - _rallyPointController.sendToVehicle(); + if (_rallyPointController.supported()) { + qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle start rally sendToVehicle"; + _rallyPointController.sendToVehicle(); + } else { + qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle Rally Points not support skipping"; + _sendRallyPointsComplete(); + } } } void PlanMasterController::_sendRallyPointsComplete(void) { if (_syncInProgress) { - qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle rally point send complete"; + qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle Rally Point send complete"; _syncInProgress = false; emit syncInProgressChanged(false); } @@ -361,8 +383,12 @@ void PlanMasterController::removeAllFromVehicle(void) { if (!offline()) { _missionController.removeAllFromVehicle(); - _geoFenceController.removeAllFromVehicle(); - _rallyPointController.removeAllFromVehicle(); + if (_geoFenceController.supported()) { + _geoFenceController.removeAllFromVehicle(); + } + if (_rallyPointController.supported()) { + _rallyPointController.removeAllFromVehicle(); + } setDirty(false); } else { qWarning() << "PlanMasterController::removeAllFromVehicle called while offline"; diff --git a/src/MissionManager/QGCFenceCircle.cc b/src/MissionManager/QGCFenceCircle.cc new file mode 100644 index 0000000000000000000000000000000000000000..3c1e1b658fd66ae1218c95e15f1e35cc4532a8f6 --- /dev/null +++ b/src/MissionManager/QGCFenceCircle.cc @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "QGCFenceCircle.h" +#include "JsonHelper.h" + +const char* QGCFenceCircle::_jsonInclusionKey = "inclusion"; + +QGCFenceCircle::QGCFenceCircle(QObject* parent) + : QGCMapCircle (parent) + , _inclusion (true) +{ + _init(); +} + +QGCFenceCircle::QGCFenceCircle(const QGeoCoordinate& center, double radius, bool inclusion, QObject* parent) + : QGCMapCircle (center, radius, parent) + , _inclusion (inclusion) +{ + _init(); +} + +QGCFenceCircle::QGCFenceCircle(const QGCFenceCircle& other, QObject* parent) + : QGCMapCircle (other, parent) + , _inclusion (other._inclusion) +{ + _init(); +} + +void QGCFenceCircle::_init(void) +{ + connect(this, &QGCFenceCircle::inclusionChanged, this, &QGCFenceCircle::_setDirty); +} + +const QGCFenceCircle& QGCFenceCircle::operator=(const QGCFenceCircle& other) +{ + QGCMapCircle::operator=(other); + + setInclusion(other._inclusion); + + return *this; +} + +void QGCFenceCircle::_setDirty(void) +{ + setDirty(true); +} + +void QGCFenceCircle::saveToJson(QJsonObject& json) +{ + QGCMapCircle::saveToJson(json); + + json[_jsonInclusionKey] = _inclusion; +} + +bool QGCFenceCircle::loadFromJson(const QJsonObject& json, QString& errorString) +{ + if (!QGCMapCircle::loadFromJson(json, errorString)) { + return false; + } + + errorString.clear(); + + QList keyInfoList = { + { _jsonInclusionKey, QJsonValue::Bool, true }, + }; + if (!JsonHelper::validateKeys(json, keyInfoList, errorString)) { + return false; + } + + setInclusion(json[_jsonInclusionKey].toBool()); + + return true; +} + +void QGCFenceCircle::setInclusion(bool inclusion) +{ + if (inclusion != _inclusion) { + _inclusion = inclusion; + emit inclusionChanged(inclusion); + } +} diff --git a/src/MissionManager/QGCFenceCircle.h b/src/MissionManager/QGCFenceCircle.h new file mode 100644 index 0000000000000000000000000000000000000000..92d6d3ea1febcb81d538c409764f668d3f12aa89 --- /dev/null +++ b/src/MissionManager/QGCFenceCircle.h @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "QGCMapCircle.h" + +/// The QGCFenceCircle class provides a cicle used by GeoFence support. +class QGCFenceCircle : public QGCMapCircle +{ + Q_OBJECT + +public: + QGCFenceCircle(QObject* parent = NULL); + QGCFenceCircle(const QGeoCoordinate& center, double radius, bool inclusion, QObject* parent = NULL); + QGCFenceCircle(const QGCFenceCircle& other, QObject* parent = NULL); + + const QGCFenceCircle& operator=(const QGCFenceCircle& other); + + Q_PROPERTY(bool inclusion READ inclusion WRITE setInclusion NOTIFY inclusionChanged) + + /// Saves the QGCFenceCircle to the json object. + /// @param json Json object to save to + void saveToJson(QJsonObject& json); + + /// Load a QGCFenceCircle from json + /// @param json Json object to load from + /// @param errorString Error string if return is false + /// @return true: success, false: failure (errorString set) + bool loadFromJson(const QJsonObject& json, QString& errorString); + + // Property methods + + bool inclusion (void) const { return _inclusion; } + void setInclusion (bool inclusion); + +signals: + void inclusionChanged(bool inclusion); + +private slots: + void _setDirty(void); + +private: + void _init(void); + + bool _inclusion; + + static const char* _jsonInclusionKey; +}; diff --git a/src/MissionManager/QGCFencePolygon.cc b/src/MissionManager/QGCFencePolygon.cc new file mode 100644 index 0000000000000000000000000000000000000000..1524b67f5c5060da61a0d76c697f008002204ce1 --- /dev/null +++ b/src/MissionManager/QGCFencePolygon.cc @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "QGCFencePolygon.h" +#include "JsonHelper.h" + +const char* QGCFencePolygon::_jsonInclusionKey = "inclusion"; + +QGCFencePolygon::QGCFencePolygon(bool inclusion, QObject* parent) + : QGCMapPolygon (parent) + , _inclusion (inclusion) +{ + _init(); +} + +QGCFencePolygon::QGCFencePolygon(const QGCFencePolygon& other, QObject* parent) + : QGCMapPolygon (other, parent) + , _inclusion (other._inclusion) +{ + _init(); +} + +void QGCFencePolygon::_init(void) +{ + connect(this, &QGCFencePolygon::inclusionChanged, this, &QGCFencePolygon::_setDirty); +} + +const QGCFencePolygon& QGCFencePolygon::operator=(const QGCFencePolygon& other) +{ + QGCMapPolygon::operator=(other); + + setInclusion(other._inclusion); + + return *this; +} + +void QGCFencePolygon::_setDirty(void) +{ + setDirty(true); +} + +void QGCFencePolygon::saveToJson(QJsonObject& json) +{ + QGCMapPolygon::saveToJson(json); + + json[_jsonInclusionKey] = _inclusion; +} + +bool QGCFencePolygon::loadFromJson(const QJsonObject& json, bool required, QString& errorString) +{ + if (!QGCMapPolygon::loadFromJson(json, required, errorString)) { + return false; + } + + errorString.clear(); + + QList keyInfoList = { + { _jsonInclusionKey, QJsonValue::Bool, true }, + }; + if (!JsonHelper::validateKeys(json, keyInfoList, errorString)) { + return false; + } + + setInclusion(json[_jsonInclusionKey].toBool()); + + return true; +} + +void QGCFencePolygon::setInclusion(bool inclusion) +{ + if (inclusion != _inclusion) { + _inclusion = inclusion; + emit inclusionChanged(inclusion); + } +} diff --git a/src/MissionManager/QGCFencePolygon.h b/src/MissionManager/QGCFencePolygon.h new file mode 100644 index 0000000000000000000000000000000000000000..5c8ed56398da2ffac11a519dcdb4cf3d7e4506d0 --- /dev/null +++ b/src/MissionManager/QGCFencePolygon.h @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "QGCMapPolygon.h" + +/// The QGCFencePolygon class provides a polygon used by GeoFence support. +class QGCFencePolygon : public QGCMapPolygon +{ + Q_OBJECT + +public: + QGCFencePolygon(bool inclusion, QObject* parent = NULL); + QGCFencePolygon(const QGCFencePolygon& other, QObject* parent = NULL); + + const QGCFencePolygon& operator=(const QGCFencePolygon& other); + + Q_PROPERTY(bool inclusion READ inclusion WRITE setInclusion NOTIFY inclusionChanged) + + /// Saves the QGCFencePolygon to the json object. + /// @param json Json object to save to + void saveToJson(QJsonObject& json); + + /// Load a QGCFencePolygon from json + /// @param json Json object to load from + /// @param required true: no polygon in object will generate error + /// @param errorString Error string if return is false + /// @return true: success, false: failure (errorString set) + bool loadFromJson(const QJsonObject& json, bool required, QString& errorString); + + // Property methods + + bool inclusion (void) const { return _inclusion; } + void setInclusion (bool inclusion); + +signals: + void inclusionChanged (bool inclusion); + +private slots: + void _setDirty(void); + +private: + void _init(void); + + bool _inclusion; + + static const char* _jsonInclusionKey; +}; diff --git a/src/MissionManager/QGCMapCircle.Facts.json b/src/MissionManager/QGCMapCircle.Facts.json new file mode 100644 index 0000000000000000000000000000000000000000..f2ded5e5efea285b70279064ba7ed926e4d11456 --- /dev/null +++ b/src/MissionManager/QGCMapCircle.Facts.json @@ -0,0 +1,10 @@ +[ +{ + "name": "Radius", + "shortDescription": "Radius for geofence circle.", + "type": "double", + "decimalPlaces": 2, + "min": 0.1, + "units": "m" +} +] diff --git a/src/MissionManager/QGCMapCircle.cc b/src/MissionManager/QGCMapCircle.cc new file mode 100644 index 0000000000000000000000000000000000000000..143956c8e25cf9f8b6221d8ba88ed9771d845bb9 --- /dev/null +++ b/src/MissionManager/QGCMapCircle.cc @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "QGCMapCircle.h" +#include "QGCGeo.h" +#include "JsonHelper.h" +#include "QGCQGeoCoordinate.h" + +#include +#include +#include + +const char* QGCMapCircle::jsonCircleKey = "circle"; +const char* QGCMapCircle::_jsonCenterKey = "center"; +const char* QGCMapCircle::_jsonRadiusKey = "radius"; +const char* QGCMapCircle::_radiusFactName = "Radius"; + +QGCMapCircle::QGCMapCircle(QObject* parent) + : QObject (parent) + , _dirty (false) + , _interactive (false) +{ + _init(); +} + +QGCMapCircle::QGCMapCircle(const QGeoCoordinate& center, double radius, QObject* parent) + : QObject (parent) + , _dirty (false) + , _center (center) + , _radius (FactSystem::defaultComponentId, _radiusFactName, FactMetaData::valueTypeDouble) + , _interactive (false) +{ + _radius.setRawValue(radius); + _init(); +} + +QGCMapCircle::QGCMapCircle(const QGCMapCircle& other, QObject* parent) + : QObject (parent) + , _dirty (false) + , _center (other._center) + , _radius (FactSystem::defaultComponentId, _radiusFactName, FactMetaData::valueTypeDouble) + , _interactive (false) +{ + _radius.setRawValue(other._radius.rawValue()); + _init(); +} + +const QGCMapCircle& QGCMapCircle::operator=(const QGCMapCircle& other) +{ + setCenter(other._center); + _radius.setRawValue(other._radius.rawValue()); + setDirty(true); + + return *this; +} + +void QGCMapCircle::_init(void) +{ + _nameToMetaDataMap = FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/QGCMapCircle.Facts.json"), this); + _radius.setMetaData(_nameToMetaDataMap[_radiusFactName]); + + connect(this, &QGCMapCircle::centerChanged, this, &QGCMapCircle::_setDirty); + connect(&_radius, &Fact::rawValueChanged, this, &QGCMapCircle::_setDirty); +} + +void QGCMapCircle::setDirty(bool dirty) +{ + if (_dirty != dirty) { + _dirty = dirty; + emit dirtyChanged(dirty); + } +} + +void QGCMapCircle::saveToJson(QJsonObject& json) +{ + QJsonValue jsonValue; + QJsonObject circleObject; + + JsonHelper::saveGeoCoordinate(_center, false /* writeAltitude*/, jsonValue); + circleObject.insert(_jsonCenterKey, jsonValue); + circleObject.insert(_jsonRadiusKey, _radius.rawValue().toDouble()); + + json.insert(jsonCircleKey, circleObject); +} + +bool QGCMapCircle::loadFromJson(const QJsonObject& json, QString& errorString) +{ + errorString.clear(); + + QList circleKeyInfo = { + { jsonCircleKey, QJsonValue::Object, true }, + }; + if (!JsonHelper::validateKeys(json, circleKeyInfo, errorString)) { + return false; + } + + QJsonObject circleObject = json[jsonCircleKey].toObject(); + + QList circleObjectKeyInfo = { + { _jsonCenterKey, QJsonValue::Array, true }, + { _jsonRadiusKey, QJsonValue::Double, true }, + }; + if (!JsonHelper::validateKeys(circleObject, circleObjectKeyInfo, errorString)) { + return false; + } + + QGeoCoordinate center; + if (!JsonHelper::loadGeoCoordinate(json[_jsonCenterKey], false /* altitudeRequired */, center, errorString)) { + return false; + } + setCenter(center); + _radius.setRawValue(json[_jsonRadiusKey].toDouble()); + + return true; +} + +void QGCMapCircle::setCenter(QGeoCoordinate newCenter) +{ + if (newCenter != _center) { + _center = newCenter; + setDirty(true); + emit centerChanged(newCenter); + } +} + +void QGCMapCircle::_setDirty(void) +{ + setDirty(true); +} + +void QGCMapCircle::setInteractive(bool interactive) +{ + if (_interactive != interactive) { + _interactive = interactive; + emit interactiveChanged(interactive); + } +} + diff --git a/src/MissionManager/QGCMapCircle.h b/src/MissionManager/QGCMapCircle.h new file mode 100644 index 0000000000000000000000000000000000000000..a1f1e8184a5e808f3b2ae5f0c5c7aec7946ad2f8 --- /dev/null +++ b/src/MissionManager/QGCMapCircle.h @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include + +#include "QmlObjectListModel.h" +#include "FactSystem.h" + +/// The QGCMapCircle represents a circular area which can be displayed on a Map control. +class QGCMapCircle : public QObject +{ + Q_OBJECT + +public: + QGCMapCircle(QObject* parent = NULL); + QGCMapCircle(const QGeoCoordinate& center, double radius, QObject* parent = NULL); + QGCMapCircle(const QGCMapCircle& other, QObject* parent = NULL); + + const QGCMapCircle& operator=(const QGCMapCircle& other); + + Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) + Q_PROPERTY(QGeoCoordinate center READ center WRITE setCenter NOTIFY centerChanged) + Q_PROPERTY(Fact* radius READ radius CONSTANT) + Q_PROPERTY(bool interactive READ interactive WRITE setInteractive NOTIFY interactiveChanged) + + /// Saves the polygon to the json object. + /// @param json Json object to save to + void saveToJson(QJsonObject& json); + + /// Load a circle from json + /// @param json Json object to load from + /// @param errorString Error string if return is false + /// @return true: success, false: failure (errorString set) + bool loadFromJson(const QJsonObject& json, QString& errorString); + + // Property methods + + bool dirty (void) const { return _dirty; } + QGeoCoordinate center (void) const { return _center; } + Fact* radius (void) { return &_radius; } + bool interactive (void) const { return _interactive; } + + void setDirty (bool dirty); + void setCenter (QGeoCoordinate newCenter); + void setInteractive (bool interactive); + + static const char* jsonCircleKey; + +signals: + void dirtyChanged (bool dirty); + void centerChanged (QGeoCoordinate center); + void interactiveChanged (bool interactive); + +private slots: + void _setDirty(void); + +private: + void _init(void); + + bool _dirty; + QGeoCoordinate _center; + Fact _radius; + bool _interactive; + + QMap _nameToMetaDataMap; + + static const char* _jsonCenterKey; + static const char* _jsonRadiusKey; + static const char* _radiusFactName; +}; diff --git a/src/MissionManager/QGCMapCircleVisuals.qml b/src/MissionManager/QGCMapCircleVisuals.qml new file mode 100644 index 0000000000000000000000000000000000000000..c7388a38f5405176d50192c405184692babc7a18 --- /dev/null +++ b/src/MissionManager/QGCMapCircleVisuals.qml @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.3 +import QtQuick.Controls 1.2 +import QtLocation 5.3 +import QtPositioning 5.3 + +import QGroundControl 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Palette 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.FlightMap 1.0 + +/// QGCMapCircle map visuals +Item { + id: _root + + property var mapControl ///< Map control to place item in + property var mapCircle ///< QGCMapCircle object + property bool interactive: mapCircle.interactive /// true: user can manipulate polygon + property color interiorColor: "transparent" + property real interiorOpacity: 1 + property int borderWidth: 0 + property color borderColor: "black" + + property var _circleComponent + property var _centerDragHandleComponent + + function addVisuals() { + _circleComponent = circleComponent.createObject(mapControl) + mapControl.addMapItem(_circleComponent) + } + + function removeVisuals() { + _circleComponent.destroy() + } + + function addHandles() { + if (!_centerDragHandleComponent) { + _centerDragHandleComponent = centerDragHandleComponent.createObject(mapControl) + } + } + + function removeHandles() { + if (_centerDragHandleComponent) { + _centerDragHandleComponent.destroy() + _centerDragHandleComponent = undefined + } + } + + onInteractiveChanged: { + if (interactive) { + addHandles() + } else { + removeHandles() + } + } + + Component.onCompleted: { + addVisuals() + if (interactive) { + addHandles() + } + } + + Component.onDestruction: { + removeVisuals() + removeHandles() + } + + Component { + id: circleComponent + + MapCircle { + color: interiorColor + opacity: interiorOpacity + border.color: borderColor + border.width: borderWidth + center: mapCircle.center + radius: mapCircle.radius.rawValue + } + } + + Component { + id: dragHandleComponent + + MapQuickItem { + id: mapQuickItem + anchorPoint.x: dragHandle.width / 2 + anchorPoint.y: dragHandle.height / 2 + z: QGroundControl.zOrderMapItems + 2 + + sourceItem: Rectangle { + id: dragHandle + width: ScreenTools.defaultFontPixelHeight * 1.5 + height: width + radius: width / 2 + color: "white" + opacity: .90 + } + } + } + + Component { + id: centerDragAreaComponent + + MissionItemIndicatorDrag { + onItemCoordinateChanged: mapCircle.center = itemCoordinate + } + } + + Component { + id: centerDragHandleComponent + + Item { + property var dragHandle + property var dragArea + + Component.onCompleted: { + dragHandle = dragHandleComponent.createObject(mapControl) + dragHandle.coordinate = Qt.binding(function() { return mapCircle.center }) + mapControl.addMapItem(dragHandle) + dragArea = centerDragAreaComponent.createObject(mapControl, { "itemIndicator": dragHandle, "itemCoordinate": mapCircle.center }) + } + + Component.onDestruction: { + dragHandle.destroy() + dragArea.destroy() + } + } + } +} + diff --git a/src/MissionManager/QGCMapPolygon.cc b/src/MissionManager/QGCMapPolygon.cc index a7c5ca9ae01194587dfaa5dce7628b84c5e1c6c3..aef9372ab181be432975d30b04e182f8e396caed 100644 --- a/src/MissionManager/QGCMapPolygon.cc +++ b/src/MissionManager/QGCMapPolygon.cc @@ -18,18 +18,49 @@ const char* QGCMapPolygon::jsonPolygonKey = "polygon"; -QGCMapPolygon::QGCMapPolygon(QObject* newCoordParent, QObject* parent) - : QObject(parent) - , _newCoordParent(newCoordParent) - , _dirty(false) - , _centerDrag(false) - , _ignoreCenterUpdates(false) +QGCMapPolygon::QGCMapPolygon(QObject* parent) + : QObject (parent) + , _dirty (false) + , _centerDrag (false) + , _ignoreCenterUpdates (false) + , _interactive (false) +{ + _init(); +} + +QGCMapPolygon::QGCMapPolygon(const QGCMapPolygon& other, QObject* parent) + : QObject (parent) + , _dirty (false) + , _centerDrag (false) + , _ignoreCenterUpdates (false) + , _interactive (false) +{ + *this = other; + + _init(); +} + +void QGCMapPolygon::_init(void) { connect(&_polygonModel, &QmlObjectListModel::dirtyChanged, this, &QGCMapPolygon::_polygonModelDirtyChanged); connect(&_polygonModel, &QmlObjectListModel::countChanged, this, &QGCMapPolygon::_polygonModelCountChanged); connect(&_polygonModel, &QmlObjectListModel::countChanged, this, &QGCMapPolygon::_updateCenter); } +const QGCMapPolygon& QGCMapPolygon::operator=(const QGCMapPolygon& other) +{ + clear(); + + QVariantList vertices = other.path(); + for (int i=0; i()); + } + + setDirty(true); + + return *this; +} + void QGCMapPolygon::clear(void) { // Bug workaround, see below @@ -132,7 +163,7 @@ void QGCMapPolygon::setPath(const QList& path) _polygonModel.clearAndDeleteContents(); foreach(const QGeoCoordinate& coord, path) { _polygonPath.append(QVariant::fromValue(coord)); - _polygonModel.append(new QGCQGeoCoordinate(coord, _newCoordParent)); + _polygonModel.append(new QGCQGeoCoordinate(coord, this)); } setDirty(true); @@ -145,7 +176,7 @@ void QGCMapPolygon::setPath(const QVariantList& path) _polygonModel.clearAndDeleteContents(); for (int i=0; i<_polygonPath.count(); i++) { - _polygonModel.append(new QGCQGeoCoordinate(_polygonPath[i].value(), _newCoordParent)); + _polygonModel.append(new QGCQGeoCoordinate(_polygonPath[i].value(), this)); } setDirty(true); @@ -179,7 +210,7 @@ bool QGCMapPolygon::loadFromJson(const QJsonObject& json, bool required, QString } for (int i=0; i<_polygonPath.count(); i++) { - _polygonModel.append(new QGCQGeoCoordinate(_polygonPath[i].value(), _newCoordParent)); + _polygonModel.append(new QGCQGeoCoordinate(_polygonPath[i].value(), this)); } setDirty(false); @@ -225,7 +256,7 @@ void QGCMapPolygon::splitPolygonSegment(int vertexIndex) void QGCMapPolygon::appendVertex(const QGeoCoordinate& coordinate) { _polygonPath.append(QVariant::fromValue(coordinate)); - _polygonModel.append(new QGCQGeoCoordinate(coordinate, _newCoordParent)); + _polygonModel.append(new QGCQGeoCoordinate(coordinate, this)); emit pathChanged(); } @@ -315,3 +346,11 @@ void QGCMapPolygon::setCenterDrag(bool centerDrag) emit centerDragChanged(centerDrag); } } + +void QGCMapPolygon::setInteractive(bool interactive) +{ + if (_interactive != interactive) { + _interactive = interactive; + emit interactiveChanged(interactive); + } +} diff --git a/src/MissionManager/QGCMapPolygon.h b/src/MissionManager/QGCMapPolygon.h index 445df1e7ae404ae39ffc67a574879f133ee02af3..b2990dbe3f11ec4d59754871860db04f1875ee25 100644 --- a/src/MissionManager/QGCMapPolygon.h +++ b/src/MissionManager/QGCMapPolygon.h @@ -24,14 +24,18 @@ class QGCMapPolygon : public QObject Q_OBJECT public: - QGCMapPolygon(QObject* newCoordParent, QObject* parent = NULL); + QGCMapPolygon(QObject* parent = NULL); + QGCMapPolygon(const QGCMapPolygon& other, QObject* parent = NULL); - Q_PROPERTY(int count READ count NOTIFY countChanged) - Q_PROPERTY(QVariantList path READ path NOTIFY pathChanged) - Q_PROPERTY(QmlObjectListModel* pathModel READ qmlPathModel CONSTANT) - Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) - Q_PROPERTY(QGeoCoordinate center READ center WRITE setCenter NOTIFY centerChanged) - Q_PROPERTY(bool centerDrag READ centerDrag WRITE setCenterDrag NOTIFY centerDragChanged) + const QGCMapPolygon& operator=(const QGCMapPolygon& other); + + Q_PROPERTY(int count READ count NOTIFY countChanged) + Q_PROPERTY(QVariantList path READ path NOTIFY pathChanged) + Q_PROPERTY(QmlObjectListModel* pathModel READ qmlPathModel CONSTANT) + Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) + Q_PROPERTY(QGeoCoordinate center READ center WRITE setCenter NOTIFY centerChanged) + Q_PROPERTY(bool centerDrag READ centerDrag WRITE setCenterDrag NOTIFY centerDragChanged) + Q_PROPERTY(bool interactive READ interactive WRITE setInteractive NOTIFY interactiveChanged) Q_INVOKABLE void clear(void); Q_INVOKABLE void appendVertex(const QGeoCoordinate& coordinate); @@ -69,6 +73,7 @@ public: void setDirty (bool dirty); QGeoCoordinate center (void) const { return _center; } bool centerDrag (void) const { return _centerDrag; } + bool interactive (void) const { return _interactive; } QVariantList path (void) const { return _polygonPath; } QmlObjectListModel* qmlPathModel(void) { return &_polygonModel; } @@ -78,6 +83,7 @@ public: void setPath (const QVariantList& path); void setCenter (QGeoCoordinate newCenter); void setCenterDrag (bool centerDrag); + void setInteractive (bool interactive); static const char* jsonPolygonKey; @@ -88,6 +94,7 @@ signals: void cleared (void); void centerChanged (QGeoCoordinate center); void centerDragChanged (bool centerDrag); + void interactiveChanged (bool interactive); private slots: void _polygonModelCountChanged(int count); @@ -95,17 +102,18 @@ private slots: void _updateCenter(void); private: + void _init(void); QPolygonF _toPolygonF(void) const; QGeoCoordinate _coordFromPointF(const QPointF& point) const; QPointF _pointFFromCoord(const QGeoCoordinate& coordinate) const; - QObject* _newCoordParent; QVariantList _polygonPath; QmlObjectListModel _polygonModel; bool _dirty; QGeoCoordinate _center; bool _centerDrag; bool _ignoreCenterUpdates; + bool _interactive; }; #endif diff --git a/src/MissionManager/QGCMapPolygonTest.cc b/src/MissionManager/QGCMapPolygonTest.cc index f6871872aba166c1a0d1a095eef8368fae1b2838..4f366d8c2084f7a8c035d77bacd1dc5a53ecadfe 100644 --- a/src/MissionManager/QGCMapPolygonTest.cc +++ b/src/MissionManager/QGCMapPolygonTest.cc @@ -32,7 +32,7 @@ void QGCMapPolygonTest::init(void) _rgModelSignals[modelCountChangedIndex] = SIGNAL(countChanged(int)); _rgModelSignals[modelDirtyChangedIndex] = SIGNAL(dirtyChanged(bool)); - _mapPolygon = new QGCMapPolygon(this, this); + _mapPolygon = new QGCMapPolygon(this); _pathModel = _mapPolygon->qmlPathModel(); QVERIFY(_pathModel); diff --git a/src/MissionManager/QGCMapPolygonVisuals.qml b/src/MissionManager/QGCMapPolygonVisuals.qml index 5d630a7a27cdc33ec6fe2f5a5ef3f3c7b711e6d4..36496a4023a26d98e8da99f35a225241cb8ffd4e 100644 --- a/src/MissionManager/QGCMapPolygonVisuals.qml +++ b/src/MissionManager/QGCMapPolygonVisuals.qml @@ -24,16 +24,16 @@ Item { property var mapControl ///< Map control to place item in property var mapPolygon ///< QGCMapPolygon object - property bool interactive: true /// true: user can manipulate polygon + property bool interactive: mapPolygon.interactive property color interiorColor: "transparent" property real interiorOpacity: 1 property int borderWidth: 0 property color borderColor: "black" - property var _polygonComponent - property var _dragHandlesComponent - property var _splitHandlesComponent - property var _centerDragHandleComponent + property var _polygonComponent + property var _dragHandlesComponent + property var _splitHandlesComponent + property var _centerDragHandleComponent property real _zorderDragHandle: QGroundControl.zOrderMapItems + 3 // Highest to prevent splitting when items overlap property real _zorderSplitHandle: QGroundControl.zOrderMapItems + 2 @@ -111,7 +111,7 @@ Item { } Component.onCompleted: { - mapPolygonVisuals.addVisuals() + addVisuals() if (interactive) { addHandles() } diff --git a/src/MissionManager/RallyPointController.cc b/src/MissionManager/RallyPointController.cc index c719e11f7ac0d8af22e54ed213665247e778056c..bc88fdce93efa183f0aa5fa6cf744dc5c18f419e 100644 --- a/src/MissionManager/RallyPointController.cc +++ b/src/MissionManager/RallyPointController.cc @@ -58,6 +58,7 @@ void RallyPointController::managerVehicleChanged(Vehicle* managerVehicle) { if (_managerVehicle) { _rallyPointManager->disconnect(this); + _managerVehicle->disconnect(this); _managerVehicle = NULL; _rallyPointManager = NULL; } @@ -74,7 +75,9 @@ void RallyPointController::managerVehicleChanged(Vehicle* managerVehicle) connect(_rallyPointManager, &RallyPointManager::removeAllComplete, this, &RallyPointController::_managerRemoveAllComplete); connect(_rallyPointManager, &RallyPointManager::inProgressChanged, this, &RallyPointController::syncInProgressChanged); - emit rallyPointsSupportedChanged(rallyPointsSupported()); + connect(_managerVehicle, &Vehicle::capabilityBitsChanged, this, &RallyPointController::supportedChanged); + + emit supportedChanged(supported()); } bool RallyPointController::load(const QJsonObject& json, QString& errorString) @@ -234,9 +237,9 @@ void RallyPointController::addPoint(QGeoCoordinate point) setDirty(true); } -bool RallyPointController::rallyPointsSupported(void) const +bool RallyPointController::supported(void) const { - return _rallyPointManager->rallyPointsSupported(); + return (_managerVehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY) && (_managerVehicle->maxProtoVersion() >= 200); } void RallyPointController::removePoint(QObject* rallyPoint) diff --git a/src/MissionManager/RallyPointController.h b/src/MissionManager/RallyPointController.h index 7a2c945817ebe07d43ca7620d8c6439578a11534..7df2dc79e8ad56cb1a5dc77ba67a10e153c2ed3a 100644 --- a/src/MissionManager/RallyPointController.h +++ b/src/MissionManager/RallyPointController.h @@ -29,7 +29,6 @@ public: RallyPointController(PlanMasterController* masterController, QObject* parent = NULL); ~RallyPointController(); - Q_PROPERTY(bool rallyPointsSupported READ rallyPointsSupported NOTIFY rallyPointsSupportedChanged) Q_PROPERTY(QmlObjectListModel* points READ points CONSTANT) Q_PROPERTY(QString editorQml READ editorQml CONSTANT) Q_PROPERTY(QObject* currentRallyPoint READ currentRallyPoint WRITE setCurrentRallyPoint NOTIFY currentRallyPointChanged) @@ -37,6 +36,7 @@ public: Q_INVOKABLE void addPoint(QGeoCoordinate point); Q_INVOKABLE void removePoint(QObject* rallyPoint); + bool supported (void) const final; void save (QJsonObject& json) final; bool load (const QJsonObject& json, QString& errorString) final; void loadFromVehicle (void) final; @@ -50,7 +50,6 @@ public: void managerVehicleChanged (Vehicle* managerVehicle) final; bool showPlanFromManagerVehicle (void) final; - bool rallyPointsSupported (void) const; QmlObjectListModel* points (void) { return &_points; } QString editorQml (void) const; QObject* currentRallyPoint (void) const { return _currentRallyPoint; } @@ -58,7 +57,6 @@ public: void setCurrentRallyPoint(QObject* rallyPoint); signals: - void rallyPointsSupportedChanged(bool rallyPointsSupported); void currentRallyPointChanged(QObject* rallyPoint); void loadComplete(void); diff --git a/src/MissionManager/RallyPointManager.cc b/src/MissionManager/RallyPointManager.cc index d101b4e890bb78fa30cc00b8aac8814b5c77ebe4..072ac4ae0d8d96157b507197d815e224be5d843d 100644 --- a/src/MissionManager/RallyPointManager.cc +++ b/src/MissionManager/RallyPointManager.cc @@ -49,3 +49,8 @@ void RallyPointManager::removeAll(void) // No support in generic vehicle emit removeAllComplete(false /* error */); } + +bool RallyPointManager::supported(void) const +{ + return (_vehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY) && (_vehicle->maxProtoVersion() >= 200); +} diff --git a/src/MissionManager/RallyPointManager.h b/src/MissionManager/RallyPointManager.h index db46f18fe39cea75b0b9503959d20c621f50337d..cf9e1747d502c331912383aa726f74fd84743884 100644 --- a/src/MissionManager/RallyPointManager.h +++ b/src/MissionManager/RallyPointManager.h @@ -29,6 +29,9 @@ public: RallyPointManager(Vehicle* vehicle); ~RallyPointManager(); + /// Returns true if GeoFence is supported by this vehicle + virtual bool supported(void) const; + /// Returns true if the manager is currently communicating with the vehicle virtual bool inProgress(void) const { return false; } @@ -44,8 +47,6 @@ public: /// Signals removeAllCompleted when done virtual void removeAll(void); - virtual bool rallyPointsSupported(void) const { return false; } - QList points(void) const { return _rgPoints; } virtual QString editorQml(void) const { return QStringLiteral("qrc:/FirmwarePlugin/RallyPointEditor.qml"); } diff --git a/src/PlanView/GeoFenceEditor.qml b/src/PlanView/GeoFenceEditor.qml index 114f85ef348dd62b36fb17687bc1ea23689bc8e9..dd592b5fe6ef150d52a78da19cbf77468424d466 100644 --- a/src/PlanView/GeoFenceEditor.qml +++ b/src/PlanView/GeoFenceEditor.qml @@ -1,5 +1,6 @@ import QtQuick 2.3 import QtQuick.Controls 1.2 +import QtQuick.Layouts 1.2 import QGroundControl 1.0 import QGroundControl.ScreenTools 1.0 @@ -10,8 +11,8 @@ import QGroundControl.FactControls 1.0 QGCFlickable { id: root width: availableWidth - height: Math.min(availableHeight, geoFenceEditorRect.height) - contentHeight: geoFenceEditorRect.height + height: availableHeight + contentHeight: editorColumn.height clip: true property real availableWidth @@ -23,99 +24,317 @@ QGCFlickable { readonly property real _margin: ScreenTools.defaultFontPixelWidth / 2 readonly property real _radius: ScreenTools.defaultFontPixelWidth / 2 - property var polygon: myGeoFenceController.polygon + Column { + id: editorColumn + anchors.left: parent.left + anchors.right: parent.right - Rectangle { - id: geoFenceEditorRect - width: parent.width - height: geoFenceItems.y + geoFenceItems.height + (_margin * 2) - radius: _radius - color: qgcPal.missionItemEditor + Rectangle { + id: geoFenceEditorRect + width: parent.width + height: geoFenceItems.y + geoFenceItems.height + (_margin * 2) + radius: _radius + color: qgcPal.missionItemEditor - QGCLabel { - id: geoFenceLabel - anchors.margins: _margin - anchors.left: parent.left - anchors.top: parent.top - text: qsTr("GeoFence") - color: "black" - } + QGCLabel { + id: geoFenceLabel + anchors.margins: _margin + anchors.left: parent.left + anchors.top: parent.top + text: qsTr("GeoFence") + color: "black" + } - Rectangle { - id: geoFenceItems - anchors.margins: _margin - anchors.left: parent.left - anchors.right: parent.right - anchors.top: geoFenceLabel.bottom - height: fenceColumn.y + fenceColumn.height + (_margin * 2) - color: qgcPal.windowShadeDark - radius: _radius - - Column { - id: fenceColumn + Rectangle { + id: geoFenceItems anchors.margins: _margin anchors.left: parent.left anchors.right: parent.right - spacing: ScreenTools.defaultFontPixelHeight / 2 + anchors.top: geoFenceLabel.bottom + height: fenceColumn.y + fenceColumn.height + (_margin * 2) + color: qgcPal.windowShadeDark + radius: _radius - QGCLabel { - id: geoLabel + Column { + id: fenceColumn + anchors.margins: _margin + anchors.top: parent.top anchors.left: parent.left anchors.right: parent.right - wrapMode: Text.WordWrap - font.pointSize: ScreenTools.smallFontPointSize - text: qsTr("GeoFencing allows you to set a virtual ‘fence’ around the area you want to fly in.") - } + spacing: ScreenTools.defaultFontPixelHeight / 2 - Repeater { - model: myGeoFenceController.params + QGCLabel { + anchors.left: parent.left + anchors.right: parent.right + wrapMode: Text.WordWrap + font.pointSize: myGeoFenceController.supported ? ScreenTools.smallFontPointSize : ScreenTools.defaultFontPointSize + text: myGeoFenceController.supported ? + qsTr("GeoFencing allows you to set a virtual ‘fence’ around the area you want to fly in.") : + qsTr("This vehicle does not support GeoFence.") + } - Item { - width: fenceColumn.width - height: textField.height + Column { + anchors.left: parent.left + anchors.right: parent.right + spacing: ScreenTools.defaultFontPixelHeight / 2 + visible: myGeoFenceController.supported - property bool showCombo: modelData.enumStrings.length > 0 + Repeater { + model: myGeoFenceController.params - QGCLabel { - id: textFieldLabel - anchors.baseline: textField.baseline - text: myGeoFenceController.paramLabels[index] + Item { + width: fenceColumn.width + height: textField.height + + property bool showCombo: modelData.enumStrings.length > 0 + + QGCLabel { + id: textFieldLabel + anchors.baseline: textField.baseline + text: myGeoFenceController.paramLabels[index] + } + + FactTextField { + id: textField + anchors.right: parent.right + width: _editFieldWidth + showUnits: true + fact: modelData + visible: !parent.showCombo + } + + FactComboBox { + id: comboField + anchors.right: parent.right + width: _editFieldWidth + indexModel: false + fact: showCombo ? modelData : _nullFact + visible: parent.showCombo + + property var _nullFact: Fact { } + } + } + } + + SectionHeader { + id: insertSection + text: qsTr("Insert GeoFence") } - FactTextField { - id: textField + QGCButton { + anchors.left: parent.left anchors.right: parent.right - width: _editFieldWidth - showUnits: true - fact: modelData - visible: !parent.showCombo + text: qsTr("Polygon Fence") + + onClicked: { + var rect = Qt.rect(flightMap.centerViewport.x, flightMap.centerViewport.y, flightMap.centerViewport.width, flightMap.centerViewport.height) + var topLeftCoord = flightMap.toCoordinate(Qt.point(rect.x, rect.y), false /* clipToViewPort */) + var bottomRightCoord = flightMap.toCoordinate(Qt.point(rect.x + rect.width, rect.y + rect.height), false /* clipToViewPort */) + myGeoFenceController.addInclusionPolygon(topLeftCoord, bottomRightCoord) + } } - FactComboBox { - id: comboField + QGCButton { + anchors.left: parent.left anchors.right: parent.right - width: _editFieldWidth - indexModel: false - fact: showCombo ? modelData : _nullFact - visible: parent.showCombo + text: qsTr("Circular Fence") - property var _nullFact: Fact { } + onClicked: { + var rect = Qt.rect(flightMap.centerViewport.x, flightMap.centerViewport.y, flightMap.centerViewport.width, flightMap.centerViewport.height) + var topLeftCoord = flightMap.toCoordinate(Qt.point(rect.x, rect.y), false /* clipToViewPort */) + var bottomRightCoord = flightMap.toCoordinate(Qt.point(rect.x + rect.width, rect.y + rect.height), false /* clipToViewPort */) + myGeoFenceController.addInclusionCircle(topLeftCoord, bottomRightCoord) + } } - } - } - QGCButton { - text: qsTr("Add fence polygon") - visible: myGeoFenceController.polygonSupported && myGeoFenceController.mapPolygon.count === 0 - onClicked: myGeoFenceController.addPolygon() - } + SectionHeader { + id: polygonSection + text: qsTr("Polygon Fences") + } + + QGCLabel { + text: qsTr("None") + visible: polygonSection.checked && myGeoFenceController.polygons.count == 0 + } + + GridLayout { + anchors.left: parent.left + anchors.right: parent.right + columns: 3 + flow: GridLayout.TopToBottom + visible: polygonSection.checked && myGeoFenceController.polygons.count > 0 + + QGCLabel { + text: qsTr("Inclusion") + Layout.column: 0 + Layout.alignment: Qt.AlignHCenter + } + + Repeater { + model: myGeoFenceController.polygons + + QGCCheckBox { + checked: object.inclusion + onClicked: object.inclusion = checked + Layout.alignment: Qt.AlignHCenter + } + } + + QGCLabel { + text: qsTr("Edit") + Layout.column: 1 + Layout.alignment: Qt.AlignHCenter + } + + Repeater { + model: myGeoFenceController.polygons + + QGCRadioButton { + checked: _interactive + Layout.alignment: Qt.AlignHCenter + + property bool _interactive: object.interactive - QGCButton { - text: qsTr("Remove fence polygon") - visible: myGeoFenceController.polygonSupported && myGeoFenceController.mapPolygon.count > 0 - onClicked: myGeoFenceController.removePolygon() + on_InteractiveChanged: checked = _interactive + + onClicked: { + myGeoFenceController.clearAllInteractive() + object.interactive = checked + } + } + } + + QGCLabel { + text: qsTr("Delete") + Layout.column: 2 + Layout.alignment: Qt.AlignHCenter + } + + Repeater { + model: myGeoFenceController.polygons + + QGCColoredImage { + width: ScreenTools.defaultFontPixelHeight + height: width + sourceSize.height: width + source: "/res/XDelete.svg" + fillMode: Image.PreserveAspectFit + color: qgcPal.text + Layout.alignment: Qt.AlignHCenter + + property int _polygonIndex: index + + QGCMouseArea { + fillItem: parent + onClicked: myGeoFenceController.deletePolygon(parent._polygonIndex) + } + } + } + } // GridLayout + + SectionHeader { + id: circleSection + text: qsTr("Circular Fences") + } + + QGCLabel { + text: qsTr("None") + visible: circleSection.checked && myGeoFenceController.circles.count == 0 + } + + GridLayout { + anchors.left: parent.left + anchors.right: parent.right + columns: 4 + flow: GridLayout.TopToBottom + visible: polygonSection.checked && myGeoFenceController.circles.count > 0 + + QGCLabel { + text: qsTr("Inclusion") + Layout.column: 0 + Layout.alignment: Qt.AlignHCenter + } + + Repeater { + model: myGeoFenceController.circles + + QGCCheckBox { + checked: object.inclusion + onClicked: object.inclusion = checked + Layout.alignment: Qt.AlignHCenter + } + } + + QGCLabel { + text: qsTr("Edit") + Layout.column: 1 + Layout.alignment: Qt.AlignHCenter + } + + Repeater { + model: myGeoFenceController.circles + + QGCRadioButton { + checked: _interactive + Layout.alignment: Qt.AlignHCenter + + property bool _interactive: object.interactive + + on_InteractiveChanged: checked = _interactive + + onClicked: { + myGeoFenceController.clearAllInteractive() + object.interactive = checked + } + } + } + + QGCLabel { + text: qsTr("Radius") + Layout.column: 2 + Layout.alignment: Qt.AlignHCenter + } + + Repeater { + model: myGeoFenceController.circles + + FactTextField { + fact: object.radius + Layout.fillWidth: true + Layout.alignment: Qt.AlignHCenter + } + } + + QGCLabel { + text: qsTr("Delete") + Layout.column: 3 + Layout.alignment: Qt.AlignHCenter + } + + Repeater { + model: myGeoFenceController.circles + + QGCColoredImage { + width: ScreenTools.defaultFontPixelHeight + height: width + sourceSize.height: width + source: "/res/XDelete.svg" + fillMode: Image.PreserveAspectFit + color: qgcPal.text + Layout.alignment: Qt.AlignHCenter + + property int _circleIndex: index + + QGCMouseArea { + fillItem: parent + onClicked: myGeoFenceController.deleteCircle(parent._polygonIndex) + } + } + } + } // GridLayout + } } } - } + } // Rectangle } } diff --git a/src/PlanView/GeoFenceMapVisuals.qml b/src/PlanView/GeoFenceMapVisuals.qml index e66cbd666233f14cca4f6d36a81d637e27c33ac1..9e41cc65f3555abe0fac680d1254836f2299eb05 100644 --- a/src/PlanView/GeoFenceMapVisuals.qml +++ b/src/PlanView/GeoFenceMapVisuals.qml @@ -30,34 +30,53 @@ Item { property var _breachReturnPointComponent property var _mouseAreaComponent - property var _circleComponent - property var _mapPolygon: myGeoFenceController.mapPolygon - property bool _interactive: interactive - property bool _circleSupported: myGeoFenceController.circleRadiusFact !== null - property bool _circleEnabled: myGeoFenceController.circleEnabled - property real _circleRadius: _circleSupported ? myGeoFenceController.circleRadiusFact.rawValue : 0 - property bool _polygonSupported: myGeoFenceController.polygonSupported - property bool _polygonEnabled: myGeoFenceController.polygonEnabled + property var _polygons: myGeoFenceController.polygons + property var _circles: myGeoFenceController.circles + + function addPolygon(inclusionPolygon) { + // Initial polygon is inset to take 2/3rds space + var rect = Qt.rect(map.centerViewport.x, map.centerViewport.y, map.centerViewport.width, map.centerViewport.height) + rect.x += (rect.width * 0.25) / 2 + rect.y += (rect.height * 0.25) / 2 + rect.width *= 0.75 + rect.height *= 0.75 + + var centerCoord = map.toCoordinate(Qt.point(rect.x + (rect.width / 2), rect.y + (rect.height / 2)), false /* clipToViewPort */) + var topLeftCoord = map.toCoordinate(Qt.point(rect.x, rect.y), false /* clipToViewPort */) + var topRightCoord = map.toCoordinate(Qt.point(rect.x + rect.width, rect.y), false /* clipToViewPort */) + var bottomLeftCoord = map.toCoordinate(Qt.point(rect.x, rect.y + rect.height), false /* clipToViewPort */) + var bottomRightCoord = map.toCoordinate(Qt.point(rect.x + rect.width, rect.y + rect.height), false /* clipToViewPort */) + + // Initial polygon has max width and height of 3000 meters + var halfWidthMeters = Math.min(topLeftCoord.distanceTo(topRightCoord), 3000) / 2 + var halfHeightMeters = Math.min(topLeftCoord.distanceTo(bottomLeftCoord), 3000) / 2 + topLeftCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 0) + topRightCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, 90).atDistanceAndAzimuth(halfHeightMeters, 0) + bottomLeftCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 180) + bottomRightCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, 90).atDistanceAndAzimuth(halfHeightMeters, 180) + + console.log(map.center) + console.log(topLeftCoord) + console.log(bottomRightCoord) + + if (inclusionPolygon) { + myGeoFenceController.addInclusion(topLeftCoord, bottomRightCoord) + } else { + myGeoFenceController.addExclusion(topLeftCoord, bottomRightCoord) + } + } Component.onCompleted: { - _circleComponent = circleComponent.createObject(map) - map.addMapItem(_circleComponent) _breachReturnPointComponent = breachReturnPointComponent.createObject(map) map.addMapItem(_breachReturnPointComponent) _mouseAreaComponent = mouseAreaComponent.createObject(map) } Component.onDestruction: { - _circleComponent.destroy() _breachReturnPointComponent.destroy() _mouseAreaComponent.destroy() } - Connections { - target: myGeoFenceController - onAddInitialFencePolygon: mapPolygonVisuals.addInitialPolygon() - } - // Mouse area to capture breach return point coordinate Component { id: mouseAreaComponent @@ -69,28 +88,29 @@ Item { } } - QGCMapPolygonVisuals { - id: mapPolygonVisuals - mapControl: map - mapPolygon: _mapPolygon - interactive: _interactive - borderWidth: 2 - borderColor: "orange" - visible: _polygonSupported && (planView || _polygonEnabled) + Instantiator { + model: _polygons + + delegate : QGCMapPolygonVisuals { + mapControl: map + mapPolygon: object + borderWidth: object.inclusion ? 2 : 0 + borderColor: "orange" + interiorColor: object.inclusion ? "transparent" : "orange" + interiorOpacity: object.inclusion ? 1: 0.2 + } } - // GeoFence circle - Component { - id: circleComponent + Instantiator { + model: _circles - MapCircle { - z: QGroundControl.zOrderMapItems - border.width: 2 - border.color: "orange" - color: "transparent" - center: homePosition ? homePosition : QtPositioning.coordinate() - radius: _circleRadius - visible: _circleSupported && _circleRadius > 0 && (planView || _circleEnabled) + delegate : QGCMapCircleVisuals { + mapControl: map + mapCircle: object + borderWidth: object.inclusion ? 2 : 0 + borderColor: "orange" + interiorColor: object.inclusion ? "transparent" : "orange" + interiorOpacity: object.inclusion ? 1: 0.2 } } diff --git a/src/PlanView/PlanView.qml b/src/PlanView/PlanView.qml index 673d930cf67be5201400dd658c4655db9a74f3d5..f4672d2c97b33526360e7748aceb32ec32e7ab9e 100644 --- a/src/PlanView/PlanView.qml +++ b/src/PlanView/PlanView.qml @@ -335,7 +335,7 @@ QGCView { } break case _layerRallyPoints: - if (_rallyPointController.rallyPointsSupported) { + if (_rallyPointController.supported) { _rallyPointController.addPoint(coordinate) } break diff --git a/src/PlanView/RallyPointEditorHeader.qml b/src/PlanView/RallyPointEditorHeader.qml index ba87f949fdde9f27239a488d9ac6cf65664a69ac..ff0a86277f00775ef7c4f7b44bcc446648543bf2 100644 --- a/src/PlanView/RallyPointEditorHeader.qml +++ b/src/PlanView/RallyPointEditorHeader.qml @@ -59,7 +59,7 @@ QGCFlickable { anchors.right: parent.right anchors.top: infoLabel.bottom wrapMode: Text.WordWrap - text: controller.rallyPointsSupported ? + text: controller.supported ? qsTr("Click in the map to add new rally points.") : qsTr("This vehicle does not support Rally Points.") } diff --git a/src/PlanView/RallyPointMapVisuals.qml b/src/PlanView/RallyPointMapVisuals.qml index f303349ad0396d461f08b4066273be8ea747f63c..57c2f5a3d229cf678d6644f3f21094dd103f191d 100644 --- a/src/PlanView/RallyPointMapVisuals.qml +++ b/src/PlanView/RallyPointMapVisuals.qml @@ -29,7 +29,7 @@ Item { property bool _interactive: interactive property var _rallyPointsComponent - property bool _rallyPointsSupported: myRallyPointController.rallyPointsSupported + property bool _rallyPointsSupported: myRallyPointController.supported property var _rallyPoints: myRallyPointController.points Component.onCompleted: { diff --git a/src/QmlControls/QGroundControl.Controls.qmldir b/src/QmlControls/QGroundControl.Controls.qmldir index b16c14e3478ed1842e80666dac1510735b5eda31..6bff369b7595ca1779ca8ccc269c87a357d5bf2c 100644 --- a/src/QmlControls/QGroundControl.Controls.qmldir +++ b/src/QmlControls/QGroundControl.Controls.qmldir @@ -43,6 +43,7 @@ QGCGroupBox 1.0 QGCGroupBox.qml QGCLabel 1.0 QGCLabel.qml QGCListView 1.0 QGCListView.qml QGCMapLabel 1.0 QGCMapLabel.qml +QGCMapCircleVisuals 1.0 QGCMapCircleVisuals.qml QGCMapPolygonVisuals 1.0 QGCMapPolygonVisuals.qml QGCMouseArea 1.0 QGCMouseArea.qml QGCMovableItem 1.0 QGCMovableItem.qml diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index e0e8a834533f48307359019123b7604a0a46502e..753faf6fa88daf51bf733181017f7c6ae2bc70be 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -116,7 +116,7 @@ Vehicle::Vehicle(LinkInterface* link, , _telemetryRNoise(0) , _maxProtoVersion(0) , _vehicleCapabilitiesKnown(false) - , _supportsMissionItemInt(false) + , _capabilityBits(0) , _connectionLost(false) , _connectionLostEnabled(true) , _initialPlanRequestComplete(false) @@ -281,7 +281,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _defaultCruiseSpeed(_settingsManager->appSettings()->offlineEditingCruiseSpeed()->rawValue().toDouble()) , _defaultHoverSpeed(_settingsManager->appSettings()->offlineEditingHoverSpeed()->rawValue().toDouble()) , _vehicleCapabilitiesKnown(true) - , _supportsMissionItemInt(false) + , _capabilityBits(_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA ? 0 : MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY) , _connectionLost(false) , _connectionLostEnabled(true) , _initialPlanRequestComplete(false) @@ -350,11 +350,11 @@ void Vehicle::_commonInit(void) connect(_parameterManager, &ParameterManager::parametersReadyChanged, this, &Vehicle::_parametersReady); // GeoFenceManager needs to access ParameterManager so make sure to create after - _geoFenceManager = _firmwarePlugin->newGeoFenceManager(this); + _geoFenceManager = new GeoFenceManager(this); connect(_geoFenceManager, &GeoFenceManager::error, this, &Vehicle::_geoFenceManagerError); connect(_geoFenceManager, &GeoFenceManager::loadComplete, this, &Vehicle::_geoFenceLoadComplete); - _rallyPointManager = _firmwarePlugin->newRallyPointManager(this); + _rallyPointManager = new RallyPointManager(this); connect(_rallyPointManager, &RallyPointManager::error, this, &Vehicle::_rallyPointManagerError); connect(_rallyPointManager, &RallyPointManager::loadComplete, this, &Vehicle::_rallyPointLoadComplete); @@ -416,6 +416,12 @@ void Vehicle::_offlineFirmwareTypeSettingChanged(QVariant value) { _firmwareType = static_cast(value.toInt()); emit firmwareTypeChanged(); + if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { + _capabilityBits = 0; + } else { + _capabilityBits = MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY; + } + emit capabilityBitsChanged(_capabilityBits); } void Vehicle::_offlineVehicleTypeSettingChanged(QVariant value) @@ -749,11 +755,10 @@ void Vehicle::_handleAltitude(mavlink_message_t& message) void Vehicle::_setCapabilities(uint64_t capabilityBits) { - if (capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_INT) { - _supportsMissionItemInt = true; - } + _capabilityBits = capabilityBits; _vehicleCapabilitiesKnown = true; emit capabilitiesKnownChanged(true); + emit capabilityBitsChanged(_capabilityBits); // This should potentially be turned into a user-facing warning // if the general experience after deployment is that users want MAVLink 2 @@ -762,7 +767,13 @@ void Vehicle::_setCapabilities(uint64_t capabilityBits) qCDebug(VehicleLog) << QString("Vehicle does support MAVLink 2 but the link does not allow for it."); } - qCDebug(VehicleLog) << QString("Vehicle %1 MISSION_ITEM_INT").arg(_supportsMissionItemInt ? QStringLiteral("supports") : QStringLiteral("does not support")); + QString supports("supports"); + QString doesNotSupport("does not support"); + + qCDebug(VehicleLog) << QString("Vehicle %1 Mavlink 2.0").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MAVLINK2 ? supports : doesNotSupport); + qCDebug(VehicleLog) << QString("Vehicle %1 MISSION_ITEM_INT").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_INT ? supports : doesNotSupport); + qCDebug(VehicleLog) << QString("Vehicle %1 GeoFence").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE ? supports : doesNotSupport); + qCDebug(VehicleLog) << QString("Vehicle %1 RallyPoints").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY ? supports : doesNotSupport); } void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& message) @@ -1908,8 +1919,13 @@ void Vehicle::_missionLoadComplete(void) // After the initial mission request completes we ask for the geofence if (!_geoFenceManagerInitialRequestSent) { _geoFenceManagerInitialRequestSent = true; - qCDebug(VehicleLog) << "_missionLoadComplete requesting geoFence"; - _geoFenceManager->loadFromVehicle(); + if (_geoFenceManager->supported()) { + qCDebug(VehicleLog) << "_missionLoadComplete requesting GeoFence"; + _geoFenceManager->loadFromVehicle(); + } else { + qCDebug(VehicleLog) << "_missionLoadComplete GeoFence not supported skipping"; + _geoFenceLoadComplete(); + } } } @@ -1918,15 +1934,23 @@ void Vehicle::_geoFenceLoadComplete(void) // After geofence request completes we ask for the rally points if (!_rallyPointManagerInitialRequestSent) { _rallyPointManagerInitialRequestSent = true; - qCDebug(VehicleLog) << "_missionLoadComplete requesting rally points"; - _rallyPointManager->loadFromVehicle(); + if (_rallyPointManager->supported()) { + qCDebug(VehicleLog) << "_missionLoadComplete requesting Rally Points"; + _rallyPointManager->loadFromVehicle(); + } else { + qCDebug(VehicleLog) << "_missionLoadComplete Rally Points not supported skipping"; + _rallyPointLoadComplete(); + } } } void Vehicle::_rallyPointLoadComplete(void) { qCDebug(VehicleLog) << "_missionLoadComplete _initialPlanRequestComplete = true"; - _initialPlanRequestComplete = true; + if (!_initialPlanRequestComplete) { + _initialPlanRequestComplete = true; + emit initialPlanRequestCompleted(); + } } void Vehicle::_parametersReady(bool parametersReady) diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 733c4d9b09c162d8d97b5abb138aafad7ba43f84..249c3de3eb5649fd268deb45a6f7648f90e74331 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -7,12 +7,7 @@ * ****************************************************************************/ - -/// @file -/// @author Don Gagne - -#ifndef Vehicle_H -#define Vehicle_H +#pragma once #include #include @@ -687,8 +682,8 @@ public: const QVariantList& toolBarIndicators (); const QVariantList& cameraList (void) const; - bool capabilitiesKnown(void) const { return _vehicleCapabilitiesKnown; } - bool supportsMissionItemInt(void) const { return _supportsMissionItemInt; } + bool capabilitiesKnown (void) const { return _vehicleCapabilitiesKnown; } + uint64_t capabilityBits (void) const { return _capabilityBits; } // Change signalled by capabilityBitsChanged /// @true: When flying a mission the vehicle is always facing towards the next waypoint bool vehicleYawsToNextWaypointInMission(void) const; @@ -731,6 +726,8 @@ signals: void firmwareTypeChanged(void); void vehicleTypeChanged(void); void capabilitiesKnownChanged(bool capabilitiesKnown); + void initialPlanRequestCompleted(void); + void capabilityBitsChanged(uint64_t capabilityBits); void messagesReceivedChanged (); void messagesSentChanged (); @@ -932,7 +929,7 @@ private: int _telemetryRNoise; unsigned _maxProtoVersion; bool _vehicleCapabilitiesKnown; - bool _supportsMissionItemInt; + uint64_t _capabilityBits; typedef struct { int component; @@ -1073,4 +1070,3 @@ private: static const char* _joystickEnabledSettingsKey; }; -#endif diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 1e0092aa66ac008cce8b50f5f567062cd9278372..1c36b72dc0a661e73ff1c2922bf42f4b9508f8fb 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -892,7 +892,7 @@ void MockLink::_respondWithAutopilotVersion(void) _vehicleComponentId, _mavlinkChannel, &msg, - 0, // capabilities, + MAV_PROTOCOL_CAPABILITY_MAVLINK2 | (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA ? 0 : MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY), flightVersion, // flight_sw_version, 0, // middleware_sw_version, 0, // os_sw_version, diff --git a/src/comm/MockLinkMissionItemHandler.cc b/src/comm/MockLinkMissionItemHandler.cc index 5fa572db1bac1dd46d27347865693cf8018da147..305e1050ebb1fdbf8733882c9f749e41bdfc3cf3 100644 --- a/src/comm/MockLinkMissionItemHandler.cc +++ b/src/comm/MockLinkMissionItemHandler.cc @@ -45,37 +45,37 @@ void MockLinkMissionItemHandler::_startMissionItemResponseTimer(void) bool MockLinkMissionItemHandler::handleMessage(const mavlink_message_t& msg) { switch (msg.msgid) { - case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: - _handleMissionRequestList(msg); - break; - - case MAVLINK_MSG_ID_MISSION_REQUEST: - _handleMissionRequest(msg); - break; - - case MAVLINK_MSG_ID_MISSION_ITEM: - _handleMissionItem(msg); - break; - - case MAVLINK_MSG_ID_MISSION_COUNT: - _handleMissionCount(msg); - break; - - case MAVLINK_MSG_ID_MISSION_ACK: - // Acks are received back for each MISSION_ITEM message - break; - - case MAVLINK_MSG_ID_MISSION_SET_CURRENT: - // Sets the currently active mission item - break; - - case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: - // Delete all mission items - _missionItems.clear(); - break; - - default: - return false; + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: + _handleMissionRequestList(msg); + break; + + case MAVLINK_MSG_ID_MISSION_REQUEST: + _handleMissionRequest(msg); + break; + + case MAVLINK_MSG_ID_MISSION_ITEM: + _handleMissionItem(msg); + break; + + case MAVLINK_MSG_ID_MISSION_COUNT: + _handleMissionCount(msg); + break; + + case MAVLINK_MSG_ID_MISSION_ACK: + // Acks are received back for each MISSION_ITEM message + break; + + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: + // Sets the currently active mission item + break; + + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: + // Delete all mission items + _missionItems.clear(); + break; + + default: + return false; } return true; @@ -100,9 +100,24 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message Q_ASSERT(request.target_system == _mockLink->vehicleId()); - int itemCount = _missionItems.count(); - if (itemCount == 0 && _sendHomePositionOnEmptyList) { - itemCount = 1; + _requestType = (MAV_MISSION_TYPE)request.mission_type; + + int itemCount; + switch (_requestType) { + case MAV_MISSION_TYPE_MISSION: + itemCount = _missionItems.count(); + if (itemCount == 0 && _sendHomePositionOnEmptyList) { + itemCount = 1; + } + break; + case MAV_MISSION_TYPE_FENCE: + itemCount = _fenceItems.count(); + break; + case MAV_MISSION_TYPE_RALLY: + itemCount = _rallyItems.count(); + break; + default: + Q_ASSERT(false); } mavlink_message_t responseMsg; @@ -114,7 +129,7 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message msg.sysid, // Target is original sender msg.compid, // Target is original sender itemCount, // Number of mission items - MAV_MISSION_TYPE_MISSION); + _requestType); _mockLink->respondWithMavlinkMessage(responseMsg); } } @@ -128,8 +143,7 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& mavlink_msg_mission_request_decode(&msg, &request); Q_ASSERT(request.target_system == _mockLink->vehicleId()); - Q_ASSERT(request.seq < _missionItems.count()); - + if (_failureMode == FailReadRequest0NoResponse && request.seq == 0) { qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest0NoResponse"; } else if (_failureMode == FailReadRequest1NoResponse && request.seq == 1) { @@ -148,20 +162,32 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& } if ((_failureMode == FailReadRequest0ErrorAck && request.seq == 0) || - (_failureMode == FailReadRequest1ErrorAck && request.seq == 1)) { + (_failureMode == FailReadRequest1ErrorAck && request.seq == 1)) { _sendAck(MAV_MISSION_ERROR); } else { - mavlink_message_t responseMsg; - - mavlink_mission_item_t item; - if (_missionItems.count() == 0 && _sendHomePositionOnEmptyList) { - item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - item.command = MAV_CMD_NAV_WAYPOINT; - item.current = false; - item.autocontinue = true; - item.param1 = item.param2 = item.param3 = item.param4 = item.x = item.y = item.z = 0; - } else { - item = _missionItems[request.seq]; + mavlink_mission_item_t item; + mavlink_message_t responseMsg; + + switch (request.mission_type) { + case MAV_MISSION_TYPE_MISSION: + if (_missionItems.count() == 0 && _sendHomePositionOnEmptyList) { + item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + item.command = MAV_CMD_NAV_WAYPOINT; + item.current = false; + item.autocontinue = true; + item.param1 = item.param2 = item.param3 = item.param4 = item.x = item.y = item.z = 0; + } else { + item = _missionItems[request.seq]; + } + break; + case MAV_MISSION_TYPE_FENCE: + item = _fenceItems[request.seq]; + break; + case MAV_MISSION_TYPE_RALLY: + item = _rallyItems[request.seq]; + break; + default: + Q_ASSERT(false); } mavlink_msg_mission_item_pack_chan(_mockLink->vehicleId(), @@ -177,7 +203,7 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& item.autocontinue, item.param1, item.param2, item.param3, item.param4, item.x, item.y, item.z, - MAV_MISSION_TYPE_MISSION); + _requestType); _mockLink->respondWithMavlinkMessage(responseMsg); } } @@ -190,12 +216,23 @@ void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& ms mavlink_msg_mission_count_decode(&msg, &missionCount); Q_ASSERT(missionCount.target_system == _mockLink->vehicleId()); + _requestType = (MAV_MISSION_TYPE)missionCount.mission_type; _writeSequenceCount = missionCount.count; Q_ASSERT(_writeSequenceCount >= 0); qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount write sequence _writeSequenceCount:" << _writeSequenceCount; - _missionItems.clear(); + switch (missionCount.mission_type) { + case MAV_MISSION_TYPE_MISSION: + _missionItems.clear(); + break; + case MAV_MISSION_TYPE_FENCE: + _fenceItems.clear(); + break; + case MAV_MISSION_TYPE_RALLY: + _rallyItems.clear(); + break; + } if (_writeSequenceCount == 0) { _sendAck(MAV_MISSION_ACCEPTED); @@ -228,28 +265,25 @@ void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber) } if ((_failureMode == FailWriteRequest0IncorrectSequence && sequenceNumber == 0) || - (_failureMode == FailWriteRequest1IncorrectSequence && sequenceNumber == 1)) { + (_failureMode == FailWriteRequest1IncorrectSequence && sequenceNumber == 1)) { sequenceNumber ++; } if ((_failureMode == FailWriteRequest0ErrorAck && sequenceNumber == 0) || - (_failureMode == FailWriteRequest1ErrorAck && sequenceNumber == 1)) { + (_failureMode == FailWriteRequest1ErrorAck && sequenceNumber == 1)) { qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem sending ack error due to failure mode"; _sendAck(MAV_MISSION_ERROR); } else { - mavlink_message_t message; - mavlink_mission_request_t missionRequest; - - memset(&missionRequest, 0, sizeof(missionRequest)); - missionRequest.target_system = _mavlinkProtocol->getSystemId(); - missionRequest.target_component = _mavlinkProtocol->getComponentId(); - missionRequest.seq = sequenceNumber; - - mavlink_msg_mission_request_encode_chan(_mockLink->vehicleId(), - MAV_COMP_ID_MISSIONPLANNER, - _mockLink->mavlinkChannel(), - &message, - &missionRequest); + mavlink_message_t message; + + mavlink_msg_mission_request_pack_chan(_mockLink->vehicleId(), + MAV_COMP_ID_MISSIONPLANNER, + _mockLink->mavlinkChannel(), + &message, + _mavlinkProtocol->getSystemId(), + _mavlinkProtocol->getComponentId(), + sequenceNumber, + _requestType); _mockLink->respondWithMavlinkMessage(message); // If response with Mission Item doesn't come before timer fires it's an error @@ -262,19 +296,16 @@ void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType) { qCDebug(MockLinkMissionItemHandlerLog) << "_sendAck write sequence complete ackType:" << ackType; - mavlink_message_t message; - mavlink_mission_ack_t missionAck; - - memset(&missionAck, 0, sizeof(missionAck)); - missionAck.target_system = _mavlinkProtocol->getSystemId(); - missionAck.target_component = _mavlinkProtocol->getComponentId(); - missionAck.type = ackType; + mavlink_message_t message; - mavlink_msg_mission_ack_encode_chan(_mockLink->vehicleId(), - MAV_COMP_ID_MISSIONPLANNER, - _mockLink->mavlinkChannel(), - &message, - &missionAck); + mavlink_msg_mission_ack_pack_chan(_mockLink->vehicleId(), + MAV_COMP_ID_MISSIONPLANNER, + _mockLink->mavlinkChannel(), + &message, + _mavlinkProtocol->getSystemId(), + _mavlinkProtocol->getComponentId(), + ackType, + _requestType); _mockLink->respondWithMavlinkMessage(message); } @@ -290,8 +321,18 @@ void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg Q_ASSERT(missionItem.target_system == _mockLink->vehicleId()); - _missionItems[missionItem.seq] = missionItem; - + switch (missionItem.mission_type) { + case MAV_MISSION_TYPE_MISSION: + _missionItems[missionItem.seq] = missionItem; + break; + case MAV_MISSION_TYPE_FENCE: + _fenceItems[missionItem.seq] = missionItem; + break; + case MAV_MISSION_TYPE_RALLY: + _rallyItems[missionItem.seq] = missionItem; + break; + } + _writeSequenceIndex++; if (_writeSequenceIndex < _writeSequenceCount) { if (_failureMode == FailWriteFinalAckMissingRequests && _writeSequenceIndex == 3) { diff --git a/src/comm/MockLinkMissionItemHandler.h b/src/comm/MockLinkMissionItemHandler.h index 765058fd62aa3fda8fedd38b06833af0ef5ac35d..eadba36edb04196fe73ae68c8a9edbe6dab0a747 100644 --- a/src/comm/MockLinkMissionItemHandler.h +++ b/src/comm/MockLinkMissionItemHandler.h @@ -98,9 +98,13 @@ private: int _writeSequenceCount; ///< Numbers of items about to be written int _writeSequenceIndex; ///< Current index being reqested - typedef QMap MissionList_t; - MissionList_t _missionItems; - + typedef QMap MissionItemList_t; + + MAV_MISSION_TYPE _requestType; + MissionItemList_t _missionItems; + MissionItemList_t _fenceItems; + MissionItemList_t _rallyItems; + QTimer* _missionItemResponseTimer; FailureMode_t _failureMode; bool _sendHomePositionOnEmptyList; diff --git a/src/qgcunittest/UnitTest.cc b/src/qgcunittest/UnitTest.cc index 8384ee09d8f34c53aff6fcb247c4cd865a359f92..a970b5f1c9d681dfd49f6813bf294de384b04cb3 100644 --- a/src/qgcunittest/UnitTest.cc +++ b/src/qgcunittest/UnitTest.cc @@ -391,6 +391,12 @@ void UnitTest::_connectMockLink(MAV_AUTOPILOT autopilot) QVERIFY(qgcApp()->toolbox()->multiVehicleManager()->parameterReadyVehicleAvailable()); _vehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle(); QVERIFY(_vehicle); + + // Wait for plan request to complete + if (!_vehicle->initialPlanRequestComplete()) { + QSignalSpy spyPlan(_vehicle, SIGNAL(initialPlanRequestCompleted())); + QCOMPARE(spyPlan.wait(10000), true); + } } void UnitTest::_disconnectMockLink(void)