123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121 |
- #include "ivdecision.h"
- #include "gnss_coordinate_convert.h"
- namespace iv {
- ivdecision::ivdecision()
- {
- }
- void ivdecision::modulerun()
- {
- while(mbrun)
- {
- //RunDecision
- //ShareMsg
- }
- }
- void ivdecision::UpdateGPSMsg(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
- {
- (void )&index;
- (void )&dt;
- (void )strmemname;
- if(nSize<1)return;
- iv::gps::gpsimu xgpsimu;
- if(!xgpsimu.ParseFromArray(strdata,nSize))
- {
- std::cout<<"ivdecision::UpdateGPSMsg error."<<std::endl;
- return;
- }
- iv::GPSData data(new iv::GPS_INS);
- data->ins_status = xgpsimu.ins_state();
- data->rtk_status = xgpsimu.rtk_state();
- data->accel_x = xgpsimu.acce_x();
- data->accel_y = xgpsimu.acce_y();
- data->accel_z = xgpsimu.acce_z();
- data->ang_rate_x = xgpsimu.gyro_x();
- data->ang_rate_y = xgpsimu.gyro_y();
- data->ang_rate_z = xgpsimu.gyro_z();
- data->gps_lat = xgpsimu.lat();
- data->gps_lng = xgpsimu.lon();
- data->gps_z = xgpsimu.height();
- data->ins_heading_angle = xgpsimu.heading();
- data->ins_pitch_angle = xgpsimu.pitch();
- data->ins_roll_angle = xgpsimu.roll();
- data->speed = sqrt(pow(xgpsimu.ve(),2)+pow(xgpsimu.vn(),2)) * 3.6;
- GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
- mMutexGPS.lock();
- iv::gps::gpsimu * pgpsimu = new iv::gps::gpsimu;
- pgpsimu->CopyFrom(xgpsimu);
- mGPSIMUptr.reset(pgpsimu);
- mNowGPS = data;
- mnGPSUpdateTime = QDateTime::currentMSecsSinceEpoch();
- mMutexGPS.unlock();
- }
- bool ivdecision::GetGPS(GPSData &xGPSData)
- {
- if((QDateTime::currentMSecsSinceEpoch() - mnGPSUpdateTime)>mnNewThresh)
- {
- xGPSData = NULL;
- return false;
- }
- xGPSData = mNowGPS;
- return true;
- }
- void ivdecision::UpdateRADARMsg(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
- {
- (void )&index;
- (void )&dt;
- (void )strmemname;
- if(nSize<1)return;
- iv::radar::radarobjectarray * pradar = new iv::radar::radarobjectarray;
- if(!pradar->ParseFromArray(strdata,nSize))
- {
- std::cout<<"ivdecision::UpdateRADARMsg Parse Error"<<std::endl;
- return;
- }
- mMutexRADAR.lock();
- mRADAR.reset(pradar);
- mnRADARUpdateTime = QDateTime::currentMSecsSinceEpoch();
- mMutexRADAR.unlock();
- }
- bool ivdecision::GetRADAR(std::shared_ptr<radar::radarobjectarray> &xRADAR)
- {
- if((QDateTime::currentMSecsSinceEpoch() - mnRADARUpdateTime)>mnNewThresh)
- {
- xRADAR = NULL;
- return false;
- }
- xRADAR = mRADAR;
- return true;
- }
- }
|