Added longitude / latitude
[gps] / gps_interpolate.c
1 /*
2   Author : Benoit Papillault <benoit.papillault@free.fr>
3   Creation : 2011-07-13
4
5   Goal : interpolate position every 100ms from a GPS log every second
6     or so
7
8   awk '{print $1" "$2" "$3}' < mygps-tgv-aller.txt  | cut -c19- > y.txt
9 */
10
11 #include <stdio.h>
12 #include <stdlib.h>
13 #include <gps.h>
14
15 struct position {
16   /* read from log file */
17   double time;
18   double longitude;
19   double latitude;
20
21   /* computed */
22   double distance;      /* distance to first position */
23   double speed;         /* average speed with prev position, valid for i>0 */
24   double acceleration;  /* average acceleration with prev speed, valid for i>1 */
25 };
26
27 int main(int argc, const char *argv[]) {
28         const char * file = argv[1]; /* FIXME : we must check the number of
29                                         command line arguments */
30         const char * data = argv[2];
31         /* FIXME : we suppose the format of this file is : <time_t>
32            <anything> and we file generate a new file called "data.gps" with
33            the following format : <time_t> <distance> <anything> */
34         FILE *fp, *fp2;
35     double time, longitude, latitude;
36     int i;
37     double ref_latitude = 49.169569, ref_longitude = 2.303519;
38     struct position * position_list = NULL;
39     int position_num = 0;
40     char buf[1024];
41
42     fp = fopen(file, "r");
43     if (fp == NULL) {
44       perror(file);
45       exit (-1);
46     }
47
48     /* each line should be : time_t longitude latitude */
49
50     while (fscanf(fp, "%lf %lf %lf", &time, &longitude, &latitude) != EOF) {
51       /*printf("time : %f longitude : %f latitude : %f\n", time, longitude, latitude);*/
52
53       /* allocate a new element in the array, if needed */
54       if ((position_num==0) || (time > position_list[position_num-1].time)) {
55         position_list = (struct position *)realloc(position_list, (++position_num)*sizeof(struct position));
56         if (!position_list) {
57           perror("realloc");
58           exit (-1);
59         }
60       }
61
62       position_list[position_num-1].time = time;
63       position_list[position_num-1].longitude = longitude;
64       position_list[position_num-1].latitude = latitude;
65     }
66
67     fclose (fp);
68
69     /* process the array */
70
71     for (i=0; i<position_num; i++) {
72
73       /* initialize with default values */
74       position_list[i].distance         = 0.0;
75       position_list[i].speed            = 0.0;
76       position_list[i].acceleration     = 0.0;
77
78       /* compute distance with first position */
79       position_list[i].distance = earth_distance(position_list[i].latitude, position_list[i].longitude,
80                                                  ref_latitude, ref_longitude);
81
82       if (i>0) {
83         position_list[i].speed = (position_list[i].distance - position_list[i-1].distance) /
84           (position_list[i].time - position_list[i-1].time);
85       }
86
87       if (i>1) {
88         position_list[i].acceleration = (position_list[i].speed - position_list[i-1].speed) /
89           (position_list[i].time - position_list[i-1].time);
90       }
91
92       /* time(s) distance(m) speed(m/s) acceleration(m/s²) */
93       /*
94       printf(
95              "time %f distance %13f speed %10f acceleration %10f\n",
96              position_list[i].time, position_list[i].distance,
97              position_list[i].speed, position_list[i].acceleration);
98       */
99     }
100
101     fp2 = fopen(data,"r");
102     if (fp2 == NULL) {
103             perror(data);
104             exit (-1);
105     }
106
107     while (fscanf(fp2, "%lf %[^\n]", &time, buf) != EOF) {
108             double distance = 0.0;
109             double speed = 0.0;
110             int found = 0;
111
112             /* convert the time from mobile clock to wall clock */
113             double wall_time = time + 22854.5;
114
115             //printf("%lf %lf %s\n", time, distance, buf);
116
117             /* find matching data in position array, ie 
118                position_list[i].time <= time < position_list[i+1].time
119             */
120
121             for (i=0; i<position_num-1; i++) {
122                     if (wall_time < position_list[i].time)
123                             break;
124                     if ((wall_time >= position_list[i].time) &&
125                         (wall_time < position_list[i+1].time)) {
126                             found = 1;
127                             break;
128                     }
129             }
130
131
132             if (found) {
133                     double weight;
134                     
135                     /*
136                       f(t) = a * t + b
137                       f(t0) = f0 = a * t0 + b
138                       f(t1) = f1 = a * t1 + b
139                       
140                       f(t1) - f(t0) = f1 - f0 = a * (t1 - t0)
141                       => a = (f1 - f0) / (t1 - t0)
142                       => b = (f0 - a * t0) = (f0 - (f1 - f0) / (t1 -t0) * t0)
143                       = (f0 * (t1 - t0) - (f1 - f0) * t0) / (t1 - t0)
144                       = (f0*t1 - f0*t0 - f1*t0 + f0*to) / (t1 - t0)
145                       = (f0*t1 - f1-t0) / (t1 - t0)
146                     */
147                     
148                     weight = (wall_time - position_list[i].time) /
149                             (position_list[i+1].time - position_list[i].time);
150                     
151                     longitude = position_list[i].longitude +
152                             (position_list[i+1].longitude - 
153                              position_list[i].longitude) * weight;
154                     
155                     latitude = position_list[i].latitude + 
156                             (position_list[i+1].latitude - 
157                              position_list[i].latitude) * weight;
158                     
159                     distance = earth_distance(latitude, longitude,
160                                               ref_latitude, ref_longitude);
161
162                     speed = position_list[i].speed +
163                             (position_list[i+1].speed -
164                              position_list[i].speed) * weight;
165             }
166
167             /*if (speed > 1.0 || speed < -1.0) {*/
168                     printf("%lf %lf %f %lf %lf %s\n",
169                            time, latitude, longitude,
170                            distance, speed, buf);
171                     /*}*/
172     }
173
174     fclose(fp2);
175     return 0;
176 }