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");
}
}
static msg_t nav_msg_thread(void *arg)
{
(void)arg;
chRegSetThreadName("nav msg");
while (TRUE) {
/* TODO: This should be trigged by a semaphore from the tracking loop, not
* just ran periodically. */
for (u8 i=0; i<nap_track_n_channels; i++) {
chThdSleepMilliseconds(100);
tracking_channel_t *ch = &tracking_channel[i];
ephemeris_t e = {.sid = ch->sid};
/* Check if there is a new nav msg subframe to process.
* TODO: move this into a function */
if ((ch->state != TRACKING_RUNNING) ||
(ch->nav_msg.subframe_start_index == 0))
continue;
/* Decode ephemeris to temporary struct */
__asm__("CPSID i;");
s8 ret = process_subframe(&ch->nav_msg, &e);
__asm__("CPSIE i;");
if (ret <= 0)
continue;
/* Decoded a new ephemeris. */
ephemeris_new(&e);
if (!es[ch->sid.sat].healthy) {
log_info("PRN %02d unhealthy", ch->sid.sat+1);
} else {
msg_ephemeris_t msg;
pack_ephemeris(&es[ch->sid.sat], &msg);
sbp_send_msg(SBP_MSG_EPHEMERIS, sizeof(msg_ephemeris_t), (u8 *)&msg);
}
}
}
return 0;
}
static void ephemeris_msg_callback(u16 sender_id, u8 len, u8 msg[], void* context)
{
(void)sender_id; (void)context;
if (len != sizeof(msg_ephemeris_t)) {
log_warn("Received bad ephemeris from peer");
return;
}
ephemeris_t e;
unpack_ephemeris((msg_ephemeris_t *)msg, &e);
if (e.sid.sat >= MAX_SATS) {
log_warn("Ignoring ephemeris for invalid sat");
return;
}
ephemeris_new(&e);
}
