|
|
@@ -2,7 +2,7 @@
|
|
|
|
|
|
#include <iostream>
|
|
|
#include <QDateTime>
|
|
|
-
|
|
|
+#include <QDebug>
|
|
|
#include "modulecomm.h"
|
|
|
#include "xmlparam.h"
|
|
|
#include "ivversion.h"
|
|
|
@@ -74,11 +74,11 @@ void ProcCANMsg(iv::can::canraw xmsg)
|
|
|
double curv, head, yaw, pitch;
|
|
|
int ldw_left, ldw_right;
|
|
|
short ihead;
|
|
|
- for(int i = 0; i< 8; i++)
|
|
|
- {
|
|
|
+// for(int i = 0; i< 8; i++)
|
|
|
+// {
|
|
|
|
|
|
- gMobEyeIvlog->verbose("mobileye","i:%d %x %d\n ",i, xdata[i], xdata[i]);
|
|
|
- }
|
|
|
+// gMobEyeIvlog->verbose("mobileye","i:%d %x %d\n ",i, xdata[i], xdata[i]);
|
|
|
+// }
|
|
|
tmp = 0;
|
|
|
//lane curv
|
|
|
tmp = (xdata[1] << 8) | xdata[0];
|
|
|
@@ -199,47 +199,51 @@ void ProcCANMsg(iv::can::canraw xmsg)
|
|
|
* is_obstacle_brake_lights bool / 0,1 0 object's brake light off or not identified, 1 object's brake light on
|
|
|
* obstacle_valid int/enum / [1:2] 1 new valid(detected this frame), 2 older valid
|
|
|
*************************************************************************************************************************************************************/
|
|
|
- else if((xmsg.id()-0x739)/3 == 0)
|
|
|
+ else if(xmsg.id() > 0x738 && xmsg.id() < 0x766)
|
|
|
{
|
|
|
- //obstacle id
|
|
|
- gobs.set_id(xdata[0]);
|
|
|
+ if((xmsg.id()-0x739)%3 == 0)
|
|
|
+ {
|
|
|
+ //obstacle id
|
|
|
+ gobs.set_id(xdata[0]);
|
|
|
|
|
|
- //obstacle pos x
|
|
|
- tmp = ((xdata[2] & 0xf) << 8) | xdata[1];
|
|
|
- gobs.set_pos_x(tmp * 0.0625);
|
|
|
- tmp = 0;
|
|
|
+ //obstacle pos x
|
|
|
+ tmp = ((xdata[2] & 0xf) << 8) | xdata[1];
|
|
|
+ gobs.set_pos_x(tmp * 0.0625);
|
|
|
+ tmp = 0;
|
|
|
|
|
|
- //obstacle pos y
|
|
|
- tmp = ((xdata[4] & 0x3) << 8) | xdata[3];
|
|
|
- tmp <<= 22;
|
|
|
- tmp >>= 22;
|
|
|
- gobs.set_pos_y(tmp * 0.0625);
|
|
|
+ //obstacle pos y
|
|
|
+ tmp = ((xdata[4] & 0x3) << 8) | xdata[3];
|
|
|
+ tmp <<= 22;
|
|
|
+ tmp >>= 22;
|
|
|
+ gobs.set_pos_y(tmp * 0.0625);
|
|
|
|
|
|
- //blinker info
|
|
|
- gobs.set_blinkerinfo((xdata[4] >> 2) & 0x7);
|
|
|
+ //blinker info
|
|
|
+ gobs.set_blinkerinfo((xdata[4] >> 2) & 0x7);
|
|
|
|
|
|
- //cut in and out
|
|
|
- gobs.set_cutstate(iv::mobileye::obs_CUTSTATE(xdata[4] >> 5));
|
|
|
+ //cut in and out
|
|
|
+ gobs.set_cutstate(iv::mobileye::obs_CUTSTATE(xdata[4] >> 5));
|
|
|
+ // gobs.set_cutstate(xdata[4] >> 5);
|
|
|
|
|
|
- //obstacle rel vel x
|
|
|
- tmp = ((xdata[6] & 0xf) << 8) | xdata[5];
|
|
|
- tmp <<= 20;
|
|
|
- tmp >>= 20;
|
|
|
- gobs.set_obs_rel_vel_x(tmp * 0.0625);
|
|
|
+ //obstacle rel vel x
|
|
|
+ tmp = ((xdata[6] & 0xf) << 8) | xdata[5];
|
|
|
+ tmp <<= 20;
|
|
|
+ tmp >>= 20;
|
|
|
+ gobs.set_obs_rel_vel_x(tmp * 0.0625);
|
|
|
|
|
|
- //obstacle type
|
|
|
- gobs.set_obstype(iv::mobileye::obs_OBSTYPE((xdata[6] >> 4) & 0x7));
|
|
|
+ //obstacle type
|
|
|
+ gobs.set_obstype(iv::mobileye::obs_OBSTYPE((xdata[6] >> 4) & 0x7));
|
|
|
|
|
|
- //obstacle status
|
|
|
- gobs.set_obssatus(iv::mobileye::obs_OBSSTATE(xdata[7] & 0x7));
|
|
|
+ //obstacle status
|
|
|
+ gobs.set_obssatus(iv::mobileye::obs_OBSSTATE(xdata[7] & 0x7));
|
|
|
|
|
|
- //obstacle brake lights
|
|
|
- gobs.set_obsbrakelights((xdata[7] >> 3) & 0x1);
|
|
|
+ //obstacle brake lights
|
|
|
+ gobs.set_obsbrakelights((xdata[7] >> 3) & 0x1);
|
|
|
|
|
|
- //obstacle valid
|
|
|
- gobs.set_obsvalid((xdata[7] >> 6) & 0x7);
|
|
|
- }
|
|
|
- /**************************************************** mobileye obstacle data b ****************************************************************************
|
|
|
+ //obstacle valid
|
|
|
+ gobs.set_obsvalid((xdata[7] >> 6) & 0x7);
|
|
|
+
|
|
|
+ }
|
|
|
+ /**************************************************** mobileye obstacle data b ****************************************************************************
|
|
|
* message ID:0x73a
|
|
|
* member: type physical unit range note
|
|
|
* obstacle_length double m [0,31] length of the obstacle(longitude axis)
|
|
|
@@ -256,50 +260,50 @@ void ProcCANMsg(iv::can::canraw xmsg)
|
|
|
* vision and radar measurements higher->smaller error, 5 high confidence match
|
|
|
* matched_radar_id int / [0:127] ID of Primary radar target matched to the vision target if applicable
|
|
|
*************************************************************************************************************************************************************/
|
|
|
- else if((xmsg.id()-0x73a)/3 == 0)
|
|
|
- {
|
|
|
- //obstacle length
|
|
|
- tmp = xdata[0];
|
|
|
- gobs.set_obslen(tmp * 0.5);
|
|
|
- tmp = 0;
|
|
|
+ else if((xmsg.id()-0x73a)%3 == 0)
|
|
|
+ {
|
|
|
+ //obstacle length
|
|
|
+ tmp = xdata[0];
|
|
|
+ gobs.set_obslen(tmp * 0.5);
|
|
|
+ tmp = 0;
|
|
|
|
|
|
- //obstacle width
|
|
|
- tmp = xdata[1];
|
|
|
- gobs.set_obswidth(tmp * 0.05);
|
|
|
+ //obstacle width
|
|
|
+ tmp = xdata[1];
|
|
|
+ gobs.set_obswidth(tmp * 0.05);
|
|
|
|
|
|
- tmp = 0;
|
|
|
+ tmp = 0;
|
|
|
|
|
|
- //obstacle age
|
|
|
- gobs.set_obsage(xdata[2]);
|
|
|
+ //obstacle age
|
|
|
+ gobs.set_obsage(xdata[2]);
|
|
|
|
|
|
- //obstacle lane
|
|
|
- gobs.set_obslane(xdata[3] & 0x3);
|
|
|
+ //obstacle lane
|
|
|
+ gobs.set_obslane(xdata[3] & 0x3);
|
|
|
|
|
|
- //is cipv flag
|
|
|
- gobs.set_cipvflag((xdata[3] >> 2) & 0x1);
|
|
|
+ //is cipv flag
|
|
|
+ gobs.set_cipvflag((xdata[3] >> 2) & 0x1);
|
|
|
|
|
|
- //radar pos x
|
|
|
- tmp = (xdata[4] << 4) | (xdata[3] >> 4);
|
|
|
- gobs.set_radarposx(tmp * 0.0625);
|
|
|
+ //radar pos x
|
|
|
+ tmp = (xdata[4] << 4) | (xdata[3] >> 4);
|
|
|
+ gobs.set_radarposx(tmp * 0.0625);
|
|
|
|
|
|
- tmp = 0;
|
|
|
+ tmp = 0;
|
|
|
|
|
|
- //radar vel x
|
|
|
- tmp = ((xdata[6] & 0xf) << 8) | xdata[5];
|
|
|
- tmp <<= 20;
|
|
|
- tmp >>= 20;
|
|
|
- gobs.set_radarvelx(tmp * 0.0625);
|
|
|
+ //radar vel x
|
|
|
+ tmp = ((xdata[6] & 0xf) << 8) | xdata[5];
|
|
|
+ tmp <<= 20;
|
|
|
+ tmp >>= 20;
|
|
|
+ gobs.set_radarvelx(tmp * 0.0625);
|
|
|
|
|
|
- tmp = 0;
|
|
|
+ tmp = 0;
|
|
|
|
|
|
- //radar match confidence
|
|
|
- gobs.set_radarmatchconfi((xdata[6] >> 4) & 0x7);
|
|
|
+ //radar match confidence
|
|
|
+ gobs.set_radarmatchconfi((xdata[6] >> 4) & 0x7);
|
|
|
|
|
|
- //matched radar ID
|
|
|
- gobs.set_matchedradarid(xdata[7] & 0x7f);
|
|
|
+ //matched radar ID
|
|
|
+ gobs.set_matchedradarid(xdata[7] & 0x7f);
|
|
|
|
|
|
- }
|
|
|
- /**************************************************** mobileye obstacle data c ****************************************************************************
|
|
|
+ }
|
|
|
+ /**************************************************** mobileye obstacle data c ****************************************************************************
|
|
|
* message ID:0x73b
|
|
|
* member: type physical unit range note
|
|
|
* obstacle_angle_rate double degree/s [-327.68, 327.68] Angle rate of Center of Obstacle, negative->moved to left
|
|
|
@@ -309,48 +313,48 @@ void ProcCANMsg(iv::can::canraw xmsg)
|
|
|
* obstacle_angle double degree [-327.68,327.68] Angle to Center of Obstacle in degrees, 0 indicates that the obstacle is in
|
|
|
* exactly in front of us, a positive angle indicates that theobstacle is to the right
|
|
|
*************************************************************************************************************************************************************/
|
|
|
- else if((xmsg.id()-0x73b)/3 == 0)
|
|
|
- {
|
|
|
- //obstacle angle rate
|
|
|
- tmp = (xdata[1] << 8) | xdata[0];
|
|
|
- tmp <<= 16;
|
|
|
- tmp >>= 16;
|
|
|
- gobs.set_obsangrate(tmp * 0.01);
|
|
|
-
|
|
|
- tmp = 0;
|
|
|
-
|
|
|
- //obstacle scale change
|
|
|
- tmp = (xdata[3] << 8) | xdata[2];
|
|
|
- tmp <<= 16;
|
|
|
- tmp >>= 16;
|
|
|
- gobs.set_obsscalechange(tmp * 0.0002);
|
|
|
-
|
|
|
- tmp = 0;
|
|
|
-
|
|
|
- //object accel x
|
|
|
- tmp = ((xdata[5] & 0x3) << 8) | xdata[4];
|
|
|
- tmp <<= 22;
|
|
|
- tmp >>= 22;
|
|
|
- gobs.set_accelx(tmp * 0.03);
|
|
|
-
|
|
|
- tmp = 0;
|
|
|
-
|
|
|
- //obstacle replaced
|
|
|
- gobs.set_obsreplaced((xdata[5] >> 4) & 0x1);
|
|
|
-
|
|
|
- //obstacle angle
|
|
|
- tmp = (xdata[7] << 8) | xdata[6];
|
|
|
- tmp <<= 16;
|
|
|
- tmp >>= 16;
|
|
|
- gobs.set_obsang(tmp * 0.01);
|
|
|
-
|
|
|
- iv::mobileye::obs * pxobs = gmobileye.add_xobj();
|
|
|
- pxobs->CopyFrom(gobs);
|
|
|
- gobs_count++;
|
|
|
- gMobEyeIvlog->verbose("mobileyeobs", "nums:%d cur_count:%d, obs_id:%d, pos_x:%f, pos_y:%f \n",\
|
|
|
- gnum_obstacles, gobs_count, gobs.id(), gobs.pos_x(), gobs.pos_y());
|
|
|
+ else if((xmsg.id()-0x73b)%3 == 0)
|
|
|
+ {
|
|
|
+ //obstacle angle rate
|
|
|
+ tmp = (xdata[1] << 8) | xdata[0];
|
|
|
+ tmp <<= 16;
|
|
|
+ tmp >>= 16;
|
|
|
+ gobs.set_obsangrate(tmp * 0.01);
|
|
|
+
|
|
|
+ tmp = 0;
|
|
|
+
|
|
|
+ //obstacle scale change
|
|
|
+ tmp = (xdata[3] << 8) | xdata[2];
|
|
|
+ tmp <<= 16;
|
|
|
+ tmp >>= 16;
|
|
|
+ gobs.set_obsscalechange(tmp * 0.0002);
|
|
|
+
|
|
|
+ tmp = 0;
|
|
|
+
|
|
|
+ //object accel x
|
|
|
+ tmp = ((xdata[5] & 0x3) << 8) | xdata[4];
|
|
|
+ tmp <<= 22;
|
|
|
+ tmp >>= 22;
|
|
|
+ gobs.set_accelx(tmp * 0.03);
|
|
|
+
|
|
|
+ tmp = 0;
|
|
|
+
|
|
|
+ //obstacle replaced
|
|
|
+ gobs.set_obsreplaced((xdata[5] >> 4) & 0x1);
|
|
|
+
|
|
|
+ //obstacle angle
|
|
|
+ tmp = (xdata[7] << 8) | xdata[6];
|
|
|
+ tmp <<= 16;
|
|
|
+ tmp >>= 16;
|
|
|
+ gobs.set_obsang(tmp * 0.01);
|
|
|
+
|
|
|
+ iv::mobileye::obs * pxobs = gmobileye.add_xobj();
|
|
|
+ pxobs->CopyFrom(gobs);
|
|
|
+ gobs_count++;
|
|
|
+ gMobEyeIvlog->verbose("mobileyeobs", "nums:%d cur_count:%d, obs_id:%d, pos_x:%f, pos_y:%f \n",\
|
|
|
+ gnum_obstacles, gobs_count, gobs.id(), gobs.pos_x(), gobs.pos_y());
|
|
|
+ }
|
|
|
}
|
|
|
-
|
|
|
/**************************************************** mobileye aftermarket lane ****************************************************************************
|
|
|
* message ID:0x669
|
|
|
* member: type physical unit range note
|
|
|
@@ -495,8 +499,8 @@ int main(int argc, char *argv[])
|
|
|
|
|
|
iv::xmlparam::Xmlparam xp(strpath.toStdString());
|
|
|
|
|
|
- std::string strmemcan = xp.GetParam("canrecv","canrecv0");
|
|
|
- std::string strmemsend = xp.GetParam("cansend","cansend0");
|
|
|
+ std::string strmemcan = xp.GetParam("canrecv","canrecv1");
|
|
|
+ std::string strmemsend = xp.GetParam("cansend","cansend1");
|
|
|
std::string strmemmobileye = xp.GetParam("mobileye","mobileye");
|
|
|
|
|
|
gTime.start();
|
fujiankuan