/** Setup file IO * Registers relevant SBP callbacks for file IO operations. */ void sbp_fileio_setup(void) { static sbp_msg_callbacks_node_t read_node; sbp_register_cbk( SBP_MSG_FILEIO_READ_REQUEST, &read_cb, &read_node ); static sbp_msg_callbacks_node_t read_dir_node; sbp_register_cbk( SBP_MSG_FILEIO_READ_DIR_REQUEST, &read_dir_cb, &read_dir_node ); static sbp_msg_callbacks_node_t remove_node; sbp_register_cbk( SBP_MSG_FILEIO_REMOVE, &remove_cb, &remove_node ); static sbp_msg_callbacks_node_t write_node; sbp_register_cbk( SBP_MSG_FILEIO_WRITE_REQUEST, &write_cb, &write_node ); }
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; }
void manage_acq_setup() { for (u8 prn=0; prn<32; prn++) { acq_prn_param[prn].state = ACQ_PRN_UNTRIED; acq_prn_param[prn].score = 0; } int fd = cfs_open("almanac", CFS_READ); if (fd != -1) { cfs_read(fd, almanac, 32*sizeof(almanac_t)); log_info("Loaded almanac from flash\n"); cfs_close(fd); } else { log_info("No almanac file present in flash, create an empty one\n"); cfs_coffee_reserve("almanac", 32*sizeof(almanac_t)); cfs_coffee_configure_log("almanac", 256, sizeof(almanac_t)); for (u8 prn=0; prn<32; prn++) { almanac[prn].valid = 0; } } sbp_register_cbk( MSG_ALMANAC, &almanac_callback, &almanac_callback_node ); chThdCreateStatic( wa_manage_acq_thread, sizeof(wa_manage_acq_thread), MANAGE_ACQ_THREAD_PRIORITY, manage_acq_thread, NULL ); }
void nap_callbacks_setup(void) { static sbp_msg_callbacks_node_t nap_dna_node; sbp_register_cbk(SBP_MSG_NAP_DEVICE_DNA_REQ, &nap_rd_dna_callback, &nap_dna_node); }
/** Must be called from main() or equivalent function before simulator runs */ void simulator_setup(void) { static sbp_msg_callbacks_node_t set_simulation_enabled_node; sbp_register_cbk( MSG_SIMULATION_ENABLED, &set_simulation_enabled_callback, &set_simulation_enabled_node ); sim_state.noisy_solution.time.wn = simulation_week_number; sim_state.noisy_solution.time.tow = 0; simulator_setup_almanacs(); SETTING("simulator", "base_ecef_x", sim_settings.base_ecef[0], TYPE_FLOAT); SETTING("simulator", "base_ecef_y", sim_settings.base_ecef[1], TYPE_FLOAT); SETTING("simulator", "base_ecef_z", sim_settings.base_ecef[2], TYPE_FLOAT); SETTING("simulator", "speed", sim_settings.speed, TYPE_FLOAT); SETTING("simulator", "radius", sim_settings.radius, TYPE_FLOAT); SETTING("simulator", "pos_sigma", sim_settings.pos_sigma, TYPE_FLOAT); SETTING("simulator", "speed_sigma", sim_settings.speed_sigma, TYPE_FLOAT); SETTING("simulator", "cn0_sigma", sim_settings.cn0_sigma, TYPE_FLOAT); SETTING("simulator", "pseudorange_sigma", sim_settings.pseudorange_sigma, TYPE_FLOAT); SETTING("simulator", "phase_sigma", sim_settings.phase_sigma, TYPE_FLOAT); SETTING("simulator", "num_sats", sim_settings.num_sats, TYPE_INT); SETTING("simulator", "mode_mask", sim_settings.mode_mask, TYPE_INT); }
/** Register callback to read Device's Unique ID. */ static void stm_unique_id_callback_register(void) { static sbp_msg_callbacks_node_t stm_unique_id_node; sbp_register_cbk(SBP_MSG_STM_UNIQUE_ID_REQ, &stm_unique_id_callback, &stm_unique_id_node); }
void manage_acq_setup() { for (u8 prn=0; prn<32; prn++) { acq_prn_param[prn].state = ACQ_PRN_ACQUIRING; for (enum acq_hint hint = 0; hint < ACQ_HINT_NUM; hint++) acq_prn_param[prn].score[hint] = 0; acq_prn_param[prn].dopp_hint_low = ACQ_FULL_CF_MIN; acq_prn_param[prn].dopp_hint_high = ACQ_FULL_CF_MAX; } int fd = cfs_open("almanac", CFS_READ); if (fd != -1) { cfs_read(fd, almanac, 32*sizeof(almanac_t)); log_info("Loaded almanac from flash"); cfs_close(fd); } else { log_info("No almanac file present in flash, create an empty one"); cfs_coffee_reserve("almanac", 32*sizeof(almanac_t)); cfs_coffee_configure_log("almanac", 256, sizeof(almanac_t)); for (u8 prn=0; prn<32; prn++) { almanac[prn].valid = 0; } } sbp_register_cbk( SBP_MSG_ALMANAC, &almanac_callback, &almanac_callback_node ); sbp_register_cbk( SBP_MSG_MASK_SATELLITE, &mask_sat_callback, &mask_sat_callback_node ); chThdCreateStatic( wa_manage_acq_thread, sizeof(wa_manage_acq_thread), MANAGE_ACQ_THREAD_PRIORITY, manage_acq_thread, NULL ); }
/** Setup timing functionality. * For now just register a callback so that a coarse time can be sent by the * host. */ void timing_setup(void) { /* TODO: Perhaps setup something to check for nap_timing_count overflows * periodically. */ static sbp_msg_callbacks_node_t set_time_node; sbp_register_cbk(SBP_MSG_SET_TIME, &set_time_callback, &set_time_node); clock_est_init(&clock_state); }
/** Register the reset_callback. */ static void reset_callback_register(void) { static sbp_msg_callbacks_node_t reset_node; sbp_register_cbk( SBP_MSG_RESET, &reset_callback, &reset_node ); }
void manage_acq_setup() { SETTING("acquisition", "sbas enabled", sbas_enabled, TYPE_BOOL); tracking_startup_fifo_init(&tracking_startup_fifo); for (u32 i=0; i<PLATFORM_SIGNAL_COUNT; i++) { acq_status[i].state = ACQ_PRN_ACQUIRING; acq_status[i].masked = false; memset(&acq_status[i].score, 0, sizeof(acq_status[i].score)); acq_status[i].dopp_hint_low = ACQ_FULL_CF_MIN; acq_status[i].dopp_hint_high = ACQ_FULL_CF_MAX; acq_status[i].sid = sid_from_global_index(i); track_mask[i] = false; almanac[i].valid = 0; if (!sbas_enabled && (sid_to_constellation(acq_status[i].sid) == CONSTELLATION_SBAS)) { acq_status[i].masked = true; track_mask[i] = true; } } sbp_register_cbk( SBP_MSG_ALMANAC, &almanac_callback, &almanac_callback_node ); sbp_register_cbk( SBP_MSG_MASK_SATELLITE, &mask_sat_callback, &mask_sat_callback_node ); chThdCreateStatic( wa_manage_acq_thread, sizeof(wa_manage_acq_thread), MANAGE_ACQ_THREAD_PRIORITY, manage_acq_thread, NULL ); }
void settings_setup(void) { TYPE_BOOL = settings_type_register_enum(bool_enum, &bool_settings_type); static sbp_msg_callbacks_node_t settings_msg_node; sbp_register_cbk( SBP_MSG_SETTINGS, &settings_msg_callback, &settings_msg_node ); static sbp_msg_callbacks_node_t settings_save_node; sbp_register_cbk( SBP_MSG_SETTINGS_SAVE, &settings_save_callback, &settings_save_node ); static sbp_msg_callbacks_node_t settings_read_by_index_node; sbp_register_cbk( SBP_MSG_SETTINGS_READ_BY_INDEX, &settings_read_by_index_callback, &settings_read_by_index_node ); }
void ephemeris_setup(void) { memset(es_candidate, 0, sizeof(es_candidate)); memset(es, 0, sizeof(es)); for (u8 i=0; i<32; i++) { es[i].sid.sat = i; } static sbp_msg_callbacks_node_t ephemeris_msg_node; sbp_register_cbk( SBP_MSG_EPHEMERIS, &ephemeris_msg_callback, &ephemeris_msg_node ); chThdCreateStatic(wa_nav_msg_thread, sizeof(wa_nav_msg_thread), NORMALPRIO-1, nav_msg_thread, NULL); }
/** SBP callback for observation messages. * SBP observation sets are potentially split across multiple SBP messages to * keep the payload within the size limit. * * The header contains a count of how many total messages there are in this set * of observations (all referring to the same observation time) and a count of * which message this is in the sequence. * * This function attempts to collect a full set of observations into a single * `obss_t` (`base_obss_rx`). Once a full set is received then update_obss() * is called. */ static void obs_callback(u16 sender_id, u8 len, u8 msg[], void* context) { (void) context; /* Keep track of where in the sequence of messages we were last time around * so we can verify we haven't dropped a message. */ static s16 prev_count = 0; static gps_time_t prev_t = {.tow = 0.0, .wn = 0}; /* As we receive observation messages we assemble them into a working * `obss_t` (`base_obss_rx`) so as not to disturb the global `base_obss` * state that may be in use. */ static obss_t base_obss_rx = {.has_pos = 0}; /* An SBP sender ID of zero means that the messages are relayed observations * from the console, not from the base station. We don't want to use them and * we don't want to create an infinite loop by forwarding them again so just * ignore them. */ if (sender_id == 0) { return; } /* Relay observations using sender_id = 0. */ sbp_send_msg_(SBP_MSG_OBS, len, msg, 0); /* GPS time of observation. */ gps_time_t t; /* Total number of messages in the observation set / sequence. */ u8 total; /* The current message number in the sequence. */ u8 count; /* Decode the message header to get the time and how far through the sequence * we are. */ unpack_obs_header((observation_header_t*)msg, &t, &total, &count); /* Check to see if the observation is aligned with our internal observations, * i.e. is it going to time match one of our local obs. */ u32 obs_freq = soln_freq / obs_output_divisor; double epoch_count = t.tow * obs_freq; double dt = fabs(epoch_count - round(epoch_count)) / obs_freq; if (dt > TIME_MATCH_THRESHOLD) { log_warn("Unaligned observation from base station ignored, " "tow = %.3f, dt = %.3f", t.tow, dt); return; } /* Calculate packet latency. */ if (time_quality >= TIME_COARSE) { gps_time_t now = get_current_time(); float latency_ms = (float) ((now.tow - t.tow) * 1000.0); log_obs_latency(latency_ms); } /* Verify sequence integrity */ if (count == 0) { prev_t = t; prev_count = 0; } else if (prev_t.tow != t.tow || prev_t.wn != t.wn || prev_count + 1 != count) { log_info("Dropped one of the observation packets! Skipping this sequence."); prev_count = -1; return; } else { prev_count = count; } /* Calculate the number of observations in this message by looking at the SBP * `len` field. */ u8 obs_in_msg = (len - sizeof(observation_header_t)) / sizeof(packed_obs_content_t); /* If this is the first packet in the sequence then reset the base_obss_rx * state. */ if (count == 0) { base_obss_rx.n = 0; base_obss_rx.t = t; } /* Pull out the contents of the message. */ packed_obs_content_t *obs = (packed_obs_content_t *)(msg + sizeof(observation_header_t)); for (u8 i=0; i<obs_in_msg; i++) { /* Check the PRN is valid. e.g. simulation mode outputs test observations * with PRNs >200. */ if (obs[i].sid > 31) { /* TODO prn - sid; assume everything below is 0x1F masked! */ continue; } /* Flag this as visible/viable to acquisition/search */ manage_set_obs_hint(obs[i].sid); /* Check if we have an ephemeris for this satellite, we will need this to * fill in satellite position etc. parameters. */ chMtxLock(&es_mutex); if (ephemeris_good(&es[obs[i].sid], t)) { /* Unpack the observation into a navigation_measurement_t. */ unpack_obs_content( &obs[i], &base_obss_rx.nm[base_obss_rx.n].raw_pseudorange, &base_obss_rx.nm[base_obss_rx.n].carrier_phase, &base_obss_rx.nm[base_obss_rx.n].snr, &base_obss_rx.nm[base_obss_rx.n].lock_counter, &base_obss_rx.nm[base_obss_rx.n].prn ); double clock_err; double clock_rate_err; /* Calculate satellite parameters using the ephemeris. */ calc_sat_state(&es[obs[i].sid], t, base_obss_rx.nm[base_obss_rx.n].sat_pos, base_obss_rx.nm[base_obss_rx.n].sat_vel, &clock_err, &clock_rate_err); /* Apply corrections to the raw pseudorange. */ /* TODO Make a function to apply some of these corrections. * They are used in a couple places. */ base_obss_rx.nm[base_obss_rx.n].pseudorange = base_obss_rx.nm[base_obss_rx.n].raw_pseudorange + clock_err * GPS_C; /* Set the time */ base_obss_rx.nm[base_obss_rx.n].tot = t; base_obss_rx.n++; } chMtxUnlock(); } /* If we can, and all the obs have been received, update to using the new * obss. */ if (count == total - 1) { update_obss(&base_obss_rx); } } /** SBP callback for the old style observation messages. * Just logs a deprecation warning. */ static void deprecated_callback(u16 sender_id, u8 len, u8 msg[], void* context) { (void) context; (void) len; (void) msg; (void) sender_id; log_error("Receiving an old deprecated observation message."); } /** Setup the base station observation handling subsystem. */ void base_obs_setup() { /* Initialise all Mutex and Semaphore objects. */ chMtxInit(&base_obs_lock); chBSemInit(&base_obs_received, TRUE); chMtxInit(&base_pos_lock); /* Register callbacks on base station messages. */ static sbp_msg_callbacks_node_t base_pos_node; sbp_register_cbk( SBP_MSG_BASE_POS, &base_pos_callback, &base_pos_node ); static sbp_msg_callbacks_node_t obs_packed_node; sbp_register_cbk( SBP_MSG_OBS, &obs_callback, &obs_packed_node ); static sbp_msg_callbacks_node_t deprecated_node; sbp_register_cbk( SBP_MSG_OBS_DEP_A, &deprecated_callback, &deprecated_node ); }