change v2r logic

src/decition/common/common/car_status.h

@@ -210,8 +210,8 @@ namespace iv {
 
 
         //对于1+x车辆V2X信息，cxw
-        unsigned char rsu_traffic_type=0;//路况信息
-        unsigned char rsu_warning_type=0;//碰撞预警信息
+        int rsu_traffic_type=0;//路况信息
+       int rsu_warning_type=0;//碰撞预警信息
         int rsu_radiation_distance=0;//事件辐射范围
         int rsu_trafficelimit_spd=200;  //200代表没有限速
         int rsu_warnlimit_spd=200;  //200代表没有限速

src/decition/decition_brain_sf_1x/decition/brain.cpp

@@ -1662,7 +1662,7 @@ void iv::decition::BrainDecition::UpdateV2r(const char *pdata, const int ndatasi
     }
     if(group_message.has_radiobroadcastgpslon())
     {
-        ServiceCarStatus.rsu_gps_lat = group_message.radiobroadcastgpslon();
+        ServiceCarStatus.rsu_gps_lng= group_message.radiobroadcastgpslon();
     }
     if(group_message.has_radiolighttype()) //1:绿灯 2：红灯 3：黄灯
     {

src/decition/decition_brain_sf_1x/decition/decide_gps_00.cpp

@@ -1986,23 +1986,25 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
     //-----------------------------------------1+X采集车车路协同,add---------------------------------------------
-    unsigned char traffic_type=ServiceCarStatus.rsu_traffic_type;//路况信息
-    unsigned char warning_type=ServiceCarStatus.rsu_warning_type;//预警信息，RSU获得
-    float distance_to_center=0;
+    int traffic_type=ServiceCarStatus.rsu_traffic_type;//路况信息
+    int warning_type=ServiceCarStatus.rsu_warning_type;//预警信息，RSU获得
+    double distance_to_center=0;
     GPS_INS traffic_center_gps;
     traffic_center_gps.gps_lat=ServiceCarStatus.rsu_gps_lat;
     traffic_center_gps.gps_lng=ServiceCarStatus.rsu_gps_lng;
-    float radiation_distance=ServiceCarStatus.rsu_radiation_distance;//事件辐射范围，从RSU获得
-    float trafficlimit_spd=ServiceCarStatus.rsu_trafficelimit_spd; //从RSU获得的限速值
-    float warnlimit_spd=ServiceCarStatus.rsu_warnlimit_spd; //从RSU获得的限速值
+    int radiation_distance=ServiceCarStatus.rsu_radiation_distance;//事件辐射范围，从RSU获得
+    int trafficlimit_spd=ServiceCarStatus.rsu_trafficelimit_spd; //从RSU获得的限速值
+    int warnlimit_spd=ServiceCarStatus.rsu_warnlimit_spd; //从RSU获得的限速值
     //以上变量信息都需要存储到log文件中
+    GaussProjCal(traffic_center_gps.gps_lng,traffic_center_gps.gps_lat, &traffic_center_gps.gps_x, &traffic_center_gps.gps_y);
+    //GaussProjCal(traffic_center_gps.gps_x,  traffic_center_gps.gps_y, &traffic_center_gps.gps_lng, &traffic_center_gps.gps_lat);
    distance_to_center=GetDistance(now_gps_ins,traffic_center_gps);//因为是长直道路所以直接计算事件中心点和当前位置的距离即可
 if((traffic_type==0x01)||(traffic_type==0x02)||(traffic_type==0x03)||(traffic_type==0x04)
         ||(warning_type==0x01)||(warning_type==0x02))
 {
    if((traffic_type==0x01)||(traffic_type==0x02)||(traffic_type==0x03))//塌方，施工，道路结冰，事件范围外10米缓慢减速，事件范围内停车
     {
-       if((distance_to_center>=radiation_distance)&&(distance_to_center<=radiation_distance+10))
+       if((distance_to_center>=radiation_distance)&&(distance_to_center<=radiation_distance+10.0))
        {
           dSpeed = min(1.0,realspeed-0.2);
           ServiceCarStatus.vehicle_state_1x = 1;
@@ -2020,7 +2022,7 @@ if((traffic_type==0x01)||(traffic_type==0x02)||(traffic_type==0x03)||(traffic_ty
     }
    else if(traffic_type==0x04)//如果事件类型是限速，则在辐射范围内限速，辐射范围外正常行驶
     {
-       if((distance_to_center>=radiation_distance)&&(distance_to_center<=radiation_distance+2))
+       if((distance_to_center>=radiation_distance)&&(distance_to_center<=radiation_distance+2.0))
        {
           dSpeed=min((double)trafficlimit_spd,realspeed-0.5);//先让速度稍微减少一点
            ServiceCarStatus.vehicle_state_1x = 1;
@@ -2295,6 +2297,8 @@ else
                       <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
                       <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_lat<< ","
                       <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
+                     <<"center_GPS_X"<< ","<< setprecision(10)<<traffic_center_gps.gps_lat<< ","
+                     <<"center_GPS_Y"<< ","<< setprecision(10)<<traffic_center_gps.gps_lng<< ","
                       <<"Trace_Point"<< ","<<PathPoint<< ","
                       <<"OBS_Speed"<< ","<<obsSpeed<< ","
                       <<"TTC"<< ","<<ttc<< ","
@@ -2311,9 +2315,10 @@ else
                      "obsSpeed: %f,realspeed: %f,minDecelerate: %f,torque: %f,brake: %f,"
                      "wheel_angle: %f","Traffic_Type: %d","Warn_Type: %d","TrafficLimit_Speed: %f",
                      "radiation_dis: %f","Dis_to_center: %f","WarnLimit_Speed: %f",
-                      vehState,PathPoint,dSpeed,obsDistance,obsSpeed,realspeed,minDecelerate,
-                     gps_decition->torque,gps_decition->brake,gps_decition->wheel_angle,traffic_type,
-                     warning_type,trafficlimit_spd,radiation_distance,distance_to_center,warnlimit_spd);
+                      vehState,PathPoint,dSpeed,obsDistance,
+                     obsSpeed,realspeed,minDecelerate, gps_decition->torque,gps_decition->brake,
+                     gps_decition->wheel_angle,traffic_type,warning_type,trafficlimit_spd,
+                     radiation_distance,distance_to_center,warnlimit_spd);
        }
 
 

src/driver/driver_gps_ins550d/main.cpp

@@ -17,7 +17,8 @@ int main(int argc, char *argv[])
     std::cout<<strpath.toStdString()<<std::endl;
     iv::xmlparam::Xmlparam xp(strpath.toStdString());
 
-    std::string strdevname = xp.GetParam("devname","/dev/ttyUSB0");
+//    std::string strdevname = xp.GetParam("devname","/dev/ttyUSB0");
+    std::string strdevname = xp.GetParam("devname","/dev/ttyUSB1"); //cxw,20211021
     ins550drecv x(strdevname.data(),"ins550d_gpsimu","ins550d_gps","ins550d_imu");
     x.start();
     return a.exec();

src/v2x/CommunicatePlatform/mainwindow.cpp

@@ -19,7 +19,9 @@ MainWindow::MainWindow(QWidget *parent) :
     //ShareMem: gps
     ModuleFun fungpsimu =std::bind(&MainWindow::UpdateGPSIMU,this,std::placeholders::_1, \
                                    std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
-    mpMemGps = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",fungpsimu);
+//    mpMemGps = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",fungpsimu);
+      mpMemGps = iv::modulecomm::RegisterRecvPlus("ins550d_gpsimu",fungpsimu);
+
     //ShareMem: ui
     ModuleFun funUI =std::bind(&MainWindow::UpdateUI,this,std::placeholders::_1, \
                                    std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
@@ -31,7 +33,7 @@ MainWindow::MainWindow(QWidget *parent) :
     //ShareMem: state
     ModuleFun funState =std::bind(&MainWindow::UpdateState,this,std::placeholders::_1, \
                                    std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
-    mpMemState = iv::modulecomm::RegisterRecvPlus("brainstate",funControl);
+    mpMemState = iv::modulecomm::RegisterRecvPlus("brainstate",funState);
 }
 
 MainWindow::~MainWindow()
@@ -276,7 +278,7 @@ void MainWindow::UpdateGPSIMU(const char * strdata,const unsigned int nSize,cons
     }
     if(xgpsimu.has_heading()) {
         isSend=true;
-        m_structMGpsImu.yaw=float(xgpsimu.heading());
+        m_structMGpsImu.yaw=float(xgpsimu.heading()+180.0);
     }
     if((xgpsimu.has_ve())&&(xgpsimu.has_vn())) {
         isSend = true;
@@ -497,6 +499,7 @@ void MainWindow::UpdateControl(const char *strdata, const unsigned int nSize, co
 
 void MainWindow::UpdateState(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
 {
+    qDebug()<<"[BRAINSTATE]:INTO";
     iv::brain::brainstate xstate;
     if(!xstate.ParseFromArray(strdata,nSize))
     {

src/v2x/CommunicatePlatform/radio.cpp

@@ -155,9 +155,9 @@ void Radio::serialPortInit()
 //    }
 
     m_serialPort_Radio = new QSerialPort();
-    m_serialPort_Radio->setPortName("/dev/ttyUSB1");
+    m_serialPort_Radio->setPortName("/dev/ttyUSB0");
     //m_serialPort_Radio->setPortName("/dev/pts/1");
-    m_serialPort_Radio->setBaudRate(QSerialPort::Baud9600);
+    m_serialPort_Radio->setBaudRate(QSerialPort::Baud115200);
     m_serialPort_Radio->setParity(QSerialPort::NoParity);
     m_serialPort_Radio->setDataBits(QSerialPort::Data8);
     m_serialPort_Radio->setStopBits(QSerialPort::OneStop);