add ultra

src/controller/controller_hapo/main.cpp

@@ -94,11 +94,15 @@ void executeDecition(const iv::brain::decition decition)
     gcontroller->control_home_light(decition.home_light());
     gcontroller->control_air_worktime(decition.air_worktime());
     gcontroller->control_air_offtime(decition.air_offtime());
-    if(decition.door())
+
+    if(decition.has_door())
     {
-    gcontroller->control_door(2);
-    }else{
-      gcontroller->control_door(3);
+        if(decition.door())
+        {
+            gcontroller->control_door(2);
+        }else{
+            gcontroller->control_door(3);
+        }
     }
 
 

src/decition/common/common/car_status.h

@@ -40,7 +40,7 @@ namespace iv {
         std::int16_t wheel_angle;		//方向盘转角
         std::uint8_t braking_pressure;	//刹车压力
          float torque_st=0;
-        bool mbRunPause = true;
+        bool mbRunPause = false;
         bool mbBrainCtring = false;
         bool status[6] = { true, false, false, false, true, false };	//bool arrive = false;	//  x4:是否到达站点（0:未到 1:到达）
                                                                 //bool people = false;	//	x3:车上是否有人（0:无人 1:有人）
@@ -183,6 +183,7 @@ namespace iv {
 
         int mnBocheComplete = 0;   //When boche comple set a value.
 
+        uint32_t ultraDistance[13];
         bool useObsPredict = false;
         //hapo station 2021/02/05
         iv::StationCmd   stationCmd;

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

@@ -4,11 +4,12 @@
 #include <math.h>
 #include <iostream>
 #include <fstream>
+#include <QTime>
 
+QTime doorOpenTime;
 
 using namespace std;
 
-
 iv::decition::HapoAdapter::HapoAdapter(){
     adapter_id=5;
     adapter_name="hapo";
@@ -274,13 +275,19 @@ iv::decition::Decition iv::decition::HapoAdapter::getAdapterDeciton(GPS_INS now_
      if(ServiceCarStatus.station_control_door==0)
      {
          (*decition)->door=false;      //CLOSE
-       givlog->debug("DOOR","STATUS is: %d",5);
-     }
-     else if((ServiceCarStatus.station_control_door==1)&&(ServiceCarStatus.station_control_door_option==false))
+         doorOpenTime.start();
+         givlog->debug("DOOR","STATUS is: %d",5);
+
+     }else if((ServiceCarStatus.station_control_door==1)&&(ServiceCarStatus.station_control_door_option==false))
      {
          (*decition)->door=true;       //OPEN
-         ServiceCarStatus.station_control_door_option=true;
-         givlog->debug("DOOR","STATUS is: %d",8);
+         (*decition)->brake =6;
+         (*decition)->torque=0;
+         if(doorOpenTime.elapsed()>6000)
+         {
+             ServiceCarStatus.station_control_door_option=true;
+             givlog->debug("DOOR","STATUS is: %d",8);
+         }
      }
 
 givlog->debug("DOOR","door is: %d",(*decition)->door);

src/decition/decition_brain/decition/brain.cpp

@@ -81,6 +81,14 @@ iv::decition::BrainDecition * gbrain;
             gbrain->Updatev2x(strdata,nSize);
         }
 
+        void Listenultra(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+        {
+            (void)&index;
+            (void)dt;
+            (void)strmemname;
+            gbrain->Updateultra(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;
@@ -148,7 +156,7 @@ iv::decition::BrainDecition::BrainDecition()
  //   iv::modulecomm::RegisterRecv("map_index",ListenMap_change_req);
 
     v2x = iv::modulecomm::RegisterRecv("v2x",iv::decition::Listenv2x);
-
+    ultra = iv::modulecomm::RegisterRecv("ultra",iv::decition::Listenv2x);
 
     mpamapreq = iv::modulecomm::RegisterSend("mapreq",1000,1);
 
@@ -1353,14 +1361,33 @@ void iv::decition::BrainDecition::Updatev2x(const char *pdata, const int ndatasi
                            v2x_message.stationid(i),ServiceCarStatus.stationCmd.stationGps[i].gps_lat,
                           ServiceCarStatus.stationCmd.stationGps[i].gps_lng);
         }
-        ServiceCarStatus.mbRunPause=false;
+        //ServiceCarStatus.mbRunPause=false;
     }
 
 }
 
+void iv::decition::BrainDecition::Updateultra(const char *pdata, const int ndatasize)
+{
+    iv::ultrasonic::ultrasonic ultraarry;
+    if(false == ultraarry.ParseFromArray(pdata,ndatasize))
+    {
+        std::cout<<"Listenultra fail."<<std::endl;
+        return;
+    }
 
-
-
+    ServiceCarStatus.ultraDistance[1]=ultraarry.sigobjdist_flcorner();
+    ServiceCarStatus.ultraDistance[2]=ultraarry.sigobjdist_flmiddle();
+    ServiceCarStatus.ultraDistance[3]=ultraarry.sigobjdist_flside();
+    ServiceCarStatus.ultraDistance[4]=ultraarry.sigobjdist_frcorner();
+    ServiceCarStatus.ultraDistance[5]=ultraarry.sigobjdist_frmiddle();
+    ServiceCarStatus.ultraDistance[6]=ultraarry.sigobjdist_frside();
+    ServiceCarStatus.ultraDistance[7]=ultraarry.sigobjdist_rlcorner();
+    ServiceCarStatus.ultraDistance[8]=ultraarry.sigobjdist_rlmiddle();
+    ServiceCarStatus.ultraDistance[9]=ultraarry.sigobjdist_rlside();
+    ServiceCarStatus.ultraDistance[10]=ultraarry.sigobjdist_rrcorner();
+    ServiceCarStatus.ultraDistance[11]=ultraarry.sigobjdist_rrmiddle();
+    ServiceCarStatus.ultraDistance[12]=ultraarry.sigobjdist_rrside();
+}
 
 
 void iv::decition::BrainDecition::UpdateSate(){

src/decition/decition_brain/decition/brain.h

@@ -44,6 +44,7 @@
 #include "formation_map.pb.h"
 #include "vbox.pb.h"
 #include "v2x.pb.h"
+#include "ultrasonic.pb.h"
 
 #include "fanyaapi.h"
 
@@ -138,6 +139,7 @@ namespace iv {
             void *mpaGroup;
             void * mpmapChangeSig;
             void * v2x;
+            void * ultra;
             std::string mstrmemmap_index;
 
             int mnOldTime;
@@ -154,6 +156,7 @@ namespace iv {
             void UpdateSate();
             void UpdateVbox(const char * pdata,const int ndatasize);
             void Updatev2x(const char * pdata,const int ndatasize);
+            void Updateultra(const char * pdata,const int ndatasize);
 
         private:
 //            Adcivstatedb * mpasd;

src/decition/decition_brain/decition/decide_gps_00.cpp

@@ -1794,7 +1794,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
     //carState == 0，紧急停车
-    if (ServiceCarStatus.emergencyStop==1)
+    if ((ServiceCarStatus.emergencyStop==1)||(adjuseultra()==true))
     {
         minDecelerate=-1.0;
     }
@@ -3744,3 +3744,37 @@ void iv::decition::DecideGps00::adjuseGpsLidarPosition(){
     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;
+     for(int i=1;i<=13;i++)
+     {
+         if((i==2)||(i==3)||(i==4)||(i==5))   //front
+         {
+             if(ServiceCarStatus.ultraDistance[i]<100)
+             {
+                 front=true;
+             }
+         }else if((i==1)||(i==12)||(i==6)||(i==7))   //left,right
+         {
+             if(ServiceCarStatus.ultraDistance[i]<30)
+             {
+                 left=true;
+             }
+         }else if((i==8)||(i==9)||(i==10)||(i==11))   //back
+         {
+             if(ServiceCarStatus.ultraDistance[i]<10)
+             {
+                 back=true;
+             }
+         }
+     }
+
+     if(front||left||back)
+         return true;
+     else
+         return false;
+
+}
+
+

src/decition/decition_brain/decition/decide_gps_00.h

@@ -204,6 +204,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/decition_brain.pro

@@ -28,7 +28,8 @@ SOURCES += $$PWD/../common/main.cpp \
     ../../include/msgtype/vbox.pb.cc \
     ../../include/msgtype/radio_send.pb.cc \
     ../../include/msgtype/formation_map.pb.cc \
-    ../../include/msgtype/v2x.pb.cc
+    ../../include/msgtype/v2x.pb.cc \
+    ../../include/msgtype/ultrasonic.pb.cc
 
 include($$PWD/../common/common/common.pri)
 include($$PWD/decition/decition.pri)
@@ -71,3 +72,4 @@ HEADERS += \
     ../../include/msgtype/radio_send.pb.h \
     ../../include/msgtype/formation_map.pb.h \
     ../../include/msgtype/v2x.pb.h \
+    ../../include/msgtype/ultrasonic.pb.h \