ivdecision.cpp 2.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121
  1. #include "ivdecision.h"
  2. #include "gnss_coordinate_convert.h"
  3. namespace iv {
  4. ivdecision::ivdecision()
  5. {
  6. }
  7. void ivdecision::modulerun()
  8. {
  9. while(mbrun)
  10. {
  11. //RunDecision
  12. //ShareMsg
  13. }
  14. }
  15. void ivdecision::UpdateGPSMsg(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
  16. {
  17. (void )&index;
  18. (void )&dt;
  19. (void )strmemname;
  20. if(nSize<1)return;
  21. iv::gps::gpsimu xgpsimu;
  22. if(!xgpsimu.ParseFromArray(strdata,nSize))
  23. {
  24. std::cout<<"ivdecision::UpdateGPSMsg error."<<std::endl;
  25. return;
  26. }
  27. iv::GPSData data(new iv::GPS_INS);
  28. data->ins_status = xgpsimu.ins_state();
  29. data->rtk_status = xgpsimu.rtk_state();
  30. data->accel_x = xgpsimu.acce_x();
  31. data->accel_y = xgpsimu.acce_y();
  32. data->accel_z = xgpsimu.acce_z();
  33. data->ang_rate_x = xgpsimu.gyro_x();
  34. data->ang_rate_y = xgpsimu.gyro_y();
  35. data->ang_rate_z = xgpsimu.gyro_z();
  36. data->gps_lat = xgpsimu.lat();
  37. data->gps_lng = xgpsimu.lon();
  38. data->gps_z = xgpsimu.height();
  39. data->ins_heading_angle = xgpsimu.heading();
  40. data->ins_pitch_angle = xgpsimu.pitch();
  41. data->ins_roll_angle = xgpsimu.roll();
  42. data->speed = sqrt(pow(xgpsimu.ve(),2)+pow(xgpsimu.vn(),2)) * 3.6;
  43. GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
  44. mMutexGPS.lock();
  45. iv::gps::gpsimu * pgpsimu = new iv::gps::gpsimu;
  46. pgpsimu->CopyFrom(xgpsimu);
  47. mGPSIMUptr.reset(pgpsimu);
  48. mNowGPS = data;
  49. mnGPSUpdateTime = QDateTime::currentMSecsSinceEpoch();
  50. mMutexGPS.unlock();
  51. }
  52. bool ivdecision::GetGPS(GPSData &xGPSData)
  53. {
  54. if((QDateTime::currentMSecsSinceEpoch() - mnGPSUpdateTime)>mnNewThresh)
  55. {
  56. xGPSData = NULL;
  57. return false;
  58. }
  59. xGPSData = mNowGPS;
  60. return true;
  61. }
  62. void ivdecision::UpdateRADARMsg(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
  63. {
  64. (void )&index;
  65. (void )&dt;
  66. (void )strmemname;
  67. if(nSize<1)return;
  68. iv::radar::radarobjectarray * pradar = new iv::radar::radarobjectarray;
  69. if(!pradar->ParseFromArray(strdata,nSize))
  70. {
  71. std::cout<<"ivdecision::UpdateRADARMsg Parse Error"<<std::endl;
  72. return;
  73. }
  74. mMutexRADAR.lock();
  75. mRADAR.reset(pradar);
  76. mnRADARUpdateTime = QDateTime::currentMSecsSinceEpoch();
  77. mMutexRADAR.unlock();
  78. }
  79. bool ivdecision::GetRADAR(std::shared_ptr<radar::radarobjectarray> &xRADAR)
  80. {
  81. if((QDateTime::currentMSecsSinceEpoch() - mnRADARUpdateTime)>mnNewThresh)
  82. {
  83. xRADAR = NULL;
  84. return false;
  85. }
  86. xRADAR = mRADAR;
  87. return true;
  88. }
  89. }