modularization/src/detection/detection_radar_delphi_esr_send/main.cpp

#include <QCoreApplication>

#include <iostream>
#include <QDateTime>
#include <math.h>
#include <thread>
#include <QDebug>

#include <signal.h>
#include <stdlib.h>
#include <setjmp.h>
#include <unistd.h>

#include "modulecomm.h"
#include "xmlparam.h"
#include "ivlog.h"
#include "ivfault.h"

#include "canmsg.pb.h"
#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 double Vehicle_Yaw_Rate;
static double Vehicle_Speed;        //< m/s
static double Steering_Angle; //< optional signal, ready to use
static int16_t Radius_Curvature = 0; //< optional signal, 8191 meter is default
static uint8_t Radar_FOV_Long_Range = 30; //< 0-30 degree
static uint8_t Radar_FOV_Mid_Range = 120; //< 0-120 degree
static uint8_t Radar_Height = 50;//< 0-125 cm
static uint16_t Radar_to_Rear_Axle = 370;//< 0-710 cm
static uint16_t Car_Wheel_Base = 270;//< 0-710 cm
static uint8_t Car_Steering_Gear_Ratio = 18;//< 0-31.875
/******************************************/

canSend4F0 canData4f0;
canSend4F1 canData4f1;
canSend5F2 canData5f2;
canSend5F4 canData5f4;

/******************************************/

iv::Ivlog * givlog;
iv::Ivfault * givfault;

iv::radar::radarobjectarray gobj;
int gntemp = 0;

void * gpa , * gpb;

QTime gTime;
int gnRadarState = 0;

#ifdef fujiankuan
/***==================================================================================***/
static int gnIndex = 0;

//static unsigned char tmp1[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0xBF,0x00};

void ExecSend()
{
    iv::can::canmsg xmsg;
    iv::can::canraw xraw;
qDebug()<<QTime::currentTime();
    xraw.set_id(0x4F0);
    xraw.set_data(canData4f0.byte,8);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(8);
    iv::can::canraw * pxraw = xmsg.add_rawmsg();
    pxraw->CopyFrom(xraw);
    xmsg.set_channel(1);
    xmsg.set_index(gnIndex);

    xraw.set_id(0x4F1);
    xraw.set_data(canData4f1.byte,8);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(8);
    pxraw = xmsg.add_rawmsg();
    pxraw->CopyFrom(xraw);
    xmsg.set_channel(1);
    xmsg.set_index(gnIndex);
/*
    xraw.set_id(0x5F2);
    xraw.set_data(canData5f2.byte,8);
    xraw.set_bext(false);

    xraw.set_bremote(false);
    xraw.set_len(8);
    pxraw = xmsg.add_rawmsg();
    pxraw->CopyFrom(xraw);
    xmsg.set_channel(1);
    xmsg.set_index(gnIndex);

    xraw.set_id(0x5F4);
    xraw.set_data(canData5f4.byte,8);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(8);
    pxraw = xmsg.add_rawmsg();
    pxraw->CopyFrom(xraw);
    xmsg.set_channel(1);
    xmsg.set_index(gnIndex);
*/
    gnIndex++;
    xmsg.set_mstime(QDateTime::currentMSecsSinceEpoch());
    int ndatasize = xmsg.ByteSize();
    char * strser = new char[ndatasize];
    std::shared_ptr<char> pstrser;
    pstrser.reset(strser);
    if(xmsg.SerializePartialToArray(strser,ndatasize))
    {
        iv::modulecomm::ModuleSendMsg(gpb,strser,ndatasize);
    }
    else
    {
        std::cout<<"MainWindow::onTimer serialize error."<<std::endl;
    }
}

void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    if(nSize<1)return;
    iv::gps::gpsimu xgpsimu;
    if(false == xgpsimu.ParseFromArray(strdata,nSize))
    {
        givlog->warn("detection_radar_delphi_esr_send--Listengpsimu parse errror. nSize is %d",nSize);
        return;
    }
    Vehicle_Yaw_Rate = xgpsimu.gyro_z() * -1;
    Vehicle_Speed = sqrt(xgpsimu.vn()*xgpsimu.vn() + xgpsimu.ve()*xgpsimu.ve() + xgpsimu.vd()*xgpsimu.vd());
}

