feat(radar_srr):add rotation function for srr

src/detection/detection_radar_delphi_srr/detection_radar_delphi_srr.xml

@@ -1,7 +1,17 @@
 <xml>	
 	<node name="detection_radar_delphi_srr">
 		<param name="canrecv" value="canrecv0" />
+		<param name="cansend" value="cansend0" />
 		<param name="radar_srr_left" value="corner_radar_left" />
 		<param name="radar_srr_right" value="corner_radar_right" />
+		<param name="gpsimu_name" value="hcp2_gpsimu" />
+		<param name="radar_left_angle" value="120" />
+		<param name="radar_left_offset_x" value="0" />
+		<param name="radar_left_offset_y" value="0" />
+		<param name="radar_left_mirror" value="0" />
+		<param name="radar_right_angle" value="-120" />
+		<param name="radar_right_offset_x" value="0" />
+		<param name="radar_right_offset_y" value="0" />
+		<param name="radar_right_mirror" value="0" />
 	</node>
 </xml>

src/detection/detection_radar_delphi_srr/main.cpp

@@ -7,6 +7,7 @@
 #include <sys/time.h>
 #include <signal.h>
 #include <QThread>
+#include <QString>
 
 #include "modulecomm.h"
 #include "xmlparam.h"
@@ -19,18 +20,27 @@
 
 /*Vcan send data*/
 std::string gstrmemgpsimu;
-std::string gstrmemcancar;
+
+QString gstrRadarLeft_Angle;
+QString gstrRadarLeft_Offset_X;
+QString gstrRadarLeft_Offset_Y;
+QString gstrRadarLeft_Mirror;
+QString gstrRadarRight_Angle;
+QString gstrRadarRight_Offset_X;
+QString gstrRadarRight_Offset_Y;
+QString gstrRadarRight_Mirror;
+
 double g_gyro_z;   //
 double g_v;        //
 
-iv::radar::radarobjectarray gobj;
 iv::Ivlog *givlog;
 
+iv::radar::radarobjectarray gobj;
 iv::radar::radarobjectarray gobjright;
 int gntemp = 0;
-int gntmpright=0;
+int gntempright = 0;
 
-void * gpa , * gpb, *gpc, *gpcanSend;
+void *gpa , *gpb , *gpc , *gpcanSend;
 
 QTime gTime;
 
@@ -125,7 +135,7 @@ void DecodeRadar(iv::can::canmsg xmsg)
             unsigned int object_power = 0;
 
             for(int j=0;j<8;j++)data[j] = cdata[j];
-////////////////////////////////RSDS_PCAN_v18.dbc
+
             detect_valid_level = (data[0] >> 5) & 0x07;
             detect_status = (data[0] >> 4) & 0x01;
             object_power = ((((data[0] & 0x0F) << 8) | data[1]) << 4) >> 4;
@@ -133,28 +143,45 @@ void DecodeRadar(iv::can::canmsg xmsg)
             range = (data[4]  << 8) | data[5];
             rate =  (data[6]  << 8) | data[7];
 
-///////////////////////////////RSDS_PCAN_v18.dbc
+            double range_temp = range*0.0078125;
+            double angle_temp = angle*0.0078125;
+            double rate_temp = rate*0.0078125;
+            double x_temp = range_temp * sin(angle_temp / 180.0 * M_PI);
+            double y_temp = range_temp * cos(angle_temp / 180.0 * M_PI);
+            double vx_temp = rate_temp * sin(angle_temp / 180.0 * M_PI);
+            double vy_temp = rate_temp * cos(angle_temp / 180.0 * M_PI);
+
+            if(gstrRadarLeft_Mirror.toInt() != 0)
+            {
+                x_temp = -x_temp;
+                vx_temp = -vx_temp;
+            }
+
+            double cos_rotation = cos(gstrRadarLeft_Angle.toDouble() / 180.0 * M_PI);
+            double sin_rotation = sin(gstrRadarLeft_Angle.toDouble() / 180.0 * M_PI);
+
+            double x = x_temp;
+            double y = y_temp;
+            x_temp = x * cos_rotation + y * sin_rotation + gstrRadarLeft_Offset_X.toDouble();
+            y_temp = y * cos_rotation - x * sin_rotation + gstrRadarLeft_Offset_Y.toDouble();
+
+            x = vx_temp;
+            y = vy_temp;
+            vx_temp = x * cos_rotation + y * sin_rotation;
+            vy_temp = y * cos_rotation - x * sin_rotation;
 
