common: Make Logger calculate and publish pace.
[gps-watch.git] / src / common / gps.rs
1 /*
2  * Copyright (c) 2019 Tilman Sauerbeck (tilman at code-monkey de)
3  *
4  * Permission is hereby granted, free of charge, to any person obtaining
5  * a copy of this software and associated documentation files (the
6  * "Software"), to deal in the Software without restriction, including
7  * without limitation the rights to use, copy, modify, merge, publish,
8  * distribute, sublicense, and/or sell copies of the Software, and to
9  * permit persons to whom the Software is furnished to do so, subject to
10  * the following conditions:
11  *
12  * The above copyright notice and this permission notice shall be
13  * included in all copies or substantial portions of the Software.
14  *
15  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
16  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
17  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
18  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
19  * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
20  * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
21  * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
22  */
23
24 use fixed15_49;
25 use systick;
26
27 type Fixed = fixed15_49::Fixed15_49;
28
29 enum ParseState {
30     Start,
31     InPacket,
32     InChecksum1,
33     InChecksum2,
34 }
35
36 pub struct Gps {
37     unix_date: u32, // Number of days passed since 1970-01-01
38     offset: usize,
39     state: ParseState,
40     checksum: u8,
41     checksum_is_valid: bool,
42     line: [u8; 256],
43 }
44
45 #[derive(Clone, Copy)]
46 pub struct TimeAndPos {
47     pub system_time: u32,
48     pub unix_time: u32,
49     pub latitude_deg: i32, // Positive means north, negative means south.
50     pub longitude_deg: i32, // Positive means east, negative means west.
51     pub latitude_rad: Fixed, // Positive means north, negative means south.
52     pub longitude_rad: Fixed, // Positive means east, negative means west.
53 }
54
55 impl TimeAndPos {
56     pub fn new() -> TimeAndPos {
57         TimeAndPos {
58             system_time: 0,
59             unix_time: 0,
60             latitude_deg: 0,
61             longitude_deg: 0,
62             latitude_rad: Fixed::from_i64(0),
63             longitude_rad: Fixed::from_i64(0),
64         }
65     }
66
67     pub fn distance_cm(&self, other: &TimeAndPos) -> i64 {
68         let a_lat = self.latitude_rad;
69         let a_lon = self.longitude_rad;
70
71         let b_lat = other.latitude_rad;
72         let b_lon = other.longitude_rad;
73
74         let x = (b_lon - a_lon) * ((b_lat + a_lat) / Fixed::from_i64(2)).cos();
75         let y = b_lat - a_lat;
76
77         let d_km = (x * x + y * y).sqrt() * Fixed::from_i64(6371);
78
79         let hundred = Fixed::from_i64(100);
80         let thousand = Fixed::from_i64(1000);
81
82         // Convert from km to cm.
83         (d_km * thousand * hundred).to_i64()
84     }
85 }
86
87 fn to_lower(c: u8) -> u8 {
88     c | 0x20
89 }
90
91 fn parse_coordinate(s: &[u8]) -> i32 {
92     // Find the position of the decimal separator for the minutes.
93     let dot_position = s.iter().enumerate().find(|(_, &c)| {
94         c == b'.'
95     }).and_then(|(i, _)| {
96         Some(i)
97     });
98
99     if dot_position.is_none() {
100         return 0;
101     }
102
103     // Minutes take two digits before the decimal separator.
104     let num_degree_digits = dot_position.unwrap() - 2;
105
106     let mut degrees = 0;
107
108     for c in s[0..num_degree_digits].iter() {
109         degrees *= 10;
110         degrees += (c - b'0') as i32;
111     }
112
113     let mut minutes = 0;
114
115     for c in s[num_degree_digits..dot_position.unwrap()].iter() {
116         minutes *= 10;
117         minutes += (c - b'0') as i32;
118     }
119
120     minutes += degrees * 60;
121
122     for c in s[dot_position.unwrap() + 1..].iter() {
123         minutes *= 10;
124         minutes += (c - b'0') as i32;
125     }
126
127     minutes
128 }
129
130 fn parse_coordinate_q(s: &[u8]) -> Fixed {
131     // Find the position of the decimal separator for the minutes.
132     let dot_position_o = s.iter().enumerate().find(|(_, &c)| {
133         c == b'.'
134     }).and_then(|(i, _)| {
135         Some(i)
136     });
137
138     if dot_position_o.is_none() {
139         return Fixed::from_i64(0);
140     }
141
142     let dot_position = dot_position_o.unwrap();
143
144     // Minutes take two digits before the decimal separator.
145     let num_degree_digits = dot_position - 2;
146
147     let degrees = s[0..num_degree_digits].iter().fold(0, |d, c| {
148         (d * 10) + (c - b'0') as i32
149     });
150
151     let minutes = s[num_degree_digits..dot_position].iter().fold(0, |d, c| {
152         (d * 10) + (c - b'0') as i32
153     });
154
155     let decimal_minutes = s[dot_position + 1..].iter().fold(0, |d, c| {
156         (d * 10) + (c - b'0') as i32
157     });
158
159     let mut result = Fixed::from_i64(decimal_minutes.into());
160     result /= Fixed::from_i64(10000);
161
162     result += Fixed::from_i64(minutes.into());
163     result /= Fixed::from_i64(60);
164
165     result += Fixed::from_i64(degrees.into());
166
167     return result;
168 }
169
170 // Only works for 2016 onwards.
171 fn is_leap_year(year: u32) -> bool {
172     (year & 3) == 0
173 }
174
175 fn parse_d2(s: &[u8]) -> u32 {
176     let a = (s[0] - b'0') as u32;
177     let b = (s[1] - b'0') as u32;
178
179     (a * 10) + b
180 }
181
182 struct FieldIter<'a> {
183     command: &'a [u8],
184     offset: usize,
185 }
186
187 impl<'a> Iterator for FieldIter<'a> {
188     type Item = &'a [u8];
189
190     fn next(&mut self) -> Option<&'a [u8]> {
191         let delimiter = b',';
192
193         if self.offset == self.command.len() {
194             self.offset += 1;
195
196             return Some(b"");
197         }
198
199         if self.offset > self.command.len() {
200             return None;
201         }
202
203         if self.command[self.offset] == delimiter {
204             self.offset += 1;
205
206             return Some(b"");
207         }
208
209         let mut start : Option<usize> = None;
210
211         // Find the start of the substring.
212         for o in self.offset..self.command.len() {
213             if self.command[o] != delimiter {
214                 start = Some(o);
215                 break
216             }
217         }
218
219         start.and_then(|start2| {
220             let mut end : Option<usize> = None;
221
222             // Find the end of the substring.
223             for o in start2..self.command.len() {
224                 if self.command[o] == delimiter {
225                     break
226                 }
227
228                 end = Some(o);
229             }
230
231             end.and_then(|end2| {
232                 let end3 = end2 + 1;
233
234                 self.offset = end3 + 1;
235
236                 Some(&self.command[start2..end3])
237             })
238         })
239     }
240 }
241
242 const SUM_EXTRA_DAYS_UNTIL_MONTH : [u32; 11] = [
243     0x03, 0x03, 0x06, 0x08, 0x0b, 0x0d,
244     0x10, 0x13, 0x15, 0x18, 0x1a,
245 ];
246
247 impl Gps {
248     pub fn new() -> Gps {
249         Gps {
250             unix_date: 0,
251             offset: 0,
252             state: ParseState::Start,
253             checksum: 0,
254             checksum_is_valid: false,
255             line: [0; 256],
256         }
257     }
258
259     pub fn update<F>(&mut self, tap: &mut TimeAndPos, mut read_func: F) -> bool
260                      where F: FnMut() -> Option<u8>
261     {
262         let hexdigits = b"0123456789abcdef";
263
264         while let Some(received) = read_func() {
265             if received == b'$' {
266                 self.state = ParseState::InPacket;
267                 self.offset = 0;
268                 self.checksum = 0x00;
269
270                 continue;
271             }
272
273             match self.state {
274                 ParseState::Start => {
275                     // Empty.
276                 },
277                 ParseState::InPacket => {
278                     if received == b'*' {
279                         self.state = ParseState::InChecksum1;
280                         self.checksum_is_valid = true;
281
282                     // Check if message fits in buffer. We subtract one
283                     // because we will need to write the sentinel later.
284                     } else if self.offset == self.line.len() - 1 {
285                         self.state = ParseState::Start;
286                     } else {
287                         self.checksum ^= received;
288                         self.line[self.offset] = received;
289                         self.offset += 1;
290                     }
291                 },
292                 ParseState::InChecksum1 => {
293                     let expected = hexdigits[(self.checksum >> 4) as usize & 0xf];
294
295                     if to_lower(received) != expected {
296                         self.checksum_is_valid = false;
297                     }
298
299                     self.state = ParseState::InChecksum2;
300                 },
301                 ParseState::InChecksum2 => {
302                     let expected = hexdigits[(self.checksum >> 0) as usize & 0xf];
303
304                     if to_lower(received) != expected {
305                         self.checksum_is_valid = false;
306                     }
307
308                     self.state = ParseState::Start;
309
310                     if self.checksum_is_valid {
311                         // Terminate and dispatch.
312                         self.line[self.offset] = b'\0';
313
314                         if self.parse(tap) {
315                             return true;
316                         }
317                     }
318                 },
319             }
320         }
321
322         false
323     }
324
325     fn parse(&mut self, tap: &mut TimeAndPos) -> bool {
326         let line_copy : [u8; 256] = self.line;
327
328         let mut field_iter = FieldIter {
329             command: &line_copy[0..self.offset],
330             offset: 0,
331         };
332
333         match field_iter.next() {
334             Some(b"GPRMC") => {
335                 self.parse_rmc(&mut field_iter);
336                 false
337             },
338             Some(b"GPGGA") => {
339                 self.parse_gga(&mut field_iter, tap)
340             },
341             _ => {
342                 // Ignore.
343                 false
344             }
345         }
346     }
347
348     fn parse_rmc(&mut self, field_iter: &mut FieldIter) {
349         let f8 = field_iter.nth(8);
350
351         if let Some(date) = f8 {
352             if date.len() >= 6 {
353                 let days = parse_d2(&date[0..2]);
354                 let months = parse_d2(&date[2..4]);
355                 let years = 2000 + parse_d2(&date[4..6]);
356
357                 let years_in_epoch = years - 1970;
358
359                 // This only works until 2100.
360                 let mut unix_date =
361                     (years_in_epoch * 365) + ((years_in_epoch + 1) / 4);
362
363                 // Add the number of days passed in this year,
364                 // excluding the current month.
365                 if months >= 2 {
366                     unix_date += 28 * (months - 1);
367                     unix_date += SUM_EXTRA_DAYS_UNTIL_MONTH[(months - 2) as usize];
368                 }
369
370                 if months >= 3 && is_leap_year(years) {
371                     unix_date += 1;
372                 }
373
374                 if days > 0 {
375                     unix_date += days - 1;
376                 }
377
378                 self.unix_date = unix_date;
379             }
380         }
381     }
382
383     fn parse_gga(&mut self, field_iter: &mut FieldIter, tap: &mut TimeAndPos) -> bool {
384         let f0 = field_iter.next();
385         let f1 = field_iter.next();
386         let f2 = field_iter.next();
387         let f3 = field_iter.next();
388         let f4 = field_iter.next();
389         let f5 = field_iter.next();
390
391         match (f0, f1, f2, f3, f4, f5) {
392             (Some(utc_time),
393              Some(latitude),
394              Some(north_south),
395              Some(longitude),
396              Some(east_west),
397              Some(pos_fix_indicator),
398             ) => {
399                 if self.unix_date == 0 {
400                     return false;
401                 }
402
403                 match pos_fix_indicator {
404                     b"1" => {
405                         // Valid standard GPS fix (low resolution).
406                     },
407                     b"2" => {
408                         // Valid differential GPS fix (high resolution).
409                     },
410                     _ => {
411                         return false;
412                     }
413                 }
414
415                 if utc_time.len() < 6 {
416                     return false;
417                 }
418
419                 let hours = parse_d2(&utc_time[0..2]);
420                 let minutes = parse_d2(&utc_time[2..4]);
421                 let seconds = parse_d2(&utc_time[4..6]);
422
423                 let mut unix_time = self.unix_date;
424                 unix_time *= 24; // Days to hours.
425                 unix_time += hours;
426                 unix_time *= 60; // Hours to minutes.
427                 unix_time += minutes;
428                 unix_time *= 60; // Minutes to seconds.
429                 unix_time += seconds;
430
431                 tap.system_time = systick::now();
432                 tap.unix_time = unix_time;
433                 tap.latitude_deg = parse_coordinate(latitude);
434                 tap.longitude_deg = parse_coordinate(longitude);
435                 tap.latitude_rad = parse_coordinate_q(latitude).to_radians();
436                 tap.longitude_rad = parse_coordinate_q(longitude).to_radians();
437
438                 if north_south == b"S" {
439                     tap.latitude_deg = -tap.latitude_deg;
440                     tap.latitude_rad = -tap.latitude_rad;
441                 }
442
443                 if east_west == b"W" {
444                     tap.longitude_deg = -tap.longitude_deg;
445                     tap.longitude_rad = -tap.longitude_rad;
446                 }
447
448                 true
449             },
450             _ => {
451                 false
452             }
453         }
454     }
455 }