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