Exemplo n.º 1
0
int main(int argc, char *argv[]) {
    if (argc < 2) {
        return 1;
    }
    
    gpsmm gps("localhost", DEFAULT_GPSD_PORT);
    if (gps.stream(WATCH_ENABLE|WATCH_JSON) == NULL) {
        printf("GPSD error!\n");
        return 1;
    }
    
    FILE *fpo = fopen(argv[1], "w");
    while (1) {
        if (gps.waiting(50000)) {
            struct gps_data_t* data;
            if ((data = gps.read()) == NULL) {
                printf("Failed to read GPS data\n");
            } else if (data->set & LATLON_SET) {
                printf("%.10f, %.10f; %f\n", data->fix.latitude, data->fix.longitude, data->fix.time);
                fprintf(fpo, "%.10f, %.10f; %f\n", data->fix.latitude, data->fix.longitude, data->fix.time);
            }
        }
        fflush(fpo);
    }
    
}
Exemplo n.º 2
0
bool RobotBrainServiceService::start(int argc, char* argv[])
#endif	
{
  MYLOGVERB = LOG_INFO;

  char adapterStr[255];

  //Create the topics
//  SimEventsUtils::createTopic(communicator(), "BeoGPSMessageTopic");

  //Create the adapter
  int port = RobotBrainObjects::RobotBrainPort;
  bool connected = false;

  // try to connect to ports until successful
  LDEBUG("Opening Connection");
  while(!connected)
  {
    try
    {
      LINFO("Trying Port:%d", port);
      sprintf(adapterStr, "default -p %i", port);
      itsAdapter = communicator()->createObjectAdapterWithEndpoints
        ("BeoGPS", adapterStr);
      connected = true;
    }
    catch(Ice::SocketException)
    {
      port++;
    }
  }

  //Create the manager and its objects
  itsMgr = new ModelManager("BeoGPSService");

  LINFO("Starting BeoGPS System");
  nub::ref<BeoGPS> gps(new BeoGPS(*itsMgr, "BeoGPS", "BeoGPS"));
  LINFO("BeoGPS created");
  itsMgr->addSubComponent(gps);
  LINFO("BeoGPS Added As a subcomponent");
  gps->init(communicator(), itsAdapter);
  LINFO("BeoGPS initiated");

  // check command line inputs/options
  if (itsMgr->parseCommandLine
      (argc, argv, "<serdev>", 0, 0) == false)
    return(1);

  // let's configure our serial device:
//   gps->configureSerial(itsMgr->getExtraArg(0));
//   LINFO("Using: %s", itsMgr->getExtraArg(0).c_str());

  // activate manager and adapter
  itsAdapter->activate();
  itsMgr->start();

  return true;
}
Exemplo n.º 3
0
void pullOutGPS(BSONObjBuilder& b, map<string,double>& readings,string gpsField){
    if(readings.count(gpsField) > 0){
        size_t pos = gpsField.find("lon") != string::npos ? gpsField.find("lon") : gpsField.find("lat");
        string fieldBase = gpsField.substr(0,pos);

        BSONArrayBuilder gps(2);
        gps.append(readings[fieldBase+"lon-lon"]);
        gps.append(readings[fieldBase+"lat-lat"]);
        b.append(fieldBase+"lonlat",gps.done());

        readings.erase(fieldBase+"lon-lon");
        readings.erase(fieldBase+"lat-lat");
    }
}
Exemplo n.º 4
0
/***********************************************************************
 * Main
 **********************************************************************/