-            //If angle and range both are 0, it is an invalid data.
-            //qDebug("range = %d angle = %d ",range,angle);
-            range=range*0.0078125;
-            angle=angle*0.0078125;
-            rate=rate*0.0078125;
-//            qDebug("range = %d angle = %d ",range,angle);
             if ((angle != 0 || range != 0) && detect_status != 0 && detect_valid_level > 1) {
                 iv::radar::radarobject * pobj = gobj.mutable_obj(radar_ID_index);
                 pobj->set_bvalid(true);
-                pobj->set_x(range  * sin(angle  / 180.0 * M_PI));
-                pobj->set_y(range  * cos(angle  / 180.0 * M_PI));
-                pobj->set_vel(rate );
-                pobj->set_vx(pobj->vel() * sin(angle / 180.0 * M_PI));
-                pobj->set_vy(pobj->vel() * cos(angle / 180.0 * M_PI));
+                pobj->set_x(x_temp);
+                pobj->set_y(y_temp);
+                pobj->set_vel(rate_temp);
+                pobj->set_vx(vx_temp);
+                pobj->set_vy(vy_temp);
                 pobj->set_power(object_power);
                 pobj->set_detect_status(detect_status);
                 pobj->set_detect_valid_level(detect_valid_level);
-               // pobj->set_rectime(QDateTime::currentMSecsSinceEpoch());
-
-               // qDebug("x = %d angle = %d ",pobj->vel());
+//                pobj->set_rectime(QDateTime::currentMSecsSinceEpoch());
 
             }
             else {
@@ -187,7 +214,6 @@ void DecodeRadar(iv::can::canmsg xmsg)
                 //ok
             }
         }
- //       qDebug("id is %08x",canmsg.id());
 
 ///////////////////////////////////////////radra_right
         if((canmsg.id() >= 0x600)&&(canmsg.id() <= 0x63f))
@@ -202,30 +228,53 @@ void DecodeRadar(iv::can::canmsg xmsg)
             unsigned int object_power = 0;
 
             for(int j=0;j<8;j++)data[j] = cdata[j];
-////////////////////////////////RSDS_PCAN_v18.dbc
+
             detect_valid_level = (data[0] >> 5) & 0x07;
             detect_status = (data[0] >> 4) & 0x01;
             object_power = ((((data[0] & 0x0F) << 8) | data[1]) << 4) >> 4;
             angle_right = (data[2]  << 8) | data[3];
             range_right = (data[4]  << 8) | data[5];
-            rate_right  =  (data[6]  << 8) | data[7];
+            rate_right  = (data[6]  << 8) | data[7];
+
+            double range_temp = range_right*0.0078125;
+            double angle_temp = angle_right*0.0078125;
+            double rate_temp = rate_right*0.0078125;
+            double x_temp = range_temp * sin(angle_temp / 180.0 * M_PI);
+            double y_temp = range_temp * cos(angle_temp / 180.0 * M_PI);
+            double vx_temp = rate_temp * sin(angle_temp / 180.0 * M_PI);
+            double vy_temp = rate_temp * cos(angle_temp / 180.0 * M_PI);
+
+            if(gstrRadarRight_Mirror.toInt() != 0)
+            {
+                x_temp = -x_temp;
+                vx_temp = -vx_temp;
+            }
+
+            double cos_rotation = cos(gstrRadarRight_Angle.toDouble() / 180.0 * M_PI);
+            double sin_rotation = sin(gstrRadarRight_Angle.toDouble() / 180.0 * M_PI);
+
+            double x = x_temp;
+            double y = y_temp;
+            x_temp = x * cos_rotation + y * sin_rotation + gstrRadarRight_Offset_X.toDouble();
+            y_temp = y * cos_rotation - x * sin_rotation + gstrRadarRight_Offset_Y.toDouble();
+
+            x = vx_temp;
+            y = vy_temp;
+            vx_temp = x * cos_rotation + y * sin_rotation;
+            vy_temp = y * cos_rotation - x * sin_rotation;
 
-            range_right=range_right*0.0078125;
-            angle_right=angle_right*0.0078125;
-            rate_right=rate_right*0.0078125;
-//            qDebug("range_right = %d angle_right = %d ",range_right,angle_right);
             if ((angle_right != 0 || range_right != 0) && detect_status != 0 && detect_valid_level > 1) {
                 iv::radar::radarobject * pobj = gobjright.mutable_obj(radar_ID_index);
                 pobj->set_bvalid(true);
-                pobj->set_x(range_right  * sin(angle_right  / 180.0 * M_PI));
-                pobj->set_y(range_right  * cos(angle_right  / 180.0 * M_PI));
-                pobj->set_vel(rate_right );
-                pobj->set_vx(pobj->vel() * sin(angle_right/ 180.0 * M_PI));
-                pobj->set_vy(pobj->vel() * cos(angle_right / 180.0 * M_PI));
+                pobj->set_x(x_temp);
+                pobj->set_y(y_temp);
+                pobj->set_vel(rate_temp);
+                pobj->set_vx(vx_temp);
+                pobj->set_vy(vy_temp);
                 pobj->set_power(object_power);
                 pobj->set_detect_status(detect_status);
                 pobj->set_detect_valid_level(detect_valid_level);
-               // pobj->set_rectime(QDateTime::currentMSecsSinceEpoch());
+//                pobj->set_rectime(QDateTime::currentMSecsSinceEpoch());
 
             }
             else {
@@ -233,28 +282,28 @@ void DecodeRadar(iv::can::canmsg xmsg)
                 pobj->set_bvalid(false);
             }
 
