static msg_t nav_msg_thread(void *arg) { (void)arg; chRegSetThreadName("nav msg"); for (u8 i=0; i<32; i++) { es[i].prn = i; } while (TRUE) { chThdSleepMilliseconds(1000); /* Check if there is a new nav msg subframe to process. * TODO: move this into a function */ /* TODO: This should be trigged by a semaphore from the tracking loop, not * just ran periodically. */ memcpy(es_old, es, sizeof(es)); for (u8 i=0; i<nap_track_n_channels; i++) { if (tracking_channel[i].state == TRACKING_RUNNING && tracking_channel[i].nav_msg.subframe_start_index) { __asm__("CPSID i;"); s8 ret = process_subframe(&tracking_channel[i].nav_msg, &es[tracking_channel[i].prn]); __asm__("CPSIE i;"); if (ret < 0) printf("PRN %02d ret %d\n", tracking_channel[i].prn+1, ret); if (ret == 1 && !es[tracking_channel[i].prn].healthy) { printf("PRN %02d unhealthy\n", tracking_channel[i].prn+1); } else { sbp_send_msg(MSG_EPHEMERIS, sizeof(ephemeris_t), (u8 *)&es[tracking_channel[i].prn]); } if (memcmp(&es[tracking_channel[i].prn], &es_old[tracking_channel[i].prn], sizeof(ephemeris_t))) { printf("New ephemeris for PRN %02d\n", tracking_channel[i].prn+1); /* TODO: This is a janky way to set the time... */ gps_time_t t; t.wn = es[tracking_channel[i].prn].toe.wn; t.tow = tracking_channel[i].TOW_ms / 1000.0; if (gpsdifftime(t, es[tracking_channel[i].prn].toe) > 2*24*3600) t.wn--; else if (gpsdifftime(t, es[tracking_channel[i].prn].toe) < 2*24*3600) t.wn++; /*set_time(TIME_COARSE, t);*/ } } } } return 0; }
static msg_t nav_msg_thread(void *arg) { (void)arg; chRegSetThreadName("nav msg"); memset(es, 0, sizeof(es)); for (u8 i=0; i<32; i++) { es[i].prn = i; } 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); /* Check if there is a new nav msg subframe to process. * TODO: move this into a function */ if (tracking_channel[i].state == TRACKING_RUNNING && tracking_channel[i].nav_msg.subframe_start_index) { /* Save old ephemeris before potentially updating. */ memcpy(&es_old[tracking_channel[i].prn], &es[tracking_channel[i].prn], sizeof(ephemeris_t)); __asm__("CPSID i;"); s8 ret = process_subframe(&tracking_channel[i].nav_msg, &es[tracking_channel[i].prn]); __asm__("CPSIE i;"); if (ret < 0) { log_info("PRN %02d ret %d\n", tracking_channel[i].prn+1, ret); } else if (ret == 1) { /* Decoded a new ephemeris. */ if (memcmp(&es[tracking_channel[i].prn], &es_old[tracking_channel[i].prn], sizeof(ephemeris_t))) { log_info("New ephemeris for PRN %02d\n", tracking_channel[i].prn+1); } if (!es[tracking_channel[i].prn].healthy) { log_info("PRN %02d unhealthy\n", tracking_channel[i].prn+1); } else { sbp_send_msg(SBP_MSG_EPHEMERIS, sizeof(ephemeris_t), (u8 *)&es[tracking_channel[i].prn]); } } } } } return 0; }
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); }
int main(void) { init(); led_toggle(LED_RED); printf("\n\nFirmware info - git: " GIT_VERSION ", built: " __DATE__ " " __TIME__ "\n"); u8 nap_git_hash[20]; nap_conf_rd_git_hash(nap_git_hash); printf("SwiftNAP git: "); for (u8 i=0; i<20; i++) printf("%02x", nap_git_hash[i]); if (nap_conf_rd_git_unclean()) printf(" (unclean)"); printf("\n"); printf("SwiftNAP configured with %d tracking channels\n\n", nap_track_n_channels); cw_setup(); manage_acq_setup(); tick_timer_setup(); timing_setup(); position_setup(); channel_measurement_t meas[nap_track_n_channels]; navigation_measurement_t nav_meas[nap_track_n_channels]; /* TODO: Think about thread safety when updating ephemerides. */ static ephemeris_t es[32]; static ephemeris_t es_old[32]; while(1) { for (u32 i = 0; i < 3000; i++) __asm__("nop"); sbp_process_messages(); manage_track(); manage_acq(); /* Check if there is a new nav msg subframe to process. * TODO: move this into a function */ memcpy(es_old, es, sizeof(es)); for (u8 i=0; i<nap_track_n_channels; i++) if (tracking_channel[i].state == TRACKING_RUNNING && tracking_channel[i].nav_msg.subframe_start_index) { s8 ret = process_subframe(&tracking_channel[i].nav_msg, &es[tracking_channel[i].prn]); if (ret < 0) printf("PRN %02d ret %d\n", tracking_channel[i].prn+1, ret); if (ret == 1 && !es[tracking_channel[i].prn].healthy) printf("PRN %02d unhealthy\n", tracking_channel[i].prn+1); if (memcmp(&es[tracking_channel[i].prn], &es_old[tracking_channel[i].prn], sizeof(ephemeris_t))) { printf("New ephemeris for PRN %02d\n", tracking_channel[i].prn+1); /* TODO: This is a janky way to set the time... */ gps_time_t t; t.wn = es[tracking_channel[i].prn].toe.wn; t.tow = tracking_channel[i].TOW_ms / 1000.0; if (gpsdifftime(t, es[tracking_channel[i].prn].toe) > 2*24*3600) t.wn--; else if (gpsdifftime(t, es[tracking_channel[i].prn].toe) < 2*24*3600) t.wn++; set_time(TIME_COARSE, t); } } DO_EVERY_TICKS(TICK_FREQ/10, u8 n_ready = 0; for (u8 i=0; i<nap_track_n_channels; i++) { if (es[tracking_channel[i].prn].valid == 1 && \ es[tracking_channel[i].prn].healthy == 1 && \ tracking_channel[i].state == TRACKING_RUNNING && \ tracking_channel[i].TOW_ms > 0) { __asm__("CPSID i;"); tracking_update_measurement(i, &meas[n_ready]); __asm__("CPSIE i;"); n_ready++; } } if (n_ready >= 4) { /* Got enough sats/ephemerides, do a solution. */ /* TODO: Instead of passing 32 LSBs of nap_timing_count do something * more intelligent with the solution time. */ calc_navigation_measurement(n_ready, meas, nav_meas, (double)((u32)nap_timing_count())/SAMPLE_FREQ, es); dops_t dops; if (calc_PVT(n_ready, nav_meas, &position_solution, &dops) == 0) { position_updated(); sbp_send_msg(MSG_SOLUTION, sizeof(gnss_solution), (u8 *) &position_solution); nmea_gpgga(&position_solution, &dops); DO_EVERY(10, sbp_send_msg(MSG_DOPS, sizeof(dops_t), (u8 *) &dops); nmea_gpgsv(n_ready, nav_meas, &position_solution); ); }