123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217 |
- #include "ivdriver_gps.h"
- #include <iostream>
- #include <math.h>
- namespace iv {
- ivdriver_gps::ivdriver_gps()
- {
- mudpSocketGPSIMU = new QUdpSocket();
- }
- void ivdriver_gps::loadxmlparam(std::string strxmlpath)
- {
- iv::xmlparam::Xmlparam xp(strxmlpath);
- mstrportname = xp.GetParam("devname","/dev/ttyUSB0");
- mstrmsgname = xp.GetParam("msg_gpsimu","hcp2_gpsimu");
- mnbaudrate = xp.GetParam("baudrate",230400);
- }
- void ivdriver_gps::ShareMsg(gps::gpsimu &xgpsimu)
- {
- char * strser;
- bool bser;
- int nbytesize;
- nbytesize = xgpsimu.ByteSize();
- strser = new char[nbytesize];
- bser = xgpsimu.SerializeToArray(strser,nbytesize);
- if(bser)
- iv::modulecomm::ModuleSendMsg(mpagpsimu,strser,nbytesize);
- else
- {
- std::cout<<"gpsimu serialize error."<<std::endl;
- // givlog->error("gpsimu serialize error.");
- // gfault->SetFaultState(1, 0, "gpsimu serialize err");
- }
- delete strser;
- }
- void ivdriver_gps::broadcastmsg(gps::gpsimu &xgpsimu)
- {
- unsigned char strSend[72];
- int i;
- for(i=0;i<72;i++)strSend[i] = 0x0;
- strSend[0] = 0xE7;
- strSend[21] = xgpsimu.ins_state();
- strSend[68] = xgpsimu.rtk_state();
- strSend[67] = 0;
- strSend[62] = 62;
- double fV = xgpsimu.lat();
- fV = fV/(180.0/M_PI);
- memcpy(strSend+23,&fV,8);
- fV = xgpsimu.lon();fV = fV/(180.0/M_PI);memcpy(strSend+31,&fV,8);
- float fF =0;memcpy(strSend+39,&fF,4);
- int iV;
- char * piV = (char *)&iV;
- piV++;
- fV = xgpsimu.ve();fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+43,piV,3);
- fV = xgpsimu.vn();fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+46,piV,3);
- fV = xgpsimu.vd();fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+49,piV,3);
- fV = xgpsimu.heading()/(180.0/M_PI);fV = fV*1000000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+52,piV,3);
- fV = xgpsimu.pitch()/(180.0/M_PI);fV = fV*1000000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+55,piV,3);
- fV = xgpsimu.roll()/(180.0/M_PI);fV = fV*1000000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+58,piV,3);
- double facc_x,facc_y,facc_z;
- facc_x = xgpsimu.acce_x();
- facc_y = xgpsimu.acce_y();
- facc_z = xgpsimu.acce_z();
- fV = facc_x;fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+3,piV,3);
- fV = facc_y;fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+6,piV,3);
- fV = facc_z;fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+9,piV,3);
- fV = xgpsimu.gyro_x();fV = fV*100000.0*M_PI/180.0;iV = (int)fV;iV = iV*256;memcpy(strSend+12,piV,3);
- fV = xgpsimu.gyro_y();fV = fV*100000.0*M_PI/180.0;iV = (int)fV;iV = iV*256;memcpy(strSend+15,piV,3);
- fV = xgpsimu.gyro_z();fV = fV*100000.0*M_PI/180.0;iV = (int)fV;iV = iV*256;memcpy(strSend+18,piV,3);
- char c;
- c = strSend[0];
- for(i=1;i<71;i++)c = c+strSend[i];
- strSend[71]= c;
- mudpSocketGPSIMU->writeDatagram((char *)strSend,72,QHostAddress::Broadcast, 3000);
- }
- void ivdriver_gps::modulerun()
- {
- std::cout<<"register msg name is "<<mstrmsgname<<std::endl;
- mpagpsimu = iv::modulecomm::RegisterSend(mstrmsgname.data(),10000,1);
- m_serialPort_GPS = new QSerialPort();
- m_serialPort_GPS->setPortName(mstrportname.data());
- m_serialPort_GPS->setBaudRate(230400);
- m_serialPort_GPS->setParity(QSerialPort::NoParity);
- m_serialPort_GPS->setDataBits(QSerialPort::Data8);
- m_serialPort_GPS->setStopBits(QSerialPort::OneStop);
- m_serialPort_GPS->setFlowControl(QSerialPort::NoFlowControl);
- m_serialPort_GPS->setReadBufferSize(0);
- mbrun = true;
- bool bPortOpen = false;
- OPENDEVICE:
- bPortOpen = false;
- std::cout<<" Open Device."<<std::endl;
- while(mbrun)
- {
- if(m_serialPort_GPS->open(QSerialPort::ReadWrite))
- {
- bPortOpen = true;
- break;
- }
- else
- {
- std::this_thread::sleep_for(std::chrono::milliseconds(1000));
- }
- std::cout<<"retry open "<<mstrportname<<" baudrate is "<<mnbaudrate<<std::endl;
- }
- int nnotrecv = 0;
- while(mbrun)
- {
- QByteArray ba = m_serialPort_GPS->readAll();
- if(ba.length() > 0)
- {
- nnotrecv = 0;
- mstrBuffer.push_back(ba);
- iv::gps::gpsimu xgpsimu;
- int nrtn = decode(xgpsimu);
- while(1 == nrtn)
- {
- ShareMsg(xgpsimu);
- broadcastmsg(xgpsimu);
- nrtn = decode(xgpsimu);
- }
- //Decode
- }
- else
- {
- nnotrecv++;
- std::this_thread::sleep_for(std::chrono::milliseconds(1));
- }
- if(nnotrecv == 1000)
- {
- std::cout<<"not recv data."<<std::endl;
- }
- if(nnotrecv > 3000)
- {
- m_serialPort_GPS->close();
- goto OPENDEVICE;
- }
- }
- if(bPortOpen)m_serialPort_GPS->close();
- delete m_serialPort_GPS;
- iv::modulecomm::Unregister(mpagpsimu);
- }
- bool ivdriver_gps::checknmeasen(const char *strsen, const unsigned int nlen)
- {
- if(nlen< 4)return false;
- int i;
- char check;
- int nstarpos = -1;
- check = strsen[1]^strsen[2];
- for(i=3;i<nlen;i++)
- {
- if(strsen[i] == '*')
- {
- nstarpos = i;
- break;
- }
- check = check^strsen[i];
- }
- if(nstarpos < 0)return false;
- if(nstarpos >(nlen -3))return false;
- char strcheck[3];
- int ncheck = check;
- snprintf(strcheck,3,"%02X",ncheck);
- char strsencheck[3];
- strsencheck[2] = 0;
- strsencheck[0] = strsen[nstarpos + 1];
- strsencheck[1] = strsen[nstarpos + 2];
- if(strncmp(strcheck,strsencheck,2) == 0)
- {
- // qDebug(" r sen is %s",strsen);
- return true;
- }
- // givlog->verbose("cal check is %s, sen check is %s",strcheck,strsencheck);
- // qDebug(" sen is %s",strsen);
- return false;
- }
- }
|