Add GPS interpolation to the input file in the output file
authorBenoit Papillault <benoit.papillault@free.fr>
Thu, 21 Jul 2011 11:42:01 +0000 (13:42 +0200)
committerBenoit Papillault <benoit.papillault@free.fr>
Thu, 21 Jul 2011 11:42:01 +0000 (13:42 +0200)
gps_interpolate.c

index 0249c4c..d0a77b5 100644 (file)
@@ -24,15 +24,20 @@ struct position {
   double acceleration; /* average acceleration with prev speed, valid for i>1 */
 };
 
-int main() {
-    const char file[] = "y.txt";
-    FILE * fp;
+int main(int argc, const char *argv[]) {
+       const char * file = argv[1]; /* FIXME : we must check the number of
+                                       command line arguments */
+       const char * data = argv[2];
+       /* FIXME : we suppose the format of this file is : <time_t>
+          <anything> and we file generate a new file called "data.gps" with
+          the following format : <time_t> <distance> <anything> */
+       FILE *fp, *fp2;
     double time, longitude, latitude;
-    int i,j,k;
-    double ref_latitude = 49.169817, ref_longitude = 2.304467;
-
+    int i;
+    double ref_latitude = 49.169569, ref_longitude = 2.303519;
     struct position * position_list = NULL;
     int position_num = 0;
+    char buf[1024];
 
     fp = fopen(file, "r");
     if (fp == NULL) {
@@ -85,98 +90,85 @@ int main() {
       }
 
       /* time(s) distance(m) speed(m/s) acceleration(m/s²) */
+      /*
       printf(
             "time %f distance %13f speed %10f acceleration %10f\n",
             position_list[i].time, position_list[i].distance,
             position_list[i].speed, position_list[i].acceleration);
+      */
     }
 
-    /* compute an interpolation for the first 2 seconds, every 100 ms
-       : 20 values.
-       j is like position_list[j].time <= time < position_list[j+1].time
-    */
-
-    double start = 1310643040.0;
-
-    for (i=0,j=0; i<100; i++) {
-      double weight;
-      double distance1, distance2;
-
-      time = start + i * 0.100;
-      for (;(j+1<position_num) && (time >= position_list[j+1].time);j++) {}
-
-      printf("interpolating for time %f i: %2d j: %2d %f <= %f < %f\n",
-            time, i, j, position_list[j].time, time, position_list[j+1].time);
-
-      weight = (time - position_list[j].time) / (position_list[j+1].time - position_list[j].time);
-
-      printf("  => weight = %f\n", weight);
-
-      longitude = position_list[j].longitude + (position_list[j+1].longitude - position_list[j].longitude) * weight;
-
-      latitude = position_list[j].latitude + (position_list[j+1].latitude - position_list[j].latitude) * weight;
-
-      distance1 = earth_distance(latitude, longitude, 
-                                position_list[j].latitude, position_list[j].longitude);
-
-      distance2 = earth_distance(latitude, longitude,
-                                position_list[j+1].latitude, position_list[j+1].longitude);
-
-      printf("  => position %f,%f (%f m) <= %f,%f < %f,%f (%f m) [%f m]\n",
-            position_list[j].longitude, position_list[j].latitude, distance1, 
-            longitude, latitude,
-            position_list[j+1].longitude, position_list[j+1].latitude, distance2,
-            distance1 + distance2);
-
-      /*
-       Fiona Gellin / album Passeport
-       f(t) = a * t + b
-       f(t0) = f0 = a * t0 + b
-       f(t1) = f1 = a * t1 + b
-
-       f(t1) - f(t0) = f1 - f0 = a * (t1 - t0)
-       => a = (f1 - f0) / (t1 - t0)
-       => b = (f0 - a * t0) = (f0 - (f1 - f0) / (t1 -t0) * t0)
-            = (f0 * (t1 - t0) - (f1 - f0) * t0) / (t1 - t0)
-            = (f0*t1 - f0*t0 - f1*t0 + f0*to) / (t1 - t0)
-            = (f0*t1 - f1-t0) / (t1 - t0)
-
-       We have computed d, v and a at discrete time t0, t1, t2
-
-       a(t) = a(ti+1) for t€[ti,ti+1]
-       v(t) = v(ti) + a(t).(t-ti) for t€[ti,ti+1]
-       d(t) = d(ti) + v(t).(t-ti) for t€[ti,ti+1]
-
-       using above formulas, we have :
-
-       a(ti) = a(ti)
-       v(ti) = v(ti)
-       d(ti) = d(ti)
-
-       still using above formulas, we have :
-
-       a(ti+1) = a(ti) (acceleration is not continuous)
-       v(ti+1) = v(ti) + a(ti+1).(ti+1-ti)
-       d(ti+1) = d(ti) + (v(ti) + a(ti+1).(ti+1-ti)).(ti+1-ti)
-
-       in practice, here is how we computed v(ti+1) and a(ti+1)
-
-       v(t0)   = 0
-       v(ti+1) = (d(ti+1)-d(ti))/(ti+1-ti)
-         => d(ti+1) = d(ti) + v(ti+1).(ti+1-ti)
-         => d(t)    = d(ti) + v(t).(t-ti)
-
-       a(t0)   = 0
-       a(t1)   = 0
-       a(ti+1) = (v(ti+1)-v(ti))/(ti+1-ti)
-         => v(ti+1) = v(ti) + a(ti+1).(ti+1-ti)
-         => v(t)    = v(ti) + a(ti+1).(t-ti)
-
-       so we can simplify the above formulas :
-       v(ti+1
-      */
+    fp2 = fopen(data,"r");
+    if (fp2 == NULL) {
+           perror(data);
+           exit (-1);
+    }
 
+    while (fscanf(fp2, "%lf %[^\n]", &time, buf) != EOF) {
+           double distance = 0.0;
+           double speed = 0.0;
+           int found = 0;
+
+           /* convert the time from mobile clock to wall clock */
+           double wall_time = time + 22854.5;
+
+           //printf("%lf %lf %s\n", time, distance, buf);
+
+           /* find matching data in position array, ie 
+              position_list[i].time <= time < position_list[i+1].time
+           */
+
+           for (i=0; i<position_num-1; i++) {
+                   if (wall_time < position_list[i].time)
+                           break;
+                   if ((wall_time >= position_list[i].time) &&
+                       (wall_time < position_list[i+1].time)) {
+                           found = 1;
+                           break;
+                   }
+           }
+
+
+           if (found) {
+                   double weight;
+                   
+                   /*
+                     f(t) = a * t + b
+                     f(t0) = f0 = a * t0 + b
+                     f(t1) = f1 = a * t1 + b
+                     
+                     f(t1) - f(t0) = f1 - f0 = a * (t1 - t0)
+                     => a = (f1 - f0) / (t1 - t0)
+                     => b = (f0 - a * t0) = (f0 - (f1 - f0) / (t1 -t0) * t0)
+                     = (f0 * (t1 - t0) - (f1 - f0) * t0) / (t1 - t0)
+                     = (f0*t1 - f0*t0 - f1*t0 + f0*to) / (t1 - t0)
+                     = (f0*t1 - f1-t0) / (t1 - t0)
+                   */
+                   
+                   weight = (wall_time - position_list[i].time) /
+                           (position_list[i+1].time - position_list[i].time);
+                   
+                   longitude = position_list[i].longitude +
+                           (position_list[i+1].longitude - 
+                            position_list[i].longitude) * weight;
+                   
+                   latitude = position_list[i].latitude + 
+                           (position_list[i+1].latitude - 
+                            position_list[i].latitude) * weight;
+                   
+                   distance = earth_distance(latitude, longitude,
+                                             ref_latitude, ref_longitude);
+
+                   speed = position_list[i].speed +
+                           (position_list[i+1].speed -
+                            position_list[i].speed) * weight;
+           }
+
+           /*if (speed > 1.0 || speed < -1.0) {*/
+                   printf("%lf %lf %lf %s\n", time, distance, speed, buf);
+                   /*}*/
     }
 
+    fclose(fp2);
     return 0;
 }