fix(grpc_BS,brain):fix fsm skip error,and path finish stop error

src/decition/common/common/sysparam_type.h

@@ -35,6 +35,7 @@ namespace iv {
 
         bool mbSideDrive=false; //vehicle side driving
         double mfSideDis=0.3;//vehicle right to lane right distance
+        int mWaitTimeCnt=3000;
 
 
     };

src/decition/common/perception_sf/impl_gps.cpp

@@ -322,7 +322,7 @@ void iv::perception::GPSSensor::UpdateGPSIMU(iv::gps::gpsimu xgpsimu)
     data->ins_pitch_angle = xgpsimu.pitch();
     data->ins_roll_angle = xgpsimu.roll();
 
-    data->speed = xgpsimu.speed(); //sqrt(pow(xgpsimu.ve(),2)+pow(xgpsimu.vn(),2)) * 3.6;
+    data->speed = xgpsimu.speed() * 3.6; //sqrt(pow(xgpsimu.ve(),2)+pow(xgpsimu.vn(),2)) * 3.6;
 
     GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
 
@@ -331,7 +331,7 @@ void iv::perception::GPSSensor::UpdateGPSIMU(iv::gps::gpsimu xgpsimu)
     ServiceCarStatus.mfGPSAcc = xgpsimu.acc_calc();
 
     ServiceCarStatus.speed = data->speed;
-    if(ServiceCarStatus.speed < 0.015)ServiceCarStatus.speed = 0.0;
+    if(ServiceCarStatus.speed < 0.1)ServiceCarStatus.speed = 0.0;
 //       qDebug("speed x is %g",ServiceCarStatus.speed);
     if(data->ins_status == 4)
         signal_gps_data->operator()(data);	//传输数据

src/decition/decition_brain_sf_jsguide/decition/brain.cpp

@@ -499,6 +499,10 @@ void iv::decition::BrainDecition::run() {
 
     ServiceCarStatus.msysparam.mfSideDis = atof(SideDis.data());
 
+    std::string WaitTimeCnt = xp.GetParam("WaitTimeCnt","3000");
+
+   ServiceCarStatus.msysparam.mWaitTimeCnt = atof(WaitTimeCnt.data());
+
 
     mstrmemmap_index = xp.GetParam("msg_mapindex","map_index");
 
@@ -512,7 +516,7 @@ void iv::decition::BrainDecition::run() {
         {
                 ServiceCarStatus.mbRunPause=true;
              //待命停车
-                if(ServiceCarStatus.mbOvertimeCnt<=3000)//待命计时
+                if(ServiceCarStatus.mbOvertimeCnt<=ServiceCarStatus.msysparam.mWaitTimeCnt)//3000)//待命计时
                 {
                     ServiceCarStatus.mbOvertimeCnt++;
                 }

src/decition/decition_brain_sf_jsguide/decition/decide_gps_00.cpp

@@ -1085,6 +1085,9 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                         static int BrakePoint=-1;
                         static bool final_brake=false;
                         static double distance_to_end=1000;
+                        givlog->debug("decition_brain","realspeed: %f",
+                                        realspeed);
+
                         if(PathPoint+forecast_final_point>gpsMapLine.size())
                         {                           
                             distance_to_end=computeDistToEnd(gpsMapLine,PathPoint);

src/driver/driver_cloud_grpc_client_BS/fsm_unit.cpp

@@ -23,6 +23,7 @@ FSMUnit::FSMUnit(void)
     QObject::connect(this,&FSMUnit::refreshFSMStatus,this,&FSMUnit::refreshFSMStatus_Slot);
     QObject::connect(&skipCMDSendTimer,&QTimer::timeout,this,&FSMUnit::skipCMDSend_Timeout);
     QObject::connect(&arriveCheckLoopTimer,&QTimer::timeout,this,&FSMUnit::arriveCheckLoop_Timeout);
+    QObject::connect(this,&FSMUnit::sendPathPlanRequest,this,&FSMUnit::sendPathPlanRequest_Slot);
     if(skipCMDSendTimer.isActive() == false)skipCMDSendTimer.start(skipCMDSendInterval);
     if(arriveCheckLoopTimer.isActive() == false)arriveCheckLoopTimer.start(100);
 }
@@ -196,10 +197,12 @@ void FSMUnit::ListenBrainStateMsg(const char * strdata,const unsigned int nSize,
 
 void FSMUnit::navagationSwitchChanged_Slot(bool status)
 {
-    if(status)
+    if(status == true && FSMState == 0)
         missionCMD = iv::brain::MissionCMD::MISSION_START;
-    else
+    else if(status == false && FSMState == 1)
         missionCMD = iv::brain::MissionCMD::MISSION_CANCEL;
+    else
+        missionCMD = iv::brain::MissionCMD::MISSION_RESERVED;
 }
 
 void FSMUnit::useStatusCMDChanged_Slot(bool status)
@@ -341,6 +344,7 @@ void FSMUnit::arriveCheckLoop_Timeout(void)
 {
     if(FSMState == 4) ///< 4返程
     {
+//        std::cout<<"status is return wait station"<<std::endl;
         if(currentDestination.latitude != waitingStation.latitude || currentDestination.longitude != waitingStation.longitude)
         {
             currentDestination.latitude = waitingStation.latitude;
@@ -394,6 +398,6 @@ void FSMUnit::sendPathPlanRequest_Slot(double latitude,double longitude)
     xodrDest.flat = latitude;
     xodrDest.lane = 1;
     std::cout<<"current destination: "<<std::setprecision(12)<<longitude<<","<<latitude<<std::endl;
-
+    std::cout<<"send a path request"<<std::endl;
     iv::modulecomm::ModuleSendMsg(shmXodrRequest.mpa,(char *)&xodrDest,sizeof(xodrobj)); ///< send request msg
 }

src/tool/tool_FSM_Test/mainwindow.cpp

@@ -20,13 +20,13 @@ MainWindow::MainWindow(QWidget *parent)
     shmFSMSkipCMD.mnBufferSize = 10000;
     shmFSMSkipCMD.mnBufferCount = 1;
 
-    shmFSMSkipCMD.mpa = iv::modulecomm::RegisterSend(shmFSMSkipCMD.mstrmsgname,shmFSMSkipCMD.mnBufferSize,shmFSMSkipCMD.mnBufferCount);
+//    shmFSMSkipCMD.mpa = iv::modulecomm::RegisterSend(shmFSMSkipCMD.mstrmsgname,shmFSMSkipCMD.mnBufferSize,shmFSMSkipCMD.mnBufferCount);
     ModuleFun funupdate = std::bind(&MainWindow::ListenBrainStateMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
     shmBrainState.mpa = iv::modulecomm::RegisterRecvPlus(shmBrainState.mstrmsgname,funupdate);
 
     QObject::connect(this,&MainWindow::refreshHMI_Signal,this,&MainWindow::refreshHMI_Slot);
-    QObject::connect(&signalSendTimer,&QTimer::timeout,this,&MainWindow::signalSend_Timeout);
-    if(signalSendTimer.isActive() == false)signalSendTimer.start(50);
+//    QObject::connect(&signalSendTimer,&QTimer::timeout,this,&MainWindow::signalSend_Timeout);
+//    if(signalSendTimer.isActive() == false)signalSendTimer.start(50);
 }
 
 MainWindow::~MainWindow()