fix(esr): 增加esr解析内容，修正速度解析

增加esr解析内容，修正速度解析错误，增加车速偏航转速等信息的发送，但尚未完全编写完成

footer

src/detection/detection_radar_delphi_esr_send/main.cpp

@@ -17,12 +17,48 @@
 #include "can_car.pb.h"
 
 /******************************************/
-#define fujiankuan    //宏开关
+#define fujiankuan    //<宏开关
 
 std::string gstrmemgpsimu;
 std::string gstrmemcancar;
-double g_gyro_z;   //
-double g_v;        //
+
+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 int16_t CAN_Yaw_Rate; //<(+)clockwise deg/s
+static uint8_t CAN_Yaw_Rate_Valid; //<(1)valid
+
+static double Vehicle_Speed;        //<
+static uint16_t CAN_Vehicle_Speed; //< m/s
+static uint8_t CAN_Vehicle_Speed_Direction; //<(0)forward
+static uint8_t CAN_Vehicle_Speed_Valid; //<(1)valid
+
+static int16_t CAN_Radius_Curvature = 8191; //<(+)clockwise m
+
+static double Steering_Angle;
+static uint16_t CAN_Steering_Angle; //<deg
+static uint8_t CAN_Steering_Angle_Sign; //<(0)turn left (counterclockwise)
+static uint8_t CAN_Steering_Angle_Valid; //<(1)valid
+
+static uint16_t CAN_Steering_Angle_Rate; //<deg/s
+static uint8_t CAN_Steering_Angle_Rate_Sign; //<(0)turn left (counterclockwise)
+
 /******************************************/
 
 iv::Ivlog * givlog;
@@ -39,10 +75,10 @@ int gnRadarState = 0;
 
 #ifdef fujiankuan
 /****==================================================================================***/
-int gnIndex = 0;
+static int gnIndex = 0;
 
-unsigned char tmp1[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0xBF,0x00};
-unsigned char canbyte[8];
+static unsigned char tmp1[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0xBF,0x00};
+static unsigned char canbyte[8];
 
 void ExecSend1(uint16_t id)
 {
@@ -85,8 +121,8 @@ void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned i
         givlog->warn("detection_radar_delphi_esr_send--Listengpsimu parse errror. nSize is %d",nSize);
         return;
     }
-    g_gyro_z = xgpsimu.gyro_z();
-    g_v = sqrt(pow(xgpsimu.vn(),2) + pow(xgpsimu.ve(),2));
+    Vehicle_Yaw_Rate = xgpsimu.gyro_z();
+    Vehicle_Speed = sqrt(pow(xgpsimu.vn(),2) + pow(xgpsimu.ve(),2));
 }
 
 void Listencancar(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
@@ -99,7 +135,7 @@ void Listencancar(const char * strdata,const unsigned int nSize,const unsigned i
         givlog->warn("detection_radar_delphi_esr_send--Listencancar parse errror. nSize is %d",nSize);
         return;
     }
-   // g_v = xcancar.v();
+   // Vehicle_Speed = xcancar.v();
 }
 
 /****==================================================================================***/
