Commit cbf03d32 authored by Jussi Kivilinna's avatar Jussi Kivilinna
Browse files

ubgps: improvements for GPS location hint handling



- add new API for feeding location hint to GPS
- adjust accuracy of location aiding based oldness of location hint
- ignore too inaccurate location hints to avoid messing GPS chip
- actually mark location information as enabled
Signed-off-by: default avatarJussi Kivilinna <jussi.kivilinna@haltian.com>
parent c7cdf522
......@@ -499,6 +499,27 @@ int ubgps_set_aiding_params(bool use_time,
int32_t altitude,
uint32_t accuracy);
/****************************************************************************
* Name: ubgps_give_location_hint
*
* Description:
* Give location hint for A-GPS
*
* Input Parameters:
* latitude - Latitude
* longitude - Longitude
* altitude - Altitude in meters
* accuracy - Horizontal accuracy in meters
*
* Returned Value:
* Status
*
****************************************************************************/
int ubgps_give_location_hint(double const latitude,
double const longitude,
int32_t const altitude,
uint32_t const accuracy);
/****************************************************************************
* Name: ubgps_setup_poll
*
......
......@@ -83,6 +83,10 @@
static struct ubgps_s g_gps;
/* A-GPS hint data structure */
static struct gps_assist_hint_s g_gps_hint;
/* GPS state machine names */
static const char * gps_sm_name[__GPS_STATE_MAX] =
......@@ -134,6 +138,7 @@ struct ubgps_s *ubgps_initialize(void)
/* Clear data */
memset(gps, 0, sizeof(struct ubgps_s));
gps->hint = &g_gps_hint;
/* Open GPS serial device */
......@@ -599,6 +604,8 @@ int ubgps_set_aiding_params(bool const use_time,
dbg_ubgps("\n");
(void)use_time;
/* Free previously set assistance data */
if (gps->assist)
......@@ -614,7 +621,6 @@ int ubgps_set_aiding_params(bool const use_time,
if (!gps->assist)
return ERROR;
gps->assist->use_time = use_time;
gps->assist->alp_file = NULL;
gps->assist->alp_file_id = 0;
......@@ -645,11 +651,88 @@ int ubgps_set_aiding_params(bool const use_time,
}
#endif
gps->assist->use_loc = use_loc;
gps->assist->latitude = (uint32_t)(latitude * 10000000.0f);
gps->assist->longitude = (uint32_t)(longitude * 10000000.0f);
gps->assist->altitude = altitude * 10;
gps->assist->accuracy = accuracy * 10;
if (use_loc)
{
if (!g_gps_hint.have_location)
{
/* Use only if GPS module does not internally already have
* location. */
(void)ubgps_give_location_hint(latitude, longitude, altitude,
accuracy);
}
}
return OK;
}
/****************************************************************************
* Name: ubgps_give_location_hint
*
* Description:
* Give location hint for A-GPS
*
* Input Parameters:
* latitude - Latitude
* longitude - Longitude
* altitude - Altitude in meters
* accuracy - Horizontal accuracy in meters
*
* Returned Value:
* Status
*
****************************************************************************/
int ubgps_give_location_hint(double const latitude,
double const longitude,
int32_t const altitude,
uint32_t const accuracy)
{
struct timespec currtime = {};
if (accuracy > HINT_LOCATION_MINIMUM_NEW_ACCURACY)
{
errno = EINVAL;
return ERROR;
}
(void)clock_gettime(CLOCK_MONOTONIC, &currtime);
if (g_gps_hint.have_location)
{
uint64_t current_accuracy;
uint32_t secs_since = currtime.tv_sec - g_gps_hint.location_time.tv_sec;
/* Adjust location accuracy by time*speed. */
current_accuracy = g_gps_hint.accuracy;
current_accuracy += ((uint64_t)HINT_LOCATION_ACCURACY_DEGRADE_SPEED_MPS *
secs_since);
/* Use old location if its accuracy is better than new. Accuracy of
* old location data will degrade over time and eventually be overridden
* with new location. */
if (current_accuracy < accuracy)
{
errno = EALREADY;
return ERROR;
}
}
g_gps_hint.location_time = currtime;
g_gps_hint.have_location = true;
g_gps_hint.latitude = latitude * 1e7;
g_gps_hint.longitude = longitude * 1e7;
g_gps_hint.altitude = altitude;
g_gps_hint.accuracy = accuracy;
dbg_ubgps("New location hint:\n");
dbg_ubgps(" lat : %d\n", g_gps_hint.latitude);
dbg_ubgps(" long : %d\n", g_gps_hint.longitude);
dbg_ubgps(" alt : %d meters\n", altitude);
dbg_ubgps(" accuracy: %u meters\n", g_gps_hint.accuracy);
return OK;
}
......@@ -1099,7 +1099,6 @@ int ubgps_send_aid_alp_poll(struct ubgps_s * const gps)
int ubgps_send_aid_ini(struct ubgps_s * const gps)
{
struct ubx_msg_s * msg;
time_t currtime;
struct tm t;
uint32_t latitude = 0;
uint32_t longitude = 0;
......@@ -1117,13 +1116,10 @@ int ubgps_send_aid_ini(struct ubgps_s * const gps)
if (ubx_busy(&gps->state.ubx_receiver))
return ERROR;
/* Check if assistance data is available */
if (!gps->assist)
return OK;
if (gps->assist->use_time)
if (board_rtc_time_is_set(NULL))
{
time_t currtime;
dbg_int("Using system time for GPS.\n");
/* Construct time and date for GPS */
......@@ -1141,24 +1137,52 @@ int ubgps_send_aid_ini(struct ubgps_s * const gps)
flags |= (1 << 1) | (1 << 10);
}
if (gps->assist->alp_file)
if (gps->assist && gps->assist->alp_file)
dbg_int("Using AssistNow Offline data from '%s', file ID %d.\n",
gps->assist->alp_file, gps->assist->alp_file_id);
if (gps->assist->use_loc)
if (gps->hint && gps->hint->have_location)
{
latitude = gps->assist->latitude;
longitude = gps->assist->longitude;
altitude = gps->assist->altitude;
accuracy = gps->assist->accuracy;
struct timespec currtime = {};
uint64_t current_accuracy;
(void)clock_gettime(CLOCK_MONOTONIC, &currtime);
/* Adjust location accuracy by time*speed. */
current_accuracy = gps->hint->accuracy;
current_accuracy +=
((uint64_t)HINT_LOCATION_ACCURACY_DEGRADE_SPEED_MPS *
(currtime.tv_sec - gps->hint->location_time.tv_sec));
dbg_int("Using last known location (lat: %.7f, long: %.7f).\n",
(double)latitude / 10000000.0f,
(double)longitude / 10000000.0f);
current_accuracy *= 100; /* m => cm */
if (current_accuracy > UINT32_MAX)
current_accuracy = UINT32_MAX;
altitude = gps->hint->altitude * 100; /* m => cm */
if (altitude > INT32_MAX)
altitude = INT32_MAX;
else if (altitude < INT32_MIN)
altitude = INT32_MIN;
latitude = gps->hint->latitude;
longitude = gps->hint->longitude;
accuracy = current_accuracy;
dbg_int("Using last known position:\n");
dbg_int(" lat : %d\n", gps->hint->latitude);
dbg_int(" long : %d\n", gps->hint->longitude);
dbg_int(" alt : %d meters\n", altitude / 100);
dbg_int(" acc_old : %u meters\n", gps->hint->accuracy);
dbg_int(" acc_curr: %u meters\n", (uint32_t)current_accuracy / 100);
/* Position is given in lat / long / alt */
flags |= (1 << 5);
/* Position is valid. */
flags |= (1 << 0);
}
/* Allocate and setup AID-INI message */
......@@ -1291,6 +1315,13 @@ int ubgps_parse_nav_pvt(struct ubgps_s * const gps, struct ubx_msg_s const * con
gps->location.ground_speed_accuracy/1000,
gps->location.heading/100000,
gps->location.heading_accuracy/100000);
/* Update location hint for A-GPS. */
(void)ubgps_give_location_hint((double)gps->location.latitude / 1e7,
(double)gps->location.longitude / 1e7,
gps->location.height / 1000,
gps->location.horizontal_accuracy / 1000);
}
return OK;
......
......@@ -40,6 +40,7 @@
#include <arpa/inet.h>
#include <netinet/in.h>
#include <pthread.h>
#include <arch/board/board-reset.h>
#include "ubgps_events.h"
#include "ubx.h"
......@@ -52,6 +53,16 @@
#define DEFAULT_NAVIGATION_RATE 1000
/* How fast location hint accuracy degrades over time. */
#define HINT_LOCATION_ACCURACY_DEGRADE_SPEED_KPH 50 /* km/h */
#define HINT_LOCATION_ACCURACY_DEGRADE_SPEED_MPS \
(HINT_LOCATION_ACCURACY_DEGRADE_SPEED_KPH * (1000) / (60 * 60)) /* m/s */
/* Minimum accuracy required for location hint. */
#define HINT_LOCATION_MINIMUM_NEW_ACCURACY 1500 /* meters */
/****************************************************************************
* Public Types
****************************************************************************/
......@@ -151,10 +162,6 @@ struct nmea_data_s
/* GPS assistance data structure */
struct gps_assistance_s {
/* Use system time */
bool use_time;
/* Allocated AlmanacPlus filename for AssistNow Offline */
char * alp_file;
......@@ -163,33 +170,41 @@ struct gps_assistance_s {
uint16_t alp_file_id;
/* Use location */
/* Aiding server address */
struct sockaddr_in alp_srv_addr;
/* Update time of current aiding data in seconds */
bool use_loc;
time_t update_time;
};
/* Latitude */
/* Additional GPS assistance hint data structure */
uint32_t latitude;
struct gps_assist_hint_s {
/* Flags */
/* Longitude */
bool have_location;
uint32_t longitude;
/* Longitude in 1e7 scale */
/* Altitude in centimeters */
int32_t longitude;
int32_t altitude;
/* Latitude in 1e7 scale */
int32_t latitude;
/* Accuracy in centimeters */
/* Horizontal accuracy estimate in mm */
uint32_t accuracy;
/* Aiding server address */
/* Height above mean sea level in mm */
struct sockaddr_in alp_srv_addr;
int32_t altitude;
/* Update time of current aiding data in seconds */
/* Date when location data was valid */
time_t update_time;
struct timespec location_time;
};
/* GPS module internal structure */
......@@ -239,6 +254,7 @@ struct ubgps_s {
/* GPS assistance data */
struct gps_assistance_s * assist;
struct gps_assist_hint_s * hint;
};
......
......@@ -586,15 +586,6 @@ static int ubgps_sm_global(struct ubgps_s * const gps, struct sm_event_s const *
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);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment