Skip to content
This repository has been archived by the owner on Apr 13, 2021. It is now read-only.

Commit

Permalink
Clean up track_channel.c
Browse files Browse the repository at this point in the history
  • Loading branch information
@jacobmcnamee
jacobmcnamee committed May 26, 2016
1 parent f22c1b7 commit 915b73a
Show file tree
Hide file tree
Showing 3 changed files with 92 additions and 69 deletions.
19 changes: 11 additions & 8 deletions src/board/v2/nap/track_channel.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,9 @@
#define NAP_TRACK_CARRIER_FREQ_UNITS_PER_HZ \
((1 << NAP_TRACK_CARRIER_FREQ_WIDTH) / (double)SAMPLE_FREQ)

#define NAP_TRACK_CARRIER_PHASE_UNITS_PER_CYCLE \
(1 << NAP_TRACK_CARRIER_FREQ_WIDTH);

#define NAP_TRACK_NOMINAL_CODE_PHASE_RATE \
(1 << (NAP_TRACK_CODE_PHASE_WIDTH - 1))

Expand Down Expand Up @@ -90,17 +93,17 @@ void nap_track_init(u8 channel, gnss_signal_t sid, u32 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) *
track_count += (SAMPLE_FREQ / 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 + carrier_freq/GPS_L1_HZ) * GPS_CA_CHIPPING_RATE;
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 -= SAMPLE_FREQ / (2*GPS_CA_CHIPPING_RATE);
track_count -= SAMPLE_FREQ / (2 * GPS_CA_CHIPPING_RATE);

s->count_snapshot = track_count;
s->carrier_phase = -s->carr_pinc;
Expand All @@ -113,7 +116,6 @@ void nap_track_init(u8 channel, gnss_signal_t sid, u32 ref_timing_count,
nap_timing_strobe_wait(100);
}


/** Write to a NAP track channel's UPDATE register.
* Write new carrier frequency and code phase rate to a NAP track channel's
* UPDATE register, which will be used to accumulate the channel's carrier and
Expand All @@ -137,7 +139,7 @@ void nap_track_update(u8 channel, double carrier_freq,
{
struct nap_ch_state *s = &nap_ch_state[channel];
s->carr_pinc_prev = s->carr_pinc;
s->carr_pinc = (s32)(carrier_freq * NAP_TRACK_CARRIER_FREQ_UNITS_PER_HZ);
s->carr_pinc = carrier_freq * NAP_TRACK_CARRIER_FREQ_UNITS_PER_HZ;
s->code_pinc_prev = s->code_pinc;
s->code_pinc = code_phase_rate * NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ;

Expand Down Expand Up @@ -165,11 +167,12 @@ void nap_track_read_results(u8 channel,
s->code_phase += (u64)sample_count * s->code_pinc_prev;
s->carrier_phase += (s64)sample_count * s->carr_pinc_prev;
s->carr_pinc_prev = s->carr_pinc;
s->code_pinc_prev = s->code_pinc_prev;

*count_snapshot = s->count_snapshot;
*code_phase_early = (double)s->code_phase / NAP_TRACK_CODE_PHASE_UNITS_PER_CHIP;
*carrier_phase = (double)s->carrier_phase / (1 << NAP_TRACK_CARRIER_FREQ_WIDTH);
*code_phase_early = (double)s->code_phase /
NAP_TRACK_CODE_PHASE_UNITS_PER_CHIP;
*carrier_phase = (double)s->carrier_phase /
NAP_TRACK_CARRIER_PHASE_UNITS_PER_CYCLE;
}

void nap_track_disable(u8 channel)
Expand Down
11 changes: 9 additions & 2 deletions src/board/v3/nap/nap_hw.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,10 +104,17 @@ typedef struct {
#define NAP_ACQ_FFT_CONFIG_SCALE_Pos (1U)
#define NAP_ACQ_FFT_CONFIG_SCALE_Msk (0x3FFFFFFFU << NAP_ACQ_FFT_CONFIG_SCALE_Pos)

#define NAP_TRK_CONTROL_SAT_Pos (3u)
#define NAP_TRK_STATUS_OVF_Pos (0U)
#define NAP_TRK_STATUS_OVF_Msk (0x3FFU << NAP_TRK_STATUS_OVF_Pos)

#define NAP_TRK_CONTROL_SAT_Pos (3U)
#define NAP_TRK_CONTROL_SAT_Msk (0x1FU << NAP_TRK_CONTROL_SAT_Pos)

#define NAP_TRK_STATUS_RUNNING (1 << 31)
#define NAP_TRK_SPACING_OUTER_Pos (0U)
#define NAP_TRK_SPACING_OUTER_Msk (0xFFFFU << NAP_TRK_SPACING_OUTER_Pos)

#define NAP_TRK_SPACING_INNER_Pos (16U)
#define NAP_TRK_SPACING_INNER_Msk (0xFFFFU << NAP_TRK_SPACING_INNER_Pos)

/* Instances */
#define NAP ((nap_t *)0x43C00000)
Expand Down
131 changes: 72 additions & 59 deletions src/board/v3/nap/track_channel.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,57 +34,69 @@

