/** 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;
}
/** 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;
}
/** SBP callback for observation messages.
* SBP observation sets are potentially split across multiple SBP messages to
* keep the payload within the size limit.
*
* The header contains a count of how many total messages there are in this set
* of observations (all referring to the same observation time) and a count of
* which message this is in the sequence.
*
* This function attempts to collect a full set of observations into a single
* `obss_t` (`base_obss_rx`). Once a full set is received then update_obss()
* is called.
*/
static void obs_callback(u16 sender_id, u8 len, u8 msg[], void* context)
{
(void) context;
/* Keep track of where in the sequence of messages we were last time around
* so we can verify we haven't dropped a message. */
static s16 prev_count = 0;
static gps_time_t prev_t = {.tow = 0.0, .wn = 0};
/* As we receive observation messages we assemble them into a working
* `obss_t` (`base_obss_rx`) so as not to disturb the global `base_obss`
* state that may be in use. */
static obss_t base_obss_rx = {.has_pos = 0};
/* An SBP sender ID of zero means that the messages are relayed observations
* from the console, not from the base station. We don't want to use them and
* we don't want to create an infinite loop by forwarding them again so just
* ignore them. */
if (sender_id == 0) {
return;
}
/* Relay observations using sender_id = 0. */
sbp_send_msg_(SBP_MSG_OBS, len, msg, 0);
/* GPS time of observation. */
gps_time_t t;
/* Total number of messages in the observation set / sequence. */
u8 total;
/* The current message number in the sequence. */
u8 count;
/* Decode the message header to get the time and how far through the sequence
* we are. */
unpack_obs_header((observation_header_t*)msg, &t, &total, &count);
/* Check to see if the observation is aligned with our internal observations,
* i.e. is it going to time match one of our local obs. */
u32 obs_freq = soln_freq / obs_output_divisor;
double epoch_count = t.tow * obs_freq;
double dt = fabs(epoch_count - round(epoch_count)) / obs_freq;
if (dt > TIME_MATCH_THRESHOLD) {
log_warn("Unaligned observation from base station ignored, "
"tow = %.3f, dt = %.3f", t.tow, dt);
return;
}
/* Calculate packet latency. */
if (time_quality >= TIME_COARSE) {
gps_time_t now = get_current_time();
float latency_ms = (float) ((now.tow - t.tow) * 1000.0);
log_obs_latency(latency_ms);
}
/* Verify sequence integrity */
if (count == 0) {
prev_t = t;
prev_count = 0;
} else if (prev_t.tow != t.tow ||
prev_t.wn != t.wn ||
prev_count + 1 != count) {
log_info("Dropped one of the observation packets! Skipping this sequence.");
prev_count = -1;
return;
} else {
prev_count = count;
}
/* Calculate the number of observations in this message by looking at the SBP
* `len` field. */
u8 obs_in_msg = (len - sizeof(observation_header_t)) / sizeof(packed_obs_content_t);
/* If this is the first packet in the sequence then reset the base_obss_rx
* state. */
if (count == 0) {
base_obss_rx.n = 0;
base_obss_rx.t = t;
}
/* Pull out the contents of the message. */
packed_obs_content_t *obs = (packed_obs_content_t *)(msg + sizeof(observation_header_t));
for (u8 i=0; i<obs_in_msg; i++) {
/* Check the PRN is valid. e.g. simulation mode outputs test observations
* with PRNs >200. */
if (obs[i].sid > 31) { /* TODO prn - sid; assume everything below is 0x1F masked! */
continue;
}
/* Flag this as visible/viable to acquisition/search */
manage_set_obs_hint(obs[i].sid);
/* Check if we have an ephemeris for this satellite, we will need this to
* fill in satellite position etc. parameters. */
chMtxLock(&es_mutex);
if (ephemeris_good(&es[obs[i].sid], t)) {
/* Unpack the observation into a navigation_measurement_t. */
unpack_obs_content(
&obs[i],
&base_obss_rx.nm[base_obss_rx.n].raw_pseudorange,
&base_obss_rx.nm[base_obss_rx.n].carrier_phase,
&base_obss_rx.nm[base_obss_rx.n].snr,
&base_obss_rx.nm[base_obss_rx.n].lock_counter,
&base_obss_rx.nm[base_obss_rx.n].prn
);
double clock_err;
double clock_rate_err;
/* Calculate satellite parameters using the ephemeris. */
calc_sat_state(&es[obs[i].sid], t,
base_obss_rx.nm[base_obss_rx.n].sat_pos,
base_obss_rx.nm[base_obss_rx.n].sat_vel,
&clock_err, &clock_rate_err);
/* Apply corrections to the raw pseudorange. */
/* TODO Make a function to apply some of these corrections.
* They are used in a couple places. */
base_obss_rx.nm[base_obss_rx.n].pseudorange =
base_obss_rx.nm[base_obss_rx.n].raw_pseudorange + clock_err * GPS_C;
/* Set the time */
base_obss_rx.nm[base_obss_rx.n].tot = t;
base_obss_rx.n++;
}
chMtxUnlock();
}
/* If we can, and all the obs have been received, update to using the new
* obss. */
if (count == total - 1) {
update_obss(&base_obss_rx);
}
}
/** SBP callback for the old style observation messages.
* Just logs a deprecation warning. */
static void deprecated_callback(u16 sender_id, u8 len, u8 msg[], void* context)
{
(void) context; (void) len; (void) msg; (void) sender_id;
log_error("Receiving an old deprecated observation message.");
}
/** Setup the base station observation handling subsystem. */
void base_obs_setup()
{
/* Initialise all Mutex and Semaphore objects. */
chMtxInit(&base_obs_lock);
chBSemInit(&base_obs_received, TRUE);
chMtxInit(&base_pos_lock);
/* Register callbacks on base station messages. */
static sbp_msg_callbacks_node_t base_pos_node;
sbp_register_cbk(
SBP_MSG_BASE_POS,
&base_pos_callback,
&base_pos_node
);
static sbp_msg_callbacks_node_t obs_packed_node;
sbp_register_cbk(
SBP_MSG_OBS,
&obs_callback,
&obs_packed_node
);
static sbp_msg_callbacks_node_t deprecated_node;
sbp_register_cbk(
SBP_MSG_OBS_DEP_A,
&deprecated_callback,
&deprecated_node
);
}
