modify lidar gps position function

src/decition/decition_brain/decition/brain.cpp

@@ -374,6 +374,10 @@ void iv::decition::BrainDecition::run() {
     std::string gpsOffset_x = xp.GetParam("gpsOffset_X","0");
     std::string gpsOffset_y = xp.GetParam("gpsOffset_Y","0");
     std::string strexternmpc = xp.GetParam("ExternMPC","false");  //If Use MPC set true
+
+
+    adjuseGpsLidarPosition();
+
     if(strexternmpc == "true")
     {
         mbUseExternMPC = true;
@@ -1365,3 +1369,13 @@ void iv::decition::BrainDecition::Updateultra(const char *pdata, const int ndata
 void iv::decition::BrainDecition::UpdateSate(){
      decitionGps00->isFirstRun=true;
 }
+
+void iv::decition::BrainDecition::adjuseGpsLidarPosition(){
+
+    ServiceCarStatus.msysparam.lidarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.radarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.rearRadarGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.rearLidarGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.frontGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.rearGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
+}

src/decition/decition_brain/decition/brain.h

@@ -161,6 +161,7 @@ namespace iv {
         private:
 //            Adcivstatedb * mpasd;
             void UpdateChassis(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+            void adjuseGpsLidarPosition();
 
             fanyaapi mmpcapi;
 

src/decition/decition_brain/decition/decide_gps_00.cpp

@@ -184,7 +184,6 @@ std::vector<double> lastDistanceVector(roadSum, -1);
 
 bool qiedianCount = false;
 //日常展示
-static int sensor_update=0;
 
 iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_gps_ins,
                                                                    const std::vector<GPSData> gpsMapLine,
@@ -234,11 +233,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                                                           DecideGps00::minDis,
                                                           DecideGps00::maxAngle);
         DecideGps00::lastGpsIndex = PathPoint;
-        if(sensor_update==0)
-        {
-            adjuseGpsLidarPosition();
-            sensor_update=1;
-        }
+
         if(ServiceCarStatus.speed_control==true){
             Compute00().getDesiredSpeed(gpsMapLine);   //add by zj
         }
@@ -3752,15 +3747,7 @@ float iv::decition::DecideGps00::computeTrafficSpeedLimt(float trafficDis){
     return limit;
 }
 
-void iv::decition::DecideGps00::adjuseGpsLidarPosition(){
 
-    ServiceCarStatus.msysparam.lidarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.radarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.rearRadarGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.rearLidarGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.frontGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.rearGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
-}
 
 bool iv::decition::DecideGps00::adjuseultra(){
      bool front=false,back=false,left=false,right=false;

src/decition/decition_brain/decition/decide_gps_00.h

@@ -203,7 +203,7 @@ public:
 
     bool computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed);
     float computeTrafficSpeedLimt(float trafficDis);
-    void adjuseGpsLidarPosition();
+
     bool adjuseultra();
 
     iv::Station getNearestStation(iv::GPS_INS now_gps_ins,std::vector<Station> station_n,std::vector<GPSData> gpsMap);

src/decition/decition_brain_sf/decition/brain.cpp

@@ -334,6 +334,10 @@ void iv::decition::BrainDecition::run() {
     std::string gpsOffset_x = xp.GetParam("gpsOffset_X","0");
     std::string gpsOffset_y = xp.GetParam("gpsOffset_Y","0");
     std::string strexternmpc = xp.GetParam("ExternMPC","false");  //If Use MPC set true
+
+
+     adjuseGpsLidarPosition();
+
     if(strexternmpc == "true")
     {
         mbUseExternMPC = true;
@@ -1260,3 +1264,13 @@ void iv::decition::BrainDecition::UpdateVbox(const char *pdata, const int ndatas
 void iv::decition::BrainDecition::UpdateSate(){
      decitionGps00->isFirstRun=true;
 }
+
+void iv::decition::BrainDecition::adjuseGpsLidarPosition(){
+
+    ServiceCarStatus.msysparam.lidarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.radarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.rearRadarGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.rearLidarGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.frontGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.rearGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
+}

src/decition/decition_brain_sf/decition/brain.h

@@ -156,6 +156,7 @@ namespace iv {
         private:
 //            Adcivstatedb * mpasd;
             void UpdateChassis(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+            void adjuseGpsLidarPosition();
 
             fanyaapi mmpcapi;
 

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -235,7 +235,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                                                           DecideGps00::maxAngle);
         DecideGps00::lastGpsIndex = PathPoint;
 
-        adjuseGpsLidarPosition();
+
         if(ServiceCarStatus.speed_control==true){
             Compute00().getDesiredSpeed(gpsMapLine);   //add by zj
         }
@@ -3400,13 +3400,4 @@ float iv::decition::DecideGps00::computeTrafficSpeedLimt(float trafficDis){
     return limit;
 }
 
-void iv::decition::DecideGps00::adjuseGpsLidarPosition(){
-
-    ServiceCarStatus.msysparam.lidarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.radarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.rearRadarGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.rearLidarGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.frontGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.rearGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
 
-}

src/decition/decition_brain_sf/decition/decide_gps_00.h

@@ -202,7 +202,7 @@ public:
 
     bool computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed);
     float computeTrafficSpeedLimt(float trafficDis);
-    void adjuseGpsLidarPosition();
+
 
     GPS_INS aim_gps_ins;
     GPS_INS chaoche_start_gps;