Example #1
0
/****************************************************************************
 * Name: ubgps_psm_timer_cb
 *
 * Description:
 *   timeout handler for PSM callback
 *
 * Input Parameters:
 *
 * Returned Values:
 *   Status
 *
 ****************************************************************************/
int ubgps_psm_timer_cb(const int timer_id, const struct timespec *date,
                       void * const priv)
{
  struct ubgps_s * const gps = (struct ubgps_s *)priv;

  /* Off-time expired, acquire fix */

  if (timer_id == gps->state.psm_timer_id)
    {
      struct sm_event_psm_event_s psm;

      psm.super.id = SM_EVENT_PSM_STATE;
      psm.enable = false;
      ubgps_sm_process(gps, (struct sm_event_s *)&psm);

      ts_core_timer_stop(gps->state.psm_timer_id);
      gps->state.psm_timer_id = -1;
    }
  else
    {
      dbg("invalid timer id\n");
    }

  return OK;
}
Example #2
0
/****************************************************************************
 * Name: ubgps_filter_location
 *
 * Description:
 *   Filter location events in power save mode. Location events are published
 *   until horizontal accuracy is below defined threshold or location filter
 *   timeout occurs.
 *
 * Input Parameters:
 *   gps         - GPS object
 *   levent      - Location event
 *
 * Returned Values:
 *
 ****************************************************************************/
static void ubgps_filter_location(struct ubgps_s * const gps,
  struct gps_event_location_s const * const levent)
{
  DEBUGASSERT(gps && levent);

  if (gps->state.navigation_rate < CONFIG_UBGPS_PSM_MODE_THRESHOLD*1000)
    {
      /* Navigation rate less than PSM threshold. */

      ubgps_publish_event(gps, (struct gps_event_s *)levent);
      return;
    }

  if (CONFIG_UBGPS_PSM_ACCURACY_FILTER_DURATION == 0 ||
      (levent->location->horizontal_accuracy / 1000 <
      CONFIG_UBGPS_PSM_ACCURACY_FILTER_THRESHOLD))
    {
      struct sm_event_psm_event_s psm;

      /* Filter disabled or accuracy below threshold */

      ubgps_publish_event(gps, (struct gps_event_s *)levent);

      if (gps->state.location_timer_id >= 0)
        {
          __ubgps_remove_timer(gps, gps->state.location_timer_id);

          gps->state.location_timer_id = -1;
        }

      gps->filt_location.horizontal_accuracy = 0;

      /* Construct power save mode event */

      psm.super.id = SM_EVENT_PSM_STATE;
      psm.enable = true;
      ubgps_sm_process(gps, (struct sm_event_s *)&psm);

      return;
    }

  if (gps->filt_location.horizontal_accuracy == 0)
    {
      /* The first location event that is not below threshold,
         start timeout timer to collect location events. */

      gps->state.location_timer_id = __ubgps_set_timer(gps,
        CONFIG_UBGPS_PSM_ACCURACY_FILTER_DURATION*1000, ubgps_timeout, gps);
    }

  if (gps->filt_location.horizontal_accuracy == 0 ||
      (levent->location->horizontal_accuracy <
      gps->filt_location.horizontal_accuracy))
    {
      /* More accurate location than previous one, update location data. */

      memcpy(&gps->filt_location, levent->location, sizeof(*levent->location));
      return;
    }
}
Example #3
0
/****************************************************************************
 * Name: ubgps_timeout
 *
 * Description:
 *   GPS timeout handling
 *
 * Input Parameters:
 *   timer_id    - Timer ID
 *   priv        - Private data
 *
 * Returned Values:
 *   Status
 *
 ****************************************************************************/
int ubgps_timeout(const int timer_id, void * const priv)
{
  struct ubgps_s * const gps = (struct ubgps_s *)priv;
  struct sm_event_timeout_s timeout;

  DEBUGASSERT(gps);

  /* Construct timeout event */

  timeout.super.id = SM_EVENT_TIMEOUT;
  timeout.timer_id = timer_id;

  /* Process event */

  ubgps_sm_process(gps, (struct sm_event_s *)&timeout);

  return OK;
}
Example #4
0
/****************************************************************************
 * Name: ubgps_handle_aid_alp
 *
 * Description:
 *   Handle AID-ALP UBX message
 *
 * Input Parameters:
 *   gps         - GPS object
 *   msg         - UBX AID-ALPSRV message
 *
 * Returned Values:
 *   Status
 *
 ****************************************************************************/
