ivdriver_gps.cpp 5.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217
  1. #include "ivdriver_gps.h"
  2. #include <iostream>
  3. #include <math.h>
  4. namespace iv {
  5. ivdriver_gps::ivdriver_gps()
  6. {
  7. mudpSocketGPSIMU = new QUdpSocket();
  8. }
  9. void ivdriver_gps::loadxmlparam(std::string strxmlpath)
  10. {
  11. iv::xmlparam::Xmlparam xp(strxmlpath);
  12. mstrportname = xp.GetParam("devname","/dev/ttyUSB0");
  13. mstrmsgname = xp.GetParam("msg_gpsimu","hcp2_gpsimu");
  14. mnbaudrate = xp.GetParam("baudrate",230400);
  15. }
  16. void ivdriver_gps::ShareMsg(gps::gpsimu &xgpsimu)
  17. {
  18. char * strser;
  19. bool bser;
  20. int nbytesize;
  21. nbytesize = xgpsimu.ByteSize();
  22. strser = new char[nbytesize];
  23. bser = xgpsimu.SerializeToArray(strser,nbytesize);
  24. if(bser)
  25. iv::modulecomm::ModuleSendMsg(mpagpsimu,strser,nbytesize);
  26. else
  27. {
  28. std::cout<<"gpsimu serialize error."<<std::endl;
  29. // givlog->error("gpsimu serialize error.");
  30. // gfault->SetFaultState(1, 0, "gpsimu serialize err");
  31. }
  32. delete strser;
  33. }
  34. void ivdriver_gps::broadcastmsg(gps::gpsimu &xgpsimu)
  35. {
  36. unsigned char strSend[72];
  37. int i;
  38. for(i=0;i<72;i++)strSend[i] = 0x0;
  39. strSend[0] = 0xE7;
  40. strSend[21] = xgpsimu.ins_state();
  41. strSend[68] = xgpsimu.rtk_state();
  42. strSend[67] = 0;
  43. strSend[62] = 62;
  44. double fV = xgpsimu.lat();
  45. fV = fV/(180.0/M_PI);
  46. memcpy(strSend+23,&fV,8);
  47. fV = xgpsimu.lon();fV = fV/(180.0/M_PI);memcpy(strSend+31,&fV,8);
  48. float fF =0;memcpy(strSend+39,&fF,4);
  49. int iV;
  50. char * piV = (char *)&iV;
  51. piV++;
  52. fV = xgpsimu.ve();fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+43,piV,3);
  53. fV = xgpsimu.vn();fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+46,piV,3);
  54. fV = xgpsimu.vd();fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+49,piV,3);
  55. fV = xgpsimu.heading()/(180.0/M_PI);fV = fV*1000000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+52,piV,3);
  56. fV = xgpsimu.pitch()/(180.0/M_PI);fV = fV*1000000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+55,piV,3);
  57. fV = xgpsimu.roll()/(180.0/M_PI);fV = fV*1000000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+58,piV,3);
  58. double facc_x,facc_y,facc_z;
  59. facc_x = xgpsimu.acce_x();
  60. facc_y = xgpsimu.acce_y();
  61. facc_z = xgpsimu.acce_z();
  62. fV = facc_x;fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+3,piV,3);
  63. fV = facc_y;fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+6,piV,3);
  64. fV = facc_z;fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+9,piV,3);
  65. fV = xgpsimu.gyro_x();fV = fV*100000.0*M_PI/180.0;iV = (int)fV;iV = iV*256;memcpy(strSend+12,piV,3);
  66. fV = xgpsimu.gyro_y();fV = fV*100000.0*M_PI/180.0;iV = (int)fV;iV = iV*256;memcpy(strSend+15,piV,3);
  67. fV = xgpsimu.gyro_z();fV = fV*100000.0*M_PI/180.0;iV = (int)fV;iV = iV*256;memcpy(strSend+18,piV,3);
  68. char c;
  69. c = strSend[0];
  70. for(i=1;i<71;i++)c = c+strSend[i];
  71. strSend[71]= c;
  72. mudpSocketGPSIMU->writeDatagram((char *)strSend,72,QHostAddress::Broadcast, 3000);
  73. }
  74. void ivdriver_gps::modulerun()
  75. {
  76. std::cout<<"register msg name is "<<mstrmsgname<<std::endl;
  77. mpagpsimu = iv::modulecomm::RegisterSend(mstrmsgname.data(),10000,1);
  78. m_serialPort_GPS = new QSerialPort();
  79. m_serialPort_GPS->setPortName(mstrportname.data());
  80. m_serialPort_GPS->setBaudRate(230400);
  81. m_serialPort_GPS->setParity(QSerialPort::NoParity);
  82. m_serialPort_GPS->setDataBits(QSerialPort::Data8);
  83. m_serialPort_GPS->setStopBits(QSerialPort::OneStop);
  84. m_serialPort_GPS->setFlowControl(QSerialPort::NoFlowControl);
  85. m_serialPort_GPS->setReadBufferSize(0);
  86. mbrun = true;
  87. bool bPortOpen = false;
  88. OPENDEVICE:
  89. bPortOpen = false;
  90. std::cout<<" Open Device."<<std::endl;
  91. while(mbrun)
  92. {
  93. if(m_serialPort_GPS->open(QSerialPort::ReadWrite))
  94. {
  95. bPortOpen = true;
  96. break;
  97. }
  98. else
  99. {
  100. std::this_thread::sleep_for(std::chrono::milliseconds(1000));
  101. }
  102. std::cout<<"retry open "<<mstrportname<<" baudrate is "<<mnbaudrate<<std::endl;
  103. }
  104. int nnotrecv = 0;
  105. while(mbrun)
  106. {
  107. QByteArray ba = m_serialPort_GPS->readAll();
  108. if(ba.length() > 0)
  109. {
  110. nnotrecv = 0;
  111. mstrBuffer.push_back(ba);
  112. iv::gps::gpsimu xgpsimu;
  113. int nrtn = decode(xgpsimu);
  114. while(1 == nrtn)
  115. {
  116. ShareMsg(xgpsimu);
  117. broadcastmsg(xgpsimu);
  118. nrtn = decode(xgpsimu);
  119. }
  120. //Decode
  121. }
  122. else
  123. {
  124. nnotrecv++;
  125. std::this_thread::sleep_for(std::chrono::milliseconds(1));
  126. }
  127. if(nnotrecv == 1000)
  128. {
  129. std::cout<<"not recv data."<<std::endl;
  130. }
  131. if(nnotrecv > 3000)
  132. {
  133. m_serialPort_GPS->close();
  134. goto OPENDEVICE;
  135. }
  136. }
  137. if(bPortOpen)m_serialPort_GPS->close();
  138. delete m_serialPort_GPS;
  139. iv::modulecomm::Unregister(mpagpsimu);
  140. }
  141. bool ivdriver_gps::checknmeasen(const char *strsen, const unsigned int nlen)
  142. {
  143. if(nlen< 4)return false;
  144. int i;
  145. char check;
  146. int nstarpos = -1;
  147. check = strsen[1]^strsen[2];
  148. for(i=3;i<nlen;i++)
  149. {
  150. if(strsen[i] == '*')
  151. {
  152. nstarpos = i;
  153. break;
  154. }
  155. check = check^strsen[i];
  156. }
  157. if(nstarpos < 0)return false;
  158. if(nstarpos >(nlen -3))return false;
  159. char strcheck[3];
  160. int ncheck = check;
  161. snprintf(strcheck,3,"%02X",ncheck);
  162. char strsencheck[3];
  163. strsencheck[2] = 0;
  164. strsencheck[0] = strsen[nstarpos + 1];
  165. strsencheck[1] = strsen[nstarpos + 2];
  166. if(strncmp(strcheck,strsencheck,2) == 0)
  167. {
  168. // qDebug(" r sen is %s",strsen);
  169. return true;
  170. }
  171. // givlog->verbose("cal check is %s, sen check is %s",strcheck,strsencheck);
  172. // qDebug(" sen is %s",strsen);
  173. return false;
  174. }
  175. }