diff --git a/rtklib/CMakeLists.txt b/rtklib/CMakeLists.txt index 3727836..616cb48 100644 --- a/rtklib/CMakeLists.txt +++ b/rtklib/CMakeLists.txt @@ -6,15 +6,27 @@ rosbuild_init() find_package(LAPACK) find_package(BLAS) -if(${LAPACK_FOUND} and ${BLAS_FOUND}) +#if(${LAPACK_FOUND} and ${BLAS_FOUND}) add_definitions(-DLAPACK) -endif() +#endif() + +rosbuild_genmsg() include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/rtklib) +rosbuild_find_ros_package(rtklib) set(src ${CMAKE_CURRENT_SOURCE_DIR}/src/src) rosbuild_add_library(rtk ${src}/rtksvr.c + ${src}/convkml.c + ${src}/datum.c + ${src}/download.c + ${src}/postpos.c + ${src}/ppp_ar.c + ${src}/qzslex.c + ${src}/rtcm3e.c + ${src}/streamsvr.c + ${src}/tle.c ${src}/rtkcmn.c ${src}/rtkpos.c ${src}/geoid.c @@ -24,6 +36,8 @@ rosbuild_add_library(rtk ${src}/stream.c ${src}/rcvraw.c ${src}/rtcm.c + ${src}/rtcm2.c + ${src}/rtcm3.c ${src}/preceph.c ${src}/options.c ${src}/pntpos.c @@ -35,8 +49,11 @@ rosbuild_add_library(rtk ${src}/rcv/skytraq.c ${src}/rcv/gw10.c ${src}/rcv/javad.c + ${src}/rcv/binex.c + ${src}/rcv/nvs.c + ${src}/rcv/rcvlex.c ${src}/ephemeris.c ${src}/rinex.c ${src}/ionex.c ) -target_link_libraries(rtk ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES}) +target_link_libraries(rtk ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES}) \ No newline at end of file diff --git a/rtklib/msg/Almanac.msg b/rtklib/msg/Almanac.msg new file mode 100644 index 0000000..d2bf827 --- /dev/null +++ b/rtklib/msg/Almanac.msg @@ -0,0 +1,22 @@ +# The almanac consists of coarse orbit and status information for each satellite in the constellation, an ionospheric model, and information to relate GNSS derived time to Coordinated Universal Time (UTC) + +int32 sat # satellite number +int32 svh # sv health (0:ok) +int32 svconf # as and sv config +int32 week # GPS/QZS: gps week, GAL: galileo week +Header toa # Toa + +# SV orbit parameters +float64 A +float64 e +float64 i0 +float64 OMG0 +float64 omg +float64 M0 +float64 OMGd + +float64 toas # Toa (s) in week + +# SV clock parameters (af0,af1) +float64 f0 +float64 f1 \ No newline at end of file diff --git a/rtklib/msg/AntennaParameter.msg b/rtklib/msg/AntennaParameter.msg new file mode 100644 index 0000000..9400426 --- /dev/null +++ b/rtklib/msg/AntennaParameter.msg @@ -0,0 +1,12 @@ +# antenna parameter type + +int32 sat # satellite number (0:receiver) +int8[] type # antenna type +int8[] code # serial number or satellite code + +time ts # valid time start +time te # valid time end + +float64[3] off # phase center offset e/n/u or x/y/z (m) +float64[19] var # phase center variation (m) + # el=90,85,...,0 or nadir=0,1,2,3,... (deg) \ No newline at end of file diff --git a/rtklib/msg/DifferentialGnss.msg b/rtklib/msg/DifferentialGnss.msg new file mode 100644 index 0000000..f31dcb1 --- /dev/null +++ b/rtklib/msg/DifferentialGnss.msg @@ -0,0 +1,7 @@ +# DGPS/GNSS correction type + +time t0 # correction time +float64 prc # pseudorange correction (PRC) (m) +float64 rrc # range rate correction (RRC) (m/s) +int32 iod # issue of data (IOD) +float64 udre # UDRE \ No newline at end of file diff --git a/rtklib/msg/Ephemeris.msg b/rtklib/msg/Ephemeris.msg new file mode 100644 index 0000000..0370d4c --- /dev/null +++ b/rtklib/msg/Ephemeris.msg @@ -0,0 +1,3 @@ +# GPS/QZS/GAL broadcast ephemeris type + +int32 sat # satellite number \ No newline at end of file diff --git a/rtklib/msg/GlonassEphemeris.msg b/rtklib/msg/GlonassEphemeris.msg new file mode 100644 index 0000000..6087c5e --- /dev/null +++ b/rtklib/msg/GlonassEphemeris.msg @@ -0,0 +1,18 @@ +# GLONASS broadcast ephemeris type + +int32 sat # satellite number +int32 iode # IODE (0-6 bit of tb field) +int32 frq # satellite frequency number +int32 svh # satellite health +int32 sva # satellite accuracy +int32 age # satellite age of operation + +time toe # epoch of epherides (gpst) +time tof # message frame time (gpst) + +geometry_msgs/Vector3 pos # satellite position (ecef) (m) +geometry_msgs/Vector3 vel # satellite velocity (ecef) (m/s) +geometry_msgs/Vector3 acc # satellite acceleration (ecef) (m/s^2) +float64 taun # SV clock bias (s) +float64 gamn # relative freq bias +float64 dtaun # delay between L1 and L2 (s) \ No newline at end of file diff --git a/rtklib/msg/Navigation.msg b/rtklib/msg/Navigation.msg new file mode 100644 index 0000000..35c4c51 --- /dev/null +++ b/rtklib/msg/Navigation.msg @@ -0,0 +1,37 @@ +# navigation data type + +Ephemeris[] eph # GPS/QZS/GAL ephemeris +GlonassEphemeris[] geph # GLONASS ephemeris +SbasEphemeris[] seph # SBAS ephemeris +PreciseEphemeris[] peph # precise ephemeris +PreciseClock[] pclk # precise clock +Almanac[] alm # almanac data +TotalElectronContent[] tec # TEC grid data +stec_t[] stec # stec grid data +erp_t erp # earth rotation parameters + +float64[4] utc_gps # GPS delta-UTC parameters {A0,A1,T,W} +float64[4] utc_glo # GLONASS UTC GPS time parameters +float64[4] utc_gal # Galileo UTC GPS time parameters +float64[4] utc_qzs # QZS UTC GPS time parameters +float64[4] utc_cmp # BeiDou UTC parameters +float64[4] utc_sbs # SBAS UTC parameters +float64[8] ion_gps # GPS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} +float64[4] ion_gal # Galileo iono model parameters {ai0,ai1,ai2,0} +float64[8] ion_qzs # QZSS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} +float64[8] ion_cmp # BeiDou iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} +int32 leaps # leap seconds (s) + +AntennaParameter[] pcvs # satellite antenna pcv +sbssat_t sbssat # SBAS satellite corrections +sbsion_t[] sbsion # SBAS ionosphere corrections +DifferentialGnss[] dgps # DGPS corrections +ssr_t[] ssr # SSR corrections +QzssEphemeris[] lexeph # LEX ephemeris +QzssIonosphereCorrection lexion # LEX ionosphere correction + +lam[] lamcwl # carrier wave lengths (m) +cbias[] codebias # code bias (0:p1-p2,1:p1-c1,2:p2-c2) (m) +float64[] wlbias # wide-lane bias (cycle) +float64[] glo_cpbias # glonass code-phase bias {1C,1P,2C,2P} (m) +uint8[] glo_fcn \ No newline at end of file diff --git a/rtklib/msg/Observation.msg b/rtklib/msg/Observation.msg new file mode 100644 index 0000000..0327b1d --- /dev/null +++ b/rtklib/msg/Observation.msg @@ -0,0 +1,3 @@ +# observation data record + +ObservationData[] data # observation data records \ No newline at end of file diff --git a/rtklib/msg/ObservationData.msg b/rtklib/msg/ObservationData.msg new file mode 100644 index 0000000..c7534b3 --- /dev/null +++ b/rtklib/msg/ObservationData.msg @@ -0,0 +1,11 @@ + # observation data records + +time time # receiver sampling time (GPST) +uint8 sat # satellite +uint8 rcv # receiver number +uint8[] SNR # signal strength (0.25 dBHz) +uint8[] LLI # loss of lock indicator +uint8[] code # code indicator (CODE_???) +float64[] L # observation data carrier-phase (cycle) +float64[] P # observation data pseudorange (m) +float32[] D # observation data doppler frequency (Hz) diff --git a/rtklib/msg/PreciseClock.msg b/rtklib/msg/PreciseClock.msg new file mode 100644 index 0000000..f17531f --- /dev/null +++ b/rtklib/msg/PreciseClock.msg @@ -0,0 +1,6 @@ +# precise clock type + +time time # time (GPST) +int32 index # clock index for multiple files +float64[] clk # satellite clock (s) +float32[] std # satellite clock std (s) \ No newline at end of file diff --git a/rtklib/msg/PreciseEphemeris.msg b/rtklib/msg/PreciseEphemeris.msg new file mode 100644 index 0000000..a82b6f5 --- /dev/null +++ b/rtklib/msg/PreciseEphemeris.msg @@ -0,0 +1,6 @@ +# precise ephemeris type + +time time # time (GPST) +int32 index # ephemeris index for multiple files +float64[] pos # satellite position/clock (ecef) (m|s) +float32[] std # satellite position/clock std (m|s) \ No newline at end of file diff --git a/rtklib/msg/QzssEphemeris.msg b/rtklib/msg/QzssEphemeris.msg new file mode 100644 index 0000000..045558a --- /dev/null +++ b/rtklib/msg/QzssEphemeris.msg @@ -0,0 +1,17 @@ +# QZSS LEX ephemeris type + +time toe # epoch time (GPST) +time tof # message frame time (GPST) + +int32 sat # satellite +uint8 health # signal health (L1,L2,L1C,L5,LEX) +uint8 ura # URA index + +geometry_msgs/Vector3 pos # satellite position (m) +geometry_msgs/Vector3 vel # satellite velocity (m/s) +geometry_msgs/Vector3 acc # satellite acceleration (m/s2) +geometry_msgs/Vector3 jerk # satellite jerk (m/s3) +float64 af0 # satellite clock bias +float64 af1 # satellite clock drift (s,s/s) +float64 tgd # TGD +float64[] isc # ISC \ No newline at end of file diff --git a/rtklib/msg/QzssIonosphereCorrection.msg b/rtklib/msg/QzssIonosphereCorrection.msg new file mode 100644 index 0000000..4801b02 --- /dev/null +++ b/rtklib/msg/QzssIonosphereCorrection.msg @@ -0,0 +1,6 @@ +# QZSS LEX ionosphere correction type + +time t0 # epoch time (GPST) +float64 tspan # valid time span (s) +float64[2] pos0 # reference position {lat,lon} (rad) +float64[6] coefs # coefficients lat x lon (3 x 2) \ No newline at end of file diff --git a/rtklib/msg/SbasEphemeris.msg b/rtklib/msg/SbasEphemeris.msg new file mode 100644 index 0000000..c9563d6 --- /dev/null +++ b/rtklib/msg/SbasEphemeris.msg @@ -0,0 +1,13 @@ +# SBAS ephemeris type + +int32 sat # satellite number +time t0 # reference epoch time (GPST) +time tof # time of message frame (GPST) +int32 sva # SV accuracy (URA index) +int32 svh # SV health (0:ok) + +geometry_msgs/Vector3 pos # satellite position (m) (ecef) +geometry_msgs/Vector3 vel # satellite velocity (m/s) (ecef) +geometry_msgs/Vector3 acc # satellite acceleration (m/s^2) (ecef) +float64 af0 # satellite clock-offset (s,s/s) +float64 af1 # satellite drift (s,s/s) \ No newline at end of file diff --git a/rtklib/msg/SbasFastCorrection.msg b/rtklib/msg/SbasFastCorrection.msg new file mode 100644 index 0000000..1aed802 --- /dev/null +++ b/rtklib/msg/SbasFastCorrection.msg @@ -0,0 +1,9 @@ +# SBAS fast correction type + +time t0 # time of applicability (TOF) +float64 prc # pseudorange correction (PRC) (m) +float64 rrc # range-rate correction (RRC) (m/s) +float64 dt # range-rate correction delta-time (s) +int32 iodf # IODF (issue of date fast corr) +int16 udre # UDRE+1 +int16 ai # degradation factor indicator \ No newline at end of file diff --git a/rtklib/msg/SbasLongTermErrorCorrection.msg b/rtklib/msg/SbasLongTermErrorCorrection.msg new file mode 100644 index 0000000..483bee2 --- /dev/null +++ b/rtklib/msg/SbasLongTermErrorCorrection.msg @@ -0,0 +1,8 @@ +# SBAS long term satellite error correction type + +time t0 # correction time +int32 iode # IODE (issue of date ephemeris) +geometry_msgs/Vector3 dpos # delta position (m) (ecef) +geometry_msgs/Vector3 dvel # delta velocity (m/s) (ecef) +float64 daf0 # delta clock-offset(s,s/s) +float64 daf1s # delta drift (s,s/s) \ No newline at end of file diff --git a/rtklib/msg/TotalElectronContent.msg b/rtklib/msg/TotalElectronContent.msg new file mode 100644 index 0000000..ec93a92 --- /dev/null +++ b/rtklib/msg/TotalElectronContent.msg @@ -0,0 +1,12 @@ +# TEC type + +time time # epoch time (GPST) + +int32[] ndata # TEC grid data size {nlat,nlon,nhgt} + +float64 rb # earth radius (km) +float64[] lats # latitude start/interval (deg) +float64[] lons # longitude start/interval (deg) +float64[] hgts # heights start/interval (km) +float64[] data # TEC grid data (tecu) +float32[] rms # RMS values (tecu) \ No newline at end of file diff --git a/rtklib/msg/cbias.msg b/rtklib/msg/cbias.msg new file mode 100644 index 0000000..c910dfe --- /dev/null +++ b/rtklib/msg/cbias.msg @@ -0,0 +1,5 @@ +# code bias (0:p1-p2,1:p1-c1,2:p2-c2) (m) + +float64 p1_p2 +float64 p1_c1 +float64 p2_c2 \ No newline at end of file diff --git a/rtklib/msg/erp_t.msg b/rtklib/msg/erp_t.msg new file mode 100644 index 0000000..a72e7e7 --- /dev/null +++ b/rtklib/msg/erp_t.msg @@ -0,0 +1,3 @@ +# earth rotation parameter type + +erpd_t[] data # earth rotation parameter data \ No newline at end of file diff --git a/rtklib/msg/erpd_t.msg b/rtklib/msg/erpd_t.msg new file mode 100644 index 0000000..e03fa09 --- /dev/null +++ b/rtklib/msg/erpd_t.msg @@ -0,0 +1,9 @@ +# earth rotation parameter data type + +float64 mjd # mjd (days) +float64 xp +float64 yp # pole offset (rad) +float64 xpr +float64 ypr # pole offset rate (rad/day) +float64 ut1_utc # ut1-utc (s) +float64 lod # length of day (s/day) \ No newline at end of file diff --git a/rtklib/msg/gtec_t.msg b/rtklib/msg/gtec_t.msg new file mode 100644 index 0000000..085f543 --- /dev/null +++ b/rtklib/msg/gtec_t.msg @@ -0,0 +1,9 @@ +# G_TEC type + +time time # epoch time (GPST) + +int32 sat # satellite number +int32 ntype # number of stec data types +int32 ndata # number of stec data + +float32[] data # G_TEC data \ No newline at end of file diff --git a/rtklib/msg/lam.msg b/rtklib/msg/lam.msg new file mode 100644 index 0000000..ccb0241 --- /dev/null +++ b/rtklib/msg/lam.msg @@ -0,0 +1,8 @@ +#Datatype for lam message type (carrier wave lengths (m) ) + +float64 L1 +float64 L2 +float64 L5 +float64 L6 +float64 L7 +float64 L8 diff --git a/rtklib/msg/sbsigp_t.msg b/rtklib/msg/sbsigp_t.msg new file mode 100644 index 0000000..48655de --- /dev/null +++ b/rtklib/msg/sbsigp_t.msg @@ -0,0 +1,7 @@ +# SBAS ionospheric correction type + +time t0 # correction time +int16 lat # latitude (deg) +int16 lon # longitude (deg) +int16 give # GIVI+1 +float32 delay # vertical delay estimate (m) \ No newline at end of file diff --git a/rtklib/msg/sbsion_t.msg b/rtklib/msg/sbsion_t.msg new file mode 100644 index 0000000..6d23dbd --- /dev/null +++ b/rtklib/msg/sbsion_t.msg @@ -0,0 +1,5 @@ +# SBAS ionospheric corrections type + +int32 iodi # IODI (issue of date ionos corr) +int32 nigp # number of igps +sbsigp_t[] igp # ionospheric correction \ No newline at end of file diff --git a/rtklib/msg/sbssat_t.msg b/rtklib/msg/sbssat_t.msg new file mode 100644 index 0000000..96072dd --- /dev/null +++ b/rtklib/msg/sbssat_t.msg @@ -0,0 +1,6 @@ +# SBAS satellite corrections type + +int32 iodp # IODP (issue of date mask) +int32 nsat # number of satellites +int32 tlat # system latency (s) +sbssatp_t[] sat # satellite correction \ No newline at end of file diff --git a/rtklib/msg/sbssatp_t.msg b/rtklib/msg/sbssatp_t.msg new file mode 100644 index 0000000..85f7b28 --- /dev/null +++ b/rtklib/msg/sbssatp_t.msg @@ -0,0 +1,5 @@ +# SBAS satellite correction type + +int32 sat # satellite number +SbasFastCorrection fcorr # fast correction +SbasLongTermErrorCorrection lcorr # long term correction \ No newline at end of file diff --git a/rtklib/msg/ssr_t.msg b/rtklib/msg/ssr_t.msg new file mode 100644 index 0000000..63c5950 --- /dev/null +++ b/rtklib/msg/ssr_t.msg @@ -0,0 +1,14 @@ + # SSR correction type + +time t0 # epoch time (GPST) +float64[] udi # SSR update interval (s) +int32[] iod +int32 iode # issue of data +int32 ura # URA indicator +int32 refd # sat ref datum (0:ITRF,1:regional) +float64[] deph # delta orbit {radial,along,cross} (m) +float64[] ddeph # dot delta orbit {radial,along,cross} (m/s) +float64[] dclk # delta clock {c0,c1,c2} (m,m/s,m/s^2) +float64 hrclk # high-rate clock corection (m) +float32[] cbias # code biases (m) +uint8 update # update flag (0:no update,1:update) \ No newline at end of file diff --git a/rtklib/msg/sta_t.msg b/rtklib/msg/sta_t.msg new file mode 100644 index 0000000..a78486f --- /dev/null +++ b/rtklib/msg/sta_t.msg @@ -0,0 +1,17 @@ +# station parameter type + +std_msgs/String name # marker name +std_msgs/String marker # marker number +std_msgs/String antdes # antenna descriptor +std_msgs/String antsno # antenna serial number +std_msgs/String rectype # receiver type descriptor +std_msgs/String recver # receiver firmware version +std_msgs/String recsno # receiver serial number + +int32 antsetup # antenna setup id +int32 itrf # ITRF realization year +int32 deltype # antenna delta type (0:enu,1:xyz) + +geometry_msgs/Vector3 pos # station position (ecef) (m) +geometry_msgs/Vector3 del # antenna position delta (e/n/u or x/y/z) (m) +int64 hgt # antenna height (m) \ No newline at end of file diff --git a/rtklib/msg/stec_t.msg b/rtklib/msg/stec_t.msg new file mode 100644 index 0000000..172dc50 --- /dev/null +++ b/rtklib/msg/stec_t.msg @@ -0,0 +1,7 @@ +# stec grid type + +float64[] pos #latitude/longitude (deg) +int32[] index #search index +int32 n +int32 nmax #number of data +stecd_t[] data #stec data diff --git a/rtklib/msg/stecd_t.msg b/rtklib/msg/stecd_t.msg new file mode 100644 index 0000000..6ae9278 --- /dev/null +++ b/rtklib/msg/stecd_t.msg @@ -0,0 +1,8 @@ +# stec data type + +Header header #time (GPST) +uint8 sat #satellite number +uint8 slip #slip flag +float32 iono #L1 ionosphere delay (m) +float32 rate #L1 ionosphere rate (m/s) +float32 rms #rms value (m) \ No newline at end of file diff --git a/rtkrcv/CMakeLists.txt b/rtkrcv/CMakeLists.txt index 28ba9cd..9d2c512 100644 --- a/rtkrcv/CMakeLists.txt +++ b/rtkrcv/CMakeLists.txt @@ -30,8 +30,10 @@ rosbuild_find_ros_package(rtklib) include_directories(${rtklib_PACKAGE_PATH}/include) find_library(LIBRTK NAMES rtk PATHS ${rtklib_PACKAGE_PATH}/lib) message(${LIBRTK}) + rosbuild_add_executable(${PROJECT_NAME} - src/main.cpp src/ros_options.cpp - src/rtkrcv.cpp) + src/rtkrcv.cpp + src/main_aisehe.cpp + ) target_link_libraries(${PROJECT_NAME} ${LIBRTK}) diff --git a/rtkrcv/src/main_aisehe.cpp b/rtkrcv/src/main_aisehe.cpp new file mode 100644 index 0000000..1d29698 --- /dev/null +++ b/rtkrcv/src/main_aisehe.cpp @@ -0,0 +1,348 @@ +#include +#include +#include +#include //for raw2rtcm, rtcm2rtcm +#include //for input_rtcm2, input_rtcm3 +#include //for input_raw +#include +#include +#include +#include +#include + +// Function to convert Observation message from raw format to ros +void obsfunc(rtklib::Observation obsarr, rtklib::ObservationData obserdata , strconv_t *conv){ + int i, j; + for(i=0; iraw.obs.nmax; i++){ + obserdata.sat=conv->raw.obs.data[i].sat; + obserdata.rcv=conv->raw.obs.data[i].rcv; + + for(j=0; j<(NFREQ+NEXOBS); j++){ + obserdata.SNR[j]=conv->raw.obs.data[i].SNR[j]; + obserdata.LLI[j]=conv->raw.obs.data[i].LLI[j]; + obserdata.code[j]=conv->raw.obs.data[i].code[j]; + obserdata.L[j]=conv->raw.obs.data[i].L[j]; + obserdata.P[j]=conv->raw.obs.data[i].P[j]; + obserdata.D[j]=conv->raw.obs.data[i].D[j]; + } + obsarr.data.push_back(obserdata); + } +} + + +// Function to convert Navigation message from raw format to ros +void navmsgfunc(rtklib::Navigation navigmsg, strconv_t* conv){ + + int i, j, p; + + for(i=0; iraw.nav.nmax; i++){ + navigmsg.eph[i].sat=conv->raw.nav.eph[i].sat; + } + for(i=0; iraw.nav.ngmax; i++){ //time not implemented + navigmsg.geph[i].sat=conv->raw.nav.geph[i].sat; + navigmsg.geph[i].iode=conv->raw.nav.geph[i].iode; + navigmsg.geph[i].frq=conv->raw.nav.geph[i].frq; + navigmsg.geph[i].svh=conv->raw.nav.geph[i].svh; + navigmsg.geph[i].sva=conv->raw.nav.geph[i].sva; + navigmsg.geph[i].age=conv->raw.nav.geph[i].age; + navigmsg.geph[i].pos.x=conv->raw.nav.geph[i].pos[0]; + navigmsg.geph[i].pos.y=conv->raw.nav.geph[i].pos[1]; + navigmsg.geph[i].pos.z=conv->raw.nav.geph[i].pos[2]; + navigmsg.geph[i].vel.x=conv->raw.nav.geph[i].vel[0]; + navigmsg.geph[i].vel.y=conv->raw.nav.geph[i].vel[1]; + navigmsg.geph[i].vel.z=conv->raw.nav.geph[i].vel[2]; + navigmsg.geph[i].acc.x=conv->raw.nav.geph[i].acc[0]; + navigmsg.geph[i].acc.y=conv->raw.nav.geph[i].acc[1]; + navigmsg.geph[i].acc.z=conv->raw.nav.geph[i].acc[2]; + navigmsg.geph[i].taun=conv->raw.nav.geph[i].taun; + navigmsg.geph[i].gamn=conv->raw.nav.geph[i].gamn; + navigmsg.geph[i].dtaun=conv->raw.nav.geph[i].dtaun; + } + for(i=0; iraw.nav.nsmax; i++){ //time not implemented + navigmsg.seph[i].sat=conv->raw.nav.seph[i].sat; + navigmsg.seph[i].sva=conv->raw.nav.seph[i].sva; + navigmsg.seph[i].svh=conv->raw.nav.seph[i].svh; + navigmsg.seph[i].af0=conv->raw.nav.seph[i].af0; + navigmsg.seph[i].af1=conv->raw.nav.seph[i].af1; + + navigmsg.seph[i].pos.x=conv->raw.nav.seph[i].pos[0]; + navigmsg.seph[i].pos.y=conv->raw.nav.seph[i].pos[1]; + navigmsg.seph[i].pos.z=conv->raw.nav.seph[i].pos[2]; + + navigmsg.seph[i].vel.x=conv->raw.nav.seph[i].vel[0]; + navigmsg.seph[i].vel.y=conv->raw.nav.seph[i].vel[1]; + navigmsg.seph[i].vel.z=conv->raw.nav.seph[i].vel[2]; + + navigmsg.seph[i].acc.x=conv->raw.nav.seph[i].acc[0]; + navigmsg.seph[i].acc.y=conv->raw.nav.seph[i].acc[1]; + navigmsg.seph[i].acc.z=conv->raw.nav.seph[i].acc[2]; + + } + for(i=0; iraw.nav.nemax; i++){ //time not implemented + int e; + + navigmsg.peph[i].index=conv->raw.nav.peph[i].index; + for(e=0; eraw.nav.peph[i].pos[e][1]; // How do I represent double array in ROS msg format + navigmsg.peph[i].std[e]=conv->raw.nav.peph[i].std[e][1]; + // } + } + } + + for(i=0; iraw.nav.ncmax; i++){ + navigmsg.pclk[i].index=conv->raw.nav.pclk[i].index; + int e; + for(e=0; eraw.nav.pclk[i].clk[e][1]; + navigmsg.pclk[i].std[e]=conv->raw.nav.pclk[i].std[e][1]; + } + } + + for(i=0; iraw.nav.namax; i++){ //time not implemented gtime_t + navigmsg.alm[i].sat=conv->raw.nav.alm[i].sat; + navigmsg.alm[i].svh=conv->raw.nav.alm[i].svh; + navigmsg.alm[i].svconf=conv->raw.nav.alm[i].svconf; + navigmsg.alm[i].week=conv->raw.nav.alm[i].week; + navigmsg.alm[i].A=conv->raw.nav.alm[i].A; + navigmsg.alm[i].e=conv->raw.nav.alm[i].e; + navigmsg.alm[i].i0=conv->raw.nav.alm[i].i0; + navigmsg.alm[i].OMG0=conv->raw.nav.alm[i].OMG0; + navigmsg.alm[i].omg=conv->raw.nav.alm[i].omg; + navigmsg.alm[i].M0=conv->raw.nav.alm[i].M0; + navigmsg.alm[i].toas=conv->raw.nav.alm[i].toas; + navigmsg.alm[i].f0=conv->raw.nav.alm[i].f0; + navigmsg.alm[i].f1=conv->raw.nav.alm[i].f1; + } + + for(i=0; iraw.nav.ntmax; i++){ //time not implemented with gtime_t + navigmsg.tec[i].rb=conv->raw.nav.tec[i].rb; + int f; + for(f=0; f<3; f++){ + navigmsg.tec[i].lats[f]=conv->raw.nav.tec[i].lats[f]; + navigmsg.tec[i].lons[f]=conv->raw.nav.tec[i].lons[f]; + navigmsg.tec[i].hgts[f]=conv->raw.nav.tec[i].hgts[f]; + navigmsg.tec[i].ndata[f]=conv->raw.nav.tec[i].ndata[f]; + //not sure about the following 2 whether their size is 3 + navigmsg.tec[i].data[f]=conv->raw.nav.tec[i].data[f]; + navigmsg.tec[i].rms[f]=conv->raw.nav.tec[i].rms[f]; + } + + } + + for(i=0; iraw.nav.nnmax; i++){ + navigmsg.stec[i].n=conv->raw.nav.stec[i].n; + navigmsg.stec[i].nmax=conv->raw.nav.stec[i].nmax; + navigmsg.stec[i].pos[1]=conv->raw.nav.stec[i].pos[1]; + navigmsg.stec[i].pos[2]=conv->raw.nav.stec[i].pos[2]; + int g,h; + for(g=0; graw.nav.stec[i].index[g]; + } + + for(h=0; hraw.nav.stec[i].nmax; h++){ + navigmsg.stec[i].data[h].sat=conv->raw.nav.stec[i].data[h].sat; + navigmsg.stec[i].data[h].slip=conv->raw.nav.stec[i].data[h].slip; + navigmsg.stec[i].data[h].iono=conv->raw.nav.stec[i].data[h].iono; + navigmsg.stec[i].data[h].rate=conv->raw.nav.stec[i].data[h].rate; + navigmsg.stec[i].data[h].rms=conv->raw.nav.stec[i].data[h].rms; + } + } + + + for(i=0; iraw.nav.erp.nmax;i++){ + navigmsg.erp.data[i].mjd =conv->raw.nav.erp.data[i].mjd; + navigmsg.erp.data[i].xp =conv->raw.nav.erp.data[i].xp; + navigmsg.erp.data[i].yp =conv->raw.nav.erp.data[i].yp; + navigmsg.erp.data[i].xpr =conv->raw.nav.erp.data[i].xpr; + navigmsg.erp.data[i].ypr =conv->raw.nav.erp.data[i].ypr; + navigmsg.erp.data[i].ut1_utc =conv->raw.nav.erp.data[i].ut1_utc; + navigmsg.erp.data[i].lod =conv->raw.nav.erp.data[i].lod; + } + + for(i=0; iraw.nav.pcvs[i].sat; //you have to add some component here + } + + navigmsg.sbssat.iodp=conv->raw.nav.sbssat.iodp; + navigmsg.sbssat.nsat=conv->raw.nav.sbssat.nsat; + navigmsg.sbssat.tlat=conv->raw.nav.sbssat.tlat; // you have to add some component here + + + + for(i=0; i<(MAXBAND+1); i++){ //because MAXBAND=10 + navigmsg.sbsion[i].iodi=conv->raw.nav.sbsion[i].iodi; + navigmsg.sbsion[i].nigp=conv->raw.nav.sbsion[i].nigp; + + for(p=0; praw.nav.sbsion[i].igp[p].lat; + navigmsg.sbsion[i].igp[p].lon=conv->raw.nav.sbsion[i].igp[p].lon; + navigmsg.sbsion[i].igp[p].give=conv->raw.nav.sbsion[i].igp[p].give; + navigmsg.sbsion[i].igp[p].delay=conv->raw.nav.sbsion[i].igp[p].delay; + } + } + + for(i=0; iraw.nav.dgps[i].prc; + navigmsg.dgps[i].rrc=conv->raw.nav.dgps[i].rrc; + navigmsg.dgps[i].iod=conv->raw.nav.dgps[i].iod; + navigmsg.dgps[i].udre=conv->raw.nav.dgps[i].udre; + } + + for(i=0; iraw.nav.ssr[i].udi[m]; + navigmsg.ssr[i].iod[m]=conv->raw.nav.ssr[i].iod[m]; + } + + for(n=0; n<3; n++){ + navigmsg.ssr[i].deph[n]=conv->raw.nav.ssr[i].deph[n]; + navigmsg.ssr[i].ddeph[n]=conv->raw.nav.ssr[i].ddeph[n]; + navigmsg.ssr[i].dclk[n]=conv->raw.nav.ssr[i].dclk[n]; + } + + navigmsg.ssr[i].iode=conv->raw.nav.ssr[i].iode; + navigmsg.ssr[i].ura=conv->raw.nav.ssr[i].ura; + navigmsg.ssr[i].refd=conv->raw.nav.ssr[i].refd; + navigmsg.ssr[i].hrclk=conv->raw.nav.ssr[i].hrclk; + navigmsg.ssr[i].update=conv->raw.nav.ssr[i].update; + + int b; + for(b=0;braw.nav.ssr[i].cbias[b]; + } + } + + for(i=0; iraw.nav.lexeph[i].af0; + navigmsg.lexeph[i].af1=conv->raw.nav.lexeph[i].af1; + navigmsg.lexeph[i].tgd=conv->raw.nav.lexeph[i].tgd; + navigmsg.lexeph[i].sat=conv->raw.nav.lexeph[i].sat; + navigmsg.lexeph[i].health=conv->raw.nav.lexeph[i].health; + navigmsg.lexeph[i].ura=conv->raw.nav.lexeph[i].ura; + navigmsg.lexeph[i].pos.x=conv->raw.nav.lexeph[i].pos[0]; + navigmsg.lexeph[i].pos.y=conv->raw.nav.lexeph[i].pos[1]; + navigmsg.lexeph[i].pos.z=conv->raw.nav.lexeph[i].pos[2]; + + navigmsg.lexeph[i].vel.x=conv->raw.nav.lexeph[i].vel[0]; + navigmsg.lexeph[i].vel.y=conv->raw.nav.lexeph[i].vel[1]; + navigmsg.lexeph[i].vel.z=conv->raw.nav.lexeph[i].vel[2]; + + navigmsg.lexeph[i].acc.x=conv->raw.nav.lexeph[i].acc[0]; + navigmsg.lexeph[i].acc.y=conv->raw.nav.lexeph[i].acc[1]; + navigmsg.lexeph[i].acc.z=conv->raw.nav.lexeph[i].acc[2]; + + navigmsg.lexeph[i].jerk.x=conv->raw.nav.lexeph[i].jerk[0]; + navigmsg.lexeph[i].jerk.y=conv->raw.nav.lexeph[i].jerk[1]; + navigmsg.lexeph[i].jerk.z=conv->raw.nav.lexeph[i].jerk[2]; + + int o; + for(o=0; o<8;o++){ + navigmsg.lexeph[i].isc[o]=conv->raw.nav.lexeph[i].isc[o]; + } + } + + navigmsg.lexion.tspan=conv->raw.nav.lexion.tspan; + navigmsg.lexion.pos0[0]=conv->raw.nav.lexion.pos0[0]; + navigmsg.lexion.pos0[1]=conv->raw.nav.lexion.pos0[1]; + /* Get this checked + navigmsg.lexion.coef[0]=conv->raw.nav.lexion.coef[1][1]; + navigmsg.lexion.coef[1]=conv->raw.nav.lexion.coef[1][2]; + navigmsg.lexion.coef[2]=conv->raw.nav.lexion.coef[2][1]; + navigmsg.lexion.coef[3]=conv->raw.nav.lexion.coef[2][2]; + navigmsg.lexion.coef[4]=conv->raw.nav.lexion.coef[3][1]; + navigmsg.lexion.coef[5]=conv->raw.nav.lexion.coef[3][2]; +*/ + + for(i=0; iraw.nav.lam[i][0]; + navigmsg.lamcwl[i].L2=conv->raw.nav.lam[i][1]; + navigmsg.lamcwl[i].L5=conv->raw.nav.lam[i][2]; + navigmsg.lamcwl[i].L6=conv->raw.nav.lam[i][3]; + navigmsg.lamcwl[i].L7=conv->raw.nav.lam[i][4]; + navigmsg.lamcwl[i].L8=conv->raw.nav.lam[i][5]; + } + + for(i=0; iraw.nav.cbias[i][0]; + navigmsg.codebias[i].p1_c1=conv->raw.nav.cbias[i][1]; + navigmsg.codebias[i].p2_c2=conv->raw.nav.cbias[i][2]; + } + + for(j=0; j<4; j++){ + navigmsg.utc_gps[j]=conv->raw.nav.utc_gps[j]; + navigmsg.utc_glo[j]=conv->raw.nav.utc_glo[j]; + navigmsg.utc_gal[j]=conv->raw.nav.utc_gal[j]; + navigmsg.utc_qzs[j]=conv->raw.nav.utc_qzs[j]; + navigmsg.utc_cmp[j]=conv->raw.nav.utc_cmp[j]; + navigmsg.utc_sbs[j]=conv->raw.nav.utc_sbs[j]; + navigmsg.ion_gal[j]=conv->raw.nav.ion_gal[j]; + } + for(j=0; j<8; j++){ + navigmsg.ion_gps[j]=conv->raw.nav.ion_gps[j]; + navigmsg.ion_qzs[j]=conv->raw.nav.ion_qzs[j]; + navigmsg.ion_cmp[j]=conv->raw.nav.ion_cmp[j]; + } + navigmsg.leaps=conv->raw.nav.leaps; + +} + +// convert stream ------------------------------------------------------------------------------- +static void raw2ros(ros::Publisher& pub_navigation_msg, ros::Publisher& pub_observation_msg, strconv_t *conv, unsigned char *buff, int n) +{ + int ret,k; + + rtklib::Navigation navmsg; + rtklib::Observation obsmsg; + rtklib::ObservationData obsdata; + + for (k=0;kitype==STRFMT_RTCM2) { + continue; + } + // input rtcm 3 messages + else if (conv->itype==STRFMT_RTCM3) { + continue; + } + + // input receiver raw messages + else { + ret=input_raw(&conv->raw,conv->itype,buff[k]); + + if(ret==1){ + obsfunc(obsmsg,obsdata,conv); + } + + if(ret==2){ + navmsgfunc(navmsg, conv); + } + + pub_navigation_msg.publish(navmsg); + pub_observation_msg.publish(obsmsg); + } + } +} + + +int main(int argc, char** argv){ + + ros::NodeHandle nh; + + ros::Publisher pub_navigation_msg = nh.advertise("nav_topic", 10); + ros::Publisher pub_observation_msg = nh.advertise("obs_topic", 10); + ros::Rate r(1); + strconv_t *conv; + unsigned char *buff; + int n; + + while (ros::ok()){ + + raw2ros(pub_navigation_msg, pub_observation_msg, conv, buff, n); + ros::spinOnce(); + } + +return 0; +} \ No newline at end of file