diff --git a/src/board/v3/nap/fft.c b/src/board/v3/nap/fft.c index 30f66d1e..8ec836f5 100644 --- a/src/board/v3/nap/fft.c +++ b/src/board/v3/nap/fft.c @@ -131,7 +131,7 @@ static void sample_stream_start(void) u32 tc_req = NAP->TIMING_COUNT + TIMING_COMPARE_DELTA; NAP->ACQ_TIMING_COMPARE = tc_req; chSysUnlock(); - if (tc_req - NAP->ACQ_TIMING_SNAPSHOT <= TIMING_COMPARE_DELTA) { + if (tc_req - NAP->ACQ_TIMING_SNAPSHOT <= (u32)TIMING_COMPARE_DELTA) { break; } } diff --git a/src/board/v3/nap/track_channel.c b/src/board/v3/nap/track_channel.c index 0d1158e9..c97a4a07 100644 --- a/src/board/v3/nap/track_channel.c +++ b/src/board/v3/nap/track_channel.c @@ -11,6 +11,7 @@ */ #include "board.h" +#include "nap_hw.h" #include "nap/nap_common.h" #include "nap/track_channel.h" #include "track.h" @@ -28,6 +29,9 @@ #define TRACK_SAMPLE_FREQ (SAMPLE_FREQ / 4) +#define TIMING_COMPARE_DELTA_MIN ( 1e-3 * TRACK_SAMPLE_FREQ) /* 1ms */ +#define TIMING_COMPARE_DELTA_MAX (100e-3 * TRACK_SAMPLE_FREQ) /* 100ms */ + /* NAP track channel parameters. */ #define NAP_TRACK_CARRIER_FREQ_WIDTH 32 #define NAP_TRACK_CODE_PHASE_FRACTIONAL_WIDTH 32 @@ -60,18 +64,8 @@ static u32 calc_length_samples(u8 codes, s32 cp_start, u32 cp_rate) } void nap_track_init(u8 channel, gnss_signal_t sid, u32 ref_timing_count, - float carrier_freq, float code_phase) + float carrier_freq, float code_phase) { - u32 now = NAP->TIMING_COUNT; - u32 track_count = now + 200000; - double 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 += (SAMPLE_FREQ/GPS_CA_CHIPPING_RATE) * (1023.0-cp) * - (1.0 + carrier_freq / GPS_L1_HZ); - u8 prn = sid.sat - 1; NAP->TRK_CH[channel].CONTROL = prn << NAP_TRK_CONTROL_SAT_Pos; /* We always start at zero code phase */ @@ -94,8 +88,34 @@ void nap_track_init(u8 channel, gnss_signal_t sid, u32 ref_timing_count, COMPILER_BARRIER(); - NAP->TRK_TIMING_COMPARE = track_count - SAMPLE_FREQ / GPS_CA_CHIPPING_RATE; - chThdSleep(CH_CFG_ST_FREQUENCY * ceil((float)(track_count - now)/SAMPLE_FREQ)); + /* Set up timing compare */ + u32 tc_req; + while (1) { + chSysLock(); + tc_req = NAP->TIMING_COUNT + TIMING_COMPARE_DELTA_MIN; + double cp = propagate_code_phase(code_phase, carrier_freq, + tc_req - ref_timing_count); + /* Contrive for the timing strobe to occur at or close to a + * PRN edge (code phase = 0) */ + tc_req += (SAMPLE_FREQ / GPS_CA_CHIPPING_RATE) * (1023.0 - cp) * + (1.0 + carrier_freq / GPS_L1_HZ); + + NAP->TRK_TIMING_COMPARE = tc_req; + chSysUnlock(); + + if (tc_req - NAP->TRK_TIMING_SNAPSHOT <= (u32)TIMING_COMPARE_DELTA_MAX) { + break; + } + } + + /* Sleep until compare match */ + s32 tc_delta; + while ((tc_delta = tc_req - NAP->TIMING_COUNT) >= 0) { + systime_t sleep_time = ceil(CH_CFG_ST_FREQUENCY * tc_delta / SAMPLE_FREQ); + /* The next system tick will always occur less than the nominal tick period + * in the future, so sleep for an extra tick. */ + chThdSleep(1 + sleep_time); + } } void nap_track_update(u8 channel, double carrier_freq,