common: Implement TimeAndPos::distance_cm().

[gps-watch.git] / src / common / gps.rs

diff --git a/src/common/gps.rs b/src/common/gps.rs
index b0539173272bbe5b5a0fc833deb8eb8b4899f278..85e1dd4a07d5d609fbdf3c1a59140c086e03c672 100644 (file)
--- a/src/common/gps.rs
+++ b/src/common/gps.rs
@@ -21,9 +21,11 @@
  * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  */
 
-use ringbuf::Ringbuf;
+use fixed15_49;
 use systick;
 
+type Fixed = fixed15_49::Fixed15_49;
+
 enum ParseState {
     Start,
     InPacket,
@@ -46,6 +48,8 @@ pub struct TimeAndPos {
     pub unix_time: u32,
     pub latitude: i32, // Positive means north, negative means south.
     pub longitude: i32, // Positive means east, negative means west.
+    pub latitude_rad: Fixed, // Positive means north, negative means south.
+    pub longitude_rad: Fixed, // Positive means east, negative means west.
 }
 
 impl TimeAndPos {
@@ -55,28 +59,35 @@ impl TimeAndPos {
             unix_time: 0,
             latitude: 0,
             longitude: 0,
+            latitude_rad: Fixed::from_i64(0),
+            longitude_rad: Fixed::from_i64(0),
         }
     }
-}
 
-fn to_lower(c: u8) -> u8 {
-    c | 0x20
-}
+    pub fn distance_cm(&self, other: &TimeAndPos) -> i64 {
+        let a_lat = self.latitude_rad;
+        let a_lon = self.longitude_rad;
 
-fn try_read() -> Option<u8> {
-    extern {
-        static mut uart0_rx_buf: Ringbuf;
-    }
+        let b_lat = other.latitude_rad;
+        let b_lon = other.longitude_rad;
 
-    unsafe {
-        if uart0_rx_buf.is_empty() {
-            None
-        } else {
-            Some(uart0_rx_buf.read())
-        }
+        let x = (b_lon - a_lon) * ((b_lat + a_lat) / Fixed::from_i64(2)).cos();
+        let y = b_lat - a_lat;
+
+        let d_km = (x * x + y * y).sqrt() * Fixed::from_i64(6371);
+
+        let hundred = Fixed::from_i64(100);
+        let thousand = Fixed::from_i64(1000);
+
+        // Convert from km to cm.
+        (d_km * thousand * hundred).to_i64()
     }
 }
 
+fn to_lower(c: u8) -> u8 {
+    c | 0x20
+}
+
 fn parse_coordinate(s: &[u8]) -> i32 {
     // Find the position of the decimal separator for the minutes.
     let dot_position = s.iter().enumerate().find(|(_, &c)| {
@@ -116,6 +127,46 @@ fn parse_coordinate(s: &[u8]) -> i32 {
     minutes
 }
 
+fn parse_coordinate_q(s: &[u8]) -> Fixed {
+    // Find the position of the decimal separator for the minutes.
+    let dot_position_o = s.iter().enumerate().find(|(_, &c)| {
+        c == b'.'
+    }).and_then(|(i, _)| {
+        Some(i)
+    });
+
+    if dot_position_o.is_none() {
+        return Fixed::from_i64(0);
+    }
+
+    let dot_position = dot_position_o.unwrap();
+
+    // Minutes take two digits before the decimal separator.
+    let num_degree_digits = dot_position - 2;
+
+    let degrees = s[0..num_degree_digits].iter().fold(0, |d, c| {
+        (d * 10) + (c - b'0') as i32
+    });
+
+    let minutes = s[num_degree_digits..dot_position].iter().fold(0, |d, c| {
+        (d * 10) + (c - b'0') as i32
+    });
+
+    let decimal_minutes = s[dot_position + 1..].iter().fold(0, |d, c| {
+        (d * 10) + (c - b'0') as i32
+    });
+
+    let mut result = Fixed::from_i64(decimal_minutes.into());
+    result /= Fixed::from_i64(10000);
+
+    result += Fixed::from_i64(minutes.into());
+    result /= Fixed::from_i64(60);
+
+    result += Fixed::from_i64(degrees.into());
+
+    return result;
+}
+
 // Only works for 2016 onwards.
 fn is_leap_year(year: u32) -> bool {
     (year & 3) == 0
@@ -205,10 +256,12 @@ impl Gps {
         }
     }
 
-    pub fn update(&mut self, tap: &mut TimeAndPos) -> bool {
+    pub fn update<F>(&mut self, tap: &mut TimeAndPos, mut read_func: F) -> bool
+                     where F: FnMut() -> Option<u8>
+    {
         let hexdigits = b"0123456789abcdef";
 
-        while let Some(received) = try_read() {
+        while let Some(received) = read_func() {
             if received == b'$' {
                 self.state = ParseState::InPacket;
                 self.offset = 0;
@@ -347,8 +400,16 @@ impl Gps {
                     return false;
                 }
 
-                if pos_fix_indicator != b"1" {
-                    return false;
+                match pos_fix_indicator {
+                    b"1" => {
+                        // Valid standard GPS fix (low resolution).
+                    },
+                    b"2" => {
+                        // Valid differential GPS fix (high resolution).
+                    },
+                    _ => {
+                        return false;
+                    }
                 }
 
                 if utc_time.len() < 6 {
@@ -371,13 +432,17 @@ impl Gps {
                 tap.unix_time = unix_time;
                 tap.latitude = parse_coordinate(latitude);
                 tap.longitude = parse_coordinate(longitude);
+                tap.latitude_rad = parse_coordinate_q(latitude).to_radians();
+                tap.longitude_rad = parse_coordinate_q(longitude).to_radians();
 
                 if north_south == b"S" {
                     tap.latitude = -tap.latitude;
+                    tap.latitude_rad = -tap.latitude_rad;
                 }
 
                 if east_west == b"W" {
                     tap.longitude = -tap.longitude;
+                    tap.longitude_rad = -tap.longitude_rad;
                 }
 
                 true