-            gntmpright++;
+            gntempright++;
 
             if(canmsg.id() == 0x63f)
             {
                 gobjright.set_mstime(QDateTime::currentMSecsSinceEpoch());
                 ShareResult();
-                gntmpright = 0;
+                gntempright = 0;
                 //ok
             }
-            if((canmsg.id()== 0x600)&&(gntmpright > 10))
+            if((canmsg.id()== 0x600)&&(gntempright > 10))
             {
                 gobjright.set_mstime(QDateTime::currentMSecsSinceEpoch());
                 ShareResult();
-                gntmpright = 0;
+                gntempright = 0;
                 //ok
             }
 
-            if(gntmpright > 100)
+            if(gntempright > 100)
             {
                 gobjright.set_mstime(QDateTime::currentMSecsSinceEpoch());
                 ShareResult();
-                gntmpright = 0;
+                gntempright = 0;
                 //ok
             }
         }
@@ -336,7 +385,7 @@ int main(int argc, char *argv[])
 
     QString strpath = QCoreApplication::applicationDirPath();
     if(argc < 2)
-        strpath = strpath + "/detection_radar_conti_sr308.xml";
+        strpath = strpath + "/detection_radar_delphi_srr.xml";
     else
         strpath = argv[1];
     std::cout<<strpath.toStdString()<<std::endl;
@@ -352,10 +401,19 @@ int main(int argc, char *argv[])
     std::string strmemcan = xp.GetParam("canrecv","canrecv0");
     std::string strmemsend = xp.GetParam("cansend","cansend0");
 
-    std::string strmemradarleft = xp.GetParam("radarleft","corner_radar_left");
-    std::string strmemradaright = xp.GetParam("radaright","corner_radar_right");
+    std::string strmemradarleft = xp.GetParam("radar_srr_left","corner_radar_left");
+    std::string strmemradaright = xp.GetParam("radar_srr_right","corner_radar_right");
     gstrmemgpsimu = xp.GetParam("gpsimu_name","hcp2_gpsimu");
 
+    gstrRadarLeft_Angle = xp.GetParam("radar_left_angle","120");
+    gstrRadarLeft_Offset_X = xp.GetParam("radar_left_offset_x","0");
+    gstrRadarLeft_Offset_Y = xp.GetParam("radar_left_offset_y","0");
+    gstrRadarLeft_Mirror = xp.GetParam("radar_left_mirror","0");
+    gstrRadarRight_Angle = xp.GetParam("radar_right_angle","-120");
+    gstrRadarRight_Offset_X = xp.GetParam("radar_right_offset_x","0");
+    gstrRadarRight_Offset_Y = xp.GetParam("radar_right_offset_y","0");
+    gstrRadarRight_Mirror = xp.GetParam("radar_right_mirror","0");
+
     gpa = iv::modulecomm::RegisterSend(strmemradarleft.data(),100000,3);
     gpb = iv::modulecomm::RegisterSend(strmemradaright.data(),100000,3);
     gpcanSend = iv::modulecomm::RegisterSend(strmemsend.data(),100000,3);

src/include/proto/radar_alarm.proto

@@ -0,0 +1,17 @@
+syntax = "proto2";
+
+package iv.radar;
+
+message radaralarm
+{
+optional bool RCW_Warning = 1; //后碰撞预警
+optional bool LCMA_L_Warning = 2; //左并线辅助预警
+optional bool LCMA_R_Warning = 3; //右并线辅助预警
+optional bool BLIS_L_Warning = 4; //左盲区监测BSD
+optional bool BLIS_R_Warning = 5; //右盲区监测BSD
+optional bool CVW_L_Warning = 6; //左高速接近预警
+optional bool CVW_R_Warning = 7; //右高速接近预警
+optional bool CTA_L_Warning = 8; //左倒车横穿预警
+optional bool CTA_R_Warning = 9; //右倒车横穿预警
+optional bool FCW_Warning = 10; //前碰撞预警
+}