@@ -131,7 +167,7 @@ void ShareResult()
 
 void DecodeRadar(iv::can::canmsg xmsg)
 {
-    int32_t range, rate, angle;
+//    int32_t range, rate, angle;
     if(xmsg.rawmsg_size() < 1)return;
 
     int i;
@@ -139,7 +175,60 @@ void DecodeRadar(iv::can::canmsg xmsg)
     {
         iv::can::canraw canmsg = xmsg.rawmsg(i);
 
-        if((canmsg.id() >= 0x500)&&(canmsg.id() <= 0x53f))
+        if(canmsg.id() == 0x4E0)
+        {
+            gnRadarState = 10;
+
+            unsigned char cdata[8];
+            memcpy(cdata,canmsg.data().data(),8);
+
+            CAN_Scan_Index = (cdata[3] << 8) | cdata[4];
+
+            gntemp++;
+
+            if(gntemp > 10)
+            {
+            gobj.set_mstime(QDateTime::currentMSecsSinceEpoch());
+            ShareResult();
+            gntemp = 0;
+            }
+
+        }
+
+        if(canmsg.id() == 0x4E1)
+        {
+            gnRadarState = 10;
+
+            unsigned char cdata[8];
+            memcpy(cdata,canmsg.data().data(),8);
+
+            gobj.set_internal_error((cdata[1] >> 5) & 0x01);
+            gobj.set_range_perf_error((cdata[1] >> 6) & 0x01);
+            gobj.set_overheat_error((cdata[1] >> 7) & 0x01);
+
+            gntemp++;
+        }
+
+        if(canmsg.id() == 0x4E3)
+        {
+            gnRadarState = 10;
+
+            unsigned char cdata[8];
+            memcpy(cdata,canmsg.data().data(),8);
+
+            gobj.set_acc_sta_obj_id(cdata[7]);
+            gobj.set_acc_mov_obj_id(cdata[1]);
+            gobj.set_cmbb_mov_obj_id(cdata[2]);
+            gobj.set_cmbb_sta_obj_id(cdata[3]);
+            gobj.set_fcw_mov_obj_id(cdata[4]);
+            gobj.set_fcw_sta_obj_id(cdata[5]);
+            gobj.set_grating_lobe_det((cdata[0] >> 6) & 0x01);
+            gobj.set_truck_target_det((cdata[0] >> 7) & 0x01);
+
+            gntemp++;
+        }
+
+        if((canmsg.id() >= 0x500)&&(canmsg.id() <= 0x53F))
         {
             gnRadarState = 10;
             int  data[8];
@@ -151,57 +240,168 @@ void DecodeRadar(iv::can::canmsg xmsg)
             int j;
             for(j=0;j<8;j++)data[j] = cdata[j];
 
-            angle = ((data[1] & 0x1F) << 5) + ((data[2] & 0xF8) / 8);
-            range = ((data[2] & 0x07) << 8) + data[3];
-            rate = ((data[6] & 0x3F) << 8) | data[7];
-            if (angle & 0x200) {
-                angle = angle | 0xFFFFFC00;
-            }
-            if (rate & 0x2000) {
-                rate = rate | 0xFFFFC000;
-            }
- //           qDebug("range = %d angle = %d ",range,angle);
-            //If angle and range both are 0, it is an invalid data.
-            if (angle != 0 || range != 0) {
-                iv::radar::radarobject * pobj = gobj.mutable_obj(radar_ID_index);
+            iv::radar::radarobject * pobj = gobj.mutable_obj(radar_ID_index);
+
+            int16_t range = ((data[2] & 0x07) << 8) | data[3];
+            pobj->set_range(range * 0.1);
+
+            int16_t range_rate = (((data[6] & 0x3F) << 8) | data[7]) << 2;
+            range_rate = range_rate >> 2;
+            pobj->set_range_rate(range_rate * 0.01);
+
+            int16_t range_accel = (((data[4] & 0x03) << 8) | data[5]) << 6;
+            range_accel = range_accel >> 6;
+            pobj->set_range_accel(range_accel * 0.05);
+
+            int16_t angle = (((data[1] & 0x1F) << 8) | (data[2] & 0xF8)) << 3;
+            angle = angle >> 6;
+            pobj->set_angle(angle * 0.1);
+
+            int8_t width = (data[4] & 0x3C) >> 2;
+            pobj->set_width(width * 0.5);
+
+            pobj->set_grouping_changed((data[0] & 0x02) >> 1);
+            pobj->set_oncoming(data[0] & 0x01);
+
+            int8_t lat_rate = data[0] & 0xFC;
+            lat_rate = lat_rate >> 2;
+            pobj->set_lat_rate(lat_rate * 0.25);
+
+            if (angle != 0 || range != 0)
+            {
+                double rad_angle = pobj->angle() / 180.0 * M_PI;
                 pobj->set_bvalid(true);
-                pobj->set_x(range * 0.1 * sin(angle * 1.0 / 1800.0 * M_PI));
-                pobj->set_y(range * 0.1 * cos(angle * 1.0 / 1800.0 * M_PI));
-                pobj->set_vel(rate * 1.0 / 100.0);
-                pobj->set_vx(pobj->vel() * sin(angle / 1800.0 * M_PI));
-                pobj->set_vy(pobj->vel() * cos(angle / 1800.0 * M_PI));
+                pobj->set_x(pobj->range() * sin(rad_angle));
+                pobj->set_y(pobj->range() * cos(rad_angle));
+                pobj->set_vel(sqrt(pobj->range_rate() * pobj->range_rate() + pobj->lat_rate() * pobj->lat_rate()));
+                pobj->set_vx(pobj->range_rate() * sin(rad_angle) - pobj->lat_rate() * cos(rad_angle));
+                pobj->set_vy(pobj->range_rate() * cos(rad_angle) + pobj->lat_rate() * sin(rad_angle));
             }
-            else {
-                iv::radar::radarobject * pobj = gobj.mutable_obj(radar_ID_index);
+            else
+            {
                 pobj->set_bvalid(false);
             }
 
+            pobj->set_med_range_mode((data[6] >> 6) & 0x03);
+
+            pobj->set_track_status((data[1] >> 5) & 0x07);
+
+            pobj->set_bridge_object((data[4] >> 7) & 0x01);
+
             gntemp++;
 
-            if(canmsg.id() == 0x53f)
+        }
+
+        if(canmsg.id() == 0x540)
+        {
+            gnRadarState = 10;
+
+            unsigned char cdata[8];
+            memcpy(cdata,canmsg.data().data(),8);
+
+            uint8_t group_id = cdata[0] & 0x0F;
+
+            if(group_id < 9)
             {
-               gobj.set_mstime(QDateTime::currentMSecsSinceEpoch());
-               ShareResult();
-                gntemp = 0;
-                //ok
+                for(int i=0;i<7;i++)
+                {
+                    iv::radar::radarobject * pobj = gobj.mutable_obj(i + group_id * 7);
+                    int8_t power = cdata[i+1] & 0x1F;
+                    pobj->set_power(power - 10);
+                    pobj->set_moving((cdata[i+1] >> 5) & 0x01);
+                    pobj->set_fast_movable((cdata[i+1] >> 7) & 0x01);
+                    pobj->set_slow_movable((cdata[i+1] >> 6) & 0x01);
+                }
             }
-            if((canmsg.id()== 0x500)&&(gntemp > 10))
+            else if(group_id == 9)
             {
-                gobj.set_mstime(QDateTime::currentMSecsSinceEpoch());
-                ShareResult();
-                gntemp = 0;
-                //ok
+                iv::radar::radarobject * pobj = gobj.mutable_obj(63);
+                int8_t power = cdata[1] & 0x1F;
+                pobj->set_power(power - 10);
+                pobj->set_moving((cdata[1] >> 5) & 0x01);
+                pobj->set_fast_movable((cdata[1] >> 7) & 0x01);
+                pobj->set_slow_movable((cdata[1] >> 6) & 0x01);
             }
 
-            if(gntemp > 100)
-            {
-                gobj.set_mstime(QDateTime::currentMSecsSinceEpoch());
-                ShareResult();
-                gntemp = 0;
-                //ok
-            }
+            gntemp++;
         }
+        
+        if(canmsg.id() == 0x5D0)
+        {
+            gnRadarState = 10;
+
+            unsigned char cdata[8];
+            memcpy(cdata,canmsg.data().data(),8);
+            
+            int16_t valid_range = (cdata[1] << 8) | cdata[2];
+            gobj.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);
+
+            int16_t valid_angle = (cdata[5] << 8) | cdata[6];
+            gobj.valid_lr_angle(valid_angle / 16.0);
+
+            gobj.valid_lr_power(cdata[7]);
+            
+            gntemp++;
+        }
+        
+        if(canmsg.id() == 0x5D1)
+        {
+            gnRadarState = 10;
+
+            unsigned char cdata[8];
+            memcpy(cdata,canmsg.data().data(),8);
+            
+            int16_t valid_range = (cdata[1] << 8) | cdata[2];
+            gobj.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);
+
+            int16_t valid_angle = (cdata[5] << 8) | cdata[6];
+            gobj.valid_mr_angle(valid_angle / 16.0);
+
+            gobj.valid_mr_power(cdata[7]);
+            
+            gntemp++;
+        }
+
+        if(canmsg.id() == 0x5E8)
+        {
+            gnRadarState = 10;
+
+            unsigned char cdata[8];
+            memcpy(cdata,canmsg.data().data(),8);
+
+            uint8_t spray_ID = (cdata[4] >> 1) & 0x7F;
+            gobj.set_water_spray_target_id(spray_ID);
+
+            int16_t xohp_acc_cipv = ((cdata[4] & 0x01) | cdata[5]) << 7;
+            xohp_acc_cipv = xohp_acc_cipv >> 7;
+            gobj.set_filtered_xohp_acc_cipv(xohp_acc_cipv * 0.03125);
+
+            gobj.set_path_id_acc_2(cdata[6]);
+            gobj.set_path_id_acc_3(cdata[7]);
+
+            gntemp++;
+
+            gobj.set_mstime(QDateTime::currentMSecsSinceEpoch());
+            ShareResult();
+            gntemp = 0;
+
+        }
+
+        if(gntemp > 100)
+        {
+        gobj.set_mstime(QDateTime::currentMSecsSinceEpoch());
+        ShareResult();
+        gntemp = 0;
+        }
+        
  //       qDebug("id is %08x",canmsg.id());
+
     }
 
 }