123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195 |
- #include "ivdriver_gps_hcp2.h"
- #include <math.h>
- namespace iv {
- ivdriver_gps_hcp2::ivdriver_gps_hcp2(std::string strxmlpath)
- {
- loadxmlparam(strxmlpath);
- mTime.start();
- }
- int ivdriver_gps_hcp2::decode(iv::gps::gpsimu & xgpsimu)
- {
- int xpos = mstrBuffer.indexOf('\n');
- QString strsen;
- if(xpos>0)
- {
- strsen = mstrBuffer.left(xpos+1);
- mstrBuffer.remove(0,xpos+1);
- }
- else
- {
- return 0;
- }
- QStringList strlistrmc;
- RETRYDECODE:
- strlistrmc = strsen.split(",");
- if(strlistrmc.size() < 23)return -1;
- if(strlistrmc.at(0) != "$GPCHC")return -2;
- if(!checknmeasen(strsen.toLatin1().data(),strsen.length()))
- {
- int nPos = strsen.indexOf('$',10);
- if(nPos > 0)
- {
- QString strnewsen = strsen.right(strsen.size()-nPos);
- // qDebug("new sen is %s",strnewsen.toLatin1().data());
- strsen = strnewsen;
- goto RETRYDECODE;
- }
- return -3;
- }
- double fheading,fLat,fLon,fVel,fPitch,fRoll;
- double fHgt,gyro_x,gyro_y,gyro_z,acc_x,acc_y,acc_z;
- int nsv1,nsv2;
- int gpsweek,gpstime;
- int insstate,rtkstate;
- QString strx = strlistrmc.at(3);
- fheading = strx.toDouble();
- strx = strlistrmc.at(1);
- gpsweek = strx.toInt();
- strx = strlistrmc.at(2);
- gpstime = strx.toInt();
- strx = strlistrmc.at(12);
- fLat = strx.toDouble();
- strx = strlistrmc.at(13);
- fLon = strx.toDouble();
- strx = strlistrmc.at(14);
- fHgt = strx.toDouble();
- double ve,vn,vu;
- strx = strlistrmc.at(15);
- ve = strx.toDouble();
- strx = strlistrmc.at(16);
- vn = strx.toDouble();
- strx = strlistrmc.at(17);
- vu = strx.toDouble();
- fVel = sqrt(ve*ve + vn* vn);
- if((mTime.elapsed()-mOldTime) >= 100)
- {
- if(mOldTime == 0)
- {
- mfCalc_acc = 0;
- mfOldVel = fVel;
- mOldTime = mTime.elapsed();
- }
- else
- {
- mfCalc_acc = (fVel - mfOldVel)/((mTime.elapsed() - mOldTime)*0.001);
- mfOldVel = fVel;
- mOldTime = mTime.elapsed();
- }
- }
- strx = strlistrmc.at(4);
- fPitch = strx.toDouble();
- strx = strlistrmc.at(5);
- fRoll = strx.toDouble();
- strx = strlistrmc.at(6);
- gyro_x = strx.toDouble();
- strx = strlistrmc.at(7);
- gyro_y = strx.toDouble();
- strx = strlistrmc.at(8);
- gyro_z = strx.toDouble();
- strx = strlistrmc.at(9);
- acc_x = strx.toDouble();
- strx = strlistrmc.at(10);
- acc_y = strx.toDouble();
- strx = strlistrmc.at(11);
- acc_z = strx.toDouble();
- strx = strlistrmc.at(19);
- nsv1 = strx.toInt();
- strx = strlistrmc.at(20);
- nsv2 = strx.toInt();
- strx = strlistrmc.at(21);
- char strstate[3];
- strstate[2] = 0;
- if(strx.size() >= 2)
- {
- memcpy(strstate,strx.data(),2);
- // qDebug(strstate);
- char xstate;
- xstate = strstate[0];
- rtkstate = 4;
- int xs;
- if(xstate == '0')xs = 0;
- if(xstate == '1')xs = 3;
- if(xstate == '2')xs = 4;
- if(xstate == '3')xs = 8;
- if(xstate == '4')xs = 6;
- if(xstate == '5')xs = 5;
- if(xstate == '6')xs = 3;
- if(xstate == '7')xs = 4;
- if(xstate == '8')xs = 5;
- if(xstate == '9')xs = 5;
- rtkstate = xs;
- // if(mstate == 0)minsstate = 0;
- // else minsstate = 4;
- xstate = strstate[1];
- if(xstate == '0')insstate = 3;
- else insstate = 4;
- if(rtkstate == 0)insstate = 3;
- }
- xgpsimu.set_vd(vu);
- xgpsimu.set_ve(ve);
- xgpsimu.set_vn(vn);
- xgpsimu.set_lat(fLat);
- xgpsimu.set_lon(fLon);
- xgpsimu.set_heading(fheading);
- xgpsimu.set_state(4);
- xgpsimu.set_msgtime(QDateTime::currentMSecsSinceEpoch());
- xgpsimu.set_roll(fRoll);
- xgpsimu.set_pitch(fPitch);
- xgpsimu.set_rtk_state(rtkstate);
- xgpsimu.set_ins_state(insstate);
- xgpsimu.set_height(fHgt);
- xgpsimu.set_satnum1(nsv1);
- xgpsimu.set_satnum2(nsv2);
- xgpsimu.set_gpsweek(gpsweek);
- xgpsimu.set_gpstime(gpstime);
- xgpsimu.set_gyro_x(gyro_x);
- xgpsimu.set_gyro_y(gyro_y);
- xgpsimu.set_gyro_z(gyro_z);
- xgpsimu.set_acce_x(acc_x);
- xgpsimu.set_acce_y(acc_y);
- xgpsimu.set_acce_z(acc_z);
- xgpsimu.set_acc_calc(mfCalc_acc);
- return 1;
- }
- }
|