/****************************************************************************
* 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;
}
}
/****************************************************************************
* Name: nmea_receiver
*
* Description:
* NMEA data handler
*
* Input Parameters:
* gps - GPS object
* data - NMEA data (one character)
*
* Returned Values:
* Status
*
****************************************************************************/
int nmea_receiver(struct ubgps_s * const gps, uint8_t const data)
{
bool nmea_send = false;
/* Check if NMEA line buffer is allocated */
if (!gps->nmea.line)
return OK;
if (data == '\r' || data == '\n')
{
if (gps->nmea.current_len)
{
/* Add string NULL termination */
if (gps->nmea.line[(gps->nmea.current_len - 1)])
gps->nmea.line[gps->nmea.current_len++] = '\0';
nmea_send = true;
}
}
else
{
/* Add NMEA data to line buffer */
gps->nmea.line[gps->nmea.current_len++] = data;
/* Check if buffer is full */
if (gps->nmea.current_len == (gps->nmea.line_size - 1))
{
gps->nmea.line[gps->nmea.current_len++] = '\0';
nmea_send = true;
}
}
/* Send NMEA data */
if (nmea_send && gps->nmea.current_len > 1)
{
struct gps_event_nmea_data_s nmea;
/* Construct and publish NMEA event */
nmea.super.id = GPS_EVENT_NMEA_DATA;
nmea.line = gps->nmea.line;
ubgps_publish_event(gps, (struct gps_event_s *)&nmea);
/* Reset NMEA line length */
gps->nmea.current_len = 0;
}
return OK;
}
/****************************************************************************
* Name: ubgps_sm_process
*
* Description:
* Inject event to GPS state machine
*
* Input Parameters:
* gps - GPS object
* event - Pointer to processed event
*
* Returned Values:
* Status
*
****************************************************************************/
int ubgps_sm_process(struct ubgps_s * const gps, struct sm_event_s const * const event)
{
gps_state_t state;
int status;
DEBUGASSERT(gps && event);
/* Inject event to state machine */
state = gps->state.current_state;
status = ubgps_sm(gps, state)->func(gps, event);
/* Check status */
if (status != OK)
{
dbg("%s: event id: %d, status %s\n", ubgps_get_statename(state),
event->id, status == OK ? "OK" : "ERROR");
}
/* Check for state change */
if (gps->state.new_state != state)
{
struct sm_event_s sevent;
struct gps_event_state_change_s cevent;
/* Construct and send exit event to old state */
sevent.id = SM_EVENT_EXIT;
(void)ubgps_sm(gps, state)->func(gps, &sevent);
/* Change to new state */
state = gps->state.new_state;
gps->state.current_state = state;
/* Construct and send entry event to new state */
sevent.id = SM_EVENT_ENTRY;
(void)ubgps_sm(gps, state)->func(gps, &sevent);
/* Publish state change event */
cevent.super.id = GPS_EVENT_STATE_CHANGE;
cevent.state = state;
ubgps_publish_event(gps, (struct gps_event_s *)&cevent);
}
return status;
}
/****************************************************************************
* Name: ubgps_report_target_state
*
* Description:
* Report target state reached status
*
* Input Parameters:
* gps - GPS object
* timeout - Timeout status
*
* Returned Values:
* Status
*
****************************************************************************/
void ubgps_report_target_state(struct ubgps_s * const gps, bool const timeout)
{
struct gps_event_target_state_s event;
DEBUGASSERT(gps);
/* Check if target state transition is in progress */
if (!gps->state.target_state_pending)
return;
/* Setup event fields */
event.current_state = gps->state.current_state;
event.target_state = gps->state.target_state;
if (timeout)
{
event.super.id = GPS_EVENT_TARGET_STATE_TIMEOUT;
}
else if (gps->state.current_state == gps->state.target_state)
{
event.super.id = GPS_EVENT_TARGET_STATE_REACHED;
}
else
{
event.super.id = GPS_EVENT_TARGET_STATE_NOT_REACHED;
}
if (gps->state.target_timer_id >= 0)
{
if (!timeout)
{
/* Stop target state timeout */
__ubgps_remove_timer(gps, gps->state.target_timer_id);
}
/* Clear timeout timer ID */
gps->state.target_timer_id = -1;
}
/* Clear target state transition pending flag */
gps->state.target_state_pending = false;
/* Publish event to listeners */
ubgps_publish_event(gps, (struct gps_event_s *)&event);
}
/****************************************************************************
* 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;
}
}
/****************************************************************************
* Name: ubgps_handle_nav_pvt
*
* Description:
* Handle NAV-PVT UBX message
*
* Input Parameters:
* gps - GPS object
* msg - UBX NAV-PVT message
*
* Returned Values:
* Status
*
****************************************************************************/
int ubgps_handle_nav_pvt(struct ubgps_s * const gps, struct ubx_msg_s const * const msg)
{
struct gps_event_time_s tevent;
struct gps_event_location_s levent;
DEBUGASSERT(gps && msg);
/* Parse UBX NAV-PVT message */
ubgps_parse_nav_pvt(gps, msg);
/* Check if GPS fix state has changed */
if (gps->location.fix_type == GPS_FIX_NOT_AVAILABLE &&
gps->state.current_state == GPS_STATE_FIX_ACQUIRED)
{
/* Change state to searching GPS fix */
ubgps_set_new_state(gps, GPS_STATE_SEARCHING_FIX);
}
else if (gps->location.fix_type > GPS_FIX_NOT_AVAILABLE &&
gps->state.current_state < GPS_STATE_FIX_ACQUIRED)
{
/* Change state to GPS fix acquired */
ubgps_set_new_state(gps, GPS_STATE_FIX_ACQUIRED);
}
/* Publish time event only when date and/or time is known */
if (gps->time.validity.date ||
gps->time.validity.time ||
gps->time.validity.fully_resolved)
{
/* Construct and publish time event */
tevent.super.id = GPS_EVENT_TIME;
tevent.time = &gps->time;
ubgps_publish_event(gps, (struct gps_event_s *)&tevent);
}
/* Publish location event only when location is known */
if (gps->location.fix_type > GPS_FIX_NOT_AVAILABLE)
{
/* Construct and publish location event */
levent.super.id = GPS_EVENT_LOCATION;
levent.time = &gps->time;
levent.location = &gps->location;
#ifdef CONFIG_UBGPS_PSM_MODE
ubgps_filter_location(gps, &levent);
#else
ubgps_publish_event(gps, (struct gps_event_s *)&levent);
#endif
}
return OK;
}