int ubgps_handle_aid_alp(struct ubgps_s * const gps, struct ubx_msg_s const * const msg)
{
  struct sm_event_aid_s event;
  long int age;

  DEBUGASSERT(gps && msg);

  age = UBX_GET_I4(msg, 8);

  dbg_int("pred_start:%lus, pred_dur:%lus, age:%ldh (%lds), pred_wk:%u, sat:%u\n",
      UBX_GET_U4(msg, 0), UBX_GET_U4(msg, 4), age/3600, age,
      UBX_GET_U2(msg, 14), UBX_GET_U1(msg, 20));

  event.super.id = SM_EVENT_AID_STATUS;
  event.age = age;
  ubgps_sm_process(gps, (struct sm_event_s *)&event);

  return OK;
}
Example #5
0
/****************************************************************************
 * Name: ubgps_sm_global
 *
 * Description:
 *   Global handler for gps_sm_searching_fix and gps_sm_fix_acquired states
 *
 ****************************************************************************/
static int ubgps_sm_global(struct ubgps_s * const gps, struct sm_event_s const * const event)
{
  DEBUGASSERT(gps && event);

  /* Check event */

  switch (event->id)
    {
      case SM_EVENT_ENTRY:
        {
          dbg_sm("SM_EVENT_ENTRY\n");
          if (gps->state.current_state == gps->state.target_state)
            {
              /* Report target reached */

              ubgps_report_target_state(gps, false);
            }

          if (gps->assist)
            {
              dbg_sm("start polling aiding status\n");

              (void)ubgps_send_aid_alp_poll(gps);
            }

          return OK;
        }

      case SM_EVENT_TARGET_STATE:
        {
          struct sm_event_target_state_s const * const target =
              (struct sm_event_target_state_s *)event;

          dbg_sm("SM_EVENT_TARGET_STATE\n");

          if (target->target_state != gps->state.current_state)
            {
              /* Setup target state transition timeout */

              ubgps_setup_timeout(gps, target->timeout);

              /* Set GPS target state */

              gps->state.target_state = target->target_state;

              /* Check current and target state */

              if (gps->state.target_state == GPS_STATE_FIX_ACQUIRED &&
                  gps->state.current_state == GPS_STATE_COLD_START)
                {
                  /* Move to GPS initialization for GPS aiding parameters */

                  ubgps_set_new_state(gps, GPS_STATE_INITIALIZATION);
                }
              else if (gps->state.target_state == GPS_STATE_FIX_ACQUIRED &&
                  gps->state.current_state < GPS_STATE_FIX_ACQUIRED)
                {
                  /* Move to search for GPS fix */

                  ubgps_set_new_state(gps, GPS_STATE_SEARCHING_FIX);
                }
              else
                {
                  /* Move to requested target state */

                  ubgps_set_new_state(gps, gps->state.target_state);
                }
            }
          else
            {
              /* Make current state the target state */

              gps->state.target_state = gps->state.current_state;

              /* Report target reached */

              ubgps_report_target_state(gps, false);
            }

          return OK;
        }

      case SM_EVENT_TIMEOUT:
        {
          struct sm_event_timeout_s const * const timeout =
              (struct sm_event_timeout_s *)event;
          dbg_sm("SM_EVENT_TIMEOUT\n");

          if (timeout->timer_id == gps->state.target_timer_id)
            {
              /* Report target state timeout */

              ubgps_report_target_state(gps, true);
            }
          else if (timeout->timer_id == gps->state.aid_timer_id)
            {
              /* poll whether ALP data has been expired */

              return ubgps_send_aid_alp_poll(gps);
            }
#ifdef CONFIG_UBGPS_PSM_MODE
          else if (timeout->timer_id == gps->state.location_timer_id)
            {
              struct gps_event_location_s levent;

              /* Publish the most accurate location event received so far. */

              dbg_sm("location filter timeout, accuracy: %um\n",
                  gps->filt_location.horizontal_accuracy / 1000);

              levent.super.id = GPS_EVENT_LOCATION;
              levent.time = &gps->time;
              levent.location = &gps->filt_location;
              ubgps_publish_event(gps, (struct gps_event_s *)&levent);

              /* Reset filtered location event. */

              gps->filt_location.horizontal_accuracy = 0;

              /* Construct power save mode event */

              if (gps->state.navigation_rate >= CONFIG_UBGPS_PSM_MODE_THRESHOLD)
                {
                  struct sm_event_psm_event_s psm;

                  psm.super.id = SM_EVENT_PSM_STATE;
                  psm.enable = true;
                  ubgps_sm_process(gps, (struct sm_event_s *)&psm);
                }
            }
#endif /* CONFIG_UBGPS_PSM_MODE */

          return OK;
        }

      case SM_EVENT_UBX_MESSAGE:
        {
          struct sm_event_ubx_msg_s const * const ubx_msg =
              (struct sm_event_ubx_msg_s *)event;
          struct ubx_msg_s const * const msg = ubx_msg->msg;

          if (msg->class_id == UBX_CLASS_NAV && msg->msg_id == UBX_NAV_PVT)
            {
              return ubgps_handle_nav_pvt(gps, msg);
            }
          else if (msg->class_id == UBX_CLASS_AID && msg->msg_id == UBX_AID_ALPSRV)
            {
              return ubgps_handle_aid_alpsrv(gps, msg);
            }
          else if (msg->class_id == UBX_CLASS_AID && msg->msg_id == UBX_AID_ALP)
            {
              return ubgps_handle_aid_alp(gps, msg);
            }

          dbg_sm("Unhandled UBX message, class:0x%02x, msg:0x%02x, len:%d.\n",
              ubx_msg->msg->class_id, ubx_msg->msg->msg_id, ubx_msg->msg->length);

          return OK;
        }

      case SM_EVENT_AID_STATUS:
        {
#ifdef CONFIG_UBGPS_ASSIST_UPDATER
          struct sm_event_aid_s const * const aid = (struct sm_event_aid_s *)event;
          struct timespec ts = {};
          uint32_t timeout = 0;
          dbg_sm("SM_EVENT_AID_STATUS\n");

          /* Check whether aiding has been enabled */

          if (!gps->assist)
            {
              return OK;
            }

          if (!gps->assist->alp_file)
            {
              /* Aiding file does not exist, start updater */

              ubgps_aid_updater_start(gps->assist);
              timeout = AID_STATUS_POLL_DELAY;
            }
          else if (aid->age < 0)
            {
              /* Status of aiding data not yet resolved */

              timeout = AID_STATUS_POLL_DELAY;
            }
          else if (aid->age < AID_VALIDITY_TIME_1D )
            {
              /* Aiding still valid, setup recheck timeout */

              timeout = AID_RECHECK_DELAY*1000;
            }
          else
            {
              /* Aiding data needs to be updated */

              clock_gettime(CLOCK_MONOTONIC, &ts);

              /* But first add nanoseconds to entropy pool. */

              add_time_randomness(ts.tv_nsec);

              if (gps->assist->update_time == 0 ||
                 (ts.tv_sec - gps->assist->update_time) > AID_RECHECK_DELAY)
                {
                  /* The first update attempt or retry after timeout */

                  dbg_sm("start aiding update, previous update: %ds\n",
                    ts.tv_sec - gps->assist->update_time);

                  gps->assist->update_time = ts.tv_sec;
                  ubgps_aid_updater_start(gps->assist);
                  timeout = AID_STATUS_POLL_DELAY;
                }
              else
                {
                  /* Set timeout for the next update attempt */

                  timeout = AID_RECHECK_DELAY*1000;
                }
            }

            if (gps->state.aid_timer_id >= 0)
              {
                __ubgps_remove_timer(gps, gps->state.aid_timer_id);
              }

            gps->state.aid_timer_id = __ubgps_set_timer(gps,
                                                       timeout,
                                                       ubgps_timeout,
                                                       gps);
#endif /* CONFIG_UBGPS_ASSIST_UPDATER */
          return OK;
        }

#ifdef CONFIG_UBGPS_PSM_MODE
      case SM_EVENT_PSM_STATE:
        {
          struct sm_event_psm_event_s const * const psm_event =
              (struct sm_event_psm_event_s *)event;
          dbg_sm("SM_EVENT_PSM_STATE -> %u\n", psm_event->enable);

          if (psm_event->enable)
            {
              /* Start SW controlled power save mode (PSM). */

              if (gps->state.navigation_rate >= CONFIG_UBGPS_PSM_MODE_THRESHOLD)
                {
                  struct timespec ts = {};

                  clock_gettime(CLOCK_MONOTONIC, &ts);
                  ts.tv_sec += gps->state.navigation_rate/1000;
                  gps->state.psm_timer_id = ts_core_timer_setup_date(&ts,
                      ubgps_psm_timer_cb, gps);

                  /* Update assist params */

                  gps->assist->use_time = true;
                  gps->assist->use_loc = true;
                  gps->assist->latitude = gps->location.latitude;
                  gps->assist->longitude = gps->location.longitude;
                  gps->assist->altitude = gps->location.height;
                  gps->assist->accuracy = gps->location.horizontal_accuracy;

                  dbg_sm("gps->state.psm_timer_id:%d, set POWER_OFF\n",
                      gps->state.psm_timer_id);
                  ubgps_set_new_state(gps, GPS_STATE_POWER_OFF);
                }
            }

          return OK;
        }
#endif /* CONFIG_UBGPS_PSM_MODE */

      default:
        return OK;
    }
}
Example #6
0
/****************************************************************************
 * Name: ubx_callback
 *
 * Description:
 *   Listen to UBX callbacks
 *
 * Input Parameters:
 *   receiver    - UBX object
 *   priv        - Private data
 *   msg         - UBX message
 *   timeout     - Timeout status
 *
 * Returned Values:
 *   Status
 *
 ****************************************************************************/
