diff --git a/Findasterx1_process.cmake b/Findasterx1_process.cmake new file mode 100644 index 0000000000000000000000000000000000000000..be4e8f55e9e377bd9671124d296573ed6cfc7bdb --- /dev/null +++ b/Findasterx1_process.cmake @@ -0,0 +1,21 @@ +#edit the following line to add the librarie's header files +FIND_PATH(asterx1_process_INCLUDE_DIR asterx1_process.h /usr/include/iridrivers /usr/local/include/iridrivers) + +FIND_LIBRARY(asterx1_process_LIBRARY + NAMES asterx1_process + PATHS /usr/lib /usr/local/lib /usr/local/lib/iridrivers) + +IF (asterx1_process_INCLUDE_DIR AND asterx1_process_LIBRARY) + SET(asterx1_process_FOUND TRUE) +ENDIF (asterx1_process_INCLUDE_DIR AND asterx1_process_LIBRARY) + +IF (asterx1_process_FOUND) + IF (NOT asterx1_process_FIND_QUIETLY) + MESSAGE(STATUS "Found asterx1_process: ${asterx1_process_LIBRARY}") + ENDIF (NOT asterx1_process_FIND_QUIETLY) +ELSE (asterx1_process_FOUND) + IF (asterx1_process_FIND_REQUIRED) + MESSAGE(FATAL_ERROR "Could not find asterx1_process") + ENDIF (asterx1_process_FIND_REQUIRED) +ENDIF (asterx1_process_FOUND) + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 4e8142104b7f1ff8f05d2cdfcb506540dd529344..8765b478eafefb90ba370fc9b0147673c2721f90 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -4,14 +4,20 @@ SET(sources asterx1_gps.cpp stream_gps.cpp asterx1exceptions.cpp) # application header files SET(headers asterx1_gps.h stream_gps.h asterx1exceptions.h gps_types.h) +# driver source files +SET(process_sources asterx1_process.cpp) + +# application header files +SET(process_headers asterx1_process.h) + # locate the the necessary dependencies FIND_PACKAGE(iriutils) FIND_PACKAGE(comm) # add the necessary include directories INCLUDE_DIRECTORIES(.) - INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR}) - INCLUDE_DIRECTORIES(${comm_INCLUDE_DIR}) +INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR}) +INCLUDE_DIRECTORIES(${comm_INCLUDE_DIR}) # create the shared library ADD_LIBRARY(asterx1_gps SHARED ${sources}) @@ -19,6 +25,7 @@ ADD_LIBRARY(asterx1_gps SHARED ${sources}) # link necessary libraries TARGET_LINK_LIBRARIES(asterx1_gps ${iriutils_LIBRARY}) TARGET_LINK_LIBRARIES(asterx1_gps ${comm_LIBRARY}) + INSTALL(TARGETS asterx1_gps RUNTIME DESTINATION bin LIBRARY DESTINATION lib/iridrivers @@ -28,4 +35,28 @@ INSTALL(FILES ${headers} DESTINATION include/iridrivers) INSTALL(FILES ../Findasterx1_gps.cmake DESTINATION ${CMAKE_ROOT}/Modules/) +FIND_LIBRARY(gpstk_LIBRARY + NAMES gpstk + PATHS /usr/lib /usr/local/lib) + +IF(gpstk_LIBRARY) + # create the shared library + ADD_LIBRARY(asterx1_process SHARED ${process_sources}) + + TARGET_LINK_LIBRARIES(asterx1_process ${iriutils_LIBRARY}) + TARGET_LINK_LIBRARIES(asterx1_process ${comm_LIBRARY}) + TARGET_LINK_LIBRARIES(asterx1_process gpstk) + + INSTALL(TARGETS asterx1_process + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib/iridrivers + ARCHIVE DESTINATION lib/iridrivers) + + INSTALL(FILES ${process_headers} DESTINATION include/iridrivers) + + INSTALL(FILES ../Findasterx1_process.cmake DESTINATION ${CMAKE_ROOT}/Modules/) + + ADD_DEFINITIONS(-DHAVE_GPSTK) +ENDIF(gpstk_LIBRARY) + ADD_SUBDIRECTORY(examples) diff --git a/src/asterx1_gps.cpp b/src/asterx1_gps.cpp index 21cd9a348ed22351a97319ad98c2658253b21cd3..4567e8f894528ab85b0116169637a7b5f2cfe0be 100644 --- a/src/asterx1_gps.cpp +++ b/src/asterx1_gps.cpp @@ -60,6 +60,8 @@ CasteRx1::CasteRx1(const std::string &name,const std::string &hwPortName) 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_gps_raw_block_event_id=name+"_new_gps_raw_block"; + this->event_server->create_event(this->new_gps_raw_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 */ @@ -67,6 +69,7 @@ CasteRx1::CasteRx1(const std::string &name,const std::string &hwPortName) 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); + this->sat_channel.resize(256,-1); } CasteRx1::~CasteRx1() @@ -93,6 +96,8 @@ CasteRx1::~CasteRx1() 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_gps_raw_block_event_id); + this->new_gps_raw_block_event_id=""; this->event_server->delete_event(this->new_pvt_block_event_id); this->new_pvt_block_event_id=""; if(this->serialPort!=NULL) @@ -149,10 +154,10 @@ void CasteRx1::startAcquisition() { 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->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; } } @@ -241,6 +246,27 @@ void CasteRx1::get_gps_utc_data(TGPSUtc &gps_utc) this->nav_access.exit(); } +std::string CasteRx1::get_new_gps_raw_data_event_id(void) +{ + return this->new_gps_raw_block_event_id; +} + +void CasteRx1::get_gps_raw_data(TGPSFrame &gps_frame) +{ + this->nav_access.enter(); + if(!this->gps_raw_frames.empty()) + { + gps_frame=this->gps_raw_frames.front(); + this->gps_raw_frames.pop(); + if(this->gps_raw_frames.empty()) + { + if(this->event_server->event_is_set(this->new_gps_raw_block_event_id)) + this->event_server->reset_event(this->new_gps_raw_block_event_id); + } + } + this->nav_access.exit(); +} + std::string CasteRx1::get_new_pvt_data_event_id(void) { return this->new_pvt_block_event_id; @@ -424,6 +450,9 @@ void CasteRx1::process_block(unsigned char *data,unsigned short int length) case GPSUtc_ID: //std::cout << "GPSUtc_ID" << std::endl; this->process_gps_utc(data,length); break; + case GPSRawCA_ID: //std::cout << "GPSRawCA_ID" << std::endl; + this->process_gps_raw_ca(data,length); + break; case ReceiverStatus_ID: //std::cout << "ReceiverStatus_ID" << std::endl; this->process_receiver_status(data,length); break; @@ -463,6 +492,8 @@ void CasteRx1::process_channel_status(unsigned char *data,unsigned short int len 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.rx_channel=gps_sat_info.rx_channel; + this->sat_channel[sat_info.sat_id]=gps_sat_info.rx_channel; sat_info.state.clear(); for(j=0;j<gps_sat_info.num_state_info;j++) { @@ -652,6 +683,27 @@ void CasteRx1::process_gps_utc(unsigned char *data,unsigned short int length) this->nav_access.exit(); } +void CasteRx1::process_gps_raw_ca(unsigned char *data,unsigned short int length) +{ + TGPSRawCA gps_raw_ca; + TGPSFrame new_frame; + unsigned int i=0; + + this->nav_access.enter(); + memcpy((unsigned char *)&gps_raw_ca,data,sizeof(TGPSRawCA)); + new_frame.gps_timestamp.wnc=gps_raw_ca.timestamp.wnc; + new_frame.gps_timestamp.tow=gps_raw_ca.timestamp.tow; + new_frame.local_timestamp=this->local_time.getTimeInSeconds(); + new_frame.sat_id=gps_raw_ca.sat_id; + new_frame.track_id=this->sat_channel[gps_raw_ca.sat_id]; + new_frame.wn=gps_raw_ca.timestamp.wnc; + for(i=0;i<10;i++) + new_frame.frame_bits[i]=gps_raw_ca.nav_bits[i]; + this->gps_raw_frames.push(new_frame); + this->event_server->set_event(new_gps_raw_block_event_id); + this->nav_access.exit(); +} + void CasteRx1::process_meas_epoch(unsigned char *data,unsigned short int length) { TGPSMeasEpoch gps_meas_epoch; @@ -1161,7 +1213,9 @@ void CasteRx1::configureStreams(void) // 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"); + //this->sendCommand("setSBFGroups,Group2,GPSNav+GPSAlm+GPSIon+GPSUtc\n"); + this->sendCommand("setSBFGroups,Group2,GPSRawCA\n"); + //this->sendCommand("setSBFGroups,Group2,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) diff --git a/src/asterx1_gps.h b/src/asterx1_gps.h index 1cbf719dd1480df0a0b4f1a98104cb54110b4d56..6c12575bac7dd9a620612d2d37af1fb9524f3e40 100644 --- a/src/asterx1_gps.h +++ b/src/asterx1_gps.h @@ -40,6 +40,7 @@ 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 GPSRawCA_ID = 4017; const unsigned short int ReceiverStatus_ID = 4014; const unsigned short int ChannelStatus_ID = 4013; const unsigned short int SatVisibility_ID = 4012; @@ -148,6 +149,16 @@ class CasteRx1 * */ void get_gps_utc_data(TGPSUtc &gps_utc); + /** + * \brief + * + */ + std::string get_new_gps_raw_data_event_id(void); + /** + * \brief + * + */ + void get_gps_raw_data(TGPSFrame &gps_frame); /** * \brief * @@ -182,6 +193,7 @@ class CasteRx1 std::string gps_process_data_thread_id; bool gps_running; CTime local_time; + std::vector<unsigned short int> sat_channel; /* header of the message currently being received */ TGPSBlockHeader current_header; @@ -204,6 +216,8 @@ class CasteRx1 TGPSIon current_gps_ion; std::string new_gps_utc_block_event_id; TGPSUtc current_gps_utc; + std::string new_gps_raw_block_event_id; + std::queue<TGPSFrame> gps_raw_frames; /* PVT blocks attributes */ CMutex pvt_access; @@ -261,6 +275,11 @@ class CasteRx1 * */ void process_gps_utc(unsigned char *data,unsigned short int length); + /** + * \brief + * + */ + void process_gps_raw_ca(unsigned char *data,unsigned short int length); /** * \brief * diff --git a/src/asterx1_process.cpp b/src/asterx1_process.cpp new file mode 100644 index 0000000000000000000000000000000000000000..17cf6900ebcc6138b49c5426d725e245f6f03129 --- /dev/null +++ b/src/asterx1_process.cpp @@ -0,0 +1,123 @@ +#include "asterx1_process.h" +#include "Epoch.hpp" +#include "UTCTime.hpp" +#include "TimeSystem.hpp" + +CAsteRx1Process::CAsteRx1Process() +{ + unsigned int i=0; + + this->ephemeris_data.resize(37);// set the ephemeris vector for each of the satellites + for(i=0;i<37;i++) + { + this->ephemeris_data[i].sync=false; + this->ephemeris_data[i].num_frames=0; + } +} + +void CAsteRx1Process::generate_gps_nav(gpstk::EngEphemeris &ephemeris,TGPSNav &gps_nav) +{ + gpstk::CommonTime time; + + try{ + time=ephemeris.getTransmitTime(); + time.setTimeSystem(gpstk::TimeSystem(gpstk::TimeSystem::GPS)); + gpstk::Epoch gps_epoch(time); + gps_nav.gps_timestamp.tow=gps_epoch.GPSsow()*1000; + gps_nav.gps_timestamp.wnc=gps_epoch.GPSweek(); + gps_nav.sat_id=ephemeris.getPRNID(); + gps_nav.wn=gps_epoch.GPSModWeek(); + gps_nav.ca_or_pon_l2=ephemeris.getCodeFlags(); + gps_nav.ura=ephemeris.getAccFlag(); + gps_nav.health=ephemeris.getHealth(); + gps_nav.l2_data_flag=ephemeris.getL2Pdata(); + gps_nav.iodc=ephemeris.getIODC(); + gps_nav.iode2=ephemeris.getIODE(); + gps_nav.iode3=ephemeris.getIODE(); + gps_nav.fit_int_flag=ephemeris.getFitInt(); + gps_nav.t_gd=ephemeris.getTgd(); + gps_nav.t_oc=ephemeris.getToc(); + gps_nav.a_f2=ephemeris.getAf2(); + gps_nav.a_f1=ephemeris.getAf1(); + gps_nav.a_f0=ephemeris.getAf0(); + gps_nav.c_rs=ephemeris.getCrs(); + gps_nav.delta_n=ephemeris.getDn(); + gps_nav.m_0=ephemeris.getM0(); + gps_nav.c_uc=ephemeris.getCuc(); + gps_nav.e=ephemeris.getEcc(); + gps_nav.c_us=ephemeris.getCus(); + gps_nav.sqrt_a=ephemeris.getAhalf(); + gps_nav.t_oe=ephemeris.getToe(); + gps_nav.c_ic=ephemeris.getCic(); + gps_nav.omega_0=ephemeris.getOmega0(); + gps_nav.c_is=ephemeris.getCis(); + gps_nav.i_0=ephemeris.getI0(); + gps_nav.c_rc=ephemeris.getCrc(); + gps_nav.omega=ephemeris.getW(); + gps_nav.omega_dot=ephemeris.getOmegaDot(); + gps_nav.i_dot=ephemeris.getIDot(); + gps_nav.wnt_oc=gps_epoch.GPSModWeek(); + gps_nav.wnt_oe=gps_epoch.GPSModWeek(); + }catch(...){ + std::cout << "Exception" << std::endl; + throw; + } +} + +void CAsteRx1Process::add_subframe(TGPSFrame &frame) +{ + TGPSNav gps_nav; + + try{ + this->ephemeris_data[frame.sat_id].ephemeris.addSubframe(frame.frame_bits,frame.wn,frame.sat_id,frame.track_id); + if(!this->ephemeris_data[frame.sat_id].sync) + { + if(this->ephemeris_data[frame.sat_id].ephemeris.isData(1))// the first frame has been received for the first time + { + this->ephemeris_data[frame.sat_id].sync=true; + this->ephemeris_data[frame.sat_id].num_frames=2; + } + } + else + { + this->ephemeris_data[frame.sat_id].num_frames--; + if(this->ephemeris_data[frame.sat_id].num_frames==0) + { + this->ephemeris_data[frame.sat_id].num_frames=3; + // process the data and build a GPSNav structure + this->generate_gps_nav(this->ephemeris_data[frame.sat_id].ephemeris,gps_nav); + gps_nav.local_timestamp=frame.local_timestamp; + this->gps_nav_data.push(gps_nav); + } + } + }catch(gpstk::InvalidParameter &e){ + /* ignore the Error for including subframes 4 and 5 */ + //std::cout << e.what() << std::endl; + }catch(gpstk::InvalidRequest &e){ + /* this should not happen */ + //std::cout << e.what() << std::endl; + } +} + +bool CAsteRx1Process::is_new_gps_nav_data_available(void) +{ + if(this->gps_nav_data.size()>0) + return true; + else + return false; +} + +void CAsteRx1Process::get_gps_nav_data(TGPSNav &gps_nav) +{ + if(this->gps_nav_data.size()>0) + { + gps_nav=this->gps_nav_data.front(); + this->gps_nav_data.pop(); + } +} + +CAsteRx1Process::~CAsteRx1Process() +{ + this->ephemeris_data.clear(); +} + diff --git a/src/asterx1_process.h b/src/asterx1_process.h new file mode 100644 index 0000000000000000000000000000000000000000..ef73f3f72d2dec156703c7ad53f4a98d922e7d51 --- /dev/null +++ b/src/asterx1_process.h @@ -0,0 +1,29 @@ +#ifndef _ASTERX1_PROCESS_H +#define _ASTERX1_PROCESS_H + +#include <EngEphemeris.hpp> +#include "gps_types.h" +#include <queue> + +typedef struct{ + gpstk::EngEphemeris ephemeris; + bool sync; + int num_frames; +}TEphemerisData; + +class CAsteRx1Process +{ + private: + std::vector<TEphemerisData> ephemeris_data; + std::queue<TGPSNav> gps_nav_data; + protected: + void generate_gps_nav(gpstk::EngEphemeris &ephemeris,TGPSNav &gps_nav); + public: + CAsteRx1Process(); + void add_subframe(TGPSFrame &frame); + bool is_new_gps_nav_data_available(void); + void get_gps_nav_data(TGPSNav &gps_nav); + ~CAsteRx1Process(); +}; + +#endif diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index 4d441f04b0b93d008f512c254c2524531b018fdd..e3afc6ce48d003d2b34d541f00d21b4ca9695b0d 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -2,7 +2,7 @@ ADD_EXECUTABLE(testasterx1 testasterx1.cpp) # edit the following line to add the necessary libraries -TARGET_LINK_LIBRARIES(testasterx1 asterx1_gps) +TARGET_LINK_LIBRARIES(testasterx1 asterx1_gps asterx1_process) #fast building # ADD_EXECUTABLE(testasterx1 testasterx1.cpp ../asterx1.cpp) diff --git a/src/examples/testasterx1.cpp b/src/examples/testasterx1.cpp index b71fc0255e84129ceb7ce4d174990db594463078..280e3bee28270bb4c0adb3279e9585c9f8334d15 100644 --- a/src/examples/testasterx1.cpp +++ b/src/examples/testasterx1.cpp @@ -17,20 +17,23 @@ USB any port #include <stdlib.h> #include "asterx1_gps.h" +#include "asterx1_process.h" #include "stream_gps.h" int main(int argc, char **argv) { - CasteRx1 myGps("gps","/dev/ttyACM0"); + CasteRx1 gps("gps","/dev/ttyACM0"); + CAsteRx1Process gps_process; CEventServer *event_server=CEventServer::instance(); std::list<std::string> events; - unsigned int i=0,index; + unsigned int index; TMeasEpoch meas_epoch; TMeasExtra meas_extra; TGPSNav gps_nav; TGPSAlm gps_alm; TGPSIon gps_ion; TGPSUtc gps_utc; + TGPSFrame gps_frame; TPVTCartesian pvt_cartesian; TPVTGeodetic pvt_geodetic; TPVTCov pvt_pos_cov_cartesian; @@ -42,49 +45,61 @@ int main(int argc, char **argv) std::cout << "\n\n TEST AsteRx1 GPS RECEIVER \n\n" << std::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++) + gps.openDevice(); + events.push_back(gps.get_new_meas_data_event_id()); + events.push_back(gps.get_new_gps_nav_data_event_id()); + events.push_back(gps.get_new_gps_alm_data_event_id()); + events.push_back(gps.get_new_gps_ion_data_event_id()); + events.push_back(gps.get_new_gps_utc_data_event_id()); + events.push_back(gps.get_new_gps_raw_data_event_id()); + events.push_back(gps.get_new_pvt_data_event_id()); + gps.startAcquisition(); for(;;) { - index=event_server->wait_first(events,10000); + index=event_server->wait_first(events); if(index==0) { - myGps.get_meas_data(meas_epoch,meas_extra); + gps.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; + gps.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; + gps.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; + gps.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; + gps.get_gps_utc_data(gps_utc); + std::cout << gps_utc << std::endl; } else if(index==5) { - 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); - std::cout << "Cartesian:" << std::endl; + gps.get_gps_raw_data(gps_frame); + gps_process.add_subframe(gps_frame); + std::cout << gps_frame << std::endl; +/* if(gps_process.is_new_gps_nav_data_available()) + { + gps_process.get_gps_nav_data(gps_nav); + if(gps_frame.sat_id==17) + std::cout << gps_nav << std::endl; + }*/ + } + else if(index==6) + { + gps.get_pvt_data(pvt_cartesian,pvt_geodetic); + gps.get_pvt_cov_data(pvt_pos_cov_cartesian,pvt_pos_cov_geodetic,pvt_vel_cov_cartesian,pvt_vel_cov_geodetic); +/* std::cout << "Cartesian:" << std::endl; std::cout << pvt_cartesian << std::endl; std::cout << "Position covariance:" << std::endl; std::cout << pvt_pos_cov_cartesian << std::endl; @@ -95,12 +110,12 @@ int main(int argc, char **argv) std::cout << "Position covariance:" << std::endl; std::cout << pvt_pos_cov_geodetic << std::endl; std::cout << "Velocity covariance:" << std::endl; - std::cout << pvt_vel_cov_geodetic << std::endl; - myGps.get_pvt_dop_data(pvt_dop); - std::cout << pvt_dop << std::endl; + std::cout << pvt_vel_cov_geodetic << std::endl;*/ + gps.get_pvt_dop_data(pvt_dop); +// std::cout << pvt_dop << std::endl; } } - myGps.stopAcquisition(); + gps.stopAcquisition(); }catch(CException &e){ std::cout << e.what() << std::endl; } diff --git a/src/gps_types.h b/src/gps_types.h index 1a70bc7c3255feeab5eea0bb47b28544eba19234..8078923b93c6c929870537495bafc0bb6dd46d37 100644 --- a/src/gps_types.h +++ b/src/gps_types.h @@ -7,6 +7,15 @@ typedef struct{ unsigned short int wnc; }TBlockTimeStamp; +typedef struct{ + TBlockTimeStamp gps_timestamp; + double local_timestamp; + long int frame_bits[10]; + int wn; + short int sat_id; + short int track_id; +}TGPSFrame; + // User data structures typedef enum {elev_setting=0,elev_rising=1,elev_unknown=3} rise_set_t; @@ -30,6 +39,7 @@ typedef struct { rise_set_t rise_set; health_t health; int elevation; + int rx_channel; std::vector<TChannelStateInfo> state; }TChannelSatInfo; @@ -867,4 +877,17 @@ typedef struct{ }TGPSSatVisibility; #pragma pack (pop) +#pragma pack (push, 1) +typedef struct{ + TGPSBlockTimeStamp timestamp; + uint8_t sat_id; + uint8_t crc_passed; + uint8_t viterbi_count; + uint8_t source; + uint8_t freq_nr; + uint8_t reserved; + uint32_t nav_bits[10]; +}TGPSRawCA; +#pragma pack (pop) + #endif diff --git a/src/stream_gps.cpp b/src/stream_gps.cpp index 447ba142da6057959f46e89c54928c951f7ed3cc..f2d69ac9bb45d53423280585d85565597fda3d3e 100644 --- a/src/stream_gps.cpp +++ b/src/stream_gps.cpp @@ -91,6 +91,8 @@ std::ostream& operator<< (std::ostream& out, TMeasExtra &meas_extra) std::ostream& operator<< (std::ostream& out, TGPSNav &gps_nav) { out << "GPSNav block for satellite " << gps_nav.sat_id << std::endl; + out << " Time of week: " << gps_nav.gps_timestamp.tow << std::endl; + out << " Week number: " << gps_nav.gps_timestamp.wnc << 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; @@ -101,7 +103,7 @@ std::ostream& operator<< (std::ostream& out, TGPSNav &gps_nav) 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 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; @@ -170,6 +172,25 @@ std::ostream& operator<< (std::ostream& out, TGPSUtc &gps_utc) return out; } +std::ostream& operator<< (std::ostream& out, TGPSFrame &gps_frame) +{ + unsigned int i=0; + + out << "GPS Raw Frame for satellite: " << gps_frame.sat_id << std::endl; + out << " Week number: " << gps_frame.wn << std::endl; + out << " Tracking channel: " << gps_frame.track_id << std::endl; + out << " Frame data: " << std::endl; + for(i=0;i<10;i++) + { + out << "0x" << std::hex << gps_frame.frame_bits[i]; + if(i<9) + out << ","; + } + out << std::dec << std::endl; + + return out; +} + std::ostream& operator<< (std::ostream& out, TPVTCartesian &pvt_cartesian) { out << "PVT mode: "; diff --git a/src/stream_gps.h b/src/stream_gps.h index 5298fe16319d22a4dac3d888ecff1fc9508009c0..1e1b5534e80578f98a1eea4a5a0731f51f22194d 100644 --- a/src/stream_gps.h +++ b/src/stream_gps.h @@ -12,6 +12,7 @@ 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); +std::ostream& operator<< (std::ostream& out, TGPSFrame &gps_frame); std::ostream& operator<< (std::ostream& out, TPVTCartesian &pvt_cartesian); std::ostream& operator<< (std::ostream& out, TPVTGeodetic &pvt_geodetic); std::ostream& operator<< (std::ostream& out, TPVTCov &pvt_cov);