diff --git a/Cargo.toml b/Cargo.toml index f8850d1..24be7a8 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -10,7 +10,7 @@ defmt = { version = "0.3", features = ["encoding-rzcobs"] } defmt-brtt = { version = "0.1", default-features = false, features = ["rtt"] } panic-probe = { version = "0.3", features = ["print-defmt"] } rtic = { version = "2.0.0", features = [ "thumbv7-backend" ] } -stm32l4xx-hal = { version = "0.7.1", features = ["stm32l442"] } +stm32l4xx-hal = { version = "0.7.1", features = ["stm32l442", "stm32-usbd"] } rtic-monotonics = { version = "1.0.0", features = ["cortex-m-systick", "systick-100hz"]} thiserror = { version = "1.0.50", package = "thiserror-core", default-features = false } embedded-graphics = "0.8.1" @@ -18,6 +18,11 @@ tinyvec = "1.6.0" ublox = { version = "0.4.5", default-features = false } nb = "1.1.0" bytemuck = { version = "1.14.3", features = ["derive"] } +stm32-usbd = "0.6.0" +usb-device = { version = "0.2.9", features = ["defmt"] } +usbd-serial = "0.1.0" +# spin = "0.9.8" +rtic-sync = "1.3.0" # cargo build/run [profile.dev] @@ -60,7 +65,8 @@ overflow-checks = false # <- [patch.crates-io] stm32l4xx-hal = { path = "./stm32l4xx-hal" } -ublox = { path = "./ublox/ublox" } +# ublox = { path = "./ublox/ublox" } +# usb-device = { path = "./usb-device" } # uncomment this to switch from the crates.io version of defmt to its git version # check app-template's README for instructions diff --git a/src/bin/minimal.rs b/src/bin/minimal.rs index c7039fc..2f3f2a0 100644 --- a/src/bin/minimal.rs +++ b/src/bin/minimal.rs @@ -4,7 +4,7 @@ use gps_watch as _; -use hal::gpio::PA1; +use hal::gpio::{PA1, PA11, PA12}; use stm32l4xx_hal::{self as hal, pac, prelude::*}; use rtic_monotonics::create_systick_token; use rtic_monotonics::systick::Systick; @@ -12,11 +12,12 @@ use stm32l4xx_hal::gpio::{Alternate, Output, PA10, PA9, PB3, PB4, PB5, PushPull} use stm32l4xx_hal::hal::spi::{Mode, Phase, Polarity}; use stm32l4xx_hal::pac::{SPI1, USART1}; use stm32l4xx_hal::spi::Spi; -use defmt::{trace, info}; +use defmt::{trace, info, error}; use embedded_graphics::prelude::*; use embedded_graphics::pixelcolor::BinaryColor; use embedded_graphics::text::Text; use stm32l4xx_hal::serial::Serial; +use usb_device::bus::UsbBusAllocator; use core::fmt::Write; use stm32l4xx_hal::rcc::{ClockSecuritySystem, CrystalBypass}; use stm32l4xx_hal::rtc::{Event, RtcClockSource, RtcConfig}; @@ -26,6 +27,12 @@ use gps_watch::gps::Gps; use embedded_graphics::mono_font::{ascii::FONT_10X20, MonoTextStyle}; use gps_watch::FmtBuf; use core::num::Wrapping; +use embedded_graphics::mono_font::iso_8859_2::FONT_4X6; +use hal::pac::{Interrupt, USB}; +use tinyvec::ArrayVec; +use usb_device::device::{UsbDeviceBuilder, UsbVidPid}; +use usbd_serial::USB_CLASS_CDC; +use rtic_sync::make_channel; // Rename type to squash generics type SharpMemDisplay = gps_watch::display::SharpMemDisplay< @@ -46,6 +53,8 @@ type GpsUart = Serial>, PA10 (Shared, Local) { + fn init(cx: init::Context) -> (Shared, Local) { trace!("init enter"); // #[cfg(debug_assertions)] @@ -119,7 +127,7 @@ mod app { polarity: Polarity::IdleLow }, true, - 1.MHz(), + 2.MHz(), clocks, &mut rcc.apb2 ); @@ -151,31 +159,31 @@ mod app { ); gps_uart.listen(serial::Event::Rxne); - // Initialize USB Serial - // let dm = gpioa.pa11.into_alternate(&mut gpioa.moder, &mut gpioa.otyper, &mut gpioa.afrh); - // let dp = gpioa.pa12.into_alternate(&mut gpioa.moder, &mut gpioa.otyper, &mut gpioa.afrh); + // Enable the USB interrupt + // let usb = unsafe {hal::pac::Peripherals::steal()}.USB; + // usb.cntr.write(|w| w.wkupm().enabled()); - // let usb = hal::usb::Peripheral { - // usb: cx.device.USB, - // pin_dm: dm, - // pin_dp: dp - // }; + // Initialize USB Serial + let dm = gpioa.pa11.into_alternate(&mut gpioa.moder, &mut gpioa.otyper, &mut gpioa.afrh); + let dp = gpioa.pa12.into_alternate(&mut gpioa.moder, &mut gpioa.otyper, &mut gpioa.afrh); - // static USB_BUS: Once = Once::new(); - // USB_BUS.set(hal::usb::UsbBus::new(usb)); + // Turn on USB power + unsafe { pac::Peripherals::steal().PWR.cr2.modify(|_, w| w.usv().set_bit())}; - // // let _: () = USB_BUS.get().unwrap(); - // let usb_serial = usbd_serial::SerialPort::new(USB_BUS.get().unwrap()); + // Create USB peripheral object + let usb = hal::usb::Peripheral { + usb: cx.device.USB, + pin_dm: dm, + pin_dp: dp + }; - // let usb_dev = UsbDeviceBuilder::new(USB_BUS.get().unwrap(), UsbVidPid(0x1209, 0x0001)) - // .manufacturer("ECE500") - // .product("Landhopper") - // .serial_number("TEST") - // .device_class(USB_CLASS_CDC) - // .build(); + let (usb_tx, usb_rx) = make_channel!(u8, 16); + // Pass to task for remaining initialization + let _ = usb_poll::spawn(usb, usb_rx); // Spawn tasks - display_task::spawn().unwrap(); + display_task::spawn(usb_tx.clone()).unwrap(); + // gps_status::spawn().unwrap(); info!("done initializing!"); trace!("init exit"); @@ -186,7 +194,7 @@ mod app { }, Local { // Initialization of local resources go here - count: 0 + // count: 0 }, ) } @@ -197,11 +205,7 @@ mod app { trace!("idle enter"); loop { - - trace!("usb"); - - - trace!("sleep"); + // trace!("sleep"); // Only sleep in release mode, since the debugger doesn't interact with sleep very nice #[cfg(debug_assertions)] core::hint::spin_loop(); @@ -210,45 +214,111 @@ mod app { } } - #[task(binds = USART1, shared = [gps], local = [count])] + #[task(priority = 1)] + async fn usb_poll(_cx: usb_poll::Context, usb: hal::usb::Peripheral, mut rx: Receiver<'static, u8, 16>) { + trace!("usb_poll enter"); + + let usb_bus = hal::usb::UsbBus::new(usb); + + let mut serial = usbd_serial::SerialPort::new(&usb_bus); + + let mut usb_dev = UsbDeviceBuilder::new(&usb_bus, UsbVidPid(0x16c0, 0x27dd)) + .manufacturer("ECE500") + .product("Landhopper") + .serial_number("TEST") + .device_class(USB_CLASS_CDC) + .build(); + + let mut tx_buf = ArrayVec::<[u8; 16]>::new(); + + loop { + Systick::delay(10.millis()).await; + + debug!("usb_poll loop"); + + while tx_buf.len() < tx_buf.capacity() { + if let Ok(b) = rx.try_recv() { + tx_buf.push(b); + } else { break; } + } + trace!("usb state: {}", usb_dev.state()); + + if !usb_dev.poll(&mut [&mut serial]) { + continue; + } + + match serial.write(&tx_buf) { + Ok(count) => { + trace!("sent {} bytes to usb", count); + tx_buf.drain(0..count).for_each(|_| ()); + }, + Err(_) => error!("usb error") + } + } + } + + #[task(binds = USART1, priority = 10, shared = [gps])] fn on_uart(mut cx: on_uart::Context) { - // cx.shared.gps.lock(|gps| { - // gps.handle(); - // }); cx.shared.gps.lock(|gps| { - if let Ok(b) = gps.serial.read() { - *cx.local.count += 1; - info!("got {:x} #{}", b, cx.local.count); - } - }) + gps.handle(); + }); + // cx.shared.gps.lock(|gps| { + // while let Ok(b) = gps.serial.read() { + // let _ = cx.shared.recv_buf.lock(|(buf, started)| { + // if b == 0xB5 || *started { + // buf.try_push(b); + // *started = true; + // } + // }); + // } + // }) } + // #[task(priority = 1, shared = [recv_buf])] + // async fn gps_status(mut cx: gps_status::Context) { + // loop { + // Systick::delay(1000.millis()).await; + // cx.shared.recv_buf.lock(|x| { + // info!("received: {:x}", x.as_slice()); + // x.clear(); + // }); + // } + // } + #[task(binds = RTC_WKUP)] fn on_rtc(_cx: on_rtc::Context) { info!("rtc wakeup!"); } - // TODO: Add tasks #[task( priority = 1, shared = [display, gps] )] - async fn display_task(mut cx: display_task::Context) { + async fn display_task(mut cx: display_task::Context, tx: Sender<'static, u8, 16>) { trace!("display_task enter"); - cx.shared.display.lock(|display| display.clear()); + cx.shared.display.lock(|display| display.clear_flush()); let mut i = Wrapping(0u8); loop { - let pos = cx.shared.gps.lock(|gps| gps.position); + debug!("display_task loop"); + // let pos = cx.shared.gps.lock(|gps| gps.position); let mut txt = FmtBuf(Default::default()); - write!(txt, "Lat: {}\nLon: {}", pos.latitude, pos.longitude).unwrap(); + // write!(txt, "Lat: {}\nLon: {}", pos.latitude, pos.longitude).unwrap(); + // cx.shared.recv_buf.lock(|(x, started)| { + // let _ = write!(txt, "{:x} {x:x?}", x.len()); + // x.clear(); + // *started = false; + // }); + cx.shared.gps.lock(|gps| { + let _ = write!(txt, "{} {:x?}", gps.count, gps.last_packet); + }); // info!("formatted: {}", txt.as_str()); cx.shared.display.lock(|display| { display.clear(); Text::new( txt.as_str().unwrap(), Point::new(30, 30), - MonoTextStyle::new(&FONT_10X20, BinaryColor::On) + MonoTextStyle::new(&FONT_4X6, BinaryColor::On) ).draw(display).unwrap(); display.flush(); }); @@ -260,7 +330,7 @@ mod app { // rect_styled.draw(display).unwrap(); // display.flush(); // }); - Systick::delay(500.millis()).await; + Systick::delay(1000.millis()).await; i += 1; } } diff --git a/src/display.rs b/src/display.rs index 08b8fd8..2d48830 100644 --- a/src/display.rs +++ b/src/display.rs @@ -151,6 +151,13 @@ impl SharpMemDisplay self.dirty = [0xFF; HEIGHT_BYTES]; self.dirty_any = true; } + + pub fn clear_flush(&mut self) { + self.driver.clear_flush(); + self.buf = [[0xFF; WIDTH_BYTES]; HEIGHT]; + self.dirty = [0; HEIGHT_BYTES]; + self.dirty_any = false; + } } impl DrawTarget for SharpMemDisplay diff --git a/src/gps.rs b/src/gps.rs index 7d32ef8..0db7d75 100644 --- a/src/gps.rs +++ b/src/gps.rs @@ -1,13 +1,13 @@ use core::fmt::{self, Write}; -use defmt::{error, info}; +use defmt::{error, info, trace}; use stm32l4xx_hal::hal::serial; use ublox::{CfgMsgAllPortsBuilder, CfgPrtUartBuilder, NavPvt, UartMode, UartPortId}; use tinyvec::ArrayVec; -use crate::{ubx::{UbxPacket, UbxParser}, FmtBuf}; +use crate::{ubx::{UbxError, UbxPacket, UbxParser}, FmtBuf}; -#[derive(Copy, Clone)] +#[derive(Debug, Copy, Clone)] pub struct Position { pub latitude: i32, pub longitude: i32, @@ -18,7 +18,8 @@ pub struct Gps { pub parser: UbxParser, pub position: Position, - count: usize + pub last_packet: Result, + pub count: usize } impl Gps @@ -26,11 +27,16 @@ impl Gps >::Error: core::fmt::Debug, >::Error: core::fmt::Debug, { - pub fn new(serial: SERIAL) -> Self { + pub fn new(mut serial: SERIAL) -> Self { + + // Busy loop waiting for the GPS to be alive + while let Err(_) = serial.read() {} + let mut s = Self { serial, parser: UbxParser::new(), position: Position { latitude: 0, longitude: 0 }, + last_packet: Ok(UbxPacket::OtherPacket), count: 0 }; @@ -70,17 +76,13 @@ impl Gps pub fn handle(&mut self) { if let Ok(b) = self.serial.read() { self.count += 1; - info!("got {:x} #{}", b, self.count); + trace!("got {:x} #{}", b, self.count); if let Some(r) = self.parser.process_byte(b) { - match r { - Ok(p) => match p { - UbxPacket::AckAck {..} => info!("ubx AckAck"), - UbxPacket::AckNak {..} => info!("ubx AckNak"), - UbxPacket::NavPvt(n) => info!("ubx lat={}, lon={}", n.lat, n.lon), - UbxPacket::OtherPacket => info!("ubx other") - }, - Err(e) => error!("ubx error: {:x}", e) + if let Ok(UbxPacket::NavPvt(ref navpvt)) = r { + self.position.latitude = navpvt.lat; + self.position.longitude = navpvt.lon; } + self.last_packet = r; } } } diff --git a/src/ubx.rs b/src/ubx.rs index 980c099..a6b076c 100644 --- a/src/ubx.rs +++ b/src/ubx.rs @@ -24,7 +24,7 @@ impl PartialEq<(u8, u8)> for UbxChecksum { } } -#[derive(defmt::Format)] +#[derive(defmt::Format, Debug)] pub enum UbxError { BadChecksum {expect: (u8, u8), saw: (u8, u8)}, BadStart {expect: u8, saw: u8}, @@ -74,6 +74,8 @@ impl UbxParser { if b == 0x62 { self.state = Sync2; None + } else if b == 0xb5 { + None } else { self.state = Start; Some(Err(BadStart {expect: 0x62, saw: b})) @@ -116,6 +118,7 @@ impl UbxParser { None } else { let _ = self.buf.try_push(b); + self.state = Payload { class, id, len, checksum: checksum.next(b) }; None }, Checksum1 { class, id, expect, found } => @@ -141,6 +144,7 @@ impl UbxParser { } } +#[derive(Debug)] pub enum UbxPacket { AckAck {class: u8, id: u8}, AckNak {class: u8, id: u8}, @@ -151,7 +155,7 @@ pub enum UbxPacket { // SAFETY: All fields are naturally aligned, so there is no padding. // Also, this device has the same endianness as UBX (little) #[repr(C)] -#[derive(Pod, Zeroable, Copy, Clone)] +#[derive(Pod, Zeroable, Copy, Clone, Debug)] pub struct NavPvt { pub i_tow: u32, pub year: u16, diff --git a/ublox/.github/workflows/build.yml b/ublox/.github/workflows/build.yml deleted file mode 100644 index d07c768..0000000 --- a/ublox/.github/workflows/build.yml +++ /dev/null @@ -1,74 +0,0 @@ -name: Rust - -on: - push: - pull_request: - -env: - CARGO_TERM_COLOR: always - -jobs: - build: - runs-on: ubuntu-latest - strategy: - matrix: - feature-args: - - '' - - --no-default-features --features alloc - - --no-default-features --features serde - - --no-default-features - steps: - - uses: actions/checkout@v4 - - name: Install libudev - run: sudo apt-get update && sudo apt-get install -y libudev-dev - - name: Install MSRV - uses: actions-rs/toolchain@v1 - with: - toolchain: 1.65.0 - override: true - components: rustfmt, clippy - - name: Build - run: cargo build --verbose ${{ matrix.feature-args }} - - name: Run tests - run: cargo test --verbose ${{ matrix.feature-args }} - - name: Coding style - run: | - cargo fmt --all -- --check - cargo clippy --all-features --all-targets -- -D warnings - - build_embedded: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - - name: Install MSRV - uses: actions-rs/toolchain@v1 - with: - toolchain: 1.70.0 - override: true - - name: Install embedded targets - run: rustup target add thumbv6m-none-eabi thumbv7m-none-eabi thumbv7em-none-eabihf - - name: Build - run: cargo build --verbose --no-default-features --target thumbv6m-none-eabi --target thumbv7m-none-eabi --target thumbv7em-none-eabihf - - name: Build (alloc) - run: cargo build --verbose --no-default-features --target thumbv6m-none-eabi --target thumbv7m-none-eabi --target thumbv7em-none-eabihf --features alloc - - name: Build (serde) - run: cargo build --verbose --no-default-features --target thumbv6m-none-eabi --target thumbv7m-none-eabi --target thumbv7em-none-eabihf --features serde - - build_examples: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - - name: Install libudev - run: sudo apt-get update && sudo apt-get install -y libudev-dev - - name: Install MSRV - uses: actions-rs/toolchain@v1 - with: - toolchain: 1.70.0 - override: true - components: rustfmt, clippy - - name: Build - run: cd ${{ github.workspace }}/examples && cargo build - - name: Coding style - run: | - cd ${{ github.workspace }}/examples && cargo fmt --all -- --check - cd ${{ github.workspace }}/examples && cargo clippy --all-features --all-targets -- -D warnings \ No newline at end of file diff --git a/ublox/.gitignore b/ublox/.gitignore deleted file mode 100644 index cd64418..0000000 --- a/ublox/.gitignore +++ /dev/null @@ -1,8 +0,0 @@ -Cargo.lock -target/ - -**/*.rs.bk -*~ - -.idea/ -.vscode diff --git a/ublox/.travis.yml b/ublox/.travis.yml deleted file mode 100644 index 900664e..0000000 --- a/ublox/.travis.yml +++ /dev/null @@ -1,27 +0,0 @@ -before_install: - - sudo apt-get install -y libudev-dev -language: rust -rust: - # If the MSRV is updated, update the readme - - 1.65.0 - - stable - - beta - - nightly -matrix: - allow_failures: - - rust: nightly - fast_finish: true -before_script: - - rustup component add rustfmt-preview - - which rustfmt -env: - - FEATURES="--no-default-features --features std" - - FEATURES="--no-default-features --features alloc" - - FEATURES="--no-default-features" -script: - - cd ublox_derive - - cargo test --verbose --workspace - - cd ../ublox - - cargo test $FEATURES - - cargo build $FEATURES --examples - - cargo doc $FEATURES diff --git a/ublox/Cargo.toml b/ublox/Cargo.toml deleted file mode 100644 index a1e81f8..0000000 --- a/ublox/Cargo.toml +++ /dev/null @@ -1,3 +0,0 @@ -[workspace] -members = ["ublox", "ublox_derive"] -resolver = "2" diff --git a/ublox/LICENSE.md b/ublox/LICENSE.md deleted file mode 100644 index 27acf41..0000000 --- a/ublox/LICENSE.md +++ /dev/null @@ -1,7 +0,0 @@ -Copyright (c) 2020 Lane Kolbly - -Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. diff --git a/ublox/README.md b/ublox/README.md deleted file mode 100644 index cb863ac..0000000 --- a/ublox/README.md +++ /dev/null @@ -1,98 +0,0 @@ -uBlox for Rust -============== - -[![Rust](https://github.com/ublox-rs/ublox/actions/workflows/build.yml/badge.svg)](https://github.com/ublox-rs/ublox/actions/workflows/build.yml) -[![ublox on docs.rs][docs-badge]][docs-url] -[![MIT licensed][mit-badge]][mit-url] -[![rustc v1.65][mrvs-badge]][mrvs-url] - -[docs-badge]: https://docs.rs/ublox/badge.svg -[docs-url]: https://docs.rs/ublox -[mit-badge]: https://img.shields.io/badge/license-MIT-blue.svg -[mit-url]: https://github.com/lkolbly/ublox/blob/master/LICENSE.md -[mrvs-url]: https://www.whatrustisit.com -[mrvs-badge]: https://img.shields.io/badge/minimum%20rustc-1.65-blue?logo=rust - -This project aims to build a pure-rust I/O library for uBlox GPS devices, specifically using the UBX protocol. - -Examples of usage of the library can be found in the [examples/](./examples) directory. A basic CLI for interacting with an uBlox device can be found in [examples/ublox-cli](./examples/ublox-cli/) directory. See the specific [examples/README](./examples/README.md). - -Constructing Packets -==================== - -Constructing packets happens using the `Builder` variant of the packet, for example: -``` -use ublox::{CfgPrtUartBuilder, UartPortId, UartMode, DataBits, Parity, StopBits, InProtoMask, OutProtoMask}; -let packet: [u8; 28] = CfgPrtUartBuilder { - portid: UartPortId::Uart1, - reserved0: 0, - tx_ready: 0, - mode: UartMode::new(DataBits::Eight, Parity::None, StopBits::One), - baud_rate: 9600, - in_proto_mask: InProtoMask::all(), - out_proto_mask: OutProtoMask::UBLOX, - flags: 0, - reserved5: 0, -}.into_packet_bytes(); -``` - -For variable-size packet like `CfgValSet`, you can construct it into a new `Vec`: -``` -use ublox::{cfg_val::CfgVal::*, CfgLayer, CfgValSetBuilder}; -let packet_vec: Vec = CfgValSetBuilder { - version: 1, - layers: CfgLayer::RAM, - reserved1: 0, - cfg_data: &[UsbOutProtNmea(true), UsbOutProtRtcm3x(true), UsbOutProtUbx(true)], -} -.into_packet_vec(); -let packet: &[u8] = packet_vec.as_slice(); -``` -Or by extending to an existing one: -``` -let mut packet_vec = Vec::new(); -CfgValSetBuilder { - version: 1, - layers: CfgLayer::RAM, - reserved1: 0, - cfg_data: &[UsbOutProtNmea(true), UsbOutProtRtcm3x(true), UsbOutProtUbx(true)], -} -.extend_to(&mut packet_vec); -let packet = packet_vec.as_slice(); -``` -See the documentation for the individual `Builder` structs for information on the fields. - -Parsing Packets -=============== - -Parsing packets happens by instantiating a `Parser` object and then adding data into it using its `consume()` method. The parser contains an internal buffer of data, and when `consume()` is called that data is copied into the internal buffer and an iterator-like object is returned to access the packets. For example: -``` -use ublox::Parser; -let mut parser = Parser::default(); -let my_raw_data = vec![1, 2, 3, 4]; // From your serial port -let mut it = parser.consume(&my_raw_data); -loop { - match it.next() { - Some(Ok(packet)) => { - // We've received a &PacketRef, we can handle it - } - Some(Err(_)) => { - // Received a malformed packet - } - None => { - // The internal buffer is now empty - break; - } - } -} -``` - -no_std Support -============== - -This library supports no_std environments with a deterministic-size `Parser`. See the documentation for more information. - -Minimum Supported Rust Version -============================== - -The library crate will support at least the previous year's worth of Rust compilers. Currently, the MSRV is `1.65.0`. Note that, as we are pre-1.0, breaking the MSRV will not force a minor update - the MSRV can change in a patch update. diff --git a/ublox/examples/Cargo.toml b/ublox/examples/Cargo.toml deleted file mode 100644 index 9843613..0000000 --- a/ublox/examples/Cargo.toml +++ /dev/null @@ -1,4 +0,0 @@ -[workspace] -exclude = ["target"] -members = ["*"] -resolver = "2" diff --git a/ublox/examples/README.md b/ublox/examples/README.md deleted file mode 100644 index d4299e8..0000000 --- a/ublox/examples/README.md +++ /dev/null @@ -1,11 +0,0 @@ -# Examples - -This folder contains usage examples. - -These examples use a more recent `rustc` compiler version than the library. - -To build the examples, you can override the toolchain used specifically to this folder by doing, e.g., -``` -rustup install 1.70 -rustup override set 1.70 -``` diff --git a/ublox/examples/ublox-cli/Cargo.toml b/ublox/examples/ublox-cli/Cargo.toml deleted file mode 100644 index 6690baf..0000000 --- a/ublox/examples/ublox-cli/Cargo.toml +++ /dev/null @@ -1,17 +0,0 @@ -[package] -authors = ["Lane Kolbly ", "Andrei Gherghescu "] -edition = "2021" -name = "ublox-cli" -publish = false -rust-version = "1.70" -version = "0.1.0" - -[dependencies] -chrono = "0.4.29" -clap = {version = "4.2.7", features = ["cargo"]} -serde_json = "1.0.105" -serialport = "4.2.2" -ublox = {path = "../../ublox"} - -[features] -alloc = ["ublox/alloc"] diff --git a/ublox/examples/ublox-cli/src/main.rs b/ublox/examples/ublox-cli/src/main.rs deleted file mode 100644 index 4e33549..0000000 --- a/ublox/examples/ublox-cli/src/main.rs +++ /dev/null @@ -1,479 +0,0 @@ -use chrono::prelude::*; -use clap::{value_parser, Arg, Command}; -use serialport::{ - DataBits as SerialDataBits, FlowControl as SerialFlowControl, Parity as SerialParity, - StopBits as SerialStopBits, -}; -use std::convert::TryInto; -use std::time::Duration; -use ublox::*; - -fn main() { - let matches = Command::new("uBlox CLI example program") - .author(clap::crate_authors!()) - .about("Demonstrates usage of the Rust uBlox API") - .arg_required_else_help(true) - .arg( - Arg::new("port") - .value_name("port") - .short('p') - .long("port") - .required(true) - .help("Serial port to open"), - ) - .arg( - Arg::new("baud") - .value_name("baud") - .short('s') - .long("baud") - .required(false) - .default_value("9600") - .value_parser(value_parser!(u32)) - .help("Baud rate of the port to open"), - ) - .arg( - Arg::new("stop-bits") - .long("stop-bits") - .help("Number of stop bits to use for open port") - .required(false) - .value_parser(["1", "2"]) - .default_value("1"), - ) - .arg( - Arg::new("data-bits") - .long("data-bits") - .help("Number of data bits to use for open port") - .required(false) - .value_parser(["7", "8"]) - .default_value("8"), - ) - .arg( - Arg::new("parity") - .long("parity") - .help("Parity to use for open port") - .required(false) - .value_parser(["even", "odd"]), - ) - .subcommand( - Command::new("configure") - .about("Configure settings for specific UART/USB port") - .arg( - Arg::new("port") - .long("select") - .required(true) - .default_value("usb") - .value_parser(value_parser!(String)) - .long_help( - "Apply specific configuration to the selected port. Supported: usb, uart1, uart2. -Configuration includes: protocol in/out, data-bits, stop-bits, parity, baud-rate", - ), - ) - .arg( - Arg::new("cfg-baud") - .value_name("baud") - .long("baud") - .required(false) - .default_value("9600") - .value_parser(value_parser!(u32)) - .help("Baud rate to set"), - ) - .arg( - Arg::new("stop-bits") - .long("stop-bits") - .help("Number of stop bits to set") - .required(false) - .value_parser(["1", "2"]) - .default_value("1"), - ) - .arg( - Arg::new("data-bits") - .long("data-bits") - .help("Number of data bits to set") - .required(false) - .value_parser(["7", "8"]) - .default_value("8"), - ) - .arg( - Arg::new("parity") - .long("parity") - .help("Parity to set") - .required(false) - .value_parser(["even", "odd"]), - ) - .arg( - Arg::new("in-ublox") - .long("in-ublox") - .default_value("true") - .action(clap::ArgAction::SetTrue) - .help("Toggle receiving UBX proprietary protocol on port"), - ) - .arg( - Arg::new("in-nmea") - .long("in-nmea") - .default_value("false") - .action(clap::ArgAction::SetTrue) - .help("Toggle receiving NMEA protocol on port"), - ) - .arg( - Arg::new("in-rtcm") - .long("in-rtcm") - .default_value("false") - .action(clap::ArgAction::SetTrue) - .help("Toggle receiving RTCM protocol on port"), - ) - .arg( - Arg::new("in-rtcm3") - .long("in-rtcm3") - .default_value("false") - .action(clap::ArgAction::SetTrue) - .help( - "Toggle receiving RTCM3 protocol on port. - Not supported on uBlox protocol versions below 20", - ), - ) - .arg( - Arg::new("out-ublox") - .long("out-ublox") - .action(clap::ArgAction::SetTrue) - .help("Toggle sending UBX proprietary protocol on port"), - ) - .arg( - Arg::new("out-nmea") - .long("out-nmea") - .action(clap::ArgAction::SetTrue) - .help("Toggle sending NMEA protocol on port"), - ) - .arg( - Arg::new("out-rtcm3") - .long("out-rtcm3") - .action(clap::ArgAction::SetTrue) - .help( - "Toggle seding RTCM3 protocol on port. - Not supported on uBlox protocol versions below 20", - ), - ), - ) - .get_matches(); - - let port = matches - .get_one::("port") - .expect("Expected required 'port' cli argumnet"); - - let baud = matches.get_one::("baud").cloned().unwrap_or(9600); - let stop_bits = match matches.get_one::("stop-bits").map(|s| s.as_str()) { - Some("2") => SerialStopBits::Two, - _ => SerialStopBits::One, - }; - - let data_bits = match matches.get_one::("data-bits").map(|s| s.as_str()) { - Some("7") => SerialDataBits::Seven, - Some("8") => SerialDataBits::Eight, - _ => { - println!("Number of DataBits supported by uBlox is either 7 or 8"); - std::process::exit(1); - }, - }; - - let parity = match matches.get_one::("parity").map(|s| s.as_str()) { - Some("odd") => SerialParity::Even, - Some("even") => SerialParity::Odd, - _ => SerialParity::None, - }; - - let builder = serialport::new(port, baud) - .stop_bits(stop_bits) - .data_bits(data_bits) - .timeout(Duration::from_millis(10)) - .parity(parity) - .flow_control(SerialFlowControl::None); - - println!("{:?}", &builder); - let port = builder.open().unwrap_or_else(|e| { - eprintln!("Failed to open \"{}\". Error: {}", port, e); - ::std::process::exit(1); - }); - - let mut device = Device::new(port); - - // Parse cli for configuring specific uBlox UART port - if let Some(("configure", sub_matches)) = matches.subcommand() { - let (port_id, port_name) = match sub_matches.get_one::("port").map(|s| s.as_str()) { - Some(x) if x == "usb" => (Some(UartPortId::Usb), x), - Some(x) if x == "uart1" => (Some(UartPortId::Uart1), x), - Some(x) if x == "uart2" => (Some(UartPortId::Uart2), x), - _ => (None, ""), - }; - - let baud = sub_matches.get_one::("baud").cloned().unwrap_or(9600); - - let stop_bits = match sub_matches - .get_one::("stop-bits") - .map(|s| s.as_str()) - { - Some("2") => SerialStopBits::Two, - _ => SerialStopBits::One, - }; - - let data_bits = match sub_matches - .get_one::("data-bits") - .map(|s| s.as_str()) - { - Some("7") => SerialDataBits::Seven, - Some("8") => SerialDataBits::Eight, - _ => { - println!("Number of DataBits supported by uBlox is either 7 or 8"); - std::process::exit(1); - }, - }; - - let parity = match sub_matches.get_one::("parity").map(|s| s.as_str()) { - Some("odd") => SerialParity::Even, - Some("even") => SerialParity::Odd, - _ => SerialParity::None, - }; - let inproto = match ( - sub_matches.get_flag("in-ublox"), - sub_matches.get_flag("in-nmea"), - sub_matches.get_flag("in-rtcm"), - sub_matches.get_flag("in-rtcm3"), - ) { - (true, false, false, false) => InProtoMask::UBLOX, - (false, true, false, false) => InProtoMask::NMEA, - (false, false, true, false) => InProtoMask::RTCM, - (false, false, false, true) => InProtoMask::RTCM3, - (true, true, false, false) => InProtoMask::union(InProtoMask::UBLOX, InProtoMask::NMEA), - (true, false, true, false) => InProtoMask::union(InProtoMask::UBLOX, InProtoMask::RTCM), - (true, false, false, true) => { - InProtoMask::union(InProtoMask::UBLOX, InProtoMask::RTCM3) - }, - (false, true, true, false) => InProtoMask::union(InProtoMask::NMEA, InProtoMask::RTCM), - (false, true, false, true) => InProtoMask::union(InProtoMask::NMEA, InProtoMask::RTCM3), - (true, true, true, false) => InProtoMask::union( - InProtoMask::union(InProtoMask::UBLOX, InProtoMask::NMEA), - InProtoMask::RTCM, - ), - (true, true, false, true) => InProtoMask::union( - InProtoMask::union(InProtoMask::UBLOX, InProtoMask::NMEA), - InProtoMask::RTCM3, - ), - (_, _, true, true) => { - eprintln!("Cannot use RTCM and RTCM3 simultaneously. Choose one or the other"); - std::process::exit(1) - }, - (false, false, false, false) => InProtoMask::UBLOX, - }; - - let outproto = match ( - sub_matches.get_flag("out-ublox"), - sub_matches.get_flag("out-nmea"), - sub_matches.get_flag("out-rtcm3"), - ) { - (true, false, false) => OutProtoMask::UBLOX, - (false, true, false) => OutProtoMask::NMEA, - (false, false, true) => OutProtoMask::RTCM3, - (true, true, false) => OutProtoMask::union(OutProtoMask::UBLOX, OutProtoMask::NMEA), - (true, false, true) => OutProtoMask::union(OutProtoMask::UBLOX, OutProtoMask::RTCM3), - (false, true, true) => OutProtoMask::union(OutProtoMask::NMEA, OutProtoMask::RTCM3), - (true, true, true) => OutProtoMask::union( - OutProtoMask::union(OutProtoMask::UBLOX, OutProtoMask::NMEA), - OutProtoMask::RTCM3, - ), - (false, false, false) => OutProtoMask::UBLOX, - }; - - if let Some(port_id) = port_id { - println!("Configuring '{}' port ...", port_name.to_uppercase()); - device - .write_all( - &CfgPrtUartBuilder { - portid: port_id, - reserved0: 0, - tx_ready: 0, - mode: UartMode::new( - ublox_databits(data_bits), - ublox_parity(parity), - ublox_stopbits(stop_bits), - ), - baud_rate: baud, - in_proto_mask: inproto, - out_proto_mask: outproto, - flags: 0, - reserved5: 0, - } - .into_packet_bytes(), - ) - .expect("Could not configure UBX-CFG-PRT-UART"); - device - .wait_for_ack::() - .expect("Could not acknowledge UBX-CFG-PRT-UART msg"); - } - } - - // Enable the NavPvt packet - // By setting 1 in the array below, we enable the NavPvt message for Uart1, Uart2 and USB - // The other positions are for I2C, SPI, etc. Consult your device manual. - println!("Enable UBX-NAV-PVT message on all serial ports: USB, UART1 and UART2 ..."); - device - .write_all( - &CfgMsgAllPortsBuilder::set_rate_for::([0, 1, 1, 1, 0, 0]).into_packet_bytes(), - ) - .expect("Could not configure ports for UBX-NAV-PVT"); - device - .wait_for_ack::() - .expect("Could not acknowledge UBX-CFG-PRT-UART msg"); - - // Send a packet request for the MonVer packet - device - .write_all(&UbxPacketRequest::request_for::().into_packet_bytes()) - .expect("Unable to write request/poll for UBX-MON-VER message"); - - // Start reading data - println!("Opened uBlox device, waiting for messages..."); - loop { - device - .update(|packet| match packet { - PacketRef::MonVer(packet) => { - println!( - "SW version: {} HW version: {}; Extensions: {:?}", - packet.software_version(), - packet.hardware_version(), - packet.extension().collect::>() - ); - println!("{:?}", packet); - }, - PacketRef::NavPvt(sol) => { - let has_time = sol.fix_type() == GpsFix::Fix3D - || sol.fix_type() == GpsFix::GPSPlusDeadReckoning - || sol.fix_type() == GpsFix::TimeOnlyFix; - let has_posvel = sol.fix_type() == GpsFix::Fix3D - || sol.fix_type() == GpsFix::GPSPlusDeadReckoning; - - if has_posvel { - let pos: Position = (&sol).into(); - let vel: Velocity = (&sol).into(); - println!( - "Latitude: {:.5} Longitude: {:.5} Altitude: {:.2}m", - pos.lat, pos.lon, pos.alt - ); - println!( - "Speed: {:.2} m/s Heading: {:.2} degrees", - vel.speed, vel.heading - ); - println!("Sol: {:?}", sol); - } - - if has_time { - let time: DateTime = (&sol) - .try_into() - .expect("Could not parse NAV-PVT time field to UTC"); - println!("Time: {:?}", time); - } - }, - _ => { - println!("{:?}", packet); - }, - }) - .unwrap(); - } -} - -fn ublox_stopbits(s: SerialStopBits) -> StopBits { - // Seriaport crate doesn't support the other StopBits option of uBlox - match s { - SerialStopBits::One => StopBits::One, - SerialStopBits::Two => StopBits::Two, - } -} - -fn ublox_databits(d: SerialDataBits) -> DataBits { - match d { - SerialDataBits::Seven => DataBits::Seven, - SerialDataBits::Eight => DataBits::Eight, - _ => { - println!("uBlox only supports Seven or Eight data bits"); - DataBits::Eight - }, - } -} - -fn ublox_parity(v: SerialParity) -> Parity { - match v { - SerialParity::Even => Parity::Even, - SerialParity::Odd => Parity::Odd, - SerialParity::None => Parity::None, - } -} - -struct Device { - port: Box, - parser: Parser>, -} - -impl Device { - pub fn new(port: Box) -> Device { - let parser = Parser::default(); - Device { port, parser } - } - - pub fn write_all(&mut self, data: &[u8]) -> std::io::Result<()> { - self.port.write_all(data) - } - - pub fn update(&mut self, mut cb: T) -> std::io::Result<()> { - loop { - const MAX_PAYLOAD_LEN: usize = 1240; - let mut local_buf = [0; MAX_PAYLOAD_LEN]; - let nbytes = self.read_port(&mut local_buf)?; - if nbytes == 0 { - break; - } - - // parser.consume adds the buffer to its internal buffer, and - // returns an iterator-like object we can use to process the packets - let mut it = self.parser.consume(&local_buf[..nbytes]); - loop { - match it.next() { - Some(Ok(packet)) => { - cb(packet); - }, - Some(Err(_)) => { - // Received a malformed packet, ignore it - }, - None => { - // We've eaten all the packets we have - break; - }, - } - } - } - Ok(()) - } - - pub fn wait_for_ack(&mut self) -> std::io::Result<()> { - let mut found_packet = false; - while !found_packet { - self.update(|packet| { - if let PacketRef::AckAck(ack) = packet { - if ack.class() == T::CLASS && ack.msg_id() == T::ID { - found_packet = true; - } - } - })?; - } - Ok(()) - } - - /// Reads the serial port, converting timeouts into "no data received" - fn read_port(&mut self, output: &mut [u8]) -> std::io::Result { - match self.port.read(output) { - Ok(b) => Ok(b), - Err(e) => { - if e.kind() == std::io::ErrorKind::TimedOut { - Ok(0) - } else { - Err(e) - } - }, - } - } -} diff --git a/ublox/rustfmt.toml b/ublox/rustfmt.toml deleted file mode 100644 index 87f8bf4..0000000 --- a/ublox/rustfmt.toml +++ /dev/null @@ -1,2 +0,0 @@ -match_block_trailing_comma = true -newline_style = "Unix" diff --git a/ublox/ublox/Cargo.toml b/ublox/ublox/Cargo.toml deleted file mode 100644 index aff6da7..0000000 --- a/ublox/ublox/Cargo.toml +++ /dev/null @@ -1,34 +0,0 @@ -[package] -authors = ["Lane Kolbly "] -description = "A crate to communicate with u-blox GPS devices using the UBX protocol" -edition = "2021" -license = "MIT" -name = "ublox" -readme = "../README.md" -repository = "https://github.com/lkolbly/ublox" -version = "0.4.5" - -[features] -alloc = [] -default = ["std", "serde"] -std = [] - -[dependencies] -bitflags = "2.3.1" -chrono = {version = "0.4.19", default-features = false, features = []} -num-traits = {version = "0.2.12", default-features = false} -serde = {version = "1.0.144", optional = true, default-features = false, features = ["derive"]} -ublox_derive = {path = "../ublox_derive", version = "=0.1.0"} -tinyvec = "1.6.0" - -[dev-dependencies] -cpu-time = "1.0.0" -cpuprofiler = "0.0.4" -# Latest depends on clap v4 which requires rustc 1.70 -criterion = "0.4.0" -rand = "0.8.5" -serde_json = "1.0.85" - -[[bench]] -harness = false -name = "packet_benchmark" diff --git a/ublox/ublox/benches/packet_benchmark.rs b/ublox/ublox/benches/packet_benchmark.rs deleted file mode 100644 index 8957949..0000000 --- a/ublox/ublox/benches/packet_benchmark.rs +++ /dev/null @@ -1,80 +0,0 @@ -use criterion::{criterion_group, criterion_main, Criterion}; -use std::path::Path; -use ublox::*; - -struct CpuProfiler; - -impl criterion::profiler::Profiler for CpuProfiler { - fn start_profiling(&mut self, benchmark_id: &str, _benchmark_dir: &Path) { - cpuprofiler::PROFILER - .lock() - .unwrap() - .start(format!("./{}.profile", benchmark_id).as_bytes()) - .unwrap(); - } - - fn stop_profiling(&mut self, _benchmark_id: &str, _benchmark_dir: &Path) { - cpuprofiler::PROFILER.lock().unwrap().stop().unwrap(); - } -} - -fn profiled() -> Criterion { - Criterion::default().with_profiler(CpuProfiler) -} - -#[allow(dead_code)] -fn parse_all(mut parser: Parser, data: &[u8], chunk_size: usize) -> usize { - let mut count = 0; - for chunk in data.chunks(chunk_size) { - let mut it = parser.consume(chunk); - loop { - match it.next() { - Some(Ok(_packet)) => { - count += 1; - }, - Some(Err(e)) => { - panic!("No errors allowed! got: {:?}", e); - }, - None => { - // We've eaten all the packets we have - break; - }, - } - } - } - count -} - -pub fn criterion_benchmark(c: &mut Criterion) { - for chunk in &[99, 100, 101, 256, 512, 1000, 1024] { - c.bench_function(&format!("vec_parse_pos_{}", chunk), |b| { - b.iter(|| { - // TODO: requires pos.ubx file - // let data = std::include_bytes!("pos.ubx"); - // let parser = Parser::default(); - // assert_eq!(parse_all(parser, data, *chunk), 2801); - todo!() - }) - }); - } - for (buf_size, chunk) in &[(256, 100), (256, 256), (256, 512), (256, 1024)] { - // let mut underlying = vec![0; *buf_size]; - c.bench_function(&format!("array_parse_pos_{}_{}", buf_size, chunk), |b| { - b.iter(|| { - // TODO: requires pos.ubx file - // let data = std::include_bytes!("pos.ubx"); - // let underlying = FixedLinearBuffer::new(&mut underlying); - // let parser = Parser::new(underlying); - // assert_eq!(parse_all(parser, data, *chunk), 2801); - todo!() - }) - }); - } -} - -criterion_group! { -name = benches; -config = profiled(); -targets = criterion_benchmark -} -criterion_main!(benches); diff --git a/ublox/ublox/src/error.rs b/ublox/ublox/src/error.rs deleted file mode 100644 index 1d1a125..0000000 --- a/ublox/ublox/src/error.rs +++ /dev/null @@ -1,93 +0,0 @@ -use core::fmt; - -#[derive(Debug)] -pub enum MemWriterError { - NotEnoughMem, - Custom(E), -} - -impl fmt::Display for MemWriterError { - fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { - match self { - MemWriterError::NotEnoughMem => f.write_str("Not enough memory error"), - MemWriterError::Custom(e) => write!(f, "MemWriterError: {}", e), - } - } -} - -#[cfg(feature = "std")] -impl std::error::Error for MemWriterError where E: std::error::Error {} - -/// Error that possible during packets parsing -#[derive(Debug, PartialEq, Eq)] -pub enum ParserError { - InvalidChecksum { - expect: u16, - got: u16, - }, - InvalidField { - packet: &'static str, - field: &'static str, - }, - InvalidPacketLen { - packet: &'static str, - expect: usize, - got: usize, - }, - /// Returned when the parser buffer is not big enough to store the packet - OutOfMemory { - required_size: usize, - }, -} - -impl fmt::Display for ParserError { - fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { - match self { - ParserError::InvalidChecksum { expect, got } => write!( - f, - "Not valid packet's checksum, expect {:x}, got {:x}", - expect, got - ), - ParserError::InvalidField { packet, field } => { - write!(f, "Invalid field {} of packet {}", field, packet) - }, - ParserError::InvalidPacketLen { - packet, - expect, - got, - } => write!( - f, - "Invalid packet({}) length, expect {}, got {}", - packet, expect, got - ), - ParserError::OutOfMemory { required_size } => write!( - f, - "Insufficient parser buffer size, required {} bytes", - required_size - ), - } - } -} - -#[cfg(feature = "std")] -impl std::error::Error for ParserError {} - -#[derive(Debug, Clone, Copy)] -pub enum DateTimeError { - InvalidDate, - InvalidTime, - InvalidNanoseconds, -} - -impl fmt::Display for DateTimeError { - fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { - match self { - DateTimeError::InvalidDate => f.write_str("invalid date"), - DateTimeError::InvalidTime => f.write_str("invalid time"), - DateTimeError::InvalidNanoseconds => f.write_str("invalid nanoseconds"), - } - } -} - -#[cfg(feature = "std")] -impl std::error::Error for DateTimeError {} diff --git a/ublox/ublox/src/lib.rs b/ublox/ublox/src/lib.rs deleted file mode 100644 index c99d0af..0000000 --- a/ublox/ublox/src/lib.rs +++ /dev/null @@ -1,84 +0,0 @@ -//! # ublox -//! -//! This project aims to build a pure-rust I/O library for ublox GPS devices, specifically using the UBX protocol. -//! -//! An example of using this library to talk to a device can be seen in the examples/ublox-cli subfolder of this project. -//! More examples are available in the examples subfolder. -//! -//! Constructing Packets -//! ==================== -//! -//! Constructing packets happens using the `Builder` variant of the packet, for example: -//! ``` -//! use ublox::{CfgPrtUartBuilder, UartPortId, UartMode, DataBits, Parity, StopBits, InProtoMask, OutProtoMask}; -//! -//! let packet: [u8; 28] = CfgPrtUartBuilder { -//! portid: UartPortId::Uart1, -//! reserved0: 0, -//! tx_ready: 0, -//! mode: UartMode::new(DataBits::Eight, Parity::None, StopBits::One), -//! baud_rate: 9600, -//! in_proto_mask: InProtoMask::all(), -//! out_proto_mask: OutProtoMask::UBLOX, -//! flags: 0, -//! reserved5: 0, -//! }.into_packet_bytes(); -//! ``` -//! See the documentation for the individual `Builder` structs for information on the fields. -//! -//! Parsing Packets -//! =============== -//! -//! Parsing packets happens by instantiating a `Parser` object and then adding data into it using its `consume()` method. The parser contains an internal buffer of data, and when `consume()` is called that data is copied into the internal buffer and an iterator-like object is returned to access the packets. For example: -//! ``` -//! # #[cfg(any(feature = "alloc", feature = "std"))] { -//! use ublox::Parser; -//! -//! let mut parser = Parser::default(); -//! let my_raw_data = vec![1, 2, 3, 4]; // From your serial port -//! let mut it = parser.consume(&my_raw_data); -//! loop { -//! match it.next() { -//! Some(Ok(packet)) => { -//! // We've received a &PacketRef, we can handle it -//! } -//! Some(Err(_)) => { -//! // Received a malformed packet -//! } -//! None => { -//! // The internal buffer is now empty -//! break; -//! } -//! } -//! } -//! # } -//! ``` -//! -//! no_std Support -//! ============== -//! -//! This library additionally supports no_std environments with a deterministic-size parser. To use this parser, simply create a FixedLinearBuffer and use it to construct a `Parser` object: -//! ``` -//! let mut buf = vec![0; 256]; -//! let buf = ublox::FixedLinearBuffer::new(&mut buf[..]); -//! let mut parser = ublox::Parser::new(buf); -//! ``` -//! The resulting parser can be used like normal. The absolute smallest recommended buffer size is 36 bytes, large enough to contain a NavPosLlh packet. - -#![cfg_attr(not(feature = "std"), no_std)] - -#[cfg(feature = "alloc")] -extern crate alloc; -extern crate core; -#[cfg(feature = "serde")] -extern crate serde; - -pub use crate::{ - error::{DateTimeError, MemWriterError, ParserError}, - parser::{FixedLinearBuffer, Parser, ParserIter, UnderlyingBuffer}, - ubx_packets::*, -}; - -mod error; -mod parser; -mod ubx_packets; diff --git a/ublox/ublox/src/parser.rs b/ublox/ublox/src/parser.rs deleted file mode 100644 index 338fc26..0000000 --- a/ublox/ublox/src/parser.rs +++ /dev/null @@ -1,972 +0,0 @@ -#[cfg(feature = "alloc")] -use alloc::vec::Vec; -use core::ops::{Index, Range}; -use tinyvec::ArrayVec; - -use crate::{ - error::ParserError, - ubx_packets::{match_packet, PacketRef, MAX_PAYLOAD_LEN, SYNC_CHAR_1, SYNC_CHAR_2}, -}; - -/// This trait represents an underlying buffer used for the Parser. We provide -/// implementations for `Vec` and for `FixedLinearBuffer`, if you want to -/// use your own struct as an underlying buffer you can implement this trait. -/// -/// Look at the `flb_*` unit tests for ideas of unit tests you can run against -/// your own implementations. -pub trait UnderlyingBuffer: - core::ops::Index, Output = [u8]> + core::ops::Index -{ - /// Removes all elements from the buffer. - fn clear(&mut self); - - /// Returns the number of elements currently stored in the buffer. - fn len(&self) -> usize; - - /// Returns the maximum capacity of this buffer. This value should be a minimum max - /// capacity - that is, `extend_from_slice` should succeed if max_capacity bytes are - /// passed to it. - /// - /// Note that, for example, the Vec implementation of this trait returns `usize::MAX`, - /// which cannot be actually allocated by a Vec. This is okay, because Vec will panic - /// if an allocation is requested that it can't handle. - fn max_capacity(&self) -> usize; - - /// Returns the number of bytes not copied over due to buffer size constraints. - /// - /// As noted for `max_capacity`, if this function is passed `max_capacity() - len()` - /// bytes it should either panic or return zero bytes, any other behaviour may cause - /// unexpected behaviour in the parser. - fn extend_from_slice(&mut self, other: &[u8]) -> usize; - - /// Removes the first `count` elements from the buffer. Cannot fail. - fn drain(&mut self, count: usize); - - /// Locates the given u8 value within the buffer, returning the index (if it is found). - fn find(&self, value: u8) -> Option { - (0..self.len()).find(|&i| self[i] == value) - } - - /// Returns whether the buffer is empty. - fn is_empty(&self) -> bool { - self.len() == 0 - } -} - -#[cfg(any(feature = "std", feature = "alloc"))] -impl UnderlyingBuffer for Vec { - fn clear(&mut self) { - self.clear(); - } - - fn len(&self) -> usize { - self.len() - } - - fn max_capacity(&self) -> usize { - core::usize::MAX - } - - fn extend_from_slice(&mut self, other: &[u8]) -> usize { - self.extend_from_slice(other); - 0 - } - - fn drain(&mut self, count: usize) { - self.drain(0..count); - } - - fn find(&self, value: u8) -> Option { - self.iter().position(|elem| *elem == value) - } -} - -impl UnderlyingBuffer for tinyvec::ArrayVec<[u8; 512]> { - fn clear(&mut self) { - self.clear(); - } - - fn len(&self) -> usize { - self.len() - } - - fn max_capacity(&self) -> usize { - self.capacity() - } - - fn extend_from_slice(&mut self, other: &[u8]) -> usize { - self.fill(other.iter().copied()).count() - } - - fn drain(&mut self, count: usize) { - self.drain(0..count); - } - - fn find(&self, value: u8) -> Option { - self.iter().position(|elem| *elem == value) - } -} - -pub struct FixedLinearBuffer<'a> { - buffer: &'a mut [u8], - len: usize, -} - -impl<'a> FixedLinearBuffer<'a> { - pub fn new(buf: &'a mut [u8]) -> Self { - Self { - buffer: buf, - len: 0, - } - } -} - -impl<'a> core::ops::Index> for FixedLinearBuffer<'a> { - type Output = [u8]; - - fn index(&self, index: core::ops::Range) -> &Self::Output { - if index.end > self.len { - panic!("Index {} is outside of our length {}", index.end, self.len); - } - self.buffer.index(index) - } -} - -impl<'a> core::ops::Index for FixedLinearBuffer<'a> { - type Output = u8; - - fn index(&self, index: usize) -> &Self::Output { - &self.buffer[index] - } -} - -impl<'a> UnderlyingBuffer for FixedLinearBuffer<'a> { - fn clear(&mut self) { - self.len = 0; - } - - fn len(&self) -> usize { - self.len - } - - fn max_capacity(&self) -> usize { - self.buffer.len() - } - - fn extend_from_slice(&mut self, other: &[u8]) -> usize { - let to_copy = core::cmp::min(other.len(), self.buffer.len() - self.len); - let uncopyable = other.len() - to_copy; - self.buffer[self.len..self.len + to_copy].copy_from_slice(&other[..to_copy]); - self.len += to_copy; - uncopyable - } - - fn drain(&mut self, count: usize) { - if count >= self.len { - self.len = 0; - return; - } - - let new_size = self.len - count; - { - let bufptr = self.buffer.as_mut_ptr(); - unsafe { - core::ptr::copy(bufptr.add(count), bufptr, new_size); - } - } - self.len = new_size; - } - - fn find(&self, value: u8) -> Option { - (0..self.len()).find(|&i| self[i] == value) - } -} - -/// Streaming parser for UBX protocol with buffer. The default constructor will build -/// a parser containing a Vec, but you can pass your own underlying buffer by passing it -/// to Parser::new(). -/// -/// If you pass your own buffer, it should be able to store at _least_ 4 bytes. In practice, -/// you won't be able to do anything useful unless it's at least 36 bytes long (the size -/// of a NavPosLlh packet). -pub struct Parser -where - T: UnderlyingBuffer, -{ - buf: T, -} - -#[cfg(any(feature = "std", feature = "alloc"))] -impl core::default::Default for Parser> { - fn default() -> Self { - Self { buf: Vec::new() } - } -} - -impl Parser { - pub fn new(underlying: T) -> Self { - Self { buf: underlying } - } - - pub fn is_buffer_empty(&self) -> bool { - self.buf.is_empty() - } - - pub fn buffer_len(&self) -> usize { - self.buf.len() - } - - pub fn consume<'a>(&'a mut self, new_data: &'a [u8]) -> ParserIter<'a, T> { - let mut buf = DualBuffer::new(&mut self.buf, new_data); - - for i in 0..buf.len() { - if buf[i] == SYNC_CHAR_1 { - buf.drain(i); - break; - } - } - - ParserIter { buf } - } -} - -/// Stores two buffers: A "base" and a "new" buffer. Exposes these as the same buffer, -/// copying data from the "new" buffer to the base buffer as required to maintain that -/// illusion. -struct DualBuffer<'a, T: UnderlyingBuffer> { - buf: &'a mut T, - off: usize, - - new_buf: &'a [u8], - new_buf_offset: usize, -} - -impl<'a, T: UnderlyingBuffer> core::ops::Index for DualBuffer<'a, T> { - type Output = u8; - - fn index(&self, index: usize) -> &u8 { - if self.off + index < self.buf.len() { - &self.buf[index + self.off] - } else { - &self.new_buf[self.new_buf_offset + index - (self.buf.len() - self.off)] - } - } -} - -impl<'a, T: UnderlyingBuffer> DualBuffer<'a, T> { - fn new(buf: &'a mut T, new_buf: &'a [u8]) -> Self { - Self { - buf, - off: 0, - new_buf, - new_buf_offset: 0, - } - } - - /// Clears all elements - equivalent to buf.drain(buf.len()) - fn clear(&mut self) { - self.drain(self.len()); - } - - /// Remove count elements without providing a view into them. - fn drain(&mut self, count: usize) { - let underlying_bytes = core::cmp::min(self.buf.len() - self.off, count); - let new_bytes = count.saturating_sub(underlying_bytes); - - self.off += underlying_bytes; - self.new_buf_offset += new_bytes; - } - - /// Return the total number of accessible bytes in this view. Note that you may - /// not be able to take() this many bytes at once, if the total number of bytes - /// is more than the underlying store can fit. - fn len(&self) -> usize { - self.buf.len() - self.off + self.new_buf.len() - self.new_buf_offset - } - - // Returns the number of bytes which would be lost (because they can't be copied into - // the underlying storage) if this DualBuffer were dropped. - fn potential_lost_bytes(&self) -> usize { - if self.len() <= self.buf.max_capacity() { - 0 - } else { - self.len() - self.buf.max_capacity() - } - } - - fn can_drain_and_take(&self, drain: usize, take: usize) -> bool { - let underlying_bytes = core::cmp::min(self.buf.len() - self.off, drain); - let new_bytes = drain.saturating_sub(underlying_bytes); - - let drained_off = self.off + underlying_bytes; - let drained_new_off = self.new_buf_offset + new_bytes; - - if take > self.buf.len() - drained_off + self.new_buf.len() - drained_new_off { - // Draining removed too many bytes, we don't have enough to take - return false; - } - - let underlying_bytes = core::cmp::min(self.buf.len() - drained_off, take); - let new_bytes = take.saturating_sub(underlying_bytes); - - if underlying_bytes == 0 { - // We would take entirely from the new buffer - return true; - } - - if new_bytes == 0 { - // We would take entirely from the underlying - return true; - } - - if new_bytes > self.buf.max_capacity() - (self.buf.len() - drained_off) { - // We wouldn't be able to fit all the new bytes into underlying - return false; - } - - true - } - - fn peek_raw(&self, range: core::ops::Range) -> (&[u8], &[u8]) { - let split = self.buf.len() - self.off; - let a = if range.start >= split { - &[] - } else { - &self.buf[range.start + self.off..core::cmp::min(self.buf.len(), range.end + self.off)] - }; - let b = if range.end <= split { - &[] - } else { - &self.new_buf[self.new_buf_offset + range.start.saturating_sub(split) - ..range.end - split + self.new_buf_offset] - }; - (a, b) - } - - /// Provide a view of the next count elements, moving data if necessary. - /// If the underlying store cannot store enough elements, no data is moved and an - /// error is returned. - fn take(&mut self, count: usize) -> Result<&[u8], ParserError> { - let underlying_bytes = core::cmp::min(self.buf.len() - self.off, count); - let new_bytes = count.saturating_sub(underlying_bytes); - - if new_bytes > self.new_buf.len() - self.new_buf_offset { - // We need to pull more bytes from new than it has - panic!( - "Cannot pull {} bytes from a buffer with {}-{}", - new_bytes, - self.new_buf.len(), - self.new_buf_offset - ); - } - - if underlying_bytes == 0 { - // We can directly return a slice from new - let offset = self.new_buf_offset; - self.new_buf_offset += count; - return Ok(&self.new_buf[offset..offset + count]); - } - - if new_bytes == 0 { - // We can directly return from underlying - let offset = self.off; - self.off += count; - return Ok(&self.buf[offset..offset + count]); - } - - if self.buf.max_capacity() < count { - // Insufficient space - return Err(ParserError::OutOfMemory { - required_size: count, - }); - } - - if new_bytes < self.buf.max_capacity() - self.buf.len() { - // Underlying has enough space to extend from new - let bytes_not_moved = self - .buf - .extend_from_slice(&self.new_buf[self.new_buf_offset..]); - self.new_buf_offset += self.new_buf.len() - self.new_buf_offset - bytes_not_moved; - let off = self.off; - self.off += count; - return Ok(&self.buf[off..off + count]); - } - - // Last case: We have to move the data in underlying, then extend it - self.buf.drain(self.off); - self.off = 0; - self.buf - .extend_from_slice(&self.new_buf[self.new_buf_offset..self.new_buf_offset + new_bytes]); - self.new_buf_offset += new_bytes; - self.off += count; - Ok(&self.buf[0..count]) - } -} - -impl<'a, T: UnderlyingBuffer> Drop for DualBuffer<'a, T> { - fn drop(&mut self) { - self.buf.drain(self.off); - self.buf - .extend_from_slice(&self.new_buf[self.new_buf_offset..]); - } -} - -/// For ubx checksum on the fly -#[derive(Default)] -struct UbxChecksumCalc { - ck_a: u8, - ck_b: u8, -} - -impl UbxChecksumCalc { - fn new() -> Self { - Self { ck_a: 0, ck_b: 0 } - } - - fn update(&mut self, bytes: &[u8]) { - let mut a = self.ck_a; - let mut b = self.ck_b; - for byte in bytes.iter() { - a = a.overflowing_add(*byte).0; - b = b.overflowing_add(a).0; - } - self.ck_a = a; - self.ck_b = b; - } - - fn result(self) -> (u8, u8) { - (self.ck_a, self.ck_b) - } -} - -/// Iterator over data stored in `Parser` buffer -pub struct ParserIter<'a, T: UnderlyingBuffer> { - buf: DualBuffer<'a, T>, -} - -impl<'a, T: UnderlyingBuffer> ParserIter<'a, T> { - fn find_sync(&self) -> Option { - (0..self.buf.len()).find(|&i| self.buf[i] == SYNC_CHAR_1) - } - - fn extract_packet(&mut self, pack_len: usize) -> Option> { - if !self.buf.can_drain_and_take(6, pack_len + 2) { - if self.buf.potential_lost_bytes() > 0 { - // We ran out of space, drop this packet and move on - self.buf.drain(2); - return Some(Err(ParserError::OutOfMemory { - required_size: pack_len + 2, - })); - } - return None; - } - let mut checksummer = UbxChecksumCalc::new(); - let (a, b) = self.buf.peek_raw(2..(4 + pack_len + 2)); - checksummer.update(a); - checksummer.update(b); - let (ck_a, ck_b) = checksummer.result(); - - let (expect_ck_a, expect_ck_b) = (self.buf[6 + pack_len], self.buf[6 + pack_len + 1]); - if (ck_a, ck_b) != (expect_ck_a, expect_ck_b) { - self.buf.drain(2); - return Some(Err(ParserError::InvalidChecksum { - expect: u16::from_le_bytes([expect_ck_a, expect_ck_b]), - got: u16::from_le_bytes([ck_a, ck_b]), - })); - } - let class_id = self.buf[2]; - let msg_id = self.buf[3]; - self.buf.drain(6); - let msg_data = match self.buf.take(pack_len + 2) { - Ok(x) => x, - Err(e) => { - return Some(Err(e)); - }, - }; - return Some(match_packet( - class_id, - msg_id, - &msg_data[..msg_data.len() - 2], // Exclude the checksum - )); - } - - #[allow(clippy::should_implement_trait)] - /// Analog of `core::iter::Iterator::next`, should be switched to - /// trait implementation after merge of `` - pub fn next(&mut self) -> Option> { - while self.buf.len() > 0 { - let pos = match self.find_sync() { - Some(x) => x, - None => { - self.buf.clear(); - return None; - }, - }; - self.buf.drain(pos); - - if self.buf.len() < 2 { - return None; - } - if self.buf[1] != SYNC_CHAR_2 { - self.buf.drain(1); - continue; - } - - if self.buf.len() < 6 { - return None; - } - - let pack_len: usize = u16::from_le_bytes([self.buf[4], self.buf[5]]).into(); - if pack_len > usize::from(MAX_PAYLOAD_LEN) { - self.buf.drain(2); - continue; - } - return self.extract_packet(pack_len); - } - None - } -} - -#[cfg(test)] -mod test { - use super::*; - use crate::ubx_packets::*; - - #[cfg(feature = "alloc")] - use alloc::vec; - - #[cfg(feature = "alloc")] - #[test] - fn dl_split_indexing() { - let mut buf = vec![1, 2, 3, 4]; - let new = [5, 6, 7, 8]; - let dual = DualBuffer::new(&mut buf, &new[..]); - for i in 0..8 { - assert_eq!(dual[i], i as u8 + 1); - } - } - - #[cfg(feature = "alloc")] - #[test] - #[should_panic] - fn dl_take_too_many() { - let mut buf = vec![1, 2, 3, 4]; - let new = []; - { - let mut dual = DualBuffer::new(&mut buf, &new[..]); - - // This should panic - let _ = dual.take(6); - } - } - - #[cfg(feature = "alloc")] - #[test] - fn dl_take_range_underlying() { - let mut buf = vec![1, 2, 3, 4]; - let new = []; - { - let mut dual = DualBuffer::new(&mut buf, &new[..]); - let x = dual.take(3).unwrap(); - assert_eq!(x, &[1, 2, 3]); - } - assert_eq!(buf, &[4]); - } - - #[cfg(feature = "alloc")] - #[test] - fn dl_take_range_new() { - let mut buf = vec![]; - let new = [1, 2, 3, 4]; - { - let mut dual = DualBuffer::new(&mut buf, &new[..]); - let x = dual.take(3).unwrap(); - assert_eq!(x, &[1, 2, 3]); - } - assert_eq!(buf, &[4]); - } - - #[cfg(feature = "alloc")] - #[test] - fn dl_take_range_overlapping() { - let mut buf = vec![1, 2, 3, 4]; - let new = [5, 6, 7, 8]; - { - let mut dual = DualBuffer::new(&mut buf, &new[..]); - let x = dual.take(6).unwrap(); - assert_eq!(x, &[1, 2, 3, 4, 5, 6]); - } - assert_eq!(buf, &[7, 8]); - } - - #[cfg(feature = "alloc")] - #[test] - fn dl_take_multi_ranges() { - let mut buf = vec![1, 2, 3, 4, 5, 6, 7]; - let new = [8, 9, 10, 11, 12]; - { - let mut dual = DualBuffer::new(&mut buf, &new[..]); - assert_eq!(dual.take(3).unwrap(), &[1, 2, 3]); - assert_eq!(dual.take(3).unwrap(), &[4, 5, 6]); - assert_eq!(dual.take(3).unwrap(), &[7, 8, 9]); - assert_eq!(dual.take(3).unwrap(), &[10, 11, 12]); - } - assert_eq!(buf, &[]); - } - - #[cfg(feature = "alloc")] - #[test] - fn dl_take_multi_ranges2() { - let mut buf = vec![1, 2, 3, 4, 5, 6, 7]; - let new = [8, 9, 10, 11, 12]; - { - let mut dual = DualBuffer::new(&mut buf, &new[..]); - assert_eq!(dual.take(3).unwrap(), &[1, 2, 3]); - assert_eq!(dual.take(6).unwrap(), &[4, 5, 6, 7, 8, 9]); - } - assert_eq!(buf, &[10, 11, 12]); - } - - #[test] - fn dl_move_then_copy() { - let mut buf = [0; 7]; - let mut buf = FixedLinearBuffer::new(&mut buf); - buf.extend_from_slice(&[1, 2, 3, 4, 5, 6, 7]); - let new = [8, 9, 10, 11, 12]; - { - let mut dual = DualBuffer::new(&mut buf, &new[..]); - assert_eq!(dual.take(3).unwrap(), &[1, 2, 3]); - assert_eq!(dual.take(6).unwrap(), &[4, 5, 6, 7, 8, 9]); - } - assert_eq!(buf.len(), 3); - } - - #[test] - #[should_panic] - fn dl_take_range_oom() { - let mut buf = [0; 4]; - let mut buf = FixedLinearBuffer::new(&mut buf); - let new = [1, 2, 3, 4, 5, 6]; - - let mut dual = DualBuffer::new(&mut buf, &new[..]); - // This should throw - assert!( - matches!(dual.take(6), Err(ParserError::OutOfMemory { required_size }) if required_size == 6) - ); - } - - #[test] - fn dl_drain_partial_underlying() { - let mut buf = [0; 4]; - let mut buf = FixedLinearBuffer::new(&mut buf); - buf.extend_from_slice(&[1, 2, 3]); - let new = [4, 5, 6, 7, 8, 9]; - let mut dual = DualBuffer::new(&mut buf, &new[..]); - - dual.drain(2); - assert_eq!(dual.len(), 7); - assert_eq!(dual.take(4).unwrap(), &[3, 4, 5, 6]); - assert_eq!(dual.len(), 3); - } - - #[test] - fn dl_drain() { - let mut buf = [0; 4]; - let mut buf = FixedLinearBuffer::new(&mut buf); - buf.extend_from_slice(&[1, 2, 3]); - let new = [4, 5, 6, 7, 8, 9]; - let mut dual = DualBuffer::new(&mut buf, &new[..]); - - dual.drain(5); - assert_eq!(dual.take(3).unwrap(), &[6, 7, 8]); - assert_eq!(dual.len(), 1); - } - - #[test] - fn dl_clear() { - let mut buf = [0; 4]; - let mut buf = FixedLinearBuffer::new(&mut buf); - buf.extend_from_slice(&[1, 2, 3]); - let new = [4, 5, 6, 7, 8, 9]; - let mut dual = DualBuffer::new(&mut buf, &new[..]); - - assert_eq!(dual.len(), 9); - dual.clear(); - assert_eq!(dual.len(), 0); - } - - #[test] - fn dl_peek_raw() { - let mut buf = [0; 4]; - let mut buf = FixedLinearBuffer::new(&mut buf); - buf.extend_from_slice(&[1, 2, 3]); - let new = [4, 5, 6, 7, 8, 9]; - let dual = DualBuffer::new(&mut buf, &new[..]); - - let (a, b) = dual.peek_raw(2..6); - assert_eq!(a, &[3]); - assert_eq!(b, &[4, 5, 6]); - } - - #[test] - fn flb_clear() { - let mut buf = [0; 16]; - let mut buf = FixedLinearBuffer::new(&mut buf); - buf.extend_from_slice(&[1, 2, 3, 4, 5, 6, 7]); - assert_eq!(buf.len(), 7); - buf.clear(); - assert_eq!(buf.len(), 0); - } - - #[test] - #[should_panic] - fn flb_index_outside_range() { - let mut buf = [0; 16]; - let mut buf = FixedLinearBuffer::new(&mut buf); - buf.extend_from_slice(&[1, 2, 3, 4, 5, 6, 7]); - let _ = buf[5..10]; - } - - #[test] - fn flb_extend_outside_range() { - let mut buf = [0; 16]; - let mut buf = FixedLinearBuffer::new(&mut buf); - buf.extend_from_slice(&[1, 2, 3, 4, 5, 6, 7]); - buf.extend_from_slice(&[1, 2, 3, 4, 5, 6, 7]); - buf.extend_from_slice(&[1, 2, 3, 4, 5, 6, 7]); - assert_eq!(buf.len(), 16); - } - - #[test] - fn flb_drain() { - let mut buf = [0; 16]; - let mut buf = FixedLinearBuffer::new(&mut buf); - buf.extend_from_slice(&[1, 2, 3, 4, 5, 6, 7]); - - buf.drain(3); - assert_eq!(buf.len(), 4); - assert_eq!(&buf[0..buf.len()], &[4, 5, 6, 7]); - - buf.extend_from_slice(&[1, 2, 3, 4, 5, 6, 7]); - assert_eq!(buf.len(), 11); - assert_eq!(&buf[0..buf.len()], &[4, 5, 6, 7, 1, 2, 3, 4, 5, 6, 7]); - } - - #[test] - fn flb_drain_all() { - let mut buf = [0; 16]; - let mut buf = FixedLinearBuffer::new(&mut buf); - buf.extend_from_slice(&[1, 2, 3, 4, 5, 6, 7]); - - buf.drain(7); - assert_eq!(buf.len(), 0); - } - - #[test] - fn flb_find() { - let mut buf = [1, 2, 3, 4, 5, 6, 7, 8]; - let mut buf = FixedLinearBuffer::new(&mut buf); - assert_eq!(buf.find(5), None); - buf.extend_from_slice(&[1, 2, 3, 4]); - assert_eq!(buf.find(5), None); - buf.extend_from_slice(&[5, 6, 7, 8]); - assert_eq!(buf.find(5), Some(4)); - } - - #[cfg(feature = "alloc")] - #[test] - fn parser_oom_processes_multiple_small_packets() { - let packet = [0xb5, 0x62, 0x5, 0x1, 0x2, 0x0, 0x4, 0x5, 0x11, 0x38]; - - let mut bytes = vec![]; - bytes.extend_from_slice(&packet); - bytes.extend_from_slice(&packet); - bytes.extend_from_slice(&packet); - bytes.extend_from_slice(&packet); - bytes.extend_from_slice(&packet); - - let mut buffer = [0; 10]; - let buffer = FixedLinearBuffer::new(&mut buffer); - let mut parser = Parser::new(buffer); - - let mut it = parser.consume(&bytes); - for _ in 0..5 { - assert!(matches!(it.next(), Some(Ok(PacketRef::AckAck(_))))); - } - assert!(it.next().is_none()); - } - - #[test] - fn parser_handle_garbage_first_byte() { - let mut buffer = [0; 12]; - let buffer = FixedLinearBuffer::new(&mut buffer); - let mut parser = Parser::new(buffer); - - let bytes = [0xb5, 0xb5, 0x62, 0x5, 0x1, 0x2, 0x0, 0x4, 0x5, 0x11, 0x38]; - - { - let mut it = parser.consume(&bytes); - assert!(matches!(it.next(), Some(Ok(PacketRef::AckAck(_))))); - assert!(it.next().is_none()); - } - } - - #[test] - fn parser_oom_clears_buffer() { - let bytes = CfgNav5Builder { - mask: CfgNav5Params::DYN, - dyn_model: CfgNav5DynModel::AirborneWithLess1gAcceleration, - fix_mode: CfgNav5FixMode::Only3D, - fixed_alt: 100.17, - fixed_alt_var: 0.0017, - min_elev_degrees: 17, - pdop: 1.7, - tdop: 1.7, - pacc: 17, - tacc: 17, - static_hold_thresh: 2.17, - dgps_time_out: 17, - cno_thresh_num_svs: 17, - cno_thresh: 17, - static_hold_max_dist: 0x1717, - utc_standard: CfgNav5UtcStandard::UtcChina, - ..CfgNav5Builder::default() - } - .into_packet_bytes(); - - let mut buffer = [0; 12]; - let buffer = FixedLinearBuffer::new(&mut buffer); - let mut parser = Parser::new(buffer); - - { - let mut it = parser.consume(&bytes[0..8]); - assert!(it.next().is_none()); - } - - { - let mut it = parser.consume(&bytes[8..]); - assert!( - matches!(it.next(), Some(Err(ParserError::OutOfMemory { required_size })) if required_size == bytes.len() - 6) - ); - assert!(it.next().is_none()); - } - - // Should now be empty, and we can parse a small packet - let bytes = [0xb5, 0x62, 0x5, 0x1, 0x2, 0x0, 0x4, 0x5, 0x11, 0x38]; - - { - let mut it = parser.consume(&bytes); - assert!(matches!(it.next(), Some(Ok(PacketRef::AckAck(_))))); - assert!(it.next().is_none()); - } - } - - #[test] - fn parser_accepts_packet_array_underlying() { - let bytes = CfgNav5Builder { - mask: CfgNav5Params::DYN, - dyn_model: CfgNav5DynModel::AirborneWithLess1gAcceleration, - fix_mode: CfgNav5FixMode::Only3D, - fixed_alt: 100.17, - fixed_alt_var: 0.0017, - min_elev_degrees: 17, - pdop: 1.7, - tdop: 1.7, - pacc: 17, - tacc: 17, - static_hold_thresh: 2.17, - dgps_time_out: 17, - cno_thresh_num_svs: 17, - cno_thresh: 17, - static_hold_max_dist: 0x1717, - utc_standard: CfgNav5UtcStandard::UtcChina, - ..CfgNav5Builder::default() - } - .into_packet_bytes(); - - let mut buffer = [0; 1024]; - let buffer = FixedLinearBuffer::new(&mut buffer); - let mut parser = Parser::new(buffer); - let mut it = parser.consume(&bytes); - assert!(matches!(it.next(), Some(Ok(PacketRef::CfgNav5(_))))); - assert!(it.next().is_none()); - } - - #[test] - #[cfg(feature = "std")] - fn parser_accepts_packet_vec_underlying() { - let bytes = CfgNav5Builder { - mask: CfgNav5Params::DYN, - dyn_model: CfgNav5DynModel::AirborneWithLess1gAcceleration, - fix_mode: CfgNav5FixMode::Only3D, - fixed_alt: 100.17, - fixed_alt_var: 0.0017, - min_elev_degrees: 17, - pdop: 1.7, - tdop: 1.7, - pacc: 17, - tacc: 17, - static_hold_thresh: 2.17, - dgps_time_out: 17, - cno_thresh_num_svs: 17, - cno_thresh: 17, - static_hold_max_dist: 0x1717, - utc_standard: CfgNav5UtcStandard::UtcChina, - ..CfgNav5Builder::default() - } - .into_packet_bytes(); - - let mut parser = Parser::default(); - let mut it = parser.consume(&bytes); - assert!(matches!(it.next(), Some(Ok(PacketRef::CfgNav5(_))))); - assert!(it.next().is_none()); - } - - #[test] - #[cfg(feature = "std")] - fn parser_accepts_multiple_packets() { - let mut data = vec![]; - data.extend_from_slice( - &CfgNav5Builder { - pacc: 21, - ..CfgNav5Builder::default() - } - .into_packet_bytes(), - ); - data.extend_from_slice( - &CfgNav5Builder { - pacc: 18, - ..CfgNav5Builder::default() - } - .into_packet_bytes(), - ); - - let mut parser = Parser::default(); - let mut it = parser.consume(&data); - match it.next() { - Some(Ok(PacketRef::CfgNav5(packet))) => { - // We're good - assert_eq!(packet.pacc(), 21); - }, - _ => { - panic!() - }, - } - match it.next() { - Some(Ok(PacketRef::CfgNav5(packet))) => { - // We're good - assert_eq!(packet.pacc(), 18); - }, - _ => { - panic!() - }, - } - assert!(it.next().is_none()); - } - - #[test] - #[allow(clippy::assertions_on_constants)] - fn test_max_payload_len() { - assert!(MAX_PAYLOAD_LEN >= 1240); - } -} diff --git a/ublox/ublox/src/ubx_packets.rs b/ublox/ublox/src/ubx_packets.rs deleted file mode 100644 index d885957..0000000 --- a/ublox/ublox/src/ubx_packets.rs +++ /dev/null @@ -1,132 +0,0 @@ -pub mod cfg_val; -mod packets; -mod types; - -use crate::error::MemWriterError; -pub use packets::*; -pub use types::*; - -/// Information about concrete UBX protocol's packet -pub trait UbxPacketMeta { - const CLASS: u8; - const ID: u8; - const FIXED_PAYLOAD_LEN: Option; - const MAX_PAYLOAD_LEN: u16; -} - -pub(crate) const SYNC_CHAR_1: u8 = 0xb5; -pub(crate) const SYNC_CHAR_2: u8 = 0x62; - -/// The checksum is calculated over the packet, starting and including -/// the CLASS field, up until, but excluding, the checksum field. -/// So slice should starts with class id. -/// Return ck_a and ck_b -pub(crate) fn ubx_checksum(data: &[u8]) -> (u8, u8) { - let mut ck_a = 0_u8; - let mut ck_b = 0_u8; - for byte in data { - ck_a = ck_a.overflowing_add(*byte).0; - ck_b = ck_b.overflowing_add(ck_a).0; - } - (ck_a, ck_b) -} - -/// For ubx checksum on the fly -#[derive(Default)] -struct UbxChecksumCalc { - ck_a: u8, - ck_b: u8, -} - -impl UbxChecksumCalc { - fn update(&mut self, chunk: &[u8]) { - for byte in chunk { - self.ck_a = self.ck_a.overflowing_add(*byte).0; - self.ck_b = self.ck_b.overflowing_add(self.ck_a).0; - } - } - fn result(self) -> (u8, u8) { - (self.ck_a, self.ck_b) - } -} - -/// Abstraction for buffer creation/reallocation -/// to storing packet -pub trait MemWriter { - type Error; - /// make sure that we have at least `len` bytes for writing - fn reserve_allocate(&mut self, len: usize) -> Result<(), MemWriterError>; - fn write(&mut self, buf: &[u8]) -> Result<(), MemWriterError>; -} - -#[cfg(feature = "std")] -impl MemWriter for Vec { - type Error = std::io::Error; - - fn reserve_allocate(&mut self, len: usize) -> Result<(), MemWriterError> { - self.reserve(len); - Ok(()) - } - fn write(&mut self, buf: &[u8]) -> Result<(), MemWriterError> { - let ret = ::write(self, buf).map_err(MemWriterError::Custom)?; - if ret == buf.len() { - Ok(()) - } else { - Err(MemWriterError::NotEnoughMem) - } - } -} - -pub trait UbxPacketCreator { - /// Create packet and store bytes sequence to somewhere using `out` - fn create_packet(self, out: &mut T) -> Result<(), MemWriterError>; -} - -/// Packet not supported yet by this crate -#[derive(Debug)] -#[cfg_attr(feature = "serde", derive(serde::Serialize))] -pub struct UbxUnknownPacketRef<'a> { - pub payload: &'a [u8], - pub class: u8, - pub msg_id: u8, -} - -/// Request specific packet -pub struct UbxPacketRequest { - req_class: u8, - req_id: u8, -} - -impl UbxPacketRequest { - pub const PACKET_LEN: usize = 8; - - #[inline] - pub fn request_for() -> Self { - Self { - req_class: T::CLASS, - req_id: T::ID, - } - } - #[inline] - pub fn request_for_unknown(req_class: u8, req_id: u8) -> Self { - Self { req_class, req_id } - } - - #[inline] - pub fn into_packet_bytes(self) -> [u8; Self::PACKET_LEN] { - let mut ret = [ - SYNC_CHAR_1, - SYNC_CHAR_2, - self.req_class, - self.req_id, - 0, - 0, - 0, - 0, - ]; - let (ck_a, ck_b) = ubx_checksum(&ret[2..6]); - ret[6] = ck_a; - ret[7] = ck_b; - ret - } -} diff --git a/ublox/ublox/src/ubx_packets/cfg_val.rs b/ublox/ublox/src/ubx_packets/cfg_val.rs deleted file mode 100644 index 00eb8c1..0000000 --- a/ublox/ublox/src/ubx_packets/cfg_val.rs +++ /dev/null @@ -1,1129 +0,0 @@ -use super::{AlignmentToReferenceTime, CfgInfMask, DataBits, Parity, StopBits}; - -pub struct KeyId(u32); - -pub enum StorageSize { - OneBit, - OneByte, - TwoBytes, - FourBytes, - EightBytes, -} - -impl StorageSize { - pub const fn to_usize(self) -> usize { - match self { - Self::OneBit | Self::OneByte => 1, - Self::TwoBytes => 2, - Self::FourBytes => 4, - Self::EightBytes => 8, - } - } -} - -impl KeyId { - pub(crate) const SIZE: usize = 4; - - pub const fn value_size(&self) -> StorageSize { - match (self.0 >> 28) & 0b111 { - 1 => StorageSize::OneBit, - 2 => StorageSize::OneByte, - 3 => StorageSize::TwoBytes, - 4 => StorageSize::FourBytes, - 5 => StorageSize::EightBytes, - - // TODO: Replace this with unreachable!() when we upgrade to MSRV 1.57 - // Since it's unreachable we get to pick an arbitrary value - //_ => unreachable!(), - _ => StorageSize::OneBit, - } - } - - pub const fn group_id(&self) -> u8 { - (self.0 >> 16) as u8 - } - - pub const fn item_id(&self) -> u8 { - self.0 as u8 - } -} - -macro_rules! from_cfg_v_bytes { - ($buf:expr, bool) => { - match $buf[0] { - 0 => false, - 1 => true, - _ => unreachable!(), - } - }; - ($buf:expr, u8) => { - $buf[0] - }; - ($buf:expr, u16) => { - u16::from_le_bytes([$buf[0], $buf[1]]) - }; - ($buf:expr, i16) => { - i16::from_le_bytes([$buf[0], $buf[1]]) - }; - ($buf:expr, u32) => { - u32::from_le_bytes([$buf[0], $buf[1], $buf[2], $buf[3]]) - }; - ($buf:expr, u64) => { - u64::from_le_bytes([ - $buf[0], $buf[1], $buf[2], $buf[3], $buf[4], $buf[5], $buf[6], $buf[7], - ]) - }; - ($buf:expr, CfgInfMask) => { - CfgInfMask::from_bits_truncate($buf[0]) - }; - ($buf:expr, DataBits) => { - match $buf[0] { - 0 => DataBits::Eight, - 1 => DataBits::Seven, - _ => unreachable!(), - } - }; - ($buf:expr, Parity) => { - match $buf[0] { - 0 => Parity::None, - 1 => Parity::Odd, - 2 => Parity::Even, - _ => unreachable!(), - } - }; - ($buf:expr, StopBits) => { - match $buf[0] { - 0 => StopBits::Half, - 1 => StopBits::One, - 2 => StopBits::OneHalf, - 3 => StopBits::Two, - _ => unreachable!(), - } - }; - ($buf:expr, AlignmentToReferenceTime) => { - match $buf[0] { - 0 => AlignmentToReferenceTime::Utc, - 1 => AlignmentToReferenceTime::Gps, - 2 => AlignmentToReferenceTime::Glo, - 3 => AlignmentToReferenceTime::Bds, - 4 => AlignmentToReferenceTime::Gal, - _ => unreachable!(), - } - }; - ($buf:expr, TpPulse) => { - match $buf[0] { - 0 => TpPulse::Period, - 1 => TpPulse::Freq, - _ => unreachable!(), - } - }; - ($buf:expr, TpPulseLength) => { - match $buf[0] { - 0 => TpPulseLength::Ratio, - 1 => TpPulseLength::Length, - _ => unreachable!(), - } - }; -} - -macro_rules! into_cfg_kv_bytes { - (@inner [$($byte:expr),+]) => {{ - let key_id = Self::KEY.0.to_le_bytes(); - - [ - key_id[0], key_id[1], key_id[2], key_id[3], - $( - $byte, - )* - ] - }}; - ($this:expr, bool) => { - into_cfg_kv_bytes!(@inner [$this.0 as u8]) - }; - ($this:expr, u8) => {{ - into_cfg_kv_bytes!(@inner [$this.0]) - }}; - ($this:expr, u16) => {{ - let bytes = $this.0.to_le_bytes(); - into_cfg_kv_bytes!(@inner [bytes[0], bytes[1]]) - }}; - ($this:expr, i16) => {{ - let bytes = $this.0.to_le_bytes(); - into_cfg_kv_bytes!(@inner [bytes[0], bytes[1]]) - }}; - ($this:expr, u32) => {{ - let bytes = $this.0.to_le_bytes(); - into_cfg_kv_bytes!(@inner [bytes[0], bytes[1], bytes[2], bytes[3]]) - }}; - ($this:expr, u64) => {{ - let bytes = $this.0.to_le_bytes(); - into_cfg_kv_bytes!(@inner [bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5], bytes[6], bytes[7]]) - }}; - ($this:expr, CfgInfMask) => { - into_cfg_kv_bytes!(@inner [ - $this.0.bits() - ]) - }; - ($this:expr, DataBits) => { - into_cfg_kv_bytes!(@inner [ - match $this.0 { - DataBits::Eight => 0, - DataBits::Seven => 1, - } - ]) - }; - ($this:expr, Parity) => { - into_cfg_kv_bytes!(@inner [ - match $this.0 { - Parity::None => 0, - Parity::Odd => 1, - Parity::Even => 2, - } - ]) - }; - ($this:expr, StopBits) => { - into_cfg_kv_bytes!(@inner [ - match $this.0 { - StopBits::Half => 0, - StopBits::One => 1, - StopBits::OneHalf => 2, - StopBits::Two => 3, - } - ]) - }; - ($this:expr, AlignmentToReferenceTime) => { - into_cfg_kv_bytes!(@inner [ - $this.0 as u8 - ]) - }; - ($this:expr, TpPulse) => { - into_cfg_kv_bytes!(@inner [ - $this.0 as u8 - ]) - }; - ($this:expr, TpPulseLength) => { - into_cfg_kv_bytes!(@inner [ - $this.0 as u8 - ]) - }; -} - -macro_rules! cfg_val { - ( - $( - $(#[$class_comment:meta])* - $cfg_item:ident, $cfg_key_id:expr, $cfg_value_type:ident, - )* - ) => { - #[derive(Debug, Clone)] - #[non_exhaustive] - pub enum CfgVal { - $( - $(#[$class_comment])* - $cfg_item($cfg_value_type), - )* - } - - impl CfgVal { - pub const fn len(&self) -> usize { - match self { - $( - Self::$cfg_item(_) => { - $cfg_item::SIZE - } - )* - } - } - - pub const fn is_empty(&self) -> bool { - self.len() == 0 - } - - #[track_caller] - pub fn parse(buf: &[u8]) -> Self { - let key_id = u32::from_le_bytes([buf[0], buf[1], buf[2], buf[3]]); - match key_id { - $( - $cfg_key_id => { - Self::$cfg_item(from_cfg_v_bytes!(&buf[4..], $cfg_value_type)) - }, - )* - _ => unimplemented!("unknown key ID: 0x{:8X}", key_id), - } - } - - pub fn extend_to(&self, buf: &mut T) -> usize - where - T: core::iter::Extend - { - match self { - $( - Self::$cfg_item(value) => { - let bytes = $cfg_item(*value).into_cfg_kv_bytes(); - let bytes_len = bytes.len(); - // TODO: extend all the bytes in one extend() call when we bump MSRV - for b in bytes.iter() { - buf.extend(core::iter::once(*b)); - } - bytes_len - } - )* - } - } - - pub fn write_to(&self, buf: &mut [u8]) -> usize { - match self { - $( - Self::$cfg_item(value) => { - let kv: [u8; $cfg_item::SIZE] = $cfg_item(*value).into_cfg_kv_bytes(); - buf[..kv.len()].copy_from_slice(&kv[..]); - kv.len() - } - )* - } - } - } - - $( - struct $cfg_item(pub $cfg_value_type); - - impl $cfg_item { - const KEY: KeyId = KeyId($cfg_key_id); - const SIZE: usize = KeyId::SIZE + Self::KEY.value_size().to_usize(); - - pub const fn into_cfg_kv_bytes(self) -> [u8; Self::SIZE] { - into_cfg_kv_bytes!(self, $cfg_value_type) - } - } - )* - } -} - -cfg_val! { - // CFG-UART1 - Uart1Baudrate, 0x40520001, u32, - Uart1StopBits, 0x20520002, StopBits, - Uart1DataBits, 0x20520003, DataBits, - Uart1Parity, 0x20520004, Parity, - Uart1Enabled, 0x10520005, bool, - - // CFG-UART1INPROT - Uart1InProtUbx, 0x10730001, bool, - Uart1InProtNmea, 0x10730002, bool, - Uart1InProtRtcm3x, 0x10730004, bool, - - // CFG-UART1OUTPROT - Uart1OutProtUbx, 0x10740001, bool, - Uart1OutProtNmea, 0x10740002, bool, - Uart1OutProtRtcm3x, 0x10740004, bool, - - // CFG-UART2 - Uart2Baudrate, 0x40530001, u32, - Uart2StopBits, 0x20530002, StopBits, - Uart2DataBits, 0x20530003, DataBits, - Uart2Parity, 0x20530004, Parity, - Uart2Enabled, 0x10530005, bool, - Uart2Remap, 0x10530006, bool, - - // CFG-UART2INPROT - Uart2InProtUbx, 0x10750001, bool, - Uart2InProtNmea, 0x10750002, bool, - Uart2InProtRtcm3x, 0x10750004, bool, - - // CFG-UART2OUTPROT - Uart2OutProtUbx, 0x10760001, bool, - Uart2OutProtNmea, 0x10760002, bool, - Uart2OutProtRtcm3x, 0x10760004, bool, - - // CFG-USB - UsbEnabled, 0x10650001, bool, - UsbSelfpow, 0x10650002, bool, - UsbVendorId, 0x3065000a, u16, - UsbProductId, 0x3065000b, u16, - UsbPower, 0x3065000c, u16, - UsbVendorStr0, 0x5065000d, u64, - UsbVendorStr1, 0x5065000e, u64, - UsbVendorStr2, 0x5065000f, u64, - UsbVendorStr3, 0x50650010, u64, - UsbProductStr0, 0x50650011, u64, - UsbProductStr1, 0x50650012, u64, - UsbProductStr2, 0x50650013, u64, - UsbProductStr3, 0x50650014, u64, - UsbSerialNoStr0, 0x50650015, u64, - UsbSerialNoStr1, 0x50650016, u64, - UsbSerialNoStr2, 0x50650017, u64, - UsbSerialNoStr3, 0x50650018, u64, - - // CFG-USBINPROT-* - UsbinprotUbx, 0x10770001, bool, - UsbinprotNmea, 0x10770002, bool, - UsbinprotRtcm3X, 0x10770004, bool, - - // CFG-USBOUTPROT-* - UsbOutProtUbx, 0x10780001, bool, - UsbOutProtNmea, 0x10780002, bool, - UsbOutProtRtcm3x, 0x10780004, bool, - - // CFG-INFMSG - InfmsgUbxI2c, 0x20920001, CfgInfMask, - InfmsgUbxUart1, 0x20920002, CfgInfMask, - InfmsgUbxUart2, 0x20920003, CfgInfMask, - InfmsgUbxUsb, 0x20920004, CfgInfMask, - InfmsgUbxSpi, 0x20920005, CfgInfMask, - InfmsgNmeaI2c, 0x20920006, CfgInfMask, - InfmsgNmeaUart1, 0x20920007, CfgInfMask, - InfmsgNmeaUart2, 0x20920008, CfgInfMask, - InfmsgNmeaUsb, 0x20920009, CfgInfMask, - InfmsgNmeaSpi, 0x2092000a, CfgInfMask, - - // CFG-RATE-* - /// Nominal time between GNSS measurements - /// (e.g. 100ms results in 10Hz measurement rate, 1000ms = 1Hz measurement rate) - RateMeas, 0x30210001, u16, - /// Ratio of number of measurements to number of navigation solutions - RateNav, 0x30210002, u16, - /// Time system to which measurements are aligned - RateTimeref, 0x20210003, AlignmentToReferenceTime, - - // CFG-MSGOUT-* - /// Output rate of the NMEA-GX-DTM message on port I2C - MsgoutNmeaIdDtmI2c, 0x209100a6, u8, - /// Output rate of the NMEA-GX-DTM message on port SPI - MsgoutNmeaIdDtmSpi, 0x209100aa, u8, - /// Output rate of the NMEA-GX-DTM message on port UART1 - MsgoutNmeaIdDtmUart1, 0x209100a7, u8, - /// Output rate of the NMEA-GX-DTM message on port UART2 - MsgoutNmeaIdDtmUart2, 0x209100a8, u8, - /// Output rate of the NMEA-GX-DTM message on port USB - MsgoutNmeaIdDtmUsb, 0x209100a9, u8, - /// Output rate of the NMEA-GX-GBS message on port I2C - MsgoutNmeaIdGbsI2c, 0x209100dd, u8, - /// Output rate of the NMEA-GX-GBS message on port SPI - MsgoutNmeaIdGbsSpi, 0x209100e1, u8, - /// Output rate of the NMEA-GX-GBS message on port UART1 - MsgoutNmeaIdGbsUart1, 0x209100de, u8, - /// Output rate of the NMEA-GX-GBS message on port UART2 - MsgoutNmeaIdGbsUart2, 0x209100df, u8, - /// Output rate of the NMEA-GX-GBS message on port USB - MsgoutNmeaIdGbsUsb, 0x209100e0, u8, - /// Output rate of the NMEA-GX-GGA message on port I2C - MsgoutNmeaIdGgaI2c, 0x209100ba, u8, - /// Output rate of the NMEA-GX-GGA message on port SPI - MsgoutNmeaIdGgaSpi, 0x209100be, u8, - /// Output rate of the NMEA-GX-GGA message on port UART1 - MsgoutNmeaIdGgaUart1, 0x209100bb, u8, - /// Output rate of the NMEA-GX-GGA message on port UART2 - MsgoutNmeaIdGgaUart2, 0x209100bc, u8, - /// Output rate of the NMEA-GX-GGA message on port USB - MsgoutNmeaIdGgaUsb, 0x209100bd, u8, - /// Output rate of the NMEA-GX-GLL message on port I2C - MsgoutNmeaIdGllI2c, 0x209100c9, u8, - /// Output rate of the NMEA-GX-GLL message on port SPI - MsgoutNmeaIdGllSpi, 0x209100cd, u8, - /// Output rate of the NMEA-GX-GLL message on port UART1 - MsgoutNmeaIdGllUart1, 0x209100ca, u8, - /// Output rate of the NMEA-GX-GLL message on port UART2 - MsgoutNmeaIdGllUart2, 0x209100cb, u8, - /// Output rate of the NMEA-GX-GLL message on port USB - MsgoutNmeaIdGllUsb, 0x209100cc, u8, - /// Output rate of the NMEA-GX-GNS message on port I2C - MsgoutNmeaIdGnsI2c, 0x209100b5, u8, - /// Output rate of the NMEA-GX-GNS message on port SPI - MsgoutNmeaIdGnsSpi, 0x209100b9, u8, - /// Output rate of the NMEA-GX-GNS message on port UART1 - MsgoutNmeaIdGnsUart1, 0x209100b6, u8, - /// Output rate of the NMEA-GX-GNS message on port UART2 - MsgoutNmeaIdGnsUart2, 0x209100b7, u8, - /// Output rate of the NMEA-GX-GNS message on port USB - MsgoutNmeaIdGnsUsb, 0x209100b8, u8, - /// Output rate of the NMEA-GX-GRS message on port I2C - MsgoutNmeaIdGrsI2c, 0x209100ce, u8, - /// Output rate of the NMEA-GX-GRS message on port SPI - MsgoutNmeaIdGrsSpi, 0x209100d2, u8, - /// Output rate of the NMEA-GX-GRS message on port UART1 - MsgoutNmeaIdGrsUart1, 0x209100cf, u8, - /// Output rate of the NMEA-GX-GRS message on port UART2 - MsgoutNmeaIdGrsUart2, 0x209100d0, u8, - /// Output rate of the NMEA-GX-GRS message on port USB - MsgoutNmeaIdGrsUsb, 0x209100d1, u8, - /// Output rate of the NMEA-GX-GSA message on port I2C - MsgoutNmeaIdGsaI2c, 0x209100bf, u8, - /// Output rate of the NMEA-GX-GSA message on port SPI - MsgoutNmeaIdGsaSpi, 0x209100c3, u8, - /// Output rate of the NMEA-GX-GSA message on port UART1 - MsgoutNmeaIdGsaUart1, 0x209100c0, u8, - /// Output rate of the NMEA-GX-GSA message on port UART2 - MsgoutNmeaIdGsaUart2, 0x209100c1, u8, - /// Output rate of the NMEA-GX-GSA message on port USB - MsgoutNmeaIdGsaUsb, 0x209100c2, u8, - /// Output rate of the NMEA-GX-GST message on port I2C - MsgoutNmeaIdGstI2c, 0x209100d3, u8, - /// Output rate of the NMEA-GX-GST message on port SPI - MsgoutNmeaIdGstSpi, 0x209100d7, u8, - /// Output rate of the NMEA-GX-GST message on port UART1 - MsgoutNmeaIdGstUart1, 0x209100d4, u8, - /// Output rate of the NMEA-GX-GST message on port UART2 - MsgoutNmeaIdGstUart2, 0x209100d5, u8, - /// Output rate of the NMEA-GX-GST message on port USB - MsgoutNmeaIdGstUsb, 0x209100d6, u8, - /// Output rate of the NMEA-GX-GSV message on port I2C - MsgoutNmeaIdGsvI2c, 0x209100c4, u8, - /// Output rate of the NMEA-GX-GSV message on port SPI - MsgoutNmeaIdGsvSpi, 0x209100c8, u8, - /// Output rate of the NMEA-GX-GSV message on port UART1 - MsgoutNmeaIdGsvUart1, 0x209100c5, u8, - /// Output rate of the NMEA-GX-GSV message on port UART2 - MsgoutNmeaIdGsvUart2, 0x209100c6, u8, - /// Output rate of the NMEA-GX-GSV message on port USB - MsgoutNmeaIdGsvUsb, 0x209100c7, u8, - /// Output rate of the NMEA-GX-RMC message on port I2C - MsgoutNmeaIdRmcI2c, 0x209100ab, u8, - /// Output rate of the NMEA-GX-RMC message on port SPI - MsgoutNmeaIdRmcSpi, 0x209100af, u8, - /// Output rate of the NMEA-GX-RMC message on port UART1 - MsgoutNmeaIdRmcUart1, 0x209100ac, u8, - /// Output rate of the NMEA-GX-RMC message on port UART2 - MsgoutNmeaIdRmcUart2, 0x209100ad, u8, - /// Output rate of the NMEA-GX-RMC message on port USB - MsgoutNmeaIdRmcUsb, 0x209100ae, u8, - /// Output rate of the NMEA-GX-VLW message on port I2C - MsgoutNmeaIdVlwI2c, 0x209100e7, u8, - /// Output rate of the NMEA-GX-VLW message on port SPI - MsgoutNmeaIdVlwSpi, 0x209100eb, u8, - /// Output rate of the NMEA-GX-VLW message on port UART1 - MsgoutNmeaIdVlwUart1, 0x209100e8, u8, - /// Output rate of the NMEA-GX-VLW message on port UART2 - MsgoutNmeaIdVlwUart2, 0x209100e9, u8, - /// Output rate of the NMEA-GX-VLW message on port USB - MsgoutNmeaIdVlwUsb, 0x209100ea, u8, - /// Output rate of the NMEA-GX-VTG message on port I2C - MsgoutNmeaIdVtgI2c, 0x209100b0, u8, - /// Output rate of the NMEA-GX-VTG message on port SPI - MsgoutNmeaIdVtgSpi, 0x209100b4, u8, - /// Output rate of the NMEA-GX-VTG message on port UART1 - MsgoutNmeaIdVtgUart1, 0x209100b1, u8, - /// Output rate of the NMEA-GX-VTG message on port UART2 - MsgoutNmeaIdVtgUart2, 0x209100b2, u8, - /// Output rate of the NMEA-GX-VTG message on port USB - MsgoutNmeaIdVtgUsb, 0x209100b3, u8, - /// Output rate of the NMEA-GX-ZDA message on port I2C - MsgoutNmeaIdZdaI2c, 0x209100d8, u8, - /// Output rate of the NMEA-GX-ZDA message on port SPI - MsgoutNmeaIdZdaSpi, 0x209100dc, u8, - /// Output rate of the NMEA-GX-ZDA message on port UART1 - MsgoutNmeaIdZdaUart1, 0x209100d9, u8, - /// Output rate of the NMEA-GX-ZDA message on port UART2 - MsgoutNmeaIdZdaUart2, 0x209100da, u8, - /// Output rate of the NMEA-GX-ZDA message on port USB - MsgoutNmeaIdZdaUsb, 0x209100db, u8, - /// Output rate of the NMEA-GX-PUBX00 message on port I2C - MsgoutPubxIdPolypI2c, 0x209100ec, u8, - /// Output rate of the NMEA-GX-PUBX00 message on port SPI - MsgoutPubxIdPolypSpi, 0x209100f0, u8, - /// Output rate of the NMEA-GX-PUBX00 message on port UART1 - MsgoutPubxIdPolypUart1, 0x209100ed, u8, - /// Output rate of the NMEA-GX-PUBX00 message on port UART2 - MsgoutPubxIdPolypUart2, 0x209100ee, u8, - /// Output rate of the NMEA-GX-PUBX00 message on port USB - MsgoutPubxIdPolypUsb, 0x209100ef, u8, - /// Output rate of the NMEA-GX-PUBX03 message on port I2C - MsgoutPubxIdPolysI2c, 0x209100f1, u8, - /// Output rate of the NMEA-GX-PUBX03 message on port SPI - MsgoutPubxIdPolysSpi, 0x209100f5, u8, - /// Output rate of the NMEA-GX-PUBX03 message on port UART1 - MsgoutPubxIdPolysUart1, 0x209100f2, u8, - /// Output rate of the NMEA-GX-PUBX03 message on port UART2 - MsgoutPubxIdPolysUart2, 0x209100f3, u8, - /// Output rate of the NMEA-GX-PUBX03 message on port USB - MsgoutPubxIdPolysUsb, 0x209100f4, u8, - /// Output rate of the NMEA-GX-PUBX04 message on port I2C - MsgoutPubxIdPolytI2c, 0x209100f6, u8, - /// Output rate of the NMEA-GX-PUBX04 message on port SPI - MsgoutPubxIdPolytSpi, 0x209100fa, u8, - /// Output rate of the NMEA-GX-PUBX04 message on port UART1 - MsgoutPubxIdPolytUart1, 0x209100f7, u8, - /// Output rate of the NMEA-GX-PUBX04 message on port UART2 - MsgoutPubxIdPolytUart2, 0x209100f8, u8, - /// Output rate of the NMEA-GX-PUBX04 message on port USB - MsgoutPubxIdPolytUsb, 0x209100f9, u8, - /// Output rate of the RTCM-3XTYPE1005 message on port I2C - MsgoutRtcm3xType1005I2c, 0x209102bd, u8, - /// Output rate of the RTCM-3XTYPE1005 message on port SPI - MsgoutRtcm3xType1005Spi, 0x209102c1, u8, - /// Output rate of the RTCM-3XTYPE1005 message on port UART1 - MsgoutRtcm3xType1005Uart1, 0x209102be, u8, - /// Output rate of the RTCM-3XTYPE1005 message on port UART2 - MsgoutRtcm3xType1005Uart2, 0x209102bf, u8, - /// Output rate of the RTCM-3XTYPE1005 message on port USB - MsgoutRtcm3xType1005Usb, 0x209102c0, u8, - /// Output rate of the RTCM-3XTYPE1074 message on port I2C - MsgoutRtcm3xType1074I2c, 0x2091035e, u8, - /// Output rate of the RTCM-3XTYPE1074 message on port SPI - MsgoutRtcm3xType1074Spi, 0x20910362, u8, - /// Output rate of the RTCM-3XTYPE1074 message on port UART1 - MsgoutRtcm3xType1074Uart1, 0x2091035f, u8, - /// Output rate of the RTCM-3XTYPE1074 message on port UART2 - MsgoutRtcm3xType1074Uart2, 0x20910360, u8, - /// Output rate of the RTCM-3XTYPE1074 message on port USB - MsgoutRtcm3xType1074Usb, 0x20910361, u8, - /// Output rate of the RTCM-3XTYPE1077 message on port I2C - MsgoutRtcm3xType1077I2c, 0x209102cc, u8, - /// Output rate of the RTCM-3XTYPE1077 message on port SPI - MsgoutRtcm3xType1077Spi, 0x209102d0, u8, - /// Output rate of the RTCM-3XTYPE1077 message on port UART1 - MsgoutRtcm3xType1077Uart1, 0x209102cd, u8, - /// Output rate of the RTCM-3XTYPE1077 message on port UART2 - MsgoutRtcm3xType1077Uart2, 0x209102ce, u8, - /// Output rate of the RTCM-3XTYPE1077 message on port USB - MsgoutRtcm3xType1077Usb, 0x209102cf, u8, - /// Output rate of the RTCM-3XTYPE1084 message on port I2C - MsgoutRtcm3xType1084I2c, 0x20910363, u8, - /// Output rate of the RTCM-3XTYPE1084 message on port SPI - MsgoutRtcm3xType1084Spi, 0x20910367, u8, - /// Output rate of the RTCM-3XTYPE1084 message on port UART1 - MsgoutRtcm3xType1084Uart1, 0x20910364, u8, - /// Output rate of the RTCM-3XTYPE1084 message on port UART2 - MsgoutRtcm3xType1084Uart2, 0x20910365, u8, - /// Output rate of the RTCM-3XTYPE1084 message on port USB - MsgoutRtcm3xType1084Usb, 0x20910366, u8, - /// Output rate of the RTCM-3XTYPE1087 message on port I2C - MsgoutRtcm3xType1087I2c, 0x209102d1, u8, - /// Output rate of the RTCM-3XTYPE1087 message on port SPI - MsgoutRtcm3xType1087Spi, 0x209102d5, u8, - /// Output rate of the RTCM-3XTYPE1087 message on port UART1 - MsgoutRtcm3xType1087Uart1, 0x209102d2, u8, - /// Output rate of the RTCM-3XTYPE1087 message on port UART2 - MsgoutRtcm3xType1087Uart2, 0x209102d3, u8, - /// Output rate of the RTCM-3XTYPE1087 message on port USB - MsgoutRtcm3xType1087Usb, 0x209102d4, u8, - /// Output rate of the RTCM-3XTYPE1094 message on port I2C - MsgoutRtcm3xType1094I2c, 0x20910368, u8, - /// Output rate of the RTCM-3XTYPE1094 message on port SPI - MsgoutRtcm3xType1094Spi, 0x2091036c, u8, - /// Output rate of the RTCM-3XTYPE1094 message on port UART1 - MsgoutRtcm3xType1094Uart1, 0x20910369, u8, - /// Output rate of the RTCM-3XTYPE1094 message on port UART2 - MsgoutRtcm3xType1094Uart2, 0x2091036a, u8, - /// Output rate of the RTCM-3XTYPE1094 message on port USB - MsgoutRtcm3xType1094Usb, 0x2091036b, u8, - /// Output rate of the RTCM-3XTYPE1097 message on port I2C - MsgoutRtcm3xType1097I2c, 0x20910318, u8, - /// Output rate of the RTCM-3XTYPE1097 message on port SPI - MsgoutRtcm3xType1097Spi, 0x2091031c, u8, - /// Output rate of the RTCM-3XTYPE1097 message on port UART1 - MsgoutRtcm3xType1097Uart1, 0x20910319, u8, - /// Output rate of the RTCM-3XTYPE1097 message on port UART2 - MsgoutRtcm3xType1097Uart2, 0x2091031a, u8, - /// Output rate of the RTCM-3XTYPE1097 message on port USB - MsgoutRtcm3xType1097Usb, 0x2091031b, u8, - /// Output rate of the RTCM-3XTYPE1124 message on port I2C - MsgoutRtcm3xType1124I2c, 0x2091036d, u8, - /// Output rate of the RTCM-3XTYPE1124 message on port SPI - MsgoutRtcm3xType1124Spi, 0x20910371, u8, - /// Output rate of the RTCM-3XTYPE1124 message on port UART1 - MsgoutRtcm3xType1124Uart1, 0x2091036e, u8, - /// Output rate of the RTCM-3XTYPE1124 message on port UART2 - MsgoutRtcm3xType1124Uart2, 0x2091036f, u8, - /// Output rate of the RTCM-3XTYPE1124 message on port USB - MsgoutRtcm3xType1124Usb, 0x20910370, u8, - /// Output rate of the RTCM-3XTYPE1127 message on port I2C - MsgoutRtcm3xType1127I2c, 0x209102d6, u8, - /// Output rate of the RTCM-3XTYPE1127 message on port SPI - MsgoutRtcm3xType1127Spi, 0x209102da, u8, - /// Output rate of the RTCM-3XTYPE1127 message on port UART1 - MsgoutRtcm3xType1127Uart1, 0x209102d7, u8, - /// Output rate of the RTCM-3XTYPE1127 message on port UART2 - MsgoutRtcm3xType1127Uart2, 0x209102d8, u8, - /// Output rate of the RTCM-3XTYPE1127 message on port USB - MsgoutRtcm3xType1127Usb, 0x209102d9, u8, - /// Output rate of the RTCM-3XTYPE1230 message on port I2C - MsgoutRtcm3xType1230I2c, 0x20910303, u8, - /// Output rate of the RTCM-3XTYPE1230 message on port SPI - MsgoutRtcm3xType1230Spi, 0x20910307, u8, - /// Output rate of the RTCM-3XTYPE1230 message on port UART1 - MsgoutRtcm3xType1230Uart1, 0x20910304, u8, - /// Output rate of the RTCM-3XTYPE1230 message on port UART2 - MsgoutRtcm3xType1230Uart2, 0x20910305, u8, - /// Output rate of the RTCM-3XTYPE1230 message on port USB - MsgoutRtcm3xType1230Usb, 0x20910306, u8, - /// Output rate of the UBX-LOG-INFO message on port I2C - MsgoutUbxLogInfoI2c, 0x20910259, u8, - /// Output rate of the UBX-LOG-INFO message on port SPI - MsgoutUbxLogInfoSpi, 0x2091025d, u8, - /// Output rate of the UBX-LOG-INFO message on port UART1 - MsgoutUbxLogInfoUart1, 0x2091025a, u8, - /// Output rate of the UBX-LOG-INFO message on port UART2 - MsgoutUbxLogInfoUart2, 0x2091025b, u8, - /// Output rate of the UBX-LOG-INFO message on port USB - MsgoutUbxLogInfoUsb, 0x2091025c, u8, - /// Output rate of the UBX-MONCOMMS message on port I2C - MsgoutUbxMonCommsI2c, 0x2091034f, u8, - /// Output rate of the UBX-MONCOMMS message on port SPI - MsgoutUbxMonCommsSpi, 0x20910353, u8, - /// Output rate of the UBX-MONCOMMS message on port UART1 - MsgoutUbxMonCommsUart1, 0x20910350, u8, - /// Output rate of the UBX-MONCOMMS message on port UART2 - MsgoutUbxMonCommsUart2, 0x20910351, u8, - /// Output rate of the UBX-MONCOMMS message on port USB - MsgoutUbxMonCommsUsb, 0x20910352, u8, - /// Output rate of the UBX-MON-HW2 message on port I2C - MsgoutUbxMonHw2I2c, 0x209101b9, u8, - /// Output rate of the UBX-MON-HW2 message on port SPI - MsgoutUbxMonHw2Spi, 0x209101bd, u8, - /// Output rate of the UBX-MON-HW2 message on port UART1 - MsgoutUbxMonHw2Uart1, 0x209101ba, u8, - /// Output rate of the UBX-MON-HW2 message on port UART2 - MsgoutUbxMonHw2Uart2, 0x209101bb, u8, - /// Output rate of the UBX-MON-HW2 message on port USB - MsgoutUbxMonHw2Usb, 0x209101bc, u8, - /// Output rate of the UBX-MON-HW3 message on port I2C - MsgoutUbxMonHw3I2c, 0x20910354, u8, - /// Output rate of the UBX-MON-HW3 message on port SPI - MsgoutUbxMonHw3Spi, 0x20910358, u8, - /// Output rate of the UBX-MON-HW3 message on port UART1 - MsgoutUbxMonHw3Uart1, 0x20910355, u8, - /// Output rate of the UBX-MON-HW3 message on port UART2 - MsgoutUbxMonHw3Uart2, 0x20910356, u8, - /// Output rate of the UBX-MON-HW3 message on port USB - MsgoutUbxMonHw3Usb, 0x20910357, u8, - /// Output rate of the UBX-MON-HW message on port I2C - MsgoutUbxMonHwI2c, 0x209101b4, u8, - /// Output rate of the UBX-MON-HW message on port SPI - MsgoutUbxMonHwSpi, 0x209101b8, u8, - /// Output rate of the UBX-MON-HW message on port UART1 - MsgoutUbxMonHwUart1, 0x209101b5, u8, - /// Output rate of the UBX-MON-HW message on port UART2 - MsgoutUbxMonHwUart2, 0x209101b6, u8, - /// Output rate of the UBX-MON-HW message on port USB - MsgoutUbxMonHwUsb, 0x209101b7, u8, - /// Output rate of the UBX-MON-IO message on port I2C - MsgoutUbxMonIoI2c, 0x209101a5, u8, - /// Output rate of the UBX-MON-IO message on port SPI - MsgoutUbxMonIoSpi, 0x209101a9, u8, - /// Output rate of the UBX-MON-IO message on port UART1 - MsgoutUbxMonIoUart1, 0x209101a6, u8, - /// Output rate of the UBX-MON-IO message on port UART2 - MsgoutUbxMonIoUart2, 0x209101a7, u8, - /// Output rate of the UBX-MON-IO message on port USB - MsgoutUbxMonIoUsb, 0x209101a8, u8, - /// Output rate of the UBX-MON-MSGPP message on port I2C - MsgoutUbxMonMsgPpI2c, 0x20910196, u8, - /// Output rate of the UBX-MON-MSGPP message on port SPI - MsgoutUbxMonMsgPpSpi, 0x2091019a, u8, - /// Output rate of the UBX-MON-MSGPP message on port UART1 - MsgoutUbxMonMsgPpUart1, 0x20910197, u8, - /// Output rate of the UBX-MON-MSGPP message on port UART2 - MsgoutUbxMonMsgPpUart2, 0x20910198, u8, - /// Output rate of the UBX-MON-MSGPP message on port USB - MsgoutUbxMonMsgPpUsb, 0x20910199, u8, - /// Output rate of the UBX-MON-RF message on port I2C - MsgoutUbxMonRfI2c, 0x20910359, u8, - /// Output rate of the UBX-MON-RF message on port SPI - MsgoutUbxMonRfSpi, 0x2091035d, u8, - /// Output rate of the UBX-MON-RF message on port UART1 - MsgoutUbxMonRfUart1, 0x2091035a, u8, - /// Output rate of the UBX-MON-RF message on port UART2 - MsgoutUbxMonRfUart2, 0x2091035b, u8, - /// Output rate of the UBX-MON-RF message on port USB - MsgoutUbxMonRfUsb, 0x2091035c, u8, - /// Output rate of the UBX-MON-RXBUF message on port I2C - MsgoutUbxMonRxbufI2c, 0x209101a0, u8, - /// Output rate of the UBX-MON-RXBUF message on port SPI - MsgoutUbxMonRxbufSpi, 0x209101a4, u8, - /// Output rate of the UBX-MON-RXBUF message on port UART1 - MsgoutUbxMonRxbufUart1, 0x209101a1, u8, - /// Output rate of the UBX-MON-RXBUF message on port UART2 - MsgoutUbxMonRxbufUart2, 0x209101a2, u8, - /// Output rate of the UBX-MON-RXBUF message on port USB - MsgoutUbxMonRxbufUsb, 0x209101a3, u8, - /// Output rate of the UBX-MON-RXR message on port I2C - MsgoutUbxMonRxrI2c, 0x20910187, u8, - /// Output rate of the UBX-MON-RXR message on port SPI - MsgoutUbxMonRxrSpi, 0x2091018b, u8, - /// Output rate of the UBX-MON-RXR message on port UART1 - MsgoutUbxMonRxrUart1, 0x20910188, u8, - /// Output rate of the UBX-MON-RXR message on port UART2 - MsgoutUbxMonRxrUart2, 0x20910189, u8, - /// Output rate of the UBX-MON-RXR message on port USB - MsgoutUbxMonRxrUsb, 0x2091018a, u8, - /// Output rate of the UBX-MON-TXBUF message on port I2C - MsgoutUbxMonTxbufI2c, 0x2091019b, u8, - /// Output rate of the UBX-MON-TXBUF message on port SPI - MsgoutUbxMonTxbufSpi, 0x2091019f, u8, - /// Output rate of the UBX-MON-TXBUF message on port UART1 - MsgoutUbxMonTxbufUart1, 0x2091019c, u8, - /// Output rate of the UBX-MON-TXBUF message on port UART2 - MsgoutUbxMonTxbufUart2, 0x2091019d, u8, - /// Output rate of the UBX-MON-TXBUF message on port USB - MsgoutUbxMonTxbufUsb, 0x2091019e, u8, - /// Output rate of the UBX-NAV-CLOCK message on port I2C - MsgoutUbxNavClockI2c, 0x20910065, u8, - /// Output rate of the UBX-NAV-CLOCK message on port SPI - MsgoutUbxNavClockSpi, 0x20910069, u8, - /// Output rate of the UBX-NAV-CLOCK message on port UART1 - MsgoutUbxNavClockUart1, 0x20910066, u8, - /// Output rate of the UBX-NAV-CLOCK message on port UART2 - MsgoutUbxNavClockUart2, 0x20910067, u8, - /// Output rate of the UBX-NAV-CLOCK message on port USB - MsgoutUbxNavClockUsb, 0x20910068, u8, - /// Output rate of the UBX-NAV-DOP message on port I2C - MsgoutUbxNavDopI2c, 0x20910038, u8, - /// Output rate of the UBX-NAV-DOP message on port SPI - MsgoutUbxNavDopSpi, 0x2091003c, u8, - /// Output rate of the UBX-NAV-DOP message on port UART1 - MsgoutUbxNavDopUart1, 0x20910039, u8, - /// Output rate of the UBX-NAV-DOP message on port UART2 - MsgoutUbxNavDopUart2, 0x2091003a, u8, - /// Output rate of the UBX-NAV-DOP message on port USB - MsgoutUbxNavDopUsb, 0x2091003b, u8, - /// Output rate of the UBX-NAV-EOE message on port I2C - MsgoutUbxNavEoeI2c, 0x2091015f, u8, - /// Output rate of the UBX-NAV-EOE message on port SPI - MsgoutUbxNavEoeSpi, 0x20910163, u8, - /// Output rate of the UBX-NAV-EOE message on port UART1 - MsgoutUbxNavEoeUart1, 0x20910160, u8, - /// Output rate of the UBX-NAV-EOE message on port UART2 - MsgoutUbxNavEoeUart2, 0x20910161, u8, - /// Output rate of the UBX-NAV-EOE message on port USB - MsgoutUbxNavEoeUsb, 0x20910162, u8, - /// Output rate of the UBX-NAVGEOFENCE message on port I2C - MsgoutUbxNavGeofenceI2c, 0x209100a1, u8, - /// Output rate of the UBX-NAVGEOFENCE message on port SPI - MsgoutUbxNavGeofenceSpi, 0x209100a5, u8, - /// Output rate of the UBX-NAVGEOFENCE message on port UART1 - MsgoutUbxNavGeofenceUart1, 0x209100a2, u8, - /// Output rate of the UBX-NAVGEOFENCE message on port UART2 - MsgoutUbxNavGeofenceUart2, 0x209100a3, u8, - /// Output rate of the UBX-NAVGEOFENCE message on port USB - MsgoutUbxNavGeofenceUsb, 0x209100a4, u8, - /// Output rate of the UBX-NAVHPPOSECEF message on port I2C - MsgoutUbxNavHpPosEcefI2c, 0x2091002e, u8, - /// Output rate of the UBX-NAVHPPOSECEF message on port SPI - MsgoutUbxNavHpPosEcefSpi, 0x20910032, u8, - /// Output rate of the UBX-NAVHPPOSECEF message on port UART1 - MsgoutUbxNavHpPosEcefUart1, 0x2091002f, u8, - /// Output rate of the UBX-NAVHPPOSECEF message on port UART2 - MsgoutUbxNavHpPosEcefUart2, 0x20910030, u8, - /// Output rate of the UBX-NAVHPPOSECEF message on port USB - MsgoutUbxNavHpPosEcefUsb, 0x20910031, u8, - /// Output rate of the UBX-NAVHPPOSLLH message on port I2C - MsgoutUbxNavHpPosllhI2c, 0x20910033, u8, - /// Output rate of the UBX-NAVHPPOSLLH message on port SPI - MsgoutUbxNavHpPosllhSpi, 0x20910037, u8, - /// Output rate of the UBX-NAVHPPOSLLH message on port UART1 - MsgoutUbxNavHpPosllhUart1, 0x20910034, u8, - /// Output rate of the UBX-NAVHPPOSLLH message on port UART2 - MsgoutUbxNavHpPosllhUart2, 0x20910035, u8, - /// Output rate of the UBX-NAVHPPOSLLH message on port USB - MsgoutUbxNavHpPosllhUsb, 0x20910036, u8, - /// Output rate of the UBX-NAV-ODO message on port I2C - MsgoutUbxNavOdoI2c, 0x2091007e, u8, - /// Output rate of the UBX-NAV-ODO message on port SPI - MsgoutUbxNavOdoSpi, 0x20910082, u8, - /// Output rate of the UBX-NAV-ODO message on port UART1 - MsgoutUbxNavOdoUart1, 0x2091007f, u8, - /// Output rate of the UBX-NAV-ODO message on port UART2 - MsgoutUbxNavOdoUart2, 0x20910080, u8, - /// Output rate of the UBX-NAV-ODO message on port USB - MsgoutUbxNavOdoUsb, 0x20910081, u8, - /// Output rate of the UBX-NAV-ORB message on port I2C - MsgoutUbxNavOrbI2c, 0x20910010, u8, - /// Output rate of the UBX-NAV-ORB message on port SPI - MsgoutUbxNavOrbSpi, 0x20910014, u8, - /// Output rate of the UBX-NAV-ORB message on port UART1 - MsgoutUbxNavOrbUart1, 0x20910011, u8, - /// Output rate of the UBX-NAV-ORB message on port UART2 - MsgoutUbxNavOrbUart2, 0x20910012, u8, - /// Output rate of the UBX-NAV-ORB message on port USB - MsgoutUbxNavOrbUsb, 0x20910013, u8, - /// Output rate of the UBX-NAV-POSECEF message on port I2C - MsgoutUbxNavPosEcefI2c, 0x20910024, u8, - /// Output rate of the UBX-NAV-POSECEF message on port SPI - MsgoutUbxNavPosEcefSpi, 0x20910028, u8, - /// Output rate of the UBX-NAV-POSECEF message on port UART1 - MsgoutUbxNavPosEcefUart1, 0x20910025, u8, - /// Output rate of the UBX-NAV-POSECEF message on port UART2 - MsgoutUbxNavPosEcefUart2, 0x20910026, u8, - /// Output rate of the UBX-NAV-POSECEF message on port USB - MsgoutUbxNavPosEcefUsb, 0x20910027, u8, - /// Output rate of the UBX-NAV-POSLLH message on port I2C - MsgoutUbxNavPosLlhI2c, 0x20910029, u8, - /// Output rate of the UBX-NAV-POSLLH message on port SPI - MsgoutUbxNavPosLlhSpi, 0x2091002d, u8, - /// Output rate of the UBX-NAV-POSLLH message on port UART1 - MsgoutUbxNavPosLlhUart1, 0x2091002a, u8, - /// Output rate of the UBX-NAV-POSLLH message on port UART2 - MsgoutUbxNavPosLlhUart2, 0x2091002b, u8, - /// Output rate of the UBX-NAV-POSLLH message on port USB - MsgoutUbxNavPosLlhUsb, 0x2091002c, u8, - /// Output rate of the UBX-NAV-PVT message on port I2C - MsgoutUbxNavPvtI2c, 0x20910006, u8, - /// Output rate of the UBX-NAV-PVT message on port SPI - MsgoutUbxNavPvtSpi, 0x2091000a, u8, - /// Output rate of the UBX-NAV-PVT message on port UART1 - MsgoutUbxNavPvtUart1, 0x20910007, u8, - /// Output rate of the UBX-NAV-PVT message on port UART2 - MsgoutUbxNavPvtUart2, 0x20910008, u8, - /// Output rate of the UBX-NAV-PVT message on port USB - MsgoutUbxNavPvtUsb, 0x20910009, u8, - /// Output rate of the UBX-NAVRELPOSNED message on port I2C - MsgoutUbxNavRelposnedI2c, 0x2091008d, u8, - /// Output rate of the UBX-NAVRELPOSNED message on port SPI - MsgoutUbxNavRelposnedSpi, 0x20910091, u8, - /// Output rate of the UBX-NAVRELPOSNED message on port UART1 - MsgoutUbxNavRelposnedUart1, 0x2091008e, u8, - /// Output rate of the UBX-NAVRELPOSNED message on port UART2 - MsgoutUbxNavRelposnedUart2, 0x2091008f, u8, - /// Output rate of the UBX-NAVRELPOSNED message on port USB - MsgoutUbxNavRelposnedUsb, 0x20910090, u8, - /// Output rate of the UBX-NAV-SAT message on port I2C - MsgoutUbxNavSatI2c, 0x20910015, u8, - /// Output rate of the UBX-NAV-SAT message on port SPI - MsgoutUbxNavSatSpi, 0x20910019, u8, - /// Output rate of the UBX-NAV-SAT message on port UART1 - MsgoutUbxNavSatUart1, 0x20910016, u8, - /// Output rate of the UBX-NAV-SAT message on port UART2 - MsgoutUbxNavSatUart2, 0x20910017, u8, - /// Output rate of the UBX-NAV-SAT message on port USB - MsgoutUbxNavSatUsb, 0x20910018, u8, - /// Output rate of the UBX-NAV-SIG message on port I2C - MsgoutUbxNavSigI2c, 0x20910345, u8, - /// Output rate of the UBX-NAV-SIG message on port SPI - MsgoutUbxNavSigSpi, 0x20910349, u8, - /// Output rate of the UBX-NAV-SIG message on port UART1 - MsgoutUbxNavSigUart1, 0x20910346, u8, - /// Output rate of the UBX-NAV-SIG message on port UART2 - MsgoutUbxNavSigUart2, 0x20910347, u8, - /// Output rate of the UBX-NAV-SIG message on port USB - MsgoutUbxNavSigUsb, 0x20910348, u8, - /// Output rate of the UBX-NAV-STATUS message on port I2C - MsgoutUbxNavStatusI2c, 0x2091001a, u8, - /// Output rate of the UBX-NAV-STATUS message on port SPI - MsgoutUbxNavStatusSpi, 0x2091001e, u8, - /// Output rate of the UBX-NAV-STATUS message on port UART1 - MsgoutUbxNavStatusUart1, 0x2091001b, u8, - /// Output rate of the UBX-NAV-STATUS message on port UART2 - MsgoutUbxNavStatusUart2, 0x2091001c, u8, - /// Output rate of the UBX-NAV-STATUS message on port USB - MsgoutUbxNavStatusUsb, 0x2091001d, u8, - /// Output rate of the UBX-NAV-SVIN message on port I2C - MsgoutUbxNavSvinI2c, 0x20910088, u8, - /// Output rate of the UBX-NAV-SVIN message on port SPI - MsgoutUbxNavSvinSpi, 0x2091008c, u8, - /// Output rate of the UBX-NAV-SVIN message on port UART1 - MsgoutUbxNavSvinUart1, 0x20910089, u8, - /// Output rate of the UBX-NAV-SVIN message on port UART2 - MsgoutUbxNavSvinUart2, 0x2091008a, u8, - /// Output rate of the UBX-NAV-SVIN message on port USB - MsgoutUbxNavSvinUsb, 0x2091008b, u8, - /// Output rate of the UBX-NAV-TIMEBDS message on port I2C - MsgoutUbxNavTimeBdsI2c, 0x20910051, u8, - /// Output rate of the UBX-NAV-TIMEBDS message on port SPI - MsgoutUbxNavTimeBdsSpi, 0x20910055, u8, - /// Output rate of the UBX-NAV-TIMEBDS message on port UART1 - MsgoutUbxNavTimeBdsUart1, 0x20910052, u8, - /// Output rate of the UBX-NAV-TIMEBDS message on port UART2 - MsgoutUbxNavTimeBdsUart2, 0x20910053, u8, - /// Output rate of the UBX-NAV-TIMEBDS message on port USB - MsgoutUbxNavTimeBdsUsb, 0x20910054, u8, - /// Output rate of the UBX-NAVTIMEGAL message on port I2C - MsgoutUbxNavTimeGalI2c, 0x20910056, u8, - /// Output rate of the UBX-NAVTIMEGAL message on port SPI - MsgoutUbxNavTimeGalSpi, 0x2091005a, u8, - /// Output rate of the UBX-NAVTIMEGAL message on port UART1 - MsgoutUbxNavTimeGalUart1, 0x20910057, u8, - /// Output rate of the UBX-NAVTIMEGAL message on port UART2 - MsgoutUbxNavTimeGalUart2, 0x20910058, u8, - /// Output rate of the UBX-NAVTIMEGAL message on port USB - MsgoutUbxNavTimeGalUsb, 0x20910059, u8, - /// Output rate of the UBX-NAVTIMEGLO message on port I2C - MsgoutUbxNavTimeGloI2c, 0x2091004c, u8, - /// Output rate of the UBX-NAVTIMEGLO message on port SPI - MsgoutUbxNavTimeGloSpi, 0x20910050, u8, - /// Output rate of the UBX-NAVTIMEGLO message on port UART1 - MsgoutUbxNavTimeGloUart1, 0x2091004d, u8, - /// Output rate of the UBX-NAVTIMEGLO message on port UART2 - MsgoutUbxNavTimeGloUart2, 0x2091004e, u8, - /// Output rate of the UBX-NAVTIMEGLO message on port USB - MsgoutUbxNavTimeGloUsb, 0x2091004f, u8, - /// Output rate of the UBX-NAV-TIMEGPS message on port I2C - MsgoutUbxNavTimeGpsI2c, 0x20910047, u8, - /// Output rate of the UBX-NAV-TIMEGPS message on port SPI - MsgoutUbxNavTimeGpsSpi, 0x2091004b, u8, - /// Output rate of the UBX-NAV-TIMEGPS message on port UART1 - MsgoutUbxNavTimeGpsUart1, 0x20910048, u8, - /// Output rate of the UBX-NAV-TIMEGPS message on port UART2 - MsgoutUbxNavTimeGpsUart2, 0x20910049, u8, - /// Output rate of the UBX-NAV-TIMEGPS message on port USB - MsgoutUbxNavTimeGpsUsb, 0x2091004a, u8, - /// Output rate of the UBX-NAV-TIMELS message on port I2C - MsgoutUbxNavTimeLsI2c, 0x20910060, u8, - /// Output rate of the UBX-NAV-TIMELS message on port SPI - MsgoutUbxNavTimeLsSpi, 0x20910064, u8, - /// Output rate of the UBX-NAV-TIMELS message on port UART1 - MsgoutUbxNavTimeLsUart1, 0x20910061, u8, - /// Output rate of the UBX-NAV-TIMELS message on port UART2 - MsgoutUbxNavTimeLsUart2, 0x20910062, u8, - /// Output rate of the UBX-NAV-TIMELS message on port USB - MsgoutUbxNavTimeLsUsb, 0x20910063, u8, - /// Output rate of the UBX-NAVTIMEUTC message on port I2C - MsgoutUbxNavTimeUtcI2c, 0x2091005b, u8, - /// Output rate of the UBX-NAVTIMEUTC message on port SPI - MsgoutUbxNavTimeUtcSpi, 0x2091005f, u8, - /// Output rate of the UBX-NAVTIMEUTC message on port UART1 - MsgoutUbxNavTimeUtcUart1, 0x2091005c, u8, - /// Output rate of the UBX-NAVTIMEUTC message on port UART2 - MsgoutUbxNavTimeUtcUart2, 0x2091005d, u8, - /// Output rate of the UBX-NAVTIMEUTC message on port USB - MsgoutUbxNavTimeUtcUsb, 0x2091005e, u8, - /// Output rate of the UBX-NAV-VELECEF message on port I2C - MsgoutUbxNavVelEcefI2c, 0x2091003d, u8, - /// Output rate of the UBX-NAV-VELECEF message on port SPI - MsgoutUbxNavVelEcefSpi, 0x20910041, u8, - /// Output rate of the UBX-NAV-VELECEF message on port UART1 - MsgoutUbxNavVelEcefUart1, 0x2091003e, u8, - /// Output rate of the UBX-NAV-VELECEF message on port UART2 - MsgoutUbxNavVelEcefUart2, 0x2091003f, u8, - /// Output rate of the UBX-NAV-VELECEF message on port USB - MsgoutUbxNavVelEcefUsb, 0x20910040, u8, - /// Output rate of the UBX-NAV-VELNED message on port I2C - MsgoutUbxNavVelNedI2c, 0x20910042, u8, - /// Output rate of the UBX-NAV-VELNED message on port SPI - MsgoutUbxNavVelNedSpi, 0x20910046, u8, - /// Output rate of the UBX-NAV-VELNED message on port UART1 - MsgoutUbxNavVelNedUart1, 0x20910043, u8, - /// Output rate of the UBX-NAV-VELNED message on port UART2 - MsgoutUbxNavVelNedUart2, 0x20910044, u8, - /// Output rate of the UBX-NAV-VELNED message on port USB - MsgoutUbxNavVelNedUsb, 0x20910045, u8, - /// Output rate of the UBX-RXM-MEASX message on port I2C - MsgoutUbxRxmMeasxI2c, 0x20910204, u8, - /// Output rate of the UBX-RXM-MEASX message on port SPI - MsgoutUbxRxmMeasxSpi, 0x20910208, u8, - /// Output rate of the UBX-RXM-MEASX message on port UART1 - MsgoutUbxRxmMeasxUart1, 0x20910205, u8, - /// Output rate of the UBX-RXM-MEASX message on port UART2 - MsgoutUbxRxmMeasxUart2, 0x20910206, u8, - /// Output rate of the UBX-RXM-MEASX message on port USB - MsgoutUbxRxmMeasxUsb, 0x20910207, u8, - /// Output rate of the UBX-RXM-RAWX message on port I2C - MsgoutUbxRxmRawxI2c, 0x209102a4, u8, - /// Output rate of the UBX-RXM-RAWX message on port SPI - MsgoutUbxRxmRawxSpi, 0x209102a8, u8, - /// Output rate of the UBX-RXM-RAWX message on port UART1 - MsgoutUbxRxmRawxUart1, 0x209102a5, u8, - /// Output rate of the UBX-RXM-RAWX message on port UART2 - MsgoutUbxRxmRawxUart2, 0x209102a6, u8, - /// Output rate of the UBX-RXM-RAWX message on port USB - MsgoutUbxRxmRawxUsb, 0x209102a7, u8, - /// Output rate of the UBX-RXM-RLM message on port I2C - MsgoutUbxRxmRlmI2c, 0x2091025e, u8, - /// Output rate of the UBX-RXM-RLM message on port SPI - MsgoutUbxRxmRlmSpi, 0x20910262, u8, - /// Output rate of the UBX-RXM-RLM message on port UART1 - MsgoutUbxRxmRlmUart1, 0x2091025f, u8, - /// Output rate of the UBX-RXM-RLM message on port UART2 - MsgoutUbxRxmRlmUart2, 0x20910260, u8, - /// Output rate of the UBX-RXM-RLM message on port USB - MsgoutUbxRxmRlmUsb, 0x20910261, u8, - /// Output rate of the UBX-RXM-RTCM message on port I2C - MsgoutUbxRxmRtcmI2c, 0x20910268, u8, - /// Output rate of the UBX-RXM-RTCM message on port SPI - MsgoutUbxRxmRtcmSpi, 0x2091026c, u8, - /// Output rate of the UBX-RXM-RTCM message on port UART1 - MsgoutUbxRxmRtcmUart1, 0x20910269, u8, - /// Output rate of the UBX-RXM-RTCM message on port UART2 - MsgoutUbxRxmRtcmUart2, 0x2091026a, u8, - /// Output rate of the UBX-RXM-RTCM message on port USB - MsgoutUbxRxmRtcmUsb, 0x2091026b, u8, - /// Output rate of the UBX-RXM-SFRBX message on port I2C - MsgoutUbxRxmSfrbxI2c, 0x20910231, u8, - /// Output rate of the UBX-RXM-SFRBX message on port SPI - MsgoutUbxRxmSfrbxSpi, 0x20910235, u8, - /// Output rate of the UBX-RXM-SFRBX message on port UART1 - MsgoutUbxRxmSfrbxUart1, 0x20910232, u8, - /// Output rate of the UBX-RXM-SFRBX message on port UART2 - MsgoutUbxRxmSfrbxUart2, 0x20910233, u8, - /// Output rate of the UBX-RXM-SFRBX message on port USB - MsgoutUbxRxmSfrbxUsb, 0x20910234, u8, - /// Output rate of the UBX-TIM-TM2 message on port I2C - MsgoutUbxTimTm2I2c, 0x20910178, u8, - /// Output rate of the UBX-TIM-TM2 message on port SPI - MsgoutUbxTimTm2Spi, 0x2091017c, u8, - /// Output rate of the UBX-TIM-TM2 message on port UART1 - MsgoutUbxTimTm2Uart1, 0x20910179, u8, - /// Output rate of the UBX-TIM-TM2 message on port UART2 - MsgoutUbxTimTm2Uart2, 0x2091017a, u8, - /// Output rate of the UBX-TIM-TM2 message on port USB - MsgoutUbxTimTm2Usb, 0x2091017b, u8, - /// Output rate of the UBX-TIM-TP message on port I2C - MsgoutUbxTimTpI2c, 0x2091017d, u8, - /// Output rate of the UBX-TIM-TP message on port SPI - MsgoutUbxTimTpSpi, 0x20910181, u8, - /// Output rate of the UBX-TIM-TP message on port UART1 - MsgoutUbxTimTpUart1, 0x2091017e, u8, - /// Output rate of the UBX-TIM-TP message on port UART2 - MsgoutUbxTimTpUart2, 0x2091017f, u8, - /// Output rate of the UBX-TIM-TP message on port USB - MsgoutUbxTimTpUsb, 0x20910180, u8, - /// Output rate of the UBX-TIM-VRFY message on port I2C - MsgoutUbxTimVrfyI2c, 0x20910092, u8, - /// Output rate of the UBX-TIM-VRFY message on port SPI - MsgoutUbxTimVrfySpi, 0x20910096, u8, - /// Output rate of the UBX-TIM-VRFY message on port UART1 - MsgoutUbxTimVrfyUart1, 0x20910093, u8, - /// Output rate of the UBX-TIM-VRFY message on port UART2 - MsgoutUbxTimVrfyUart2, 0x20910094, u8, - /// Output rate of the UBX-TIM-VRFY message on port USB - MsgoutUbxTimVrfyUsb, 0x20910095, u8, - - // CFG-SIGNAL-* - SignalGpsEna, 0x1031001f, bool, - SignalGpsL1caEna, 0x10310001, bool, - SignalGpsL2cEna, 0x10310003, bool, - SignalGalEna, 0x10310021, bool, - SignalGalE1Ena, 0x10310007, bool, - SignalGalE5bEna, 0x1031000a, bool, - SignalBdsEna, 0x10310022, bool, - SignalBdsB1Ena, 0x1031000d, bool, - SignalBdsB2Ena, 0x1031000e, bool, - SignalQzssEna, 0x10310024, bool, - SignalQzssL1caEna, 0x10310012, bool, - SignalQzssL2cEna, 0x10310015, bool, - SignalGloEna, 0x10310025, bool, - SignalGloL1Ena, 0x10310018, bool, - SignalGLoL2Ena, 0x1031001a, bool, - - // CFG-TP-* - TpPulseDef, 0x20050023, TpPulse, - TpPulseLengthDef, 0x20050030, TpPulseLength, - TpAntCableDelay, 0x30050001, i16, - TpPeriodTp1, 0x40050002, u32, - TpPeriodLockTp1, 0x40050003, u32, - TpFreqTp1, 0x40050024, u32, - TpFreqLockTp1, 0x40050025, u32, - TpLenTp1, 0x40050004, u32, - TpLenLockTp1, 0x40050005, u32, - TpTp1Ena, 0x10050007, bool, - TpSyncGnssTp1, 0x10050008, bool, - TpUseLockedTp1, 0x10050009, bool, - TpAlignToTowTp1, 0x1005000a, bool, - TpPolTp1, 0x1005000b, bool, - TpTimegridTp1, 0x2005000c, AlignmentToReferenceTime, -} - -#[derive(Debug, Clone, Copy, PartialEq, Eq)] -pub enum TpPulse { - /// Time pulse period - Period = 0, - /// Time pulse frequency - Freq = 1, -} - -#[derive(Debug, Clone, Copy, PartialEq, Eq)] -pub enum TpPulseLength { - /// Time pulse ratio - Ratio = 0, - /// Time pulse length - Length = 1, -} diff --git a/ublox/ublox/src/ubx_packets/packets.rs b/ublox/ublox/src/ubx_packets/packets.rs deleted file mode 100644 index e203d33..0000000 --- a/ublox/ublox/src/ubx_packets/packets.rs +++ /dev/null @@ -1,3741 +0,0 @@ -use crate::cfg_val::CfgVal; -use core::convert::TryInto; -use core::fmt; - -#[cfg(feature = "alloc")] -use alloc::vec::Vec; - -use bitflags::bitflags; -use chrono::prelude::*; -use num_traits::cast::{FromPrimitive, ToPrimitive}; -use num_traits::float::FloatCore; - -use ublox_derive::{ - define_recv_packets, ubx_extend, ubx_extend_bitflags, ubx_packet_recv, ubx_packet_recv_send, - ubx_packet_send, -}; - -use crate::error::{MemWriterError, ParserError}; -#[cfg(feature = "serde")] -use crate::serde::ser::SerializeMap; -use crate::ubx_packets::packets::mon_ver::is_cstr_valid; - -use super::{ - ubx_checksum, MemWriter, Position, UbxChecksumCalc, UbxPacketCreator, UbxPacketMeta, - UbxUnknownPacketRef, SYNC_CHAR_1, SYNC_CHAR_2, -}; - -/// Used to help serialize the packet's fields flattened within a struct containing the msg_id and class fields, but -/// without using the serde FlatMapSerializer which requires alloc. -#[cfg(feature = "serde")] -pub(crate) trait SerializeUbxPacketFields { - fn serialize_fields(&self, serializer: &mut S) -> Result<(), S::Error> - where - S: serde::ser::SerializeMap; -} - -/// Geodetic Position Solution -#[ubx_packet_recv] -#[ubx(class = 1, id = 2, fixed_payload_len = 28)] -struct NavPosLlh { - /// GPS Millisecond Time of Week - itow: u32, - - /// Longitude - #[ubx(map_type = f64, scale = 1e-7, alias = lon_degrees)] - lon: i32, - - /// Latitude - #[ubx(map_type = f64, scale = 1e-7, alias = lat_degrees)] - lat: i32, - - /// Height above Ellipsoid - #[ubx(map_type = f64, scale = 1e-3)] - height_meters: i32, - - /// Height above mean sea level - #[ubx(map_type = f64, scale = 1e-3)] - height_msl: i32, - - /// Horizontal Accuracy Estimate - #[ubx(map_type = f64, scale = 1e-3)] - h_ack: u32, - - /// Vertical Accuracy Estimate - #[ubx(map_type = f64, scale = 1e-3)] - v_acc: u32, -} - -/// Velocity Solution in NED -#[ubx_packet_recv] -#[ubx(class = 1, id = 0x12, fixed_payload_len = 36)] -struct NavVelNed { - /// GPS Millisecond Time of Week - itow: u32, - - /// north velocity (m/s) - #[ubx(map_type = f64, scale = 1e-2)] - vel_north: i32, - - /// east velocity (m/s) - #[ubx(map_type = f64, scale = 1e-2)] - vel_east: i32, - - /// down velocity (m/s) - #[ubx(map_type = f64, scale = 1e-2)] - vel_down: i32, - - /// Speed 3-D (m/s) - #[ubx(map_type = f64, scale = 1e-2)] - speed_3d: u32, - - /// Ground speed (m/s) - #[ubx(map_type = f64, scale = 1e-2)] - ground_speed: u32, - - /// Heading of motion 2-D (degrees) - #[ubx(map_type = f64, scale = 1e-5, alias = heading_degrees)] - heading: i32, - - /// Speed Accuracy Estimate (m/s) - #[ubx(map_type = f64, scale = 1e-2)] - speed_accuracy_estimate: u32, - - /// Course / Heading Accuracy Estimate (degrees) - #[ubx(map_type = f64, scale = 1e-5)] - course_heading_accuracy_estimate: u32, -} - -/// High Precision Geodetic Position Solution -#[ubx_packet_recv] -#[ubx(class = 0x01, id = 0x14, fixed_payload_len = 36)] -struct NavHpPosLlh { - /// Message version (0 for protocol version 27) - version: u8, - - reserved1: [u8; 3], - - /// GPS Millisecond Time of Week - itow: u32, - - /// Longitude (deg) - #[ubx(map_type = f64, scale = 1e-7, alias = lon_degrees)] - lon: i32, - - /// Latitude (deg) - #[ubx(map_type = f64, scale = 1e-7, alias = lat_degrees)] - lat: i32, - - /// Height above Ellipsoid (m) - #[ubx(map_type = f64, scale = 1e-3)] - height_meters: i32, - - /// Height above mean sea level (m) - #[ubx(map_type = f64, scale = 1e-3)] - height_msl: i32, - - /// High precision component of longitude - /// Must be in the range -99..+99 - /// Precise longitude in deg * 1e-7 = lon + (lonHp * 1e-2) - #[ubx(map_type = f64, scale = 1e-9, alias = lon_hp_degrees)] - lon_hp: i8, - - /// High precision component of latitude - /// Must be in the range -99..+99 - /// Precise latitude in deg * 1e-7 = lat + (latHp * 1e-2) - #[ubx(map_type = f64, scale = 1e-9, alias = lat_hp_degrees)] - lat_hp: i8, - - /// High precision component of height above ellipsoid - /// Must be in the range -9..+9 - /// Precise height in mm = height + (heightHp * 0.1) - #[ubx(map_type = f64, scale = 1e-1)] - height_hp_meters: i8, - - /// High precision component of height above mean sea level - /// Must be in range -9..+9 - /// Precise height in mm = hMSL + (hMSLHp * 0.1) - #[ubx(map_type = f64, scale = 1e-1)] - height_hp_msl: i8, - - /// Horizontal accuracy estimate (mm) - #[ubx(map_type = f64, scale = 1e-1)] - horizontal_accuracy: u32, - - /// Vertical accuracy estimate (mm) - #[ubx(map_type = f64, scale = 1e-1)] - vertical_accuracy: u32, -} - -#[ubx_extend_bitflags] -#[ubx(from, into_raw, rest_reserved)] -bitflags! { - #[derive(Default, Debug)] - pub struct NavHpPosEcefFlags: u8 { - const INVALID_ECEF = 1; - - } -} - -/// High Precision Geodetic Position Solution (ECEF) -#[ubx_packet_recv] -#[ubx(class = 0x01, id = 0x13, fixed_payload_len = 28)] -struct NavHpPosEcef { - /// Message version (0 for protocol version 27) - version: u8, - - reserved1: [u8; 3], - - /// GPS Millisecond Time of Week - itow: u32, - - /// ECEF X coordinate - #[ubx(map_type = f64, alias = ecef_x_cm)] - ecef_x: i32, - - /// ECEF Y coordinate - #[ubx(map_type = f64, alias = ecef_y_cm)] - ecef_y: i32, - - /// ECEF Z coordinate - #[ubx(map_type = f64, alias = ecef_z_cm)] - ecef_z: i32, - - /// High precision component of X - /// Must be in the range -99..+99 - /// Precise coordinate in cm = ecef_x + (ecef_x_hp * 1e-2). - #[ubx(map_type = f64, scale = 1e-1, alias = ecef_x_hp_mm)] - ecef_x_hp: i8, - - /// High precision component of Y - /// Must be in the range -99..+99 - /// 9. Precise coordinate in cm = ecef_y + (ecef_y_hp * 1e-2). - #[ubx(map_type = f64, scale = 1e-1, alias = ecef_y_hp_mm)] - ecef_y_hp: i8, - - /// High precision component of Z - /// Must be in the range -99..+99 - /// Precise coordinate in cm = ecef_z + (ecef_z_hp * 1e-2). - #[ubx(map_type = f64, scale = 1e-1, alias = ecef_z_hp_mm)] - ecef_z_hp: i8, - - #[ubx(map_type = NavHpPosEcefFlags)] - flags: u8, - - /// Horizontal accuracy estimate (mm) - #[ubx(map_type = f64, scale = 1e-1)] - p_acc: u32, -} - -/// Navigation Position Velocity Time Solution -#[ubx_packet_recv] -#[ubx(class = 1, id = 0x07, fixed_payload_len = 92)] -struct NavPvt { - /// GPS Millisecond Time of Week - itow: u32, - year: u16, - month: u8, - day: u8, - hour: u8, - min: u8, - sec: u8, - valid: u8, - time_accuracy: u32, - nanosecond: i32, - - /// GNSS fix Type - #[ubx(map_type = GpsFix)] - fix_type: u8, - #[ubx(map_type = NavPvtFlags)] - flags: u8, - #[ubx(map_type = NavPvtFlags2)] - flags2: u8, - num_satellites: u8, - #[ubx(map_type = f64, scale = 1e-7, alias = lon_degrees)] - lon: i32, - #[ubx(map_type = f64, scale = 1e-7, alias = lat_degrees)] - lat: i32, - - /// Height above Ellipsoid - #[ubx(map_type = f64, scale = 1e-3)] - height_meters: i32, - - /// Height above mean sea level - #[ubx(map_type = f64, scale = 1e-3)] - height_msl: i32, - horiz_accuracy: u32, - vert_accuracy: u32, - - /// north velocity (m/s) - #[ubx(map_type = f64, scale = 1e-3)] - vel_north: i32, - - /// east velocity (m/s) - #[ubx(map_type = f64, scale = 1e-3)] - vel_east: i32, - - /// down velocity (m/s) - #[ubx(map_type = f64, scale = 1e-3)] - vel_down: i32, - - /// Ground speed (m/s) - #[ubx(map_type = f64, scale = 1e-3)] - ground_speed: u32, - - /// Heading of motion 2-D (degrees) - #[ubx(map_type = f64, scale = 1e-5, alias = heading_degrees)] - heading: i32, - - /// Speed Accuracy Estimate (m/s) - #[ubx(map_type = f64, scale = 1e-3)] - speed_accuracy_estimate: u32, - - /// Heading accuracy estimate (both motionand vehicle) (degrees) - #[ubx(map_type = f64, scale = 1e-5)] - heading_accuracy_estimate: u32, - - /// Position DOP - pdop: u16, - reserved1: [u8; 6], - #[ubx(map_type = f64, scale = 1e-5, alias = heading_of_vehicle_degrees)] - heading_of_vehicle: i32, - #[ubx(map_type = f64, scale = 1e-2, alias = magnetic_declination_degrees)] - magnetic_declination: i16, - #[ubx(map_type = f64, scale = 1e-2, alias = magnetic_declination_accuracy_degrees)] - magnetic_declination_accuracy: u16, -} - -#[ubx_extend_bitflags] -#[ubx(from, rest_reserved)] -bitflags! { - /// Fix status flags for `NavPvt` - #[derive(Debug)] - pub struct NavPvtFlags: u8 { - /// position and velocity valid and within DOP and ACC Masks - const GPS_FIX_OK = 1; - /// DGPS used - const DIFF_SOLN = 2; - /// 1 = heading of vehicle is valid - const HEAD_VEH_VALID = 0x20; - const CARR_SOLN_FLOAT = 0x40; - const CARR_SOLN_FIXED = 0x80; - } -} - -#[ubx_extend_bitflags] -#[ubx(from, rest_reserved)] -bitflags! { - /// Additional flags for `NavPvt` - #[derive(Debug)] - pub struct NavPvtFlags2: u8 { - /// 1 = information about UTC Date and Time of Day validity confirmation - /// is available. This flag is only supported in Protocol Versions - /// 19.00, 19.10, 20.10, 20.20, 20.30, 22.00, 23.00, 23.01,27 and 28. - const CONFIRMED_AVAI = 0x20; - /// 1 = UTC Date validity could be confirmed - /// (confirmed by using an additional independent source) - const CONFIRMED_DATE = 0x40; - /// 1 = UTC Time of Day could be confirmed - /// (confirmed by using an additional independent source) - const CONFIRMED_TIME = 0x80; - } -} - -/// Receiver Navigation Status -#[ubx_packet_recv] -#[ubx(class = 1, id = 3, fixed_payload_len = 16)] -struct NavStatus { - /// GPS Millisecond Time of Week - itow: u32, - - /// GPS fix Type, this value does not qualify a fix as - - /// valid and within the limits - #[ubx(map_type = GpsFix)] - fix_type: u8, - - /// Navigation Status Flags - #[ubx(map_type = NavStatusFlags)] - flags: u8, - - /// Fix Status Information - #[ubx(map_type = FixStatusInfo)] - fix_stat: u8, - - /// further information about navigation output - #[ubx(map_type = NavStatusFlags2)] - flags2: u8, - - /// Time to first fix (millisecond time tag) - time_to_first_fix: u32, - - /// Milliseconds since Startup / Reset - uptime_ms: u32, -} - -/// Dilution of precision -#[ubx_packet_recv] -#[ubx(class = 1, id = 4, fixed_payload_len = 18)] -struct NavDop { - /// GPS Millisecond Time of Week - itow: u32, - #[ubx(map_type = f32, scale = 1e-2)] - geometric_dop: u16, - #[ubx(map_type = f32, scale = 1e-2)] - position_dop: u16, - #[ubx(map_type = f32, scale = 1e-2)] - time_dop: u16, - #[ubx(map_type = f32, scale = 1e-2)] - vertical_dop: u16, - #[ubx(map_type = f32, scale = 1e-2)] - horizontal_dop: u16, - #[ubx(map_type = f32, scale = 1e-2)] - northing_dop: u16, - #[ubx(map_type = f32, scale = 1e-2)] - easting_dop: u16, -} - -/// End of Epoch Marker -#[ubx_packet_recv] -#[ubx(class = 0x01, id = 0x61, fixed_payload_len = 4)] -struct NavEoe { - /// GPS time of week for navigation epoch - itow: u32, -} - -/// Navigation Solution Information -#[ubx_packet_recv] -#[ubx(class = 1, id = 6, fixed_payload_len = 52)] -struct NavSolution { - /// GPS Millisecond Time of Week - itow: u32, - - /// Fractional part of iTOW (range: +/-500000). - ftow_ns: i32, - - /// GPS week number of the navigation epoch - week: i16, - - /// GPS fix Type - #[ubx(map_type = GpsFix)] - fix_type: u8, - - /// Navigation Status Flags - #[ubx(map_type = NavStatusFlags)] - flags: u8, - - /// ECEF X coordinate (meters) - #[ubx(map_type = f64, scale = 1e-2)] - ecef_x: i32, - - /// ECEF Y coordinate (meters) - #[ubx(map_type = f64, scale = 1e-2)] - ecef_y: i32, - - /// ECEF Z coordinate (meters) - #[ubx(map_type = f64, scale = 1e-2)] - ecef_z: i32, - - /// 3D Position Accuracy Estimate - #[ubx(map_type = f64, scale = 1e-2)] - position_accuracy_estimate: u32, - - /// ECEF X velocity (m/s) - #[ubx(map_type = f64, scale = 1e-2)] - ecef_vx: i32, - - /// ECEF Y velocity (m/s) - #[ubx(map_type = f64, scale = 1e-2)] - ecef_vy: i32, - - /// ECEF Z velocity (m/s) - #[ubx(map_type = f64, scale = 1e-2)] - ecef_vz: i32, - - /// Speed Accuracy Estimate - #[ubx(map_type = f64, scale = 1e-2)] - speed_accuracy_estimate: u32, - - /// Position DOP - #[ubx(map_type = f32, scale = 1e-2)] - pdop: u16, - reserved1: u8, - - /// Number of SVs used in Nav Solution - num_sv: u8, - reserved2: [u8; 4], -} - -/// GPS fix Type -#[ubx_extend] -#[ubx(from, rest_reserved)] -#[repr(u8)] -#[derive(Debug, Copy, Clone, PartialEq, Eq)] -pub enum GpsFix { - NoFix = 0, - DeadReckoningOnly = 1, - Fix2D = 2, - Fix3D = 3, - GPSPlusDeadReckoning = 4, - TimeOnlyFix = 5, -} - -#[ubx_extend_bitflags] -#[ubx(from, rest_reserved)] -bitflags! { - /// Navigation Status Flags - #[derive(Debug)] - pub struct NavStatusFlags: u8 { - /// position and velocity valid and within DOP and ACC Masks - const GPS_FIX_OK = 1; - /// DGPS used - const DIFF_SOLN = 2; - /// Week Number valid - const WKN_SET = 4; - /// Time of Week valid - const TOW_SET = 8; - } -} - -/// Fix Status Information -#[repr(transparent)] -#[derive(Copy, Clone)] -#[cfg_attr(feature = "serde", derive(serde::Serialize))] -pub struct FixStatusInfo(u8); - -impl FixStatusInfo { - pub const fn has_pr_prr_correction(self) -> bool { - (self.0 & 1) == 1 - } - pub fn map_matching(self) -> MapMatchingStatus { - let bits = (self.0 >> 6) & 3; - match bits { - 0 => MapMatchingStatus::None, - 1 => MapMatchingStatus::Valid, - 2 => MapMatchingStatus::Used, - 3 => MapMatchingStatus::Dr, - _ => unreachable!(), - } - } - pub const fn from(x: u8) -> Self { - Self(x) - } -} - -impl fmt::Debug for FixStatusInfo { - fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { - f.debug_struct("FixStatusInfo") - .field("has_pr_prr_correction", &self.has_pr_prr_correction()) - .field("map_matching", &self.map_matching()) - .finish() - } -} - -#[derive(Copy, Clone, Debug)] -#[cfg_attr(feature = "serde", derive(serde::Serialize))] -pub enum MapMatchingStatus { - None = 0, - /// valid, i.e. map matching data was received, but was too old - Valid = 1, - /// used, map matching data was applied - Used = 2, - /// map matching was the reason to enable the dead reckoning - /// gpsFix type instead of publishing no fix - Dr = 3, -} - -/// Further information about navigation output -/// Only for FW version >= 7.01; undefined otherwise -#[ubx_extend] -#[ubx(from, rest_reserved)] -#[repr(u8)] -#[derive(Debug, Copy, Clone)] -enum NavStatusFlags2 { - Acquisition = 0, - Tracking = 1, - PowerOptimizedTracking = 2, - Inactive = 3, -} - -#[repr(transparent)] -#[derive(Copy, Clone)] -#[cfg_attr(feature = "serde", derive(serde::Serialize))] -pub struct NavSatSvFlags(u32); - -impl NavSatSvFlags { - pub fn quality_ind(self) -> NavSatQualityIndicator { - let bits = self.0 & 0x7; - match bits { - 0 => NavSatQualityIndicator::NoSignal, - 1 => NavSatQualityIndicator::Searching, - 2 => NavSatQualityIndicator::SignalAcquired, - 3 => NavSatQualityIndicator::SignalDetected, - 4 => NavSatQualityIndicator::CodeLock, - 5..=7 => NavSatQualityIndicator::CarrierLock, - _ => { - panic!("Unexpected 3-bit bitfield value {}!", bits); - }, - } - } - - pub fn sv_used(self) -> bool { - (self.0 >> 3) & 0x1 != 0 - } - - pub fn health(self) -> NavSatSvHealth { - let bits = (self.0 >> 4) & 0x3; - match bits { - 1 => NavSatSvHealth::Healthy, - 2 => NavSatSvHealth::Unhealthy, - x => NavSatSvHealth::Unknown(x as u8), - } - } - - pub fn differential_correction_available(self) -> bool { - (self.0 >> 6) & 0x1 != 0 - } - - pub fn smoothed(self) -> bool { - (self.0 >> 7) & 0x1 != 0 - } - - pub fn orbit_source(self) -> NavSatOrbitSource { - let bits = (self.0 >> 8) & 0x7; - match bits { - 0 => NavSatOrbitSource::NoInfoAvailable, - 1 => NavSatOrbitSource::Ephemeris, - 2 => NavSatOrbitSource::Almanac, - 3 => NavSatOrbitSource::AssistNowOffline, - 4 => NavSatOrbitSource::AssistNowAutonomous, - x => NavSatOrbitSource::Other(x as u8), - } - } - - pub fn ephemeris_available(self) -> bool { - (self.0 >> 11) & 0x1 != 0 - } - - pub fn almanac_available(self) -> bool { - (self.0 >> 12) & 0x1 != 0 - } - - pub fn an_offline_available(self) -> bool { - (self.0 >> 13) & 0x1 != 0 - } - - pub fn an_auto_available(self) -> bool { - (self.0 >> 14) & 0x1 != 0 - } - - pub fn sbas_corr(self) -> bool { - (self.0 >> 16) & 0x1 != 0 - } - - pub fn rtcm_corr(self) -> bool { - (self.0 >> 17) & 0x1 != 0 - } - - pub fn slas_corr(self) -> bool { - (self.0 >> 18) & 0x1 != 0 - } - - pub fn spartn_corr(self) -> bool { - (self.0 >> 19) & 0x1 != 0 - } - - pub fn pr_corr(self) -> bool { - (self.0 >> 20) & 0x1 != 0 - } - - pub fn cr_corr(self) -> bool { - (self.0 >> 21) & 0x1 != 0 - } - - pub fn do_corr(self) -> bool { - (self.0 >> 22) & 0x1 != 0 - } - - pub const fn from(x: u32) -> Self { - Self(x) - } -} - -impl fmt::Debug for NavSatSvFlags { - fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { - f.debug_struct("NavSatSvFlags") - .field("quality_ind", &self.quality_ind()) - .field("sv_used", &self.sv_used()) - .field("health", &self.health()) - .field( - "differential_correction_available", - &self.differential_correction_available(), - ) - .field("smoothed", &self.smoothed()) - .field("orbit_source", &self.orbit_source()) - .field("ephemeris_available", &self.ephemeris_available()) - .field("almanac_available", &self.almanac_available()) - .field("an_offline_available", &self.an_offline_available()) - .field("an_auto_available", &self.an_auto_available()) - .field("sbas_corr", &self.sbas_corr()) - .field("rtcm_corr", &self.rtcm_corr()) - .field("slas_corr", &self.slas_corr()) - .field("spartn_corr", &self.spartn_corr()) - .field("pr_corr", &self.pr_corr()) - .field("cr_corr", &self.cr_corr()) - .field("do_corr", &self.do_corr()) - .finish() - } -} - -#[derive(Copy, Clone, Debug)] -#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] -pub enum NavSatQualityIndicator { - NoSignal, - Searching, - SignalAcquired, - SignalDetected, - CodeLock, - CarrierLock, -} - -#[derive(Copy, Clone, Debug)] -#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] -pub enum NavSatSvHealth { - Healthy, - Unhealthy, - Unknown(u8), -} - -#[derive(Copy, Clone, Debug)] -#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] -pub enum NavSatOrbitSource { - NoInfoAvailable, - Ephemeris, - Almanac, - AssistNowOffline, - AssistNowAutonomous, - Other(u8), -} - -#[ubx_packet_recv] -#[ubx(class = 0x01, id = 0x35, fixed_payload_len = 12)] -struct NavSatSvInfo { - gnss_id: u8, - sv_id: u8, - cno: u8, - elev: i8, - azim: i16, - pr_res: i16, - - #[ubx(map_type = NavSatSvFlags)] - flags: u32, -} - -#[derive(Debug, Clone)] -pub struct NavSatIter<'a> { - data: &'a [u8], - offset: usize, -} - -impl<'a> NavSatIter<'a> { - fn new(data: &'a [u8]) -> Self { - Self { data, offset: 0 } - } - - fn is_valid(bytes: &[u8]) -> bool { - bytes.len() % 12 == 0 - } -} - -impl<'a> core::iter::Iterator for NavSatIter<'a> { - type Item = NavSatSvInfoRef<'a>; - - fn next(&mut self) -> Option { - if self.offset < self.data.len() { - let data = &self.data[self.offset..self.offset + 12]; - self.offset += 12; - Some(NavSatSvInfoRef(data)) - } else { - None - } - } -} - -#[ubx_packet_recv] -#[ubx(class = 0x01, id = 0x35, max_payload_len = 1240)] -struct NavSat { - /// GPS time of week in ms - itow: u32, - - /// Message version, should be 1 - version: u8, - - num_svs: u8, - - reserved: [u8; 2], - - #[ubx( - map_type = NavSatIter, - from = NavSatIter::new, - is_valid = NavSatIter::is_valid, - may_fail, - get_as_ref, - )] - svs: [u8; 0], -} - -/// Odometer solution -#[ubx_packet_recv] -#[ubx(class = 0x01, id = 0x09, fixed_payload_len = 20)] -struct NavOdo { - version: u8, - reserved: [u8; 3], - itow: u32, - distance: u32, - total_distance: u32, - distance_std: u32, -} - -/// Reset odometer -#[ubx_packet_send] -#[ubx(class = 0x01, id = 0x10, fixed_payload_len = 0)] -struct NavResetOdo {} - -/// Configure odometer -#[ubx_packet_recv_send] -#[ubx( - class = 0x06, - id = 0x1E, - fixed_payload_len = 20, - flags = "default_for_builder" -)] -struct CfgOdo { - version: u8, - reserved: [u8; 3], - /// Odometer COG filter flags. See [OdoCogFilterFlags] for details. - #[ubx(map_type = OdoCogFilterFlags)] - flags: u8, - #[ubx(map_type = OdoProfile, may_fail)] - odo_cfg: u8, - reserved2: [u8; 6], - cog_max_speed: u8, - cog_max_pos_acc: u8, - reserved3: [u8; 2], - vel_lp_gain: u8, - cog_lp_gain: u8, - reserved4: [u8; 2], -} - -#[ubx_extend_bitflags] -#[ubx(from, into_raw, rest_reserved)] -bitflags! { - #[derive(Default, Debug)] - pub struct OdoCogFilterFlags: u8 { - /// Odometer enabled flag - const USE_ODO = 0x01; - /// Low-speed COG filter enabled flag - const USE_COG = 0x02; - /// Output low-pass filtered velocity flag - const OUT_LP_VEL = 0x04; - /// Output low-pass filtered heading (COG) flag - const OUT_LP_COG = 0x08; - } -} - -/// Odometer configuration profile -#[derive(Default)] -#[ubx_extend] -#[ubx(from_unchecked, into_raw, rest_error)] -#[repr(u8)] -#[derive(Clone, Copy, Debug)] -pub enum OdoProfile { - #[default] - Running = 0, - Cycling = 1, - Swimming = 2, - Car = 3, - Custom = 4, -} - -/// Configure Jamming interference monitoring -#[ubx_packet_recv_send] -#[ubx(class = 0x06, id = 0x39, fixed_payload_len = 8)] -struct CfgItfm { - /// Interference config Word - #[ubx(map_type = CfgItfmConfig)] - config: u32, - /// Extra settings - #[ubx(map_type = CfgItfmConfig2)] - config2: u32, -} - -#[derive(Debug, Copy, Clone, Default)] -#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] -pub struct CfgItfmConfig { - /// enable interference detection - enable: bool, - /// Broadband jamming detection threshold (dB) - bb_threshold: CfgItfmBbThreshold, - /// CW jamming detection threshold (dB) - cw_threshold: CfgItfmCwThreshold, - /// Reserved algorithm settings - /// should be set to 0x16B156 default value - /// for correct settings - algorithm_bits: CfgItfmAlgoBits, -} - -impl CfgItfmConfig { - pub fn new(enable: bool, bb_threshold: u32, cw_threshold: u32) -> Self { - Self { - enable, - bb_threshold: bb_threshold.into(), - cw_threshold: cw_threshold.into(), - algorithm_bits: CfgItfmAlgoBits::default(), - } - } - - const fn into_raw(self) -> u32 { - (self.enable as u32) << 31 - | self.cw_threshold.into_raw() - | self.bb_threshold.into_raw() - | self.algorithm_bits.into_raw() - } -} - -impl From for CfgItfmConfig { - fn from(cfg: u32) -> Self { - let enable = (cfg & 0x80000000) > 0; - let bb_threshold = CfgItfmBbThreshold::from(cfg); - let cw_threshold = CfgItfmCwThreshold::from(cfg); - let algorithm_bits = CfgItfmAlgoBits::from(cfg); - Self { - enable, - bb_threshold, - cw_threshold, - algorithm_bits, - } - } -} - -#[derive(Debug, Copy, Clone)] -#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] -pub struct CfgItfmBbThreshold(u32); - -impl CfgItfmBbThreshold { - const POSITION: u32 = 0; - const LENGTH: u32 = 4; - const MASK: u32 = (1 << Self::LENGTH) - 1; - const fn into_raw(self) -> u32 { - (self.0 & Self::MASK) << Self::POSITION - } -} - -impl Default for CfgItfmBbThreshold { - fn default() -> Self { - Self(3) // from UBX specifications - } -} - -impl From for CfgItfmBbThreshold { - fn from(thres: u32) -> Self { - Self(thres) - } -} - -#[derive(Debug, Copy, Clone)] -#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] -pub struct CfgItfmCwThreshold(u32); - -impl CfgItfmCwThreshold { - const POSITION: u32 = 4; - const LENGTH: u32 = 5; - const MASK: u32 = (1 << Self::LENGTH) - 1; - const fn into_raw(self) -> u32 { - (self.0 & Self::MASK) << Self::POSITION - } -} - -impl Default for CfgItfmCwThreshold { - fn default() -> Self { - Self(15) // from UBX specifications - } -} - -impl From for CfgItfmCwThreshold { - fn from(thres: u32) -> Self { - Self(thres) - } -} - -#[derive(Debug, Copy, Clone)] -#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] -pub struct CfgItfmAlgoBits(u32); - -impl CfgItfmAlgoBits { - const POSITION: u32 = 9; - const LENGTH: u32 = 22; - const MASK: u32 = (1 << Self::LENGTH) - 1; - const fn into_raw(self) -> u32 { - (self.0 & Self::MASK) << Self::POSITION - } -} - -impl Default for CfgItfmAlgoBits { - fn default() -> Self { - Self(0x16B156) // from UBX specifications - } -} - -impl From for CfgItfmAlgoBits { - fn from(thres: u32) -> Self { - Self(thres) - } -} - -#[derive(Debug, Copy, Clone)] -#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] -pub struct CfgItfmConfig2 { - /// General settings, should be set to - /// 0x31E default value, for correct setting - general: CfgItfmGeneralBits, - /// antenna settings - antenna: CfgItfmAntennaSettings, - /// Set to true to scan auxillary bands on ublox-M8, - /// ignored otherwise - scan_aux_bands: bool, -} - -impl CfgItfmConfig2 { - pub fn new(antenna: CfgItfmAntennaSettings, scan_aux_bands: bool) -> Self { - Self { - general: CfgItfmGeneralBits::default(), - antenna, - scan_aux_bands, - } - } - - const fn into_raw(self) -> u32 { - ((self.scan_aux_bands as u32) << 14) - | self.general.into_raw() - | self.antenna.into_raw() as u32 - } -} - -impl From for CfgItfmConfig2 { - fn from(cfg: u32) -> Self { - let scan_aux_bands = (cfg & 0x4000) > 0; - let general = CfgItfmGeneralBits::from(cfg); - let antenna = CfgItfmAntennaSettings::from(cfg); - Self { - scan_aux_bands, - general, - antenna, - } - } -} - -#[derive(Debug, Copy, Clone)] -#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] -pub struct CfgItfmGeneralBits(u32); - -impl CfgItfmGeneralBits { - const POSITION: u32 = 0; - const LENGTH: u32 = 12; - const MASK: u32 = (1 << Self::LENGTH) - 1; - const fn into_raw(self) -> u32 { - (self.0 & Self::MASK) << Self::POSITION - } -} - -impl Default for CfgItfmGeneralBits { - fn default() -> Self { - Self(0x31E) // from UBX specifications - } -} - -impl From for CfgItfmGeneralBits { - fn from(thres: u32) -> Self { - Self(thres) - } -} - -/// ITFM Antenna settings helps the interference -/// monitoring module -#[derive(Default)] -#[ubx_extend] -#[ubx(from_unchecked, into_raw, rest_error)] -#[repr(u8)] -#[derive(Debug, Copy, Clone)] -#[cfg_attr(feature = "serde", derive(serde::Deserialize))] -pub enum CfgItfmAntennaSettings { - /// Type of Antenna is not known - #[default] - Unknown = 0, - /// Active antenna - Active = 1, - /// Passive antenna - Passive = 2, -} - -impl From for CfgItfmAntennaSettings { - fn from(cfg: u32) -> Self { - let cfg = (cfg & 0x3000) >> 12; - match cfg { - 1 => CfgItfmAntennaSettings::Active, - 2 => CfgItfmAntennaSettings::Passive, - _ => CfgItfmAntennaSettings::Unknown, - } - } -} - -/// Information message conifg -#[ubx_packet_recv_send] -#[ubx( - class = 0x06, - id = 0x2, - fixed_payload_len = 10, - flags = "default_for_builder" -)] -struct CfgInf { - protocol_id: u8, - reserved: [u8; 3], - #[ubx(map_type = CfgInfMask)] - inf_msg_mask_0: u8, - #[ubx(map_type = CfgInfMask)] - inf_msg_mask_1: u8, - #[ubx(map_type = CfgInfMask)] - inf_msg_mask_2: u8, - #[ubx(map_type = CfgInfMask)] - inf_msg_mask_3: u8, - #[ubx(map_type = CfgInfMask)] - inf_msg_mask_4: u8, - #[ubx(map_type = CfgInfMask)] - inf_msg_mask_5: u8, -} - -#[ubx_extend_bitflags] -#[ubx(from, into_raw, rest_reserved)] -bitflags! { - /// `CfgInfMask` parameters bitmask - #[derive(Default, Debug, Clone, Copy)] - pub struct CfgInfMask: u8 { - const ERROR = 0x1; - const WARNING = 0x2; - const NOTICE = 0x4; - const TEST = 0x08; - const DEBUG = 0x10; - } -} - -#[ubx_packet_recv] -#[ubx( - class = 0x4, - id = 0x0, - max_payload_len = 1240, - flags = "default_for_builder" -)] -struct InfError { - #[ubx(map_type = Option<&str>, - may_fail, - is_valid = inf::is_valid, - from = inf::convert_to_str, - get_as_ref)] - message: [u8; 0], -} - -#[ubx_packet_recv] -#[ubx( - class = 0x4, - id = 0x2, - max_payload_len = 1240, - flags = "default_for_builder" -)] -struct InfNotice { - #[ubx(map_type = Option<&str>, - may_fail, - is_valid = inf::is_valid, - from = inf::convert_to_str, - get_as_ref)] - message: [u8; 0], -} - -#[ubx_packet_recv] -#[ubx( - class = 0x4, - id = 0x3, - max_payload_len = 1240, - flags = "default_for_builder" -)] -struct InfTest { - #[ubx(map_type = Option<&str>, - may_fail, - is_valid = inf::is_valid, - from = inf::convert_to_str, - get_as_ref)] - message: [u8; 0], -} - -#[ubx_packet_recv] -#[ubx( - class = 0x4, - id = 0x1, - max_payload_len = 1240, - flags = "default_for_builder" -)] -struct InfWarning { - #[ubx(map_type = Option<&str>, - may_fail, - is_valid = inf::is_valid, - from = inf::convert_to_str, - get_as_ref)] - message: [u8; 0], -} - -#[ubx_packet_recv] -#[ubx( - class = 0x4, - id = 0x4, - max_payload_len = 1240, - flags = "default_for_builder" -)] -struct InfDebug { - #[ubx(map_type = Option<&str>, - may_fail, - is_valid = inf::is_valid, - from = inf::convert_to_str, - get_as_ref)] - message: [u8; 0], -} - -mod inf { - pub(crate) fn convert_to_str(bytes: &[u8]) -> Option<&str> { - match core::str::from_utf8(bytes) { - Ok(msg) => Some(msg), - Err(_) => None, - } - } - - pub(crate) fn is_valid(_bytes: &[u8]) -> bool { - // Validity is checked in convert_to_str - true - } -} - -#[ubx_packet_send] -#[ubx( - class = 0x0B, - id = 0x01, - fixed_payload_len = 48, - flags = "default_for_builder" -)] -struct AidIni { - ecef_x_or_lat: i32, - ecef_y_or_lon: i32, - ecef_z_or_alt: i32, - pos_accuracy: u32, - time_cfg: u16, - week_or_ym: u16, - tow_or_hms: u32, - tow_ns: i32, - tm_accuracy_ms: u32, - tm_accuracy_ns: u32, - clk_drift_or_freq: i32, - clk_drift_or_freq_accuracy: u32, - flags: u32, -} - -impl AidIniBuilder { - pub fn set_position(mut self, pos: Position) -> Self { - self.ecef_x_or_lat = (pos.lat * 10_000_000.0) as i32; - self.ecef_y_or_lon = (pos.lon * 10_000_000.0) as i32; - self.ecef_z_or_alt = (pos.alt * 100.0) as i32; // Height is in centimeters, here - self.flags |= (1 << 0) | (1 << 5); - self - } - - pub fn set_time(mut self, tm: DateTime) -> Self { - self.week_or_ym = (match tm.year_ce() { - (true, yr) => yr - 2000, - (false, _) => { - panic!("AID-INI packet only supports years after 2000"); - }, - } * 100 - + tm.month0()) as u16; - self.tow_or_hms = tm.hour() * 10000 + tm.minute() * 100 + tm.second(); - self.tow_ns = tm.nanosecond() as i32; - self.flags |= (1 << 1) | (1 << 10); - self - } -} - -/// ALP client requests AlmanacPlus data from server -#[ubx_packet_recv] -#[ubx(class = 0x0B, id = 0x32, fixed_payload_len = 16)] -struct AlpSrv { - pub id_size: u8, - pub data_type: u8, - pub offset: u16, - pub size: u16, - pub file_id: u16, - pub data_size: u16, - pub id1: u8, - pub id2: u8, - pub id3: u32, -} - -/// Messages in this class are sent as a result of a CFG message being -/// received, decoded and processed by thereceiver. -#[ubx_packet_recv] -#[ubx(class = 5, id = 1, fixed_payload_len = 2)] -struct AckAck { - /// Class ID of the Acknowledged Message - class: u8, - - /// Message ID of the Acknowledged Message - msg_id: u8, -} - -impl<'a> AckAckRef<'a> { - pub fn is_ack_for(&self) -> bool { - self.class() == T::CLASS && self.msg_id() == T::ID - } -} - -/// Message Not-Acknowledge -#[ubx_packet_recv] -#[ubx(class = 5, id = 0, fixed_payload_len = 2)] -struct AckNak { - /// Class ID of the Acknowledged Message - class: u8, - - /// Message ID of the Acknowledged Message - msg_id: u8, -} - -impl<'a> AckNakRef<'a> { - pub fn is_nak_for(&self) -> bool { - self.class() == T::CLASS && self.msg_id() == T::ID - } -} - -/// Reset Receiver / Clear Backup Data Structures -#[ubx_packet_send] -#[ubx(class = 6, id = 4, fixed_payload_len = 4)] -struct CfgRst { - /// Battery backed RAM sections to clear - #[ubx(map_type = NavBbrMask)] - nav_bbr_mask: u16, - - /// Reset Type - #[ubx(map_type = ResetMode)] - reset_mode: u8, - reserved1: u8, -} - -/// Reset Receiver / Clear Backup Data Structures -#[ubx_packet_recv_send] -#[ubx(class = 6, id = 0x13, fixed_payload_len = 4)] -struct CfgAnt { - /// Antenna flag mask. See [AntFlags] for details. - #[ubx(map_type = AntFlags)] - flags: u16, - /// Antenna pin configuration. See 32.10.1.1 in receiver spec for details. - pins: u16, -} - -#[ubx_extend_bitflags] -#[ubx(from, into_raw, rest_reserved)] -bitflags! { - #[derive(Default, Debug)] - pub struct AntFlags: u16 { - /// Enable supply voltage control signal - const SVCS = 0x01; - /// Enable short circuit detection - const SCD = 0x02; - /// Enable open circuit detection - const OCD = 0x04; - /// Power down on short circuit detection - const PDWN_ON_SCD = 0x08; - /// Enable automatic recovery from short circuit state - const RECOVERY = 0x10; - } -} - -/// TP5: "Time Pulse" Config frame (32.10.38.4) -#[ubx_packet_recv_send] -#[ubx( - class = 0x06, - id = 0x31, - fixed_payload_len = 32, - flags = "default_for_builder" -)] -struct CfgTp5 { - #[ubx(map_type = CfgTp5TimePulseMode, may_fail)] - tp_idx: u8, - version: u8, - reserved1: [u8; 2], - /// Antenna cable delay [ns] - #[ubx(map_type = f32, scale = 1.0)] - ant_cable_delay: i16, - /// RF group delay [ns] - #[ubx(map_type = f32, scale = 1.0)] - rf_group_delay: i16, - /// Frequency in Hz or Period in us, - /// depending on `flags::IS_FREQ` bit - #[ubx(map_type = f64, scale = 1.0)] - freq_period: u32, - /// Frequency in Hz or Period in us, - /// when locked to GPS time. - /// Only used when `flags::LOCKED_OTHER_SET` is set - #[ubx(map_type = f64, scale = 1.0)] - freq_period_lock: u32, - /// Pulse length or duty cycle, [us] or [*2^-32], - /// depending on `flags::LS_LENGTH` bit - #[ubx(map_type = f64, scale = 1.0)] - pulse_len_ratio: u32, - /// Pulse Length in us or duty cycle (*2^-32), - /// when locked to GPS time. - /// Only used when `flags::LOCKED_OTHER_SET` is set - #[ubx(map_type = f64, scale = 1.0)] - pulse_len_ratio_lock: u32, - /// User configurable time pulse delay in [ns] - #[ubx(map_type = f64, scale = 1.0)] - user_delay: i32, - /// Configuration flags, see [CfgTp5Flags] - #[ubx(map_type = CfgTp5Flags)] - flags: u32, -} - -/// Time pulse selection, used in CfgTp5 frame -#[derive(Default)] -#[ubx_extend] -#[ubx(from_unchecked, into_raw, rest_error)] -#[repr(u8)] -#[derive(Clone, Copy, Debug)] -pub enum CfgTp5TimePulseMode { - #[default] - TimePulse = 0, - TimePulse2 = 1, -} - -/// Time MODE2 Config Frame (32.10.36.1) -/// only available on `timing` receivers -#[ubx_packet_recv_send] -#[ubx( - class = 0x06, - id = 0x3d, - fixed_payload_len = 28, - flags = "default_for_builder" -)] -struct CfgTmode2 { - /// Time transfer modes, see [CfgTmode2TimeXferModes] for details - #[ubx(map_type = CfgTmode2TimeXferModes, may_fail)] - time_transfer_mode: u8, - reserved1: u8, - #[ubx(map_type = CfgTmode2Flags)] - flags: u16, - /// WGS84 ECEF.x coordinate in [m] or latitude in [deg° *1E-5], - /// depending on `flags` field - #[ubx(map_type = f64, scale = 1e-2)] - ecef_x_or_lat: i32, - /// WGS84 ECEF.y coordinate in [m] or longitude in [deg° *1E-5], - /// depending on `flags` field - #[ubx(map_type = f64, scale = 1e-2)] - ecef_y_or_lon: i32, - /// WGS84 ECEF.z coordinate or altitude, both in [m], - /// depending on `flags` field - #[ubx(map_type = f64, scale = 1e-2)] - ecef_z_or_alt: i32, - /// Fixed position 3D accuracy in [m] - #[ubx(map_type = f64, scale = 1e-3)] - fixed_pos_acc: u32, - /// Survey in minimum duration in [s] - survey_in_min_duration: u32, - /// Survey in position accuracy limit in [m] - #[ubx(map_type = f64, scale = 1e-3)] - survery_in_accur_limit: u32, -} - -/// Time transfer modes (32.10.36) -#[derive(Default)] -#[ubx_extend] -#[ubx(from_unchecked, into_raw, rest_error)] -#[repr(u8)] -#[derive(Debug, Copy, Clone, PartialEq, Eq)] -pub enum CfgTmode2TimeXferModes { - #[default] - Disabled = 0, - SurveyIn = 1, - /// True position information required - /// when using `fixed mode` - FixedMode = 2, -} - -#[ubx_extend_bitflags] -#[ubx(from, into_raw, rest_reserved)] -bitflags! { - #[derive(Default, Debug)] - pub struct CfgTmode2Flags :u16 { - /// Position given in LAT/LON/ALT - /// default being WGS84 ECEF - const LLA = 0x01; - /// In case LLA was set, Altitude value is not valid - const ALT_INVALID = 0x02; - } -} - -/// Time mode survey-in status -#[ubx_packet_recv] -#[ubx(class = 0x0d, id = 0x04, fixed_payload_len = 28)] -struct TimSvin { - /// Passed survey-in minimum duration - /// Units: s - dur: u32, - /// Current survey-in mean position ECEF X coordinate - mean_x: i32, - /// Current survey-in mean position ECEF Y coordinate - mean_y: i32, - /// Current survey-in mean position ECEF Z coordinate - mean_z: i32, - /// Current survey-in mean position 3D variance - mean_v: i32, - /// Number of position observations used during survey-in - obs: u32, - /// Survey-in position validity flag, 1 = valid, otherwise 0 - valid: u8, - /// Survey-in in progress flag, 1 = in-progress, otherwise 0 - active: u8, - reserved: [u8; 2], -} - -/// Leap second event information -#[ubx_packet_recv] -#[ubx(class = 0x01, id = 0x26, fixed_payload_len = 24)] -struct NavTimeLs { - /// GPS time of week of the navigation epoch in ms. - itow: u32, - ///Message version (0x00 for this version) - version: u8, - reserved_1: [u8; 3], - /// Information source for the current number of leap seconds. - /// 0: Default (hardcoded in the firmware, can be outdated) - /// 1: Derived from time difference between GPS and GLONASS time - /// 2: GPS - /// 3: SBAS - /// 4: BeiDou - /// 5: Galileo - /// 6: Aided data 7: Configured 8: NavIC - /// 255: Unknown - src_of_curr_ls: u8, - /// Current number of leap seconds since start of GPS time (Jan 6, 1980). It reflects how much - /// GPS time is ahead of UTC time. Galileo number of leap seconds is the same as GPS. BeiDou - /// number of leap seconds is 14 less than GPS. GLONASS follows UTC time, so no leap seconds. - current_ls: i8, - /// Information source for the future leap second event. - /// 0: No source - /// 2: GPS - /// 3: SBAS - /// 4: BeiDou - /// 5: Galileo - /// 6: GLONASS 7: NavIC - src_of_ls_change: u8, - /// Future leap second change if one is scheduled. +1 = positive leap second, -1 = negative - /// leap second, 0 = no future leap second event scheduled or no information available. - ls_change: i8, - /// Number of seconds until the next leap second event, or from the last leap second event if - /// no future event scheduled. If > 0 event is in the future, = 0 event is now, < 0 event is in - /// the past. Valid only if validTimeToLsEvent = 1. - time_to_ls_event: i32, - /// GPS week number (WN) of the next leap second event or the last one if no future event - /// scheduled. Valid only if validTimeToLsEvent = 1. - date_of_ls_gps_wn: u16, - /// GPS day of week number (DN) for the next leap second event or the last one if no future - /// event scheduled. Valid only if validTimeToLsEvent = 1. (GPS and Galileo DN: from 1 = Sun to - /// 7 = Sat. BeiDou DN: from 0 = Sun to 6 = Sat.) - date_of_ls_gps_dn: u16, - reserved_2: [u8; 3], - /// Validity flags see `NavTimeLsFlags` - #[ubx(map_type = NavTimeLsFlags)] - valid: u8, -} - -#[ubx_extend_bitflags] -#[ubx(from, rest_reserved)] -bitflags! { - /// Fix status flags for `NavTimeLsFlags` - #[derive(Debug)] - pub struct NavTimeLsFlags: u8 { - /// 1 = Valid current number of leap seconds value. - const VALID_CURR_LS = 1; - /// Valid time to next leap second event or from the last leap second event if no future - /// event scheduled. - const VALID_TIME_TO_LS_EVENT = 2; - } -} - -/// Time MODE3 Config Frame (32.10.37.1) -/// only available on `timing` receivers -#[ubx_packet_recv_send] -#[ubx( - class = 0x06, - id = 0x71, - fixed_payload_len = 40, - flags = "default_for_builder" -)] -struct CfgTmode3 { - version: u8, - reserved1: u8, - /// Receiver mode, see [CfgTmode3RcvrMode] enum - #[ubx(map_type = CfgTmode3RcvrMode)] - rcvr_mode: u8, - #[ubx(map_type = CfgTmode3Flags)] - flags: u8, - /// WGS84 ECEF.x coordinate in [m] or latitude in [deg° *1E-5], - /// depending on `flags` field - #[ubx(map_type = f64, scale = 1e-2)] - ecef_x_or_lat: i32, - /// WGS84 ECEF.y coordinate in [m] or longitude in [deg° *1E-5], - /// depending on `flags` field - #[ubx(map_type = f64, scale = 1e-2)] - ecef_y_or_lon: i32, - /// WGS84 ECEF.z coordinate or altitude, both in [m], - /// depending on `flags` field - #[ubx(map_type = f64, scale = 1e-2)] - ecef_z_or_alt: i32, - /// High precision WGS84 ECEF.x coordinate in [tenths of mm], - /// or high precision latitude, in nano degrees, - /// depending on `flags` field. - #[ubx(map_type = f32, scale = 1.0)] - ecef_x_or_lat_hp: i8, - /// High precision WGS84 ECEF.y coordinate in [tenths of mm] - /// or high precision longitude, in nano degrees, - /// depending on `flags` field. - #[ubx(map_type = f32, scale = 1.0)] - ecef_y_or_lon_hp: i8, - /// High precision WGS84 ECEF.z coordinate or altitude, - /// both if tenths of [mm], - /// depending on `flags` field. - #[ubx(map_type = f32, scale = 1.0)] - ecef_z_or_alt_hp: i8, - reserved2: u8, - /// Fixed position 3D accuracy [0.1 mm] - #[ubx(map_type = f64, scale = 1e-4)] - fixed_pos_acc: u32, - /// Survey in minimum duration [s] - sv_in_min_duration: u32, - /// Survey in position accuracy limit [0.1 mm] - #[ubx(map_type = f64, scale = 1e-4)] - sv_in_accur_limit: u32, - reserved3: [u8; 8], -} - -#[ubx_extend_bitflags] -#[ubx(from, into_raw, rest_reserved)] -bitflags! { - #[derive(Default, Debug)] - pub struct CfgTmode3RcvrMode: u8 { - const DISABLED = 0x01; - const SURVEY_IN = 0x02; - /// True ARP position is required in `FixedMode` - const FIXED_MODE = 0x04; - } -} - -#[ubx_extend_bitflags] -#[ubx(from, into_raw, rest_reserved)] -bitflags! { - #[derive(Default, Debug)] - pub struct CfgTp5Flags: u32 { - // Enables time pulse - const ACTIVE = 0x01; - /// Synchronize time pulse to GNSS as - /// soon as GNSS time is valid. - /// Uses local lock otherwise. - const LOCK_GNSS_FREQ = 0x02; - /// use `freq_period_lock` and `pulse_len_ratio_lock` - /// fields as soon as GPS time is valid. Uses - /// `freq_period` and `pulse_len_ratio` when GPS time is invalid. - const LOCKED_OTHER_SET = 0x04; - /// `freq_period` and `pulse_len_ratio` fields - /// are interprated as frequency when this bit is set - const IS_FREQ = 0x08; - /// Interprate pulse lengths instead of duty cycle - const IS_LENGTH = 0x10; - /// Align pulse to top of second - /// Period time must be integer fraction of `1sec` - /// `LOCK_GNSS_FREQ` is expected, to unlock this feature - const ALIGN_TO_TOW = 0x20; - /// Pulse polarity, - /// 0: falling edge @ top of second, - /// 1: rising edge @ top of second, - const POLARITY = 0x40; - /// UTC time grid - const UTC_TIME_GRID = 0x80; - /// GPS time grid - const GPS_TIME_GRID = 0x100; - /// GLO time grid - const GLO_TIME_GRID = 0x200; - /// BDS time grid - const BDS_TIME_GRID = 0x400; - /// GAL time grid - /// not supported in protocol < 18 - const GAL_TIME_GRID = 0x800; - /// Switches to FreqPeriodLock and PulseLenRatio - /// as soon as Sync Manager has an accurate time, - /// never switches back - const SYNC_MODE_0 = 0x1000; - /// Switches to FreqPeriodLock and PulseLenRatioLock - /// as soon as Sync Manager has an accurante time, - /// and switch back to FreqPeriodLock and PulseLenRatio - /// when time gets inaccurate - const SYNC_MODE_1 = 0x2000; - } -} - -#[ubx_extend_bitflags] -#[ubx(from, into_raw, rest_reserved)] -bitflags! { - #[derive(Default, Debug)] - pub struct CfgTmode3Flags: u8 { - /// Set if position is given in Lat/Lon/Alt, - /// ECEF coordinates being used otherwise - const LLA = 0x01; - } -} - -#[ubx_extend_bitflags] -#[ubx(into_raw, rest_reserved)] -bitflags! { - /// Battery backed RAM sections to clear - pub struct NavBbrMask: u16 { - const EPHEMERIS = 1; - const ALMANACH = 2; - const HEALTH = 4; - const KLOBUCHARD = 8; - const POSITION = 16; - const CLOCK_DRIFT = 32; - const OSCILATOR_PARAMETER = 64; - const UTC_CORRECTION_PARAMETERS = 0x80; - const RTC = 0x100; - const SFDR_PARAMETERS = 0x800; - const SFDR_VEHICLE_MONITORING_PARAMETERS = 0x1000; - const TCT_PARAMETERS = 0x2000; - const AUTONOMOUS_ORBIT_PARAMETERS = 0x8000; - } -} - -/// Predefined values for `NavBbrMask` -#[derive(Clone, Copy)] -#[repr(transparent)] -pub struct NavBbrPredefinedMask(u16); - -impl From for NavBbrMask { - fn from(x: NavBbrPredefinedMask) -> Self { - Self::from_bits_truncate(x.0) - } -} - -impl NavBbrPredefinedMask { - pub const HOT_START: NavBbrPredefinedMask = NavBbrPredefinedMask(0); - pub const WARM_START: NavBbrPredefinedMask = NavBbrPredefinedMask(1); - pub const COLD_START: NavBbrPredefinedMask = NavBbrPredefinedMask(0xFFFF); -} - -/// Reset Type -#[repr(u8)] -#[derive(Clone, Copy, Debug)] -#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] -pub enum ResetMode { - /// Hardware reset (Watchdog) immediately - HardwareResetImmediately = 0, - ControlledSoftwareReset = 0x1, - ControlledSoftwareResetGpsOnly = 0x02, - /// Hardware reset (Watchdog) after shutdown (>=FW6.0) - HardwareResetAfterShutdown = 0x04, - ControlledGpsStop = 0x08, - ControlledGpsStart = 0x09, -} - -impl ResetMode { - const fn into_raw(self) -> u8 { - self as u8 - } -} - -#[ubx_packet_send] -#[ubx( - class = 0x06, - id = 0x8a, - max_payload_len = 772, // 4 + (4 + 8) * 64 -)] -struct CfgValSet<'a> { - /// Message version - version: u8, - /// The layers from which the configuration items should be retrieved - #[ubx(map_type = CfgLayer)] - layers: u8, - reserved1: u16, - cfg_data: &'a [CfgVal], -} - -#[derive(Debug, Clone)] -pub struct CfgValIter<'a> { - pub(crate) data: &'a [u8], - pub(crate) offset: usize, -} - -impl<'a> CfgValIter<'a> { - pub fn new(data: &'a mut [u8], values: &[CfgVal]) -> Self { - let mut offset = 0; - - for value in values { - offset += value.write_to(&mut data[offset..]); - } - - Self { - data: &data[..offset], - offset: 0, - } - } -} - -impl<'a> core::iter::Iterator for CfgValIter<'a> { - type Item = CfgVal; - - fn next(&mut self) -> Option { - if self.offset < self.data.len() { - let cfg_val = CfgVal::parse(&self.data[self.offset..]); - - self.offset += cfg_val.len(); - - Some(cfg_val) - } else { - None - } - } -} - -#[ubx_extend_bitflags] -#[ubx(from, into_raw, rest_reserved)] -bitflags! { - /// A mask describing where configuration is applied. - pub struct CfgLayer: u8 { - const RAM = 0b001; - const BBR = 0b010; - const FLASH = 0b100; - } -} - -impl Default for CfgLayer { - fn default() -> Self { - Self::RAM | Self::BBR | Self::FLASH - } -} - -/// Port Configuration for I2C -#[ubx_packet_recv_send] -#[ubx( - class = 0x06, - id = 0x00, - fixed_payload_len = 20, - flags = "default_for_builder" -)] -struct CfgPrtI2c { - #[ubx(map_type = I2cPortId, may_fail)] - portid: u8, - reserved1: u8, - /// TX ready PIN configuration - tx_ready: u16, - /// I2C Mode Flags - mode: u32, - reserved2: u32, - #[ubx(map_type = InProtoMask)] - in_proto_mask: u16, - #[ubx(map_type = OutProtoMask)] - out_proto_mask: u16, - flags: u16, - reserved3: u16, -} - -/// Port Identifier Number (= 0 for I2C ports) -#[derive(Default)] -#[ubx_extend] -#[ubx(from_unchecked, into_raw, rest_error)] -#[repr(u8)] -#[derive(Debug, Copy, Clone)] -pub enum I2cPortId { - #[default] - I2c = 0, -} - -/// Port Configuration for UART -#[ubx_packet_recv_send] -#[ubx(class = 0x06, id = 0x00, fixed_payload_len = 20)] -struct CfgPrtUart { - #[ubx(map_type = UartPortId, may_fail)] - portid: u8, - reserved0: u8, - tx_ready: u16, - #[ubx(map_type = UartMode)] - mode: u32, - baud_rate: u32, - #[ubx(map_type = InProtoMask)] - in_proto_mask: u16, - #[ubx(map_type = OutProtoMask)] - out_proto_mask: u16, - flags: u16, - reserved5: u16, -} - -/// Port Identifier Number (= 1 or 2 for UART ports) -#[ubx_extend] -#[ubx(from_unchecked, into_raw, rest_error)] -#[repr(u8)] -#[derive(Debug, Copy, Clone)] -pub enum UartPortId { - Uart1 = 1, - Uart2 = 2, - Usb = 3, -} - -#[derive(Debug, Copy, Clone)] -#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] -pub struct UartMode { - data_bits: DataBits, - parity: Parity, - stop_bits: StopBits, -} - -impl UartMode { - pub const fn new(data_bits: DataBits, parity: Parity, stop_bits: StopBits) -> Self { - Self { - data_bits, - parity, - stop_bits, - } - } - - const fn into_raw(self) -> u32 { - self.data_bits.into_raw() | self.parity.into_raw() | self.stop_bits.into_raw() - } -} - -impl From for UartMode { - fn from(mode: u32) -> Self { - let data_bits = DataBits::from(mode); - let parity = Parity::from(mode); - let stop_bits = StopBits::from(mode); - - Self { - data_bits, - parity, - stop_bits, - } - } -} - -#[derive(Debug, Clone, Copy)] -#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] -pub enum DataBits { - Seven, - Eight, -} - -impl DataBits { - const POSITION: u32 = 6; - const MASK: u32 = 0b11; - - const fn into_raw(self) -> u32 { - (match self { - Self::Seven => 0b10, - Self::Eight => 0b11, - }) << Self::POSITION - } -} - -impl From for DataBits { - fn from(mode: u32) -> Self { - match (mode >> Self::POSITION) & Self::MASK { - 0b00 => unimplemented!("five data bits"), - 0b01 => unimplemented!("six data bits"), - 0b10 => Self::Seven, - 0b11 => Self::Eight, - _ => unreachable!(), - } - } -} - -#[derive(Debug, Clone, Copy)] -#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] -pub enum Parity { - Even, - Odd, - None, -} - -impl Parity { - const POSITION: u32 = 9; - const MASK: u32 = 0b111; - - const fn into_raw(self) -> u32 { - (match self { - Self::Even => 0b000, - Self::Odd => 0b001, - Self::None => 0b100, - }) << Self::POSITION - } -} - -impl From for Parity { - fn from(mode: u32) -> Self { - match (mode >> Self::POSITION) & Self::MASK { - 0b000 => Self::Even, - 0b001 => Self::Odd, - 0b100 | 0b101 => Self::None, - 0b010 | 0b011 | 0b110 | 0b111 => unimplemented!("reserved"), - _ => unreachable!(), - } - } -} - -#[derive(Debug, Clone, Copy)] -#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] -pub enum StopBits { - One, - OneHalf, - Two, - Half, -} - -impl StopBits { - const POSITION: u32 = 12; - const MASK: u32 = 0b11; - - const fn into_raw(self) -> u32 { - (match self { - Self::One => 0b00, - Self::OneHalf => 0b01, - Self::Two => 0b10, - Self::Half => 0b11, - }) << Self::POSITION - } -} - -impl From for StopBits { - fn from(mode: u32) -> Self { - match (mode >> Self::POSITION) & Self::MASK { - 0b00 => Self::One, - 0b01 => Self::OneHalf, - 0b10 => Self::Two, - 0b11 => Self::Half, - _ => unreachable!(), - } - } -} - -/// Port Configuration for SPI Port -#[ubx_packet_recv_send] -#[ubx( - class = 0x06, - id = 0x00, - fixed_payload_len = 20, - flags = "default_for_builder" -)] -struct CfgPrtSpi { - #[ubx(map_type = SpiPortId, may_fail)] - portid: u8, - reserved0: u8, - /// TX ready PIN configuration - tx_ready: u16, - /// SPI Mode Flags - mode: u32, - reserved3: u32, - #[ubx(map_type = InProtoMask)] - in_proto_mask: u16, - #[ubx(map_type = OutProtoMask)] - out_proto_mask: u16, - flags: u16, - reserved5: u16, -} - -#[ubx_extend_bitflags] -#[ubx(from, into_raw, rest_reserved)] -bitflags! { - /// A mask describing which input protocols are active - /// Each bit of this mask is used for a protocol. - /// Through that, multiple protocols can be defined on a single port - /// Used in `CfgPrtSpi` and `CfgPrtI2c` - #[derive(Default, Debug)] - pub struct InProtoMask: u16 { - const UBLOX = 1; - const NMEA = 2; - const RTCM = 4; - /// The bitfield inRtcm3 is not supported in protocol - /// versions less than 20 - const RTCM3 = 0x20; - } -} - -#[ubx_extend_bitflags] -#[ubx(from, into_raw, rest_reserved)] -bitflags! { - /// A mask describing which output protocols are active. - /// Each bit of this mask is used for a protocol. - /// Through that, multiple protocols can be defined on a single port - /// Used in `CfgPrtSpi` and `CfgPrtI2c` - #[derive(Default, Debug)] - pub struct OutProtoMask: u16 { - const UBLOX = 1; - const NMEA = 2; - /// The bitfield outRtcm3 is not supported in protocol - /// versions less than 20 - const RTCM3 = 0x20; - } -} - -/// Port Identifier Number (= 4 for SPI port) -#[derive(Default)] -#[ubx_extend] -#[ubx(from_unchecked, into_raw, rest_error)] -#[repr(u8)] -#[derive(Debug, Copy, Clone)] -pub enum SpiPortId { - #[default] - Spi = 4, -} - -/// UTC Time Solution -#[ubx_packet_recv] -#[ubx(class = 1, id = 0x21, fixed_payload_len = 20)] -struct NavTimeUTC { - /// GPS Millisecond Time of Week - itow: u32, - time_accuracy_estimate_ns: u32, - - /// Nanoseconds of second, range -1e9 .. 1e9 - nanos: i32, - - /// Year, range 1999..2099 - year: u16, - - /// Month, range 1..12 - month: u8, - - /// Day of Month, range 1..31 - day: u8, - - /// Hour of Day, range 0..23 - hour: u8, - - /// Minute of Hour, range 0..59 - min: u8, - - /// Seconds of Minute, range 0..59 - sec: u8, - - /// Validity Flags - #[ubx(map_type = NavTimeUtcFlags)] - valid: u8, -} - -#[ubx_extend_bitflags] -#[ubx(from, rest_reserved)] -bitflags! { - /// Validity Flags of `NavTimeUTC` - #[derive(Default, Debug)] - pub struct NavTimeUtcFlags: u8 { - /// Valid Time of Week - const VALID_TOW = 1; - /// Valid Week Number - const VALID_WKN = 2; - /// Valid UTC (Leap Seconds already known) - const VALID_UTC = 4; - } -} - -/// Navigation/Measurement Rate Settings -#[ubx_packet_send] -#[ubx(class = 6, id = 8, fixed_payload_len = 6)] -struct CfgRate { - /// Measurement Rate, GPS measurements are taken every `measure_rate_ms` milliseconds - measure_rate_ms: u16, - - /// Navigation Rate, in number of measurement cycles. - - /// On u-blox 5 and u-blox 6, this parametercannot be changed, and is always equals 1. - nav_rate: u16, - - /// Alignment to reference time - #[ubx(map_type = AlignmentToReferenceTime)] - time_ref: u16, -} - -/// Alignment to reference time -#[repr(u16)] -#[derive(Clone, Copy, Debug)] -pub enum AlignmentToReferenceTime { - Utc = 0, - Gps = 1, - Glo = 2, - Bds = 3, - Gal = 4, -} - -impl AlignmentToReferenceTime { - const fn into_raw(self) -> u16 { - self as u16 - } -} - -/// Set Message Rate the current port -#[ubx_packet_send] -#[ubx(class = 6, id = 1, fixed_payload_len = 3)] -struct CfgMsgSinglePort { - msg_class: u8, - msg_id: u8, - - /// Send rate on current Target - rate: u8, -} - -impl CfgMsgSinglePortBuilder { - #[inline] - pub fn set_rate_for(rate: u8) -> Self { - Self { - msg_class: T::CLASS, - msg_id: T::ID, - rate, - } - } -} - -/// Set Message rate configuration -/// Send rate is relative to the event a message is registered on. -/// For example, if the rate of a navigation message is set to 2, -/// the message is sent every second navigation solution -#[ubx_packet_send] -#[ubx(class = 6, id = 1, fixed_payload_len = 8)] -struct CfgMsgAllPorts { - msg_class: u8, - msg_id: u8, - - /// Send rate on I/O Port (6 Ports) - rates: [u8; 6], -} - -impl CfgMsgAllPortsBuilder { - #[inline] - pub fn set_rate_for(rates: [u8; 6]) -> Self { - Self { - msg_class: T::CLASS, - msg_id: T::ID, - rates, - } - } -} - -/// Navigation Engine Settings -#[ubx_packet_recv_send] -#[ubx( - class = 0x06, - id = 0x24, - fixed_payload_len = 36, - flags = "default_for_builder" -)] -struct CfgNav5 { - /// Only the masked parameters will be applied - #[ubx(map_type = CfgNav5Params)] - mask: u16, - #[ubx(map_type = CfgNav5DynModel, may_fail)] - dyn_model: u8, - #[ubx(map_type = CfgNav5FixMode, may_fail)] - fix_mode: u8, - - /// Fixed altitude (mean sea level) for 2D fixmode (m) - #[ubx(map_type = f64, scale = 0.01)] - fixed_alt: i32, - - /// Fixed altitude variance for 2D mode (m^2) - #[ubx(map_type = f64, scale = 0.0001)] - fixed_alt_var: u32, - - /// Minimum Elevation for a GNSS satellite to be used in NAV (deg) - min_elev_degrees: i8, - - /// Reserved - dr_limit: u8, - - /// Position DOP Mask to use - #[ubx(map_type = f32, scale = 0.1)] - pdop: u16, - - /// Time DOP Mask to use - #[ubx(map_type = f32, scale = 0.1)] - tdop: u16, - - /// Position Accuracy Mask (m) - pacc: u16, - - /// Time Accuracy Mask - /// according to manual unit is "m", but this looks like typo - tacc: u16, - - /// Static hold threshold - #[ubx(map_type = f32, scale = 0.01)] - static_hold_thresh: u8, - - /// DGNSS timeout (seconds) - dgps_time_out: u8, - - /// Number of satellites required to have - /// C/N0 above `cno_thresh` for a fix to be attempted - cno_thresh_num_svs: u8, - - /// C/N0 threshold for deciding whether toattempt a fix (dBHz) - cno_thresh: u8, - reserved1: [u8; 2], - - /// Static hold distance threshold (beforequitting static hold) - static_hold_max_dist: u16, - - /// UTC standard to be used - #[ubx(map_type = CfgNav5UtcStandard, may_fail)] - utc_standard: u8, - reserved2: [u8; 5], -} - -#[ubx_extend_bitflags] -#[ubx(from, into_raw, rest_reserved)] -bitflags! { - /// `CfgNav5` parameters bitmask - #[derive(Default, Debug, PartialEq, Eq)] - pub struct CfgNav5Params: u16 { - /// Apply dynamic model settings - const DYN = 1; - /// Apply minimum elevation settings - const MIN_EL = 2; - /// Apply fix mode settings - const POS_FIX_MODE = 4; - /// Reserved - const DR_LIM = 8; - /// position mask settings - const POS_MASK_APPLY = 0x10; - /// Apply time mask settings - const TIME_MASK = 0x20; - /// Apply static hold settings - const STATIC_HOLD_MASK = 0x40; - /// Apply DGPS settings - const DGPS_MASK = 0x80; - /// Apply CNO threshold settings (cnoThresh, cnoThreshNumSVs) - const CNO_THRESHOLD = 0x100; - /// Apply UTC settings (not supported in protocol versions less than 16) - const UTC = 0x400; - } -} - -/// Dynamic platform model -#[derive(Default)] -#[ubx_extend] -#[ubx(from_unchecked, into_raw, rest_error)] -#[repr(u8)] -#[derive(Debug, Copy, Clone, PartialEq, Eq)] -pub enum CfgNav5DynModel { - Portable = 0, - Stationary = 2, - Pedestrian = 3, - Automotive = 4, - Sea = 5, - AirborneWithLess1gAcceleration = 6, - AirborneWithLess2gAcceleration = 7, - #[default] - AirborneWith4gAcceleration = 8, - /// not supported in protocol versions less than 18 - WristWornWatch = 9, - /// supported in protocol versions 19.2 - Bike = 10, -} - -/// Position Fixing Mode -#[derive(Default)] // default needs to be derived before ubx_extend -#[ubx_extend] -#[ubx(from_unchecked, into_raw, rest_error)] -#[repr(u8)] -#[derive(Debug, Copy, Clone, PartialEq, Eq)] -pub enum CfgNav5FixMode { - Only2D = 1, - Only3D = 2, - #[default] - Auto2D3D = 3, -} - -/// UTC standard to be used -#[derive(Default)] -#[ubx_extend] -#[ubx(from_unchecked, into_raw, rest_error)] -#[repr(u8)] -#[derive(Debug, Copy, Clone, PartialEq, Eq)] -pub enum CfgNav5UtcStandard { - /// receiver selects based on GNSS configuration (see GNSS timebases) - #[default] - Automatic = 0, - /// UTC as operated by the U.S. NavalObservatory (USNO); - /// derived from GPStime - Usno = 3, - /// UTC as operated by the former Soviet Union; derived from GLONASS time - UtcSu = 6, - /// UTC as operated by the National TimeService Center, China; - /// derived from BeiDou time - UtcChina = 7, -} - -#[derive(Clone, Copy)] -#[repr(transparent)] -struct ScaleBack(T); - -impl ScaleBack { - fn as_i8(self, x: T) -> i8 { - let x = (x * self.0).round(); - if x < T::from_i8(i8::min_value()).unwrap() { - i8::min_value() - } else if x > T::from_i8(i8::max_value()).unwrap() { - i8::max_value() - } else { - x.to_i8().unwrap() - } - } - - fn as_i16(self, x: T) -> i16 { - let x = (x * self.0).round(); - if x < T::from_i16(i16::min_value()).unwrap() { - i16::min_value() - } else if x > T::from_i16(i16::max_value()).unwrap() { - i16::max_value() - } else { - x.to_i16().unwrap() - } - } - - fn as_i32(self, x: T) -> i32 { - let x = (x * self.0).round(); - if x < T::from_i32(i32::MIN).unwrap() { - i32::MIN - } else if x > T::from_i32(i32::MAX).unwrap() { - i32::MAX - } else { - x.to_i32().unwrap() - } - } - - fn as_u32(self, x: T) -> u32 { - let x = (x * self.0).round(); - if !x.is_sign_negative() { - if x <= T::from_u32(u32::MAX).unwrap() { - x.to_u32().unwrap() - } else { - u32::MAX - } - } else { - 0 - } - } - - fn as_u16(self, x: T) -> u16 { - let x = (x * self.0).round(); - if !x.is_sign_negative() { - if x <= T::from_u16(u16::MAX).unwrap() { - x.to_u16().unwrap() - } else { - u16::MAX - } - } else { - 0 - } - } - - fn as_u8(self, x: T) -> u8 { - let x = (x * self.0).round(); - if !x.is_sign_negative() { - if x <= T::from_u8(u8::MAX).unwrap() { - x.to_u8().unwrap() - } else { - u8::MAX - } - } else { - 0 - } - } -} - -/// Navigation Engine Expert Settings -#[ubx_packet_recv_send] -#[ubx( - class = 0x06, - id = 0x23, - fixed_payload_len = 40, - flags = "default_for_builder" -)] -struct CfgNavX5 { - /// Only version 2 supported - version: u16, - - /// Only the masked parameters will be applied - #[ubx(map_type = CfgNavX5Params1)] - mask1: u16, - - #[ubx(map_type = CfgNavX5Params2)] - mask2: u32, - - /// Reserved - reserved1: [u8; 2], - - /// Minimum number of satellites for navigation - min_svs: u8, - - ///Maximum number of satellites for navigation - max_svs: u8, - - /// Minimum satellite signal level for navigation - min_cno_dbhz: u8, - - /// Reserved - reserved2: u8, - - /// initial fix must be 3D - ini_fix_3d: u8, - - /// Reserved - reserved3: [u8; 2], - - /// issue acknowledgements for assistance message input - ack_aiding: u8, - - /// GPS week rollover number - wkn_rollover: u16, - - /// Permanently attenuated signal compensation - sig_atten_comp_mode: u8, - - /// Reserved - reserved4: u8, - reserved5: [u8; 2], - reserved6: [u8; 2], - - /// Use Precise Point Positioning (only available with the PPP product variant) - use_ppp: u8, - - /// AssistNow Autonomous configuration - aop_cfg: u8, - - /// Reserved - reserved7: [u8; 2], - - /// Maximum acceptable (modeled) AssistNow Autonomous orbit error - aop_orb_max_err: u16, - - /// Reserved - reserved8: [u8; 4], - reserved9: [u8; 3], - - /// Enable/disable ADR/UDR sensor fusion - use_adr: u8, -} - -#[ubx_extend_bitflags] -#[ubx(from, into_raw, rest_reserved)] -bitflags! { - /// `CfgNavX51` parameters bitmask - #[derive(Default, Debug)] - pub struct CfgNavX5Params1: u16 { - /// apply min/max SVs settings - const MIN_MAX = 0x4; - /// apply minimum C/N0 setting - const MIN_CNO = 0x8; - /// apply initial 3D fix settings - const INITIAL_3D_FIX = 0x40; - /// apply GPS weeknumber rollover settings - const WKN_ROLL = 0x200; - /// apply assistance acknowledgement settings - const AID_ACK = 0x400; - /// apply usePPP flag - const USE_PPP = 0x2000; - /// apply aopCfg (useAOP flag) and aopOrbMaxErr settings (AssistNow Autonomous) - const AOP_CFG = 0x4000; - } -} - -#[ubx_extend_bitflags] -#[ubx(from, into_raw, rest_reserved)] -bitflags! { - /// `CfgNavX5Params2` parameters bitmask - #[derive(Default, Debug)] - pub struct CfgNavX5Params2: u32 { - /// apply ADR/UDR sensor fusion on/off setting - const USE_ADR = 0x40; - /// apply signal attenuation compensation feature settings - const USE_SIG_ATTEN_COMP = 0x80; - } -} - -/// GNSS Assistance ACK UBX-MGA-ACK -#[ubx_packet_recv] -#[ubx(class = 0x13, id = 0x60, fixed_payload_len = 8)] -struct MgaAck { - /// Type of acknowledgment: 0 -> not used, 1 -> accepted - ack_type: u8, - - /// Version 0 - version: u8, - - /// Provides greater information on what the receiver chose to do with the message contents. - /// See [MsgAckInfoCode]. - #[ubx(map_type = MsgAckInfoCode)] - info_code: u8, - - /// UBX message ID of the acknowledged message - msg_id: u8, - - /// The first 4 bytes of the acknowledged message's payload - msg_payload_start: [u8; 4], -} - -#[ubx_extend] -#[ubx(from, rest_reserved)] -#[repr(u8)] -#[derive(Debug, Copy, Clone, PartialEq, Eq)] -pub enum MsgAckInfoCode { - Accepted = 0, - RejectedNoTime = 1, - RejectedBadVersion = 2, - RejectedBadSize = 3, - RejectedDBStoreFailed = 4, - RejectedNotReady = 5, - RejectedUnknownType = 6, -} - -#[ubx_packet_recv] -#[ubx(class = 0x13, id = 0x06, fixed_payload_len = 48)] -struct MgaGloEph { - msg_type: u8, - version: u8, - sv_id: u8, - reserved1: u8, - ft: u8, - b: u8, - m: u8, - h: i8, - x: i32, - y: i32, - z: i32, - dx: i32, - dy: i32, - dz: i32, - ddx: i8, - ddy: i8, - ddz: i8, - tb: u8, - gamma: u16, - e: u8, - delta_tau: u8, - tau: i32, - reserved2: [u8; 4], -} - -#[ubx_packet_recv] -#[ubx(class = 0x13, id = 0x00, fixed_payload_len = 16)] -struct MgaGpsIono { - /// Message type: 0x06 for this type - msg_type: u8, - /// Message version: 0x00 for this version - version: u8, - reserved1: [u8; 2], - /// Ionospheric parameter alpha0 [s] - #[ubx(map_type = f64, scale = 1.0)] // 2^-30 - alpha0: i8, - /// Ionospheric parameter alpha1 [s/semi-circle] - #[ubx(map_type = f64, scale = 1.0)] // 2^-27 - alpha1: i8, - /// Ionospheric parameter alpha1 [s/semi-circle^2] - #[ubx(map_type = f64, scale = 1.0)] // 2^-24 - alpha2: i8, - /// Ionospheric parameter alpha1 [s/semi-circle^3] - #[ubx(map_type = f64, scale = 1.0)] // 2^-24 - alpha3: i8, - /// Ionospheric parameter beta0 [s] - #[ubx(map_type = f64, scale = 1.0)] // 2^-11 - beta0: i8, - /// Ionospheric parameter beta0 [s/semi-circle] - #[ubx(map_type = f64, scale = 1.0)] // 2^-14 - beta1: i8, - /// Ionospheric parameter beta0 [s/semi-circle^2] - #[ubx(map_type = f64, scale = 1.0)] // 2^-16 - beta2: i8, - /// Ionospheric parameter beta0 [s/semi-circle^3] - #[ubx(map_type = f64, scale = 1.0)] // 2^-16 - beta3: i8, - reserved2: [u8; 4], -} - -#[ubx_packet_recv] -#[ubx(class = 0x13, id = 0x00, fixed_payload_len = 68)] -struct MgaGpsEph { - msg_type: u8, - version: u8, - sv_id: u8, - reserved1: u8, - fit_interval: u8, - ura_index: u8, - sv_health: u8, - tgd: i8, - iodc: u16, - toc: u16, - reserved2: u8, - af2: i8, - af1: i16, - af0: i32, - crs: i16, - delta_n: i16, - m0: i32, - cuc: i16, - cus: i16, - e: u32, - sqrt_a: u32, - toe: u16, - cic: i16, - omega0: i32, - cis: i16, - crc: i16, - i0: i32, - omega: i32, - omega_dot: i32, - idot: i16, - reserved3: [u8; 2], -} - -/// Time pulse time data -#[ubx_packet_recv] -#[ubx(class = 0x0d, id = 0x01, fixed_payload_len = 16)] -struct TimTp { - /// Time pulse time of week according to time base - tow_ms: u32, - /// Submillisecond part of towMS (scaling: 2^(-32)) - tow_sub_ms: u32, - /// Quantization error of time pulse - q_err: i32, - /// Time pulse week number according to time base - week: u16, - /// Flags - #[ubx(map_type = TimTpFlags, from = TimTpFlags)] - flags: u8, - /// Time reference information - #[ubx(map_type = TimTpRefInfo, from = TimTpRefInfo)] - ref_info: u8, -} - -#[derive(Debug, Clone)] -#[cfg_attr(feature = "serde", derive(serde::Serialize))] -pub struct TimTpFlags(u8); - -impl TimTpFlags { - /// Time base - pub fn time_base(&self) -> TimTpTimeBase { - if self.0 & 0b1 == 0 { - TimTpTimeBase::Gnss - } else { - TimTpTimeBase::Utc - } - } - - /// UTC availability - pub fn utc_available(&self) -> bool { - self.0 & 0b10 != 0 - } - - /// (T)RAIM state - /// - /// Returns `None` if unavailale. - pub fn raim_active(&self) -> Option { - match (self.0 >> 2) & 0b11 { - // Inactive. - 0b01 => Some(false), - // Active. - 0b10 => Some(true), - // Unavailable. - _ => None, - } - } - - /// Quantization error validity - pub fn q_err_valid(&self) -> bool { - self.0 & 0b10000 == 0 - } -} - -#[derive(Debug, Clone, Copy, PartialEq, Eq)] -pub enum TimTpTimeBase { - Gnss, - Utc, -} - -#[derive(Debug, Clone)] -#[cfg_attr(feature = "serde", derive(serde::Serialize))] -pub struct TimTpRefInfo(u8); - -impl TimTpRefInfo { - /// GNSS reference information. Only valid if time base is GNSS. - pub fn time_ref_gnss(&self) -> Option { - Some(match self.0 & 0b1111 { - 0 => TimTpRefInfoTimeRefGnss::Gps, - 1 => TimTpRefInfoTimeRefGnss::Glo, - 2 => TimTpRefInfoTimeRefGnss::Bds, - 3 => TimTpRefInfoTimeRefGnss::Gal, - 4 => TimTpRefInfoTimeRefGnss::NavIc, - _ => return None, - }) - } - - /// UTC standard identifier. Only valid if time base is UTC. - pub fn utc_standard(&self) -> Option { - Some(match self.0 >> 4 { - 1 => TimTpRefInfoUtcStandard::Crl, - 2 => TimTpRefInfoUtcStandard::Nist, - 3 => TimTpRefInfoUtcStandard::Usno, - 4 => TimTpRefInfoUtcStandard::Bipm, - 5 => TimTpRefInfoUtcStandard::Eu, - 6 => TimTpRefInfoUtcStandard::Su, - 7 => TimTpRefInfoUtcStandard::Ntsc, - 8 => TimTpRefInfoUtcStandard::Npli, - _ => return None, - }) - } -} - -#[derive(Debug, Copy, Clone, PartialEq, Eq)] -pub enum TimTpRefInfoTimeRefGnss { - Gps, - Glo, - Bds, - Gal, - NavIc, -} - -#[derive(Debug, Copy, Clone, PartialEq, Eq)] -pub enum TimTpRefInfoUtcStandard { - Crl, - Nist, - Usno, - Bipm, - Eu, - Su, - Ntsc, - Npli, -} - -/// Time mark data -#[ubx_packet_recv] -#[ubx(class = 0x0d, id = 0x03, fixed_payload_len = 28)] -struct TimTm2 { - /// Channel (i.e. EXTINT) upon which the pulse was measured - ch: u8, - /// Flags - #[ubx(map_type = TimTm2Flags, from = TimTm2Flags)] - flags: u8, - /// Rising edge counter - count: u16, - /// Week number of last rising edge - wn_r: u16, - /// Week number of last falling edge - wn_f: u16, - /// Tow of rising edge - tow_ms_r: u32, - /// Millisecond fraction of tow of rising edge in nanoseconds - tow_sub_ms_r: u32, - /// Tow of falling edge - tow_ms_f: u32, - /// Millisecond fraction of tow of falling edge in nanoseconds - tow_sub_ms_f: u32, - /// Accuracy estimate - acc_est: u32, -} - -#[derive(Debug, Clone)] -#[cfg_attr(feature = "serde", derive(serde::Serialize))] -pub struct TimTm2Flags(u8); - -impl TimTm2Flags { - pub fn mode(&self) -> TimTm2Mode { - if self.0 & 0b1 == 0 { - TimTm2Mode::Single - } else { - TimTm2Mode::Running - } - } - - pub fn run(&self) -> TimTm2Run { - if self.0 & 0b10 == 0 { - TimTm2Run::Armed - } else { - TimTm2Run::Stopped - } - } - - pub fn new_falling_edge(&self) -> bool { - self.0 & 0b100 != 0 - } - - pub fn new_rising_edge(&self) -> bool { - self.0 & 0b10000000 != 0 - } - - pub fn time_base(&self) -> TimTm2TimeBase { - match self.0 & 0b11000 { - 0 => TimTm2TimeBase::Receiver, - 1 => TimTm2TimeBase::Gnss, - 2 => TimTm2TimeBase::Utc, - _ => unreachable!(), - } - } - - /// UTC availability - pub fn utc_available(&self) -> bool { - self.0 & 0b100000 != 0 - } - - pub fn time_valid(&self) -> bool { - self.0 & 0b1000000 != 0 - } -} - -pub enum TimTm2Mode { - Single, - Running, -} - -pub enum TimTm2Run { - Armed, - Stopped, -} - -pub enum TimTm2TimeBase { - Receiver, - Gnss, - Utc, -} - -#[ubx_packet_recv] -#[ubx(class = 0x02, id = 0x15, max_payload_len = 8176)] // 16 + 255 * 32 -struct RxmRawx { - /// Measurement time of week in receiver local time approximately aligned to the GPS time system. - rcv_tow: f64, - /// GPS week number in receiver local time. - week: u16, - /// GPS leap seconds (GPS-UTC) - leap_s: i8, - /// Number of measurements to follow - num_meas: u8, - /// Receiver tracking status bitfield - #[ubx(map_type = RecStatFlags)] - rec_stat: u8, - /// Message version - version: u8, - reserved1: [u8; 2], - /// Extended software information strings - #[ubx( - map_type = RxmRawxInfoIter, - from = RxmRawxInfoIter::new, - may_fail, - is_valid = RxmRawxInfoIter::is_valid, - )] - measurements: [u8; 0], -} - -#[ubx_extend_bitflags] -#[ubx(from, into_raw, rest_reserved)] -bitflags! { - /// `CfgNavX5Params2` parameters bitmask - #[derive(Default, Debug)] - pub struct RecStatFlags: u8 { - /// Leap seconds have been determined - const LEAP_SEC = 0x1; - /// Clock reset applied. - const CLK_RESET = 0x2; - } -} - -/// Hardware status -#[ubx_packet_recv] -#[ubx(class = 0x0a, id = 0x09, fixed_payload_len = 60)] -struct MonHw { - pin_sel: u32, - pin_bank: u32, - pin_dir: u32, - pin_val: u32, - noise_per_ms: u16, - agc_cnt: u16, - #[ubx(map_type = AntennaStatus)] - a_status: u8, - #[ubx(map_type = AntennaPower)] - a_power: u8, - flags: u8, - reserved1: u8, - used_mask: u32, - vp: [u8; 17], - jam_ind: u8, - reserved2: [u8; 2], - pin_irq: u32, - pull_h: u32, - pull_l: u32, -} - -#[ubx_extend] -#[ubx(from, rest_reserved)] -#[repr(u8)] -#[derive(Debug, Copy, Clone, PartialEq, Eq)] -pub enum AntennaStatus { - Init = 0, - DontKnow = 1, - Ok = 2, - Short = 3, - Open = 4, -} - -/// GNSS status monitoring, -/// gives currently selected constellations -#[ubx_packet_recv] -#[ubx(class = 0x0a, id = 0x28, fixed_payload_len = 8)] -struct MonGnss { - /// Message version: 0x00 - version: u8, - /// Supported major constellations bit mask - #[ubx(map_type = MonGnssConstellMask)] - supported: u8, - /// Default major GNSS constellations bit mask - #[ubx(map_type = MonGnssConstellMask)] - default: u8, - /// Currently enabled major constellations bit mask - #[ubx(map_type = MonGnssConstellMask)] - enabled: u8, - /// Maximum number of concurent Major GNSS - /// that can be supported by this receiver - simultaneous: u8, - reserved1: [u8; 3], -} - -#[ubx_extend_bitflags] -#[ubx(from, into_raw, rest_reserved)] -bitflags! { - /// Selected / available Constellation Mask - #[derive(Default, Debug)] - pub struct MonGnssConstellMask: u8 { - /// GPS constellation - const GPS = 0x01; - /// GLO constellation - const GLO = 0x02; - /// BDC constellation - const BDC = 0x04; - /// GAL constellation - const GAL = 0x08; - } -} - -#[ubx_extend] -#[ubx(from, rest_reserved)] -#[repr(u8)] -#[derive(Debug, Copy, Clone, PartialEq, Eq)] -pub enum AntennaPower { - Off = 0, - On = 1, - DontKnow = 2, -} - -#[derive(Debug, Clone)] -pub struct MonVerExtensionIter<'a> { - data: &'a [u8], - offset: usize, -} - -impl<'a> MonVerExtensionIter<'a> { - fn new(data: &'a [u8]) -> Self { - Self { data, offset: 0 } - } - - fn is_valid(payload: &[u8]) -> bool { - payload.len() % 30 == 0 && payload.chunks(30).all(is_cstr_valid) - } -} - -impl<'a> core::iter::Iterator for MonVerExtensionIter<'a> { - type Item = &'a str; - - fn next(&mut self) -> Option { - if self.offset < self.data.len() { - let data = &self.data[self.offset..self.offset + 30]; - self.offset += 30; - Some(mon_ver::convert_to_str_unchecked(data)) - } else { - None - } - } -} - -/// Receiver/Software Version -#[ubx_packet_recv] -#[ubx(class = 0x0a, id = 0x04, max_payload_len = 1240)] -struct MonVer { - #[ubx(map_type = &str, may_fail, from = mon_ver::convert_to_str_unchecked, - is_valid = mon_ver::is_cstr_valid, get_as_ref)] - software_version: [u8; 30], - #[ubx(map_type = &str, may_fail, from = mon_ver::convert_to_str_unchecked, - is_valid = mon_ver::is_cstr_valid, get_as_ref)] - hardware_version: [u8; 10], - - /// Extended software information strings - #[ubx(map_type = MonVerExtensionIter, may_fail, - from = MonVerExtensionIter::new, - is_valid = MonVerExtensionIter::is_valid)] - extension: [u8; 0], -} - -mod mon_ver { - pub(crate) fn convert_to_str_unchecked(bytes: &[u8]) -> &str { - let null_pos = bytes - .iter() - .position(|x| *x == 0) - .expect("is_cstr_valid bug?"); - core::str::from_utf8(&bytes[0..null_pos]) - .expect("is_cstr_valid should have prevented this code from running") - } - - pub(crate) fn is_cstr_valid(bytes: &[u8]) -> bool { - let null_pos = match bytes.iter().position(|x| *x == 0) { - Some(pos) => pos, - None => { - return false; - }, - }; - core::str::from_utf8(&bytes[0..null_pos]).is_ok() - } -} - -#[ubx_packet_recv] -#[ubx(class = 0x02, id = 0x32, fixed_payload_len = 8)] -struct RxmRtcm { - version: u8, - flags: u8, - sub_type: u16, - ref_station: u16, - msg_type: u16, -} - -#[ubx_packet_recv] -#[ubx(class = 0x10, id = 0x02, max_payload_len = 1240)] -struct EsfMeas { - time_tag: u32, - flags: u16, - id: u16, - #[ubx( - map_type = EsfMeasDataIter, - from = EsfMeasDataIter::new, - size_fn = data_len, - is_valid = EsfMeasDataIter::is_valid, - may_fail, - )] - data: [u8; 0], - #[ubx( - map_type = Option, - from = EsfMeas::calib_tag, - size_fn = calib_tag_len, - )] - calib_tag: [u8; 0], -} - -impl EsfMeas { - fn calib_tag(bytes: &[u8]) -> Option { - bytes.try_into().ok().map(u32::from_le_bytes) - } -} - -impl<'a> EsfMeasRef<'a> { - fn data_len(&self) -> usize { - ((self.flags() >> 11 & 0x1f) as usize) * 4 - } - - fn calib_tag_len(&self) -> usize { - if self.flags() & 0x8 != 0 { - 4 - } else { - 0 - } - } -} - -#[derive(Debug)] -#[cfg_attr(feature = "serde", derive(serde::Serialize))] -pub struct EsfMeasData { - pub data_type: u8, - pub data_field: u32, -} - -#[derive(Debug, Clone)] -pub struct EsfMeasDataIter<'a>(core::slice::ChunksExact<'a, u8>); - -impl<'a> EsfMeasDataIter<'a> { - fn new(bytes: &'a [u8]) -> Self { - Self(bytes.chunks_exact(4)) - } - - fn is_valid(bytes: &'a [u8]) -> bool { - bytes.len() % 4 == 0 - } -} - -impl<'a> core::iter::Iterator for EsfMeasDataIter<'a> { - type Item = EsfMeasData; - - fn next(&mut self) -> Option { - let data = self.0.next()?.try_into().map(u32::from_le_bytes).unwrap(); - Some(EsfMeasData { - data_type: ((data & 0x3F000000) >> 24).try_into().unwrap(), - data_field: data & 0xFFFFFF, - }) - } -} - -#[ubx_packet_recv] -#[ubx(class = 0x10, id = 0x03, max_payload_len = 1240)] -struct EsfRaw { - msss: u32, - #[ubx( - map_type = EsfRawDataIter, - from = EsfRawDataIter::new, - is_valid = EsfRawDataIter::is_valid, - may_fail, - )] - data: [u8; 0], -} - -#[derive(Debug)] -#[cfg_attr(feature = "serde", derive(serde::Serialize))] -pub struct EsfRawData { - pub data_type: u8, - pub data_field: u32, - pub sensor_time_tag: u32, -} - -#[derive(Debug, Clone)] -pub struct EsfRawDataIter<'a>(core::slice::ChunksExact<'a, u8>); - -impl<'a> EsfRawDataIter<'a> { - fn new(bytes: &'a [u8]) -> Self { - Self(bytes.chunks_exact(8)) - } - - fn is_valid(bytes: &'a [u8]) -> bool { - bytes.len() % 8 == 0 - } -} - -impl<'a> core::iter::Iterator for EsfRawDataIter<'a> { - type Item = EsfRawData; - - fn next(&mut self) -> Option { - let chunk = self.0.next()?; - let data = u32::from_le_bytes(chunk[0..4].try_into().unwrap()); - let sensor_time_tag = u32::from_le_bytes(chunk[4..8].try_into().unwrap()); - Some(EsfRawData { - data_type: ((data >> 24) & 0xFF).try_into().unwrap(), - data_field: data & 0xFFFFFF, - sensor_time_tag, - }) - } -} - -#[ubx_packet_recv] -#[ubx(class = 0x10, id = 0x15, fixed_payload_len = 36)] -struct EsfIns { - #[ubx(map_type = EsfInsBitFlags)] - bit_field: u32, - reserved: [u8; 4], - itow: u32, - - #[ubx(map_type = f64, scale = 1e-3, alias = x_angular_rate)] - x_ang_rate: i32, - - #[ubx(map_type = f64, scale = 1e-3, alias = y_angular_rate)] - y_ang_rate: i32, - - #[ubx(map_type = f64, scale = 1e-3, alias = z_angular_rate)] - z_ang_rate: i32, - - #[ubx(map_type = f64, scale = 1e-2, alias = x_acceleration)] - x_accel: i32, - - #[ubx(map_type = f64, scale = 1e-2, alias = y_acceleration)] - y_accel: i32, - - #[ubx(map_type = f64, scale = 1e-2, alias = z_acceleration)] - z_accel: i32, -} - -#[ubx_extend_bitflags] -#[ubx(from, rest_reserved)] -bitflags! { - #[derive(Debug)] - pub struct EsfInsBitFlags: u32 { - const VERSION = 1; - const X_ANG_RATE_VALID = 0x100; - const Y_ANG_RATE_VALID = 0x200; - const Z_ANG_RATE_VALID = 0x400; - const X_ACCEL_VALID = 0x800; - const Y_ACCEL_VALID = 0x1000; - const Z_ACCEL_VALID = 0x2000; - } -} - -#[ubx_packet_recv] -#[ubx(class = 0x28, id = 0x01, fixed_payload_len = 32)] -struct HnrAtt { - itow: u32, - version: u8, - reserved1: [u8; 3], - #[ubx(map_type = f64, scale = 1e-5, alias = vehicle_roll)] - roll: i32, - #[ubx(map_type = f64, scale = 1e-5, alias = vehicle_pitch)] - pitch: i32, - #[ubx(map_type = f64, scale = 1e-5, alias = vehicle_heading)] - heading: i32, - #[ubx(map_type = f64, scale = 1e-5, alias = vehicle_roll_accuracy)] - acc_roll: u32, - #[ubx(map_type = f64, scale = 1e-5, alias = vehicle_pitch_accuracy)] - acc_pitch: u32, - #[ubx(map_type = f64, scale = 1e-5, alias = vehicle_heading_accuracy)] - acc_heading: u32, -} - -#[ubx_packet_recv] -#[ubx(class = 0x28, id = 0x02, fixed_payload_len = 36)] -pub struct HnrIns { - #[ubx(map_type = HnrInsBitFlags)] - bit_field: u32, - reserved: [u8; 4], - itow: u32, - - #[ubx(map_type = f64, scale = 1e-3, alias = x_angular_rate)] - x_ang_rate: i32, - - #[ubx(map_type = f64, scale = 1e-3, alias = y_angular_rate)] - y_ang_rate: i32, - - #[ubx(map_type = f64, scale = 1e-3, alias = z_angular_rate)] - z_ang_rate: i32, - - #[ubx(map_type = f64, scale = 1e-2, alias = x_acceleration)] - x_accel: i32, - - #[ubx(map_type = f64, scale = 1e-2, alias = y_acceleration)] - y_accel: i32, - - #[ubx(map_type = f64, scale = 1e-2, alias = z_acceleration)] - z_accel: i32, -} - -#[ubx_extend_bitflags] -#[ubx(from, rest_reserved)] -bitflags! { - #[derive(Debug)] - pub struct HnrInsBitFlags: u32 { - const VERSION = 1; - const X_ANG_RATE_VALID = 0x100; - const Y_ANG_RATE_VALID = 0x200; - const Z_ANG_RATE_VALID = 0x400; - const X_ACCEL_VALID = 0x800; - const Y_ACCEL_VALID = 0x1000; - const Z_ACCEL_VALID = 0x2000; - } -} - -#[ubx_packet_recv] -#[ubx(class = 0x28, id = 0x00, fixed_payload_len = 72)] -#[derive(Debug)] -struct HnrPvt { - itow: u32, - year: u16, - month: u8, - day: u8, - hour: u8, - min: u8, - sec: u8, - - #[ubx(map_type = HnrPvtValidFlags)] - valid: u8, - nano: i32, - #[ubx(map_type = GpsFix)] - gps_fix: u8, - - #[ubx(map_type = HnrPvtFlags)] - flags: u8, - - reserved1: [u8; 2], - - #[ubx(map_type = f64, scale = 1e-7, alias = longitude)] - lon: i32, - - #[ubx(map_type = f64, scale = 1e-7, alias = latitude)] - lat: i32, - - #[ubx(map_type = f64, scale = 1e-3, alias = height_above_ellipsoid)] - height: i32, - - #[ubx(map_type = f64, scale = 1e-3, alias = height_msl)] - height_msl: i32, - - #[ubx(map_type = f64, scale = 1e-3, alias = ground_speed_2d)] - g_speed: i32, - - #[ubx(map_type = f64, scale = 1e-3, alias = speed_3d)] - speed: i32, - - #[ubx(map_type = f64, scale = 1e-5, alias = heading_motion)] - head_mot: i32, - - #[ubx(map_type = f64, scale = 1e-5, alias = heading_vehicle)] - head_veh: i32, - - h_acc: u32, - v_acc: u32, - s_acc: u32, - - #[ubx(map_type = f64, scale = 1e-5, alias = heading_accurracy)] - head_acc: u32, - - reserved2: [u8; 4], -} - -#[ubx_packet_recv] -#[ubx(class = 0x01, id = 0x05, fixed_payload_len = 32)] -struct NavAtt { - itow: u32, - version: u8, - reserved1: [u8; 3], - #[ubx(map_type = f64, scale = 1e-5, alias = vehicle_roll)] - roll: i32, - #[ubx(map_type = f64, scale = 1e-5, alias = vehicle_pitch)] - pitch: i32, - #[ubx(map_type = f64, scale = 1e-5, alias = vehicle_heading)] - heading: i32, - #[ubx(map_type = f64, scale = 1e-5, alias = vehicle_roll_accuracy)] - acc_roll: u32, - #[ubx(map_type = f64, scale = 1e-5, alias = vehicle_pitch_accuracy)] - acc_pitch: u32, - #[ubx(map_type = f64, scale = 1e-5, alias = vehicle_heading_accuracy)] - acc_heading: u32, -} - -#[ubx_packet_recv] -#[ubx(class = 0x01, id = 0x22, fixed_payload_len = 20)] -struct NavClock { - itow: u32, - clk_b: i32, - clk_d: i32, - t_acc: u32, - f_acc: u32, -} - -#[ubx_extend_bitflags] -#[ubx(from, rest_reserved)] -bitflags! { - #[derive(Debug)] - /// Fix status flags for `HnrPvt` - pub struct HnrPvtFlags: u8 { - /// position and velocity valid and within DOP and ACC Masks - const GPS_FIX_OK = 0x01; - /// DGPS used - const DIFF_SOLN = 0x02; - /// 1 = heading of vehicle is valid - const WKN_SET = 0x04; - const TOW_SET = 0x08; - const HEAD_VEH_VALID = 0x10; - } -} - -#[ubx_extend_bitflags] -#[ubx(from, rest_reserved)] -bitflags! { - #[derive(Debug)] - pub struct HnrPvtValidFlags: u8 { - const VALID_DATE = 0x01; - const VALID_TIME = 0x02; - const FULLY_RESOLVED = 0x04; - } -} - -#[ubx_packet_recv] -#[ubx(class = 0x02, id = 0x13, max_payload_len = 72)] -struct RxmSfrbx { - gnss_id: u8, - sv_id: u8, - reserved1: u8, - freq_id: u8, - num_words: u8, - reserved2: u8, - version: u8, - reserved3: u8, - #[ubx( - map_type = DwrdIter, - from = DwrdIter::new, - is_valid = DwrdIter::is_valid, - may_fail, - )] - dwrd: [u8; 0], -} - -#[derive(Debug, Clone)] -pub struct DwrdIter<'a>(core::slice::ChunksExact<'a, u8>); - -impl<'a> DwrdIter<'a> { - fn new(bytes: &'a [u8]) -> Self { - DwrdIter(bytes.chunks_exact(4)) - } - - fn is_valid(bytes: &'a [u8]) -> bool { - bytes.len() % 4 == 0 - } -} - -impl<'a> core::iter::Iterator for DwrdIter<'a> { - type Item = u32; - - fn next(&mut self) -> Option { - self.0 - .next() - .map(|bytes| u32::from_le_bytes(bytes.try_into().unwrap())) - } -} - -#[ubx_packet_recv] -#[ubx(class = 0x01, id = 0x11, fixed_payload_len = 20)] -struct NavVelECEF { - itow: u32, - ecef_vx: i32, - ecef_vy: i32, - ecef_vz: u32, - s_acc: u32, -} - -#[ubx_packet_recv] -#[ubx(class = 0x13, id = 0x00, fixed_payload_len = 68)] -struct MgaGpsEPH { - msg_type: u8, - version: u8, - sv_id: u8, - reserved1: u8, - fit_interval: u8, - ura_index: u8, - sv_health: u8, - #[ubx(map_type = f64, scale = 2e-31)] - tgd: i8, - iodc: u16, - #[ubx(map_type = f64, scale = 2e+4)] - toc: u16, - reserved2: u8, - #[ubx(map_type = f64, scale = 2e-55)] - af2: i8, - #[ubx(map_type = f64, scale = 2e-43)] - afl: i16, - #[ubx(map_type = f64, scale = 2e-31)] - af0: i32, - #[ubx(map_type = f64, scale = 2e-5)] - crs: i16, - #[ubx(map_type = f64, scale = 2e-43)] - delta_n: i16, - #[ubx(map_type = f64, scale = 2e-31)] - m0: i32, - #[ubx(map_type = f64, scale = 2e-29)] - cuc: i16, - #[ubx(map_type = f64, scale = 2e-29)] - cus: i16, - #[ubx(map_type = f64, scale = 2e-33)] - e: u32, - #[ubx(map_type = f64, scale = 2e-19)] - sqrt_a: u32, - #[ubx(map_type = f64, scale = 2e+4)] - toe: u16, - #[ubx(map_type = f64, scale = 2e-29)] - cic: i16, - #[ubx(map_type = f64, scale = 2e-31)] - omega0: i32, - #[ubx(map_type = f64, scale = 2e-29)] - cis: i16, - #[ubx(map_type = f64, scale = 2e-5)] - crc: i16, - #[ubx(map_type = f64, scale = 2e-31)] - i0: i32, - #[ubx(map_type = f64, scale = 2e-31)] - omega: i32, - #[ubx(map_type = f64, scale = 2e-43)] - omega_dot: i32, - #[ubx(map_type = f64, scale = 2e-43)] - idot: i16, - reserved3: [u8; 2], -} - -#[ubx_packet_recv] -#[ubx(class = 0x02, id = 0x15, fixed_payload_len = 32)] -#[derive(Debug)] -pub struct RxmRawxInfo { - pr_mes: f64, - cp_mes: f64, - do_mes: f32, - gnss_id: u8, - sv_id: u8, - reserved2: u8, - freq_id: u8, - lock_time: u16, - cno: u8, - #[ubx(map_type = StdevFlags)] - pr_stdev: u8, - #[ubx(map_type = StdevFlags)] - cp_stdev: u8, - #[ubx(map_type = StdevFlags)] - do_stdev: u8, - #[ubx(map_type = TrkStatFlags)] - trk_stat: u8, - reserved3: u8, -} - -#[ubx_extend_bitflags] -#[ubx(from, rest_reserved)] -bitflags! { - #[derive(Debug)] - pub struct StdevFlags: u8 { - const STD_1 = 0x01; - const STD_2 = 0x02; - const STD_3 = 0x04; - const STD_4 = 0x08; - } -} - -#[ubx_extend_bitflags] -#[ubx(from, rest_reserved)] -bitflags! { - #[derive(Debug)] - pub struct TrkStatFlags: u8 { - const PR_VALID = 0x01; - const CP_VALID = 0x02; - const HALF_CYCLE = 0x04; - const SUB_HALF_CYCLE = 0x08; - } -} - -#[derive(Debug, Clone)] -pub struct RxmRawxInfoIter<'a>(core::slice::ChunksExact<'a, u8>); - -impl<'a> RxmRawxInfoIter<'a> { - fn new(data: &'a [u8]) -> Self { - Self(data.chunks_exact(32)) - } - - fn is_valid(bytes: &'a [u8]) -> bool { - bytes.len() % 32 == 0 - } -} - -impl<'a> core::iter::Iterator for RxmRawxInfoIter<'a> { - type Item = RxmRawxInfoRef<'a>; - - fn next(&mut self) -> Option { - self.0.next().map(RxmRawxInfoRef) - } -} - -/// This message is used to retrieve a unique chip identifier -#[ubx_packet_recv] -#[ubx(class = 0x27, id = 0x03, fixed_payload_len = 9)] -struct SecUniqId { - version: u8, - reserved1: [u8; 3], - unique_id: [u8; 5], -} - -define_recv_packets!( - enum PacketRef { - _ = UbxUnknownPacketRef, - NavPosLlh, - NavStatus, - NavDop, - NavPvt, - NavSolution, - NavVelNed, - NavHpPosLlh, - NavHpPosEcef, - NavTimeUTC, - NavTimeLs, - NavSat, - NavEoe, - NavOdo, - CfgOdo, - MgaAck, - MgaGpsIono, - MgaGpsEph, - MgaGloEph, - AlpSrv, - AckAck, - AckNak, - CfgItfm, - CfgPrtI2c, - CfgPrtSpi, - CfgPrtUart, - CfgNav5, - CfgAnt, - CfgTmode2, - CfgTmode3, - CfgTp5, - InfError, - InfWarning, - InfNotice, - InfTest, - InfDebug, - RxmRawx, - TimTp, - TimTm2, - MonVer, - MonGnss, - MonHw, - RxmRtcm, - EsfMeas, - EsfIns, - HnrAtt, - HnrIns, - HnrPvt, - NavAtt, - NavClock, - NavVelECEF, - MgaGpsEPH, - RxmSfrbx, - EsfRaw, - TimSvin, - SecUniqId, - } -); - -#[test] -fn test_mon_ver_interpret() { - let payload: [u8; 160] = [ - 82, 79, 77, 32, 67, 79, 82, 69, 32, 51, 46, 48, 49, 32, 40, 49, 48, 55, 56, 56, 56, 41, 0, - 0, 0, 0, 0, 0, 0, 0, 48, 48, 48, 56, 48, 48, 48, 48, 0, 0, 70, 87, 86, 69, 82, 61, 83, 80, - 71, 32, 51, 46, 48, 49, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 80, 82, 79, 84, 86, - 69, 82, 61, 49, 56, 46, 48, 48, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 71, 80, - 83, 59, 71, 76, 79, 59, 71, 65, 76, 59, 66, 68, 83, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 83, 66, 65, 83, 59, 73, 77, 69, 83, 59, 81, 90, 83, 83, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, - ]; - assert_eq!(Ok(()), ::validate(&payload)); - let ver = MonVerRef(&payload); - assert_eq!("ROM CORE 3.01 (107888)", ver.software_version()); - assert_eq!("00080000", ver.hardware_version()); - let mut it = ver.extension(); - assert_eq!("FWVER=SPG 3.01", it.next().unwrap()); - assert_eq!("PROTVER=18.00", it.next().unwrap()); - assert_eq!("GPS;GLO;GAL;BDS", it.next().unwrap()); - assert_eq!("SBAS;IMES;QZSS", it.next().unwrap()); - assert_eq!(None, it.next()); -} diff --git a/ublox/ublox/src/ubx_packets/types.rs b/ublox/ublox/src/ubx_packets/types.rs deleted file mode 100644 index 4b93b7d..0000000 --- a/ublox/ublox/src/ubx_packets/types.rs +++ /dev/null @@ -1,152 +0,0 @@ -use super::packets::*; -use crate::error::DateTimeError; -use chrono::prelude::*; -use core::{convert::TryFrom, fmt}; - -/// Represents a world position, can be constructed from NavPosLlh and NavPvt packets. -#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] -#[derive(Debug, Clone, Copy)] -pub struct Position { - /// Logitude in degrees - pub lon: f64, - - /// Latitude in degrees - pub lat: f64, - - /// Altitude in meters - pub alt: f64, -} - -/// Represents a world position in the ECEF coordinate system -#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] -#[derive(Debug, Clone, Copy)] -pub struct PositionECEF { - /// x coordinates in meters - pub x: f64, - - /// y coordinates in meters - pub y: f64, - - /// z coordinates in meters - pub z: f64, -} - -#[derive(Debug, Clone, Copy)] -#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] -pub struct Velocity { - /// m/s over the ground - pub speed: f64, - - /// Heading in degrees - pub heading: f64, // degrees -} - -impl<'a> From<&NavPosLlhRef<'a>> for Position { - fn from(packet: &NavPosLlhRef<'a>) -> Self { - Position { - lon: packet.lon_degrees(), - lat: packet.lat_degrees(), - alt: packet.height_msl(), - } - } -} - -impl<'a> From<&NavHpPosLlhRef<'a>> for Position { - fn from(packet: &NavHpPosLlhRef<'a>) -> Self { - Position { - lon: packet.lon_degrees() + packet.lon_hp_degrees(), - lat: packet.lat_degrees() + packet.lat_hp_degrees(), - alt: packet.height_msl() + packet.height_hp_msl(), - } - } -} - -impl<'a> From<&NavHpPosEcefRef<'a>> for PositionECEF { - fn from(packet: &NavHpPosEcefRef<'a>) -> Self { - PositionECEF { - x: 10e-2 * (packet.ecef_x_cm() + 0.1 * packet.ecef_x_hp_mm()), - y: 10e-2 * (packet.ecef_y_cm() + 0.1 * packet.ecef_y_hp_mm()), - z: 10e-2 * (packet.ecef_z_cm() + 0.1 * packet.ecef_z_hp_mm()), - } - } -} - -impl<'a> From<&NavVelNedRef<'a>> for Velocity { - fn from(packet: &NavVelNedRef<'a>) -> Self { - Velocity { - speed: packet.ground_speed(), - heading: packet.heading_degrees(), - } - } -} - -impl<'a> From<&NavPvtRef<'a>> for Position { - fn from(packet: &NavPvtRef<'a>) -> Self { - Position { - lon: packet.lon_degrees(), - lat: packet.lat_degrees(), - alt: packet.height_msl(), - } - } -} - -impl<'a> From<&NavPvtRef<'a>> for Velocity { - fn from(packet: &NavPvtRef<'a>) -> Self { - Velocity { - speed: packet.ground_speed(), - heading: packet.heading_degrees(), - } - } -} - -impl<'a> TryFrom<&NavPvtRef<'a>> for DateTime { - type Error = DateTimeError; - fn try_from(sol: &NavPvtRef<'a>) -> Result { - let date = NaiveDate::from_ymd_opt( - i32::from(sol.year()), - u32::from(sol.month()), - u32::from(sol.day()), - ) - .ok_or(DateTimeError::InvalidDate)?; - let time = NaiveTime::from_hms_opt( - u32::from(sol.hour()), - u32::from(sol.min()), - u32::from(sol.sec()), - ) - .ok_or(DateTimeError::InvalidTime)?; - const NANOS_LIM: u32 = 1_000_000_000; - if (sol.nanosecond().wrapping_abs() as u32) >= NANOS_LIM { - return Err(DateTimeError::InvalidNanoseconds); - } - - let dt = NaiveDateTime::new(date, time) - + chrono::Duration::nanoseconds(i64::from(sol.nanosecond())); - - Ok(DateTime::from_naive_utc_and_offset(dt, Utc)) - } -} - -pub(crate) struct FieldIter(pub(crate) I); - -impl fmt::Debug for FieldIter -where - I: fmt::Debug, -{ - fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { - self.0.fmt(f) - } -} - -#[cfg(feature = "serde")] -impl serde::Serialize for FieldIter -where - I: Iterator + Clone, - I::Item: serde::Serialize, -{ - fn serialize(&self, serializer: S) -> Result - where - S: serde::Serializer, - { - serializer.collect_seq(self.0.clone()) - } -} diff --git a/ublox/ublox/tests/generator_test.rs b/ublox/ublox/tests/generator_test.rs deleted file mode 100644 index afad89c..0000000 --- a/ublox/ublox/tests/generator_test.rs +++ /dev/null @@ -1,14 +0,0 @@ -use ublox::{CfgMsgSinglePortBuilder, NavPosLlh, NavStatus}; - -#[test] -fn test_cfg_msg_simple() { - assert_eq!( - [0xb5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x02, 0x01, 0x0E, 0x47], - CfgMsgSinglePortBuilder::set_rate_for::(1).into_packet_bytes() - ); - - assert_eq!( - [0xb5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x03, 0x01, 0x0F, 0x49], - CfgMsgSinglePortBuilder::set_rate_for::(1).into_packet_bytes() - ); -} diff --git a/ublox/ublox/tests/parser_binary_dump_test.rs b/ublox/ublox/tests/parser_binary_dump_test.rs deleted file mode 100644 index b1df650..0000000 --- a/ublox/ublox/tests/parser_binary_dump_test.rs +++ /dev/null @@ -1,135 +0,0 @@ -#![cfg(feature = "alloc")] - -use cpu_time::ProcessTime; -use rand::{thread_rng, Rng}; -use std::{env, ffi::OsString, fs, path::Path}; -use ublox::{PacketRef, Parser, ParserError}; - -/// To run test against file with path X, -/// use such command (if use shell compatible with /bin/sh). -/// ```sh -/// UBX_BIG_LOG_PATH=X time cargo test --release test_parse_big_dump -- --ignored --nocapture -/// ``` -/// Binary dump should be at path X and at path X.meta, should be file with meta -/// information about what packets you expect find in dump, example: -/// ```sh -/// $ cat /var/tmp/gps.bin.meta -///wrong_chksum=0 -///other_errors=0 -///nav_pos_llh=38291 -///nav_stat=38291 -///unknown=120723 -///ack_ack=1 -/// ``` -#[test] -#[ignore] -fn test_parse_big_dump() { - let ubx_big_log_path = env::var("UBX_BIG_LOG_PATH").unwrap(); - let ubx_big_log_path = Path::new(&ubx_big_log_path); - - let meta_ext: OsString = if let Some(ext) = ubx_big_log_path.extension() { - let mut ext: OsString = ext.into(); - ext.push(".meta"); - ext - } else { - "meta".into() - }; - let ubx_big_log_path_meta = ubx_big_log_path.with_extension(meta_ext); - let meta_data = fs::read_to_string(ubx_big_log_path_meta).unwrap(); - let expect = parse_meta_data(&meta_data).unwrap(); - - let biglog = fs::read(ubx_big_log_path).unwrap(); - const MAX_SIZE: usize = 100; - let mut read_sizes = Vec::with_capacity(biglog.len() / MAX_SIZE / 2); - let mut rng = thread_rng(); - let mut i = 0; - while i < biglog.len() { - let chunk: usize = rng.gen_range(1..MAX_SIZE); - let chunk = (biglog.len() - i).min(chunk); - read_sizes.push(chunk); - i += chunk; - } - - let mut meta = Meta::default(); - let mut log = biglog.as_slice(); - let mut parser = Parser::default(); - - let start = ProcessTime::now(); - for chunk_size in &read_sizes { - let (buf, rest) = log.split_at(*chunk_size); - log = rest; - let mut it = parser.consume(buf); - while let Some(pack) = it.next() { - match pack { - Ok(pack) => match pack { - PacketRef::AckAck(_) => meta.ack_ack += 1, - PacketRef::NavPosLlh(_) => meta.nav_pos_llh += 1, - PacketRef::NavStatus(_) => meta.nav_stat += 1, - _ => meta.unknown += 1, - }, - Err(ParserError::InvalidChecksum { .. }) => meta.wrong_chksum += 1, - Err(_) => meta.other_errors += 1, - } - } - } - let cpu_time = start.elapsed(); - println!( - "parse time of {}: {:?}", - ubx_big_log_path.display(), - cpu_time - ); - - assert_eq!(expect, meta); -} - -#[derive(Default, PartialEq, Debug)] -struct Meta { - wrong_chksum: usize, - other_errors: usize, - nav_pos_llh: usize, - nav_stat: usize, - ack_ack: usize, - unknown: usize, -} - -fn parse_meta_data(text: &str) -> Result { - let mut wrong_chksum = None; - let mut other_errors = None; - let mut nav_pos_llh = None; - let mut nav_stat = None; - let mut ack_ack = None; - let mut unknown = None; - - for line in text.lines() { - let mut it = line.split('='); - let name = it - .next() - .ok_or_else(|| "missed variable name".to_string())? - .trim(); - let value = it - .next() - .ok_or_else(|| "missed variable value".to_string())? - .trim(); - let value: usize = value - .parse() - .map_err(|err| format!("Can not parse integer as usize: {}", err))?; - match name { - "wrong_chksum" => wrong_chksum = Some(value), - "other_errors" => other_errors = Some(value), - "nav_pos_llh" => nav_pos_llh = Some(value), - "nav_stat" => nav_stat = Some(value), - "ack_ack" => ack_ack = Some(value), - "unknown" => unknown = Some(value), - _ => return Err(format!("wrong field name: '{}'", name)), - } - } - let missed = || "missed field".to_string(); - Ok(Meta { - wrong_chksum: wrong_chksum.ok_or_else(missed)?, - other_errors: other_errors.ok_or_else(missed)?, - nav_pos_llh: nav_pos_llh.ok_or_else(missed)?, - nav_stat: nav_stat.ok_or_else(missed)?, - ack_ack: ack_ack.ok_or_else(missed)?, - unknown: unknown.ok_or_else(missed)?, - }) -} diff --git a/ublox/ublox/tests/parser_tests.rs b/ublox/ublox/tests/parser_tests.rs deleted file mode 100644 index ef27d13..0000000 --- a/ublox/ublox/tests/parser_tests.rs +++ /dev/null @@ -1,323 +0,0 @@ -#![cfg(feature = "alloc")] - -use ublox::{ - CfgNav5Builder, CfgNav5DynModel, CfgNav5FixMode, CfgNav5Params, CfgNav5UtcStandard, PacketRef, - Parser, ParserError, ParserIter, -}; - -macro_rules! my_vec { - ($($x:expr),*) => {{ - let v: Vec> = vec![$($x),*]; - v - }} - } - -fn extract_only_ack_ack( - mut it: ParserIter, -) -> Vec> { - let mut ret = vec![]; - while let Some(pack) = it.next() { - match pack { - Ok(PacketRef::AckAck(pack)) => { - ret.push(Ok((pack.class(), pack.msg_id()))); - }, - Err(err) => ret.push(Err(err)), - _ => panic!(), - } - } - ret -} - -static FULL_ACK_ACK_PACK: [u8; 10] = [0xb5, 0x62, 0x5, 0x1, 0x2, 0x0, 0x6, 0x1, 0xf, 0x38]; - -#[test] -fn test_parse_empty_buffer() { - let mut parser = Parser::default(); - assert!(parser.is_buffer_empty()); - assert_eq!(my_vec![], extract_only_ack_ack(parser.consume(&[]))); - assert!(parser.is_buffer_empty()); -} - -#[test] -fn test_parse_ack_ack_byte_by_byte() { - let mut parser = Parser::default(); - for b in FULL_ACK_ACK_PACK.iter().take(FULL_ACK_ACK_PACK.len() - 1) { - assert_eq!(my_vec![], extract_only_ack_ack(parser.consume(&[*b]))); - assert!(!parser.is_buffer_empty()); - } - let last_byte = FULL_ACK_ACK_PACK[FULL_ACK_ACK_PACK.len() - 1]; - assert_eq!( - my_vec![Ok((6, 1))], - extract_only_ack_ack(parser.consume(&[last_byte])), - ); - assert!(parser.is_buffer_empty()); -} - -#[test] -fn test_parse_ack_ack_in_one_go() { - let mut parser = Parser::default(); - assert_eq!( - my_vec![Ok((6, 1))], - extract_only_ack_ack(parser.consume(&FULL_ACK_ACK_PACK)), - ); - assert!(parser.is_buffer_empty()); -} - -#[test] -fn test_parse_ack_ack_bad_checksum() { - let mut parser = Parser::default(); - let mut bad_pack = FULL_ACK_ACK_PACK; - bad_pack[bad_pack.len() - 3] = 5; - assert_eq!( - my_vec![Err(ParserError::InvalidChecksum { - expect: 0x380f, - got: 0x3c13 - })], - extract_only_ack_ack(parser.consume(&bad_pack)), - ); - assert_eq!(0, parser.buffer_len()); - - let mut two_packs = FULL_ACK_ACK_PACK.to_vec(); - two_packs.extend_from_slice(&FULL_ACK_ACK_PACK); - assert_eq!( - my_vec![Ok((6, 1)), Ok((6, 1))], - extract_only_ack_ack(parser.consume(&two_packs)), - ); - assert!(parser.is_buffer_empty()); -} - -#[test] -fn test_parse_ack_ack_parted_two_packets() { - let mut parser = Parser::default(); - assert_eq!( - my_vec![], - extract_only_ack_ack(parser.consume(&FULL_ACK_ACK_PACK[0..5])), - ); - assert_eq!(5, parser.buffer_len()); - let mut rest_and_next = (FULL_ACK_ACK_PACK[5..]).to_vec(); - rest_and_next.extend_from_slice(&FULL_ACK_ACK_PACK); - assert_eq!( - my_vec![Ok((6, 1)), Ok((6, 1))], - extract_only_ack_ack(parser.consume(&rest_and_next)), - ); - assert!(parser.is_buffer_empty()); -} - -#[test] -fn test_parse_ack_ack_two_in_one_go() { - let mut parser = Parser::default(); - let mut two_packs = FULL_ACK_ACK_PACK.to_vec(); - two_packs.extend_from_slice(&FULL_ACK_ACK_PACK); - assert_eq!( - my_vec![Ok((6, 1)), Ok((6, 1))], - extract_only_ack_ack(parser.consume(&two_packs)) - ); - assert!(parser.is_buffer_empty()); -} - -#[test] -fn test_parse_ack_ack_garbage_before() { - let mut parser = Parser::default(); - let mut garbage_before = vec![0x00, 0x06, 0x01, 0x0f, 0x38]; - garbage_before.extend_from_slice(&FULL_ACK_ACK_PACK); - assert_eq!( - my_vec![Ok((6, 1))], - extract_only_ack_ack(parser.consume(&garbage_before)), - "garbage before1" - ); - assert!(parser.is_buffer_empty()); - - let mut garbage_before = vec![0xb5, 0xb5, 0x62, 0x62, 0x38]; - garbage_before.extend_from_slice(&FULL_ACK_ACK_PACK); - assert_eq!( - my_vec![Ok((6, 1))], - extract_only_ack_ack(parser.consume(&garbage_before)), - "garbage before2" - ); - assert!(parser.is_buffer_empty()); -} - -#[test] -fn test_parse_cfg_nav5() { - let bytes = CfgNav5Builder { - mask: CfgNav5Params::DYN, - dyn_model: CfgNav5DynModel::AirborneWithLess1gAcceleration, - fix_mode: CfgNav5FixMode::Only3D, - fixed_alt: 100.17, - fixed_alt_var: 0.0017, - min_elev_degrees: 17, - pdop: 1.7, - tdop: 1.7, - pacc: 17, - tacc: 17, - static_hold_thresh: 2.17, - dgps_time_out: 17, - cno_thresh_num_svs: 17, - cno_thresh: 17, - static_hold_max_dist: 0x1717, - utc_standard: CfgNav5UtcStandard::UtcChina, - ..CfgNav5Builder::default() - } - .into_packet_bytes(); - - let mut parser = Parser::default(); - let mut found = false; - let mut it = parser.consume(&bytes); - while let Some(pack) = it.next() { - match pack { - Ok(PacketRef::CfgNav5(pack)) => { - found = true; - - assert_eq!(CfgNav5Params::DYN, pack.mask()); - assert_eq!( - CfgNav5DynModel::AirborneWithLess1gAcceleration, - pack.dyn_model() - ); - assert_eq!(CfgNav5FixMode::Only3D, pack.fix_mode()); - assert!((pack.fixed_alt() - 100.17).abs() < 0.01); - assert_eq!(pack.fixed_alt_raw(), 10017); - assert!((pack.fixed_alt_var() - 0.0017).abs() < 0.000_1); - assert_eq!(17, pack.min_elev_degrees()); - assert!((pack.pdop() - 1.7).abs() < 0.1); - assert!((pack.tdop() - 1.7).abs() < 0.1); - assert_eq!(17, pack.pacc()); - assert_eq!(17, pack.tacc()); - assert!((pack.static_hold_thresh() - 2.17) < 0.01); - assert_eq!(17, pack.dgps_time_out()); - assert_eq!(17, pack.cno_thresh_num_svs()); - assert_eq!(17, pack.cno_thresh()); - assert_eq!(0x1717, pack.static_hold_max_dist()); - assert_eq!(CfgNav5UtcStandard::UtcChina, pack.utc_standard()); - }, - _ => panic!(), - } - } - assert!(found); -} - -#[test] -#[cfg(feature = "serde")] -fn test_esf_meas_serialize() { - let ret = [ - 181, 98, 16, 2, 16, 0, 243, 121, 129, 1, 24, 8, 0, 0, 77, 100, 0, 11, 211, 148, 129, 1, - 213, 198, - ]; - - let mut parser = Parser::default(); - let mut found = false; - let mut it = parser.consume(&ret); - - while let Some(pack) = it.next() { - match pack { - Ok(pack) => { - let expected = serde_json::json! { - { - "class": 16, - "msg_id": 2, - "time_tag": 25262579, - "flags": 2072, - "id": 0, - "data": [ - { - "data_type": 11, - "data_field": 25677 - } - ], - "calib_tag": 25269459 - } - }; - let actual = serde_json::to_value(&pack).unwrap(); - assert_eq!(expected, actual); - if let PacketRef::EsfMeas(pack) = pack { - let expected = serde_json::json! { - { - "time_tag": 25262579, - "flags": 2072, - "id": 0, - "data": [ - { - "data_type": 11, - "data_field": 25677 - } - ], - "calib_tag": 25269459 - } - }; - let actual = serde_json::to_value(&pack).unwrap(); - assert_eq!(expected, actual); - } else { - panic!(); - } - found = true; - }, - _ => panic!(), - } - } - assert!(found); -} - -#[test] -fn test_zero_sized_ackack() { - let ack_ack = [0xb5, 0x62, 0x05, 0x01, 0x00, 0x00, 0x06, 0x17]; - let mut parser = Parser::default(); - let mut it = parser.consume(&ack_ack); - match it.next() { - Some(Ok(PacketRef::Unknown(_))) => { - // This is expected - }, - _ => panic!(), - } - assert!(it.next().is_none()); -} - -#[test] -fn test_double_start_at_end() { - #[rustfmt::skip] - let bytes = [ - 0xb5, 0x62, // Extraneous start header - 0xb5, 0x62, 0x05, 0x01, 0x00, 0x00, 0x06, 0x17, // Zero-sized packet - ]; - - let mut buf = [0; 10]; - let buf = ublox::FixedLinearBuffer::new(&mut buf[..]); - let mut parser = ublox::Parser::new(buf); - - for byte in bytes.iter() { - parser.consume(&[*byte]); - } - - let ack_ack = [0xb5, 0x62, 0x5, 0x1, 0x2, 0x0, 0x4, 0x5, 0x11, 0x38]; - { - let mut it = parser.consume(&ack_ack); - match it.next() { - Some(Err(_)) => { - // First, a buffer-too-small error - }, - _ => panic!(), - } - match it.next() { - Some(Ok(PacketRef::Unknown(_))) => { - // Then an unknown packet - }, - _ => panic!(), - } - match it.next() { - Some(Ok(PacketRef::AckAck(_))) => { - // Then the ackack we passed - }, - _ => panic!(), - } - assert!(it.next().is_none()); - } - let mut it = parser.consume(&ack_ack); - match it.next() { - Some(Ok(ublox::PacketRef::AckAck { .. })) => { - // This is what we expect - }, - _ => { - // Parsing other packets or ending the iteration is a failure - panic!(); - }, - } - assert!(it.next().is_none()); -} diff --git a/ublox/ublox_derive/Cargo.toml b/ublox/ublox_derive/Cargo.toml deleted file mode 100644 index 4dcfdf8..0000000 --- a/ublox/ublox_derive/Cargo.toml +++ /dev/null @@ -1,21 +0,0 @@ -[package] -authors = ["Lane Kolbly "] -description = "Utility macros for the ublox crate" -edition = "2021" -license = "MIT" -name = "ublox_derive" -version = "0.1.0" - -[lib] -proc-macro = true - -[dependencies] -proc-macro2 = "1.0" -quote = "1.0" -# cannot be bumped to major relese: -# see API changes at https://github.com/dtolnay/syn/releases/tag/2.0.0 -syn = {version = "1.0.109", features = ["extra-traits", "full"]} - -[dev-dependencies] -proc-macro2 = {version = "1.0", features = ["span-locations"]} -which = {version = "4.4.0", default-features = false} diff --git a/ublox/ublox_derive/src/input.rs b/ublox/ublox_derive/src/input.rs deleted file mode 100644 index 28df6cb..0000000 --- a/ublox/ublox_derive/src/input.rs +++ /dev/null @@ -1,704 +0,0 @@ -use crate::types::{ - BitFlagsMacro, BitFlagsMacroItem, PackDesc, PackField, PackFieldMapDesc, PackHeader, - PacketFlag, PayloadLen, RecvPackets, UbxEnumRestHandling, UbxExtendEnum, UbxTypeFromFn, - UbxTypeIntoFn, -}; -use proc_macro2::{Span, TokenStream}; -use quote::ToTokens; - -use std::num::NonZeroUsize; -use syn::{ - braced, parse::Parse, punctuated::Punctuated, spanned::Spanned, Attribute, Error, Fields, - Ident, Token, Type, -}; - -pub fn parse_packet_description( - struct_name: Ident, - attrs: Vec, - fields: Fields, -) -> syn::Result { - let main_sp = struct_name.span(); - - let header = parse_ubx_attr(&attrs, &struct_name)?; - let struct_comment = extract_item_comment(&attrs)?; - - let name = struct_name.to_string(); - let fields = parse_fields(fields)?; - - if let Some(field) = fields - .iter() - .rev() - .skip(1) - .find(|f| f.size_bytes.is_none() && f.size_fn().is_none()) - { - return Err(Error::new( - field.name.span(), - "Non-finite size for field which is not the last field", - )); - } - - let ret = PackDesc { - name, - header, - comment: struct_comment, - fields, - }; - - if ret.header.payload_len.fixed().map(usize::from) == ret.packet_payload_size() { - Ok(ret) - } else { - Err(Error::new( - main_sp, - format!( - "Calculated packet size ({:?}) doesn't match specified ({:?})", - ret.packet_payload_size(), - ret.header.payload_len - ), - )) - } -} - -pub fn parse_ubx_enum_type( - enum_name: Ident, - attrs: Vec, - in_variants: Punctuated, -) -> syn::Result { - let (from_fn, into_fn, rest_handling) = - parse_ubx_extend_attrs("#[ubx_extend]", enum_name.span(), &attrs)?; - - let attr = attrs - .iter() - .find(|a| a.path.is_ident("repr")) - .ok_or_else(|| { - Error::new( - enum_name.span(), - format!("No repr attribute for ubx_type enum {}", enum_name), - ) - })?; - let meta = attr.parse_meta()?; - let repr: Type = match meta { - syn::Meta::List(list) if list.nested.len() == 1 => { - if let syn::NestedMeta::Meta(syn::Meta::Path(ref p)) = list.nested[0] { - if !p.is_ident("u8") { - unimplemented!(); - } - } else { - return Err(Error::new( - list.nested[0].span(), - "Invalid repr attribute for ubx_type enum", - )); - } - syn::parse_quote! { u8 } - }, - _ => { - return Err(Error::new( - attr.span(), - "Invalid repr attribute for ubx_type enum", - )) - }, - }; - let mut variants = Vec::with_capacity(in_variants.len()); - for var in in_variants { - if syn::Fields::Unit != var.fields { - return Err(Error::new( - var.fields.span(), - "Invalid variant for ubx_type enum", - )); - } - let var_sp = var.ident.span(); - let (_, expr) = var - .discriminant - .ok_or_else(|| Error::new(var_sp, "ubx_type enum variant should has value"))?; - let variant_value = if let syn::Expr::Lit(syn::ExprLit { - lit: syn::Lit::Int(litint), - .. - }) = expr - { - litint.base10_parse::()? - } else { - return Err(Error::new( - expr.span(), - "Invalid variant value for ubx_type enum", - )); - }; - variants.push((var.ident, variant_value)); - } - - let attrs = attrs - .into_iter() - .filter(|x| !x.path.is_ident("ubx") && !x.path.is_ident("ubx_extend")) - .collect(); - - Ok(UbxExtendEnum { - attrs, - name: enum_name, - repr, - from_fn, - into_fn, - rest_handling, - variants, - }) -} - -pub fn parse_bitflags(mac: syn::ItemMacro) -> syn::Result { - let (from_fn, into_fn, rest_handling) = - parse_ubx_extend_attrs("#[ubx_extend_bitflags]", mac.span(), &mac.attrs)?; - - let ast: BitFlagsAst = syn::parse2(mac.mac.tokens)?; - - let valid_types: [(Type, u32); 3] = [ - (syn::parse_quote!(u8), 1), - (syn::parse_quote!(u16), 2), - (syn::parse_quote!(u32), 4), - ]; - let nbits = if let Some((_ty, size)) = valid_types.iter().find(|x| x.0 == ast.repr_ty) { - size * 8 - } else { - let mut valid_type_names = String::with_capacity(200); - for (t, _) in &valid_types { - if !valid_type_names.is_empty() { - valid_type_names.push_str(", "); - } - valid_type_names.push_str(&t.into_token_stream().to_string()); - } - return Err(Error::new( - ast.repr_ty.span(), - format!("Not supported type, expect one of {:?}", valid_type_names), - )); - }; - - let mut consts = Vec::with_capacity(ast.items.len()); - for item in ast.items { - consts.push(BitFlagsMacroItem { - attrs: item.attrs, - name: item.name, - value: item.value.base10_parse()?, - }); - } - - Ok(BitFlagsMacro { - nbits, - vis: ast.vis, - attrs: ast.attrs, - name: ast.ident, - repr_ty: ast.repr_ty, - consts, - from_fn, - into_fn, - rest_handling, - }) -} - -pub fn parse_idents_list(input: proc_macro2::TokenStream) -> syn::Result { - syn::parse2(input) -} - -fn parse_ubx_extend_attrs( - ubx_extend_name: &str, - item_sp: Span, - attrs: &[Attribute], -) -> syn::Result<( - Option, - Option, - Option, -)> { - let attr = attrs - .iter() - .find(|a| a.path.is_ident("ubx")) - .ok_or_else(|| Error::new(item_sp, format!("No ubx attribute for {}", ubx_extend_name)))?; - let meta = attr.parse_meta()?; - let mut from_fn = None; - let mut rest_handling = None; - let mut into_fn = None; - let meta_sp = meta.span(); - match meta { - syn::Meta::List(list) => { - for item in list.nested { - if let syn::NestedMeta::Meta(syn::Meta::Path(p)) = item { - if p.is_ident("from") { - from_fn = Some(UbxTypeFromFn::From); - } else if p.is_ident("into_raw") { - into_fn = Some(UbxTypeIntoFn::Raw); - } else if p.is_ident("from_unchecked") { - from_fn = Some(UbxTypeFromFn::FromUnchecked); - } else if p.is_ident("rest_reserved") || p.is_ident("rest_error") { - if rest_handling.is_some() { - return Err(Error::new( - p.span(), - "rest_reserved or rest_error already defined", - )); - } - - rest_handling = Some(if p.is_ident("rest_reserved") { - UbxEnumRestHandling::Reserved - } else { - UbxEnumRestHandling::ErrorProne - }); - } else { - return Err(Error::new(p.span(), "Invalid ubx attribute")); - } - } else { - return Err(Error::new(item.span(), "Invalid ubx attribute")); - } - } - }, - _ => return Err(Error::new(attr.span(), "Invalid ubx attributes")), - } - - if from_fn == Some(UbxTypeFromFn::From) - && rest_handling == Some(UbxEnumRestHandling::ErrorProne) - { - return Err(Error::new( - meta_sp, - "you should use rest_error with from_unchecked", - )); - } - - Ok((from_fn, into_fn, rest_handling)) -} - -fn parse_ubx_attr(attrs: &[Attribute], struct_name: &Ident) -> syn::Result { - let attr = attrs - .iter() - .find(|a| a.path.is_ident("ubx")) - .ok_or_else(|| { - Error::new( - struct_name.span(), - format!("No ubx attribute for struct {}", struct_name), - ) - })?; - let meta = attr.parse_meta()?; - let meta = match meta { - syn::Meta::List(x) => x, - _ => return Err(Error::new(meta.span(), "Invalid ubx attribute syntax")), - }; - - let mut class = None; - let mut id = None; - let mut fixed_payload_len = None; - let mut flags = Vec::new(); - let mut max_payload_len = None; - - for e in &meta.nested { - match e { - syn::NestedMeta::Meta(syn::Meta::NameValue(syn::MetaNameValue { - path, lit, .. - })) => { - if path.is_ident("class") { - if class.is_some() { - return Err(Error::new(e.span(), "Duplicate \"class\" attribute")); - } - class = match lit { - syn::Lit::Int(x) => Some(x.base10_parse::()?), - _ => return Err(Error::new(lit.span(), "Should be integer literal")), - }; - } else if path.is_ident("id") { - if id.is_some() { - return Err(Error::new(e.span(), "Duplicate \"id\" attribute")); - } - id = match lit { - syn::Lit::Int(x) => Some(x.base10_parse::()?), - _ => return Err(Error::new(lit.span(), "Should be integer literal")), - }; - } else if path.is_ident("fixed_payload_len") { - if fixed_payload_len.is_some() { - return Err(Error::new( - e.span(), - "Duplicate \"fixed_payload_len\" attribute", - )); - } - fixed_payload_len = match lit { - syn::Lit::Int(x) => Some(x.base10_parse::()?), - _ => return Err(Error::new(lit.span(), "Should be integer literal")), - }; - } else if path.is_ident("max_payload_len") { - if max_payload_len.is_some() { - return Err(Error::new( - e.span(), - "Duplicate \"max_payload_len\" attribute", - )); - } - max_payload_len = match lit { - syn::Lit::Int(x) => Some(x.base10_parse::()?), - _ => return Err(Error::new(lit.span(), "Should be integer literal")), - }; - } else if path.is_ident("flags") { - if !flags.is_empty() { - return Err(Error::new(path.span(), "Duplicate flags")); - } - let my_flags = match lit { - syn::Lit::Str(x) => x.parse::()?, - _ => return Err(Error::new(lit.span(), "Should be string literal")), - }; - flags = my_flags.0.into_iter().collect(); - } else { - return Err(Error::new(path.span(), "Unsupported attribute")); - } - }, - _ => return Err(Error::new(e.span(), "Unsupported attribute")), - } - } - let class = class.ok_or_else(|| Error::new(meta.span(), "No \"class\" attribute"))?; - let id = id.ok_or_else(|| Error::new(meta.span(), "No \"id\" attribute"))?; - - let payload_len = match (max_payload_len, fixed_payload_len) { - (Some(x), None) => PayloadLen::Max(x), - (None, Some(x)) => PayloadLen::Fixed(x), - (Some(_), Some(_)) => { - return Err(Error::new( - meta.span(), - "You should not note max_payload_len AND fixed_payload_len", - )) - }, - (None, None) => { - return Err(Error::new( - meta.span(), - "You should note max_payload_len or fixed_payload_len", - )) - }, - }; - - Ok(PackHeader { - class, - id, - payload_len, - flags, - }) -} - -fn extract_item_comment(attrs: &[Attribute]) -> syn::Result { - let mut doc_comments = String::new(); - for a in attrs { - if a.path.is_ident("doc") { - let meta = a.parse_meta()?; - match meta { - syn::Meta::NameValue(syn::MetaNameValue { lit, .. }) => { - let lit = match lit { - syn::Lit::Str(s) => s, - _ => return Err(Error::new(lit.span(), "Invalid comment")), - }; - doc_comments.push_str(&lit.value()); - }, - _ => return Err(Error::new(a.span(), "Invalid comments")), - } - } - } - Ok(doc_comments) -} - -fn parse_fields(fields: Fields) -> syn::Result> { - let fields = match fields { - syn::Fields::Named(x) => x, - _ => { - return Err(Error::new(fields.span(), "Unsupported fields format")); - }, - }; - let mut ret = Vec::with_capacity(fields.named.len()); - for f in fields.named { - let f_sp = f.span(); - let syn::Field { - ident: name, - attrs, - ty, - .. - } = f; - let size_bytes = field_size_bytes(&ty)?; - - let name = name.ok_or_else(|| Error::new(f_sp, "No field name"))?; - let comment = extract_item_comment(&attrs)?; - let mut map = PackFieldMap::default(); - for a in attrs { - if !a.path.is_ident("doc") { - if !map.is_none() { - return Err(Error::new( - a.span(), - "Two map attributes for the same field", - )); - } - map = a.parse_args::()?; - } - } - - if let Some(ref map_ty) = map.map_type { - if map_ty.ty == ty { - return Err(Error::new( - map_ty.ty.span(), - "You map type to the same type", - )); - } - } - - let map = PackFieldMapDesc::new(map, &ty); - - ret.push(PackField { - name, - ty, - map, - comment, - size_bytes, - }); - } - - Ok(ret) -} - -mod kw { - syn::custom_keyword!(map_type); - syn::custom_keyword!(scale); - syn::custom_keyword!(alias); - syn::custom_keyword!(default_for_builder); - syn::custom_keyword!(may_fail); - syn::custom_keyword!(from); - syn::custom_keyword!(is_valid); - syn::custom_keyword!(get_as_ref); - syn::custom_keyword!(into); - syn::custom_keyword!(size_fn); -} - -#[derive(Default)] -pub struct PackFieldMap { - pub map_type: Option, - pub scale: Option, - pub alias: Option, - pub convert_may_fail: bool, - pub get_as_ref: bool, -} - -impl PackFieldMap { - fn is_none(&self) -> bool { - self.map_type.is_none() && self.scale.is_none() && self.alias.is_none() - } -} - -pub struct MapType { - pub ty: Type, - pub from_fn: Option, - pub is_valid_fn: Option, - pub into_fn: Option, - pub size_fn: Option, -} - -impl Parse for PackFieldMap { - fn parse(input: syn::parse::ParseStream) -> syn::Result { - let mut map = PackFieldMap::default(); - let mut map_ty = None; - let mut custom_from_fn: Option = None; - let mut custom_into_fn: Option = None; - let mut custom_is_valid_fn: Option = None; - let mut custom_size_fn: Option = None; - while !input.is_empty() { - let lookahead = input.lookahead1(); - - if lookahead.peek(kw::map_type) { - input.parse::()?; - input.parse::()?; - map_ty = Some(input.parse()?); - } else if lookahead.peek(kw::scale) { - input.parse::()?; - input.parse::()?; - map.scale = Some(input.parse()?); - } else if lookahead.peek(kw::alias) { - input.parse::()?; - input.parse::()?; - map.alias = Some(input.parse()?); - } else if lookahead.peek(kw::may_fail) { - input.parse::()?; - map.convert_may_fail = true; - } else if lookahead.peek(kw::from) { - input.parse::()?; - input.parse::()?; - custom_from_fn = Some(input.parse()?); - } else if lookahead.peek(kw::is_valid) { - input.parse::()?; - input.parse::()?; - custom_is_valid_fn = Some(input.parse()?); - } else if lookahead.peek(kw::size_fn) { - input.parse::()?; - input.parse::()?; - custom_size_fn = Some(input.parse()?); - } else if lookahead.peek(kw::get_as_ref) { - input.parse::()?; - map.get_as_ref = true; - } else if lookahead.peek(kw::into) { - input.parse::()?; - input.parse::()?; - custom_into_fn = Some(input.parse()?); - } else { - return Err(lookahead.error()); - } - - if input.peek(Token![,]) { - input.parse::()?; - } - } - - if let Some(map_ty) = map_ty { - map.map_type = Some(MapType { - ty: map_ty, - from_fn: custom_from_fn.map(ToTokens::into_token_stream), - is_valid_fn: custom_is_valid_fn.map(ToTokens::into_token_stream), - into_fn: custom_into_fn.map(ToTokens::into_token_stream), - size_fn: custom_size_fn.map(ToTokens::into_token_stream), - }); - } - - Ok(map) - } -} - -struct Comment(String); - -impl Parse for Comment { - fn parse(input: syn::parse::ParseStream) -> syn::Result { - if input.peek(Token![#]) && input.peek2(syn::token::Bracket) && input.peek3(Ident) { - let attrs = input.call(Attribute::parse_outer)?; - - Ok(Comment(extract_item_comment(&attrs)?)) - } else { - Ok(Comment(String::new())) - } - } -} - -fn field_size_bytes(ty: &Type) -> syn::Result> { - let valid_types: [(Type, NonZeroUsize); 8] = [ - (syn::parse_quote!(u8), NonZeroUsize::new(1).unwrap()), - (syn::parse_quote!(i8), NonZeroUsize::new(1).unwrap()), - (syn::parse_quote!(u16), NonZeroUsize::new(2).unwrap()), - (syn::parse_quote!(i16), NonZeroUsize::new(2).unwrap()), - (syn::parse_quote!(u32), NonZeroUsize::new(4).unwrap()), - (syn::parse_quote!(i32), NonZeroUsize::new(4).unwrap()), - (syn::parse_quote!(f32), NonZeroUsize::new(4).unwrap()), - (syn::parse_quote!(f64), NonZeroUsize::new(8).unwrap()), - ]; - if let Some((_ty, size)) = valid_types.iter().find(|x| x.0 == *ty) { - Ok(Some(*size)) - } else if let syn::Type::Array(ref fixed_array) = ty { - if *fixed_array.elem != syn::parse_quote!(u8) { - return Err(Error::new(fixed_array.elem.span(), "Only u8 supported")); - } - if let syn::Expr::Lit(syn::ExprLit { - lit: syn::Lit::Int(ref len), - .. - }) = fixed_array.len - { - let len_val: usize = len.base10_parse()?; - Ok(NonZeroUsize::new(len_val)) - } else { - Err(Error::new( - fixed_array.len.span(), - "Can not interpret array length", - )) - } - } else if let syn::Type::Reference(_) = ty { - Ok(None) - } else { - let mut valid_type_names = String::with_capacity(200); - for (t, _) in &valid_types { - if !valid_type_names.is_empty() { - valid_type_names.push_str(", "); - } - valid_type_names.push_str(&t.into_token_stream().to_string()); - } - Err(Error::new( - ty.span(), - format!("Not supported type, expect one of {:?}", valid_type_names), - )) - } -} - -struct BitFlagsAst { - attrs: Vec, - vis: syn::Visibility, - _struct_token: Token![struct], - ident: Ident, - _colon_token: Token![:], - repr_ty: Type, - _brace_token: syn::token::Brace, - items: Punctuated, -} - -struct BitFlagsAstConst { - attrs: Vec, - _const_token: Token![const], - name: Ident, - _eq_token: Token![=], - value: syn::LitInt, -} - -impl Parse for BitFlagsAst { - fn parse(input: syn::parse::ParseStream) -> syn::Result { - let attrs = input.call(Attribute::parse_outer)?; - let vis = input.parse()?; - let struct_token = input.parse()?; - let ident = input.parse()?; - let colon_token = input.parse()?; - let repr_ty = input.parse()?; - let content; - let brace_token = braced!(content in input); - let items = content.parse_terminated(BitFlagsAstConst::parse)?; - Ok(Self { - attrs, - vis, - _struct_token: struct_token, - ident, - _colon_token: colon_token, - repr_ty, - _brace_token: brace_token, - items, - }) - } -} - -impl Parse for BitFlagsAstConst { - fn parse(input: syn::parse::ParseStream) -> syn::Result { - Ok(Self { - attrs: input.call(Attribute::parse_outer)?, - _const_token: input.parse()?, - name: input.parse()?, - _eq_token: input.parse()?, - value: input.parse()?, - }) - } -} - -impl Parse for PacketFlag { - fn parse(input: syn::parse::ParseStream) -> syn::Result { - let lookahead = input.lookahead1(); - - if lookahead.peek(kw::default_for_builder) { - input.parse::()?; - Ok(PacketFlag::DefaultForBuilder) - } else { - Err(lookahead.error()) - } - } -} - -struct StructFlags(Punctuated); - -impl Parse for StructFlags { - fn parse(input: syn::parse::ParseStream) -> syn::Result { - let flags = input.parse_terminated(PacketFlag::parse)?; - Ok(Self(flags)) - } -} - -impl Parse for RecvPackets { - fn parse(input: syn::parse::ParseStream) -> syn::Result { - input.parse::()?; - let union_enum_name: Ident = input.parse()?; - let content; - let _brace_token: syn::token::Brace = braced!(content in input); - content.parse::()?; - content.parse::()?; - let unknown_ty: Ident = content.parse()?; - content.parse::()?; - let packs: Punctuated = content.parse_terminated(Ident::parse)?; - Ok(Self { - union_enum_name, - unknown_ty, - all_packets: packs.into_iter().collect(), - }) - } -} diff --git a/ublox/ublox_derive/src/lib.rs b/ublox/ublox_derive/src/lib.rs deleted file mode 100644 index 3e6324c..0000000 --- a/ublox/ublox_derive/src/lib.rs +++ /dev/null @@ -1,191 +0,0 @@ -extern crate proc_macro; - -mod input; -mod output; -#[cfg(test)] -mod tests; -mod types; - -use proc_macro2::TokenStream; -use quote::ToTokens; - -use syn::{ - parse_macro_input, punctuated::Punctuated, spanned::Spanned, Attribute, Data, DeriveInput, - Fields, Ident, Type, Variant, -}; - -#[proc_macro_attribute] -pub fn ubx_packet_recv( - _attr: proc_macro::TokenStream, - input: proc_macro::TokenStream, -) -> proc_macro::TokenStream { - let input = parse_macro_input!(input as DeriveInput); - let ret = if let Data::Struct(data) = input.data { - generate_code_for_recv_packet(input.ident, input.attrs, data.fields) - } else { - Err(syn::Error::new( - input.ident.span(), - "This attribute can only be used for struct", - )) - }; - - ret.map(|x| x.into()) - .unwrap_or_else(|err| err.to_compile_error().into()) -} - -#[proc_macro_attribute] -pub fn ubx_packet_send( - _attr: proc_macro::TokenStream, - input: proc_macro::TokenStream, -) -> proc_macro::TokenStream { - let input = parse_macro_input!(input as DeriveInput); - let ret = if let Data::Struct(data) = input.data { - generate_code_for_send_packet(input.ident, input.attrs, data.fields) - } else { - Err(syn::Error::new( - input.ident.span(), - "This attribute can only be used for struct", - )) - }; - - ret.map(|x| x.into()) - .unwrap_or_else(|err| err.to_compile_error().into()) -} - -#[proc_macro_attribute] -pub fn ubx_packet_recv_send( - _attr: proc_macro::TokenStream, - input: proc_macro::TokenStream, -) -> proc_macro::TokenStream { - let input = parse_macro_input!(input as DeriveInput); - let ret = if let Data::Struct(data) = input.data { - generate_code_for_recv_send_packet(input.ident, input.attrs, data.fields) - } else { - Err(syn::Error::new( - input.ident.span(), - "This attribute can only be used for struct", - )) - }; - - ret.map(|x| x.into()) - .unwrap_or_else(|err| err.to_compile_error().into()) -} - -#[proc_macro_attribute] -pub fn ubx_extend( - _attr: proc_macro::TokenStream, - input: proc_macro::TokenStream, -) -> proc_macro::TokenStream { - let input = parse_macro_input!(input as DeriveInput); - let ret = if let Data::Enum(data) = input.data { - extend_enum(input.ident, input.attrs, data.variants) - } else { - Err(syn::Error::new( - input.ident.span(), - "This attribute can only be used for enum", - )) - }; - - ret.map(|x| x.into()) - .unwrap_or_else(|err| err.to_compile_error().into()) -} - -#[proc_macro_attribute] -pub fn ubx_extend_bitflags( - _attr: proc_macro::TokenStream, - input: proc_macro::TokenStream, -) -> proc_macro::TokenStream { - let input = parse_macro_input!(input as syn::ItemMacro); - - extend_bitflags(input) - .map(|x| x.into()) - .unwrap_or_else(|err| err.to_compile_error().into()) -} - -#[proc_macro] -pub fn define_recv_packets(input: proc_macro::TokenStream) -> proc_macro::TokenStream { - do_define_recv_packets(input.into()) - .map(|x| x.into()) - .unwrap_or_else(|err| err.to_compile_error().into()) -} - -fn generate_code_for_recv_packet( - pack_name: Ident, - attrs: Vec, - fields: Fields, -) -> syn::Result { - let pack_desc = input::parse_packet_description(pack_name, attrs, fields)?; - - let mut code = output::generate_types_for_packet(&pack_desc); - let recv_code = output::generate_recv_code_for_packet(&pack_desc); - code.extend(recv_code); - Ok(code) -} - -fn generate_code_for_send_packet( - pack_name: Ident, - attrs: Vec, - fields: Fields, -) -> syn::Result { - let pack_desc = input::parse_packet_description(pack_name, attrs, fields)?; - - let mut code = output::generate_types_for_packet(&pack_desc); - let send_code = output::generate_send_code_for_packet(&pack_desc); - code.extend(send_code); - Ok(code) -} - -fn generate_code_for_recv_send_packet( - pack_name: Ident, - attrs: Vec, - fields: Fields, -) -> syn::Result { - let pack_desc = input::parse_packet_description(pack_name, attrs, fields)?; - - let mut code = output::generate_types_for_packet(&pack_desc); - let send_code = output::generate_send_code_for_packet(&pack_desc); - code.extend(send_code); - let recv_code = output::generate_recv_code_for_packet(&pack_desc); - code.extend(recv_code); - Ok(code) -} - -fn extend_enum( - name: Ident, - attrs: Vec, - variants: Punctuated, -) -> syn::Result { - let ext_enum = input::parse_ubx_enum_type(name, attrs, variants)?; - let code = output::generate_code_to_extend_enum(&ext_enum); - Ok(code) -} - -fn extend_bitflags(mac: syn::ItemMacro) -> syn::Result { - if !mac.mac.path.is_ident("bitflags") { - return Err(syn::Error::new( - mac.ident - .as_ref() - .map(|x| x.span()) - .unwrap_or_else(|| mac.span()), - format!( - "Expect bitflags invocation here, instead got '{}'", - mac.mac.path.into_token_stream() - ), - )); - } - let bitflags = input::parse_bitflags(mac)?; - output::generate_code_to_extend_bitflags(bitflags) -} - -fn do_define_recv_packets(input: TokenStream) -> syn::Result { - let recv_packs = input::parse_idents_list(input)?; - Ok(output::generate_code_for_parse(&recv_packs)) -} - -fn type_is_option(ty: &Type) -> bool { - matches!(ty, Type::Path(ref typepath) if typepath.qself.is_none() && path_is_option(&typepath.path)) -} - -fn path_is_option(path: &syn::Path) -> bool { - path.segments.len() == 1 && path.segments.iter().next().unwrap().ident == "Option" -} diff --git a/ublox/ublox_derive/src/output.rs b/ublox/ublox_derive/src/output.rs deleted file mode 100644 index 7e1ba73..0000000 --- a/ublox/ublox_derive/src/output.rs +++ /dev/null @@ -1,851 +0,0 @@ -use crate::types::BitFlagsMacro; -use crate::types::{ - PackDesc, PackField, PacketFlag, PayloadLen, RecvPackets, UbxEnumRestHandling, UbxExtendEnum, - UbxTypeFromFn, UbxTypeIntoFn, -}; -use proc_macro2::{Span, TokenStream}; -use quote::{format_ident, quote, ToTokens}; -use std::{collections::HashSet, convert::TryFrom}; -use syn::{parse_quote, Ident, Type}; - -fn generate_debug_impl(pack_name: &str, ref_name: &Ident, pack_descr: &PackDesc) -> TokenStream { - let fields = pack_descr.fields.iter().map(|field| { - let field_name = &field.name; - let field_accessor = field.intermediate_field_name(); - quote! { - .field(stringify!(#field_name), &self.#field_accessor()) - } - }); - quote! { - impl core::fmt::Debug for #ref_name<'_> { - fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { - f.debug_struct(#pack_name) - #(#fields)* - .finish() - } - } - } -} - -fn generate_serialize_impl( - _pack_name: &str, - ref_name: &Ident, - pack_descr: &PackDesc, -) -> TokenStream { - let fields = pack_descr.fields.iter().map(|field| { - let field_name = &field.name; - let field_accessor = field.intermediate_field_name(); - if field.size_bytes.is_some() || field.is_optional() { - quote! { - state.serialize_entry(stringify!(#field_name), &self.#field_accessor())?; - } - } else { - quote! { - state.serialize_entry( - stringify!(#field_name), - &crate::ubx_packets::FieldIter(self.#field_accessor()) - )?; - } - } - }); - quote! { - #[cfg(feature = "serde")] - impl SerializeUbxPacketFields for #ref_name<'_> { - fn serialize_fields(&self, state: &mut S) -> Result<(), S::Error> - where - S: serde::ser::SerializeMap, - { - #(#fields)* - Ok(()) - } - } - - #[cfg(feature = "serde")] - impl serde::Serialize for #ref_name<'_> { - fn serialize(&self, serializer: S) -> Result - where - S: serde::Serializer, - { - let mut state = serializer.serialize_map(None)?; - self.serialize_fields(&mut state)?; - state.end() - } - } - } -} - -pub fn generate_recv_code_for_packet(pack_descr: &PackDesc) -> TokenStream { - let pack_name = &pack_descr.name; - let ref_name = format_ident!("{}Ref", pack_descr.name); - - let mut getters = Vec::with_capacity(pack_descr.fields.len()); - let mut field_validators = Vec::new(); - let mut size_fns = Vec::new(); - - let mut off = 0usize; - for (field_index, f) in pack_descr.fields.iter().enumerate() { - let ty = f.intermediate_type(); - let get_name = f.intermediate_field_name(); - let field_comment = &f.comment; - - if let Some(size_bytes) = f.size_bytes.map(|x| x.get()) { - let get_raw = get_raw_field_code(f, off, quote! {self.0}); - let new_line = quote! { let val = #get_raw; }; - let mut get_value_lines = vec![new_line]; - - if let Some(ref out_ty) = f.map.map_type { - let get_raw_name = format_ident!("{}_raw", get_name); - let slicetype = syn::parse_str("&[u8]").unwrap(); - let raw_ty = if f.is_field_raw_ty_byte_array() { - &slicetype - } else { - &f.ty - }; - getters.push(quote! { - #[doc = #field_comment] - #[inline] - pub fn #get_raw_name(&self) -> #raw_ty { - #(#get_value_lines)* - val - } - }); - - if f.map.convert_may_fail { - let get_val = get_raw_field_code(f, off, quote! { payload }); - let is_valid_fn = &out_ty.is_valid_fn; - field_validators.push(quote! { - let val = #get_val; - if !#is_valid_fn(val) { - return Err(ParserError::InvalidField{ - packet: #pack_name, - field: stringify!(#get_name) - }); - } - }); - } - let from_fn = &out_ty.from_fn; - get_value_lines.push(quote! { - let val = #from_fn(val); - }); - } - - if let Some(ref scale) = f.map.scale { - get_value_lines.push(quote! { let val = val * #scale; }); - } - getters.push(quote! { - #[doc = #field_comment] - #[inline] - pub fn #get_name(&self) -> #ty { - #(#get_value_lines)* - val - } - }); - off += size_bytes; - } else { - assert!(field_index == pack_descr.fields.len() - 1 || f.size_fn().is_some()); - - let range = if let Some(size_fn) = f.size_fn() { - let range = quote! { - { - let offset = #off #(+ self.#size_fns())*; - offset..offset+self.#size_fn() - } - }; - size_fns.push(size_fn); - range - } else { - quote! { #off.. } - }; - - let mut get_value_lines = vec![quote! { &self.0[#range] }]; - if let Some(ref out_ty) = f.map.map_type { - let get_raw = &get_value_lines[0]; - let new_line = quote! { let val = #get_raw ; }; - get_value_lines[0] = new_line; - let from_fn = &out_ty.from_fn; - get_value_lines.push(quote! { - #from_fn(val) - }); - - if f.map.convert_may_fail { - let is_valid_fn = &out_ty.is_valid_fn; - field_validators.push(quote! { - let val = &payload[#off..]; - if !#is_valid_fn(val) { - return Err(ParserError::InvalidField{ - packet: #pack_name, - field: stringify!(#get_name) - }); - } - }); - } - } - let out_ty = if f.has_intermediate_type() { - ty.clone() - } else { - parse_quote! { &[u8] } - }; - getters.push(quote! { - #[doc = #field_comment] - #[inline] - pub fn #get_name(&self) -> #out_ty { - #(#get_value_lines)* - } - }); - } - } - let struct_comment = &pack_descr.comment; - let validator = if let Some(payload_len) = pack_descr.packet_payload_size() { - quote! { - fn validate(payload: &[u8]) -> Result<(), ParserError> { - let expect = #payload_len; - let got = payload.len(); - if got == expect { - #(#field_validators)* - Ok(()) - } else { - Err(ParserError::InvalidPacketLen{ packet: #pack_name, expect, got }) - } - } - } - } else { - let size_fns: Vec<_> = pack_descr - .fields - .iter() - .filter_map(|f| f.size_fn()) - .collect(); - - let min_size = if size_fns.is_empty() { - let size = pack_descr - .packet_payload_size_except_last_field() - .expect("except last all fields should have fixed size"); - quote! { - #size; - } - } else { - let size = pack_descr - .packet_payload_size_except_size_fn() - .unwrap_or_default(); - quote! { - { - if got < #size { - return Err(ParserError::InvalidPacketLen{ packet: #pack_name, expect: #size, got }); - } - #size #(+ #ref_name(payload).#size_fns())* - } - } - }; - - quote! { - fn validate(payload: &[u8]) -> Result<(), ParserError> { - let got = payload.len(); - let min = #min_size; - if got >= min { - #(#field_validators)* - Ok(()) - } else { - Err(ParserError::InvalidPacketLen{ packet: #pack_name, expect: min, got }) - } - } - } - }; - - let debug_impl = generate_debug_impl(pack_name, &ref_name, pack_descr); - let serialize_impl = generate_serialize_impl(pack_name, &ref_name, pack_descr); - - quote! { - #[doc = #struct_comment] - #[doc = "Contains a reference to an underlying buffer, contains accessor methods to retrieve data."] - pub struct #ref_name<'a>(&'a [u8]); - impl<'a> #ref_name<'a> { - #[inline] - pub fn as_bytes(&self) -> &[u8] { - self.0 - } - - #(#getters)* - - #validator - } - - #debug_impl - #serialize_impl - } -} - -pub fn generate_types_for_packet(pack_descr: &PackDesc) -> TokenStream { - let name = Ident::new(&pack_descr.name, Span::call_site()); - let class = pack_descr.header.class; - let id = pack_descr.header.id; - let fixed_payload_len = match pack_descr.header.payload_len.fixed() { - Some(x) => quote! { Some(#x) }, - None => quote! { None }, - }; - let struct_comment = &pack_descr.comment; - let max_payload_len = match pack_descr.header.payload_len { - PayloadLen::Fixed(x) => x, - PayloadLen::Max(x) => x, - }; - quote! { - - #[doc = #struct_comment] - pub struct #name; - impl UbxPacketMeta for #name { - const CLASS: u8 = #class; - const ID: u8 = #id; - const FIXED_PAYLOAD_LEN: Option = #fixed_payload_len; - const MAX_PAYLOAD_LEN: u16 = #max_payload_len; - } - } -} - -pub fn generate_send_code_for_packet(pack_descr: &PackDesc) -> TokenStream { - let main_name = Ident::new(&pack_descr.name, Span::call_site()); - let payload_struct = format_ident!("{}Builder", pack_descr.name); - - let mut builder_needs_lifetime = false; - - let mut fields = Vec::with_capacity(pack_descr.fields.len()); - let mut pack_fields = Vec::with_capacity(pack_descr.fields.len()); - let mut write_fields = Vec::with_capacity(pack_descr.fields.len()); - let mut extend_fields = Vec::with_capacity(pack_descr.fields.len()); - let mut off = 6usize; - for (fi, f) in pack_descr.fields.iter().enumerate() { - let ty = f.intermediate_type(); - let name = f.intermediate_field_name(); - let field_comment = &f.comment; - fields.push(quote! { - #[doc = #field_comment] - pub #name: #ty - }); - - let size_bytes = match f.size_bytes { - Some(x) => x.get(), - None => { - // Iterator with `data` field. - extend_fields.push(quote! { - for f in self.#name { - len_bytes += f.extend_to(out); - } - }); - - builder_needs_lifetime = true; - - assert_eq!( - fi, - pack_descr.fields.len() - 1, - "Iterator field must be the last field." - ); - break; - }, - }; - - if let Some(into_fn) = f.map.map_type.as_ref().map(|x| &x.into_fn) { - pack_fields.push(quote! { - let bytes = #into_fn(self.#name).to_le_bytes() - }); - } else if !f.is_field_raw_ty_byte_array() { - pack_fields.push(quote! { - let bytes = self.#name.to_le_bytes() - }); - } else { - pack_fields.push(quote! { - let bytes: &[u8] = &self.#name; - }); - } - - write_fields.push(pack_fields.last().unwrap().clone()); - write_fields.push(quote! { - out.write(&bytes)?; - checksum_calc.update(&bytes) - }); - - extend_fields.push(pack_fields.last().unwrap().clone()); - extend_fields.push(quote! { - len_bytes += bytes.len(); - // TODO: Extend all at once when we bump MSRV - //out.extend(bytes.into_iter().cloned()); - for b in bytes.iter() { - out.extend(core::iter::once(*b)); - } - }); - - for i in 0..size_bytes { - let byte_off = off.checked_add(i).unwrap(); - pack_fields.push(quote! { - ret[#byte_off] = bytes[#i] - }); - } - - off += size_bytes; - } - let builder_attr = if pack_descr - .header - .flags - .iter() - .any(|x| *x == PacketFlag::DefaultForBuilder) - { - quote! { #[derive(Default)] } - } else { - quote! {} - }; - let struct_comment = &pack_descr.comment; - - let payload_struct_lifetime = if builder_needs_lifetime { - quote! { <'a> } - } else { - quote! {} - }; - - let mut ret = quote! { - #[doc = #struct_comment] - #[doc = "Struct that is used to construct packets, see the crate-level documentation for more information"] - #builder_attr - pub struct #payload_struct #payload_struct_lifetime { - #(#fields),* - } - }; - - if let Some(packet_payload_size) = pack_descr.packet_payload_size() { - let packet_size = packet_payload_size + 8; - let packet_payload_size_u16 = u16::try_from(packet_payload_size).unwrap(); - ret.extend(quote! { - impl #payload_struct_lifetime #payload_struct #payload_struct_lifetime { - pub const PACKET_LEN: usize = #packet_size; - - #[inline] - pub fn into_packet_bytes(self) -> [u8; Self::PACKET_LEN] { - let mut ret = [0u8; Self::PACKET_LEN]; - ret[0] = SYNC_CHAR_1; - ret[1] = SYNC_CHAR_2; - ret[2] = #main_name::CLASS; - ret[3] = #main_name::ID; - let pack_len_bytes = #packet_payload_size_u16 .to_le_bytes(); - ret[4] = pack_len_bytes[0]; - ret[5] = pack_len_bytes[1]; - #(#pack_fields);*; - let (ck_a, ck_b) = ubx_checksum(&ret[2..(Self::PACKET_LEN - 2)]); - ret[Self::PACKET_LEN - 2] = ck_a; - ret[Self::PACKET_LEN - 1] = ck_b; - ret - } - } - impl From<#payload_struct> for [u8; #packet_size] { - fn from(x: #payload_struct) -> Self { - x.into_packet_bytes() - } - } - - impl UbxPacketCreator for #payload_struct { - #[inline] - fn create_packet(self, out: &mut T) -> Result<(), MemWriterError> { - out.reserve_allocate(#packet_size)?; - let len_bytes = #packet_payload_size_u16 .to_le_bytes(); - let header = [SYNC_CHAR_1, SYNC_CHAR_2, #main_name::CLASS, #main_name::ID, len_bytes[0], len_bytes[1]]; - out.write(&header)?; - let mut checksum_calc = UbxChecksumCalc::default(); - checksum_calc.update(&header[2..]); - #(#write_fields);*; - let (ck_a, ck_b) = checksum_calc.result(); - out.write(&[ck_a, ck_b])?; - Ok(()) - } - } - }); - } else { - ret.extend(quote! { - impl #payload_struct_lifetime #payload_struct #payload_struct_lifetime { - #[cfg(feature = "alloc")] - #[inline] - pub fn into_packet_vec(self) -> Vec { - let mut vec = Vec::new(); - self.extend_to(&mut vec); - vec - } - - #[inline] - pub fn extend_to(self, out: &mut T) - where - T: core::iter::Extend + - core::ops::DerefMut - { - // TODO: Enable when `extend_one` feature is stable. - // out.extend_reserve(6); - let mut len_bytes = 0; - let header = [SYNC_CHAR_1, SYNC_CHAR_2, #main_name::CLASS, #main_name::ID, 0, 0]; - // TODO: Extend all at once when we bump MSRV - //out.extend(header); - for b in header.iter() { - out.extend(core::iter::once(*b)); - } - #(#extend_fields);*; - - let len_bytes = len_bytes.to_le_bytes(); - out[4] = len_bytes[0]; - out[5] = len_bytes[1]; - - let (ck_a, ck_b) = ubx_checksum(&out[2..]); - out.extend(core::iter::once(ck_a)); - out.extend(core::iter::once(ck_b)); - } - } - }) - } - - ret -} - -pub fn generate_code_to_extend_enum(ubx_enum: &UbxExtendEnum) -> TokenStream { - assert_eq!(ubx_enum.repr, { - let ty: Type = parse_quote! { u8 }; - ty - }); - let name = &ubx_enum.name; - let mut variants = ubx_enum.variants.clone(); - let attrs = &ubx_enum.attrs; - if let Some(UbxEnumRestHandling::Reserved) = ubx_enum.rest_handling { - let defined: HashSet = ubx_enum.variants.iter().map(|x| x.1).collect(); - for i in 0..=u8::max_value() { - if !defined.contains(&i) { - let name = format_ident!("Reserved{}", i); - variants.push((name, i)); - } - } - } - let repr_ty = &ubx_enum.repr; - let from_code = match ubx_enum.from_fn { - Some(UbxTypeFromFn::From) => { - assert_ne!( - Some(UbxEnumRestHandling::ErrorProne), - ubx_enum.rest_handling - ); - let mut match_branches = Vec::with_capacity(variants.len()); - for (id, val) in &variants { - match_branches.push(quote! { #val => #name :: #id }); - } - - quote! { - impl #name { - fn from(x: #repr_ty) -> Self { - match x { - #(#match_branches),* - } - } - } - } - }, - Some(UbxTypeFromFn::FromUnchecked) => { - assert_ne!(Some(UbxEnumRestHandling::Reserved), ubx_enum.rest_handling); - let mut match_branches = Vec::with_capacity(variants.len()); - for (id, val) in &variants { - match_branches.push(quote! { #val => #name :: #id }); - } - - let mut values = Vec::with_capacity(variants.len()); - for (i, (_, val)) in variants.iter().enumerate() { - if i != 0 { - values.push(quote! { | #val }); - } else { - values.push(quote! { #val }); - } - } - - quote! { - impl #name { - fn from_unchecked(x: #repr_ty) -> Self { - match x { - #(#match_branches),*, - _ => unreachable!(), - } - } - fn is_valid(x: #repr_ty) -> bool { - match x { - #(#values)* => true, - _ => false, - } - } - } - } - }, - None => quote! {}, - }; - - let to_code = match ubx_enum.into_fn { - None => quote! {}, - Some(UbxTypeIntoFn::Raw) => quote! { - impl #name { - const fn into_raw(self) -> #repr_ty { - self as #repr_ty - } - } - }, - }; - - let mut enum_variants = Vec::with_capacity(variants.len()); - for (id, val) in &variants { - enum_variants.push(quote! { #id = #val }); - } - - let code = quote! { - #(#attrs)* - pub enum #name { - #(#enum_variants),* - } - - #from_code - #to_code - - #[cfg(feature = "serde")] - impl serde::Serialize for #name { - fn serialize(&self, serializer: S) -> Result - where - S: serde::Serializer, - { - serializer.serialize_u8(*self as u8) - } - } - }; - code -} - -pub fn generate_code_to_extend_bitflags(bitflags: BitFlagsMacro) -> syn::Result { - match bitflags.rest_handling { - Some(UbxEnumRestHandling::ErrorProne) | None => { - return Err(syn::Error::new( - bitflags.name.span(), - "Only reserved supported", - )) - }, - Some(UbxEnumRestHandling::Reserved) => (), - } - - let mut known_flags = HashSet::new(); - let mut items = Vec::with_capacity(usize::try_from(bitflags.nbits).unwrap()); - let repr_ty = &bitflags.repr_ty; - - for bit in 0..bitflags.nbits { - let flag_bit = 1u64.checked_shl(bit).unwrap(); - if let Some(item) = bitflags.consts.iter().find(|x| x.value == flag_bit) { - known_flags.insert(flag_bit); - let name = &item.name; - let attrs = &item.attrs; - if bit != 0 { - items.push(quote! { - #(#attrs)* - const #name = ((1 as #repr_ty) << #bit) - }); - } else { - items.push(quote! { - #(#attrs)* - const #name = (1 as #repr_ty) - }); - } - } else { - let name = format_ident!("RESERVED{}", bit); - if bit != 0 { - items.push(quote! { const #name = ((1 as #repr_ty) << #bit)}); - } else { - items.push(quote! { const #name = (1 as #repr_ty) }); - } - } - } - - if known_flags.len() != bitflags.consts.len() { - let user_flags: HashSet<_> = bitflags.consts.iter().map(|x| x.value).collect(); - let set = user_flags.difference(&known_flags); - return Err(syn::Error::new( - bitflags.name.span(), - format!("Strange flags, not power of 2?: {:?}", set), - )); - } - - let vis = &bitflags.vis; - let attrs = &bitflags.attrs; - let name = &bitflags.name; - - let from = match bitflags.from_fn { - None => quote! {}, - Some(UbxTypeFromFn::From) => quote! { - impl #name { - const fn from(x: #repr_ty) -> Self { - Self::from_bits_truncate(x) - } - } - }, - Some(UbxTypeFromFn::FromUnchecked) => unimplemented!(), - }; - - let into = match bitflags.into_fn { - None => quote! {}, - Some(UbxTypeIntoFn::Raw) => quote! { - impl #name { - const fn into_raw(self) -> #repr_ty { - self.bits() - } - } - }, - }; - - let serialize_fn = format_ident!("serialize_{}", repr_ty.to_token_stream().to_string()); - let serde = quote! { - #[cfg(feature = "serde")] - impl serde::Serialize for #name { - fn serialize(&self, serializer: S) -> Result - where - S: serde::Serializer, - { - serializer.#serialize_fn(self.bits()) - } - } - }; - - Ok(quote! { - bitflags! { - #(#attrs)* - #vis struct #name : #repr_ty { - #(#items);*; - } - } - #from - #into - #serde - }) -} - -pub fn generate_code_for_parse(recv_packs: &RecvPackets) -> TokenStream { - let union_enum_name = &recv_packs.union_enum_name; - - let mut pack_enum_variants = Vec::with_capacity(recv_packs.all_packets.len()); - let mut matches = Vec::with_capacity(recv_packs.all_packets.len()); - let mut class_id_matches = Vec::with_capacity(recv_packs.all_packets.len()); - let mut serializers = Vec::with_capacity(recv_packs.all_packets.len()); - - for name in &recv_packs.all_packets { - let ref_name = format_ident!("{}Ref", name); - pack_enum_variants.push(quote! { - #name(#ref_name <'a>) - }); - - matches.push(quote! { - (#name::CLASS, #name::ID) if <#ref_name>::validate(payload).is_ok() => { - Ok(#union_enum_name::#name(#ref_name(payload))) - } - }); - - class_id_matches.push(quote! { - #union_enum_name::#name(_) => (#name::CLASS, #name::ID) - }); - - serializers.push(quote! { - #union_enum_name::#name(ref msg) => crate::ubx_packets::PacketSerializer { - class: #name::CLASS, - msg_id: #name::ID, - msg, - } - .serialize(serializer) - }); - } - - let unknown_var = &recv_packs.unknown_ty; - - let max_payload_len_calc = recv_packs - .all_packets - .iter() - .fold(quote! { 0u16 }, |prev, name| { - quote! { max_u16(#name::MAX_PAYLOAD_LEN, #prev) } - }); - - quote! { - #[doc = "All possible packets enum"] - #[derive(Debug)] - pub enum #union_enum_name<'a> { - #(#pack_enum_variants),*, - Unknown(#unknown_var<'a>) - } - - impl<'a> #union_enum_name<'a> { - pub fn class_and_msg_id(&self) -> (u8, u8) { - match *self { - #(#class_id_matches),*, - #union_enum_name::Unknown(ref pack) => (pack.class, pack.msg_id), - } - } - } - - pub(crate) fn match_packet(class: u8, msg_id: u8, payload: &[u8]) -> Result<#union_enum_name, ParserError> { - match (class, msg_id) { - #(#matches)* - _ => Ok(#union_enum_name::Unknown(#unknown_var { - payload, - class, - msg_id - })), - } - } - - const fn max_u16(a: u16, b: u16) -> u16 { - [a, b][(a < b) as usize] - } - pub(crate) const MAX_PAYLOAD_LEN: u16 = #max_payload_len_calc; - #[cfg(feature = "serde")] - pub struct PacketSerializer<'a, T> { - class: u8, - msg_id: u8, - msg: &'a T, - } - - #[cfg(feature = "serde")] - impl<'a, T: SerializeUbxPacketFields> serde::Serialize for PacketSerializer<'a, T> { - fn serialize(&self, serializer: S) -> Result - where - S: serde::Serializer, - { - let mut state = serializer.serialize_map(None)?; - state.serialize_entry("class", &self.class)?; - state.serialize_entry("msg_id", &self.msg_id)?; - self.msg.serialize_fields(&mut state)?; - state.end() - } - } - - #[cfg(feature = "serde")] - impl serde::Serialize for #union_enum_name<'_> { - fn serialize(&self, serializer: S) -> Result - where - S: serde::Serializer, - { - match *self { - #(#serializers),*, - #union_enum_name::Unknown(ref pack) => pack.serialize(serializer), - } - } - } - } -} - -fn get_raw_field_code(field: &PackField, cur_off: usize, data: TokenStream) -> TokenStream { - let size_bytes = match field.size_bytes { - Some(x) => x, - None => unimplemented!(), - }; - - let mut bytes = Vec::with_capacity(size_bytes.get()); - for i in 0..size_bytes.get() { - let byte_off = cur_off.checked_add(i).unwrap(); - bytes.push(quote! { #data[#byte_off] }); - } - let raw_ty = &field.ty; - - let signed_byte: Type = parse_quote! { i8 }; - - if field.map.get_as_ref { - let size_bytes: usize = size_bytes.into(); - quote! { &#data[#cur_off .. (#cur_off + #size_bytes)] } - } else if field.is_field_raw_ty_byte_array() { - quote! { [#(#bytes),*] } - } else if size_bytes.get() != 1 || *raw_ty == signed_byte { - quote! { <#raw_ty>::from_le_bytes([#(#bytes),*]) } - } else { - quote! { #data[#cur_off] } - } -} diff --git a/ublox/ublox_derive/src/tests.rs b/ublox/ublox_derive/src/tests.rs deleted file mode 100644 index 1f01048..0000000 --- a/ublox/ublox_derive/src/tests.rs +++ /dev/null @@ -1,814 +0,0 @@ -use super::*; -use quote::quote; -use std::{ - io::{self, Write}, - process::{Command, Stdio}, - sync::Arc, -}; -use syn::Error; -use which::which; - -#[test] -fn test_ubx_packet_recv_simple() { - let src_code = quote! { - #[ubx_packet_recv] - #[ubx(class = 1, id = 2, fixed_payload_len = 16)] - #[doc = "Some comment"] - struct Test { - itow: u32, - #[doc = "this is lat"] - #[ubx(map_type = f64, scale = 1e-7, alias = lat_degrees)] - lat: i32, - #[doc = "this is a"] - a: u8, - reserved1: [u8; 5], - #[ubx(map_type = Flags, may_fail)] - flags: u8, - b: i8, - } - }; - let src_code = src_code.to_string(); - - let code: syn::ItemStruct = syn::parse_str(&src_code) - .unwrap_or_else(|err| panic_on_parse_error("test_ubx_packet_recv", &src_code, &err)); - let tokens = generate_code_for_recv_packet(code.ident, code.attrs, code.fields) - .unwrap_or_else(|err| panic_on_parse_error("test_ubx_packet_recv", &src_code, &err)); - - run_compare_test( - tokens, - quote! { - #[doc = "Some comment"] - pub struct Test; - - impl UbxPacketMeta for Test { - const CLASS: u8 = 1u8; - const ID: u8 = 2u8; - const FIXED_PAYLOAD_LEN: Option = Some(16u16); - const MAX_PAYLOAD_LEN: u16 = 16u16; - } - - #[doc = "Some comment"] - #[doc = "Contains a reference to an underlying buffer, contains accessor methods to retrieve data."] - pub struct TestRef<'a>(&'a [u8]); - - impl<'a> TestRef<'a> { - #[inline] - pub fn as_bytes(&self) -> &[u8] { - self.0 - } - - #[doc = ""] - #[inline] - pub fn itow(&self) -> u32 { - let val = ::from_le_bytes([ - self.0[0usize], - self.0[1usize], - self.0[2usize], - self.0[3usize], - ]); - val - } - - #[doc = "this is lat"] - #[inline] - pub fn lat_degrees_raw(&self) -> i32 { - let val = ::from_le_bytes([ - self.0[4usize], - self.0[5usize], - self.0[6usize], - self.0[7usize], - ]); - val - } - - #[doc = "this is lat"] - #[inline] - pub fn lat_degrees(&self) -> f64 { - let val = ::from_le_bytes([ - self.0[4usize], - self.0[5usize], - self.0[6usize], - self.0[7usize], - ]); - let val = ::from(val); - let val = val * 1e-7; - val - } - - #[doc = "this is a"] - #[inline] - pub fn a(&self) -> u8 { - let val = self.0[8usize]; - val - } - - #[doc = ""] - #[inline] - pub fn reserved1(&self) -> [u8; 5] { - let val = [ - self.0[9usize], - self.0[10usize], - self.0[11usize], - self.0[12usize], - self.0[13usize], - ]; - val - } - - #[doc = ""] - #[inline] - pub fn flags_raw(&self) -> u8 { - let val = self.0[14usize]; - val - } - - #[doc = ""] - #[inline] - pub fn flags(&self) -> Flags { - let val = self.0[14usize]; - let val = ::from_unchecked(val); - val - } - - #[doc = ""] - #[inline] - pub fn b(&self) -> i8 { - let val = ::from_le_bytes([self.0[15usize]]); - val - } - - fn validate(payload: &[u8]) -> Result<(), ParserError> { - let expect = 16usize; - let got = payload.len(); - if got == expect { - let val = payload[14usize]; - if !::is_valid(val) { - return Err(ParserError::InvalidField { - packet: "Test", - field: stringify!(flags), - }); - } - Ok(()) - } else { - Err(ParserError::InvalidPacketLen { - packet: "Test", - expect, - got, - }) - } - } - } - - impl core::fmt::Debug for TestRef<'_> { - fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { - f.debug_struct("Test") - .field(stringify!(itow), &self.itow()) - .field(stringify!(lat), &self.lat_degrees()) - .field(stringify!(a), &self.a()) - .field(stringify!(reserved1), &self.reserved1()) - .field(stringify!(flags), &self.flags()) - .field(stringify!(b), &self.b()) - .finish() - } - } - #[cfg(feature = "serde")] - impl SerializeUbxPacketFields for TestRef<'_> { - fn serialize_fields(&self, state: &mut S) -> Result<(), S::Error> - where - S: serde::ser::SerializeMap, - { - state.serialize_entry(stringify!(itow), &self.itow())?; - state.serialize_entry(stringify!(lat), &self.lat_degrees())?; - state.serialize_entry(stringify!(a), &self.a())?; - state.serialize_entry(stringify!(reserved1), &self.reserved1())?; - state.serialize_entry(stringify!(flags), &self.flags())?; - state.serialize_entry(stringify!(b), &self.b())?; - Ok(()) - } - } - #[cfg(feature = "serde")] - impl serde::Serialize for TestRef<'_> { - fn serialize(&self, serializer: S) -> Result - where - S: serde::Serializer, - { - let mut state = serializer.serialize_map(None)?; - self.serialize_fields(&mut state)?; - state.end() - } - } - }, - ); -} - -#[test] -fn test_ubx_packet_recv_dyn_len() { - let src_code = quote! { - #[ubx_packet_recv] - #[ubx(class = 1, id = 2, max_payload_len = 38)] - struct Test { - #[ubx(map_type = &str, get_as_ref, from = unpack_str)] - f1: [u8; 8], - rest: [u8; 0], - } - }; - let src_code = src_code.to_string(); - - let code: syn::ItemStruct = syn::parse_str(&src_code).unwrap_or_else(|err| { - panic_on_parse_error("test_ubx_packet_recv_dyn_len", &src_code, &err) - }); - let tokens = - generate_code_for_recv_packet(code.ident, code.attrs, code.fields).unwrap_or_else(|err| { - panic_on_parse_error("test_ubx_packet_recv_dyn_len", &src_code, &err) - }); - - run_compare_test( - tokens, - quote! { - #[doc = ""] - pub struct Test; - - impl UbxPacketMeta for Test { - const CLASS: u8 = 1u8; - const ID: u8 = 2u8; - const FIXED_PAYLOAD_LEN: Option = None; - const MAX_PAYLOAD_LEN: u16 = 38u16; - } - - #[doc = ""] - #[doc = "Contains a reference to an underlying buffer, contains accessor methods to retrieve data."] - pub struct TestRef<'a>(&'a [u8]); - - impl<'a> TestRef<'a> { - #[inline] - pub fn as_bytes(&self) -> &[u8] { - self.0 - } - - #[doc = ""] - #[inline] - pub fn f1_raw(&self) -> &[u8] { - let val = &self.0[0usize..(0usize + 8usize)]; - val - } - - #[doc = ""] - #[inline] - pub fn f1(&self) -> &str { - let val = &self.0[0usize..(0usize + 8usize)]; - let val = unpack_str(val); - val - } - - #[doc = ""] - #[inline] - pub fn rest(&self) -> &[u8] { - &self.0[8usize..] - } - - fn validate(payload: &[u8]) -> Result<(), ParserError> { - let got = payload.len(); - let min = 8usize; - if got >= min { - Ok(()) - } else { - Err(ParserError::InvalidPacketLen { - packet: "Test", - expect: min, - got, - }) - } - } - } - - impl core::fmt::Debug for TestRef<'_> { - fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { - f.debug_struct("Test") - .field(stringify!(f1), &self.f1()) - .field(stringify!(rest), &self.rest()) - .finish() - } - } - #[cfg(feature = "serde")] - impl SerializeUbxPacketFields for TestRef<'_> { - fn serialize_fields(&self, state: &mut S) -> Result<(), S::Error> - where - S: serde::ser::SerializeMap, - { - state.serialize_entry(stringify!(f1), &self.f1())?; - state.serialize_entry( - stringify!(rest), - &crate::ubx_packets::FieldIter(self.rest()), - )?; - Ok(()) - } - } - #[cfg(feature = "serde")] - impl serde::Serialize for TestRef<'_> { - fn serialize(&self, serializer: S) -> Result - where - S: serde::Serializer, - { - let mut state = serializer.serialize_map(None)?; - self.serialize_fields(&mut state)?; - state.end() - } - } - }, - ); -} - -#[test] -fn test_ubx_packet_send() { - let src_code = quote! { - #[ubx_packet_send] - #[ubx(class = 1, id = 2, fixed_payload_len = 9, flags = "default_for_builder")] - #[doc = "Some comment"] - struct Test { - itow: u32, - #[doc = "this is lat"] - #[ubx(map_type = f64, scale = 1e-7, alias = lat_degrees)] - lat: i32, - #[doc = "this is a"] - a: u8, - } - }; - let src_code = src_code.to_string(); - - let code: syn::ItemStruct = syn::parse_str(&src_code) - .unwrap_or_else(|err| panic_on_parse_error("test_ubx_packet_send", &src_code, &err)); - let tokens = generate_code_for_send_packet(code.ident, code.attrs, code.fields) - .unwrap_or_else(|err| panic_on_parse_error("test_ubx_packet_send", &src_code, &err)); - - run_compare_test( - tokens, - quote! { - #[doc = "Some comment"] - pub struct Test; - - impl UbxPacketMeta for Test { - const CLASS: u8 = 1u8; - const ID: u8 = 2u8; - const FIXED_PAYLOAD_LEN: Option = Some(9u16); - const MAX_PAYLOAD_LEN: u16 = 9u16; - } - - #[doc = "Some comment"] - #[doc = "Struct that is used to construct packets, see the crate-level documentation for more information"] - #[derive(Default)] - pub struct TestBuilder { - #[doc = ""] - pub itow: u32, - #[doc = "this is lat"] - pub lat_degrees: f64, - #[doc = "this is a"] - pub a: u8, - } - impl TestBuilder { - pub const PACKET_LEN: usize = 17usize; - - #[inline] - pub fn into_packet_bytes(self) -> [u8; Self::PACKET_LEN] { - let mut ret = [0u8; Self::PACKET_LEN]; - ret[0] = SYNC_CHAR_1; - ret[1] = SYNC_CHAR_2; - ret[2] = Test::CLASS; - ret[3] = Test::ID; - let pack_len_bytes = 9u16.to_le_bytes(); - ret[4] = pack_len_bytes[0]; - ret[5] = pack_len_bytes[1]; - let bytes = self.itow.to_le_bytes(); - ret[6usize] = bytes[0usize]; - ret[7usize] = bytes[1usize]; - ret[8usize] = bytes[2usize]; - ret[9usize] = bytes[3usize]; - let bytes = ScaleBack::(1. / 1e-7) - .as_i32(self.lat_degrees) - .to_le_bytes(); - ret[10usize] = bytes[0usize]; - ret[11usize] = bytes[1usize]; - ret[12usize] = bytes[2usize]; - ret[13usize] = bytes[3usize]; - let bytes = self.a.to_le_bytes(); - ret[14usize] = bytes[0usize]; - let (ck_a, ck_b) = ubx_checksum(&ret[2..(Self::PACKET_LEN - 2)]); - ret[Self::PACKET_LEN - 2] = ck_a; - ret[Self::PACKET_LEN - 1] = ck_b; - ret - } - } - impl From for [u8; 17usize] { - fn from(x: TestBuilder) -> Self { - x.into_packet_bytes() - } - } - impl UbxPacketCreator for TestBuilder { - #[inline] - fn create_packet(self, out: &mut T) -> Result<(), MemWriterError> { - out.reserve_allocate(17usize)?; - let len_bytes = 9u16.to_le_bytes(); - let header = [ - SYNC_CHAR_1, - SYNC_CHAR_2, - Test::CLASS, - Test::ID, - len_bytes[0], - len_bytes[1], - ]; - out.write(&header)?; - let mut checksum_calc = UbxChecksumCalc::default(); - checksum_calc.update(&header[2..]); - let bytes = self.itow.to_le_bytes(); - out.write(&bytes)?; - checksum_calc.update(&bytes); - let bytes = ScaleBack::(1. / 1e-7) - .as_i32(self.lat_degrees) - .to_le_bytes(); - out.write(&bytes)?; - checksum_calc.update(&bytes); - let bytes = self.a.to_le_bytes(); - out.write(&bytes)?; - checksum_calc.update(&bytes); - let (ck_a, ck_b) = checksum_calc.result(); - out.write(&[ck_a, ck_b])?; - Ok(()) - } - } - }, - ); -} - -#[test] -fn test_upgrade_enum() { - let src_code = quote! { - #[doc = "GPS fix Type"] - #[ubx_extend] - #[ubx(from, rest_reserved)] - #[repr(u8)] - #[derive(Debug, Copy, Clone)] - enum GpsFix { - NoFix = 0, - DeadReckoningOnly = 1, - Fix2D = 2, - Fix3D = 3, - GPSPlusDeadReckoning = 4, - TimeOnlyFix = 5, - } - }; - let src_code = src_code.to_string(); - - let code: syn::ItemEnum = syn::parse_str(&src_code) - .unwrap_or_else(|err| panic_on_parse_error("test_upgrade_enum", &src_code, &err)); - let tokens = extend_enum(code.ident, code.attrs, code.variants) - .unwrap_or_else(|err| panic_on_parse_error("test_upgrade_enum", &src_code, &err)); - - let mut reserved_fields = Vec::with_capacity(256); - let mut rev_reserved_fields = Vec::with_capacity(256); - for i in 6..=255 { - let val = i as u8; - let ident = quote::format_ident!("Reserved{}", val); - reserved_fields.push(quote! { #ident = #val }); - rev_reserved_fields.push(quote! { #val => GpsFix::#ident }); - } - - run_compare_test( - tokens, - quote! { - #[doc = "GPS fix Type"] - #[repr(u8)] - #[derive(Debug, Copy, Clone)] - pub enum GpsFix { - NoFix = 0u8, - DeadReckoningOnly = 1u8, - Fix2D = 2u8, - Fix3D = 3u8, - GPSPlusDeadReckoning = 4u8, - TimeOnlyFix = 5u8, - #(#reserved_fields),* - } - impl GpsFix { - fn from(x: u8) -> Self { - match x { - 0u8 => GpsFix::NoFix, - 1u8 => GpsFix::DeadReckoningOnly, - 2u8 => GpsFix::Fix2D, - 3u8 => GpsFix::Fix3D, - 4u8 => GpsFix::GPSPlusDeadReckoning, - 5u8 => GpsFix::TimeOnlyFix, - #(#rev_reserved_fields),* - } - } - } - #[cfg(feature = "serde")] - impl serde::Serialize for GpsFix { - fn serialize(&self, serializer: S) -> Result - where - S: serde::Serializer, - { - serializer.serialize_u8(*self as u8) - } - } - }, - ); -} - -#[test] -fn test_define_recv_packets() { - let src_code = quote! { - enum PacketRef { - _ = UnknownPacketRef, - Pack1, - Pack2 - } - }; - let src_code = src_code.to_string(); - let tokens: TokenStream = syn::parse_str(&src_code) - .unwrap_or_else(|err| panic_on_parse_error("test_define_recv_packets", &src_code, &err)); - let output = do_define_recv_packets(tokens) - .unwrap_or_else(|err| panic_on_parse_error("test_define_recv_packets", &src_code, &err)); - run_compare_test( - output, - quote! { - #[doc = "All possible packets enum"] - #[derive(Debug)] - pub enum PacketRef<'a> { - Pack1(Pack1Ref<'a>), - Pack2(Pack2Ref<'a>), - Unknown(UnknownPacketRef<'a>), - } - - impl<'a> PacketRef<'a> { - pub fn class_and_msg_id(&self) -> (u8, u8) { - match *self { - PacketRef::Pack1(_) => (Pack1::CLASS, Pack1::ID), - PacketRef::Pack2(_) => (Pack2::CLASS, Pack2::ID), - PacketRef::Unknown(ref pack) => (pack.class, pack.msg_id), - } - } - } - - pub(crate) fn match_packet( - class: u8, - msg_id: u8, - payload: &[u8], - ) -> Result { - match (class, msg_id) { - (Pack1::CLASS, Pack1::ID) if ::validate(payload).is_ok() => { - Ok(PacketRef::Pack1(Pack1Ref(payload))) - } - (Pack2::CLASS, Pack2::ID) if ::validate(payload).is_ok() => { - Ok(PacketRef::Pack2(Pack2Ref(payload))) - } - _ => Ok(PacketRef::Unknown(UnknownPacketRef { - payload, - class, - msg_id, - })), - } - } - - const fn max_u16(a: u16, b: u16) -> u16 { - [a, b][(a < b) as usize] - } - - pub(crate) const MAX_PAYLOAD_LEN: u16 = max_u16( - Pack2::MAX_PAYLOAD_LEN, - max_u16(Pack1::MAX_PAYLOAD_LEN, 0u16), - ); - - #[cfg(feature = "serde")] - pub struct PacketSerializer<'a, T> { - class: u8, - msg_id: u8, - msg: &'a T, - } - - #[cfg(feature = "serde")] - impl<'a, T: SerializeUbxPacketFields> serde::Serialize for PacketSerializer<'a, T> { - fn serialize(&self, serializer: S) -> Result - where - S: serde::Serializer, - { - let mut state = serializer.serialize_map(None)?; - state.serialize_entry("class", &self.class)?; - state.serialize_entry("msg_id", &self.msg_id)?; - self.msg.serialize_fields(&mut state)?; - state.end() - } - } - - #[cfg(feature = "serde")] - impl serde::Serialize for PacketRef<'_> { - fn serialize(&self, serializer: S) -> Result - where - S: serde::Serializer, - { - match *self { - PacketRef::Pack1(ref msg) => crate::ubx_packets::PacketSerializer { - class: Pack1::CLASS, - msg_id: Pack1::ID, - msg, - } - .serialize(serializer), - PacketRef::Pack2(ref msg) => crate::ubx_packets::PacketSerializer { - class: Pack2::CLASS, - msg_id: Pack2::ID, - msg, - } - .serialize(serializer), - PacketRef::Unknown(ref pack) => pack.serialize(serializer), - } - } - } - }, - ); -} - -#[test] -fn test_extend_bitflags() { - let src_code = quote! { - #[ubx_extend_bitflags] - #[ubx(from, rest_reserved)] - bitflags! { - #[doc = "Navigation Status Flags"] - pub struct Test: u8 { - #[doc = "position and velocity valid and within DOP and ACC Masks"] - const F1 = 1; - #[doc = "DGPS used"] - const F2 = 2; - #[doc = "Week Number valid"] - const F3 = 4; - #[doc = "Time of Week valid"] - const F4 = 8; - } - } - }; - let src_code = src_code.to_string(); - - let mac: syn::ItemMacro = syn::parse_str(&src_code) - .unwrap_or_else(|err| panic_on_parse_error("test_extend_bitflags", &src_code, &err)); - let tokens = extend_bitflags(mac) - .unwrap_or_else(|err| panic_on_parse_error("test_extend_bitflags", &src_code, &err)); - run_compare_test( - tokens, - quote! { - bitflags! { - # [doc = "Navigation Status Flags"] - pub struct Test : u8 { - # [doc = "position and velocity valid and within DOP and ACC Masks"] - const F1 = (1 as u8) ; - # [doc = "DGPS used"] - const F2 = ((1 as u8) << 1u32) ; - # [doc = "Week Number valid"] - const F3 = ((1 as u8) << 2u32) ; - # [doc = "Time of Week valid"] - const F4 = ((1 as u8) << 3u32) ; - const RESERVED4 = ((1 as u8) << 4u32) ; - const RESERVED5 = ((1 as u8) << 5u32) ; - const RESERVED6 = ((1 as u8) << 6u32) ; - const RESERVED7 = ((1 as u8) << 7u32) ; - } - } - impl Test { - const fn from(x: u8) -> Self { - Self::from_bits_truncate(x) - } - } - - #[cfg(feature = "serde")] - impl serde::Serialize for Test { - fn serialize(&self, serializer: S) -> Result - where - S: serde::Serializer, - { - serializer.serialize_u8(self.bits()) - } - } - }, - ); -} - -fn run_compare_test(output: TokenStream, expect_output: TokenStream) { - let output = output.to_string(); - let output = String::from_utf8(rustfmt_cnt(output.into_bytes()).unwrap()).unwrap(); - let expect_output = expect_output.to_string(); - let expect_output = - String::from_utf8(rustfmt_cnt(expect_output.into_bytes()).unwrap()).unwrap(); - - if expect_output != output { - for (e, g) in expect_output.lines().zip(output.lines()) { - if e != g { - println!("first mismatch:\ne {}\ng {}", e, g); - break; - } - } - panic!("Expect:\n{}\nGot:\n{}\n", expect_output, output); - } -} - -fn rustfmt_cnt(source: Vec) -> io::Result> { - let rustfmt = - which("rustfmt").map_err(|e| io::Error::new(io::ErrorKind::Other, format!("{}", e)))?; - - let mut cmd = Command::new(&*rustfmt); - - cmd.stdin(Stdio::piped()) - .stdout(Stdio::piped()) - .stderr(Stdio::piped()); - - let mut child = cmd.spawn()?; - let mut child_stdin = child.stdin.take().unwrap(); - let mut child_stdout = child.stdout.take().unwrap(); - let mut child_stderr = child.stderr.take().unwrap(); - let src_len = source.len(); - let src = Arc::new(source); - // Write to stdin in a new thread, so that we can read from stdout on this - // thread. This keeps the child from blocking on writing to its stdout which - // might block us from writing to its stdin. - let stdin_handle = ::std::thread::spawn(move || { - let _ = child_stdin.write_all(src.as_slice()); - src - }); - - // Read from stderr in a new thread also - let stderr_handle = ::std::thread::spawn(move || { - let mut stderr = vec![]; - io::copy(&mut child_stderr, &mut stderr).unwrap(); - stderr - }); - - let mut output = Vec::with_capacity(src_len); - io::copy(&mut child_stdout, &mut output)?; - let status = child.wait()?; - let src = stdin_handle.join().expect( - "The thread writing to rustfmt's stdin doesn't do \ - anything that could panic", - ); - let stderr = stderr_handle.join().unwrap(); - let src = - Arc::try_unwrap(src).expect("Internal error: rusftfmt_cnt should only one Arc refernce"); - match status.code() { - Some(0) => Ok(output), - Some(2) => Err(io::Error::new( - io::ErrorKind::Other, - "Rustfmt parsing errors.".to_string(), - )), - Some(3) => { - println!("warning=Rustfmt could not format some lines."); - println!("{}", std::str::from_utf8(&stderr).unwrap()); - Ok(src) - }, - _ => { - println!("warning=Internal rustfmt error"); - println!("{}", std::str::from_utf8(&stderr).unwrap()); - Ok(src) - }, - } -} - -fn panic_on_parse_error(name: &str, src_cnt: &str, err: &Error) -> ! { - use std::fmt::Write; - - let span = err.span(); - let start = span.start(); - let end = span.end(); - - let mut code_problem = String::new(); - let nlines = end.line - start.line + 1; - for (i, line) in src_cnt - .lines() - .skip(start.line - 1) - .take(nlines) - .enumerate() - { - code_problem.push_str(line); - code_problem.push('\n'); - if i == 0 && start.column > 0 { - write!(&mut code_problem, "{:1$}", ' ', start.column).expect("write to String failed"); - } - let code_problem_len = if i == 0 { - if i == nlines - 1 { - end.column - start.column - } else { - line.len() - start.column - 1 - } - } else if i != nlines - 1 { - line.len() - } else { - end.column - }; - writeln!(&mut code_problem, "{:^^1$}", '^', code_problem_len).expect("Not enought memory"); - if i == end.line { - break; - } - } - - panic!( - "parsing of {name} failed\nerror: {err}\n{code_problem}\nAt {name}:{line_s}:{col_s}", - name = name, - err = err, - code_problem = code_problem, - line_s = start.line, - col_s = start.column, - ); -} diff --git a/ublox/ublox_derive/src/types.rs b/ublox/ublox_derive/src/types.rs deleted file mode 100644 index 777fdcd..0000000 --- a/ublox/ublox_derive/src/types.rs +++ /dev/null @@ -1,230 +0,0 @@ -use proc_macro2::TokenStream; -use quote::{quote, ToTokens}; -use std::num::NonZeroUsize; -use syn::{Attribute, Ident, Type}; - -pub struct PackDesc { - pub name: String, - pub header: PackHeader, - pub comment: String, - pub fields: Vec, -} - -impl PackDesc { - /// if packet has variable size, then `None` - pub fn packet_payload_size(&self) -> Option { - PackDesc::fields_size(self.fields.iter()) - } - - pub fn packet_payload_size_except_last_field(&self) -> Option { - PackDesc::fields_size(self.fields.iter().rev().skip(1)) - } - - pub fn packet_payload_size_except_size_fn(&self) -> Option { - PackDesc::fields_size(self.fields.iter().filter(|f| f.size_fn().is_none())) - } - - fn fields_size<'a, I: Iterator>(iter: I) -> Option { - let mut ret: usize = 0; - for f in iter { - let size = f.size_bytes?; - ret = ret - .checked_add(size.get()) - .expect("overflow during packet size calculation"); - } - Some(ret) - } -} - -pub struct PackHeader { - pub class: u8, - pub id: u8, - pub payload_len: PayloadLen, - pub flags: Vec, -} - -#[derive(Debug, Clone, Copy)] -pub enum PayloadLen { - Fixed(u16), - Max(u16), -} - -impl PayloadLen { - pub fn fixed(&self) -> Option { - if let PayloadLen::Fixed(len) = self { - Some(*len) - } else { - None - } - } -} - -#[derive(Debug)] -pub struct PackField { - pub name: Ident, - pub ty: Type, - pub map: PackFieldMapDesc, - pub comment: String, - pub size_bytes: Option, -} - -impl PackField { - pub fn size_fn(&self) -> Option<&TokenStream> { - self.map.map_type.as_ref().and_then(|m| m.size_fn.as_ref()) - } - - pub fn is_optional(&self) -> bool { - self.map - .map_type - .as_ref() - .map_or(false, |m| crate::type_is_option(&m.ty)) - } -} - -#[derive(Debug)] -pub struct PackFieldMapDesc { - pub map_type: Option, - pub scale: Option, - pub alias: Option, - pub convert_may_fail: bool, - pub get_as_ref: bool, -} - -#[derive(Debug)] -pub struct MapTypeDesc { - pub ty: Type, - pub from_fn: TokenStream, - pub is_valid_fn: TokenStream, - pub into_fn: TokenStream, - pub size_fn: Option, -} - -impl PackFieldMapDesc { - pub fn new(x: crate::input::PackFieldMap, raw_ty: &Type) -> Self { - let convert_may_fail = x.convert_may_fail; - let scale_back = x.scale.as_ref().map(|x| quote! { 1. / #x }); - let map_type = x.map_type.map(|map_type| { - let ty = map_type.ty; - let from_fn = map_type.from_fn.unwrap_or_else(|| { - if !convert_may_fail { - quote! { <#ty>::from } - } else { - quote! { <#ty>::from_unchecked } - } - }); - - let is_valid_fn = map_type.is_valid_fn.unwrap_or_else(|| { - quote! { <#ty>::is_valid } - }); - - let into_fn = map_type.into_fn.unwrap_or_else(|| { - if ty == syn::parse_quote! {f32} || ty == syn::parse_quote! {f64} { - if let Some(scale_back) = scale_back { - let conv_method = - quote::format_ident!("as_{}", raw_ty.into_token_stream().to_string()); - - return quote! { - ScaleBack::<#ty>(#scale_back).#conv_method - }; - } - } - - quote! { <#ty>::into_raw } - }); - - MapTypeDesc { - ty, - from_fn, - is_valid_fn, - into_fn, - size_fn: map_type.size_fn, - } - }); - Self { - map_type, - scale: x.scale, - alias: x.alias, - convert_may_fail: x.convert_may_fail, - get_as_ref: x.get_as_ref, - } - } -} - -impl PackField { - pub fn has_intermediate_type(&self) -> bool { - self.map.map_type.is_some() - } - pub fn intermediate_type(&self) -> &Type { - self.map - .map_type - .as_ref() - .map(|x| &x.ty) - .unwrap_or(&self.ty) - } - pub fn intermediate_field_name(&self) -> &Ident { - self.map.alias.as_ref().unwrap_or(&self.name) - } - pub fn is_field_raw_ty_byte_array(&self) -> bool { - if let syn::Type::Array(ref fixed_array) = self.ty { - *fixed_array.elem == syn::parse_quote!(u8) - } else { - false - } - } -} - -pub struct UbxExtendEnum { - pub name: Ident, - pub repr: Type, - pub from_fn: Option, - pub rest_handling: Option, - pub into_fn: Option, - pub variants: Vec<(Ident, u8)>, - pub attrs: Vec, -} - -#[derive(Clone, Copy, PartialEq, Eq)] -pub enum UbxTypeFromFn { - From, - FromUnchecked, -} - -#[derive(Clone, Copy, PartialEq, Eq)] -pub enum UbxTypeIntoFn { - Raw, -} - -#[derive(Clone, Copy, PartialEq, Eq, Debug)] -pub enum UbxEnumRestHandling { - Reserved, - ErrorProne, -} - -pub struct BitFlagsMacro { - pub nbits: u32, - pub vis: syn::Visibility, - pub attrs: Vec, - pub name: Ident, - pub repr_ty: Type, - pub consts: Vec, - pub from_fn: Option, - pub into_fn: Option, - pub rest_handling: Option, -} - -pub struct BitFlagsMacroItem { - pub attrs: Vec, - pub name: Ident, - pub value: u64, -} - -#[derive(PartialEq, Eq, Clone, Copy)] -pub enum PacketFlag { - DefaultForBuilder, -} - -pub struct RecvPackets { - pub union_enum_name: Ident, - pub unknown_ty: Ident, - pub all_packets: Vec, -}