diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 371ddcb..1cd5352 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -65,7 +65,7 @@ jobs: # MSRV - os: ubuntu-latest binary_target: x86_64-unknown-linux-gnu - toolchain: 1.85.0 + toolchain: 1.87.0 steps: - uses: actions/checkout@v5 with: diff --git a/Cargo.toml b/Cargo.toml index f5c4cc1..dc3fda2 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,5 +1,3 @@ [workspace] -members = [ - "swiftnav", - # "swiftnav-sys" -] +resolver = "3" +members = ["swiftnav"] diff --git a/rustfmt.toml b/rustfmt.toml new file mode 100644 index 0000000..c9a1eda --- /dev/null +++ b/rustfmt.toml @@ -0,0 +1,8 @@ +max_width = 100 +wrap_comments = true +comment_width = 100 +format_strings = true +edition = "2024" +imports_granularity = "Crate" +group_imports = "StdExternalCrate" +style_edition = "2024" diff --git a/swiftnav/Cargo.toml b/swiftnav/Cargo.toml index 5d27706..7880145 100644 --- a/swiftnav/Cargo.toml +++ b/swiftnav/Cargo.toml @@ -2,26 +2,26 @@ name = "swiftnav" version = "0.10.0" authors = ["Swift Navigation "] -edition = "2018" +edition = "2024" description = "GNSS positioning and related utilities" readme = "README.md" repository = "https://github.com/swift-nav/swiftnav-rs" license = "LGPL-3.0" -rust-version = "1.85.0" +rust-version = "1.87.0" [dependencies] -rustversion = "1.0" -chrono = { version = "0.4", optional = true } -strum = { version = "0.27", features = ["derive"] } -nalgebra = "0.33" -thiserror = "2.0" +bon = "3.8.1" +rustversion = "1.0.22" +chrono = { version = "0.4.42" } +strum = { version = "0.27.2", features = ["derive"] } +nalgebra = "0.34.1" +thiserror = "2.0.17" [dev-dependencies] float_eq = "1.0.1" -proptest = "1.5" +proptest = "1.9.0" # This tells docs.rs to include the katex header for math formatting # To do this locally [package.metadata.docs.rs] -rustdoc-args = [ "--html-in-header", "katex-header.html" ] - +rustdoc-args = ["--html-in-header", "katex-header.html"] diff --git a/swiftnav/src/coords/ecef.rs b/swiftnav/src/coords/ecef.rs index 9774214..8ef2038 100644 --- a/swiftnav/src/coords/ecef.rs +++ b/swiftnav/src/coords/ecef.rs @@ -1,6 +1,7 @@ -use nalgebra::Vector3; use std::ops::{Add, AddAssign, Mul, MulAssign, Sub, SubAssign}; +use nalgebra::Vector3; + use crate::{ coords::{AzimuthElevation, Ellipsoid, LLHDegrees, LLHRadians, NED, WGS84}, math, diff --git a/swiftnav/src/coords/hemisphere.rs b/swiftnav/src/coords/hemisphere.rs new file mode 100644 index 0000000..fd49bd3 --- /dev/null +++ b/swiftnav/src/coords/hemisphere.rs @@ -0,0 +1,43 @@ +use core::fmt; + +pub enum LatitudinalHemisphere { + North, + South, +} + +impl fmt::Display for LatitudinalHemisphere { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + match self { + LatitudinalHemisphere::North => write!(f, "N"), + LatitudinalHemisphere::South => write!(f, "S"), + } + } +} + +pub enum LongitudinalHemisphere { + East, + West, +} + +impl fmt::Display for LongitudinalHemisphere { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + match self { + LongitudinalHemisphere::East => write!(f, "E"), + LongitudinalHemisphere::West => write!(f, "W"), + } + } +} + +pub enum Hemisphere { + Latitudinal(LatitudinalHemisphere), + Longitudinal(LongitudinalHemisphere), +} + +impl fmt::Display for Hemisphere { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + match self { + Hemisphere::Latitudinal(hemisphere) => write!(f, "{hemisphere}"), + Hemisphere::Longitudinal(hemisphere) => write!(f, "{hemisphere}"), + } + } +} diff --git a/swiftnav/src/coords/llh.rs b/swiftnav/src/coords/llh.rs index 174fbb2..6e8e61d 100644 --- a/swiftnav/src/coords/llh.rs +++ b/swiftnav/src/coords/llh.rs @@ -1,9 +1,12 @@ -use super::{Ellipsoid, ECEF, WGS84}; use nalgebra::Vector3; +use super::{ECEF, Ellipsoid, WGS84}; +use crate::coords::{LatitudinalHemisphere, LongitudinalHemisphere}; + /// WGS84 geodetic coordinates (Latitude, Longitude, Height), with angles in degrees. /// -/// Internally stored as an array of 3 [f64](std::f64) values: latitude, longitude, and height above the ellipsoid in meters +/// Internally stored as an array of 3 [f64](std::f64) values: latitude, longitude, and height above +/// the ellipsoid in meters #[derive(Copy, Clone, Debug, PartialEq, PartialOrd, Default)] pub struct LLHDegrees(Vector3); @@ -44,12 +47,64 @@ impl LLHDegrees { self.0.x } + /// Get the latitude in degrees and decimal minutes + #[must_use] + pub fn latitude_degree_decimal_minutes(&self) -> (u16, f64) { + let lat = self.latitude(); + + #[expect( + clippy::cast_possible_truncation, + clippy::cast_sign_loss, + reason = "We are using trunc() and abs() already to remove the fractional part" + )] + let degrees = lat.trunc().abs() as u16; + let minutes = lat.fract().abs() * 60.0; + + (degrees, minutes) + } + + /// Get the latitudinal hemisphere + #[must_use] + pub fn latitudinal_hemisphere(&self) -> LatitudinalHemisphere { + if self.latitude() >= 0.0 { + LatitudinalHemisphere::North + } else { + LatitudinalHemisphere::South + } + } + /// Get the longitude component #[must_use] pub fn longitude(&self) -> f64 { self.0.y } + /// Get the latitude in degrees and decimal minutes + #[must_use] + pub fn longitude_degree_decimal_minutes(&self) -> (u16, f64) { + let lon = self.longitude(); + + #[expect( + clippy::cast_possible_truncation, + clippy::cast_sign_loss, + reason = "We are using trunc() and abs() already to remove the fractional part" + )] + let degrees = lon.trunc().abs() as u16; + let minutes = lon.fract().abs() * 60.0; + + (degrees, minutes) + } + + /// Get the longitudinal hemisphere + #[must_use] + pub fn longitudinal_hemisphere(&self) -> LongitudinalHemisphere { + if self.longitude() >= 0.0 { + LongitudinalHemisphere::East + } else { + LongitudinalHemisphere::West + } + } + /// Get the height component #[must_use] pub fn height(&self) -> f64 { @@ -135,7 +190,8 @@ impl AsMut> for LLHDegrees { /// WGS84 geodetic coordinates (Latitude, Longitude, Height), with angles in radians. /// -/// Internally stored as an array of 3 [f64](std::f64) values: latitude, longitude, and height above the ellipsoid in meters +/// Internally stored as an array of 3 [f64](std::f64) values: latitude, longitude, and height above +/// the ellipsoid in meters #[derive(Copy, Clone, Debug, PartialEq, PartialOrd, Default)] pub struct LLHRadians(Vector3); diff --git a/swiftnav/src/coords/mod.rs b/swiftnav/src/coords/mod.rs index 198f035..f8bab7c 100644 --- a/swiftnav/src/coords/mod.rs +++ b/swiftnav/src/coords/mod.rs @@ -44,11 +44,10 @@ //! the more common algortihms based on the Newton-Raphson method. //! //! ## References -//! * "A comparison of methods used in rectangular to Geodetic Coordinates -//! Transformations", Burtch R. R. (2006), American Congress for Surveying -//! and Mapping Annual Conference. Orlando, Florida. -//! * "Transformation from Cartesian to Geodetic Coordinates Accelerated by -//! Halley’s Method", T. Fukushima (2006), Journal of Geodesy. +//! * "A comparison of methods used in rectangular to Geodetic Coordinates Transformations", Burtch +//! R. R. (2006), American Congress for Surveying and Mapping Annual Conference. Orlando, Florida. +//! * "Transformation from Cartesian to Geodetic Coordinates Accelerated by Halley’s Method", T. +//! Fukushima (2006), Journal of Geodesy. //! //! # Examples //! @@ -76,22 +75,27 @@ mod ecef; mod ellipsoid; +mod hemisphere; mod llh; mod ned; pub use ecef::*; pub use ellipsoid::*; +pub use hemisphere::*; pub use llh::*; +use nalgebra::Vector2; pub use ned::*; use crate::{reference_frame::ReferenceFrame, time::GpsTime}; -use nalgebra::Vector2; -/// WGS84 local horizontal coordinates consisting of an Azimuth and Elevation, with angles stored as radians +/// WGS84 local horizontal coordinates consisting of an Azimuth and Elevation, with angles stored as +/// radians /// -/// Azimuth can range from $0$ to $2\pi$. North has an azimuth of $0$, east has an azimuth of $\frac{\pi}{2}$ +/// Azimuth can range from $0$ to $2\pi$. North has an azimuth of $0$, east has an azimuth of +/// $\frac{\pi}{2}$ /// -/// Elevation can range from $-\frac{\pi}{2}$ to $\frac{\pi}{2}$. Up has an elevation of $\frac{\pi}{2}$, down an elevation of $-\frac{\pi}{2}$ +/// Elevation can range from $-\frac{\pi}{2}$ to $\frac{\pi}{2}$. Up has an elevation of +/// $\frac{\pi}{2}$, down an elevation of $-\frac{\pi}{2}$ #[derive(Copy, Clone, Debug, PartialEq, PartialOrd, Default)] pub struct AzimuthElevation(Vector2); @@ -290,11 +294,10 @@ impl Coordinate { #[cfg(test)] mod tests { use float_eq::assert_float_eq; - - use crate::time::UtcTime; + use proptest::prelude::*; use super::*; - use proptest::prelude::*; + use crate::time::UtcTime; /* Maximum allowable error in quantities with units of length (in meters). */ const MAX_DIST_ERROR_M: f64 = 1e-6; @@ -535,11 +538,11 @@ mod tests { let lat_err = llh_input.latitude() - llh_output.latitude(); assert!(lat_err.abs() < MAX_ANGLE_ERROR_RAD, - "Converting random WGS84 LLH to ECEF and back again does not return the original values. Initial: {:?}, ECEF: {:?}, Final: {:?}, Lat error (rad): {}", llh_input, ecef, llh_output, lat_err); + "Converting random WGS84 LLH to ECEF and back again does not return the original values. Initial: {llh_input:?}, ECEF: {ecef:?}, Final: {llh_output:?}, Lat error (rad): {lat_err}"); let lon_err = llh_input.longitude() - llh_output.longitude(); assert!(lon_err.abs() < MAX_ANGLE_ERROR_RAD, - "Converting random WGS84 LLH to ECEF and back again does not return the original values. Initial: {:?}, ECEF: {:?}, Final: {:?}, Lon error (rad): {}", llh_input, ecef, llh_output, lon_err); + "Converting random WGS84 LLH to ECEF and back again does not return the original values. Initial: {llh_input:?}, ECEF: {ecef:?}, Final: {llh_output:?}, Lon error (rad): {lon_err}"); let hgt_err = llh_input.height() - llh_output.height(); assert!(hgt_err.abs() < MAX_DIST_ERROR_M, diff --git a/swiftnav/src/edc.rs b/swiftnav/src/edc.rs index c593e3b..5d8f841 100644 --- a/swiftnav/src/edc.rs +++ b/swiftnav/src/edc.rs @@ -326,9 +326,10 @@ pub fn compute_crc24q(buf: &[u8], initial_value: u32) -> u32 { #[cfg(test)] mod tests { - use super::*; use proptest::prelude::*; + use super::*; + const TEST_DATA: &[u8] = "123456789".as_bytes(); /// Helper function to append a CRC-24Q value as 3 bytes (big-endian) to a buffer @@ -353,15 +354,13 @@ mod tests { let crc = compute_crc24q(&TEST_DATA[0..0], 0); assert!( crc == 0, - "CRC of empty buffer with starting value 0 should be 0, not {}", - crc + "CRC of empty buffer with starting value 0 should be 0, not {crc}", ); let crc = compute_crc24q(&TEST_DATA[0..0], 22); assert!( crc == 22, - "CRC of empty buffer with starting value 22 should be 22, not {}", - crc + "CRC of empty buffer with starting value 22 should be 22, not {crc}", ); /* Test value taken from python crcmod package tests, see: @@ -369,8 +368,7 @@ mod tests { let crc = compute_crc24q(TEST_DATA, 0x00B7_04CE); assert!( crc == 0x0021_CF02, - "CRC of \"123456789\" with init value 0xB704CE should be 0x21CF02, not {}", - crc + "CRC of \"123456789\" with init value 0xB704CE should be 0x21CF02, not {crc}", ); } diff --git a/swiftnav/src/lib.rs b/swiftnav/src/lib.rs index 2dd39f4..5ae6d3d 100644 --- a/swiftnav/src/lib.rs +++ b/swiftnav/src/lib.rs @@ -58,6 +58,7 @@ pub mod coords; pub mod edc; mod math; +pub mod nmea; pub mod reference_frame; pub mod signal; pub mod time; diff --git a/swiftnav/src/math.rs b/swiftnav/src/math.rs index 47977fb..38026d8 100644 --- a/swiftnav/src/math.rs +++ b/swiftnav/src/math.rs @@ -10,11 +10,7 @@ /// We define a `const` max function since [`std::cmp::max`] isn't `const` pub(crate) const fn compile_time_max_u16(a: u16, b: u16) -> u16 { - if b < a { - a - } else { - b - } + if b < a { a } else { b } } /// Computes the square root of a given number at compile time using the Newton-Raphson method. @@ -35,7 +31,8 @@ pub(crate) const fn compile_time_max_u16(a: u16, b: u16) -> u16 { /// # Notes /// /// - This function is marked as `const`, allowing it to be evaluated at compile time. -/// - The algorithm iteratively refines the approximation of the square root until the result stabilizes. +/// - The algorithm iteratively refines the approximation of the square root until the result +/// stabilizes. #[expect(clippy::many_single_char_names, reason = "It's math, whatyagonnado?")] pub(crate) const fn compile_time_sqrt(s: f64) -> f64 { assert!( @@ -57,7 +54,8 @@ pub(crate) const fn compile_time_sqrt(s: f64) -> f64 { x } -/// Calculate the rotation matrix for rotating between an [`crate::coords::ECEF`] and [`crate::coords::NED`] frames +/// Calculate the rotation matrix for rotating between an [`crate::coords::ECEF`] and +/// [`crate::coords::NED`] frames #[must_use] pub(crate) fn ecef2ned_matrix(llh: crate::coords::LLHRadians) -> nalgebra::Matrix3 { let sin_lat = llh.latitude().sin(); @@ -80,10 +78,11 @@ pub(crate) fn ecef2ned_matrix(llh: crate::coords::LLHRadians) -> nalgebra::Matri #[cfg(test)] mod tests { - use super::*; use float_eq::assert_float_eq; use proptest::prelude::*; + use super::*; + proptest! { #![proptest_config(ProptestConfig::with_cases(1000))] diff --git a/swiftnav/src/nmea/checksum.rs b/swiftnav/src/nmea/checksum.rs new file mode 100644 index 0000000..5595eba --- /dev/null +++ b/swiftnav/src/nmea/checksum.rs @@ -0,0 +1,105 @@ +// Copyright (c) 2025 Swift Navigation Inc. +// Contact: Swift Navigation +// +// This source is subject to the license found in the file 'LICENSE' which must +// be be distributed together with this source. All other rights reserved. +// +// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, +// EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + +fn u8_to_nibbles(byte: u8) -> (u8, u8) { + // The high nibble is obtained by shifting the byte 4 bits to the right. + // This discards the lower 4 bits and moves the upper 4 bits into the lower 4 bit positions. + let high_nibble = (byte >> 4) & 0x0F; + + // The low nibble is obtained by masking the byte with 0x0F (binary 0000_1111). + // This keeps only the lower 4 bits and sets the upper 4 bits to zero. + let low_nibble = byte & 0x0F; + + (high_nibble, low_nibble) +} + +/// Convert a nibble (4 bits) to its ASCII character representation +fn u8_to_ascii_char(nibble: u8) -> char { + if nibble <= 0x9 { + (nibble + b'0') as char + } else { + (nibble - 10 + b'A') as char + } +} + +// Calculate the NMEA checksum for a given sentence +// https://forum.arduino.cc/t/nmea-checksums-explained/1046083 +#[must_use] +pub fn calculate_checksum(sentence: &str) -> String { + let mut checksum = 0; + + for (i, byte) in sentence.bytes().enumerate() { + // Skip the starting '$' + if i == 0 && byte == b'$' { + continue; + } + + if byte == b'*' { + break; + } + + checksum ^= byte; + } + + let (nibble1, nibble2) = u8_to_nibbles(checksum); + + let char1 = u8_to_ascii_char(nibble1); + let char2 = u8_to_ascii_char(nibble2); + + format!("{char1}{char2}") +} + +#[cfg(test)] +mod tests { + #[test] + fn test_calculate_checksum() { + let sentence = "GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,"; + let checksum = super::calculate_checksum(sentence); + assert_eq!(checksum, "47"); + + let sentence = "$GPGLL,5057.970,N,00146.110,E,142451,A"; + let checksum = super::calculate_checksum(sentence); + assert_eq!(checksum, "27"); + + let sentence = "$GPVTG,089.0,T,,,15.2,N,,"; + let checksum = super::calculate_checksum(sentence); + assert_eq!(checksum, "7F"); + } + + #[test] + fn calculate_checksum_ignores_dollar_and_asterisk_tails() { + // NOTE(ted): All of these examples should produce the same checksum + + // check with '$' + let sentence = "$GPGGA,0189.00,34.052200,N,-118.243700,W,2,8,1.2,0.0,M,1.00,2,42"; + let checksum = super::calculate_checksum(sentence); + assert_eq!(checksum, "37"); + + //check with '$' and '*' + let sentence = "$GPGGA,0189.00,34.052200,N,-118.243700,W,2,8,1.2,0.0,M,1.00,2,42*"; + let checksum = super::calculate_checksum(sentence); + assert_eq!(checksum, "37"); + + //check with '$' and '*' and fake checksum + let sentence = "$GPGGA,0189.00,34.052200,N,-118.243700,W,2,8,1.2,0.0,M,1.00,2,42*00"; + let checksum = super::calculate_checksum(sentence); + assert_eq!(checksum, "37"); + + //check '*' + let sentence = "GPGGA,0189.00,34.052200,N,-118.243700,W,2,8,1.2,0.0,M,1.00,2,42*"; + let checksum = super::calculate_checksum(sentence); + assert_eq!(checksum, "37"); + + //check '*' and fake checksum + let sentence = "GPGGA,0189.00,34.052200,N,-118.243700,W,2,8,1.2,0.0,M,1.00,2,42*00"; + let checksum = super::calculate_checksum(sentence); + assert_eq!(checksum, "37"); + } +} diff --git a/swiftnav/src/nmea/gga.rs b/swiftnav/src/nmea/gga.rs new file mode 100644 index 0000000..cd710c8 --- /dev/null +++ b/swiftnav/src/nmea/gga.rs @@ -0,0 +1,230 @@ +// Copyright (c) 2025 Swift Navigation Inc. +// Contact: Swift Navigation +// +// This source is subject to the license found in the file 'LICENSE' which must +// be be distributed together with this source. All other rights reserved. +// +// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, +// EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + +use std::{ + fmt::{self}, + time::Duration, +}; + +use bon::Builder; +use chrono::{DateTime, Timelike, Utc}; + +use crate::{ + coords::LLHDegrees, + nmea::{self, Source}, +}; + +/// Quality of GPS solution +#[derive(Debug, PartialEq, Clone, Copy, Default)] +pub enum GPSQuality { + /// Fix not available or invalid + #[default] + NoFix, + /// GPS SPS Mode, fix valid + SPS, + /// Differential GPS, SPS Mode, fix valid + DGPS, + /// GPS PPS (precise positioning service, military encrypted signals), fix valid + PPS, + /// RTK (real time kinematic). System used in RTK mode with fixed integers + RTK, + /// Float RTK, satelite system used in RTK mode, floating integers + FRTK, + /// Estimated (dead reckoning) mode. + DeadReckoning, + /// Manual input mode + Manual, + /// Simulated mode + Simulated, +} + +impl fmt::Display for GPSQuality { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + match self { + GPSQuality::NoFix => write!(f, "0"), + GPSQuality::SPS => write!(f, "1"), + GPSQuality::DGPS => write!(f, "2"), + GPSQuality::PPS => write!(f, "3"), + GPSQuality::RTK => write!(f, "4"), + GPSQuality::FRTK => write!(f, "5"), + GPSQuality::DeadReckoning => write!(f, "6"), + GPSQuality::Manual => write!(f, "7"), + GPSQuality::Simulated => write!(f, "8"), + } + } +} + +#[derive(thiserror::Error, Debug)] +pub enum GGAParseError { + #[error("Invalid time format")] + InvalidTimeFormat, + + #[error("Invalid or missing GPS quality")] + InvalidGPSQuality, +} + +/// Global Positioning System Fix Data including time, position and fix related data for a GNSS +/// receiver +#[derive(Debug, PartialEq, Clone, Builder)] +pub struct GGA { + #[builder(default)] + pub source: Source, + /// Time of fix in UTC. + #[builder(default = Utc::now())] + pub time: DateTime, + /// Latitude, longitude and height in degrees. + pub llh: LLHDegrees, + /// Quality of GPS solution. + #[builder(default)] + pub gps_quality: GPSQuality, + /// Sattelites in use + pub sat_in_use: Option, + /// Horizontal dilusion of presicion + pub hdop: Option, + /// The difference between reference ellipsoid surface and mean-sea-level. + pub geoidal_separation: Option, + /// DGPS data age + pub age_dgps: Option, + /// ID of reference DGPS station used for fix + pub reference_station_id: Option, +} + +impl GGA { + /// converts the GGA struct into an NMEA sentence + /// + /// + /// + /// ```text + /// 1 2 3 4 5 6 7 8 9 10 | 12 13 14 15 + /// | | | | | | | | | | | | | | | + /// $--GGA,hhmmss.ss,ddmm.mm,a,ddmm.mm,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx*hh + /// ``` + #[must_use] + pub fn to_sentence(&self) -> String { + let talker_id = self.source.to_nmea_talker_id(); + // NOTE(ted): We are formatting here a bit strange because for some ungodly reason, + // chrono chose not to allow for abitrary fractional seconds precision when formatting + // Construct timestamp in HHMMSS.SS format + let hour = self.time.hour(); + let minute = self.time.minute(); + let second = f64::from(self.time.second()); + let second_fracs = f64::from(self.time.nanosecond()) / 1_000_000_000.0; + + let timestamp = format!("{hour}{minute}{:.2}", second + second_fracs); + + let (lat_deg, lat_mins) = self.llh.latitude_degree_decimal_minutes(); + let lat_hemisphere = self.llh.latitudinal_hemisphere(); + + let (lon_deg, lon_mins) = self.llh.longitude_degree_decimal_minutes(); + let lon_hemisphere = self.llh.longitudinal_hemisphere(); + + let gps_quality = self.gps_quality; + + let sat_in_use = self.sat_in_use.map_or(String::new(), |sat| sat.to_string()); + + let hdop = self.hdop.map_or(String::new(), |hdop| format!("{hdop:.1}")); + + // NOTE(ted): This is actually not the right value to use, however, we don't really use + // height for finding information like nearest station so it's ok to use for now + let height = "0.0"; + + let age_dgps = self + .age_dgps + .map_or(String::new(), |age| format!("{:.1}", age.as_secs_f64())); + + let geoidal_separation = self + .geoidal_separation + .map_or(String::new(), |sep| format!("{sep:.2}")); + + let reference_station_id = self + .reference_station_id + .map_or(String::new(), |id| id.to_string()); + + let sentence = format!( + "{talker_id}GGA,{timestamp},{lat_deg:02}{lat_mins:010.7},{lat_hemisphere},{lon_deg:\ + 03}{lon_mins:010.7},{lon_hemisphere},{gps_quality},{sat_in_use},{hdop},{height:.6},M,\ + {geoidal_separation},{age_dgps:.1},{reference_station_id}", + ); + + let checksum = nmea::calculate_checksum(&sentence); + + let sentence = format!("${sentence}*{checksum}\r\n"); + + sentence + } +} + +#[cfg(test)] +mod test { + use super::*; + + #[test] + fn gga_can_be_turned_into_an_nmea_sentence() { + let gga = GGA::builder() + .sat_in_use(12) + .time(DateTime::from_timestamp(1_761_351_489, 0).unwrap()) + .gps_quality(GPSQuality::SPS) + .hdop(0.9) + .llh(super::LLHDegrees::new(37.7749, -122.4194, 10.0)) + .build(); + + let sentence = gga.to_sentence(); + + assert_eq!( + sentence, + "$GPGGA,0189.00,3746.4940000,N,12225.1640000,W,1,12,0.9,0.0,M,,,*01\r\n" + ); + } + + #[test] + fn gga_with_dgps_can_be_turned_into_an_nmea_sentence() { + let gga = GGA::builder() + .sat_in_use(8) + .time(DateTime::from_timestamp(1_761_351_489, 0).unwrap()) + .hdop(1.2) + .llh(super::LLHDegrees::new(34.0522, -18.2437, 15.0)) + .gps_quality(GPSQuality::DGPS) + .age_dgps(Duration::from_secs_f64(2.5)) + .geoidal_separation(1.0) + .reference_station_id(42) + .build(); + + let sentence = gga.to_sentence(); + + assert_eq!( + sentence, + "$GPGGA,0189.00,3403.1320000,N,01814.6220000,W,2,8,1.2,0.0,M,1.00,2,42*1C\r\n" + ); + } + + #[test] + fn gga_sentence_is_always_less_than_82_characters() { + // we are going to set some very large decimal places and the highest possible values in + // terms of character count to ensure our sentence is always below 82 characters + let gga = GGA::builder() + .sat_in_use(12) + .time(DateTime::from_timestamp(1_761_351_489, 0).unwrap()) + .hdop(1.210_123_1) + .llh(super::LLHDegrees::new( + -90.000_000_001, + -180.000_000_000_1, + 1_000.000_000_000, + )) + .gps_quality(GPSQuality::DGPS) + .age_dgps(Duration::from_secs_f64(2.500_000_000_001)) + .geoidal_separation(1.00) + .reference_station_id(1023) // 1023 is the max value for a 4 digit station ID + .build(); + + let sentence = gga.to_sentence(); + + assert!(sentence.len() < 82); + } +} diff --git a/swiftnav/src/nmea/mod.rs b/swiftnav/src/nmea/mod.rs new file mode 100644 index 0000000..dfdce89 --- /dev/null +++ b/swiftnav/src/nmea/mod.rs @@ -0,0 +1,21 @@ +// Copyright (c) 2025 Swift Navigation Inc. +// Contact: Swift Navigation +// +// This source is subject to the license found in the file 'LICENSE' which must +// be be distributed together with this source. All other rights reserved. +// +// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, +// EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + +//! This module contains National Marine Electronics Association (NMEA) related structures and +//! formatting utilities. Notably, it contains (or eventually will contain) structures related to +//! NMEA sentences and parsing/serialization of those sentences. + +mod checksum; +mod gga; +mod source; + +pub use checksum::*; +pub use gga::*; +pub use source::*; diff --git a/swiftnav/src/reference_frame/mod.rs b/swiftnav/src/reference_frame/mod.rs index 208e11a..9a5f08b 100644 --- a/swiftnav/src/reference_frame/mod.rs +++ b/swiftnav/src/reference_frame/mod.rs @@ -9,9 +9,9 @@ // WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. //! Geodetic reference frame transformations //! -//! Transform coordinates between geodetic reference frames using time-dependent Helmert transformations. -//! Supports both global frames (ITRF series) and regional frames (ETRF, NAD83, etc.) with runtime -//! parameter loading. +//! Transform coordinates between geodetic reference frames using time-dependent Helmert +//! transformations. Supports both global frames (ITRF series) and regional frames (ETRF, NAD83, +//! etc.) with runtime parameter loading. //! //! # Key Concepts //! @@ -68,14 +68,16 @@ //! repo.add_transformation(custom_transform); //! ``` -use crate::coords::{Coordinate, ECEF}; -use nalgebra::{Matrix3, Vector3}; use std::{ collections::{HashMap, HashSet, VecDeque}, fmt, }; + +use nalgebra::{Matrix3, Vector3}; use strum::{Display, EnumIter, EnumString}; +use crate::coords::{Coordinate, ECEF}; + mod params; /// Geodetic reference frame identifiers @@ -195,10 +197,10 @@ impl PartialEq for &ReferenceFrame { /// # Parameter Units /// /// Input parameters are stored in standard geodetic units: -/// - **Translation** (`tx`, `ty`, `tz`): millimeters (mm) +/// - **Translation** (`tx`, `ty`, `tz`): millimeters (mm) /// - **Translation rates** (`tx_dot`, `ty_dot`, `tz_dot`): mm/year /// - **Scale** (`s`): parts per billion (ppb) -/// - **Scale rate** (`s_dot`): ppb/year +/// - **Scale rate** (`s_dot`): ppb/year /// - **Rotation** (`rx`, `ry`, `rz`): milliarcseconds (mas) /// - **Rotation rates** (`rx_dot`, `ry_dot`, `rz_dot`): mas/year /// - **Reference epoch** (`epoch`): decimal years @@ -258,7 +260,8 @@ impl TimeDependentHelmertParams { } } - /// Reverses the transformation. Since this is a linear transformation we simply negate all terms + /// Reverses the transformation. Since this is a linear transformation we simply negate all + /// terms #[must_use] pub fn invert(mut self) -> Self { self.t *= -1.0; @@ -574,10 +577,12 @@ fn builtin_transformations() -> Vec { #[cfg(test)] mod tests { - use super::*; + use std::str::FromStr; + use float_eq::assert_float_eq; use params::TRANSFORMATIONS; - use std::str::FromStr; + + use super::*; #[expect(clippy::too_many_lines)] #[test] diff --git a/swiftnav/src/signal/code.rs b/swiftnav/src/signal/code.rs index 2d90814..1339dc1 100644 --- a/swiftnav/src/signal/code.rs +++ b/swiftnav/src/signal/code.rs @@ -8,7 +8,7 @@ // EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED // WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. -use super::{consts, Constellation}; +use super::{Constellation, consts}; /// Code identifiers #[derive( diff --git a/swiftnav/src/signal/mod.rs b/swiftnav/src/signal/mod.rs index e1a3c0d..3722519 100644 --- a/swiftnav/src/signal/mod.rs +++ b/swiftnav/src/signal/mod.rs @@ -16,7 +16,8 @@ //! This module provides: //! - [`Constellation`] - Representing the supporting GNSS constellations //! - [`Code`] - Representing the codes broadcast from the GNSS satellites -//! - [`GnssSignal`] - Represents a [`Code`] broadcast by a specific satellite, using the satellite PRN as the identifier +//! - [`GnssSignal`] - Represents a [`Code`] broadcast by a specific satellite, using the satellite +//! PRN as the identifier //! //! # Examples //! @@ -37,9 +38,10 @@ mod code; mod constellation; pub mod consts; +use std::fmt; + pub use code::*; pub use constellation::*; -use std::fmt; /// GNSS Signal identifier #[derive(Debug, Copy, Clone, Ord, PartialOrd, Eq, PartialEq, Hash)] diff --git a/swiftnav/src/time/gnss.rs b/swiftnav/src/time/gnss.rs index 4b9b9eb..ddb4f6c 100644 --- a/swiftnav/src/time/gnss.rs +++ b/swiftnav/src/time/gnss.rs @@ -7,12 +7,13 @@ // THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, // EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED // WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. -use crate::time::{consts, UtcParams, UtcTime, MJD, UTC_LEAPS, WEEK}; use std::{ ops::{Add, AddAssign, Sub, SubAssign}, time::Duration, }; +use crate::time::{MJD, UTC_LEAPS, UtcParams, UtcTime, WEEK, consts}; + /// Representation of GPS Time #[derive(Debug, Copy, Clone)] pub struct GpsTime { @@ -189,7 +190,7 @@ impl GpsTime { assert!(utc_time.hour() == 23); assert!(utc_time.minute() == 59); assert!(utc_time.seconds_int() == 59); - /* add the extra second back in*/ + /* add the extra second back in */ utc_time.add_second(); } diff --git a/swiftnav/src/time/mjd.rs b/swiftnav/src/time/mjd.rs index 197b34e..c2565dd 100644 --- a/swiftnav/src/time/mjd.rs +++ b/swiftnav/src/time/mjd.rs @@ -15,9 +15,10 @@ reason = "We need to review the math for overflows" )] -use crate::time::{consts, GpsTime, UtcParams, UtcTime}; use std::time::Duration; +use crate::time::{GpsTime, UtcParams, UtcTime, consts}; + /// Representation of modified julian dates (MJD) #[derive(Copy, Clone, Debug, PartialEq, PartialOrd)] pub struct MJD(f64); @@ -51,7 +52,13 @@ impl MJD { #[must_use] pub fn from_parts(year: u16, month: u8, day: u8, hour: u8, minute: u8, seconds: f64) -> MJD { // Enforce our assumption that the date is just the MJD period - assert!(year > 1858 || (year == 1858 && month > 11) || (year == 1858 && month == 11 && day >= 17), "Attempting to convert a date prior to the start of the Modified Julian Date system ({}-{}-{}T{}:{}:{}Z", year, month, day, hour, minute, seconds); + assert!( + year > 1858 + || (year == 1858 && month > 11) + || (year == 1858 && month == 11 && day >= 17), + "Attempting to convert a date prior to the start of the Modified Julian Date system \ + ({year}-{month}-{day}T{hour}:{minute}:{seconds}Z" + ); let full_days = 367 * i64::from(year) - 7 * (i64::from(year) + (i64::from(month) + 9) / 12) / 4 diff --git a/swiftnav/src/time/mod.rs b/swiftnav/src/time/mod.rs index 0dc0788..6022986 100644 --- a/swiftnav/src/time/mod.rs +++ b/swiftnav/src/time/mod.rs @@ -79,7 +79,7 @@ pub const WEEK: Duration = Duration::from_secs(consts::WEEK_SECS as u64); /// All year values are treated as if they are in the Gregorian calendar #[must_use] pub fn is_leap_year(year: u16) -> bool { - ((year % 4 == 0) && (year % 100 != 0)) || (year % 400 == 0) + (year.is_multiple_of(4) && !year.is_multiple_of(100)) || year.is_multiple_of(400) } #[cfg(test)] @@ -119,11 +119,8 @@ mod tests { let diff = test_case.diff(&round_trip).abs(); assert!( diff < TOW_TOL, - "gps2mjd2gps failure. original: {:?}, round trip: {:?}, diff: {}, TOW_TOL: {}", - test_case, - round_trip, - diff, - TOW_TOL + "gps2mjd2gps failure. original: {test_case:?}, round trip: {round_trip:?}, diff: \ + {diff}, TOW_TOL: {TOW_TOL}" ); // test mjd -> date -> mjd @@ -132,11 +129,8 @@ mod tests { let diff = (mjd.as_f64() - round_trip.as_f64()).abs(); assert!( diff < TOW_TOL, - "mjd2date2mjd failure. original: {:?}, round trip: {:?}, diff: {}, TOW_TOL: {}", - mjd, - round_trip, - diff, - TOW_TOL + "mjd2date2mjd failure. original: {mjd:?}, round trip: {round_trip:?}, diff: \ + {diff}, TOW_TOL: {TOW_TOL}" ); // test mjd -> utc -> mjd @@ -145,11 +139,8 @@ mod tests { let diff = (mjd.as_f64() - round_trip.as_f64()).abs(); assert!( diff < TOW_TOL, - "mjd2utc2mjd failure. original: {:?}, round trip: {:?}, diff: {}, TOW_TOL: {}", - mjd, - round_trip, - diff, - TOW_TOL + "mjd2utc2mjd failure. original: {mjd:?}, round trip: {round_trip:?}, diff: \ + {diff}, TOW_TOL: {TOW_TOL}" ); // test gps -> date -> gps @@ -158,11 +149,8 @@ mod tests { let diff = test_case.diff(&round_trip).abs(); assert!( diff < TOW_TOL, - "gps2date2gps failure. original: {:?}, round trip: {:?}, diff: {}, TOW_TOL: {}", - test_case, - round_trip, - diff, - TOW_TOL + "gps2date2gps failure. original: {test_case:?}, round trip: {round_trip:?}, diff: \ + {diff}, TOW_TOL: {TOW_TOL}" ); // test utc -> date -> utc @@ -171,11 +159,8 @@ mod tests { let diff = utc.to_mjd().as_f64() - mjd.as_f64(); assert!( diff < TOW_TOL, - "utc2date2utc failure. original: {:?}, round trip: {:?}, diff: {}, TOW_TOL: {}", - mjd, - round_trip, - diff, - TOW_TOL + "utc2date2utc failure. original: {mjd:?}, round trip: {round_trip:?}, diff: \ + {diff}, TOW_TOL: {TOW_TOL}" ); } } diff --git a/swiftnav/src/time/utc.rs b/swiftnav/src/time/utc.rs index 3547a3c..1ea9f6f 100644 --- a/swiftnav/src/time/utc.rs +++ b/swiftnav/src/time/utc.rs @@ -10,9 +10,10 @@ #![allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)] -use crate::time::{consts, is_leap_year, GpsTime, MJD}; use std::time::Duration; +use crate::time::{GpsTime, MJD, consts, is_leap_year}; + /// GPS UTC correction parameters #[derive(Clone)] pub struct UtcParams { @@ -399,7 +400,6 @@ impl From for UtcTime { } } -#[cfg(feature = "chrono")] impl From for chrono::DateTime { fn from(utc: UtcTime) -> chrono::DateTime { use chrono::prelude::*; @@ -425,7 +425,6 @@ impl From for chrono::DateTime { } } -#[cfg(feature = "chrono")] impl From> for UtcTime { fn from(chrono: chrono::DateTime) -> UtcTime { use chrono::prelude::*; @@ -557,7 +556,16 @@ mod tests { #[expect(clippy::float_cmp)] { - assert!(d_utc == test_case.d_utc && is_lse == test_case.is_lse, "test_case.t: {:?}, test_case.d_utc: {}, test_case.is_lse: {}, d_utc: {}, is_lse: {}", test_case.t, test_case.d_utc, test_case.is_lse, d_utc, is_lse); + assert!( + d_utc == test_case.d_utc && is_lse == test_case.is_lse, + "test_case.t: {:?}, test_case.d_utc: {}, test_case.is_lse: {}, d_utc: {}, \ + is_lse: {}", + test_case.t, + test_case.d_utc, + test_case.is_lse, + d_utc, + is_lse + ); } } } @@ -1090,7 +1098,6 @@ mod tests { } } - #[cfg(feature = "chrono")] #[test] fn chrono_conversions() { use chrono::prelude::*;