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);
}
}