/* NAP track channel parameters. */
#define NAP_TRACK_CARRIER_FREQ_WIDTH 32
#define NAP_TRACK_CARRIER_PHASE_FRACTIONAL_WIDTH 32
#define NAP_TRACK_CODE_PHASE_FRACTIONAL_WIDTH 32

#define NAP_TRACK_CARRIER_FREQ_UNITS_PER_HZ \
(((u64)1 << NAP_TRACK_CARRIER_FREQ_WIDTH) / (double)TRACK_SAMPLE_FREQ)

#define NAP_TRACK_CODE_PHASE_UNITS_PER_CHIP \
((u64)1 << NAP_TRACK_CODE_PHASE_FRACTIONAL_WIDTH)
#define NAP_TRACK_CARRIER_PHASE_UNITS_PER_CYCLE \
((u64)1 << NAP_TRACK_CARRIER_PHASE_FRACTIONAL_WIDTH)

#define NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ \
(NAP_TRACK_CODE_PHASE_UNITS_PER_CHIP / TRACK_SAMPLE_FREQ)
(NAP_TRACK_CODE_PHASE_UNITS_PER_CHIP / (double)TRACK_SAMPLE_FREQ)

#define SPACING_HALF_CHIP ((u16)(TRACK_SAMPLE_FREQ / GPS_CA_CHIPPING_RATE) / 2)
#define NAP_TRACK_CODE_PHASE_UNITS_PER_CHIP \
((u64)1 << NAP_TRACK_CODE_PHASE_FRACTIONAL_WIDTH)

BSEMAPHORE_DECL(timing_strobe_sem, TRUE);
#define SPACING_HALF_CHIP ((u16)(TRACK_SAMPLE_FREQ / GPS_CA_CHIPPING_RATE) / 2)

