Merge branch 'master' of http://191.168.14.36:3000/adc_pilot/modularization

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

@@ -171,7 +171,7 @@ iv::decition::Decition iv::decition::HapoAdapter::getAdapterDeciton(GPS_INS now_
     }
 
     //0227 10m nei xianzhi shache
-    if(obsDistance<10 &&obsDistance>0){
+    if(obsDistance<6 &&obsDistance>0){
         controlSpeed=0;
         controlBrake=max(controlBrake,0.8f);
     }

src/decition/decition_brain/decition/adc_tools/compute_00.cpp

@@ -107,11 +107,11 @@ int iv::decition::Compute00::getDesiredSpeed(std::vector<GPSData> gpsMap)
                                 {
                                 if(MarkingCount<150)
                                 {
-                                     if((BackAveFive-FrontAveFive)<4)
+                                     if((BackAveFive-FrontAveFive)<=4.0)
                                      {
                                            (*gpsMap[MarkingIndex]).roadMode=14;   //弯道，2米,14
                                      }
-                                     else if((BackAveFive-FrontAveFive)>4)
+                                     else if((BackAveFive-FrontAveFive)>4.0)
                                      {
                                            (*gpsMap[MarkingIndex]).roadMode=15;
                                      }

src/decition/decition_brain/decition/decide_gps_00.cpp

@@ -1239,6 +1239,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }else if (gpsMapLine[PathPoint]->roadMode == 11)
     {
         dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode11,dSpeed);
+        gps_decition->leftlamp = false;
+        gps_decition->rightlamp = false;
 
     }else if (gpsMapLine[PathPoint]->roadMode == 14)
     {
@@ -1258,18 +1260,21 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     {
         // dSpeed = min(10.0,dSpeed);
         dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode16,dSpeed);
-        gps_decition->leftlamp = true;
-        gps_decition->rightlamp = false;
+        //gps_decition->leftlamp = true;
+        //gps_decition->rightlamp = false;
     }else if (gpsMapLine[PathPoint]->roadMode == 17)
     {
         //  dSpeed = min(10.0,dSpeed);
         dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode17,dSpeed);
-        gps_decition->leftlamp = false;
-        gps_decition->rightlamp = true;
+        //gps_decition->leftlamp = false;
+        //gps_decition->rightlamp = true;
     }else if (gpsMapLine[PathPoint]->roadMode == 18)
     {
         // dSpeed = min(10.0,dSpeed);
         dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode18,dSpeed);
+        gps_decition->leftlamp = false;
+        gps_decition->rightlamp = false;
+
         /*if(gpsMapLine[PathPoint]->light_left_or_right == 1)
         {
             gps_decition->leftlamp = true;

src/detection/detection_radar_delphi_esr_send/can_send_data_struct.h

@@ -12,11 +12,11 @@ struct canBit4F0
     //2
     unsigned char canYawRateL : 8;//<(+)clockwise deg/s
     //3
-    unsigned char canRadiusCurvatureH: 6;//<(+)clockwise m set:p8191
+    unsigned char canRadiusCurvatureH: 6;//<(+)clockwise m set:(8191)
     unsigned char canSteeringAngleRateSign : 1;//<(0)turn left (counterclockwise)
     unsigned char canYawRateValid : 1;//<(1)valid
     //4
-    unsigned char canRadiusCurvatureL: 8;//<(+)clockwise m set:p8191
+    unsigned char canRadiusCurvatureL: 8;//<(+)clockwise m (8191)
     //5
     unsigned char canSteeringAngleH : 6;//<deg
     unsigned char canSteeringAngleSign : 1;//<(0)turn left (counterclockwise)
@@ -53,13 +53,13 @@ struct canBit4F1
     //5
     unsigned char canLateralMountingOffset : 8;
     //6
-    unsigned char canMaximumTracks : 6;
+    unsigned char canMaximumTracks : 6; // <64
     unsigned char canBlockageDisable : 1;
-    unsigned char canRadarCmdRadiate : 1;
+    unsigned char canRadarCmdRadiate : 1; //<1
     //7
     unsigned char canRawDataEnable : 1;
     unsigned char canWiperStatus : 1;
-    unsigned char canGroupingMode : 2;
+    unsigned char canGroupingMode : 2;//<3
     unsigned char canMmrUpsideDown : 1;
     unsigned char canVehicleSpeedValid : 1;
     unsigned char canTurnSignalStatus : 2;

src/detection/detection_radar_delphi_esr_send/main.cpp

@@ -3,8 +3,8 @@
 #include <iostream>
 #include <QDateTime>
 #include <math.h>
-
 #include <thread>
+#include <linux/timer.h>
 
 #include "modulecomm.h"
 #include "xmlparam.h"
@@ -15,37 +15,27 @@
 #include "radarobjectarray.pb.h"
 #include "gpsimu.pb.h"
 #include "can_car.pb.h"
+#include "can_send_data_struct.h"
 
 /******************************************/
 #define fujiankuan    //<宏开关
 
 std::string gstrmemgpsimu;
 std::string gstrmemcancar;
