- new implementation for course and distance calculation

doc/TODO allgemein.txt

@@ -34,5 +34,10 @@ Do later:
         - navigation
         - autopilot
 
+Have to:
+    -> check new distance calculation between points
+    -> check new course calculation between points
+    -> menu route data und ui disconnecten
+
 
 

lib/Point/point.cpp

@@ -56,14 +56,29 @@ bool Point::operator==(const Point &rhs) const
 
 double Point::distanceTo(const Coordinates &point) const
 {
-    const Coordinates begin = this->coordinates;
+    // Old implementation
-    const Coordinates end = point;
+    // const Coordinates begin = this->coordinates;
+    // const Coordinates end = point;
 
-    const double lat = (begin.lat + end.lat) / 2 * ROUTE_DEGREE_TO_RADIANT;
+    // const double lat = (begin.lat + end.lat) / 2 * ROUTE_DEGREE_TO_RADIANT;
-    const double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (begin.lat - end.lat);
+    // const double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (begin.lat - end.lat);
-    const double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (begin.lon - end.lon);
+    // const double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (begin.lon - end.lon);
 
-    return sqrt(dx * dx + dy * dy);
+    // return sqrt(dx * dx + dy * dy);
+
+    double latStart = this->toRad(this->coordinates.lat);
+    double lonStart = this->toRad(this->coordinates.lon);
+    double latEnd = this->toRad(point.lat);
+    double lonEnd = this->toRad(point.lon);
+
+    const double earthRadius = 6371.0; //km
+    double cartesianX_1 = cos(latStart) * lonStart * earthRadius;
+    double cartesianY_1 = latStart * earthRadius;
+    double cartesianX_2 = cos(latEnd) * lonEnd * earthRadius;
+    double cartesianY_2 = latEnd * earthRadius;
+
+    return sqrt(pow(cartesianX_2 - cartesianX_1, 2)
+              + pow(cartesianY_2 - cartesianY_1, 2));
 }
 
 double Point::distanceTo(const Point &point) const
@@ -73,13 +88,24 @@ double Point::distanceTo(const Point &point) const
 
 int16_t Point::courseTo(const Coordinates &point) const
 {
-    const Coordinates begin = this->coordinates;
+    // const Coordinates begin = this->coordinates;
-    const Coordinates end = point;
+    // const Coordinates end = point;
 
-    const double phi = log(tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4));
+    // const double phi = log(tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4));
-    const double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT);
+    // const double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT);
 
-    return static_cast<int16_t>(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1;
+    // return static_cast<int16_t>(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1;
+
+    double latStart = this->toRad(this->coordinates.lat);
+    double lonStart = this->toRad(this->coordinates.lon);
+    double latEnd = this->toRad(point.lat);
+    double lonEnd = this->toRad(point.lon);
+
+    double y = sin(lonEnd - lonStart) * cos(latEnd);
+    double x = cos(latStart) * sin(latEnd) - sin(latStart) * cos(latEnd) * cos(lonEnd - lonStart);
+    double theta = atan2(y, x);
+
+    return static_cast<int16_t>(this->toDeg(theta)) % 360;
 }
 
 int16_t Point::courseTo(const Point &point) const

lib/Point/point.h

@@ -131,6 +131,17 @@ public:
 
 private:
     void init(uint32_t horizontalAccuracy, uint32_t creationTime);
+    double toRad (double degree) const
+    {
+        constexpr double radFactor = M_PI / 180.0;
+        return degree * radFactor;
+    };
+
+    double toDeg(double radiant) const
+    {
+        constexpr double degFactor = 180.0 / M_PI;
+        return radiant * degFactor;
+    };
 
     Accuracy accuracy = Accuracy::none;
     Coordinates coordinates{0, 0};

lib/Route/route.cpp

@@ -88,6 +88,7 @@ Point Route::getPreviousPoint()
         this->currentPoint--;
         return *this->it;
     }
+    
     return point;
 }