From a75f4b6daf21023a06957f4008437b79128c755c Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 31 Aug 2016 19:27:50 -0700 Subject: [PATCH] More work on GeoFence support - GeoFence support comes from plugin - Circle fence support - Better param support --- qgroundcontrol.pro | 2 + src/AutoPilotPlugins/AutoPilotPlugin.cc | 2 +- src/AutoPilotPlugins/AutoPilotPlugin.h | 2 +- src/FirmwarePlugin/APM/APMFirmwarePlugin.h | 38 ++- src/FirmwarePlugin/APM/APMGeoFenceManager.cc | 316 ++++++++++++++++++ src/FirmwarePlugin/APM/APMGeoFenceManager.h | 73 ++++ .../APMParameterFactMetaData.Copter.3.4.xml | 4 + .../APM/ArduCopterFirmwarePlugin.cc | 14 +- .../APM/ArduCopterFirmwarePlugin.h | 7 +- src/FirmwarePlugin/FirmwarePlugin.h | 13 +- src/FlightDisplay/FlightDisplayViewMap.qml | 23 +- src/MissionEditor/GeoFenceEditor.qml | 19 +- src/MissionEditor/MissionEditor.qml | 59 ++-- src/MissionManager/GeoFenceController.cc | 195 ++++++----- src/MissionManager/GeoFenceController.h | 67 ++-- src/MissionManager/GeoFenceManager.cc | 227 +------------ src/MissionManager/GeoFenceManager.h | 84 ++--- src/MissionManager/MissionController.cc | 46 ++- src/MissionManager/MissionController.h | 9 +- src/MissionManager/MissionManager.cc | 2 +- src/MissionManager/PlanElementController.cc | 4 +- src/MissionManager/PlanElementController.h | 4 +- src/Vehicle/Vehicle.cc | 11 +- src/Vehicle/Vehicle.h | 2 +- 24 files changed, 780 insertions(+), 443 deletions(-) create mode 100644 src/FirmwarePlugin/APM/APMGeoFenceManager.cc create mode 100644 src/FirmwarePlugin/APM/APMGeoFenceManager.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 3a78fc0a3..0f8f6abde 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -692,6 +692,7 @@ HEADERS+= \ src/FirmwarePlugin/FirmwarePluginManager.h \ src/FirmwarePlugin/FirmwarePlugin.h \ src/FirmwarePlugin/APM/APMFirmwarePlugin.h \ + src/FirmwarePlugin/APM/APMGeoFenceManager.h \ src/FirmwarePlugin/APM/APMParameterMetaData.h \ src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h \ src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h \ @@ -751,6 +752,7 @@ SOURCES += \ src/AutoPilotPlugins/PX4/SensorsComponentController.cc \ src/AutoPilotPlugins/PX4/PX4TuningComponent.cc \ src/FirmwarePlugin/APM/APMFirmwarePlugin.cc \ + src/FirmwarePlugin/APM/APMGeoFenceManager.cc \ src/FirmwarePlugin/APM/APMParameterMetaData.cc \ src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc \ src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.cc \ diff --git a/src/AutoPilotPlugins/AutoPilotPlugin.cc b/src/AutoPilotPlugins/AutoPilotPlugin.cc index 8100333ea..529b1e7c4 100644 --- a/src/AutoPilotPlugins/AutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/AutoPilotPlugin.cc @@ -117,7 +117,7 @@ void AutoPilotPlugin::refreshParametersPrefix(int componentId, const QString& na _vehicle->getParameterLoader()->refreshParametersPrefix(componentId, namePrefix); } -bool AutoPilotPlugin::parameterExists(int componentId, const QString& name) +bool AutoPilotPlugin::parameterExists(int componentId, const QString& name) const { return _vehicle->getParameterLoader()->parameterExists(componentId, name); } diff --git a/src/AutoPilotPlugins/AutoPilotPlugin.h b/src/AutoPilotPlugins/AutoPilotPlugin.h index 5cc8c3c95..b0910c27f 100644 --- a/src/AutoPilotPlugins/AutoPilotPlugin.h +++ b/src/AutoPilotPlugins/AutoPilotPlugin.h @@ -67,7 +67,7 @@ Q_INVOKABLE void refreshParametersPrefix(int componentId, const QString& namePrefix); /// Returns true if the specifed parameter exists from the default component - Q_INVOKABLE bool parameterExists(int componentId, const QString& name); + Q_INVOKABLE bool parameterExists(int componentId, const QString& name) const; /// Returns all parameter names QStringList parameterNames(int componentId); diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 4240cdacf..fd6824d85 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -17,6 +17,7 @@ #include "FirmwarePlugin.h" #include "QGCLoggingCategory.h" #include "APMParameterMetaData.h" +#include "APMGeoFenceManager.h" #include @@ -73,24 +74,25 @@ public: QList componentsForVehicle(AutoPilotPlugin* vehicle) final; QList supportedMissionCommands(void) final; - bool isCapable (const Vehicle *vehicle, FirmwareCapabilities capabilities); - QStringList flightModes (Vehicle* vehicle) final; - QString flightMode (uint8_t base_mode, uint32_t custom_mode) const final; - bool setFlightMode (const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode) final; - bool isGuidedMode (const Vehicle* vehicle) const final; - void pauseVehicle (Vehicle* vehicle); - int manualControlReservedButtonCount(void); - bool adjustIncomingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message) final; - void adjustOutgoingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message) final; - void initializeVehicle (Vehicle* vehicle) final; - bool sendHomePositionToVehicle (void) final; - void addMetaDataToFact (QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType) final; - QString getDefaultComponentIdParam (void) const final { return QString("SYSID_SW_TYPE"); } - QString missionCommandOverrides (MAV_TYPE vehicleType) const; - QString getVersionParam (void) final { return QStringLiteral("SYSID_SW_MREV"); } - QString internalParameterMetaDataFile (void) final { return QString(":/FirmwarePlugin/APM/APMParameterFactMetaData.xml"); } - void getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) final { APMParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); } - QObject* loadParameterMetaData (const QString& metaDataFile); + bool isCapable (const Vehicle *vehicle, FirmwareCapabilities capabilities); + QStringList flightModes (Vehicle* vehicle) final; + QString flightMode (uint8_t base_mode, uint32_t custom_mode) const final; + bool setFlightMode (const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode) final; + bool isGuidedMode (const Vehicle* vehicle) const final; + void pauseVehicle (Vehicle* vehicle); + int manualControlReservedButtonCount(void); + bool adjustIncomingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message) final; + void adjustOutgoingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message) final; + void initializeVehicle (Vehicle* vehicle) final; + bool sendHomePositionToVehicle (void) final; + void addMetaDataToFact (QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType) final; + QString getDefaultComponentIdParam (void) const final { return QString("SYSID_SW_TYPE"); } + QString missionCommandOverrides (MAV_TYPE vehicleType) const; + QString getVersionParam (void) final { return QStringLiteral("SYSID_SW_MREV"); } + QString internalParameterMetaDataFile (void) final { return QString(":/FirmwarePlugin/APM/APMParameterFactMetaData.xml"); } + void getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) final { APMParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); } + QObject* loadParameterMetaData (const QString& metaDataFile); + GeoFenceManager* newGeoFenceManager (Vehicle* vehicle) { return new APMGeoFenceManager(vehicle); } QString getParameterMetaDataFile(Vehicle* vehicle); diff --git a/src/FirmwarePlugin/APM/APMGeoFenceManager.cc b/src/FirmwarePlugin/APM/APMGeoFenceManager.cc new file mode 100644 index 000000000..61eea5e55 --- /dev/null +++ b/src/FirmwarePlugin/APM/APMGeoFenceManager.cc @@ -0,0 +1,316 @@ +/**************************************************************************** + * + * (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 "ParameterLoader.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) + , _breachReturnSupported(vehicle->fixedWing()) + , _firstParamLoadComplete(false) + , _circleRadiusFact(NULL) + , _readTransactionInProgress(false) + , _writeTransactionInProgress(false) + , _fenceEnableFact(NULL) + , _fenceTypeFact(NULL) +{ + connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &APMGeoFenceManager::_mavlinkMessageReceived); + connect(_vehicle->getParameterLoader(), &ParameterLoader::parametersReady, this, &APMGeoFenceManager::_parametersReady); +} + +APMGeoFenceManager::~APMGeoFenceManager() +{ + +} + +void APMGeoFenceManager::sendToVehicle(void) +{ + if (_readTransactionInProgress) { + _sendError(InternalError, QStringLiteral("Geo-Fence write attempted while read in progress.")); + return; + } + + if (!_geoFenceSupported()) { + return; + } + + // Validate + if (polygonSupported()) { + if (_polygon.count() < 3) { + _sendError(TooFewPoints, QStringLiteral("Geo-Fence polygon must contain at least 3 points.")); + return; + } + if (_polygon.count() > std::numeric_limits::max()) { + _sendError(TooManyPoints, QStringLiteral("Geo-Fence polygon has too many points: %1.").arg(_polygon.count())); + return; + } + } + + // First thing is to turn off geo fence while we are updating. This prevents the vehicle from going haywire it is in the air. + // Unfortunately the param to do this with differs between plane and copter. + const char* enableParam = _vehicle->fixedWing() ? _fenceActionParam : _fenceEnableParam; + Fact* fenceEnableFact = _vehicle->getParameterFact(FactSystem::defaultComponentId, enableParam); + QVariant savedEnableState = fenceEnableFact->rawValue(); + fenceEnableFact->setRawValue(0); + + // Total point count, +1 polygon close in last index, +1 for breach in index 0 + _cWriteFencePoints = _polygon.count() + 1 + 1 ; + _vehicle->getParameterFact(FactSystem::defaultComponentId, _fenceTotalParam)->setRawValue(_cWriteFencePoints); + + // FIXME: No validation of correct fence received + for (uint8_t index=0; index<_cWriteFencePoints; index++) { + _sendFencePoint(index); + } + + fenceEnableFact->setRawValue(savedEnableState); +} + +void APMGeoFenceManager::loadFromVehicle(void) +{ + _polygon.clear(); + + if (!_geoFenceSupported()) { + return; + } + + // Point 0: Breach return point (ArduPlane only) + // Point [1,N]: Polygon points + // Point N+1: Close polygon point (same as point 1) + int cFencePoints = _vehicle->getParameterFact(FactSystem::defaultComponentId, _fenceTotalParam)->rawValue().toInt(); + int minFencePoints = 6; + qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::loadFromVehicle" << cFencePoints; + if (cFencePoints == 0) { + // No fence, no more work to do, fence data has already been cleared + 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 out 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 + qCWarning(GeoFenceManagerLog) << "Indices out of sync" << fencePoint.idx << _currentFencePoint; + return; + } + + if (fencePoint.idx == 0) { + setBreachReturnPoint(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 + _readTransactionInProgress = false; + emit polygonChanged(_polygon); + } + } + } +} + +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(mavlink->getSystemId(), + mavlink->getComponentId(), + &msg, + _vehicle->id(), + _vehicle->defaultComponentId(), + pointIndex); + _vehicle->sendMessageOnPriorityLink(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(mavlink->getSystemId(), + mavlink->getComponentId(), + &msg, + _vehicle->id(), + _vehicle->defaultComponentId(), + pointIndex, // Index of point to set + totalPointCount, + fenceCoord.latitude(), + fenceCoord.longitude()); + _vehicle->sendMessageOnPriorityLink(msg); +} + +bool APMGeoFenceManager::inProgress(void) const +{ + return _readTransactionInProgress || _writeTransactionInProgress; +} + +bool APMGeoFenceManager::_geoFenceSupported(void) +{ + // FIXME: MockLink doesn't support geo fence yet + if (qgcApp()->runningUnitTests()) { + return false; + } + + if (!_vehicle->parameterExists(FactSystem::defaultComponentId, _fenceTotalParam) || + !_vehicle->parameterExists(FactSystem::defaultComponentId, _fenceActionParam)) { + return false; + } else { + return true; + } +} + +void APMGeoFenceManager::_updateSupportedFlags(void) +{ + emit circleSupportedChanged(circleSupported()); + emit polygonSupportedChanged(polygonSupported()); +} + +void APMGeoFenceManager::_parametersReady(void) +{ + if (!_firstParamLoadComplete) { + _firstParamLoadComplete = true; + + _fenceSupported = _vehicle->parameterExists(FactSystem::defaultComponentId, QStringLiteral("FENCE_ACTION")); + + if (_fenceSupported) { + QStringList paramNames; + QStringList paramLabels; + + if (_vehicle->multiRotor()) { + _fenceEnableFact = _vehicle->getParameterFact(FactSystem::defaultComponentId, QStringLiteral("FENCE_ENABLE")); + _fenceTypeFact = _vehicle->getParameterFact(FactSystem::defaultComponentId, QStringLiteral("FENCE_TYPE")); + + connect(_fenceEnableFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_updateSupportedFlags); + connect(_fenceTypeFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_updateSupportedFlags); + + _circleRadiusFact = _vehicle->getParameterFact(FactSystem::defaultComponentId, QStringLiteral("FENCE_RADIUS")); + connect(_circleRadiusFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_circleRadiusRawValueChanged); + emit circleRadiusChanged(circleRadius()); + + 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()) { + 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; iparameterExists(FactSystem::defaultComponentId, paramName)) { + Fact* paramFact = _vehicle->getParameterFact(FactSystem::defaultComponentId, paramName); + _params << QVariant::fromValue(paramFact); + _paramLabels << paramLabels[i]; + } + } + emit paramsChanged(_params); + emit paramLabelsChanged(_paramLabels); + + emit fenceSupportedChanged(_fenceSupported); + emit circleSupportedChanged(circleSupported()); + emit polygonSupportedChanged(polygonSupported()); + } + + qCDebug(GeoFenceManagerLog) << "fenceSupported:circleSupported:polygonSupported:breachReturnSupported" << + _fenceSupported << circleSupported() << polygonSupported() << _breachReturnSupported; + } +} + +float APMGeoFenceManager::circleRadius(void) const +{ + if (_circleRadiusFact) { + return _circleRadiusFact->rawValue().toFloat(); + } else { + return 0.0; + } +} + +void APMGeoFenceManager::_circleRadiusRawValueChanged(QVariant value) +{ + emit circleRadiusChanged(value.toFloat()); +} + +bool APMGeoFenceManager::circleSupported(void) const +{ + if (_fenceSupported && _vehicle->multiRotor() && _fenceEnableFact && _fenceTypeFact) { + return _fenceEnableFact->rawValue().toBool() && (_fenceTypeFact->rawValue().toInt() & 2); + } + + return false; +} + +bool APMGeoFenceManager::polygonSupported(void) const +{ + if (_fenceSupported) { + if (_vehicle->multiRotor()) { + if (_fenceEnableFact && _fenceTypeFact) { + return _fenceEnableFact->rawValue().toBool() && (_fenceTypeFact->rawValue().toInt() & 4); + } + } else if (_vehicle->fixedWing()) { + return true; + } + } + + return false; +} diff --git a/src/FirmwarePlugin/APM/APMGeoFenceManager.h b/src/FirmwarePlugin/APM/APMGeoFenceManager.h new file mode 100644 index 000000000..6141c5a08 --- /dev/null +++ b/src/FirmwarePlugin/APM/APMGeoFenceManager.h @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * (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 APMGeoFenceManager : public GeoFenceManager +{ + Q_OBJECT + +public: + APMGeoFenceManager(Vehicle* vehicle); + ~APMGeoFenceManager(); + + // Overrides from GeoFenceManager + bool inProgress (void) const final; + void loadFromVehicle (void) final; + void sendToVehicle (void) final; + bool fenceSupported (void) const final { return _fenceSupported; } + bool circleSupported (void) const final; + bool polygonSupported (void) const final; + bool breachReturnSupported (void) const final { return _breachReturnSupported; } + float circleRadius (void) const final; + QVariantList params (void) const final { return _params; } + QStringList paramLabels (void) const final { return _paramLabels; } + +private slots: + void _mavlinkMessageReceived(const mavlink_message_t& message); + void _updateSupportedFlags(void); + void _circleRadiusRawValueChanged(QVariant value); + void _parametersReady(void); + +private: + void _requestFencePoint(uint8_t pointIndex); + void _sendFencePoint(uint8_t pointIndex); + bool _geoFenceSupported(void); + +private: + bool _fenceSupported; + bool _breachReturnSupported; + bool _firstParamLoadComplete; + + QVariantList _params; + QStringList _paramLabels; + + Fact* _circleRadiusFact; + + bool _readTransactionInProgress; + bool _writeTransactionInProgress; + + uint8_t _cReadFencePoints; + uint8_t _cWriteFencePoints; + uint8_t _currentFencePoint; + + Fact* _fenceEnableFact; + Fact* _fenceTypeFact; + + static const char* _fenceTotalParam; + static const char* _fenceActionParam; + static const char* _fenceEnableParam; +}; + +#endif diff --git a/src/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.4.xml b/src/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.4.xml index 18afd1083..e943e00eb 100644 --- a/src/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.4.xml +++ b/src/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.4.xml @@ -6512,6 +6512,10 @@ Altitude Circle Altitude and Circle +Polygon +Altitude and Polygon +Circle and Polygon +All diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index 6aa3e0542..a1cf9da4c 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -98,9 +98,13 @@ int ArduCopterFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVe return majorVersionNumber == 3 ? 4: Vehicle::versionNotSetValue; } -bool ArduCopterFirmwarePlugin::isCapable(const Vehicle* /*vehicle*/, FirmwareCapabilities capabilities) +bool ArduCopterFirmwarePlugin::isCapable(const Vehicle* vehicle, FirmwareCapabilities capabilities) { - return (capabilities & (SetFlightModeCapability | GuidedModeCapability | PauseVehicleCapability)) == capabilities; + Q_UNUSED(vehicle); + + uint32_t vehicleCapabilities = SetFlightModeCapability | GuidedModeCapability | PauseVehicleCapability; + + return (capabilities & vehicleCapabilities) == capabilities; } void ArduCopterFirmwarePlugin::guidedModeRTL(Vehicle* vehicle) @@ -208,3 +212,9 @@ bool ArduCopterFirmwarePlugin::multiRotorXConfig(Vehicle* vehicle) { return vehicle->autopilotPlugin()->getParameterFact(FactSystem::defaultComponentId, "FRAME")->rawValue().toInt() != 0; } + +QString ArduCopterFirmwarePlugin::geoFenceRadiusParam(Vehicle* vehicle) +{ + Q_UNUSED(vehicle); + return QStringLiteral("FENCE_RADIUS"); +} diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h index 394dd10ba..06cd34ed4 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h @@ -63,9 +63,10 @@ public: void guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final; void guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel) final; const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; } - virtual int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final; - virtual bool multiRotorCoaxialMotors(Vehicle* vehicle) final; - virtual bool multiRotorXConfig(Vehicle* vehicle) final; + int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final; + bool multiRotorCoaxialMotors(Vehicle* vehicle) final; + bool multiRotorXConfig(Vehicle* vehicle) final; + QString geoFenceRadiusParam(Vehicle* vehicle) final; private: static bool _remapParamNameIntialized; diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 0c7625032..17ea75c40 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -17,6 +17,7 @@ #include "QGCMAVLink.h" #include "VehicleComponent.h" #include "AutoPilotPlugin.h" +#include "GeoFenceManager.h" #include #include @@ -40,8 +41,8 @@ public: SetFlightModeCapability = 1 << 0, ///< FirmwarePlugin::setFlightMode method is supported MavCmdPreflightStorageCapability = 1 << 1, ///< MAV_CMD_PREFLIGHT_STORAGE is supported PauseVehicleCapability = 1 << 2, ///< Vehicle supports pausing at current location - GuidedModeCapability = 1 << 3, ///< Vehicle Supports guided mode commands - OrbitModeCapability = 1 << 4, ///< Vehicle Supports orbit mode + GuidedModeCapability = 1 << 3, ///< Vehicle supports guided mode commands + OrbitModeCapability = 1 << 4, ///< Vehicle supports orbit mode } FirmwareCapabilities; /// Maps from on parameter name to another @@ -207,6 +208,14 @@ public: /// @return true: X confiuration, false: Plus configuration virtual bool multiRotorXConfig(Vehicle* vehicle) { Q_UNUSED(vehicle); return false; } + + /// Returns a newly create geofence manager for this vehicle. Returns NULL if this vehicle + /// does not support geofence. You should make sense to check vehicle capabilites for geofence + /// support. + 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(); } }; #endif diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index 0f0eaba7d..bcaea7788 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -27,7 +27,7 @@ FlightMap { anchors.fill: parent mapName: _mapName - property alias missionController: _missionController + property alias missionController: missionController property var flightWidgets property bool _followVehicle: true @@ -54,12 +54,12 @@ FlightMap { QGCPalette { id: qgcPal; colorGroupEnabled: true } MissionController { - id: _missionController + id: missionController Component.onCompleted: start(false /* editMode */) } GeoFenceController { - id: _geoFenceController + id: geoFenceController Component.onCompleted: start(false /* editMode */) } @@ -93,25 +93,34 @@ FlightMap { // Add the mission items to the map MissionItemView { - model: _mainIsMap ? _missionController.visualItems : 0 + model: _mainIsMap ? missionController.visualItems : 0 } // Add lines between waypoints MissionLineView { - model: _mainIsMap ? _missionController.waypointLines : 0 + model: _mainIsMap ? missionController.waypointLines : 0 } // GeoFence polygon MapPolygon { border.color: "#80FF0000" border.width: 3 - path: _geoFenceController.polygon.path + path: geoFenceController.polygonSupported ? geoFenceController.polygon.path : undefined + } + + // GeoFence circle + MapCircle { + border.color: "#80FF0000" + border.width: 3 + center: missionController.plannedHomePosition + radius: geoFenceController.circleSupported ? geoFenceController.circleRadius : 0 } // GeoFence breach return point MapQuickItem { anchorPoint: Qt.point(sourceItem.width / 2, sourceItem.height / 2) - coordinate: _geoFenceController.breachReturnPoint + coordinate: geoFenceController.breachReturnPoint + visible: geoFenceController.breachReturnSupported sourceItem: MissionItemIndexLabel { label: "F" } } diff --git a/src/MissionEditor/GeoFenceEditor.qml b/src/MissionEditor/GeoFenceEditor.qml index 58a556ea3..f5fc9eef2 100644 --- a/src/MissionEditor/GeoFenceEditor.qml +++ b/src/MissionEditor/GeoFenceEditor.qml @@ -7,6 +7,7 @@ import QGroundControl.Controls 1.0 import QGroundControl.FactControls 1.0 import QGroundControl.Palette 1.0 import QGroundControl.FlightMap 1.0 +import QGroundControl.FactSystem 1.0 QGCFlickable { id: root @@ -15,7 +16,7 @@ QGCFlickable { contentHeight: geoFenceEditorRect.height clip: true - readonly property real _editFieldWidth: Math.min(width - _margin * 2, ScreenTools.defaultFontPixelWidth * 12) + readonly property real _editFieldWidth: Math.min(width - _margin * 2, ScreenTools.defaultFontPixelWidth * 15) readonly property real _margin: ScreenTools.defaultFontPixelWidth / 2 readonly property real _radius: ScreenTools.defaultFontPixelWidth / 2 @@ -70,6 +71,7 @@ QGCFlickable { anchors.right: parent.right wrapMode: Text.WordWrap text: qsTr("Click in map to set breach return point.") + visible: geoFenceController.breachReturnSupported } QGCLabel { text: qsTr("Fence Settings:") } @@ -96,7 +98,7 @@ QGCFlickable { QGCLabel { id: textFieldLabel anchors.baseline: textField.baseline - text: modelData.name + text: geoFenceController.paramLabels[index] } FactTextField { @@ -105,6 +107,18 @@ QGCFlickable { width: _editFieldWidth showUnits: true fact: modelData + visible: !comboField.visible + } + + FactComboBox { + id: comboField + anchors.right: parent.right + width: _editFieldWidth + indexModel: false + fact: visible ? modelData : _nullFact + visible: modelData.enumStrings.length + + property var _nullFact: Fact { } } } } @@ -115,6 +129,7 @@ QGCFlickable { flightMap: editorMap polygon: root.polygon sectionLabel: qsTr("Fence Polygon:") + visible: geoFenceController.polygonSupported } } } diff --git a/src/MissionEditor/MissionEditor.qml b/src/MissionEditor/MissionEditor.qml index cb6cd177f..5989968e0 100644 --- a/src/MissionEditor/MissionEditor.qml +++ b/src/MissionEditor/MissionEditor.qml @@ -154,6 +154,12 @@ QGCView { id: geoFenceController Component.onCompleted: start(true /* editMode */) + + onFenceSupportedChanged: { + if (!fenceSupported && _editingLayer == _layerGeoFence) { + _editingLayer = _layerMission + } + } } QGCPalette { id: qgcPal; colorGroupEnabled: enabled } @@ -515,7 +521,7 @@ QGCView { setCurrentItem(sequenceNumber) } - onMoveHomeToMapCenter: missionController.visualItems.get(0).coordinate = editorMap.center + onMoveHomeToMapCenter: _visualItems.get(0).coordinate = editorMap.center } } // ListView } // Item - Mission Item editor @@ -537,16 +543,31 @@ QGCView { MapPolygon { border.color: "#80FF0000" border.width: 3 - path: geoFenceController.polygon.path + path: geoFenceController.polygonSupported ? geoFenceController.polygon.path : undefined + } + + // GeoFence circle + MapCircle { + border.color: "#80FF0000" + border.width: 3 + center: missionController.plannedHomePosition + radius: geoFenceController.circleSupported ? geoFenceController.circleRadius : 0 + } + + // GeoFence circle + MapCircle { + border.color: "#80FF0000" + border.width: 3 + center: missionController.plannedHomePosition + radius: geoFenceController.circleSupported ? geoFenceController.circleRadius : 0 } // GeoFence breach return point MapQuickItem { anchorPoint: Qt.point(sourceItem.width / 2, sourceItem.height / 2) coordinate: geoFenceController.breachReturnPoint - sourceItem: MissionItemIndexLabel { - label: "F" - } + visible: geoFenceController.breachReturnSupported + sourceItem: MissionItemIndexLabel { label: "F" } } //-- Dismiss Drop Down (if any) @@ -583,10 +604,10 @@ QGCView { DropButton { id: layerButton dropDirection: dropRight - //buttonImage: "/qmlimages/MapCenter.svg" viewportMargins: ScreenTools.defaultFontPixelWidth / 2 exclusiveGroup: _dropButtonsExclusiveGroup lightBorders: _lightWidgetBorders + visible: geoFenceController.fenceSupported dropDownComponent: Component { Column { @@ -775,19 +796,19 @@ QGCView { } MissionItemStatus { - id: waypointValuesDisplay - anchors.margins: ScreenTools.defaultFontPixelWidth - anchors.left: parent.left - anchors.bottom: parent.bottom - z: QGroundControl.zOrderTopMost - currentMissionItem: _currentMissionItem - missionItems: missionController.visualItems - expandedWidth: missionItemEditor.x - (ScreenTools.defaultFontPixelWidth * 2) - missionDistance: missionController.missionDistance - missionMaxTelemetry: missionController.missionMaxTelemetry - cruiseDistance: missionController.cruiseDistance - hoverDistance: missionController.hoverDistance - visible: _editingLayer == _layerMission && !ScreenTools.isShortScreen + id: waypointValuesDisplay + anchors.margins: ScreenTools.defaultFontPixelWidth + anchors.left: parent.left + anchors.bottom: parent.bottom + z: QGroundControl.zOrderTopMost + currentMissionItem: _currentMissionItem + missionItems: missionController.visualItems + expandedWidth: missionItemEditor.x - (ScreenTools.defaultFontPixelWidth * 2) + missionDistance: missionController.missionDistance + missionMaxTelemetry: missionController.missionMaxTelemetry + cruiseDistance: missionController.cruiseDistance + hoverDistance: missionController.hoverDistance + visible: _editingLayer == _layerMission && !ScreenTools.isShortScreen } } // FlightMap } // Item - split view container diff --git a/src/MissionManager/GeoFenceController.cc b/src/MissionManager/GeoFenceController.cc index 7f0398b11..71bc2b46d 100644 --- a/src/MissionManager/GeoFenceController.cc +++ b/src/MissionManager/GeoFenceController.cc @@ -24,7 +24,7 @@ GeoFenceController::GeoFenceController(QObject* parent) : PlanElementController(parent) , _dirty(false) { - _clearGeoFence(); + } GeoFenceController::~GeoFenceController() @@ -38,83 +38,61 @@ void GeoFenceController::start(bool editMode) PlanElementController::start(editMode); - connect(_multiVehicleMgr, &MultiVehicleManager::parameterReadyVehicleAvailableChanged, this, &GeoFenceController::_parameterReadyVehicleAvailableChanged); - connect(&_geoFence.polygon, &QGCMapPolygon::dirtyChanged, this, &GeoFenceController::_polygonDirtyChanged); -} - -void GeoFenceController::setFenceType(GeoFenceTypeEnum fenceType) -{ - if (_geoFence.fenceType != (GeoFenceManager::GeoFenceType_t)fenceType) { - _geoFence.fenceType = (GeoFenceManager::GeoFenceType_t)fenceType; - emit fenceTypeChanged(fenceType); - } -} - -void GeoFenceController::setCircleRadius(float circleRadius) -{ - if (qFuzzyCompare(_geoFence.circleRadius, circleRadius)) { - _geoFence.circleRadius = circleRadius; - emit circleRadiusChanged(circleRadius); - } + connect(&_polygon, &QGCMapPolygon::dirtyChanged, this, &GeoFenceController::_polygonDirtyChanged); } void GeoFenceController::setBreachReturnPoint(const QGeoCoordinate& breachReturnPoint) { - if (_geoFence.breachReturnPoint != breachReturnPoint) { - _geoFence.breachReturnPoint = breachReturnPoint; + if (_breachReturnPoint != breachReturnPoint) { + _breachReturnPoint = breachReturnPoint; + setDirty(true); emit breachReturnPointChanged(breachReturnPoint); } } -void GeoFenceController::_setParams(void) +void GeoFenceController::_signalAll(void) { - if (_params.count() == 0 && _activeVehicle && _multiVehicleMgr->parameterReadyVehicleAvailable()) { - QStringList skipList; - skipList << QStringLiteral("FENCE_TOTAL") << QStringLiteral("FENCE_ENABLE"); - - QStringList allNames = _activeVehicle->autopilotPlugin()->parameterNames(-1); - foreach (const QString& paramName, allNames) { - if (paramName.startsWith(QStringLiteral("FENCE_")) && !skipList.contains(paramName)) { - _params << QVariant::fromValue(_activeVehicle->autopilotPlugin()->getParameterFact(-1, paramName)); - } - } - emit paramsChanged(); - } + emit fenceSupportedChanged(fenceSupported()); + emit circleSupportedChanged(circleSupported()); + emit polygonSupportedChanged(polygonSupported()); + emit breachReturnSupportedChanged(breachReturnSupported()); + emit circleRadiusChanged(circleRadius()); + emit paramsChanged(params()); + emit paramLabelsChanged(paramLabels()); } -void GeoFenceController::_activeVehicleBeingRemoved(void) +void GeoFenceController::_activeVehicleBeingRemoved(Vehicle* vehicle) { _clearGeoFence(); - _params.clear(); - emit paramsChanged(); - _activeVehicle->geoFenceManager()->disconnect(this); + _signalAll(); + vehicle->geoFenceManager()->disconnect(this); } void GeoFenceController::_activeVehicleSet(void) { - connect(_activeVehicle->geoFenceManager(), &GeoFenceManager::newGeoFenceAvailable, this, &GeoFenceController::_newGeoFenceAvailable); - - _setParams(); - - if (_activeVehicle->getParameterLoader()->parametersAreReady() && !syncInProgress()) { - // We are switching between two previously existing vehicles. We have to manually ask for the items from the Vehicle. - // We don't request mission items for new vehicles since that will happen autamatically. - loadFromVehicle(); + GeoFenceManager* geoFenceManager = _activeVehicle->geoFenceManager(); + connect(geoFenceManager, &GeoFenceManager::circleSupportedChanged, this, &GeoFenceController::_setDirty); + connect(geoFenceManager, &GeoFenceManager::polygonSupportedChanged, this, &GeoFenceController::_setDirty); + connect(geoFenceManager, &GeoFenceManager::fenceSupportedChanged, this, &GeoFenceController::fenceSupportedChanged); + connect(geoFenceManager, &GeoFenceManager::circleSupportedChanged, this, &GeoFenceController::circleSupportedChanged); + connect(geoFenceManager, &GeoFenceManager::polygonSupportedChanged, this, &GeoFenceController::polygonSupportedChanged); + connect(geoFenceManager, &GeoFenceManager::breachReturnSupportedChanged, this, &GeoFenceController::breachReturnSupportedChanged); + connect(geoFenceManager, &GeoFenceManager::circleRadiusChanged, this, &GeoFenceController::circleRadiusChanged); + connect(geoFenceManager, &GeoFenceManager::polygonChanged, this, &GeoFenceController::_setPolygon); + connect(geoFenceManager, &GeoFenceManager::breachReturnPointChanged, this, &GeoFenceController::setBreachReturnPoint); + connect(geoFenceManager, &GeoFenceManager::paramsChanged, this, &GeoFenceController::paramsChanged); + connect(geoFenceManager, &GeoFenceManager::paramLabelsChanged, this, &GeoFenceController::paramLabelsChanged); + + if (_activeVehicle->getParameterLoader()->parametersAreReady()) { + _signalAll(); + if (!syncInProgress()) { + // We are switching between two previously existing vehicles. We have to manually ask for the items from the Vehicle. + // We don't request mission items for new vehicles since that will happen autamatically. + loadFromVehicle(); + } } } -void GeoFenceController::_parameterReadyVehicleAvailableChanged(bool parameterReadyVehicleAvailable) -{ - Q_UNUSED(parameterReadyVehicleAvailable); - _setParams(); -} - -void GeoFenceController::_newGeoFenceAvailable(void) -{ - _setGeoFence(_activeVehicle->geoFenceManager()->geoFence()); - setDirty(false); -} - void GeoFenceController::loadFromFilePicker(void) { @@ -143,7 +121,7 @@ void GeoFenceController::removeAll(void) void GeoFenceController::loadFromVehicle(void) { if (_activeVehicle && _activeVehicle->getParameterLoader()->parametersAreReady() && !syncInProgress()) { - _activeVehicle->geoFenceManager()->requestGeoFence(); + _activeVehicle->geoFenceManager()->loadFromVehicle(); } else { qCWarning(GeoFenceControllerLog) << "GeoFenceController::loadFromVehicle call at wrong time" << _activeVehicle << _activeVehicle->getParameterLoader()->parametersAreReady() << syncInProgress(); } @@ -152,11 +130,11 @@ void GeoFenceController::loadFromVehicle(void) void GeoFenceController::sendToVehicle(void) { if (_activeVehicle && _activeVehicle->getParameterLoader()->parametersAreReady() && !syncInProgress()) { - // FIXME: Hack - setFenceType(GeoFencePolygon); + _activeVehicle->geoFenceManager()->setPolygon(polygon()); + _activeVehicle->geoFenceManager()->setBreachReturnPoint(breachReturnPoint()); setDirty(false); - _geoFence.polygon.setDirty(false); - _activeVehicle->geoFenceManager()->setGeoFence(_geoFence); + _polygon.setDirty(false); + _activeVehicle->geoFenceManager()->sendToVehicle(); } else { qCWarning(GeoFenceControllerLog) << "GeoFenceController::loadFromVehicle call at wrong time" << _activeVehicle << _activeVehicle->getParameterLoader()->parametersAreReady() << syncInProgress(); } @@ -164,19 +142,8 @@ void GeoFenceController::sendToVehicle(void) void GeoFenceController::_clearGeoFence(void) { - setFenceType(GeoFenceNone); - setCircleRadius(0.0); setBreachReturnPoint(QGeoCoordinate()); - _geoFence.polygon.clear(); -} - -void GeoFenceController::_setGeoFence(const GeoFenceManager::GeoFence_t& geoFence) -{ - _clearGeoFence(); - setFenceType(static_cast(geoFence.fenceType)); - setCircleRadius(geoFence.circleRadius); - setBreachReturnPoint(geoFence.breachReturnPoint); - _geoFence.polygon = geoFence.polygon; + _polygon.clear(); } bool GeoFenceController::syncInProgress(void) const @@ -190,7 +157,7 @@ bool GeoFenceController::syncInProgress(void) const bool GeoFenceController::dirty(void) const { - return _dirty | _geoFence.polygon.dirty(); + return _dirty | _polygon.dirty(); } @@ -199,7 +166,7 @@ void GeoFenceController::setDirty(bool dirty) if (dirty != _dirty) { _dirty = dirty; if (!dirty) { - _geoFence.polygon.setDirty(dirty); + _polygon.setDirty(dirty); } emit dirtyChanged(dirty); } @@ -211,3 +178,77 @@ void GeoFenceController::_polygonDirtyChanged(bool dirty) setDirty(true); } } + +bool GeoFenceController::fenceSupported(void) const +{ + if (_activeVehicle) { + return _activeVehicle->geoFenceManager()->fenceSupported(); + } else { + return true; + } +} + +bool GeoFenceController::circleSupported(void) const +{ + if (_activeVehicle) { + return _activeVehicle->geoFenceManager()->circleSupported(); + } else { + return true; + } +} + +bool GeoFenceController::polygonSupported(void) const +{ + if (_activeVehicle) { + return _activeVehicle->geoFenceManager()->polygonSupported(); + } else { + return true; + } +} + +bool GeoFenceController::breachReturnSupported(void) const +{ + if (_activeVehicle) { + return _activeVehicle->geoFenceManager()->breachReturnSupported(); + } else { + return true; + } +} + +void GeoFenceController::_setDirty(void) +{ + setDirty(true); +} + +void GeoFenceController::_setPolygon(const QList& polygon) +{ + _polygon.setPath(polygon); + setDirty(true); +} + +float GeoFenceController::circleRadius(void) const +{ + if (_activeVehicle) { + return _activeVehicle->geoFenceManager()->circleRadius(); + } else { + return 0.0; + } +} + +QVariantList GeoFenceController::params(void) const +{ + if (_activeVehicle) { + return _activeVehicle->geoFenceManager()->params(); + } else { + return QVariantList(); + } +} + +QStringList GeoFenceController::paramLabels(void) const +{ + if (_activeVehicle) { + return _activeVehicle->geoFenceManager()->paramLabels(); + } else { + return QStringList(); + } +} diff --git a/src/MissionManager/GeoFenceController.h b/src/MissionManager/GeoFenceController.h index a5a6d88be..52cb99e3b 100644 --- a/src/MissionManager/GeoFenceController.h +++ b/src/MissionManager/GeoFenceController.h @@ -27,17 +27,15 @@ public: GeoFenceController(QObject* parent = NULL); ~GeoFenceController(); - enum GeoFenceTypeEnum { - GeoFenceNone = GeoFenceManager::GeoFenceNone, - GeoFenceCircle = GeoFenceManager::GeoFenceCircle, - GeoFencePolygon = GeoFenceManager::GeoFencePolygon, - }; - - Q_PROPERTY(GeoFenceTypeEnum fenceType READ fenceType WRITE setFenceType NOTIFY fenceTypeChanged) - Q_PROPERTY(float circleRadius READ circleRadius WRITE setCircleRadius NOTIFY circleRadiusChanged) - Q_PROPERTY(QGCMapPolygon* polygon READ polygon CONSTANT) - Q_PROPERTY(QGeoCoordinate breachReturnPoint READ breachReturnPoint WRITE setBreachReturnPoint NOTIFY breachReturnPointChanged) - Q_PROPERTY(QVariantList params READ params NOTIFY paramsChanged) + Q_PROPERTY(bool fenceSupported READ fenceSupported NOTIFY fenceSupportedChanged) + Q_PROPERTY(bool circleSupported READ circleSupported NOTIFY circleSupportedChanged) + Q_PROPERTY(bool polygonSupported READ polygonSupported NOTIFY polygonSupportedChanged) + Q_PROPERTY(bool breachReturnSupported READ breachReturnSupported NOTIFY breachReturnSupportedChanged) + Q_PROPERTY(float circleRadius READ circleRadius NOTIFY circleRadiusChanged) + Q_PROPERTY(QGCMapPolygon* polygon READ polygon CONSTANT) + Q_PROPERTY(QGeoCoordinate breachReturnPoint READ breachReturnPoint WRITE setBreachReturnPoint NOTIFY breachReturnPointChanged) + Q_PROPERTY(QVariantList params READ params NOTIFY paramsChanged) + Q_PROPERTY(QStringList paramLabels READ paramLabels NOTIFY paramLabelsChanged) void start (bool editMode) final; void loadFromVehicle (void) final; @@ -51,39 +49,46 @@ public: bool dirty (void) const final; void setDirty (bool dirty) final; - GeoFenceTypeEnum fenceType (void) const { return (GeoFenceTypeEnum)_geoFence.fenceType; } - float circleRadius (void) const { return _geoFence.circleRadius; } - QGCMapPolygon* polygon (void) { return &_geoFence.polygon; } - QGeoCoordinate breachReturnPoint (void) const { return _geoFence.breachReturnPoint; } - QVariantList params (void) { return _params; } + bool fenceSupported (void) const; + bool circleSupported (void) const; + bool polygonSupported (void) const; + bool breachReturnSupported (void) const; + float circleRadius (void) const; + QGCMapPolygon* polygon (void) { return &_polygon; } + QGeoCoordinate breachReturnPoint (void) const { return _breachReturnPoint; } + QVariantList params (void) const; + QStringList paramLabels (void) const; - void setFenceType(GeoFenceTypeEnum fenceType); - void setCircleRadius(float circleRadius); +public slots: void setBreachReturnPoint(const QGeoCoordinate& breachReturnPoint); signals: - void fenceTypeChanged (GeoFenceTypeEnum fenceType); - void circleRadiusChanged (float circleRadius); - void polygonPathChanged (const QVariantList& polygonPath); - void breachReturnPointChanged (QGeoCoordinate breachReturnPoint); - void paramsChanged (void); + void fenceSupportedChanged (bool fenceSupported); + void circleSupportedChanged (bool circleSupported); + void polygonSupportedChanged (bool polygonSupported); + void breachReturnSupportedChanged (bool breachReturnSupported); + void circleRadiusChanged (float circleRadius); + void polygonPathChanged (const QVariantList& polygonPath); + void breachReturnPointChanged (QGeoCoordinate breachReturnPoint); + void paramsChanged (QVariantList params); + void paramLabelsChanged (QStringList paramLabels); private slots: - void _parameterReadyVehicleAvailableChanged(bool parameterReadyVehicleAvailable); - void _newGeoFenceAvailable(void); void _polygonDirtyChanged(bool dirty); + void _setDirty(void); + void _setPolygon(const QList& polygon); private: - void _setParams(void); void _clearGeoFence(void); - void _setGeoFence(const GeoFenceManager::GeoFence_t& geoFence); + void _signalAll(void); - void _activeVehicleBeingRemoved(void) final; + void _activeVehicleBeingRemoved(Vehicle* vehicle) final; void _activeVehicleSet(void) final; - bool _dirty; - GeoFenceManager::GeoFence_t _geoFence; - QVariantList _params; + bool _dirty; + QGCMapPolygon _polygon; + QGeoCoordinate _breachReturnPoint; + QVariantList _params; }; #endif diff --git a/src/MissionManager/GeoFenceManager.cc b/src/MissionManager/GeoFenceManager.cc index 9bc9aced0..e34ea2a1b 100644 --- a/src/MissionManager/GeoFenceManager.cc +++ b/src/MissionManager/GeoFenceManager.cc @@ -7,27 +7,16 @@ * ****************************************************************************/ - -/// @file -/// @author Don Gagne - #include "GeoFenceManager.h" #include "Vehicle.h" -#include "FirmwarePlugin.h" -#include "MAVLinkProtocol.h" -#include "QGCApplication.h" QGC_LOGGING_CATEGORY(GeoFenceManagerLog, "GeoFenceManagerLog") -const char* GeoFenceManager::_fenceTotalParam = "FENCE_TOTAL"; -const char* GeoFenceManager::_fenceActionParam = "FENCE_ACTION"; - GeoFenceManager::GeoFenceManager(Vehicle* vehicle) - : _vehicle(vehicle) - , _readTransactionInProgress(false) - , _writeTransactionInProgress(false) + : QObject(vehicle) + , _vehicle(vehicle) { - connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &GeoFenceManager::_mavlinkMessageReceived); + } GeoFenceManager::~GeoFenceManager() @@ -42,213 +31,29 @@ void GeoFenceManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg) emit error(errorCode, errorMsg); } -void GeoFenceManager::setGeoFence(const GeoFence_t& geoFence) +void GeoFenceManager::loadFromVehicle(void) { - if (_readTransactionInProgress) { - _sendError(InternalError, QStringLiteral("Geo-Fence write attempted while read in progress.")); - return; - } - - if (!_geoFenceSupported()) { - return; - } - - // Validate - switch (geoFence.fenceType) { - case GeoFencePolygon: - if (geoFence.polygon.count() < 3) { - _sendError(TooFewPoints, QStringLiteral("Geo-Fence polygon must contain at least 3 points.")); - return; - } - if (geoFence.polygon.count() > std::numeric_limits::max()) { - _sendError(TooManyPoints, QStringLiteral("Geo-Fence polygon has too many points: %1.").arg(geoFence.polygon.count())); - return; - } - break; - case GeoFenceCircle: - if (geoFence.circleRadius <= 0.0) { - _sendError(InvalidCircleRadius, QStringLiteral("Geo-Fence circle radius must be greater than 0.")); - return; - } - default: - break; - } - - _geoFence.fenceType = geoFence.fenceType; - _geoFence.circleRadius = geoFence.circleRadius; - _geoFence.polygon = geoFence.polygon; - _geoFence.breachReturnPoint = geoFence.breachReturnPoint; - - if (_geoFence.fenceType != GeoFencePolygon) { - // Circle is just params, so no more work to do - return; - } - - emit newGeoFenceAvailable(); - - // First thing is to turn off geo fence while we are updating. This prevents the vehicle from going haywire it is in the air - Fact* fenceActionFact = _vehicle->getParameterFact(FactSystem::defaultComponentId, _fenceActionParam); - _savedWriteFenceAction = fenceActionFact->rawValue(); - fenceActionFact->setRawValue(0); - - // Fence total param includes: - // index 0: breach return - // last index: polygon close (same as first polygon point) - _vehicle->getParameterFact(FactSystem::defaultComponentId, _fenceTotalParam)->setRawValue(_geoFence.polygon.count() + 2); - - // FIXME: No validation of correct fence received - // Send points: - // breach return - // polygon fence points - // polygon close - // Put back previous fence action to start geofence again - _sendFencePoint(0); - for (uint8_t index=0; index<_geoFence.polygon.count(); index++) { - _sendFencePoint(index + 1); - } - _sendFencePoint(_geoFence.polygon.count() + 1); - fenceActionFact->setRawValue(_savedWriteFenceAction); + // No geofence support in unknown vehicle } -void GeoFenceManager::requestGeoFence(void) +void GeoFenceManager::sendToVehicle(void) { - _clearGeoFence(); - - if (!_geoFenceSupported()) { - return; - } - - // Point 0: Breach return point - // Point [1,N]: Polygon points - // Point N+1: Close polygon point (same as point 1) - int cFencePoints = _vehicle->getParameterFact(FactSystem::defaultComponentId, _fenceTotalParam)->rawValue().toInt(); - qCDebug(GeoFenceManagerLog) << "GeoFenceManager::requestGeoFence" << cFencePoints; - if (cFencePoints == 0) { - // No fence, no more work to do, fence data has already been cleared - return; - } - if (cFencePoints < 0 || (cFencePoints > 0 && cFencePoints < 6)) { - _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 out vehicle is received -void GeoFenceManager::_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); - if (fencePoint.idx != _currentFencePoint) { - // FIXME: Protocol out of whack - qCWarning(GeoFenceManagerLog) << "Indices out of sync" << fencePoint.idx << _currentFencePoint; - return; - } - - if (fencePoint.idx == 0) { - _geoFence.breachReturnPoint = QGeoCoordinate(fencePoint.lat, fencePoint.lng); - qCDebug(GeoFenceManagerLog) << "From vehicle: breach return point" << _geoFence.breachReturnPoint; - _requestFencePoint(++_currentFencePoint); - } else if (fencePoint.idx < _cReadFencePoints - 1) { - QGeoCoordinate polyCoord(fencePoint.lat, fencePoint.lng); - _geoFence.polygon.addCoordinate(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 - _readTransactionInProgress = false; - emit newGeoFenceAvailable(); - } - } - } -} - -void GeoFenceManager::_clearGeoFence(void) -{ - _geoFence.fenceType = GeoFenceNone; - _geoFence.circleRadius = 0.0; - _geoFence.polygon.clear(); - _geoFence.breachReturnPoint = QGeoCoordinate(); - emit newGeoFenceAvailable(); + // No geofence support in unknown vehicle } -void GeoFenceManager::_requestFencePoint(uint8_t pointIndex) +void GeoFenceManager::setPolygon(QGCMapPolygon* polygon) { - mavlink_message_t msg; - MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); - - qCDebug(GeoFenceManagerLog) << "GeoFenceManager::_requestFencePoint" << pointIndex; - mavlink_msg_fence_fetch_point_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &msg, - _vehicle->id(), - _vehicle->defaultComponentId(), - pointIndex); - _vehicle->sendMessageOnPriorityLink(msg); -} - -void GeoFenceManager::_sendFencePoint(uint8_t pointIndex) -{ - mavlink_message_t msg; - MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); - - QGeoCoordinate fenceCoord; - if (pointIndex == 0) { - fenceCoord = _geoFence.breachReturnPoint; - } else if (pointIndex - 1 < _geoFence.polygon.count()) { - fenceCoord = _geoFence.polygon[pointIndex - 1]; - } else { - // Polygon close point - fenceCoord = _geoFence.polygon[0]; + _polygon.clear(); + for (int index=0; indexcount(); index++) { + _polygon.append((*polygon)[index]); } - - mavlink_msg_fence_point_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &msg, - _vehicle->id(), - _vehicle->defaultComponentId(), - pointIndex, // Index of point to set - _geoFence.polygon.count() + 2, // Total point count, +1 for breach in index 0, +1 polygon close in last index - fenceCoord.latitude(), - fenceCoord.longitude()); - _vehicle->sendMessageOnPriorityLink(msg); + emit polygonChanged(_polygon); } -bool GeoFenceManager::inProgress(void) const +void GeoFenceManager::setBreachReturnPoint(const QGeoCoordinate& breachReturnPoint) { - return _readTransactionInProgress || _writeTransactionInProgress; -} - -bool GeoFenceManager::_geoFenceSupported(void) -{ - // FIXME: MockLink doesn't support geo fence yet - if (qgcApp()->runningUnitTests()) { - return false; - } - - // FIXME: Hack to get around lack of plugin-ized version of code - if (_vehicle->apmFirmware()) { - if (!_vehicle->parameterExists(FactSystem::defaultComponentId, _fenceTotalParam) || - !_vehicle->parameterExists(FactSystem::defaultComponentId, _fenceActionParam)) { - _sendError(InternalError, QStringLiteral("Vehicle does not support Geo-Fence implementation.")); - return false; - } else { - return true; - } - } else { - return false; + if (breachReturnPoint != _breachReturnPoint) { + _breachReturnPoint = breachReturnPoint; + emit breachReturnPointChanged(breachReturnPoint); } } diff --git a/src/MissionManager/GeoFenceManager.h b/src/MissionManager/GeoFenceManager.h index bfbbd60f6..4a0f4e68d 100644 --- a/src/MissionManager/GeoFenceManager.h +++ b/src/MissionManager/GeoFenceManager.h @@ -11,19 +11,17 @@ #define GeoFenceManager_H #include -#include #include -#include "MissionItem.h" -#include "QGCMAVLink.h" #include "QGCLoggingCategory.h" -#include "LinkInterface.h" #include "QGCMapPolygon.h" class Vehicle; Q_DECLARE_LOGGING_CATEGORY(GeoFenceManagerLog) +/// This is the base class for firmware specific geofence managers. A geofence manager is responsible +/// for communicating with the vehicle to set/get geofence settings. class GeoFenceManager : public QObject { Q_OBJECT @@ -32,29 +30,30 @@ public: GeoFenceManager(Vehicle* vehicle); ~GeoFenceManager(); - typedef enum { - GeoFenceNone, - GeoFenceCircle, - GeoFencePolygon, - } GeoFenceType_t; + /// Returns true if the manager is currently communicating with the vehicle + virtual bool inProgress(void) const { return false; } + + /// Load the current settings from teh vehicle + virtual void loadFromVehicle(void); - typedef struct { - GeoFenceType_t fenceType; - float circleRadius; - QGCMapPolygon polygon; - QGeoCoordinate breachReturnPoint; - } GeoFence_t; + /// Send the current settings to the vehicle + virtual void sendToVehicle(void); - bool inProgress(void) const; + // Support flags + virtual bool fenceSupported (void) const { return false; } + virtual bool circleSupported (void) const { return false; } + virtual bool polygonSupported (void) const { return false; } + virtual bool breachReturnSupported (void) const { return false; } - /// Request the geo fence from the vehicle - void requestGeoFence(void); + virtual float circleRadius (void) const { return 0.0; } + QList polygon (void) const { return _polygon; } + QGeoCoordinate breachReturnPoint (void) const { return _breachReturnPoint; } - /// Returns the current geofence settings - const GeoFence_t& geoFence(void) const { return _geoFence; } + virtual QVariantList params (void) const { return QVariantList(); } + virtual QStringList paramLabels (void) const { return QStringList(); } - /// Set and send the specified geo fence to the vehicle - void setGeoFence(const GeoFence_t& geoFence); + void setPolygon(QGCMapPolygon* polygon); + void setBreachReturnPoint(const QGeoCoordinate& breachReturnPoint); /// Error codes returned in error signal typedef enum { @@ -65,35 +64,24 @@ public: } ErrorCode_t; signals: - void newGeoFenceAvailable(void); - void inProgressChanged(bool inProgress); - void error(int errorCode, const QString& errorMsg); + void fenceSupportedChanged (bool fenceSupported); + void circleSupportedChanged (bool circleSupported); + void polygonSupportedChanged (bool polygonSupported); + void breachReturnSupportedChanged (bool fenceSupported); + void circleRadiusChanged (float circleRadius); + void polygonChanged (QList polygon); + void breachReturnPointChanged (QGeoCoordinate breachReturnPoint); + void inProgressChanged (bool inProgress); + void error (int errorCode, const QString& errorMsg); + void paramsChanged (QVariantList params); + void paramLabelsChanged (QStringList paramLabels); -private slots: - void _mavlinkMessageReceived(const mavlink_message_t& message); - //void _ackTimeout(void); - -private: +protected: void _sendError(ErrorCode_t errorCode, const QString& errorMsg); - void _clearGeoFence(void); - void _requestFencePoint(uint8_t pointIndex); - void _sendFencePoint(uint8_t pointIndex); - bool _geoFenceSupported(void); - -private: - Vehicle* _vehicle; - - bool _readTransactionInProgress; - bool _writeTransactionInProgress; - - uint8_t _cReadFencePoints; - uint8_t _currentFencePoint; - QVariant _savedWriteFenceAction; - - GeoFence_t _geoFence; - static const char* _fenceTotalParam; - static const char* _fenceActionParam; + Vehicle* _vehicle; + QList _polygon; + QGeoCoordinate _breachReturnPoint; }; #endif diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 63d3ac032..d73604a97 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -623,8 +623,6 @@ void MissionController::_recalcWaypointLines(void) if (!homeItem) { qWarning() << "Home item is not SimpleMissionItem"; - } else { - connect(homeItem, &VisualMissionItem::coordinateChanged, this, &MissionController::_recalcAltitudeRangeBearing); } bool showHomePosition = homeItem->showHomePosition(); @@ -944,6 +942,10 @@ void MissionController::_initAllVisualItems(void) homeItem->setShowHomePosition(true); } + emit plannedHomePositionChanged(plannedHomePosition()); + + connect(homeItem, &VisualMissionItem::coordinateChanged, this, &MissionController::_homeCoordinateChanged); + QmlObjectListModel* newComplexItems = new QmlObjectListModel(this); for (int i=0; i<_visualItems->count(); i++) { VisualMissionItem* item = qobject_cast(_visualItems->get(i)); @@ -1021,17 +1023,17 @@ void MissionController::_itemCommandChanged(void) _recalcWaypointLines(); } -void MissionController::_activeVehicleBeingRemoved(void) +void MissionController::_activeVehicleBeingRemoved(Vehicle* vehicle) { qCDebug(MissionControllerLog) << "_activeVehicleSet _activeVehicleBeingRemoved"; - MissionManager* missionManager = _activeVehicle->missionManager(); + MissionManager* missionManager = vehicle->missionManager(); disconnect(missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionController::_newMissionItemsAvailableFromVehicle); disconnect(missionManager, &MissionManager::inProgressChanged, this, &MissionController::_inProgressChanged); disconnect(missionManager, &MissionManager::currentItemChanged, this, &MissionController::_currentMissionItemChanged); - disconnect(_activeVehicle, &Vehicle::homePositionAvailableChanged, this, &MissionController::_activeVehicleHomePositionAvailableChanged); - disconnect(_activeVehicle, &Vehicle::homePositionChanged, this, &MissionController::_activeVehicleHomePositionChanged); + disconnect(vehicle, &Vehicle::homePositionAvailableChanged, this, &MissionController::_activeVehicleHomePositionAvailableChanged); + disconnect(vehicle, &Vehicle::homePositionChanged, this, &MissionController::_activeVehicleHomePositionChanged); // We always remove all items on vehicle change. This leaves a user model hole: // If the user has unsaved changes in the Plan view they will lose them @@ -1069,6 +1071,7 @@ void MissionController::_activeVehicleHomePositionAvailableChanged(bool homePosi if (homeItem) { homeItem->setShowHomePosition(homePositionAvailable); + emit plannedHomePositionChanged(plannedHomePosition()); _recalcWaypointLines(); } else { qWarning() << "Unabled to cast home item to SimpleMissionItem"; @@ -1079,8 +1082,17 @@ void MissionController::_activeVehicleHomePositionAvailableChanged(bool homePosi void MissionController::_activeVehicleHomePositionChanged(const QGeoCoordinate& homePosition) { if (!_editMode && _visualItems) { - qobject_cast(_visualItems->get(0))->setCoordinate(homePosition); - _recalcWaypointLines(); + VisualMissionItem* item = qobject_cast(_visualItems->get(0)); + if (item) { + if (item->coordinate() != homePosition) { + item->setCoordinate(homePosition); + qCDebug(MissionControllerLog) << "Home position update" << homePosition; + emit plannedHomePositionChanged(plannedHomePosition()); + _recalcWaypointLines(); + } + } else { + qWarning() << "Unabled to cast home item to VisualMissionItem"; + } } } @@ -1256,3 +1268,21 @@ void MissionController::setDirty(bool dirty) _visualItems->setDirty(dirty); } } + +QGeoCoordinate MissionController::plannedHomePosition(void) +{ + if (_visualItems && _visualItems->count() > 0) { + SimpleMissionItem* item = qobject_cast(_visualItems->get(0)); + if (item && item->showHomePosition()) { + return item->coordinate(); + } + } + + return QGeoCoordinate(); +} + +void MissionController::_homeCoordinateChanged(void) +{ + emit plannedHomePositionChanged(plannedHomePosition()); + _recalcAltitudeRangeBearing(); +} diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index ff5f7b227..a662c7e78 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -34,6 +34,7 @@ public: MissionController(QObject* parent = NULL); ~MissionController(); + Q_PROPERTY(QGeoCoordinate plannedHomePosition READ plannedHomePosition NOTIFY plannedHomePositionChanged) Q_PROPERTY(QmlObjectListModel* visualItems READ visualItems NOTIFY visualItemsChanged) Q_PROPERTY(QmlObjectListModel* complexVisualItems READ complexVisualItems NOTIFY complexVisualItemsChanged) Q_PROPERTY(QmlObjectListModel* waypointLines READ waypointLines NOTIFY waypointLinesChanged) @@ -70,6 +71,7 @@ public: // Property accessors + QGeoCoordinate plannedHomePosition (void); QmlObjectListModel* visualItems (void) { return _visualItems; } QmlObjectListModel* complexVisualItems (void) { return _complexItems; } QmlObjectListModel* waypointLines (void) { return &_waypointLines; } @@ -79,7 +81,6 @@ public: double cruiseDistance (void) const { return _cruiseDistance; } double hoverDistance (void) const { return _hoverDistance; } - void setMissionDistance (double missionDistance ); void setMissionMaxTelemetry (double missionMaxTelemetry); void setCruiseDistance (double cruiseDistance ); @@ -88,6 +89,7 @@ public: static const char* jsonSimpleItemsKey; ///< Key for simple items in a json file signals: + void plannedHomePositionChanged(QGeoCoordinate plannedHomePosition); void visualItemsChanged(void); void complexVisualItemsChanged(void); void waypointLinesChanged(void); @@ -105,7 +107,8 @@ private slots: void _inProgressChanged(bool inProgress); void _currentMissionItemChanged(int sequenceNumber); void _recalcWaypointLines(void); - void _recalcAltitudeRangeBearing(); + void _recalcAltitudeRangeBearing(void); + void _homeCoordinateChanged(void); private: void _recalcSequence(void); @@ -128,7 +131,7 @@ private: int _nextSequenceNumber(void); // Overrides from PlanElementController - void _activeVehicleBeingRemoved(void) final; + void _activeVehicleBeingRemoved(Vehicle* vehicle) final; void _activeVehicleSet(void) final; private: diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index 05ebd3e31..981b5e776 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -574,8 +574,8 @@ void MissionManager::_handleMissionCurrent(const mavlink_message_t& message) mavlink_msg_mission_current_decode(&message, &missionCurrent); - qCDebug(MissionManagerLog) << "_handleMissionCurrent seq:" << missionCurrent.seq; if (missionCurrent.seq != _currentMissionItem) { + qCDebug(MissionManagerLog) << "_handleMissionCurrent seq:" << missionCurrent.seq; _currentMissionItem = missionCurrent.seq; emit currentItemChanged(_currentMissionItem); } diff --git a/src/MissionManager/PlanElementController.cc b/src/MissionManager/PlanElementController.cc index 11c2acc96..e87559031 100644 --- a/src/MissionManager/PlanElementController.cc +++ b/src/MissionManager/PlanElementController.cc @@ -34,7 +34,9 @@ void PlanElementController::start(bool editMode) void PlanElementController::_activeVehicleChanged(Vehicle* activeVehicle) { if (_activeVehicle) { - _activeVehicleBeingRemoved(); + Vehicle* vehicleSave = _activeVehicle; + _activeVehicle = NULL; + _activeVehicleBeingRemoved(vehicleSave); } _activeVehicle = activeVehicle; diff --git a/src/MissionManager/PlanElementController.h b/src/MissionManager/PlanElementController.h index b7d26c153..63b8ec73f 100644 --- a/src/MissionManager/PlanElementController.h +++ b/src/MissionManager/PlanElementController.h @@ -57,8 +57,8 @@ protected: bool _editMode; /// Called when the current active vehicle has been removed. Derived classes should override - /// to implement custom behavior. - virtual void _activeVehicleBeingRemoved(void) = 0; + /// to implement custom behavior. When this is called _activeVehicle has already been cleared. + virtual void _activeVehicleBeingRemoved(Vehicle* removedVehicle) = 0; /// Called when a new active vehicle has been set. Derived classes should override /// to implement custom behavior. diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 8947836e9..0be53140c 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -199,13 +199,14 @@ Vehicle::Vehicle(LinkInterface* link, connect(_missionManager, &MissionManager::error, this, &Vehicle::_missionManagerError); connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_newMissionItemsAvailable); - _geoFenceManager = new GeoFenceManager(this); - connect(_geoFenceManager, &GeoFenceManager::error, this, &Vehicle::_geoFenceManagerError); - _parameterLoader = new ParameterLoader(this); connect(_parameterLoader, &ParameterLoader::parametersReady, _autopilotPlugin, &AutoPilotPlugin::_parametersReadyPreChecks); connect(_parameterLoader, &ParameterLoader::parameterListProgress, _autopilotPlugin, &AutoPilotPlugin::parameterListProgress); + // GeoFenceManager needs to access ParameterLoader so make sure to create afters + _geoFenceManager = _firmwarePlugin->newGeoFenceManager(this); + connect(_geoFenceManager, &GeoFenceManager::error, this, &Vehicle::_geoFenceManagerError); + // Ask the vehicle for firmware version info. This must be MAV_COMP_ID_ALL since we don't know default component id yet. mavlink_message_t versionMsg; @@ -1826,7 +1827,7 @@ void Vehicle::motorTest(int motor, int percent, int timeoutSecs) #endif /// Returns true if the specifed parameter exists from the default component -bool Vehicle::parameterExists(int componentId, const QString& name) +bool Vehicle::parameterExists(int componentId, const QString& name) const { return _autopilotPlugin->parameterExists(componentId, name); } @@ -1844,7 +1845,7 @@ void Vehicle::_newMissionItemsAvailable(void) // After the initial mission request complets we ask for the geofence if (!_geoFenceManagerInitialRequestComplete) { _geoFenceManagerInitialRequestComplete = true; - _geoFenceManager->requestGeoFence(); + _geoFenceManager->loadFromVehicle(); } } diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index a2cfe9579..2a6def0d5 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -570,7 +570,7 @@ public: bool xConfigMotors(void); /// Returns true if the specifed parameter exists from the default component - bool parameterExists(int componentId, const QString& name); + bool parameterExists(int componentId, const QString& name) const; /// Returns the specified parameter Fact from the default component /// WARNING: Returns a default Fact if parameter does not exists. If that possibility exists, check for existence first with -- 2.22.0