Commit 15d20367 authored by Don Gagne's avatar Don Gagne

Update fence visibility flags

parent 2732e9a5
...@@ -21,14 +21,15 @@ const char* APMGeoFenceManager::_fenceEnableParam = "FENCE_ENABLE"; ...@@ -21,14 +21,15 @@ const char* APMGeoFenceManager::_fenceEnableParam = "FENCE_ENABLE";
APMGeoFenceManager::APMGeoFenceManager(Vehicle* vehicle) APMGeoFenceManager::APMGeoFenceManager(Vehicle* vehicle)
: GeoFenceManager(vehicle) : GeoFenceManager(vehicle)
, _fenceSupported(false) , _fenceSupported(false)
, _breachReturnSupported(vehicle->fixedWing()) , _breachReturnEnabled(vehicle->fixedWing())
, _circleSupported(false) , _circleEnabled(false)
, _polygonSupported(false) , _polygonEnabled(false)
, _firstParamLoadComplete(false) , _firstParamLoadComplete(false)
, _circleRadiusFact(NULL)
, _readTransactionInProgress(false) , _readTransactionInProgress(false)
, _writeTransactionInProgress(false) , _writeTransactionInProgress(false)
, _fenceTypeFact(NULL) , _fenceTypeFact(NULL)
, _fenceEnableFact(NULL)
, _circleRadiusFact(NULL)
{ {
connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &APMGeoFenceManager::_mavlinkMessageReceived); connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &APMGeoFenceManager::_mavlinkMessageReceived);
connect(_vehicle->parameterManager(), &ParameterManager::parametersReadyChanged, this, &APMGeoFenceManager::_parametersReady); connect(_vehicle->parameterManager(), &ParameterManager::parametersReadyChanged, this, &APMGeoFenceManager::_parametersReady);
...@@ -54,15 +55,15 @@ void APMGeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, const ...@@ -54,15 +55,15 @@ void APMGeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, const
return; return;
} }
if (!_geoFenceSupported()) { if (!_fenceSupported) {
return; return;
} }
// Validate // Validate
int validatedPolygonCount = polygon.count(); int validatedPolygonCount = 0;
if (polygonSupported()) { if (polygonEnabled()) {
if (polygon.count() < 3) { if (polygon.count() >= 3) {
validatedPolygonCount = 0; validatedPolygonCount = polygon.count();
} }
if (polygon.count() > std::numeric_limits<uint8_t>::max()) { if (polygon.count() > std::numeric_limits<uint8_t>::max()) {
_sendError(TooManyPoints, QStringLiteral("Geo-Fence polygon has too many points: %1.").arg(_polygon.count())); _sendError(TooManyPoints, QStringLiteral("Geo-Fence polygon has too many points: %1.").arg(_polygon.count()));
...@@ -71,7 +72,11 @@ void APMGeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, const ...@@ -71,7 +72,11 @@ void APMGeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, const
} }
_breachReturnPoint = breachReturn; _breachReturnPoint = breachReturn;
_polygon = polygon; if (validatedPolygonCount) {
_polygon = polygon;
} else {
_polygon.clear();
}
// Total point count, +1 polygon close in last index, +1 for breach in index 0 // Total point count, +1 polygon close in last index, +1 for breach in index 0
_cWriteFencePoints = validatedPolygonCount ? validatedPolygonCount + 1 + 1 : 0; _cWriteFencePoints = validatedPolygonCount ? validatedPolygonCount + 1 + 1 : 0;
...@@ -94,7 +99,7 @@ void APMGeoFenceManager::loadFromVehicle(void) ...@@ -94,7 +99,7 @@ void APMGeoFenceManager::loadFromVehicle(void)
_breachReturnPoint = QGeoCoordinate(); _breachReturnPoint = QGeoCoordinate();
_polygon.clear(); _polygon.clear();
if (!_geoFenceSupported()) { if (!_fenceSupported) {
return; return;
} }
...@@ -217,57 +222,27 @@ bool APMGeoFenceManager::inProgress(void) const ...@@ -217,57 +222,27 @@ bool APMGeoFenceManager::inProgress(void) const
return _readTransactionInProgress || _writeTransactionInProgress; return _readTransactionInProgress || _writeTransactionInProgress;
} }
bool APMGeoFenceManager::_geoFenceSupported(void) void APMGeoFenceManager::_updateEnabledFlags(void)
{ {
// FIXME: MockLink doesn't support geo fence yet bool fenceEnabled;
if (qgcApp()->runningUnitTests()) { if (_fenceEnableFact) {
return false; fenceEnabled = _fenceEnableFact->rawValue().toBool();
}
if (!_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _fenceTotalParam) ||
!_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _fenceActionParam)) {
return false;
} else { } else {
return true; fenceEnabled = true;
}
}
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"; bool newCircleEnabled = _fenceSupported && fenceEnabled && _fenceTypeFact && (_fenceTypeFact->rawValue().toInt() & 2);
return true; if (newCircleEnabled != _circleEnabled) {
} _circleEnabled = newCircleEnabled;
emit circleEnabledChanged(newCircleEnabled);
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);
if (newCircleSupported != _circleSupported) {
_circleSupported = newCircleSupported;
emit circleSupportedChanged(newCircleSupported);
} }
bool newPolygonSupported = _fenceSupported && bool newPolygonEnabled = _fenceSupported && fenceEnabled &&
((_vehicle->multiRotor() && _fenceTypeFact && (_fenceTypeFact->rawValue().toInt() & 4)) || ((_vehicle->multiRotor() && _fenceTypeFact && (_fenceTypeFact->rawValue().toInt() & 4)) ||
_vehicle->fixedWing()); _vehicle->fixedWing());
if (newPolygonSupported != _polygonSupported) { if (newPolygonEnabled != _polygonEnabled) {
_polygonSupported = newPolygonSupported; _polygonEnabled = newPolygonEnabled;
emit polygonSupportedChanged(newPolygonSupported); emit polygonEnabledChanged(newPolygonEnabled);
} }
} }
...@@ -276,20 +251,21 @@ void APMGeoFenceManager::_parametersReady(void) ...@@ -276,20 +251,21 @@ void APMGeoFenceManager::_parametersReady(void)
if (!_firstParamLoadComplete) { if (!_firstParamLoadComplete) {
_firstParamLoadComplete = true; _firstParamLoadComplete = true;
_fenceSupported = _vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("FENCE_ACTION")); _fenceSupported = _vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("FENCE_ACTION")) &&
!qgcApp()->runningUnitTests();
if (_fenceSupported) { if (_fenceSupported) {
QStringList paramNames; QStringList paramNames;
QStringList paramLabels; QStringList paramLabels;
if (_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _fenceEnableParam)) { if (_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _fenceEnableParam)) {
connect(_vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceEnableParam), &Fact::rawValueChanged, this, &APMGeoFenceManager::_fenceEnabledRawValueChanged); _fenceEnableFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceEnableParam);
connect(_fenceEnableFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_updateEnabledFlags);
} }
if (_vehicle->multiRotor()) { if (_vehicle->multiRotor()) {
_fenceTypeFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("FENCE_TYPE")); _fenceTypeFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("FENCE_TYPE"));
connect(_fenceTypeFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_updateEnabledFlags);
connect(_fenceTypeFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_updateSupportedFlags);
_circleRadiusFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("FENCE_RADIUS")); _circleRadiusFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("FENCE_RADIUS"));
connect(_circleRadiusFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_circleRadiusRawValueChanged); connect(_circleRadiusFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_circleRadiusRawValueChanged);
...@@ -319,18 +295,17 @@ void APMGeoFenceManager::_parametersReady(void) ...@@ -319,18 +295,17 @@ void APMGeoFenceManager::_parametersReady(void)
emit paramsChanged(_params); emit paramsChanged(_params);
emit paramLabelsChanged(_paramLabels); emit paramLabelsChanged(_paramLabels);
emit fenceSupportedChanged(_fenceSupported); _updateEnabledFlags();
_updateSupportedFlags();
} }
qCDebug(GeoFenceManagerLog) << "fenceSupported:circleSupported:polygonSupported:breachReturnSupported" << qCDebug(GeoFenceManagerLog) << "fenceSupported:circleEnabled:polygonEnabled:breachReturnEnabled" <<
_fenceSupported << circleSupported() << polygonSupported() << _breachReturnSupported; _fenceSupported << _circleEnabled << _polygonEnabled << _breachReturnEnabled;
} }
} }
float APMGeoFenceManager::circleRadius(void) const float APMGeoFenceManager::circleRadius(void) const
{ {
if (_circleRadiusFact) { if (_circleEnabled) {
return _circleRadiusFact->rawValue().toFloat(); return _circleRadiusFact->rawValue().toFloat();
} else { } else {
return 0.0; return 0.0;
...@@ -342,16 +317,6 @@ void APMGeoFenceManager::_circleRadiusRawValueChanged(QVariant value) ...@@ -342,16 +317,6 @@ void APMGeoFenceManager::_circleRadiusRawValueChanged(QVariant value)
emit circleRadiusChanged(value.toFloat()); emit circleRadiusChanged(value.toFloat());
} }
bool APMGeoFenceManager::circleSupported(void) const
{
return _circleSupported;
}
bool APMGeoFenceManager::polygonSupported(void) const
{
return _polygonSupported;
}
QString APMGeoFenceManager::editorQml(void) const QString APMGeoFenceManager::editorQml(void) const
{ {
return _vehicle->multiRotor() ? return _vehicle->multiRotor() ?
......
...@@ -23,43 +23,37 @@ public: ...@@ -23,43 +23,37 @@ public:
~APMGeoFenceManager(); ~APMGeoFenceManager();
// Overrides from GeoFenceManager // Overrides from GeoFenceManager
bool inProgress (void) const final; bool inProgress (void) const final;
void loadFromVehicle (void) final; void loadFromVehicle (void) final;
void sendToVehicle (const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon) final; void sendToVehicle (const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon) final;
bool fenceSupported (void) const final { return _fenceSupported; } bool circleEnabled (void) const final { return _circleEnabled; }
bool fenceEnabled (void) const final; bool polygonEnabled (void) const final { return _polygonEnabled; }
bool circleSupported (void) const final; bool breachReturnEnabled (void) const final { return _breachReturnEnabled; }
bool polygonSupported (void) const final; float circleRadius (void) const final;
bool breachReturnSupported (void) const final { return _breachReturnSupported; } QVariantList params (void) const final { return _params; }
float circleRadius (void) const final; QStringList paramLabels (void) const final { return _paramLabels; }
QVariantList params (void) const final { return _params; } QString editorQml (void) const final;
QStringList paramLabels (void) const final { return _paramLabels; }
QString editorQml (void) const final;
private slots: private slots:
void _mavlinkMessageReceived(const mavlink_message_t& message); void _mavlinkMessageReceived(const mavlink_message_t& message);
void _updateSupportedFlags(void); void _updateEnabledFlags(void);
void _circleRadiusRawValueChanged(QVariant value); void _circleRadiusRawValueChanged(QVariant value);
void _parametersReady(void); void _parametersReady(void);
void _fenceEnabledRawValueChanged(QVariant value);
private: private:
void _requestFencePoint(uint8_t pointIndex); void _requestFencePoint(uint8_t pointIndex);
void _sendFencePoint(uint8_t pointIndex); void _sendFencePoint(uint8_t pointIndex);
bool _geoFenceSupported(void);
private: private:
bool _fenceSupported; bool _fenceSupported;
bool _breachReturnSupported; bool _breachReturnEnabled;
bool _circleSupported; bool _circleEnabled;
bool _polygonSupported; bool _polygonEnabled;
bool _firstParamLoadComplete; bool _firstParamLoadComplete;
QVariantList _params; QVariantList _params;
QStringList _paramLabels; QStringList _paramLabels;
Fact* _circleRadiusFact;
bool _readTransactionInProgress; bool _readTransactionInProgress;
bool _writeTransactionInProgress; bool _writeTransactionInProgress;
...@@ -68,6 +62,8 @@ private: ...@@ -68,6 +62,8 @@ private:
uint8_t _currentFencePoint; uint8_t _currentFencePoint;
Fact* _fenceTypeFact; Fact* _fenceTypeFact;
Fact* _fenceEnableFact;
Fact* _circleRadiusFact;
static const char* _fenceTotalParam; static const char* _fenceTotalParam;
static const char* _fenceActionParam; static const char* _fenceActionParam;
......
...@@ -15,6 +15,7 @@ ...@@ -15,6 +15,7 @@
PX4GeoFenceManager::PX4GeoFenceManager(Vehicle* vehicle) PX4GeoFenceManager::PX4GeoFenceManager(Vehicle* vehicle)
: GeoFenceManager(vehicle) : GeoFenceManager(vehicle)
, _firstParamLoadComplete(false) , _firstParamLoadComplete(false)
, _circleEnabled(false)
, _circleRadiusFact(NULL) , _circleRadiusFact(NULL)
{ {
connect(_vehicle->parameterManager(), &ParameterManager::parametersReadyChanged, this, &PX4GeoFenceManager::_parametersReady); connect(_vehicle->parameterManager(), &ParameterManager::parametersReadyChanged, this, &PX4GeoFenceManager::_parametersReady);
...@@ -57,10 +58,11 @@ void PX4GeoFenceManager::_parametersReady(void) ...@@ -57,10 +58,11 @@ void PX4GeoFenceManager::_parametersReady(void)
emit paramsChanged(_params); emit paramsChanged(_params);
emit paramLabelsChanged(_paramLabels); emit paramLabelsChanged(_paramLabels);
emit circleSupportedChanged(circleSupported()); _circleEnabled = true;
emit circleEnabledChanged(true);
qCDebug(GeoFenceManagerLog) << "fenceSupported:circleSupported:polygonSupported:breachReturnSupported" << qCDebug(GeoFenceManagerLog) << "fenceSupported:circleSupported:polygonSupported:breachReturnSupported" <<
fenceSupported() << circleSupported() << polygonSupported() << breachReturnSupported(); _circleEnabled << polygonEnabled() << breachReturnEnabled();
} }
} }
...@@ -76,14 +78,5 @@ float PX4GeoFenceManager::circleRadius(void) const ...@@ -76,14 +78,5 @@ float PX4GeoFenceManager::circleRadius(void) const
void PX4GeoFenceManager::_circleRadiusRawValueChanged(QVariant value) void PX4GeoFenceManager::_circleRadiusRawValueChanged(QVariant value)
{ {
emit circleRadiusChanged(value.toFloat()); emit circleRadiusChanged(value.toFloat());
emit circleSupportedChanged(circleSupported());
} }
bool PX4GeoFenceManager::circleSupported(void) const
{
if (_circleRadiusFact) {
return _circleRadiusFact->rawValue().toFloat() >= 0.0;
}
return false;
}
...@@ -23,12 +23,11 @@ public: ...@@ -23,12 +23,11 @@ public:
~PX4GeoFenceManager(); ~PX4GeoFenceManager();
// Overrides from GeoFenceManager // Overrides from GeoFenceManager
bool fenceSupported (void) const final { return true; } bool circleEnabled (void) const final { return _circleEnabled; }
bool circleSupported (void) const final; float circleRadius (void) const final;
float circleRadius (void) const final; QVariantList params (void) const final { return _params; }
QVariantList params (void) const final { return _params; } QStringList paramLabels (void) const final { return _paramLabels; }
QStringList paramLabels (void) const final { return _paramLabels; } QString editorQml (void) const final { return QStringLiteral("qrc:/FirmwarePlugin/PX4/PX4GeoFenceEditor.qml"); }
QString editorQml (void) const final { return QStringLiteral("qrc:/FirmwarePlugin/PX4/PX4GeoFenceEditor.qml"); }
private slots: private slots:
void _circleRadiusRawValueChanged(QVariant value); void _circleRadiusRawValueChanged(QVariant value);
...@@ -39,6 +38,7 @@ private: ...@@ -39,6 +38,7 @@ private:
QVariantList _params; QVariantList _params;
QStringList _paramLabels; QStringList _paramLabels;
bool _circleEnabled;
Fact* _circleRadiusFact; Fact* _circleRadiusFact;
}; };
......
...@@ -111,7 +111,7 @@ FlightMap { ...@@ -111,7 +111,7 @@ FlightMap {
border.color: "#80FF0000" border.color: "#80FF0000"
border.width: 3 border.width: 3
path: geoFenceController.polygon.path path: geoFenceController.polygon.path
visible: geoFenceController.fenceEnabled && geoFenceController.polygonSupported visible: geoFenceController.polygonEnabled
} }
// GeoFence circle // GeoFence circle
...@@ -119,16 +119,16 @@ FlightMap { ...@@ -119,16 +119,16 @@ FlightMap {
border.color: "#80FF0000" border.color: "#80FF0000"
border.width: 3 border.width: 3
center: missionController.plannedHomePosition center: missionController.plannedHomePosition
radius: (geoFenceController.fenceEnabled && geoFenceController.circleSupported) ? geoFenceController.circleRadius : 0 radius: geoFenceController.circleRadius
z: QGroundControl.zOrderMapItems z: QGroundControl.zOrderMapItems
visible: geoFenceController.fenceEnabled && geoFenceController.circleSupported visible: geoFenceController.circleEnabled
} }
// GeoFence breach return point // GeoFence breach return point
MapQuickItem { MapQuickItem {
anchorPoint: Qt.point(sourceItem.width / 2, sourceItem.height / 2) anchorPoint: Qt.point(sourceItem.width / 2, sourceItem.height / 2)
coordinate: geoFenceController.breachReturnPoint coordinate: geoFenceController.breachReturnPoint
visible: geoFenceController.fenceEnabled && geoFenceController.breachReturnSupported visible: geoFenceController.breachReturnEnabled
sourceItem: MissionItemIndexLabel { label: "F" } sourceItem: MissionItemIndexLabel { label: "F" }
z: QGroundControl.zOrderMapItems z: QGroundControl.zOrderMapItems
} }
......
...@@ -145,14 +145,14 @@ QGCView { ...@@ -145,14 +145,14 @@ QGCView {
} }
function addFenceItemCoordsForFit(coordList) { function addFenceItemCoordsForFit(coordList) {
if (geoFenceController.circleSupported) { if (geoFenceController.circleEnabled) {
var azimuthList = [ 0, 180, 90, 270 ] var azimuthList = [ 0, 180, 90, 270 ]
for (var i=0; i<azimuthList.length; i++) { for (var i=0; i<azimuthList.length; i++) {
var edgeCoordinate = _visualItems.get(0).coordinate.atDistanceAndAzimuth(geoFenceController.circleRadius, azimuthList[i]) var edgeCoordinate = _visualItems.get(0).coordinate.atDistanceAndAzimuth(geoFenceController.circleRadius, azimuthList[i])
coordList.push(edgeCoordinate) coordList.push(edgeCoordinate)
} }
} }
if (geoFenceController.polygonSupported && geoFenceController.polygon.count() > 2) { if (geoFenceController.polygonEnabled && geoFenceController.polygon.count() > 2) {
for (var i=0; i<geoFenceController.polygon.count(); i++) { for (var i=0; i<geoFenceController.polygon.count(); i++) {
coordList.push(geoFenceController.polygon.path[i]) coordList.push(geoFenceController.polygon.path[i])
} }
...@@ -477,7 +477,7 @@ QGCView { ...@@ -477,7 +477,7 @@ QGCView {
} }
break break
case _layerGeoFence: case _layerGeoFence:
if (geoFenceController.breachReturnSupported) { if (geoFenceController.breachReturnEnabled) {
geoFenceController.breachReturnPoint = coordinate geoFenceController.breachReturnPoint = coordinate
geoFenceController.validateBreachReturn() geoFenceController.validateBreachReturn()
} }
...@@ -788,7 +788,7 @@ QGCView { ...@@ -788,7 +788,7 @@ QGCView {
border.width: 3 border.width: 3
path: geoFenceController.polygon.path path: geoFenceController.polygon.path
z: QGroundControl.zOrderMapItems z: QGroundControl.zOrderMapItems
visible: geoFenceController.polygonSupported visible: geoFenceController.polygonEnabled
} }
// GeoFence circle // GeoFence circle
...@@ -796,16 +796,16 @@ QGCView { ...@@ -796,16 +796,16 @@ QGCView {
border.color: "#80FF0000" border.color: "#80FF0000"
border.width: 3 border.width: 3
center: missionController.plannedHomePosition center: missionController.plannedHomePosition
radius: geoFenceController.circleSupported ? geoFenceController.circleRadius : 0 radius: geoFenceController.circleRadius
z: QGroundControl.zOrderMapItems z: QGroundControl.zOrderMapItems
visible: geoFenceController.circleSupported visible: geoFenceController.circleEnabled
} }
// GeoFence breach return point // GeoFence breach return point
MapQuickItem { MapQuickItem {
anchorPoint: Qt.point(sourceItem.width / 2, sourceItem.height / 2) anchorPoint: Qt.point(sourceItem.width / 2, sourceItem.height / 2)
coordinate: geoFenceController.breachReturnPoint coordinate: geoFenceController.breachReturnPoint
visible: geoFenceController.breachReturnSupported visible: geoFenceController.breachReturnEnabled
sourceItem: MissionItemIndexLabel { label: "F" } sourceItem: MissionItemIndexLabel { label: "F" }
z: QGroundControl.zOrderMapItems z: QGroundControl.zOrderMapItems
} }
......
...@@ -63,11 +63,9 @@ void GeoFenceController::setBreachReturnPoint(const QGeoCoordinate& breachReturn ...@@ -63,11 +63,9 @@ void GeoFenceController::setBreachReturnPoint(const QGeoCoordinate& breachReturn
void GeoFenceController::_signalAll(void) void GeoFenceController::_signalAll(void)
{ {
emit fenceSupportedChanged(fenceSupported()); emit circleEnabledChanged(circleEnabled());
emit fenceEnabledChanged(fenceEnabled()); emit polygonEnabledChanged(polygonEnabled());
emit circleSupportedChanged(circleSupported()); emit breachReturnEnabledChanged(breachReturnEnabled());
emit polygonSupportedChanged(polygonSupported());
emit breachReturnSupportedChanged(breachReturnSupported());
emit breachReturnPointChanged(breachReturnPoint()); emit breachReturnPointChanged(breachReturnPoint());
emit circleRadiusChanged(circleRadius()); emit circleRadiusChanged(circleRadius());
emit paramsChanged(params()); emit paramsChanged(params());
...@@ -84,18 +82,15 @@ void GeoFenceController::_activeVehicleBeingRemoved(void) ...@@ -84,18 +82,15 @@ void GeoFenceController::_activeVehicleBeingRemoved(void)
void GeoFenceController::_activeVehicleSet(void) void GeoFenceController::_activeVehicleSet(void)
{ {
GeoFenceManager* geoFenceManager = _activeVehicle->geoFenceManager(); GeoFenceManager* geoFenceManager = _activeVehicle->geoFenceManager();
connect(geoFenceManager, &GeoFenceManager::fenceSupportedChanged, this, &GeoFenceController::fenceSupportedChanged); connect(geoFenceManager, &GeoFenceManager::polygonEnabledChanged, this, &GeoFenceController::_setDirty);
connect(geoFenceManager, &GeoFenceManager::fenceEnabledChanged, this, &GeoFenceController::fenceEnabledChanged); connect(geoFenceManager, &GeoFenceManager::circleEnabledChanged, this, &GeoFenceController::circleEnabledChanged);
connect(geoFenceManager, &GeoFenceManager::circleSupportedChanged, this, &GeoFenceController::_setDirty); connect(geoFenceManager, &GeoFenceManager::polygonEnabledChanged, this, &GeoFenceController::polygonEnabledChanged);
connect(geoFenceManager, &GeoFenceManager::polygonSupportedChanged, this, &GeoFenceController::_setDirty); connect(geoFenceManager, &GeoFenceManager::breachReturnEnabledChanged, this, &GeoFenceController::breachReturnEnabledChanged);
connect(geoFenceManager, &GeoFenceManager::circleSupportedChanged, this, &GeoFenceController::circleSupportedChanged); connect(geoFenceManager, &GeoFenceManager::circleRadiusChanged, this, &GeoFenceController::circleRadiusChanged);
connect(geoFenceManager, &GeoFenceManager::polygonSupportedChanged, this, &GeoFenceController::polygonSupportedChanged); connect(geoFenceManager, &GeoFenceManager::paramsChanged, this, &GeoFenceController::paramsChanged);
connect(geoFenceManager, &GeoFenceManager::breachReturnSupportedChanged, this, &GeoFenceController::breachReturnSupportedChanged); connect(geoFenceManager, &GeoFenceManager::paramLabelsChanged, this, &GeoFenceController::paramLabelsChanged);
connect(geoFenceManager, &GeoFenceManager::circleRadiusChanged, this, &GeoFenceController::circleRadiusChanged); connect(geoFenceManager, &GeoFenceManager::loadComplete, this, &GeoFenceController::_loadComplete);
connect(geoFenceManager, &GeoFenceManager::paramsChanged, this, &GeoFenceController::paramsChanged); connect(geoFenceManager, &GeoFenceManager::inProgressChanged, this, &GeoFenceController::syncInProgressChanged);
connect(geoFenceManager, &GeoFenceManager::paramLabelsChanged, this, &GeoFenceController::paramLabelsChanged);
connect(geoFenceManager, &GeoFenceManager::loadComplete, this, &GeoFenceController::_loadComplete);
connect(geoFenceManager, &GeoFenceManager::inProgressChanged, this, &GeoFenceController::syncInProgressChanged);
if (!geoFenceManager->inProgress()) { if (!geoFenceManager->inProgress()) {
_loadComplete(geoFenceManager->breachReturnPoint(), geoFenceManager->polygon()); _loadComplete(geoFenceManager->breachReturnPoint(), geoFenceManager->polygon());
...@@ -136,7 +131,7 @@ bool GeoFenceController::_loadJsonFile(QJsonDocument& jsonDoc, QString& errorStr ...@@ -136,7 +131,7 @@ bool GeoFenceController::_loadJsonFile(QJsonDocument& jsonDoc, QString& errorStr
return false; return false;
} }
if (breachReturnSupported()) { if (breachReturnEnabled()) {
if (json.contains(_jsonBreachReturnKey) if (json.contains(_jsonBreachReturnKey)
&& !JsonHelper::loadGeoCoordinate(json[_jsonBreachReturnKey], false /* altitudeRequired */, _breachReturnPoint, errorString)) { && !JsonHelper::loadGeoCoordinate(json[_jsonBreachReturnKey], false /* altitudeRequired */, _breachReturnPoint, errorString)) {
return false; return false;
...@@ -145,7 +140,7 @@ bool GeoFenceController::_loadJsonFile(QJsonDocument& jsonDoc, QString& errorStr ...@@ -145,7 +140,7 @@ bool GeoFenceController::_loadJsonFile(QJsonDocument& jsonDoc, QString& errorStr
_breachReturnPoint = QGeoCoordinate(); _breachReturnPoint = QGeoCoordinate();
} }
if (polygonSupported()) { if (polygonEnabled()) {
if (!_polygon.loadFromJson(json, false /* reauired */, errorString)) { if (!_polygon.loadFromJson(json, false /* reauired */, errorString)) {
return false; return false;
} }
...@@ -289,13 +284,13 @@ void GeoFenceController::saveToFile(const QString& filename) ...@@ -289,13 +284,13 @@ void GeoFenceController::saveToFile(const QString& filename)
paramMgr->saveToJson(paramMgr->defaultComponentId(), paramNames, fenceFileObject); paramMgr->saveToJson(paramMgr->defaultComponentId(), paramNames, fenceFileObject);
} }
if (breachReturnSupported()) { if (breachReturnEnabled()) {
QJsonValue jsonBreachReturn; QJsonValue jsonBreachReturn;
JsonHelper::saveGeoCoordinate(_breachReturnPoint, false /* writeAltitude */, jsonBreachReturn); JsonHelper::saveGeoCoordinate(_breachReturnPoint, false /* writeAltitude */, jsonBreachReturn);
fenceFileObject[_jsonBreachReturnKey] = jsonBreachReturn; fenceFileObject[_jsonBreachReturnKey] = jsonBreachReturn;
} }
if (polygonSupported()) { if (polygonEnabled()) {
_polygon.saveToJson(fenceFileObject); _polygon.saveToJson(fenceFileObject);
} }
...@@ -373,29 +368,19 @@ void GeoFenceController::_polygonDirtyChanged(bool dirty) ...@@ -373,29 +368,19 @@ void GeoFenceController::_polygonDirtyChanged(bool dirty)
} }
} }
bool GeoFenceController::fenceSupported(void) const bool GeoFenceController::circleEnabled(void) const
{ {
return _activeVehicle->geoFenceManager()->fenceSupported(); return _activeVehicle->geoFenceManager()->circleEnabled();
} }
bool GeoFenceController::fenceEnabled(void) const bool GeoFenceController::polygonEnabled(void) const
{ {
return _activeVehicle->geoFenceManager()->fenceEnabled(); return _activeVehicle->geoFenceManager()->polygonEnabled();
} }
bool GeoFenceController::circleSupported(void) const bool GeoFenceController::breachReturnEnabled(void) const
{ {
return _activeVehicle->geoFenceManager()->circleSupported(); return _activeVehicle->geoFenceManager()->breachReturnEnabled();
}
bool GeoFenceController::polygonSupported(void) const
{
return _activeVehicle->geoFenceManager()->polygonSupported();
}
bool GeoFenceController::breachReturnSupported(void) const
{
return _activeVehicle->geoFenceManager()->breachReturnSupported();
} }
void GeoFenceController::_setDirty(void) void GeoFenceController::_setDirty(void)
......
...@@ -29,17 +29,26 @@ public: ...@@ -29,17 +29,26 @@ public:
GeoFenceController(QObject* parent = NULL); GeoFenceController(QObject* parent = NULL);
~GeoFenceController(); ~GeoFenceController();
Q_PROPERTY(bool circleEnabled READ circleEnabled NOTIFY circleEnabledChanged)
Q_PROPERTY(float circleRadius READ circleRadius NOTIFY circleRadiusChanged)
Q_PROPERTY(bool polygonEnabled READ polygonEnabled NOTIFY polygonEnabledChanged)
Q_PROPERTY(QGCMapPolygon* polygon READ polygon CONSTANT)
Q_PROPERTY(bool breachReturnEnabled READ breachReturnEnabled NOTIFY breachReturnEnabledChanged)
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)
Q_PROPERTY(QString editorQml READ editorQml NOTIFY editorQmlChanged)
#if 0
Q_PROPERTY(bool fenceSupported READ fenceSupported NOTIFY fenceSupportedChanged) Q_PROPERTY(bool fenceSupported READ fenceSupported NOTIFY fenceSupportedChanged)
Q_PROPERTY(bool fenceEnabled READ fenceEnabled NOTIFY fenceEnabledChanged) Q_PROPERTY(bool fenceEnabled READ fenceEnabled NOTIFY fenceEnabledChanged)
Q_PROPERTY(bool circleSupported READ circleSupported NOTIFY circleSupportedChanged) Q_PROPERTY(bool circleSupported READ circleSupported NOTIFY circleSupportedChanged)
Q_PROPERTY(bool polygonSupported READ polygonSupported NOTIFY polygonSupportedChanged) Q_PROPERTY(bool polygonSupported READ polygonSupported NOTIFY polygonSupportedChanged)
Q_PROPERTY(bool breachReturnSupported READ breachReturnSupported NOTIFY breachReturnSupportedChanged) Q_PROPERTY(bool breachReturnSupported READ breachReturnSupported NOTIFY breachReturnSupportedChanged)
Q_PROPERTY(float circleRadius READ circleRadius NOTIFY circleRadiusChanged) #endif
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)
Q_PROPERTY(QString editorQml READ editorQml NOTIFY editorQmlChanged)
void start (bool editMode) final; void start (bool editMode) final;
void loadFromVehicle (void) final; void loadFromVehicle (void) final;
...@@ -55,33 +64,29 @@ public: ...@@ -55,33 +64,29 @@ public:
QString fileExtension(void) const final; QString fileExtension(void) const final;
bool fenceSupported (void) const; bool circleEnabled (void) const;
bool fenceEnabled (void) const; bool polygonEnabled (void) const;
bool circleSupported (void) const; bool breachReturnEnabled (void) const;
bool polygonSupported (void) const; float circleRadius (void) const;
bool breachReturnSupported (void) const; QGCMapPolygon* polygon (void) { return &_polygon; }
float circleRadius (void) const; QGeoCoordinate breachReturnPoint (void) const { return _breachReturnPoint; }
QGCMapPolygon* polygon (void) { return &_polygon; } QVariantList params (void) const;
QGeoCoordinate breachReturnPoint (void) const { return _breachReturnPoint; } QStringList paramLabels (void) const;
QVariantList params (void) const; QString editorQml (void) const;
QStringList paramLabels (void) const;
QString editorQml (void) const;
void setBreachReturnPoint(const QGeoCoordinate& breachReturnPoint); void setBreachReturnPoint(const QGeoCoordinate& breachReturnPoint);
signals: signals:
void fenceSupportedChanged (bool fenceSupported); void circleEnabledChanged (bool circleEnabled);
void fenceEnabledChanged (bool fenceEnabled); void polygonEnabledChanged (bool polygonEnabled);
void circleSupportedChanged (bool circleSupported); void breachReturnEnabledChanged (bool breachReturnEnabled);
void polygonSupportedChanged (bool polygonSupported); void circleRadiusChanged (float circleRadius);
void breachReturnSupportedChanged (bool breachReturnSupported); void polygonPathChanged (const QVariantList& polygonPath);
void circleRadiusChanged (float circleRadius); void breachReturnPointChanged (QGeoCoordinate breachReturnPoint);
void polygonPathChanged (const QVariantList& polygonPath); void paramsChanged (QVariantList params);
void breachReturnPointChanged (QGeoCoordinate breachReturnPoint); void paramLabelsChanged (QStringList paramLabels);
void paramsChanged (QVariantList params); void editorQmlChanged (QString editorQml);
void paramLabelsChanged (QStringList paramLabels); void loadComplete (void);
void editorQmlChanged (QString editorQml);
void loadComplete (void);
private slots: private slots:
void _polygonDirtyChanged(bool dirty); void _polygonDirtyChanged(bool dirty);
......
...@@ -38,12 +38,9 @@ public: ...@@ -38,12 +38,9 @@ public:
/// Send the current settings to the vehicle /// Send the current settings to the vehicle
virtual void sendToVehicle(const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon); virtual void sendToVehicle(const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon);
// Support flags virtual bool circleEnabled (void) const { return false; }
virtual bool fenceSupported (void) const { return false; } virtual bool polygonEnabled (void) const { return false; }
virtual bool fenceEnabled (void) const { return false; } virtual bool breachReturnEnabled (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; }
virtual float circleRadius (void) const { return 0.0; } virtual float circleRadius (void) const { return 0.0; }
QList<QGeoCoordinate> polygon (void) const { return _polygon; } QList<QGeoCoordinate> polygon (void) const { return _polygon; }
...@@ -63,17 +60,15 @@ public: ...@@ -63,17 +60,15 @@ public:
} ErrorCode_t; } ErrorCode_t;
signals: signals:
void loadComplete (const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon); void loadComplete (const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon);
void fenceSupportedChanged (bool fenceSupported); void circleEnabledChanged (bool circleEnabled);
void fenceEnabledChanged (bool fenceEnabled); void polygonEnabledChanged (bool polygonEnabled);
void circleSupportedChanged (bool circleSupported); void breachReturnEnabledChanged (bool fenceEnabled);
void polygonSupportedChanged (bool polygonSupported); void circleRadiusChanged (float circleRadius);
void breachReturnSupportedChanged (bool fenceSupported); void inProgressChanged (bool inProgress);
void circleRadiusChanged (float circleRadius); void error (int errorCode, const QString& errorMsg);
void inProgressChanged (bool inProgress); void paramsChanged (QVariantList params);
void error (int errorCode, const QString& errorMsg); void paramLabelsChanged (QStringList paramLabels);
void paramsChanged (QVariantList params);
void paramLabelsChanged (QStringList paramLabels);
protected: protected:
void _sendError(ErrorCode_t errorCode, const QString& errorMsg); void _sendError(ErrorCode_t errorCode, const QString& errorMsg);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment