diff --git a/src/MissionManager/ComplexMissionItem.cc b/src/MissionManager/ComplexMissionItem.cc index bd753a3..2b22a3e 100644 --- a/src/MissionManager/ComplexMissionItem.cc +++ b/src/MissionManager/ComplexMissionItem.cc @@ -124,9 +124,9 @@ void ComplexMissionItem::addKMLVisuals(KMLPlanDomDocument& /* domDocument */) // Default implementation has no visuals } -void ComplexMissionItem::_appendFlightPathSegment(const QGeoCoordinate& coord1, double coord1AMSLAlt, const QGeoCoordinate& coord2, double coord2AMSLAlt) +void ComplexMissionItem::_appendFlightPathSegment(FlightPathSegment::SegmentType segmentType, const QGeoCoordinate& coord1, double coord1AMSLAlt, const QGeoCoordinate& coord2, double coord2AMSLAlt) { - FlightPathSegment* segment = new FlightPathSegment(coord1, coord1AMSLAlt, coord2, coord2AMSLAlt, true /* queryTerrainData */, this /* parent */); + FlightPathSegment* segment = new FlightPathSegment(segmentType, coord1, coord1AMSLAlt, coord2, coord2AMSLAlt, true /* queryTerrainData */, this /* parent */); connect(segment, &FlightPathSegment::terrainCollisionChanged, this, &ComplexMissionItem::_segmentTerrainCollisionChanged); connect(segment, &FlightPathSegment::terrainCollisionChanged, _missionController, &MissionController::recalcTerrainProfile, Qt::QueuedConnection); diff --git a/src/MissionManager/ComplexMissionItem.h b/src/MissionManager/ComplexMissionItem.h index 02cfdff..4ab762e 100644 --- a/src/MissionManager/ComplexMissionItem.h +++ b/src/MissionManager/ComplexMissionItem.h @@ -15,6 +15,7 @@ #include "SettingsManager.h" #include "KMLPlanDomDocument.h" #include "QmlObjectListModel.h" +#include "FlightPathSegment.h" #include @@ -117,7 +118,7 @@ protected slots: protected: void _savePresetJson (const QString& name, QJsonObject& presetObject); QJsonObject _loadPresetJson (const QString& name); - void _appendFlightPathSegment(const QGeoCoordinate& coord1, double coord1AMSLAlt, const QGeoCoordinate& coord2, double coord2AMSLAlt); + void _appendFlightPathSegment(FlightPathSegment::SegmentType segmentType, const QGeoCoordinate& coord1, double coord1AMSLAlt, const QGeoCoordinate& coord2, double coord2AMSLAlt); bool _isIncomplete = true; int _cTerrainCollisionSegments = 0; diff --git a/src/MissionManager/FixedWingLandingComplexItem.cc b/src/MissionManager/FixedWingLandingComplexItem.cc index e6d3bc0..7be0cd4 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.cc +++ b/src/MissionManager/FixedWingLandingComplexItem.cc @@ -170,10 +170,10 @@ void FixedWingLandingComplexItem::_updateFlightPathSegmentsDontCallDirectly(void _flightPathSegments.beginReset(); _flightPathSegments.clearAndDeleteContents(); if (useLoiterToAlt()->rawValue().toBool()) { - _appendFlightPathSegment(finalApproachCoordinate(), amslEntryAlt(), loiterTangentCoordinate(), amslEntryAlt()); // Best we can do to simulate loiter circle terrain profile - _appendFlightPathSegment(loiterTangentCoordinate(), amslEntryAlt(), landingCoordinate(), amslExitAlt()); + _appendFlightPathSegment(FlightPathSegment::SegmentTypeGeneric, finalApproachCoordinate(), amslEntryAlt(), loiterTangentCoordinate(), amslEntryAlt()); // Best we can do to simulate loiter circle terrain profile + _appendFlightPathSegment(FlightPathSegment::SegmentTypeLand, loiterTangentCoordinate(), amslEntryAlt(), landingCoordinate(), amslExitAlt()); } else { - _appendFlightPathSegment(finalApproachCoordinate(), amslEntryAlt(), landingCoordinate(), amslExitAlt()); + _appendFlightPathSegment(FlightPathSegment::SegmentTypeLand, finalApproachCoordinate(), amslEntryAlt(), landingCoordinate(), amslExitAlt()); } _flightPathSegments.endReset(); diff --git a/src/MissionManager/LandingComplexItemTest.cc b/src/MissionManager/LandingComplexItemTest.cc index 59f16d1..44176cc 100644 --- a/src/MissionManager/LandingComplexItemTest.cc +++ b/src/MissionManager/LandingComplexItemTest.cc @@ -386,10 +386,10 @@ void SimpleLandingComplexItem::_updateFlightPathSegmentsDontCallDirectly(void) _flightPathSegments.beginReset(); _flightPathSegments.clearAndDeleteContents(); if (useLoiterToAlt()->rawValue().toBool()) { - _appendFlightPathSegment(finalApproachCoordinate(), amslEntryAlt(), loiterTangentCoordinate(), amslEntryAlt()); // Best we can do to simulate loiter circle terrain profile - _appendFlightPathSegment(loiterTangentCoordinate(), amslEntryAlt(), landingCoordinate(), amslExitAlt()); + _appendFlightPathSegment(FlightPathSegment::SegmentTypeGeneric, finalApproachCoordinate(), amslEntryAlt(), loiterTangentCoordinate(), amslEntryAlt()); // Best we can do to simulate loiter circle terrain profile + _appendFlightPathSegment(FlightPathSegment::SegmentTypeLand, loiterTangentCoordinate(), amslEntryAlt(), landingCoordinate(), amslExitAlt()); } else { - _appendFlightPathSegment(finalApproachCoordinate(), amslEntryAlt(), landingCoordinate(), amslExitAlt()); + _appendFlightPathSegment(FlightPathSegment::SegmentTypeLand, finalApproachCoordinate(), amslEntryAlt(), landingCoordinate(), amslExitAlt()); } _flightPathSegments.endReset(); diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 78f1518..f53ad84 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -1163,7 +1163,14 @@ FlightPathSegment* MissionController::_createFlightPathSegmentWorker(VisualItemP double coord2AMSLAlt = pair.second->amslEntryAlt(); double coord1AMSLAlt = takeoffStraightUp ? coord2AMSLAlt : pair.first->amslExitAlt(); - FlightPathSegment* segment = new FlightPathSegment(coord1, coord1AMSLAlt, coord2, coord2AMSLAlt, !_flyView /* queryTerrainData */, this); + FlightPathSegment::SegmentType segmentType = FlightPathSegment::SegmentTypeGeneric; + if (pair.second->isTakeoffItem()) { + segmentType = FlightPathSegment::SegmentTypeTakeoff; + } else if (pair.second->isLandCommand()) { + segmentType = FlightPathSegment::SegmentTypeLand; + } + + FlightPathSegment* segment = new FlightPathSegment(segmentType, coord1, coord1AMSLAlt, coord2, coord2AMSLAlt, !_flyView /* queryTerrainData */, this); if (takeoffStraightUp) { connect(pair.second, &VisualMissionItem::amslEntryAltChanged, segment, &FlightPathSegment::setCoord1AMSLAlt); @@ -1391,13 +1398,15 @@ void MissionController::_recalcFlightPathSegments(void) // Pair already exists in old table, pull from old to new and reuse _flightPathSegmentHashTable[lastSegmentVisualItemPair] = coordVector = oldSegmentTable.take(lastSegmentVisualItemPair); } else { - // Create a new segment. Since this is the fly view there is no need to wire change signals. - coordVector = new FlightPathSegment(lastSegmentVisualItemPair.first->isSimpleItem() ? lastSegmentVisualItemPair.first->coordinate() : lastSegmentVisualItemPair.first->exitCoordinate(), - lastSegmentVisualItemPair.first->isSimpleItem() ? lastSegmentVisualItemPair.first->amslEntryAlt() : lastSegmentVisualItemPair.first->amslExitAlt(), - lastSegmentVisualItemPair.second->coordinate(), - lastSegmentVisualItemPair.second->amslEntryAlt(), - !_flyView /* queryTerrainData */, - this); + // Create a new segment. Since this is the fly view there is no need to wire change signals or worry about correct SegmentType + coordVector = new FlightPathSegment( + FlightPathSegment::SegmentTypeGeneric, + lastSegmentVisualItemPair.first->isSimpleItem() ? lastSegmentVisualItemPair.first->coordinate() : lastSegmentVisualItemPair.first->exitCoordinate(), + lastSegmentVisualItemPair.first->isSimpleItem() ? lastSegmentVisualItemPair.first->amslEntryAlt() : lastSegmentVisualItemPair.first->amslExitAlt(), + lastSegmentVisualItemPair.second->coordinate(), + lastSegmentVisualItemPair.second->amslEntryAlt(), + !_flyView /* queryTerrainData */, + this); _flightPathSegmentHashTable[lastSegmentVisualItemPair] = coordVector; } diff --git a/src/MissionManager/StructureScanComplexItem.cc b/src/MissionManager/StructureScanComplexItem.cc index 34c4e11..dde31ad 100644 --- a/src/MissionManager/StructureScanComplexItem.cc +++ b/src/MissionManager/StructureScanComplexItem.cc @@ -712,23 +712,23 @@ void StructureScanComplexItem::_updateFlightPathSegmentsDontCallDirectly(void) // Entrance to first layer entrance double entranceAlt = _entranceAltFact.rawValue().toDouble() + _missionController->plannedHomePosition().altitude(); - _appendFlightPathSegment(layerEntranceCoord, entranceAlt, layerEntranceCoord, layerAltitude); + _appendFlightPathSegment(FlightPathSegment::SegmentTypeGeneric, layerEntranceCoord, entranceAlt, layerEntranceCoord, layerAltitude); // Segments for each layer for (int layerIndex=0; layerIndex<_layersFact.rawValue().toInt(); layerIndex++) { // Move from one layer to the next if (layerIndex != 0) { - _appendFlightPathSegment(layerEntranceCoord, prevLayerAltitude, layerEntranceCoord, layerAltitude); + _appendFlightPathSegment(FlightPathSegment::SegmentTypeGeneric, layerEntranceCoord, prevLayerAltitude, layerEntranceCoord, layerAltitude); } QGeoCoordinate prevCoord = QGeoCoordinate(); for (const QGeoCoordinate& coord: _flightPolygon.coordinateList()) { if (prevCoord.isValid()) { - _appendFlightPathSegment(prevCoord, layerAltitude, coord, layerAltitude); + _appendFlightPathSegment(FlightPathSegment::SegmentTypeGeneric, prevCoord, layerAltitude, coord, layerAltitude); } prevCoord = coord; } - _appendFlightPathSegment(_flightPolygon.coordinateList().last(), layerAltitude, _flightPolygon.coordinateList().first(), layerAltitude); + _appendFlightPathSegment(FlightPathSegment::SegmentTypeGeneric, _flightPolygon.coordinateList().last(), layerAltitude, _flightPolygon.coordinateList().first(), layerAltitude); // Move to next layer altitude prevLayerAltitude = layerAltitude; @@ -740,7 +740,7 @@ void StructureScanComplexItem::_updateFlightPathSegmentsDontCallDirectly(void) } // Last layer exit back to entrance - _appendFlightPathSegment(layerEntranceCoord, prevLayerAltitude, layerEntranceCoord, entranceAlt); + _appendFlightPathSegment(FlightPathSegment::SegmentTypeGeneric, layerEntranceCoord, prevLayerAltitude, layerEntranceCoord, entranceAlt); } _flightPathSegments.endReset(); diff --git a/src/MissionManager/TransectStyleComplexItem.cc b/src/MissionManager/TransectStyleComplexItem.cc index 774f32f..b54ee3a 100644 --- a/src/MissionManager/TransectStyleComplexItem.cc +++ b/src/MissionManager/TransectStyleComplexItem.cc @@ -490,7 +490,7 @@ void TransectStyleComplexItem::_updateFlightPathSegmentsDontCallDirectly(void) for (const MissionItem* missionItem: _loadedMissionItems) { if (missionItem->command() == MAV_CMD_NAV_WAYPOINT || missionItem->command() == MAV_CMD_CONDITION_GATE) { if (prevCoord.isValid()) { - _appendFlightPathSegment(prevCoord, prevAlt, missionItem->coordinate(), missionItem->param7()); + _appendFlightPathSegment(FlightPathSegment::SegmentTypeGeneric, prevCoord, prevAlt, missionItem->coordinate(), missionItem->param7()); } prevCoord = missionItem->coordinate(); prevAlt = missionItem->param7(); @@ -505,7 +505,7 @@ void TransectStyleComplexItem::_updateFlightPathSegmentsDontCallDirectly(void) const QGeoCoordinate& fromCoord = _rgFlightPathCoordInfo[i].coord; const QGeoCoordinate& toCoord = _rgFlightPathCoordInfo[i+1].coord; //qDebug() << _rgFlightPathCoordInfo.count() << fromCoord << _rgFlightPathCoordInfo[i].coordType << toCoord << _rgFlightPathCoordInfo[i+1].coordType; - _appendFlightPathSegment(fromCoord, fromCoord.altitude(), toCoord, toCoord.altitude()); + _appendFlightPathSegment(FlightPathSegment::SegmentTypeGeneric, fromCoord, fromCoord.altitude(), toCoord, toCoord.altitude()); } } } @@ -518,7 +518,7 @@ void TransectStyleComplexItem::_updateFlightPathSegmentsDontCallDirectly(void) for (const QVariant& varCoord: _visualTransectPoints) { QGeoCoordinate thisCoord = varCoord.value(); if (prevCoord.isValid()) { - _appendFlightPathSegment(prevCoord, surveyAlt, thisCoord, surveyAlt); + _appendFlightPathSegment(FlightPathSegment::SegmentTypeGeneric, prevCoord, surveyAlt, thisCoord, surveyAlt); } prevCoord = thisCoord; } diff --git a/src/MissionManager/VTOLLandingComplexItem.cc b/src/MissionManager/VTOLLandingComplexItem.cc index 8337279..5063efd 100644 --- a/src/MissionManager/VTOLLandingComplexItem.cc +++ b/src/MissionManager/VTOLLandingComplexItem.cc @@ -133,12 +133,12 @@ void VTOLLandingComplexItem::_updateFlightPathSegmentsDontCallDirectly(void) _flightPathSegments.beginReset(); _flightPathSegments.clearAndDeleteContents(); if (useLoiterToAlt()->rawValue().toBool()) { - _appendFlightPathSegment(finalApproachCoordinate(), amslEntryAlt(), loiterTangentCoordinate(), amslEntryAlt()); // Best we can do to simulate loiter circle terrain profile - _appendFlightPathSegment(loiterTangentCoordinate(), amslEntryAlt(), landingCoordinate(), amslEntryAlt()); + _appendFlightPathSegment(FlightPathSegment::SegmentTypeGeneric, finalApproachCoordinate(), amslEntryAlt(), loiterTangentCoordinate(), amslEntryAlt()); // Best we can do to simulate loiter circle terrain profile + _appendFlightPathSegment(FlightPathSegment::SegmentTypeGeneric, loiterTangentCoordinate(), amslEntryAlt(), landingCoordinate(), amslEntryAlt()); } else { - _appendFlightPathSegment(finalApproachCoordinate(), amslEntryAlt(), landingCoordinate(), amslEntryAlt()); + _appendFlightPathSegment(FlightPathSegment::SegmentTypeGeneric, finalApproachCoordinate(), amslEntryAlt(), landingCoordinate(), amslEntryAlt()); } - _appendFlightPathSegment(landingCoordinate(), amslEntryAlt(), landingCoordinate(), amslExitAlt()); + _appendFlightPathSegment(FlightPathSegment::SegmentTypeLand, landingCoordinate(), amslEntryAlt(), landingCoordinate(), amslExitAlt()); _flightPathSegments.endReset(); if (_cTerrainCollisionSegments != 0) { diff --git a/src/QmlControls/FlightPathSegment.cc b/src/QmlControls/FlightPathSegment.cc index a0bde57..69a6f34 100644 --- a/src/QmlControls/FlightPathSegment.cc +++ b/src/QmlControls/FlightPathSegment.cc @@ -12,13 +12,14 @@ QGC_LOGGING_CATEGORY(FlightPathSegmentLog, "FlightPathSegmentLog") -FlightPathSegment::FlightPathSegment(const QGeoCoordinate& coord1, double amslCoord1Alt, const QGeoCoordinate& coord2, double amslCoord2Alt, bool queryTerrainData, QObject* parent) +FlightPathSegment::FlightPathSegment(SegmentType segmentType, const QGeoCoordinate& coord1, double amslCoord1Alt, const QGeoCoordinate& coord2, double amslCoord2Alt, bool queryTerrainData, QObject* parent) : QObject (parent) , _coord1 (coord1) , _coord2 (coord2) , _coord1AMSLAlt (amslCoord1Alt) , _coord2AMSLAlt (amslCoord2Alt) , _queryTerrainData (queryTerrainData) + , _segmentType (segmentType) { _delayedTerrainPathQueryTimer.setSingleShot(true); _delayedTerrainPathQueryTimer.setInterval(200); @@ -148,11 +149,21 @@ void FlightPathSegment::_updateTerrainCollision(void) bool newTerrainCollision = false; double x = 0; for (int i=0; i<_amslTerrainHeights.count(); i++) { - double y = _amslTerrainHeights[i].value(); - if (y > (slope * x) + yIntercept) { - newTerrainCollision = true; - break; + bool ignoreCollision = false; + if (_segmentType == SegmentTypeTakeoff && x < _collisionIgnoreMeters) { + ignoreCollision = true; + } else if (_segmentType == SegmentTypeLand && x > _totalDistance - _collisionIgnoreMeters) { + ignoreCollision = true; } + + if (!ignoreCollision) { + double y = _amslTerrainHeights[i].value(); + if (y > (slope * x) + yIntercept) { + newTerrainCollision = true; + break; + } + } + if (i == _amslTerrainHeights.count() - 2) { x += _finalDistanceBetween; } else { diff --git a/src/QmlControls/FlightPathSegment.h b/src/QmlControls/FlightPathSegment.h index 1eb0797..9dfa8cf 100644 --- a/src/QmlControls/FlightPathSegment.h +++ b/src/QmlControls/FlightPathSegment.h @@ -24,7 +24,14 @@ class FlightPathSegment : public QObject Q_OBJECT public: - FlightPathSegment(const QGeoCoordinate& coord1, double coord1AMSLAlt, const QGeoCoordinate& coord2, double coord2AMSLAlt, bool queryTerrainData, QObject* parent); + enum SegmentType { + SegmentTypeTakeoff, // Takeoff segments ignore the first part of the segment for terrain collisions + SegmentTypeGeneric, // Generic segments take into account the entire segment for terrain collisions + SegmentTypeLand // Land segments ignore the last part of the segment for terrain collisions + }; + Q_ENUM(SegmentType) + + FlightPathSegment(SegmentType segmentType, const QGeoCoordinate& coord1, double coord1AMSLAlt, const QGeoCoordinate& coord2, double coord2AMSLAlt, bool queryTerrainData, QObject* parent); Q_PROPERTY(QGeoCoordinate coordinate1 MEMBER _coord1 NOTIFY coordinate1Changed) Q_PROPERTY(QGeoCoordinate coordinate2 MEMBER _coord2 NOTIFY coordinate2Changed) @@ -35,7 +42,8 @@ public: Q_PROPERTY(double distanceBetween MEMBER _distanceBetween NOTIFY distanceBetweenChanged) Q_PROPERTY(double finalDistanceBetween MEMBER _finalDistanceBetween NOTIFY finalDistanceBetweenChanged) Q_PROPERTY(double totalDistance MEMBER _totalDistance NOTIFY totalDistanceChanged) - Q_PROPERTY(bool terrainCollision MEMBER _terrainCollision NOTIFY terrainCollisionChanged); + Q_PROPERTY(bool terrainCollision MEMBER _terrainCollision NOTIFY terrainCollisionChanged) + Q_PROPERTY(SegmentType segmentType MEMBER _segmentType CONSTANT) QGeoCoordinate coordinate1 (void) const { return _coord1; } QGeoCoordinate coordinate2 (void) const { return _coord2; } @@ -47,6 +55,7 @@ public: double totalDistance (void) const { return _totalDistance; } bool specialVisual (void) const { return _specialVisual; } bool terrainCollision (void) const { return _terrainCollision; } + SegmentType segmentType (void) const { return _segmentType; } void setSpecialVisual(bool specialVisual); @@ -88,4 +97,7 @@ private: double _distanceBetween = 0; double _finalDistanceBetween = 0; double _totalDistance = 0; + SegmentType _segmentType = SegmentTypeGeneric; + + static constexpr double _collisionIgnoreMeters = 10; // Distance to ignore for takeoff/land segments };