static void decoder_gps_l1ca_process(const decoder_channel_info_t *channel_info,
decoder_data_t *decoder_data)
{
gps_l1ca_decoder_data_t *data = decoder_data;
/* Process incoming nav bits */
s8 soft_bit;
while (tracking_channel_nav_bit_get(channel_info->tracking_channel,
&soft_bit)) {
/* Update TOW */
bool bit_val = soft_bit >= 0;
s32 TOW_ms = nav_msg_update(&data->nav_msg, bit_val);
s8 bit_polarity = data->nav_msg.bit_polarity;
if ((TOW_ms >= 0) && (bit_polarity != BIT_POLARITY_UNKNOWN)) {
if (!tracking_channel_time_sync(channel_info->tracking_channel, TOW_ms,
bit_polarity)) {
log_warn_sid(channel_info->sid, "TOW set failed");
}
}
}
/* Check if there is a new nav msg subframe to process. */
if (!subframe_ready(&data->nav_msg))
return;
/* Decode ephemeris to temporary struct */
ephemeris_t e = {.sid = channel_info->sid};
s8 ret = process_subframe(&data->nav_msg, &e);;
if (ret <= 0)
return;
/* Decoded a new ephemeris. */
ephemeris_new(&e);
ephemeris_t *eph = ephemeris_get(channel_info->sid);
if (!eph->valid) {
log_info_sid(channel_info->sid, "ephemeris is invalid");
}
}
/** Update tracking channels after the end of an integration period.
* Update update_count, sample_count, TOW, run loop filters and update
* SwiftNAP tracking channel frequencies.
* \param channel Tracking channel to update.
*/
void tracking_channel_update(u8 channel)
{
tracking_channel_t* chan = &tracking_channel[channel];
switch(chan->state)
{
case TRACKING_RUNNING:
{
chan->update_count++;
chan->sample_count += chan->corr_sample_count;
/* TODO: check TOW_ms = 0 case is correct, 0 is a valid TOW. */
if (chan->TOW_ms > 0) {
chan->TOW_ms++;
if (chan->TOW_ms == 7*24*60*60*1000)
chan->TOW_ms = 0;
}
chan->code_phase_early = (u64)chan->code_phase_early + (u64)chan->corr_sample_count*chan->code_phase_rate_fp_prev;
chan->carrier_phase += chan->carrier_freq_fp_prev * chan->corr_sample_count;
/* TODO: Fix this in the FPGA - first integration is one sample short. */
if (chan->update_count == 1)
chan->carrier_phase -= chan->carrier_freq_fp_prev;
#if 0
u64 cp;
s32 cf;
nap_track_phase_rd_blocking(channel, &cf, &cp);
/*if ((cp&0xFFFFFFFF) != chan->code_phase_early) {*/
DO_EVERY_TICKS(TICK_FREQ,
struct { s32 xtend : 24; } sign;
sign.xtend = chan->carrier_phase & 0xFFFFFF;
s32 x = sign.xtend;
/*printf("%d %u CPR: 0x%08X, count: %d, NAP: 0x%011llX, STM: 0x%08X\n", chan->prn+1, (unsigned int)chan->update_count, (unsigned int)chan->code_phase_rate_fp_prev, (unsigned int)chan->corr_sample_count, (unsigned long long)cp, (unsigned int)chan->code_phase_early);*/
printf("CF: %f 0x%08X, NAP: %d (%f), STM: %f (%d %f) - %d\n", chan->carrier_freq, (int)(chan->carrier_freq*NAP_TRACK_CARRIER_FREQ_UNITS_PER_HZ), (int)cf, (double)cf / (double)(1<<24), (double)chan->carrier_phase / (double)(1<<24), (int)x, (double)x / (double)(1<<24), (int)(cf - x));
);
/*}*/
#endif
/* Correlations should already be in chan->cs thanks to
* tracking_channel_get_corrs.
*/
corr_t* cs = chan->cs;
/* Update I and Q magnitude filters for SNR calculation.
* filter = (1 - 2^-FILTER_COEFF)*filter + correlation_magnitude
* If filters are uninitialised (=0) then initialise them with the
* first set of correlations.
*/
if (chan->I_filter == 0 && chan->Q_filter == 0) {
chan->I_filter = abs(cs[1].I) << I_FILTER_COEFF;
chan->Q_filter = abs(cs[1].Q) << Q_FILTER_COEFF;
} else {
chan->I_filter -= chan->I_filter >> I_FILTER_COEFF;
chan->I_filter += abs(cs[1].I);
chan->Q_filter -= chan->Q_filter >> Q_FILTER_COEFF;
chan->Q_filter += abs(cs[1].Q);
}
/* Run the loop filters. */
/* TODO: Make this more elegant. */
correlation_t cs2[3];
for (u32 i = 0; i < 3; i++) {
cs2[i].I = cs[2-i].I;
cs2[i].Q = cs[2-i].Q;
}
comp_tl_update(&(chan->tl_state), cs2);
chan->carrier_freq = chan->tl_state.carr_freq;
chan->code_phase_rate = chan->tl_state.code_freq + 1.023e6;
/* TODO: check TOW_ms = 0 case is correct, 0 is a valid TOW. */
s32 TOW_ms = nav_msg_update(&chan->nav_msg, cs[1].I);
if (TOW_ms > 0 && chan->TOW_ms != TOW_ms) {
printf("PRN %d TOW mismatch: %u, %u\n",(int)chan->prn + 1, (unsigned int)chan->TOW_ms, (unsigned int)TOW_ms);
chan->TOW_ms = TOW_ms;
}
chan->code_phase_rate_fp_prev = chan->code_phase_rate_fp;
chan->code_phase_rate_fp = chan->code_phase_rate*NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ;
chan->carrier_freq_fp_prev = chan->carrier_freq_fp;
chan->carrier_freq_fp = chan->carrier_freq*NAP_TRACK_CARRIER_FREQ_UNITS_PER_HZ;
nap_track_update_wr_blocking(channel, \
chan->carrier_freq_fp, \
chan->code_phase_rate_fp);
break;
}
case TRACKING_DISABLED:
default:
/* TODO: WTF? */
tracking_channel_disable(channel);
break;
}