Ejemplo n.º 1
0
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);
}
Ejemplo n.º 2
0
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;
}
Ejemplo n.º 3
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));
  }
Ejemplo n.º 4
0
/** 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);
}
Ejemplo n.º 5
0
/** 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);
}
Ejemplo n.º 6
0
/** 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);
}
Ejemplo n.º 7
0
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;
}
Ejemplo n.º 8
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;
}
Ejemplo n.º 9
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);
}
Ejemplo n.º 10
0
/** 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);
}
Ejemplo n.º 11
0
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;
}
Ejemplo n.º 12
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;
}
Ejemplo n.º 13
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();
    );
  }
Ejemplo n.º 14
0
/* 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);
}
Ejemplo n.º 15
0
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);
    );
  }
Ejemplo n.º 16
0
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);
    );
  }
Ejemplo n.º 17
0
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);
}
Ejemplo n.º 18
0
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);
  }
}
Ejemplo n.º 19
0
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");
}
Ejemplo n.º 20
0
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);
  }
}
Ejemplo n.º 21
0
/** 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);
}
Ejemplo n.º 22
0
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);
}
Ejemplo n.º 23
0
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);
          );
        }
Ejemplo n.º 24
0
/** 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);
}