modularization/src/tool/view_gps/mainwindow.cpp

#include "mainwindow.h"
#include "ui_mainwindow.h"
#include <iostream>

extern char gstr_memname[256];

//std::string gstrItem[] = {"Pitch","Roll","Heading","gyro_x","gyro_y","gyro_z","acce_x","acce_y",
//                         "acce_z","lat","lon","hgt","vn","ve","vd","state","time","type",
//                         "latstd","lonstd","hstd","vnstd","vestd","vdstd","rollstd",
//                         "pitchstd","yawstd","temp","posstate","headstate","satnum","wheeldata"};

std::string gstrItem[] = {"Pitch","Roll","Heading",
                         "lat","lon","hgt","vn","ve","vd","insstate","rtkstate","satnum",
                         "gyro_x","gyro_y","gyro_z","acc_x","acc_y","acc_z"};


std::string gstrDiffItem[] ={"PitchDiff","RollDiff","HeadingDiff","latdiff","londiff","hgtdiff",
                             "vndiff","vediff","vddiff","avglatdiff","avglondiff","avghgtdiff",
                            "maxPitchDiff","maxRollDiff","maxHeadDiff",
                             "maxlatdiff","maxlondiff","maxhgtdiff"};

double gdiff[18];
int gdiffcount = 0;
double gdifftotal[6];

std::string gstrCount[] = {"Total","Posstd","Velstd","Posestd","Devtemp","GPSState","WheelData"};

int gcountRaw[] = {0,0,0,0,0,0,0,0};
int gcountFusion[] = {0,0,0,0,0,0,0,0};

MainWindow * gw;

void ListenRaw(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    iv::gps::gpsimu xgpsimu;
    if(!xgpsimu.ParseFromArray(strdata,nSize))
    {
        std::cout<<"ListenRaw Parse error."<<std::endl;
    }

    gw->mMutexraw.lock();
    while(gw->mqueueraw.size() > 1000)
    {
        gw->mqueueraw.pop();
    }
    gw->mqueueraw.push(xgpsimu);
    gcountRaw[0]++;
    switch (xgpsimu.type()) {
    case 0:
        gcountRaw[1]++;
        break;
    case 1:
        gcountRaw[2]++;
        break;
    case 2:
        gcountRaw[3]++;
        break;
    case 22:
        gcountRaw[4]++;
        break;
    case 32:
        gcountRaw[5]++;
        break;
    case 33:
        gcountRaw[6]++;
        break;
    default:
        break;
    }
    gw->mMutexraw.unlock();
}

void ListenFusion(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    iv::gps::gpsimu xgpsimu;
    if(!xgpsimu.ParseFromArray(strdata,nSize))
    {
        std::cout<<"ListenRaw Parse error."<<std::endl;
    }

    gw->mMutexfusion.lock();
    while(gw->mqueuefusion.size() > 1000)
    {
        gw->mqueuefusion.pop();
    }
    gw->mqueuefusion.push(xgpsimu);
    gcountRaw[0]++;
    switch (xgpsimu.type()) {
    case 0:
        gcountRaw[1]++;
        break;
    case 1:
        gcountRaw[2]++;
        break;
    case 2:
        gcountRaw[3]++;
        break;
    case 22:
        gcountRaw[4]++;
        break;
    case 32:
        gcountRaw[5]++;
        break;
    case 33:
        gcountRaw[6]++;
        break;
    default:
        break;
    }
    gw->mMutexfusion.unlock();
}

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    gw = this;
    CreateView();

    void * paraw = iv::modulecomm::RegisterRecv(gstr_memname,ListenRaw);
    void * pafusion = iv::modulecomm::RegisterRecv("fusion_gpsimu",ListenFusion);


    QTimer * timer = new QTimer(this);
    connect(timer,SIGNAL(timeout()),this,SLOT(onTimer()));
    timer->start(5);
    setWindowTitle("view_gps");
}

MainWindow::~MainWindow()
{
    delete ui;
}

void MainWindow::CreateView()
{
    QTabWidget * p = new QTabWidget(ui->centralWidget);
    p->setGeometry(30,30,300,300);

    CreateTab1View(p);
//    CreateTab2View(p);
//    CreateTab3View(p);

    mTabMain = p;
}

