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