void Listencancar(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    if(nSize<1)return;
    iv::can_car::can_car xcancar;

    if(false == xcancar.ParseFromArray(strdata,nSize))
    {
        givlog->warn("detection_radar_delphi_esr_send--Listencancar parse errror. nSize is %d",nSize);
        return;
    }
   // Vehicle_Speed = xcancar.v();
}

/****==================================================================================***/
#endif

/****************timer*********************/

void signalHandler(int num)
{
    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;
    if(Vehicle_Speed >= 0.0)
        canData4f0.bit.can_VehicleSpeedDirection = 0x00;
    else
        canData4f0.bit.can_VehicleSpeedDirection = 0x01;

    canData4f0.bit.canYawRateH = yawRate >> 8;
    canData4f0.bit.canYawRateL = yawRate & 0xFF;
    canData4f0.bit.canYawRateValid = 0x01;
    canData4f0.bit.canRadiusCurvatureH = Radius_Curvature >> 8;
    canData4f0.bit.canRadiusCurvatureL = Radius_Curvature & 0x3F;

    canData4f1.bit.canScanIndexAckH = CAN_Scan_Index >> 8;
    canData4f1.bit.canScanIndexAckL = CAN_Scan_Index & 0xFF;

    canData4f1.bit.canMaximumTracks = 0x3F;
    canData4f1.bit.canRadarCmdRadiate = 0x01;

    canData4f1.bit.canGroupingMode = 0x03;
    canData4f1.bit.canVehicleSpeedValid = 0x01;

    canData5f2.bit.canRadarFovLrH = Radar_FOV_Long_Range >> 1;
    canData5f2.bit.canRadarFovLrL = Radar_FOV_Long_Range & 0x01;
    canData5f2.bit.canRadarFovMr = Radar_FOV_Mid_Range & 0x7F;

    canData5f2.bit.canRadarHeight = Radar_Height & 0x7F;
    canData5f2.bit.canAalignAvgCtrTotal = 0x03;

    canData5f4.bit.canBeamwidthVert = 0x47;
    canData5f4.bit.canDistanceRearAxle = (uint8_t)((Radar_to_Rear_Axle - 200) / 2.0);
//    qDebug()<<canData5f4.bit.canDistanceRearAxle;
    canData5f4.bit.canWheelBase = (uint8_t)((Car_Wheel_Base - 200) / 2.0);
    canData5f4.bit.canSteeringGearRatio = (uint8_t)(Car_Steering_Gear_Ratio / 0.125);

    ExecSend();
}

/*****************************************/
void ShareResult()
{
    char * str = new char[gobj.ByteSize()];
    int nsize = gobj.ByteSize();
    if(gobj.SerializeToArray(str,nsize))
    {
        iv::modulecomm::ModuleSendMsg(gpa,str,nsize);
    }

    static qint64 oldrecvtime;

    if((QDateTime::currentMSecsSinceEpoch() - oldrecvtime)>100)
    {
        givlog->warn("radar interval is more than 100ms.  value is %ld",QDateTime::currentMSecsSinceEpoch() - oldrecvtime);
    }

    oldrecvtime = QDateTime::currentMSecsSinceEpoch();

    givlog->verbose("share time is %d ",gTime.elapsed());
//    qDebug("share time is %d ",gTime.elapsed());

    delete str;
}