-
 static uint16_t CAN_Scan_Index;
-static int8_t CAN_Lateral_Mounting_Offset = 0;
-static int8_t CAN_Angle_Misalignment = 0;
-static uint8_t CAN_Max_Tracks = 64; //<max number of track objects
-static uint8_t CAN_Cmd_Radiate = 1; //<
-static uint8_t CAN_Wiper_Status = 0; //<(0)off
-static uint8_t CAN_Raw_Data_Enable = 0; //<(0)filtered radar data(1)raw radar data
-static uint8_t CAN_Grouping_Mode = 3; //<(0)no(1)grouping moving target(2)grouping stationary target(3)grouping both target
-static uint8_t CAN_Upside_Down = 0; //<(0)normal setup
-static uint8_t CAN_Turn_Signal_Status = 0; //<(0)none(1)left(2)right
-static uint8_t CAN_Blockage_Disable = 0; //<used in EMC testing. should be set 0 normally
-static uint8_t CAN_Use_Angle_Misalignment = 0; //<
-static uint8_t CAN_Clear_Faults = 0; //<clear faults codes
-static uint8_t CAN_High_Yaw_Angle = 0; //<
-static uint8_t CAN_LR_Only_Transmit = 0; //<
-static uint8_t CAN_MR_Only_Transmit = 0; //<
-static uint8_t CAN_VOLVO_Short_Track_Roc = 0; //<
-
 static double Vehicle_Yaw_Rate;
 
 static double Vehicle_Speed;        //<
 static uint8_t CAN_Vehicle_Speed_Valid; //<(1)valid
 
 static double Steering_Angle;
+static int16_t CAN_Radius_Curvature = 8191;
+/******************************************/
+
+canSend4F0 canData4f0;
+canSend4F1 canData4f1;
+canSend5F2 canData5f2;
+canSend5F4 canData5f4;
 
 /******************************************/
 
@@ -61,19 +51,20 @@ QTime gTime;
 int gnRadarState = 0;
 
 
+
 #ifdef fujiankuan
 /****==================================================================================***/
 static int gnIndex = 0;
 
-static unsigned char tmp1[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0xBF,0x00};
+//static unsigned char tmp1[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0xBF,0x00};
 static unsigned char canbyte[8];
 
-void ExecSend1(uint16_t id)
+void ExecSend1(uint16_t id, unsigned char canData[8],int size)
 {
     iv::can::canmsg xmsg;
     iv::can::canraw xraw;
 
-    memcpy(canbyte,tmp1,sizeof(tmp1));
+    memcpy(canbyte,canData,size);
 
     xraw.set_id(id);
     xraw.set_data(canbyte,8);
@@ -322,15 +313,15 @@ void DecodeRadar(iv::can::canmsg xmsg)
             memcpy(cdata,canmsg.data().data(),8);
             
             int16_t valid_range = (cdata[1] << 8) | cdata[2];
-            gobj.valid_lr_range(valid_range / 128.0);
+            gobj.set_valid_lr_range(valid_range / 128.0);
 
             int16_t valid_range_rate = (cdata[3] << 8) | cdata[4];
-            gobj.valid_lr_range_rate(valid_range_rate / 128.0);
+            gobj.set_valid_lr_range_rate(valid_range_rate / 128.0);
 
             int16_t valid_angle = (cdata[5] << 8) | cdata[6];
-            gobj.valid_lr_angle(valid_angle / 16.0);
+            gobj.set_valid_lr_angle(valid_angle / 16.0);
 
-            gobj.valid_lr_power(cdata[7]);
+            gobj.set_valid_lr_power(cdata[7]);
             
             gntemp++;
         }
