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