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             if received == b'$' {
 213                 self.state = ParseState::InPacket;
 214                 self.offset = 0;
 215                 self.checksum = 0x00;
 216
 217                 continue;
 218             }
 219
 220             match self.state {
 221                 ParseState::Start => {
 222                     // Empty.
 223                 },
 224                 ParseState::InPacket => {
 225                     if received == b'*' {
 226                         self.state = ParseState::InChecksum1;
 227                         self.checksum_is_valid = true;
 228
 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;
 233                     } else {
 234                         self.checksum ^= received;
 235                         self.line[self.offset] = received;
 236                         self.offset += 1;
 237                     }
 238                 },
 239                 ParseState::InChecksum1 => {
 240                     let expected = hexdigits[(self.checksum >> 4) as usize & 0xf];
 241
 242                     if to_lower(received) != expected {
 243                         self.checksum_is_valid = false;
 244                     }
 245
 246                     self.state = ParseState::InChecksum2;
 247                 },
 248                 ParseState::InChecksum2 => {
 249                     let expected = hexdigits[(self.checksum >> 0) as usize & 0xf];
 250
 251                     if to_lower(received) != expected {
 252                         self.checksum_is_valid = false;
 253                     }
 254
 255                     self.state = ParseState::Start;
 256
 257                     if self.checksum_is_valid {
 258                         // Terminate and dispatch.
 259                         self.line[self.offset] = b'\0';
 260
 261                         if self.parse(tap) {
 262                             return true;
 263                         }
 264                     }
 265                 },
 266             }
 267         }
 268
 269         false
 270     }
 271
 272     fn parse(&mut self, tap: &mut TimeAndPos) -> bool {
 273         let line_copy : [u8; 256] = self.line;
 274
 275         let mut field_iter = FieldIter {
 276             command: &line_copy[0..self.offset],
 277             offset: 0,
 278         };
 279
 280         match field_iter.next() {
 281             Some(b"GPRMC") => {
 282                 self.parse_rmc(&mut field_iter);
 283                 false
 284             },
 285             Some(b"GPGGA") => {
 286                 self.parse_gga(&mut field_iter, tap)
 287             },
 288             _ => {
 289                 // Ignore.
 290                 false
 291             }
 292         }
 293     }
 294
 295     fn parse_rmc(&mut self, field_iter: &mut FieldIter) {
 296         let f8 = field_iter.nth(8);
 297
 298         if let Some(date) = f8 {
 299             if date.len() >= 6 {
 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]);
 303
 304                 let years_in_epoch = years - 1970;
 305
 306                 // This only works until 2100.
 307                 let mut unix_date =
 308                     (years_in_epoch * 365) + ((years_in_epoch + 1) / 4);
 309
 310                 // Add the number of days passed in this year,
 311                 // excluding the current month.
 312                 if months >= 2 {
 313                     unix_date += 28 * (months - 1);
 314                     unix_date += SUM_EXTRA_DAYS_UNTIL_MONTH[(months - 2) as usize];
 315                 }
 316
 317                 if months >= 3 && is_leap_year(years) {
 318                     unix_date += 1;
 319                 }
 320
 321                 if days > 0 {
 322                     unix_date += days - 1;
 323                 }
 324
 325                 self.unix_date = unix_date;
 326             }
 327         }
 328     }
 329
 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();
 337
 338         match (f0, f1, f2, f3, f4, f5) {
 339             (Some(utc_time),
 340              Some(latitude),
 341              Some(north_south),
 342              Some(longitude),
 343              Some(east_west),
 344              Some(pos_fix_indicator),
 345             ) => {
 346                 if self.unix_date == 0 {
 347                     return false;
 348                 }
 349
 350                 if pos_fix_indicator != b"1" {
 351                     return false;
 352                 }
 353
 354                 if utc_time.len() < 6 {
 355                     return false;
 356                 }
 357
 358                 let hours = parse_d2(&utc_time[0..2]);
 359                 let minutes = parse_d2(&utc_time[2..4]);
 360                 let seconds = parse_d2(&utc_time[4..6]);
 361
 362                 let mut unix_time = self.unix_date;
 363                 unix_time *= 24; // Days to hours.
 364                 unix_time += hours;
 365                 unix_time *= 60; // Hours to minutes.
 366                 unix_time += minutes;
 367                 unix_time *= 60; // Minutes to seconds.
 368                 unix_time += seconds;
 369
 370                 tap.system_time = systick::now();
 371                 tap.unix_time = unix_time;
 372                 tap.latitude = parse_coordinate(latitude);
 373                 tap.longitude = parse_coordinate(longitude);
 374
 375                 if north_south == b"S" {
 376                     tap.latitude = -tap.latitude;
 377                 }
 378
 379                 if east_west == b"W" {
 380                     tap.longitude = -tap.longitude;
 381                 }
 382
 383                 true
 384             },
 385             _ => {
 386                 false
 387             }
 388         }
 389     }
 390 }