diff --git a/.github/workflows/daily.yml b/.github/workflows/daily.yml index d3cf462..9d5d107 100644 --- a/.github/workflows/daily.yml +++ b/.github/workflows/daily.yml @@ -22,7 +22,7 @@ jobs: - uses: actions-rs/toolchain@v1 name: Install Rust with: - toolchain: 1.82.0 + toolchain: 1.72.0 override: true - uses: actions-rs/cargo@v1 diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml index f08814c..cdd5a66 100644 --- a/.github/workflows/rust.yml +++ b/.github/workflows/rust.yml @@ -33,7 +33,7 @@ jobs: - uses: actions-rs/toolchain@v1 name: Install Rust with: - toolchain: 1.82.0 + toolchain: 1.72.0 override: true - uses: actions-rs/cargo@v1 diff --git a/.gitignore b/.gitignore index 6bf5637..84cd695 100644 --- a/.gitignore +++ b/.gitignore @@ -20,3 +20,5 @@ Cargo.lock *.swo *.swp +*.rs~ +*.txt diff --git a/.gitmodules b/.gitmodules index 83362c7..3481ce4 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,6 @@ [submodule "tools"] path = tools url = https://github.com/rtk-rs/tools +[submodule "data"] + path = data + url = git@github.com:nav-solutions/data.git diff --git a/Cargo.toml b/Cargo.toml index 5f3ef5c..c03aa3d 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,11 +1,11 @@ [package] name = "gnss-protos" -version = "0.0.2" +version = "0.1.0" license = "MPL-2.0" authors = ["Guillaume W. Bres "] description = "GNSS protos encoder and decoder" -homepage = "https://github.com/rtk-rs" -repository = "https://github.com/rtk-rs/gnss-protos" +homepage = "https://github.com/nav-solutions" +repository = "https://github.com/nav-solutions/gnss-protos" keywords = ["geo", "gps", "galileo"] categories = ["science", "science::geo", "parsing"] edition = "2021" @@ -16,12 +16,12 @@ exclude = [ ] [features] -default = [] +default = ["gps"] # STD support. Currently only used in test mode. std = [] -# Unlock GPS protocol +# Unlock GPS (+QZSS) protocol gps = [] [package.metadata.docs.rs] @@ -29,6 +29,7 @@ all-features = true rustdoc-args = ["--cfg", "docrs", "--generate-link-to-definition"] [dependencies] +thiserror = "1" log = { version = "0.4", optional = true } num-traits = { version = "0.2", default-features = false } diff --git a/README.md b/README.md index 23f9d08..81d1d1d 100644 --- a/README.md +++ b/README.md @@ -1,34 +1,71 @@ # GNSS-Protos -[![Rust](https://github.com/rtk-rs/gnss-protos/actions/workflows/rust.yml/badge.svg)](https://github.com/rtk-rs/gnss-protos/actions/workflows/rust.yml) -[![Rust](https://github.com/rtk-rs/gnss-protos/actions/workflows/daily.yml/badge.svg)](https://github.com/rtk-rs/gnss-protos/actions/workflows/daily.yml) +[![Rust](https://github.com/nav-solutions/gnss-protos/actions/workflows/rust.yml/badge.svg)](https://github.com/nav-solutions/gnss-protos/actions/workflows/rust.yml) +[![Rust](https://github.com/nav-solutions/gnss-protos/actions/workflows/daily.yml/badge.svg)](https://github.com/nav-solutions/gnss-protos/actions/workflows/daily.yml) [![crates.io](https://img.shields.io/crates/v/gnss-protos.svg)](https://crates.io/crates/gnss-protos) [![crates.io](https://docs.rs/gnss-protos/badge.svg)](https://docs.rs/gnss-protos/badge.svg) -[![License](https://img.shields.io/badge/license-MPL_2.0-orange?style=for-the-badge&logo=mozilla)](https://github.com/rtk-rs/gnss-protos/blob/main/LICENSE) +[![MRSV](https://img.shields.io/badge/MSRV-1.72.0-orange?style=for-the-badge)](https://github.com/rust-lang/rust/releases/tag/1.72.0) +[![License](https://img.shields.io/badge/license-MPL_2.0-orange?style=for-the-badge&logo=mozilla)](https://github.com/nav-solutions/gnss-protos/blob/main/LICENSE) GNSS-Protos =========== -Ths library offers small and efficient decoders for GNSS protocols. +The idea behind this library is to offer an easy to use, efficient and regrouped +framework to encode & decode GNSS protocols. -This library currently supports the following protos: +You can use this API to learn and go deeper into each protocol, as we strive to document +each data bit correctly (refer to the online API). -- `GPS`: available on `gps` crate feature. +All protocols are gated under a library feature, so you can narrow it down to your use case. -GPS proto -========= +Supported protocols +=================== -Available on `gps` crate feature. +- GPS / QZSS protocol available on `gps` crate feature, activated by default. -We support the following GPS frames: +GPS (US) / QZSS (Jap) protocol +============================== -- Ephemeris #1 -- Ephemeris #2 -- Ephemeris #3 +The `gps` library feature activates support of GPS/QZSS protocol. + +Currently we support Ephemeris frames 1, 2 and 3, which is sufficient for real-time +navigation (we do not support the Almanach frames). +Frames parity is not fully implemented either. + +We provide methods to both encode and decode data frames, and methods +to work from a stream of padded bytes (re-aligned) (2) or a bit stream buffer for real-time interfaces (1): + +1. The `GpsDecoder` is the solution when working with real-time GPS/QZSS streams. +It is capable to synchronize itself to the frame start (which is not aligned to a byte). +But you have to manage your buffer and operate the API correctly. +This method returns the number of processed _bits_ (not bytes). You are expected +to discard all processed _bits_ each time you invoke the decoder, not to process +the same frame twice. If you discard bytes not bits, you will inevitably loose messages. + +```rust +use gnss_protos::gps::GpsDecoder; + +// The decoder does not verify parity at the moment +let mut decoder = GpsDecoder::default(); + +// Feed one of our test frames into it, +// which is equivalent to real-time acquisition +``` + +2. `GpsQzssFrame` supports a `decode()` that works with a possibly padded Byte. +This is the prefered option when working with a stream that was already manipulated by a machine +and therefore, re-aligned to bytes. Each byte may have padding (or not). The stream must +start with the sync byte. +We used this approach to interface correctly to U-Blox receivers for example +(that pad and align the frames internally). + +```rust +use gnss_protos::gps::GpsDecoder; +``` License ======= -This library is part of the [RTK-rs framework](https://github.com/rtk-rs) which -is delivered under the [Mozilla V2 Public](https://www.mozilla.org/en-US/MPL/2.0) license. +This library is part of the [Nav-solutions framework](https://github.com/nav-solutions) and is +licensed under [Mozilla V2 Public](https://www.mozilla.org/en-US/MPL/2.0) license. diff --git a/data b/data new file mode 160000 index 0000000..2a0ef71 --- /dev/null +++ b/data @@ -0,0 +1 @@ +Subproject commit 2a0ef7163d1998e04f2c0eea7cf0c6be338eef33 diff --git a/src/gps/bytes.rs b/src/gps/bytes.rs index b0a6785..e21a14a 100644 --- a/src/gps/bytes.rs +++ b/src/gps/bytes.rs @@ -1,36 +1,51 @@ -#[derive(Debug)] +#[derive(Copy, Clone, PartialEq)] /// [GpsDataByte] aligned to 32 bits pub enum GpsDataByte { - /// 2-bit MSB padding. - /// Usually used at the beginning or stream termination by computers. - MsbPadded(u8), - - /// 2-bit LSB padding. - /// Usually used at the beginning or stream termination by computers. + /// 2-bit LSB padding, used to align the received bits to [u32]. LsbPadded(u8), - /// Plain byte + /// Plain byte (no padding, only meaningful bits). Byte(u8), } -impl core::fmt::LowerExp for GpsDataByte { +impl std::fmt::Debug for GpsDataByte { + fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result { + write!(f, "{:x}", self) + } +} + +impl core::fmt::LowerHex for GpsDataByte { fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - match self { - Self::MsbPadded(value) => write!(f, "0x{:02}X", value), - Self::LsbPadded(value) => write!(f, "0x{:02}X", (value << 2)), - Self::Byte(value) => write!(f, "0x{:02}X", value), - } + write!(f, "0x{:02X}", self.as_u8()) + } +} + +impl Default for GpsDataByte { + /// Creates a default null [GpsDataByte::Byte]. + fn default() -> Self { + Self::Byte(0) } } impl GpsDataByte { - /// Stores provided byte as 2-bit MSB padded [u8] - pub fn msb_padded(byte: u8) -> Self { - Self::MsbPadded(byte & 0x3f) + /// Stores provided un-padded byte with 2-bit LSB padding, + /// to align one word to [u32]. + pub fn padded(byte: u8) -> Self { + Self::LsbPadded(byte << 2) + } + + /// Interprets internal value as [u8] to process + /// internal byte correctly. + pub(crate) fn as_u8(&self) -> u8 { + match self { + Self::LsbPadded(value) => (value >> 2) & 0x3f, + Self::Byte(value) => *value, + } } - /// Stores provided byte as 2-bit LSB padded [u8] - pub fn lsb_padded(byte: u8) -> Self { - Self::LsbPadded((byte & 0xfc) >> 2) + /// Interprets internal value as [u32] to process + /// internal byte correctly. + pub(crate) fn as_u32(&self) -> u32 { + self.as_u8() as u32 } } diff --git a/src/gps/cdma.rs b/src/gps/cdma.rs new file mode 100644 index 0000000..f1ba29f --- /dev/null +++ b/src/gps/cdma.rs @@ -0,0 +1,22 @@ +use crate::gps::GpsQzssFrame; + +/// CDMA modulator, used to scramble a stream of bits +/// that are then GPS/QZSS compatible. +#[derive(Copy, Clone, PartialEq)] +pub struct CDMA { + /// SV ID + sat_id: u8, +} + +impl CDMA { + /// Creates a new [CDMA] modulator ready to scramble a stream of bits + /// for this particular SV-ID + pub fn from_satellite_id(sat_id: u8) -> Self { + Self { sat_id } + } + + /// Returns the + + /// [CDMA] encode this buffer, ready to transmit + pub fn scramble(buffer: &mut [u8], size: usize) {} +} diff --git a/src/gps/decoder.rs b/src/gps/decoder.rs index 9523340..f101a6b 100644 --- a/src/gps/decoder.rs +++ b/src/gps/decoder.rs @@ -1,844 +1,324 @@ use crate::gps::{ - frame1::{ - Word10 as Ephemeris1Word10, Word3 as Ephemeris1Word3, Word4 as Ephemeris1Word4, - Word5 as Ephemeris1Word5, Word6 as Ephemeris1Word6, Word7 as Ephemeris1Word7, - Word8 as Ephemeris1Word8, Word9 as Ephemeris1Word9, - }, - frame2::{ - Word10 as Ephemeris2Word10, Word3 as Ephemeris2Word3, Word4 as Ephemeris2Word4, - Word5 as Ephemeris2Word5, Word6 as Ephemeris2Word6, Word7 as Ephemeris2Word7, - Word8 as Ephemeris2Word8, Word9 as Ephemeris2Word9, - }, - frame3::{ - Word10 as Ephemeris3Word10, Word3 as Ephemeris3Word3, Word4 as Ephemeris3Word4, - Word5 as Ephemeris3Word5, Word6 as Ephemeris3Word6, Word7 as Ephemeris3Word7, - Word8 as Ephemeris3Word8, Word9 as Ephemeris3Word9, - }, - GpsDataByte, GpsQzssFrame, GpsQzssFrameId, GpsQzssHow, GpsQzssSubframe, GpsQzssTelemetry, + GpsQzssFrame, GpsQzssHow, GpsQzssTelemetry, GPS_FRAME_BITS, GPS_FRAME_BYTES, GPS_PREAMBLE_BYTE, }; #[cfg(feature = "log")] -use log::{error, trace}; - -#[derive(Default, Debug, Copy, Clone, PartialEq)] -enum State { - /// Telemetry dword parsing - #[default] - Telemetry, - - /// How dword parsing - How, - - /// Data word parsing - DataWord, -} +use log::{debug, error, trace}; +/// [GpsQzssDecoder] can decode GPS (or QZSS) protocol. +/// By [Default], our [GpsQzssDecoder] does not verify parity. +#[derive(Debug, Copy, Clone)] pub struct GpsQzssDecoder { - /// Frame counter - ptr: usize, - - /// data word storage - pub(crate) dword: u32, - - /// total number of bits collected - pub(crate) collected: usize, - - /// Current [State] - state: State, - - /// Latest [GpsQzssTelemetry] - tlm: GpsQzssTelemetry, - - /// Latest [GpsQzssHow] - how: GpsQzssHow, + /// Aligned bytes + buffer: [u8; GPS_FRAME_BYTES], - /// Pending [GpsQzssSubframe] - subframe: GpsQzssSubframe, - - /// u32 storage for words that overlap - storage: u32, - - /// True if parity verification is requested - parity_check: bool, + /// True when parity verification is requested + parity_verification: bool, } impl Default for GpsQzssDecoder { - /// Creates a default [GpsQzssDecoder] with parity bit verification + /// Creates a default [GpsQzssDecoder] that does not verify parity. fn default() -> Self { - let state = State::default(); - Self { - state, - ptr: 0, - dword: 0, - collected: 0, - storage: 0, - parity_check: true, - tlm: Default::default(), - how: Default::default(), - subframe: Default::default(), + buffer: [0; GPS_FRAME_BYTES], + parity_verification: false, } } } impl GpsQzssDecoder { - /// Parses a GPS/QZSS stream of [GpsDataByte]s, returns a [GpsQzssFrame] when - /// one is identified and the last data word has been correctly processed. - pub fn parse(&mut self, byte: GpsDataByte) -> Option { - // collect bytes - match byte { - GpsDataByte::LsbPadded(byte) => { - self.collected += 6; - self.dword <<= 6; - self.dword |= (byte as u32) >> 2; - }, - GpsDataByte::MsbPadded(byte) => { - self.collected += 6; - self.dword <<= 6; - self.dword |= (byte & 0x3f) as u32; - }, - GpsDataByte::Byte(byte) => { - self.collected += 8; - self.dword <<= 8; - self.dword |= byte as u32; - }, - } - - #[cfg(feature = "log")] - trace!( - "GPS - data collection - ptr={} dword=0x{:08x}", - self.ptr, - self.dword - ); + /// Creates a new [GpsQzssDecoder] with parity verification. + /// Our [Default] implementation does not verify the parity bits, and + /// tolerates parity error. When switching to this implementation, + /// the parity bits must be correct for the decoder to accept a frame. + pub fn with_parity_verification(mut self) -> Self { + self.parity_verification = true; + self + } - if self.collected < 30 { - // Can't proceed - return None; + /// Packs 38 bytes (10x 30-bit + 4bit padding) correcty aligned to [u8], ready to process. + /// + /// ## Input + /// - slice: &[u8], will panic if not [GPS_FRAME_BYTES] byte long! + /// - preamble_offset in bits! + pub(crate) fn resync_align(&mut self, slice: &[u8], preamble_offset_bit: usize) { + // byte index + let byte_index = preamble_offset_bit / 8; + + // bit index within byte + let bit_index = preamble_offset_bit % 8; + + if bit_index == 0 { + self.buffer + .copy_from_slice(&slice[byte_index..byte_index + GPS_FRAME_BYTES]); + } else { + panic!("not supported yet"); + // slice[byte_index..byte_index + GPS_FRAME_BYTES] + // .into_iter() + // .map(|b| { + // *b + // }) + // .collect() } + } - // data word processing - let dword = self.dword; + /// Locates the preamble bit marker (sync byte) within a buffer + /// + /// ## Input + /// - slice: slice of bytes, must be [GPS_FRAME_BYTES] byte long + /// - size: total number of bytes + /// + /// ## Returns + /// - offset in bits ! + pub(crate) fn find_preamble(slice: &[u8], size: usize) -> Option { + for i in 0..size - GPS_FRAME_BYTES { + if slice[i] == GPS_PREAMBLE_BYTE { + return Some(i * 8); + } - let mut ret = Option::::None; + // intra byte test + let mut byte1_mask = 0x7F; + let mut byte2_mask = 0x80; - // buffer decoding attempt - #[cfg(feature = "log")] - trace!( - "GPS - state={:?} - decoding dword=0x{:08x}", - self.state, - dword - ); + for j in 1..8 { + let mut value = slice[i + 1]; + value >>= 8 - j; + value |= (slice[i] & byte1_mask) << j; - match self.state { - State::Telemetry => match GpsQzssTelemetry::decode(dword) { - Ok(tlm) => { - self.tlm = tlm; - self.state = State::How; - #[cfg(feature = "log")] - trace!("GPS - TLM {:?}", tlm); - }, - #[cfg(feature = "log")] - Err(e) => { - error!("GPS - TLM decoding error: {:?}", e); - }, - #[cfg(not(feature = "log"))] - Err(_) => {}, - }, - - State::How => match GpsQzssHow::decode(dword) { - Ok(how) => { - self.how = how; - self.state = State::DataWord; - self.ptr = 3; - - match how.frame_id { - GpsQzssFrameId::Ephemeris1 => { - self.subframe = GpsQzssSubframe::Ephemeris1(Default::default()); - }, - GpsQzssFrameId::Ephemeris2 => { - self.subframe = GpsQzssSubframe::Ephemeris2(Default::default()); - }, - GpsQzssFrameId::Ephemeris3 => { - self.subframe = GpsQzssSubframe::Ephemeris3(Default::default()); - }, - } - #[cfg(feature = "log")] - trace!("GPS - HOW {:?}", how); - }, - #[cfg(feature = "log")] - Err(e) => { - error!("GPS - HOW decoding error: {:?}", e); - self.ptr = 0; - self.dword = 0; - self.state = State::default(); - }, - #[cfg(not(feature = "log"))] - Err(_) => {}, - }, + byte1_mask >>= 1; + byte2_mask |= 0x1 << 8 - j; - State::DataWord => { - match self.how.frame_id { - GpsQzssFrameId::Ephemeris1 => match self.ptr { - 3 => { - let word = Ephemeris1Word3::decode(self.dword); - - let frame = self.subframe.as_mut_eph1().expect("internal error"); - - frame.week = word.week; - frame.ura = word.ura; - frame.ca_or_p_l2 = word.ca_or_p_l2; - frame.health = word.health; - frame.iodc |= (word.iodc_msb as u16) << 8; - #[cfg(feature = "log")] - trace!("GPS - EPH #1 Word#3 {:?}", word); - }, - 4 => { - let word = Ephemeris1Word4::decode(self.dword); - let frame = self.subframe.as_mut_eph1().expect("internal error"); - frame.l2_p_data_flag = word.l2_p_data_flag; - frame.reserved_word4 = word.reserved; - #[cfg(feature = "log")] - trace!("GPS - EPH #1 Word#4 {:?}", word); - }, - 5 => { - let word = Ephemeris1Word5::decode(self.dword); - let frame = self.subframe.as_mut_eph1().expect("internal error"); - frame.reserved_word5 = word.reserved; - #[cfg(feature = "log")] - trace!("GPS - EPH #1 Word#5 {:?}", word); - }, - 6 => { - let word = Ephemeris1Word6::decode(self.dword); - let frame = self.subframe.as_mut_eph1().expect("internal error"); - frame.reserved_word6 = word.reserved; - #[cfg(feature = "log")] - trace!("GPS - EPH #1 Word#6 {:?}", word); - }, - 7 => { - let word = Ephemeris1Word7::decode(self.dword); - let frame = self.subframe.as_mut_eph1().expect("internal error"); - frame.tgd = (word.tgd as f64) / 2_f64.powi(31); - frame.reserved_word7 = word.reserved; - #[cfg(feature = "log")] - trace!("GPS - EPH #1 Word#7 {:?}", word); - }, - 8 => { - let word = Ephemeris1Word8::decode(self.dword); - let frame = self.subframe.as_mut_eph1().expect("internal error"); - frame.toc = (word.toc as u32) * 16; - frame.iodc |= word.iodc_lsb as u16; - #[cfg(feature = "log")] - trace!("GPS - EPH #1 Word#8 {:?}", word); - }, - 9 => { - let word = Ephemeris1Word9::decode(self.dword); - let frame = self.subframe.as_mut_eph1().expect("internal error"); - frame.af2 = (word.af2 as f64) / 2.0_f64.powi(55); - frame.af1 = (word.af1 as f64) / 2.0_f64.powi(43); - #[cfg(feature = "log")] - trace!("GPS - EPH #1 Word#9 {:?}", word); - }, - 10 => { - let word = Ephemeris1Word10::decode(self.dword); - let frame = self.subframe.as_mut_eph1().expect("internal error"); - frame.af0 = (word.af0 as f64) / 2.0_f64.powi(31); - #[cfg(feature = "log")] - trace!("GPS - EPH #1 Word#10 {:?}", word); - }, - _ => { - unreachable!("invalid state"); - }, - }, - GpsQzssFrameId::Ephemeris2 => match self.ptr { - 3 => { - let word = Ephemeris2Word3::decode(self.dword); - let frame = self.subframe.as_mut_eph2().expect("internal error"); - frame.crs = (word.crs as f64) / 2.0_f64.powi(5); - frame.iode = word.iode; - #[cfg(feature = "log")] - trace!("GPS - EPH #2 Word#3 {:?}", word); - }, - 4 => { - let word = Ephemeris2Word4::decode(self.dword); - let frame = self.subframe.as_mut_eph2().expect("internal error"); - frame.dn = (word.dn as f64) / 2.0_f64.powi(43); - self.storage = word.m0_msb as u32; - #[cfg(feature = "log")] - trace!("GPS - EPH #2 Word#4 {:?}", word); - }, - 5 => { - let word = Ephemeris2Word5::decode(self.dword); - let frame = self.subframe.as_mut_eph2().expect("internal error"); - - let mut m0 = self.storage; - m0 <<= 24; - m0 |= word.m0_lsb as u32; - - frame.m0 = ((m0 as i32) as f64) / 2.0_f64.powi(31); - - #[cfg(feature = "log")] - trace!("GPS - EPH #2 Word#5 {:?}", word); - }, - 6 => { - let word = Ephemeris2Word6::decode(self.dword); - let frame = self.subframe.as_mut_eph2().expect("internal error"); - frame.cuc = (word.cuc as f64) / 2.0_f64.powi(29); - self.storage = word.e_msb as u32; - #[cfg(feature = "log")] - trace!("GPS - EPH #2 Word#6 {:?}", word); - }, - 7 => { - let word = Ephemeris2Word7::decode(self.dword); - let frame = self.subframe.as_mut_eph2().expect("internal error"); - - let mut e = self.storage; - e <<= 24; - e |= word.e_lsb; - - frame.e = (e as f64) / 2.0_f64.powi(33); - #[cfg(feature = "log")] - trace!("GPS - EPH #2 Word#7 {:?}", word); - }, - 8 => { - let word = Ephemeris2Word8::decode(self.dword); - let frame = self.subframe.as_mut_eph2().expect("internal error"); - - frame.cus = (word.cus as f64) / 2.0_f64.powi(29); - self.storage = word.sqrt_a_msb as u32; - #[cfg(feature = "log")] - trace!("GPS - EPH #2 Word#8 {:?}", word); - }, - 9 => { - let word = Ephemeris2Word9::decode(self.dword); - let frame = self.subframe.as_mut_eph2().expect("internal error"); - - let mut sqrt_a = self.storage; - sqrt_a <<= 24; - sqrt_a |= word.sqrt_a_lsb; - - frame.sqrt_a = (sqrt_a as f64) / 2.0_f64.powi(19); - - #[cfg(feature = "log")] - trace!("GPS - EPH #2 Word#9 {:?}", word); - }, - 10 => { - let word = Ephemeris2Word10::decode(self.dword); - let frame = self.subframe.as_mut_eph2().expect("internal error"); - - frame.aodo = word.aodo; - frame.fit_int_flag = word.fitint; - frame.toe = (word.toe as u32) * 16; - - #[cfg(feature = "log")] - trace!("GPS - EPH #2 Word#10 {:?}", word); - }, - _ => { - unreachable!("invalid state"); - }, - }, - GpsQzssFrameId::Ephemeris3 => match self.ptr { - 3 => { - let word = Ephemeris3Word3::decode(self.dword); - let frame = self.subframe.as_mut_eph3().expect("internal error"); - frame.cic = (word.cic as f64) / 2.0_f64.powi(29); - self.storage = word.omega0_msb as u32; - - #[cfg(feature = "log")] - trace!("GPS - EPH #3 Word#3 {:?}", word); - }, - 4 => { - let word = Ephemeris3Word4::decode(self.dword); - let frame = self.subframe.as_mut_eph3().expect("internal error"); - - let mut omega0 = self.storage; - omega0 <<= 24; - omega0 |= word.omega0_lsb; - - frame.omega0 = ((omega0 as i32) as f64) / 2.0_f64.powi(31); - - #[cfg(feature = "log")] - trace!("GPS - EPH #3 Word#4 {:?}", word); - }, - 5 => { - let word = Ephemeris3Word5::decode(self.dword); - let frame = self.subframe.as_mut_eph3().expect("internal error"); - - frame.cis = (word.cis as f64) / 2.0_f64.powi(29); - - self.storage = word.i0_msb as u32; - - #[cfg(feature = "log")] - trace!("GPS - EPH #2 Word#5 {:?}", word); - }, - 6 => { - let word = Ephemeris3Word6::decode(self.dword); - let frame = self.subframe.as_mut_eph3().expect("internal error"); - - let mut i0 = self.storage; - i0 <<= 24; - i0 |= word.i0_lsb; - - frame.i0 = (i0 as f64) / 2.0_f64.powi(31); - - #[cfg(feature = "log")] - trace!("GPS - EPH #3 Word#6 {:?}", word); - }, - 7 => { - let word = Ephemeris3Word7::decode(self.dword); - let frame = self.subframe.as_mut_eph3().expect("internal error"); - - frame.crc = (word.crc as f64) / 2.0_f64.powi(5); - self.storage = word.omega_msb as u32; - - #[cfg(feature = "log")] - trace!("GPS - EPH #3 Word#7 {:?}", word); - }, - 8 => { - let word = Ephemeris3Word8::decode(self.dword); - let frame = self.subframe.as_mut_eph3().expect("internal error"); - - let mut omega = self.storage; - omega <<= 24; - omega |= word.omega_lsb; - - frame.omega = ((omega as i32) as f64) / 2.0_f64.powi(31); - - #[cfg(feature = "log")] - trace!("GPS - EPH #3 Word#8 {:?}", word); - }, - 9 => { - let word = Ephemeris3Word9::decode(self.dword); - let frame = self.subframe.as_mut_eph3().expect("internal error"); - - frame.omega_dot = (word.omega_dot as f64) / 2.0_f64.powi(43); - - #[cfg(feature = "log")] - trace!("GPS - EPH #3 Word#9 {:?}", word); - }, - 10 => { - let word = Ephemeris3Word10::decode(self.dword); - let frame = self.subframe.as_mut_eph3().expect("internal error"); - - frame.idot = (word.idot as f64) / 2.0_f64.powi(43); - frame.iode = word.iode; - - #[cfg(feature = "log")] - trace!("GPS - EPH #3 Word#10 {:?}", word); - }, - _ => { - unreachable!("invalid state"); - }, - }, + if value == GPS_PREAMBLE_BYTE { + return Some(i * 8 + j); } - - self.ptr += 1; - }, - } - - // reset - self.collected = 0; - self.dword = 0; - - if self.ptr == 11 { - ret = Some(GpsQzssFrame { - how: self.how, - telemetry: self.tlm, - subframe: self.subframe, - }); - - self.ptr = 0; - self.state = State::Telemetry; + } } - ret + None } - /// Create a new [GpsQzssDecoder] that will not verify the parity bits - pub fn without_parity_verification(&self) -> Self { - Self { - ptr: self.ptr, - collected: 0, - dword: 0, - storage: 0, - state: self.state, - parity_check: false, - tlm: self.tlm.clone(), - how: self.how.clone(), - subframe: self.subframe.clone(), + /// Decodes the first valid [GpsQzssFrame] found in this read-only [u8] buffer. + /// [GpsQzssDecoder] will align itself to the Sync byte, which is not aligned to [u8], + /// because GPS/QZSS is made of 30 bit data words. + /// + /// ## Input + /// - buffer: read-only [u8] buffer + /// - size: buffer size (in bytes) + /// + /// ## Ouput + /// - Total number of _bits_ that were consumed (not bytes!). + /// You are expected to discard all processed _bits_ not to decode the same frame twice. + /// - Optional [GpsQzssFrame] correctly decoded. First in order of appearance in the buffer. + pub fn decode(&mut self, buffer: &[u8], size: usize) -> (usize, Option) { + // locate preamble + let preamble_offset_bit = Self::find_preamble(buffer, size); + + if preamble_offset_bit.is_none() { + return (size * 8 - GPS_FRAME_BITS, None); } - } -} -#[cfg(test)] -mod test { - use crate::{ - gps::{GpsDataByte, GpsQzssDecoder, GpsQzssSubframe}, - GpsQzssFrameId, - }; + // realign and pack as [u8] + let preamble_offset_bit = preamble_offset_bit.unwrap(); - use crate::tests::from_ublox_be_bytes; - - #[cfg(all(feature = "std", feature = "log"))] - use crate::tests::init_logger; - - #[test] - fn test_tlm_decoding() { - #[cfg(all(feature = "std", feature = "log"))] - init_logger(); + #[cfg(feature = "log")] + trace!("(GPS/QZSS) [preamble]: pos={}", preamble_offset_bit); - let bytes = [ - GpsDataByte::Byte(0x8B), - GpsDataByte::Byte(0x00), - GpsDataByte::Byte(0x0D), - GpsDataByte::lsb_padded(0x00), // TLM - ]; + self.resync_align(buffer, preamble_offset_bit); - let mut decoder = GpsQzssDecoder::default(); + let telemetry_dword = u32::from_be_bytes([ + self.buffer[0], + self.buffer[1], + self.buffer[2], + self.buffer[3], + ]); - for byte in bytes { - let _ = decoder.parse(byte); - } + let shifted_dword = telemetry_dword >> 2; + let parity = (telemetry_dword & 0xfc) >> 2; - assert_eq!(decoder.tlm.message, 3); - assert_eq!(decoder.tlm.integrity, false); - assert_eq!(decoder.tlm.reserved_bits, true); - } + #[cfg(feature = "log")] + trace!("(GPS/QZSS) [telemetry]: par={}", parity); - #[test] - fn test_ublox_tlm_decoding() { - #[cfg(all(feature = "std", feature = "log"))] - init_logger(); + let telemetry = match GpsQzssTelemetry::decode(shifted_dword) { + Ok(telemetry) => { + #[cfg(feature = "log")] + debug!("(GPS/QZSS) [telemetry]: {}", telemetry); + telemetry + }, + #[cfg(not(feature = "log"))] + Err(_) => { + return (preamble_offset_bit + GPS_FRAME_BITS, None); + }, + #[cfg(feature = "log")] + Err(e) => { + error!("(GPS/QZSS) [telemetry]: {}", e); + return (preamble_offset_bit + GPS_FRAME_BITS, None); + }, + }; - let bytes = [ - GpsDataByte::msb_padded(0x22), - GpsDataByte::Byte(0xC1), - GpsDataByte::Byte(0x3E), - GpsDataByte::Byte(0x1B), // TLM - ]; + let how_dword = u32::from_be_bytes([ + self.buffer[4], + self.buffer[5], + self.buffer[6], + self.buffer[7], + ]); - let mut decoder = GpsQzssDecoder::default(); + let mut shifted_dword = (telemetry_dword & 0x3) << 30; + shifted_dword |= how_dword >> 6; - for byte in bytes { - let _ = decoder.parse(byte); - } + let parity = (how_dword & 0xfc) >> 2; + trace!("(GPS/QZSS) [how]: ={:08X}", shifted_dword); - assert_eq!(decoder.tlm.message, 0x13E); - assert_eq!(decoder.tlm.integrity, false); - assert_eq!(decoder.tlm.reserved_bits, false); + #[cfg(feature = "log")] + trace!("(GPS/QZSS) [how]: par={}", parity); - let bytes = [0x22, 0xC1, 0x3E, 0x1B]; - let bytes = from_ublox_be_bytes(&bytes); + let how = match GpsQzssHow::decode(shifted_dword) { + Ok(how) => { + #[cfg(feature = "log")] + debug!("(GPS/QZSS) [how]: {}", how); + how + }, + #[cfg(not(feature = "log"))] + Err(_) => { + return (preamble_offset_bit + GPS_FRAME_BITS, None); + }, + #[cfg(feature = "log")] + Err(e) => { + error!("(GPS/QZSS) [how]: {}", e); + return (preamble_offset_bit + GPS_FRAME_BITS, None); + }, + }; - let mut decoder = GpsQzssDecoder::default(); + // now process the 8 data words + for i in 0..8 {} - for byte in bytes { - let _ = decoder.parse(byte); - } + let frame = GpsQzssFrame { + telemetry, + how, + subframe: Default::default(), + }; - assert_eq!(decoder.tlm.message, 0x13E); - assert_eq!(decoder.tlm.integrity, false); - assert_eq!(decoder.tlm.reserved_bits, false); + return (preamble_offset_bit + GPS_FRAME_BITS, Some(frame)); } +} - #[test] - fn test_tlmhow_decoding() { - #[cfg(all(feature = "std", feature = "log"))] - init_logger(); - - let bytes = [ - GpsDataByte::msb_padded(0x22), - GpsDataByte::Byte(0xC1), - GpsDataByte::Byte(0x3E), - GpsDataByte::Byte(0x1B), // TLM - GpsDataByte::MsbPadded(0x15), - GpsDataByte::Byte(0x27), - GpsDataByte::Byte(0xC9), - GpsDataByte::Byte(0x73), // HOW - ]; - - let mut decoder = GpsQzssDecoder::default(); - - for byte in bytes { - let _ = decoder.parse(byte); - } +#[cfg(test)] +mod decoder { + use std::{fs::File, io::Read}; - assert_eq!(decoder.tlm.message, 0x13E); - assert_eq!(decoder.tlm.integrity, false); - assert_eq!(decoder.tlm.reserved_bits, false); + use crate::{ + gps::{GpsQzssDecoder, GPS_FRAME_BITS, GPS_FRAME_BYTES}, + tests::{insert_zeros, GPS_EPH1_DATA}, + }; - assert_eq!(decoder.how.alert, false); - assert_eq!(decoder.how.anti_spoofing, true); - assert_eq!(decoder.how.frame_id, GpsQzssFrameId::Ephemeris1); - } + #[cfg(all(feature = "std", feature = "log"))] + use crate::tests::init_logger; #[test] - fn test_ublox_tlmhow_decoding() { + fn preamble() { #[cfg(all(feature = "std", feature = "log"))] init_logger(); - let bytes = [ - GpsDataByte::MsbPadded(0x22), - GpsDataByte::Byte(0xC1), - GpsDataByte::Byte(0x3E), - GpsDataByte::Byte(0x1B), // TLM - GpsDataByte::MsbPadded(0x15), - GpsDataByte::Byte(0x27), - GpsDataByte::Byte(0xC9), - GpsDataByte::Byte(0x73), // HOW - ]; - - let mut decoder = GpsQzssDecoder::default(); - - for byte in bytes { - let _ = decoder.parse(byte); + let mut buffer = [0; 1024]; + let mut file = File::open("data/GPS/eph-1.bin").unwrap(); + file.read(&mut buffer).unwrap(); + + assert_eq!(GpsQzssDecoder::find_preamble(&buffer, 1024), Some(0)); + + // test delay < 1 byte + for i in 1..7 { + let delayed = insert_zeros(&buffer, i); + assert_eq!( + GpsQzssDecoder::find_preamble(&delayed, 1024), + Some(i), + "failed for bit position {}", + i + ); } - assert_eq!(decoder.tlm.message, 0x13E); - assert_eq!(decoder.tlm.integrity, false); - assert_eq!(decoder.tlm.reserved_bits, false); - - assert_eq!(decoder.how.alert, false); - assert_eq!(decoder.how.anti_spoofing, true); - assert_eq!(decoder.how.frame_id, GpsQzssFrameId::Ephemeris1); - - let bytes = from_ublox_be_bytes(&[0x22, 0xC1, 0x3E, 0x1B, 0x15, 0x27, 0xC9, 0x73]); - - let mut decoder = GpsQzssDecoder::default(); + // test 1 byte offset + let delayed = insert_zeros(&buffer, 8); + assert_eq!( + GpsQzssDecoder::find_preamble(&delayed, 1024), + Some(8), + "failed for bit position 8" + ); - for byte in bytes { - let _ = decoder.parse(byte); + // test 1 byte + bits + for i in 1..7 { + let delayed = insert_zeros(&buffer, i + 8); + assert_eq!( + GpsQzssDecoder::find_preamble(&delayed, 1024), + Some(8 + i), + "failed for bit position {}", + 8 + i + ); } - assert_eq!(decoder.tlm.message, 0x13E); - assert_eq!(decoder.tlm.integrity, false); - assert_eq!(decoder.tlm.reserved_bits, false); - - assert_eq!(decoder.how.alert, false); - assert_eq!(decoder.how.anti_spoofing, true); - assert_eq!(decoder.how.frame_id, GpsQzssFrameId::Ephemeris1); + // test other values + for i in 2..9 { + let delayed = insert_zeros(&buffer, i * 8); + assert_eq!( + GpsQzssDecoder::find_preamble(&delayed, 1024), + Some(i * 8), + "failed for bit position {}", + i * 8 + ); + + for j in 1..7 { + let delayed = insert_zeros(&buffer, i * 8 + j); + assert_eq!( + GpsQzssDecoder::find_preamble(&delayed, 1024), + Some(i * 8 + j), + "failed for bit position {}", + i * 8 + j + ); + } + } } #[test] - fn test_incomplete_frame() { + fn decoder_test_data() { #[cfg(all(feature = "std", feature = "log"))] init_logger(); - let mut found = false; - - let bytes = [ - GpsDataByte::Byte(0x8B), - GpsDataByte::Byte(0x00), - GpsDataByte::Byte(0x0D), - GpsDataByte::lsb_padded(0x00), // TLM - ]; - let mut decoder = GpsQzssDecoder::default(); - for byte in bytes { - if let Some(_) = decoder.parse(byte) { - found = true; - } - } + let (size, decoded) = decoder.decode(&GPS_EPH1_DATA, GPS_EPH1_DATA.len()); - assert!(!found, "Invalid GPS frame decoded!"); - - let bytes = [ - GpsDataByte::Byte(0x8B), - GpsDataByte::Byte(0x00), - GpsDataByte::Byte(0x0D), - GpsDataByte::lsb_padded(0x00), // TLM - GpsDataByte::Byte(0x8B), - GpsDataByte::Byte(0x00), - GpsDataByte::Byte(0x0D), - GpsDataByte::lsb_padded(0x00), // HOW - GpsDataByte::Byte(0x8B), - GpsDataByte::Byte(0x00), - GpsDataByte::Byte(0x0D), - GpsDataByte::lsb_padded(0x00), // WORD3 - GpsDataByte::Byte(0x8B), - GpsDataByte::Byte(0x00), - GpsDataByte::Byte(0x0D), - GpsDataByte::lsb_padded(0x00), // WORD4 - GpsDataByte::Byte(0x8B), - GpsDataByte::Byte(0x00), - GpsDataByte::Byte(0x0D), - GpsDataByte::lsb_padded(0x00), // WORD5 - GpsDataByte::Byte(0x8B), - GpsDataByte::Byte(0x00), - GpsDataByte::Byte(0x0D), - GpsDataByte::lsb_padded(0x00), // WORD6 - GpsDataByte::Byte(0x8B), - GpsDataByte::Byte(0x00), - GpsDataByte::Byte(0x0D), - GpsDataByte::lsb_padded(0x00), // WORD7 - GpsDataByte::Byte(0x8B), - GpsDataByte::Byte(0x00), - GpsDataByte::Byte(0x0D), - GpsDataByte::lsb_padded(0x00), // WORD8 - GpsDataByte::Byte(0x8B), - GpsDataByte::Byte(0x00), - GpsDataByte::Byte(0x0D), - ]; + assert_eq!(size, GPS_FRAME_BITS, "returned invalid size"); - let mut decoder = GpsQzssDecoder::default(); + let decoded = decoded.unwrap_or_else(|| { + panic!("did not decoded GPS frame!"); + }); - for byte in bytes { - if let Some(_) = decoder.parse(byte) { - found = true; - } - } - - assert!(!found, "Invalid GPS frame decoded"); + assert_eq!(decoded.telemetry.message, 0); + assert_eq!(decoded.telemetry.integrity, false); + assert_eq!(decoded.telemetry.reserved_bit, false); } #[test] - fn test_eph1_frame() { + fn decoder_raw_data() { #[cfg(all(feature = "std", feature = "log"))] init_logger(); - let mut found = false; - - let bytes = from_ublox_be_bytes(&[ - // TLM - 0x22, 0xC1, 0x3E, 0x1B, // HOW - 0x15, 0x27, 0xC9, 0x73, // WORD3 - 0x13, 0xE4, 0x00, 0x04, //WORD4 - 0x10, 0x4F, 0x5D, 0x31, //WORD5 - 0x97, 0x44, 0xE6, 0xD7, // WORD6 - 0x07, 0x75, 0x57, 0x83, //WORD7 - 0x33, 0x0C, 0x80, 0xB5, // WORD8 - 0x92, 0x50, 0x42, 0xA1, // WORD9 - 0x80, 0x00, 0x16, 0x84, //WORD10 - 0x31, 0x2C, 0x30, 0x33, - ]); + assert_eq!(GPS_FRAME_BYTES, 38); - let mut decoder = GpsQzssDecoder::default().without_parity_verification(); - - for byte in bytes { - if let Some(frame) = decoder.parse(byte) { - assert_eq!(decoder.tlm.message, 0x13E); - assert_eq!(decoder.tlm.integrity, false); - assert_eq!(decoder.tlm.reserved_bits, false); - - assert_eq!(decoder.how.alert, false); - assert_eq!(decoder.how.anti_spoofing, true); - assert_eq!(decoder.how.frame_id, GpsQzssFrameId::Ephemeris1); - - match frame.subframe { - GpsQzssSubframe::Ephemeris1(frame1) => { - assert_eq!(frame1.af2, 0.0); - assert!((frame1.af1 - 1.023181539495e-011).abs() < 1e-14); - assert!((frame1.af0 - -4.524961113930e-004).abs() < 1.0e-11); - assert_eq!(frame1.week, 318); - assert_eq!(frame1.toc, 266_400); - assert_eq!(frame1.health, 0); - found = true; - }, - _ => panic!("incorrect subframe decoded!"), - } - } - } - - assert!(found, "GPS decoding failed!"); - } - - #[test] - fn test_eph2_frame() { - #[cfg(all(feature = "std", feature = "log"))] - init_logger(); + let mut buffer = [0; 1024]; + let mut file = File::open("data/GPS/eph-1.bin").unwrap(); + file.read(&mut buffer).unwrap(); - let mut found = false; - - let bytes = from_ublox_be_bytes(&[ - // TLM - 0x22, 0xC1, 0x3E, 0x1B, // HOW - 0x15, 0x27, 0xEA, 0x1B, // WORD3 - 0x12, 0x7F, 0xF1, 0x65, //WORD4 - 0x8C, 0x68, 0x1F, 0x7C, //WORD5 - 0x02, 0x49, 0x34, 0x15, // WORD6 - 0xBF, 0xF8, 0x81, 0x1E, //WORD7 - 0x99, 0x1B, 0x81, 0x14, // W0RD8 - 0x04, 0x3E, 0x68, 0x6E, // WORD9 - 0x83, 0x34, 0x72, 0x21, // WORD10 - 0x90, 0x42, 0x9F, 0x7B, - ]); - - let mut decoder = GpsQzssDecoder::default().without_parity_verification(); - - for byte in bytes { - if let Some(frame) = decoder.parse(byte) { - assert_eq!(decoder.tlm.message, 0x13E); - assert_eq!(decoder.tlm.integrity, false); - assert_eq!(decoder.tlm.reserved_bits, false); - - assert_eq!(decoder.how.alert, false); - assert_eq!(decoder.how.anti_spoofing, true); - assert_eq!(decoder.how.frame_id, GpsQzssFrameId::Ephemeris2); - - match frame.subframe { - GpsQzssSubframe::Ephemeris2(frame2) => { - assert_eq!(frame2.toe, 266_400); - assert_eq!(frame2.crs, -1.843750000000e+000); - assert!((frame2.sqrt_a - 5.153602432251e+003).abs() < 1e-9); - assert!((frame2.m0 - 9.768415465951e-001).abs() < 1e-9); - assert!((frame2.cuc - -5.587935447693e-008).abs() < 1e-9); - assert!((frame2.e - 8.578718174249e-003).abs() < 1e-9); - assert!((frame2.cus - 8.093193173409e-006).abs() < 1e-9); - assert!((frame2.cuc - -5.587935447693e-008).abs() < 1e-6); - assert!((frame2.dn - 1.444277586415e-009).abs() < 1e-9); - assert_eq!(frame2.fit_int_flag, false); - found = true; - }, - _ => panic!("incorrect subframe decoded!"), - } - } - } - - assert!(found, "GPS decoding failed!"); - } - - #[test] - fn test_eph3_frame() { - #[cfg(all(feature = "std", feature = "log"))] - init_logger(); + let mut decoder = GpsQzssDecoder::default(); - let mut found = false; + let (size, decoded) = decoder.decode(&buffer, 1024); - let bytes = from_ublox_be_bytes(&[ - 0x22, 0xC1, 0x3E, 0x1B, 0x15, 0x28, 0x0B, 0xDB, 0x00, 0x0A, 0xEA, 0x34, 0x03, 0x3C, - 0xFF, 0xEE, 0xBF, 0xE5, 0xC9, 0xEB, 0x13, 0x6F, 0xB6, 0x4E, 0x86, 0xF4, 0xAB, 0x2C, - 0x06, 0x71, 0xEB, 0x44, 0x3F, 0xEA, 0xF6, 0x02, 0x92, 0x45, 0x52, 0x13, - ]); + assert_eq!(size, GPS_FRAME_BITS, "returned invalid size"); - let mut decoder = GpsQzssDecoder::default().without_parity_verification(); - - for byte in bytes { - if let Some(frame) = decoder.parse(byte) { - assert_eq!(decoder.tlm.message, 0x13E); - assert_eq!(decoder.tlm.integrity, false); - assert_eq!(decoder.tlm.reserved_bits, false); - - assert_eq!(decoder.how.alert, false); - assert_eq!(decoder.how.anti_spoofing, true); - assert_eq!(decoder.how.frame_id, GpsQzssFrameId::Ephemeris3); - - match frame.subframe { - GpsQzssSubframe::Ephemeris3(frame3) => { - assert!((frame3.cic - 8.009374141693e-008).abs() < 1e-9); - assert!((frame3.cis - -1.955777406693e-007).abs() < 1E-9); - assert!((frame3.crc - 2.225625000000e+002).abs() < 1E-9); - assert!((frame3.i0 - 3.070601043291e-001).abs() < 1e-9); - assert!((frame3.idot - 1.548414729768e-010).abs() < 1E-9); - assert!((frame3.omega0 - -6.871047024615e-001).abs() < 1e-9); - assert!((frame3.omega_dot - -2.449269231874e-009).abs() < 1e-9); - assert!((frame3.omega - -6.554632573389e-001).abs() < 1e-9); - found = true; - }, - _ => panic!("incorrect subframe decoded!"), - } - } - } + let decoded = decoded.unwrap_or_else(|| { + panic!("did not decoded GPS frame!"); + }); - assert!(found, "GPS decoding failed!"); + assert_eq!(decoded.telemetry.message, 0); + assert_eq!(decoded.telemetry.integrity, false); + assert_eq!(decoded.telemetry.reserved_bit, false); } } diff --git a/src/gps/decoding.rs b/src/gps/decoding.rs new file mode 100644 index 0000000..7780173 --- /dev/null +++ b/src/gps/decoding.rs @@ -0,0 +1,227 @@ +use crate::gps::{ + bytes::{ByteArray, GpsDataByte}, + GpsQzssFrame, GpsQzssFrame1, GpsQzssFrameId, GpsQzssHow, GpsQzssSubframe, GpsQzssTelemetry, + GPS_FRAME_BITS, GPS_FRAME_BYTES, GPS_PREAMBLE_BYTE, GPS_WORDS_PER_FRAME, GPS_WORD_BITS, +}; + +#[cfg(doc)] +use crate::gps::GpsQzssDecoder; + +#[cfg(feature = "log")] +use log::{debug, error, trace}; + +impl GpsQzssFrame { + /// Decodes a collected slice of [GpsDataByte]s that must start + /// correctly on the first synchronization byte. + /// + /// This method is not capable to synchronize itself anywhere other than + /// the very first byte. When working with raw stream of bits, + /// you will prefer working with the [GpsQzssDecoder]. + /// + /// ## Input + /// - array of at least [GPS_FRAME_BYTES] [GpsDataByte]s + /// - size: total size of this slice + /// - check_parity: true if parity verification is required. + /// In this case, the method will return None on parity errors. + /// + /// ## Output + /// - Decoded [GpsQzssFrame] + pub fn decode(bytes: &[GpsDataByte], size: usize, check_parity: bool) -> Option { + if size < GPS_FRAME_BYTES { + return None; + } + + // TLM + let telemetry = ByteArray::new(&bytes[0..4]).value_u32(); + + let telemetry = match GpsQzssTelemetry::decode(telemetry) { + Ok(tlm) => tlm, + #[cfg(not(feature = "log"))] + Err(_) => { + return None; + }, + #[cfg(feature = "log")] + Err(_) => { + error!("invalid telemetry: 0x{:08X}", telemetry); + return None; + }, + }; + + // HOW + let how = ByteArray::new(&bytes[4..8]).value_u32(); + + let how = match GpsQzssHow::decode(how) { + Ok(how) => how, + #[cfg(not(feature = "log"))] + Err(_) => { + return None; + }, + #[cfg(feature = "log")] + Err(e) => { + error!("invalid frame-id: 0x{:08X}", how); + return None; + }, + }; + + Some(GpsQzssFrame { + subframe: GpsQzssSubframe::decode(how.frame_id, &bytes[8..]), + telemetry, + how, + }) + } +} + +#[cfg(test)] +mod test { + use std::{fs::File, io::Read}; + + use crate::{ + gps::{GpsQzssFrame, GpsQzssFrameId, GPS_FRAME_BYTES}, + tests::{from_ublox_be_bytes, insert_zeros}, + }; + + #[cfg(all(feature = "std", feature = "log"))] + use crate::tests::init_logger; + + #[test] + fn eph1_bytes_decoding_noparity() { + #[cfg(all(feature = "std", feature = "log"))] + init_logger(); + + let mut found = false; + + let bytes = from_ublox_be_bytes(&[ + // TLM + 0x22, 0xC1, 0x3E, 0x1B, // HOW + 0x15, 0x27, 0xC9, 0x73, // WORD3 + 0x13, 0xE4, 0x00, 0x04, // WORD4 + 0x10, 0x4F, 0x5D, 0x31, // WORD5 + 0x97, 0x44, 0xE6, 0xD7, // WORD6 + 0x07, 0x75, 0x57, 0x83, // WORD7 + 0x33, 0x0C, 0x80, 0xB5, // WORD8 + 0x92, 0x50, 0x42, 0xA1, // WORD9 + 0x80, 0x00, 0x16, 0x84, // WORD10 + 0x31, 0x2C, 0x30, 0x33, + ]); + + let decoded = GpsQzssFrame::decode(&bytes, 40, false).unwrap_or_else(|| { + panic!("Failed to decode valid message"); + }); + + assert_eq!(decoded.telemetry.message, 0x13E); + assert_eq!(decoded.telemetry.integrity, false); + assert_eq!(decoded.telemetry.reserved_bit, false); + + assert_eq!(decoded.how.alert, false); + assert_eq!(decoded.how.anti_spoofing, true); + assert_eq!(decoded.how.frame_id, GpsQzssFrameId::Ephemeris1); + + let frame1 = decoded.subframe.as_eph1().unwrap_or_else(|| { + panic!("Decoded invalid subframe"); + }); + + assert!((frame1.af1 - 1.023181539495E-11).abs() < 1e-14); + assert!((frame1.af0 - -4.524961113930E-04).abs() < 1.0e-11); + assert_eq!(frame1.af2, 0.0); + + assert_eq!(frame1.week, 318); + assert_eq!(frame1.toc, 266_400); + assert_eq!(frame1.health, 0); + } + + #[test] + fn eph2_bytes_decoding_noparity() { + #[cfg(all(feature = "std", feature = "log"))] + init_logger(); + + let mut found = false; + + let bytes = from_ublox_be_bytes(&[ + // TLM + 0x22, 0xC1, 0x3E, 0x1B, // HOW + 0x15, 0x27, 0xEA, 0x1B, // WORD3 + 0x12, 0x7F, 0xF1, 0x65, // WORD4 + 0x8C, 0x68, 0x1F, 0x7C, // WORD5 + 0x02, 0x49, 0x34, 0x15, // WORD6 + 0xBF, 0xF8, 0x81, 0x1E, // WORD7 + 0x99, 0x1B, 0x81, 0x14, // W0RD8 + 0x04, 0x3E, 0x68, 0x6E, // WORD9 + 0x83, 0x34, 0x72, 0x21, // WORD10 + 0x90, 0x42, 0x9F, 0x7B, + ]); + + let decoded = GpsQzssFrame::decode(&bytes, 40, false).unwrap_or_else(|| { + panic!("Failed to decode valid message"); + }); + + assert_eq!(decoded.telemetry.message, 0x13E); + assert_eq!(decoded.telemetry.integrity, false); + assert_eq!(decoded.telemetry.reserved_bit, false); + + assert_eq!(decoded.how.alert, false); + assert_eq!(decoded.how.anti_spoofing, true); + assert_eq!(decoded.how.frame_id, GpsQzssFrameId::Ephemeris2); + + let frame2 = decoded.subframe.as_eph2().unwrap_or_else(|| { + panic!("Decoded invalid subframe"); + }); + + assert_eq!(frame2.toe, 266_400); + assert_eq!(frame2.crs, -1.843750000000e+000); + assert!((frame2.sqrt_a - 5.153602432251e+003).abs() < 1e-9); + assert!((frame2.m0 - 9.768415465951e-001).abs() < 1e-9); + assert!((frame2.cuc - -5.587935447693e-008).abs() < 1e-9); + assert!((frame2.e - 8.578718174249e-003).abs() < 1e-9); + assert!((frame2.cus - 8.093193173409e-006).abs() < 1e-9); + assert!((frame2.cuc - -5.587935447693e-008).abs() < 1e-6); + assert!((frame2.dn - 1.444277586415e-009).abs() < 1e-9); + assert_eq!(frame2.fit_int_flag, false); + } + + #[test] + fn eph3_bytes_decoding_noparity() { + #[cfg(all(feature = "std", feature = "log"))] + init_logger(); + + let mut found = false; + + let bytes = from_ublox_be_bytes(&[ + // TLM + 0x22, 0xC1, 0x3E, 0x1B, // HOW + 0x15, 0x28, 0x0B, 0xDB, // WORD3 + 0x00, 0x0A, 0xEA, 0x34, // WORD4 + 0x03, 0x3C, 0xFF, 0xEE, // WORD5 + 0xBF, 0xE5, 0xC9, 0xEB, // WORD6 + 0x13, 0x6F, 0xB6, 0x4E, // WORD7 + 0x86, 0xF4, 0xAB, 0x2C, // WORD8 + 0x06, 0x71, 0xEB, 0x44, // WORD9 + 0x3F, 0xEA, 0xF6, 0x02, // WORD10 + 0x92, 0x45, 0x52, 0x13, + ]); + + let decoded = GpsQzssFrame::decode(&bytes, 40, false).unwrap_or_else(|| { + panic!("Failed to decode valid message"); + }); + + assert_eq!(decoded.telemetry.message, 0x13E); + assert_eq!(decoded.telemetry.integrity, false); + assert_eq!(decoded.telemetry.reserved_bit, false); + + assert_eq!(decoded.how.alert, false); + assert_eq!(decoded.how.anti_spoofing, true); + assert_eq!(decoded.how.frame_id, GpsQzssFrameId::Ephemeris3); + + let frame3 = decoded.subframe.as_eph3().unwrap_or_else(|| { + panic!("Decoded invalid subframe"); + }); + + assert!((frame3.cic - 8.009374141693e-008).abs() < 1e-9); + assert!((frame3.cis - -1.955777406693e-007).abs() < 1E-9); + assert!((frame3.crc - 2.225625000000e+002).abs() < 1E-9); + assert!((frame3.i0 - 3.070601043291e-001).abs() < 1e-9); + assert!((frame3.idot - 1.548414729768e-010).abs() < 1E-9); + assert!((frame3.omega0 - -6.871047024615e-001).abs() < 1e-9); + assert!((frame3.omega_dot - -2.449269231874e-009).abs() < 1e-9); + assert!((frame3.omega - -6.554632573389e-001).abs() < 1e-9); + } +} diff --git a/src/gps/encoding.rs b/src/gps/encoding.rs index cdc1315..3bea231 100644 --- a/src/gps/encoding.rs +++ b/src/gps/encoding.rs @@ -1,120 +1,982 @@ -#[cfg(feature = "log")] -use log::debug; +use crate::gps::{ + GpsDataByte, GpsQzssFrame, GpsQzssFrameId, GpsQzssHow, GpsQzssSubframe, GpsQzssTelemetry, + GPS_FRAME_BITS, GPS_FRAME_BYTES, GPS_PREAMBLE_BYTE, GPS_WORDS_PER_FRAME, GPS_WORD_BITS, +}; -use crate::gps::{GpsError, GpsQzssFrame, State, GPS_MIN_SIZE, GPS_PREAMBLE_MASK}; +#[cfg(feature = "log")] +use log::{error, trace}; impl GpsQzssFrame { - /// Encodes this [GpsQzssFrame] into preallocated buffer in which - /// the frame must fit completely. We consider the minimal - /// allocation should be [GPS_MIN_SIZE]. - /// - /// GPS is MSB first, - /// least significant bit first inside each bytes, - /// and uses 30 bit data words, NB: the output is not aligned - /// to 32 bits. - /// - /// ## Input - /// - buf: preallocated buffer. - /// - size: available size in buffer. If this value - /// is not up to date, the program may panic. - /// If size available is greater than [GPS_MIN_SIZE], this will never happen. - /// - /// ## Output - /// - Ok(size) : encoded size, when everything went fine - pub fn encode(&self, buf: &mut [u8], size: usize) -> Result { - if size < GPS_MIN_SIZE { - return Err(GpsError::WouldNotFit); + /// Returns total number of bytes needed to encode this [GpsQzssFrame] to binary + /// aligned to [u8] + pub const fn encoding_size() -> usize { + GPS_FRAME_BYTES + } + + /// Returns exact number of bits needed to encode this [GpsQzssFrame] + pub const fn encoding_bits() -> usize { + GPS_FRAME_BITS + } + + /// Encodes this [GpsQzssFrame] as a 300 bit burst (38 bytes). + /// Because [GpsQzssFrame] is not aligned to [u8], the very last byte contains 4 MSB padding bits, set to zeros + /// (unsigned). If you leave it to that, any streaming/transmitter looses a little bit of efficiency + /// any time a [GpsQzssFrame] is encoded/transmitted. The only solution then, is to manually remove this padding + /// an truly concatenate your frames, but one can't expect easy processes when working with poorly designed and very old protocols. + /// A true synchronous [GpsQzssFrame] emitter is supposed to transmit one frame every 6 seconds, + /// that is 50 bits per second. + /// NB: this [GpsQzssFrame] is not ready to transmit as-is and must be CDMA encoded + /// using [GpsQzssFrame::cdma_encoding]. + pub fn encode(&self) -> [u8; GPS_FRAME_BYTES] { + let mut encoded = [0; GPS_FRAME_BYTES]; + + encoded[0] = GPS_PREAMBLE_BYTE; + encoded[1] = ((self.telemetry.message & 0xff00) >> 8) as u8; + + encoded[2] = (self.telemetry.message & 0xff) as u8; + encoded[2] <<= 2; + + if self.telemetry.integrity { + encoded[2] |= 0x02; + } + + if self.telemetry.reserved_bit { + encoded[2] |= 0x01; } - let mut ptr = 0; - let mut binoffset = 0; + encoded[3] <<= 2; // TODO + encoded[3] |= ((self.how.tow & 0x1_8000) >> 15) as u8; - let mut state = State::default(); - let mut prev_state = state; + encoded[4] = ((self.how.tow & 0x0_7f80) >> 7) as u8; + + encoded[5] = (self.how.tow & 0x0_007f) as u8; + encoded[5] <<= 1; + + if self.how.alert { + encoded[5] |= 0x01; + } - loop { - if ptr == size { - // would panic - unreachable!("buffer to small"); - } - #[cfg(feature = "log")] - debug!("state={:?} | ptr={}", state, ptr); + if self.how.anti_spoofing { + encoded[6] |= 0x80; } - Ok(size) + encoded[6] |= self.how.frame_id.encode() << 4; + + encoded[7] <<= 4; // TODO + + match self.how.frame_id { + GpsQzssFrameId::Ephemeris1 => { + let subf = self.subframe.as_eph1().unwrap_or_default(); + + encoded[7] |= ((subf.week & 0x3c0) >> 6) as u8; + encoded[8] = (subf.week & 0x03f) as u8; + encoded[8] <<= 2; + encoded[8] |= (subf.ca_or_p_l2) & 0x03; + + encoded[9] = subf.ura & 0x0f; + encoded[9] <<= 4; + encoded[9] |= (subf.health & 0x3c) >> 2; + + encoded[10] |= (subf.health & 0x03); + encoded[10] <<= 6; + encoded[10] |= (((subf.iodc & 0x300) >> 8) as u8) << 4; + + encoded[11] <<= 6; // TODO + + if subf.l2_p_data_flag { + encoded[11] |= 0x20; + } + + encoded[11] |= ((subf.reserved_word4 & 0x7c_0000) >> 18) as u8; + encoded[12] = ((subf.reserved_word4 & 0x03_fc00) >> 10) as u8; + encoded[13] = ((subf.reserved_word4 & 0x00_03fc) >> 2) as u8; + + encoded[14] = (subf.reserved_word4 & 0x3) as u8; + encoded[14] <<= 6; // TODO + + encoded[15] = ((subf.reserved_word5 & 0xff_0000) >> 16) as u8; + encoded[16] = ((subf.reserved_word5 & 0x00_ff00) >> 8) as u8; + encoded[17] = ((subf.reserved_word5 & 0x00_00ff) >> 0) as u8; + encoded[18] <<= 2; // TODO + + encoded[18] |= ((subf.reserved_word6 & 0xc0_0000) >> 22) as u8; + encoded[19] = ((subf.reserved_word6 & 0x3f_c000) >> 14) as u8; + encoded[20] = ((subf.reserved_word6 & 0x00_3fc0) >> 6) as u8; + encoded[21] = ((subf.reserved_word6 & 0x00_003f) >> 0) as u8; + encoded[21] <<= 2; + encoded[22] <<= 4; // TODO + + encoded[22] |= ((subf.reserved_word7 & 0xf000) >> 12) as u8; + encoded[23] = ((subf.reserved_word7 & 0x0ff0) >> 4) as u8; + encoded[24] = ((subf.reserved_word7 & 0x000f) >> 0) as u8; + encoded[24] <<= 4; + + let tgd = (subf.tgd * 2.0_f64.powi(31)).round() as u8; + encoded[24] |= (tgd & 0xf0) >> 4; + + encoded[25] = tgd & 0x0f; + encoded[25] <<= 4; // TODO + encoded[26] <<= 6; // TODO + encoded[26] |= ((subf.iodc & 0x0fc) >> 2) as u8; + + encoded[27] = (subf.iodc & 0x03) as u8; + encoded[27] <<= 6; + + let toc = subf.toc * 16; + encoded[27] = ((toc & 0xfc00) >> 10) as u8; + encoded[28] = ((toc & 0x03fc) >> 2) as u8; + encoded[29] = (toc & 0x0003) as u8; + encoded[29] <<= 6; // TODO + + let af2 = (subf.af2 * 2.0_f64.powi(10)).round() as u8; + let af1 = (subf.af1 * 2.0_f64.powi(10)).round() as u16; + let af0 = (subf.af0 * 2.0_f64.powi(10)).round() as u32; + + encoded[30] = af2; + encoded[31] = ((af1 & 0xff00) >> 8) as u8; + encoded[32] = ((af1 & 0x00ff) >> 0) as u8; + encoded[33] <<= 2; // TODO + + encoded[33] |= ((af0 & 0x30_0000) >> 20) as u8; + encoded[34] = ((af0 & 0x0f_f000) >> 12) as u8; + encoded[35] = ((af0 & 0x00_0ff0) >> 4) as u8; + encoded[36] = ((af0 & 0x00_000f) >> 0) as u8; + encoded[36] <<= 4; // TODO + + encoded[37] <<= 4; + }, + GpsQzssFrameId::Ephemeris2 => { + let subf = self.subframe.as_eph2().unwrap_or_default(); + + encoded[7] |= (subf.iode & 0xf0) >> 4; + encoded[8] |= subf.iode & 0x0f; + encoded[8] <<= 4; + + let crs = (subf.crs * 2.0_f64.powi(5)).round() as u16; + encoded[8] |= ((crs & 0xf000) >> 12) as u8; + encoded[9] |= ((crs & 0x0ff0) >> 4) as u8; + encoded[10] |= (crs & 0x000f) as u8; + encoded[10] <<= 4; // TODO + + let dn = (subf.dn * 2.0_f64.powi(43)).round() as u16; + encoded[11] |= ((dn & 0xfc00) >> 10) as u8; + + encoded[12] |= ((dn & 0x03fc) >> 2) as u8; + encoded[13] |= (dn & 0x0003) as u8; + encoded[13] <<= 6; + + let m0 = (subf.m0 * 2.0_f64.powi(31)).round() as u32; + + encoded[13] |= ((m0 & 0xfc000000) >> 26) as u8; + encoded[14] |= ((m0 & 0x03000000) >> 24) as u8; + encoded[14] <<= 6; //TODO + + encoded[15] |= ((m0 & 0x00ff0000) >> 16) as u8; + encoded[16] |= ((m0 & 0x0000ff00) >> 8) as u8; + encoded[17] |= (m0 & 0x000000ff) as u8; + + encoded[18] <<= 2; // TODO + + let cuc = (subf.cuc * 2.0_f64.powi(29)).round() as u16; + encoded[18] |= ((cuc & 0xc000) >> 14) as u8; + encoded[19] |= ((cuc & 0x3fc0) >> 6) as u8; + encoded[20] |= (cuc & 0x003f) as u8; + encoded[20] <<= 2; + + let e = (subf.e * 2.0_f64.powi(33)).round() as u32; + + encoded[20] |= ((e & 0xc0000000) >> 30) as u8; + encoded[21] |= ((e & 0x3f000000) >> 24) as u8; + encoded[21] <<= 2; // TODO + encoded[22] <<= 4; // TODO + + encoded[22] |= ((e & 0x00f00000) >> 20) as u8; + encoded[23] |= ((e & 0x000ff000) >> 12) as u8; + encoded[24] |= ((e & 0x00000ff0) >> 4) as u8; + encoded[25] |= (e & 0x0000000f) as u8; + encoded[25] <<= 4; // TODO + encoded[26] <<= 2; // TODO + + let cus = (subf.cus * 2.0_f64.powi(29)).round() as u16; + encoded[26] |= ((cus & 0xfc00) >> 10) as u8; + encoded[27] |= ((cus & 0x03fc) >> 2) as u8; + encoded[28] |= (cus & 0x3) as u8; + encoded[28] <<= 6; + + let sqrt_a = (subf.sqrt_a * 2.0_f64.powi(19)).round() as u32; + encoded[28] |= ((sqrt_a & 0xfc000000) >> 26) as u8; + encoded[29] |= ((sqrt_a & 0x03000000) >> 24) as u8; + encoded[29] <<= 6; // TODO + + encoded[30] |= ((sqrt_a & 0x00ff0000) >> 16) as u8; + encoded[31] |= ((sqrt_a & 0x0000ff00) >> 8) as u8; + encoded[32] |= (sqrt_a & 0x000000ff) as u8; + + let toe = (subf.toe * 16) as u16; + + encoded[33] <<= 2; // TODO + encoded[33] |= ((toe & 0xc000) >> 14) as u8; + encoded[34] |= ((toe & 0x3fc0) >> 6) as u8; + encoded[35] |= (toe & 0x003f) as u8; + encoded[35] <<= 2; + + if subf.fit_int_flag { + encoded[35] |= 0x02; + } + + encoded[35] |= (subf.aodo & 0x10) >> 4; + + encoded[36] |= subf.aodo & 0x0f; + encoded[36] <<= 4; + + encoded[36] |= 0x00; // two non-information bits for parity calculations + encoded[37] |= 0x00; + encoded[37] <<= 4; // TODO + }, + GpsQzssFrameId::Ephemeris3 => { + let subf = self.subframe.as_eph3().unwrap_or_default(); + + let cic = (subf.cic * 2.0_f64.powi(29)).round() as u16; + + encoded[7] |= ((cic & 0xf000) >> 12) as u8; + encoded[8] |= (cic & 0x0ff0) as u8; + encoded[8] <<= 4; + + encoded[9] |= (cic & 0x000f) as u8; + encoded[9] <<= 4; + }, + } + + encoded + } + + /// Encodes this [GpsQzssFrame] into mutable [u8] buffer directly. + /// Returns the total number of _bits_ (not bytes) that were encoded. + /// Note that [GpsQzssFrame] is not aligned to [u8] so the final bytes is padded. + /// You can use the returned size here, to create a truly contiguous stream, + /// otherwise, you slighthly degraed the transmission efficiency. + /// + /// This method is particularly suited when working with a real-time GPS decoder. + /// Otherwise, you might want to use our [GpsQzssDecoder] structure. + /// + /// This is [] mirror operation. + /// + /// ## Inputs + /// - buffer: mutable [u8] buffer + /// + /// ## Output + /// - Err if buffer can't accept this [GpsQzssFrame] entirely + /// - total number of bits that were encoded + pub fn encode_to_buffer(&self, buffer: &mut [u8]) -> std::io::Result { + if buffer.len() < GPS_FRAME_BYTES { + return Err(std::io::Error::new( + std::io::ErrorKind::StorageFull, + "would not fit", + )); + } + + let encoded = self.encode(); + buffer[..GPS_FRAME_BYTES].copy_from_slice(&self.encode()); + Ok(GPS_FRAME_BYTES) } } #[cfg(test)] -mod test { - use crate::{ - gps::{ - GpsQzssFrame, GpsQzssFrameId, GpsQzssHow, GpsQzssSubframe, GpsQzssTelemetry, - GPS_MIN_SIZE, - }, - GpsQzssDecoder, GPS_PREAMBLE_MASK, - }; +mod encoding { + use std::fs::File; + use std::io::{Read, Write}; - #[cfg(feature = "log")] + #[cfg(all(feature = "std", feature = "log"))] use crate::tests::init_logger; - #[cfg(feature = "log")] - use log::info; + use crate::gps::{ + GpsQzssDecoder, GpsQzssFrame, GpsQzssFrame1, GpsQzssFrame2, GpsQzssFrame3, GpsQzssFrameId, + GpsQzssHow, GpsQzssSubframe, GpsQzssTelemetry, GPS_FRAME_BITS, GPS_FRAME_BYTES, + }; + + #[test] + fn encoding_size() { + assert_eq!(GpsQzssFrame::encoding_size(), 300 / 8 + 1); + } + + #[test] + fn encoding_bits() { + assert_eq!(GpsQzssFrame::encoding_bits(), 300); + } + + #[test] + fn default_frame() { + let default = GpsQzssFrame::default(); + let encoded = default.encode(); + let encoded_size = encoded.len(); + + assert_eq!(GpsQzssFrame::encoding_size(), 300 / 8 + 1); + assert_eq!(GpsQzssFrame::encoding_bits(), 300); + + assert_eq!(encoded_size, GPS_FRAME_BYTES, "encoded invalid size!"); + + assert_eq!(encoded[0], 0x8B, "does not start with preamble bits"); + assert_eq!(encoded[1], 0x00); + assert_eq!(encoded[2], 0x00); + assert_eq!(encoded[3], 0x00); + assert_eq!(encoded[4], 0x00); + assert_eq!(encoded[5], 0x00); + assert_eq!(encoded[6], 0x10); + assert_eq!(encoded[7], 0x00); + assert_eq!(encoded[8], 0x00); + assert_eq!(encoded[9], 0x00); + assert_eq!(encoded[10], 0x00); + assert_eq!(encoded[11], 0x00); + assert_eq!(encoded[12], 0x00); + assert_eq!(encoded[13], 0x00); + assert_eq!(encoded[14], 0x00); + assert_eq!(encoded[15], 0x00); + assert_eq!(encoded[16], 0x00); + assert_eq!(encoded[17], 0x00); + assert_eq!(encoded[18], 0x00); + assert_eq!(encoded[19], 0x00); + assert_eq!(encoded[20], 0x00); + assert_eq!(encoded[21], 0x00); + assert_eq!(encoded[22], 0x00); + assert_eq!(encoded[23], 0x00); + assert_eq!(encoded[24], 0x00); + assert_eq!(encoded[25], 0x00); + assert_eq!(encoded[26], 0x00); + assert_eq!(encoded[27], 0x00); + assert_eq!(encoded[28], 0x00); + assert_eq!(encoded[29], 0x00); + assert_eq!(encoded[30], 0x00); + assert_eq!(encoded[31], 0x00); + assert_eq!(encoded[32], 0x00); + assert_eq!(encoded[33], 0x00); + assert_eq!(encoded[34], 0x00); + assert_eq!(encoded[35], 0x00); + assert_eq!(encoded[36], 0x00); + assert_eq!(encoded[37], 0x00); + + // let mut decoder = GpsQzssDecoder::default(); + + // let (size, decoded) = decoder.decode(&encoded, encoded_size); + // assert_eq!(size, GPS_FRAME_BITS, "invalid size processed!"); + // assert_eq!(decoded, Some(default), "reciprocal failed"); + } #[test] - fn gps_qzss_frame1_encoding() { - #[cfg(feature = "log")] + fn ephemeris1_0() { + #[cfg(all(feature = "std", feature = "log"))] init_logger(); - let frame = GpsQzssFrame { - how: crate::GpsQzssHow { - tow: 1, - alert: true, - anti_spoofing: false, - frame_id: GpsQzssFrameId::Ephemeris1, - }, - telemetry: GpsQzssTelemetry { - message: 10, - integrity: true, - reserved_bits: false, - }, - subframe: GpsQzssSubframe::Eph1(crate::GpsQzssFrame1 { - week: 11, - ca_or_p_l2: 4, - ura: 5, - health: 6, - iodc: 7, - toc: 8, - tgd: 9.0, - af2: 10.0, - af1: 11.0, - af0: 12.0, - reserved_word4: 13, - l2_p_data_flag: false, - reserved_word5: 14, - reserved_word6: 15, - reserved_word7: 16, - }), - }; - - let mut buffer = [0u8; GPS_MIN_SIZE]; - - let encoded = frame.encode(&mut buffer, GPS_MIN_SIZE).unwrap(); - - assert_eq!( - buffer[0], GPS_PREAMBLE_MASK, - "encoded first byte is not preamble!" - ); - - assert_eq!(buffer[1], 0x00); - - #[cfg(feature = "log")] - info!("decoding!"); - - let mut decoder = GpsQzssDecoder::default().without_parity_verification(); - - // for byte in buffer.iter() { - // let decoded = decoder.parse(Byte::byte(*byte)); - // } + let mut decoder = GpsQzssDecoder::default(); + + let frame = GpsQzssFrame::default() + .with_telemetry( + GpsQzssTelemetry::default() + .with_message(0x1234) + .with_integrity() + .with_reserved_bit(), + ) + .with_how_word( + GpsQzssHow::default() + .with_tow_seconds(0x5_6789) + .with_alert_bit() + .with_anti_spoofing(), + ) + .with_subframe(GpsQzssSubframe::Ephemeris1( + GpsQzssFrame1::default() + .with_week(0x123) + .with_iodc(0x123) + .with_all_signals_ok() + .with_l2p_flag() + .with_reserved23_word(0x12_3456) + .with_reserved24_word1(0x34_5678) + .with_reserved24_word2(0x98_7654) + .with_total_group_delay_nanos(5.0) + .with_ca_or_p_l2_mask(0x3) + .with_user_range_accuracy_m(4.0), + )); + + let encoded = frame.encode(); + + assert_eq!(encoded[0], 0x8B, "does not start with preamble bits"); + assert_eq!(encoded[1], 0x12); + assert_eq!(encoded[2], 0x34 << 2 | 0x02 | 0x01); + assert_eq!(encoded[3], 0x02); + + assert_eq!(encoded[4], 0xCF); + assert_eq!(encoded[5], 0x13); + assert_eq!(encoded[6], 0x90); + assert_eq!(encoded[7], 0x04); + + assert_eq!(encoded[8], 0x8f); + assert_eq!(encoded[9], 0x20); + assert_eq!(encoded[10], 0x10); + assert_eq!(encoded[11], 0x24); + assert_eq!(encoded[12], 0x8D); + assert_eq!(encoded[13], 0x15); + assert_eq!(encoded[14], 0x80); + assert_eq!(encoded[15], 0x34); + assert_eq!(encoded[16], 0x56); + assert_eq!(encoded[17], 0x78); + assert_eq!(encoded[18], 0x02); + assert_eq!(encoded[19], 0x61); + assert_eq!(encoded[20], 0xD9); + assert_eq!(encoded[21], 0x50); + assert_eq!(encoded[22], 0x00); + assert_eq!(encoded[23], 0x00); + assert_eq!(encoded[24], 0x00); + assert_eq!(encoded[25], 0x0B); + assert_eq!(encoded[26], 0x00); + assert_eq!(encoded[27], 0x00); + assert_eq!(encoded[28], 0x00); + assert_eq!(encoded[29], 0x00); + assert_eq!(encoded[30], 0x00); + assert_eq!(encoded[31], 0x00); + assert_eq!(encoded[32], 0x00); + assert_eq!(encoded[33], 0x00); + assert_eq!(encoded[34], 0x00); + assert_eq!(encoded[35], 0x00); + assert_eq!(encoded[36], 0x00); + assert_eq!(encoded[37], 0x00); + + let frame = GpsQzssFrame::default() + .with_telemetry( + GpsQzssTelemetry::default() + .with_message(0x1234) + .without_integrity() + .with_reserved_bit(), + ) + .with_how_word( + GpsQzssHow::default() + .with_tow_seconds(0x4_6789) + .with_alert_bit() + .without_anti_spoofing(), + ) + .with_subframe(GpsQzssSubframe::Ephemeris1( + GpsQzssFrame1::default() + .with_week(0x123) + .with_iodc(0x345) + .with_all_signals_ok() + .with_ca_or_p_l2_mask(0x1) + .with_user_range_accuracy_m(24.0), + )); + + let encoded = frame.encode(); + + assert_eq!(encoded[0], 0x8B, "does not start with preamble bits"); + assert_eq!(encoded[1], 0x12); + assert_eq!(encoded[2], 0x34 << 2 | 0x01); + assert_eq!(encoded[3], 0x00); + + assert_eq!(encoded[4], 0xCF); + assert_eq!(encoded[5], 0x13); + assert_eq!(encoded[6], 0x10); + assert_eq!(encoded[7], 0x04); + + assert_eq!(encoded[8], 0x8D); + assert_eq!(encoded[9], 0x60); + assert_eq!(encoded[10], 0x30); + assert_eq!(encoded[11], 0x00); + assert_eq!(encoded[12], 0x00); + assert_eq!(encoded[13], 0x00); + assert_eq!(encoded[14], 0x00); + assert_eq!(encoded[15], 0x00); + assert_eq!(encoded[16], 0x00); + assert_eq!(encoded[17], 0x00); + assert_eq!(encoded[18], 0x00); + assert_eq!(encoded[19], 0x00); + assert_eq!(encoded[20], 0x00); + assert_eq!(encoded[21], 0x00); + assert_eq!(encoded[22], 0x00); + assert_eq!(encoded[23], 0x00); + assert_eq!(encoded[24], 0x00); + assert_eq!(encoded[25], 0x00); + assert_eq!(encoded[26], 0x00); + assert_eq!(encoded[27], 0x00); + assert_eq!(encoded[28], 0x00); + assert_eq!(encoded[29], 0x00); + assert_eq!(encoded[30], 0x00); + assert_eq!(encoded[31], 0x00); + assert_eq!(encoded[32], 0x00); + assert_eq!(encoded[33], 0x00); + assert_eq!(encoded[34], 0x00); + assert_eq!(encoded[35], 0x00); + assert_eq!(encoded[36], 0x00); + assert_eq!(encoded[37], 0x00); + + let frame = GpsQzssFrame::default() + .with_telemetry( + GpsQzssTelemetry::default() + .with_message(0x0123) + .without_integrity() + .without_reserved_bit(), + ) + .with_how_word( + GpsQzssHow::default() + .with_tow_seconds(0x0_1234) + .without_alert_bit() + .without_anti_spoofing(), + ) + .with_subframe(GpsQzssSubframe::default()); + + let encoded = frame.encode(); + + assert_eq!(encoded[0], 0x8B, "does not start with preamble bits"); + assert_eq!(encoded[1], 0x01); + assert_eq!(encoded[2], 0x23 << 2); + assert_eq!(encoded[3], 0x00); + + assert_eq!(encoded[4], 0x24); + assert_eq!(encoded[5], 0x68); + assert_eq!(encoded[6], 0x10); + assert_eq!(encoded[7], 0x00); + + assert_eq!(encoded[8], 0x00); + assert_eq!(encoded[9], 0x00); + assert_eq!(encoded[10], 0x00); + assert_eq!(encoded[11], 0x00); + assert_eq!(encoded[12], 0x00); + assert_eq!(encoded[13], 0x00); + assert_eq!(encoded[14], 0x00); + assert_eq!(encoded[15], 0x00); + assert_eq!(encoded[16], 0x00); + assert_eq!(encoded[17], 0x00); + assert_eq!(encoded[18], 0x00); + assert_eq!(encoded[19], 0x00); + assert_eq!(encoded[20], 0x00); + assert_eq!(encoded[21], 0x00); + assert_eq!(encoded[22], 0x00); + assert_eq!(encoded[23], 0x00); + assert_eq!(encoded[24], 0x00); + assert_eq!(encoded[25], 0x00); + assert_eq!(encoded[26], 0x00); + assert_eq!(encoded[27], 0x00); + assert_eq!(encoded[28], 0x00); + assert_eq!(encoded[29], 0x00); + assert_eq!(encoded[30], 0x00); + assert_eq!(encoded[31], 0x00); + assert_eq!(encoded[32], 0x00); + assert_eq!(encoded[33], 0x00); + assert_eq!(encoded[34], 0x00); + assert_eq!(encoded[35], 0x00); + assert_eq!(encoded[36], 0x00); + assert_eq!(encoded[37], 0x00); + } + + #[test] + fn ephemeris1_1() { + #[cfg(all(feature = "std", feature = "log"))] + init_logger(); + + let mut decoder = GpsQzssDecoder::default(); + + for (ith, (tow, alert, anti_spoofing, frame_id, message, integrity, tlm_reserved_bit)) in + [( + 259956, + false, + false, + GpsQzssFrameId::Ephemeris1, + 0x13E, + false, + false, + )] + .iter() + .enumerate() + { + let frame = GpsQzssFrame { + how: GpsQzssHow { + tow: *tow, + alert: *alert, + frame_id: *frame_id, + anti_spoofing: *anti_spoofing, + }, + telemetry: GpsQzssTelemetry { + message: *message, + integrity: *integrity, + reserved_bit: *tlm_reserved_bit, + }, + subframe: Default::default(), + }; + + let encoded = frame.encode(); + let encoded_size = encoded.len(); + assert_eq!(encoded_size, GPS_FRAME_BYTES, "encoded invalid size!"); + + // let (size, decoded) = decoder.decode(&encoded, encoded_size); + // assert_eq!(size, GPS_FRAME_BITS, "invalid size processed!"); + // assert_eq!(decoded, Some(frame), "reciprocal failed"); + } + } + + #[test] + fn ephemeris2_0() { + #[cfg(all(feature = "std", feature = "log"))] + init_logger(); + + let mut decoder = GpsQzssDecoder::default(); + + let frame = GpsQzssFrame::default() + .with_telemetry( + GpsQzssTelemetry::default() + .with_message(0x9999) + .with_integrity() + .with_reserved_bit(), + ) + .with_how_word( + GpsQzssHow::default() + .with_tow_seconds(0x9_9999) + .with_alert_bit() + .with_anti_spoofing(), + ) + .with_subframe(GpsQzssSubframe::Ephemeris2( + GpsQzssFrame2::default() + .with_iode(0x12) + .with_crs_meters(1.8) + .with_mean_motion_difference_semi_circles(100.0) + .with_mean_anomaly_semi_circles(9.768415465951e-001) + .with_toe(266_400) + .with_square_root_semi_major_axis(5.153602432251e+003) + .with_fit_interval_flag() + .with_aodo(0x15), + )); + + let encoded = frame.encode(); + + assert_eq!(encoded[0], 0x8B, "does not start with preamble bits"); + assert_eq!(encoded[1], 0x19); + assert_eq!(encoded[2], 0x99 << 2 | 0x02 | 0x01); + assert_eq!(encoded[3], 0x03); + + assert_eq!(encoded[4], 0x33); + assert_eq!(encoded[5], 0x33); + assert_eq!(encoded[6], 0xA0); + assert_eq!(encoded[7], 0x01); + + assert_eq!(encoded[8], 0x20); + assert_eq!(encoded[9], 0x03); + assert_eq!(encoded[10], 0xA0); + assert_eq!(encoded[11], 0x3F); + + assert_eq!(encoded[12], 0xFF); + assert_eq!(encoded[13], 0xDF); + assert_eq!(encoded[14], 0x40); + assert_eq!(encoded[15], 0x09); + assert_eq!(encoded[16], 0x24); + assert_eq!(encoded[17], 0xD0); + assert_eq!(encoded[18], 0x00); + assert_eq!(encoded[19], 0x00); + assert_eq!(encoded[20], 0x00); + assert_eq!(encoded[21], 0x00); + assert_eq!(encoded[22], 0x00); + assert_eq!(encoded[23], 0x00); + assert_eq!(encoded[24], 0x00); + assert_eq!(encoded[25], 0x00); + assert_eq!(encoded[26], 0x00); + assert_eq!(encoded[27], 0x00); + assert_eq!(encoded[28], 40); + assert_eq!(encoded[29], 64); + assert_eq!(encoded[30], 12); + assert_eq!(encoded[31], 209); + assert_eq!(encoded[32], 200); + assert_eq!(encoded[33], 0x00); + assert_eq!(encoded[34], 40); + assert_eq!(encoded[35], 0x03); + assert_eq!(encoded[36], 0x50); + assert_eq!(encoded[37], 0x00); + + let frame = GpsQzssFrame::default() + .with_telemetry( + GpsQzssTelemetry::default() + .with_message(0x9999) + .with_integrity() + .with_reserved_bit(), + ) + .with_how_word( + GpsQzssHow::default() + .with_tow_seconds(0x9_9999) + .without_alert_bit() + .with_anti_spoofing(), + ) + .with_subframe(GpsQzssSubframe::Ephemeris2( + GpsQzssFrame2::default().with_iode(0x34), + )); + + let encoded = frame.encode(); + + assert_eq!(encoded[0], 0x8B, "does not start with preamble bits"); + assert_eq!(encoded[1], 0x19); + assert_eq!(encoded[2], 0x99 << 2 | 0x02 | 0x01); + assert_eq!(encoded[3], 0x03); + + assert_eq!(encoded[4], 0x33); + assert_eq!(encoded[5], 0x32); + assert_eq!(encoded[6], 0xA0); + assert_eq!(encoded[7], 0x03); + + assert_eq!(encoded[8], 0x40); + assert_eq!(encoded[9], 0x00); + assert_eq!(encoded[10], 0x00); + assert_eq!(encoded[11], 0x00); + + assert_eq!(encoded[12], 0x00); + assert_eq!(encoded[13], 0x00); + assert_eq!(encoded[14], 0x00); + assert_eq!(encoded[15], 0x00); + + assert_eq!(encoded[16], 0x00); + assert_eq!(encoded[17], 0x00); + assert_eq!(encoded[18], 0x00); + assert_eq!(encoded[19], 0x00); + + assert_eq!(encoded[20], 0x00); + assert_eq!(encoded[21], 0x00); + assert_eq!(encoded[22], 0x00); + assert_eq!(encoded[23], 0x00); + assert_eq!(encoded[24], 0x00); + assert_eq!(encoded[25], 0x00); + assert_eq!(encoded[26], 0x00); + assert_eq!(encoded[27], 0x00); + assert_eq!(encoded[28], 0x00); + assert_eq!(encoded[29], 0x00); + assert_eq!(encoded[30], 0x00); + assert_eq!(encoded[31], 0x00); + assert_eq!(encoded[32], 0x00); + assert_eq!(encoded[33], 0x00); + assert_eq!(encoded[34], 0x00); + assert_eq!(encoded[35], 0x00); + assert_eq!(encoded[36], 0x00); + assert_eq!(encoded[37], 0x00); + } + + #[test] + fn generate_eph1_bin() { + let mut fd = File::create("data/GPS/eph1.bin").unwrap_or_else(|e| { + panic!("Failed to create file: {}", e); + }); + + let mut frame = GpsQzssFrame::default() + .with_telemetry( + GpsQzssTelemetry::default() + .with_message(0x1234) + .with_integrity() + .with_reserved_bit(), + ) + .with_how_word( + GpsQzssHow::default() + .with_tow_seconds(0x5_6789) + .with_alert_bit() + .with_anti_spoofing(), + ) + .with_subframe(GpsQzssSubframe::Ephemeris1( + GpsQzssFrame1::default() + .with_week(0x123) + .with_iodc(0x1) + .with_all_signals_ok() + .with_l2p_flag() + .with_reserved23_word(0x12_3456) + .with_reserved24_word1(0x34_5678) + .with_reserved24_word2(0x98_7654) + .with_total_group_delay_nanos(5.0) + .with_ca_or_p_l2_mask(0x3) + .with_user_range_accuracy_m(4.0), + )); + + let encoded = frame.encode(); + + for i in 0..128 { + let encoded = frame.encode(); + + fd.write(&encoded).unwrap_or_else(|e| { + panic!("Failed to write encoded frame #{}: {}", i, e); + }); + + frame.telemetry.message += 1; + frame.telemetry.integrity = !frame.telemetry.integrity; + frame.telemetry.reserved_bit = !frame.telemetry.reserved_bit; + + frame.how.tow += 1; + frame.how.alert = !frame.how.alert; + frame.how.anti_spoofing = !frame.how.anti_spoofing; + + let subframe = frame.subframe.as_mut_eph1().unwrap(); + + subframe.week += 1; + subframe.ura += 1; + subframe.ca_or_p_l2 = subframe.ca_or_p_l2 ^ 0x3; + subframe.iodc += 1; + subframe.health += 1; + subframe.toc += 1; + + subframe.af0 += 1.0E-9; + subframe.af1 += 1.0E-10; + subframe.af2 += 1.0E-11; + + subframe.tgd += 1.0E-9; + + subframe.reserved_word4 += 1; + subframe.reserved_word5 += 1; + subframe.reserved_word6 += 1; + subframe.reserved_word7 += 1; + + subframe.l2_p_data_flag = !subframe.l2_p_data_flag; + } + } + + #[test] + fn generate_eph2_bin() { + let mut fd = File::create("data/GPS/eph2.bin").unwrap_or_else(|e| { + panic!("Failed to create file: {}", e); + }); + + let mut frame = GpsQzssFrame::default() + .with_telemetry( + GpsQzssTelemetry::default() + .with_message(0x3456) + .with_integrity() + .with_reserved_bit(), + ) + .with_how_word( + GpsQzssHow::default() + .with_tow_seconds(0x9_8765) + .with_alert_bit() + .with_anti_spoofing(), + ) + .with_subframe(GpsQzssSubframe::Ephemeris2( + GpsQzssFrame2::default() + .with_toe(54_321) + .with_iode(0x01) + .with_mean_anomaly_semi_circles(0.1) + .with_mean_motion_difference_semi_circles(0.1) + .with_square_root_semi_major_axis(0.1) + .with_eccentricity(0.1) + .with_aodo(0x12) + .with_cuc_radians(0.1) + .with_cus_radians(0.1) + .with_fit_interval_flag(), + )); + + let encoded = frame.encode(); + + for i in 0..128 { + let encoded = frame.encode(); + + fd.write(&encoded).unwrap_or_else(|e| { + panic!("Failed to write encoded frame #{}: {}", i, e); + }); + + frame.telemetry.message += 1; + frame.telemetry.integrity = !frame.telemetry.integrity; + frame.telemetry.reserved_bit = !frame.telemetry.reserved_bit; + + frame.how.tow += 1; + frame.how.alert = !frame.how.alert; + frame.how.anti_spoofing = !frame.how.anti_spoofing; + + let subframe = frame.subframe.as_mut_eph2().unwrap(); + + subframe.toe += 1; + subframe.aodo += 1; + subframe.iode += 1; + subframe.fit_int_flag = !subframe.fit_int_flag; + } + } + + #[test] + fn generate_burst_bin() { + let mut fd = File::create("data/GPS/burst.bin").unwrap_or_else(|e| { + panic!("Failed to create file: {}", e); + }); + + let mut eph_1 = GpsQzssFrame::default() + .with_telemetry( + GpsQzssTelemetry::default() + .with_message(0x3456) + .with_integrity() + .with_reserved_bit(), + ) + .with_how_word( + GpsQzssHow::default() + .with_tow_seconds(0x9_8765) + .with_alert_bit() + .with_anti_spoofing(), + ) + .with_subframe(GpsQzssSubframe::Ephemeris1( + GpsQzssFrame1::default() + .with_week(0x123) + .with_iodc(0x1) + .with_all_signals_ok() + .with_l2p_flag() + .with_reserved23_word(0x12_3456) + .with_reserved24_word1(0x34_5678) + .with_reserved24_word2(0x98_7654) + .with_total_group_delay_nanos(5.0) + .with_ca_or_p_l2_mask(0x3) + .with_user_range_accuracy_m(4.0), + )); + + let mut eph_2 = GpsQzssFrame::default() + .with_telemetry( + GpsQzssTelemetry::default() + .with_message(0x3457) + .with_integrity() + .with_reserved_bit(), + ) + .with_how_word( + GpsQzssHow::default() + .with_tow_seconds(0x9_8765) + .with_alert_bit() + .with_anti_spoofing(), + ) + .with_subframe(GpsQzssSubframe::Ephemeris2( + GpsQzssFrame2::default() + .with_toe(54_326) + .with_iode(0x01) + .with_mean_anomaly_semi_circles(0.1) + .with_mean_motion_difference_semi_circles(0.1) + .with_square_root_semi_major_axis(0.1) + .with_eccentricity(0.1) + .with_aodo(0x12) + .with_cuc_radians(0.1) + .with_cus_radians(0.1) + .with_fit_interval_flag(), + )); + + for i in 0..128 { + let encoded = eph_1.encode(); + + fd.write(&encoded).unwrap_or_else(|e| { + panic!("Failed to write encoded frame #{}: {}", i, e); + }); + + eph_1.telemetry.message += 1; + eph_1.telemetry.integrity = !eph_1.telemetry.integrity; + eph_1.telemetry.reserved_bit = !eph_1.telemetry.reserved_bit; + + eph_1.how.tow += 1; + eph_1.how.alert = !eph_1.how.alert; + eph_1.how.anti_spoofing = !eph_1.how.anti_spoofing; + + let subframe = eph_1.subframe.as_mut_eph1().unwrap(); + + let encoded = eph_2.encode(); + + fd.write(&encoded).unwrap_or_else(|e| { + panic!("Failed to write encoded frame #{}: {}", i, e); + }); + + eph_2.telemetry.message += 1; + eph_2.telemetry.integrity = !eph_2.telemetry.integrity; + eph_2.telemetry.reserved_bit = !eph_2.telemetry.reserved_bit; + + eph_2.how.tow += 1; + eph_2.how.alert = !eph_2.how.alert; + eph_2.how.anti_spoofing = !eph_2.how.anti_spoofing; + + let subframe = eph_2.subframe.as_mut_eph2().unwrap(); + + subframe.toe += 1; + subframe.aodo += 1; + subframe.iode += 1; + subframe.fit_int_flag = !subframe.fit_int_flag; + } } } diff --git a/src/gps/errors.rs b/src/gps/errors.rs index 59f952e..d60cc92 100644 --- a/src/gps/errors.rs +++ b/src/gps/errors.rs @@ -1,11 +1,22 @@ -#[derive(Debug)] +use thiserror::Error; + +#[derive(Error, Debug)] pub enum GpsError { /// Not a valid GPS preamble + #[error("invalid GPS preamble")] InvalidPreamble, /// Frame Type is either invalid or not supported + #[error("unknown GPS subframe type")] UnknownFrameType, + /// Internal error: internal FSM reached invalid state, + /// most likely due to corruption in the handling of successive + /// data words in the stream. Should never happen. + #[error("internal FSM error")] + InternalFSM, + /// Size is too small to encode a correct data frame + #[error("buffer to small for this GPS frame")] WouldNotFit, } diff --git a/src/gps/frame.rs b/src/gps/frame.rs new file mode 100644 index 0000000..9c241cb --- /dev/null +++ b/src/gps/frame.rs @@ -0,0 +1,46 @@ +use crate::gps::{ + GpsQzssHow, + GpsQzssTelemetry, + GpsQzssSubframe, +}; + +/// GPS / QZSS interpreted frame. +#[derive(Debug, Default, Copy, Clone, PartialEq)] +pub struct GpsQzssFrame { + /// [GpsQzssHow] describes following frame. + pub how: GpsQzssHow, + + /// [GpsQzssTelemetry] describes following frame. + pub telemetry: GpsQzssTelemetry, + + /// [GpsQzssSubframe] depends on associated How. + pub subframe: GpsQzssSubframe, +} + +impl GpsQzssFrame { + /// Copies and returns with updated [GpsQzssHow] data word + pub fn with_how_word(mut self, how: GpsQzssHow) -> Self { + self.how = how; + self + } + + /// Copies and returns with updated [GpsQzssTelemetry] data word + pub fn with_telemetry(mut self, telemetry: GpsQzssTelemetry) -> Self { + self.telemetry = telemetry; + self + } + + /// Copies and returns an updated [GpsQzssSubframe] + pub fn with_subframe(mut self, subframe: GpsQzssSubframe) -> Self { + self.subframe = subframe; + + match subframe { + GpsQzssSubframe::Ephemeris1(_) => self.how.frame_id = GpsQzssFrameId::Ephemeris1, + GpsQzssSubframe::Ephemeris2(_) => self.how.frame_id = GpsQzssFrameId::Ephemeris2, + GpsQzssSubframe::Ephemeris3(_) => self.how.frame_id = GpsQzssFrameId::Ephemeris3, + } + + self + } +} + diff --git a/src/gps/frame1.rs b/src/gps/frame1.rs index 212c32a..7d57a07 100644 --- a/src/gps/frame1.rs +++ b/src/gps/frame1.rs @@ -1,4 +1,7 @@ -use crate::twos_complement; +use crate::{ + gps::{GpsDataByte, GpsDataWord, GpsError, GPS_SUBFRAME_BITS, GPS_WORDS_PER_FRAME}, + twos_complement, +}; const WORD3_WEEK_MASK: u32 = 0x3ff00000; const WORD3_WEEK_SHIFT: u32 = 20; @@ -39,10 +42,10 @@ const WORD9_AF1_SHIFT: u32 = 6; const WORD10_AF0_MASK: u32 = 0x3fffff00; const WORD10_AF0_SHIFT: u32 = 8; -/// GPS / QZSS Frame #1 interpretation +/// [GpsQzssFrame1] Ephemeris #1 frame interpretation. #[derive(Debug, Default, Copy, Clone, PartialEq)] pub struct GpsQzssFrame1 { - /// 10-bit week counter (no rollover compensation). + /// 10-bit wrapped week counter. pub week: u16, /// 2-bit C/A or P ON L2. @@ -66,10 +69,6 @@ pub struct GpsQzssFrame1 { /// - 13: 1536.00 < ura <= 3072.00 /// - 14: 3072.00 < ura <= 6144.00 /// - 15: 6144.00 < ura - /// - /// For each URA index, users may compute a nominal URA value (x) - /// - ura < 6: 2**(1+N/2) - /// - ura > 6: 2**(N-2) pub ura: u8, /// 6-bit SV Health. 0 means all good. @@ -84,18 +83,19 @@ pub struct GpsQzssFrame1 { /// 8-bit TGD (in seconds) pub tgd: f64, - /// af2 (in seconds per s^-^&) + /// af2 (in seconds per squared second) pub af2: f64, - /// af1 (in seconds per seconds) + /// af1 (in seconds per second) pub af1: f64, - /// af0 (in seconds) + /// 22-bit af0 (in seconds) pub af0: f64, - /// 32-bit reserved word #4 + /// 23-bit reserved word #4 pub reserved_word4: u32, + /// 1-bit flag pub l2_p_data_flag: bool, /// 24-bit reserved word #5 @@ -104,54 +104,388 @@ pub struct GpsQzssFrame1 { /// 24-bit reserved word #6 pub reserved_word6: u32, - ///16-bit reserved word #7 + /// 16-bit reserved word #7 pub reserved_word7: u16, } impl GpsQzssFrame1 { + /// Computes binary URA from value in meters + fn compute_ura(value_m: f64) -> u8 { + if value_m <= 2.4 { + 0 + } else if value_m <= 3.4 { + 1 + } else if value_m <= 4.85 { + 2 + } else if value_m <= 6.85 { + 3 + } else if value_m <= 9.65 { + 4 + } else if value_m <= 13.65 { + 5 + } else if value_m <= 24.0 { + 6 + } else if value_m <= 48.0 { + 7 + } else if value_m <= 96.0 { + 8 + } else if value_m <= 192.0 { + 9 + } else if value_m <= 384.0 { + 10 + } else if value_m <= 768.0 { + 11 + } else if value_m <= 1536.0 { + 12 + } else if value_m <= 3072.0 { + 13 + } else if value_m <= 6144.0 { + 14 + } else { + 15 + } + } + + /// Calculates nominal User Range Accuracy in meters + pub fn nominal_user_range_accuracy(&self) -> f64 { + // For each URA index, users may compute a nominal URA value (x) + // - ura < 6: 2**(1+N/2) + // - ura > 6: 2**(N-2) + if self.ura <= 6 { + 2.0_f64.powi((1 + self.ura / 2) as i32) + } else { + 2.0_f64.powi((self.ura / 2) as i32) + } + } + + /// Copies and returns [GpsQzssFrame1] with updated Week number pub fn with_week(mut self, week: u16) -> Self { - self.week = week; + self.week = week & 0x3ff; + self + } + + /// Copies and returns [GpsQzssFrame1] with updated 10-bit IODC mask + pub fn with_iodc(mut self, iodc: u16) -> Self { + self.iodc = iodc & 0x3ff; + self + } + + /// Copies and returns [GpsQzssFrame1] with asserted L2P data flag + pub fn with_l2p_flag(mut self) -> Self { + self.l2_p_data_flag = true; + self + } + + /// Copies and returns [GpsQzssFrame1] with deasserted L2P data flag + pub fn without_l2p_flag(mut self) -> Self { + self.l2_p_data_flag = false; + self + } + + /// Copies and returns [GpsQzssFrame1] with updated 23-bit reserved word + pub fn with_reserved23_word(mut self, reserved: u32) -> Self { + self.reserved_word4 = reserved & 0x7f_ffff; + self + } + + /// Copies and returns [GpsQzssFrame1] with updated (first) 24-bit reserved word + pub fn with_reserved24_word1(mut self, reserved: u32) -> Self { + self.reserved_word5 = reserved & 0xff_ffff; + self + } + + /// Copies and returns [GpsQzssFrame1] with updated (second) 24-bit reserved word + pub fn with_reserved24_word2(mut self, reserved: u32) -> Self { + self.reserved_word6 = reserved & 0xff_ffff; + self + } + + /// Returns true if [GpsQzssFrame1] indicates all-signals are OK. + pub fn healthy(&self) -> bool { + self.health == 0 + } + + /// Returns true if [GpsQzssFrame1] indicates this satellite is temporarily + /// out of service + pub fn unavailable(&self) -> bool { + self.health == 0x1C + } + + /// Returns true if [GpsQzssFrame1] indicates this satellite has a pending + /// maintenance operation (should be used with caution) + pub fn pending_maintenance(&self) -> bool { + self.health == 0x1D + } + + /// Returns true if [GpsQzssFrame1] indicates this satellite is experiencing + /// code modulation or tranmission issues. + pub fn transmission_issues(&self) -> bool { + !self.healthy() + && !self.pending_maintenance() + && !self.unavailable() + && self.health != 0x1E + && self.health != 0x1F + } + + /// Copies and returns [GpsQzssFrame1] with all-signals marked as OK. + pub fn with_all_signals_ok(mut self) -> Self { + self.health = 0; + self + } + + /// Copies and returns [GpsQzssFrame1] with special status code marking + /// temporary unavailability (under maintenance operation). + pub fn with_unavailable_access(mut self) -> Self { + self.health = 0x1C; + self + } + + /// Copies and returns [GpsQzssFrame1] with special status code marking + /// future (scheduled) unavailability (pending maintenance operation). + pub fn with_pending_maintenance(mut self) -> Self { + self.health = 0x1D; + self + } + + /// Copies and returns [GpsQzssFrame1] with special status code marking + /// a transmission issue. + pub fn with_transmission_issue(mut self) -> Self { + self.health = 0x0C; + self + } + + /// Copies and returns [GpsQzssFrame1] with updated 6-bit health mask. + /// The MSB can be used to mask the non-healthiness. + /// The 5-LSB are health mask for each signal components. + pub fn with_health_mask(mut self, health: u8) -> Self { + self.health = health & 0x3f; + self + } + + /// Copies and returns [GpsQzssFrame1] with updated time of clock in seconds. + pub fn with_time_of_clock_seconds(mut self, toc_s: u32) -> Self { + self.toc = toc_s; self } - pub fn with_health(mut self, health: u8) -> Self { - self.health = health; + /// Copies and returns [GpsQzssFrame1] with updated Total Group Delay (TGD) in seconds + pub fn with_total_group_delay_seconds(mut self, tgd_s: f64) -> Self { + self.tgd = tgd_s; self } - pub fn with_toc(mut self, toc: u32) -> Self { - self.toc = toc; + /// Copies and returns [GpsQzssFrame1] with updated Total Group Delay (TGD) in nanoseconds + pub fn with_total_group_delay_nanos(mut self, tgd_nanos: f64) -> Self { + self.tgd = tgd_nanos * 1e-9; self } - pub fn with_tgd(mut self, tgd: f64) -> Self { - self.tgd = tgd; + /// Copies and returns [GpsQzssFrame1] with updated User Range Accuracy + /// in meters. + pub fn with_user_range_accuracy_m(mut self, ura_m: f64) -> Self { + self.ura = Self::compute_ura(ura_m); self } - pub fn with_ura(mut self, ura: u8) -> Self { - self.ura = ura; + /// Copies and returns [GpsQzssFrame1] with updated 2-bit C/A or P ON L2 mask. + pub fn with_ca_or_p_l2_mask(mut self, mask: u8) -> Self { + self.ca_or_p_l2 = mask & 0x3; self } + /// Copies and returns [GpsQzssFrame1] with updated User Range Accuracy + /// from a nominal User Range Accuracy in meters. + pub fn with_nominal_user_range_accuracy_m(mut self, ura_m: f64) -> Self { + // For each URA index, users may compute a nominal URA value (x) + // - ura < 6: 2**(1+N/2) + // - ura > 6: 2**(N-2) + let ura = if ura_m <= 24.0 { + 2.0 * (ura_m.log2() - 1.0) + } else { + 2.0 + ura_m.log2() + }; + + self.ura = ura.round() as u8; + self + } + + /// Copies and returns [GpsQzssFrame1] with updated clock correction (0) term pub fn with_af0(mut self, af0: f64) -> Self { self.af0 = af0; self } + /// Copies and returns [GpsQzssFrame1] with updated clock correction (1) term pub fn with_af1(mut self, af1: f64) -> Self { self.af1 = af1; self } + /// Copies and returns [GpsQzssFrame1] with updated clock correction (2) term pub fn with_af2(mut self, af2: f64) -> Self { self.af2 = af2; self } + + /// Decodes [Self] from 8 [GpsDataWord]s. + /// This method does not care for frames parity. + pub(crate) fn from_words(words: &[GpsDataWord; GPS_WORDS_PER_FRAME - 2]) -> Self { + let mut s = Self::default(); + + for i in 0..GPS_WORDS_PER_FRAME - 2 { + match i { + 0 => s.set_word3(Word3::from_word(words[i])), + 1 => s.set_word4(Word4::from_word(words[i])), + 2 => s.set_word5(Word5::from_word(words[i])), + 3 => s.set_word6(Word6::from_word(words[i])), + 4 => s.set_word7(Word7::from_word(words[i])), + 5 => s.set_word8(Word8::from_word(words[i])), + 6 => s.set_word9(Word9::from_word(words[i])), + 7 => s.set_word10(Word10::from_word(words[i])), + _ => unreachable!("expecting 8 data words"), + } + } + + s + } + + /// Decodes [Self] from a 240-bit stream. + /// This method does not care for frames parity. + pub(crate) fn from_raw(bytes: &[u8; GPS_SUBFRAME_BITS]) -> Self { + let mut s = Self::default(); + + s + } + + /// Updates scaled content from [Word3] + fn set_word3(&mut self, word: Word3) { + self.week = word.week; + self.ura = word.ura; + self.ca_or_p_l2 = word.ca_or_p_l2; + self.health = word.health; + self.iodc = (word.iodc_msb as u16) << 8; + } + + /// Encodes a [Word3] from [GpsQzssFrame1] + fn word3(&self) -> Word3 { + Word3 { + week: self.week, + ura: self.ura, + health: self.health, + ca_or_p_l2: self.ca_or_p_l2, + iodc_msb: ((self.iodc & 0x300) >> 8) as u8, + } + } + + /// Updates scaled content from [Word4] + fn set_word4(&mut self, word: Word4) { + self.l2_p_data_flag = word.l2_p_data_flag; + self.reserved_word4 = word.reserved; + } + + /// Encodes a [Word4] from [GpsQzssFrame1] + fn word4(&self) -> Word4 { + Word4 { + reserved: self.reserved_word4, + l2_p_data_flag: self.l2_p_data_flag, + } + } + + /// Updates scaled content from [Word5] + fn set_word5(&mut self, word: Word5) { + self.reserved_word5 = word.reserved; + } + + /// Encodes a [Word5] from [GpsQzssFrame1] + fn word5(&self) -> Word5 { + Word5 { + reserved: self.reserved_word5, + } + } + + /// Updates scaled content from [Word6] + fn set_word6(&mut self, word: Word6) { + self.reserved_word6 = word.reserved; + } + + /// Encodes a [Word6] from [GpsQzssFrame1] + fn word6(&self) -> Word6 { + Word6 { + reserved: self.reserved_word6, + } + } + + /// Updates scaled content from [Word7] + fn set_word7(&mut self, word: Word7) { + self.reserved_word7 = word.reserved; + self.tgd = (word.tgd as f64) / 2.0_f64.powi(31); + } + + /// Encodes a [Word7] from [GpsQzssFrame1] + fn word7(&self) -> Word7 { + Word7 { + reserved: self.reserved_word7, + tgd: (self.tgd * 2.0_f64.powi(31)).round() as i8, + } + } + + /// Updates scaled content from [Word8] + fn set_word8(&mut self, word: Word8) { + self.toc = (word.toc as u32) * 16; + self.iodc |= word.iodc_lsb as u16; + } + + /// Encodes a [Word8] from [GpsQzssFrame1] + fn word8(&self) -> Word8 { + Word8 { + toc: (self.toc / 16) as u16, + iodc_lsb: (self.iodc & 0xff) as u8, + } + } + + /// Updates scaled content from [Word9] + fn set_word9(&mut self, word: Word9) { + self.af2 = (word.af2 as f64) * 2.0_f64.powi(-55); + self.af1 = (word.af1 as f64) * 2.0_f64.powi(-43); + } + + /// Encodes a [Word9] from [GpsQzssFrame1] + fn word9(&self) -> Word9 { + Word9 { + af2: (self.af2 * 2.0_f64.powi(55)).round() as i8, + af1: (self.af1 * 2.0_f64.powi(43)).round() as i16, + } + } + + /// Updates scaled content from [Word10] + fn set_word10(&mut self, word: Word10) { + self.af0 = (word.af0 as f64) * 2.0_f64.powi(-31); + } + + /// Encodes a [Word10] from [GpsQzssFrame1] + fn word10(&self) -> Word10 { + Word10 { + af0: (self.af0 * 2.0_f64.powi(31)).round() as i32, + } + } + + /// Encodes this [GpsQzssFrame1] as a burst of 8 [GpsDataWord]s. + pub(crate) fn encode(&self) -> [GpsDataWord; GPS_WORDS_PER_FRAME - 2] { + [ + self.word3().to_word(), + self.word4().to_word(), + self.word5().to_word(), + self.word6().to_word(), + self.word7().to_word(), + self.word8().to_word(), + self.word9().to_word(), + self.word10().to_word(), + ] + } } -#[derive(Debug, Default, Clone)] -pub struct Word3 { +#[derive(Debug, Copy, Default, Clone, PartialEq)] +pub(crate) struct Word3 { /// 10-bit week counter pub week: u16, @@ -169,12 +503,15 @@ pub struct Word3 { } impl Word3 { - pub(crate) fn decode(dword: u32) -> Self { - let week = ((dword & WORD3_WEEK_MASK) >> WORD3_WEEK_SHIFT) as u16; - let ca_or_p_l2 = ((dword & WORD3_CA_P_L2_MASK) >> WORD3_CA_P_L2_SHIFT) as u8; - let ura = ((dword & WORD3_URA_MASK) >> WORD3_URA_SHIFT) as u8; - let health = ((dword & WORD3_HEALTH_MASK) >> WORD3_HEALTH_SHIFT) as u8; - let iodc_msb = ((dword & WORD3_IODC_MASK) >> WORD3_IODC_SHIFT) as u8; + /// Interprets this [GpsDataWord] as [Word3]. + pub fn from_word(word: GpsDataWord) -> Self { + let value = word.value(); + + let week = ((value & WORD3_WEEK_MASK) >> WORD3_WEEK_SHIFT) as u16; + let ca_or_p_l2 = ((value & WORD3_CA_P_L2_MASK) >> WORD3_CA_P_L2_SHIFT) as u8; + let ura = ((value & WORD3_URA_MASK) >> WORD3_URA_SHIFT) as u8; + let health = ((value & WORD3_HEALTH_MASK) >> WORD3_HEALTH_SHIFT) as u8; + let iodc_msb = ((value & WORD3_IODC_MASK) >> WORD3_IODC_SHIFT) as u8; Self { week, @@ -184,53 +521,102 @@ impl Word3 { iodc_msb, } } + + /// Encodes this [Word3] as [GpsDataWord]. + pub fn to_word(&self) -> GpsDataWord { + let mut value = 0u32; + + value |= ((self.week & 0x3ff) as u32) << WORD3_WEEK_SHIFT; + value |= ((self.ca_or_p_l2 & 0x3) as u32) << WORD3_CA_P_L2_SHIFT; + value |= ((self.ura & 0x07) as u32) << WORD3_URA_SHIFT; + value |= ((self.health & 0x3f) as u32) << WORD3_HEALTH_SHIFT; + value |= ((self.iodc_msb & 0x03) as u32) << WORD3_IODC_SHIFT; + + GpsDataWord::from(value) + } } -#[derive(Debug, Default, Clone)] -pub struct Word4 { +#[derive(Debug, Default, Clone, PartialEq)] +pub(crate) struct Word4 { pub l2_p_data_flag: bool, pub reserved: u32, } impl Word4 { - pub(crate) fn decode(dword: u32) -> Self { - let l2_p_data_flag = (dword & WORD4_L2P_DATA_MASK) > 0; - let reserved = ((dword & WORD4_RESERVED_MASK) >> WORD4_RESERVED_SHIFT) as u32; - Self { - l2_p_data_flag, - reserved, - } + /// Interprets this [GpsDataWord] as [Word4]. + pub fn from_word(word: GpsDataWord) -> Self { + Self::default() + // let l2_p_data_flag = (dword & WORD4_L2P_DATA_MASK) > 0; + // let reserved = ((dword & WORD4_RESERVED_MASK) >> WORD4_RESERVED_SHIFT) as u32; + // Self { + // l2_p_data_flag, + // reserved, + // } + } + + /// Encodes this [Word4] as [GpsDataWord] + pub fn to_word(&self) -> GpsDataWord { + // let mut value = 0; + + // if self.l2_p_data_flag { + // value |= WORD4_L2P_DATA_MASK; + // } + + // value |= (self.reserved & 0x7fffff) << WORD4_RESERVED_SHIFT; + + // value + Default::default() } } -#[derive(Debug, Default, Clone)] -pub struct Word5 { +#[derive(Debug, Default, Clone, PartialEq)] +pub(crate) struct Word5 { /// 24-bit reserved pub reserved: u32, } impl Word5 { - pub(crate) fn decode(dword: u32) -> Self { - let reserved = (dword & WORD5_RESERVED_MASK) >> WORD5_RESERVED_SHIFT; - Self { reserved } + /// Interprets this [GpsDataWord] as [Word5]. + pub fn from_word(word: GpsDataWord) -> Self { + Self::default() + // let reserved = (dword & WORD5_RESERVED_MASK) >> WORD5_RESERVED_SHIFT; + // Self { reserved } + } + + /// Encodes this [Word5] as [GpsDataWord] + pub fn to_word(&self) -> GpsDataWord { + // let mut value = 0; + // value |= (self.reserved & 0x0ffffff) << WORD5_RESERVED_SHIFT; + // value + Default::default() } } -#[derive(Debug, Default, Clone)] -pub struct Word6 { +#[derive(Debug, Default, PartialEq, Clone)] +pub(crate) struct Word6 { /// 24-bit reserved pub reserved: u32, } impl Word6 { - pub(crate) fn decode(dword: u32) -> Self { - let reserved = (dword & WORD6_RESERVED_MASK) >> WORD6_RESERVED_SHIFT; - Self { reserved } + /// Interprets this [GpsDataWord] as [Word6]. + pub fn from_word(word: GpsDataWord) -> Self { + Self::default() + // let reserved = (dword & WORD6_RESERVED_MASK) >> WORD6_RESERVED_SHIFT; + // Self { reserved } + } + + /// Encodes this [Word6] as [GpsDataWord] + pub fn to_word(&self) -> GpsDataWord { + // let mut value = 0; + // value |= (self.reserved & 0x0ffffff) << WORD6_RESERVED_SHIFT; + // value + Default::default() } } -#[derive(Debug, Default, Clone)] -pub struct Word7 { +#[derive(Debug, Default, Clone, PartialEq)] +pub(crate) struct Word7 { /// 16-bit reserved pub reserved: u16, @@ -239,15 +625,26 @@ pub struct Word7 { } impl Word7 { - pub(crate) fn decode(dword: u32) -> Self { - let reserved = ((dword & WORD7_RESERVED_MASK) >> WORD7_RESERVED_SHIFT) as u16; - let tgd = ((dword & WORD7_TGD_MASK) >> WORD7_TGD_SHIFT) as i8; - Self { reserved, tgd } + /// Interprets this [GpsDataWord] as [Word7]. + pub fn from_word(word: GpsDataWord) -> Self { + Self::default() + // let reserved = ((dword & WORD7_RESERVED_MASK) >> WORD7_RESERVED_SHIFT) as u16; + // let tgd = ((dword & WORD7_TGD_MASK) >> WORD7_TGD_SHIFT) as i8; + // Self { reserved, tgd } + } + + /// Encodes this [Word7] as [GpsDataWord] + pub fn to_word(&self) -> GpsDataWord { + // let mut value = 0; + // value |= ((self.reserved as u32) & 0x0ffff) << WORD7_RESERVED_SHIFT; + // value |= ((self.tgd as u32) & 0xff) << WORD7_TGD_SHIFT; + // value + Default::default() } } -#[derive(Debug, Default, Clone)] -pub struct Word8 { +#[derive(Debug, Default, Clone, PartialEq)] +pub(crate) struct Word8 { /// 8-bit IODC LSB to associate with Word # 3 pub iodc_lsb: u8, @@ -256,15 +653,26 @@ pub struct Word8 { } impl Word8 { - pub(crate) fn decode(dword: u32) -> Self { - let iodc_lsb = ((dword & WORD8_IODC_MASK) >> WORD8_IODC_SHIFT) as u8; - let toc = ((dword & WORD8_TOC_MASK) >> WORD8_TOC_SHIFT) as u16; - Self { iodc_lsb, toc } + /// Interprets this [GpsDataWord] as [Word8]. + pub fn from_word(word: GpsDataWord) -> Self { + Self::default() + // let iodc_lsb = ((dword & WORD8_IODC_MASK) >> WORD8_IODC_SHIFT) as u8; + // let toc = ((dword & WORD8_TOC_MASK) >> WORD8_TOC_SHIFT) as u16; + // Self { iodc_lsb, toc } + } + + /// Encodes this [Word8] as [GpsDataWord] + pub fn to_word(&self) -> GpsDataWord { + // let mut value = 0; + // value |= ((self.iodc_lsb as u32) & 0xff) << WORD8_IODC_SHIFT; + // value |= ((self.toc as u32) & 0x0ffff) << WORD8_TOC_SHIFT; + // value + Default::default() } } -#[derive(Debug, Default, Clone)] -pub struct Word9 { +#[derive(Debug, Default, Clone, PartialEq)] +pub(crate) struct Word9 { /// 8 bit af2 pub af2: i8, @@ -273,23 +681,304 @@ pub struct Word9 { } impl Word9 { - pub(crate) fn decode(dword: u32) -> Self { - let af2 = ((dword & WORD9_AF2_MASK) >> WORD9_AF2_SHIFT) as i8; - let af1 = ((dword & WORD9_AF1_MASK) >> WORD9_AF1_SHIFT) as i16; - Self { af2, af1 } + /// Interprets this [GpsDataWord] as [Word9]. + pub fn from_word(word: GpsDataWord) -> Self { + Self::default() + // let af2 = ((dword & WORD9_AF2_MASK) >> WORD9_AF2_SHIFT) as i8; + // let af1 = ((dword & WORD9_AF1_MASK) >> WORD9_AF1_SHIFT) as i16; + // Self { af2, af1 } + } + + /// Encodes this [Word9] as [GpsDataWord] + pub fn to_word(&self) -> GpsDataWord { + // let mut value = 0; + // value |= ((self.af2 as u32) & 0x0ff) << WORD9_AF2_SHIFT; + // value |= ((self.af1 as u32) & 0x0ffff) << WORD9_AF1_SHIFT; + // value + Default::default() } } -#[derive(Debug, Default, Clone)] -pub struct Word10 { +#[derive(Debug, Default, Clone, PartialEq)] +pub(crate) struct Word10 { /// 22-bit af0 pub af0: i32, } impl Word10 { - pub(crate) fn decode(dword: u32) -> Self { - let af0 = ((dword & WORD10_AF0_MASK) >> WORD10_AF0_SHIFT) as u32; - let af0 = twos_complement(af0, 0x3fffff, 0x200000); - Self { af0 } + /// Interprets this [GpsDataWord] as [Word10]. + pub fn from_word(word: GpsDataWord) -> Self { + Self::default() + // let af0 = ((dword & WORD10_AF0_MASK) >> WORD10_AF0_SHIFT) as u32; + // let af0 = twos_complement(af0, 0x3fffff, 0x200000); + // Self { af0 } + } + + /// Encodes this [Word10] as [GpsDataWord] + pub fn to_word(&self) -> GpsDataWord { + // ((self.af0 & 0x3fffff) as u32) << WORD10_AF0_SHIFT + Default::default() + } +} + +#[cfg(test)] +mod frame1 { + use super::*; + + #[test] + fn dword3_encoding() { + for dword3 in [ + Word3 { + week: 1, + ca_or_p_l2: 0, + ura: 0, + health: 0, + iodc_msb: 0, + }, + Word3 { + week: 0, + ca_or_p_l2: 1, + ura: 0, + health: 0, + iodc_msb: 0, + }, + Word3 { + week: 0, + ca_or_p_l2: 0, + ura: 1, + health: 0, + iodc_msb: 0, + }, + Word3 { + week: 0, + ca_or_p_l2: 0, + ura: 0, + health: 1, + iodc_msb: 0, + }, + Word3 { + week: 0, + ca_or_p_l2: 0, + ura: 0, + health: 0, + iodc_msb: 1, + }, + Word3 { + week: 1, + ca_or_p_l2: 2, + ura: 5, + health: 1, + iodc_msb: 0, + }, + ] { + let encoded = dword3.to_word(); + let decoded = Word3::from_word(encoded); + assert_eq!(decoded, dword3); + } } + + // #[test] + // fn dword4_encoding() { + // for dword4 in [ + // Word4 { + // l2_p_data_flag: true, + // reserved: 0, + // }, + // Word4 { + // l2_p_data_flag: false, + // reserved: 1, + // }, + // Word4 { + // l2_p_data_flag: true, + // reserved: 123, + // }, + // ] { + // let encoded = dword4.encode(); + // let decoded = Word4::decode(encoded); + // assert_eq!(decoded, dword4); + // } + // } + + // #[test] + // fn dword5_encoding() { + // for dword5 in [Word5 { reserved: 0 }, Word5 { reserved: 120 }] { + // let encoded = dword5.encode(); + // let decoded = Word5::decode(encoded); + // assert_eq!(decoded, dword5); + // } + // } + + // #[test] + // fn dword6_encoding() { + // for dword6 in [Word6 { reserved: 0 }, Word6 { reserved: 120 }] { + // let encoded = dword6.encode(); + // let decoded = Word6::decode(encoded); + // assert_eq!(decoded, dword6); + // } + // } + + // #[test] + // fn dword7_encoding() { + // for dword7 in [ + // Word7 { + // reserved: 0, + // tgd: 1, + // }, + // Word7 { + // reserved: 120, + // tgd: 0, + // }, + // Word7 { + // reserved: 120, + // tgd: 23, + // }, + // ] { + // let encoded = dword7.encode(); + // let decoded = Word7::decode(encoded); + // assert_eq!(decoded, dword7); + // } + // } + + // #[test] + // fn dword8_encoding() { + // for dword8 in [ + // Word8 { + // iodc_lsb: 10, + // toc: 30, + // }, + // Word8 { + // iodc_lsb: 30, + // toc: 10, + // }, + // ] { + // let encoded = dword8.encode(); + // let decoded = Word8::decode(encoded); + // assert_eq!(decoded, dword8); + // } + // } + + // #[test] + // fn dword9_encoding() { + // for dword9 in [Word9 { af2: 10, af1: 9 }, Word9 { af2: 9, af1: 100 }] { + // let encoded = dword9.encode(); + // let decoded = Word9::decode(encoded); + // assert_eq!(decoded, dword9); + // } + // } + + // #[test] + // fn dword10_encoding() { + // for dword10 in [ + // Word10 { af0: 0 }, + // Word10 { af0: 100 }, + // Word10 { af0: -1230 }, + // Word10 { af0: -3140 }, + // ] { + // let encoded = dword10.encode(); + // let decoded = Word10::decode(encoded); + // assert_eq!(decoded, dword10); + // } + // } + + // #[test] + // fn frame1_encoding() { + // for ( + // week, + // ca_or_p_l2, + // ura, + // health, + // iodc, + // toc, + // tgd, + // af0, + // af1, + // af2, + // l2_p_data_flag, + // reserved_word4, + // reserved_word5, + // reserved_word6, + // reserved_word7, + // ) in [ + // (1, 2, 3, 4, 5, 6, 7.0, 8.0, 9.0, 10.0, false, 11, 12, 13, 14), + // (0, 1, 2, 3, 4, 5, 6.0, 7.0, 8.0, 9.0, true, 10, 11, 12, 13), + // ] { + // let frame1 = GpsQzssFrame1 { + // week, + // ca_or_p_l2, + // ura, + // health, + // iodc, + // toc, + // tgd: tgd * 1.0E-9, + // af0: af0 * 1.0E-10, + // af1: af1 * 1.0E-11, + // af2: af2 * 1.0E-11, + // l2_p_data_flag, + // reserved_word4, + // reserved_word5, + // reserved_word6, + // reserved_word7, + // }; + + // let encoded = frame1.encode(); + + // let mut decoded = GpsQzssFrame1::default(); + + // // for (i, dword) in encoded.iter().enumerate() { + // // decoded.decode_word(i + 3, *dword).unwrap_or_else(|_| { + // // panic!("Failed to decode dword {:3}=0x{:08X}", i, dword); + // // }); + // // } + + // assert_eq!(decoded.ura, frame1.ura); + // assert_eq!(decoded.week, frame1.week); + // assert_eq!(decoded.toc, frame1.toc); + // assert_eq!(decoded.ca_or_p_l2, frame1.ca_or_p_l2); + // assert_eq!(decoded.l2_p_data_flag, frame1.l2_p_data_flag); + // assert_eq!(decoded.reserved_word4, frame1.reserved_word4); + // assert_eq!(decoded.reserved_word5, frame1.reserved_word5); + // assert_eq!(decoded.reserved_word6, frame1.reserved_word6); + // assert_eq!(decoded.reserved_word7, frame1.reserved_word7); + + // assert!((decoded.af0 - frame1.af0).abs() < 1E-9); + // // assert!((decoded.af1 - frame1.af1).abs() < 1E-9); + // assert!((decoded.af2 - frame1.af2).abs() < 1E-9); + // } + // } + + // #[test] + // fn user_range_accuracy() { + // for (value_m, encoded_ura) in [ + // (0.1, 0), + // (1.0, 0), + // (2.4, 0), + // (2.5, 1), + // (2.6, 1), + // (3.4, 1), + // (3.5, 2), + // (95.0, 8), + // (96.0, 8), + // (96.1, 9), + // (3071.0, 13), + // (3072.0, 13), + // (3072.1, 14), + // (4000.1, 14), + // ] { + // let ura = GpsQzssFrame1::compute_ura(value_m); + // assert_eq!(ura, encoded_ura, "encoded incorrect URA from {}m", value_m); + + // let mut frame1 = GpsQzssFrame1::default().with_user_range_accuracy_m(value_m); + + // assert_eq!(frame1.ura, encoded_ura); + + // let mut expected = GpsQzssFrame1::default(); + // expected.ura = encoded_ura; + + // // assert_eq!( + // // frame1.with_nominal_user_range_accuracy_m(value_m).ura, + // // encoded_ura, + // // "failed for value={}m, encoded={}", value_m, encoded_ura, + // // ); + // } + // } } diff --git a/src/gps/frame2.rs b/src/gps/frame2.rs index 16dd7d4..a2f06dd 100644 --- a/src/gps/frame2.rs +++ b/src/gps/frame2.rs @@ -1,4 +1,12 @@ -use crate::twos_complement; +use crate::{ + gps::{ + bytes::{ByteArray, GpsDataByte}, + GpsError, + }, + twos_complement, +}; + +use core::f64::consts::PI; const WORD3_IODE_MASK: u32 = 0x3fc00000; const WORD3_IODE_SHIFT: u32 = 22; @@ -35,34 +43,34 @@ const WORD10_FITINT_MASK: u32 = 0x00002000; const WORD10_AODO_MASK: u32 = 0x00001f00; const WORD10_AODO_SHIFT: u32 = 8; -/// GPS / QZSS Frame #2 interpretation +/// [GpsQzssFrame2] Ephemeris #2 frame interpretation. #[derive(Debug, Default, Copy, Clone, PartialEq)] pub struct GpsQzssFrame2 { /// Time of issue of ephemeris (in seconds of week) pub toe: u32, - /// IODE: Issue of Data (Ephemeris) + /// 8-bit IODE (Issue of Data) pub iode: u8, - /// Mean anomaly at reference time (in semi circles) + /// Mean anomaly at reference time (in semi-circles) pub m0: f64, - /// Mean motion difference from computed value (in semi circles) + /// Mean motion difference from computed value (in semi-circles) pub dn: f64, - /// Latitude cosine harmonic correction term + /// Latitude (cosine harmonic) in semi-circles. pub cuc: f64, - /// Latitude sine harmonic correction term + /// Latitude (sine harmonic) in semi-circles. pub cus: f64, - /// Orbit radius sine harmonic correction term + /// Orbit radius (sine harmonic) in meters. pub crs: f64, - /// Eccentricity + /// Orbit eccentricity. pub e: f64, - /// Sqrt(a) + /// Square root of semi-major axis, in square root of meters. pub sqrt_a: f64, /// Fit interval flag @@ -72,8 +80,249 @@ pub struct GpsQzssFrame2 { pub aodo: u8, } -#[derive(Debug, Default, Clone)] -pub struct Word3 { +impl GpsQzssFrame2 { + /// Copies and returns [GpsQzssFrame2] with updated time of issue of Ephemeris + /// in seconds of week. + pub fn with_toe(mut self, toe_seconds: u32) -> Self { + self.toe = toe_seconds; + self + } + + /// Copies and returns [GpsQzssFrame2] with updated IODE value. + pub fn with_iode(mut self, iode: u8) -> Self { + self.iode = iode; + self + } + + /// Copies and returns [GpsQzssFrame2] with updated mean anomaly at reference time, expressed + /// in semi-circles. + pub fn with_mean_anomaly_semi_circles(mut self, m0_semi_circles: f64) -> Self { + self.m0 = m0_semi_circles; + self + } + + /// Copies and returns [GpsQzssFrame2] with updated mean anomaly at reference time, expressed + /// in radians. + pub fn with_mean_anomaly_radians(mut self, m0_rad: f64) -> Self { + self.m0 = m0_rad * PI / 2.0f64.powi(31); + self + } + + /// Copies and returns [GpsQzssFrame2] with updated mean motion difference (in semi circles) + pub fn with_mean_motion_difference_semi_circles(mut self, dn_semi_circles: f64) -> Self { + self.dn = dn_semi_circles; + self + } + + /// Copies and returns [GpsQzssFrame2] with updated mean motion difference (in radians) + pub fn with_mean_motion_difference_radians(mut self, dn_rad: f64) -> Self { + self.dn = dn_rad * PI / 2.0f64.powi(31); + self + } + + /// Copies and returns [GpsQzssFrame2] with updated semi-major axis (in meters) + pub fn with_semi_major_axis_meters(mut self, semi_major_m: f64) -> Self { + self.sqrt_a = semi_major_m.sqrt(); + self + } + + /// Copies and returns [GpsQzssFrame2] with updated square root of semi-major axis (in square root meters) + pub fn with_square_root_semi_major_axis(mut self, sqrt_semi_major_m: f64) -> Self { + self.sqrt_a = sqrt_semi_major_m; + self + } + + /// Copies and returns [GpsQzssFrame2] with updated orbit eccentricity. + pub fn with_eccentricity(mut self, e: f64) -> Self { + self.e = e; + self + } + + /// Copies and returns [GpsQzssFrame2] with updated 5-bit AODO mask. + pub fn with_aodo(mut self, aodo: u8) -> Self { + self.aodo = aodo & 0x1f; + self + } + + /// Copies and returns [GpsQzssFrame2] with updated radius (sine component) in meters. + pub fn with_crs_meters(mut self, crs_m: f64) -> Self { + self.crs = crs_m; + self + } + + /// Copies and returns [GpsQzssFrame2] with updated latitude cosine harmnoic correction term. + pub fn with_cuc_radians(mut self, cuc_rad: f64) -> Self { + self.cuc = cuc_rad; + self + } + + /// Copies and returns [GpsQzssFrame2] with updated latitude sine harmnoic correction term. + pub fn with_cus_radians(mut self, cus_rad: f64) -> Self { + self.cus = cus_rad; + self + } + + /// Copies and returns [GpsQzssFrame2] with fit interval flag asserted. + pub fn with_fit_interval_flag(mut self) -> Self { + self.fit_int_flag = true; + self + } + + /// Copies and returns [GpsQzssFrame2] with fit interval flag deasserted. + pub fn without_fit_interval_flag(mut self) -> Self { + self.fit_int_flag = false; + self + } + + /// Decodes [Self] from a burst of 8 [GpsDataByte]s + pub(crate) fn decode(bytes: &[GpsDataByte]) -> Self { + let mut extra = 0u32; + let mut s = Self::default(); + + for i in 0..8 { + let array = ByteArray::new(&bytes[i * 4..i * 4 + 4]); + let dword = array.value_u32(); + + match i { + 0 => s.set_word3(Word3::decode(dword)), + 1 => s.set_word4(Word4::decode(dword), &mut extra), + 2 => s.set_word5(Word5::decode(dword), extra), + 3 => s.set_word6(Word6::decode(dword), &mut extra), + 4 => s.set_word7(Word7::decode(dword), extra), + 5 => s.set_word8(Word8::decode(dword), &mut extra), + 6 => s.set_word9(Word9::decode(dword), extra), + 7 => s.set_word10(Word10::decode(dword)), + _ => unreachable!("compiler issue"), + } + } + s + } + + fn word3(&self) -> Word3 { + Word3 { + iode: self.iode, + crs: (self.crs * 2.0_f64.powi(5)) as i32, + } + } + + fn set_word3(&mut self, word: Word3) { + self.crs = (word.crs as f64) / 2.0_f64.powi(5); + self.iode = word.iode; + } + + fn word4(&self) -> Word4 { + let m0 = (self.m0 * 2.0_f64.powi(31)) as u32; + Word4 { + m0_msb: ((m0 & 0xff000000) >> 24) as u8, + dn: (self.dn * 2.0_f64.powi(43)).round() as i16, + } + } + + fn set_word4(&mut self, word: Word4, extra: &mut u32) { + *extra = word.m0_msb as u32; + self.dn = (word.dn as f64) / 2.0_f64.powi(43); + } + + fn word5(&self) -> Word5 { + let m0 = (self.m0 * 2.0_f64.powi(31)).round() as i32; + + Word5 { + m0_lsb: (m0 & 0x00ffffff) as u32, + } + } + + fn set_word5(&mut self, word: Word5, m0_msb: u32) { + let mut m0 = m0_msb << 24; + m0 |= word.m0_lsb as u32; + self.m0 = ((m0 as i32) as f64) / 2.0_f64.powi(31); + } + + fn word6(&self) -> Word6 { + let e = (self.e * 2.0_f64.powi(33)).round() as u32; + + Word6 { + e_msb: ((e & 0xff000000) >> 24) as u8, + cuc: (self.cuc * 2.0_f64.powi(29)).round() as i16, + } + } + + fn set_word6(&mut self, word: Word6, extra: &mut u32) { + *extra = word.e_msb as u32; + self.cuc = (word.cuc as f64) / 2.0_f64.powi(29); + } + + fn word7(&self) -> Word7 { + let e = (self.e * 2.0_f64.powi(33)).round() as i32; + Word7 { + e_lsb: (e & 0x00ffffff) as u32, + } + } + + fn set_word7(&mut self, word: Word7, e_msb: u32) { + let mut e = e_msb << 24; + e |= word.e_lsb; + + self.e = (e as f64) / 2.0_f64.powi(33); + } + + fn word8(&self) -> Word8 { + let sqrt_a = (self.sqrt_a * 2.0_f64.powi(19)).round() as u32; + Word8 { + sqrt_a_msb: ((sqrt_a & 0xff000000) >> 24) as u8, + cus: (self.cus * 2.0_f64.powi(29)).round() as i32, + } + } + + fn set_word8(&mut self, word: Word8, extra: &mut u32) { + *extra = word.sqrt_a_msb as u32; + self.cus = (word.cus as f64) / 2.0_f64.powi(29); + } + + fn word9(&self) -> Word9 { + let sqrt_a = (self.sqrt_a * 2.0_f64.powi(19)).round() as i32; + Word9 { + sqrt_a_lsb: (sqrt_a & 0x00ffffff) as u32, + } + } + + fn set_word9(&mut self, word: Word9, sqrt_a_msb: u32) { + let mut sqrt_a = sqrt_a_msb << 24; + sqrt_a |= word.sqrt_a_lsb; + self.sqrt_a = (sqrt_a as f64) / 2.0_f64.powi(19); + } + + fn word10(&self) -> Word10 { + Word10 { + aodo: self.aodo, + fitint: self.fit_int_flag, + toe: (self.toe / 16) as u16, + } + } + + fn set_word10(&mut self, word: Word10) { + self.aodo = word.aodo; + self.fit_int_flag = word.fitint; + self.toe = (word.toe as u32) * 16; + } + + /// Encodes this [GpsQzssFrame2] as a burst of 8 [u32] data words + /// starting from [Word3] to [Word10]. + pub(crate) fn encode(&self) -> [u32; 8] { + [ + self.word3().encode(), + self.word4().encode(), + self.word5().encode(), + self.word6().encode(), + self.word7().encode(), + self.word8().encode(), + self.word9().encode(), + self.word10().encode(), + ] + } +} + +#[derive(Debug, Default, Clone, PartialEq)] +pub(crate) struct Word3 { pub iode: u8, pub crs: i32, } @@ -85,10 +334,19 @@ impl Word3 { let crs = twos_complement(crs, 0xffff, 0x8000); Self { iode, crs } } + + pub(crate) fn encode(&self) -> u32 { + let mut value = 0; + + value |= (self.iode as u32) << WORD3_IODE_SHIFT; + value |= ((self.crs as u32) & 0xffff) << WORD3_CRS_SHIFT; + + value + } } -#[derive(Debug, Default, Clone)] -pub struct Word4 { +#[derive(Debug, Default, PartialEq, Clone)] +pub(crate) struct Word4 { /// Delta n pub dn: i16, @@ -102,10 +360,17 @@ impl Word4 { let m0_msb = ((dword & WORD4_M0_MSB_MASK) >> WORD4_M0_MSB_SHIFT) as u8; Self { dn, m0_msb } } + + pub(crate) fn encode(&self) -> u32 { + let mut value = 0; + value |= (self.dn as u32) << WORD4_DELTA_N_SHIFT; + value |= (self.m0_msb as u32) << WORD4_M0_MSB_SHIFT; + value + } } -#[derive(Debug, Default, Clone)] -pub struct Word5 { +#[derive(Debug, Default, Clone, PartialEq)] +pub(crate) struct Word5 { /// M0 (24) lsb, you need to associate this to Subframe #2 Word #4 pub m0_lsb: u32, } @@ -115,10 +380,16 @@ impl Word5 { let m0_lsb = ((dword & WORD5_M0_LSB_MASK) >> WORD5_M0_LSB_SHIFT) as u32; Self { m0_lsb } } + + pub(crate) fn encode(&self) -> u32 { + let mut value = 0; + value |= ((self.m0_lsb & 0x00ffffff) as u32) << WORD5_M0_LSB_SHIFT; + value + } } -#[derive(Debug, Default, Clone)] -pub struct Word6 { +#[derive(Debug, Default, Clone, PartialEq)] +pub(crate) struct Word6 { pub cuc: i16, /// MSB(8) eccentricity, you need to associate this to Subframe #2 Word #7 @@ -131,10 +402,17 @@ impl Word6 { let e_msb = ((dword & WORD6_E_MSB_MASK) >> WORD6_E_MSB_SHIFT) as u8; Self { cuc, e_msb } } + + pub(crate) fn encode(&self) -> u32 { + let mut value = 0; + value |= (self.cuc as u32) << WORD6_CUC_SHIFT; + value |= (self.e_msb as u32) << WORD6_E_MSB_SHIFT; + value + } } -#[derive(Debug, Default, Clone)] -pub struct Word7 { +#[derive(Debug, Default, Clone, PartialEq)] +pub(crate) struct Word7 { /// LSB(24) eccentricity, you need to associate this to Subframe #2 Word #6 pub e_lsb: u32, } @@ -144,10 +422,14 @@ impl Word7 { let e_lsb = ((dword & WORD7_E_LSB_MASK) >> WORD7_E_LSB_SHIFT) as u32; Self { e_lsb } } + + pub(crate) fn encode(&self) -> u32 { + (self.e_lsb as u32) << WORD7_E_LSB_SHIFT + } } -#[derive(Debug, Default, Clone)] -pub struct Word8 { +#[derive(Debug, Default, Clone, PartialEq)] +pub(crate) struct Word8 { pub cus: i32, /// MSB(8) A⁻¹: you need to associate this to Subframe #2 Word #9 @@ -162,10 +444,17 @@ impl Word8 { let sqrt_a_msb = ((dword & WORD8_SQRTA_MSB_MASK) >> WORD8_SQRTA_MSB_SHIFT) as u8; Self { cus, sqrt_a_msb } } + + pub(crate) fn encode(&self) -> u32 { + let mut value = 0; + value |= (self.sqrt_a_msb as u32) << WORD8_SQRTA_MSB_SHIFT; + value |= (self.cus as u32) << WORD8_CUS_SHIFT; + value + } } -#[derive(Debug, Default, Clone)] -pub struct Word9 { +#[derive(Debug, Default, Clone, PartialEq)] +pub(crate) struct Word9 { /// LSB(24) A⁻¹: you need to associate this to Subframe #2 Word #8 pub sqrt_a_lsb: u32, } @@ -175,10 +464,14 @@ impl Word9 { let sqrt_a_lsb = ((dword & WORD9_SQRTA_LSB_MASK) >> WORD9_SQRTA_LSB_SHIFT) as u32; Self { sqrt_a_lsb } } + + pub(crate) fn encode(&self) -> u32 { + ((self.sqrt_a_lsb as u32) & 0x00ffffff) << WORD9_SQRTA_LSB_SHIFT + } } -#[derive(Debug, Default, Clone)] -pub struct Word10 { +#[derive(Debug, Default, Clone, PartialEq)] +pub(crate) struct Word10 { /// Time of issue of Ephemeris (u16) pub toe: u16, @@ -196,49 +489,204 @@ impl Word10 { let aodo = ((dword & WORD10_AODO_MASK) >> WORD10_AODO_SHIFT) as u8; Self { toe, fitint, aodo } } + + pub(crate) fn encode(&self) -> u32 { + let mut value = 0; + value |= ((self.aodo & 0x1f) as u32) << WORD10_AODO_SHIFT; + + if self.fitint { + value |= WORD10_FITINT_MASK; + } + + value |= (self.toe as u32) << WORD10_TOE_SHIFT; + value + } } -// impl UnscaledFrame { -// pub fn scale(&self) -> GpsQzssFrame2 { -// GpsQzssFrame2 { -// iode: self.word3.iode, -// toe: (self.word10.toe as u32) * 16, -// crs: (self.word3.crs as f64) / 2.0_f64.powi(5), -// cus: (self.word8.cus as f64) / 2.0_f64.powi(29), -// cuc: (self.word6.cuc as f64) / 2.0_f64.powi(29), +#[cfg(test)] +mod frame2 { + use super::*; + + #[test] + fn dword3_encoding() { + for dword3 in [ + Word3 { crs: 1, iode: 0 }, + Word3 { crs: 2, iode: 1 }, + Word3 { crs: 10, iode: 2 }, + Word3 { crs: 10, iode: 100 }, + ] { + let encoded = dword3.encode(); + let decoded = Word3::decode(encoded); + assert_eq!(decoded, dword3); + } + } -// dn: { -// let dn = self.word4.dn as f64; -// dn / 2.0_f64.powi(43) -// }, + #[test] + fn dword4_encoding() { + for dword4 in [ + Word4 { dn: 0, m0_msb: 1 }, + Word4 { dn: 1, m0_msb: 3 }, + Word4 { + dn: 10, + m0_msb: 100, + }, + ] { + let encoded = dword4.encode(); + let decoded = Word4::decode(encoded); + assert_eq!(decoded, dword4); + } + } -// m0: { -// let mut m0 = self.word4.m0_msb as u32; -// m0 <<= 24; -// m0 |= self.word5.m0_lsb as u32; + #[test] + fn dword5_encoding() { + for dword5 in [ + Word5 { m0_lsb: 1 }, + Word5 { m0_lsb: 10 }, + Word5 { m0_lsb: 0 }, + Word5 { m0_lsb: 100 }, + ] { + let encoded = dword5.encode(); + let decoded = Word5::decode(encoded); + assert_eq!(decoded, dword5); + } + } -// let m0 = (m0 as i32) as f64; -// m0 / 2.0_f64.powi(31) -// }, + #[test] + fn dword6_encoding() { + for dword6 in [Word6 { e_msb: 0, cuc: 10 }, Word6 { e_msb: 1, cuc: 100 }] { + let encoded = dword6.encode(); + let decoded = Word6::decode(encoded); + assert_eq!(decoded, dword6); + } + } -// e: { -// let mut e = self.word6.e_msb as u32; -// e <<= 24; -// e |= self.word7.e_lsb; + #[test] + fn dword7_encoding() { + for dword7 in [ + Word7 { e_lsb: 0 }, + Word7 { e_lsb: 1 }, + Word7 { e_lsb: 10 }, + Word7 { e_lsb: 100 }, + ] { + let encoded = dword7.encode(); + let decoded = Word7::decode(encoded); + assert_eq!(decoded, dword7); + } + } -// (e as f64) / 2.0_f64.powi(33) -// }, + #[test] + fn dword8_encoding() { + for dword8 in [ + Word8 { + cus: 0, + sqrt_a_msb: 10, + }, + Word8 { + cus: 10, + sqrt_a_msb: 1, + }, + Word8 { + cus: 10, + sqrt_a_msb: 0, + }, + Word8 { + cus: 0, + sqrt_a_msb: 12, + }, + ] { + let encoded = dword8.encode(); + let decoded = Word8::decode(encoded); + assert_eq!(decoded, dword8); + } + } -// sqrt_a: { -// let mut sqrt_a = self.word8.sqrt_a_msb as u32; -// sqrt_a <<= 24; -// sqrt_a |= self.word9.sqrt_a_lsb; + #[test] + fn dword9_decoding() { + for dword9 in [ + Word9 { sqrt_a_lsb: 0 }, + Word9 { sqrt_a_lsb: 1 }, + Word9 { sqrt_a_lsb: 10 }, + Word9 { sqrt_a_lsb: 255 }, + ] { + let encoded = dword9.encode(); + let decoded = Word9::decode(encoded); + assert_eq!(decoded, dword9); + } + } -// (sqrt_a as f64) / 2.0_f64.powi(19) -// }, + #[test] + fn dword10_decoding() { + for dword10 in [ + Word10 { + toe: 0, + fitint: false, + aodo: 0, + }, + Word10 { + toe: 0, + fitint: false, + aodo: 1, + }, + Word10 { + toe: 10, + fitint: true, + aodo: 10, + }, + ] { + let encoded = dword10.encode(); + let decoded = Word10::decode(encoded); + assert_eq!(decoded, dword10); + } + } -// aodo: self.word10.aodo, -// fit_int_flag: self.word10.fitint, -// } -// } -// } + #[test] + fn frame2_encoding() { + for (toe, iode, m0, dn, cuc, cus, crs, e, sqrt_a, fit_int_flag, aodo) in [ + ( + 2300, 10, 11.0, 12.0, 13.0, 14.0, 15.0, 16.0, 16.0, false, 10, + ), + ( + 4800, 11, 12.0, 13.0, 14.0, 15.0, 16.0, 17.0, 18.0, true, 255, + ), + ] { + let frame2 = GpsQzssFrame2 { + toe, + iode, + m0, + dn, + cuc, + cus, + crs, + e, + sqrt_a, + fit_int_flag, + aodo, + }; + + let encoded = frame2.encode(); + + let mut extra = 0; + let mut decoded = GpsQzssFrame2::default(); + + for (i, dword) in encoded.iter().enumerate() { + // decoded + // .decode_word(i + 3, *dword, &mut extra) + // .unwrap_or_else(|_| { + // panic!("Failed to decode dword {:3}=0x{:08X}", i, dword); + // }); + } + + assert_eq!(frame2.toe, toe); + assert_eq!(frame2.iode, iode); + assert_eq!(frame2.m0, m0); + assert_eq!(frame2.dn, dn); + assert_eq!(frame2.cuc, cuc); + assert_eq!(frame2.cus, cus); + assert_eq!(frame2.crs, crs); + assert_eq!(frame2.e, e); + assert_eq!(frame2.sqrt_a, sqrt_a); + assert_eq!(frame2.fit_int_flag, fit_int_flag); + assert_eq!(frame2.aodo, aodo); + } + } +} diff --git a/src/gps/frame3.rs b/src/gps/frame3.rs index b23efae..c69f136 100644 --- a/src/gps/frame3.rs +++ b/src/gps/frame3.rs @@ -1,4 +1,10 @@ -use crate::twos_complement; +use crate::{ + gps::{ + bytes::{ByteArray, GpsDataByte}, + GpsError, + }, + twos_complement, +}; const WORD3_CIC_MASK: u32 = 0x3fffc000; const WORD3_CIC_SHIFT: u32 = 14; @@ -32,7 +38,7 @@ const WORD10_IODE_SHIFT: u32 = 22; const WORD10_IDOT_MASK: u32 = 0x003fff00; const WORD10_IDOT_SHIFT: u32 = 8; -/// GPS / QZSS Frame #3 interpretation +/// [GpsQzssFrame3] Ephemeris #3 frame interpretation. #[derive(Debug, Default, Copy, Clone, PartialEq)] pub struct GpsQzssFrame3 { /// Inclination angle cosine harmonic correction term @@ -63,9 +69,192 @@ pub struct GpsQzssFrame3 { pub omega_dot: f64, } -#[derive(Debug, Default, Clone)] -pub struct Word3 { - pub cic: i32, +impl GpsQzssFrame3 { + /// Copies and returns [GpsQzssFrame3] with updated Cic correction term + pub fn with_cic_radians(mut self, cic_rad: f64) -> Self { + self.cic = cic_rad; + self + } + + /// Copies and returns [GpsQzssFrame3] with updated Cis correction term + pub fn with_cis_radians(mut self, cis_rad: f64) -> Self { + self.cis = cis_rad; + self + } + + /// Copies and returns [GpsQzssFrame3] with updated inclination angle at reference time (in semi circles) + pub fn with_i0_semi_circles(mut self, i0: f64) -> Self { + self.i0 = i0; + self + } + + /// Copies and returns [GpsQzssFrame3] with updated orbit radius cosine harmonic term (meters) + pub fn with_crc_meters(mut self, crc: f64) -> Self { + self.crc = crc; + self + } + + /// Copies and returns [GpsQzssFrame3] with updated longitude of ascending node (in semi circles) + pub fn with_omega0_semi_circles(mut self, omega0: f64) -> Self { + self.omega0 = omega0; + self + } + + /// Copies and returns [GpsQzssFrame3] with updated omega (in semi circles) + pub fn with_omega_semi_circles(mut self, omega: f64) -> Self { + self.omega = omega; + self + } + + /// Copies and returns [GpsQzssFrame3] with updated omega velocity (in semi circles.s⁻¹) + pub fn with_omega_dot_semi_circles_sec(mut self, omega_dot: f64) -> Self { + self.omega_dot = omega_dot; + self + } + + /// Decodes [Self] from a burst of 8 [GpsDataByte]s + pub(crate) fn decode(bytes: &[GpsDataByte]) -> Self { + let mut extra = 0u32; + let mut s = Self::default(); + + for i in 0..8 { + let array = ByteArray::new(&bytes[i * 4..i * 4 + 4]); + let dword = array.value_u32(); + + match i { + 0 => s.set_word3(Word3::decode(dword), &mut extra), + 1 => s.set_word4(Word4::decode(dword), extra), + 2 => s.set_word5(Word5::decode(dword), &mut extra), + 3 => s.set_word6(Word6::decode(dword), extra), + 4 => s.set_word7(Word7::decode(dword), &mut extra), + 5 => s.set_word8(Word8::decode(dword), extra), + 6 => s.set_word9(Word9::decode(dword)), + 7 => s.set_word10(Word10::decode(dword)), + _ => unreachable!("compiler issue"), + } + } + s + } + + fn set_word3(&mut self, word: Word3, extra: &mut u32) { + *extra = word.omega0_msb as u32; + self.cic = (word.cic as f64) / 2.0_f64.powi(29); + } + + fn word3(&self) -> Word3 { + let omega0 = (self.omega0 * 2.0_f64.powi(31)).round() as u32; + Word3 { + omega0_msb: ((omega0 & 0xff000000) >> 24) as u8, + cic: (self.cic * 2.0_f64.powi(29)).round() as i16, + } + } + + fn set_word4(&mut self, word: Word4, omega0_msb: u32) { + let mut omega0 = omega0_msb << 24; + omega0 |= word.omega0_lsb; + self.omega0 = ((omega0 as i32) as f64) / 2.0_f64.powi(31); + } + + fn word4(&self) -> Word4 { + let omega0 = (self.omega0 * 2.0_f64.powi(31)).round() as u32; + Word4 { + omega0_lsb: (omega0 & 0x00ffffff) as u32, + } + } + + fn set_word5(&mut self, word: Word5, extra: &mut u32) { + *extra = word.i0_msb as u32; + self.cis = (word.cis as f64) / 2.0_f64.powi(29); + } + + fn word5(&self) -> Word5 { + let i0 = (self.i0 * 2.0_f64.powi(31)).round() as u32; + Word5 { + i0_msb: ((i0 & 0xff000000) >> 24) as u8, + cis: (self.cis * 2.0_f64.powi(29)) as i32, + } + } + + fn set_word6(&mut self, word: Word6, i0_msb: u32) { + let mut i0 = i0_msb << 24; + i0 |= word.i0_lsb; + self.i0 = (i0 as f64) / 2.0_f64.powi(31); + } + + fn word6(&self) -> Word6 { + let i0 = (self.i0 * 2.0_f64.powi(31)).round() as u32; + Word6 { + i0_lsb: (i0 & 0x00ffffff) as u32, + } + } + + fn set_word7(&mut self, word: Word7, extra: &mut u32) { + *extra = word.omega_msb as u32; + self.crc = (word.crc as f64) / 2.0_f64.powi(5); + } + + fn word7(&self) -> Word7 { + let omega = (self.omega * 2.0_f64.powi(31)).round() as u32; + Word7 { + crc: (self.crc * 2.0_f64.powi(5)) as i32, + omega_msb: ((omega & 0xff000000) >> 24) as u8, + } + } + + fn set_word8(&mut self, word: Word8, omega_msb: u32) { + let mut omega = omega_msb << 24; + omega |= word.omega_lsb; + self.omega = ((omega as i32) as f64) / 2.0_f64.powi(31); + } + + fn word8(&self) -> Word8 { + let omega = (self.omega * 2.0_f64.powi(31)).round() as u32; + Word8 { + omega_lsb: (omega & 0x00ffffff) as u32, + } + } + + fn set_word9(&mut self, word: Word9) { + self.omega_dot = (word.omega_dot as f64) / 2.0_f64.powi(43); + } + + fn word9(&self) -> Word9 { + Word9 { + omega_dot: (self.omega_dot * 2.0_f64.powi(43)).round() as i32, + } + } + + fn set_word10(&mut self, word: Word10) { + self.idot = (word.idot as f64) / 2.0_f64.powi(43); + self.iode = word.iode; + } + + fn word10(&self) -> Word10 { + Word10 { + iode: self.iode, + idot: (self.idot * 2.0_f64.powi(43)).round() as i32, + } + } + + /// Encodes this [GpsQzssFrame3] as burst of 8 32-bit data words + pub(crate) fn encode(&self) -> [u32; 8] { + [ + self.word3().encode(), + self.word4().encode(), + self.word5().encode(), + self.word6().encode(), + self.word7().encode(), + self.word8().encode(), + self.word9().encode(), + self.word10().encode(), + ] + } +} + +#[derive(Debug, Default, Clone, PartialEq)] +pub(crate) struct Word3 { + /// 16 bit Ci (cosine) component in radians + pub cic: i16, /// Omega0 (8) MSB, you will have to associate this to Word #4 pub omega0_msb: u8, @@ -74,14 +263,21 @@ pub struct Word3 { impl Word3 { pub(crate) fn decode(dword: u32) -> Self { let cic = ((dword & WORD3_CIC_MASK) >> WORD3_CIC_SHIFT) as u32; - let cic = twos_complement(cic, 0xffff, 0x8000); + let cic = twos_complement(cic, 0xffff, 0x8000) as i16; let omega0_msb = ((dword & WORD3_OMEGA0_MASK) >> WORD3_OMEGA0_SHIFT) as u8; Self { cic, omega0_msb } } + + pub(crate) fn encode(&self) -> u32 { + let mut value = 0; + value |= (self.cic as u32) << WORD3_CIC_SHIFT; + value |= (self.omega0_msb as u32) << WORD3_OMEGA0_SHIFT; + value + } } -#[derive(Debug, Default, Clone)] -pub struct Word4 { +#[derive(Debug, Default, Clone, PartialEq)] +pub(crate) struct Word4 { /// Omega0 (24) LSB, you will have to associate this to Word #3 pub omega0_lsb: u32, } @@ -91,9 +287,13 @@ impl Word4 { let omega0_lsb = ((dword & WORD4_OMEGA0_MASK) >> WORD4_OMEGA0_SHIFT) as u32; Self { omega0_lsb } } + + pub(crate) fn encode(&self) -> u32 { + (self.omega0_lsb as u32) << WORD4_OMEGA0_SHIFT + } } -#[derive(Debug, Default, Clone)] -pub struct Word5 { +#[derive(Debug, Default, Clone, PartialEq)] +pub(crate) struct Word5 { pub cis: i32, /// I0 (8) MSB, you will have to associate this to Word #6 @@ -107,10 +307,17 @@ impl Word5 { let i0_msb = ((dword & WORD5_I0_MASK) >> WORD5_I0_SHIFT) as u8; Self { cis, i0_msb } } + + pub(crate) fn encode(&self) -> u32 { + let mut value = 0; + value |= ((self.cis as u32) & 0xffff) << WORD5_CIS_SHIFT; + value |= (self.i0_msb as u32) << WORD5_I0_SHIFT; + value + } } -#[derive(Debug, Default, Clone)] -pub struct Word6 { +#[derive(Debug, Default, Clone, PartialEq)] +pub(crate) struct Word6 { /// I0 (24) LSB, you will have to associate this to Word #5 pub i0_lsb: u32, } @@ -120,10 +327,14 @@ impl Word6 { let i0_lsb = ((dword & WORD6_I0_MASK) >> WORD6_I0_SHIFT) as u32; Self { i0_lsb } } + + pub(crate) fn encode(&self) -> u32 { + (self.i0_lsb as u32) << WORD6_I0_SHIFT + } } -#[derive(Debug, Default, Clone)] -pub struct Word7 { +#[derive(Debug, Default, Clone, PartialEq)] +pub(crate) struct Word7 { pub crc: i32, /// Omega (8) MSB, you will have to associate this to Word #8 @@ -137,10 +348,17 @@ impl Word7 { let omega_msb = ((dword & WORD7_OMEGA_MASK) >> WORD7_OMEGA_SHIFT) as u8; Self { crc, omega_msb } } + + pub(crate) fn encode(&self) -> u32 { + let mut value = 0; + value |= ((self.crc as u32) & 0xffff) << WORD7_CRC_SHIFT; + value |= (self.omega_msb as u32) << WORD7_OMEGA_SHIFT; + value + } } -#[derive(Debug, Default, Clone)] -pub struct Word8 { +#[derive(Debug, Default, Clone, PartialEq)] +pub(crate) struct Word8 { /// Omega (24) LSB, you will have to associate this to Word #7 pub omega_lsb: u32, } @@ -150,10 +368,14 @@ impl Word8 { let omega_lsb = ((dword & WORD8_OMEGA_MASK) >> WORD8_OMEGA_SHIFT) as u32; Self { omega_lsb } } + + pub(crate) fn encode(&self) -> u32 { + (self.omega_lsb as u32) << WORD8_OMEGA_SHIFT + } } -#[derive(Debug, Default, Clone)] -pub struct Word9 { +#[derive(Debug, Default, Clone, PartialEq)] +pub(crate) struct Word9 { // 24-bit Omega_dot pub omega_dot: i32, } @@ -164,10 +386,14 @@ impl Word9 { let omega_dot = twos_complement(omega_dot, 0xffffff, 0x800000); Self { omega_dot } } + + pub(crate) fn encode(&self) -> u32 { + ((self.omega_dot & 0xffffff) as u32) << WORD9_OMEGADOT_SHIFT + } } -#[derive(Debug, Default, Clone)] -pub struct Word10 { +#[derive(Debug, Default, Clone, PartialEq)] +pub(crate) struct Word10 { /// 8-bit IODE pub iode: u8, @@ -185,4 +411,219 @@ impl Word10 { Self { iode, idot } } + + pub(crate) fn encode(&self) -> u32 { + let mut value = 0; + value |= (self.iode as u32) << WORD10_IODE_SHIFT; + value |= ((self.idot as u32) & 0x3fff) << WORD10_IDOT_SHIFT; + value + } +} + +#[cfg(test)] +mod frame3 { + use super::*; + + #[test] + fn word3_encoding() { + for dword3 in [ + Word3 { + omega0_msb: 0, + cic: 1, + }, + Word3 { + omega0_msb: 1, + cic: 0, + }, + Word3 { + omega0_msb: 10, + cic: 12, + }, + Word3 { + omega0_msb: 255, + cic: 12, + }, + ] { + let encoded = dword3.encode(); + let decoded = Word3::decode(encoded); + assert_eq!(decoded, dword3); + } + } + + #[test] + fn word4_encoding() { + for dword4 in [ + Word4 { omega0_lsb: 0 }, + Word4 { omega0_lsb: 10 }, + Word4 { omega0_lsb: 250 }, + Word4 { omega0_lsb: 255 }, + ] { + let encoded = dword4.encode(); + let decoded = Word4::decode(encoded); + assert_eq!(decoded, dword4); + } + } + + #[test] + fn word5_encoding() { + for dword5 in [ + Word5 { cis: 0, i0_msb: 1 }, + Word5 { cis: 1, i0_msb: 0 }, + Word5 { cis: 4, i0_msb: 3 }, + Word5 { + cis: 10, + i0_msb: 255, + }, + Word5 { + cis: -100, + i0_msb: 255, + }, + Word5 { + cis: -9999, + i0_msb: 255, + }, + ] { + let encoded = dword5.encode(); + let decoded = Word5::decode(encoded); + assert_eq!(decoded, dword5); + } + } + + #[test] + fn word6_encoding() { + for dword6 in [ + Word6 { i0_lsb: 0 }, + Word6 { i0_lsb: 1 }, + Word6 { i0_lsb: 255 }, + ] { + let encoded = dword6.encode(); + let decoded = Word6::decode(encoded); + assert_eq!(decoded, dword6); + } + } + + #[test] + fn word7_encoding() { + for dword7 in [ + Word7 { + crc: 0, + omega_msb: 1, + }, + Word7 { + crc: 1, + omega_msb: 255, + }, + Word7 { + crc: -1, + omega_msb: 0, + }, + Word7 { + crc: -1000, + omega_msb: 250, + }, + Word7 { + crc: 1000, + omega_msb: 254, + }, + ] { + let encoded = dword7.encode(); + let decoded = Word7::decode(encoded); + assert_eq!(decoded, dword7); + } + } + + #[test] + fn word8_encoding() { + for dword8 in [ + Word8 { omega_lsb: 0 }, + Word8 { omega_lsb: 1 }, + Word8 { omega_lsb: 255 }, + ] { + let encoded = dword8.encode(); + let decoded = Word8::decode(encoded); + assert_eq!(decoded, dword8); + } + } + + #[test] + fn word9_encoding() { + for dword9 in [ + Word9 { omega_dot: 0 }, + Word9 { omega_dot: 10 }, + Word9 { omega_dot: -10 }, + Word9 { omega_dot: -1000 }, + Word9 { omega_dot: 1000 }, + Word9 { omega_dot: 250 }, + ] { + let encoded = dword9.encode(); + let decoded = Word9::decode(encoded); + assert_eq!(decoded, dword9); + } + } + + #[test] + fn word10_encoding() { + for dword10 in [ + Word10 { iode: 0, idot: 10 }, + Word10 { + iode: 10, + idot: -10, + }, + Word10 { + iode: 255, + idot: -1000, + }, + Word10 { + iode: 254, + idot: 1000, + }, + ] { + let encoded = dword10.encode(); + let decoded = Word10::decode(encoded); + assert_eq!(decoded, dword10); + } + } + + #[test] + fn frame3_encoding() { + for (cic, cis, crc, i0, iode, idot, omega0, omega, omega_dot) in [ + (10.0, 11.0, 12.0, 13.0, 20, 30.0, 40.0, 50.0, 60.0), + (11.0, 12.0, 13.0, 14.0, 25, 36.0, 47.0, 58.0, 69.0), + ] { + let frame3 = GpsQzssFrame3 { + cic, + cis, + crc, + i0, + iode, + idot, + omega0, + omega, + omega_dot, + }; + + let encoded = frame3.encode(); + + let mut extra = 0; + let mut decoded = GpsQzssFrame3::default(); + + for (i, dword) in encoded.iter().enumerate() { + // decoded + // .decode_word(i + 3, *dword, &mut extra) + // .unwrap_or_else(|_| { + // panic!("Failed to decode dword {:3}=0x{:08X}", i, dword); + // }); + } + + assert_eq!(frame3.cic, cic); + assert_eq!(frame3.cis, cis); + assert_eq!(frame3.crc, crc); + assert_eq!(frame3.i0, i0); + assert_eq!(frame3.iode, iode); + assert_eq!(frame3.idot, idot); + assert_eq!(frame3.omega0, omega0); + assert_eq!(frame3.omega, omega); + assert_eq!(frame3.omega_dot, omega_dot); + } + } } diff --git a/src/gps/frame_id.rs b/src/gps/frame_id.rs index 7423e37..437e9d9 100644 --- a/src/gps/frame_id.rs +++ b/src/gps/frame_id.rs @@ -13,6 +13,17 @@ pub enum GpsQzssFrameId { Ephemeris3, } +#[cfg(feature = "std")] +impl std::fmt::Display for GpsQzssFrameId { + fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result { + match self { + Self::Ephemeris1 => write!(f, "EPH-1"), + Self::Ephemeris2 => write!(f, "EPH-2"), + Self::Ephemeris3 => write!(f, "EPH-3"), + } + } +} + impl GpsQzssFrameId { /// [GpsQzssFrameId] decoding attempt pub(crate) fn decode(mask: u8) -> Result { @@ -23,6 +34,15 @@ impl GpsQzssFrameId { _ => Err(GpsError::UnknownFrameType), } } + + /// Encodes this [GpsQzssFrameId] as [u8] + pub fn encode(&self) -> u8 { + match self { + Self::Ephemeris1 => 1, + Self::Ephemeris2 => 2, + Self::Ephemeris3 => 3, + } + } } #[cfg(test)] diff --git a/src/gps/how.rs b/src/gps/how.rs index 8cd0f97..c50540f 100644 --- a/src/gps/how.rs +++ b/src/gps/how.rs @@ -1,4 +1,4 @@ -use crate::gps::GpsError; +use crate::gps::{GpsDataWord, GpsError, GPS_PARITY_MASK, GPS_PARITY_SIZE}; const TOW_MASK: u32 = 0x3fffE000; const TOW_SHIFT: u32 = 13; @@ -14,11 +14,11 @@ use crate::gps::GpsQzssFrameId; #[cfg(doc)] use crate::gps::GpsQzssTelemetry; -/// [GpsQzssHow] marks the beginning of each frame, following [GpsQzssTelemetry] +/// [GpsQzssHow] (GPS HandOver Word) marks the beginning of each frame, following [GpsQzssTelemetry], +/// and defines the content to follow. #[derive(Debug, Default, Copy, Clone, PartialEq)] -/// [GpsHowWord] pub struct GpsQzssHow { - /// TOW (in seconds) + /// 17-bit TOW (in seconds) pub tow: u32, /// When alert is asserted, the SV URA may be worse than indicated in subframe 1 @@ -32,7 +32,55 @@ pub struct GpsQzssHow { pub frame_id: GpsQzssFrameId, } +#[cfg(feature = "std")] +impl std::fmt::Display for GpsQzssHow { + fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result { + write!( + f, + "TOW={} - ALERT={} - A/S={} - ID={}", + self.tow, self.alert, self.anti_spoofing, self.frame_id + ) + } +} + impl GpsQzssHow { + /// Copies and returns [GpsQzssHow] with updated TOW in seconds + pub fn with_tow_seconds(mut self, tow_seconds: u32) -> Self { + self.tow = tow_seconds; + self + } + + /// Copies and returns [GpsQzssHow] with updated [GpsQzssFrameId] + pub fn with_frame_id(mut self, frame_id: GpsQzssFrameId) -> Self { + self.frame_id = frame_id; + self + } + + /// Copies and returns [GpsQzssHow] with updated alert bit asserted + pub fn with_alert_bit(mut self) -> Self { + self.alert = true; + self + } + + /// Copies and returns [GpsQzssHow] with updated alert bit deasserted + pub fn without_alert_bit(mut self) -> Self { + self.alert = false; + self + } + + /// Copies and returns [GpsQzssHow] with A/S bit asserted + pub fn with_anti_spoofing(mut self) -> Self { + self.anti_spoofing = true; + self + } + + /// Copies and returns [GpsQzssHow] with A/S bit deasserted + pub fn without_anti_spoofing(mut self) -> Self { + self.anti_spoofing = false; + self + } + + /// Constructs a default EPH-1 [GpsQzssHow] pub fn ephemeris1() -> Self { Self { tow: 0, @@ -42,6 +90,7 @@ impl GpsQzssHow { } } + /// Constructs a default EPH-2 [GpsQzssHow] pub fn ephemeris2() -> Self { Self { tow: 0, @@ -51,6 +100,7 @@ impl GpsQzssHow { } } + /// Constructs a default EPH-3 [GpsQzssHow] pub fn ephemeris3() -> Self { Self { tow: 0, @@ -60,11 +110,15 @@ impl GpsQzssHow { } } - pub(crate) fn decode(dword: u32) -> Result { - let tow = ((dword & TOW_MASK) >> TOW_SHIFT) as u32; - let frame_id = GpsQzssFrameId::decode(((dword & FRAMEID_MASK) >> FRAMEID_SHIFT) as u8)?; - let alert = (dword & ALERT_MASK) > 0; - let anti_spoofing = (dword & AS_MASK) > 0; + /// Decodes [GpsQzssHow] from this [GpsDataWord]. + /// Subframe must be supported for this to work. + pub(crate) fn from_word(word: GpsDataWord) -> Result { + let value = word.value(); + + let tow = (value & TOW_MASK) >> TOW_SHIFT; + let frame_id = GpsQzssFrameId::decode(((value & FRAMEID_MASK) >> FRAMEID_SHIFT) as u8)?; + let alert = (value & ALERT_MASK) > 0; + let anti_spoofing = (value & AS_MASK) > 0; Ok(Self { alert, @@ -73,4 +127,69 @@ impl GpsQzssHow { tow: tow * 6, }) } + + /// Encodes this [GpsQzssHow] word as [GpsDataWord]. + pub(crate) fn to_word(&self) -> GpsDataWord { + let mut value = 0u32; + + if self.alert { + value |= ALERT_MASK; + } + + if self.anti_spoofing { + value |= AS_MASK; + } + + value |= ((self.tow / 6) & 0x1ffff) << TOW_SHIFT; + value += (self.frame_id.encode() as u32) << FRAMEID_SHIFT; + + // TODO parity + + value <<= 2; + + GpsDataWord::from(value) + } +} + +#[cfg(test)] +mod test { + use super::{TOW_MASK, TOW_SHIFT}; + use crate::gps::{GpsDataWord, GpsQzssFrameId, GpsQzssHow}; + + #[test] + fn encoding() { + for (dword, tow, frame_id, alert, anti_spoofing) in [ + (0x12344400, 0x02468, GpsQzssFrameId::Ephemeris1, true, false), + ( + 0x12340400, + 0x02468, + GpsQzssFrameId::Ephemeris1, + false, + false, + ), + (0x12342400, 0x02468, GpsQzssFrameId::Ephemeris1, false, true), + (0x12342800, 0x02468, GpsQzssFrameId::Ephemeris2, false, true), + (0x12342C00, 0x02468, GpsQzssFrameId::Ephemeris3, false, true), + (0x32342C00, 0x06468, GpsQzssFrameId::Ephemeris3, false, true), + (0x42342C00, 0x08468, GpsQzssFrameId::Ephemeris3, false, true), + (0x82342C00, 0x10468, GpsQzssFrameId::Ephemeris3, false, true), + ] { + let gps_word = GpsDataWord::from(dword); + + let gps_how = GpsQzssHow::from_word(gps_word).unwrap_or_else(|e| { + panic!("failed to decode gps-how from 0x{:08X} : {}", dword, e); + }); + + assert_eq!(gps_how.tow, tow * 6); + assert_eq!(gps_how.alert, alert); + assert_eq!(gps_how.frame_id, frame_id); + assert_eq!(gps_how.anti_spoofing, anti_spoofing); + assert_eq!( + gps_how.to_word(), + gps_word, + "Encoding reciprocal issue for dword=0x{:08X}", + dword + ); + } + } } diff --git a/src/gps/mod.rs b/src/gps/mod.rs index 9dfe766..338ca63 100644 --- a/src/gps/mod.rs +++ b/src/gps/mod.rs @@ -1,144 +1,64 @@ -//! GPS / QZSS protocol +/// GPS preamble (SYNC) byte +pub const GPS_PREAMBLE_BYTE: u8 = 0x8B; -/// GPS uses 30 bit data words, and is not aligned. -pub const GPS_WORD_SIZE: usize = 30; +/// GPS data word size (in bits!) +pub const GPS_WORD_BITS: usize = 30; -/// Minimal allocation to encode a correct GPS data frame. -pub const GPS_MIN_SIZE: usize = GPS_WORD_SIZE * 10; +/// Number of words in a frame +pub const GPS_WORDS_PER_FRAME: usize = 10; + +/// Total GPS/QZSS frame size (in bits!) +pub const GPS_FRAME_BITS: usize = GPS_WORDS_PER_FRAME * GPS_WORD_BITS; + +/// Total GPS/QZSS frame size (in bytes!) +pub const GPS_FRAME_BYTES: usize = (GPS_FRAME_BITS / 8) + 1; + +/// Total number of bits in a subframe +pub(crate) const GPS_SUBFRAME_BITS: usize = (GPS_WORDS_PER_FRAME - 2) * GPS_WORD_BITS; + +/// Parity bit mask (for each [GpsDataWord]) +pub(crate) const GPS_PARITY_MASK: u32 = 0x0000_003f; + +/// Number of parity bits for each [GpsDataWord] +pub(crate) const GPS_PARITY_SIZE: usize = 6; mod bytes; -mod decoder; +pub use bytes::GpsDataByte; + +mod word; +pub use word::GpsDataWord; + +// mod cdma; + +// mod decoder; +// pub use decoder::GpsQzssDecoder; + +// mod decoding; +// mod encoding; + mod errors; +pub use errors::GpsError; + mod frame1; -mod frame2; -mod frame3; -mod frame_id; -mod how; -mod tlm; +pub use frame1::GpsQzssFrame1; -pub use bytes::GpsDataByte; -pub use decoder::GpsQzssDecoder; -pub use errors::GpsError; +// mod frame2; +// pub use frame2::GpsQzssFrame2; + +// mod frame3; +// pub use frame3::GpsQzssFrame3; + +mod frame_id; pub use frame_id::GpsQzssFrameId; + +mod how; pub use how::GpsQzssHow; + +mod tlm; pub use tlm::GpsQzssTelemetry; -pub use frame1::GpsQzssFrame1; -pub use frame2::GpsQzssFrame2; -pub use frame3::GpsQzssFrame3; - -/// GPS / QZSS interpreted frame. -#[derive(Debug, Copy, Clone, PartialEq)] -pub struct GpsQzssFrame { - /// [GpsQzssHow] describes following frame. - pub how: GpsQzssHow, - - /// [GpsQzssTelemetry] describes following frame. - pub telemetry: GpsQzssTelemetry, - - /// [GpsQzssSubframe] depends on associated How. - pub subframe: GpsQzssSubframe, -} - -/// GPS / QZSS Interpreted subframes -#[derive(Debug, Copy, Clone, PartialEq)] -pub enum GpsQzssSubframe { - /// GPS Ephemeris Frame #1 - Ephemeris1(GpsQzssFrame1), - - /// GPS Ephemeris Frame #2 - Ephemeris2(GpsQzssFrame2), - - /// GPS Ephemeris Frame #3 - Ephemeris3(GpsQzssFrame3), -} - -impl Default for GpsQzssSubframe { - fn default() -> Self { - Self::Ephemeris1(Default::default()) - } -} - -impl GpsQzssSubframe { - /// Unwraps self as [GpsQzssFrame1] reference (if feasible) - pub fn as_eph1(&self) -> Option<&GpsQzssFrame1> { - match self { - Self::Ephemeris1(frame) => Some(frame), - _ => None, - } - } - - /// Unwraps self as mutable [GpsQzssFrame1] reference (if feasible) - pub fn as_mut_eph1(&mut self) -> Option<&mut GpsQzssFrame1> { - match self { - Self::Ephemeris1(frame) => Some(frame), - _ => None, - } - } - - /// Unwraps self as [GpsQzssFrame2] reference (if feasible) - pub fn as_eph2(&self) -> Option<&GpsQzssFrame2> { - match self { - Self::Ephemeris2(frame) => Some(frame), - _ => None, - } - } - - /// Unwraps self as [GpsQzssFrame2] reference (if feasible) - pub fn as_mut_eph2(&mut self) -> Option<&mut GpsQzssFrame2> { - match self { - Self::Ephemeris2(frame) => Some(frame), - _ => None, - } - } - - /// Unwraps self as [GpsQzssFrame3] reference (if feasible) - pub fn as_eph3(&self) -> Option<&GpsQzssFrame3> { - match self { - Self::Ephemeris3(frame) => Some(frame), - _ => None, - } - } - - /// Unwraps self as [GpsQzssFrame3] reference (if feasible) - pub fn as_mut_eph3(&mut self) -> Option<&mut GpsQzssFrame3> { - match self { - Self::Ephemeris3(frame) => Some(frame), - _ => None, - } - } -} - -// /// Verifies 24-bit LSB (right aligned) parity -// pub(crate) fn check_parity(value: u32) -> bool { -// let data = value >> 6; -// let expected = parity_encoding(data); -// let parity = (value & 0x3f) as u8; -// -// if expected == parity { -// true -// } else { -// #[cfg(feature = "log")] -// error!( -// "GPS: parity error - expected 0x{:02X} - got 0x{:02X}", -// expected, parity -// ); -// -// false -// } -// } -// -// /// Encodes 24-bit LSB into 6 bit parity (right aligned!) -// pub(crate) fn parity_encoding(value: u32) -> u8 { -// let generator = 0x61_u32; -// let mut reg = value << 6; -// -// for _ in 0..24 { -// if reg & (1 << 29) != 0 { -// reg ^= generator << 23; -// } -// reg <<= 1; -// } -// -// ((reg >> 30) & 0x3f) as u8 -// } +// mod frame; +// pub use frame::GpsQzssFrame; + +// mod subframe; +// pub subframe::GpsQzssSubframe; diff --git a/src/gps/reader.rs b/src/gps/reader.rs new file mode 100644 index 0000000..b341b8a --- /dev/null +++ b/src/gps/reader.rs @@ -0,0 +1,30 @@ +pub(crate) struct BitReader<'a> { + buffer: &'a [u8], + byte_index: usize, + bit_index: u8, // 0..7 +} + +impl<'a> BitReader<'a> { + pub fn new(buffer: &'a [u8], start_bit: usize) -> Self { + Self { + buffer, + byte_index: start_bit / 8, + bit_index: (start_bit % 8) as u8, + } + } + + /// Grabs a new [u32] with two MSB padding from the input stream + pub fn read_u32(&mut self) -> Option { + let mut acc = 0u32; + + // grab 4 bytes + let mut bytes = [0; 4]; + + bytes[0] = *self.buffer.get(self.byte_index)?; + bytes[1] = *self.buffer.get(self.byte_index + 1)?; + bytes[2] = *self.buffer.get(self.byte_index + 1)?; + bytes[3] = *self.buffer.get(self.byte_index + 1)?; + + Some(u32::from_be_bytes(bytes)) + } +} diff --git a/src/gps/subframe.rs b/src/gps/subframe.rs new file mode 100644 index 0000000..2dec91c --- /dev/null +++ b/src/gps/subframe.rs @@ -0,0 +1,101 @@ +use crate::gps::{ + GpsQzssFrame1, + GpsQzssFrame2, + GpsQzssFrame3, +}; + +/// GPS / QZSS Interpreted subframes +#[derive(Debug, Copy, Clone, PartialEq)] +pub enum GpsQzssSubframe { + /// GPS Ephemeris Frame #1 + Ephemeris1(GpsQzssFrame1), + + /// GPS Ephemeris Frame #2 + Ephemeris2(GpsQzssFrame2), + + /// GPS Ephemeris Frame #3 + Ephemeris3(GpsQzssFrame3), +} + +impl Default for GpsQzssSubframe { + /// Builds a default [GpsQzssSubFrame::Ephemeris1] + fn default() -> Self { + Self::Ephemeris1(Default::default()) + } +} + +impl GpsQzssSubframe { + /// Unwraps self as [GpsQzssFrame1] reference (if feasible) + pub fn as_eph1(&self) -> Option { + match self { + Self::Ephemeris1(frame) => Some(*frame), + _ => None, + } + } + + /// Unwraps self as mutable [GpsQzssFrame1] reference (if feasible) + pub fn as_mut_eph1(&mut self) -> Option<&mut GpsQzssFrame1> { + match self { + Self::Ephemeris1(frame) => Some(frame), + _ => None, + } + } + + /// Unwraps self as [GpsQzssFrame2] reference (if feasible) + pub fn as_eph2(&self) -> Option { + match self { + Self::Ephemeris2(frame) => Some(*frame), + _ => None, + } + } + + /// Unwraps self as [GpsQzssFrame2] reference (if feasible) + pub fn as_mut_eph2(&mut self) -> Option<&mut GpsQzssFrame2> { + match self { + Self::Ephemeris2(frame) => Some(frame), + _ => None, + } + } + + /// Unwraps self as [GpsQzssFrame3] reference (if feasible) + pub fn as_eph3(&self) -> Option { + match self { + Self::Ephemeris3(frame) => Some(*frame), + _ => None, + } + } + + /// Unwraps self as [GpsQzssFrame3] reference (if feasible) + pub fn as_mut_eph3(&mut self) -> Option<&mut GpsQzssFrame3> { + match self { + Self::Ephemeris3(frame) => Some(frame), + _ => None, + } + } + + /// Decodes a [GpsQzssSubframe] from a 8 [GpsDataByte] slice + /// supports padding on each word termination (not intra words) + pub(crate) fn decode(frame_id: GpsQzssFrameId, bytes: &[GpsDataByte]) -> Self { + // for i in 0..8 { + // let dword = ByteArray::new(&bytes[8+i*4..8+i*4+4]) + // .value_u32(); + + // let _ = subframe.decode_word(i +3, dword); // IMPROVE + // + + match frame_id { + GpsQzssFrameId::Ephemeris1 => Self::Ephemeris1(GpsQzssFrame1::decode(bytes)), + GpsQzssFrameId::Ephemeris2 => Self::Ephemeris2(GpsQzssFrame2::decode(bytes)), + GpsQzssFrameId::Ephemeris3 => Self::Ephemeris3(GpsQzssFrame3::decode(bytes)), + } + } + + /// Encode this [GpsQzssSubframe] into 8 [u32] data burst. + pub(crate) fn encode(&self) -> [u32; 8] { + match self { + Self::Ephemeris1(subframe) => subframe.encode(), + Self::Ephemeris2(subframe) => subframe.encode(), + Self::Ephemeris3(subframe) => subframe.encode(), + } + } +} diff --git a/src/gps/tlm.rs b/src/gps/tlm.rs index 0492bde..9367c60 100644 --- a/src/gps/tlm.rs +++ b/src/gps/tlm.rs @@ -1,12 +1,10 @@ -use crate::gps::GpsError; +use crate::gps::{GpsDataWord, GpsError, GPS_PARITY_MASK, GPS_PARITY_SIZE}; -const PREAMBLE_MASK: u32 = 0x42C00000; - -const MESSAGE_MASK: u32 = 0x003fff00; +const PREAMBLE_MASK: u32 = 0x22C0_0000; const MESSAGE_SHIFT: u32 = 8; - -const INTEGRITY_BIT_MASK: u32 = 0x00000080; -const RESERVED_BIT_MASK: u32 = 0x00000040; +const MESSAGE_MASK: u32 = 0x003f_ff00; +const INTEGRITY_BIT_MASK: u32 = 0x0000_0080; +const RESERVED_BIT_MASK: u32 = 0x0000_0040; /// [GpsQzssTelemetry] marks the beginning of each frame #[derive(Debug, Default, Copy, Clone, PartialEq)] @@ -18,27 +16,117 @@ pub struct GpsQzssTelemetry { /// with an enhanced level of integrity assurance. pub integrity: bool, - /// Reserved bits - pub reserved_bits: bool, + /// Reserved bit + pub reserved_bit: bool, +} + +#[cfg(feature = "std")] +impl std::fmt::Display for GpsQzssTelemetry { + fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result { + write!( + f, + "INTEGRITY={} - MSG=0x{:08X} - reserved={}", + self.integrity, self.message, self.reserved_bit + ) + } } impl GpsQzssTelemetry { - /// [GpsQzssTelemetry] decoding attempt. - /// The special GPS marker must be present on the MSB for this to pass. - /// When parity_check is requested, the parity check must pass as well. - pub fn decode(dword: u32) -> Result { - if dword & PREAMBLE_MASK == PREAMBLE_MASK { + /// Copies and returns new [GpsQzssTelemetry] with updated 14-bit TLM message + pub fn with_message(mut self, message_14b: u16) -> Self { + self.message = message_14b & 0x3fff; + self + } + + /// Copies and returns new [GpsQzssTelemetry] with message integrity asserted + pub fn with_integrity(mut self) -> Self { + self.integrity = true; + self + } + + /// Copies and returns new [GpsQzssTelemetry] with message integrity deasserted + pub fn without_integrity(mut self) -> Self { + self.integrity = false; + self + } + + /// Copies and returns new [GpsQzssTelemetry] with reserved bit asserted + pub fn with_reserved_bit(mut self) -> Self { + self.reserved_bit = true; + self + } + + /// Copies and returns new [GpsQzssTelemetry] with reserved bit deasserted + pub fn without_reserved_bit(mut self) -> Self { + self.reserved_bit = false; + self + } + + /// [GpsQzssTelemetry] decoding attempt from a [GpsDataWord]. + /// The special GPS marker must be present (MSB) for this to pass. + pub(crate) fn from_word(word: GpsDataWord) -> Result { + let value = word.value(); + + if value & PREAMBLE_MASK != PREAMBLE_MASK { return Err(GpsError::InvalidPreamble); }; - let message = ((dword & MESSAGE_MASK) >> MESSAGE_SHIFT) as u16; - let integrity = (dword & INTEGRITY_BIT_MASK) > 0; - let reserved_bits = (dword & RESERVED_BIT_MASK) > 0; + let message = ((value & MESSAGE_MASK) >> MESSAGE_SHIFT) as u16; + let integrity = (value & INTEGRITY_BIT_MASK) > 0; + let reserved_bit = (value & RESERVED_BIT_MASK) > 0; Ok(Self { message, integrity, - reserved_bits, + reserved_bit, }) } + + /// Encodes this [GpsQzssTelemetry] as [GpsDataWord]. + pub(crate) fn to_word(&self) -> GpsDataWord { + let mut value = PREAMBLE_MASK; + + value |= (self.message as u32) << MESSAGE_SHIFT; + + if self.integrity { + value |= INTEGRITY_BIT_MASK; + } + + if self.reserved_bit { + value |= RESERVED_BIT_MASK; + } + + // TODO parity + + value <<= 2; + + GpsDataWord::from(value) + } +} + +#[cfg(test)] +mod tlm { + use crate::gps::{GpsDataWord, GpsQzssTelemetry}; + + #[test] + fn encoding() { + for (dword, message, integrity, reserved_bit) in [ + (0x8B000000, 0x0000, false, false), + (0x8B123400, 0x048D, false, false), + (0x8B123600, 0x048D, true, false), + (0x8B123700, 0x048D, true, true), + (0x8B123500, 0x048D, false, true), + ] { + let gps_word = GpsDataWord::from(dword); + + let tlm = GpsQzssTelemetry::from_word(gps_word).unwrap_or_else(|e| { + panic!("failed to decode gps-tlm from 0x{:08X} - {}", dword, e); + }); + + assert_eq!(tlm.message, message); + assert_eq!(tlm.integrity, integrity); + assert_eq!(tlm.reserved_bit, reserved_bit); + assert_eq!(tlm.to_word(), gps_word, "Reciprocal issue"); + } + } } diff --git a/src/gps/word.rs b/src/gps/word.rs new file mode 100755 index 0000000..7fb5a3a --- /dev/null +++ b/src/gps/word.rs @@ -0,0 +1,118 @@ +use crate::gps::GpsDataByte; + +/// [GpsDataWord] is used to pack [GpsDataByte]s. +#[derive(Debug, Copy, Clone, PartialEq)] +pub struct GpsDataWord { + /// [GpsDataByte]s stored with "big endianness" + bytes: [GpsDataByte; 4], +} + +impl Default for GpsDataWord { + /// Creates a default null [GpsDataWord]. + fn default() -> Self { + Self { + bytes: Default::default(), + } + } +} + +impl From for GpsDataWord { + fn from(value: u32) -> GpsDataWord { + GpsDataWord::from_be_bytes(&value.to_be_bytes()) + } +} + +impl Into for GpsDataWord { + fn into(self) -> u32 { + self.value() + } +} + +impl GpsDataWord { + /// Creates a [GpsDataWord] from a 30-bit slice stored as 4 big-endian bytes. + pub fn from_be_bytes(slice: &[u8; 4]) -> Self { + Self { + bytes: [ + GpsDataByte::Byte(slice[0]), + GpsDataByte::Byte(slice[1]), + GpsDataByte::Byte(slice[2]), + GpsDataByte::LsbPadded(slice[3]), + ], + } + } + + /// Creates a [GpsDataWord] from a 30-bit slice stored as 4 little-endian bytes. + pub fn from_le_bytes(slice: &[u8; 4]) -> Self { + Self { + bytes: [ + GpsDataByte::Byte(slice[3]), + GpsDataByte::Byte(slice[2]), + GpsDataByte::Byte(slice[1]), + GpsDataByte::LsbPadded(slice[0]), + ], + } + } + + /// Converts this [GpsDataWord] to [u32] + pub fn value(&self) -> u32 { + let mut value = self.bytes[3].as_u32(); + value |= self.bytes[2].as_u32() << 6; + value |= self.bytes[1].as_u32() << 14; + value |= self.bytes[0].as_u32() << 22; + value + } +} + +#[cfg(test)] +mod test { + use super::*; + + #[test] + fn from_be_bytes() { + for (bytes, value) in [ + ([0x8B, 0x12, 0x48, 0xCA], 0x22C49232), + ([0x8B, 0xAA, 0xAA, 0xAA], 0x22EAAAAA), + ([0x8B, 0xA5, 0xA5, 0xA5], 0x22E96969), + ([0x8B, 0x5A, 0x5A, 0x5A], 0x22D69696), + ] { + let word = GpsDataWord::from_be_bytes(&bytes); + let found = word.value(); + assert_eq!( + found, value, + "expected 0x{:08X}, got 0x{:08X}", + value, found + ); + } + } + + #[test] + fn from_le_bytes() { + for (bytes, value) in [ + ([0xCA, 0x48, 0x12, 0x8B], 0x22C49232), + ([0xAA, 0xAA, 0xAA, 0x8B], 0x22EAAAAA), + ([0xA5, 0xA5, 0xA5, 0x8B], 0x22E96969), + ([0x5A, 0x5A, 0x5A, 0x8B], 0x22D69696), + ] { + let word = GpsDataWord::from_le_bytes(&bytes); + let found = word.value(); + assert_eq!( + found, value, + "expected 0x{:08X}, got 0x{:08X}", + value, found + ); + } + } + + #[test] + fn from_u32() { + for (value, expected_value) in [ + (0x32563412u32, 0x32563412), + (0xF2563412u32, 0x32563412), + (0x00563410u32, 0x00563410), + (0x00333410u32, 0x00333410), + ] { + let word = GpsDataWord::from(value); + assert_eq!(word.value(), expected_value); + } + } +} diff --git a/src/tests.rs b/src/tests.rs index 71a523a..725c17b 100644 --- a/src/tests.rs +++ b/src/tests.rs @@ -1,3 +1,5 @@ +use std::{fs::File, io::Read}; + #[cfg(feature = "std")] use std::sync::Once; @@ -20,34 +22,223 @@ pub fn init_logger() { }); } -#[cfg(feature = "gps")] -pub fn from_ublox_be_bytes(bytes: &[u8; N]) -> Vec { - let mut ret = Vec::new(); - let mut word32 = 0u32; - - for (index, byte) in bytes.iter().enumerate() { - if index % 4 == 0 { - ret.push(GpsDataByte::msb_padded(*byte)); - } else { - ret.push(GpsDataByte::Byte(*byte)); +// TLM : 10001011 0000000000000000 101010 (préambule + padding + parité fictive) +// HOW : 00000000000000000 0001 111000 (Z-count fictif + SubframeID=1 + parité) +// W3 : 010101010101010101010101 101010 +// W4 : 001100110011001100110011 110011 +// W5 : 111100001111000011110000 000111 +// W6 : 000111000111000111000111 111000 +// W7 : 101010101010101010101010 010101 +// W8 : 011001100110011001100110 101101 +// W9 : 110000110000110000110000 111100 +// W10 : 111111000000111111000000 000111 +pub const GPS_EPH1_DATA: [u8; 40] = [ + 0x8B, // PREAMBLE + 0x00, 0x00, // TLM-MSG+I+R + 0x17, // PAR(TLM=5) + 2b HOW=3 + 0x00, 0x00, // HOW.TOW + A + 0x90, // A/S + FID=1 + 0x00, // PAR(HOW) + 0x00, // P1, P2, PAR(MSBx2) + 0x00, // PAR(LSBx4), BIT29=0, BIT30=0 + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +]; + +/// Inserts desired number of zero (bits) at the begginning of a frame +pub fn insert_zeros(slice: &[u8], shift_bits: usize) -> Vec { + let size = slice.len(); + let mut out = vec![0u8; size]; + + let byte_shift = shift_bits / 8; + let bit_shift = shift_bits % 8; + + for (i, &byte) in slice.iter().enumerate() { + let out_idx = i + byte_shift; + + if out_idx >= size { + break; + } + + out[out_idx] |= byte >> bit_shift; + + if bit_shift != 0 && out_idx + 1 < size { + out[out_idx + 1] |= byte << (8 - bit_shift); + } + } + + out +} + +/// A custom [FileReader] with possible offset (delay) +/// in the stream. +pub struct FileReader { + // FD + fd: File, + + // RD pointer + rd_ptr: usize, + + // WR pointer + wr_ptr: usize, + + // Initial offset in bits + initial_offset_bits: usize, + + // internal buufer + buffer: [u8; N], +} + +impl std::io::Read for FileReader { + fn read(&mut self, buffer: &mut [u8]) -> std::io::Result { + let mut capacity = buffer.len(); + + let avail_in_buffer = self.rd_ptr; + + if self.rd_ptr > 0 { + // provide existing content + if capacity > self.rd_ptr { + buffer[..avail_in_buffer].copy_from_slice(&self.buffer[self.rd_ptr..]); + + self.rd_ptr = 0; + capacity -= self.rd_ptr; + } else { + // provide capacity and return + self.rd_ptr += capacity; + return Ok(capacity); + } + } + + // read new content + let new_size = self.fd.read(&mut self.buffer[self.rd_ptr..])?; + + if new_size == 0 { + if avail_in_buffer == 0 { + // end of stream + return Ok(0); + } else { + // already copied + self.rd_ptr += new_size; + return Ok(avail_in_buffer); + } + } + + if capacity >= new_size { + // can absorb new content entirely + buffer[avail_in_buffer..].copy_from_slice(&self.buffer[self.rd_ptr..]); + + self.rd_ptr += new_size; + + return Ok(avail_in_buffer + new_size); + } + + // can only absorb a part of it + + Ok(0) + } +} + +impl FileReader { + /// Creates new [FileReader] with possible fake offset in bits + pub fn new(fp: &str, initial_offset_bits: usize) -> Self { + let initial_offset_bytes = initial_offset_bits / 8; + + let mut buffer = [0; N]; + + let mut fd = File::open(fp).unwrap_or_else(|e| { + panic!("Failed to open file {}: {}", fp, e); + }); + + Self { + fd, + buffer, + initial_offset_bits, + wr_ptr: 0, + rd_ptr: 0, } } +} + +#[test] +fn test_zeros_padder() { + let mut buffer = [0; 1024]; + let mut file = FileReader::<1024>::new("data/GPS/eph-1.bin", 0); + + file.read(&mut buffer).unwrap_or_else(|e| { + panic!("Failed to read test data: {}", e); + }); + + let size = buffer.len(); + + // test offset by bytes + for i in 1..16 { + // insert 1 byte + let delayed = insert_zeros(&buffer, i * 8); + + assert_eq!(delayed.len(), size); // size should never change + } + + // test offset by bits + for i in 1..7 { + let delayed = insert_zeros(&buffer, i); + + assert_eq!(delayed[0], 0x8B >> i); + assert_eq!(delayed.len(), size); // size should never change + } +} - ret +#[test] +fn test_file_reader() { + let mut buffer = [0; 1024]; + let mut file = FileReader::<1024>::new("data/GPS/eph1.bin", 0); + + file.read(&mut buffer).unwrap_or_else(|e| { + panic!("Failed to read test data: {}", e); + }); + + assert_eq!(buffer[0], 0x8B); + assert_eq!(buffer[1], 0x48); + assert_eq!(buffer[2], 0x10); + assert_eq!(buffer[3], 0x04); +} + +#[cfg(feature = "gps")] +pub fn from_ublox_be_bytes(bytes: &[u8; N]) -> Vec { + bytes + .iter() + .enumerate() + .map(|(index, byte)| { + if index % 4 == 0 { + GpsDataByte::padded(*byte) + } else { + GpsDataByte::Byte(*byte) + } + }) + .collect() } #[test] #[cfg(feature = "gps")] fn test_from_ublox_be_bytes() { - let bytes = [ + let ubx_bytes = [ // TLM 0x22, 0xC1, 0x3E, 0x1B, ]; - let bytes = from_ublox_be_bytes(&bytes); + let bytes = from_ublox_be_bytes(&ubx_bytes); assert_eq!(bytes.len(), 4); - let bytes = [ + assert_eq!( + bytes, + [ + GpsDataByte::LsbPadded(0x22), + GpsDataByte::Byte(0xC1), + GpsDataByte::Byte(0x3E), + GpsDataByte::Byte(0x1B) + ] + ); + + let ubx_bytes = [ // TLM 0x22, 0xC1, 0x3E, 0x1B, // HOW 0x73, 0xC9, 0x27, 0x15, // WORD3 @@ -61,6 +252,32 @@ fn test_from_ublox_be_bytes() { 0x31, 0x2C, 0x30, 0x33, ]; - let bytes = from_ublox_be_bytes(&bytes); + let bytes = from_ublox_be_bytes(&ubx_bytes); + assert_eq!(bytes.len(), 40); + + assert_eq!(bytes[0], GpsDataByte::LsbPadded(0x22)); + assert_eq!(bytes[1], GpsDataByte::Byte(0xC1)); + assert_eq!(bytes[2], GpsDataByte::Byte(0x3E)); + assert_eq!(bytes[3], GpsDataByte::Byte(0x1B)); + + assert_eq!(bytes[4], GpsDataByte::LsbPadded(0x33)); + assert_eq!(bytes[5], GpsDataByte::Byte(0xC9)); + assert_eq!(bytes[6], GpsDataByte::Byte(0x27)); + assert_eq!(bytes[7], GpsDataByte::Byte(0x15)); + + assert_eq!(bytes[8], GpsDataByte::LsbPadded(0x13)); + assert_eq!(bytes[9], GpsDataByte::Byte(0xE4)); + assert_eq!(bytes[10], GpsDataByte::Byte(0x00)); + assert_eq!(bytes[11], GpsDataByte::Byte(0x04)); + + assert_eq!(bytes[12], GpsDataByte::LsbPadded(0x10)); + assert_eq!(bytes[13], GpsDataByte::Byte(0x4F)); + assert_eq!(bytes[14], GpsDataByte::Byte(0x5D)); + assert_eq!(bytes[15], GpsDataByte::Byte(0x31)); + + assert_eq!(bytes[16], GpsDataByte::LsbPadded(0x17)); + assert_eq!(bytes[17], GpsDataByte::Byte(0x44)); + assert_eq!(bytes[18], GpsDataByte::Byte(0xE6)); + assert_eq!(bytes[19], GpsDataByte::Byte(0xD7)); }