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