--- /dev/null
+/*
+ * 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<u8> {
+ 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<usize> = 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<usize> = 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
+ }
+ }
+ }
+}
projects / gps-watch.git / commitdiff