fix heading calcu

incl/fconfig.h

@@ -5,6 +5,7 @@
 #include "single_ton.hpp"
 #include <ros/ros.h>
 #include <yaml-cpp/yaml.h>
+#include "falgo.h"
 
 using namespace std;
 
@@ -100,7 +101,7 @@ public:
         to.lon = vec[4];
         to.lat = vec[5];
 
-        double direct = jfx::GetDirectionXS(from.lon, from.lat, to.lon, to.lat) + 1.26;
+        double direct = FusionAlgo::geoGetHeading(from.lon, from.lat, to.lon, to.lat) + 1.26;
         direct = direct > 360 ? (direct - 360) : direct;
         if (vec.size() == 6) {
             from.heading = to.heading = direct;

src/ffobj.cpp

@@ -2,6 +2,7 @@
 #include "ffobj.h"
 #include "gaussian.h"
 #include <ros/ros.h>
+#include "falgo.h"
 
 void FusionObject::setTObj(TOBJPtr& tObjPtr)
 {
@@ -110,7 +111,7 @@ double FusionObject::getHeading(double curLon, double curLat)
     int pos = mFusionHisData.size() > 5 ? mFusionHisData.size() - 5 : 0;
     STObjTrajPoint& hisPoint = mFusionHisData[pos];
 
-    mHeading = jfx::GetDirectionXS(hisPoint.lon, hisPoint.lat, curLon, curLat);
+    mHeading = FusionAlgo::geoGetHeading(hisPoint.lon, hisPoint.lat, curLon, curLat);
 
     mLastCalcuHeadingLon = curLon;
     mLastCalcuHeadingLat = curLat;