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 }