ui&brain&platform

src/decition/common/common/car_status.h

@@ -222,6 +222,7 @@ namespace iv {
         //0x2: 绿色
         int vehicle_state_1x=0; //0正常行驶，1减速，2 停车
         float target_spd_1x=0;//减速时的目标速度
+        float ui_limit_spd=200;
 
 //        float rsu_limit_spd=200;  //200代表没有限速
         //int iTrafficeLightColor = 2;    //0x0: 红色

src/decition/decition_brain_sf_1x/decition/brain.cpp

@@ -1328,6 +1328,10 @@ void iv::decition::BrainDecition::UpdateHMI(const char *pdata, const int ndatasi
 
 //      std::cout<<"enter  for butn %d  "<<ServiceCarStatus.mbRunPause <<std::endl;
     }
+    if(xhmimsg.has_speedlimit())
+    {
+         ServiceCarStatus.ui_limit_spd = xhmimsg.speedlimit();
+    }
 //    ServiceCarStatus.mbRunPause = xhmimsg.mbpause();
 
     if(xhmimsg.mbbochemode()){

src/decition/decition_brain_sf_1x/decition/decide_gps_00.cpp

@@ -280,8 +280,19 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
 //        getV2XTrafficPositionVector(gpsMapLine);//20211029,cxw, 1+x中只有一个红绿灯信息所以还是用原来的，直接取经纬度
-        for (int var = 0; var < gpsMapLine.size(); var++)
+        for (unsigned int var = 0; var < gpsMapLine.size(); var++)
         {
+             if(ServiceCarStatus.txt_log_on==true)
+             {
+               ofstream outfile;
+                outfile.open("map.txt", ostream::app);
+
+//                outfile<<"***********************the vehicle at map start point!*************************"<<endl;
+                //outfile<<"the number of lap  is "<<":    "<<file_num<<""<<endl;
+                outfile<<"map_num"<<","<<var<<","
+                       <<"mode2"<< ","  <<gpsMapLine[var]->mode2<< ","<<endl;
+             }
+
             if(gpsMapLine[var]->roadMode==6|| gpsMapLine[var]->mode2==1000001)
             {
                 //v2xTrafficVector.push_back(var);
@@ -290,6 +301,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 break;
             }
         }
+
+
         //        group_ori_gps=*gpsMapLine[0];
         ServiceCarStatus.bocheMode=false;
         ServiceCarStatus.daocheMode=false;
@@ -304,6 +317,19 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                         0,0);
     }
 
