fix: getheading error

src/falgo.cpp

@@ -3,6 +3,11 @@
 #include "falgo.h"
 #include "fconfig.h"
 #include "gaussian.h"
+#include "GeographicLib/Geodesic.hpp"
+#include "GeographicLib/GeodesicLine.hpp"
+#include "GeographicLib/Constants.hpp"
+#include <GeographicLib/TransverseMercator.hpp>
+#include <GeographicLib/UTMUPS.hpp>
 
 FusionAlgo::FusionAlgo()
 {
@@ -19,7 +24,13 @@ int FusionAlgo::forecastTObjRealPoint(TOBJPtr objPtr, uint64_t realTimeMS)
 
 double FusionAlgo::geoGetHeading(double lng1, double lat1, double lng2, double lat2)
 {
-    return jfx::GetDirectionXS(lng1, lat1, lng2, lat2);
+    double azi1 = 0;
+    double azi2 = 0;
+    GeographicLib::Geodesic::WGS84().Inverse(lat1, lng1, lat2, lng2, azi1, azi2);
+    if (azi1 < 0) {
+        azi1 += 360;
+    }
+    return azi1;
 }
 
 double FusionAlgo::calcuObjCurDist(int pos, TOBJPtr obj1, TOBJPtr obj2, uint64_t& obj2MS, uint64_t& dms)
@@ -86,10 +97,10 @@ double FusionAlgo::calcuObjCurDist(int pos, TOBJPtr obj1, TOBJPtr obj2, uint64_t
         pVec2.push_back(jfx::GetPoint(p2.lon, p2.lat, -1 * offset2 * i / offsetPointCount, p2.headingReal));
     }
 
-    // 
+    //
     double minDist = 999;
     for (auto& pa1 : pVec1) {
-        for (auto& pa2: pVec2) {
+        for (auto& pa2 : pVec2) {
             double dist = jfx::GetDistance(pa1[0], pa1[1], pa2[0], pa2[1]);
             if (dist < minDist) {
                 minDist = dist;
@@ -102,10 +113,11 @@ double FusionAlgo::calcuObjCurDist(int pos, TOBJPtr obj1, TOBJPtr obj2, uint64_t
     return minDist;
 }
 
-double FusionAlgo::calcuObjCurDist2(int pos, TOBJPtr obj1, TOBJPtr obj2) {
+double FusionAlgo::calcuObjCurDist2(int pos, TOBJPtr obj1, TOBJPtr obj2)
+{
     uint64_t obj2MS = 0, dms = 0;
     return calcuObjCurDist(pos, obj1, obj2, obj2MS, dms);
-}   
+}
 
 int FusionAlgo::ifCanCheckNear(TOBJPtr& tObjPtr, TOBJPtr& o)
 {