/** Simulates real observations for the current position and the satellite almanac and week
* given in simulator_data.
*
* NOTES:
*
* - This simulates the pseudorange as the true distance to the satellite + noise.
* - This simulates the carrier phase as the true distance in wavelengths + bais + noise.
* - The bias is an integer multiple of 10 for easy debugging.
* - The satellite SNR/CN0 is proportional to the elevation of the satellite.
*
* USES:
* - Pipe observations into internals for testing
* - For integration testing with other devices that has to carry the radio signal.
*
* \param elapsed Number of seconds elapsed since last simulation step.
*/
void simulation_step_tracking_and_observations(double elapsed)
{
(void)elapsed;
u8 week = -1;
double t = sim_state.noisy_solution.time.tow;
/* First we calculate all the current sat positions, velocities */
for (u8 i=0; i<simulation_num_almanacs; i++) {
calc_sat_state_almanac(&simulation_almanacs[i], t, week,
simulation_sats_pos[i], simulation_sats_vel[i]);
}
/* Calculate the first sim_settings.num_sats amount of visible sats */
u8 num_sats_selected = 0;
double az, el;
for (u8 i=0; i<simulation_num_almanacs; i++) {
calc_sat_az_el_almanac(&simulation_almanacs[i], t, week,
sim_state.pos, &az, &el);
if (el > 0 &&
num_sats_selected < sim_settings.num_sats &&
num_sats_selected < MAX_CHANNELS) {
/* Generate a code measurement which is just the pseudorange: */
double points_to_sat[3];
double base_points_to_sat[3];
vector_subtract(3, simulation_sats_pos[i], sim_state.pos, points_to_sat);
vector_subtract(3, simulation_sats_pos[i], sim_settings.base_ecef, base_points_to_sat);
double distance_to_sat = vector_norm(3, points_to_sat);
double base_distance_to_sat = vector_norm(3, base_points_to_sat);
/* Fill out the observation details into the NAV_MEAS structure for this satellite, */
/* We simulate the pseudorange as a noisy range measurement, and */
/* the carrier phase as a noisy range in wavelengths + an integer offset. */
populate_nav_meas(&sim_state.nav_meas[num_sats_selected],
distance_to_sat, el, i);
populate_nav_meas(&sim_state.base_nav_meas[num_sats_selected],
base_distance_to_sat, el, i);
/* As for tracking, we just set each sat consecutively in each channel. */
/* This will cause weird jumps when a satellite rises or sets. */
sim_state.tracking_channel[num_sats_selected].state = TRACKING_RUNNING;
sim_state.tracking_channel[num_sats_selected].prn = simulation_almanacs[i].prn + SIM_PRN_OFFSET;
sim_state.tracking_channel[num_sats_selected].cn0 = sim_state.nav_meas[num_sats_selected].snr;
num_sats_selected++;
}
}
sim_state.noisy_solution.n_used = num_sats_selected;
}
/** Using available almanac and ephemeris information, determine
* whether a satellite is in view and the range of doppler frequencies
* in which we expect to find it.
*
* \param prn 0-indexed PRN
* \param t Time at which to evaluate ephemeris and almanac (typically system's
* estimate of current time)
* \param dopp_hint_low, dopp_hint_high Pointers to store doppler search range
* from ephemeris or almanac, if available and elevation > mask
* \return Score (higher is better)
*/
static u16 manage_warm_start(u8 prn, gps_time_t t,
float *dopp_hint_low, float *dopp_hint_high)
{
/* Do we have any idea where/when we are? If not, no score. */
/* TODO: Stricter requirement on time and position uncertainty?
We ought to keep track of a quantitative uncertainty estimate. */
if (time_quality < TIME_GUESS &&
position_quality < POSITION_GUESS)
return SCORE_COLDSTART;
float el = 0;
double el_d, _, dopp_hint = 0, dopp_uncertainty = DOPP_UNCERT_ALMANAC;
/* Do we have a suitable ephemeris for this sat? If so, use
that in preference to the almanac. */
if (ephemeris_good(&es[prn], t)) {
double sat_pos[3], sat_vel[3], el_d;
calc_sat_state(&es[prn], t, sat_pos, sat_vel, &_, &_);
wgsecef2azel(sat_pos, position_solution.pos_ecef, &_, &el_d);
el = (float)(el_d) * R2D;
if (el < elevation_mask)
return SCORE_BELOWMASK;
vector_subtract(3, sat_pos, position_solution.pos_ecef, sat_pos);
vector_normalize(3, sat_pos);
/* sat_pos now holds unit vector from us to satellite */
vector_subtract(3, sat_vel, position_solution.vel_ecef, sat_vel);
/* sat_vel now holds velocity of sat relative to us */
dopp_hint = -GPS_L1_HZ * (vector_dot(3, sat_pos, sat_vel) / GPS_C
+ position_solution.clock_bias);
/* TODO: Check sign of receiver frequency offset correction */
if (time_quality >= TIME_FINE)
dopp_uncertainty = DOPP_UNCERT_EPHEM;
} else if (almanac[prn].valid) {
calc_sat_az_el_almanac(&almanac[prn], t.tow, t.wn-1024,
position_solution.pos_ecef, &_, &el_d);
el = (float)(el_d) * R2D;
if (el < elevation_mask)
return SCORE_BELOWMASK;
dopp_hint = -calc_sat_doppler_almanac(&almanac[prn], t.tow, t.wn,
position_solution.pos_ecef);
} else {
return SCORE_COLDSTART; /* Couldn't determine satellite state. */
}
/* Return the doppler hints and a score proportional to elevation */
*dopp_hint_low = dopp_hint - dopp_uncertainty;
*dopp_hint_high = dopp_hint + dopp_uncertainty;
return SCORE_COLDSTART + SCORE_WARMSTART * el / 90.f;
}
static void manage_calc_almanac_scores(void)
{
double az, el;
gps_time_t t = get_current_time();
for (u8 prn=0; prn<32; prn++) {
if (!almanac[prn].valid ||
time_quality == TIME_UNKNOWN ||
position_quality == POSITION_UNKNOWN) {
/* No almanac or position/time information, give it the benefit of the
* doubt. */
acq_prn_param[prn].score[ACQ_HINT_ALMANAC] = 0;
} else {
calc_sat_az_el_almanac(&almanac[prn], t.tow, t.wn-1024, position_solution.pos_ecef, &az, &el);
float dopp = -calc_sat_doppler_almanac(&almanac[prn], t.tow, t.wn,
position_solution.pos_ecef);
acq_prn_param[prn].score[ACQ_HINT_ALMANAC] =
el > 0 ? (u16)(SCORE_ALMANAC * 2 * el / M_PI) : 0;
acq_prn_param[prn].dopp_hint_low = dopp - ALMANAC_DOPPLER_WINDOW;
acq_prn_param[prn].dopp_hint_high = dopp + ALMANAC_DOPPLER_WINDOW;
}
}
}
static void manage_calc_scores(void)
{
double az, el;
gps_time_t t;
if (time_quality != TIME_UNKNOWN)
t = get_current_time();
for (u8 prn=0; prn<32; prn++) {
if (!almanac[prn].valid ||
time_quality == TIME_UNKNOWN ||
position_quality == POSITION_UNKNOWN) {
/* No almanac or position/time information, give it the benefit of the
* doubt. */
acq_prn_param[prn].score = 0;
} else {
calc_sat_az_el_almanac(&almanac[prn], t.tow, t.wn-1024, position_solution.pos_ecef, &az, &el);
acq_prn_param[prn].score = (s8)(el/D2R);
gps_time_t toa;
toa.wn = almanac[prn].week + 1024;
toa.tow = almanac[prn].toa;
double dt_alm = fabs(gpsdifftime(t, toa));
double dt_pos = fabs(gpsdifftime(t, position_solution.time));
if (time_quality == TIME_GUESS ||
dt_pos > 1*24*3600 ||
dt_alm > 4*24*3600) {
/* Don't exclude other sats if our time is just a guess, our last
* position solution was ages ago or our almanac is old. */
if (acq_prn_param[prn].score < 0)
acq_prn_param[prn].score = 0;
}
}
}
}
/** Using available almanac and ephemeris information, determine
* whether a satellite is in view and the range of doppler frequencies
* in which we expect to find it.
*
* \param prn 0-indexed PRN
* \param t Time at which to evaluate ephemeris and almanac (typically system's
* estimate of current time)
* \param dopp_hint_low, dopp_hint_high Pointers to store doppler search range
* from ephemeris or almanac, if available and elevation > mask
* \return Score (higher is better)
*/
static u16 manage_warm_start(gnss_signal_t sid, const gps_time_t* t,
float *dopp_hint_low, float *dopp_hint_high)
{
/* Do we have any idea where/when we are? If not, no score. */
/* TODO: Stricter requirement on time and position uncertainty?
We ought to keep track of a quantitative uncertainty estimate. */
if (time_quality < TIME_GUESS &&
position_quality < POSITION_GUESS)
return SCORE_COLDSTART;
float el = 0;
double _, dopp_hint = 0, dopp_uncertainty = DOPP_UNCERT_ALMANAC;
bool ready = false;
/* Do we have a suitable ephemeris for this sat? If so, use
that in preference to the almanac. */
const ephemeris_t *e = ephemeris_get(sid);
u8 eph_valid;
s8 ss_ret;
double sat_pos[3], sat_vel[3], el_d;
ephemeris_lock();
eph_valid = ephemeris_valid(e, t);
if (eph_valid) {
ss_ret = calc_sat_state(e, t, sat_pos, sat_vel, &_, &_);
}
ephemeris_unlock();
if (eph_valid && (ss_ret == 0)) {
wgsecef2azel(sat_pos, position_solution.pos_ecef, &_, &el_d);
el = (float)(el_d) * R2D;
if (el < elevation_mask)
return SCORE_BELOWMASK;
vector_subtract(3, sat_pos, position_solution.pos_ecef, sat_pos);
vector_normalize(3, sat_pos);
/* sat_pos now holds unit vector from us to satellite */
vector_subtract(3, sat_vel, position_solution.vel_ecef, sat_vel);
/* sat_vel now holds velocity of sat relative to us */
dopp_hint = -GPS_L1_HZ * (vector_dot(3, sat_pos, sat_vel) / GPS_C
+ position_solution.clock_bias);
/* TODO: Check sign of receiver frequency offset correction */
if (time_quality >= TIME_FINE)
dopp_uncertainty = DOPP_UNCERT_EPHEM;
ready = true;
}
if(!ready) {
const almanac_t *a = &almanac[sid_to_global_index(sid)];
if (a->valid &&
calc_sat_az_el_almanac(a, t, position_solution.pos_ecef,
&_, &el_d) == 0) {
el = (float)(el_d) * R2D;
if (el < elevation_mask)
return SCORE_BELOWMASK;
if (calc_sat_doppler_almanac(a, t, position_solution.pos_ecef,
&dopp_hint) != 0) {
return SCORE_COLDSTART;
}
dopp_hint = -dopp_hint;
} else {
return SCORE_COLDSTART; /* Couldn't determine satellite state. */
}
}
/* Return the doppler hints and a score proportional to elevation */
*dopp_hint_low = dopp_hint - dopp_uncertainty;
*dopp_hint_high = dopp_hint + dopp_uncertainty;
return SCORE_COLDSTART + SCORE_WARMSTART * el / 90.f;
}
