/** 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);
}
/** 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;
}
/** Retrieve a channel measurement for a tracker channel.
*
* \param id ID of the tracker channel to use.
* \param ref_tc Reference timing count.
* \param meas Pointer to output channel_measurement_t.
*/
void tracking_channel_measurement_get(tracker_channel_id_t id, u64 ref_tc,
channel_measurement_t *meas)
{
tracker_channel_t *tracker_channel = tracker_channel_get(id);
tracker_internal_data_t *internal_data =
&tracker_channel->internal_data;
const tracker_common_data_t *common_data = &tracker_channel->common_data;
/* Update our channel measurement. */
meas->sid = tracker_channel->info.sid;
meas->code_phase_chips = common_data->code_phase_early;
meas->code_phase_rate = common_data->code_phase_rate;
meas->carrier_phase = common_data->carrier_phase;
meas->carrier_freq = common_data->carrier_freq;
meas->time_of_week_ms = common_data->TOW_ms;
meas->rec_time_delta = (double)((s32)(common_data->sample_count - (u32)ref_tc))
/ SAMPLE_FREQ;
meas->snr = common_data->cn0;
if (internal_data->bit_polarity == BIT_POLARITY_INVERTED) {
meas->carrier_phase += 0.5;
}
meas->lock_counter = internal_data->lock_counter;
/* Adjust carrier phase initial integer offset to be approximately equal to
pseudorange. */
if ((time_quality == TIME_FINE)
&& (internal_data->carrier_phase_offset == 0.0)) {
gps_time_t tor = rx2gpstime(ref_tc + meas->rec_time_delta);
gps_time_t tot;
tot.tow = 1e-3 * meas->time_of_week_ms;
tot.tow += meas->code_phase_chips / GPS_CA_CHIPPING_RATE;
gps_time_match_weeks(&tot, &tor);
internal_data->carrier_phase_offset = round(GPS_L1_HZ
* gpsdifftime(&tor, &tot));
}
meas->carrier_phase -= internal_data->carrier_phase_offset;
}
/** 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);
}
