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);
}
}
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;
}
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");
}
}
/***********************************************************************
* 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;
}
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();
}
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(""));
}
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;
}
int32_t main(int32_t a_argc, char **a_argv)
{
opendlv::proxy::sensor::gps::Gps gps(a_argc, a_argv);
return gps.runModule();
}
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;
}
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;
}
//////////////////////////////////////////////////////////////////////////
// loop! Autogenerated by RubyToC, sorry it's ugly.
//////////////////////////////////////////////////////////////////////////
void
loop() {
digitalWrite(led(), 1);
serial_print(read(gps()));
}
/**
* @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.");
}
}
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;
}
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;
}
