modularization/src/detection/detection_gps_hcp2/main.cpp

#include <QCoreApplication>

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

#include "modulecomm.h"
#include "xmlparam.h"
#include "ivexit.h"
#include "ivfault.h"
#include "ivlog.h"
#include "ivversion.h"
#include "ivbacktrace.h"

#include "canmsg.pb.h"
#include "gpsimu.pb.h"
#include "gps.pb.h"
#include <cmath>
#include <cstring>
#include <thread>


iv::gps::gpsimu hcp2_obj;
int gntemp = 0;

void * gpa , * gpb;

QTime gTime;

bool gbRun = true;

std::thread * gstatethread;

iv::Ivfault *gfault = nullptr;
iv::Ivlog *givlog = nullptr;

int gnOldRtkState = 0;

int gnogpsdata = 0;

static inline void bigtolittle(char * strbig, char * strlittle, int n)
{
    int i;
    for(i = 0; i < n; i++)
    {
        strlittle[i] = strbig[n-1-i];
    }
}

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

       // qDebug("share time is %d ",gTime.elapsed());
         qDebug("time is %s",  qPrintable( QDateTime::fromMSecsSinceEpoch(QDateTime::currentMSecsSinceEpoch()).toString("yyyy--MM-dd hh:mm:ss:zzz") ));
        delete str;
}

