add station door control

src/decition/common/common/car_status.h

@@ -52,6 +52,8 @@ namespace iv {
 
         int carState = 0;	// 0:停车	1:正常循迹	2:前往站点
         int emergencyStop=0;
+        int station_control_door=0;//0: door close    1:door open
+        bool station_control_door_option=false;
                 int istostation = 0;
                 //int ctostation = 0;
                 int currentstation = 0;

src/decition/decition_brain/decition/adc_adapter/hapo_adapter.cpp

@@ -264,9 +264,23 @@ iv::decition::Decition iv::decition::HapoAdapter::getAdapterDeciton(GPS_INS now_
             (*decition)->nearLight=0;
              (*decition)->farLight=0;
 
+     if(ServiceCarStatus.stationCmd.mode_manual_drive==true)
+     {
+          (*decition)->mode=0;       //云平台控制切手动驾驶
+     }
+
+
+     if(ServiceCarStatus.station_control_door==0)
+     {
+         (*decition)->door=3;       //关门
+     }
+     else if((ServiceCarStatus.station_control_door==1)&&(ServiceCarStatus.station_control_door_option==false))
+     {
+         (*decition)->door=2;       //到达站点开门
+         ServiceCarStatus.station_control_door_option=true;
+     }
 
 
-       (*decition)->door=3;
 
 std::cout<<"==========================================="<<std::endl;
      std::cout<<"dSpeed:"<<dSpeed<<"   realSpeed:"<<realSpeed<<"   acc:"<<accAim<<"  torque_st:"<<ServiceCarStatus.torque_st

src/decition/decition_brain/decition/decide_gps_00.cpp

@@ -1772,6 +1772,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             {
                 ServiceCarStatus.carState = 1;
                 busMode=false;
+                ServiceCarStatus.station_control_door=0;
+                ServiceCarStatus.station_control_door_option=false;
             }
         }
         if(ServiceCarStatus.stationCmd.has_carMode)
@@ -1794,7 +1796,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     //carState == 0，紧急停车
     if (ServiceCarStatus.emergencyStop==1)
     {
-
         minDecelerate=-1.0;
     }
 
@@ -1845,6 +1846,11 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             dSpeed = min(20.0, dSpeed);
         }
 
+        if((abs(realspeed)<0.1)&&(ServiceCarStatus.station_control_door==0))
+        {
+                ServiceCarStatus.station_control_door=1;   //open door
+        }
+
         //站点停车log
         givlog->debug("brain_v2x","aim_gps_ins.gps_lat: %.7f, aim_gps_ins.gps_lng: %.7f, dis: %.2f, dSpeed: %.1f",
                       aim_gps_ins.gps_lat, aim_gps_ins.gps_lng,
@@ -1909,17 +1915,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     // givlog->debug("SPEED","dSpeed is %f",dSpeed);
     phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
 
-
-    if(ServiceCarStatus.stationCmd.mode_manual_drive==true)
-    {
-        gps_decition->mode=0;       //云平台控制切手动驾驶
-    }
-
-
     gps_decition->wheel_angle = 0.0 - controlAng;
 
-
-
     lastAngle=gps_decition->wheel_angle;
 
     //   qDebug("decide1 time is %d",xTime.elapsed());