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