mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-16 04:05:46 +00:00
Code Issues Releases Wiki Activity

Fix the TOW weel crossover situation in observables and PVT

Browse Source
This commit is contained in:
Javier Arribas 2019-05-08 14:55:14 +02:00
parent f43fdece82
commit 7ed88c26b6
5 changed files with 59 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/algorithms/libs/rtklib/rtklib_ephemeris.cc
View File

@ -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;

32
src/algorithms/libs/rtklib/rtklib_rtkcmn.cc
View File

@ -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

1
src/algorithms/libs/rtklib/rtklib_rtkcmn.h
View File

@ -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);

26
src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc
View File

@ -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<double>(d_gnss_synchro_history->at(ch, t1_idx).TOW_at_current_symbol_ms) + (static_cast<double>(d_gnss_synchro_history->at(ch, t2_idx).TOW_at_current_symbol_ms) - static_cast<double>(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<double>(d_gnss_synchro_history->at(ch, t1_idx).TOW_at_current_symbol_ms) + (static_cast<double>(d_gnss_synchro_history->at(ch, t2_idx).TOW_at_current_symbol_ms) - static_cast<double>(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<double>(d_gnss_synchro_history->at(ch, t1_idx).TOW_at_current_symbol_ms) + (static_cast<double>(d_gnss_synchro_history->at(ch, t2_idx).TOW_at_current_symbol_ms + 604800000) - static_cast<double>(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<Gnss_Synchro> &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<Gnss_Synchro> &data)
// std::cout.precision(17);
// std::cout << " T_rx_TOW_ms: " << static_cast<double>(T_rx_TOW_ms) << std::endl;
std::vector<Gnss_Synchro>::iterator it;
double current_T_rx_TOW_s = (static_cast<double>(T_rx_TOW_ms - T_rx_remnant_to_20ms) + GPS_STARTOFFSET_MS) / 1000.0;
double current_T_rx_TOW_ms = (static_cast<double>(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
//

1
src/core/system_parameters/MATH_CONSTANTS.h
View File

@ -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_ */