C++ (Cpp) dgnss_iar_num_hyps Examples

Example #1

File: system_monitor.c Project: StoneAerospace/piksi_firmware

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

Example #2

File: system_monitor.c Project: margaret/piksi_firmware

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