int main(void) { led_setup(); led_on(LED_GREEN); led_on(LED_RED); /* Setup and hold the FPGA PROGRAM_B line low so that the FPGA does not * contest the flash SPI bus */ nap_conf_b_setup(); nap_conf_b_clear(); spi_setup(); sbp_setup(0); m25_setup(); stm_flash_callbacks_setup(); for (u32 i = 0; i < 600000; i++) __asm__("nop"); printf("\n\nFirmware info - git: " GIT_VERSION ", built: " __DATE__ " " __TIME__ "\n"); printf("--- M25 FLASH TEST ---\n"); while (1) { DO_EVERY(300, led_toggle(LED_GREEN); led_toggle(LED_RED); ); sbp_process_messages(); }
int main(void) { init(1); printf("\n\nFirmware info - git: " GIT_VERSION ", built: " __DATE__ " " __TIME__ "\n"); printf("--- SWIFT BINARY PROTOCOL RX STRESS TEST ---\n"); static sbp_msg_callbacks_node_t callback_node; sbp_register_cbk(0x22, &callback, &callback_node); for (u8 i=0; i<30; i++) { guard_below[i] = 0; guard_above[i] = 0; } while(1) { /* Check the guards for buffer over/underrun. */ for (u8 i=0; i<30; i++) { if (guard_below[i] != 0) screaming_death("Detected buffer underrun in guard area\n"); if (guard_above[i] != 0) screaming_death("Detected buffer overrun in guard area\n"); } sbp_process_messages(); //for (u32 i = 0; i < 1000; i++) // __asm__("nop"); } while (1); return 0; }
static msg_t sbp_thread(void *arg) { (void)arg; chRegSetThreadName("SBP"); uart_state_msg.latency.avg = -1; uart_state_msg.latency.lmin = 0; uart_state_msg.latency.lmax = 0; uart_state_msg.latency.current = -1; while (TRUE) { chThdSleepMilliseconds(10); sbp_process_messages(); DO_EVERY(100, uart_state_msg.uart_a.tx_throughput = usart_tx_throughput(&uarta_state.tx); uart_state_msg.uart_a.rx_throughput = usart_rx_throughput(&uarta_state.rx); uart_state_msg.uart_a.io_error_count = uarta_state.rx.errors + uarta_state.tx.errors; uart_state_msg.uart_b.tx_throughput = usart_tx_throughput(&uartb_state.tx); uart_state_msg.uart_b.rx_throughput = usart_rx_throughput(&uartb_state.rx); uart_state_msg.uart_b.io_error_count = uartb_state.rx.errors + uartb_state.tx.errors; uart_state_msg.uart_ftdi.tx_throughput = usart_tx_throughput(&ftdi_state.tx); uart_state_msg.uart_ftdi.rx_throughput = usart_rx_throughput(&ftdi_state.rx); uart_state_msg.uart_ftdi.io_error_count = ftdi_state.rx.errors + ftdi_state.tx.errors; if (latency_count > 0) { uart_state_msg.latency.avg = (s32) (latency_accum_ms / latency_count); } sbp_send_msg(SBP_MSG_UART_STATE, sizeof(msg_uart_state_t), (u8*)&uart_state_msg); uart_state_msg.uart_a.tx_buffer_level = 0; uart_state_msg.uart_a.rx_buffer_level = 0; uart_state_msg.uart_b.tx_buffer_level = 0; uart_state_msg.uart_b.rx_buffer_level = 0; uart_state_msg.uart_ftdi.tx_buffer_level = 0; uart_state_msg.uart_ftdi.rx_buffer_level = 0; log_obs_latency_tick(); ); }
int main(void) { /* Don't check FPGA authentication hash status, purpose of this test is to * read the FPGA DNA and then program the authentication hash into the flash. */ init(0); led_on(LED_GREEN); led_on(LED_RED); printf("\n\nFirmware info - git: " GIT_VERSION ", built: " __DATE__ " " __TIME__ "\n"); printf("--- DNA TEST ---\n"); while (1) { for (u32 i = 0; i < 10000; i++) __asm__("nop"); DO_EVERY(50, led_toggle(LED_GREEN); led_toggle(LED_RED); ); sbp_process_messages(); }
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); ); }