static void decoder_gps_l1ca_process(const decoder_channel_info_t *channel_info,
decoder_data_t *decoder_data)
{
gps_l1ca_decoder_data_t *data = decoder_data;
/* Process incoming nav bits */
s8 soft_bit;
while (tracking_channel_nav_bit_get(channel_info->tracking_channel,
&soft_bit)) {
/* Update TOW */
bool bit_val = soft_bit >= 0;
s32 TOW_ms = nav_msg_update(&data->nav_msg, bit_val);
s8 bit_polarity = data->nav_msg.bit_polarity;
if ((TOW_ms >= 0) && (bit_polarity != BIT_POLARITY_UNKNOWN)) {
if (!tracking_channel_time_sync(channel_info->tracking_channel, TOW_ms,
bit_polarity)) {
log_warn_sid(channel_info->sid, "TOW set failed");
}
}
}
/* Check if there is a new nav msg subframe to process. */
if (!subframe_ready(&data->nav_msg))
return;
/* Decode ephemeris to temporary struct */
ephemeris_t e = {.sid = channel_info->sid};
s8 ret = process_subframe(&data->nav_msg, &e);;
if (ret <= 0)
return;
/* Decoded a new ephemeris. */
ephemeris_new(&e);
ephemeris_t *eph = ephemeris_get(channel_info->sid);
if (!eph->valid) {
log_info_sid(channel_info->sid, "ephemeris is invalid");
}
}
/** 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;
}
