modularization/src/detection/detection_mobileye/main.cpp

#include <QCoreApplication>

#include <iostream>
#include <QDateTime>
#include <QDebug>
#include "modulecomm.h"
#include "xmlparam.h"
#include "ivversion.h"
#include "ivbacktrace.h"

#include "canmsg.pb.h"
#include "mobileye.pb.h"
#include "mobileye_lane.pb.h"
#include "mobileye_obs.pb.h"
#include "mobileye_tsr.pb.h"
#include "ivfault.h"
#include "ivlog.h"

int gntemp = 0;

void * gpa;
void * gpb;

iv::Ivfault *gMobEyeFault = nullptr;
iv::Ivlog *gMobEyeIvlog = nullptr;

//static int gnNotSend = 10;

QTime gTime;
int8_t gnum_obstacles = 0;
int8_t gobs_count = 0;
int glane_sig = 0;
//static bool gbinit = false;


iv::mobileye::mobileye gmobileye;
iv::mobileye::obs gobs;
iv::mobileye::lane glane;


void ShareResult()
{
    char * str = new char[gmobileye.ByteSize()];
    int nsize = gmobileye.ByteSize();

    gobs.pos_y();

    if(gmobileye.SerializeToArray(str,nsize))
    {
        iv::modulecomm::ModuleSendMsg(gpa,str,nsize);
    }

    gMobEyeIvlog->info("mobileye","obj size is %d",gmobileye.xobj_size());

//    mMobEyeIvlog->info("share time is %d ",gTime.elapsed());
    delete str;
}

