static void settings_read_by_index_callback(u16 sender_id, u8 len, u8 msg[], void* context)
{
(void)sender_id; (void) context;
struct setting *s = settings_head;
char buf[256];
u8 buflen = 0;
if (len != 2) {
log_error("Invalid length for settings read by index!");
return;
}
u16 index = (msg[1] << 8) | msg[0];
for (int i = 0; (i < index) && s; i++, s = s->next)
;
if (s == NULL) {
sbp_send_msg(SBP_MSG_SETTINGS_READ_BY_INDEX, 0, NULL);
return;
}
/* build and send reply */
buf[buflen++] = msg[0];
buf[buflen++] = msg[1];
buflen += settings_format_setting(s, buf + buflen, sizeof(buf) - buflen);
sbp_send_msg(SBP_MSG_SETTINGS_READ_BY_INDEX, buflen, (void*)buf);
}
static msg_t system_monitor_thread(void *arg)
{
(void)arg;
chRegSetThreadName("system monitor");
while (TRUE) {
chThdSleepMilliseconds(heartbeat_period_milliseconds);
u32 status_flags = 0;
sbp_send_msg(SBP_HEARTBEAT, sizeof(status_flags), (u8 *)&status_flags);
/* If we are in base station mode then broadcast our known location. */
if (base_station_mode) {
sbp_send_msg(MSG_BASE_POS, sizeof(msg_base_pos_t), (u8 *)&base_llh);
}
msg_iar_state_t iar_state;
if (simulation_enabled_for(SIMULATION_MODE_RTK)) {
iar_state.num_hyps = 1;
} else {
iar_state.num_hyps = dgnss_iar_num_hyps();
}
sbp_send_msg(MSG_IAR_STATE, sizeof(msg_iar_state_t), (u8 *)&iar_state);
send_thread_states();
u32 err = nap_error_rd_blocking();
if (err)
printf("Error: 0x%08X\n", (unsigned int)err);
}
return 0;
}
static void system_monitor_thread(void *arg)
{
(void)arg;
chRegSetThreadName("system monitor");
systime_t time = chVTGetSystemTime();
bool ant_status = 0;
while (TRUE) {
if (ant_status != frontend_ant_status()) {
ant_status = frontend_ant_status();
if (ant_status && frontend_ant_setting() == AUTO) {
log_info("Now using external antenna.");
}
else if (frontend_ant_setting() == AUTO) {
log_info("Now using patch antenna.");
}
}
u32 status_flags = ant_status << 31 | SBP_MAJOR_VERSION << 16 | SBP_MINOR_VERSION << 8;
sbp_send_msg(SBP_MSG_HEARTBEAT, sizeof(status_flags), (u8 *)&status_flags);
/* If we are in base station mode then broadcast our known location. */
if (broadcast_surveyed_position && position_quality == POSITION_FIX) {
double tmp[3];
double base_ecef[3];
double base_distance;
llhdeg2rad(base_llh, tmp);
wgsllh2ecef(tmp, base_ecef);
vector_subtract(3, base_ecef, position_solution.pos_ecef, tmp);
base_distance = vector_norm(3, tmp);
if (base_distance > BASE_STATION_DISTANCE_THRESHOLD) {
log_warn("Invalid surveyed position coordinates\n");
} else {
sbp_send_msg(SBP_MSG_BASE_POS_ECEF, sizeof(msg_base_pos_ecef_t), (u8 *)&base_ecef);
}
}
msg_iar_state_t iar_state;
if (simulation_enabled_for(SIMULATION_MODE_RTK)) {
iar_state.num_hyps = 1;
} else {
iar_state.num_hyps = dgnss_iar_num_hyps();
}
sbp_send_msg(SBP_MSG_IAR_STATE, sizeof(msg_iar_state_t), (u8 *)&iar_state);
DO_EVERY(2,
send_thread_states();
);
sleep_until(&time, MS2ST(heartbeat_period_milliseconds));
}
/** File read callback.
* Responds to a SBP_MSG_FILEIO_READ_REQUEST message.
*
* Reads a certain length (up to 255 bytes) from a given offset. Returns the
* data in a SBP_MSG_FILEIO_READ_RESPONSE message where the message length field
* indicates how many bytes were succesfully read.
*/
static void read_cb(u16 sender_id, u8 len, u8 msg[], void* context)
{
(void)context;
if (sender_id != SBP_SENDER_ID) {
log_error("Invalid sender!\n");
return;
}
if ((len < 9) || (msg[len-1] != '\0')) {
log_error("Invalid fileio read message!\n");
return;
}
u32 offset = ((u32)msg[3] << 24) | ((u32)msg[2] << 16) | (msg[1] << 8) | msg[0];
u8 readlen = MIN(msg[4], SBP_FRAMING_MAX_PAYLOAD_SIZE - len);
u8 buf[256];
memcpy(buf, msg, len);
int f = cfs_open((char*)&msg[5], CFS_READ);
cfs_seek(f, offset, CFS_SEEK_SET);
len += cfs_read(f, buf + len, readlen);
cfs_close(f);
sbp_send_msg(SBP_MSG_FILEIO_READ_RESPONSE, len, buf);
}
/** Directory listing callback.
* Responds to a SBP_MSG_FILEIO_READ_DIR_REQUEST message.
*
* The offset parameter can be used to skip the first n elements of the file
* list.
*
* Returns a SBP_MSG_FILEIO_READ_DIR_RESPONSE message containing the directory
* listings as a NULL delimited list. The listing is chunked over multiple SBP
* packets and the end of the list is identified by an entry containing just
* the character 0xFF.
*/
static void read_dir_cb(u16 sender_id, u8 len, u8 msg[], void* context)
{
(void)context;
if (sender_id != SBP_SENDER_ID) {
log_error("Invalid sender!\n");
return;
}
if ((len < 5) || (msg[len-1] != '\0')) {
log_error("Invalid fileio read dir message!\n");
return;
}
u32 offset = ((u32)msg[3] << 24) | ((u32)msg[2] << 16) | (msg[1] << 8) | msg[0];
struct cfs_dir dir;
struct cfs_dirent dirent;
u8 buf[256];
memcpy(buf, msg, len);
cfs_opendir(&dir, (char*)&msg[4]);
while (offset && (cfs_readdir(&dir, &dirent) == 0))
offset--;
while ((cfs_readdir(&dir, &dirent) == 0) && (len < SBP_FRAMING_MAX_PAYLOAD_SIZE)) {
strncpy((char*)buf + len, dirent.name, SBP_FRAMING_MAX_PAYLOAD_SIZE - len);
len += strlen(dirent.name) + 1;
}
if (len < SBP_FRAMING_MAX_PAYLOAD_SIZE)
buf[len++] = 0xff;
cfs_closedir(&dir);
sbp_send_msg(SBP_MSG_FILEIO_READ_DIR_RESPONSE, len, buf);
}
/** Service an external event interrupt
*
* When an event occurs (i.e. pin edge) that matches the NAP's trigger
* condition, the NAP will latch the time, pin number and trigger direction.
* It will also set an IRQ bit which will lead to an EXTI. The firmware
* EXTI handling routine handle_nap_exti() will call this function, which
* reads out the details and spits them out as an SBP message to our host.
*
*/
void ext_event_service(void)
{
u8 event_pin;
ext_event_trigger_t event_trig;
/* Read the details, and also clear IRQ + set up for next time */
u32 event_nap_time = nap_rw_ext_event(&event_pin, &event_trig, trigger);
/* We have to infer the most sig word (i.e. # of 262-second rollovers) */
union {
u32 half[2];
u64 full;
} tc;
tc.full = nap_timing_count();
if (tc.half[0] < event_nap_time) /* Rollover occurred since event */
tc.half[1]--;
tc.half[0] = event_nap_time;
/* Prepare the MSG_EXT_EVENT */
msg_ext_event_t msg;
msg.flags = (event_trig == TRIG_RISING) ? (1<<0) : (0<<0);
if (time_quality == TIME_FINE)
msg.flags |= (1 << 1);
msg.pin = event_pin;
/* Convert to the SBP convention of rounded ms + signed ns residual */
gps_time_t gpst = rx2gpstime(tc.full);
msg_gps_time_t mgt;
sbp_make_gps_time(&mgt, &gpst, 0);
msg.wn = mgt.wn;
msg.tow = mgt.tow;
msg.ns = mgt.ns;
sbp_send_msg(SBP_MSG_EXT_EVENT, sizeof(msg), (u8 *)&msg);
}
static msg_t nav_msg_thread(void *arg)
{
(void)arg;
chRegSetThreadName("nav msg");
for (u8 i=0; i<32; i++) {
es[i].prn = i;
}
while (TRUE) {
chThdSleepMilliseconds(1000);
/* Check if there is a new nav msg subframe to process.
* TODO: move this into a function */
/* TODO: This should be trigged by a semaphore from the tracking loop, not
* just ran periodically. */
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) {
__asm__("CPSID i;");
s8 ret = process_subframe(&tracking_channel[i].nav_msg,
&es[tracking_channel[i].prn]);
__asm__("CPSIE i;");
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);
} else {
sbp_send_msg(MSG_EPHEMERIS, sizeof(ephemeris_t), (u8 *)&es[tracking_channel[i].prn]);
}
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);*/
}
}
}
}
return 0;
}
static msg_t nav_msg_thread(void *arg)
{
(void)arg;
chRegSetThreadName("nav msg");
memset(es, 0, sizeof(es));
for (u8 i=0; i<32; i++) {
es[i].prn = i;
}
while (TRUE) {
/* TODO: This should be trigged by a semaphore from the tracking loop, not
* just ran periodically. */
for (u8 i=0; i<nap_track_n_channels; i++) {
chThdSleepMilliseconds(100);
/* Check if there is a new nav msg subframe to process.
* TODO: move this into a function */
if (tracking_channel[i].state == TRACKING_RUNNING &&
tracking_channel[i].nav_msg.subframe_start_index) {
/* Save old ephemeris before potentially updating. */
memcpy(&es_old[tracking_channel[i].prn],
&es[tracking_channel[i].prn],
sizeof(ephemeris_t));
__asm__("CPSID i;");
s8 ret = process_subframe(&tracking_channel[i].nav_msg,
&es[tracking_channel[i].prn]);
__asm__("CPSIE i;");
if (ret < 0) {
log_info("PRN %02d ret %d\n", tracking_channel[i].prn+1, ret);
} else if (ret == 1) {
/* Decoded a new ephemeris. */
if (memcmp(&es[tracking_channel[i].prn],
&es_old[tracking_channel[i].prn],
sizeof(ephemeris_t))) {
log_info("New ephemeris for PRN %02d\n", tracking_channel[i].prn+1);
}
if (!es[tracking_channel[i].prn].healthy) {
log_info("PRN %02d unhealthy\n", tracking_channel[i].prn+1);
} else {
sbp_send_msg(SBP_MSG_EPHEMERIS,
sizeof(ephemeris_t),
(u8 *)&es[tracking_channel[i].prn]);
}
}
}
}
}
return 0;
}
/** Send tracking state SBP message.
* Send information on each tracking channel to host.
*/
void tracking_send_state()
{
tracking_channel_state_t states[nap_track_n_channels];
if (simulation_enabled_for(SIMULATION_MODE_TRACKING)) {
u8 num_sats = simulation_current_num_sats();
for (u8 i=0; i < num_sats; i++) {
states[i] = simulation_current_tracking_state(i);
}
if (num_sats < nap_track_n_channels) {
for (u8 i = num_sats; i < nap_track_n_channels; i++) {
states[i].state = 0;
states[i].sid.code = 0;
states[i].sid.sat = 0;
states[i].cn0 = -1;
}
}
} else {
for (u8 i=0; i<nap_track_n_channels; i++) {
tracker_channel_t *tracker_channel = tracker_channel_get(i);
const tracker_common_data_t *common_data = &tracker_channel->common_data;
bool running;
gnss_signal_t sid;
float cn0;
tracker_channel_lock(tracker_channel);
{
running =
(tracker_channel_state_get(tracker_channel) == STATE_ENABLED);
sid = tracker_channel->info.sid;
cn0 = common_data->cn0;
}
tracker_channel_unlock(tracker_channel);
if (!running) {
states[i].state = 0;
states[i].sid.code = 0;
states[i].sid.sat = 0;
states[i].cn0 = -1;
} else {
states[i].state = 1;
states[i].sid = sid_to_sbp(sid);
states[i].cn0 = cn0;
}
}
}
sbp_send_msg(SBP_MSG_TRACKING_STATE, sizeof(states), (u8*)states);
}
/** Send results of an acquisition to the host.
*
* \param sid SID of the acquisition
* \param snr Signal to noise ratio of best point from acquisition.
* \param cp Code phase of best point.
* \param cf Carrier frequency of best point.
*/
static void acq_result_send(gnss_signal_t sid, float snr, float cp, float cf)
{
msg_acq_result_t acq_result_msg;
acq_result_msg.sid = sid_to_sbp(sid);
acq_result_msg.snr = snr;
acq_result_msg.cp = cp;
acq_result_msg.cf = cf;
sbp_send_msg(SBP_MSG_ACQ_RESULT,
sizeof(msg_acq_result_t),
(u8 *)&acq_result_msg);
}
void send_thread_states()
{
thread_t *tp = chRegFirstThread();
while (tp) {
msg_thread_state_t tp_state;
u16 cpu = 1000.0f * tp->p_ctime / (float)g_ctime;
tp_state.cpu = cpu;
tp_state.stack_free = check_stack_free(tp);
strncpy(tp_state.name, chRegGetThreadNameX(tp), sizeof(tp_state.name));
sbp_send_msg(SBP_MSG_THREAD_STATE, sizeof(tp_state), (u8 *)&tp_state);
tp->p_ctime = 0; /* Reset thread CPU cycle count */
tp = chRegNextThread(tp);
}
g_ctime = 0;
}
void send_thread_states()
{
Thread *tp = chRegFirstThread();
while (tp) {
msg_thread_state_t tp_state;
u16 cpu = 1000.0f * tp->p_ctime / (float)g_ctime;
tp_state.cpu = cpu;
tp_state.stack_free = check_stack_free(tp);
strncpy(tp_state.name, chRegGetThreadName(tp), sizeof(tp_state.name));
sbp_send_msg(MSG_THREAD_STATE, sizeof(tp_state), (u8 *)&tp_state);
/* This works because chThdGetTicks is actually a define that pulls out a
* value from a struct, hopefully if that fact changes then this statement
* will no longer compile. */
tp->p_ctime = 0;
tp = chRegNextThread(tp);
}
g_ctime = 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();
);
}
/* Write to file callback.
* Responds to a SBP_MSG_FILEIO_WRITE_REQUEST message.
*
* Writes a certain length (up to 255 bytes) at a given offset. Returns a copy
* of the original SBP_MSG_FILEIO_WRITE_RESPONSE message to check integrity of
* the write.
*/
static void write_cb(u16 sender_id, u8 len, u8 msg[], void* context)
{
(void)context;
if (sender_id != SBP_SENDER_ID) {
log_error("Invalid sender!\n");
return;
}
if (len < 6) {
log_error("Invalid fileio write message!\n");
return;
}
u32 offset = ((u32)msg[3] << 24) | ((u32)msg[2] << 16) | (msg[1] << 8) | msg[0];
u8 headerlen = 4 + strlen((char*)&msg[4]) + 1;
int f = cfs_open((char*)&msg[4], CFS_WRITE);
cfs_seek(f, offset, CFS_SEEK_SET);
cfs_write(f, msg + headerlen, len - headerlen);
cfs_close(f);
sbp_send_msg(SBP_MSG_FILEIO_WRITE_RESPONSE, headerlen, msg);
}
void solution_send_sbp(gnss_solution *soln, dops_t *dops, bool clock_jump)
{
if (soln) {
/* Send GPS_TIME message first. */
msg_gps_time_t gps_time;
sbp_make_gps_time(&gps_time, &soln->time, 0);
sbp_send_msg(SBP_MSG_GPS_TIME, sizeof(gps_time), (u8 *) &gps_time);
if (chVTTimeElapsedSinceX(last_dgnss) > DGNSS_TIMEOUT(soln_freq)) {
/* Position in LLH. */
msg_pos_llh_t pos_llh;
sbp_make_pos_llh(&pos_llh, soln, 0);
sbp_send_msg(SBP_MSG_POS_LLH, sizeof(pos_llh), (u8 *) &pos_llh);
/* Position in ECEF. */
msg_pos_ecef_t pos_ecef;
sbp_make_pos_ecef(&pos_ecef, soln, 0);
sbp_send_msg(SBP_MSG_POS_ECEF, sizeof(pos_ecef), (u8 *) &pos_ecef);
}
/* Velocity in NED. */
/* Do not send if there has been a clock jump. Velocity may be unreliable.*/
if (!clock_jump) {
msg_vel_ned_t vel_ned;
sbp_make_vel_ned(&vel_ned, soln, 0);
sbp_send_msg(SBP_MSG_VEL_NED, sizeof(vel_ned), (u8 *) &vel_ned);
/* Velocity in ECEF. */
msg_vel_ecef_t vel_ecef;
sbp_make_vel_ecef(&vel_ecef, soln, 0);
sbp_send_msg(SBP_MSG_VEL_ECEF, sizeof(vel_ecef), (u8 *) &vel_ecef);
}
}
if (dops) {
DO_EVERY(10,
msg_dops_t sbp_dops;
sbp_make_dops(&sbp_dops, dops, &(soln->time));
sbp_send_msg(SBP_MSG_DOPS, sizeof(msg_dops_t), (u8 *) &sbp_dops);
);
}
void solution_send_sbp(gnss_solution *soln, dops_t *dops)
{
if (soln) {
/* Send GPS_TIME message first. */
sbp_gps_time_t gps_time;
sbp_make_gps_time(&gps_time, &soln->time, 0);
sbp_send_msg(SBP_GPS_TIME, sizeof(gps_time), (u8 *) &gps_time);
/* Position in LLH. */
sbp_pos_llh_t pos_llh;
sbp_make_pos_llh(&pos_llh, soln, 0);
sbp_send_msg(SBP_POS_LLH, sizeof(pos_llh), (u8 *) &pos_llh);
/* Position in ECEF. */
sbp_pos_ecef_t pos_ecef;
sbp_make_pos_ecef(&pos_ecef, soln, 0);
sbp_send_msg(SBP_POS_ECEF, sizeof(pos_ecef), (u8 *) &pos_ecef);
/* Velocity in NED. */
sbp_vel_ned_t vel_ned;
sbp_make_vel_ned(&vel_ned, soln, 0);
sbp_send_msg(SBP_VEL_NED, sizeof(vel_ned), (u8 *) &vel_ned);
/* Velocity in ECEF. */
sbp_vel_ecef_t vel_ecef;
sbp_make_vel_ecef(&vel_ecef, soln, 0);
sbp_send_msg(SBP_VEL_ECEF, sizeof(vel_ecef), (u8 *) &vel_ecef);
}
if (dops) {
DO_EVERY(10,
sbp_dops_t sbp_dops;
sbp_make_dops(&sbp_dops, dops);
sbp_send_msg(SBP_DOPS, sizeof(sbp_dops_t), (u8 *) &sbp_dops);
);
}
static msg_t nav_msg_thread(void *arg)
{
(void)arg;
chRegSetThreadName("nav msg");
while (TRUE) {
/* TODO: This should be trigged by a semaphore from the tracking loop, not
* just ran periodically. */
for (u8 i=0; i<nap_track_n_channels; i++) {
chThdSleepMilliseconds(100);
tracking_channel_t *ch = &tracking_channel[i];
ephemeris_t e = {.sid = ch->sid};
/* Check if there is a new nav msg subframe to process.
* TODO: move this into a function */
if ((ch->state != TRACKING_RUNNING) ||
(ch->nav_msg.subframe_start_index == 0))
continue;
/* Decode ephemeris to temporary struct */
__asm__("CPSID i;");
s8 ret = process_subframe(&ch->nav_msg, &e);
__asm__("CPSIE i;");
if (ret <= 0)
continue;
/* Decoded a new ephemeris. */
ephemeris_new(&e);
if (!es[ch->sid.sat].healthy) {
log_info("PRN %02d unhealthy", ch->sid.sat+1);
} else {
msg_ephemeris_t msg;
pack_ephemeris(&es[ch->sid.sat], &msg);
sbp_send_msg(SBP_MSG_EPHEMERIS, sizeof(msg_ephemeris_t), (u8 *)&msg);
}
}
}
return 0;
}
static void ephemeris_msg_callback(u16 sender_id, u8 len, u8 msg[], void* context)
{
(void)sender_id; (void)context;
if (len != sizeof(msg_ephemeris_t)) {
log_warn("Received bad ephemeris from peer");
return;
}
ephemeris_t e;
unpack_ephemeris((msg_ephemeris_t *)msg, &e);
if (e.sid.sat >= MAX_SATS) {
log_warn("Ignoring ephemeris for invalid sat");
return;
}
ephemeris_new(&e);
}
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);
}
}
static void settings_msg_callback(u16 sender_id, u8 len, u8 msg[], void* context)
{
(void)sender_id; (void) context;
static struct setting *s = NULL;
const char *section = NULL, *setting = NULL, *value = NULL;
char buf[256];
u8 buflen;
if (len == 0) {
/* Empty message is for parameter enumeration */
if (s && s->next)
s = s->next;
else
s = settings_head;
} else {
if (msg[len-1] != '\0')
goto error;
/* Extract parameters from message:
* 2 or 3 null terminated strings: section, setting and (optional) value
* If value is present the message is an assignment.
*/
section = (const char *)msg;
for (int i = 0, tok = 0; i < len; i++) {
if (msg[i] == '\0') {
tok++;
switch (tok) {
case 1:
setting = (const char *)&msg[i+1];
break;
case 2:
if (i + 1 < len)
value = (const char *)&msg[i+1];
break;
case 3:
if (i == len-1)
break;
default:
goto error;
}
}
}
s = settings_lookup(section, setting);
}
if (s == NULL)
goto error;
if (value != NULL) {
/* This is an assignment, call notify function */
if (!s->notify(s, value))
goto error;
s->dirty = true;
}
buflen = settings_format_setting(s, buf, sizeof(buf));
sbp_send_msg(SBP_MSG_SETTINGS, buflen, (void*)buf);
return;
error:
log_error("Error in settings read message\n");
}
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);
}
}
/** Callback to read STM32F4's hardcoded unique ID.
* Sends STM32F4 unique ID (12 bytes) back to host.
*/
static void stm_unique_id_callback(u16 sender_id, u8 len, u8 msg[], void* context)
{
(void)sender_id; (void)len; (void)msg; (void) context;
sbp_send_msg(SBP_MSG_STM_UNIQUE_ID_RESP, 12, (u8*)STM_UNIQUE_ID_ADDR);
}
static void nap_rd_dna_callback(u16 sender_id, u8 len, u8 msg[], void* context)
{
(void)sender_id; (void)len; (void)msg; (void) context;
sbp_send_msg(SBP_MSG_NAP_DEVICE_DNA_RESP, NAP_DNA_LENGTH, nap_dna);
}
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);
);
}
/** Sends an MSG_SIMULATION_ENABLED message with
*payload 1 if the simulation is enabled, payload 0 otherwise.
*/
void sbp_send_simulation_enabled(void)
{
sbp_send_msg(MSG_SIMULATION_ENABLED,
sizeof(u8),
&sim_enabled);
}
