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 systick;
  25
  26 enum ParseState {
  27     Start,
  28     InPacket,
  29     InChecksum1,
  30     InChecksum2,
  31 }
  32
  33 pub struct Gps {
  34     unix_date: u32, // Number of days passed since 1970-01-01
  35     offset: usize,
  36     state: ParseState,
  37     checksum: u8,
  38     checksum_is_valid: bool,
  39     line: [u8; 256],
  40 }
  41
  42 #[derive(Clone, Copy)]
  43 pub struct TimeAndPos {
  44     pub system_time: u32,
  45     pub unix_time: u32,
  46     pub latitude: i32, // Positive means north, negative means south.
  47     pub longitude: i32, // Positive means east, negative means west.
  48 }
  49
  50 impl TimeAndPos {
  51     pub fn new() -> TimeAndPos {
  52         TimeAndPos {
  53             system_time: 0,
  54             unix_time: 0,
  55             latitude: 0,
  56             longitude: 0,
  57         }
  58     }
  59 }
  60
  61 fn to_lower(c: u8) -> u8 {
  62     c | 0x20
  63 }
  64
  65 fn parse_coordinate(s: &[u8]) -> i32 {
  66     // Find the position of the decimal separator for the minutes.
  67     let dot_position = s.iter().enumerate().find(|(_, &c)| {
  68         c == b'.'
  69     }).and_then(|(i, _)| {
  70         Some(i)
  71     });
  72
  73     if dot_position.is_none() {
  74         return 0;
  75     }
  76
  77     // Minutes take two digits before the decimal separator.
  78     let num_degree_digits = dot_position.unwrap() - 2;
  79
  80     let mut degrees = 0;
  81
  82     for c in s[0..num_degree_digits].iter() {
  83         degrees *= 10;
  84         degrees += (c - b'0') as i32;
  85     }
  86
  87     let mut minutes = 0;
  88
  89     for c in s[num_degree_digits..dot_position.unwrap()].iter() {
  90         minutes *= 10;
  91         minutes += (c - b'0') as i32;
  92     }
  93
  94     minutes += degrees * 60;
  95
  96     for c in s[dot_position.unwrap() + 1..].iter() {
  97         minutes *= 10;
  98         minutes += (c - b'0') as i32;
  99     }
 100
 101     minutes
 102 }
 103
 104 // Only works for 2016 onwards.
 105 fn is_leap_year(year: u32) -> bool {
 106     (year & 3) == 0
 107 }
 108
 109 fn parse_d2(s: &[u8]) -> u32 {
 110     let a = (s[0] - b'0') as u32;
 111     let b = (s[1] - b'0') as u32;
 112
 113     (a * 10) + b
 114 }
 115
 116 struct FieldIter<'a> {
 117     command: &'a [u8],
 118     offset: usize,
 119 }
 120
 121 impl<'a> Iterator for FieldIter<'a> {
 122     type Item = &'a [u8];
 123
 124     fn next(&mut self) -> Option<&'a [u8]> {
 125         let delimiter = b',';
 126
 127         if self.offset == self.command.len() {
 128             self.offset += 1;
 129
 130             return Some(b"");
 131         }
 132
 133         if self.offset > self.command.len() {
 134             return None;
 135         }
 136
 137         if self.command[self.offset] == delimiter {
 138             self.offset += 1;
 139
 140             return Some(b"");
 141         }
 142
 143         let mut start : Option<usize> = None;
 144
 145         // Find the start of the substring.
 146         for o in self.offset..self.command.len() {
 147             if self.command[o] != delimiter {
 148                 start = Some(o);
 149                 break
 150             }
 151         }
 152
 153         start.and_then(|start2| {
 154             let mut end : Option<usize> = None;
 155
 156             // Find the end of the substring.
 157             for o in start2..self.command.len() {
 158                 if self.command[o] == delimiter {
 159                     break
 160                 }
 161
 162                 end = Some(o);
 163             }
 164
 165             end.and_then(|end2| {
 166                 let end3 = end2 + 1;
 167
 168                 self.offset = end3 + 1;
 169
 170                 Some(&self.command[start2..end3])
 171             })
 172         })
 173     }
 174 }
 175
 176 const SUM_EXTRA_DAYS_UNTIL_MONTH : [u32; 11] = [
 177     0x03, 0x03, 0x06, 0x08, 0x0b, 0x0d,
 178     0x10, 0x13, 0x15, 0x18, 0x1a,
 179 ];
 180
 181 impl Gps {
 182     pub fn new() -> Gps {
 183         Gps {
 184             unix_date: 0,
 185             offset: 0,
 186             state: ParseState::Start,
 187             checksum: 0,
 188             checksum_is_valid: false,
 189             line: [0; 256],
 190         }
 191     }
 192
 193     pub fn update<F>(&mut self, tap: &mut TimeAndPos, mut read_func: F) -> bool
 194                      where F: FnMut() -> Option<u8>
 195     {
 196         let hexdigits = b"0123456789abcdef";
 197
 198         while let Some(received) = read_func() {
 199             if received == b'$' {
 200                 self.state = ParseState::InPacket;
 201                 self.offset = 0;
 202                 self.checksum = 0x00;
 203
 204                 continue;
 205             }
 206
 207             match self.state {
 208                 ParseState::Start => {
 209                     // Empty.
 210                 },
 211                 ParseState::InPacket => {
 212                     if received == b'*' {
 213                         self.state = ParseState::InChecksum1;
 214                         self.checksum_is_valid = true;
 215
 216                     // Check if message fits in buffer. We subtract one
 217                     // because we will need to write the sentinel later.
 218                     } else if self.offset == self.line.len() - 1 {
 219                         self.state = ParseState::Start;
 220                     } else {
 221                         self.checksum ^= received;
 222                         self.line[self.offset] = received;
 223                         self.offset += 1;
 224                     }
 225                 },
 226                 ParseState::InChecksum1 => {
 227                     let expected = hexdigits[(self.checksum >> 4) as usize & 0xf];
 228
 229                     if to_lower(received) != expected {
 230                         self.checksum_is_valid = false;
 231                     }
 232
 233                     self.state = ParseState::InChecksum2;
 234                 },
 235                 ParseState::InChecksum2 => {
 236                     let expected = hexdigits[(self.checksum >> 0) as usize & 0xf];
 237
 238                     if to_lower(received) != expected {
 239                         self.checksum_is_valid = false;
 240                     }
 241
 242                     self.state = ParseState::Start;
 243
 244                     if self.checksum_is_valid {
 245                         // Terminate and dispatch.
 246                         self.line[self.offset] = b'\0';
 247
 248                         if self.parse(tap) {
 249                             return true;
 250                         }
 251                     }
 252                 },
 253             }
 254         }
 255
 256         false
 257     }
 258
 259     fn parse(&mut self, tap: &mut TimeAndPos) -> bool {
 260         let line_copy : [u8; 256] = self.line;
 261
 262         let mut field_iter = FieldIter {
 263             command: &line_copy[0..self.offset],
 264             offset: 0,
 265         };
 266
 267         match field_iter.next() {
 268             Some(b"GPRMC") => {
 269                 self.parse_rmc(&mut field_iter);
 270                 false
 271             },
 272             Some(b"GPGGA") => {
 273                 self.parse_gga(&mut field_iter, tap)
 274             },
 275             _ => {
 276                 // Ignore.
 277                 false
 278             }
 279         }
 280     }
 281
 282     fn parse_rmc(&mut self, field_iter: &mut FieldIter) {
 283         let f8 = field_iter.nth(8);
 284
 285         if let Some(date) = f8 {
 286             if date.len() >= 6 {
 287                 let days = parse_d2(&date[0..2]);
 288                 let months = parse_d2(&date[2..4]);
 289                 let years = 2000 + parse_d2(&date[4..6]);
 290
 291                 let years_in_epoch = years - 1970;
 292
 293                 // This only works until 2100.
 294                 let mut unix_date =
 295                     (years_in_epoch * 365) + ((years_in_epoch + 1) / 4);
 296
 297                 // Add the number of days passed in this year,
 298                 // excluding the current month.
 299                 if months >= 2 {
 300                     unix_date += 28 * (months - 1);
 301                     unix_date += SUM_EXTRA_DAYS_UNTIL_MONTH[(months - 2) as usize];
 302                 }
 303
 304                 if months >= 3 && is_leap_year(years) {
 305                     unix_date += 1;
 306                 }
 307
 308                 if days > 0 {
 309                     unix_date += days - 1;
 310                 }
 311
 312                 self.unix_date = unix_date;
 313             }
 314         }
 315     }
 316
 317     fn parse_gga(&mut self, field_iter: &mut FieldIter, tap: &mut TimeAndPos) -> bool {
 318         let f0 = field_iter.next();
 319         let f1 = field_iter.next();
 320         let f2 = field_iter.next();
 321         let f3 = field_iter.next();
 322         let f4 = field_iter.next();
 323         let f5 = field_iter.next();
 324
 325         match (f0, f1, f2, f3, f4, f5) {
 326             (Some(utc_time),
 327              Some(latitude),
 328              Some(north_south),
 329              Some(longitude),
 330              Some(east_west),
 331              Some(pos_fix_indicator),
 332             ) => {
 333                 if self.unix_date == 0 {
 334                     return false;
 335                 }
 336
 337                 match pos_fix_indicator {
 338                     b"1" => {
 339                         // Valid standard GPS fix (low resolution).
 340                     },
 341                     b"2" => {
 342                         // Valid differential GPS fix (high resolution).
 343                     },
 344                     _ => {
 345                         return false;
 346                     }
 347                 }
 348
 349                 if utc_time.len() < 6 {
 350                     return false;
 351                 }
 352
 353                 let hours = parse_d2(&utc_time[0..2]);
 354                 let minutes = parse_d2(&utc_time[2..4]);
 355                 let seconds = parse_d2(&utc_time[4..6]);
 356
 357                 let mut unix_time = self.unix_date;
 358                 unix_time *= 24; // Days to hours.
 359                 unix_time += hours;
 360                 unix_time *= 60; // Hours to minutes.
 361                 unix_time += minutes;
 362                 unix_time *= 60; // Minutes to seconds.
 363                 unix_time += seconds;
 364
 365                 tap.system_time = systick::now();
 366                 tap.unix_time = unix_time;
 367                 tap.latitude = parse_coordinate(latitude);
 368                 tap.longitude = parse_coordinate(longitude);
 369
 370                 if north_south == b"S" {
 371                     tap.latitude = -tap.latitude;
 372                 }
 373
 374                 if east_west == b"W" {
 375                     tap.longitude = -tap.longitude;
 376                 }
 377
 378                 true
 379             },
 380             _ => {
 381                 false
 382             }
 383         }
 384     }
 385 }