diff --git a/src/FirmwarePlugin/APM/APMGeoFenceManager.cc b/src/FirmwarePlugin/APM/APMGeoFenceManager.cc index 0590c11..a2562dd 100644 --- a/src/FirmwarePlugin/APM/APMGeoFenceManager.cc +++ b/src/FirmwarePlugin/APM/APMGeoFenceManager.cc @@ -14,9 +14,9 @@ #include "QGCApplication.h" #include "ParameterManager.h" -const char* APMGeoFenceManager::_fenceTotalParam = "FENCE_TOTAL"; -const char* APMGeoFenceManager::_fenceActionParam = "FENCE_ACTION"; -const char* APMGeoFenceManager::_fenceEnableParam = "FENCE_ENABLE"; +const char* APMGeoFenceManager::_fenceTotalParam = "FENCE_TOTAL"; +const char* APMGeoFenceManager::_fenceActionParam = "FENCE_ACTION"; +const char* APMGeoFenceManager::_fenceEnableParam = "FENCE_ENABLE"; APMGeoFenceManager::APMGeoFenceManager(Vehicle* vehicle) : GeoFenceManager(vehicle) @@ -73,13 +73,6 @@ void APMGeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, const _breachReturnPoint = breachReturn; _polygon = polygon; - // 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->parameterManager()->getParameter(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 = validatedPolygonCount ? validatedPolygonCount + 1 + 1 : 0; _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceTotalParam)->setRawValue(_cWriteFencePoints); @@ -89,8 +82,6 @@ void APMGeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, const _sendFencePoint(index); } - fenceEnableFact->setRawValue(savedEnableState); - emit loadComplete(_breachReturnPoint, _polygon); } @@ -241,6 +232,28 @@ bool APMGeoFenceManager::_geoFenceSupported(void) } } +bool APMGeoFenceManager::fenceEnabled(void) const +{ + if (qgcApp()->runningUnitTests()) { + return false; + } + + if (_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _fenceEnableParam)) { + bool fenceEnabled = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceEnableParam)->rawValue().toBool(); + qCDebug(GeoFenceManagerLog) << "FENCE_ENABLE available" << fenceEnabled; + return fenceEnabled; + } + + qCDebug(GeoFenceManagerLog) << "FENCE_ENABLE not available"; + return true; +} + +void APMGeoFenceManager::_fenceEnabledRawValueChanged(QVariant value) +{ + qCDebug(GeoFenceManagerLog) << "FENCE_ENABLE changed" << value.toBool(); + emit fenceEnabledChanged(!qgcApp()->runningUnitTests() && value.toBool()); +} + void APMGeoFenceManager::_updateSupportedFlags(void) { bool newCircleSupported = _fenceSupported && _vehicle->multiRotor() && _fenceTypeFact && (_fenceTypeFact->rawValue().toInt() & 2); @@ -269,6 +282,10 @@ void APMGeoFenceManager::_parametersReady(void) QStringList paramNames; QStringList paramLabels; + if (_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _fenceEnableParam)) { + connect(_vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceEnableParam), &Fact::rawValueChanged, this, &APMGeoFenceManager::_fenceEnabledRawValueChanged); + } + if (_vehicle->multiRotor()) { _fenceTypeFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("FENCE_TYPE")); diff --git a/src/FirmwarePlugin/APM/APMGeoFenceManager.h b/src/FirmwarePlugin/APM/APMGeoFenceManager.h index 33fe159..8002de6 100644 --- a/src/FirmwarePlugin/APM/APMGeoFenceManager.h +++ b/src/FirmwarePlugin/APM/APMGeoFenceManager.h @@ -27,6 +27,7 @@ public: void loadFromVehicle (void) final; void sendToVehicle (const QGeoCoordinate& breachReturn, const QList& polygon) final; bool fenceSupported (void) const final { return _fenceSupported; } + bool fenceEnabled (void) const final; bool circleSupported (void) const final; bool polygonSupported (void) const final; bool breachReturnSupported (void) const final { return _breachReturnSupported; } @@ -40,6 +41,7 @@ private slots: void _updateSupportedFlags(void); void _circleRadiusRawValueChanged(QVariant value); void _parametersReady(void); + void _fenceEnabledRawValueChanged(QVariant value); private: void _requestFencePoint(uint8_t pointIndex); diff --git a/src/MissionManager/GeoFenceController.cc b/src/MissionManager/GeoFenceController.cc index 64d78bc..d331c40 100644 --- a/src/MissionManager/GeoFenceController.cc +++ b/src/MissionManager/GeoFenceController.cc @@ -63,6 +63,7 @@ void GeoFenceController::setBreachReturnPoint(const QGeoCoordinate& breachReturn void GeoFenceController::_signalAll(void) { emit fenceSupportedChanged(fenceSupported()); + emit fenceEnabledChanged(fenceEnabled()); emit circleSupportedChanged(circleSupported()); emit polygonSupportedChanged(polygonSupported()); emit breachReturnSupportedChanged(breachReturnSupported()); @@ -82,9 +83,10 @@ void GeoFenceController::_activeVehicleBeingRemoved(void) void GeoFenceController::_activeVehicleSet(void) { GeoFenceManager* geoFenceManager = _activeVehicle->geoFenceManager(); + connect(geoFenceManager, &GeoFenceManager::fenceSupportedChanged, this, &GeoFenceController::fenceSupportedChanged); + connect(geoFenceManager, &GeoFenceManager::fenceEnabledChanged, this, &GeoFenceController::fenceEnabledChanged); 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); @@ -375,6 +377,11 @@ bool GeoFenceController::fenceSupported(void) const return _activeVehicle->geoFenceManager()->fenceSupported(); } +bool GeoFenceController::fenceEnabled(void) const +{ + return _activeVehicle->geoFenceManager()->fenceEnabled(); +} + bool GeoFenceController::circleSupported(void) const { return _activeVehicle->geoFenceManager()->circleSupported(); diff --git a/src/MissionManager/GeoFenceController.h b/src/MissionManager/GeoFenceController.h index 654b933..ae5b8c6 100644 --- a/src/MissionManager/GeoFenceController.h +++ b/src/MissionManager/GeoFenceController.h @@ -30,6 +30,7 @@ public: ~GeoFenceController(); Q_PROPERTY(bool fenceSupported READ fenceSupported NOTIFY fenceSupportedChanged) + Q_PROPERTY(bool fenceEnabled READ fenceEnabled NOTIFY fenceEnabledChanged) Q_PROPERTY(bool circleSupported READ circleSupported NOTIFY circleSupportedChanged) Q_PROPERTY(bool polygonSupported READ polygonSupported NOTIFY polygonSupportedChanged) Q_PROPERTY(bool breachReturnSupported READ breachReturnSupported NOTIFY breachReturnSupportedChanged) @@ -55,6 +56,7 @@ public: QString fileExtension(void) const final; bool fenceSupported (void) const; + bool fenceEnabled (void) const; bool circleSupported (void) const; bool polygonSupported (void) const; bool breachReturnSupported (void) const; @@ -69,6 +71,7 @@ public: signals: void fenceSupportedChanged (bool fenceSupported); + void fenceEnabledChanged (bool fenceEnabled); void circleSupportedChanged (bool circleSupported); void polygonSupportedChanged (bool polygonSupported); void breachReturnSupportedChanged (bool breachReturnSupported); diff --git a/src/MissionManager/GeoFenceManager.h b/src/MissionManager/GeoFenceManager.h index 20294ed..eb2e8f6 100644 --- a/src/MissionManager/GeoFenceManager.h +++ b/src/MissionManager/GeoFenceManager.h @@ -40,6 +40,7 @@ public: // Support flags virtual bool fenceSupported (void) const { return false; } + virtual bool fenceEnabled (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; } @@ -64,6 +65,7 @@ public: signals: void loadComplete (const QGeoCoordinate& breachReturn, const QList& polygon); void fenceSupportedChanged (bool fenceSupported); + void fenceEnabledChanged (bool fenceEnabled); void circleSupportedChanged (bool circleSupported); void polygonSupportedChanged (bool polygonSupported); void breachReturnSupportedChanged (bool fenceSupported);