2 * Copyright (c) 2019 Tilman Sauerbeck (tilman at code-monkey de)
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:
12 * The above copyright notice and this permission notice shall be
13 * included in all copies or substantial portions of the Software.
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.
27 type Fixed = fixed15_49::Fixed15_49;
37 unix_date: u32, // Number of days passed since 1970-01-01
41 checksum_is_valid: bool,
45 #[derive(Clone, Copy)]
46 pub struct TimeAndPos {
49 pub latitude_deg: i32, // Positive means north, negative means south.
50 pub longitude_deg: 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.
56 pub fn new() -> TimeAndPos {
62 latitude_rad: Fixed::from_i64(0),
63 longitude_rad: Fixed::from_i64(0),
67 pub fn distance_cm(&self, other: &TimeAndPos) -> i64 {
68 let a_lat = self.latitude_rad;
69 let a_lon = self.longitude_rad;
71 let b_lat = other.latitude_rad;
72 let b_lon = other.longitude_rad;
74 let x = (b_lon - a_lon) * ((b_lat + a_lat) / Fixed::from_i64(2)).cos();
75 let y = b_lat - a_lat;
77 let d_km = (x * x + y * y).sqrt() * Fixed::from_i64(6371);
79 let hundred = Fixed::from_i64(100);
80 let thousand = Fixed::from_i64(1000);
82 // Convert from km to cm.
83 (d_km * thousand * hundred).to_i64()
87 fn to_lower(c: u8) -> u8 {
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)| {
95 }).and_then(|(i, _)| {
99 if dot_position.is_none() {
103 // Minutes take two digits before the decimal separator.
104 let num_degree_digits = dot_position.unwrap() - 2;
108 for c in s[0..num_degree_digits].iter() {
110 degrees += (c - b'0') as i32;
115 for c in s[num_degree_digits..dot_position.unwrap()].iter() {
117 minutes += (c - b'0') as i32;
120 minutes += degrees * 60;
122 for c in s[dot_position.unwrap() + 1..].iter() {
124 minutes += (c - b'0') as i32;
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)| {
134 }).and_then(|(i, _)| {
138 if dot_position_o.is_none() {
139 return Fixed::from_i64(0);
142 let dot_position = dot_position_o.unwrap();
144 // Minutes take two digits before the decimal separator.
145 let num_degree_digits = dot_position - 2;
147 let degrees = s[0..num_degree_digits].iter().fold(0, |d, c| {
148 (d * 10) + (c - b'0') as i32
151 let minutes = s[num_degree_digits..dot_position].iter().fold(0, |d, c| {
152 (d * 10) + (c - b'0') as i32
155 let decimal_minutes = s[dot_position + 1..].iter().fold(0, |d, c| {
156 (d * 10) + (c - b'0') as i32
159 let mut result = Fixed::from_i64(decimal_minutes.into());
160 result /= Fixed::from_i64(10000);
162 result += Fixed::from_i64(minutes.into());
163 result /= Fixed::from_i64(60);
165 result += Fixed::from_i64(degrees.into());
170 // Only works for 2016 onwards.
171 fn is_leap_year(year: u32) -> bool {
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;
182 struct FieldIter<'a> {
187 impl<'a> Iterator for FieldIter<'a> {
188 type Item = &'a [u8];
190 fn next(&mut self) -> Option<&'a [u8]> {
191 let delimiter = b',';
193 if self.offset == self.command.len() {
199 if self.offset > self.command.len() {
203 if self.command[self.offset] == delimiter {
209 let mut start : Option<usize> = None;
211 // Find the start of the substring.
212 for o in self.offset..self.command.len() {
213 if self.command[o] != delimiter {
219 start.and_then(|start2| {
220 let mut end : Option<usize> = None;
222 // Find the end of the substring.
223 for o in start2..self.command.len() {
224 if self.command[o] == delimiter {
231 end.and_then(|end2| {
234 self.offset = end3 + 1;
236 Some(&self.command[start2..end3])
242 const SUM_EXTRA_DAYS_UNTIL_MONTH : [u32; 11] = [
243 0x03, 0x03, 0x06, 0x08, 0x0b, 0x0d,
244 0x10, 0x13, 0x15, 0x18, 0x1a,
248 pub fn new() -> Gps {
252 state: ParseState::Start,
254 checksum_is_valid: false,
259 pub fn update<F>(&mut self, tap: &mut TimeAndPos, mut read_func: F) -> bool
260 where F: FnMut() -> Option<u8>
262 let hexdigits = b"0123456789abcdef";
264 while let Some(received) = read_func() {
265 if received == b'$' {
266 self.state = ParseState::InPacket;
268 self.checksum = 0x00;
274 ParseState::Start => {
277 ParseState::InPacket => {
278 if received == b'*' {
279 self.state = ParseState::InChecksum1;
280 self.checksum_is_valid = true;
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;
287 self.checksum ^= received;
288 self.line[self.offset] = received;
292 ParseState::InChecksum1 => {
293 let expected = hexdigits[(self.checksum >> 4) as usize & 0xf];
295 if to_lower(received) != expected {
296 self.checksum_is_valid = false;
299 self.state = ParseState::InChecksum2;
301 ParseState::InChecksum2 => {
302 let expected = hexdigits[(self.checksum >> 0) as usize & 0xf];
304 if to_lower(received) != expected {
305 self.checksum_is_valid = false;
308 self.state = ParseState::Start;
310 if self.checksum_is_valid {
311 // Terminate and dispatch.
312 self.line[self.offset] = b'\0';
325 fn parse(&mut self, tap: &mut TimeAndPos) -> bool {
326 let line_copy : [u8; 256] = self.line;
328 let mut field_iter = FieldIter {
329 command: &line_copy[0..self.offset],
333 match field_iter.next() {
335 self.parse_rmc(&mut field_iter);
339 self.parse_gga(&mut field_iter, tap)
348 fn parse_rmc(&mut self, field_iter: &mut FieldIter) {
349 let f8 = field_iter.nth(8);
351 if let Some(date) = f8 {
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]);
357 let years_in_epoch = years - 1970;
359 // This only works until 2100.
361 (years_in_epoch * 365) + ((years_in_epoch + 1) / 4);
363 // Add the number of days passed in this year,
364 // excluding the current month.
366 unix_date += 28 * (months - 1);
367 unix_date += SUM_EXTRA_DAYS_UNTIL_MONTH[(months - 2) as usize];
370 if months >= 3 && is_leap_year(years) {
375 unix_date += days - 1;
378 self.unix_date = unix_date;
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();
391 match (f0, f1, f2, f3, f4, f5) {
397 Some(pos_fix_indicator),
399 if self.unix_date == 0 {
403 match pos_fix_indicator {
405 // Valid standard GPS fix (low resolution).
408 // Valid differential GPS fix (high resolution).
415 if utc_time.len() < 6 {
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]);
423 let mut unix_time = self.unix_date;
424 unix_time *= 24; // Days to hours.
426 unix_time *= 60; // Hours to minutes.
427 unix_time += minutes;
428 unix_time *= 60; // Minutes to seconds.
429 unix_time += seconds;
431 tap.system_time = systick::now();
432 tap.unix_time = unix_time;
433 tap.latitude_deg = parse_coordinate(latitude);
434 tap.longitude_deg = parse_coordinate(longitude);
435 tap.latitude_rad = parse_coordinate_q(latitude).to_radians();
436 tap.longitude_rad = parse_coordinate_q(longitude).to_radians();
438 if north_south == b"S" {
439 tap.latitude_deg = -tap.latitude_deg;
440 tap.latitude_rad = -tap.latitude_rad;
443 if east_west == b"W" {
444 tap.longitude_deg = -tap.longitude_deg;
445 tap.longitude_rad = -tap.longitude_rad;