void MainWindow::CreateTab1View(QTabWidget * p)
{
    QGroupBox * pGroup = new QGroupBox();
    pGroup->setGeometry(0,0,1300,800);

    const int TWidth = 1300;
    const int nRowDis = 50;
    const int nColDis = 200;
    const int nIWidth = 95;
    const int nIHeight = 30;

    int i;
    int j;
    i=1;j = 0;
    int index = 0;
    for(auto x : gstrItem)
    {
        QLabel * pLabel;
        QLineEdit * pLineEdit;
        pLabel = new QLabel(pGroup);
        std::string strname = x;
        pLabel->setText(strname.data());
        pLineEdit = new QLineEdit(pGroup);
        pLabel->setGeometry(10+j*nColDis,i*nRowDis,nIWidth,nIHeight);
        pLineEdit->setGeometry(10+j*nColDis+nColDis/2,i*nRowDis,nIWidth,nIHeight);
        pLabel->setAlignment(Qt::AlignRight|Qt::AlignCenter);
        mpLabelRaw[index] = pLabel;
        mpLERaw[index] = pLineEdit;
        j++;
        if((j+1)*nColDis>TWidth)
        {
            j= 0;
            i++;
        }
        index++;
    }

    i++;
    i++;
    j = 0;

    mnCountIndexBase = index;

    QScrollArea * pScroll = new QScrollArea();
    pScroll->setWidget(pGroup);

    p->addTab(pScroll,"gpsimu");
}

void MainWindow::CreateTab2View(QTabWidget * p)
{
    QGroupBox * pGroup = new QGroupBox();
    pGroup->setGeometry(0,0,1300,800);

    const int TWidth = 1300;
    const int nRowDis = 50;
    const int nColDis = 200;
    const int nIWidth = 95;
    const int nIHeight = 30;

    int i;
    int j;
    i=1;j = 0;
    int index = 0;
    for(auto x : gstrItem)
    {
        QLabel * pLabel;
        QLineEdit * pLineEdit;
        pLabel = new QLabel(pGroup);
        std::string strname = x;
        pLabel->setText(strname.data());
        pLineEdit = new QLineEdit(pGroup);
        pLabel->setGeometry(10+j*nColDis,i*nRowDis,nIWidth,nIHeight);
        pLineEdit->setGeometry(10+j*nColDis+nColDis/2,i*nRowDis,nIWidth,nIHeight);
        pLabel->setAlignment(Qt::AlignRight|Qt::AlignCenter);
        mpLabelFusion[index] = pLabel;
        mpLEFusion[index] = pLineEdit;
        j++;
        if((j+1)*nColDis>TWidth)
        {
            j= 0;
            i++;
        }
        index++;
    }

    i++;i++;j= 0;

    for(auto x : gstrCount)
    {
        QLabel * pLabel;
        QLineEdit * pLineEdit;
        pLabel = new QLabel(pGroup);
        std::string strname = x;
        pLabel->setText(strname.data());
        pLineEdit = new QLineEdit(pGroup);
        pLabel->setGeometry(10+j*nColDis,i*nRowDis,nIWidth,nIHeight);
        pLineEdit->setGeometry(10+j*nColDis+nColDis/2,i*nRowDis,nIWidth,nIHeight);
        pLabel->setAlignment(Qt::AlignRight|Qt::AlignCenter);
        mpLabelFusion[index] = pLabel;
        mpLEFusion[index] = pLineEdit;
        j++;
        if((j+1)*nColDis>TWidth)
        {
            j= 0;
            i++;
        }
        index++;
    }

    QScrollArea * pScroll = new QScrollArea();
    pScroll->setWidget(pGroup);

    p->addTab(pScroll,"Fusion");
}

void MainWindow::CreateTab3View(QTabWidget * p)
{
    QGroupBox * pGroup = new QGroupBox();
    pGroup->setGeometry(0,0,1300,800);

    const int TWidth = 1300;
    const int nRowDis = 50;
    const int nColDis = 200;
    const int nIWidth = 95;
    const int nIHeight = 30;

    int i;
    int j;
    i=1;j = 0;
    int index = 0;
    for(auto x : gstrDiffItem)
    {
        QLabel * pLabel;
        QLineEdit * pLineEdit;
        pLabel = new QLabel(pGroup);
        std::string strname = x;
        pLabel->setText(strname.data());
        pLineEdit = new QLineEdit(pGroup);
        pLabel->setGeometry(10+j*nColDis,i*nRowDis,nIWidth,nIHeight);
        pLineEdit->setGeometry(10+j*nColDis+nColDis/2,i*nRowDis,nIWidth,nIHeight);
        pLabel->setAlignment(Qt::AlignRight|Qt::AlignCenter);
        mpLabelDiff[index] = pLabel;
        mpLEDiff[index] = pLineEdit;
        j++;
        if((j+1)*nColDis>TWidth)
        {
            j= 0;
            i++;
        }
        index++;
    }

    QScrollArea * pScroll = new QScrollArea();
    pScroll->setWidget(pGroup);

    p->addTab(pScroll,"Diff");
}

