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); ); }
int main(void) { /* Initialise SysTick timer that will be used as the ChibiOS kernel tick * timer. */ STBase->RVR = SYSTEM_CLOCK / CH_FREQUENCY - 1; STBase->CVR = 0; STBase->CSR = CLKSOURCE_CORE_BITS | ENABLE_ON_BITS | TICKINT_ENABLED_BITS; /* Kernel initialization, the main() function becomes a thread and the RTOS * is active. */ chSysInit(); /* Piksi hardware initialization. */ init(); settings_setup(); usarts_setup(); check_nap_auth(); static char nap_version_string[64] = {0}; nap_conf_rd_version_string(nap_version_string); log_info("NAP firmware version: %s\n", nap_version_string); /* Check we are running a compatible version of the NAP firmware. */ const char *required_nap_version = "v0.9-46"; if (compare_version(nap_version_string, required_nap_version) < 0) { log_error("NAP firmware version newer than %s required, please update!\n" "(instructions can be found at http://docs.swift-nav.com/)\n", required_nap_version); while (1) { chThdSleepSeconds(60); } } static s32 serial_number; serial_number = nap_conf_rd_serial_number(); max2769_setup(); timing_setup(); position_setup(); manage_acq_setup(); manage_track_setup(); system_monitor_setup(); base_obs_setup(); solution_setup(); simulator_setup(); sbp_fileio_setup(); ext_setup(); if (serial_number < 0) { READ_ONLY_PARAMETER("system_info", "serial_number", "(unknown)", TYPE_STRING); } else { READ_ONLY_PARAMETER("system_info", "serial_number", serial_number, TYPE_INT); } READ_ONLY_PARAMETER("system_info", "firmware_version", GIT_VERSION, TYPE_STRING); READ_ONLY_PARAMETER("system_info", "firmware_built", __DATE__ " " __TIME__, TYPE_STRING); static struct setting hw_rev = { "system_info", "hw_revision", NULL, 0, settings_read_only_notify, NULL, NULL, false }; hw_rev.addr = (char *)nap_conf_rd_hw_rev_string(); hw_rev.len = strlen(hw_rev.addr); settings_register(&hw_rev, TYPE_STRING); READ_ONLY_PARAMETER("system_info", "nap_version", nap_version_string, TYPE_STRING); READ_ONLY_PARAMETER("system_info", "nap_channels", nap_track_n_channels, TYPE_INT); READ_ONLY_PARAMETER("system_info", "nap_fft_index_bits", nap_acq_fft_index_bits, TYPE_INT); chThdCreateStatic(wa_nav_msg_thread, sizeof(wa_nav_msg_thread), NORMALPRIO-1, nav_msg_thread, NULL); /* Send message to inform host we are up and running. */ u32 startup_flags = 0; sbp_send_msg(SBP_MSG_STARTUP, sizeof(startup_flags), (u8 *)&startup_flags); while (1) { chThdSleepSeconds(60); } }
int main(void) { halInit(); /* Kernel initialization, the main() function becomes a thread with * priority NORMALPRIO and the RTOS is active. */ chSysInit(); /* Piksi hardware initialization. */ init(); settings_setup(); signal_init(); check_nap_auth(); static char nap_version_string[64] = {0}; nap_conf_rd_version_string(nap_version_string); log_info("NAP firmware version: %s", nap_version_string); /* Check we are running a compatible version of the NAP firmware. */ const char *required_nap_version = "v0.16"; if (compare_version(nap_version_string, required_nap_version) < 0) { while (1) { log_error("NAP firmware version >= %s required, please update!" "(instructions can be found at http://docs.swift-nav.com/)", required_nap_version); chThdSleepSeconds(2); } } static s32 serial_number; serial_number = nap_conf_rd_serial_number(); frontend_setup(); timing_setup(); ext_event_setup(); position_setup(); track_setup(); track_gps_l1ca_register(); decode_setup(); decode_gps_l1_register(); manage_acq_setup(); manage_track_setup(); system_monitor_setup(); base_obs_setup(); solution_setup(); simulator_setup(); sbp_fileio_setup(); ext_setup(); pps_setup(); READ_ONLY_PARAMETER("system_info", "serial_number", serial_number, TYPE_INT); READ_ONLY_PARAMETER("system_info", "firmware_version", GIT_VERSION, TYPE_STRING); READ_ONLY_PARAMETER("system_info", "firmware_built", __DATE__ " " __TIME__, TYPE_STRING); static struct setting hw_rev = { "system_info", "hw_revision", NULL, 0, settings_read_only_notify, NULL, NULL, false }; hw_rev.addr = (char *)nap_conf_rd_hw_rev_string(); hw_rev.len = strlen(hw_rev.addr); settings_register(&hw_rev, TYPE_STRING); READ_ONLY_PARAMETER("system_info", "nap_version", nap_version_string, TYPE_STRING); READ_ONLY_PARAMETER("system_info", "nap_channels", nap_track_n_channels, TYPE_INT); READ_ONLY_PARAMETER("system_info", "nap_fft_index_bits", nap_acq_fft_index_bits, TYPE_INT); ephemeris_setup(); /* Send message to inform host we are up and running. */ u32 startup_flags = 0; sbp_send_msg(SBP_MSG_STARTUP, sizeof(startup_flags), (u8 *)&startup_flags); while (1) { chThdSleepSeconds(60); } }