diff --git a/apps/include/system/ubgps.h b/apps/include/system/ubgps.h
index 241578491182344c196d7c3862ae4fd7555869b5..a75431bcea7ee2eeff90f0f1ad1c7a33edb6d607 100644
--- a/apps/include/system/ubgps.h
+++ b/apps/include/system/ubgps.h
@@ -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
*
diff --git a/apps/system/ubgps/ubgps.c b/apps/system/ubgps/ubgps.c
index fc5332855c9769b8fc500b754bce29c450ce90bc..675d9d34650c12115630279b5f31d4e9c89fe702 100644
--- a/apps/system/ubgps/ubgps.c
+++ b/apps/system/ubgps/ubgps.c
@@ -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;
}
diff --git a/apps/system/ubgps/ubgps_internal.c b/apps/system/ubgps/ubgps_internal.c
index b95f2721e31a08ff7149daed51e759cbd62add8f..33366a5518aa84e121c4018ed4fef11fd4ae5e82 100644
--- a/apps/system/ubgps/ubgps_internal.c
+++ b/apps/system/ubgps/ubgps_internal.c
@@ -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;
diff --git a/apps/system/ubgps/ubgps_internal.h b/apps/system/ubgps/ubgps_internal.h
index 008aaa9ed8435db07fb0420bafb9e3eb53fcb262..b359992bd2020b61c829852ea470165520193281 100644
--- a/apps/system/ubgps/ubgps_internal.h
+++ b/apps/system/ubgps/ubgps_internal.h
@@ -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;
};
diff --git a/apps/system/ubgps/ubgps_state.c b/apps/system/ubgps/ubgps_state.c
index 566b126228925962ff06acdc03920f1201af166b..1a95b01e6772d08cacd840b29135b6959eb28971 100644
--- a/apps/system/ubgps/ubgps_state.c
+++ b/apps/system/ubgps/ubgps_state.c
@@ -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);