diff --git a/src/MissionManager/SurveyMissionItem.cc b/src/MissionManager/SurveyMissionItem.cc
index 2930d09ca4b22aef32ecec5ec3f24ed7714f6d3f..54bab4591ef6874edf3247ca75377ff64124c917 100644
--- a/src/MissionManager/SurveyMissionItem.cc
+++ b/src/MissionManager/SurveyMissionItem.cc
@@ -444,16 +444,56 @@ void SurveyMissionItem::_convertTransectToGeo(const QList<QList<QPointF>>& trans
     }
 }
 
-void SurveyMissionItem::_convertPointsToGeo(const QList<QPointF>& pointsNED, const QGeoCoordinate& tangentOrigin, QVariantList& pointsGeo)
+void SurveyMissionItem::_optimizeReflySegments(void)
 {
-    pointsGeo.clear();
+    // The last flight point of the initial pass
+    QGeoCoordinate initialPassLastCoord = _transectSegments.last().last();
+
+    // Now determine where we should start the refly pass
+
+    double rgTransectDistance[4];
+    rgTransectDistance[0] = _reflyTransectSegments.first().first().distanceTo(initialPassLastCoord);
+    rgTransectDistance[1] = _reflyTransectSegments.first().last().distanceTo(initialPassLastCoord);
+    rgTransectDistance[2] = _reflyTransectSegments.last().first().distanceTo(initialPassLastCoord);
+    rgTransectDistance[3] = _reflyTransectSegments.last().last().distanceTo(initialPassLastCoord);
+
+    int shortestIndex = 0;
+    double shortestDistance = rgTransectDistance[0];
+    for (int i=1; i<3; i++) {
+        if (rgTransectDistance[i] < shortestDistance) {
+            shortestIndex = i;
+            shortestDistance = rgTransectDistance[i];
+        }
+    }
 
-    for (int i=0; i<pointsNED.count(); i++) {
-        QGeoCoordinate geoCoord;
-        const QPointF& point = pointsNED[i];
+    if (shortestIndex > 1) {
+        qDebug() << "Reverse segments";
+        // We need to reverse the order of segments
+        QList<QList<QGeoCoordinate>> rgReversedTransects;
+        for (int i=_reflyTransectSegments.count() - 1; i>=0; i--) {
+            rgReversedTransects.append(_reflyTransectSegments[i]);
+        }
+        _reflyTransectSegments = rgReversedTransects;
+    }
+    if (shortestIndex & 1) {
+        qDebug() << "Reverse points";
+        // We need to reverse the points within each segment
+        for (int i=0; i<_reflyTransectSegments.count(); i++) {
+            QList<QGeoCoordinate> rgReversedCoords;
+            QList<QGeoCoordinate>& rgOriginalCoords = _reflyTransectSegments[i];
+            for (int j=rgOriginalCoords.count()-1; j>=0; j++) {
+                rgReversedCoords.append(rgOriginalCoords[j]);
+            }
+            _reflyTransectSegments[i] = rgReversedCoords;
+        }
+    }
+}
 
-        convertNedToGeo(-point.y(), point.x(), 0, tangentOrigin, &geoCoord);
-        pointsGeo.append(QVariant::fromValue(geoCoord));
+void SurveyMissionItem::_appendGridPointsFromTransects(QList<QList<QGeoCoordinate>>& rgTransectSegments)
+{
+    for (int i=0; i<rgTransectSegments.count(); i++) {
+        _simpleGridPoints.append(QVariant::fromValue(rgTransectSegments[i].first()));
+        _simpleGridPoints.append(QVariant::fromValue(rgTransectSegments[i].last()));
     }
 }
 
@@ -473,7 +513,6 @@ void SurveyMissionItem::_generateGrid(void)
     _reflyTransectSegments.clear();
 
     QList<QPointF>          polygonPoints;
-    QList<QPointF>          gridPoints;
     QList<QList<QPointF>>   transectSegments;
 
     // Convert polygon to Qt coordinate system (y positive is down)
@@ -499,18 +538,17 @@ void SurveyMissionItem::_generateGrid(void)
 
     // Generate grid
     int cameraShots = 0;
-    cameraShots += _gridGenerator(polygonPoints, gridPoints, transectSegments, false /* refly */);
-    _convertPointsToGeo(gridPoints, tangentOrigin, _simpleGridPoints);
+    cameraShots += _gridGenerator(polygonPoints, transectSegments, false /* refly */);
     _convertTransectToGeo(transectSegments, tangentOrigin, _transectSegments);
+    _appendGridPointsFromTransects(_transectSegments);
     if (_refly90Degrees) {
         QVariantList reflyPointsGeo;
 
-        gridPoints.clear();
         transectSegments.clear();
-        cameraShots += _gridGenerator(polygonPoints, gridPoints, transectSegments, true /* refly */);
-        _convertPointsToGeo(gridPoints, tangentOrigin, reflyPointsGeo);
+        cameraShots += _gridGenerator(polygonPoints, transectSegments, true /* refly */);
         _convertTransectToGeo(transectSegments, tangentOrigin, _reflyTransectSegments);
-        _simpleGridPoints.append(reflyPointsGeo);
+        _optimizeReflySegments();
+        _appendGridPointsFromTransects(_reflyTransectSegments);
     }
 
     // Calc survey distance
@@ -681,7 +719,7 @@ void SurveyMissionItem::_adjustLineDirection(const QList<QLineF>& lineList, QLis
     }
 }
 