void MainWindow::resizeEvent(QResizeEvent *event)
{
//    qDebug("resize");
    QSize sizemain = ui->centralWidget->size();
//    qDebug("size x = %d y=%d",sizemain.width(),sizemain.height());

    AdjustWPos(sizemain);

}

void MainWindow::AdjustWPos(QSize sizemain)
{
    mTabMain->setGeometry(10,10,sizemain.width() - 20,sizemain.height()-10);
}

inline void MainWindow::SetLEView(int nTab, const char *strName, double value, const char * strvalue,const int nType)
{
    int i;
    QLineEdit * pLE = 0;
    int nBase = 0;
    if(nType == 3)nBase = mnCountIndexBase;
    if(nTab == 0)
    {
        i = 0;
        if(nBase == 0)
        {
            for(auto x : gstrItem)
            {
                std::string str = x;
                const char * y = str.data();
                if(strcmp(str.data(), strName) == 0)
                {

                    pLE = mpLERaw[i+nBase];
                    break;
                }
                i++;
            }

        }
        else
        {
            for(auto x : gstrCount)
            {
                std::string str = x;
                const char * y = str.data();
                if(strcmp(str.data(), strName) == 0)
                {

                    pLE = mpLERaw[i+nBase];
                    break;
                }
                i++;
            }

        }
    }
    if(nTab == 1)
    {
        i = 0;
        if(nBase == 0)
        {
            for(auto x : gstrItem)
            {
                std::string str = x;
                if(strcmp(str.data(),strName) == 0)
                {
                    pLE = mpLEFusion[i+nBase];
                    break;
                }
                i++;
            }
        }
        else
        {
            for(auto x : gstrCount)
            {
                std::string str = x;
                const char * y = str.data();
                if(strcmp(str.data(), strName) == 0)
                {

                    pLE = mpLEFusion[i+nBase];
                    break;
                }
                i++;
            }

        }
    }
    if(nTab == 2)
    {
        i = 0;
        for(auto x : gstrDiffItem)
        {
            std::string str = x;
            if(strcmp(str.data(), strName) == 0)
            {
                pLE = mpLEFusion[i];
                break;
            }
            i++;
        }
    }

    if(pLE == 0)
    {
        return;
    }

    if(nType == 0)
    {
        pLE->setText(QString::number(value));
    }
    if(nType == 1)
    {
        pLE->setText(QString::number(value,'f',7));
    }
    if(nType == 2)
    {
        pLE->setText(strvalue);
    }
    if(nType == 3)
    {
        pLE->setText(QString::number(value));
    }


}

void MainWindow::UpdateCompView()
{
    const int tab = 2;
    int i;
    for(i=0;i<18;i++)SetLEView(tab,gstrDiffItem[i].data(),gdiff[i]);
}

