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