+//    for (int var = 0; var < gpsMapLine.size(); var++)
+//    {
+//        if(gpsMapLine[var]->roadMode==6|| gpsMapLine[var]->mode2==1000001)
+//        {
+//            //v2xTrafficVector.push_back(var);
+//            ServiceCarStatus.iTrafficeLightLat=gpsMapLine[var]->gps_lat;
+//            ServiceCarStatus.iTrafficeLightLon=gpsMapLine[var]->gps_lng;
+//            break;
+//        }
+
+//        int a=0;
+//    }
+
 
     if(ServiceCarStatus.msysparam.gpsOffset_y!=0 || ServiceCarStatus.msysparam.gpsOffset_x!=0){
         GPS_INS gpsOffset = Coordinate_UnTransfer(ServiceCarStatus.msysparam.gpsOffset_x, ServiceCarStatus.msysparam.gpsOffset_y, now_gps_ins);
@@ -2016,7 +2042,7 @@ if((traffic_type==0x01)||(traffic_type==0x02)||(traffic_type==0x03)||(traffic_ty
     {
        if((distance_to_center>=radiation_distance)&&(distance_to_center<=radiation_distance+10.0))
        {
-          dSpeed = min(1.0,realspeed-0.2);
+          dSpeed = max(1.0,realspeed-0.2);
           ServiceCarStatus.vehicle_state_1x = 1;
           ServiceCarStatus.target_spd_1x = 1.0;
        }
@@ -2028,27 +2054,36 @@ if((traffic_type==0x01)||(traffic_type==0x02)||(traffic_type==0x03)||(traffic_ty
            ServiceCarStatus.target_spd_1x=0.0;
        }
        else
-       {}
+       {
+          ServiceCarStatus.vehicle_state_1x = 0;
+          ServiceCarStatus.target_spd_1x=dSpeed;
+       }
     }
    else if(traffic_type==0x04)//如果事件类型是限速，则在辐射范围内限速，辐射范围外正常行驶
     {
        if((distance_to_center>=radiation_distance)&&(distance_to_center<=radiation_distance+2.0))
        {
-          dSpeed=min((double)trafficlimit_spd,realspeed-0.5);//先让速度稍微减少一点
+          dSpeed=max((double)trafficlimit_spd,realspeed-0.5);//先让速度稍微减少一点
            ServiceCarStatus.vehicle_state_1x = 1;
            ServiceCarStatus.target_spd_1x = trafficlimit_spd;
        }
        else if(distance_to_center<radiation_distance)
        {
-          dSpeed=min((double)trafficlimit_spd,realspeed);
+          dSpeed=max((double)trafficlimit_spd,realspeed);
            ServiceCarStatus.vehicle_state_1x = 1;
            ServiceCarStatus.target_spd_1x = trafficlimit_spd;
        }
        else
-       {}
+       {
+           ServiceCarStatus.vehicle_state_1x = 0;
+           ServiceCarStatus.target_spd_1x=dSpeed;
+       }
     }
     else
-    {}
+    {
+//       ServiceCarStatus.vehicle_state_1x = 0;
+//       ServiceCarStatus.target_spd_1x=dSpeed;
+   }
 //碰撞预警，1减速，2 停车
     if(warning_type==0x01)
     {
@@ -2063,7 +2098,10 @@ if((traffic_type==0x01)||(traffic_type==0x02)||(traffic_type==0x03)||(traffic_ty
          ServiceCarStatus.target_spd_1x = 0;
     }
     else
-    {}
+    {
+//        ServiceCarStatus.vehicle_state_1x = 0;
+//        ServiceCarStatus.target_spd_1x=dSpeed;
+    }
 }
 else
 {
@@ -2077,11 +2115,10 @@ else
     //-------------------------------traffic light----------------------------------------------------------------------------------------
 
 //ServiceCarStatus.milightCheckTimer.elapsed();
-if(ServiceCarStatus.milightCheckTimer.elapsed()>3)
+if(ServiceCarStatus.milightCheckTimer.elapsed()>5*1000)
 {
    ServiceCarStatus.iTrafficeLightColor=1;//lvdeng
    ServiceCarStatus.iTrafficeLightTime=200;
-
 }
 
 
@@ -2138,12 +2175,15 @@ if(ServiceCarStatus.milightCheckTimer.elapsed()>3)
     //需要增加接收到路册单元限速或停车的命令后对dspeed或这minDecelerate做进一步限制，仅限于hunter车辆
     //当收到停车命令后，直接给出最小减速度
     //收到减速命令后，直接将目标速度给成限速值
+    dSpeed = min((double)ServiceCarStatus.ui_limit_spd,dSpeed);//cxw,20211126,ui  xiansu
     phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
 
 
     givlog->debug("decition_brain","vehState: %d,PathPoint: %d,dSpeed: %f,obsDistance: %f,obsSpeed: %f,realspeed: %f,minDecelerate: %f,torque: %f,brake: %f,wheel_angle: %f,obs_speed_for_avoid: %f,mode2: %d",
             vehState,PathPoint,dSpeed,obsDistance,obsSpeed,realspeed,minDecelerate,gps_decition->torque,gps_decition->brake,gps_decition->wheel_angle,obs_speed_for_avoid,gpsMapLine[PathPoint]->mode2);
 
+
+    givlog->debug("decition_brain","decition_mode: %d",gps_decition->mode);
    //shuju cunchu gognnenng add,20210624,cxw
    if(ServiceCarStatus.txt_log_on==true)
    {
@@ -2324,10 +2364,13 @@ if(ServiceCarStatus.milightCheckTimer.elapsed()>3)
                        <<"Traffic_Type"<< ","<<traffic_type<< ","
                        <<"Warn_Type"<< ","<<warning_type<< ","
                        <<"TrafficLimit_Speed"<< ","<<trafficlimit_spd<< ","
+                        <<"WarnLimit_Speed"<< ","<<warnlimit_spd<< ","
                        <<"Radation_Dis"<< ","<<radiation_distance<< ","
                        <<"Dis_to_center"<< ","<<distance_to_center<< ","
                        <<"Light_colour"<< ","<<ServiceCarStatus.iTrafficeLightColor<< ","
                        <<"Light_time"<< ","<<ServiceCarStatus.iTrafficeLightTime<< ","
+                      <<"Light_GPS_X"<< ","<< setprecision(10)<<traffic_light_gps.gps_lat<< ","
+                      <<"Light_GPS_Y"<< ","<< setprecision(10)<<traffic_light_gps.gps_lng<< ","
                       <<endl;
                outfile.close();
            }
@@ -2335,9 +2378,11 @@ if(ServiceCarStatus.milightCheckTimer.elapsed()>3)
        givlog->debug("decition_brain","vehState: %d,PathPoint: %d,dSpeed: %f,obsDistance: %f,obsSpeed: %f,realspeed: %f,minDecelerate: %f,torque: %f,brake: %f,wheel_angle: %f",
                       vehState,PathPoint,dSpeed,obsDistance,obsSpeed,realspeed,minDecelerate, gps_decition->torque,gps_decition->brake,
                      gps_decition->wheel_angle);
-       givlog->debug("decition_brain","Traffic_Type: %d,Warn_Type: %d,TrafficLimit_Speed: %f,radiation_dis: %f,Dis_to_center: %f,WarnLimit_Speed: %f",
+       givlog->debug("decition_brain","Traffic_Type: %d,Warn_Type: %d,TrafficLimit_Speed: %d,radiation_dis: %d,Dis_to_center: %f,WarnLimit_Speed: %d",
                      traffic_type,warning_type,trafficlimit_spd,radiation_distance,distance_to_center,warnlimit_spd);
        givlog->debug("decition_brain","Light_colour: %d,Light_time: %d",ServiceCarStatus.iTrafficeLightColor,ServiceCarStatus.iTrafficeLightTime);
+
+       givlog->debug("decition_brain","Light_GPS_X: %f,Light_GPS_Y: %f",traffic_light_gps.gps_lat,traffic_light_gps.gps_lng);
    }
 
 
@@ -3776,7 +3821,11 @@ float  iv::decition::DecideGps00::ComputeTrafficLightSpeed(int traffic_light_col
     //    }
 
 
-    if(traffic_light_color==0 && traffic_dis<10){
+//    if(traffic_light_color==0 && traffic_dis<10){
+//        traffic_speed=0;
+//    }
+
+    if(traffic_light_color==2 && traffic_dis<10){  //cxw,1xhonglvdeng gaibian
         traffic_speed=0;
     }
     //    else   //20200108

src/v2x/CommunicatePlatform/mainwindow.cpp

@@ -540,19 +540,23 @@ void MainWindow::on_pushButton_warning_clicked()
 {
     collisionEarlyWarningMessage warning;
     warning.warningType=0x01;
-    warning.speedLimit=30;
+    warning.speedLimit=3;
     m_radio->outCollisionWarning(warning);
 }
 
 void MainWindow::on_pushButton_realTimeMessage_clicked()
 {
-    realtimeTrafficMessage realtime;
-    realtime.lat=39*1000000;
-    realtime.lng=137*1000000;
-    realtime.scope=100;
-    realtime.trafficInfo=0x01;
-    realtime.speedLimit=0xFF;
-    m_radio->outRealtimeTraffic(realtime);
+//    realtimeTrafficMessage realtime;
+//    realtime.lat=39*1000000;
+//    realtime.lng=137*1000000;
+//    realtime.scope=100;
+//    realtime.trafficInfo=0x01;
+//    realtime.speedLimit=0xFF;
+//    m_radio->outRealtimeTraffic(realtime);
+    collisionEarlyWarningMessage warning;
+    warning.warningType=0x02;
+    warning.speedLimit=0;
+    m_radio->outCollisionWarning(warning);
 }
 
 void MainWindow::on_pushButton_busy_clicked()