common: Store GPS coordinates in radians, too.

Use the Q15.49 fixed point format to store those values.

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

diff --git a/test/gps_test.rs b/test/gps_test.rs
index b43e080c918251d514922ddb6b7e8ea726540a0a..1cff088019cb99a020477a8f0e2cd045d3dd7cb7 100644 (file)
--- a/test/gps_test.rs
+++ b/test/gps_test.rs
@@ -84,4 +84,6 @@ $GPGGA,110338.000,1234.5678,N,12345.6789,E,\
     assert_eq!(1477998218, tap.unix_time);
     assert_eq!(7545678, tap.latitude);
     assert_eq!(74256789, tap.longitude);
+    assert_eq!(0.21949487565883447, tap.latitude_rad.to_f32());
+    assert_eq!(2.160042433347846, tap.longitude_rad.to_f32());
 }

diff --git a/test/logger_test.rs b/test/logger_test.rs
index cf4984a3d4dd562ec7d642311cc039061d993e60..7b0cdbd1391455d5361d9cc0672f2c4d6a84ad32 100644 (file)
--- a/test/logger_test.rs
+++ b/test/logger_test.rs
@@ -24,10 +24,13 @@
 use std::io::{Read, Write};
 use std::os::unix::io::FromRawFd;
 use common::buffer::{Buffer, BufferUserData};
+use common::fixed15_49;
 use common::gps;
 use common::storage::{Storage, Error};
 use common::logger::{MEMORY_SIZE, Logger, Error as LoggerError};
 
+type Fixed = fixed15_49::Fixed15_49;
+
 struct FakeStorage {
     expected: Box<[u8]>,
     actual: Box<[u8]>,
@@ -115,6 +118,8 @@ fn first_recording() {
         unix_time: 1478026311,
         latitude: 0x73234e,
         longitude: 0x73234f,
+        latitude_rad: Fixed::from_f32(12.57613).to_radians(),
+        longitude_rad: Fixed::from_f32(12.576131666666667).to_radians(),
     };
 
     let recording_id = logger.start_recording(&tap);
@@ -169,6 +174,8 @@ fn second_recording() {
         unix_time: 1478026312,
         latitude: 0x73234e,
         longitude: 0x73234f,
+        latitude_rad: Fixed::from_f32(12.57613).to_radians(),
+        longitude_rad: Fixed::from_f32(12.576131666666667).to_radians(),
     };
 
     let recording_id = logger.start_recording(&tap);
@@ -214,6 +221,8 @@ fn multi_sector_recording() {
         unix_time: 1578425250,
         latitude: 0x73234e,
         longitude: 0x73234f,
+        latitude_rad: Fixed::from_f32(12.57613).to_radians(),
+        longitude_rad: Fixed::from_f32(12.576131666666667).to_radians(),
     };
 
     let recording_id = logger.start_recording(&tap);
@@ -227,6 +236,10 @@ fn multi_sector_recording() {
             unix_time: prev_tap.unix_time + 1,
             latitude: prev_tap.latitude + 1,
             longitude: prev_tap.longitude + 1,
+            latitude_rad: Fixed::from_f32(
+                (prev_tap.latitude + 1) as f32 / 600000.0).to_radians(),
+            longitude_rad: Fixed::from_f32(
+                (prev_tap.longitude + 1) as f32 / 600000.0).to_radians(),
         };
 
         logger.log(&prev_tap, &tap);
@@ -324,6 +337,8 @@ fn get_recording_valid() {
         unix_time: 1478026311,
         latitude: 0x73234e,
         longitude: 0x73234f,
+        latitude_rad: Fixed::from_f32(12.57613).to_radians(),
+        longitude_rad: Fixed::from_f32(12.576131666666667).to_radians(),
     };
 
     logger.start_recording(&tap0);
@@ -333,6 +348,8 @@ fn get_recording_valid() {
         unix_time: 1478026311 + 1,
         latitude: 0x73234e + 5,
         longitude: 0x73234f + 5,
+        latitude_rad: Fixed::from_f32(12.576138333333333).to_radians(),
+        longitude_rad: Fixed::from_f32(12.57614).to_radians(),
     };
 
     logger.log(&tap0, &tap1);
@@ -342,6 +359,8 @@ fn get_recording_valid() {
         unix_time: 1478026311 + 2,
         latitude: 0x73234e + 10,
         longitude: 0x73234f + 10,
+        latitude_rad: Fixed::from_f32(12.576146666666666).to_radians(),
+        longitude_rad: Fixed::from_f32(12.576148333333334).to_radians(),
     };
 
     logger.log(&tap1, &tap2);