X-Git-Url: http://git.code-monkey.de/?a=blobdiff_plain;f=src%2Fcommon%2Fgps.rs;h=bda3670c25e5ca540da84ebd48a9abeb73fce177;hb=8131a71527b5279bca9b80aac37596f6264ea2a1;hp=7e635fce6f37854a39ef9f81fe66dad60f3a02b4;hpb=c4d55664636e0df02fa19c079c4d9367e2d25917;p=gps-watch.git

diff --git a/src/common/gps.rs b/src/common/gps.rs
index 7e635fc..bda3670 100644
--- a/src/common/gps.rs
+++ b/src/common/gps.rs
@@ -46,8 +46,8 @@ pub struct Gps {
 pub struct TimeAndPos {
     pub system_time: u32,
     pub unix_time: u32,
-    pub latitude: i32, // Positive means north, negative means south.
-    pub longitude: i32, // Positive means east, negative means west.
+    pub latitude_deg: i32, // Positive means north, negative means south.
+    pub longitude_deg: 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.
 }
@@ -57,12 +57,31 @@ impl TimeAndPos {
         TimeAndPos {
             system_time: 0,
             unix_time: 0,
-            latitude: 0,
-            longitude: 0,
+            latitude_deg: 0,
+            longitude_deg: 0,
             latitude_rad: Fixed::from_i64(0),
             longitude_rad: Fixed::from_i64(0),
         }
     }
+
+    pub fn distance_cm(&self, other: &TimeAndPos) -> i64 {
+        let a_lat = self.latitude_rad;
+        let a_lon = self.longitude_rad;
+
+        let b_lat = other.latitude_rad;
+        let b_lon = other.longitude_rad;
+
+        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 {
@@ -411,18 +430,18 @@ impl Gps {
 
                 tap.system_time = systick::now();
                 tap.unix_time = unix_time;
-                tap.latitude = parse_coordinate(latitude);
-                tap.longitude = parse_coordinate(longitude);
+                tap.latitude_deg = parse_coordinate(latitude);
+                tap.longitude_deg = 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_deg = -tap.latitude_deg;
                     tap.latitude_rad = -tap.latitude_rad;
                 }
 
                 if east_west == b"W" {
-                    tap.longitude = -tap.longitude;
+                    tap.longitude_deg = -tap.longitude_deg;
                     tap.longitude_rad = -tap.longitude_rad;
                 }