add hapo station park

src/decition/common/common/gps_type.h

@@ -107,12 +107,18 @@ namespace iv {
      public:
          bool received;
          uint32_t carID,carMode,emergencyStop,stationStop;
+         bool has_carID,has_carMode,has_emergencyStop,has_stationStop,mode_manual_drive;
          uint32_t stationID[20];
          GPS_INS  stationGps[20];
          uint32_t stationTotalNum;
          StationCmd()
          {
              received=false;
+             has_carID=false;
+             has_carMode=false;
+             has_emergencyStop=false;
+             has_stationStop=false;
+             mode_manual_drive=false;
              carID=0;
              carMode=0;
              emergencyStop=0;

src/decition/decition_brain/decition/brain.cpp

@@ -73,6 +73,14 @@ iv::decition::BrainDecition * gbrain;
             gbrain->UpdateGroupInfo(strdata,nSize);
         }
 
+        void Listenv2x(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+        {
+            (void)&index;
+            (void)dt;
+            (void)strmemname;
+            gbrain->Updatev2x(strdata,nSize);
+        }
+
     /*    void ListenMap_change_req(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
         {
             iv::formation_map_index::map map;
@@ -139,7 +147,7 @@ iv::decition::BrainDecition::BrainDecition()
     mpaGroup = iv::modulecomm::RegisterRecv("p900_send",iv::decition::ListenGroup);
  //   iv::modulecomm::RegisterRecv("map_index",ListenMap_change_req);
 
-
+    v2x = iv::modulecomm::RegisterRecv("v2x",iv::decition::Listenv2x);
 
 
     mpamapreq = iv::modulecomm::RegisterSend("mapreq",1000,1);
@@ -1288,13 +1296,32 @@ void iv::decition::BrainDecition::Updatev2x(const char *pdata, const int ndatasi
 
 //  解析v2x信息
     ServiceCarStatus.stationCmd.received=true;
-    ServiceCarStatus.stationCmd.carID=v2x_message.carid();
-    ServiceCarStatus.stationCmd.carMode=v2x_message.carmode();
-    ServiceCarStatus.stationCmd.emergencyStop=v2x_message.emergencystop();
-    ServiceCarStatus.stationCmd.stationStop=v2x_message.stationstop();
 
-    ServiceCarStatus.stationCmd.stationTotalNum=v2x_message.stationid_size();
+    ServiceCarStatus.stationCmd.has_carID=v2x_message.has_carid();
+    if(ServiceCarStatus.stationCmd.has_carID)
+    {
+            ServiceCarStatus.stationCmd.carID=v2x_message.carid();
+    }
 
+    ServiceCarStatus.stationCmd.has_carMode=v2x_message.has_carmode();
+    if(ServiceCarStatus.stationCmd.has_carMode)
+    {
+            ServiceCarStatus.stationCmd.carMode=v2x_message.carmode();
+    }
+
+    ServiceCarStatus.stationCmd.has_emergencyStop=v2x_message.has_emergencystop();
+    if(ServiceCarStatus.stationCmd.has_emergencyStop)
+    {
+            ServiceCarStatus.stationCmd.emergencyStop=v2x_message.emergencystop();
+    }
+
+    ServiceCarStatus.stationCmd.has_stationStop=v2x_message.has_stationstop();
+    if(ServiceCarStatus.stationCmd.has_stationStop)
+    {
+            ServiceCarStatus.stationCmd.stationStop=v2x_message.stationstop();
+    }
+
+    ServiceCarStatus.stationCmd.stationTotalNum=v2x_message.stationid_size();
     for(int i=0;i++;i<ServiceCarStatus.stationCmd.stationTotalNum)
     {
         ServiceCarStatus.stationCmd.stationGps[i].gps_lat=v2x_message.stgps(v2x_message.stationid(i)).lat();

src/decition/decition_brain/decition/brain.h

@@ -137,6 +137,7 @@ namespace iv {
             void * mpaPlatform;
             void *mpaGroup;
             void * mpmapChangeSig;
+            void * v2x;
             std::string mstrmemmap_index;
 
             int mnOldTime;

src/decition/decition_brain/decition/decide_gps_00.cpp

@@ -1726,44 +1726,71 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     {
         std::vector<Station>  station_received;
         Station station_aa,station_nearest;
-        station_received.clear();
-        for(int i=0;i<ServiceCarStatus.stationCmd.stationTotalNum;i++)
+
+        if(ServiceCarStatus.stationCmd.has_emergencyStop)
         {
-            station_aa.index=i;
-            station_aa.station_location.gps_lat=ServiceCarStatus.stationCmd.stationGps[i].gps_lat;
-            station_aa.station_location.gps_lng=ServiceCarStatus.stationCmd.stationGps[i].gps_lng;
-            station_received.push_back(station_aa);
+            if(ServiceCarStatus.stationCmd.emergencyStop==0x01)
+            {
+                ServiceCarStatus.carState = 0;
+                busMode=true;
+            }
+            else
+            {
+                ServiceCarStatus.carState = 1;
+                busMode=false;
+            }
         }
-        station_nearest=getNearestStation(now_gps_ins,station_received,gpsMapLine);
 
-        if(ServiceCarStatus.stationCmd.emergencyStop==0x01)
+        if(ServiceCarStatus.stationCmd.has_stationStop)
         {
-            ServiceCarStatus.carState = 0;
-        }
+            //寻找最近站点
+            station_received.clear();
+            for(int i=0;i<ServiceCarStatus.stationCmd.stationTotalNum;i++)
+            {
+                station_aa.index=i;
+                station_aa.station_location.gps_lat=ServiceCarStatus.stationCmd.stationGps[i].gps_lat;
+                station_aa.station_location.gps_lng=ServiceCarStatus.stationCmd.stationGps[i].gps_lng;
+                station_received.push_back(station_aa);
+            }
+            station_nearest=getNearestStation(now_gps_ins,station_received,gpsMapLine);
 
-        if((ServiceCarStatus.stationCmd.stationStop==0x00))
+            //进入站点模式
+            if((ServiceCarStatus.stationCmd.stationStop==0x00))
+            {
+                ServiceCarStatus.carState = 2;
+                ServiceCarStatus.amilat=station_nearest.station_location.gps_lat;
+                ServiceCarStatus.amilng=station_nearest.station_location.gps_lng;
+                busMode=true;
+            }else
+            {
+                ServiceCarStatus.carState = 1;
+                busMode=false;
+            }
+        }
+        if(ServiceCarStatus.stationCmd.has_carMode)
         {
-            ServiceCarStatus.carState = 2;
-            ServiceCarStatus.amilat=station_nearest.station_location.gps_lat;
-            ServiceCarStatus.amilng=station_nearest.station_location.gps_lng;
+            if(ServiceCarStatus.stationCmd.carMode==0x00)
+            {
+               ServiceCarStatus.stationCmd.mode_manual_drive=true;
+            }else
+            {
+               ServiceCarStatus.stationCmd.mode_manual_drive=false;
+            }
         }
 
         ServiceCarStatus.stationCmd.received=false;
-        busMode=true;
     }
 
 
 
 
-
+    //carState == 0，紧急停车
     if (ServiceCarStatus.carState == 0 && busMode)
     {
 
         minDecelerate=-1.0;
     }
 
-
-
     if (ServiceCarStatus.carState == 3 && busMode)
     {
 
@@ -1777,14 +1804,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     }
 
-
-
-
-
-
-    ///kkcai, 2020-01-03
-    //if (ServiceCarStatus.carState == 2 && busMode)
-    if (ServiceCarStatus.carState == 2)
+    //carState==2,站点停车
+    if ((ServiceCarStatus.carState==2)&&busMode)
     {
 
         aim_gps_ins.gps_lat=ServiceCarStatus.amilat;
@@ -1838,7 +1859,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     if (gpsMapLine[PathPoint]->runMode == 2)
     {
         obsDistance = -1;
-
     }
 
     std::cout<<"juecesudu0="<<dSpeed<<std::endl;
@@ -1888,7 +1908,10 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
 
 
-
+    if(ServiceCarStatus.stationCmd.mode_manual_drive==true)
+    {
+        gps_decition->driveMode=0;
+    }
 
 
     gps_decition->wheel_angle = 0.0 - controlAng;