common: Implement Time::fmt_time().
[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 ringbuf::Ringbuf;
25 use systick;
26
27 enum ParseState {
28     Start,
29     InPacket,
30     InChecksum1,
31     InChecksum2,
32 }
33
34 pub struct Gps {
35     unix_date: u32, // Number of days passed since 1970-01-01
36     offset: usize,
37     state: ParseState,
38     checksum: u8,
39     checksum_is_valid: bool,
40     line: [u8; 256],
41 }
42
43 #[derive(Clone, Copy)]
44 pub struct TimeAndPos {
45     pub system_time: u32,
46     pub unix_time: u32,
47     pub latitude: i32, // Positive means north, negative means south.
48     pub longitude: i32, // Positive means east, negative means west.
49 }
50
51 impl TimeAndPos {
52     pub fn new() -> TimeAndPos {
53         TimeAndPos {
54             system_time: 0,
55             unix_time: 0,
56             latitude: 0,
57             longitude: 0,
58         }
59     }
60 }
61
62 fn to_lower(c: u8) -> u8 {
63     c | 0x20
64 }
65
66 fn try_read() -> Option<u8> {
67     extern {
68         static mut uart0_rx_buf: Ringbuf;
69     }
70
71     unsafe {
72         if uart0_rx_buf.is_empty() {
73             None
74         } else {
75             Some(uart0_rx_buf.read())
76         }
77     }
78 }
79
80 fn parse_coordinate(s: &[u8]) -> i32 {
81     // Find the position of the decimal separator for the minutes.
82     let dot_position = s.iter().enumerate().find(|(_, &c)| {
83         c == b'.'
84     }).and_then(|(i, _)| {
85         Some(i)
86     });
87
88     if dot_position.is_none() {
89         return 0;
90     }
91
92     // Minutes take two digits before the decimal separator.
93     let num_degree_digits = dot_position.unwrap() - 2;
94
95     let mut degrees = 0;
96
97     for c in s[0..num_degree_digits].iter() {
98         degrees *= 10;
99         degrees += (c - b'0') as i32;
100     }
101
102     let mut minutes = 0;
103
104     for c in s[num_degree_digits..dot_position.unwrap()].iter() {
105         minutes *= 10;
106         minutes += (c - b'0') as i32;
107     }
108
109     minutes += degrees * 60;
110
111     for c in s[dot_position.unwrap() + 1..].iter() {
112         minutes *= 10;
113         minutes += (c - b'0') as i32;
114     }
115
116     minutes
117 }
118
119 // Only works for 2016 onwards.
120 fn is_leap_year(year: u32) -> bool {
121     (year & 3) == 0
122 }
123
124 fn parse_d2(s: &[u8]) -> u32 {
125     let a = (s[0] - b'0') as u32;
126     let b = (s[1] - b'0') as u32;
127
128     (a * 10) + b
129 }
130
131 struct FieldIter<'a> {
132     command: &'a [u8],
133     offset: usize,
134 }
135
136 impl<'a> Iterator for FieldIter<'a> {
137     type Item = &'a [u8];
138
139     fn next(&mut self) -> Option<&'a [u8]> {
140         let delimiter = b',';
141
142         if self.offset == self.command.len() {
143             self.offset += 1;
144
145             return Some(b"");
146         }
147
148         if self.offset > self.command.len() {
149             return None;
150         }
151
152         if self.command[self.offset] == delimiter {
153             self.offset += 1;
154
155             return Some(b"");
156         }
157
158         let mut start : Option<usize> = None;
159
160         // Find the start of the substring.
161         for o in self.offset..self.command.len() {
162             if self.command[o] != delimiter {
163                 start = Some(o);
164                 break
165             }
166         }
167
168         start.and_then(|start2| {
169             let mut end : Option<usize> = None;
170
171             // Find the end of the substring.
172             for o in start2..self.command.len() {
173                 if self.command[o] == delimiter {
174                     break
175                 }
176
177                 end = Some(o);
178             }
179
180             end.and_then(|end2| {
181                 let end3 = end2 + 1;
182
183                 self.offset = end3 + 1;
184
185                 Some(&self.command[start2..end3])
186             })
187         })
188     }
189 }
190
191 const SUM_EXTRA_DAYS_UNTIL_MONTH : [u32; 11] = [
192     0x03, 0x03, 0x06, 0x08, 0x0b, 0x0d,
193     0x10, 0x13, 0x15, 0x18, 0x1a,
194 ];
195
196 impl Gps {
197     pub fn new() -> Gps {
198         Gps {
199             unix_date: 0,
200             offset: 0,
201             state: ParseState::Start,
202             checksum: 0,
203             checksum_is_valid: false,
204             line: [0; 256],
205         }
206     }
207
208     pub fn update(&mut self, tap: &mut TimeAndPos) -> bool {
209         let hexdigits = b"0123456789abcdef";
210
211         while let Some(received) = try_read() {
212             match self.state {
213                 ParseState::Start => {
214                     if received == b'$' {
215                         self.state = ParseState::InPacket;
216                         self.offset = 0;
217                         self.checksum = 0x00;
218                     }
219                 },
220                 ParseState::InPacket => {
221                     if received == b'*' {
222                         self.state = ParseState::InChecksum1;
223                         self.checksum_is_valid = true;
224
225                     // Check if message fits in buffer. We subtract one
226                     // because we will need to write the sentinel later.
227                     } else if self.offset == self.line.len() - 1 {
228                         self.state = ParseState::Start;
229                     } else {
230                         self.checksum ^= received;
231                         self.line[self.offset] = received;
232                         self.offset += 1;
233                     }
234                 },
235                 ParseState::InChecksum1 => {
236                     let expected = hexdigits[(self.checksum >> 4) as usize & 0xf];
237
238                     if to_lower(received) != expected {
239                         self.checksum_is_valid = false;
240                     }
241
242                     self.state = ParseState::InChecksum2;
243                 },
244                 ParseState::InChecksum2 => {
245                     let expected = hexdigits[(self.checksum >> 0) as usize & 0xf];
246
247                     if to_lower(received) != expected {
248                         self.checksum_is_valid = false;
249                     }
250
251                     self.state = ParseState::Start;
252
253                     if self.checksum_is_valid {
254                         // Terminate and dispatch.
255                         self.line[self.offset] = b'\0';
256
257                         if self.parse(tap) {
258                             return true;
259                         }
260                     }
261                 },
262             }
263         }
264
265         false
266     }
267
268     fn parse(&mut self, tap: &mut TimeAndPos) -> bool {
269         let line_copy : [u8; 256] = self.line;
270
271         let mut field_iter = FieldIter {
272             command: &line_copy[0..self.offset],
273             offset: 0,
274         };
275
276         match field_iter.next() {
277             Some(b"GPRMC") => {
278                 self.parse_rmc(&mut field_iter);
279                 false
280             },
281             Some(b"GPGGA") => {
282                 self.parse_gga(&mut field_iter, tap)
283             },
284             _ => {
285                 // Ignore.
286                 false
287             }
288         }
289     }
290
291     fn parse_rmc(&mut self, field_iter: &mut FieldIter) {
292         let f8 = field_iter.nth(8);
293
294         if let Some(date) = f8 {
295             if date.len() >= 6 {
296                 let days = parse_d2(&date[0..2]);
297                 let months = parse_d2(&date[2..4]);
298                 let years = 2000 + parse_d2(&date[4..6]);
299
300                 let years_in_epoch = years - 1970;
301
302                 // This only works until 2100.
303                 let mut unix_date =
304                     (years_in_epoch * 365) + ((years_in_epoch + 1) / 4);
305
306                 // Add the number of days passed in this year,
307                 // excluding the current month.
308                 if months >= 2 {
309                     unix_date += 28 * (months - 1);
310                     unix_date += SUM_EXTRA_DAYS_UNTIL_MONTH[(months - 2) as usize];
311                 }
312
313                 if months >= 3 && is_leap_year(years) {
314                     unix_date += 1;
315                 }
316
317                 if days > 0 {
318                     unix_date += days - 1;
319                 }
320
321                 self.unix_date = unix_date;
322             }
323         }
324     }
325
326     fn parse_gga(&mut self, field_iter: &mut FieldIter, tap: &mut TimeAndPos) -> bool {
327         let f0 = field_iter.next();
328         let f1 = field_iter.next();
329         let f2 = field_iter.next();
330         let f3 = field_iter.next();
331         let f4 = field_iter.next();
332         let f5 = field_iter.next();
333
334         match (f0, f1, f2, f3, f4, f5) {
335             (Some(utc_time),
336              Some(latitude),
337              Some(north_south),
338              Some(longitude),
339              Some(east_west),
340              Some(pos_fix_indicator),
341             ) => {
342                 if self.unix_date == 0 {
343                     return false;
344                 }
345
346                 if pos_fix_indicator != b"1" {
347                     return false;
348                 }
349
350                 if utc_time.len() < 6 {
351                     return false;
352                 }
353
354                 let hours = parse_d2(&utc_time[0..2]);
355                 let minutes = parse_d2(&utc_time[2..4]);
356                 let seconds = parse_d2(&utc_time[4..6]);
357
358                 let mut unix_time = self.unix_date;
359                 unix_time *= 24; // Days to hours.
360                 unix_time += hours;
361                 unix_time *= 60; // Hours to minutes.
362                 unix_time += minutes;
363                 unix_time *= 60; // Minutes to seconds.
364                 unix_time += seconds;
365
366                 tap.system_time = systick::now();
367                 tap.unix_time = unix_time;
368                 tap.latitude = parse_coordinate(latitude);
369                 tap.longitude = parse_coordinate(longitude);
370
371                 if north_south == b"S" {
372                     tap.latitude = -tap.latitude;
373                 }
374
375                 if east_west == b"W" {
376                     tap.longitude = -tap.longitude;
377                 }
378
379                 true
380             },
381             _ => {
382                 false
383             }
384         }
385     }
386 }