void MainWindow::UpdateGPSView(const int tab,iv::gps::gpsimu xgpsimu)
{
    SetLEView(tab,gstrItem[0].data(),xgpsimu.pitch());
    SetLEView(tab,gstrItem[1].data(),xgpsimu.roll());
    SetLEView(tab,gstrItem[2].data(),xgpsimu.heading());
    SetLEView(tab,gstrItem[3].data(),xgpsimu.lat(),0,1);
    SetLEView(tab,gstrItem[4].data(),xgpsimu.lon(),0,1);
    SetLEView(tab,gstrItem[5].data(),xgpsimu.height());
    SetLEView(tab,gstrItem[6].data(),xgpsimu.vn());
    SetLEView(tab,gstrItem[7].data(),xgpsimu.ve());
    SetLEView(tab,gstrItem[8].data(),xgpsimu.vd());
    SetLEView(tab,gstrItem[9].data(),xgpsimu.ins_state());
    SetLEView(tab,gstrItem[10].data(),xgpsimu.rtk_state());
    SetLEView(tab,gstrItem[11].data(),xgpsimu.satnum1());
    SetLEView(tab,gstrItem[12].data(),xgpsimu.gyro_x());
    SetLEView(tab,gstrItem[13].data(),xgpsimu.gyro_y());
    SetLEView(tab,gstrItem[14].data(),xgpsimu.gyro_z());
    SetLEView(tab,gstrItem[15].data(),xgpsimu.acce_x());
    SetLEView(tab,gstrItem[16].data(),xgpsimu.acce_y());
    SetLEView(tab,gstrItem[17].data(),xgpsimu.acce_z());
    if(fabs(xgpsimu.acce_z()+1.0)>0.1)
    {
        qDebug("acc is %f gyro z is %f h:%f",xgpsimu.acce_z(),xgpsimu.gyro_z(),
               xgpsimu.height());
    }
    if(fabs(xgpsimu.vd())>0.5)
    {
        qDebug("vd is %f",xgpsimu.vd());
    }
//    qDebug("state is %d %d %d",xgpsimu.gps_state().pos_state(),xgpsimu.gps_state().heading_state(),xgpsimu.gps_state().satnum());
//    SetLEView(tab,gstrItem[12].data(),xgpsimu.);

}

void MainWindow::CompareData(iv::gps::gpsimu gi, iv::gps::gpsimu gf)
{
    gdiff[0] = fabs(gf.pitch() - gi.pitch());
    gdiff[1] = fabs(gf.roll() - gi.roll());
    gdiff[2] = fabs(gf.heading() - gi.heading());
    gdiff[3] = fabs(gf.lat() - gi.lat());
    gdiff[4] = fabs(gf.lon() - gi.lon());
    gdiff[5] = fabs(gf.height() - gi.height());
    gdiff[6] = fabs(gf.vn() - gi.vn());
    gdiff[7] = fabs(gf.ve() - gi.ve());
    gdiff[8] = fabs(gf.vd() - gi.vd());
    gdiffcount++;
    gdifftotal[0] += gdiff[3];
    gdifftotal[1] += gdiff[4];
    gdifftotal[2] += gdiff[5];

    gdiff[9] = gdifftotal[0]/gdiffcount;
    gdiff[10] = gdifftotal[1]/gdiffcount;
    gdiff[11] = gdifftotal[2]/gdiffcount;

    int i;
    for(i=0;i<6;i++)
        if(gdiff[i] > gdiff[12+i])gdiff[12+i]= gdiff[i];

}

void MainWindow::onTimer()
{
    static int nLastRaw = 0;
    static int nLastFusion = 0;
    bool bRawGet = false;
    bool bFusionGet = false;
    iv::gps::gpsimu rawgpsimu,fusiongpsimu;
    if(nLastRaw < gcountRaw[0])
    {
        mMutexraw.lock();
        while(mqueueraw.size() > 0)
        {
            rawgpsimu = mqueueraw.front();
            mqueueraw.pop();
            mqueuecpraw.push(rawgpsimu);
            bRawGet = true;
        }
        nLastRaw = gcountRaw[0];
        mMutexraw.unlock();
    }
    if(nLastFusion < gcountFusion[0])
    {
        mMutexfusion.lock();
        while(mqueuefusion.size() > 0)
        {
            rawgpsimu = mqueuefusion.front();
            mqueuefusion.pop();
            mqueuecpfusion.push(fusiongpsimu);
            bFusionGet = true;
        }
        nLastFusion = gcountFusion[0];
        mMutexfusion.unlock();
    }
    if(bRawGet)UpdateGPSView(0,rawgpsimu);
//    if(bFusionGet)UpdateGPSView(1,fusiongpsimu);
//    if(bRawGet && bFusionGet)
//    {
//        while((mqueuecpfusion.size()>0)&&(mqueuecpraw.size()>0))
//        {
//            iv::gps::gpsimu gi,fi;
//            gi = mqueuecpraw.front();
//            fi = mqueuecpfusion.front();
//            if(gi.time() == fi.time())
//            {
//                //Compare
//                CompareData(gi,fi);
//                mqueuecpfusion.pop();
//                mqueuecpraw.pop();
//                continue;
//            }
//            if(gi.time() > fi.time())
//            {
//                mqueuecpfusion.pop();
//            }
//            if(gi.time() < fi.time())
//            {
//                mqueuecpraw.pop();
//            }
//        }
//    }
}