@@ -343,15 +334,15 @@ void DecodeRadar(iv::can::canmsg xmsg)
             memcpy(cdata,canmsg.data().data(),8);
             
             int16_t valid_range = (cdata[1] << 8) | cdata[2];
-            gobj.valid_mr_range(valid_range / 128.0);
+            gobj.set_valid_mr_range(valid_range / 128.0);
 
             int16_t valid_range_rate = (cdata[3] << 8) | cdata[4];
-            gobj.valid_mr_range_rate(valid_range_rate / 128.0);
+            gobj.set_valid_mr_range_rate(valid_range_rate / 128.0);
 
             int16_t valid_angle = (cdata[5] << 8) | cdata[6];
-            gobj.valid_mr_angle(valid_angle / 16.0);
+            gobj.set_valid_mr_angle(valid_angle / 16.0);
 
-            gobj.valid_mr_power(cdata[7]);
+            gobj.set_valid_mr_power(cdata[7]);
             
             gntemp++;
         }
@@ -436,6 +427,24 @@ void threadstate()
 
     }
 }
+void onTimerSend()
+{
+    unsigned int speed = (unsigned int)(Vehicle_Speed / 0.0625);
+    int yawRate = (int)(Vehicle_Yaw_Rate / 0.0625);
+    canData4f0.bit.canVehicleSpeedH = speed >> 8;
+    canData4f0.bit.canVehicleSpeedL = speed & 0xFF;
+    canData4f0.bit.canYawRateH = yawRate >> 8;
+    canData4f0.bit.canYawRateL = yawRate & 0xFF;
+    canData4f0.bit.canRadiusCurvatureH = CAN_Radius_Curvature >> 8;
+    canData4f0.bit.canRadiusCurvatureL = CAN_Radius_Curvature & 0x3F;
+    ExecSend1(0x4f0,canData4f0.byte,8);
+
+    canData4f1.bit.canScanIndexAckH = CAN_Scan_Index >> 8;
+    canData4f1.bit.canScanIndexAckL = CAN_Scan_Index & 0xFF;
+    ExecSend1(0x4f1,canData4f1.byte,8);
+    ExecSend1(0x5f2,canData5f2.byte,8);
+    ExecSend1(0x5f4,canData5f4.byte,8);
+}
 int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
@@ -453,11 +462,21 @@ int main(int argc, char *argv[])
         xobj->set_vel(0);
         xobj->set_bvalid(false);
     }
-
     gobj.CopyFrom(x);
 
     gTime.start();
 
+    memset(canData4f0.byte,0,8);
+    memset(canData4f1.byte,0,8);
+    Vehicle_Yaw_Rate = 0;
+    Vehicle_Speed = 0;
+    canData4f1.bit.canMaximumTracks = 64;
+    canData4f1.bit.canRadarCmdRadiate = 1;
+    canData4f1.bit.canGroupingMode = 3;
+    canData4f1.bit.canVehicleSpeedValid = 1;
+    memset(canData5f2.byte,0,8);
+    memset(canData5f4.byte,0,8);
+
     QString strpath = QCoreApplication::applicationDirPath();
     if(argc < 2)
         strpath = strpath + "/detection_radar_esr.xml";
@@ -492,10 +511,12 @@ int main(int argc, char *argv[])
     void * pc = iv::modulecomm::RegisterRecv(gstrmemcancar.data(),Listencancar);
 #endif
 
-    ExecSend1(0x04f1);
-
     std::thread threadfault(threadstate);
 
+    QTimer * timer = new QTimer(); //car数据发送定时器，每隔30ms发送一次
+//    connect(timer,SIGNAL(timeout()),this,SLOT(onTimerSend()));
+    timer->setTimerType(Qt::PreciseTimer);
+    timer->start(20);
 
     return a.exec();
 }

