common: Store GPS coordinates in radians, too.

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

diff --git a/src/common/gps.rs b/src/common/gps.rs
index 178e55198879edffbd1fb2dec11947579320daa6..7e635fce6f37854a39ef9f81fe66dad60f3a02b4 100644 (file)
--- a/src/common/gps.rs
+++ b/src/common/gps.rs
@@ -21,8 +21,11 @@
  * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  */
 
+use fixed15_49;
 use systick;
 
+type Fixed = fixed15_49::Fixed15_49;
+
 enum ParseState {
     Start,
     InPacket,
@@ -45,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 {
@@ -54,6 +59,8 @@ impl TimeAndPos {
             unix_time: 0,
             latitude: 0,
             longitude: 0,
+            latitude_rad: Fixed::from_i64(0),
+            longitude_rad: Fixed::from_i64(0),
         }
     }
 }
@@ -101,6 +108,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
@@ -366,13 +413,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