2 * Copyright (c) 2019 Tilman Sauerbeck (tilman at code-monkey de)
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:
12 * The above copyright notice and this permission notice shall be
13 * included in all copies or substantial portions of the Software.
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.
35 unix_date: u32, // Number of days passed since 1970-01-01
39 checksum_is_valid: bool,
43 #[derive(Clone, Copy)]
44 pub struct TimeAndPos {
47 pub latitude: i32, // Positive means north, negative means south.
48 pub longitude: i32, // Positive means east, negative means west.
52 pub fn new() -> TimeAndPos {
62 fn to_lower(c: u8) -> u8 {
66 fn try_read() -> Option<u8> {
68 static mut uart0_rx_buf: Ringbuf;
72 if uart0_rx_buf.is_empty() {
75 Some(uart0_rx_buf.read())
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)| {
84 }).and_then(|(i, _)| {
88 if dot_position.is_none() {
92 // Minutes take two digits before the decimal separator.
93 let num_degree_digits = dot_position.unwrap() - 2;
97 for c in s[0..num_degree_digits].iter() {
99 degrees += (c - b'0') as i32;
104 for c in s[num_degree_digits..dot_position.unwrap()].iter() {
106 minutes += (c - b'0') as i32;
109 minutes += degrees * 60;
111 for c in s[dot_position.unwrap() + 1..].iter() {
113 minutes += (c - b'0') as i32;
119 // Only works for 2016 onwards.
120 fn is_leap_year(year: u32) -> bool {
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;
131 struct FieldIter<'a> {
136 impl<'a> Iterator for FieldIter<'a> {
137 type Item = &'a [u8];
139 fn next(&mut self) -> Option<&'a [u8]> {
140 let delimiter = b',';
142 if self.offset == self.command.len() {
148 if self.offset > self.command.len() {
152 if self.command[self.offset] == delimiter {
158 let mut start : Option<usize> = None;
160 // Find the start of the substring.
161 for o in self.offset..self.command.len() {
162 if self.command[o] != delimiter {
168 start.and_then(|start2| {
169 let mut end : Option<usize> = None;
171 // Find the end of the substring.
172 for o in start2..self.command.len() {
173 if self.command[o] == delimiter {
180 end.and_then(|end2| {
183 self.offset = end3 + 1;
185 Some(&self.command[start2..end3])
191 const SUM_EXTRA_DAYS_UNTIL_MONTH : [u32; 11] = [
192 0x03, 0x03, 0x06, 0x08, 0x0b, 0x0d,
193 0x10, 0x13, 0x15, 0x18, 0x1a,
197 pub fn new() -> Gps {
201 state: ParseState::Start,
203 checksum_is_valid: false,
208 pub fn update(&mut self, tap: &mut TimeAndPos) -> bool {
209 let hexdigits = b"0123456789abcdef";
211 while let Some(received) = try_read() {
212 if received == b'$' {
213 self.state = ParseState::InPacket;
215 self.checksum = 0x00;
221 ParseState::Start => {
224 ParseState::InPacket => {
225 if received == b'*' {
226 self.state = ParseState::InChecksum1;
227 self.checksum_is_valid = true;
229 // Check if message fits in buffer. We subtract one
230 // because we will need to write the sentinel later.
231 } else if self.offset == self.line.len() - 1 {
232 self.state = ParseState::Start;
234 self.checksum ^= received;
235 self.line[self.offset] = received;
239 ParseState::InChecksum1 => {
240 let expected = hexdigits[(self.checksum >> 4) as usize & 0xf];
242 if to_lower(received) != expected {
243 self.checksum_is_valid = false;
246 self.state = ParseState::InChecksum2;
248 ParseState::InChecksum2 => {
249 let expected = hexdigits[(self.checksum >> 0) as usize & 0xf];
251 if to_lower(received) != expected {
252 self.checksum_is_valid = false;
255 self.state = ParseState::Start;
257 if self.checksum_is_valid {
258 // Terminate and dispatch.
259 self.line[self.offset] = b'\0';
272 fn parse(&mut self, tap: &mut TimeAndPos) -> bool {
273 let line_copy : [u8; 256] = self.line;
275 let mut field_iter = FieldIter {
276 command: &line_copy[0..self.offset],
280 match field_iter.next() {
282 self.parse_rmc(&mut field_iter);
286 self.parse_gga(&mut field_iter, tap)
295 fn parse_rmc(&mut self, field_iter: &mut FieldIter) {
296 let f8 = field_iter.nth(8);
298 if let Some(date) = f8 {
300 let days = parse_d2(&date[0..2]);
301 let months = parse_d2(&date[2..4]);
302 let years = 2000 + parse_d2(&date[4..6]);
304 let years_in_epoch = years - 1970;
306 // This only works until 2100.
308 (years_in_epoch * 365) + ((years_in_epoch + 1) / 4);
310 // Add the number of days passed in this year,
311 // excluding the current month.
313 unix_date += 28 * (months - 1);
314 unix_date += SUM_EXTRA_DAYS_UNTIL_MONTH[(months - 2) as usize];
317 if months >= 3 && is_leap_year(years) {
322 unix_date += days - 1;
325 self.unix_date = unix_date;
330 fn parse_gga(&mut self, field_iter: &mut FieldIter, tap: &mut TimeAndPos) -> bool {
331 let f0 = field_iter.next();
332 let f1 = field_iter.next();
333 let f2 = field_iter.next();
334 let f3 = field_iter.next();
335 let f4 = field_iter.next();
336 let f5 = field_iter.next();
338 match (f0, f1, f2, f3, f4, f5) {
344 Some(pos_fix_indicator),
346 if self.unix_date == 0 {
350 match pos_fix_indicator {
352 // Valid standard GPS fix (low resolution).
355 // Valid differential GPS fix (high resolution).
362 if utc_time.len() < 6 {
366 let hours = parse_d2(&utc_time[0..2]);
367 let minutes = parse_d2(&utc_time[2..4]);
368 let seconds = parse_d2(&utc_time[4..6]);
370 let mut unix_time = self.unix_date;
371 unix_time *= 24; // Days to hours.
373 unix_time *= 60; // Hours to minutes.
374 unix_time += minutes;
375 unix_time *= 60; // Minutes to seconds.
376 unix_time += seconds;
378 tap.system_time = systick::now();
379 tap.unix_time = unix_time;
380 tap.latitude = parse_coordinate(latitude);
381 tap.longitude = parse_coordinate(longitude);
383 if north_south == b"S" {
384 tap.latitude = -tap.latitude;
387 if east_west == b"W" {
388 tap.longitude = -tap.longitude;