src/include/proto/radarobject.proto

@@ -18,8 +18,8 @@ message radarobject
   optional bool Grouping_Changed =12; //组别变化
   optional bool Oncoming =13; //相向移动
   optional double Lat_Rate =14; //横向速度，极坐标，逆时针正，米每秒
-  optional bytes Med_Range_Mode =15; //目标被追踪模式，0无追踪，1中距雷达追踪，2长距雷达追踪，3中、长距追踪
-  optional bytes Track_Status =16; //追踪状态，0无目标，1新的目标，2新的更新过的目标，3更新过的目标，4平稳运动的目标，5渐融的目标，6不可信的平稳运动目标，7新的平稳运动目标
+  optional int32 Med_Range_Mode =15; //目标被追踪模式，0无追踪，1中距雷达追踪，2长距雷达追踪，3中、长距追踪
+  optional int32 Track_Status =16; //追踪状态，0无目标，1新的目标，2新的更新过的目标，3更新过的目标，4平稳运动的目标，5渐融的目标，6不可信的平稳运动目标，7新的平稳运动目标
   optional bool Bridge_Object =17; //桥梁标志
   optional bool Moving =18; //运动
   optional bool Fast_Movable =19; //快速运动

src/include/proto/radarobjectarray.proto

@@ -9,19 +9,19 @@ message radarobjectarray
 {
   repeated radarobject obj = 1;
   optional int64 mstime = 2;
-  optional bytes ACC_Sta_Obj_ID =3; //路线上自适应巡航静态或相对靠近目标推荐id
-  optional bytes ACC_Mov_Obj_ID =4; //路线上自适应巡航动态或可动目标推荐id
-  optional bytes CMbB_Sta_Obj_ID =5; //路线上刹车减缓碰撞静态目标推荐id
-  optional bytes CMbB_Mov_Obj_ID =6; //路线上刹车减缓碰撞动态目标推荐id
-  optional bytes FCW_Sta_Obj_ID =7; //路线上前碰撞预警静态目标推荐id
-  optional bytes FCW_Mov_Obj_ID =8; //路线上前碰撞预警动态目标推荐id
+  optional int32 ACC_Sta_Obj_ID =3; //路线上自适应巡航静态或相对靠近目标推荐id
+  optional int32 ACC_Mov_Obj_ID =4; //路线上自适应巡航动态或可动目标推荐id
+  optional int32 CMbB_Sta_Obj_ID =5; //路线上刹车减缓碰撞静态目标推荐id
+  optional int32 CMbB_Mov_Obj_ID =6; //路线上刹车减缓碰撞动态目标推荐id
+  optional int32 FCW_Sta_Obj_ID =7; //路线上前碰撞预警静态目标推荐id
+  optional int32 FCW_Mov_Obj_ID =8; //路线上前碰撞预警动态目标推荐id
   optional bool Grating_Lobe_Det =9; //acc目标为栅瓣目标，可能并非路线上最近物体，需谨慎判断
   optional bool Truck_Target_Det =10; //acc目标是卡车类，可能并非路线上的最近物体，需谨慎判断
 
-  optional bytes Water_Spray_Target_ID = 11; //最近的溅水目标，该目标可能是虚影（雨雪），也可能是实物（车轮溅水）
+  optional int32 Water_Spray_Target_ID = 11; //最近的溅水目标，该目标可能是虚影（雨雪），也可能是实物（车轮溅水）
   optional double Filtered_XOHP_ACC_CIPV = 12; //路线上最近ACC目标的滤波后的在主车驶路线上的横向偏移，单位米
-  optional bytes Path_ID_ACC_2 = 13; //路线上第二近ACC目标id
-  optional bytes Path_ID_ACC_3 = 14; //路线上第三近ACC目标id
+  optional int32 Path_ID_ACC_2 = 13; //路线上第二近ACC目标id
+  optional int32 Path_ID_ACC_3 = 14; //路线上第三近ACC目标id
 
   optional bool Internal_Error = 15; //雷达内部错误标志，0为正常
   optional bool Range_Perf_Error = 16; //雷达受遮挡错误标志，0为正常