From 0ed02a94b60d886b568f4803a7ec09eef29ad1ca Mon Sep 17 00:00:00 2001 From: Tilman Sauerbeck Date: Mon, 30 Dec 2019 10:41:00 +0100 Subject: [PATCH] common: Add the gps module. --- SConscript.libcommon | 1 + src/common/gps.rs | 386 +++++++++++++++++++++++++++++++++++++++++++ src/common/lib.rs | 1 + 3 files changed, 388 insertions(+) create mode 100644 src/common/gps.rs diff --git a/SConscript.libcommon b/SConscript.libcommon index f15e239..f4abd4b 100644 --- a/SConscript.libcommon +++ b/SConscript.libcommon @@ -18,6 +18,7 @@ source_files_rs = [ 'src/common/usb_serial.rs', 'src/common/display.rs', 'src/common/screen.rs', + 'src/common/gps.rs', ] source_files_c = [ diff --git a/src/common/gps.rs b/src/common/gps.rs new file mode 100644 index 0000000..b9fcbc3 --- /dev/null +++ b/src/common/gps.rs @@ -0,0 +1,386 @@ +/* + * Copyright (c) 2019 Tilman Sauerbeck (tilman at code-monkey de) + * + * Permission is hereby granted, free of charge, to any person obtaining + * a copy of this software and associated documentation files (the + * "Software"), to deal in the Software without restriction, including + * without limitation the rights to use, copy, modify, merge, publish, + * distribute, sublicense, and/or sell copies of the Software, and to + * permit persons to whom the Software is furnished to do so, subject to + * the following conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE + * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION + * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION + * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +use ringbuf::Ringbuf; +use systick; + +enum ParseState { + Start, + InPacket, + InChecksum1, + InChecksum2, +} + +pub struct Gps { + unix_date: u32, // Number of days passed since 1970-01-01 + offset: usize, + state: ParseState, + checksum: u8, + checksum_is_valid: bool, + line: [u8; 256], +} + +#[derive(Clone, Copy)] +pub struct TimeAndPos { + pub system_time: u32, + pub unix_time: u32, + pub latitude: i32, // Positive means north, negative means south. + pub longitude: i32, // Positive means east, negative means west. +} + +impl TimeAndPos { + pub fn new() -> TimeAndPos { + TimeAndPos { + system_time: 0, + unix_time: 0, + latitude: 0, + longitude: 0, + } + } +} + +fn to_lower(c: u8) -> u8 { + c | 0x20 +} + +fn try_read() -> Option { + extern { + static mut uart0_rx_buf: Ringbuf; + } + + unsafe { + if uart0_rx_buf.is_empty() { + None + } else { + Some(uart0_rx_buf.read()) + } + } +} + +fn parse_coordinate(s: &[u8]) -> i32 { + // Find the position of the decimal separator for the minutes. + let dot_position = s.iter().enumerate().find(|(_, &c)| { + c == b'.' + }).and_then(|(i, _)| { + Some(i) + }); + + if dot_position.is_none() { + return 0; + } + + // Minutes take two digits before the decimal separator. + let num_degree_digits = dot_position.unwrap() - 2; + + let mut degrees = 0; + + for c in s[0..num_degree_digits].iter() { + degrees *= 10; + degrees += (c - b'0') as i32; + } + + let mut minutes = 0; + + for c in s[num_degree_digits..dot_position.unwrap()].iter() { + minutes *= 10; + minutes += (c - b'0') as i32; + } + + minutes += degrees * 60; + + for c in s[dot_position.unwrap() + 1..].iter() { + minutes *= 10; + minutes += (c - b'0') as i32; + } + + minutes +} + +// Only works for 2016 onwards. +fn is_leap_year(year: u32) -> bool { + (year & 3) == 0 +} + +fn parse_d2(s: &[u8]) -> u32 { + let a = (s[0] - b'0') as u32; + let b = (s[1] - b'0') as u32; + + (a * 10) + b +} + +struct FieldIter<'a> { + command: &'a [u8], + offset: usize, +} + +impl<'a> Iterator for FieldIter<'a> { + type Item = &'a [u8]; + + fn next(&mut self) -> Option<&'a [u8]> { + let delimiter = b','; + + if self.offset == self.command.len() { + self.offset += 1; + + return Some(b""); + } + + if self.offset > self.command.len() { + return None; + } + + if self.command[self.offset] == delimiter { + self.offset += 1; + + return Some(b""); + } + + let mut start : Option = None; + + // Find the start of the substring. + for o in self.offset..self.command.len() { + if self.command[o] != delimiter { + start = Some(o); + break + } + } + + start.and_then(|start2| { + let mut end : Option = None; + + // Find the end of the substring. + for o in start2..self.command.len() { + if self.command[o] == delimiter { + break + } + + end = Some(o); + } + + end.and_then(|end2| { + let end3 = end2 + 1; + + self.offset = end3 + 1; + + Some(&self.command[start2..end3]) + }) + }) + } +} + +const SUM_EXTRA_DAYS_UNTIL_MONTH : [u32; 11] = [ + 0x03, 0x03, 0x06, 0x08, 0x0b, 0x0d, + 0x10, 0x13, 0x15, 0x18, 0x1a, +]; + +impl Gps { + pub fn new() -> Gps { + Gps { + unix_date: 0, + offset: 0, + state: ParseState::Start, + checksum: 0, + checksum_is_valid: false, + line: [0; 256], + } + } + + pub fn update(&mut self, tap: &mut TimeAndPos) -> bool { + let hexdigits = b"0123456789abcdef"; + + while let Some(received) = try_read() { + match self.state { + ParseState::Start => { + if received == b'$' { + self.state = ParseState::InPacket; + self.offset = 0; + self.checksum = 0x00; + } + }, + ParseState::InPacket => { + if received == b'*' { + self.state = ParseState::InChecksum1; + self.checksum_is_valid = true; + + // Check if message fits in buffer. We subtract one + // because we will need to write the sentinel later. + } else if self.offset == self.line.len() - 1 { + self.state = ParseState::Start; + } else { + self.checksum ^= received; + self.line[self.offset] = received; + self.offset += 1; + } + }, + ParseState::InChecksum1 => { + let expected = hexdigits[(self.checksum >> 4) as usize & 0xf]; + + if to_lower(received) != expected { + self.checksum_is_valid = false; + } + + self.state = ParseState::InChecksum2; + }, + ParseState::InChecksum2 => { + let expected = hexdigits[(self.checksum >> 0) as usize & 0xf]; + + if to_lower(received) != expected { + self.checksum_is_valid = false; + } + + self.state = ParseState::Start; + + if self.checksum_is_valid { + // Terminate and dispatch. + self.line[self.offset] = b'\0'; + + if self.parse(tap) { + return true; + } + } + }, + } + } + + false + } + + fn parse(&mut self, tap: &mut TimeAndPos) -> bool { + let line_copy : [u8; 256] = self.line; + + let mut field_iter = FieldIter { + command: &line_copy[0..self.offset], + offset: 0, + }; + + match field_iter.next() { + Some(b"GPRMC") => { + self.parse_rmc(&mut field_iter); + false + }, + Some(b"GPGGA") => { + self.parse_gga(&mut field_iter, tap) + }, + _ => { + // Ignore. + false + } + } + } + + fn parse_rmc(&mut self, field_iter: &mut FieldIter) { + let f8 = field_iter.nth(8); + + if let Some(date) = f8 { + if date.len() >= 6 { + let days = parse_d2(&date[0..2]); + let months = parse_d2(&date[2..4]); + let years = 2000 + parse_d2(&date[4..6]); + + let years_in_epoch = years - 1970; + + // This only works until 2100. + let mut unix_date = + (years_in_epoch * 365) + ((years_in_epoch + 1) / 4); + + // Add the number of days passed in this year, + // excluding the current month. + if months >= 2 { + unix_date += 28 * (months - 1); + unix_date += SUM_EXTRA_DAYS_UNTIL_MONTH[(months - 2) as usize]; + } + + if months >= 3 && is_leap_year(years) { + unix_date += 1; + } + + if days > 0 { + unix_date += days - 1; + } + + self.unix_date = unix_date; + } + } + } + + fn parse_gga(&mut self, field_iter: &mut FieldIter, tap: &mut TimeAndPos) -> bool { + let f0 = field_iter.next(); + let f1 = field_iter.next(); + let f2 = field_iter.next(); + let f3 = field_iter.next(); + let f4 = field_iter.next(); + let f5 = field_iter.next(); + + match (f0, f1, f2, f3, f4, f5) { + (Some(utc_time), + Some(latitude), + Some(north_south), + Some(longitude), + Some(east_west), + Some(pos_fix_indicator), + ) => { + if self.unix_date == 0 { + return false; + } + + if pos_fix_indicator != b"1" { + return false; + } + + if utc_time.len() < 6 { + return false; + } + + let hours = parse_d2(&utc_time[0..2]); + let minutes = parse_d2(&utc_time[2..4]); + let seconds = parse_d2(&utc_time[4..6]); + + let mut unix_time = self.unix_date; + unix_time *= 24; // Days to hours. + unix_time += hours; + unix_time *= 60; // Hours to minutes. + unix_time += minutes; + unix_time *= 60; // Minutes to seconds. + unix_time += seconds; + + tap.system_time = systick::now(); + tap.unix_time = unix_time; + tap.latitude = parse_coordinate(latitude); + tap.longitude = parse_coordinate(longitude); + + if north_south == b"S" { + tap.latitude = -tap.latitude; + } + + if east_west == b"W" { + tap.longitude = -tap.longitude; + } + + true + }, + _ => { + false + } + } + } +} diff --git a/src/common/lib.rs b/src/common/lib.rs index 2f7d894..2cae469 100644 --- a/src/common/lib.rs +++ b/src/common/lib.rs @@ -40,6 +40,7 @@ pub mod buffer; pub mod usb_serial; pub mod screen; pub mod display; +pub mod gps; use core::panic::PanicInfo; -- 2.30.2