Merge pull request #5954 from DonLakeFlyer/GeoToNed

Fix NaN in convertGeoToNed

src/MissionManager/QGCMapPolygon.cc

@@ -302,10 +302,11 @@ void QGCMapPolygon::_updateCenter(void)
             }
             center = _coordFromPointF(QPointF(centroid.x() / polygonF.count(), centroid.y() / polygonF.count()));
         }
-
+        if (_center != center) {
             _center = center;
             emit centerChanged(center);
         }
+    }
 }
 
 void QGCMapPolygon::setCenter(QGeoCoordinate newCenter)

src/MissionManager/QGCMapPolygonTest.cc

@@ -122,7 +122,12 @@ void QGCMapPolygonTest::_testVertexManipulation(void)
         QCOMPARE(_mapPolygon->count(), i);
 
         _mapPolygon->appendVertex(_polyPoints[i]);
+        if (i >= 2) {
+            // Center is no recalculated until there are 3 points or more
             QVERIFY(_multiSpyPolygon->checkOnlySignalByMask(pathChangedMask | polygonDirtyChangedMask | polygonCountChangedMask | centerChangedMask));
+        } else {
+            QVERIFY(_multiSpyPolygon->checkOnlySignalByMask(pathChangedMask | polygonDirtyChangedMask | polygonCountChangedMask));
+        }
         QVERIFY(_multiSpyModel->checkOnlySignalByMask(modelDirtyChangedMask | modelCountChangedMask));
         QCOMPARE(_multiSpyPolygon->pullIntFromSignalIndex(polygonCountChangedIndex), i+1);
         QCOMPARE(_multiSpyModel->pullIntFromSignalIndex(modelCountChangedIndex), i+1);

src/QGCGeo.cc

@@ -28,7 +28,13 @@
 
 static const float epsilon = std::numeric_limits<double>::epsilon();
 
-void convertGeoToNed(QGeoCoordinate coord, QGeoCoordinate origin, double* x, double* y, double* z) {
+void convertGeoToNed(QGeoCoordinate coord, QGeoCoordinate origin, double* x, double* y, double* z)
+{
+    if (coord == origin) {
+        // Short circuit to prevent NaNs in calculation
+        *x = *y = *z = 0;
+        return;
+    }
 
     double lat_rad = coord.latitude() * M_DEG_TO_RAD;
     double lon_rad = coord.longitude() * M_DEG_TO_RAD;