/** 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;
}
bool acq_search(gnss_signal_t sid, float cf_min, float cf_max,
float cf_bin_width, acq_result_t *acq_result)
{
acq_set_sid(sid);
/* We have our SID chosen, now load some fresh data
* into the acquisition ram on the Swift NAP for
* an initial coarse acquisition.
*/
u32 sample_count;
do {
sample_count = nap_timing_count() + 20000;
/* acq_load could timeout if we're preempted and miss the timing strobe */
} while (!acq_load(sample_count));
acq_search_begin(cf_min, cf_max, cf_bin_width);
/* 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_result->sample_count = sample_count;
acq_get_results(&acq_result->cp, &acq_result->cf, &acq_result->cn0);
return true;
}
/** Service an external event interrupt
*
* When an event occurs (i.e. pin edge) that matches the NAP's trigger
* condition, the NAP will latch the time, pin number and trigger direction.
* It will also set an IRQ bit which will lead to an EXTI. The firmware
* EXTI handling routine handle_nap_exti() will call this function, which
* reads out the details and spits them out as an SBP message to our host.
*
*/
void ext_event_service(void)
{
u8 event_pin;
ext_event_trigger_t event_trig;
/* Read the details, and also clear IRQ + set up for next time */
u32 event_nap_time = nap_rw_ext_event(&event_pin, &event_trig, trigger);
/* We have to infer the most sig word (i.e. # of 262-second rollovers) */
union {
u32 half[2];
u64 full;
} tc;
tc.full = nap_timing_count();
if (tc.half[0] < event_nap_time) /* Rollover occurred since event */
tc.half[1]--;
tc.half[0] = event_nap_time;
/* Prepare the MSG_EXT_EVENT */
msg_ext_event_t msg;
msg.flags = (event_trig == TRIG_RISING) ? (1<<0) : (0<<0);
if (time_quality == TIME_FINE)
msg.flags |= (1 << 1);
msg.pin = event_pin;
/* Convert to the SBP convention of rounded ms + signed ns residual */
gps_time_t gpst = rx2gpstime(tc.full);
msg_gps_time_t mgt;
sbp_make_gps_time(&mgt, &gpst, 0);
msg.wn = mgt.wn;
msg.tow = mgt.tow;
msg.ns = mgt.ns;
sbp_send_msg(SBP_MSG_EXT_EVENT, sizeof(msg), (u8 *)&msg);
}
/** 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);
}
/** Get current GPS time.
*
* \note The GPS time may only be a guess or completely unknown. time_quality
* should be checked first to determine the quality of the GPS time
* estimate.
*
* This function should be used only for approximate timing purposes as simply
* calling this function does not give a well defined instant at which the GPS
* time is queried.
*
* \return Current GPS time.
*/
gps_time_t get_current_time(void)
{
/* TODO: Return invalid when TIME_UNKNOWN. */
/* TODO: Think about what happens when nap_timing_count overflows. */
u64 tc = nap_timing_count();
gps_time_t t = rx2gpstime(tc);
return t;
}
/** Update GPS time estimate.
*
* This function may be called to update the GPS time estimate. If the time is
* already known more precisely than the new estimate, the new estimate will be
* ignored.
*
* This function should not be used to give an estimate with TIME_FINE quality
* as this must be referenced to a particular value of the SwiftNAP timing
* count.
*
* \param quality Quality of the time estimate.
* \param t GPS time estimate.
*/
void set_time(time_quality_t quality, gps_time_t t)
{
if (quality > time_quality) {
clock_state.t0_gps = t;
clock_state.t0_gps.tow -= nap_timing_count() * RX_DT_NOMINAL;
clock_state.t0_gps = normalize_gps_time(clock_state.t0_gps);
time_quality = quality;
time_t unix_t = gps2time(t);
log_info("Time set to: %s (quality=%d)", ctime(&unix_t), quality);
}
}
/** 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);
}
int main(void)
{
init();
led_toggle(LED_RED);
printf("\n\nFirmware info - git: " GIT_VERSION ", built: " __DATE__ " " __TIME__ "\n");
u8 nap_git_hash[20];
nap_conf_rd_git_hash(nap_git_hash);
printf("SwiftNAP git: ");
for (u8 i=0; i<20; i++)
printf("%02x", nap_git_hash[i]);
if (nap_conf_rd_git_unclean())
printf(" (unclean)");
printf("\n");
printf("SwiftNAP configured with %d tracking channels\n\n", nap_track_n_channels);
cw_setup();
manage_acq_setup();
tick_timer_setup();
timing_setup();
position_setup();
channel_measurement_t meas[nap_track_n_channels];
navigation_measurement_t nav_meas[nap_track_n_channels];
/* TODO: Think about thread safety when updating ephemerides. */
static ephemeris_t es[32];
static ephemeris_t es_old[32];
while(1)
{
for (u32 i = 0; i < 3000; i++)
__asm__("nop");
sbp_process_messages();
manage_track();
manage_acq();
/* Check if there is a new nav msg subframe to process.
* TODO: move this into a function */
memcpy(es_old, es, sizeof(es));
for (u8 i=0; i<nap_track_n_channels; i++)
if (tracking_channel[i].state == TRACKING_RUNNING && tracking_channel[i].nav_msg.subframe_start_index) {
s8 ret = process_subframe(&tracking_channel[i].nav_msg, &es[tracking_channel[i].prn]);
if (ret < 0)
printf("PRN %02d ret %d\n", tracking_channel[i].prn+1, ret);
if (ret == 1 && !es[tracking_channel[i].prn].healthy)
printf("PRN %02d unhealthy\n", tracking_channel[i].prn+1);
if (memcmp(&es[tracking_channel[i].prn], &es_old[tracking_channel[i].prn], sizeof(ephemeris_t))) {
printf("New ephemeris for PRN %02d\n", tracking_channel[i].prn+1);
/* TODO: This is a janky way to set the time... */
gps_time_t t;
t.wn = es[tracking_channel[i].prn].toe.wn;
t.tow = tracking_channel[i].TOW_ms / 1000.0;
if (gpsdifftime(t, es[tracking_channel[i].prn].toe) > 2*24*3600)
t.wn--;
else if (gpsdifftime(t, es[tracking_channel[i].prn].toe) < 2*24*3600)
t.wn++;
set_time(TIME_COARSE, t);
}
}
DO_EVERY_TICKS(TICK_FREQ/10,
u8 n_ready = 0;
for (u8 i=0; i<nap_track_n_channels; i++) {
if (es[tracking_channel[i].prn].valid == 1 && \
es[tracking_channel[i].prn].healthy == 1 && \
tracking_channel[i].state == TRACKING_RUNNING && \
tracking_channel[i].TOW_ms > 0) {
__asm__("CPSID i;");
tracking_update_measurement(i, &meas[n_ready]);
__asm__("CPSIE i;");
n_ready++;
}
}
if (n_ready >= 4) {
/* Got enough sats/ephemerides, do a solution. */
/* TODO: Instead of passing 32 LSBs of nap_timing_count do something
* more intelligent with the solution time.
*/
calc_navigation_measurement(n_ready, meas, nav_meas, (double)((u32)nap_timing_count())/SAMPLE_FREQ, es);
dops_t dops;
if (calc_PVT(n_ready, nav_meas, &position_solution, &dops) == 0) {
position_updated();
sbp_send_msg(MSG_SOLUTION, sizeof(gnss_solution), (u8 *) &position_solution);
nmea_gpgga(&position_solution, &dops);
DO_EVERY(10,
sbp_send_msg(MSG_DOPS, sizeof(dops_t), (u8 *) &dops);
nmea_gpgsv(n_ready, nav_meas, &position_solution);
);
}