-int SurveyMissionItem::_gridGenerator(const QList<QPointF>& polygonPoints,  QList<QPointF>& simpleGridPoints, QList<QList<QPointF>>& transectSegments, bool refly)
+int SurveyMissionItem::_gridGenerator(const QList<QPointF>& polygonPoints,  QList<QList<QPointF>>& transectSegments, bool refly)
 {
     int cameraShots = 0;
 
@@ -690,7 +728,6 @@ int SurveyMissionItem::_gridGenerator(const QList<QPointF>& polygonPoints,  QLis
 
     qCDebug(SurveyMissionItemLog) << "SurveyMissionItem::_gridGenerator gridSpacing:gridAngle" << gridSpacing << gridAngle;
 
-    simpleGridPoints.clear();
     transectSegments.clear();
 
     // Convert polygon to bounding rect
@@ -793,9 +830,6 @@ int SurveyMissionItem::_gridGenerator(const QList<QPointF>& polygonPoints,  QLis
             transectPoints.append(transectLine.pointAt(1 + turnaroundPosition));
         }
 
-        simpleGridPoints.append(transectPoints[0]);
-        simpleGridPoints.append(transectPoints[transectPoints.count() - 1]);
-
         transectSegments.append(transectPoints);
     }
 
diff --git a/src/MissionManager/SurveyMissionItem.h b/src/MissionManager/SurveyMissionItem.h
index 4778cb17a763efd891657e50a76274a249705b62..77ce7c73f5df2d50402daea0c7d1add2082b191f 100644
--- a/src/MissionManager/SurveyMissionItem.h
+++ b/src/MissionManager/SurveyMissionItem.h
@@ -178,7 +178,7 @@ private:
     void _setExitCoordinate(const QGeoCoordinate& coordinate);
     void _generateGrid(void);
     void _updateCoordinateAltitude(void);
-    int _gridGenerator(const QList<QPointF>& polygonPoints, QList<QPointF>& simpleGridPoints, QList<QList<QPointF>>& transectSegments, bool refly);
+    int _gridGenerator(const QList<QPointF>& polygonPoints, QList<QList<QPointF>>& transectSegments, bool refly);
     QPointF _rotatePoint(const QPointF& point, const QPointF& origin, double angle);
     void _intersectLinesWithRect(const QList<QLineF>& lineList, const QRectF& boundRect, QList<QLineF>& resultLines);
     void _intersectLinesWithPolygon(const QList<QLineF>& lineList, const QPolygonF& polygon, QList<QLineF>& resultLines);
@@ -196,8 +196,9 @@ private:
     bool _hasTurnaround(void) const;
     double _turnaroundDistance(void) const;
     void _convertTransectToGeo(const QList<QList<QPointF>>& transectSegmentsNED, const QGeoCoordinate& tangentOrigin, QList<QList<QGeoCoordinate>>& transectSegmentsGeo);
-    void _convertPointsToGeo(const QList<QPointF>& pointsNED, const QGeoCoordinate& tangentOrigin, QVariantList& pointsGeo);
     bool _appendMissionItemsWorker(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, bool hasRefly, bool buildRefly);
+    void _optimizeReflySegments(void);
+    void _appendGridPointsFromTransects(QList<QList<QGeoCoordinate>>& rgTransectSegments);
 
     int                             _sequenceNumber;
     bool                            _dirty;