/**************************************************************************** * 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_init_process_phase * * Description: * Send UBX messages according to initialization phase * ****************************************************************************/ static int ubgps_init_process_phase(struct ubgps_s * const gps, bool next) { bool waiting_for_response = false; int status = OK; static bool init_retry_done = false; DEBUGASSERT(gps); /* Moving to next initialization phase */ if (next) { /* Reset retry counter */ gps->state.init_count = 0; /* Move to next phase */ gps->state.init_phase++; } if (gps->state.init_count > 0) ubx_reset(gps); if (gps->state.init_count++ == (GPS_INIT_RETRY_COUNT + 1)) { dbg("GPS initialization failed at phase %d, retries %d.\n", gps->state.init_phase, GPS_INIT_RETRY_COUNT); if (init_retry_done) { /* Fail back to power down state */ ubgps_set_new_state(gps, GPS_STATE_POWER_OFF); return ERROR; } else { /* Workaround for rarely happening initialization failures. Try to recover by toggling supply voltage and starting init from the beginning. */ struct termios uart; int ret; dbg_sm("Retry init from the beginning\n"); init_retry_done = true; gps->state.init_phase = 0; /* Toggle supply voltage */ board_gps_power(false); usleep(5000); board_gps_power(true); /* Configure initial baud rate to GPS UART */ ret = ioctl(gps->fd, TCGETS, (unsigned long)&uart); if (ret != OK) { ubgps_set_new_state(gps, GPS_STATE_POWER_OFF); return ERROR; } uart.c_cflag = CS8; uart.c_speed = GPS_INITIAL_BAUD_RATE; ret = ioctl(gps->fd, TCSETS, (unsigned long)&uart); if (ret != OK) { ubgps_set_new_state(gps, GPS_STATE_POWER_OFF); return ERROR; } usleep(GPS_POWERUP_DELAY*1000); } } while (status == OK && !waiting_for_response) { switch (gps->state.init_phase) { case INIT_PHASE_CFG_PRT: { dbg_sm("INIT_PHASE_CFG_PRT\n"); /* Re-configure UART port with given settings */ status = ubgps_send_cfg_prt(gps, GPS_BAUD_RATE); if (status == OK) { size_t count = 0; /* Wait for UART TX buffer to empty */ while (!board_gps_tx_buffer_empty(gps->fd) && count++ < (1 << 16)); /* Reset receiver and don't wait for acknowledgment */ ubx_reset(gps); /* Sleep that baud rate configuration takes effect */ usleep(50000); /* Move to next state */ gps->state.init_phase++; } break; } case INIT_PHASE_CFG_PRT_BAUD: { struct termios uart; dbg_sm("INIT_PHASE_CFG_PRT_BAUD\n"); /* Set UART baud rate */ status = ioctl(gps->fd, TCGETS, (unsigned long)&uart); if(status != OK) break; uart.c_cflag = CS8; uart.c_speed = GPS_BAUD_RATE; status = ioctl(gps->fd, TCSETS, (unsigned long)&uart); if(status != OK) break; /* Re-configure UART port with given settings */ status = ubgps_send_cfg_prt(gps, GPS_BAUD_RATE); if (status == OK) { /* Exit and wait for response */ waiting_for_response = true; } break; } #ifdef CONFIG_SYSTEM_UBGPS_LNA_CONTROL case INIT_PHASE_CFG_ANT_LNA: { dbg_sm("INIT_PHASE_CFG_ANT_LNA\n"); if (gps->state.target_state_pending && gps->state.target_state == GPS_STATE_COLD_START) { /* Move to next state */ gps->state.init_phase++; break; } /* Send CFG-ANT message. Setup LNA pin and reconfigure. */ status = ubgps_send_cfg_ant(gps, 0, (CONFIG_SYSTEM_UBGPS_LNA_PIN << ANT_PIN_LNA_CTRL_SHIFT) | ANT_PIN_RECONFIG); if (status == OK) { /* Exit and wait for response */ waiting_for_response = true; } break; } #endif /* CONFIG_SYSTEM_UBGPS_LNA_CONTROL */ case INIT_PHASE_SBAS_MODE: { dbg_sm("INIT_PHASE_SBAS_MODE\n"); if (gps->state.target_state_pending && gps->state.target_state == GPS_STATE_COLD_START) { /* Move to next state */ gps->state.init_phase++; break; } /* Send CFG-SBAS message. Disable SBAS subsystem when PSM is enabled */ status = ubgps_send_cfg_sbas(gps, SBAS_ENABLED); if (status == OK) { /* Exit and wait for response */ waiting_for_response = true; } break; } case INIT_PHASE_RECEIVER_MODE: { dbg_sm("INIT_PHASE_RECEIVER_MODE\n"); if (gps->state.target_state_pending && gps->state.target_state == GPS_STATE_COLD_START) { /* Move to next state */ gps->state.init_phase++; break; } /* Send CFG-RXM message */ status = ubgps_send_cfg_rxm(gps, RXM_CONTINOUS); if (status == OK) { /* Exit and wait for response */ waiting_for_response = true; } break; } case INIT_PHASE_PM_MODE: { dbg_sm("INIT_PHASE_PM_MODE\n"); if (gps->state.target_state_pending && gps->state.target_state == GPS_STATE_COLD_START) { /* Move to next state */ gps->state.init_phase++; break; } /* Send CFG-PM2 message */ status = ubgps_send_cfg_pm2(gps, PM2_MODE_CYCLIC | PM2_NO_FIX_NO_OFF, gps->state.navigation_rate, 10000); if (status == OK) { /* Exit and wait for response */ waiting_for_response = true; } break; } case INIT_PHASE_AID_SET_TIME: { dbg_sm("INIT_PHASE_AID_SET_TIME\n"); /* Check if need to setup GPS aiding */ if (gps->state.target_state_pending && gps->state.target_state == GPS_STATE_COLD_START) { /* Move to next state */ gps->state.init_phase++; break; } /* Send AID-INI message */ status = ubgps_send_aid_ini(gps); if (status == OK) { /* Move to next state */ gps->state.init_phase++; } break; } case INIT_PHASE_AID_ALPSRV_ENABLE: { dbg_sm("INIT_PHASE_AID_ALPSRV_ENABLE\n"); /* Check if need to setup GPS aiding */ if (gps->state.target_state_pending && gps->state.target_state == GPS_STATE_COLD_START) { /* Move to next state */ gps->state.init_phase++; break; } /* Check if AssistNow Offline is enabled */ if (gps->assist) { /* Enable AID-ALPSRV message */ status = ubgps_send_cfg_msg(gps, UBX_CLASS_AID, UBX_AID_ALPSRV, 1); if (status == OK) { /* Exit and wait for response */ waiting_for_response = true; } } else { /* Move to next state */ gps->state.init_phase++; } break; } case INIT_PHASE_NAV_PVT_RATE: { dbg_sm("INIT_PHASE_NAV_PVT_RATE\n"); /* Check if NAV-PVT setup is needed */ if (gps->state.target_state_pending && gps->state.target_state == GPS_STATE_COLD_START) { /* Move to next state */ gps->state.init_phase++; break; } /* Set NAV-PVT message rate */ status = ubgps_send_cfg_msg(gps, UBX_CLASS_NAV, UBX_NAV_PVT, 1); if (status == OK) { /* Exit and wait for response */ waiting_for_response = true; } break; } case INIT_PHASE_NAVIGATION_RATE: { dbg_sm("INIT_PHASE_NAVIGATION_RATE\n"); /* Check if navigation rate setup is needed */ if (gps->state.target_state_pending && gps->state.target_state == GPS_STATE_COLD_START) { /* Move to next state */ gps->state.init_phase++; break; } /* Set GPS navigation rate */ #ifdef CONFIG_UBGPS_PSM_MODE if (gps->state.navigation_rate >= CONFIG_UBGPS_PSM_MODE_THRESHOLD*1000) { /* Use default navigation rate for SW controlled PSM */ status = ubgps_send_cfg_rate(gps, DEFAULT_NAVIGATION_RATE); } else { status = ubgps_send_cfg_rate(gps, gps->state.navigation_rate); } #else status = ubgps_send_cfg_rate(gps, gps->state.navigation_rate); #endif if (status == OK) { /* Exit and wait for response */ waiting_for_response = true; } break; } case INIT_PHASE_DONE: { dbg_sm("INIT_PHASE_DONE\n"); if (gps->state.target_state == GPS_STATE_INITIALIZATION) { /* Report target state reached */ ubgps_report_target_state(gps, false); /* Move to search for GPS fix */ ubgps_set_new_state(gps, GPS_STATE_SEARCHING_FIX); } else if (gps->state.target_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); } /* Reset init retry flag */ init_retry_done = false; return OK; } } } if (status != OK) { dbg_sm("Status fail for init phase:%u\n", gps->state.init_phase); /* Start initialization retry timer */ gps->state.init_timer_id = __ubgps_set_timer(gps, GPS_RETRY_DELAY, ubgps_timeout, gps); if (gps->state.init_timer_id < 0) { /* Fail back to power down state */ ubgps_set_new_state(gps, GPS_STATE_POWER_OFF); return ERROR; } } 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: 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; } }