/****************************************************************************
* 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;
}
/****************************************************************************
* 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_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;
}
/****************************************************************************
* 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;
}
/****************************************************************************
* 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: 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;
}
