From: Tilman Sauerbeck Date: Sun, 19 Jan 2020 11:20:16 +0000 (+0100) Subject: common: Store GPS coordinates in radians, too. X-Git-Url: http://git.code-monkey.de/?a=commitdiff_plain;h=c4d55664636e0df02fa19c079c4d9367e2d25917;p=gps-watch.git 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 178e551..7e635fc 100644 --- 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 b43e080..1cff088 100644 --- 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 cf4984a..7b0cdbd 100644 --- 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);