ivdriver_gps_hcp2.cpp 4.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195
  1. #include "ivdriver_gps_hcp2.h"
  2. #include <math.h>
  3. namespace iv {
  4. ivdriver_gps_hcp2::ivdriver_gps_hcp2(std::string strxmlpath)
  5. {
  6. loadxmlparam(strxmlpath);
  7. mTime.start();
  8. }
  9. int ivdriver_gps_hcp2::decode(iv::gps::gpsimu & xgpsimu)
  10. {
  11. int xpos = mstrBuffer.indexOf('\n');
  12. QString strsen;
  13. if(xpos>0)
  14. {
  15. strsen = mstrBuffer.left(xpos+1);
  16. mstrBuffer.remove(0,xpos+1);
  17. }
  18. else
  19. {
  20. return 0;
  21. }
  22. QStringList strlistrmc;
  23. RETRYDECODE:
  24. strlistrmc = strsen.split(",");
  25. if(strlistrmc.size() < 23)return -1;
  26. if(strlistrmc.at(0) != "$GPCHC")return -2;
  27. if(!checknmeasen(strsen.toLatin1().data(),strsen.length()))
  28. {
  29. int nPos = strsen.indexOf('$',10);
  30. if(nPos > 0)
  31. {
  32. QString strnewsen = strsen.right(strsen.size()-nPos);
  33. // qDebug("new sen is %s",strnewsen.toLatin1().data());
  34. strsen = strnewsen;
  35. goto RETRYDECODE;
  36. }
  37. return -3;
  38. }
  39. double fheading,fLat,fLon,fVel,fPitch,fRoll;
  40. double fHgt,gyro_x,gyro_y,gyro_z,acc_x,acc_y,acc_z;
  41. int nsv1,nsv2;
  42. int gpsweek,gpstime;
  43. int insstate,rtkstate;
  44. QString strx = strlistrmc.at(3);
  45. fheading = strx.toDouble();
  46. strx = strlistrmc.at(1);
  47. gpsweek = strx.toInt();
  48. strx = strlistrmc.at(2);
  49. gpstime = strx.toInt();
  50. strx = strlistrmc.at(12);
  51. fLat = strx.toDouble();
  52. strx = strlistrmc.at(13);
  53. fLon = strx.toDouble();
  54. strx = strlistrmc.at(14);
  55. fHgt = strx.toDouble();
  56. double ve,vn,vu;
  57. strx = strlistrmc.at(15);
  58. ve = strx.toDouble();
  59. strx = strlistrmc.at(16);
  60. vn = strx.toDouble();
  61. strx = strlistrmc.at(17);
  62. vu = strx.toDouble();
  63. fVel = sqrt(ve*ve + vn* vn);
  64. if((mTime.elapsed()-mOldTime) >= 100)
  65. {
  66. if(mOldTime == 0)
  67. {
  68. mfCalc_acc = 0;
  69. mfOldVel = fVel;
  70. mOldTime = mTime.elapsed();
  71. }
  72. else
  73. {
  74. mfCalc_acc = (fVel - mfOldVel)/((mTime.elapsed() - mOldTime)*0.001);
  75. mfOldVel = fVel;
  76. mOldTime = mTime.elapsed();
  77. }
  78. }
  79. strx = strlistrmc.at(4);
  80. fPitch = strx.toDouble();
  81. strx = strlistrmc.at(5);
  82. fRoll = strx.toDouble();
  83. strx = strlistrmc.at(6);
  84. gyro_x = strx.toDouble();
  85. strx = strlistrmc.at(7);
  86. gyro_y = strx.toDouble();
  87. strx = strlistrmc.at(8);
  88. gyro_z = strx.toDouble();
  89. strx = strlistrmc.at(9);
  90. acc_x = strx.toDouble();
  91. strx = strlistrmc.at(10);
  92. acc_y = strx.toDouble();
  93. strx = strlistrmc.at(11);
  94. acc_z = strx.toDouble();
  95. strx = strlistrmc.at(19);
  96. nsv1 = strx.toInt();
  97. strx = strlistrmc.at(20);
  98. nsv2 = strx.toInt();
  99. strx = strlistrmc.at(21);
  100. char strstate[3];
  101. strstate[2] = 0;
  102. if(strx.size() >= 2)
  103. {
  104. memcpy(strstate,strx.data(),2);
  105. // qDebug(strstate);
  106. char xstate;
  107. xstate = strstate[0];
  108. rtkstate = 4;
  109. int xs;
  110. if(xstate == '0')xs = 0;
  111. if(xstate == '1')xs = 3;
  112. if(xstate == '2')xs = 4;
  113. if(xstate == '3')xs = 8;
  114. if(xstate == '4')xs = 6;
  115. if(xstate == '5')xs = 5;
  116. if(xstate == '6')xs = 3;
  117. if(xstate == '7')xs = 4;
  118. if(xstate == '8')xs = 5;
  119. if(xstate == '9')xs = 5;
  120. rtkstate = xs;
  121. // if(mstate == 0)minsstate = 0;
  122. // else minsstate = 4;
  123. xstate = strstate[1];
  124. if(xstate == '0')insstate = 3;
  125. else insstate = 4;
  126. if(rtkstate == 0)insstate = 3;
  127. }
  128. xgpsimu.set_vd(vu);
  129. xgpsimu.set_ve(ve);
  130. xgpsimu.set_vn(vn);
  131. xgpsimu.set_lat(fLat);
  132. xgpsimu.set_lon(fLon);
  133. xgpsimu.set_heading(fheading);
  134. xgpsimu.set_state(4);
  135. xgpsimu.set_msgtime(QDateTime::currentMSecsSinceEpoch());
  136. xgpsimu.set_roll(fRoll);
  137. xgpsimu.set_pitch(fPitch);
  138. xgpsimu.set_rtk_state(rtkstate);
  139. xgpsimu.set_ins_state(insstate);
  140. xgpsimu.set_height(fHgt);
  141. xgpsimu.set_satnum1(nsv1);
  142. xgpsimu.set_satnum2(nsv2);
  143. xgpsimu.set_gpsweek(gpsweek);
  144. xgpsimu.set_gpstime(gpstime);
  145. xgpsimu.set_gyro_x(gyro_x);
  146. xgpsimu.set_gyro_y(gyro_y);
  147. xgpsimu.set_gyro_z(gyro_z);
  148. xgpsimu.set_acce_x(acc_x);
  149. xgpsimu.set_acce_y(acc_y);
  150. xgpsimu.set_acce_z(acc_z);
  151. xgpsimu.set_acc_calc(mfCalc_acc);
  152. return 1;
  153. }
  154. }