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