int UHD_SAFE_MAIN(int argc, char *argv[]){
    uhd::set_thread_priority_safe();

    //variables to be set by po
    std::string addr;
    std::string bind;

    //setup the program options
    po::options_description desc("Allowed options");
    desc.add_options()
        ("help", "help message")
        ("addr", po::value<std::string>(&addr), "the resolvable address of the usrp (must be specified)")
        ("bind", po::value<std::string>(&bind)->default_value("0.0.0.0"), "bind the server to this network address (default: any)")
    ;
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    //print the help message
    if (vm.count("help") or not vm.count("addr")){
        std::cout
            << boost::format("UHD Network Relay %s") % desc << std::endl
            << "Runs a network relay between UHD on one computer and a USRP on the network.\n"
            << "This example is basically for test purposes. Use at your own convenience.\n"
            << std::endl;
        return EXIT_FAILURE;
    }

    {
        boost::shared_ptr<udp_relay_type> ctrl  (new udp_relay_type(bind, addr, "49152"));
        boost::shared_ptr<udp_relay_type> rxdsp0(new udp_relay_type(bind, addr, "49156", 0, tx_dsp_buff_size, rx_dsp_buff_size, 0));
        boost::shared_ptr<udp_relay_type> txdsp0(new udp_relay_type(bind, addr, "49157", tx_dsp_buff_size, 0, 0, tx_dsp_buff_size));
        boost::shared_ptr<udp_relay_type> rxdsp1(new udp_relay_type(bind, addr, "49158", 0, tx_dsp_buff_size, rx_dsp_buff_size, 0));
        boost::shared_ptr<udp_relay_type> gps   (new udp_relay_type(bind, addr, "49172"));

        std::signal(SIGINT, &sig_int_handler);
        std::cout << "Press Ctrl + C to stop streaming..." << std::endl;

        while (not stop_signal_called){
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }

    //finished
    std::cout << std::endl << "Done!" << std::endl << std::endl;

    return EXIT_SUCCESS;
}
Exemplo n.º 5
0
void SBGInsIg500N::init(){
	
	SBGSetNavVelocitySrc vel(true,SBGSetNavVelocitySrc::USER);
	SBGSetLowPowerMode gps(true,SBGSetLowPowerMode::SBG_GPS_OFF_MODE);
	SBGSetDefaultOutputMask output(true, EULER );
	SBGSetContinuousMode mode;
	
	if ( RS232_SendBuf(port, vel.to_bytes(), vel.getMessageLength()) != vel.getMessageLength() )
		ROS_INFO("Problem while sending Velocity source");
	getNextAck();
	if ( RS232_SendBuf(port, gps.to_bytes(), gps.getMessageLength()) != gps.getMessageLength() )
		ROS_INFO("Problem while sending GPS power mode");
	getNextAck();
	if ( RS232_SendBuf(port, output.to_bytes(), output.getMessageLength()) != output.getMessageLength() )
		ROS_INFO("Problem while sending default output mask");
	getNextAck();
	if ( RS232_SendBuf(port, mode.to_bytes(), mode.getMessageLength()) != mode.getMessageLength() )
		ROS_INFO("Problem while sending the continuous mode data");
	getNextAck();
}
Exemplo n.º 6
0
void ut_metadata::testClearGps()
{
    QTemporaryFile file;
    file.open();
    sourceImage.save(file.fileName(), "jpg");

    QuillMetadata gps("/usr/share/libquillmetadata-tests/images/gps.jpg");
    gps.removeEntries(QuillMetadata::TagGroup_GPS);
    QVERIFY(gps.write(file.fileName()));

    QCOMPARE(gps.entry(QuillMetadata::Tag_GPSLatitude).toString(), QString());
    QCOMPARE(gps.entry(QuillMetadata::Tag_GPSLongitude).toString(), QString());
    QCOMPARE(gps.entry(QuillMetadata::Tag_GPSAltitude).toString(), QString());

    QuillMetadata writtenMetadata(file.fileName());
    QVERIFY(writtenMetadata.isValid());

    QCOMPARE(writtenMetadata.entry(QuillMetadata::Tag_GPSLatitude).toString(), QString(""));
    QCOMPARE(writtenMetadata.entry(QuillMetadata::Tag_GPSLongitude).toString(), QString(""));
    QCOMPARE(writtenMetadata.entry(QuillMetadata::Tag_GPSAltitude).toString(), QString(""));
}
Exemplo n.º 7
0
int CGps::SetCurGps( const void* v_pGps, BYTE v_bytSetType )
{
	int iRet = 0;
	tagGpsShare objGpsShare;
	tagGPSData gps(0);
	memcpy( &gps, v_pGps, sizeof(gps) );

	_SemP();

	memcpy( &objGpsShare, m_pShareMem, sizeof(objGpsShare) );
	
	if( GPS_REAL & v_bytSetType )
	{
		objGpsShare.m_objRealGps = gps;
		objGpsShare.m_dwSetGpsTm = GetTickCount();

		if( !(GPS_LSTVALID & v_bytSetType) )
		{
			if( 'A' == gps.valid && gps.speed <= BELIV_MAXSPEED ) // (要满足速度不超过最大可信速度)
			{
				objGpsShare.m_objValidGps = gps;
			}
		}
	}

	if( (GPS_LSTVALID & v_bytSetType) && gps.speed <= BELIV_MAXSPEED ) // (要满足速度不超过最大可信速度)
	{
		objGpsShare.m_objValidGps = gps;
	}

	memcpy( m_pShareMem, &objGpsShare, sizeof(objGpsShare) );

	_SemV();

	return iRet;
}
Exemplo n.º 8
0
int32_t main(int32_t a_argc, char **a_argv)
{
  opendlv::proxy::sensor::gps::Gps gps(a_argc, a_argv);
  return gps.runModule();
}
Exemplo n.º 9
0
unsigned int CGpsSrc::P_GpsReadThread()
{
 	int iRet = 0;
 	GPSDATA gps(0);
 	
 	/// 循环读取GPS共享内存数据
 	char buf[GPS_BUFSIZE];
// 	struct timeval tagCurrTimeVal;//当前系统时间,gettimeofday
// 	struct timezone tagCurrTimeZone;
	unsigned int uiGpsTickChk;

 	char* szGps;
	char szSta[2];

	DWORD dwContinueCount = 0; // 移动/静止 持续次数
	BYTE bytRealMoveType = 0; // 实时状态, 移动 = 1, 静止 = 2.
	BYTE bytSndMoveType = 0; // 发送状态

	static bool sta_bDoneSetTime = false; // 是否已校时
	DWORD dwABegin; // GPS连续有效起始时刻
	DWORD dwVBegin; // GPS连续无效起始时刻
	
	DWORD dwCt = 0;
	unsigned char uszResult;
	static unsigned int uiStaWaitKillTime = GetTickCount();
	static unsigned int uiStaAccValidTime = GetTickCount();

	dwABegin = dwVBegin = GetTickCount();

	szSta[0] = BYTE( 0xf0 );

	char* pgpGga =NULL;	//GGA数据起始地址
	char* pgpRmc =NULL;	//RMC数据起始地址
	char* pgpGsa =NULL;	//GSA数据起始地址
	char* pgpVtg =NULL;	//VTG数据起始地址
	char* pgpGsvBeg =NULL;//GSV数据起始地址
	char* pTemp;
	bool bGpsInvalid = false;
	bool bGpsUnComplete = false;

	while(!g_bProgExit)
 	{
		usleep(500000);

		if (g_bProgExit)
		{
			goto _LOOP_READ_END;
		}

// 		// 升级root前,先把GPS进程关闭
// 		if( m_bUpdateQuit )
// 		{
// 			_ReleaseGpsSrc();
// 			return 0;
// 		}

		if( m_bNeedGpsOn )
		{
			m_bNeedGpsOn = false;
			if( stSleep == m_iGpsSta )
			{
				uiStaAccValidTime = GetTickCount();
				m_iGpsSta = stInit;
			}
		}

		switch(m_iGpsSta)
		{
		case stInit:
			{	
				PRTMSG(MSG_NOR,"GPS STA: stInit\n");

				// 通知QianExe,GPS退出省电状态
				char szSta = 0xf8;	// 0xf1表示退出省电
				DataPush(&szSta, 1, DEV_GPS, DEV_QIAN, LV1);

				if(false == m_bFinishKill)
				{
					if ( (GetTickCount()-uiStaWaitKillTime) > 60*1000 )
					{
						goto _LOOP_READ_END;
					}
					usleep(100000);
					continue;
				}
				
				if( !_InitGpsSrc() ) 
				{
					//g_bProgExit = true;
					G_ResetSystem();
				}

				if(true == m_bGpsInit)
				{
					m_pShm = (gps_data_share *)_GetShm(m_iShmid);//获取GPS共享内存数据
					
					if( NULL == m_pShm)
					{
						PRTMSG(MSG_ERR,"获取GPS共享内存数据失败\n");
#if VEHICLE_TYPE == VEHICLE_V8 || VEHICLE_TYPE == VEHICLE_M2
						g_objDiaodu.show_diaodu("GPS模块异常");
						RenewMemInfo(0x02, 0x00, 0x00, 0x00);
#endif
						m_iGpsSta = stReset;
						break;						
					}
					sleep(1);
					m_iGpsSta = stGetData;
					PRTMSG(MSG_NOR,"stInit succ,intering stGetData!\n");
				}
				else
				{
					m_iGpsSta = stReset;
				}
			}
			break;
		case stReset:
			{
				PRTMSG(MSG_NOR,"GPS STA: stReset\n");
				
				//reset GPS 前先置uiStaWaitKillTime
				uiStaWaitKillTime = GetTickCount();

				if ( false ==_ReleaseGpsSrc() )
				{
					break;
				}

				//获取共享内存que

				m_iGpsSta = stWaitKill;
			}
			break;
			
		case stSleep:
			{	
				//PRTMSG(MSG_NOR,"GPS STA: stSleep\n");
				IOGet((byte)IOS_ACC, &uszResult);
				if( IO_ACC_OFF == uszResult || GpsSwitch)
				{
					sleep(1);
				}
				else
				{
					uiStaAccValidTime = GetTickCount();
					m_bGpsSleep = false;
					m_iGpsSta = stInit;
				}
			}
			break;

		case stWaitKill:
			{
				PRTMSG(MSG_NOR,"GPS STA: stWaitKill\n");
				
				//reset GPS 前先置uiStaWaitKillTime
				if ( (GetTickCount()-uiStaWaitKillTime) > 20*1000 )
				{
					goto _LOOP_READ_END;
				}
				usleep(200000);
				pid_t pid = waitpid(-1,NULL,WNOHANG);
				if (-1 == pid )
				{
					perror("waitpid: ");
				}
				else if ( pid == m_ChildPid )
				{
					m_bFinishKill = true;
					
					if ( true == m_bGpsSleep )
					{
						PRTMSG(MSG_NOR,"Intering stSleep !\n");
						m_iGpsSta = stSleep;

						// 通知QianExe,GPS进入省电状态
						char szSta = 0xf7;	// 0xf0表示进入省电
						DataPush(&szSta, 1, DEV_GPS, DEV_QIAN, LV1);
					}
					else
					{
						sleep(30);//gps的复位操作,复位后等待一段时间再重新初始化
						m_iGpsSta = stInit;
					}
				}
				PRTMSG(MSG_DBG, "waitpid:%d\n",pid);
			}
			break;

		case stGetData:
			{
				iRet = _GetGpsOrigData(&m_objGpsDataShm);
				if (true != iRet)
				{
					m_iGpsSta = stReset;
					pgpGga = pgpRmc = pgpGsa = pgpVtg = pgpGsvBeg =NULL;
					PRTMSG(MSG_ERR, "获取GPS原始数据失败:无法获取共享内存数据!\n");

#if VEHICLE_TYPE == VEHICLE_V8 || VEHICLE_TYPE == VEHICLE_M2
					g_objDiaodu.show_diaodu("GPS模块异常");
					RenewMemInfo(0x02, 0x00, 0x00, 0x00);
#endif
					break;
				}

				pgpGga = (m_objGpsDataShm.combine_data[0]).gga_data;
				pgpRmc = (m_objGpsDataShm.combine_data[0]).rmc_data;
				pgpGsa = (m_objGpsDataShm.combine_data[0]).gsa_data;
				pgpVtg = (m_objGpsDataShm.combine_data[0]).vtg_data;
				pTemp = pgpVtg + DATA_LENGTH;
				pgpGsvBeg = strstr(pTemp, "$GPGSV");

				if(0 == pgpGsvBeg)
				{
					m_iGpsSta = stReset;
					pgpGga = pgpRmc = pgpGsa = pgpVtg = pgpGsvBeg =NULL;
					break;
				}

#if PRINT_GPS_DATA == 1
				usleep(500000);
				printf("\n");
				printf("%s", (m_objGpsDataShm.combine_data[0]).gga_data);
				printf("%s", (m_objGpsDataShm.combine_data[0]).rmc_data);
				printf("%s", (m_objGpsDataShm.combine_data[0]).gsa_data);
				printf("%s", (m_objGpsDataShm.combine_data[0]).vtg_data);
				printf("%s", (m_objGpsDataShm.combine_data[0]).gsv_data);
				printf("\n");
#endif

#if VEHICLE_TYPE == VEHICLE_M
				//使用组件GetTickCount进行时间戳检查
				uiGpsTickChk = GetTickCount();
				if( (uiGpsTickChk - m_objGpsDataShm.n1_time) > 30*1000 )//30s
				{
					PRTMSG(MSG_ERR,"GPS data invalod too long ,reset the gps\n");
					m_objGpsDataShm.n1_time = uiGpsTickChk; // 避免重复复位
					m_iGpsSta = stReset;
					break;
				}
#endif
 				/// 解析前初始化
 				gps.Init();
				bGpsInvalid = false;
				bGpsUnComplete = false;
 				
 				/// 解析RMC格式数据
 				szGps = strstr(pgpRmc,"$GPRMC,");
 				if( szGps )
 				{
 					if( !_DecodeRMCData( szGps, gps ) )
					{
						bGpsUnComplete = true;
						//continue;
					}
 				}
 				else 
				{
					bGpsUnComplete = true;
					//continue;
				}

				if( gps.valid != 'A' ) bGpsInvalid = true;

				// GPS长时间无效检查
				static unsigned int sta_uiGpsBeginInvalid = uiGpsTickChk;
				{
					if( bGpsInvalid || bGpsUnComplete )
					{
						if( uiGpsTickChk - sta_uiGpsBeginInvalid >180000 )
						{
							PRTMSG(MSG_ERR,"GPS data invalod too long ,reset the gps\n");
							sta_uiGpsBeginInvalid = uiGpsTickChk; // 避免重复复位
							m_iGpsSta = stReset;
							break;
						}
						else if( bGpsUnComplete )
						{
							continue;
						}
					}
					else
					{
						sta_uiGpsBeginInvalid = uiGpsTickChk;
					}
				}
		#ifdef GPSGSA_ENB
				/// 解析GSA格式数据
				{
					static DWORD sta_dwNoGsaCt = 0;
					
					szGps = strstr( pgpGsa, "$GPGSA," );
					if( szGps )
					{
						sta_dwNoGsaCt = 0;
						_DecodeGSAData(szGps, gps);
					}
					else continue; 
				}
		#endif

				// 解析GSV格式数据
 				szGps = strstr( pgpGsvBeg, "$GPGSV," );
 				if( szGps )
 				{
					tagSatelliteInfo objSatsInfo;
					if( _DecodeGSVData(szGps, objSatsInfo) ) // 成功解析后
					{
						pthread_mutex_lock( &m_hMutexGsv );
						m_objSatsInfo = objSatsInfo;
						m_dwGsvGetTm = GetTickCount();
						pthread_mutex_unlock( &m_hMutexGsv );
					}
					else
						PRTMSG(MSG_ERR,"Decode GSV Fail !\n");
				}

				// 定位标志若不一致则纠正,以RMC格式的数据为准
				if( 'A' != gps.valid ) gps.valid = 'V';
				if( 'A' == gps.valid && ('2' != gps.m_cFixType && '3' != gps.m_cFixType) )
					gps.m_cFixType = '2';
				else if( 'A' != gps.valid && ('2' == gps.m_cFixType || '3' == gps.m_cFixType) )
					gps.m_cFixType = '1';
 
				
				//通过GPS时间校时设置系统时间
				// 设置时间
				if( !sta_bDoneSetTime )
				{
#if CHK_VER == 1 || GPSVALID_FASTFIXTIME == 1
					if( 'A' == gps.valid )
					{
						// 校时
						if( true == _SetGpsTime( gps.nYear, gps.nMonth, gps.nDay, gps.nHour, gps.nMinute, gps.nSecond ) )
						{
							PRTMSG(MSG_DBG, "Set SysTime: %04d%02d%02d %02d:%02d:%02d\n", gps.nYear, gps.nMonth, gps.nDay, gps.nHour, gps.nMinute, gps.nSecond );
							// 通知QianExe
							BYTE byt = 0xf1;
							DataPush((char*)&byt, 1, DEV_GPS, DEV_QIAN, 2);
							
							sta_bDoneSetTime = true;
						}
					}
#else
					
					DWORD dwCur = GetTickCount();
					static DWORD sta_dwLstChk = dwCur;
					
					if( dwCur - sta_dwLstChk >= 5000 ) // 若2次GPS接收时刻超过5秒
					{
						dwABegin = dwCur; // GPS连续有效的计时重新开始计算
					}
					
					if( 'A' == gps.valid )
					{
						if( dwCur - dwABegin >= TIMEBELIV_GPSAPERIOD ) // 连续有效超过一定时间,则认为GPS时间可信,可以设置为系统时间
						{
							// 校时
							if( true == _SetGpsTime( gps.nYear, gps.nMonth, gps.nDay, gps.nHour, gps.nMinute, gps.nSecond ) )
							{
								PRTMSG(MSG_DBG, "Set SysTime: %04d%02d%02d %02d:%02d:%02d\n", gps.nYear, gps.nMonth, gps.nDay, gps.nHour, gps.nMinute, gps.nSecond );
								
								// 通知QianExe
								BYTE byt = 0xf1;
								DataPush((char*)&byt, 1, DEV_GPS, DEV_QIAN, 2);
								
								sta_bDoneSetTime = true;
							}
						}
						else
						{
							dwVBegin = dwCur; // 确保以后的GPS无效时长都可以重新计时
						}
					}
					else
					{
						if( dwCur - dwVBegin >= TIMEUNBLV_GPSVMINPERIOD ) // 若GPS连续无效超过一定时间
						{
							dwABegin = dwCur; // GPS连续有效的计时重新开始计算
						}
					}
					
					sta_dwLstChk = dwCur; // 更新上次检查时间
#endif
				}

 				/// { 格林威治->北京时区
				// 无效数据也要转换,因为可能时间是正确的 if( 'A' == gps.valid ) // 若数据有效
 				{
 					bool bCarry = false; // 时间计算时的进位标志
 
 					gps.nHour += 8;
 					if( gps.nHour >= 24 )
 					{
 						gps.nHour -= 24;
 						bCarry = true;
 					}
 
 					gps.nDay += (bCarry ? 1 : 0);
 					int nMaxDay = 31;
 					if( 2 == gps.nMonth )
 					{
 						if( 0 == gps.nYear % 400 
 							|| (0 == gps.nYear % 4 && 0 != gps.nYear % 100) ) // 闰年
 						{
 							nMaxDay = 29;
 						}
 						else nMaxDay = 28;
 					}
 					else if( gps.nMonth <= 7 )
 					{
 						if( 0 == gps.nMonth % 2 ) nMaxDay = 30;
 					}
 					else
 					{
 						if( 1 == gps.nMonth % 2 ) nMaxDay = 30;
 					}
 					if( gps.nDay > nMaxDay )
 					{
 						gps.nDay -= nMaxDay;
 						bCarry = true;
 					}
 					else
 					{
 						bCarry = false;
 					}
 
 					gps.nMonth += (bCarry ? 1 : 0);
 					if( gps.nMonth > 12 )
 					{
 						gps.nMonth -= 12;
 						bCarry = true;
 					}
 					else
 					{
 						bCarry = false;
 					}
 
 					gps.nYear += (bCarry ? 1 : 0);
 				}
 				/// } 格林威治->北京时区

		// 		if( sta_bFstValid )
		// 		{
		// 			if( 'A' == gps.valid ) // || (gps.nYear > 2007) || (2007 == gps.nYear && gps.nMonth >= 5) )
		// 			{
		// 				_SetGpsTime( BYTE(gps.nYear - 2000), BYTE(gps.nMonth), BYTE(gps.nDay), BYTE(gps.nHour),
		// 					BYTE(gps.nMinute), BYTE(gps.nSecond) );
		// 				sta_bFstValid = false;
		// 			}
		// 		}

				// 若速度不可信,则设为无效
				if( gps.speed > BELIV_MAXSPEED )
				{
					gps.valid = 'V';
					gps.m_cFixType = '1';
				}
				
				if( 'A' == gps.valid ) // 若GPS数据有效
				{
					if( gps.speed > SPEED_MOVE ) // 若现在是移动的
					{
						if( 1 != bytRealMoveType ) // 若原先不是移动的
						{				
							bytRealMoveType = 1; // 修改为移动标志
							dwContinueCount = 1; // 重置状态持续次数
						}
						else
						{
							++ dwContinueCount; // 递增该状态持续次数
						}

						// 若持续次数达到设定,且上次发送状态不是移动,发送车辆变为移动的通知
						if( SPEED_COUNT == dwContinueCount && 1 != bytSndMoveType )
						{
							szSta[1] = 1;
							PRTMSG(MSG_DBG, "Send car move msg!\n" );
							DataPush(szSta, 2, DEV_GPS, DEV_QIAN, 2);
							bytSndMoveType = 1;
						}
					}
					else if( gps.speed < SPEED_STOP ) // 若现在是静止的
					{
						if( 2 != bytRealMoveType ) // 若原先不是静止的
						{				
							bytRealMoveType = 2; // 修改为静止标志
							dwContinueCount = 1; // 重置状态持续次数
						}
						else
						{
							++ dwContinueCount; // 递增该状态持续次数
						}

						// 若持续次数达到设定,且上次发送状态不是静止,发送车辆变为静止的通知
						if( SPEED_COUNT == dwContinueCount && 2 != bytSndMoveType )
						{
							szSta[1] = 2;
							PRTMSG(MSG_DBG, "Send car silent msg!\n" );
							DataPush(szSta, 2, DEV_GPS, DEV_QIAN, 2);

							bytSndMoveType = 2;
						}
					}
				}
				else
				{
					// dwContinueCount = 0; // GPS数据无效,重置状态持续计数
					// bytRealMoveType = 0; // 清除移动/静止标志
				}

				// ACC无效时,强制让速度等于0
				IOGet((byte)IOS_ACC, &uszResult);
				if( IO_ACC_OFF == uszResult ) 
				{
					gps.speed = 0;
				}
				else
				{
					uiStaAccValidTime = GetTickCount();
				}
				
				//acc无效达20分钟,关闭SiRfNav进程,gps进入省电
				if ( (GetTickCount()-uiStaAccValidTime) > 20*60*1000 || GpsSwitch)
				{
					PRTMSG(MSG_ERR,"acc invalid too long or close Gps, gps sleep, GpsSwitch=%d\n", GpsSwitch);
					m_bGpsSleep = true;
					m_iGpsSta = stReset;
				}

				gps.m_bytMoveType = bytSndMoveType; // 与发送状态同步
				if( gps.speed <= BELIV_MAXSPEED ) // 速度可信的才保留
				{
					SetCurGps( &gps, GPS_REAL );
				}

				if( 0 == dwCt ++ % 3 )
				{
// 					PRTMSG(MSG_DBG,"Gps:%c %u:%u:%u", char(gps.valid), gps.nHour, gps.nMinute, gps.nSecond);
// 					PRTMSG(MSG_DBG, "Lon=%0.6f", gps.longitude );
// 					PRTMSG(MSG_DBG, "Lat=%0.6f", gps.latitude );
				}
 			}
			break;

		default:
			{
				PRTMSG(MSG_ERR,"GPS sta Unknown, Reset gps\n");
				m_iGpsSta = stReset;
			}
		}
	}

_LOOP_READ_END:
	g_bProgExit = true;
	return 0;
}
Exemplo n.º 10
0
unsigned int CGpsSrc::P_GpsReadThread()
{
 	int iRet = 0;
 	GPSDATA gps(0);
 	
 	/// 循环读取GPS共享内存数据
 	char buf[GPS_BUFSIZE];
// 	struct timeval tagCurrTimeVal;//当前系统时间,gettimeofday
// 	struct timezone tagCurrTimeZone;
	unsigned int uiGpsTickChk;

 	char* szGps;
	char szSta[2];

	DWORD dwContinueCount = 0; // 移动/静止 持续次数
	BYTE bytRealMoveType = 0; // 实时状态, 移动 = 1, 静止 = 2.
	BYTE bytSndMoveType = 0; // 发送状态

	static bool sta_bDoneSetTime = false; // 是否已校时
	DWORD dwABegin; // GPS连续有效起始时刻
	DWORD dwVBegin; // GPS连续无效起始时刻
	
	DWORD dwCt = 0;
	unsigned char uszResult;
	static unsigned int uiStaWaitKillTime = GetTickCount();

	dwABegin = dwVBegin = GetTickCount();

	szSta[0] = BYTE( 0xf0 );

// 	iRet = pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
// 	if( 0 != iRet )
// 		PRTMSG(MSG_ERR,"ComRcv设置允许线程接收取消请求失败: %x\n",iRet);
// 	
// 	iRet = pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
// 	if( 0 != iRet )
// 		PRTMSG(MSG_ERR,"ComRcv线程接收到取消请求后退出失败: %x\n", iRet);

	char* pgpGga =NULL;	//GGA数据起始地址
	char* pgpRmc =NULL;	//RMC数据起始地址
	char* pgpGsa =NULL;	//GSA数据起始地址
	char* pgpVtg =NULL;	//VTG数据起始地址
	char* pgpGsvBeg =NULL;//GSV数据起始地址
	char* pTemp;

	_mGpsComOpen();

	while(!g_bProgExit)
 	{
		sleep(1);
		
		if (g_bProgExit)
		{
			goto _LOOP_READ_END;
		}
		
		int iLen = read(m_imGpsComPort,buf,sizeof(buf));

		//PRTMSG(MSG_DBG, "mcom Recv %s\n", buf);

		buf[iLen] = 0;
		pgpGga = strstr(buf,"$GPGGA,");
		pgpRmc = strstr(buf,"$GPRMC,");
		pgpGsa = strstr(buf,"$GPGSA,");
		pgpGsvBeg = strstr(buf,"$GPGSV");

		if(NULL == pgpGga || NULL == pgpRmc || NULL == pgpGsa || NULL == pgpGsvBeg )
		{
//			PRTMSG(MSG_ERR, "pgpGga is null!\n");
			pgpGga = pgpRmc = pgpGsa = pgpVtg = pgpGsvBeg =NULL;
			continue;
		}

// 	//testc
// 		sleep(1);
// 		PRTMSG(MSG_DBG,pgpGga);
// 		PRTMSG(MSG_DBG,"gps.valid:%c, gps.speed:%d\n",gps.valid,(int)(gps.speed));
// 
	//testc

 		/// 解析前初始化
 		gps.Init();
 		
 		/// 解析RMC格式数据
 		szGps = strstr(pgpRmc,"$GPRMC,");
 		if( szGps )
 		{
 			if( !_DecodeRMCData( szGps, gps ) ) continue;
 		}
 		else 
		{
			continue;
		}
#ifdef GPSGSA_ENB
		/// 解析GSA格式数据
		{
			static DWORD sta_dwNoGsaCt = 0;
			
			szGps = strstr( pgpGsa, "$GPGSA," );
			if( szGps )
			{
				sta_dwNoGsaCt = 0;
				_DecodeGSAData(szGps, gps);
			}
			else continue; 
		}
#endif

		// 解析GSV格式数据
 		szGps = strstr( pgpGsvBeg, "$GPGSV," );
 		if( szGps )
 		{
			tagSatelliteInfo objSatsInfo;
			if( _DecodeGSVData(szGps, objSatsInfo) ) // 成功解析后
			{
				pthread_mutex_lock( &m_hMutexGsv );
				m_objSatsInfo = objSatsInfo;
				m_dwGsvGetTm = GetTickCount();
				pthread_mutex_unlock( &m_hMutexGsv );
			}
			else
				PRTMSG(MSG_ERR,"Decode GSV Fail !\n");
		}

		// 定位标志若不一致则纠正,以RMC格式的数据为准
		if( 'A' != gps.valid ) gps.valid = 'V';
		if( 'A' == gps.valid && ('2' != gps.m_cFixType && '3' != gps.m_cFixType) )
			gps.m_cFixType = '2';
		else if( 'A' != gps.valid && ('2' == gps.m_cFixType || '3' == gps.m_cFixType) )
			gps.m_cFixType = '1';

		
		//通过GPS时间校时设置系统时间
		// 设置时间
		if( !sta_bDoneSetTime )
		{
#if CHK_VER == 1 || GPSVALID_FASTFIXTIME == 1
			if( 'A' == gps.valid )
			{
				// 校时
				if( true == _SetGpsTime( gps.nYear, gps.nMonth, gps.nDay, gps.nHour, gps.nMinute, gps.nSecond ) )
				{
					PRTMSG(MSG_DBG, "Set SysTime: %04d%02d%02d %02d:%02d:%02d\n", gps.nYear, gps.nMonth, gps.nDay, gps.nHour, gps.nMinute, gps.nSecond );
					// 通知QianExe
					BYTE byt = 0xf1;
					DataPush((char*)&byt, 1, DEV_GPS, DEV_QIAN, 2);
					
					sta_bDoneSetTime = true;
				}
			}
#else
			
			DWORD dwCur = GetTickCount();
			static DWORD sta_dwLstChk = dwCur;
			
			if( dwCur - sta_dwLstChk >= 5000 ) // 若2次GPS接收时刻超过5秒
			{
				dwABegin = dwCur; // GPS连续有效的计时重新开始计算
			}
			
			if( 'A' == gps.valid )
			{
				if( dwCur - dwABegin >= TIMEBELIV_GPSAPERIOD ) // 连续有效超过一定时间,则认为GPS时间可信,可以设置为系统时间
				{
					// 校时
					if( true == _SetGpsTime( gps.nYear, gps.nMonth, gps.nDay, gps.nHour, gps.nMinute, gps.nSecond ) )
					{
						PRTMSG(MSG_DBG, "Set SysTime: %04d%02d%02d %02d:%02d:%02d\n", gps.nYear, gps.nMonth, gps.nDay, gps.nHour, gps.nMinute, gps.nSecond );
						
						// 通知QianExe
						BYTE byt = 0xf1;
						DataPush((char*)&byt, 1, DEV_GPS, DEV_QIAN, 2);
						
						sta_bDoneSetTime = true;
					}
				}
				else
				{
					dwVBegin = dwCur; // 确保以后的GPS无效时长都可以重新计时
				}
			}
			else
			{
				if( dwCur - dwVBegin >= TIMEUNBLV_GPSVMINPERIOD ) // 若GPS连续无效超过一定时间
				{
					dwABegin = dwCur; // GPS连续有效的计时重新开始计算
				}
			}
			
			sta_dwLstChk = dwCur; // 更新上次检查时间
#endif
		}

 		/// { 格林威治->北京时区
		// 无效数据也要转换,因为可能时间是正确的 if( 'A' == gps.valid ) // 若数据有效
 		{
 			bool bCarry = false; // 时间计算时的进位标志

 			gps.nHour += 8;
 			if( gps.nHour >= 24 )
 			{
 				gps.nHour -= 24;
 				bCarry = true;
 			}

 			gps.nDay += (bCarry ? 1 : 0);
 			int nMaxDay = 31;
 			if( 2 == gps.nMonth )
 			{
 				if( 0 == gps.nYear % 400 
 					|| (0 == gps.nYear % 4 && 0 != gps.nYear % 100) ) // 闰年
 				{
 					nMaxDay = 29;
 				}
 				else nMaxDay = 28;
 			}
 			else if( gps.nMonth <= 7 )
 			{
 				if( 0 == gps.nMonth % 2 ) nMaxDay = 30;
 			}
 			else
 			{
 				if( 1 == gps.nMonth % 2 ) nMaxDay = 30;
 			}
 			if( gps.nDay > nMaxDay )
 			{
 				gps.nDay -= nMaxDay;
 				bCarry = true;
 			}
 			else
 			{
 				bCarry = false;
 			}

 			gps.nMonth += (bCarry ? 1 : 0);
 			if( gps.nMonth > 12 )
 			{
 				gps.nMonth -= 12;
 				bCarry = true;
 			}
 			else
 			{
 				bCarry = false;
 			}

 			gps.nYear += (bCarry ? 1 : 0);
 		}
 		/// } 格林威治->北京时区

// 		if( sta_bFstValid )
// 		{
// 			if( 'A' == gps.valid ) // || (gps.nYear > 2007) || (2007 == gps.nYear && gps.nMonth >= 5) )
// 			{
// 				_SetGpsTime( BYTE(gps.nYear - 2000), BYTE(gps.nMonth), BYTE(gps.nDay), BYTE(gps.nHour),
// 					BYTE(gps.nMinute), BYTE(gps.nSecond) );
// 				sta_bFstValid = false;
// 			}
// 		}

		// 若速度不可信,则设为无效
		if( gps.speed > BELIV_MAXSPEED )
		{
			gps.valid = 'V';
			gps.m_cFixType = '1';
		}
		
		if( 'A' == gps.valid ) // 若GPS数据有效
		{
			if( gps.speed > SPEED_MOVE ) // 若现在是移动的
			{
				if( 1 != bytRealMoveType ) // 若原先不是移动的
				{				
					bytRealMoveType = 1; // 修改为移动标志
					dwContinueCount = 1; // 重置状态持续次数
				}
				else
				{
					++ dwContinueCount; // 递增该状态持续次数
				}

				// 若持续次数达到设定,且上次发送状态不是移动,发送车辆变为移动的通知
				if( SPEED_COUNT == dwContinueCount && 1 != bytSndMoveType )
				{
					szSta[1] = 1;
					PRTMSG(MSG_DBG, "Send car move msg!\n" );
					DataPush(szSta, 2, DEV_GPS, DEV_QIAN, 2);
					bytSndMoveType = 1;
				}
			}
			else if( gps.speed < SPEED_STOP ) // 若现在是静止的
			{
				if( 2 != bytRealMoveType ) // 若原先不是静止的
				{				
					bytRealMoveType = 2; // 修改为静止标志
					dwContinueCount = 1; // 重置状态持续次数
				}
				else
				{
					++ dwContinueCount; // 递增该状态持续次数
				}

				// 若持续次数达到设定,且上次发送状态不是静止,发送车辆变为静止的通知
				if( SPEED_COUNT == dwContinueCount && 2 != bytSndMoveType )
				{
					szSta[1] = 2;
					PRTMSG(MSG_DBG, "Send car silent msg!\n" );
					DataPush(szSta, 2, DEV_GPS, DEV_QIAN, 2);

					bytSndMoveType = 2;
				}
			}
		}
		else
		{
			// dwContinueCount = 0; // GPS数据无效,重置状态持续计数
			// bytRealMoveType = 0; // 清除移动/静止标志
		}

		// ACC无效时,强制让速度等于0
		IOGet((byte)IOS_ACC, &uszResult);
		if( IO_ACC_OFF == uszResult ) gps.speed = 0;

		gps.m_bytMoveType = bytSndMoveType; // 与发送状态同步
		if( gps.speed <= BELIV_MAXSPEED ) // 速度可信的才保留
		{
			SetCurGps( &gps, GPS_REAL );
		}

		if( 0 == dwCt ++ % 3 )
		{
// 					PRTMSG(MSG_DBG,"Gps:%c %u:%u:%u", char(gps.valid), gps.nHour, gps.nMinute, gps.nSecond);
// 					PRTMSG(MSG_DBG, "Lon=%0.6f", gps.longitude );
// 					PRTMSG(MSG_DBG, "Lat=%0.6f", gps.latitude );
		}
	}

_LOOP_READ_END:
	g_bProgExit = true;
	return 0;
}
Exemplo n.º 11
0
//////////////////////////////////////////////////////////////////////////
// loop!  Autogenerated by RubyToC, sorry it's ugly.
//////////////////////////////////////////////////////////////////////////
void
loop() {
digitalWrite(led(), 1);
serial_print(read(gps()));
}
Exemplo n.º 12
0
/**
 * @brief Creates an XsOutputConfigurationArray and pushes it to the sensor.
 * @details
 * - Configures the sensor with desired modules
 * - Refer to xsdataidentifier.h
 */
void mtiG::configure(){

	XsOutputConfigurationArray configArray;

	if(mSettings.orientationData){			//Quaternion - containsOrientation
			XsOutputConfiguration quat(XDI_Quaternion, mSettings.orientationFreq);// para pedir quaternion
			configArray.push_back(quat);
	}
	
	if(mSettings.gpsData){
			//LATITUDE E LONGITUDE -containsLatitudeLongitude
			XsOutputConfiguration gps(XDI_LatLon, mSettings.gpsFreq);// para pedir gps, //XDI_Quaternion 06/04
			configArray.push_back(gps);

			XsOutputConfiguration gps_age(XDI_GpsAge, mSettings.gpsFreq);// para pedir gps, //XDI_Quaternion 06/04
			configArray.push_back(gps_age);

			XsOutputConfiguration gps_sol(XDI_GpsSol, mSettings.gpsFreq);// para pedir gps, //XDI_Quaternion 06/04
			configArray.push_back(gps_sol);

			XsOutputConfiguration gps_dop(XDI_GpsDop, mSettings.gpsFreq);// para pedir gps, //XDI_Quaternion 06/04
			configArray.push_back(gps_dop);
	}
	
	if(mSettings.temperatureData){	
			//TEMPERATURA - containsTemperature
			XsOutputConfiguration temp(XDI_Temperature, mSettings.temperatureFreq);
			configArray.push_back(temp);
	}	
	
	if(mSettings.accelerationData){
			//ACCELERATION - containsCalibratedAcceleration
			XsOutputConfiguration accel(XDI_Acceleration, mSettings.accelerationFreq);
			configArray.push_back(accel);
	}

	if(mSettings.pressureData){	
			//PRESSURE - containsPressure
			XsOutputConfiguration baro(XDI_BaroPressure, mSettings.pressureFreq);
			configArray.push_back(baro);
	}

	if(mSettings.magneticData){
			//MAGNETIC FIELD - containsCalibratedMagneticField
			XsOutputConfiguration magnet(XDI_MagneticField, mSettings.magneticFreq);
			configArray.push_back(magnet);
	}

	if(mSettings.altitudeData){
			//ALTITUDE - containsAltitude
			XsOutputConfiguration alt(XDI_AltitudeEllipsoid, mSettings.altitudeFreq);
			configArray.push_back(alt);
	}

	if(mSettings.gyroscopeData){
			//GYRO - containsCalibratedGyroscopeData
			XsOutputConfiguration gyro(XDI_RateOfTurn, mSettings.gyroscopeFreq);
			configArray.push_back(gyro);
	}	

	if(mSettings.velocityData){
			//VELOCIDADE XYZ
			XsOutputConfiguration vel_xyz(XDI_VelocityXYZ, mSettings.velocityFreq);
			configArray.push_back(vel_xyz);
	}
	
	// Puts configArray into the device, overwriting the current configuration
	if (!device->setOutputConfiguration(configArray))
	{
			throw std::runtime_error("Could not configure MTmk4 device. Aborting.");
	}
}
Exemplo n.º 13
0
int CGps::GetCurGps( void* v_pGps, const unsigned int v_uiSize, BYTE v_bytGetType )
{
	// 可以同时设置实时GPS和有效GPS

	if( v_uiSize < sizeof(tagGPSData) ) return ERR_BUFLACK;
	
	int iRet = 0;

	tagGpsShare objGpsShare;
	tagGPSData gps(0);

	_SemP();

	memcpy( &objGpsShare, m_pShareMem, sizeof(objGpsShare) );

	_SemV();

	switch( v_bytGetType )
	{
	case GPS_REAL:
		{
			gps = objGpsShare.m_objRealGps;
			if( gps.speed > BELIV_MAXSPEED ) // 若实时GPS数据速度超过最大可信速度,则认为不可信,使用上次有效数据
			{
				gps = objGpsShare.m_objValidGps;
				gps.valid = 'V';
				gps.m_cFixType = '1';
			}
		}
		break;

	case GPS_LSTVALID:
	default:
		gps = objGpsShare.m_objValidGps;
	}

	if( GetTickCount() - objGpsShare.m_dwSetGpsTm >= 5000 // 若连续一段时间没有设置GPS数据
		||
		objGpsShare.m_objRealGps.valid != 'A' ) // 或GPS无效
	{
		// 用系统时间修正无效的GPS数据,并且GPS数据强制为无效的
		
		struct tm pCurrentTime;
		G_GetTime(&pCurrentTime);
		
		gps.valid = 'V';
		gps.m_cFixType = '1';
		gps.nYear = pCurrentTime.tm_year + 1900;
		gps.nMonth = pCurrentTime.tm_mon + 1;
		gps.nDay = pCurrentTime.tm_mday;
		gps.nHour = pCurrentTime.tm_hour;
		gps.nMinute = pCurrentTime.tm_min;
		gps.nSecond = pCurrentTime.tm_sec;
		
	}

	if( gps.nYear < 2001 ) 
	{
		gps.nYear = 2001; // 前置机有bug,不支持2000年以前的,而2000年会使得上发的数据中存在0
	}

	if( gps.speed >= BELIV_MAXSPEED ) // 若速度超过最大可信速度,则都设为无效数据
	{
		gps.valid = 'V';
		gps.m_cFixType = '1';
	}
	else if( gps.speed < 0 ) // 若速度小于0,则强制修正速度,并且都设为无效数据
	{
		gps.speed = 0;
		gps.valid = 'V';
		gps.m_cFixType = '1';
	}

	memcpy( v_pGps, &gps, sizeof(tagGPSData) );

	return iRet;
}
double testPointing(double dtheta = 0, int run = 332, int event = 55448680)
// double testPointing(double dtheta = 0, int run = 352, int event = 60832108)   
{

  AnitaDataset d(run); 
  d.getEvent(event); 

  UsefulAdu5Pat gps(d.gps()); 

  double wais_phi, wais_theta; 
  gps.getThetaAndPhiWaveWaisDivide(wais_theta, wais_phi); 
  wais_theta += dtheta * TMath::DegToRad(); 

  double rampdem_wais_alt = AnitaLocations::getWaisAltitude(); 
  printf("Real WAIS lat/lon/alt: %g %g %g\n", AnitaLocations::getWaisLatitude(), AnitaLocations::getWaisLongitude(), AnitaLocations::getWaisAltitude()); 
  printf(" According to AnitaGeomTool, WAIS at phi=%g theta=%g\n", wais_phi* TMath::RadToDeg(), wais_theta* TMath::RadToDeg()); 

  PayloadParameters pp0(d.gps(), AntarcticCoord(AntarcticCoord::WGS84, AnitaLocations::getWaisLatitude(), AnitaLocations::getWaisLongitude(), rampdem_wais_alt )); 
  printf(" According to PayloadParameters, wais at phi=%g, theta=%g\n", pp0.source_phi, pp0.source_theta); 


  double lat,lon,alt; 
  int trace = gps.traceBackToContinent(wais_phi, wais_theta, &lon, &lat,&alt,0); 

  printf("traceBackToContent projects to lat/lon/alt: %g %g %g\n",lat,lon,alt); 



  double payload_phi, payload_theta; 
  gps.getThetaAndPhiWave(lon,lat,alt, payload_theta, payload_phi); 
  printf("In payload coordinates that is: phi=%g theta=%g\n", payload_phi * TMath::RadToDeg(), payload_theta * TMath::RadToDeg()); 


  AntarcticCoord c(AntarcticCoord::WGS84,lat,lon,alt); 
  PayloadParameters pp(d.gps(), c); 
  printf("Payload parameters thinks it's at phi=%g, theta=%g\n", pp.source_phi, pp.source_theta); 

  int get = gps.getSourceLonAndLatAtAlt(wais_phi, wais_theta, lon, lat, alt);
  printf("getSourceLonAndLatAtAlt projects to lat/lon/alt: %g %g %g\n",lat,lon,alt); 


  gps.getThetaAndPhiWave(lon,lat,alt, payload_theta, payload_phi); 
  printf("In payload coordinates that is: phi=%g theta=%g\n", payload_phi * TMath::RadToDeg(), payload_theta * TMath::RadToDeg()); 

  AntarcticCoord c2(AntarcticCoord::WGS84,lat,lon,alt); 
  PayloadParameters pp2(d.gps(), c2); 
  printf("Payload parameters thinks it's at phi=%g, theta=%g\n", pp2.source_phi, pp2.source_theta); 


  PayloadParameters pp3; 
  int success = PayloadParameters::findSourceOnContinent(pp0.source_theta, pp0.source_phi, d.gps(), &pp3); 

  if (success==1) 
  {
    AntarcticCoord c3 = pp3.source.as(AntarcticCoord::WGS84); 
    printf("findSourceOnContinent projects to lat/lon/alt: %g %g %g\n",c3.x,c3.y,c3.z); 
    printf("Payload parameters thinks it's at phi=%g, theta=%g\n", pp3.source_phi, pp3.source_theta); 
  }
  else
  {
    printf("findSourceOnContinent found no solution :(\n"); 

  }

  double x0 = pp0.payload.as(AntarcticCoord::STEREOGRAPHIC).x; 
  double y0 = pp0.payload.as(AntarcticCoord::STEREOGRAPHIC).y; 


  TFile * fout = new TFile("refraction.root","RECREATE"); 
  TH2I * old_h = new TH2I("traceBack","TraceBack", 1000, x0 - 800e3, x0+800e3, 1000, y0 - 800e3, y0+800e3);
  TH2I * new_h3 = new TH2I("traceBack3","TraceBack3", 1000, x0 - 800e3, x0+800e3, 1000, y0 - 800e3, y0+800e3);   
  TH2I * new_h = new TH2I("findSource","FindSource", 1000, x0 - 800e3, x0+800e3, 1000, y0 - 800e3, y0+800e3); 
  TH2I * new_h_col = new TH2I("findSourceColl","FindSourceColl", 1000, x0 - 800e3, x0+800e3, 1000, y0 - 800e3, y0+800e3); 
  TH2I * new_h_ref = new TH2I("findSourceRef","FindSourceRefract", 1000, x0 - 800e3, x0+800e3, 1000, y0 - 800e3, y0+800e3); 
  TH2 * old_payload_el = new TProfile2D("traceBackPayloadEl","TraceBackPayloadEl", 1000, x0 - 800e3, x0+800e3, 1000, y0 - 800e3, y0+800e3);
  TH2 * new_h3_payload_el = new TProfile2D("traceBack3PayloadEl","TraceBack3PayloadEl", 1000, x0 - 800e3, x0+800e3, 1000, y0 - 800e3, y0+800e3);   
  TH2 * new_payload_el = new TProfile2D("findSourcePayloadEl","findSourcePayloadEl", 1000, x0 - 800e3, x0+800e3, 1000, y0 - 800e3, y0+800e3); 
  TH2 * new_payload_el_col = new TProfile2D("findSourcePayloadElCol","findSourcePayloadElCol", 1000, x0 - 800e3, x0+800e3, 1000, y0 - 800e3, y0+800e3); 
  TH2 * new_payload_el_ref = new TProfile2D("findSourcePayloadElRef","findSourcePayloadElRef", 1000, x0 - 800e3, x0+800e3, 1000, y0 - 800e3, y0+800e3); 

  Refraction::SphRay m; 
  for (double phi = 0; phi <=360; phi+= 0.2) 
  {
    printf("%g\n",phi); 
    double step = 0.001;
    for (double sin_theta = 0.0; sin_theta < 0.9; sin_theta+= step)
    {
        double theta = asin(sin_theta*sin_theta) * TMath::RadToDeg(); 
        trace = gps.traceBackToContinent(phi*TMath::DegToRad(), theta*TMath::DegToRad(), &lon, &lat,&alt,0); 
        if (trace) 
        {
          AntarcticCoord w(AntarcticCoord::WGS84,lat,lon,alt); 
          w.to(AntarcticCoord::STEREOGRAPHIC); 
          old_h->Fill(w.x, w.y); 
          PayloadParameters pw(d.gps(), w); 
          old_payload_el->Fill(w.x,w.y, pw.payload_el); 

        }


        trace = gps.traceBackToContinent3(phi*TMath::DegToRad(), theta*TMath::DegToRad(), &lon, &lat,&alt,0); 
        if (trace)
        {
          AntarcticCoord w(AntarcticCoord::WGS84,lat,lon,alt);
          w.to(AntarcticCoord::STEREOGRAPHIC);
          new_h3->Fill(w.x, w.y);
          PayloadParameters pw(d.gps(), w);
          new_h3_payload_el->Fill(w.x,w.y, pw.payload_el);
        }

        PayloadParameters::findSourceOnContinent(theta,phi, d.gps(), &pp3); 
        AntarcticCoord w = pp3.source.as(AntarcticCoord::STEREOGRAPHIC); 
        new_h->Fill(w.x,w.y); 
        new_payload_el->Fill(w.x,w.y, pp3.payload_el); 

        PayloadParameters::findSourceOnContinent(theta,phi, d.gps(), &pp3,0, 50); 
        w = pp3.source.as(AntarcticCoord::STEREOGRAPHIC); 
        new_h_col->Fill(w.x,w.y); 
        new_payload_el_col->Fill(w.x,w.y, pp3.payload_el); 

        PayloadParameters::findSourceOnContinent(theta,phi, d.gps(), &pp3,&m); 
        w = pp3.source.as(AntarcticCoord::STEREOGRAPHIC); 
        new_h_ref->Fill(w.x,w.y); 
        new_payload_el_ref->Fill(w.x,w.y, pp3.payload_el); 


    }
  }

  TCanvas * cc = new TCanvas; 
  cc->Divide(5,2);
  cc->cd(1);
  old_h->Draw("colz"); 
  cc->cd(2);
  new_h->Draw("colz"); 
  cc->cd(3);
  new_h_col->Draw("colz"); 
  cc->cd(4);
  new_h_ref->Draw("colz"); 
  cc->cd(5);
  new_h3->Draw("colz");

  cc->cd(6);
  old_payload_el->Draw("colz");
  cc->cd(7);
  new_payload_el->Draw("colz");
  cc->cd(8);
  new_payload_el_col->Draw("colz"); 
  cc->cd(9);
  new_payload_el_ref->Draw("colz");
  cc->cd(10);
  new_h3_payload_el->Draw("colz");


  cc->SaveAs("refraction.pdf"); 

  fout->cd(); 
  old_h->Write(); 
  new_h->Write(); 
  new_h_col->Write(); 
  new_h_ref->Write(); 
  old_payload_el->Write(); 
  new_payload_el->Write(); 
  new_payload_el_col->Write(); 
  new_payload_el_ref->Write(); 


  return wais_phi * TMath::RadToDeg() - pp0.source_phi; 

}
Exemplo n.º 15
0
int main(int argc, const char * argv[]) {
    
    
    std::ofstream to_log(to_log_file_path.c_str());
    std::ofstream to_rpy(rpy_file_path.c_str());
    std::ofstream gps_out(gps_out_path.c_str());
    std::ofstream gps_filter_out(gps_filter_out_path.c_str());
    std::ofstream mag_out(mag_out_path.c_str());
    
    try {
        
    // Declaring objects used for inertial navigation
        ACCELEROMETER acc(acc_file_path, Captor::COLD_START);
        GYRO gyro(gyro_file_path, Captor::COLD_START);
        MAGNETOMETER mag(mag_file_path, Captor::COLD_START);
        GPS gps(gps_file_path,GPS::UBLOX);
        GPS_Filter gps_filter(&gps);
        EKF ekf(&gps_filter,&acc,&gyro,&mag);
        
    
    
    
    // Getting offset and gain for accelerometer, gyroscope and magnetometer. Getting GPS HOME field.
    // This routine will warn you if one of the offset couldn't be updated
        int ret = acc.initOffsets();
        if (ret != 1) std::cout << "Couldn't get acc offsets !" << std::endl;
        else std::cout << "Acc offsets updated" << std::endl;
        ret = acc.initGain();
        if (ret != 1) std::cout << "Couldn't get acc gain !" << std::endl;
        else std::cout << "Acc gain updated" << std::endl;
        
        // Advanced calibration for acc (offset and gain)
#ifdef ADCALIBRATION
        ret = acc.performAdvancedAccCalibration();
        if (ret !=1) std::cout << "Couldn't perform acc advanced calibration method" << std::endl;
        else {
            std::cout << "Acc advanced calibration performed" << std::endl;
            acc.correct_GN();
        }
#endif
        
        ret = gyro.initOffsets();
        if (ret != 1) std::cout << "Couldn't get gyro offsets !" << std::endl;
        else std::cout << "Gyro offsets updated" << std::endl;
        ret = gyro.initGain();
        if (ret != 1) std::cout << "Couldn't get gyro gain !" << std::endl;
        else std::cout << "Gyro gain updated" << std::endl;
        
        ret = mag.initOffsets();
        if (ret != 1) std::cout << "Couldn't get magnetometer offsets !" << std::endl;
        else std::cout << "Mag offsets updated" << std::endl;
        
        ret = gps.setHome();
        if (ret != 1) std::cout << "Couldn't get HOME !" << std::endl;
        else std::cout << "HOME updated" << std::endl;
        
        
        
    // Reading lines
        // We create the buffer for the sensor's output values
        Eigen::Vector3f acc_vector_buffer;
        Eigen::Vector3f gyro_vector_buffer;
        Eigen::Vector3d gps_buffer_vector;
        Eigen::Vector3d gps_position_vector;
        
       // We update and correct the magnetometer
        mag._init_earth_even_magnetic_field();
        mag.update_correct();
        // We initialize the state vector giving the initial heading
        ekf.init_state_vector(mag.getHeading());
        //ekf.init_state_vector();
        to_rpy << TODEG*(ekf.toRPY(ekf.get_state_vector())).transpose() << std::endl;           // Storing operation
        
        
        
        while (!acc.line_end && !gyro.line_end){
            
            // Reading from inertial captors and correcting the outputs
            acc.getOutput(&acc_vector_buffer);
            acc.correctOutput(&acc_vector_buffer, ekf.getDCM());
            gyro.getOutput(&gyro_vector_buffer);
            gyro.correctOutput(&gyro_vector_buffer);
            // Updating and correcting magnetometer
            mag.update_correct();
            // Updating GPS and storing into the &gps_buffer_vector
            gps.update(&gps_buffer_vector);
            
            
        
            // Predicting the state vector
            // Extended Kalman Filter according step is the prediction step. We first build the Jacobian matrix linearized around the current state and we then multiply the currrent state vector by such a matrix
            ekf.build_jacobian_matrix(&acc_vector_buffer, &gyro_vector_buffer);
            ekf.predict();
            
            to_rpy << TODEG*(ekf.getRPY()).transpose() << std::endl;                            // Storing operation for data exploitation
            to_log << ekf.get_state_vector().transpose() << std::endl;                          // Storing operation for data exploitation
            //mag_out << TODEG*mag.getHeading() << std::endl;                                   // Storing operation for data exploitation
            mag_out << TODEG*mag.getHeading((ekf.getRPY())(1),(ekf.getRPY())(0)) << std::endl;  // Storing operation for data exploitation

            
    

            
            // Checking inf a new gps data is availaible
            if (gps.isAvailable()){
                // If yes, we first update the GPS filter
                gps.calculatePositionFromHome(&gps_buffer_vector);
                gps_out << gps.getActualPosition().transpose() << std::endl;
                gps.actualizeInternDatas(&gps_buffer_vector);
                gps_filter.predict();
                gps_filter.updateFilter();
                gps_filter_out << gps_filter.getState().transpose() << std::endl;               // Storing operation for data exploitation
                // And we then correct the EKF filter by correcting in order position, speed and quaternion attitude vector
                //ekf.correct();
                //gps_filter.updateSpeedEKF(ekf.getCurrentSpeed());
            }
            
        }

    }
    catch (std::exception const& e)
    { std::cout << e.what() <<  " file " << std::endl; }
    
    

    
    return 0;
}