int ubx_callback(struct ubx_receiver_s const * const receiver,
                        void * priv,
                        struct ubx_msg_s const * const msg,
                        bool const timeout)
{
  struct ubgps_s * const gps = (struct ubgps_s *)priv;

  DEBUGASSERT(receiver && gps && msg);

  /* Check for timeout or ACK/NAK event */

  if (timeout ||
      (msg->class_id == UBX_CLASS_ACK && (msg->msg_id == UBX_ACK_ACK || msg->msg_id == UBX_ACK_NAK)))
    {
      struct sm_event_ubx_status_s ubx_status;
      uint8_t class_id;
      uint8_t msg_id;

      if (timeout)
        {
          /* Get message ID's from received message */

          class_id = msg->class_id;
          msg_id = msg->msg_id;
        }
      else
        {
          /* Check that UBX ACK-ACK/NAK message has correct length */

          if (msg->length != UBX_ACK_ACK_LEN)
            return ERROR;

          /* Get message ID's from received message */

          class_id = UBX_GET_U1(msg, 0);
          msg_id = UBX_GET_U1(msg, 1);
        }

      /* Construct UBX status event */

      ubx_status.super.id = SM_EVENT_UBX_STATUS;
      ubx_status.class_id = class_id;
      ubx_status.msg_id = msg_id;

      if (timeout)
        {
          ubx_status.status = UBX_STATUS_TIMEOUT;
        }
      else if (msg->msg_id == UBX_ACK_ACK)
        {
          ubx_status.status = UBX_STATUS_ACK;
        }
      else
        {
          ubx_status.status = UBX_STATUS_NAK;
        }

      /* Process event */

      ubgps_sm_process(gps, (struct sm_event_s *)&ubx_status);
    }
  else
    {
      struct sm_event_ubx_msg_s ubx_msg;

      /* Construct UBX message event */

      ubx_msg.super.id = SM_EVENT_UBX_MESSAGE;
      ubx_msg.msg = msg;

      /* Process event */

      ubgps_sm_process(gps, (struct sm_event_s *)&ubx_msg);
    }

  return OK;
}