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