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