void Decode_hcp2(iv::can::canmsg xmsg)
{

    static bool x1ok = false;
    static bool x2ok = false;
    static bool x3ok = false;
    static bool x4ok = false;
    static bool x5ok = false;

    double acc_x = 0.0, acc_y = 0.0, acc_z = 0.0, ins_pitch = 0.0, ins_roll = 0.0, ins_heading = 0.0,
            ins_northspd = 0.0, ins_eastspd = 0.0, ins_skyspd = 0.0, ins_carspd = 0.0,
            gyro_x = 0.0, gyro_y = 0.0, gyro_z = 0.0, ins_long = 0.0, ins_lat = 0.0, ins_localheight = 0.0, ins_time = 0.0;

    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() == 803)//323
        {
            x1ok = true;
            int  data[8];
            unsigned char cdata[8];
            memset(cdata,0,8);
            memcpy(cdata,canmsg.data().data(),8);
            int j;
            for(j=0;j<8;j++)
                data[j] = cdata[j];

            //unsigned char ins_status,rtk_status,sat_num;

            int ins_status,rtk_status,sat_num;


            if(0 == data[0])
            {
                ins_status = 3;
            }
            else
            {
                ins_status = 4;
            }

            sat_num = data[1];

            switch(data[2])
            {
            case 0:
                rtk_status = 0;
                break;
            case 1:
                rtk_status = 3;
                break;
            case 2:
                rtk_status = 4;
                break;
            case 3:
                rtk_status = 8;
                break;
            case 4:
                rtk_status = 6;
                break;
            case 5:
                rtk_status = 5;
                break;
            case 6:
                rtk_status = 3;
                break;
            case 7:
                rtk_status = 3;
                break;
            case 8:
                rtk_status = 5;
                break;
            case 9:
                rtk_status = 5;
                break;
            default:
                rtk_status = 0;
                break;
            }

            std::cout<<"rtk_status="<<rtk_status<<std::endl;
            std::cout<<"ins_status="<<ins_status<<std::endl;

            hcp2_obj.set_rtk_state(rtk_status);
            hcp2_obj.set_ins_state(ins_status);
            hcp2_obj.set_satnum1(sat_num);  //....

        }



        if(canmsg.id()==804){//0x324
            x2ok = true;
            int  data[8];
            unsigned char cdata[8];
            memset(cdata,0,8);
            memcpy(cdata,canmsg.data().data(),8);

            qDebug("candata(id=804) = %02x %02x %02x %02x %02x %02x %02x %02x \n",cdata[7],cdata[6],cdata[5],cdata[4],cdata[3],cdata[2],cdata[1],cdata[0]);;

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

            int ins_latt,ins_latt1;
            int ins_longg,ins_longg1;

                        ins_latt1=((data[0]<<24)|(data[1]<<16)|(data[2]<<8)|data[3]);
                        ins_longg1=((data[4]<<24)|(data[5]<<16)|(data[6]<<8)|data[7]);
//            char strlitlat[4],strlitlon[4];
//            bigtolittle((char *)&data[0],strlitlat,4);
//            bigtolittle((char *)&data[4],strlitlon,4);
//            memcpy(&ins_latt,strlitlat,4);
//            memcpy(&ins_longg,strlitlon,4);

//            ins_lat=ins_latt*0.0000001;
//            ins_long=ins_longg*0.0000001;

            double ins_lat1,ins_long1;
            ins_lat1=ins_latt1*0.0000001;
            ins_long1=ins_longg1*0.0000001;

            hcp2_obj.set_lat(ins_lat1);
            hcp2_obj.set_lon(ins_long1);
//             std::cout<<"ins_latt= \n"<<ins_lat;
//             std::cout<<"ins_longg= \n"<<ins_long;

             std::cout<<"ins_latt1= "<<ins_lat1<<std::endl;
             std::cout<<"ins_longg1= "<<ins_long1<<std::endl;
        }


        if(canmsg.id()==805){//0x325
            x3ok = true;
            int  data[8];
            unsigned char cdata[8];
            memset(cdata,0,8);
            memcpy(cdata,canmsg.data().data(),8);
            int j;
            for(j=0;j<8;j++)
                data[j] = cdata[j];

            int localheight;

            localheight=((data[0]<<24)+(data[1]<<16)+(data[2]<<8)+data[3]);
            ins_localheight=localheight*0.001;

            hcp2_obj.set_height(ins_localheight);

            std::cout<<"ins_localheight= "<<ins_localheight<<std::endl;
        }


        if(canmsg.id()==807){//327
            x4ok = true;
            int16_t data[8];
            unsigned char cdata[8];
            memset(cdata,0,8);
            memcpy(cdata,canmsg.data().data(),8);
            int j;
            for(j=0;j<8;j++)
                data[j] = cdata[j];

            short northspd;
            short eastspd;
            short skyspd;
            short carspd;

            eastspd=((data[0]<<8) + data[1]);
            northspd=((data[2]<<8) + data[3]);
            skyspd=((data[4]<<8) + data[5]);
            carspd=((data[6]<<8) + data[7]);

            ins_northspd=northspd*0.01;
            ins_eastspd=eastspd*0.01;
            ins_skyspd=skyspd*0.01;
            ins_carspd=carspd*0.01;
    qDebug("e: %f, n: %f, c: %f", ins_eastspd, ins_northspd, ins_carspd);
            hcp2_obj.set_vn(ins_northspd);
            hcp2_obj.set_ve(ins_eastspd);
            hcp2_obj.set_vd(ins_skyspd);
            hcp2_obj.set_speed(ins_carspd);
        }


        if(canmsg.id()==810){//32A
            x5ok = true;
        //    short  data[8];
            char cdata[8];
            memset(cdata,0,8);
            memcpy(cdata,canmsg.data().data(),8);
         //   int j;
         //   for(j=0;j<8;j++)
         //       data[j] = cdata[j];

            unsigned short heading;
            short pitch;
            short roll;

            heading=((cdata[0]<<8)| cdata[1]);
            pitch=((cdata[2]<<8)| cdata[3]);
            roll=((cdata[4]<<8) | cdata[5]);

            ins_pitch=pitch*0.01;
            ins_roll=roll*0.01;
            ins_heading=heading*0.01;

            if(ins_heading < 0)ins_heading = ins_heading + 360.0;
            if(ins_heading >= 360)ins_heading = ins_heading - 360;

            hcp2_obj.set_heading(ins_heading);
            hcp2_obj.set_pitch(ins_pitch);
            hcp2_obj.set_roll(ins_roll);
        }



        if(x1ok && x2ok && x3ok && x4ok && x5ok)
        {
            x1ok = false;
            x2ok = false;
            x3ok = false;
            x4ok = false;
            x5ok = false;

            hcp2_obj.set_msgtime(QDateTime::currentMSecsSinceEpoch());
            hcp2_obj.set_time(QDateTime::currentMSecsSinceEpoch());
            std::cout<<"time is "<<gTime.elapsed()<<std::endl;
            givlog->verbose("gps lat:%11.7f lon:%11.7f heading:%11.3f rtk:%d",hcp2_obj.lat(),hcp2_obj.lon(),hcp2_obj.heading(),hcp2_obj.rtk_state());

            if(hcp2_obj.rtk_state() != gnOldRtkState)
            {
                gnOldRtkState = hcp2_obj.rtk_state();
                if(hcp2_obj.rtk_state() == 6)
                {
                    givlog->info("GPS Fix.");
                    gfault->SetFaultState(0,0,"GPS Fix");
                }
                else
                {
                    givlog->info("GPS Not Fix,  RTK State is %d",hcp2_obj.rtk_state());
                    gfault->SetFaultState(1,2,"GPS Not Fix.");
                }
            }

            ShareResult();
        }

}


}
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<<"hcp2 Listencan0 fail."<<std::endl;
        return;
    }

    Decode_hcp2(xmsg);

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

}
void Listencan1(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 Listencan1 fail."<<std::endl;
        return;
    }

   Decode_hcp2(xmsg);

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

}


