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: Jussi Kivilinna <jussi.kivilinna@haltian.com>

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
  *

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

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. */
 
-      dbg_int("Using last known location (lat: %.7f, long: %.7f).\n",
-          (double)latitude / 10000000.0f,
-          (double)longitude / 10000000.0f);
+      current_accuracy = gps->hint->accuracy;
+      current_accuracy +=
+          ((uint64_t)HINT_LOCATION_ACCURACY_DEGRADE_SPEED_MPS *
+           (currtime.tv_sec - gps->hint->location_time.tv_sec));
+
+      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;

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

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);