Ignore terrain collision on beginning/end of takeoff/land segments

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);

src/MissionManager/ComplexMissionItem.h

@@ -15,6 +15,7 @@
 #include "SettingsManager.h"
 #include "KMLPlanDomDocument.h"
 #include "QmlObjectListModel.h"
+#include "FlightPathSegment.h"
 
 #include <QSettings>
 
@@ -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;

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();
 

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();
 

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;
         }
 

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();

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<QGeoCoordinate>();
             if (prevCoord.isValid()) {
-                _appendFlightPathSegment(prevCoord,  surveyAlt, thisCoord,  surveyAlt);
+                _appendFlightPathSegment(FlightPathSegment::SegmentTypeGeneric, prevCoord,  surveyAlt, thisCoord,  surveyAlt);
             }
             prevCoord = thisCoord;
         }

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) {

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<double>();
-        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<double>();
+            if (y > (slope * x) + yIntercept) {
+                newTerrainCollision = true;
+                break;
+            }
+        }
+
         if (i == _amslTerrainHeights.count() - 2) {
             x += _finalDistanceBetween;
         } else {

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
 };