void exitfunc()
{
    std::cout<<"enter exitfunc."<<std::endl;
    gbRun = false;
    gstatethread->join();
    iv::modulecomm::Unregister(gpa);
    iv::modulecomm::Unregister(gpb);
    std::cout<<"Complete exitfunc."<<std::endl;
}

void statethread()
{
    int nstate = 0;
    int noldstate = 0;
    while(gbRun)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        if(gnogpsdata < 1000000)gnogpsdata++;

        if(gnogpsdata > 100)nstate = 1;
        if(gnogpsdata > 6000)nstate = 2;
        if(nstate != noldstate)
        {
            noldstate = nstate;
            switch (nstate) {
            case 0:
                gfault->SetFaultState(0,0,"OK.");
                givlog->info("GPS OK.");
                break;
            case 1:
                gfault->SetFaultState(1,1,"More than 1 second not have data.");
                givlog->warn("More than 1 second not have data.");
                break;
            case 2:
                gfault->SetFaultState(2,3,"More than 60 seconds not have data.");
                givlog->error("More than 60 seconds not have data.");
                break;
            default:
                break;
            }
        }
    }
}
int main(int argc, char *argv[])
{
    RegisterIVBackTrace();
    showversion("detection_gps_hcp2");
    QCoreApplication a(argc, argv);

    gTime.start();

    gfault = new iv::Ivfault("detection_gps_hcp2");
    givlog = new iv::Ivlog("detection_gps_hcp2");

    gfault->SetFaultState(0,0,"Initialize.");

    QString strpath = QCoreApplication::applicationDirPath();
    if(argc < 2)
        strpath = strpath + "/detection_gps_hcp2.xml";
    else
        strpath = argv[1];
    std::cout<<strpath.toStdString()<<std::endl;

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

    std::string strmemcan0 = xp.GetParam("canrecv","canrecv1");
    std::string strmemsend0 = xp.GetParam("cansend","hcp2_gpsimu");


    gpa = iv::modulecomm::RegisterSend(strmemsend0.data(),100000,3);
    gpb = iv::modulecomm::RegisterRecv(strmemcan0.data(),Listencan0);

    gstatethread = new std::thread(statethread);

    iv::ivexit::RegIVExitCall(exitfunc);


    return a.exec();
}