double predict_range(double rx_pos[3],
gps_time_t tot,
ephemeris_t *ephemeris)
{
double sat_pos[3];
double sat_vel[3];
double temp[3];
double clock_err, clock_rate_err;
calc_sat_pos(sat_pos, sat_vel, &clock_err, &clock_rate_err, ephemeris, tot);
vector_subtract(3, sat_pos, rx_pos, temp); // temp = sat_pos - rx_pos
return vector_norm(3, temp);
}
void calc_navigation_measurement_(u8 n_channels, channel_measurement_t* meas[], navigation_measurement_t* nav_meas[], double nav_time, ephemeris_t* ephemerides[])
{
double TOTs[n_channels];
double min_TOT = DBL_MAX;
for (u8 i=0; i<n_channels; i++) {
TOTs[i] = 1e-3 * meas[i]->time_of_week_ms;
TOTs[i] += meas[i]->code_phase_chips / 1.023e6;
TOTs[i] += (nav_time - meas[i]->receiver_time) * meas[i]->code_phase_rate / 1.023e6;
/** \todo Handle GPS time properly here, e.g. week rollover */
nav_meas[i]->tot.wn = ephemerides[i]->toe.wn;
nav_meas[i]->tot.tow = TOTs[i];
if (gpsdifftime(nav_meas[i]->tot, ephemerides[i]->toe) > 3*24*3600)
nav_meas[i]->tot.wn -= 1;
if (TOTs[i] < min_TOT)
min_TOT = TOTs[i];
nav_meas[i]->raw_doppler = meas[i]->carrier_freq;
nav_meas[i]->snr = meas[i]->snr;
nav_meas[i]->prn = meas[i]->prn;
nav_meas[i]->carrier_phase = meas[i]->carrier_phase;
nav_meas[i]->carrier_phase += (nav_time - meas[i]->receiver_time) * meas[i]->carrier_freq;
nav_meas[i]->lock_counter = meas[i]->lock_counter;
}
double clock_err, clock_rate_err;
for (u8 i=0; i<n_channels; i++) {
nav_meas[i]->raw_pseudorange = (min_TOT - TOTs[i])*GPS_C + GPS_NOMINAL_RANGE;
calc_sat_pos(nav_meas[i]->sat_pos, nav_meas[i]->sat_vel, &clock_err, &clock_rate_err, ephemerides[i], nav_meas[i]->tot);
nav_meas[i]->pseudorange = nav_meas[i]->raw_pseudorange \
+ clock_err*GPS_C;
nav_meas[i]->doppler = nav_meas[i]->raw_doppler + clock_rate_err*GPS_L1_HZ;
nav_meas[i]->tot.tow -= clock_err;
nav_meas[i]->tot = normalize_gps_time(nav_meas[i]->tot);
}
}
