From 7ed88c26b603c193c6e8988e13955c738f40e6f8 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Wed, 8 May 2019 14:55:14 +0200 Subject: [PATCH] Fix the TOW weel crossover situation in observables and PVT --- .../libs/rtklib/rtklib_ephemeris.cc | 24 +++++++------- src/algorithms/libs/rtklib/rtklib_rtkcmn.cc | 32 +++++++++++++++---- src/algorithms/libs/rtklib/rtklib_rtkcmn.h | 1 + .../gnuradio_blocks/hybrid_observables_gs.cc | 26 +++++++++++---- src/core/system_parameters/MATH_CONSTANTS.h | 1 + 5 files changed, 59 insertions(+), 25 deletions(-) diff --git a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc index bd72b7f9f..5c7383a5b 100644 --- a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc +++ b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc @@ -128,7 +128,7 @@ void alm2pos(gtime_t time, const alm_t *alm, double *rs, double *dts) trace(4, "alm2pos : time=%s sat=%2d\n", time_str(time, 3), alm->sat); - tk = timediff(time, alm->toa); + tk = timediffweekcrossover(time, alm->toa); if (alm->A <= 0.0) { @@ -181,7 +181,7 @@ double eph2clk(gtime_t time, const eph_t *eph) trace(4, "eph2clk : time=%s sat=%2d\n", time_str(time, 3), eph->sat); - t = timediff(time, eph->toc); + t = timediffweekcrossover(time, eph->toc); for (i = 0; i < 2; i++) { @@ -218,7 +218,7 @@ void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts, rs[0] = rs[1] = rs[2] = *dts = *var = 0.0; return; } - tk = timediff(time, eph->toe); + tk = timediffweekcrossover(time, eph->toe); switch ((sys = satsys(eph->sat, &prn))) { @@ -288,7 +288,7 @@ void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts, rs[1] = x * sinO + y * cosi * cosO; rs[2] = y * sin(i); } - tk = timediff(time, eph->toc); + tk = timediffweekcrossover(time, eph->toc); *dts = eph->f0 + eph->f1 * tk + eph->f2 * tk * tk; /* relativity correction */ @@ -433,7 +433,7 @@ double seph2clk(gtime_t time, const seph_t *seph) trace(4, "seph2clk: time=%s sat=%2d\n", time_str(time, 3), seph->sat); - t = timediff(time, seph->t0); + t = timediffweekcrossover(time, seph->t0); for (i = 0; i < 2; i++) { @@ -461,7 +461,7 @@ void seph2pos(gtime_t time, const seph_t *seph, double *rs, double *dts, trace(4, "seph2pos: time=%s sat=%2d\n", time_str(time, 3), seph->sat); - t = timediff(time, seph->t0); + t = timediffweekcrossover(time, seph->t0); for (i = 0; i < 3; i++) { @@ -508,7 +508,7 @@ eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav) { continue; } - if ((t = fabs(timediff(nav->eph[i].toe, time))) > tmax) + if ((t = fabs(timediffweekcrossover(nav->eph[i].toe, time))) > tmax) { continue; } @@ -588,7 +588,7 @@ seph_t *selseph(gtime_t time, int sat, const nav_t *nav) { continue; } - if ((t = fabs(timediff(nav->seph[i].t0, time))) > tmax) + if ((t = fabs(timediffweekcrossover(nav->seph[i].t0, time))) > tmax) { continue; } @@ -792,9 +792,9 @@ int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav, *svh = -1; return 0; } - t1 = timediff(time, ssr->t0[0]); - t2 = timediff(time, ssr->t0[1]); - t3 = timediff(time, ssr->t0[2]); + t1 = timediffweekcrossover(time, ssr->t0[0]); + t2 = timediffweekcrossover(time, ssr->t0[1]); + t3 = timediffweekcrossover(time, ssr->t0[2]); /* ssr orbit and clock correction (ref [4]) */ if (fabs(t1) > MAXAGESSR || fabs(t2) > MAXAGESSR) @@ -847,7 +847,7 @@ int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav, } /* satellite clock by clock parameters */ - tk = timediff(time, eph->toc); + tk = timediffweekcrossover(time, eph->toc); dts[0] = eph->f0 + eph->f1 * tk + eph->f2 * tk * tk; dts[1] = eph->f1 + 2.0 * eph->f2 * tk; diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc index 83935ef39..884c9db03 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc @@ -223,12 +223,11 @@ const unsigned int TBL_CR_C24_Q[] = { 0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538}; -extern "C" -{ - void dgemm_(char *, char *, int *, int *, int *, double *, double *, int *, double *, int *, double *, double *, int *); - extern void dgetrf_(int *, int *, double *, int *, int *, int *); - extern void dgetri_(int *, double *, int *, int *, double *, int *, int *); - extern void dgetrs_(char *, int *, int *, double *, int *, int *, double *, int *, int *); +extern "C" { +void dgemm_(char *, char *, int *, int *, int *, double *, double *, int *, double *, int *, double *, double *, int *); +extern void dgetrf_(int *, int *, double *, int *, int *, int *); +extern void dgetri_(int *, double *, int *, int *, double *, int *, int *); +extern void dgetrs_(char *, int *, int *, double *, int *, int *, double *, int *, int *); } @@ -1690,7 +1689,28 @@ double timediff(gtime_t t1, gtime_t t2) return difftime(t1.time, t2.time) + t1.sec - t2.sec; } +/* time difference accounting with week crossovers ------------------------------------------------------------- + * difference between gtime_t structs + * args : gtime_t t1,t2 I gtime_t structs + * return : time difference (t1-t2) (s) + *-----------------------------------------------------------------------------*/ +double timediffweekcrossover(gtime_t t1, gtime_t t2) +{ + //as stated in IS-GPS-200J table 20-IV footnote among other parts of the ICD, + //if tk=(t - toe) > 302400s then tk = tk - s + //if tk=(t - toe) < -302400s then tk = tk + 604800s + double tk = difftime(t1.time, t2.time) + t1.sec - t2.sec; + if (tk > 302400.0) + { + tk -= 604800.0; + } + else if (tk < -302400.0) + { + tk += 604800.0; + } + return tk; +} /* get current time in utc ----------------------------------------------------- * get current time in utc * args : none diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.h b/src/algorithms/libs/rtklib/rtklib_rtkcmn.h index 8f98bfeb0..ea7a870dc 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.h +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.h @@ -173,6 +173,7 @@ gtime_t bdt2time(int week, double sec); double time2bdt(gtime_t t, int *week); gtime_t timeadd(gtime_t t, double sec); double timediff(gtime_t t1, gtime_t t2); +double timediffweekcrossover(gtime_t t1, gtime_t t2); gtime_t timeget(void); void timeset(gtime_t t); int read_leaps_text(FILE *fp); diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc index 0945bd1e6..24c53587c 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc @@ -441,8 +441,16 @@ bool hybrid_observables_gs::interp_trk_obs(Gnss_Synchro &interpolated_obs, const // CARRIER DOPPLER INTERPOLATION interpolated_obs.Carrier_Doppler_hz = d_gnss_synchro_history->at(ch, t1_idx).Carrier_Doppler_hz + (d_gnss_synchro_history->at(ch, t2_idx).Carrier_Doppler_hz - d_gnss_synchro_history->at(ch, t1_idx).Carrier_Doppler_hz) * time_factor; // TOW INTERPOLATION - interpolated_obs.interp_TOW_ms = static_cast(d_gnss_synchro_history->at(ch, t1_idx).TOW_at_current_symbol_ms) + (static_cast(d_gnss_synchro_history->at(ch, t2_idx).TOW_at_current_symbol_ms) - static_cast(d_gnss_synchro_history->at(ch, t1_idx).TOW_at_current_symbol_ms)) * time_factor; - + // check TOW rollover + if ((d_gnss_synchro_history->at(ch, t2_idx).TOW_at_current_symbol_ms - d_gnss_synchro_history->at(ch, t1_idx).TOW_at_current_symbol_ms) > 0) + { + interpolated_obs.interp_TOW_ms = static_cast(d_gnss_synchro_history->at(ch, t1_idx).TOW_at_current_symbol_ms) + (static_cast(d_gnss_synchro_history->at(ch, t2_idx).TOW_at_current_symbol_ms) - static_cast(d_gnss_synchro_history->at(ch, t1_idx).TOW_at_current_symbol_ms)) * time_factor; + } + else + { + //TOW rollover situation + interpolated_obs.interp_TOW_ms = static_cast(d_gnss_synchro_history->at(ch, t1_idx).TOW_at_current_symbol_ms) + (static_cast(d_gnss_synchro_history->at(ch, t2_idx).TOW_at_current_symbol_ms + 604800000) - static_cast(d_gnss_synchro_history->at(ch, t1_idx).TOW_at_current_symbol_ms)) * time_factor; + } // LOG(INFO) << "Channel " << ch << " int idx: " << t1_idx << " TOW Int: " << interpolated_obs.interp_TOW_ms // << " TOW p1 : " << d_gnss_synchro_history->at(ch, t1_idx).TOW_at_current_symbol_ms @@ -507,9 +515,9 @@ void hybrid_observables_gs::update_TOW(const std::vector &data) else { T_rx_TOW_ms += T_rx_step_ms; //the tow time step increment must match the ref time channel step - //todo: check what happens during the week rollover if (T_rx_TOW_ms >= 604800000) { + DLOG(INFO) << "TOW RX TIME rollover!"; T_rx_TOW_ms = T_rx_TOW_ms % 604800000; } } @@ -521,15 +529,19 @@ void hybrid_observables_gs::compute_pranges(std::vector &data) // std::cout.precision(17); // std::cout << " T_rx_TOW_ms: " << static_cast(T_rx_TOW_ms) << std::endl; std::vector::iterator it; - double current_T_rx_TOW_s = (static_cast(T_rx_TOW_ms - T_rx_remnant_to_20ms) + GPS_STARTOFFSET_MS) / 1000.0; + double current_T_rx_TOW_ms = (static_cast(T_rx_TOW_ms - T_rx_remnant_to_20ms)); + double current_T_rx_TOW_s = current_T_rx_TOW_ms / 1000.0; for (it = data.begin(); it != data.end(); it++) { if (it->Flag_valid_word) { - double traveltime_s = current_T_rx_TOW_s - it->interp_TOW_ms / 1000.0; - //todo: check what happens during the week rollover (TOW rollover at 604800000ms) + double traveltime_ms = current_T_rx_TOW_ms - it->interp_TOW_ms; + if (fabs(traveltime_ms) > 302400) //check TOW roll over + { + traveltime_ms = 604800000.0 + current_T_rx_TOW_ms - it->interp_TOW_ms; + } it->RX_time = current_T_rx_TOW_s; - it->Pseudorange_m = traveltime_s * SPEED_OF_LIGHT; + it->Pseudorange_m = traveltime_ms * SPEED_OF_LIGHT_MS; it->Flag_valid_pseudorange = true; // debug code // diff --git a/src/core/system_parameters/MATH_CONSTANTS.h b/src/core/system_parameters/MATH_CONSTANTS.h index 3e7fa3c16..dec9eef93 100644 --- a/src/core/system_parameters/MATH_CONSTANTS.h +++ b/src/core/system_parameters/MATH_CONSTANTS.h @@ -115,6 +115,7 @@ const double AS2R = (D2R / 3600.0); //!< arc sec to radian const double DEFAULT_OMEGA_EARTH_DOT = 7.2921151467e-5; //!< Default Earth rotation rate, [rad/s] const double SPEED_OF_LIGHT = 299792458.0; //!< [m/s] +const double SPEED_OF_LIGHT_MS = 299792.4580; //!< [m/ms] const double AU = 149597870691.0; //!< 1 Astronomical Unit AU (m) distance from Earth to the Sun. #endif /* GNSS_SDR_MATH_CONSTANTS_H_ */