/** Do a blocking acquisition search in two stages : coarse and fine. * Do a coarse acqusition to find the approximate code phase and carrier * frequency, and then a more fine grained acquisition to find the code phase * and carrier frequency more precisely. * * \param prn PRN to search (nap_acq_code_wr_blocking must be called prior) * \param cp Code phase of the acquisition result * \param cf Carrier frequency of the acquisition result * \param snr SNR of the acquisition result */ u32 acq_full_two_stage(u8 prn, float* cp, float* cf, float* snr) { /* Initial coarse acq. */ float coarse_code_phase; float coarse_carrier_freq; float coarse_snr; u32 coarse_count = nap_timing_count() + 1000; acq_schedule_load(coarse_count); while(!acq_get_load_done()); acq_start(prn, 0, 1023, -7000, 7000, 300); while(!acq_get_done()); acq_get_results(&coarse_code_phase, &coarse_carrier_freq, &coarse_snr); /* Fine acq. */ u32 fine_count = nap_timing_count() + 2000; acq_schedule_load(fine_count); while(!acq_get_load_done()); float fine_cp = propagate_code_phase(coarse_code_phase, coarse_carrier_freq, fine_count - coarse_count); acq_start(prn, fine_cp-20, fine_cp+20, coarse_carrier_freq-300, coarse_carrier_freq+300, 100); while(!acq_get_done()); acq_get_results(cp, cf, snr); return fine_count; }
/** Write to a NAP track channel's INIT register. * Sets PRN (deprecated), initial carrier phase, and initial code phase of a * NAP track channel. The tracking channel will start correlating with these * parameters at the falling edge of the next NAP internal timing strobe. * Also write CA code to track channel's code ram. * * \note The track channel's UPDATE register, which sets the carrier and * code phase rates, must also be written to before the internal timing * strobe goes low. * * \param channel NAP track channel whose INIT register to write. * \param prn CA code PRN (0-31) to track. (deprecated) * \param carrier_phase Initial code phase. * \param code_phase Initial carrier phase. */ void nap_track_init(u8 channel, gnss_signal_t sid, u32 ref_timing_count, float carrier_freq, float code_phase) { struct nap_ch_state *s = &nap_ch_state[channel]; memset(s, 0, sizeof(*s)); u32 track_count = nap_timing_count() + 20000; float cp = propagate_code_phase(code_phase, carrier_freq, track_count - ref_timing_count); /* Contrive for the timing strobe to occur at or close to a * PRN edge (code phase = 0) */ track_count += (NAP_FRONTEND_SAMPLE_RATE_Hz / GPS_CA_CHIPPING_RATE) * (1023.0-cp) * (1.0 + carrier_freq / GPS_L1_HZ); nap_track_code_wr_blocking(channel, sid); nap_track_init_wr_blocking(channel, 0, 0, 0); double cp_rate = (1.0 + carrier_freq / GPS_L1_HZ) * GPS_CA_CHIPPING_RATE; nap_track_update(channel, carrier_freq, cp_rate, 0, 0); /* Schedule the timing strobe for start_sample_count. */ track_count -= NAP_FRONTEND_SAMPLE_RATE_Hz / (2 * GPS_CA_CHIPPING_RATE); s->count_snapshot = track_count; s->carrier_phase = -s->carr_pinc; s->carr_pinc_prev = s->carr_pinc; s->code_pinc_prev = s->code_pinc; COMPILER_BARRIER(); nap_timing_strobe(track_count); nap_timing_strobe_wait(100); }
/** Manages acquisition searches and starts tracking channels after successful acquisitions. */ static void manage_acq() { /* Decide which PRN to try and then start it acquiring. */ u8 prn = choose_prn(); if (prn == (u8)-1) return; u32 timer_count; float snr, cp, cf; acq_set_prn(prn); /* We have our PRN chosen, now load some fresh data * into the acquisition ram on the Swift NAP for * an initial coarse acquisition. */ do { timer_count = nap_timing_count() + 20000; /* acq_load could timeout if we're preempted and miss the timing strobe */ } while (!acq_load(timer_count)); acq_search(acq_prn_param[prn].dopp_hint_low, acq_prn_param[prn].dopp_hint_high, ACQ_FULL_CF_STEP); /* Done with the coarse acquisition, check if we have found a * satellite, if so save the results and start the loading * for the fine acquisition. If not, start again choosing a * different PRN. */ acq_get_results(&cp, &cf, &snr); /* Send result of an acquisition to the host. */ acq_send_result(prn, snr, cp, cf); if (snr < ACQ_THRESHOLD) { /* Didn't find the satellite :( */ /* Double the size of the doppler search space for next time. */ float dilute = (acq_prn_param[prn].dopp_hint_high - acq_prn_param[prn].dopp_hint_low) / 2; acq_prn_param[prn].dopp_hint_high = MIN(acq_prn_param[prn].dopp_hint_high + dilute, ACQ_FULL_CF_MAX); acq_prn_param[prn].dopp_hint_low = MAX(acq_prn_param[prn].dopp_hint_low - dilute, ACQ_FULL_CF_MIN); /* Decay hint scores */ for (u8 i = 0; i < ACQ_HINT_NUM; i++) acq_prn_param[prn].score[i] = (acq_prn_param[prn].score[i] * 3) / 4; /* Reset hint score for acuisition. */ acq_prn_param[prn].score[ACQ_HINT_ACQ] = 0; return; } log_info("acq: PRN %d found @ %d Hz, %d SNR\n", prn + 1, (int)cf, (int)snr); u8 chan = manage_track_new_acq(); if (chan == MANAGE_NO_CHANNELS_FREE) { /* No channels are free to accept our new satellite :( */ /* TODO: Perhaps we can try to warm start this one * later using another fine acq. */ log_info("No channels free :(\n"); if (snr > ACQ_RETRY_THRESHOLD) { acq_prn_param[prn].score[ACQ_HINT_ACQ] = SCORE_ACQ + (snr - 20) / 20; acq_prn_param[prn].dopp_hint_low = cf - ACQ_FULL_CF_STEP; acq_prn_param[prn].dopp_hint_high = cf + ACQ_FULL_CF_STEP; } return; } /* Transition to tracking. */ u32 track_count = nap_timing_count() + 20000; cp = propagate_code_phase(cp, cf, track_count - timer_count); // Contrive for the timing strobe to occur at or close to a PRN edge (code phase = 0) track_count += 16*(1023.0-cp)*(1.0 + cf / GPS_L1_HZ); tracking_channel_init(chan, prn, cf, track_count, snr); acq_prn_param[prn].state = ACQ_PRN_TRACKING; nap_timing_strobe_wait(100); }
/** Manages acquisition searches and starts tracking channels after successful acquisitions. */ static void manage_acq() { /* Decide which PRN to try and then start it acquiring. */ u8 prn = choose_prn(); if (prn == (u8)-1) return; gnss_signal_t sid = {.sat = prn}; u32 timer_count; float cn0, cp, cf; acq_set_prn(prn); /* We have our PRN chosen, now load some fresh data * into the acquisition ram on the Swift NAP for * an initial coarse acquisition. */ do { timer_count = nap_timing_count() + 20000; /* acq_load could timeout if we're preempted and miss the timing strobe */ } while (!acq_load(timer_count)); /* Check for NaNs in dopp hints, or low > high */ if (!(acq_prn_param[prn].dopp_hint_low <= acq_prn_param[prn].dopp_hint_high)) { log_error("Acq: caught bogus dopp_hints (%f, %f)", acq_prn_param[prn].dopp_hint_low, acq_prn_param[prn].dopp_hint_high); acq_prn_param[prn].dopp_hint_high = ACQ_FULL_CF_MAX; acq_prn_param[prn].dopp_hint_low = ACQ_FULL_CF_MIN; } acq_search(acq_prn_param[prn].dopp_hint_low, acq_prn_param[prn].dopp_hint_high, ACQ_FULL_CF_STEP); /* Done with the coarse acquisition, check if we have found a * satellite, if so save the results and start the loading * for the fine acquisition. If not, start again choosing a * different PRN. */ acq_get_results(&cp, &cf, &cn0); /* Send result of an acquisition to the host. */ acq_send_result(sid, cn0, cp, cf); if (cn0 < ACQ_THRESHOLD) { /* Didn't find the satellite :( */ /* Double the size of the doppler search space for next time. */ float dilute = (acq_prn_param[prn].dopp_hint_high - acq_prn_param[prn].dopp_hint_low) / 2; acq_prn_param[prn].dopp_hint_high = MIN(acq_prn_param[prn].dopp_hint_high + dilute, ACQ_FULL_CF_MAX); acq_prn_param[prn].dopp_hint_low = MAX(acq_prn_param[prn].dopp_hint_low - dilute, ACQ_FULL_CF_MIN); /* Decay hint scores */ for (u8 i = 0; i < ACQ_HINT_NUM; i++) acq_prn_param[prn].score[i] = (acq_prn_param[prn].score[i] * 3) / 4; /* Reset hint score for acquisition. */ acq_prn_param[prn].score[ACQ_HINT_PREV_ACQ] = 0; return; } u8 chan = manage_track_new_acq(); if (chan == MANAGE_NO_CHANNELS_FREE) { /* No channels are free to accept our new satellite :( */ /* TODO: Perhaps we can try to warm start this one * later using another fine acq. */ if (cn0 > ACQ_RETRY_THRESHOLD) { acq_prn_param[prn].score[ACQ_HINT_PREV_ACQ] = SCORE_ACQ + (cn0 - ACQ_THRESHOLD); acq_prn_param[prn].dopp_hint_low = cf - ACQ_FULL_CF_STEP; acq_prn_param[prn].dopp_hint_high = cf + ACQ_FULL_CF_STEP; } return; } /* Transition to tracking. */ u32 track_count = nap_timing_count() + 20000; cp = propagate_code_phase(cp, cf, track_count - timer_count); // Contrive for the timing strobe to occur at or close to a PRN edge (code phase = 0) track_count += 16*(1023.0-cp)*(1.0 + cf / GPS_L1_HZ); /* Start the tracking channel */ tracking_channel_init(chan, sid, cf, track_count, cn0, TRACKING_ELEVATION_UNKNOWN); /* TODO: Initialize elevation from ephemeris if we know it precisely */ acq_prn_param[prn].state = ACQ_PRN_TRACKING; nap_timing_strobe_wait(100); }
/** Manages acquisition searches and starts tracking channels after successful acquisitions. */ void manage_acq() { /* Decide which PRN to try and then start it acquiring. */ u8 prn = best_prn(); if (prn == (u8)-1) return; u32 timer_count; float snr, cp, cf; acq_set_prn(prn); /* We have our PRN chosen, now load some fresh data * into the acquisition ram on the Swift NAP for * an initial coarse acquisition. */ acq_prn_param[prn].state = ACQ_PRN_ACQUIRING; do { timer_count = nap_timing_count() + 20000; /* acq_load could timeout if we're preempted and miss the timing strobe */ } while (!acq_load(timer_count)); /* Done loading, now lets set that coarse acquisition going. */ if (almanac[prn].valid && time_quality == TIME_COARSE) { gps_time_t t = rx2gpstime(timer_count); double dopp = -calc_sat_doppler_almanac(&almanac[prn], t.tow, t.wn, position_solution.pos_ecef); /* TODO: look into accuracy of prediction and possibilities for * improvement, e.g. use clock bias estimated by PVT solution. */ /*log_info("Expecting PRN %02d @ %.1f\n", prn+1, dopp);*/ acq_search(dopp - 4000, dopp + 4000, ACQ_FULL_CF_STEP); } else { acq_search(ACQ_FULL_CF_MIN, ACQ_FULL_CF_MAX, ACQ_FULL_CF_STEP); } /* Done with the coarse acquisition, check if we have found a * satellite, if so save the results and start the loading * for the fine acquisition. If not, start again choosing a * different PRN. */ acq_get_results(&cp, &cf, &snr); /* Send result of an acquisition to the host. */ acq_send_result(prn, snr, cp, cf); if (snr < ACQ_THRESHOLD) { /* Didn't find the satellite :( */ acq_prn_param[prn].state = ACQ_PRN_TRIED; return; } log_info("acq: PRN %d found @ %d Hz, %d SNR\n", prn + 1, (int)cf, (int)snr); u8 chan = manage_track_new_acq(snr); if (chan == MANAGE_NO_CHANNELS_FREE) { /* No channels are free to accept our new satellite :( */ /* TODO: Perhaps we can try to warm start this one * later using another fine acq. */ log_info("No channels free :(\n"); acq_prn_param[prn].state = ACQ_PRN_TRIED; return; } /* Transition to tracking. */ u32 track_count = nap_timing_count() + 20000; cp = propagate_code_phase(cp, cf, track_count - timer_count); // Contrive for the timing strobe to occur at or close to a PRN edge (code phase = 0) track_count += 16*(1023.0-cp)*(1.0 + cf / GPS_L1_HZ); tracking_channel_init(chan, prn, cf, track_count, snr); acq_prn_param[prn].state = ACQ_PRN_TRACKING; nap_timing_strobe_wait(100); }