void ProcCANMsg(iv::can::canraw xmsg)
{
    unsigned char xdata[8];
    memcpy(xdata,xmsg.data().data(),8);
    int32_t tmp;
    /******************************************************mobileye lane***********************************************************
     *  message ID:0x737
     *  member:                 type            physical unit       range                   note
     *  curvature:              double             1/meter      [-0.12, 0.12]       "a" in the equation:y = ax^2+bx+c
     *  heading:                double                /          [-1.0, 1.0]        "b" in the equation:y = ax^2+bx+c
     *  construction area:      bool                  /               /                       /
     *  pitch angle:            double             radians      [-0.05, 0.05]      pitch of the host vehicle(derived from lanes analysis)
     *  yaw angle:              double             radians            /            yaw of the host vehicle(derived from lanes analysis)
     *  right_LDW:              bit/bool              /              0,1           0 stands for unavailable, 1 for available
     *  left_LDW:               bit/bool              /              0,1           0 stands for unavailable, 1 for available
     *******************************************************************************************************************************/
    if(xmsg.id() == 0x737)
    {
        double curv, head, yaw, pitch;
        int ldw_left, ldw_right;
 //       short ihead;
//        for(int i = 0; i< 8; i++)
//        {

//            gMobEyeIvlog->verbose("mobileye","i:%d %x %d\n ",i, xdata[i], xdata[i]);
//        }
        tmp = 0;
        //lane curv
        tmp = (xdata[1] << 8) | xdata[0];
        tmp <<=16;
        tmp >>=16;
        curv = tmp * 3.81 * 1e-6;
        tmp = 0;

        //lane heading
        tmp = (((xdata[3] & 0xf) << 8) | xdata[2]);
        tmp <<= 20;
        tmp >>= 20;
        head = tmp * 0.0005;

        tmp = 0;


        //construction area
        //is_ca = (xdata[3] >> 4) & 0x1;

        //left_LDW
        ldw_left = (xdata[3] >> 5) & 0x1;

        //right_LDW
        ldw_right = (xdata[3] >> 6) & 0x1;

        //yaw
        tmp = (xdata[5] << 8) | xdata[4];
        tmp <<= 16;
        tmp >>= 16;
        //ServiceCarStatus.Lane.yaw = (tmp - 0x7fff) / 1024.0;
        yaw = tmp * 0.000977;
        tmp = 0;

        //pitch
        tmp = (xdata[7] << 8) | xdata[6];
        tmp <<= 16;
        tmp >>= 16;
        //ServiceCarStatus.Lane.pitch = (tmp - 0x7fff) / 1024.0 / 512;
        pitch = tmp * 0.000002;

        gMobEyeIvlog->verbose("mobileye","%d ldw_left:%d, ldw_right:%d, curv:%f, head:%f, yaw:%f, pitch:%f \n ",\
                              QTime::currentTime().toString("HH-mm-ss").data() ,ldw_left, ldw_right, curv, head, yaw, pitch);

        glane.set_bleftldwavil(ldw_left);
        glane.set_brightldwavil(ldw_right);
        glane.set_curvature(curv);
        glane.set_heading(head);
        glane.set_pitchang(pitch);
        glane.set_yawang(yaw);
        glane_sig = glane_sig | 0x01;
//        ShareResult();
    }
    /****************************************************  mobileye obstacle status  ****************************************************************************
     *  message ID:0x738
     *  member:                  type           physical unit       range               note
     *  num_obstacles            int                   /           [0:255]                /
     *  timestamp                int                   ms          [0:255]          only the lowest 8 bits of the timestamp is given
     *  app_version:             int                   /           [0:255]          vesion info consists of X.Y.Z.W
     *  active_version           int/2bits             /            [0:3]           index of the active section of app_version signal
     *  left_close_rang_cut_in   bool                  /             0,1            0 false, 1 true
     *  right_close_rang_cut_in  bool                  /             0,1            0 false, 1 true
     *  go                       enum                  /           0,1...15         0 stop, 1 go, 2 undecided, 3 driver decisions is required, 4-14 unused, 15 not calculated
     *  protocol version         uchar/8bit            /          0x00...0xff             /
     *  is_close_car             bool                  /             0,1            0 no close, 1 close car exists
     *  failsafe                 int/4bit              /            [0:7]           failsafe situation, 0000 no failsafe, 0001 low sun, 0010 blur image, 0100 1000 unused
     *************************************************************************************************************************************************************/
    else if(xmsg.id() == 0x738)
    {
        int num_obstacles;
        int msgtime;
        (void)msgtime;
        //num_obstacles
        num_obstacles = xdata[0];
        //std::cout << ServiceCarStatus.obstacleStatus.num_obstacles<< std::endl;
        gnum_obstacles = num_obstacles;

        gobs_count = 0;
        gmobileye.clear_xobj();
        gmobileye.clear_xmsgtime();
        gmobileye.clear_numobstacles();
        glane_sig = 0;

        //timestamp
        msgtime = xdata[1];

        //app version
//        ServiceCarStatus.obstacleStatus.app_version = xdata[2];

//        //active version
//        ServiceCarStatus.obstacleStatus.active_version = xdata[3] & 0x3;

//        //is left close rang cut in
//        ServiceCarStatus.obstacleStatus.is_left_close_rang_cut_in = (xdata[3] >> 2) & 0x1;

//        //is right close rang cut in
//        ServiceCarStatus.obstacleStatus.is_right_close_rang_cut_in = (xdata[3] >> 3) & 0x1;

//        //go
//        ServiceCarStatus.obstacleStatus.go = xdata[3] >> 4;

//        //protocol version
//        ServiceCarStatus.obstacleStatus.protocol_version = xdata[4];

//        //close car
//        ServiceCarStatus.obstacleStatus.is_close_car = xdata[5] & 0x1;

//        //failsafe
//        ServiceCarStatus.obstacleStatus.failsafe = (xdata[5] >> 1) & 0xf;
        gmobileye.set_numobstacles(num_obstacles);
        gmobileye.set_xmsgtime(QDateTime::currentMSecsSinceEpoch());

        glane_sig = glane_sig | 0x02;
    }
    /****************************************************  mobileye obstacle data a  ****************************************************************************
     *  message ID:0x739
     *  member:                  type           physical unit       range               note
     *  obstacle_ID              int                   /           [0:63]           new obstacles are given the last used free ID
     *  obstacle_pos_x           double                m           [0,250]          longtitude position of the obstacle relative to the ref point
     *  obstacle_pos_y           double                m       [-31.93,31.93]       lateral position of the obstacle, error relative to pos_x  below 10% or 2meters
     *  blinker_info             int/enum              /            [0:4]           0 unavailable, 1 off, 2 left, 3 right, 4 both   indicated blinkers status
     *  cut_in_and_out           int                   /            [0:4]           0 undefined, 1 in_host_lane, 2 out_host_lane, 3 cut_in, 4 cut_out
     *                                                                              cut_in: target entering the host lane, cut_out: exiting  but not distinguish between sides
     *  obstacle_rel_vel_x       double               m/s      [-127.93,127.93]     relative longtitude velocity of the obstacle
     *  obstacle_status          int/enum              /            [0:6]           0 undefined, 1 standing(never moved, back lights on), 2 stopped(moveable), 3 moving
     *                                                                              4 oncoming,  5 parked(never moved, back lights off), 6 unused
     *  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() > 0x738 && xmsg.id() < 0x766)
    {
        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 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);

            //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 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 brake lights
            gobs.set_obsbrakelights((xdata[7] >> 3) & 0x1);

            //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)
     *  obstacle_width           double                m           [0,12.5]         width of  the obstacle(lateral axis)
     *  obstacle_age             int                   /           [0:255]          value starts at 1 when it is first detected, increaments in i each frame
     *  obstacle_lane            int/enum              /           [0:3]            0 not assigned, 1 ego lane, 2 next lane(left or right) or next next lane,
     *                                                                              3 invalid signal
     *  is_cipv_flag             int/enum              /            [0:1]           0 not cipv, 1 cipv(closest in path vehicle)
     *  radar_pos_x              double                m           [0,250]          longtitude postion of the primary radar target matched to the vision target, dist=relative to ref point
     *                                                                              if no radar target is matched, value = 0xfff
     *  radar_vel_x              double               m/s       [-127.93,127.93]    longitude velocity of the radar target matched to the vision targets, if no radar target is matched value=0xfff
     *                                                                              4 oncoming,  5 parked(never moved, back lights off), 6 unused
     *  radar_match_confidence   int                   /            [0:5]           0 no match, 1 multi match: radar doesn't describe well, 2-4 vision-radar match:with bounded error between
     *                                                                              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;

            //obstacle width
            tmp = xdata[1];
            gobs.set_obswidth(tmp * 0.05);

            tmp = 0;

            //obstacle age
            gobs.set_obsage(xdata[2]);

            //obstacle lane
            gobs.set_obslane(xdata[3] & 0x3);

            //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);

            tmp = 0;

            //radar vel x
            tmp = ((xdata[6] & 0xf) << 8) | xdata[5];
            tmp <<= 20;
            tmp >>= 20;
            gobs.set_radarvelx(tmp * 0.0625);

            tmp = 0;

            //radar match confidence
            gobs.set_radarmatchconfi((xdata[6] >> 4) & 0x7);

            //matched radar ID
            gobs.set_matchedradarid(xdata[7] & 0x7f);

        }
        /****************************************************  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
     *  obstcale_scale_change    double            pix/s       [-6.5532, 6.5532]    /
     *  object_accel_x           double            m/s2         [-14.97, 14.97]     longtitude acceleration of the object
     *  obstacle_replaced        bool                /               0,1            0 not replaced in this frame, 1 replace in this frame
     *  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=(xmsg.id()-0x73b)/3+1;
            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
     *  lane_conf_left           int/2bit              /            [0:3]            0 lowest, 3 highest
     *  is_ldw_availablity_left  bool                  /             0,1             lane_conf>=2->on, speed>55km/h, configuration of LDW>=1
     *  lane_type_left           int/enum              /            [0:6]            0 dashed, 1 solid, 2 none, 3 road edge, 4 double lane mark, 5 sbott's dots, 6 invalid
     *  dist_to_lane_l           double                m           [-40:40]          "c" in the equation:y = ax^2+bx+c
     *  lane_conf_right          int/2bit              /            [0:3]            0 lowest, 3 highest
     *  is_ldw_availablity_right  bool                  /             0,1             lane_conf>=2->on, speed>55km/h, configuration of LDW>=1
     *  lane_type_right          int/enum              /            [0:6]            0 dashed, 1 solid, 2 none, 3 road edge, 4 double lane mark, 5 sbott's dots, 6 invalid
     *  dist_to_lane_r           double                m           [-40:40]          "c" in the equation:y = ax^2+bx+c
     *************************************************************************************************************************************************************/
    else if(xmsg.id() == 0x669)
    {
        //lane confidence left
        uint8_t conf;
        conf = xdata[0] & 0x3;
//        gMobEyeIvlog->error("mobileye","conf l %d",conf);

        glane.set_laneconfleft(conf);

        tmp = 0;

        //is LDW availability left
        glane.set_isldwavaileft((xdata[0] >> 2) & 0x1);

        //lane type left
        tmp = (xdata[0] >> 4) & 0xf;
//        tmp <<= 28;
//        tmp >>= 28;
        glane.set_lanetypeleft(tmp);

        tmp = 0;

        //dist to left lane

        tmp = (xdata[2] << 4) | ((xdata[1] >> 4) & 0xf);
        tmp <<= 20;
        tmp >>= 20;
        glane.set_disttolaneleft(tmp * 0.02);
        tmp = 0;

        //lane confidence right
        conf = xdata[5] & 0x3;
//        tmp <<= 30;
//        tmp >>= 30;
        glane.set_laneconfright(conf);

//        gMobEyeIvlog->error("mobileye","conf r %d",conf);

        tmp = 0;

        //is LDW availability right
        glane.set_isldwavairight((xdata[5] >> 2) & 0x1);

        //lane type right
        tmp = (xdata[5] >> 4) & 0xf;
//        tmp <<= 28;
//        tmp >>= 28;
        glane.set_lanetyperight(tmp);

        tmp = 0;

        //dist to right lane
        tmp = (xdata[7] << 4) | ((xdata[6] >> 4) & 0xf);
        tmp <<= 20;
        tmp >>= 20;
        glane.set_distolaneright(tmp * 0.02);
        gMobEyeIvlog->verbose("mobileye",\
                              "aftermark lane:\n con_l:%d, ldw_avai_l:%d,type_l:%d, dist_to_l:%f \n con_r:%d, ldw_avai_r:%d, type_r:%d, dist_to_r:%f", \
                              glane.laneconfleft(), glane.isldwavaileft(), glane.lanetypeleft(), glane.disttolaneleft(), \
                              glane.laneconfright(), glane.isldwavairight(), glane.lanetyperight(), glane.distolaneright());
    }

    if (gobs_count == gnum_obstacles && glane_sig == 3)
    {

        gmobileye.clear_xlane();
        iv::mobileye::lane * pxlane = gmobileye.add_xlane();
        pxlane->CopyFrom(glane);
        ShareResult();
        gobs_count = 0;
        gmobileye.clear_xobj();
        gmobileye.clear_xmsgtime();
        gmobileye.clear_numobstacles();
        gnum_obstacles = 0;
        glane_sig = 0;
    }
}

