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