diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index f76fd514f1a4ebe76a353db1600d50c1c4f301a2..69c932ef1650dddf0eaebb9bb128b6ee73e3fc23 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,8 +1,8 @@
# driver source files
-SET(sources asterx1_gps.cpp)
+SET(sources asterx1_gps.cpp stream_gps.cpp)
# application header files
-SET(headers asterx1_gps.h)
+SET(headers asterx1_gps.h stream_gps.h)
# locate the the necessary dependencies
FIND_PACKAGE(iriutils)
diff --git a/src/asterx1_gps.cpp b/src/asterx1_gps.cpp
index 7533be0d94b28c7accb095655d4d240a7040fb94..5d437070e1aabee3256a5009feae09f563f7f270 100644
--- a/src/asterx1_gps.cpp
+++ b/src/asterx1_gps.cpp
@@ -1,849 +1,1107 @@
#include "asterx1_gps.h"
-
-CasteRx1::CasteRx1(const string & hwPortName, const int acqRate)
+#include "gps_types.h"
+#include "ctime.h"
+
+/* Look up table for fast computation of the CCITT 16-bit CRC. */
+static const unsigned short CRC_16CCIT_LookUp[256] = {
+ 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7,
+ 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef,
+ 0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6,
+ 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de,
+ 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485,
+ 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d,
+ 0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4,
+ 0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc,
+ 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823,
+ 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b,
+ 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12,
+ 0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a,
+ 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41,
+ 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49,
+ 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70,
+ 0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78,
+ 0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f,
+ 0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067,
+ 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e,
+ 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256,
+ 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d,
+ 0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
+ 0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c,
+ 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634,
+ 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab,
+ 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3,
+ 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a,
+ 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92,
+ 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9,
+ 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1,
+ 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8,
+ 0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0
+};
+
+CasteRx1::CasteRx1(const std::string &name,const std::string &hwPortName)
{
- status = 0;
- config.portName = hwPortName;
- config.acqPeriod = acqPeriodValues[acqRate];
+ this->serialPort=NULL;
+ this->serial_device=hwPortName;
+ this->gps_running=false;
+ /* initialize events */
+ this->event_server=CEventServer::instance();
+ this->finish_thread_event_id=name+"_finish_thread_event";
+ this->event_server->create_event(this->finish_thread_event_id);
+ this->new_reply_event_id=name+"_new_reply_event";
+ this->event_server->create_event(this->new_reply_event_id);
+ this->new_meas_block_event_id=name+"_new_meas_block";
+ this->event_server->create_event(this->new_meas_block_event_id);
+ this->new_gps_nav_block_event_id=name+"_new_gps_nav_block";
+ this->event_server->create_event(this->new_gps_nav_block_event_id);
+ this->new_gps_alm_block_event_id=name+"_new_gps_alm_block";
+ this->event_server->create_event(this->new_gps_alm_block_event_id);
+ this->new_gps_ion_block_event_id=name+"_new_gps_ion_block";
+ this->event_server->create_event(this->new_gps_ion_block_event_id);
+ this->new_gps_utc_block_event_id=name+"_new_gps_utc_block";
+ this->event_server->create_event(this->new_gps_utc_block_event_id);
+ this->new_pvt_block_event_id=name+"_new_pvt_block";
+ this->event_server->create_event(this->new_pvt_block_event_id);
+ /* initialize threads */
+ this->thread_server=CThreadServer::instance();
+ this->gps_process_data_thread_id=name+"_gps_process_data_thread";
+ this->thread_server->create_thread(this->gps_process_data_thread_id);
+ this->thread_server->attach_thread(this->gps_process_data_thread_id,this->gps_process_data_thread,this);
}
CasteRx1::~CasteRx1()
{
- blockList.clear();
- if( (status & ACQUISITION_RUNNING) ) stopAcquisition();
- if( (status & DEVICE_OPEN) ) closeDevice();
- delete serialPort;
- cout << "CasteRx1::~CasteRx1(). End of CasteRx1 destructor" << endl; //bye bye message
-}
-
-int CasteRx1::openDevice()
-{
- TRS232_config serialConfig;
- int retValue = BASIC_SUCCESS;
-
- try{
- serialPort = new CRS232(config.portName); //creates a new object to manage serial connection
- }catch(CException &e)
- {
- cout << e.what() << endl;
- cout << "CasteRx1::openDevice(). ERROR. Allocation error when creating new object CRS232" << endl;
- retValue = PORT_OPEN_ERROR;
- }
-
- if ( retValue == BASIC_SUCCESS ) //if successfully created , we can open it
- {
- try{
- serialPort->open( (void*)&config.portName );
- }catch(CCommException &e)
- {
- cout << e.what() << endl;
- cout << "CasteRx1::openDevice(). ERROR. Fail to open serial port" << endl;
- retValue = PORT_OPEN_ERROR;
- }
- }
-
- if ( retValue == BASIC_SUCCESS ) //if open is successful, we can configure
- {
- serialConfig.baud = ASTERX_BAUDRATE;
- serialConfig.num_bits = 8;
- serialConfig.parity = none;
- serialConfig.stop_bits = 1;
- try{
- serialPort->config( &serialConfig );
- }catch(CCommException &e)
- {
- cout << e.what() << endl;
- cout << "CasteRx1::openDevice(). ERROR. Fail to configure serial port" << endl;
- retValue = PORT_CONFIG_ERROR;
- }
- }
-
- if (retValue == BASIC_SUCCESS)
- {
- status |= DEVICE_OPEN;
- cout << "CasteRx1::openDevice(). SUCCESS. Serial port " << config.portName << " opened successfully." << endl;
-
- }
-
- return retValue;
-}
-
-int CasteRx1::closeDevice()
-{
- int retValue = BASIC_SUCCESS;
-
- if ( (status & DEVICE_OPEN) )
- {
- try{
- serialPort->close();
- //status &= ~DEVICE_OPEN;
- }catch(CException &e)// catch(CCommException &e)
- {
- cout << e.what() << endl;
- cout << "CasteRx1::openDevice(). ERROR. Fail to close serial port " << config.portName << endl;
- retValue = PORT_CLOSE_ERROR;
- //status |= DEVICE_OPEN; //close() has failed , so it is still opened
- }
- }
- if ( retValue == BASIC_SUCCESS ) //if close() is successful, we delete the object
- {
- status &= ~DEVICE_OPEN;
- cout << "CasteRx1::closeDevice(). SUCCESS. Serial port " << config.portName << " closed successfully." << endl;
- }
-
- return retValue;
-}
-
-int CasteRx1::startAcquisition()
-{
- int retValue=0;
- sbfBlockManager *blockMng;
- string cmnd;
-
- //enables commands at USB1 device input
- retValue += sendCommand("setDataInOut,USB1,CMD,none\n");
-
- //stops data transmission if it was running
- retValue += sendCommand("setSBFOutput,all,USB1,none\n");
-
- //enables septentrio binary format (SBF) as output at USB1
- retValue += sendCommand("setDataInOut,USB1,CMD,+SBF\n");
-
- //GPS signal processing options for urban mobile robots
- retValue += sendCommand("setMultipathMitigation,on\n");//enables processing for multipath removing (pag.26)
- retValue += sendCommand("setReceiverDynamics,Moderate\n");//Mobile robots moves at moderate speeds
- retValue += sendCommand("setPVTMode,Rover,StandAlone\n");//rover mode and enables standalone
-
- //data configuration. Add blocks of interest to "group 1"
- retValue += sendCommand("setSBFGroups,Group1,PVTCartesian\n");
- blockMng = new sbfBlockManager; blockMng->blockId = PVTCartesian_ID; blockMng->received = false;
- blockList.push_back(*blockMng);
-
- retValue += sendCommand("setSBFGroups,Group1,+PosCovCartesian\n");
- blockMng = new sbfBlockManager; blockMng->blockId = PosCovCartesian_ID; blockMng->received = false;
- blockList.push_back(*blockMng);
-
- retValue += sendCommand("setSBFGroups,Group1,+DOP\n");
- blockMng = new sbfBlockManager; blockMng->blockId = DOP_ID; blockMng->received = false;
- blockList.push_back(*blockMng);
-
- retValue += sendCommand("setSBFGroups,Group1,+PVTGeodetic\n");
- blockMng = new sbfBlockManager; blockMng->blockId = PVTGeodetic_ID; blockMng->received = false;
- blockList.push_back(*blockMng);
-
- //requests "Group1" data to be output to USB1 at specified rate
- cmnd = "setSBFOutput,Stream1,USB1,Group1,";
- cmnd.append(config.acqPeriod);
- cmnd.append("\n");
- retValue += sendCommand(cmnd);
-
- if ( retValue == BASIC_SUCCESS*11 ) //success on all commands
- {
- status |= ACQUISITION_RUNNING;
- retValue = BASIC_SUCCESS;
- cout << "CasteRx1::startAcquisition(). SUCCESS on device configuration. The device is now transmitting data ..." << endl;
- }
- else //otherwise
- {
- status &= ~ACQUISITION_RUNNING;
- retValue = ASTERX1_CONFIG_ERROR;
- cout << "CasteRx1::startAcquisition(). ERROR on device configuration for data transmission" << endl;
- }
-
- return retValue;
-}
-
-int CasteRx1::stopAcquisition()
-{
- int retValue=0;
-
- if( (status & ACQUISITION_RUNNING) )
- {
- blockList.clear();
-
- retValue += sendCommand("setSBFOutput,Stream1,USB1,none\n");//stops data transmission from device
- retValue += sendCommand("setDataInOut,USB1,CMD,none\n");//enables commands as input at USB1
- if ( retValue == BASIC_SUCCESS*2 )
- {
- status &= ~ACQUISITION_RUNNING;
- retValue = BASIC_SUCCESS;
- cout << "CasteRx1::stopAcquisition(). SUCCESS on stopping the device data transmission." << endl;
- }
- else
- {
- retValue = ASTERX1_STOPPINTG_ERROR;
- cout << "CasteRx1::stopAcquisition(). ERROR on stopping the device data transmission." << endl;
- }
- }
- else
- {
- cout << "CasteRx1::stopAcquisition(). Nothing to do, the acquisition was already stopped." << endl;
- retValue = BASIC_SUCCESS;
- }
-
- return retValue;
-}
-
-int CasteRx1::readDataFromDevice()
-{
- int retValue;
- unsigned char buffer[MAX_BLOCK_SIZE];
- unsigned short int crcValue, crcComputed; //crc value from device and crc computed
- unsigned short int blockId; //data block id from receiver
- unsigned short int blockLength; //size of the SBF block in bytes
- timeval tmStmp;
- list<sbfBlockManager>::iterator iiBlock;
- bool allReceived = false;
-
- //reset received field of block managers
- for (iiBlock=blockList.begin();iiBlock!=blockList.end();iiBlock++){ iiBlock->received = false; }
-
- //acquisition up to all requested blocks are received
- while (!allReceived)
- {
- //Sets Time Stamp (this is the system time stamp, not the GPS time!)
- gettimeofday(&tmStmp, NULL);
- timeStamp = (double)(tmStmp.tv_sec + tmStmp.tv_usec/1e6);
-
- retValue = synchronizeHeader((const unsigned char *)"$@", DATA_TIMEOUT);
- if ( retValue != BASIC_SUCCESS ) //if !BASIC_SUCCESS, status will be either COMMTIMEOUT or SERIALCOMMERROR
- {
- status &= ~DATA_BLOCK_OK;
- cout << "CasteRx1::readDataFromDevice(). ERROR. when synchronizing with header. Expected header = $@" << endl;
- return ACQUISITION_ERROR;
- }
-
- //starts reading header
- retValue = 0;
- retValue += readNbytes(&buffer[0],2,DATA_TIMEOUT);//reads transmitted crc value
- crcValue = getUI2(&buffer[0]); //decodes crc value
- retValue += readNbytes(&buffer[0],2,DATA_TIMEOUT);//reads block id field
- blockId = (buffer[0] & 0xff)|((buffer[1] & 0x1f)<<8);//decodes blockId (uses only the 13 LS bits)
- retValue += readNbytes(&buffer[2],2,DATA_TIMEOUT);//reads block length
- blockLength = getUI2(&buffer[2]);//decodes block length (in bytes)
-
- //check for some errors
- if ( retValue != 3*BASIC_SUCCESS )
- {
- status &= ~DATA_BLOCK_OK;
- cout << "CasteRx1::readDataFromDevice(). ERROR. Error reading block header." << endl;
- return ACQUISITION_ERROR;
- }
- if ( (blockLength%4!=0) || (blockLength < MIN_BLOCK_SIZE) || (blockLength > MAX_BLOCK_SIZE) )
- {
- status &= ~DATA_BLOCK_OK;
- cout << "CasteRx1::readDataFromDevice(). ERROR. SBF block Length not correct: " << blockLength << " bytes" << endl;
- return ACQUISITION_ERROR;
- }
-
- //cout << "readDataFromDevice(): "<< __LINE__ << "; blockLength = " << blockLength << endl;
-
- //reads the rest of the block and loads it to the buffer
- retValue = readNbytes(&buffer[4],(blockLength-8),DATA_TIMEOUT);
- if ( retValue != BASIC_SUCCESS )
- {
- status &= ~DATA_BLOCK_OK;
- cout << "CasteRx1::readDataFromDevice(). ERROR. Error reading block data." << endl;
- return ACQUISITION_ERROR;
- }
-
- //Computes & checks CRC
- crcComputed = CRC_compute16CCITT(buffer, blockLength-4);
- //cout << "Expected value: " << crcValue << "; Computed value: " << crcComputed << endl;
- if ( crcComputed != crcValue )
- {
- status &= ~DATA_BLOCK_OK;
- cout << "CasteRx1::readDataFromDevice(). ERROR. CRC error. Expected value: " << crcValue << "; Computed value: " << crcComputed << endl;
- return ACQUISITION_ERROR;
- }
-
- //decodes SBF block
- switch(blockId)
- {
- case PVTCartesian_ID:
- retValue = decodePVTCartesian(&buffer[4]);
- markBlockAsReceived(PVTCartesian_ID);
- break;
-
- case PVTGeodetic_ID:
- retValue = decodePVTGeodetic(&buffer[4]);
- markBlockAsReceived(PVTGeodetic_ID);
- break;
-
- case PosCovCartesian_ID:
- retValue = decodePosCovCartesian(&buffer[4]);
- markBlockAsReceived(PosCovCartesian_ID);
- break;
-
- case DOP_ID:
- retValue = decodeDOP(&buffer[4]);
- markBlockAsReceived(DOP_ID);
- break;
-
- default:
- cout << "CasteRx1::readDataFromDevice(). ERROR. Unknown SBF blockId: " << blockId << endl;
- retValue = ACQUISITION_ERROR;
- break;
- }
-
- //check resulting status of the decoding function
- if ( retValue != BASIC_SUCCESS )
- {
- status &= ~DATA_BLOCK_OK;
- cout << "CasteRx1::readDataFromDevice(). ERROR on decoding SBF block. BlockId = " << blockId << ". Status = " << status << endl;
- return ACQUISITION_ERROR;
- }
- else//sets status and checks recursively if all blocks have been received
- {
- status |= DATA_BLOCK_OK;
- allReceived = true;
- for (iiBlock=blockList.begin();iiBlock!=blockList.end();iiBlock++){ allReceived = iiBlock->received & allReceived; }
- }
- }
-
- //check gps availability
- if ( pvtError == 0 )
- {
- status |= GPS_AVAILABLE;
- fromWgsToMap(); //conversion to map coordinates
- computeTMenu(); //compute rotation matrix form WGS to local tangential ENU frame
- computeENUcovs(); //compute covariances from WGS to local tangential ENU frame
- }
- else
- {
- status &= ~GPS_AVAILABLE;
- }
-
- return BASIC_SUCCESS;
-}
-
-void CasteRx1::printData(ostream & ost)
-{
- streamsize nn;
-
- ost << status << " ";
- nn=ost.precision(12); ost << timeStamp << " "; ost.precision(nn);
- ost << wnc << " " << tow << " " << numSatellites << " " << PDOP << " ";
- nn=ost.precision(12); ost << lat << " " << lon << " " << alt << " "; ost.precision(nn);
- ost << xWgs << " " << yWgs << " " << zWgs << " ";
- ost << xMap << " " << yMap << " " << zMap << " ";
- ost << vxWgs << " " << vyWgs << " " << vzWgs << " ";
- ost << vxMap << " " << vyMap << " " << vzMap << " ";
- ost << cxxWgs << " " << cyyWgs << " " << czzWgs << " " << cxyWgs << " ";
- ost << cxxMap << " " << cyyMap << " " << czzMap << " " << cxyMap << " ";
- ost << endl;
-}
-
-void CasteRx1::setMapOrigin(double mapLat0, double mapLon0, double mapAlt0, double mapAlpha0)
-{
- lat0 = mapLat0*M_PI/180.0;
- lon0 = mapLon0*M_PI/180.0;
- alt0 = mapAlt0;
- alpha0 = mapAlpha0*M_PI/180.0;
-
- computeTM();
-}
-
-void CasteRx1::setPortName(string pName)
-{
- if( (status & DEVICE_OPEN) ) //if device is open, port name can't be changed
- {
- cout << "CasteRx1::setPortName(): WARNING! Serial port resource can't be changed when port is open. Keeping old settings: " << config.portName << endl;
- }
- else
- {
- config.portName = pName;
- cout << "CasteRx1::setPortName(): Serial port name setting has changed. New serial port is " << config.portName << endl;
- }
-}
-
-void CasteRx1::setAcquisitionRate(int rateIndex)
-{
- if (status & ACQUISITION_RUNNING)
- {
- cout << "CasteRx1::setAcquisitionRate(): WARNING! Rate can't be changed when acquisition is running. Keeping old settings: " << config.acqPeriod << endl;
- }
- else
- {
- if ( ( rateIndex<ONCHANGE )|| ( rateIndex>SEC10 ) )
- {
- cout << "CasteRx1::setAcquisitionRate(): WARNING! Invalid rate index. Keeping old settings: " << config.acqPeriod << endl;
- }
- else
- {
- config.acqPeriod = acqPeriodValues[rateIndex];
- cout << "CasteRx1::setAcquisitionRate(): Acquisition rate has changed. New rate is " << config.acqPeriod << endl;
- }
- }
-}
-
-void CasteRx1::printTM()
-{
- streamsize nn;
-
- cout << endl << "MAP ORIGIN (GEODETIC) ----------------------------------" << endl;
- nn=cout.precision(15);
- cout << "lat0 = " << lat0 << endl << "lon0 = " << lon0 << endl << "alt0 = " << alt0 << endl << "alpha0 = " << alpha0 << endl;
- cout.precision(nn);
-
- cout << endl << "MATRIX TRANSFORM FROM ECEF to MAP-----------------------" << endl;
- cout << TM[0][0] << '\t' << TM[0][1] << '\t' << TM[0][2] << '\t' << TM[0][3] << endl
- << TM[1][0] << '\t' << TM[1][1] << '\t' << TM[1][2] << '\t' << TM[1][3] << endl
- << TM[2][0] << '\t' << TM[2][1] << '\t' << TM[2][2] << '\t' << TM[2][3] << endl
- << TM[3][0] << '\t' << '\t' << TM[3][1] << '\t' << '\t' << TM[3][2] << '\t' << '\t' << TM[3][3] << endl;
- cout << "--------------------------------------------------------" << endl << endl;
-}
-
-unsigned int CasteRx1::getStatus(){ return status; }
-double CasteRx1::getTimeStamp(){ return timeStamp; }
-unsigned short int CasteRx1::getWnc(){ return wnc; }
-unsigned int CasteRx1::getTow(){ return tow; }
-unsigned int CasteRx1::getNumSatellites(){ return numSatellites; }
-float CasteRx1::getPDOP(){ return PDOP; }
-float CasteRx1::getTDOP(){ return TDOP; }
-float CasteRx1::getHDOP(){ return HDOP; }
-float CasteRx1::getVDOP(){ return VDOP; }
-float CasteRx1::getUndulation(){ return undulation; }
-unsigned short int CasteRx1::getPVTerror(){ return pvtError;}
-
-double CasteRx1::getLat(bool units)
-{
- if ( units == inRADS ) return lat;
- else return lat*180.0/M_PI;
-}
-
-double CasteRx1::getLon(bool units)
-{
- if ( units == inRADS ) return lon;
- else return lon*180.0/M_PI;
-}
-
-double CasteRx1::getAlt(){ return alt; }
-double CasteRx1::getXWgs(){ return xWgs; }
-double CasteRx1::getYWgs(){ return yWgs; }
-double CasteRx1::getZWgs(){ return zWgs; }
-double CasteRx1::getXMap(){ return xMap; }
-double CasteRx1::getYMap(){ return yMap; }
-double CasteRx1::getZMap(){ return zMap; }
-double CasteRx1::getVxWgs(){ return vxWgs; }
-double CasteRx1::getVyWgs(){ return vyWgs; }
-double CasteRx1::getVzWgs(){ return vzWgs; }
-double CasteRx1::getVxMap(){ return vxMap; }
-double CasteRx1::getVyMap(){ return vyMap; }
-double CasteRx1::getVzMap(){ return vzMap; }
-double CasteRx1::getCxxWgs(){ return cxxWgs; }
-double CasteRx1::getCyyWgs(){ return cyyWgs; }
-double CasteRx1::getCzzWgs(){ return czzWgs; }
-double CasteRx1::getCxyWgs(){ return cxyWgs; }
-double CasteRx1::getCxxMap(){ return cxxMap; }
-double CasteRx1::getCyyMap(){ return cyyMap; }
-double CasteRx1::getCzzMap(){ return czzMap; }
-double CasteRx1::getCxyMap(){ return cxyMap; }
-double CasteRx1::getCxxEnu(){ return cxxEnu; }
-double CasteRx1::getCyyEnu(){ return cyyEnu; }
-double CasteRx1::getCzzEnu(){ return czzEnu; }
-double CasteRx1::getCxyEnu(){ return cxyEnu; }
-double CasteRx1::getCxzEnu(){ return cxzEnu; }
-double CasteRx1::getCyzEnu(){ return cyzEnu; }
+ // stop data acquiaition
+ this->stopAcquisition();
+ // close the gps module
+ this->closeDevice();
+ // delete threads and events
+ this->thread_server->detach_thread(gps_process_data_thread_id);
+ this->thread_server->delete_thread(gps_process_data_thread_id);
+ this->gps_process_data_thread_id="";
+ this->event_server->delete_event(this->finish_thread_event_id);
+ this->finish_thread_event_id="";
+ this->event_server->delete_event(this->new_reply_event_id);
+ this->new_reply_event_id="";
+ this->event_server->delete_event(this->new_meas_block_event_id);
+ this->new_meas_block_event_id="";
+ this->event_server->delete_event(this->new_gps_nav_block_event_id);
+ this->new_gps_nav_block_event_id="";
+ this->event_server->delete_event(this->new_gps_alm_block_event_id);
+ this->new_gps_alm_block_event_id="";
+ this->event_server->delete_event(this->new_gps_ion_block_event_id);
+ this->new_gps_ion_block_event_id="";
+ this->event_server->delete_event(this->new_gps_utc_block_event_id);
+ this->new_gps_utc_block_event_id="";
+ this->event_server->delete_event(this->new_pvt_block_event_id);
+ this->new_pvt_block_event_id="";
+ if(this->serialPort!=NULL)
+ {
+ delete this->serialPort;
+ this->serialPort=NULL;
+ }
+}
+
+void CasteRx1::openDevice()
+{
+ TRS232_config serialConfig;
+
+ try
+ {
+ this->serialPort = new CRS232(this->serial_device); //creates a new object to manage serial connection
+ this->serialPort->open((void*)&this->serial_device);
+ serialConfig.baud = 115200;
+ serialConfig.num_bits = 8;
+ serialConfig.parity = none;
+ serialConfig.stop_bits = 1;
+ this->serialPort->config(&serialConfig);
+ // start the GPS processing data thread
+ this->thread_server->start_thread(this->gps_process_data_thread_id);
+ // configure the data streams
+ this->configureStreams();
+ }catch(CCommException &e){
+ if(this->serialPort!=NULL)
+ {
+ delete this->serialPort;
+ this->serialPort=NULL;
+ }
+ }
+}
+
+void CasteRx1::closeDevice()
+{
+ // stop the internal acquisition thread if necessary
+ this->stopAcquisition();
+ // close the serial port
+ if(this->thread_server->get_thread_state(this->gps_process_data_thread_id)==active)
+ {
+ this->event_server->set_event(this->finish_thread_event_id);
+ this->thread_server->end_thread(this->gps_process_data_thread_id);
+ }
+ if(this->serialPort!=NULL)
+ this->serialPort->close();
+}
+
+void CasteRx1::startAcquisition()
+{
+ if(this->thread_server->get_thread_state(this->gps_process_data_thread_id)==active)
+ {
+ if(!this->gps_running)
+ {
+ this->sendCommand("setSBFOutput,Stream1,USB1,Group1,onChange\n");
+ this->sendCommand("setSBFOutput,Stream2,USB1,Group2,onChange\n");
+ this->sendCommand("setSBFOutput,Stream3,USB1,Group3,onChange\n");
+ this->sendCommand("setSBFOutput,Stream4,USB1,Group4,onChange\n");
+ this->gps_running=true;
+ }
+ }
+}
+
+void CasteRx1::stopAcquisition()
+{
+ if(this->gps_running)
+ {
+ this->sendCommand("setSBFOutput,Stream1,USB1,none\n");
+ this->sendCommand("setSBFOutput,Stream2,USB1,none\n");
+ this->sendCommand("setSBFOutput,Stream3,USB1,none\n");
+ this->sendCommand("setSBFOutput,Stream4,USB1,none\n");
+ this->gps_running=false;
+ }
+}
+
+std::string CasteRx1::get_new_meas_data_event_id(void)
+{
+ return this->new_meas_block_event_id;
+}
+
+void CasteRx1::get_meas_data(TMeasEpoch &meas_epoch, TMeasExtra &meas_extra)
+{
+ this->meas_access.enter();
+ meas_epoch=this->current_meas_epoch;
+ meas_extra=this->current_meas_extra;
+ if(this->event_server->event_is_set(this->new_meas_block_event_id))
+ this->event_server->reset_event(this->new_meas_block_event_id);
+ this->meas_access.exit();
+}
+
+std::string CasteRx1::get_new_gps_nav_data_event_id(void)
+{
+ return this->new_gps_nav_block_event_id;
+}
+
+void CasteRx1::get_gps_nav_data(TGPSNav &gps_nav)
+{
+ this->nav_access.enter();
+ gps_nav=this->current_gps_nav;
+ if(this->event_server->event_is_set(this->new_gps_nav_block_event_id))
+ this->event_server->reset_event(this->new_gps_nav_block_event_id);
+ this->nav_access.exit();
+}
+
+std::string CasteRx1::get_new_gps_alm_data_event_id(void)
+{
+ return this->new_gps_alm_block_event_id;
+}
+
+void CasteRx1::get_gps_alm_data(TGPSAlm &gps_alm)
+{
+ this->nav_access.enter();
+ gps_alm=this->current_gps_alm;
+ if(this->event_server->event_is_set(this->new_gps_alm_block_event_id))
+ this->event_server->reset_event(this->new_gps_alm_block_event_id);
+ this->nav_access.exit();
+}
+
+std::string CasteRx1::get_new_gps_ion_data_event_id(void)
+{
+ return this->new_gps_ion_block_event_id;
+}
+
+void CasteRx1::get_gps_ion_data(TGPSIon &gps_ion)
+{
+ this->nav_access.enter();
+ gps_ion=this->current_gps_ion;
+ if(this->event_server->event_is_set(this->new_gps_ion_block_event_id))
+ this->event_server->reset_event(this->new_gps_ion_block_event_id);
+ this->nav_access.exit();
+}
+
+std::string CasteRx1::get_new_gps_utc_data_event_id(void)
+{
+ return this->new_gps_utc_block_event_id;
+}
+
+void CasteRx1::get_gps_utc_data(TGPSUtc &gps_utc)
+{
+ this->nav_access.enter();
+ gps_utc=this->current_gps_utc;
+ if(this->event_server->event_is_set(this->new_gps_utc_block_event_id))
+ this->event_server->reset_event(this->new_gps_utc_block_event_id);
+ this->nav_access.exit();
+}
+
+std::string CasteRx1::get_new_pvt_data_event_id(void)
+{
+ return this->new_pvt_block_event_id;
+}
+void CasteRx1::get_pvt_data(TPVTCartesian &pvt_cart,TPVTGeodetic &pvt_geo)
+{
+ this->pvt_access.enter();
+ pvt_cart=this->current_pvt_cartesian;
+ pvt_geo=this->current_pvt_geodetic;
+ if(this->event_server->event_is_set(this->new_pvt_block_event_id))
+ this->event_server->reset_event(this->new_pvt_block_event_id);
+ this->pvt_access.exit();
+}
+
+void CasteRx1::get_pvt_cov_data(TPVTCov &pvt_pos_cov_cart,TPVTCov &pvt_pos_cov_geo,TPVTCov &pvt_vel_cov_cart,TPVTCov &pvt_vel_cov_geo)
+{
+ this->pvt_access.enter();
+ pvt_pos_cov_cart=this->current_pvt_pos_cov_cartesian;
+ pvt_pos_cov_geo=this->current_pvt_pos_cov_geodetic;
+ pvt_vel_cov_cart=this->current_pvt_vel_cov_cartesian;
+ pvt_vel_cov_geo=this->current_pvt_vel_cov_geodetic;
+ if(this->event_server->event_is_set(this->new_pvt_block_event_id))
+ this->event_server->reset_event(this->new_pvt_block_event_id);
+ this->pvt_access.exit();
+}
+
+void CasteRx1::get_pvt_dop_data(TPVTDOP &pvt_dop)
+{
+ this->pvt_access.enter();
+ pvt_dop=this->current_pvt_dop;
+ if(this->event_server->event_is_set(this->new_pvt_block_event_id))
+ this->event_server->reset_event(this->new_pvt_block_event_id);
+ this->pvt_access.exit();
+}
/******************************* PROTECTED MEMBERS ************************************************/
-int CasteRx1::readNbytes(unsigned char *buffer, unsigned int nn, unsigned int msecTimeOut)
-{
- CEventServer *eventServer=CEventServer::instance();
- string rxEvent;
- list<string> eventList;
- unsigned int bytesRead = 0;
- unsigned int queueSize = 0;
- unsigned int desiredBytes = 0;
- int retValue = BASIC_SUCCESS;
-
- //sets the event list with the reception event from serialPort object
- rxEvent = serialPort->get_rx_event_id();
- eventList.push_back(rxEvent);
-
- //first check if some byte is already in the queue
- queueSize = serialPort->get_num_data();
- if ( queueSize > 0)
- {
- desiredBytes = min( queueSize , (nn-bytesRead) );
- bytesRead += serialPort->read((unsigned char *)(buffer+bytesRead),desiredBytes);
- }
-
- //starts the waiting loop if not all bytes are already read
- while ( (bytesRead<nn) && (retValue == BASIC_SUCCESS) )
- {
- try{
- eventServer->wait_all(eventList, msecTimeOut);//waits for reception event
- }catch(CEventServerException &e)
- {
- cout << e.what() << ". Line: " << __LINE__ << endl;
- retValue = PORT_READ_TIMEOUT;
- }
- if (retValue == BASIC_SUCCESS)
- {
- queueSize = serialPort->get_num_data();
- desiredBytes = min( queueSize , (nn-bytesRead) );
- bytesRead += serialPort->read((unsigned char *)(buffer+bytesRead),desiredBytes);
- }
- }
-
- return retValue;
-}
-
-int CasteRx1::synchronizeHeader(const unsigned char *mark, int msecTimeOut, unsigned int markSize)
-{
- unsigned char cc;
- string readMark, targetMark;
- int retValue = BASIC_SUCCESS;
- unsigned int ii=0;
-
- //sets targetMark
- targetMark.append((const char*)mark,markSize);
-
- //reads mark from comm device
- for (ii=0; ii<markSize; ii++)
- {
- retValue = readNbytes(&cc,1,msecTimeOut);
- readMark.push_back((char)cc);
- }
-
- //continue reading while mark is not reached or while timeout
- while ( (readMark.compare(targetMark) != 0) && (retValue == BASIC_SUCCESS) )
- {
- for (ii=1; ii<markSize; ii++) readMark.at(ii-1) = readMark.at(ii); //left displacement of all bytes
- retValue = readNbytes(&cc,1,msecTimeOut); //gets a new byte
- readMark.at(markSize-1) = (char)cc; //replace the last byte by the new one
- //cout << endl << "readMark: " << readMark << endl << "targetMark: " << targetMark << endl;
- }
-
- return retValue;
-}
-
-int CasteRx1::sendCommand(const string & cmnd)
-{
- int retValue;
- unsigned char replyBuffer[200];
- string receivedReply, expectedReply;
-
- //relax if we concatenate commands. (200 ms were suggested by Septentrio support team in a personal communication). 50ms tested and it seems it is not enough!!!
- usleep(100000);
-
- //sends the command string
- serialPort->write((unsigned char*)cmnd.c_str(),cmnd.size());
-
- //synchronize with the reply header
- retValue = synchronizeHeader((const unsigned char *)"$R",500);
-
- //cout << "CasteRx1. Line: " << __LINE__ << ". Reply Sync achieved" << endl;
- if ( retValue != BASIC_SUCCESS )
- {
- cout << "CasteRx1::sendCommand(). ERROR. Synchronization with repky header $R not achieved after timeout" << endl;
- return retValue; //it will be either COMMTIMEOUT or SERIALCOMMERROR
- }
-
- //reads response up to receive expected length bytes (2 + cmnd.size()). Expected response : "$R: <cmnd>", but $R has been already read
- retValue = readNbytes(&replyBuffer[0],cmnd.size()+2,500);
- if ( retValue != BASIC_SUCCESS )
- {
- cout << "CasteRx1::sendCommand(). ERROR. Expected reply string not received after timeout" << endl;
- return retValue; //it will be either COMMTIMEOUT or SERIALCOMMERROR
- }
-
- //check response
- if ( replyBuffer[0] == '?' )
- {
- cout << "CasteRx1::sendCommand(). ERROR. Unknown command: " << cmnd << endl;
- return ASTERX1_INVALID_COMMAND;
- }
- receivedReply.append((const char*)replyBuffer,cmnd.size()+2);
- receivedReply.append("\n");
- expectedReply = ": ";
- expectedReply.append(cmnd);
- expectedReply.append("\n");
- if ( receivedReply.compare(0, cmnd.size()+1, expectedReply, 0, cmnd.size()+1) == 0) //expected = received
- {
- //cout << "CasteRx1::sendCommand(). Command Successful: " << cmnd << endl;
- return BASIC_SUCCESS;
- }
- else //otherwise
- {
- cout << "CasteRx1::sendCommand(). Error. " << endl;
- cout << " Sent command: " << cmnd << endl;
- cout << " Device Reply: " << receivedReply << endl;
- cout << " Expected Reply: " << expectedReply << endl;
- return ASTERX1_CONFIG_ERROR;
- }
-}
-
-unsigned short int CasteRx1::getUI1(unsigned char *buf)
-{
- //return ( (buf[0] & 0xff) | (0x0 << 8) );
- return (buf[0] & 0x00ff);
-}
-
-unsigned short int CasteRx1::getUI2(unsigned char *buf)
-{
- return ( (buf[0] & 0xff) | ((buf[1]&0xff)<<8) );
-}
-
-unsigned int CasteRx1::getUI4(unsigned char *buf)
-{
- return ( (buf[0]&0xff) | ((buf[1]&0xff)<<8) | ((buf[2]&0xff)<<16) | ((buf[3]&0xff)<<24) );
-}
-
-float CasteRx1::getFloat(unsigned char *buf)
-{
- unsigned int uintL;
- uniFloat uniF;
-
- uintL=(buf[0]&0xff)|((buf[1]&0xff)<<8)|((buf[2]&0xff)<<16)|((buf[3]&0xff)<<24);
- uniF.i = uintL;
-
- return uniF.flt;
-}
-
-double CasteRx1::getDouble(unsigned char *buf)
-{
- unsigned int uintL;
- unsigned long long int uintH;
- uniDouble uniD;
-
- uintL=(buf[0]&0xff)|((buf[1]&0xff)<<8)|((buf[2]&0xff)<<16)|((buf[3]&0xff)<<24);
- uintH=(buf[4]&0xff)|((buf[5]&0xff)<<8)|((buf[6]&0xff)<<16)|((buf[7]&0xff)<<24);
- //uniD.i = uintL|((uintH&0xffff)<<32);
- uniD.i = uintL|uintH<<32;
-
- return uniD.dbl;
-}
-
-int CasteRx1::decodePVTCartesian(unsigned char *sbfMsg)
-{
- unsigned short int mode;//PVT computation mode (e.g. standAlone, fixed, differential, rtk,...)
-
- tow = getUI4(&sbfMsg[0]);
- wnc = getUI2(&sbfMsg[4]);
- mode = getUI1(&sbfMsg[6]);
- pvtError = getUI1(&sbfMsg[7]);
- if ( pvtError!=0 ) //GPS error to obtain position (this is not an acquisition error)
- {
- //cout << "CasteRx1::decodePVTCartesian(): PVT_ERROR: " << pvtError << endl;
- }
- else
- {
- xWgs = getDouble(&sbfMsg[8]);
- yWgs = getDouble(&sbfMsg[16]);
- zWgs = getDouble(&sbfMsg[24]);
- undulation = (double) getFloat(&sbfMsg[32]);
- vxWgs = (double) getFloat(&sbfMsg[36]);
- vyWgs = (double) getFloat(&sbfMsg[40]);
- vzWgs = (double) getFloat(&sbfMsg[44]);
- numSatellites = getUI1(&sbfMsg[66]);
- }
-
- //cout << "CasteRx1::decodePVTCartesian()" << endl;
- return BASIC_SUCCESS;
-}
-
-int CasteRx1::decodePVTGeodetic(unsigned char *sbfMsg)
-{
- unsigned short int mode;//PVT computation mode (e.g. standAlone, fixed, differential, rtk,...)
-
- tow = getUI4(&sbfMsg[0]);
- wnc = getUI2(&sbfMsg[4]);
- mode = getUI1(&sbfMsg[6]);
- pvtError = getUI1(&sbfMsg[7]);
- if ( pvtError!=0 ) //GPS error to obtain position (this is not an acquisition error)
- {
- //cout << "CasteRx1::decodePVTGeodetic(): PVT_ERROR: " << pvtError << endl;
- }
- else
- {
- lat = getDouble(&sbfMsg[8]);
- lon = getDouble(&sbfMsg[16]);
- alt = getDouble(&sbfMsg[24]);
- }
-
- //cout << "CasteRx1::decodePVTGeodetic()" << endl;
- return BASIC_SUCCESS;
-}
-
-int CasteRx1::decodePosCovCartesian(unsigned char *sbfMsg)
-{
- unsigned short int mode;//PVT computation mode (e.g. standAlone, fixed, differential, rtk,...)
-
- tow = getUI4(&sbfMsg[0]);
- wnc = getUI2(&sbfMsg[4]);
- mode = getUI1(&sbfMsg[6]);
- pvtError = getUI1(&sbfMsg[7]);
- if ( pvtError!=0 ) //GPS error to obtain position (this is not an acquisition error)
- {
- //cout << "CasteRx1::decodePosCovCartesian(): PVT_ERROR: " << pvtError << endl;
- }
- else
- {
- cxxWgs = (double) getFloat(&sbfMsg[8]);
- cyyWgs = (double) getFloat(&sbfMsg[12]);
- czzWgs = (double) getFloat(&sbfMsg[16]);
- cxyWgs = (double) getFloat(&sbfMsg[24]);
- cxzWgs = (double) getFloat(&sbfMsg[28]);
- cyzWgs = (double) getFloat(&sbfMsg[36]);
- }
-
- //cout << "CasteRx1::decodePosCovCartesian()" << endl;
- return BASIC_SUCCESS;
-}
-
-int CasteRx1::decodeDOP(unsigned char *sbfMsg)
-{
- unsigned char nrsv;//total number of satellites for DOP computation
-
- tow = getUI4(&sbfMsg[0]);
- wnc = getUI2(&sbfMsg[4]);
- nrsv = getUI1(&sbfMsg[6]);
-
- PDOP = (double)getUI2(&sbfMsg[8])/100.0;
- TDOP = (double)getUI2(&sbfMsg[10])/100.0;
- HDOP = (double)getUI2(&sbfMsg[12])/100.0;
- VDOP = (double)getUI2(&sbfMsg[14])/100.0;
-
- //cout << "CasteRx1::decodeDOP()" << endl;
- return BASIC_SUCCESS;
-}
-
-//unsigned short CasteRx1::CRC_compute16CCITT(const void *buf, size_t buf_length)
-unsigned short CasteRx1::CRC_compute16CCITT(unsigned char *buf, unsigned short int nBytes)
-{
- unsigned int ii;
- unsigned short crc = 0;
- //const unsigned char *buf8 = buf; /* Convert the type to access by byte. */
-
- /* see for example the BINEX web site */
- for (ii=0; ii<nBytes; ii++) {
- crc = (crc << 8) ^ CRC_16CCIT_LookUp[ (crc >> 8) ^ buf[ii] ];
+void *CasteRx1::gps_process_data_thread(void *params)
+{
+ unsigned int event_id,num,i,length=0;
+ CasteRx1 *gps=(CasteRx1 *)params;
+ static gps_state_t state=header1;
+ std::list<std::string> events;
+ unsigned char *data,*sbf_data;
+ std::string reply;
+ bool end=false;
+
+ events.push_back(gps->finish_thread_event_id);
+ events.push_back(gps->serialPort->get_rx_event_id());
+ while(!end)
+ {
+ event_id=gps->event_server->wait_first(events);
+ if(event_id==0)// finish the thread
+ end=true;
+ else if(event_id==1)// new data available
+ {
+ num=gps->serialPort->get_num_data();
+ if(data!=NULL)
+ delete[] data;
+ data=new unsigned char[num];
+ gps->serialPort->read(data,num);
+ for(i=0;i<num;i++)
+ {
+ switch(state)
+ {
+ case header1: if(data[i]=='$')
+ state=header2;
+ else
+ state=header1;
+ break;
+ case header2: if(data[i]=='R')// answer to a command
+ state=get_cmd_reply;
+ else if(data[i]=='@')// gps data
+ {
+ state=get_block_header;
+ length=2;
+ gps->current_header.sync[0]='$';
+ gps->current_header.sync[1]='@';
+ }
+ break;
+ case get_cmd_reply: if(data[i]=='\r')
+ {
+ // signal that an answer is available
+ gps->reply_queue.push(reply);
+ if(!gps->event_server->event_is_set(gps->new_reply_event_id))
+ gps->event_server->set_event(gps->new_reply_event_id);
+ reply="";
+ state=header1;
+ }
+ else// add the data to the answer string
+ {
+ reply.push_back(data[i]);
+ state=get_cmd_reply;
+ }
+ break;
+ case get_block_header: ((unsigned char *)&gps->current_header)[length]=data[i];
+ length++;
+ if(length==sizeof(TGPSBlockHeader))
+ {
+ if((gps->current_header.length%4)!=0)// length must be a multiple of 4
+ state=header1;
+ else
+ {
+ if(sbf_data!=NULL)
+ delete sbf_data;
+ sbf_data=new unsigned char[gps->current_header.length];
+ state=get_sbf_data;
+ }
+ }
+ break;
+ case get_sbf_data: sbf_data[length-sizeof(TGPSBlockHeader)]=data[i];
+ length++;
+ if(length==gps->current_header.length)
+ {
+ /* compute CRC and check it */
+ if(gps->compute_crc(sbf_data,length-8)==gps->current_header.crc)// invalid Block CRC
+ gps->process_block(sbf_data,length-8);
+ else
+ std::cout << "CRC error" << std::endl;
+ delete[] sbf_data;
+ sbf_data=0;
+ state=header1;
+ }
+ break;
+ }
+ }
+ delete[] data;
+ data=NULL;
+ }
+ }
+ pthread_exit(NULL);
+}
+
+void CasteRx1::process_block(unsigned char *data,unsigned short int length)
+{
+ switch(this->current_header.id.id)
+ {
+ case PVTCartesian_ID: //std::cout << "PVTCartesian_ID" << std::endl;
+ this->process_pvt_cartesian(data,length);
+ break;
+ case PVTGeodetic_ID: //std::cout << "PVTGeodetic_ID" << std::endl;
+ this->process_pvt_geodetic(data,length);
+ break;
+ case PosCovCartesian_ID: //std::cout << "PosCovCartesian_ID" << std::endl;
+ this->process_pos_cov_cartesian(data,length);
+ break;
+ case PosCovGeodetic_ID: //std::cout << "PosCovGeodetic_ID" << std::endl;
+ this->process_pos_cov_geodetic(data,length);
+ break;
+ case VelCovCartesian_ID: //std::cout << "VelCovCartesian_ID" << std::endl;
+ this->process_vel_cov_cartesian(data,length);
+ break;
+ case VelCovGeodetic_ID: //std::cout << "VelCovGeodetic_ID" << std::endl;
+ this->process_vel_cov_geodetic(data,length);
+ break;
+ case DOP_ID: //std::cout << "DOP_ID" << std::endl;
+ this->process_dop(data,length);
+ break;
+ case EndOfPVT_ID: //std::cout << "EndofPVT_ID" << std::endl;
+ this->process_end_of_pvt(data,length);
+ break;
+ case MeasEpoch_ID: //std::cout << "MeasEpoch_ID" << std::endl;
+ this->process_meas_epoch(data,length);
+ break;
+ case MeasExtra_ID: //std::cout << "MeasExtra_ID" << std::endl;
+ this->process_meas_extra(data,length);
+ break;
+ case EndOfMeas_ID: //std::cout << "EndofMeas_ID" << std::endl;
+ this->process_end_of_meas(data,length);
+ break;
+ case GPSNav_ID: //std::cout << "GPSNav_ID" << std::endl;
+ this->process_gps_nav(data,length);
+ break;
+ case GPSAlm_ID: //std::cout << "GPSAlm_ID" << std::endl;
+ this->process_gps_alm(data,length);
+ break;
+ case GPSIon_ID: //std::cout << "GPSIon_ID" << std::endl;
+ this->process_gps_ion(data,length);
+ break;
+ case GPSUtc_ID: //std::cout << "GPSUtc_ID" << std::endl;
+ this->process_gps_utc(data,length);
+ break;
+ case ReceiverStatus_ID: //std::cout << "ReceiverStatus_ID" << std::endl;
+ this->process_receiver_status(data,length);
+ break;
+ case ChannelStatus_ID: //std::cout << "ChannelStatus_ID" << std::endl;
+ this->process_channel_status(data,length);
+ break;
+ case SatVisibility_ID: //std::cout << "SatVisibility_ID" << std::endl;
+ this->process_sat_visibility(data,length);
+ break;
+ default: std::cout << "Unknown block ID: " << this->current_header.id.id << std::endl;
+ break;
+ }
+}
+
+void CasteRx1::process_channel_status(unsigned char *data,unsigned short int length)
+{
+ TGPSChannelStatus gps_channel_status;
+ TGPSChannelSatInfo gps_sat_info;
+ TGPSChannelStateInfo gps_state_info;
+ unsigned short int index=0,i,j;
+ TChannelStatus channel_status;
+ TChannelSatInfo sat_info;
+ TChannelStateInfo state_info;
+
+ memcpy((unsigned char *)&gps_channel_status,data,sizeof(TGPSChannelStatus));
+ index+=sizeof(TGPSChannelStatus);
+ channel_status.gps_timestamp.wnc=gps_channel_status.timestamp.wnc;
+ channel_status.gps_timestamp.tow=gps_channel_status.timestamp.tow;
+ channel_status.local_timestamp=this->local_time.getTimeInSeconds();
+ channel_status.info.clear();
+ for(i=0;i<gps_channel_status.num_ch;i++)
+ {
+ memcpy((unsigned char *)&gps_sat_info,&data[index],sizeof(TGPSChannelSatInfo));
+ index+=gps_channel_status.length_ch_sat_info;
+ sat_info.sat_id=gps_sat_info.sat_id;
+ sat_info.azimuth=gps_sat_info.azimuth_rise_set.azimuth;
+ sat_info.rise_set=(rise_set_t)gps_sat_info.azimuth_rise_set.rise_set;
+ sat_info.health=(health_t)gps_sat_info.health;
+ sat_info.elevation=gps_sat_info.elevation;
+ sat_info.state.clear();
+ for(j=0;j<gps_sat_info.num_state_info;j++)
+ {
+ memcpy((unsigned char *)&gps_state_info,&data[index],sizeof(TGPSChannelStateInfo));
+ index+=gps_channel_status.length_ch_state_info;
+ state_info.antenna_id=(antenna_t)gps_state_info.antenna;
+ state_info.track_status=(track_status_t)gps_state_info.tracking_status;
+ state_info.pvt_status=(pvt_status_t)gps_state_info.pvt_status;
+ sat_info.state.push_back(state_info);
+ }
+ }
+}
+
+void CasteRx1::process_receiver_status(unsigned char *data,unsigned short int length)
+{
+ TGPSReceiverStatus gps_receiver_status;
+ unsigned short int index=0,i;
+ TGPSAGCData gps_agc_data;
+ TAGCData agc_data;
+
+ memcpy((unsigned char *)&gps_receiver_status,data,sizeof(TGPSReceiverStatus));
+ index+=sizeof(TGPSReceiverStatus);
+ this->receiver_status.gps_timestamp.wnc=gps_receiver_status.timestamp.wnc;
+ this->receiver_status.gps_timestamp.tow=gps_receiver_status.timestamp.tow;
+ this->receiver_status.local_timestamp=this->local_time.getTimeInSeconds();
+ this->receiver_status.cpu_load=gps_receiver_status.cpu_load;
+ this->receiver_status.up_time=gps_receiver_status.up_time;
+ // state flags
+ this->receiver_status.state=*((int*)&gps_receiver_status.rx_state);
+ this->receiver_status.error=*((int*)&gps_receiver_status.rx_error);
+ this->receiver_status.agc.clear();
+ for(i=0;i<gps_receiver_status.num_agc_states;i++)
+ {
+ memcpy((unsigned char *)&gps_agc_data,&data[index],sizeof(TGPSAGCData));
+ index+=gps_receiver_status.length_agc_states;
+ agc_data.frontend_id=(frontend_t)gps_agc_data.frontend_id.code;
+ agc_data.antenna_id=(antenna_t)gps_agc_data.frontend_id.antenna_id;
+ agc_data.gain=gps_agc_data.gain;
+ agc_data.sample_var=gps_agc_data.sample_var;
+ agc_data.blanking_stat=gps_agc_data.blanking_stat;
+ this->receiver_status.agc.push_back(agc_data);
+ }
+}
+
+void CasteRx1::process_sat_visibility(unsigned char *data,unsigned short int length)
+{
+ TGPSSatVisibility gps_sat_visibility;
+ unsigned short int index=0,i;
+ TGPSSatInfo gps_sat_info;
+ TSatInfo sat_info;
+
+ memcpy((unsigned char *)&gps_sat_visibility,data,sizeof(TGPSSatVisibility));
+ index+=sizeof(TGPSSatVisibility);
+ for(i=0;i<gps_sat_visibility.num_sat;i++)
+ {
+ memcpy((unsigned char *)&gps_sat_info,&data[index],sizeof(TGPSSatInfo));
+ index+=gps_sat_visibility.length_sat;
+ sat_info.gps_timestamp.wnc=gps_sat_visibility.timestamp.wnc;
+ sat_info.gps_timestamp.tow=gps_sat_visibility.timestamp.tow;
+ sat_info.local_timestamp=this->local_time.getTimeInSeconds();
+ sat_info.sat_id=gps_sat_info.sat_id;
+ sat_info.azimuth=gps_sat_info.azimuth*0.01;
+ sat_info.elevation=gps_sat_info.elevation*0.01;
+ sat_info.rise_set=(rise_set_t)gps_sat_info.rise_set;
+ sat_info.sat_info=(sat_info_t)gps_sat_info.sat_info;
+ }
+}
+
+void CasteRx1::process_gps_nav(unsigned char *data,unsigned short int length)
+{
+ TGPSGPSNav gps_gps_nav;
+
+ this->nav_access.enter();
+ memcpy((unsigned char *)&gps_gps_nav,data,sizeof(TGPSGPSNav));
+ this->current_gps_nav.gps_timestamp.wnc=gps_gps_nav.timestamp.wnc;
+ this->current_gps_nav.gps_timestamp.tow=gps_gps_nav.timestamp.tow;
+ this->current_gps_nav.local_timestamp=this->local_time.getTimeInSeconds();
+ this->current_gps_nav.sat_id=gps_gps_nav.prn;
+ this->current_gps_nav.wn=gps_gps_nav.wn;
+ this->current_gps_nav.ca_or_pon_l2=gps_gps_nav.ca_or_pon_l2;
+ this->current_gps_nav.ura=gps_gps_nav.ura;
+ this->current_gps_nav.health=gps_gps_nav.health;
+ this->current_gps_nav.l2_data_flag=gps_gps_nav.l2_data_flag;
+ this->current_gps_nav.iodc=gps_gps_nav.iodc;
+ this->current_gps_nav.iode2=gps_gps_nav.iode2;
+ this->current_gps_nav.iode3=gps_gps_nav.iode3;
+ this->current_gps_nav.fit_int_flag=gps_gps_nav.fit_int_flag;
+ this->current_gps_nav.t_gd=gps_gps_nav.t_gd;
+ this->current_gps_nav.t_oc=gps_gps_nav.t_oc;
+ this->current_gps_nav.a_f2=gps_gps_nav.a_f2;
+ this->current_gps_nav.a_f1=gps_gps_nav.a_f1;
+ this->current_gps_nav.a_f0=gps_gps_nav.a_f0;
+ this->current_gps_nav.c_rs=gps_gps_nav.c_rs;
+ this->current_gps_nav.delta_n=gps_gps_nav.delta_n;
+ this->current_gps_nav.m_0=gps_gps_nav.m_0;
+ this->current_gps_nav.c_uc=gps_gps_nav.c_uc;
+ this->current_gps_nav.e=gps_gps_nav.e;
+ this->current_gps_nav.c_us=gps_gps_nav.c_us;
+ this->current_gps_nav.sqrt_a=gps_gps_nav.sqrt_a;
+ this->current_gps_nav.t_oe=gps_gps_nav.t_oe;
+ this->current_gps_nav.c_ic=gps_gps_nav.c_ic;
+ this->current_gps_nav.omega_0=gps_gps_nav.omega_0;
+ this->current_gps_nav.c_is=gps_gps_nav.c_is;
+ this->current_gps_nav.i_0=gps_gps_nav.i_0;
+ this->current_gps_nav.c_rc=gps_gps_nav.c_rc;
+ this->current_gps_nav.omega=gps_gps_nav.omega;
+ this->current_gps_nav.omega_dot=gps_gps_nav.omega_dot;
+ this->current_gps_nav.i_dot=gps_gps_nav.i_dot;
+ this->current_gps_nav.wnt_oc=gps_gps_nav.wnt_oc;
+ this->current_gps_nav.wnt_oe=gps_gps_nav.wnt_oe;
+ if(!this->event_server->event_is_set(this->new_gps_nav_block_event_id))
+ this->event_server->set_event(new_gps_nav_block_event_id);
+ this->nav_access.exit();
+}
+
+void CasteRx1::process_gps_alm(unsigned char *data,unsigned short int length)
+{
+ TGPSGPSAlm gps_gps_alm;
+
+ this->nav_access.enter();
+ memcpy((unsigned char *)&gps_gps_alm,data,sizeof(TGPSGPSAlm));
+ this->current_gps_alm.gps_timestamp.wnc=gps_gps_alm.timestamp.wnc;
+ this->current_gps_alm.gps_timestamp.tow=gps_gps_alm.timestamp.tow;
+ this->current_gps_alm.local_timestamp=this->local_time.getTimeInSeconds();
+ this->current_gps_alm.sat_id=gps_gps_alm.prn;
+ this->current_gps_alm.e=gps_gps_alm.e;
+ this->current_gps_alm.t_oa=gps_gps_alm.t_oa;
+ this->current_gps_alm.delta_i=gps_gps_alm.delta_i;
+ this->current_gps_alm.omega_dot=gps_gps_alm.omega_dot;
+ this->current_gps_alm.sqrt_a=gps_gps_alm.sqrt_a;
+ this->current_gps_alm.omega_0=gps_gps_alm.omega_0;
+ this->current_gps_alm.omega=gps_gps_alm.omega;
+ this->current_gps_alm.m_0=gps_gps_alm.m_0;
+ this->current_gps_alm.a_f1=gps_gps_alm.a_f1;
+ this->current_gps_alm.a_f0=gps_gps_alm.a_f0;
+ this->current_gps_alm.wn_a=gps_gps_alm.wn_a;
+ this->current_gps_alm.as_config=gps_gps_alm.as_config;
+ this->current_gps_alm.health8=gps_gps_alm.health8;
+ this->current_gps_alm.health6=gps_gps_alm.health6;
+ if(!this->event_server->event_is_set(this->new_gps_alm_block_event_id))
+ this->event_server->set_event(new_gps_alm_block_event_id);
+ this->nav_access.exit();
+}
+
+void CasteRx1::process_gps_ion(unsigned char *data,unsigned short int length)
+{
+ TGPSGPSIon gps_gps_ion;
+
+ this->nav_access.enter();
+ memcpy((unsigned char *)&gps_gps_ion,data,sizeof(TGPSGPSIon));
+ this->current_gps_ion.gps_timestamp.wnc=gps_gps_ion.timestamp.wnc;
+ this->current_gps_ion.gps_timestamp.tow=gps_gps_ion.timestamp.tow;
+ this->current_gps_ion.local_timestamp=this->local_time.getTimeInSeconds();
+ this->current_gps_ion.sat_id=gps_gps_ion.prn;
+ this->current_gps_ion.alpha_0=gps_gps_ion.alpha_0;
+ this->current_gps_ion.alpha_1=gps_gps_ion.alpha_1;
+ this->current_gps_ion.alpha_2=gps_gps_ion.alpha_2;
+ this->current_gps_ion.alpha_3=gps_gps_ion.alpha_3;
+ this->current_gps_ion.beta_0=gps_gps_ion.beta_0;
+ this->current_gps_ion.beta_1=gps_gps_ion.beta_1;
+ this->current_gps_ion.beta_2=gps_gps_ion.beta_2;
+ this->current_gps_ion.beta_3=gps_gps_ion.beta_3;
+ if(!this->event_server->event_is_set(this->new_gps_ion_block_event_id))
+ this->event_server->set_event(new_gps_ion_block_event_id);
+ this->nav_access.exit();
+}
+
+void CasteRx1::process_gps_utc(unsigned char *data,unsigned short int length)
+{
+ TGPSGPSUtc gps_gps_utc;
+
+ this->nav_access.enter();
+ memcpy((unsigned char *)&gps_gps_utc,data,sizeof(TGPSGPSUtc));
+ this->current_gps_utc.gps_timestamp.wnc=gps_gps_utc.timestamp.wnc;
+ this->current_gps_utc.gps_timestamp.tow=gps_gps_utc.timestamp.tow;
+ this->current_gps_utc.local_timestamp=this->local_time.getTimeInSeconds();
+ this->current_gps_utc.sat_id=gps_gps_utc.prn;
+ this->current_gps_utc.a_1=gps_gps_utc.a_1;
+ this->current_gps_utc.a_0=gps_gps_utc.a_0;
+ this->current_gps_utc.t_ot=gps_gps_utc.t_ot;
+ this->current_gps_utc.wn_t=gps_gps_utc.wn_t;
+ this->current_gps_utc.del_t_ls=gps_gps_utc.del_t_ls;
+ this->current_gps_utc.wn_lsf=gps_gps_utc.wn_lsf;
+ this->current_gps_utc.dn=gps_gps_utc.dn;
+ this->current_gps_utc.del_t_lsf=gps_gps_utc.del_t_lsf;
+ if(!this->event_server->event_is_set(this->new_gps_utc_block_event_id))
+ this->event_server->set_event(new_gps_utc_block_event_id);
+ this->nav_access.exit();
+}
+
+void CasteRx1::process_meas_epoch(unsigned char *data,unsigned short int length)
+{
+ TGPSMeasEpoch gps_meas_epoch;
+ TGPSMeasBlockType1 gps_meas_epoch_type1;
+ TGPSMeasBlockType2 gps_meas_epoch_type2;
+ unsigned short int index=0,i,j;
+ TMeasEpochType1 meas_epoch_type1;
+ TMeasEpochType2 meas_epoch_type2;
+
+ this->meas_access.enter();
+ memcpy((unsigned char *)&gps_meas_epoch,data,sizeof(TGPSMeasEpoch));
+ index+=sizeof(TGPSMeasEpoch);
+ this->current_meas_epoch.gps_timestamp.wnc=gps_meas_epoch.timestamp.wnc;
+ this->current_meas_epoch.gps_timestamp.tow=gps_meas_epoch.timestamp.tow;
+ this->current_meas_epoch.local_timestamp=this->local_time.getTimeInSeconds();
+ this->current_meas_epoch.flags=gps_meas_epoch.common_flags;
+ this->current_meas_epoch.type1_data.clear();
+ for(i=0;i<gps_meas_epoch.num_type1;i++)
+ {
+ memcpy((unsigned char *)&gps_meas_epoch_type1,&data[index],sizeof(TGPSMeasBlockType1));
+ index+=gps_meas_epoch.length_type1;
+ meas_epoch_type1.rx_channel=gps_meas_epoch_type1.rx_channel;
+ meas_epoch_type1.signal_type=(signal_t)gps_meas_epoch_type1.type.master_signal_type;
+ meas_epoch_type1.antenna_id=(antenna_t)gps_meas_epoch_type1.type.antenna_id;
+ meas_epoch_type1.sat_id=gps_meas_epoch_type1.svid;
+ if(gps_meas_epoch_type1.code.msb==0 && gps_meas_epoch_type1.code.lsb==0)// the pseudo range is not valid
+ meas_epoch_type1.pseudo_range=0.0;
+ else
+ meas_epoch_type1.pseudo_range=((double)gps_meas_epoch_type1.code.msb)*4294967.296+((double)gps_meas_epoch_type1.code.lsb)/1000.0;
+ meas_epoch_type1.doppler=gps_meas_epoch_type1.doppler*0.0001;
+ if(gps_meas_epoch_type1.carrier.msb==-128 && gps_meas_epoch_type1.carrier.lsb==0)// the carrier phase is not valid
+ meas_epoch_type1.carrier_phase=0.0;
+ else
+ meas_epoch_type1.carrier_phase=((double)gps_meas_epoch_type1.carrier.msb)*65.536+((double)gps_meas_epoch_type1.carrier.lsb)/1000.0;
+ meas_epoch_type1.cn0=((double)gps_meas_epoch_type1.cn0)*0.25;
+ if(meas_epoch_type1.signal_type==GPS_L1_PY || meas_epoch_type1.signal_type==GPS_L2_PY)
+ meas_epoch_type1.cn0+=10;
+ meas_epoch_type1.lock_time=gps_meas_epoch_type1.lock_time;
+ if(gps_meas_epoch_type1.info.smoothed==1)
+ meas_epoch_type1.pseudorange_smoothed=true;
+ else
+ meas_epoch_type1.pseudorange_smoothed=false;
+ if(gps_meas_epoch_type1.info.smoothing_int_reached==1)
+ meas_epoch_type1.smoothing_int_reached=true;
+ else
+ meas_epoch_type1.smoothing_int_reached=false;
+ if(gps_meas_epoch_type1.info.phase_ambiguity==1)
+ meas_epoch_type1.half_cycle_ambiguity=true;
+ else
+ meas_epoch_type1.half_cycle_ambiguity=false;
+ meas_epoch_type1.type2_data.clear();
+ for(j=0;j<gps_meas_epoch_type1.num_type2;j++)
+ {
+ memcpy((unsigned char *)&gps_meas_epoch_type2,&data[index],sizeof(TGPSMeasBlockType2));
+ index+=gps_meas_epoch.length_type2;
+ meas_epoch_type2.signal_type=(signal_t)gps_meas_epoch_type2.type.master_signal_type;
+ meas_epoch_type2.antenna_id=(antenna_t)gps_meas_epoch_type2.type.antenna_id;
+ meas_epoch_type2.lock_time=gps_meas_epoch_type2.lock_time;
+ meas_epoch_type2.cn0=((double)gps_meas_epoch_type2.cn0)*0.25;
+ if(meas_epoch_type2.signal_type==GPS_L1_PY || meas_epoch_type2.signal_type==GPS_L2_PY)
+ meas_epoch_type2.cn0+=10;
+ if(gps_meas_epoch_type2.offsets_msb.code_offset_msb==-4 && gps_meas_epoch_type2.code_offset_lsb==0)// the pseudo range is not valid
+ meas_epoch_type2.pseudo_range=0.0;
+ else
+ meas_epoch_type2.pseudo_range=((double)gps_meas_epoch_type2.offsets_msb.code_offset_msb)*65.536+((double)gps_meas_epoch_type2.code_offset_lsb)/1000.0;
+ if(gps_meas_epoch_type2.offsets_msb.doppler_offset_msb==-128 && gps_meas_epoch_type2.doppler_offset_lsb==0)// the doppler is not valid
+ meas_epoch_type2.doppler=0.0;
+ else
+ meas_epoch_type2.doppler=((double)gps_meas_epoch_type2.offsets_msb.doppler_offset_msb)*6.5536+((double)gps_meas_epoch_type2.doppler_offset_lsb)/10000.0;
+ if(gps_meas_epoch_type2.carrier_msb==-16 && gps_meas_epoch_type2.carrier_lsb==0)// the carrier phase is not valid
+ meas_epoch_type2.carrier_phase=0.0;
+ else
+ meas_epoch_type2.carrier_phase=((double)gps_meas_epoch_type2.carrier_msb)*65.536+((double)gps_meas_epoch_type2.carrier_lsb)/1000.0;
+ if(gps_meas_epoch_type2.info.smoothed==1)
+ meas_epoch_type2.pseudorange_smoothed=true;
+ else
+ meas_epoch_type2.pseudorange_smoothed=false;
+ if(gps_meas_epoch_type2.info.smoothing_int_reached==1)
+ meas_epoch_type2.smoothing_int_reached=true;
+ else
+ meas_epoch_type2.smoothing_int_reached=false;
+ if(gps_meas_epoch_type2.info.phase_ambiguity==1)
+ meas_epoch_type2.half_cycle_ambiguity=true;
+ else
+ meas_epoch_type2.half_cycle_ambiguity=false;
+ meas_epoch_type1.type2_data.push_back(meas_epoch_type2);
+ }
+ this->current_meas_epoch.type1_data.push_back(meas_epoch_type1);
}
+ this->meas_access.exit();
+}
+
+void CasteRx1::process_meas_extra(unsigned char *data,unsigned short int length)
+{
+ TGPSMeasExtra gps_meas_extra;
+ TGPSMeasExtraBlock gps_meas_extra_block;
+ unsigned short int index=0,i;
+ TMeasExtraBlock meas_extra_block;
+
+ this->meas_access.enter();
+ memcpy((unsigned char *)&gps_meas_extra,data,sizeof(TGPSMeasExtra));
+ index+=sizeof(TGPSMeasExtra);
+ this->current_meas_extra.gps_timestamp.wnc=gps_meas_extra.timestamp.wnc;
+ this->current_meas_extra.gps_timestamp.tow=gps_meas_extra.timestamp.tow;
+ this->current_meas_extra.local_timestamp=this->local_time.getTimeInSeconds();
+ this->current_meas_extra.doppler_var_factor=gps_meas_extra.doppler_var_factor;
+ this->current_meas_extra.blocks.clear();
+ for(i=0;i<gps_meas_extra.num_blocks;i++)
+ {
+ memcpy((unsigned char *)&gps_meas_extra_block,&data[index],sizeof(TGPSMeasExtraBlock));
+ index+=gps_meas_extra.length_blocks;
+ meas_extra_block.rx_channel=gps_meas_extra_block.rx_channel;
+ meas_extra_block.signal_type=(signal_t)gps_meas_extra_block.type.master_signal_type;
+ meas_extra_block.antenna_id=(antenna_t)gps_meas_extra_block.type.antenna_id;
+ meas_extra_block.mp_correction=((double)gps_meas_extra_block.mp_correction)/1000.0;
+ meas_extra_block.smoothing_correction=((double)gps_meas_extra_block.smoothing_correction)/1000.0;
+ meas_extra_block.code_var=((double)gps_meas_extra_block.code_var)/10000.0;
+ meas_extra_block.carrier_var=gps_meas_extra_block.carrier_var;
+ meas_extra_block.lock_time=gps_meas_extra_block.lock_time;
+ meas_extra_block.cum_lost_cont=gps_meas_extra_block.cum_lost_cont;
+ this->current_meas_extra.blocks.push_back(meas_extra_block);
+ }
+ this->meas_access.exit();
+}
+
+void CasteRx1::process_end_of_meas(unsigned char *data,unsigned short int length)
+{
+ TGPSEndOfMeas gps_end_of_meas;
+
+ memcpy((unsigned char *)&gps_end_of_meas,data,sizeof(TGPSEndOfMeas));
+ if(!this->event_server->event_is_set(this->new_meas_block_event_id))
+ this->event_server->set_event(new_meas_block_event_id);
+}
+
+void CasteRx1::process_pvt_cartesian(unsigned char *data,unsigned short int length)
+{
+ TGPSPVTCartesian gps_pvt_cartesian;
+
+ this->pvt_access.enter();
+ memcpy((unsigned char *)&gps_pvt_cartesian,data,sizeof(TGPSPVTCartesian));
+ this->current_pvt_cartesian.gps_timestamp.wnc=gps_pvt_cartesian.timestamp.wnc;
+ this->current_pvt_cartesian.gps_timestamp.tow=gps_pvt_cartesian.timestamp.tow;
+ this->current_pvt_cartesian.local_timestamp=this->local_time.getTimeInSeconds();
+ this->current_pvt_cartesian.mode=(pvt_mode_t)gps_pvt_cartesian.mode.mode;
+ this->current_pvt_cartesian.solution=(solution_t)gps_pvt_cartesian.mode.solution_type;
+ this->current_pvt_cartesian.error=gps_pvt_cartesian.error;
+ this->current_pvt_cartesian.x=gps_pvt_cartesian.x;
+ this->current_pvt_cartesian.y=gps_pvt_cartesian.y;
+ this->current_pvt_cartesian.z=gps_pvt_cartesian.z;
+ this->current_pvt_cartesian.undulation=gps_pvt_cartesian.undulation;
+ this->current_pvt_cartesian.vx=gps_pvt_cartesian.vx;
+ this->current_pvt_cartesian.vy=gps_pvt_cartesian.vy;
+ this->current_pvt_cartesian.vz=gps_pvt_cartesian.vz;
+ this->current_pvt_cartesian.cog=gps_pvt_cartesian.cog;
+ this->current_pvt_cartesian.rx_clk_bias=gps_pvt_cartesian.rx_clk_bias;
+ this->current_pvt_cartesian.rx_clk_drift=gps_pvt_cartesian.rx_clk_drift;
+ this->current_pvt_cartesian.time_system=(sys_time_t)gps_pvt_cartesian.time_system;
+ this->current_pvt_cartesian.datum=(datum_t)gps_pvt_cartesian.datum;
+ this->current_pvt_cartesian.num_sat=gps_pvt_cartesian.num_sat;
+ this->current_pvt_cartesian.wa_corr_info=gps_pvt_cartesian.wa_corr_info;
+ if(gps_pvt_cartesian.corr_info.orbit_clock_corr==1)
+ this->current_pvt_cartesian.orbit_sat_clock_corr=true;
+ else
+ this->current_pvt_cartesian.orbit_sat_clock_corr=false;
+ if(gps_pvt_cartesian.corr_info.range_corr==1)
+ this->current_pvt_cartesian.range_corr_info=true;
+ else
+ this->current_pvt_cartesian.range_corr_info=false;
+ if(gps_pvt_cartesian.corr_info.ion_info==1)
+ this->current_pvt_cartesian.ion_info=true;
+ else
+ this->current_pvt_cartesian.ion_info=false;
+ if(gps_pvt_cartesian.corr_info.orbit_acc_info==1)
+ this->current_pvt_cartesian.orbit_accuracy_info=true;
+ else
+ this->current_pvt_cartesian.orbit_accuracy_info=false;
+ if(gps_pvt_cartesian.corr_info.do229_mode==1)
+ this->current_pvt_cartesian.DO299_used=true;
+ else
+ this->current_pvt_cartesian.DO299_used=false;
+ this->current_pvt_cartesian.reference_id=gps_pvt_cartesian.reference_id;
+ this->current_pvt_cartesian.mean_corr_age=gps_pvt_cartesian.mean_corr_age;
+ this->current_pvt_cartesian.signal_info=gps_pvt_cartesian.signal_info;
+ if(gps_pvt_cartesian.alert_flag.int_mon==1)
+ this->current_pvt_cartesian.integrity_mon=true;
+ else
+ this->current_pvt_cartesian.integrity_mon=false;
+ if(gps_pvt_cartesian.alert_flag.int_failed_raim==1)
+ this->current_pvt_cartesian.integrity_failed_RAIM=true;
+ else
+ this->current_pvt_cartesian.integrity_failed_RAIM=false;
+ if(gps_pvt_cartesian.alert_flag.int_failed_hpca==1)
+ this->current_pvt_cartesian.integrity_failed_HPCA=true;
+ else
+ this->current_pvt_cartesian.integrity_failed_HPCA=false;
+ this->pvt_access.exit();
+}
+
+void CasteRx1::process_pvt_geodetic(unsigned char *data,unsigned short int length)
+{
+ TGPSPVTGeodetic gps_pvt_geodetic;
+
+ this->pvt_access.enter();
+ memcpy((unsigned char *)&gps_pvt_geodetic,data,sizeof(TGPSPVTGeodetic));
+ this->current_pvt_geodetic.gps_timestamp.wnc=gps_pvt_geodetic.timestamp.wnc;
+ this->current_pvt_geodetic.gps_timestamp.tow=gps_pvt_geodetic.timestamp.tow;
+ this->current_pvt_geodetic.local_timestamp=this->local_time.getTimeInSeconds();
+ this->current_pvt_geodetic.mode=(pvt_mode_t)gps_pvt_geodetic.mode.mode;
+ this->current_pvt_geodetic.solution=(solution_t)gps_pvt_geodetic.mode.solution_type;
+ this->current_pvt_geodetic.error=gps_pvt_geodetic.error;
+ this->current_pvt_geodetic.latitude=gps_pvt_geodetic.latitude;
+ this->current_pvt_geodetic.longitude=gps_pvt_geodetic.longitude;
+ this->current_pvt_geodetic.h=gps_pvt_geodetic.h;
+ this->current_pvt_geodetic.undulation=gps_pvt_geodetic.undulation;
+ this->current_pvt_geodetic.vn=gps_pvt_geodetic.vn;
+ this->current_pvt_geodetic.ve=gps_pvt_geodetic.ve;
+ this->current_pvt_geodetic.vu=gps_pvt_geodetic.vu;
+ this->current_pvt_geodetic.cog=gps_pvt_geodetic.cog;
+ this->current_pvt_geodetic.rx_clk_bias=gps_pvt_geodetic.rx_clk_bias;
+ this->current_pvt_geodetic.rx_clk_drift=gps_pvt_geodetic.rx_clk_drift;
+ this->current_pvt_geodetic.time_system=(sys_time_t)gps_pvt_geodetic.time_system;
+ this->current_pvt_geodetic.datum=(datum_t)gps_pvt_geodetic.datum;
+ this->current_pvt_geodetic.num_sat=gps_pvt_geodetic.num_sat;
+ this->current_pvt_geodetic.wa_corr_info=gps_pvt_geodetic.wa_corr_info;
+ if(gps_pvt_geodetic.corr_info.orbit_clock_corr==1)
+ this->current_pvt_geodetic.orbit_sat_clock_corr=true;
+ else
+ this->current_pvt_geodetic.orbit_sat_clock_corr=false;
+ if(gps_pvt_geodetic.corr_info.range_corr==1)
+ this->current_pvt_geodetic.range_corr_info=true;
+ else
+ this->current_pvt_geodetic.range_corr_info=false;
+ if(gps_pvt_geodetic.corr_info.ion_info==1)
+ this->current_pvt_geodetic.ion_info=true;
+ else
+ this->current_pvt_geodetic.ion_info=false;
+ if(gps_pvt_geodetic.corr_info.orbit_acc_info==1)
+ this->current_pvt_geodetic.orbit_accuracy_info=true;
+ else
+ this->current_pvt_geodetic.orbit_accuracy_info=false;
+ if(gps_pvt_geodetic.corr_info.do229_mode==1)
+ this->current_pvt_geodetic.DO299_used=true;
+ else
+ this->current_pvt_geodetic.DO299_used=false;
+ this->current_pvt_geodetic.reference_id=gps_pvt_geodetic.reference_id;
+ this->current_pvt_geodetic.mean_corr_age=gps_pvt_geodetic.mean_corr_age;
+ this->current_pvt_geodetic.signal_info=gps_pvt_geodetic.signal_info;
+ if(gps_pvt_geodetic.alert_flag.int_mon==1)
+ this->current_pvt_geodetic.integrity_mon=true;
+ else
+ this->current_pvt_geodetic.integrity_mon=false;
+ if(gps_pvt_geodetic.alert_flag.int_failed_raim==1)
+ this->current_pvt_geodetic.integrity_failed_RAIM=true;
+ else
+ this->current_pvt_geodetic.integrity_failed_RAIM=false;
+ if(gps_pvt_geodetic.alert_flag.int_failed_hpca==1)
+ this->current_pvt_geodetic.integrity_failed_HPCA=true;
+ else
+ this->current_pvt_geodetic.integrity_failed_HPCA=false;
+ this->pvt_access.exit();
+}
+
+void CasteRx1::process_pos_cov_cartesian(unsigned char *data,unsigned short int length)
+{
+ TGPSPVTCov gps_pvt_pos_cov_cartesian;
+
+ this->pvt_access.enter();
+ memcpy((unsigned char *)&gps_pvt_pos_cov_cartesian,data,sizeof(TGPSPVTCov));
+ this->current_pvt_pos_cov_cartesian.gps_timestamp.wnc=gps_pvt_pos_cov_cartesian.timestamp.wnc;
+ this->current_pvt_pos_cov_cartesian.gps_timestamp.tow=gps_pvt_pos_cov_cartesian.timestamp.tow;
+ this->current_pvt_pos_cov_cartesian.local_timestamp=this->local_time.getTimeInSeconds();
+ this->current_pvt_pos_cov_cartesian.mode=(pvt_mode_t)gps_pvt_pos_cov_cartesian.mode.mode;
+ this->current_pvt_pos_cov_cartesian.solution=(solution_t)gps_pvt_pos_cov_cartesian.mode.solution_type;
+ this->current_pvt_pos_cov_cartesian.error=gps_pvt_pos_cov_cartesian.error;
+ this->current_pvt_pos_cov_cartesian.cov_00=gps_pvt_pos_cov_cartesian.cov_00;
+ this->current_pvt_pos_cov_cartesian.cov_11=gps_pvt_pos_cov_cartesian.cov_11;
+ this->current_pvt_pos_cov_cartesian.cov_22=gps_pvt_pos_cov_cartesian.cov_22;
+ this->current_pvt_pos_cov_cartesian.cov_33=gps_pvt_pos_cov_cartesian.cov_33;
+ this->current_pvt_pos_cov_cartesian.cov_01=gps_pvt_pos_cov_cartesian.cov_01;
+ this->current_pvt_pos_cov_cartesian.cov_02=gps_pvt_pos_cov_cartesian.cov_02;
+ this->current_pvt_pos_cov_cartesian.cov_03=gps_pvt_pos_cov_cartesian.cov_03;
+ this->current_pvt_pos_cov_cartesian.cov_12=gps_pvt_pos_cov_cartesian.cov_12;
+ this->current_pvt_pos_cov_cartesian.cov_13=gps_pvt_pos_cov_cartesian.cov_13;
+ this->current_pvt_pos_cov_cartesian.cov_23=gps_pvt_pos_cov_cartesian.cov_23;
+ this->pvt_access.exit();
+}
+
+void CasteRx1::process_pos_cov_geodetic(unsigned char *data,unsigned short int length)
+{
+ TGPSPVTCov gps_pvt_pos_cov_geodetic;
+
+ this->pvt_access.enter();
+ memcpy((unsigned char *)&gps_pvt_pos_cov_geodetic,data,sizeof(TGPSPVTCov));
+ this->current_pvt_pos_cov_geodetic.gps_timestamp.wnc=gps_pvt_pos_cov_geodetic.timestamp.wnc;
+ this->current_pvt_pos_cov_geodetic.gps_timestamp.tow=gps_pvt_pos_cov_geodetic.timestamp.tow;
+ this->current_pvt_pos_cov_geodetic.local_timestamp=this->local_time.getTimeInSeconds();
+ this->current_pvt_pos_cov_geodetic.mode=(pvt_mode_t)gps_pvt_pos_cov_geodetic.mode.mode;
+ this->current_pvt_pos_cov_geodetic.solution=(solution_t)gps_pvt_pos_cov_geodetic.mode.solution_type;
+ this->current_pvt_pos_cov_geodetic.error=gps_pvt_pos_cov_geodetic.error;
+ this->current_pvt_pos_cov_geodetic.cov_00=gps_pvt_pos_cov_geodetic.cov_00;
+ this->current_pvt_pos_cov_geodetic.cov_11=gps_pvt_pos_cov_geodetic.cov_11;
+ this->current_pvt_pos_cov_geodetic.cov_22=gps_pvt_pos_cov_geodetic.cov_22;
+ this->current_pvt_pos_cov_geodetic.cov_33=gps_pvt_pos_cov_geodetic.cov_33;
+ this->current_pvt_pos_cov_geodetic.cov_01=gps_pvt_pos_cov_geodetic.cov_01;
+ this->current_pvt_pos_cov_geodetic.cov_02=gps_pvt_pos_cov_geodetic.cov_02;
+ this->current_pvt_pos_cov_geodetic.cov_03=gps_pvt_pos_cov_geodetic.cov_03;
+ this->current_pvt_pos_cov_geodetic.cov_12=gps_pvt_pos_cov_geodetic.cov_12;
+ this->current_pvt_pos_cov_geodetic.cov_13=gps_pvt_pos_cov_geodetic.cov_13;
+ this->current_pvt_pos_cov_geodetic.cov_23=gps_pvt_pos_cov_geodetic.cov_23;
+ this->pvt_access.exit();
+}
+
+void CasteRx1::process_vel_cov_cartesian(unsigned char *data,unsigned short int length)
+{
+ TGPSPVTCov gps_pvt_vel_cov_cartesian;
+
+ this->pvt_access.enter();
+ memcpy((unsigned char *)&gps_pvt_vel_cov_cartesian,data,sizeof(TGPSPVTCov));
+ this->current_pvt_vel_cov_cartesian.gps_timestamp.wnc=gps_pvt_vel_cov_cartesian.timestamp.wnc;
+ this->current_pvt_vel_cov_cartesian.gps_timestamp.tow=gps_pvt_vel_cov_cartesian.timestamp.tow;
+ this->current_pvt_vel_cov_cartesian.local_timestamp=this->local_time.getTimeInSeconds();
+ this->current_pvt_vel_cov_cartesian.mode=(pvt_mode_t)gps_pvt_vel_cov_cartesian.mode.mode;
+ this->current_pvt_vel_cov_cartesian.solution=(solution_t)gps_pvt_vel_cov_cartesian.mode.solution_type;
+ this->current_pvt_vel_cov_cartesian.error=gps_pvt_vel_cov_cartesian.error;
+ this->current_pvt_vel_cov_cartesian.cov_00=gps_pvt_vel_cov_cartesian.cov_00;
+ this->current_pvt_vel_cov_cartesian.cov_11=gps_pvt_vel_cov_cartesian.cov_11;
+ this->current_pvt_vel_cov_cartesian.cov_22=gps_pvt_vel_cov_cartesian.cov_22;
+ this->current_pvt_vel_cov_cartesian.cov_33=gps_pvt_vel_cov_cartesian.cov_33;
+ this->current_pvt_vel_cov_cartesian.cov_01=gps_pvt_vel_cov_cartesian.cov_01;
+ this->current_pvt_vel_cov_cartesian.cov_02=gps_pvt_vel_cov_cartesian.cov_02;
+ this->current_pvt_vel_cov_cartesian.cov_03=gps_pvt_vel_cov_cartesian.cov_03;
+ this->current_pvt_vel_cov_cartesian.cov_12=gps_pvt_vel_cov_cartesian.cov_12;
+ this->current_pvt_vel_cov_cartesian.cov_13=gps_pvt_vel_cov_cartesian.cov_13;
+ this->current_pvt_vel_cov_cartesian.cov_23=gps_pvt_vel_cov_cartesian.cov_23;
+ this->pvt_access.exit();
+}
+
+void CasteRx1::process_vel_cov_geodetic(unsigned char *data,unsigned short int length)
+{
+ TGPSPVTCov gps_pvt_vel_cov_geodetic;
+
+ this->pvt_access.enter();
+ memcpy((unsigned char *)&gps_pvt_vel_cov_geodetic,data,sizeof(TGPSPVTCov));
+ this->current_pvt_vel_cov_geodetic.gps_timestamp.wnc=gps_pvt_vel_cov_geodetic.timestamp.wnc;
+ this->current_pvt_vel_cov_geodetic.gps_timestamp.tow=gps_pvt_vel_cov_geodetic.timestamp.tow;
+ this->current_pvt_vel_cov_geodetic.local_timestamp=this->local_time.getTimeInSeconds();
+ this->current_pvt_vel_cov_geodetic.mode=(pvt_mode_t)gps_pvt_vel_cov_geodetic.mode.mode;
+ this->current_pvt_vel_cov_geodetic.solution=(solution_t)gps_pvt_vel_cov_geodetic.mode.solution_type;
+ this->current_pvt_vel_cov_geodetic.error=gps_pvt_vel_cov_geodetic.error;
+ this->current_pvt_vel_cov_geodetic.cov_00=gps_pvt_vel_cov_geodetic.cov_00;
+ this->current_pvt_vel_cov_geodetic.cov_11=gps_pvt_vel_cov_geodetic.cov_11;
+ this->current_pvt_vel_cov_geodetic.cov_22=gps_pvt_vel_cov_geodetic.cov_22;
+ this->current_pvt_vel_cov_geodetic.cov_33=gps_pvt_vel_cov_geodetic.cov_33;
+ this->current_pvt_vel_cov_geodetic.cov_01=gps_pvt_vel_cov_geodetic.cov_01;
+ this->current_pvt_vel_cov_geodetic.cov_02=gps_pvt_vel_cov_geodetic.cov_02;
+ this->current_pvt_vel_cov_geodetic.cov_03=gps_pvt_vel_cov_geodetic.cov_03;
+ this->current_pvt_vel_cov_geodetic.cov_12=gps_pvt_vel_cov_geodetic.cov_12;
+ this->current_pvt_vel_cov_geodetic.cov_13=gps_pvt_vel_cov_geodetic.cov_13;
+ this->current_pvt_vel_cov_geodetic.cov_23=gps_pvt_vel_cov_geodetic.cov_23;
+ this->pvt_access.exit();
+}
+
+void CasteRx1::process_dop(unsigned char *data,unsigned short int length)
+{
+ TGPSPVTDOP gps_pvt_dop;
+
+ this->pvt_access.enter();
+ memcpy((unsigned char *)&gps_pvt_dop,data,sizeof(TGPSPVTDOP));
+ this->current_pvt_dop.gps_timestamp.wnc=gps_pvt_dop.timestamp.wnc;
+ this->current_pvt_dop.gps_timestamp.tow=gps_pvt_dop.timestamp.tow;
+ this->current_pvt_dop.local_timestamp=this->local_time.getTimeInSeconds();
+ this->current_pvt_dop.num_sat=gps_pvt_dop.num_sat;
+ this->current_pvt_dop.p_dop=gps_pvt_dop.p_dop;
+ this->current_pvt_dop.t_dop=gps_pvt_dop.t_dop;
+ this->current_pvt_dop.t_dop=gps_pvt_dop.h_dop;
+ this->current_pvt_dop.v_dop=gps_pvt_dop.v_dop;
+ this->current_pvt_dop.hpl=gps_pvt_dop.hpl;
+ this->current_pvt_dop.vpl=gps_pvt_dop.vpl;
+ this->pvt_access.exit();
+}
+
+void CasteRx1::process_end_of_pvt(unsigned char *data,unsigned short int length)
+{
+ TGPSEndOfPVT gps_end_of_pvt;
+
+ memcpy((unsigned char *)&gps_end_of_pvt,data,sizeof(TGPSEndOfMeas));
+ if(!this->event_server->event_is_set(this->new_pvt_block_event_id))
+ this->event_server->set_event(new_pvt_block_event_id);
+}
+
+unsigned short int CasteRx1::compute_crc(unsigned char *data,unsigned short int length)
+{
+ unsigned short int crc=0;
+ unsigned int i=0;
+
+ // compute the crc from part of the header
+ for(i=0;i<4;i++)
+ crc=(crc<<8)^CRC_16CCIT_LookUp[(crc>>8)^((unsigned char *)&this->current_header)[i+4]];
+ // compute the crc from the data
+ for(i=0;i<length;i++)
+ crc=(crc<<8)^CRC_16CCIT_LookUp[(crc>>8)^data[i]];
return crc;
}
-void CasteRx1::computeTM()
-{
- double kk; //constant
- double xx0,yy0,zz0; //wgs-ecef coordinates of the origin
- double auxTM[4][4];
-
- //sets the rotation part of TM, from ECEF to ENU coordinates
- auxTM[0][0] = -sin(lon0);
- auxTM[0][1] = cos(lon0);
- auxTM[0][2] = 0.0;
- auxTM[1][0] = -sin(lat0)*cos(lon0);
- auxTM[1][1] = -sin(lat0)*sin(lon0);
- auxTM[1][2] = cos(lat0);
- auxTM[2][0] = cos(lat0)*cos(lon0);
- auxTM[2][1] = cos(lat0)*sin(lon0);
- auxTM[2][2] = sin(lat0);
-
- //computes final alignement of x with respect to the campus baseline as a rotation of alpha0 on z axis
- //Ra=[cos(alpha0) sin(alpha0) 0;
- // -sin(alpha0) cos(alpha0) 0;
- // 0 0 1];
- TM[0][0]=cos(alpha0)*auxTM[0][0] + sin(alpha0)*auxTM[1][0];
- TM[0][1]=cos(alpha0)*auxTM[0][1] + sin(alpha0)*auxTM[1][1];
- TM[0][2]=cos(alpha0)*auxTM[0][2] + sin(alpha0)*auxTM[1][2];
- TM[1][0]=-sin(alpha0)*auxTM[0][0] + cos(alpha0)*auxTM[1][0];
- TM[1][1]=-sin(alpha0)*auxTM[0][1] + cos(alpha0)*auxTM[1][1];
- TM[1][2]=-sin(alpha0)*auxTM[0][2] + cos(alpha0)*auxTM[1][2];
- TM[2][0]=auxTM[2][0];
- TM[2][1]=auxTM[2][1];
- TM[2][2]=auxTM[2][2];
-
- //computes the translation part
- kk = EARTH_RADIUS/sqrt(1-(pow(EARTH_EXCENTRICITY*sin(lat0),2.0)));
- xx0 = (kk + alt0) * cos(lat0) * cos(lon0);
- yy0 = (kk + alt0) * cos(lat0) * sin(lon0);
- zz0 = ((1-EARTH_EXCENTRICITY*EARTH_EXCENTRICITY)* kk + alt0) * sin(lat0);
- TM[0][3] = - ( TM[0][0]*xx0 + TM[0][1]*yy0 + TM[0][2]*zz0 );
- TM[1][3] = - ( TM[1][0]*xx0 + TM[1][1]*yy0 + TM[1][2]*zz0 );
- TM[2][3] = - ( TM[2][0]*xx0 + TM[2][1]*yy0 + TM[2][2]*zz0 );
- TM[3][0] = 0.0;
- TM[3][1] = 0.0;
- TM[3][2] = 0.0;
- TM[3][3] = 1.0;
-}
-
-void CasteRx1::computeTMenu()
-{
- //uses current position to compute ENU rotation
- TMenu[0][0] = -sin(lon);
- TMenu[0][1] = cos(lon);
- TMenu[0][2] = 0.0;
- TMenu[1][0] = -sin(lat)*cos(lon);
- TMenu[1][1] = -sin(lat)*sin(lon);
- TMenu[1][2] = cos(lat);
- TMenu[2][0] = cos(lat)*cos(lon);
- TMenu[2][1] = cos(lat)*sin(lon);
- TMenu[2][2] = sin(lat);
-}
-
-void CasteRx1::fromWgsToMap()
-{
- double auxM[3][3];
- int ii;
-
- //compute map coords
- xMap=TM[0][0]*xWgs+TM[0][1]*yWgs+TM[0][2]*zWgs+TM[0][3];
- yMap=TM[1][0]*xWgs+TM[1][1]*yWgs+TM[1][2]*zWgs+TM[1][3];
- zMap=TM[2][0]*xWgs+TM[2][1]*yWgs+TM[2][2]*zWgs+TM[2][3];
-
- //compute velocities
- vxMap=TM[0][0]*vxWgs+TM[0][1]*vyWgs+TM[0][2]*vzWgs;
- vyMap=TM[1][0]*vxWgs+TM[1][1]*vyWgs+TM[1][2]*vzWgs;
- vzMap=TM[2][0]*vxWgs+TM[2][1]*vyWgs+TM[2][2]*vzWgs;
-
- //compute covariance matrix
- //firts step: auxM=TM*covEcef
- for(ii=0;ii<3;ii++)
- {
- auxM[ii][0]=TM[ii][0]*cxxWgs+TM[ii][1]*cxyWgs+TM[ii][2]*cxzWgs;
- auxM[ii][1]=TM[ii][0]*cxyWgs+TM[ii][1]*cyyWgs+TM[ii][2]*cyzWgs;
- auxM[ii][2]=TM[ii][0]*cxzWgs+TM[ii][1]*cyzWgs+TM[ii][2]*czzWgs;
- }
- //second step: covMap = auxM*TM^T
- cxxMap=auxM[0][0]*TM[0][0]+auxM[0][1]*TM[0][1]+auxM[0][2]*TM[0][2];
- cyyMap=auxM[1][0]*TM[1][0]+auxM[1][1]*TM[1][1]+auxM[1][2]*TM[1][2];
- czzMap=auxM[2][0]*TM[2][0]+auxM[2][1]*TM[2][1]+auxM[2][2]*TM[2][2];
- cxyMap=auxM[0][0]*TM[1][0]+auxM[0][1]*TM[1][1]+auxM[0][2]*TM[1][2];
-}
-
-void CasteRx1::computeENUcovs()
-{
- double auxM[3][3];
- int ii;
-
- //compute covariance matrix
- //firts step: auxM=TMenu*covEcef
- for(ii=0;ii<3;ii++)
- {
- auxM[ii][0]=TMenu[ii][0]*cxxWgs+TMenu[ii][1]*cxyWgs+TMenu[ii][2]*cxzWgs;
- auxM[ii][1]=TMenu[ii][0]*cxyWgs+TMenu[ii][1]*cyyWgs+TMenu[ii][2]*cyzWgs;
- auxM[ii][2]=TMenu[ii][0]*cxzWgs+TMenu[ii][1]*cyzWgs+TMenu[ii][2]*czzWgs;
- }
- //second step: covMap = auxM*TMenu^T
- cxxEnu=auxM[0][0]*TMenu[0][0]+auxM[0][1]*TMenu[0][1]+auxM[0][2]*TMenu[0][2];
- cyyEnu=auxM[1][0]*TMenu[1][0]+auxM[1][1]*TMenu[1][1]+auxM[1][2]*TMenu[1][2];
- czzEnu=auxM[2][0]*TMenu[2][0]+auxM[2][1]*TMenu[2][1]+auxM[2][2]*TMenu[2][2];
- cxyEnu=auxM[0][0]*TMenu[1][0]+auxM[0][1]*TMenu[1][1]+auxM[0][2]*TMenu[1][2];
- cxzEnu=auxM[0][0]*TMenu[2][0]+auxM[0][1]*TMenu[2][1]+auxM[0][2]*TMenu[2][2];
- cyzEnu=auxM[1][0]*TMenu[2][0]+auxM[1][1]*TMenu[2][1]+auxM[1][2]*TMenu[2][2];
-}
-
-void CasteRx1::markBlockAsReceived(unsigned short int id)
-{
- list<sbfBlockManager>::iterator iiBlock;
-
- for (iiBlock=blockList.begin();iiBlock!=blockList.end();iiBlock++)
- {
- if ( iiBlock->blockId == id ) iiBlock->received = true;
- }
+void CasteRx1::sendCommand(const std::string &cmd)
+{
+ std::list<std::string> events;
+ std::string reply;
+
+ //relax if we concatenate commands. (200 ms were suggested by Septentrio support team in a personal communication). 50ms tested and it seems it is not enough!!!
+ usleep(100000);
+
+ events.push_back(this->new_reply_event_id);
+ try{
+ this->serialPort->write((unsigned char *)cmd.c_str(),cmd.size());
+ this->event_server->wait_all(events,500);
+ // compare the answer with the command
+ reply=this->reply_queue.front();// get the reply
+ this->reply_queue.pop();// remove it from the queue
+ if(reply.find("Invalid command!")!=std::string::npos)
+ {
+ /* handle invalid command */
+ std::cout << "Invalid Command" << std::endl;
+ }
+ else if(reply.find(cmd.c_str(),0,cmd.size()-1)==std::string::npos)
+ {
+ /* handle unexpected answer */
+ std::cout << "Unexpected answer" << std::endl;
+ }
+ else
+ std::cout << "Command ok" << std::endl;
+ }catch(CEventTimeoutException &e){
+ // no answer in the allowed time
+ std::cout << e.what() << std::endl;
+ }
+}
+
+void CasteRx1::configureStreams(void)
+{
+ // stop any incomming data
+ this->sendCommand("setSBFOutput,all,USB1,none\n");
+ //enables septentrio binary format (SBF) as output at USB1
+ this->sendCommand("setDataInOut,USB1,CMD,SBF\n");
+ // create group 1 blocks (PVTCartesian,PosCovCartesian,DOP,PVTGeodetic)
+ this->sendCommand("setSBFGroups,Group1,PVTCartesian+PosCovCartesian+VelCovCartesian+DOP+PVTGeodetic+PosCovGeodetic+VelCovGeodetic+EndOfPVT\n");
+ // create group 2 blocks (GPSNav,GPSAlm,GPSIon,GPSUtc)
+ this->sendCommand("setSBFGroups,Group2,GPSNav+GPSAlm+GPSIon+GPSUtc\n");
+ // create group 3 blocks (MeasEpoch,MeasExtra,EndOfMeas)
+ this->sendCommand("setSBFGroups,Group3,MeasEpoch+MeasExtra+EndOfMeas\n");
+ // create group 4 blocks (ChannelStatus,ReceiverStatus,SatVisivility)
+ this->sendCommand("setSBFGroups,Group4,ChannelStatus+ReceiverStatus+SatVisibility\n");
}
+
diff --git a/src/asterx1_gps.h b/src/asterx1_gps.h
index bfd759b39fb6ef63b239859a17f9973a1622be17..0dde8715073093870e827abb0380ebfd8317359e 100644
--- a/src/asterx1_gps.h
+++ b/src/asterx1_gps.h
@@ -1,75 +1,24 @@
-
#ifndef asterx1_gps_h
#define asterx1_gps_h
//include
#include <iostream>
#include <list>
+#include <vector>
+#include <queue>
#include <math.h>
+#include <time.h>
+#include <sys/time.h>
+#include <stdint.h>
#include "rs232.h"
#include "eventserver.h"
+#include "threadserver.h"
#include "commexceptions.h"
#include "eventexceptions.h"
-#include <time.h>
-#include <sys/time.h>
-
-using namespace std;
+#include "gps_types.h"
+#include "ctime.h"
-/* Look up table for fast computation of the CCITT 16-bit CRC. */
-static const unsigned short CRC_16CCIT_LookUp[256] = {
- 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7,
- 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef,
- 0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6,
- 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de,
- 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485,
- 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d,
- 0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4,
- 0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc,
- 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823,
- 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b,
- 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12,
- 0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a,
- 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41,
- 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49,
- 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70,
- 0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78,
- 0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f,
- 0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067,
- 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e,
- 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256,
- 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d,
- 0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
- 0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c,
- 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634,
- 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab,
- 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3,
- 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a,
- 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92,
- 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9,
- 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1,
- 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8,
- 0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0
-};
-
-union uniFloat {
- float flt;
- unsigned int i;
-};
-
-union uniDouble {
- double dbl;
- unsigned long long int i;
-};
-
-struct sbfBlockManager {
- unsigned short int blockId;
- bool received;
-};
-
-struct asterx1DeviceConfig {
- string portName; //serial port system name
- string acqPeriod; //string indicating acquisition period. Takes one of the values of the vector "acqPeriodValues"
-};
+typedef enum {header1,header2,get_cmd_reply,get_block_header,get_sbf_data} gps_state_t;
//physical constants
const double EARTH_RADIUS = 6378137.0;
@@ -79,45 +28,28 @@ const double EARTH_EXCENTRICITY = 0.0818191908426220;
const unsigned short int PVTCartesian_ID = 4006;
const unsigned short int PVTGeodetic_ID = 4007;
const unsigned short int PosCovCartesian_ID = 5905;
+const unsigned short int PosCovGeodetic_ID = 5906;
+const unsigned short int VelCovCartesian_ID = 5907;
+const unsigned short int VelCovGeodetic_ID = 5908;
const unsigned short int DOP_ID = 4001;
+const unsigned short int EndOfPVT_ID = 5921;
+const unsigned short int MeasEpoch_ID = 4027;
+const unsigned short int MeasExtra_ID = 4000;
+const unsigned short int EndOfMeas_ID = 5922;
+const unsigned short int GPSNav_ID = 5891;
+const unsigned short int GPSAlm_ID = 5892;
+const unsigned short int GPSIon_ID = 5893;
+const unsigned short int GPSUtc_ID = 5894;
+const unsigned short int ReceiverStatus_ID = 4014;
+const unsigned short int ChannelStatus_ID = 4013;
+const unsigned short int SatVisibility_ID = 4012;
//acquisition constants
-const unsigned int ASTERX_BAUDRATE = 115200;
const int DATA_TIMEOUT = 11000; //msec
const int CONFIG_TIMEOUT = 500; //msec
-const string acqPeriodValues[] = {"onChange","msec20","msec50","msec100","msec200","msec500","sec1","sec2","sec5","sec10"};
-enum acqPeriodValuesIndex {ONCHANGE = 0, MSEC20, MSEC50, MSEC100, MSEC200, MSEC500, SEC1, SEC2, SEC5, SEC10};
//data sizes
const unsigned int BLOCK_HEADER_SIZE = 8;
-const unsigned int MIN_BLOCK_SIZE = BLOCK_HEADER_SIZE +1;
-const unsigned int MAX_BLOCK_SIZE = 200;
-
-/**
- Member functions that returns an integer associated to success or not, should return one of these values
-*/
-const int BASIC_SUCCESS = 1;
-const int PORT_OPEN_ERROR = -1;
-const int PORT_CLOSE_ERROR = -2;
-const int PORT_CONFIG_ERROR = -3;
-const int ASTERX1_CONFIG_ERROR = -4;
-const int ASTERX1_INVALID_COMMAND = -5;
-const int ASTERX1_STOPPINTG_ERROR = -6;
-const int PORT_READ_TIMEOUT = -7;
-const int ACQUISITION_ERROR = -8;
-
-/**
- Status of a class object is a bitwise OR of different values. The given bit is set when positive cases, and reset if negative significance.
-*/
-const unsigned int DEVICE_OPEN = 0x1;
-const unsigned int ACQUISITION_RUNNING = 0x2;
-const unsigned int DATA_BLOCK_OK = 0x4;
-const unsigned int GPS_AVAILABLE = 0x8;
-const unsigned int ALL_OK = DEVICE_OPEN | ACQUISITION_RUNNING | DATA_BLOCK_OK | GPS_AVAILABLE;
-
-//unit identifiers
-const bool inRADS = 0;
-const bool inDEGREES = 1;
/**
CasteRx1 implements continuous data acquisition from the asteRx1 GPS receiver.
@@ -125,286 +57,279 @@ const bool inDEGREES = 1;
Detailed documentation about SBF data transfer protocol can be found in the device manual.
PLEASE NOTE: This driver only implements a part of the AsteRx1 device functionalities (the most useful part for mobile robot navigation).
PLEASE NOTE: For extended functionalities , please refer to the manual.
- Programmer: Andreu Corominas Murtra, acorominas@iri.upc.edu
- Device website: www.septentrio.com
-*/
+Programmer: Andreu Corominas Murtra, acorominas@iri.upc.edu
+Device website: www.septentrio.com
+ */
class CasteRx1
{
- public:
- /** \brief Default constructor
- *
- * Constructor. Requires port file name and, optionally, acquisition rate.
- *
- */
- CasteRx1(const string & hwPortName = "/dev/ttyACM0", const int acqRate = MSEC500);
-
- /** \brief Destructor
- *
- * Destructor: Stops acquisition in case it was running and closes serial Port in case it was opened.
- *
- */
- virtual ~CasteRx1();
-
- /** \brief Open and configure serial device.
- *
- * Open and configure serial device to 8N1 Byte transmission.
- * Launches a thread for serial in/out comms
- * Return values are:
- * BASIC_SUCCESS
- * PORT_OPEN_ERROR if thread can't be created or port can't be opened
- * PORT_CONFIG_ERROR if port can't be configured
- */
- int openDevice();
-
- /** \brief Closes serial comm
- *
- * Closes serial comm's , stops comm thread and deletes comm object.
- * If success , resets status bit DEVICE_OPEN.
- * Return values are:
- * BASIC_SUCCESS
- * PORT_CLOSE_ERROR
- */
- int closeDevice();
-
- /** \brief Starts basic data acquisition
- *
- * Configures the device to perform some signal processing basics for mobile robotics.
- * Starts a mode of operation of continuos data flow from the device at a rate of 2Hz.
- * Return values are:
- * BASIC_SUCCESS
- * ASTERX1_CONFIG_ERROR
- */
- int startAcquisition();
-
- /** \brief Stops acquisition
- *
- * Stops continuous acquisition mode
- * Return values are:
- * BASIC_SUCCESS when device stop is ok
- * ASTERX1_STOPPINTG_ERROR otherwise.
- */
- int stopAcquisition();
-
- /** \brief Decodes the device SBF stream
- *
- * Reads current data produced by the device. Ends when all data blocks requested have been received.
- * Sets status and all data fields.
- * Return values are:
- * BASIC_SUCCESS if all blocks are successfully decodes.
- * ACQUISITION_ERROR otherwise.
- *
- * After calling this function, call getStatus() to check for gps availability.
- */
- int readDataFromDevice();
-
- /**
- Prints all current data in a single row of the given ostream (cout or file)
- */
- void printData(ostream & ost);
-
- /**
- Sets map origin and computes transformation matrix.
- Angles in degrees and altitude in meters.
- A call to this function is mandatory if local coordinate related position is required.
- */
- void setMapOrigin(double mapLat0, double mapLon0, double mapAlt0, double mapAlpha0);
-
- /**
- Sets serial port name
- */
- void setPortName(string pName);
-
- /**
- Sets acquisition rate as on of the rateId indicated by the acqPeriodValuesIndex enum variable
- */
- void setAcquisitionRate(int rateIndex);
-
- /**
- Prints the homogeneous transformation matrix used to convert data from ECEF-WGS to local map.
- */
- void printTM();
-
- /**
- Get data ...
- TimeStamp is set by the computer running the driver.
- Tow is the GPS "time of week".
- PVTerror codes are explained in the device manual, pag XX.
- Lat and Lon are expressed in radians by default. Pass "inDEGREES" if degree values are required.
- *Wgs is data with respect to the ECEF-WGS coordinate frame.
- *Map is data with respect to the local (map) coordinate frame.
- */
- unsigned int getStatus();
- double getTimeStamp();
- unsigned short int getWnc();
- unsigned int getTow();
- unsigned int getNumSatellites();
- float getPDOP();
- float getTDOP();
- float getHDOP();
- float getVDOP();
- float getUndulation();
- unsigned short int getPVTerror();
- double getLat(bool units=inRADS);
- double getLon(bool units=inRADS);
- double getAlt();
- double getXWgs();
- double getYWgs();
- double getZWgs();
- double getXMap();
- double getYMap();
- double getZMap();
- double getVxWgs();
- double getVyWgs();
- double getVzWgs();
- double getVxMap();
- double getVyMap();
- double getVzMap();
- double getCxxWgs();
- double getCyyWgs();
- double getCzzWgs();
- double getCxyWgs();
- double getCxxMap();
- double getCyyMap();
- double getCzzMap();
- double getCxyMap();
- double getCxxEnu();
- double getCyyEnu();
- double getCzzEnu();
- double getCxyEnu();
- double getCxzEnu();
- double getCyzEnu();
-
- protected:
- CRS232 *serialPort; /**<Object for serial comms. This object launch an independent thread managing byte I/O through serial port (USB or RS232)*/
- asterx1DeviceConfig config; /**<Devices configuration. Currently a single string with serial port name*/
+ public:
+ /** \brief Default constructor
+ *
+ * Constructor. Requires port file name and, optionally, acquisition rate.
+ *
+ */
+ CasteRx1(const std::string &name,const std::string &hwPortName = "/dev/ttyACM0");
+ /** \brief Destructor
+ *
+ * Destructor: Stops acquisition in case it was running and closes serial Port in case it was opened.
+ *
+ */
+ virtual ~CasteRx1();
+ /** \brief Open and configure serial device.
+ *
+ * Open and configure serial device to 8N1 Byte transmission.
+ * Launches a thread for serial in/out comms
+ */
+ void openDevice();
+ /** \brief Closes serial comm
+ *
+ * Closes serial comm's , stops comm thread and deletes comm object.
+ * If success , resets status bit DEVICE_OPEN.
+ */
+ void closeDevice();
+ /** \brief Starts basic data acquisition
+ *
+ * Configures the device to perform some signal processing basics for mobile robotics.
+ * Starts a mode of operation of continuos data flow from the device at a rate of 2Hz.
+ */
+ void startAcquisition();
+ /** \brief Stops acquisition
+ *
+ * Stops continuous acquisition mode
+ */
+ void stopAcquisition();
+ /**
+ * \brief
+ *
+ */
+ std::string get_new_meas_data_event_id(void);
+ /**
+ * \brief
+ *
+ */
+ void get_meas_data(TMeasEpoch &meas_epoch, TMeasExtra &meas_extra);
+ /**
+ * \brief
+ *
+ */
+ std::string get_new_gps_nav_data_event_id(void);
+ /**
+ * \brief
+ *
+ */
+ void get_gps_nav_data(TGPSNav &gps_nav);
+ /**
+ * \brief
+ *
+ */
+ std::string get_new_gps_alm_data_event_id(void);
+ /**
+ * \brief
+ *
+ */
+ void get_gps_alm_data(TGPSAlm &gps_alm);
+ /**
+ * \brief
+ *
+ */
+ std::string get_new_gps_ion_data_event_id(void);
+ /**
+ * \brief
+ *
+ */
+ void get_gps_ion_data(TGPSIon &gps_ion);
+ /**
+ * \brief
+ *
+ */
+ std::string get_new_gps_utc_data_event_id(void);
+ /**
+ * \brief
+ *
+ */
+ void get_gps_utc_data(TGPSUtc &gps_utc);
+ /**
+ * \brief
+ *
+ */
+ std::string get_new_pvt_data_event_id(void);
+ /**
+ * \brief
+ *
+ */
+ void get_pvt_data(TPVTCartesian &pvt_cart,TPVTGeodetic &pvt_geo);
+ /**
+ * \brief
+ *
+ */
+ void get_pvt_cov_data(TPVTCov &pvt_pos_cov_cart,TPVTCov &pvt_pos_cov_geo,TPVTCov &pvt_vel_cov_cart,TPVTCov &pvt_vel_cov_geo);
+ /**
+ * \brief
+ *
+ */
+ void get_pvt_dop_data(TPVTDOP &pvt_dop);
+ private:
+ CRS232 *serialPort; /**<Object for serial comms. This object launch an independent thread managing byte I/O through serial port (USB or RS232)*/
+ std::string serial_device;
+ /* internal queue for command replies */
+ std::queue<std::string> reply_queue;
+ /* event server and events */
+ CEventServer *event_server;
+ std::string finish_thread_event_id;
+ std::string new_reply_event_id;
+ /* thread server and threads */
+ CThreadServer *thread_server;
+ std::string gps_process_data_thread_id;
+ bool gps_running;
+ CTime local_time;
- //variables for device operation and configuration
- unsigned int status;/**<Status related to the execution of this process*/
- list<sbfBlockManager> blockList; /**<this list manages which data blocks are requested and has been (or not) received in the current iteration*/
- double lat0, lon0, alt0, alpha0; /**<coordinates of the map origin. Alpha is the orientation of the north vector wrt the map X axis*/
- double TM[4][4]; /**homogeneous transformation matrix to convert from ecef-wgs84 to map coordinates*/
- double TMenu[3][3]; /**Rotation transformation matrix to convert from ecef-wgs84 to local tangential ENU (east north up) coordinates. Useful to output covariances following current ENU axis*/
-
- //GPS data to be published
- double timeStamp; /**<time stamp set by the computer executing the process (not GPS time)*/
- unsigned short int wnc; /**<gps week number*/
- unsigned int tow; /**<gps time of week in milliseconds*/
- unsigned int numSatellites; /**<number of visible satellites*/
- double PDOP, TDOP, HDOP, VDOP; /**<Dillution of precsion DOP's*/
- double undulation; /**<local geoid undulation between Datum ellipsoide and reference geoide*/
- unsigned short int pvtError; /**<error codes related to GPS position computation. (see page 50 of Firmware User Manual)*/
- double lat, lon, alt; /**<position in geodetic frame*/
- double xWgs, yWgs, zWgs; /**<position in WGS frame*/
- double xMap, yMap, zMap; /**<position in Map frame, a given reference frame*/
- double vxWgs,vyWgs,vzWgs; /**<velocities referred to WGS*/
- double vxMap, vyMap, vzMap; /**<velocities in Map frame, a given reference frame*/
- double cxxWgs,cyyWgs,czzWgs,cxyWgs,cxzWgs,cyzWgs; /**<covariance parameters of WGS position*/
- double cxxMap, cyyMap, czzMap, cxyMap; /**<covariance parameters in Map frame, a given reference frame*/
- double cxxEnu,cyyEnu,czzEnu,cxyEnu,cxzEnu,cyzEnu; /**<covariance parameters in the local tangential plane of the current position following the East North Up axis*/
+ /* header of the message currently being received */
+ TGPSBlockHeader current_header;
+ TReceiverStatus receiver_status;
+
+ /* Meas blocks attributes */
+ CMutex meas_access;
+ std::string new_meas_block_event_id;
+ TMeasEpoch current_meas_epoch;
+ TMeasExtra current_meas_extra;
- /**
- Reads nn bytes from the serial communication. If there are no bytes available, this function blocks up to timeout is reached.
- Success when ALL bytes have been received, it returns BASIC_SUCCESS
- If timeout, it returns PORT_READ_TIMEOUT.
- */
- int readNbytes(unsigned char *buffer, unsigned int nn, unsigned int msecTimeOut);
-
- /**
- Synchronizes data reading with a given header mark. (markSixe in bytes).
- Return values are:
- BASIC_SUCCESS if success: the mark is found at the ffdd file descriptor input
- PORT_READ_TIMEOUT if timeout is reached and the mark is not found
- */
- int synchronizeHeader(const unsigned char *mark, int msecTimeOut=1000, unsigned int markSize=2);
-
- /**
- Sends a command to the AsteRx1 device.
- Return values are :
- BASIC_SUCCESS if device has acknowledged the command.
- PORT_READ_TIMEOUT if timeout is reached and the receiver has not replied with an acknowledment.
- ASTERX1_INVALID_COMMAND when device doesn't know the command.
- ASTERX1_CONFIG_ERROR otherwise.
- */
- int sendCommand(const string & cmnd);
-
- /**
- Returns 1 byte integer in buffer (little-endian reading)
- */
- unsigned short int getUI1(unsigned char *buf);
+ /* GPS naviagtion blocks attributes */
+ CMutex nav_access;
+ std::string new_gps_nav_block_event_id;
+ TGPSNav current_gps_nav;
+ std::string new_gps_alm_block_event_id;
+ TGPSAlm current_gps_alm;
+ std::string new_gps_ion_block_event_id;
+ TGPSIon current_gps_ion;
+ std::string new_gps_utc_block_event_id;
+ TGPSUtc current_gps_utc;
- /**
- Returns 2 bytes integer in buffer (little-endian reading)
- */
- unsigned short int getUI2(unsigned char *buf);
-
- /**
- Returns 4 bytes integer in buffer (little-endian reading)
- */
- unsigned int getUI4(unsigned char *buf);
-
- /**
- Returns 4 byte float in buffer (little-endian reading)
- */
- float getFloat(unsigned char *buf);
-
- /**
- Returns 8 byte float in buffer (little-endian reading)
- */
- double getDouble(unsigned char *buf);
+ /* PVT blocks attributes */
+ CMutex pvt_access;
+ std::string new_pvt_block_event_id;
+ TPVTCartesian current_pvt_cartesian;
+ TPVTGeodetic current_pvt_geodetic;
+ TPVTCov current_pvt_pos_cov_cartesian;
+ TPVTCov current_pvt_vel_cov_cartesian;
+ TPVTCov current_pvt_pos_cov_geodetic;
+ TPVTCov current_pvt_vel_cov_geodetic;
+ TPVTDOP current_pvt_dop;
+ protected:
+ /**
+ * \brief
+ *
+ */
+ static void *gps_process_data_thread(void *param);
+ /**
+ * \brief
+ *
+ */
+ void process_block(unsigned char *data,unsigned short int length);
+ /**
+ * \brief
+ *
+ */
+ void process_channel_status(unsigned char *data,unsigned short int length);
+ /**
+ * \brief
+ *
+ */
+ void process_receiver_status(unsigned char *data,unsigned short int length);
+ /**
+ * \brief
+ *
+ */
+ void process_sat_visibility(unsigned char *data,unsigned short int length);
+ /**
+ * \brief
+ *
+ */
+ void process_gps_nav(unsigned char *data,unsigned short int length);
+ /**
+ * \brief
+ *
+ */
+ void process_gps_alm(unsigned char *data,unsigned short int length);
+ /**
+ * \brief
+ *
+ */
+ void process_gps_ion(unsigned char *data,unsigned short int length);
+ /**
+ * \brief
+ *
+ */
+ void process_gps_utc(unsigned char *data,unsigned short int length);
+ /**
+ * \brief
+ *
+ */
+ void process_meas_epoch(unsigned char *data,unsigned short int length);
+ /**
+ * \brief
+ *
+ */
+ void process_meas_extra(unsigned char *data,unsigned short int length);
+ /**
+ * \brief
+ *
+ */
+ void process_end_of_meas(unsigned char *data,unsigned short int length);
+ /**
+ * \brief
+ *
+ */
+ void process_pvt_cartesian(unsigned char *data,unsigned short int length);
+ /**
+ * \brief
+ *
+ */
+ void process_pos_cov_cartesian(unsigned char *data,unsigned short int length);
+ /**
+ * \brief
+ *
+ */
+ void process_pos_cov_geodetic(unsigned char *data,unsigned short int length);
+ /**
+ * \brief
+ *
+ */
+ void process_vel_cov_cartesian(unsigned char *data,unsigned short int length);
+ /**
+ * \brief
+ *
+ */
+ void process_vel_cov_geodetic(unsigned char *data,unsigned short int length);
+ /**
+ * \brief
+ *
+ */
+ void process_pvt_geodetic(unsigned char *data,unsigned short int length);
+ /**
+ * \brief
+ *
+ */
+ void process_dop(unsigned char *data,unsigned short int length);
+ /**
+ * \brief
+ *
+ */
+ void process_end_of_pvt(unsigned char *data,unsigned short int length);
+ /**
+ * \brief
+ *
+ */
+ void configureStreams(void);
+ /**
+ * \brief
+ *
+ */
+ void sendCommand(const std::string &cmnd);
- /**
- Decodes PVTCartesian data block. Implementation of the SBF data transfer protocol.
- */
- int decodePVTCartesian(unsigned char *sbfMsg);
-
- /**
- Decodes PVTGeodetic data block. Implementation of the SBF data transfer protocol.
- */
- int decodePVTGeodetic(unsigned char *sbfMsg);
-
- /**
- Decodes PosCovCartesian data block. Implementation of the SBF data transfer protocol.
- */
- int decodePosCovCartesian(unsigned char *sbfMsg);
-
- /**
- Decodes DOP data block. Implementation of the SBF data transfer protocol.
- */
- int decodeDOP(unsigned char *sbfMsg);
-
- /**
- computes the crc-ccitt of buf with polynomic 0x1021
- */
- //unsigned short int computeCrc(unsigned char *buf, unsigned short int nBytes);
- //unsigned short int CRC_compute16CCITT(const void * buf, size_t buf_length);
- unsigned short int CRC_compute16CCITT(unsigned char *buf, unsigned short int nBytes);
-
- /**
- Computes the matrix to transform positions from ECEF-WGS to a local coordinate frame.
- Local coordinate frame origin should be set by public function setMapOrigin().
- */
- void computeTM();
-
- /**
- Computes the matrix to transform positions from ECEF-WGS to the local tangential plane of the current position folloing the East North Up (ENU) axis
- This matris is useful to output a covariance matrix of the WGS position following the ENU local tangential coordinates
- */
- void computeTMenu();
-
- /**
- Transforms ECEF-WGS positions, velocities and covariances to local coordinate frame.
- Local coordinate frame origin shouold be set by public function setMapOrigin().
- */
- void fromWgsToMap();
-
- /**
- Computes covariance elements of the position referenced to the ENU axis of the current local tangential plane
- */
- void computeENUcovs();
-
- /**
- Sets to true the received field of the member in blockList that have blockId = id
- */
- void markBlockAsReceived(unsigned short int id);
+ /**
+ computes the crc-ccitt of buf with polynomic 0x1021
+ */
+ unsigned short int compute_crc(unsigned char *data,unsigned short int length);
};
+
#endif
diff --git a/src/examples/testasterx1.cpp b/src/examples/testasterx1.cpp
index 0ef7afd7dab8a5f1fa0a8c619a7f9e605579acc8..a64735ddd6b0a862ff32a31a909c0be80dff3765 100644
--- a/src/examples/testasterx1.cpp
+++ b/src/examples/testasterx1.cpp
@@ -16,119 +16,179 @@ USB any port
*/
#include <stdlib.h>
-#include <asterx1_gps.h>
-
-using namespace std;
+#include "asterx1_gps.h"
+#include "stream_gps.h"
int main(int argc, char **argv)
{
- int opt;
- int ii;
- int retValue;
- int nIterations = 10;
- int rate = MSEC500; //ONCHANGE, MSEC20, ... , MSEC100, ... SEC1, SEC5
- string serial_device="/dev/ttyACM0";
- streamsize tsDigits;
- CasteRx1 *myGps;
+ CasteRx1 myGps("gps","/dev/ttyACM0");
+ CEventServer *event_server=CEventServer::instance();
+ std::list<std::string> events;
+ unsigned int i=0,index;
+ TMeasEpoch meas_epoch;
+ TMeasExtra meas_extra;
+ TGPSNav gps_nav;
+ TGPSAlm gps_alm;
+ TGPSIon gps_ion;
+ TGPSUtc gps_utc;
+ TPVTCartesian pvt_cartesian;
+ TPVTGeodetic pvt_geodetic;
+ TPVTCov pvt_pos_cov_cartesian;
+ TPVTCov pvt_vel_cov_cartesian;
+ TPVTCov pvt_pos_cov_geodetic;
+ TPVTCov pvt_vel_cov_geodetic;
+ TPVTDOP pvt_dop;
+
+ std::cout << "\n\n TEST AsteRx1 GPS RECEIVER \n\n" << std::endl;
- cout << "\n\n TEST AsteRx1 GPS RECEIVER \n\n" << endl;
+ try{
+ myGps.openDevice();
+ events.push_back(myGps.get_new_meas_data_event_id());
+ events.push_back(myGps.get_new_gps_nav_data_event_id());
+ events.push_back(myGps.get_new_gps_alm_data_event_id());
+ events.push_back(myGps.get_new_gps_ion_data_event_id());
+ events.push_back(myGps.get_new_gps_utc_data_event_id());
+ events.push_back(myGps.get_new_pvt_data_event_id());
+ myGps.startAcquisition();
+ //for(i=0;i<100;i++)
+ for(;;)
+ {
+ index=event_server->wait_first(events,10000);
+ if(index==0)
+ {
+ myGps.get_meas_data(meas_epoch,meas_extra);
+// std::cout << meas_epoch << std::endl;
+// std::cout << meas_extra << std::endl;
+ }
+ else if(index==1)
+ {
+ myGps.get_gps_nav_data(gps_nav);
+ std::cout << gps_nav << std::endl;
+ }
+ else if(index==2)
+ {
+ myGps.get_gps_alm_data(gps_alm);
+ std::cout << gps_alm << std::endl;
+ }
+ else if(index==3)
+ {
+ myGps.get_gps_ion_data(gps_ion);
+ std::cout << gps_ion << std::endl;
+ }
+ else if(index==4)
+ {
+ myGps.get_gps_utc_data(gps_utc);
+ std::cout << gps_utc << std::endl;
+ }
+ else if(index==5)
+ {
+ std::cout << "GPS PVT" << std::endl;
+ myGps.get_pvt_data(pvt_cartesian,pvt_geodetic);
+ myGps.get_pvt_cov_data(pvt_pos_cov_cartesian,pvt_pos_cov_geodetic,pvt_vel_cov_cartesian,pvt_vel_cov_geodetic);
+ myGps.get_pvt_dop_data(pvt_dop);
+ }
+ }
+ myGps.stopAcquisition();
+ }catch(CException &e){
+ std::cout << e.what() << std::endl;
+ }
- //cout << "acqPeriodValues[ONCHANGE]: " << acqPeriodValues[ONCHANGE] << endl;
-
- // Argument management
- while ((opt = getopt(argc, argv, "u:n:r:h?"))!= -1)
+ //cout << "acqPeriodValues[ONCHANGE]: " << acqPeriodValues[ONCHANGE] << endl;
+
+ // Argument management
+/* while ((opt = getopt(argc, argv, "u:n:r:h?"))!= -1)
+ {
+ switch (opt)
+ {
+ case 'u': // change USB device
+ serial_device = optarg;
+ break;
+ case 'n':
+ nIterations = atoi(optarg);
+ break;
+ case 'r':
+ rate = atoi(optarg);
+ break;
+ case '?': // help
+ case 'h':
+ default:
+ cout << " USAGE" << endl
+ << " " << argv[0] << " [options]" << endl
+ << " OPTIONS" << endl
+ << " -u SERIAL_DEVICE (default: " << serial_device << ")" << endl
+ << " -n NUMBER OF ITERATIONS (default: " << nIterations << ")" << endl
+ << " -r ACQUISITION RATE: (default: " << rate << ")" << endl;
+ for (ii=0; ii<10; ii++)
{
- switch (opt)
- {
- case 'u': // change USB device
- serial_device = optarg;
- break;
- case 'n':
- nIterations = atoi(optarg);
- break;
- case 'r':
- rate = atoi(optarg);
- break;
- case '?': // help
- case 'h':
- default:
- cout << " USAGE" << endl
- << " " << argv[0] << " [options]" << endl
- << " OPTIONS" << endl
- << " -u SERIAL_DEVICE (default: " << serial_device << ")" << endl
- << " -n NUMBER OF ITERATIONS (default: " << nIterations << ")" << endl
- << " -r ACQUISITION RATE: (default: " << rate << ")" << endl;
- for (ii=0; ii<10; ii++)
- {
- cout << " " << ii << ": " << acqPeriodValues[ii] << endl;
- }
- cout << endl;
- return 1;
- }
+ cout << " " << ii << ": " << acqPeriodValues[ii] << endl;
}
-
- myGps = new CasteRx1(serial_device,rate);
-
- myGps->setMapOrigin(41.388595, 2.113133, 120, 44.2); //sets map origin for coordinate transformation
- //myGps->printTM(); //prints to stdout the transformation matrix
-
- retValue = myGps->openDevice();//open device comm's
-
- retValue = myGps->closeDevice();//close device comm's (stressing tests ...)
- retValue = myGps->openDevice();//open device comm's, again
-
- myGps->setPortName("/dev/ttyS1"); //checking that when device is open, port configuration can't be achieved
- myGps->setAcquisitionRate(56); //checking that when invalid rate indexes prdouces an error and don't change the rate
- myGps->setAcquisitionRate(SEC2); //checking that when device is open rate configuration can be achieved
- myGps->setAcquisitionRate(rate); //checking that when device is open rate configuration can be achieved
-
- if ( retValue == BASIC_SUCCESS )
+ cout << endl;
+ return 1;
+ }
+ }
+
+ myGps = new CasteRx1(serial_device,rate);
+
+ myGps->setMapOrigin(41.388595, 2.113133, 120, 44.2); //sets map origin for coordinate transformation
+ //myGps->printTM(); //prints to stdout the transformation matrix
+
+ retValue = myGps->openDevice();//open device comm's
+
+ retValue = myGps->closeDevice();//close device comm's (stressing tests ...)
+ retValue = myGps->openDevice();//open device comm's, again
+
+ myGps->setPortName("/dev/ttyS1"); //checking that when device is open, port configuration can't be achieved
+ myGps->setAcquisitionRate(56); //checking that when invalid rate indexes prdouces an error and don't change the rate
+ myGps->setAcquisitionRate(SEC2); //checking that when device is open rate configuration can be achieved
+ myGps->setAcquisitionRate(rate); //checking that when device is open rate configuration can be achieved
+
+ if ( retValue == BASIC_SUCCESS )
+ {
+ retValue = myGps->startAcquisition();//start data acquisition
+ retValue = myGps->stopAcquisition();//stop data acquisition (stressing tests ...)
+ retValue = myGps->startAcquisition();//start data acquisition, again
+ if ( retValue == BASIC_SUCCESS )
+ {
+ for (ii=0; ii<nIterations; ii++)
+ {
+ cout << endl;
+ myGps->readDataFromDevice();
+ //myGps->printData(cout); //prints to cout all current data in a single row. (cout can be repolaced by another ostream object reference)
+ cout << "Status = " << myGps->getStatus() << endl;
+ tsDigits=cout.precision(12);
+ cout << "TimeStamp = " << myGps->getTimeStamp() << endl;
+ cout.precision(tsDigits);
+ cout << "PVT error = " << myGps->getPVTerror() << endl;
+ cout << "TOW = " << myGps->getTow() << endl;
+
+ if ( myGps->getStatus() == ALL_OK )
{
- retValue = myGps->startAcquisition();//start data acquisition
- retValue = myGps->stopAcquisition();//stop data acquisition (stressing tests ...)
- retValue = myGps->startAcquisition();//start data acquisition, again
- if ( retValue == BASIC_SUCCESS )
- {
- for (ii=0; ii<nIterations; ii++)
- {
- cout << endl;
- myGps->readDataFromDevice();
- //myGps->printData(cout); //prints to cout all current data in a single row. (cout can be repolaced by another ostream object reference)
- cout << "Status = " << myGps->getStatus() << endl;
- tsDigits=cout.precision(12);
- cout << "TimeStamp = " << myGps->getTimeStamp() << endl;
- cout.precision(tsDigits);
- cout << "PVT error = " << myGps->getPVTerror() << endl;
- cout << "TOW = " << myGps->getTow() << endl;
-
- if ( myGps->getStatus() == ALL_OK )
- {
- cout << "Num Of Satellites = " << myGps->getNumSatellites() << endl;
- tsDigits=cout.precision(12);
- cout << "lat = " << myGps->getLat(inDEGREES) << endl;
- cout << "lon = " << myGps->getLon(inDEGREES) << endl;
- cout.precision(tsDigits);
- cout << "alt = " << myGps->getAlt() << endl;
- cout << "xWgs = " << myGps->getXWgs() << endl;
- cout << "yWgs = " << myGps->getYWgs() << endl;
- cout << "zWgs = " << myGps->getZWgs() << endl;
- cout << "xMap = " << myGps->getXMap() << endl;
- cout << "yMap = " << myGps->getYMap() << endl;
- cout << "zMap = " << myGps->getZMap() << endl;
- cout << "vxMap = " << myGps->getVxMap() << endl;
- cout << "vyMap = " << myGps->getVyMap() << endl;
- cout << "vzMap = " << myGps->getVzMap() << endl;
- cout << "PDOP = " << myGps->getPDOP() << endl;
- }
- }
-
- retValue = myGps->stopAcquisition();
- }
-
- retValue = myGps->closeDevice();
+ cout << "Num Of Satellites = " << myGps->getNumSatellites() << endl;
+ tsDigits=cout.precision(12);
+ cout << "lat = " << myGps->getLat(inDEGREES) << endl;
+ cout << "lon = " << myGps->getLon(inDEGREES) << endl;
+ cout.precision(tsDigits);
+ cout << "alt = " << myGps->getAlt() << endl;
+ cout << "xWgs = " << myGps->getXWgs() << endl;
+ cout << "yWgs = " << myGps->getYWgs() << endl;
+ cout << "zWgs = " << myGps->getZWgs() << endl;
+ cout << "xMap = " << myGps->getXMap() << endl;
+ cout << "yMap = " << myGps->getYMap() << endl;
+ cout << "zMap = " << myGps->getZMap() << endl;
+ cout << "vxMap = " << myGps->getVxMap() << endl;
+ cout << "vyMap = " << myGps->getVyMap() << endl;
+ cout << "vzMap = " << myGps->getVzMap() << endl;
+ cout << "PDOP = " << myGps->getPDOP() << endl;
}
+ }
+
+ retValue = myGps->stopAcquisition();
+ }
+
+ retValue = myGps->closeDevice();
+ }
+
+ delete myGps;*/
- delete myGps;
-
- return 0;
+ return 0;
}
diff --git a/src/gps_types.h b/src/gps_types.h
new file mode 100644
index 0000000000000000000000000000000000000000..cd5ec73fd315e3e2287e02574187a472ab025229
--- /dev/null
+++ b/src/gps_types.h
@@ -0,0 +1,870 @@
+#ifndef _GPS_TYPES_H
+#define _GPS_TYPES_H
+
+// global satellite information
+typedef struct{
+ unsigned int tow;
+ unsigned short int wnc;
+}TBlockTimeStamp;
+
+// User data structures
+typedef enum {elev_setting=0,elev_rising=1,elev_unknown=3} rise_set_t;
+
+typedef enum {health_unknown=0, healthy=1, unhealthy=3} health_t;
+
+typedef enum {track_idle=0, track_search=1, track_sync=2, tracking=3} track_status_t;
+
+typedef enum {pvt_not_used=0, pvt_waiting=1,pvt_used=2, pvt_rejected=3} pvt_status_t;
+
+typedef enum {main_antenna=0,aux1_antenna=1,aux2_antenna=2} antenna_t;
+
+typedef struct {
+ antenna_t antenna_id;
+ track_status_t track_status;
+ pvt_status_t pvt_status;
+}TChannelStateInfo;
+
+typedef struct {
+ int sat_id;
+ unsigned int azimuth;
+ rise_set_t rise_set;
+ health_t health;
+ int elevation;
+ std::vector<TChannelStateInfo> state;
+}TChannelSatInfo;
+
+typedef struct {
+ TBlockTimeStamp gps_timestamp;
+ double local_timestamp;
+ std::vector<TChannelSatInfo> info;
+}TChannelStatus;
+
+typedef enum {gps_sbas_galileo_l1=0,glonass_l1=1,galileo_e6=2,gps_l2=3,glonass_l2=4,gps_sbas_galileo_l5_e5a=5,galileo_e5b=6,galileo_e5a_b=7} frontend_t;
+
+enum {vbat_ok=0x00000001,
+ antenna_connected=0x00000001,
+ ext_ref=0x00000002,
+ pps_in=0x00000004,
+ wn_set=0x00000008,
+ tow_set=0x00000010,
+ fine_time=0x00000020,
+ disk_activity=0x00000040,
+ disk_full=0x00000080,
+ disk_mounted=0x00000100};
+
+enum {nvram=0x00000001,
+ baseband=0x00000002,
+ rtc=0x00000004,
+ software=0x00000008,
+ watchdog=0x00000010,
+ antenna_ok=0x00000020,
+ congestion=0x00000040,
+ sis_error=0x00000080,
+ missed_event=0x00000100,
+ cpu_overload=0x00000200,
+ invalid_config=0x00000400};
+
+typedef struct {
+ frontend_t frontend_id;
+ antenna_t antenna_id;
+ int gain;
+ int sample_var;
+ int blanking_stat;
+}TAGCData;
+
+typedef struct {
+ TBlockTimeStamp gps_timestamp;
+ double local_timestamp;
+ unsigned int cpu_load;
+ unsigned int up_time;
+ unsigned int state;
+ unsigned int error;
+ std::vector<TAGCData> agc;
+}TReceiverStatus;
+
+typedef enum {almanac=1, ephemeris=2, unkown_info=255} sat_info_t;
+
+typedef struct {
+ TBlockTimeStamp gps_timestamp;
+ double local_timestamp;
+ unsigned int sat_id;
+ double azimuth;
+ double elevation;
+ rise_set_t rise_set;
+ sat_info_t sat_info;
+}TSatInfo;
+
+typedef enum {GPS_L1_CA=0,GPS_L1_PY=1,GPS_L2_PY=2,GPS_L2C=3,GPS_L5=4,GLO_L1_CA=8,GLO_L1_P=9,GLO_L2_P=10,GLO_L2_CA=11,GAL_L1A=16,
+ GAL_L1BC=17,GAL_E6B=18,GAL_E6BC=19,GAL_E5a=20,GAL_E5b=21,GAL_E5=22,GEO_L1CA=24} signal_t;
+
+const std::string signal_type_name[] = {"GPS_L1_CA (1575.42 MHz)",
+ "GPS_L1_P(Y) (1575.42 MHz)",
+ "GPS_L2_P(Y) (1227.60 MHz)",
+ "GPS_L2C (1227.60 MHz)",
+ "GPS_L5 (1176.45 MHz)",
+ "",
+ "",
+ "",
+ "GLO_L1_CA",
+ "GLO_L1_P",
+ "GLO_L2_P",
+ "GLO_L1_CA",
+ "",
+ "",
+ "",
+ "",
+ "GAL_L1A (1575.42 MHz)",
+ "GAL_L1BC (1575.42 MHz)",
+ "GAL_E6A (1278.75 MHz)",
+ "GAL_E6BC (1278.75 MHz)",
+ "GAL_E5a (1176.45 MHz)",
+ "GAL_E5b (1207.14 MHz)",
+ "GAL_E5 (1191.795 MHz)",
+ "",
+ "GEO_L1CA (1575.42 MHz)"};
+
+typedef struct{
+ signal_t signal_type;
+ antenna_t antenna_id;
+ unsigned int lock_time;
+ double cn0;
+ double pseudo_range;
+ double doppler;
+ double carrier_phase;
+ bool pseudorange_smoothed;
+ bool smoothing_int_reached;
+ bool half_cycle_ambiguity;
+}TMeasEpochType2;
+
+typedef struct{
+ unsigned int rx_channel;
+ signal_t signal_type;
+ antenna_t antenna_id;
+ unsigned int sat_id;
+ double pseudo_range;
+ double doppler;
+ double carrier_phase;
+ double cn0;
+ unsigned int lock_time;
+ bool pseudorange_smoothed;
+ bool smoothing_int_reached;
+ bool half_cycle_ambiguity;
+ std::vector<TMeasEpochType2> type2_data;
+}TMeasEpochType1;
+
+enum {multipath_mitigation=0x01,smoothing_of_code=0x02};
+
+typedef struct{
+ TBlockTimeStamp gps_timestamp;
+ double local_timestamp;
+ unsigned int flags;
+ std::vector<TMeasEpochType1> type1_data;
+}TMeasEpoch;
+
+typedef struct{
+ unsigned int rx_channel;
+ signal_t signal_type;
+ antenna_t antenna_id;
+ int mp_correction;
+ int smoothing_correction;
+ unsigned int code_var;
+ unsigned int carrier_var;
+ unsigned int lock_time;
+ unsigned int cum_lost_cont;
+}TMeasExtraBlock;
+
+typedef struct{
+ TBlockTimeStamp gps_timestamp;
+ double local_timestamp;
+ float doppler_var_factor;
+ std::vector<TMeasExtraBlock> blocks;
+}TMeasExtra;
+
+typedef struct{
+ TBlockTimeStamp gps_timestamp;
+ double local_timestamp;
+ unsigned int sat_id;
+ int wn;
+ unsigned int ca_or_pon_l2;
+ unsigned int ura;
+ unsigned int health;
+ unsigned int l2_data_flag;
+ unsigned int iodc;
+ unsigned int iode2;
+ unsigned int iode3;
+ unsigned int fit_int_flag;
+ double t_gd;
+ unsigned int t_oc;
+ double a_f2;
+ double a_f1;
+ double a_f0;
+ double c_rs;
+ double delta_n;
+ double m_0;
+ double c_uc;
+ double e;
+ double c_us;
+ double sqrt_a;
+ unsigned int t_oe;
+ double c_ic;
+ double omega_0;
+ double c_is;
+ double i_0;
+ double c_rc;
+ double omega;
+ double omega_dot;
+ double i_dot;
+ unsigned int wnt_oc;
+ unsigned int wnt_oe;
+}TGPSNav;
+
+typedef struct{
+ TBlockTimeStamp gps_timestamp;
+ double local_timestamp;
+ unsigned int sat_id;
+ double e;
+ unsigned int t_oa;
+ double delta_i;
+ double omega_dot;
+ double sqrt_a;
+ double omega_0;
+ double omega;
+ double m_0;
+ double a_f1;
+ double a_f0;
+ unsigned int wn_a;
+ unsigned int as_config;
+ unsigned int health8;
+ unsigned int health6;
+}TGPSAlm;
+
+typedef struct{
+ TBlockTimeStamp gps_timestamp;
+ double local_timestamp;
+ unsigned int sat_id;
+ double alpha_0;
+ double alpha_1;
+ double alpha_2;
+ double alpha_3;
+ double beta_0;
+ double beta_1;
+ double beta_2;
+ double beta_3;
+}TGPSIon;
+
+typedef struct{
+ TBlockTimeStamp gps_timestamp;
+ double local_timestamp;
+ unsigned int sat_id;
+ double a_1;
+ double a_0;
+ unsigned int t_ot;
+ unsigned int wn_t;
+ int del_t_ls;
+ unsigned int wn_lsf;
+ unsigned int dn;
+ int del_t_lsf;
+}TGPSUtc;
+
+typedef enum {no_pvt=0,stand_alone_pvt=1,diff_pvt=2,fixed_loc=3,rtk_fixed_amb=4,rtk_float_amb=5,sbas_pvt=6} pvt_mode_t;
+
+typedef enum {pvt_3d=0,pvt_2d=1} solution_t;
+
+enum {no_error=0,not_enough_meas=1,not_enough_eph=2,dop_too_large=3,res_too_large=4,no_conv=5,not_enough_meas_outliers=6,export_laws=7,not_enough_diff_corr=8,no_base_coord=9};
+
+typedef enum {gps_time=0,galileo_time=1} sys_time_t;
+
+typedef enum {wgs_84=0,galileo=1,pz_90_02=2,user_datum1=250,user_datum2=251} datum_t;
+
+typedef struct{
+ TBlockTimeStamp gps_timestamp;
+ double local_timestamp;
+ pvt_mode_t mode;
+ solution_t solution;
+ unsigned int error;
+ double x;
+ double y;
+ double z;
+ double undulation;
+ double vx;
+ double vy;
+ double vz;
+ double cog;
+ double rx_clk_bias;
+ double rx_clk_drift;
+ sys_time_t time_system;
+ datum_t datum;
+ unsigned int num_sat;
+ unsigned int wa_corr_info;
+ bool orbit_sat_clock_corr;
+ bool range_corr_info;
+ bool ion_info;
+ bool orbit_accuracy_info;
+ bool DO299_used;
+ unsigned int reference_id;
+ unsigned int mean_corr_age;
+ unsigned int signal_info;
+ bool integrity_mon;
+ bool integrity_failed_RAIM;
+ bool integrity_failed_HPCA;
+}TPVTCartesian;
+
+typedef struct{
+ TBlockTimeStamp gps_timestamp;
+ double local_timestamp;
+ pvt_mode_t mode;
+ solution_t solution;
+ unsigned int error;
+ double latitude;
+ double longitude;
+ double h;
+ double undulation;
+ double vn;
+ double ve;
+ double vu;
+ double cog;
+ double rx_clk_bias;
+ double rx_clk_drift;
+ sys_time_t time_system;
+ datum_t datum;
+ unsigned int num_sat;
+ unsigned int wa_corr_info;
+ bool orbit_sat_clock_corr;
+ bool range_corr_info;
+ bool ion_info;
+ bool orbit_accuracy_info;
+ bool DO299_used;
+ unsigned int reference_id;
+ unsigned int mean_corr_age;
+ unsigned int signal_info;
+ bool integrity_mon;
+ bool integrity_failed_RAIM;
+ bool integrity_failed_HPCA;
+}TPVTGeodetic;
+
+typedef struct{
+ TBlockTimeStamp gps_timestamp;
+ double local_timestamp;
+ pvt_mode_t mode;
+ solution_t solution;
+ unsigned int error;
+ double cov_00;
+ double cov_11;
+ double cov_22;
+ double cov_33;
+ double cov_01;
+ double cov_02;
+ double cov_03;
+ double cov_12;
+ double cov_13;
+ double cov_23;
+}TPVTCov;
+
+typedef struct
+{
+ TBlockTimeStamp gps_timestamp;
+ double local_timestamp;
+ unsigned int num_sat;
+ unsigned int p_dop;
+ unsigned int t_dop;
+ unsigned int h_dop;
+ unsigned int v_dop;
+ double hpl;
+ double vpl;
+}TPVTDOP;
+
+// these precompiler commands force the byte aligment in memory of the data structures
+//#pragma pack (push, 1)
+//#pragma pack (pop)
+
+// block ID structure
+#pragma pack (push, 1)
+typedef struct{
+ uint16_t id:13;
+ uint16_t rev:3;
+}TGPSBlockId;
+#pragma pack (pop)
+
+// header structure
+#pragma pack (push, 1)
+typedef struct{
+ uint8_t sync[2];
+ uint16_t crc;
+ TGPSBlockId id;
+ uint16_t length;
+}TGPSBlockHeader;
+#pragma pack (pop)
+
+// block time stamp structure
+#pragma pack (push, 1)
+typedef struct{
+ uint32_t tow;
+ uint16_t wnc;
+}TGPSBlockTimeStamp;
+#pragma pack (pop)
+
+// Measure Epoch structure
+#pragma pack (push, 1)
+typedef struct{
+ uint8_t master_signal_type:5;
+ uint8_t antenna_id:3;
+}TGPSMeasBlockType;
+#pragma pack (pop)
+
+#pragma pack (push, 1)
+typedef struct{
+ uint8_t smoothed:1;
+ uint8_t smoothing_int_reached:1;
+ uint8_t phase_ambiguity:1;
+ uint8_t freq_nr:5;
+}TGPSMeasBlockObsInfo;
+#pragma pack (pop)
+
+#pragma pack (push, 1)
+typedef struct{
+ uint8_t code_offset_msb:3;
+ uint8_t doppler_offset_msb:5;
+}TGPSMeasBlockType2Offsets;
+#pragma pack (pop)
+
+#pragma pack (push, 1)
+typedef struct{
+ TGPSMeasBlockType type;
+ uint8_t lock_time;
+ uint8_t cn0;
+ TGPSMeasBlockType2Offsets offsets_msb;
+ int8_t carrier_msb;
+ TGPSMeasBlockObsInfo info;
+ uint16_t code_offset_lsb;
+ uint16_t carrier_lsb;
+ uint16_t doppler_offset_lsb;
+}TGPSMeasBlockType2;
+#pragma pack (pop)
+
+#pragma pack (push, 1)
+typedef struct{
+ uint8_t msb:4;
+ uint8_t reserved:4;
+ uint32_t lsb;
+}TGPSMeasBlockType1Code;
+#pragma pack (pop)
+
+#pragma pack (push, 1)
+typedef struct{
+ uint16_t lsb;
+ int8_t msb;
+}TGPSMeasBlockType1Carrier;
+#pragma pack (pop)
+
+#pragma pack (push, 1)
+typedef struct{
+ uint8_t rx_channel;
+ TGPSMeasBlockType type;
+ uint8_t svid;
+ TGPSMeasBlockType1Code code;
+ int32_t doppler;
+ TGPSMeasBlockType1Carrier carrier;
+ uint8_t cn0;
+ uint16_t lock_time;
+ TGPSMeasBlockObsInfo info;
+ uint8_t num_type2;
+}TGPSMeasBlockType1;
+#pragma pack (pop)
+
+#pragma pack (push, 1)
+typedef struct{
+ TGPSBlockTimeStamp timestamp;
+ uint8_t num_type1;
+ uint8_t length_type1;
+ uint8_t length_type2;
+ uint8_t common_flags;
+ uint8_t reserved[2];
+}TGPSMeasEpoch;
+#pragma pack (pop)
+
+// Measurement extra structure
+#pragma pack (push, 1)
+typedef struct{
+ uint8_t rx_channel;
+ TGPSMeasBlockType type;
+ int16_t mp_correction;
+ int16_t smoothing_correction;
+ uint16_t code_var;
+ uint16_t carrier_var;
+ uint16_t lock_time;
+ uint8_t cum_lost_cont;
+ uint8_t reserved[3];
+}TGPSMeasExtraBlock;
+#pragma pack (pop)
+
+#pragma pack (push, 1)
+typedef struct{
+ TGPSBlockTimeStamp timestamp;
+ uint8_t num_blocks;
+ uint8_t length_blocks;
+ float doppler_var_factor;
+}TGPSMeasExtra;
+#pragma pack (pop)
+
+// measurement end structure
+#pragma pack (push, 1)
+typedef struct{
+ TGPSBlockTimeStamp timestamp;
+}TGPSEndOfMeas;
+#pragma pack (pop)
+
+// GPS NAV structure
+#pragma pack (push, 1)
+typedef struct{
+ TGPSBlockTimeStamp timestamp;
+ uint8_t prn;
+ uint8_t reserved1;
+ int16_t wn;
+ uint8_t ca_or_pon_l2;
+ uint8_t ura;
+ uint8_t health;
+ uint8_t l2_data_flag;
+ uint16_t iodc;
+ uint8_t iode2;
+ uint8_t iode3;
+ uint8_t fit_int_flag;
+ uint8_t reserved2;
+ float t_gd;
+ uint32_t t_oc;
+ float a_f2;
+ float a_f1;
+ float a_f0;
+ float c_rs;
+ float delta_n;
+ double m_0;
+ float c_uc;
+ double e;
+ float c_us;
+ double sqrt_a;
+ uint32_t t_oe;
+ float c_ic;
+ double omega_0;
+ float c_is;
+ double i_0;
+ float c_rc;
+ double omega;
+ float omega_dot;
+ float i_dot;
+ uint16_t wnt_oc;
+ uint16_t wnt_oe;
+}TGPSGPSNav;
+#pragma pack (pop)
+
+// APG Alm structure
+#pragma pack (push, 1)
+typedef struct{
+ TGPSBlockTimeStamp timestamp;
+ uint8_t prn;
+ uint8_t reserved1;
+ float e;
+ uint32_t t_oa;
+ float delta_i;
+ float omega_dot;
+ float sqrt_a;
+ float omega_0;
+ float omega;
+ float m_0;
+ float a_f1;
+ float a_f0;
+ uint8_t wn_a;
+ uint8_t as_config;
+ uint8_t health8;
+ uint8_t health6;
+}TGPSGPSAlm;
+#pragma pack (pop)
+
+// GPS Ion structure
+#pragma pack (push, 1)
+typedef struct{
+ TGPSBlockTimeStamp timestamp;
+ uint8_t prn;
+ uint8_t reserved1;
+ float alpha_0;
+ float alpha_1;
+ float alpha_2;
+ float alpha_3;
+ float beta_0;
+ float beta_1;
+ float beta_2;
+ float beta_3;
+}TGPSGPSIon;
+#pragma pack (pop)
+
+// GPs UTC structure
+#pragma pack (push, 1)
+typedef struct{
+ TGPSBlockTimeStamp timestamp;
+ uint8_t prn;
+ uint8_t reserved1;
+ float a_1;
+ double a_0;
+ uint32_t t_ot;
+ uint8_t wn_t;
+ int8_t del_t_ls;
+ uint8_t wn_lsf;
+ uint8_t dn;
+ int8_t del_t_lsf;
+}TGPSGPSUtc;
+#pragma pack (pop)
+
+// Position, velocity and time Cartesian structure
+#pragma pack (push, 1)
+typedef struct{
+ uint8_t mode:4;
+ uint8_t reserved:3;
+ uint8_t solution_type:1;
+}TGPSPVTMode;
+#pragma pack (pop)
+
+#pragma pack (push, 1)
+typedef struct{
+ uint8_t orbit_clock_corr:1;
+ uint8_t range_corr:1;
+ uint8_t ion_info:1;
+ uint8_t orbit_acc_info:1;
+ uint8_t do229_mode:1;
+ uint8_t reserver:3;
+}TGPSPVTInfo;
+#pragma pack (pop)
+
+#pragma pack (push, 1)
+typedef struct{
+ uint8_t int_mon:1;
+ uint8_t int_failed_raim:1;
+ uint8_t int_failed_hpca:1;
+ uint8_t galileo_storm_flag:1;
+ uint8_t reserved:4;
+}TGPSPVTAlert;
+#pragma pack (pop)
+
+#pragma pack (push, 1)
+typedef struct{
+ TGPSBlockTimeStamp timestamp;
+ TGPSPVTMode mode;
+ uint8_t error;
+ double x;
+ double y;
+ double z;
+ float undulation;
+ float vx;
+ float vy;
+ float vz;
+ float cog;
+ double rx_clk_bias;
+ float rx_clk_drift;
+ uint8_t time_system;
+ uint8_t datum;
+ uint8_t num_sat;
+ uint8_t wa_corr_info;
+ TGPSPVTInfo corr_info;
+ uint16_t reference_id;
+ uint16_t mean_corr_age;
+ uint32_t signal_info;
+ TGPSPVTAlert alert_flag;
+ uint8_t reserved[3];
+}TGPSPVTCartesian;
+#pragma pack (pop)
+
+// Position, velocity and time Geodetic structure
+#pragma pack (push, 1)
+typedef struct{
+ TGPSBlockTimeStamp timestamp;
+ TGPSPVTMode mode;
+ uint8_t error;
+ double latitude;
+ double longitude;
+ double h;
+ float undulation;
+ float vn;
+ float ve;
+ float vu;
+ float cog;
+ double rx_clk_bias;
+ float rx_clk_drift;
+ uint8_t time_system;
+ uint8_t datum;
+ uint8_t num_sat;
+ uint8_t wa_corr_info;
+ TGPSPVTInfo corr_info;
+ uint16_t reference_id;
+ uint16_t mean_corr_age;
+ uint32_t signal_info;
+ TGPSPVTAlert alert_flag;
+ uint8_t reserved[3];
+}TGPSPVTGeodetic;
+#pragma pack (pop)
+
+// Position, velocity and time Covariance Cartesian structure
+#pragma pack (push, 1)
+typedef struct{
+ TGPSBlockTimeStamp timestamp;
+ TGPSPVTMode mode;
+ uint8_t error;
+ float cov_00;
+ float cov_11;
+ float cov_22;
+ float cov_33;
+ float cov_01;
+ float cov_02;
+ float cov_03;
+ float cov_12;
+ float cov_13;
+ float cov_23;
+}TGPSPVTCov;
+#pragma pack (pop)
+
+// Position, velocity and time DOP structure
+#pragma pack (push, 1)
+typedef struct
+{
+ TGPSBlockTimeStamp timestamp;
+ uint8_t num_sat;
+ uint8_t reserved;
+ uint16_t p_dop;
+ uint16_t t_dop;
+ uint16_t h_dop;
+ uint16_t v_dop;
+ float hpl;
+ float vpl;
+}TGPSPVTDOP;
+#pragma pack (pop)
+
+#pragma pack (push, 1)
+typedef struct{
+ TGPSBlockTimeStamp timestamp;
+}TGPSEndOfPVT;
+#pragma pack (pop)
+
+// channel status structure
+#pragma pack (push, 1)
+typedef struct{
+ uint8_t antenna;
+ uint8_t reserved;
+ uint16_t tracking_status;
+ uint16_t pvt_status;
+ uint16_t pvy_info;
+}TGPSChannelStateInfo;
+#pragma pack (pop)
+
+#pragma pack (push, 1)
+typedef struct{
+ uint16_t azimuth:9;
+ uint16_t reserved:5;
+ uint16_t rise_set:2;
+}TGPSAzimuthRiseSet;
+#pragma pack (pop)
+
+#pragma pack (push, 1)
+typedef struct{
+ uint8_t sat_id;
+ uint8_t freq_nr;
+ uint16_t reserved1;
+ TGPSAzimuthRiseSet azimuth_rise_set;
+ uint16_t health;
+ int8_t elevation;
+ uint8_t num_state_info;
+ uint8_t rx_channel;
+ uint8_t reserved2;
+}TGPSChannelSatInfo;
+#pragma pack (pop)
+
+#pragma pack (push, 1)
+typedef struct{
+ TGPSBlockTimeStamp timestamp;
+ uint8_t num_ch;
+ uint8_t length_ch_sat_info;
+ uint8_t length_ch_state_info;
+ uint8_t reserved[3];
+}TGPSChannelStatus;
+#pragma pack (pop)
+
+// Receiver status structure
+#pragma pack (push, 1)
+typedef struct{
+ uint32_t vbat_ok:1;
+ uint32_t antenna_connected:1;
+ uint32_t ext_ref:1;
+ uint32_t pps_in:1;
+ uint32_t wn_set:1;
+ uint32_t tow_set:1;
+ uint32_t fine_time:1;
+ uint32_t disk_activity:1;
+ uint32_t disk_full:1;
+ uint32_t disk_mounted:1;
+ uint32_t reserved:22;
+}TGPSReceiverStatusRxState;
+#pragma pack (pop)
+
+#pragma pack (push, 1)
+typedef struct{
+ uint32_t nvram:1;
+ uint32_t baseband:1;
+ uint32_t rtc:1;
+ uint32_t software:1;
+ uint32_t watchdog:1;
+ uint32_t antenna_ok:1;
+ uint32_t congestion:1;
+ uint32_t sis_error:1;
+ uint32_t missed_event:1;
+ uint32_t cpu_overload:1;
+ uint32_t invalid_config:1;
+ uint32_t reserved:21;
+}TGPSReceiverStatusRxError;
+#pragma pack (pop)
+
+#pragma pack (push, 1)
+typedef struct{
+ uint8_t code:4;
+ uint8_t antenna_id:4;
+}TGPSAGCDataFrontendId;
+#pragma pack (pop)
+
+#pragma pack (push, 1)
+typedef struct{
+ TGPSAGCDataFrontendId frontend_id;
+ int8_t gain;
+ uint8_t sample_var;
+ uint8_t blanking_stat;
+}TGPSAGCData;
+#pragma pack (pop)
+
+#pragma pack (push, 1)
+typedef struct{
+ TGPSBlockTimeStamp timestamp;
+ uint8_t cpu_load;
+ uint8_t reserved1;
+ uint32_t up_time;
+ TGPSReceiverStatusRxState rx_state;
+ TGPSReceiverStatusRxError rx_error;
+ uint8_t num_agc_states;
+ uint8_t length_agc_states;
+ uint8_t reserved2[2];
+}TGPSReceiverStatus;
+#pragma pack (pop)
+
+// satellite visibility structure
+#pragma pack (push, 1)
+typedef struct{
+ uint8_t sat_id;
+ uint8_t freq_nr;
+ uint16_t azimuth;
+ int16_t elevation;
+ uint8_t rise_set;
+ uint8_t sat_info;
+}TGPSSatInfo;
+#pragma pack (pop)
+
+#pragma pack (push, 1)
+typedef struct{
+ TGPSBlockTimeStamp timestamp;
+ uint8_t num_sat;
+ uint8_t length_sat;
+}TGPSSatVisibility;
+#pragma pack (pop)
+
+#endif
diff --git a/src/stream_gps.cpp b/src/stream_gps.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d02a4b01bcc0ccce57fc3a7f75441d94099064f1
--- /dev/null
+++ b/src/stream_gps.cpp
@@ -0,0 +1,213 @@
+#include "stream_gps.h"
+
+std::ostream& operator<< (std::ostream& out, TMeasEpoch &meas_epoch)
+{
+ unsigned int i,j;
+
+ out << "Common flags: " << std::endl;
+ if(meas_epoch.flags&multipath_mitigation)
+ out << " Multipath mitigation enabled" << std::endl;
+ else
+ out << " Multipath mitigation disabled" << std::endl;
+ if(meas_epoch.flags&smoothing_of_code)
+ out << " At least one of the code measurements is smoothed" << std::endl;
+ else
+ out << " None of the code measurements are smoothed" << std::endl;
+ for(i=0;i<meas_epoch.type1_data.size();i++)
+ {
+ out << "Sattelite " << meas_epoch.type1_data[i].sat_id << " on channel " << meas_epoch.type1_data[i].rx_channel << std::endl;
+ out << " Master signal: " << signal_type_name[meas_epoch.type1_data[i].signal_type] << std::endl;
+ out << " Antenna: " << (int)meas_epoch.type1_data[i].antenna_id << std::endl;
+ out << " Pseudo range: " << meas_epoch.type1_data[i].pseudo_range << " m" << std::endl;
+ out << " Doppler: " << meas_epoch.type1_data[i].doppler << " Hz" << std::endl;
+ out << " Carrier phase: " << meas_epoch.type1_data[i].carrier_phase << " cycles" << std::endl;
+ out << " Carrier/noise ratio: " << meas_epoch.type1_data[i].cn0 << " dB-Hz" << std::endl;
+ out << " Lock time: " << meas_epoch.type1_data[i].lock_time << " s" << std::endl;
+ if(meas_epoch.type1_data[i].pseudorange_smoothed)
+ out << " Pseudo ranfe measurement is smoothed" << std::endl;
+ else
+ out << " Pseudo ranfe measurement is not smoothed" << std::endl;
+ if(meas_epoch.type1_data[i].smoothing_int_reached)
+ out << " The smoothing filter has reached the requested smoothing interval" << std::endl;
+ else
+ out << " The smoothing filter has not reached the requested smoothing interval" << std::endl;
+ if(meas_epoch.type1_data[i].half_cycle_ambiguity)
+ out << " Carrier phase has a half-cycle ambiguity" << std::endl;
+ else
+ out << " Carrier phase does not have a half-cycle ambiguity" << std::endl;
+ if(meas_epoch.type1_data[i].type2_data.size()>0)
+ {
+ for(j=0;j<meas_epoch.type1_data[i].type2_data.size();j++)
+ {
+ out << " Inter-channel differences" << std::endl;
+ out << " Master signal: " << signal_type_name[meas_epoch.type1_data[i].type2_data[j].signal_type] << std::endl;
+ out << " Antenna: " << (int)meas_epoch.type1_data[i].type2_data[j].antenna_id << std::endl;
+ out << " Pseudo range: " << meas_epoch.type1_data[i].type2_data[j].pseudo_range << " m" << std::endl;
+ out << " Doppler: " << meas_epoch.type1_data[i].type2_data[j].doppler << " Hz" << std::endl;
+ out << " Carrier phase: " << meas_epoch.type1_data[i].type2_data[j].carrier_phase << " cycles" << std::endl;
+ out << " Carrier/noise ratio: " << meas_epoch.type1_data[i].type2_data[j].cn0 << " dB-Hz" << std::endl;
+ out << " Lock time: " << meas_epoch.type1_data[i].type2_data[j].lock_time << " s" << std::endl;
+ if(meas_epoch.type1_data[i].type2_data[j].pseudorange_smoothed)
+ out << " Pseudo ranfe measurement is smoothed" << std::endl;
+ else
+ out << " Pseudo ranfe measurement is not smoothed" << std::endl;
+ if(meas_epoch.type1_data[i].type2_data[j].smoothing_int_reached)
+ out << " The smoothing filter has reached the requested smoothing interval" << std::endl;
+ else
+ out << " The smoothing filter has not reached the requested smoothing interval" << std::endl;
+ if(meas_epoch.type1_data[i].type2_data[j].half_cycle_ambiguity)
+ out << " Carrier phase has a half-cycle ambiguity" << std::endl;
+ else
+ out << " Carrier phase does not have a half-cycle ambiguity" << std::endl;
+ }
+ }
+ }
+
+ return out;
+}
+
+std::ostream& operator<< (std::ostream& out, TMeasExtra &meas_extra)
+{
+ unsigned int i;
+
+ out << "Doppler variance factor: " << meas_extra.doppler_var_factor << " Hz2/cycles2" << std::endl;
+ for(i=0;i<meas_extra.blocks.size();i++)
+ {
+ out << "Block:" << std::endl;
+ out << " Receiver channel: " << meas_extra.blocks[i].rx_channel << std::endl;
+ out << " Master signal: " << signal_type_name[meas_extra.blocks[i].signal_type] << std::endl;
+ out << " Antenna: " << meas_extra.blocks[i].antenna_id << std::endl;
+ out << " Multipath correction: " << meas_extra.blocks[i].mp_correction << " m" << std::endl;
+ out << " Smoothing correction: " << meas_extra.blocks[i].smoothing_correction << " m" << std::endl;
+ out << " Code tracking noise variance: " << meas_extra.blocks[i].code_var << " m2" << std::endl;
+ out << " Carrier tracking noise variance: " << meas_extra.blocks[i].carrier_var << " mcycle2" << std::endl;
+ out << " Lock time: " << meas_extra.blocks[i].lock_time << " s" << std::endl;
+ }
+
+ return out;
+}
+
+std::ostream& operator<< (std::ostream& out, TGPSNav &gps_nav)
+{
+ out << "GPSNav block for satellite " << gps_nav.sat_id << std::endl;
+ out << " Code on L2 channel: " << gps_nav.ca_or_pon_l2 << std::endl;
+ out << " User range accuracy index: " << gps_nav.ura << std::endl;
+ out << " Health: " << gps_nav.health << std::endl;
+ out << " Data flag for L2 p-code: " << gps_nav.l2_data_flag << std::endl;
+ out << " Issue of data, clock: " << gps_nav.iodc << std::endl;
+ out << " Issue of data, ephemeris (subframe 2): " << gps_nav.iode2 << std::endl;
+ out << " Issue of data, ephemeris (subframe 3): " << gps_nav.iode3 << std::endl;
+ out << " Curve fit interval: " << gps_nav.fit_int_flag << std::endl;
+ out << " Estimated group delay differential (Tgd): " << gps_nav.t_gd << " s" << std::endl;
+ out << " Clock data reference time (Toc): " << gps_nav.t_oc << " s" << std::endl;
+ out << " SV clock aging" << gps_nav.a_f2 << " s/s2" << std::endl;
+ out << " SV clock drift: " << gps_nav.a_f1 << " s/s" << std::endl;
+ out << " SV clock bias: " << gps_nav.a_f0 << " s" << std::endl;
+ out << " Amplitude of the sine harmonic correction term to the orbit radius (Crs): " << gps_nav.c_rs << " m" << std::endl;
+ out << " Mean motion difference: " << gps_nav.delta_n << " semi-circle/s" << std::endl;
+ out << " Mean anomaly at reference time: " << gps_nav.m_0 << " semi-circle" << std::endl;
+ out << " Amplitude of the cosine harmonic correction term to the argument of latitude (Cuc): " << gps_nav.c_uc << " rad" << std::endl;
+ out << " Eccentricity: " << gps_nav.e << std::endl;
+ out << " Amplitude of the cosine harmonic correction term to the argument of latitude (Cus): " << gps_nav.c_us << " rad" << std::endl;
+ out << " Square root of the semi-major axis: " << gps_nav.sqrt_a << " m1/2" << std::endl;
+ out << " Reference time ephemeris (Toe): " << gps_nav.t_oe << " s" << std::endl;
+ out << " Amplitude of the cosine harmonic correction term to the angle of inclination (Cic): " << gps_nav.c_ic << " rad" << std::endl;
+ out << " Longitude of ascending node of orbit plane at weekly epoch: " << gps_nav.omega_0 << " semi-circle" << std::endl;
+ out << " Amplitude of the sine harmonic correction term to the angle of inclination (Cis): " << gps_nav.c_is << " rad" << std::endl;
+ out << " Inclination angle at reference time: " << gps_nav.i_0 << " semi-circle" << std::endl;
+ out << " Amplitude of the cosine harmonic correction term to the orbit radius (Crc): " << gps_nav.c_rc << " m" << std::endl;
+ out << " Argument of peregee: " << gps_nav.omega << " semi-circle" << std::endl;
+ out << " Rate of right ascension: " << gps_nav.omega_dot << " semi-circle/s" << std::endl;
+ out << " Rate of inclination angle: " << gps_nav.i_dot << " semi-circle/s" << std::endl;
+
+ return out;
+}
+
+std::ostream& operator<< (std::ostream& out, TGPSAlm &gps_alm)
+{
+ out << "GPSAlm block for satellite: " << gps_alm.sat_id << std::endl;
+ out << "Eccentricity: " << gps_alm.e << std::endl;
+ out << "Almanac reference time of week (Toa): " << gps_alm.t_oa << " s" << std::endl;
+ out << "Inclinationa ngle at reference time: " << gps_alm.delta_i << " semi-circle" << std::endl;
+ out << "Rate of right ascension: " << gps_alm.omega_dot << " semi-circle/s" << std::endl;
+ out << "Square root of the semi-major axis: " << gps_alm.sqrt_a << " m1/2" << std::endl;
+ out << "Longitude of ascending node of orbit plane at weekly epoch: " << gps_alm.omega_0 << " semi-circle" << std::endl;
+ out << "Argument of peregee: " << gps_alm.omega << " semi-circle" << std::endl;
+ out << "Mean anomaly at reference time: " << gps_alm.m_0 << " semi-circle" << std::endl;
+ out << "SV clock drift: " << gps_alm.a_f1 << " s/s" << std::endl;
+ out << "SV clock bias: " << gps_alm.a_f0 << " s" << std::endl;
+
+ return out;
+}
+
+std::ostream& operator<< (std::ostream& out, TGPSIon &gps_ion)
+{
+ out << "GPSIon block for satellite: " << gps_ion.sat_id << std::endl;
+ out << "Vertical delay coefficient 0: " << gps_ion.alpha_0 << " s" << std::endl;
+ out << "Vertical delay coefficient 1: " << gps_ion.alpha_1 << " s/semi-circle" << std::endl;
+ out << "Vertical delay coefficient 2: " << gps_ion.alpha_2 << " s/semi-circle2" << std::endl;
+ out << "Vertical delay coefficient 3: " << gps_ion.alpha_3 << " s/semi-circle3" << std::endl;
+ out << "Model period coefficient 0: " << gps_ion.beta_0 << " s" << std::endl;
+ out << "Model period coefficient 1: " << gps_ion.beta_1 << " s/semi-circle" << std::endl;
+ out << "Model period coefficient 2: " << gps_ion.beta_2 << " s/semi-circle2" << std::endl;
+ out << "Model period coefficient 3: " << gps_ion.beta_3 << " s/semi-circle3" << std::endl;
+
+ return out;
+}
+
+std::ostream& operator<< (std::ostream& out, TGPSUtc &gps_utc)
+{
+ out << "GPSUtc block for satellite: " << gps_utc.sat_id << std::endl;
+ out << "First order term of polynomial: " << gps_utc.a_1 << " s/s" << std::endl;
+ out << "constant term of polynomial: " << gps_utc.a_0 << " s" << std::endl;
+ out << "Reference time for UTC data (Tot): " << gps_utc.t_ot << " s" << std::endl;
+ out << "Delta time due to leap seconds when effectivity time is not in the past: " << gps_utc.del_t_ls << " s" << std::endl;
+ out << "Effectivity time of leap seconds (week): " << gps_utc.wn_lsf << " weelk" << std::endl;
+ out << "Effectivity time of leap seconds (day): " << gps_utc.dn << " day" << std::endl;
+ out << "Delta time due to leap seconds when effectivity time is not in the past: " << gps_utc.del_t_lsf << " s" << std::endl;
+
+ return out;
+}
+
+// ChannelStatus
+/* std::cout << "gps timestamp: " << this->channel_status.gps_timestamp << std::endl;
+ std::cout << "num channles: " << this->channel_status.info.size() << std::endl;
+ for(i=0;i<this->channel_status.info.size();i++)
+ {
+ std::cout << " Sat ID: " << (int)this->channel_status.info[i].sat_id << std::endl;
+ std::cout << " Azimuth: " << this->channel_status.info[i].azimuth << std::endl;
+ std::cout << " rise/set: " << this->channel_status.info[i].rise_set << std::endl;
+ std::cout << " Health: " << this->channel_status.info[i].health << std::endl;
+ std::cout << " Elevation: " << this->channel_status.info[i].elevation << std::endl;
+ for(j=0;j<this->channel_status.info[i].state.size();j++)
+ {
+ std::cout << " Antenna ID: " << (int)this->channel_status.info[i].state[j].antenna_id << std::endl;
+ std::cout << " Tracking status: " << this->channel_status.info[i].state[j].track_status << std::endl;
+ std::cout << " PVT status: " << this->channel_status.info[i].state[j].pvt_status << std::endl;
+ }
+ }*/
+
+//ReceiverStatus
+/* std::cout << "gps timestamp: " << this->receiver_status.gps_timestamp << std::endl;
+ std::cout << "CPU load: " << this->receiver_status.cpu_load << std::endl;
+ std::cout << "up time: " << this->receiver_status.up_time << std::endl;
+ for(i=0;i<this->receiver_status.agc.size();i++)
+ {
+ std::cout << " Frontend: " << this->receiver_status.agc[i].frontend_id << std::endl;;
+ std::cout << " Antenna: " << this->receiver_status.agc[i].antenna_id << std::endl;;
+ std::cout << " Gain: " << this->receiver_status.agc[i].gain << std::endl;;
+ std::cout << " Sample var: " << this->receiver_status.agc[i].sample_var << std::endl;;
+ std::cout << " Blanking stat: " << this->receiver_status.agc[i].blanking_stat << std::endl;;
+ }*/
+
+// SatVisibility
+/* std::cout << "Num sat: " << this->sat_info.size() << std::endl;
+ for(i=0;i<this->sat_info.size();i++)
+ {
+ std::cout << "Sat ID: " << this->sat_info[i].sat_id << std::endl;
+ std::cout << "Azmimuth: " << this->sat_info[i].azimuth << std::endl;
+ std::cout << "Elevation: " << this->sat_info[i].elevation << std::endl;
+ std::cout << "rise/set: " << this->sat_info[i].rise_set << std::endl;
+ std::cout << "Sat info: " << this->sat_info[i].sat_info << std::endl;
+ }*/
+
diff --git a/src/stream_gps.h b/src/stream_gps.h
new file mode 100644
index 0000000000000000000000000000000000000000..597aacd827249f34b3920968e554490be63bf263
--- /dev/null
+++ b/src/stream_gps.h
@@ -0,0 +1,16 @@
+#ifndef _STREAM_GPS_H
+#define _STREAM_GPS_H
+
+#include <iostream>
+#include <vector>
+#include <stdint.h>
+#include "gps_types.h"
+
+std::ostream& operator<< (std::ostream& out, TMeasEpoch &meas_epoch);
+std::ostream& operator<< (std::ostream& out, TMeasExtra &meas_extra);
+std::ostream& operator<< (std::ostream& out, TGPSNav &gps_nav);
+std::ostream& operator<< (std::ostream& out, TGPSAlm &gps_alm);
+std::ostream& operator<< (std::ostream& out, TGPSIon &gps_ion);
+std::ostream& operator<< (std::ostream& out, TGPSUtc &gps_utc);
+
+#endif