void DecodeRadar(iv::can::canmsg xmsg)
{
//    int32_t range, rate, angle;
    if(xmsg.rawmsg_size() < 1)return;

    int i;
    for(i=0;i<xmsg.rawmsg_size();i++)
    {
        iv::can::canraw canmsg = xmsg.rawmsg(i);

        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];
            unsigned char cdata[8];
            memcpy(cdata,canmsg.data().data(),8);

            int radar_ID_index = canmsg.id() - 0x500;

            int j;
            for(j=0;j<8;j++)data[j] = cdata[j];

            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(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
            {
                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() == 0x540)
        {
            gnRadarState = 10;

            unsigned char cdata[8];
            memcpy(cdata,canmsg.data().data(),8);

            uint8_t group_id = cdata[0] & 0x0F;

            if(group_id < 9)
            {
                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);
//                    qDebug()<<power;
                    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);
                }
            }
            else if(group_id == 9)
            {
                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);
            }

            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.set_valid_lr_range(valid_range / 128.0);

            int16_t valid_range_rate = (cdata[3] << 8) | cdata[4];
            gobj.set_valid_lr_range_rate(valid_range_rate / 128.0);

            int16_t valid_angle = (cdata[5] << 8) | cdata[6];
            gobj.set_valid_lr_angle(valid_angle / 16.0);

            gobj.set_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.set_valid_mr_range(valid_range / 128.0);

            int16_t valid_range_rate = (cdata[3] << 8) | cdata[4];
            gobj.set_valid_mr_range_rate(valid_range_rate / 128.0);

            int16_t valid_angle = (cdata[5] << 8) | cdata[6];
            gobj.set_valid_mr_angle(valid_angle / 16.0);

            gobj.set_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());

    }

}

void Listencan0(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    if(nSize<1)return;
    iv::can::canmsg xmsg;
    if(false == xmsg.ParseFromArray(strdata,nSize))
    {
        std::cout<<"esr Listencan0 fail."<<std::endl;
        return;
    }

    DecodeRadar(xmsg);

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

}

void threadstate()
{
    while(1)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        if(gnRadarState > -100)gnRadarState--;
        if(gnRadarState > 0)
        {
            givfault->SetFaultState(0,0,"OK");
        }
        else
        {
            if(gnRadarState > -100)
            {
                givfault->SetFaultState(1,1,"无CAN数据");
            }
            else
            {
                givfault->SetFaultState(2,2,"无CAN数据");
            }
        }

    }
}

int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);

    iv::radar::radarobjectarray x;
    x.set_mstime(0);
    int i;
    for(i=0;i<64;i++)
    {
        iv::radar::radarobject * xobj = x.add_obj();
        xobj->set_x(0);
        xobj->set_y(0);
        xobj->set_vx(0);
        xobj->set_vy(0);
        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";
    else
        strpath = argv[1];
    std::cout<<strpath.toStdString()<<std::endl;

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

    std::string strmemcan = xp.GetParam("canrecv","canrecv1");
    std::string strmemsend = xp.GetParam("cansend","cansend1");
    std::string strmemradar = xp.GetParam("radar","radar");
    std::string strmodulename = xp.GetParam("modulename","detection_radar_delphi_esr");
#ifdef fujiankuan
    gstrmemgpsimu = xp.GetParam("gpsimu_name","hcp2_gpsimu");
    gstrmemcancar = xp.GetParam("cancar","can_car");
#endif


    givlog = new iv::Ivlog(strmodulename.data());
    givfault = new iv::Ivfault(strmodulename.data());

    givfault->SetFaultState(1,1,"初始化");
    givlog->info("Initialized");

    gpa = iv::modulecomm::RegisterSend(strmemradar.data(),100000,3);
    gpb = iv::modulecomm::RegisterSend(strmemsend.data(),100000,1);
    void * pa = iv::modulecomm::RegisterRecv(strmemcan.data(),Listencan0);

#ifdef fujiankuan
    void * pb = iv::modulecomm::RegisterRecv(gstrmemgpsimu.data(),Listengpsimu);
    void * pc = iv::modulecomm::RegisterRecv(gstrmemcancar.data(),Listencancar);
#endif

    std::thread threadfault(threadstate);
//    signal(SIGALRM, signalHandler);
//    ualarm(2000,2000);

    signal(SIGALRM, signalHandler);

    struct itimerval new_value, old_value;
    new_value.it_value.tv_sec = 0;
    new_value.it_value.tv_usec = 1;
    new_value.it_interval.tv_sec = 0;
    new_value.it_interval.tv_usec = 20000;
    setitimer(ITIMER_REAL, &new_value, NULL);
    return a.exec();
}