From: Tilman Sauerbeck <tilman@code-monkey.de>
Date: Mon, 30 Dec 2019 09:41:00 +0000 (+0100)
Subject: common: Add the gps module.
X-Git-Url: http://git.code-monkey.de/?a=commitdiff_plain;h=0ed02a94b60d886b568f4803a7ec09eef29ad1ca;p=gps-watch.git

common: Add the gps module.
---

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<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
+            }
+        }
+    }
+}
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;