diff --git a/src/FirmwarePlugin/APM/APMGeoFenceManager.cc b/src/FirmwarePlugin/APM/APMGeoFenceManager.cc index 7b019f73be3030872af4faa5ec65ab6b2a4a1c7b..0590c11efe3b909ccd7660cfd6f8c40abcbce6ae 100644 --- a/src/FirmwarePlugin/APM/APMGeoFenceManager.cc +++ b/src/FirmwarePlugin/APM/APMGeoFenceManager.cc @@ -22,11 +22,12 @@ APMGeoFenceManager::APMGeoFenceManager(Vehicle* vehicle) : GeoFenceManager(vehicle) , _fenceSupported(false) , _breachReturnSupported(vehicle->fixedWing()) + , _circleSupported(false) + , _polygonSupported(false) , _firstParamLoadComplete(false) , _circleRadiusFact(NULL) , _readTransactionInProgress(false) , _writeTransactionInProgress(false) - , _fenceEnableFact(NULL) , _fenceTypeFact(NULL) { connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &APMGeoFenceManager::_mavlinkMessageReceived); @@ -80,7 +81,7 @@ void APMGeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, const fenceEnableFact->setRawValue(0); // Total point count, +1 polygon close in last index, +1 for breach in index 0 - _cWriteFencePoints = (validatedPolygonCount ? (validatedPolygonCount + 1) : 0) + 1 ; + _cWriteFencePoints = validatedPolygonCount ? validatedPolygonCount + 1 + 1 : 0; _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceTotalParam)->setRawValue(_cWriteFencePoints); // FIXME: No validation of correct fence received @@ -106,11 +107,11 @@ void APMGeoFenceManager::loadFromVehicle(void) return; } - // Point 0: Breach return point (ArduPlane only) + // Point 0: Breach return point (always sent, but supported by ArduPlane only) // Point [1,N]: Polygon points // Point N+1: Close polygon point (same as point 1) int cFencePoints = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceTotalParam)->rawValue().toInt(); - int minFencePoints = 6; + int minFencePoints = 5; qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::loadFromVehicle" << cFencePoints; if (cFencePoints == 0) { // No fence @@ -242,8 +243,19 @@ bool APMGeoFenceManager::_geoFenceSupported(void) void APMGeoFenceManager::_updateSupportedFlags(void) { - emit circleSupportedChanged(circleSupported()); - emit polygonSupportedChanged(polygonSupported()); + bool newCircleSupported = _fenceSupported && _vehicle->multiRotor() && _fenceTypeFact && (_fenceTypeFact->rawValue().toInt() & 2); + if (newCircleSupported != _circleSupported) { + _circleSupported = newCircleSupported; + emit circleSupportedChanged(newCircleSupported); + } + + bool newPolygonSupported = _fenceSupported && + ((_vehicle->multiRotor() && _fenceTypeFact && (_fenceTypeFact->rawValue().toInt() & 4)) || + _vehicle->fixedWing()); + if (newPolygonSupported != _polygonSupported) { + _polygonSupported = newPolygonSupported; + emit polygonSupportedChanged(newPolygonSupported); + } } void APMGeoFenceManager::_parametersReady(void) @@ -258,11 +270,9 @@ void APMGeoFenceManager::_parametersReady(void) QStringList paramLabels; if (_vehicle->multiRotor()) { - _fenceEnableFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("FENCE_ENABLE")); _fenceTypeFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("FENCE_TYPE")); - connect(_fenceEnableFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_updateSupportedFlags); - connect(_fenceTypeFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_updateSupportedFlags); + connect(_fenceTypeFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_updateSupportedFlags); _circleRadiusFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("FENCE_RADIUS")); connect(_circleRadiusFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_circleRadiusRawValueChanged); @@ -293,8 +303,7 @@ void APMGeoFenceManager::_parametersReady(void) emit paramLabelsChanged(_paramLabels); emit fenceSupportedChanged(_fenceSupported); - emit circleSupportedChanged(circleSupported()); - emit polygonSupportedChanged(polygonSupported()); + _updateSupportedFlags(); } qCDebug(GeoFenceManagerLog) << "fenceSupported:circleSupported:polygonSupported:breachReturnSupported" << @@ -318,26 +327,12 @@ void APMGeoFenceManager::_circleRadiusRawValueChanged(QVariant value) bool APMGeoFenceManager::circleSupported(void) const { - if (_fenceSupported && _vehicle->multiRotor() && _fenceEnableFact && _fenceTypeFact) { - return _fenceEnableFact->rawValue().toBool() && (_fenceTypeFact->rawValue().toInt() & 2); - } - - return false; + return _circleSupported; } 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; + return _polygonSupported; } QString APMGeoFenceManager::editorQml(void) const diff --git a/src/FirmwarePlugin/APM/APMGeoFenceManager.h b/src/FirmwarePlugin/APM/APMGeoFenceManager.h index fe98aaeb93c00395b597910178aa774869c57ea7..33fe15912e4ae8ae773d3766d4174c9b732d0edd 100644 --- a/src/FirmwarePlugin/APM/APMGeoFenceManager.h +++ b/src/FirmwarePlugin/APM/APMGeoFenceManager.h @@ -49,6 +49,8 @@ private: private: bool _fenceSupported; bool _breachReturnSupported; + bool _circleSupported; + bool _polygonSupported; bool _firstParamLoadComplete; QVariantList _params; @@ -63,7 +65,6 @@ private: uint8_t _cWriteFencePoints; uint8_t _currentFencePoint; - Fact* _fenceEnableFact; Fact* _fenceTypeFact; static const char* _fenceTotalParam; diff --git a/src/FirmwarePlugin/APM/APMRallyPointManager.cc b/src/FirmwarePlugin/APM/APMRallyPointManager.cc index 8061f8e57433ae94f08aa7beb136568537934217..76eb2a8769b108b62e81ac36b5b4c40a4fec3205 100644 --- a/src/FirmwarePlugin/APM/APMRallyPointManager.cc +++ b/src/FirmwarePlugin/APM/APMRallyPointManager.cc @@ -90,7 +90,7 @@ void APMRallyPointManager::_mavlinkMessageReceived(const mavlink_message_t& mess QGeoCoordinate point((float)rallyPoint.lat / 1e7, (float)rallyPoint.lng / 1e7, rallyPoint.alt); _rgPoints.append(point); - if (rallyPoint.idx < _cReadRallyPoints - 2) { + if (rallyPoint.idx < _cReadRallyPoints - 1) { // Still more points to request _requestRallyPoint(++_currentRallyPoint); } else { diff --git a/src/MissionEditor/MissionEditor.qml b/src/MissionEditor/MissionEditor.qml index 34dea1a2fb5116657a4e02dd4fedb6b359b502ba..550cf2461a5698337c4ac3bc2208e9af3a585cc1 100644 --- a/src/MissionEditor/MissionEditor.qml +++ b/src/MissionEditor/MissionEditor.qml @@ -86,31 +86,114 @@ QGCView { return lon + 180.0 } - /// Fix the map viewport to the current mission items. - function fitViewportToMissionItems() { - if (_visualItems.count == 1) { + /// Fits the visible region of the map to inclues all of the specified coordinates. If no coordinates + /// are specified the map will fit to the home position + function fitMapViewportToAllCoordinates(coordList) { + if (coordList.length == 0) { editorMap.center = _visualItems.get(0).coordinate - } else { - var missionItem = _visualItems.get(0) - var north = normalizeLat(missionItem.coordinate.latitude) - var south = north - var east = normalizeLon(missionItem.coordinate.longitude) - var west = east - - for (var i=1; i<_visualItems.count; i++) { - missionItem = _visualItems.get(i) - - if (missionItem.specifiesCoordinate && !missionItem.isStandaloneCoordinate) { - var lat = normalizeLat(missionItem.coordinate.latitude) - var lon = normalizeLon(missionItem.coordinate.longitude) - - north = Math.max(north, lat) - south = Math.min(south, lat) - east = Math.max(east, lon) - west = Math.min(west, lon) - } + return + } + + // Determine the size of the inner portion of the map available for display + var toolbarHeight = qgcView.height - ScreenTools.availableHeight + var rightPanelWidth = _rightPanelWidth + var leftToolWidth = centerMapButton.x + centerMapButton.width + var availableWidth = qgcView.width - rightPanelWidth - leftToolWidth + var availableHeight = qgcView.height - toolbarHeight + + // Create the normalized lat/lon corners for the coordinate bounding rect from the list of coordinates + var north = normalizeLat(coordList[0].latitude) + var south = north + var east = normalizeLon(coordList[0].longitude) + var west = east + for (var i=1; i 2) { + for (var i=0; iparameterManager()->parametersReady() && !syncInProgress()) { - setDirty(false); - _polygon.setDirty(false); _activeVehicle->geoFenceManager()->sendToVehicle(_breachReturnPoint, _polygon.coordinateList()); + _polygon.setDirty(false); + setDirty(false); } else { qCWarning(GeoFenceControllerLog) << "GeoFenceController::loadFromVehicle call at wrong time" << _activeVehicle->parameterManager()->parametersReady() << syncInProgress(); } @@ -433,6 +433,7 @@ void GeoFenceController::_loadComplete(const QGeoCoordinate& breachReturn, const _setReturnPointFromManager(breachReturn); _setPolygonFromManager(polygon); setDirty(false); + emit loadComplete(); } QString GeoFenceController::fileExtension(void) const diff --git a/src/MissionManager/GeoFenceController.h b/src/MissionManager/GeoFenceController.h index d5f14cf3f970a6c786034693a2816237588d3914..654b933c642f57dc740f2c94c9828d461c7dbaca 100644 --- a/src/MissionManager/GeoFenceController.h +++ b/src/MissionManager/GeoFenceController.h @@ -78,6 +78,7 @@ signals: void paramsChanged (QVariantList params); void paramLabelsChanged (QStringList paramLabels); void editorQmlChanged (QString editorQml); + void loadComplete (void); private slots: void _polygonDirtyChanged(bool dirty); diff --git a/src/MissionManager/GeoFenceManager.cc b/src/MissionManager/GeoFenceManager.cc index 24d8d55347557bcc9df290a8c7fa3e84cc8d7807..331cfe5eb237a0ba553edf3e8d0ff83883f101ae 100644 --- a/src/MissionManager/GeoFenceManager.cc +++ b/src/MissionManager/GeoFenceManager.cc @@ -34,7 +34,7 @@ void GeoFenceManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg) void GeoFenceManager::loadFromVehicle(void) { // No geofence support in unknown vehicle - loadComplete(QGeoCoordinate(), QList()); + emit loadComplete(QGeoCoordinate(), QList()); } void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, const QList& polygon) diff --git a/src/MissionManager/RallyPointController.cc b/src/MissionManager/RallyPointController.cc index e924c2531f7414f671e875dabef395cea5bba263..0b3bfeb0196be76c08358e8cd2f200650acc3e62 100644 --- a/src/MissionManager/RallyPointController.cc +++ b/src/MissionManager/RallyPointController.cc @@ -254,6 +254,7 @@ void RallyPointController::_loadComplete(const QList rgPoints) _points.swapObjectList(pointList); setDirty(false); _setFirstPointCurrent(); + emit loadComplete(); } QString RallyPointController::fileExtension(void) const diff --git a/src/MissionManager/RallyPointController.h b/src/MissionManager/RallyPointController.h index 7ac1af8be575a9edb11b66bbf04e0ee68a3d7b2c..4585d93e93a31ee93605aa58901586ad5e446436 100644 --- a/src/MissionManager/RallyPointController.h +++ b/src/MissionManager/RallyPointController.h @@ -61,6 +61,7 @@ public: signals: void rallyPointsSupportedChanged(bool rallyPointsSupported); void currentRallyPointChanged(QObject* rallyPoint); + void loadComplete(void); private slots: void _loadComplete(const QList rgPoints); diff --git a/src/MissionManager/RallyPointManager.cc b/src/MissionManager/RallyPointManager.cc index 2698fc5df512cb9fb20de36a17ef3374df7e0b45..ba92da6a14bf18a6a63dabe71f76be0ef3b7b2dd 100644 --- a/src/MissionManager/RallyPointManager.cc +++ b/src/MissionManager/RallyPointManager.cc @@ -34,7 +34,7 @@ void RallyPointManager::_sendError(ErrorCode_t errorCode, const QString& errorMs void RallyPointManager::loadFromVehicle(void) { // No support in generic vehicle - loadComplete(QList()); + emit loadComplete(QList()); } void RallyPointManager::sendToVehicle(const QList& rgPoints)