static struct {
u32 code_phase;
u32 len;
static struct nap_ch_state {
u32 code_phase; /**< Fractional part of code phase. */
} nap_ch_state[NAP_MAX_N_TRACK_CHANNELS];

static u32 calc_length_samples(u8 codes, s32 cp_start, u32 cp_rate)
static u32 calc_length_samples(u8 codes, u32 cp_start_frac_units,
u32 cp_rate_units)
{
u16 chips = codes * 1023;
u64 cp_units = (1ULL << NAP_TRACK_CODE_PHASE_FRACTIONAL_WIDTH) * chips
- cp_start;
u32 samples = cp_units / cp_rate;
u64 cp_end_units = codes * 1023 * NAP_TRACK_CODE_PHASE_UNITS_PER_CHIP;
/* cp_start_frac_units is reinterpreted as a signed value. This works
* because NAP_TRACK_CODE_PHASE_FRACTIONAL_WIDTH is equal to 32 */
u64 cp_units = cp_end_units - (s32)cp_start_frac_units;
u32 samples = cp_units / cp_rate_units;
return samples;
}

void nap_track_init(u8 channel, gnss_signal_t sid, u32 ref_timing_count,
float carrier_freq, float code_phase)
{
u8 prn = sid.sat - 1;
NAP->TRK_CH[channel].CONTROL = prn << NAP_TRK_CONTROL_SAT_Pos;
assert(sid.code == CODE_GPS_L1CA);

nap_trk_regs_t *t = &NAP->TRK_CH[channel];
struct nap_ch_state *s = &nap_ch_state[channel];
memset(s, 0, sizeof(*s));

u8 prn = sid.sat - GPS_FIRST_PRN;
t->CONTROL = prn << NAP_TRK_CONTROL_SAT_Pos;

/* We always start at zero code phase */
NAP->TRK_CH[channel].CODE_INIT_INT = 0;
NAP->TRK_CH[channel].CODE_INIT_FRAC = 0;
NAP->TRK_CH[channel].CODE_INIT_G1 = 0x3ff;
NAP->TRK_CH[channel].CODE_INIT_G2 = 0x3ff;
t->CODE_INIT_INT = 0;
t->CODE_INIT_FRAC = 0;
t->CODE_INIT_G1 = 0x3ff;
t->CODE_INIT_G2 = 0x3ff;

t->SPACING = (SPACING_HALF_CHIP << NAP_TRK_SPACING_OUTER_Pos) |
(SPACING_HALF_CHIP << NAP_TRK_SPACING_INNER_Pos);

NAP->TRK_CH[channel].SPACING = (SPACING_HALF_CHIP << 16) | SPACING_HALF_CHIP;
double cp_rate = (1.0 + carrier_freq / GPS_L1_HZ) * GPS_CA_CHIPPING_RATE;
nap_track_update(channel, carrier_freq, cp_rate, 0, 0);

u32 cp_rate = (1.0 + carrier_freq/GPS_L1_HZ) * GPS_CA_CHIPPING_RATE *
NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ;
/* First integration is one sample short */
t->LENGTH += 1;

NAP->TRK_CH[channel].CARR_PINC = -carrier_freq *
NAP_TRACK_CARRIER_FREQ_UNITS_PER_HZ;
NAP->TRK_CH[channel].CODE_PINC = cp_rate;
nap_ch_state[channel].len = NAP->TRK_CH[channel].LENGTH = calc_length_samples(1, 0, cp_rate)+1;
nap_ch_state[channel].code_phase = (NAP->TRK_CH[channel].LENGTH) * cp_rate;
NAP->TRK_CONTROL |= (1 << channel); /* Set to start on the timing strobe */
s->code_phase += t->LENGTH * t->CODE_PINC;

/* Set to start on the timing strobe */
NAP->TRK_CONTROL |= (1 << channel);

COMPILER_BARRIER();

Expand Down Expand Up @@ -124,51 +136,52 @@ void nap_track_update(u8 channel, double carrier_freq,
{
(void)corr_spacing; /* This is always written as 0 now... */

u32 cp_rate_fp = code_phase_rate * NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ;
NAP->TRK_CH[channel].CARR_PINC = -carrier_freq *
NAP_TRACK_CARRIER_FREQ_UNITS_PER_HZ;
NAP->TRK_CH[channel].CODE_PINC = cp_rate_fp;
NAP->TRK_CH[channel].LENGTH = calc_length_samples(rollover_count + 1,
nap_ch_state[channel].code_phase,
cp_rate_fp);

volatile u16 cp_int = NAP->TRK_CH[channel].CODE_PHASE_INT;
if ((cp_int != 0x3fe)) /* Sanity check, we should be just before rollover */
asm("nop");
nap_trk_regs_t *t = &NAP->TRK_CH[channel];
struct nap_ch_state *s = &nap_ch_state[channel];

u32 cp_rate_units = code_phase_rate * NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ;
t->CARR_PINC = -carrier_freq * NAP_TRACK_CARRIER_FREQ_UNITS_PER_HZ;
t->CODE_PINC = cp_rate_units;
t->LENGTH = calc_length_samples(rollover_count + 1,
s->code_phase, cp_rate_units);
}

void nap_track_read_results(u8 channel,
u32* count_snapshot, corr_t corrs[],
double *code_phase_early,
double *carrier_phase)
{
nap_trk_regs_t *t = &NAP->TRK_CH[channel];
struct nap_ch_state *s = &nap_ch_state[channel];

u32 ovf = (t->STATUS & NAP_TRK_STATUS_OVF_Msk) >> NAP_TRK_STATUS_OVF_Pos;
if (ovf) {
log_warn("Track correlator overflow 0x%04X on channel %d", ovf, channel);
}

corr_t lc[5];
if (NAP->TRK_CH[channel].STATUS & 0x3F)
log_warn("Track correlator overflow 0x%08X on channel %d",
NAP->TRK_CH[channel].STATUS, channel);
for (u8 i = 0; i < 5; i++) {
lc[i].I = NAP->TRK_CH[channel].CORR[i].I >> 8;
lc[i].Q = NAP->TRK_CH[channel].CORR[i].Q >> 8;
lc[i].I = t->CORR[i].I >> 8;
lc[i].Q = t->CORR[i].Q >> 8;
}
*count_snapshot = NAP->TRK_CH[channel].START_SNAPSHOT;
u64 nap_code_phase = ((u64)NAP->TRK_CH[channel].CODE_PHASE_INT << 32) |
NAP->TRK_CH[channel].CODE_PHASE_FRAC;
s64 nap_carr_phase = ((s64)NAP->TRK_CH[channel].CARR_PHASE_INT << 32) |
NAP->TRK_CH[channel].CARR_PHASE_FRAC;
*code_phase_early = (double)nap_code_phase / NAP_TRACK_CODE_PHASE_UNITS_PER_CHIP;
*carrier_phase = (double)-nap_carr_phase / (1ull << 32);
memcpy(corrs, &lc[1], sizeof(corr_t)*3);
if (nap_ch_state[channel].code_phase != NAP->TRK_CH[channel].CODE_PHASE_FRAC)
asm("nop");
nap_ch_state[channel].code_phase +=
NAP->TRK_CH[channel].LENGTH * NAP->TRK_CH[channel].CODE_PINC;
nap_ch_state[channel].len = NAP->TRK_CH[channel].LENGTH;
if (!(NAP->STATUS & 1))
asm("bkpt");

u64 nap_code_phase = ((u64)t->CODE_PHASE_INT << 32) |
t->CODE_PHASE_FRAC;
s64 nap_carr_phase = ((s64)t->CARR_PHASE_INT << 32) |
t->CARR_PHASE_FRAC;

s->code_phase += t->LENGTH * t->CODE_PINC;

*count_snapshot = t->START_SNAPSHOT;
*code_phase_early = (double)nap_code_phase /
NAP_TRACK_CODE_PHASE_UNITS_PER_CHIP;
*carrier_phase = (double)-nap_carr_phase /
NAP_TRACK_CARRIER_PHASE_UNITS_PER_CYCLE;
}

void nap_track_disable(u8 channel)
{
NAP->TRK_CONTROL &= ~(1 << channel); /* Set to start on the timing strobe */
NAP->TRK_CONTROL &= ~(1 << channel);
}

0 comments on commit 915b73a

Please sign in to comment.