void DecodeMsg(iv::can::canmsg xmsgvetor)
{

    int i;

    for(i=0;i<xmsgvetor.rawmsg_size();i++)
    {
        ProcCANMsg(xmsgvetor.rawmsg(i));
    }

}

void Listencanmsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    (void)index;
    (void)dt;
    (void)strmemname;

    if(nSize<1)return;
    iv::can::canmsg xmsg;
    if(false == xmsg.ParseFromArray(strdata,nSize))
    {
        gMobEyeIvlog->error("mobileye","mobileye Listencanmsg fail");
        gMobEyeFault->SetFaultState(0, 0, "mobileye Listencanmsg fail!");
        return;
    }

    DecodeMsg(xmsg);
    gMobEyeFault->SetFaultState(0, 0, "ok");

//    qDebug("can size is %d",xmsg.rawmsg_size());
//    xt = QDateTime::currentMSecsSinceEpoch();
//    qDebug("latence = %ld ",xt-pic.time());

}


int main(int argc, char *argv[])
{
    RegisterIVBackTrace();
    showversion("detection_mobileye");
    QCoreApplication a(argc, argv);

    gMobEyeFault = new iv::Ivfault("detection_mobileye");
    gMobEyeIvlog = new iv::Ivlog("detection_mobileye");

    QString strpath = QCoreApplication::applicationDirPath();
    if(argc < 2)
        strpath = strpath + "/detection_mobileye.xml";
    else
        strpath = argv[1];
    std::cout<<strpath.toStdString()<<std::endl;
    gMobEyeIvlog->info("%s", strpath.data());

    iv::xmlparam::Xmlparam xp(strpath.toStdString());

    std::string strmemcan = xp.GetParam("canrecv","canrecv1");
    std::string strmemsend = xp.GetParam("cansend","cansend1");
    std::string strmemmobileye = xp.GetParam("mobileye","mobileye");

    gTime.start();
    gpa = iv::modulecomm::RegisterSend(strmemmobileye.data(),100000,3);
    gpb = iv::modulecomm::RegisterSend(strmemsend.data(),100000,3);
    void * pa = iv::modulecomm::RegisterRecv(strmemcan.data(),Listencanmsg);
    (void)pa;


    return a.exec();
}