/****************************************************************************
* Name: ubgps_setup_timeout
*
* Description:
* Setup target state transition timeout
*
* Input Parameters:
* gps - GPS object
* timeout - Timeout in seconds
*
* Returned Values:
* Status
*
****************************************************************************/
int ubgps_setup_timeout(struct ubgps_s * const gps, uint32_t const timeout)
{
DEBUGASSERT(gps);
/* Check for running timeout timer */
if (gps->state.target_timer_id >= 0)
{
__ubgps_remove_timer(gps, gps->state.target_timer_id);
gps->state.target_timer_id = -1;
}
/* Check for timeout */
if (timeout > 0)
{
gps->state.target_timer_id = __ubgps_set_timer(gps,
timeout * 1000,
ubgps_timeout,
gps);
if (gps->state.target_timer_id < 0)
return ERROR;
}
return OK;
}
/****************************************************************************
* 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: 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_sm_poweroff
*
* Description:
* State machine for GPS power off
*
****************************************************************************/
static int ubgps_sm_poweroff(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.powered)
{
/* Power down GPS chip */
board_gps_power(false);
/* Mark GPS power down */
gps->state.powered = false;
/* Reset UBX receiver */
ubx_reset(gps);
}
/* Report target reached status */
ubgps_report_target_state(gps, false);
/* Reset PSM accuracy filter */
gps->filt_location.horizontal_accuracy = 0;
/* Make sure that timers are not running */
if (gps->state.location_timer_id >= 0)
{
__ubgps_remove_timer(gps, gps->state.location_timer_id);
gps->state.location_timer_id = -1;
}
#ifdef CONFIG_UBGPS_ASSIST_UPDATER
/* Stop aiding updater and timers */
if (gps->state.aid_timer_id >= 0)
{
__ubgps_remove_timer(gps, gps->state.aid_timer_id);
gps->state.aid_timer_id = -1;
}
if (gps->assist)
{
if (ubgps_aid_updater_stop(gps->assist))
{
dbg("Aiding updater stop failed\n");
}
}
#endif
/* Free GPS assistance data if PSM is not active */
if (gps->assist && gps->state.psm_timer_id < 0)
{
if (gps->assist->alp_file)
free(gps->assist->alp_file);
free(gps->assist);
gps->assist = NULL;
}
return OK;
}
case SM_EVENT_TARGET_STATE:
{
struct sm_event_target_state_s * const target =
(struct sm_event_target_state_s *)event;
dbg_sm("SM_EVENT_TARGET_STATE -> %u\n", target->target_state);
/* Stop PSM timer */
if (gps->state.psm_timer_id > 0)
{
ts_core_timer_stop(gps->state.psm_timer_id);
gps->state.psm_timer_id = -1;
}
if (target->target_state != GPS_STATE_POWER_OFF)
{
/* Setup target state transition timeout */
ubgps_setup_timeout(gps, target->timeout);
/* Set GPS target state */
gps->state.target_state = target->target_state;
/* Set next GPS state */
ubgps_set_new_state(gps, GPS_STATE_INITIALIZATION);
}
else
{
/* Make current state the target state */
/* Free GPS assistance data */
if (gps->assist)
{
if (gps->assist->alp_file)
free(gps->assist->alp_file);
free(gps->assist);
gps->assist = NULL;
}
gps->state.target_state = gps->state.current_state;
/* Report target reached */
ubgps_report_target_state(gps, false);
}
return OK;
}
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)
{
gps->state.target_state = GPS_STATE_FIX_ACQUIRED;
ubgps_set_new_state(gps, GPS_STATE_INITIALIZATION);
}
return OK;
}
case SM_EVENT_EXIT:
{
dbg_sm("SM_EVENT_EXIT\n");
if (!gps->state.powered)
{
struct termios uart;
int ret;
/* Configure initial baud rate to GPS UART */
ret = ioctl(gps->fd, TCGETS, (unsigned long)&uart);
DEBUGASSERT(ret == OK);
uart.c_cflag = CS8;
uart.c_speed = GPS_INITIAL_BAUD_RATE;
ret = ioctl(gps->fd, TCSETS, (unsigned long)&uart);
DEBUGASSERT(ret == OK);
/* Power up GPS chip */
board_gps_power(true);
/* Mark GPS power up */
gps->state.powered = true;
}
return OK;
}
default:
break;
}
return OK;
}
/****************************************************************************
* Name: ubgps_sm_initialization
*
* Description:
* State machine for GPS initialization
*
****************************************************************************/
static int ubgps_sm_initialization(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");
/* Start initialization timer */
gps->state.init_timer_id = __ubgps_set_timer(gps,
GPS_POWERUP_DELAY,
ubgps_timeout,
gps);
if (gps->state.init_timer_id < 0)
{
dbg("Error while setting up initialization timer.\n");
/* Fail back to power down state */
ubgps_set_new_state(gps, GPS_STATE_POWER_OFF);
return ERROR;
}
/* Initialize GPS location data */
memset(&gps->location, 0, sizeof(gps->location));
/* Reset initialization phase & retry counter */
gps->state.init_phase = 0;
gps->state.init_count = 0;
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.init_timer_id)
{
/* Clear initialization timer ID */
gps->state.init_timer_id = -1;
/* Continue initialization process */
return ubgps_init_process_phase(gps, false);
}
return OK;
}
case SM_EVENT_UBX_STATUS:
{
struct sm_event_ubx_status_s const * const ubx =
(struct sm_event_ubx_status_s *)event;
dbg_sm("SM_EVENT_UBX_STATUS - class:%02X, msg:%02X\n",
ubx->class_id, ubx->msg_id);
if (ubx->class_id == UBX_CLASS_CFG &&
(ubx->msg_id == UBX_CFG_PRT || ubx->msg_id == UBX_CFG_MSG ||
ubx->msg_id == UBX_CFG_RATE || ubx->msg_id == UBX_CFG_SBAS ||
ubx->msg_id == UBX_CFG_PM2 || ubx->msg_id == UBX_CFG_RXM ||
ubx->msg_id == UBX_CFG_ANT))
{
if (ubx->status != UBX_STATUS_ACK)
{
/* Retry initialization phase */
dbg_sm("SM_EVENT_UBX_STATUS - NACK\n");
return ubgps_init_process_phase(gps, false);
}
/* Move to next phase */
return ubgps_init_process_phase(gps, true);
}
dbg_sm("Unhandled SM_EVENT_UBX_STATUS. Class 0x%02x, message 0x%02x\n",
ubx->class_id, ubx->msg_id);
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;
dbg_sm("SM_EVENT_UBX_MESSAGE\n");
if (msg->class_id == UBX_CLASS_NAV && msg->msg_id == UBX_NAV_PVT)
{
dbg_sm("Unexpected NAV message in initialization phase???\n");
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);
}
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_INITIALIZATION)
{
/* Setup target state transition timeout */
ubgps_setup_timeout(gps, target->timeout);
/* Set GPS target state */
gps->state.target_state = target->target_state;
if (target->target_state == GPS_STATE_POWER_OFF)
{
/* Set next GPS state */
ubgps_set_new_state(gps, GPS_STATE_POWER_OFF);
}
}
else if (gps->state.init_phase == INIT_PHASE_DONE)
{
/* 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_EXIT:
{
dbg_sm("SM_EVENT_EXIT\n");
/* Stop pending initialization timer */
if (gps->state.init_timer_id >= 0)
{
__ubgps_remove_timer(gps, gps->state.init_timer_id);
gps->state.init_timer_id = -1;
}
return OK;
}
default:
return OK;
}
}