From 36d55df2f0435b7b0f87753ba0f9ecee5200c2ca Mon Sep 17 00:00:00 2001 From: Gary B Date: Mon, 26 Feb 2024 17:48:07 -0500 Subject: [PATCH] UBX parser --- Cargo.toml | 2 +- src/bin/minimal.rs | 81 +++++++++++++++---- src/display.rs | 7 +- src/gps.rs | 122 +++++----------------------- src/lib.rs | 4 +- src/nmea.rs | 57 ------------- src/ubx.rs | 196 +++++++++++++++++++++++++++++++++++++++++++++ 7 files changed, 288 insertions(+), 181 deletions(-) delete mode 100644 src/nmea.rs create mode 100644 src/ubx.rs diff --git a/Cargo.toml b/Cargo.toml index d47374e..f8850d1 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -15,9 +15,9 @@ rtic-monotonics = { version = "1.0.0", features = ["cortex-m-systick", "systick- thiserror = { version = "1.0.50", package = "thiserror-core", default-features = false } embedded-graphics = "0.8.1" tinyvec = "1.6.0" -#nmea0183 = "0.4.0" ublox = { version = "0.4.5", default-features = false } nb = "1.1.0" +bytemuck = { version = "1.14.3", features = ["derive"] } # cargo build/run [profile.dev] diff --git a/src/bin/minimal.rs b/src/bin/minimal.rs index f47eaa9..c7039fc 100644 --- a/src/bin/minimal.rs +++ b/src/bin/minimal.rs @@ -4,29 +4,28 @@ use gps_watch as _; +use hal::gpio::PA1; use stm32l4xx_hal::{self as hal, pac, prelude::*}; use rtic_monotonics::create_systick_token; use rtic_monotonics::systick::Systick; -use stm32l4xx_hal::gpio::{Alternate, Output, PA10, PA11, PA9, PB3, PB4, PB5, PushPull}; +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 core::num::Wrapping; -use embedded_graphics::primitives::PrimitiveStyle; use embedded_graphics::prelude::*; use embedded_graphics::pixelcolor::BinaryColor; -use embedded_graphics::primitives::Rectangle; use embedded_graphics::text::Text; use stm32l4xx_hal::serial::Serial; use core::fmt::Write; -use rtic::Mutex; -use stm32l4xx_hal::pac::Interrupt; use stm32l4xx_hal::rcc::{ClockSecuritySystem, CrystalBypass}; use stm32l4xx_hal::rtc::{Event, RtcClockSource, RtcConfig}; use stm32l4xx_hal::serial; use stm32l4xx_hal::serial::Config; use gps_watch::gps::Gps; +use embedded_graphics::mono_font::{ascii::FONT_10X20, MonoTextStyle}; +use gps_watch::FmtBuf; +use core::num::Wrapping; // Rename type to squash generics type SharpMemDisplay = gps_watch::display::SharpMemDisplay< @@ -34,7 +33,7 @@ type SharpMemDisplay = gps_watch::display::SharpMemDisplay< PB3>, PB4>, PB5> - )>, PA11>>; + )>, PA1>>; type GpsUart = Serial>, PA10>)>; @@ -46,6 +45,8 @@ type GpsUart = Serial>, PA10 = Once::new(); + // USB_BUS.set(hal::usb::UsbBus::new(usb)); + + // // let _: () = USB_BUS.get().unwrap(); + // let usb_serial = usbd_serial::SerialPort::new(USB_BUS.get().unwrap()); + + // 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(); + // Spawn tasks display_task::spawn().unwrap(); @@ -161,6 +186,7 @@ mod app { }, Local { // Initialization of local resources go here + count: 0 }, ) } @@ -171,6 +197,10 @@ mod app { trace!("idle enter"); loop { + + trace!("usb"); + + trace!("sleep"); // Only sleep in release mode, since the debugger doesn't interact with sleep very nice #[cfg(debug_assertions)] @@ -180,12 +210,17 @@ mod app { } } - #[task(binds = USART1, shared = [gps])] + #[task(binds = USART1, shared = [gps], local = [count])] fn on_uart(mut cx: on_uart::Context) { + // cx.shared.gps.lock(|gps| { + // gps.handle(); + // }); cx.shared.gps.lock(|gps| { - gps.handle(); - }); - + if let Ok(b) = gps.serial.read() { + *cx.local.count += 1; + info!("got {:x} #{}", b, cx.local.count); + } + }) } #[task(binds = RTC_WKUP)] @@ -204,13 +239,27 @@ mod app { let mut i = Wrapping(0u8); loop { - let stroke = PrimitiveStyle::with_stroke(BinaryColor::On, 3); - let rect_styled = Rectangle { top_left: Point {x: i.0 as i32, y: i.0 as i32}, size: Size { width: 20, height: 20 } } - .into_styled(stroke); + let pos = cx.shared.gps.lock(|gps| gps.position); + let mut txt = FmtBuf(Default::default()); + write!(txt, "Lat: {}\nLon: {}", pos.latitude, pos.longitude).unwrap(); + // info!("formatted: {}", txt.as_str()); cx.shared.display.lock(|display| { - rect_styled.draw(display).unwrap(); + display.clear(); + Text::new( + txt.as_str().unwrap(), + Point::new(30, 30), + MonoTextStyle::new(&FONT_10X20, BinaryColor::On) + ).draw(display).unwrap(); display.flush(); }); + + // let stroke = PrimitiveStyle::with_stroke(BinaryColor::On, 3); + // let rect_styled = Rectangle { top_left: Point {x: i.0 as i32, y: i.0 as i32}, size: Size { width: 20, height: 20 } } + // .into_styled(stroke); + // cx.shared.display.lock(|display| { + // rect_styled.draw(display).unwrap(); + // display.flush(); + // }); Systick::delay(500.millis()).await; i += 1; } diff --git a/src/display.rs b/src/display.rs index 984cb05..08b8fd8 100644 --- a/src/display.rs +++ b/src/display.rs @@ -81,7 +81,7 @@ impl SharpMemDisplayDriver SpiTransaction::start(self, command) } - pub fn clear(&mut self) { + pub fn clear_flush(&mut self) { self.start(CLEAR_BIT).send(&[0x00]).finish(); } @@ -147,10 +147,9 @@ impl SharpMemDisplay } pub fn clear(&mut self) { - self.driver.clear(); self.buf = [[0xFF; WIDTH_BYTES]; HEIGHT]; - self.dirty = [0; HEIGHT_BYTES]; - self.dirty_any = false; + self.dirty = [0xFF; HEIGHT_BYTES]; + self.dirty_any = true; } } diff --git a/src/gps.rs b/src/gps.rs index b180945..7d32ef8 100644 --- a/src/gps.rs +++ b/src/gps.rs @@ -2,39 +2,23 @@ use core::fmt::{self, Write}; use defmt::{error, info}; use stm32l4xx_hal::hal::serial; -use ublox::{CfgMsgAllPortsBuilder, CfgPrtUartBuilder, NavPvt, PacketRef, Parser, UartMode, UartPortId}; +use ublox::{CfgMsgAllPortsBuilder, CfgPrtUartBuilder, NavPvt, UartMode, UartPortId}; use tinyvec::ArrayVec; -use crate::FmtBuf; - - - -const UBLOX_BUFFER_SIZE: usize = 512; - -#[derive(Debug, defmt::Format)] -pub enum State { - Sync0, - Sync1, - Class, - Id, - Length0, - Length1 { lo: u8 }, - Body { remaining: u16 }, - Checksum0, - Checksum1, -} +use crate::{ubx::{UbxPacket, UbxParser}, FmtBuf}; +#[derive(Copy, Clone)] pub struct Position { - latitude: u32, - longitude: u32, + pub latitude: i32, + pub longitude: i32, } pub struct Gps { pub serial: SERIAL, - pub parser: Parser>, - pub state: State, + pub parser: UbxParser, pub position: Position, + count: usize } impl Gps @@ -45,9 +29,9 @@ impl Gps pub fn new(serial: SERIAL) -> Self { let mut s = Self { serial, - parser: Parser::new(ArrayVec::new()), - state: State::Sync0, - position: Position { latitude: 0, longitude: 0 } + parser: UbxParser::new(), + position: Position { latitude: 0, longitude: 0 }, + count: 0 }; s.configure(); @@ -85,81 +69,17 @@ impl Gps pub fn handle(&mut self) { if let Ok(b) = self.serial.read() { - //let mut it = self.parser.consume(core::slice::from_ref(&b)); - - //info!("stuff {}", self.state); - - match self.state { - State::Sync0 => { - if b == 0xB5 { - self.parser.consume(core::slice::from_ref(&b)); - self.state = State::Sync1; - } - } - State::Sync1 => { - if b == 0x62 { - self.parser.consume(core::slice::from_ref(&b)); - self.state = State::Class; - } else { - self.state = State::Sync0; - } - } - State::Class => { - self.parser.consume(core::slice::from_ref(&b)); - self.state = State::Id; - } - State::Id => { - self.parser.consume(core::slice::from_ref(&b)); - self.state = State::Length0; - } - State::Length0 => { - self.parser.consume(core::slice::from_ref(&b)); - self.state = State::Length1 { lo: b } - } - State::Length1 { lo } => { - self.parser.consume(core::slice::from_ref(&b)); - let remaining = u16::from_le_bytes([lo, b]); - if remaining == 0 { - self.state = State::Checksum0; - } else { - self.state = State::Body { remaining }; - } - } - State::Body { remaining } => { - self.parser.consume(core::slice::from_ref(&b)); - if remaining == 1 { - self.state = State::Checksum0; - } else { - self.state = State::Body { remaining: remaining - 1 } - } - } - State::Checksum0 => { - self.parser.consume(core::slice::from_ref(&b)); - self.state = State::Checksum1; - } - State::Checksum1 => { - let mut it = self.parser.consume(core::slice::from_ref(&b)); - - loop { - match it.next() { - Some(Ok(pkt)) => match pkt { - PacketRef::NavPvt(navpvt) => info!("lat: {}, lon: {}", navpvt.lat_degrees(), navpvt.lon_degrees()), - _ => info!("other packet") - }, - Some(Err(err)) => { - let mut s = FmtBuf(Default::default()); - write!(&mut s, "{err:?}").unwrap(); - if let Some(s) = s.as_str() { - error!("ubx error {}", s); - } else { - error!("ubx error"); - } - } - None => break, - } - } - - self.state = State::Sync0; + self.count += 1; + info!("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) } } } diff --git a/src/lib.rs b/src/lib.rs index 0d168e8..24850e8 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -12,7 +12,7 @@ use tinyvec::ArrayVec; // memory layout pub mod display; pub mod gps; -pub mod nmea; +pub mod ubx; // same panicking *behavior* as `panic-probe` but doesn't print a panic message // this prevents the panic message being printed *twice* when `defmt::panic` is invoked @@ -36,7 +36,7 @@ pub fn exit() -> ! { } } -pub struct FmtBuf(ArrayVec<[u8; 256]>); +pub struct FmtBuf(pub ArrayVec<[u8; 256]>); impl Write for FmtBuf { fn write_str(&mut self, s: &str) -> fmt::Result { diff --git a/src/nmea.rs b/src/nmea.rs deleted file mode 100644 index a76464b..0000000 --- a/src/nmea.rs +++ /dev/null @@ -1,57 +0,0 @@ -// use tinyvec::ArrayVec; -// -// -// pub struct NmeaParser { -// buf: ArrayVec<[u8; 100]> -// } -// -// impl NmeaParser { -// pub fn new() -> Self { -// Self { -// buf: ArrayVec::new() -// } -// } -// -// pub fn parse_byte(&mut self, byte: u8) -> Option> { -// if byte == b'\n' { -// Some(self.parse_buf()) -// } -// else { -// None -// } -// } -// -// fn parse_buf(&mut self) -> Result { -// // self.buf -// self.buf.clear(); -// } -// } -// -// pub enum NmeaSentence { -// Gga { -// utc: f32, -// lat: f32, -// lon: f32, -// -// } -// } -// -// // type Text = ArrayVec<[u8; len]>; -// // enum ParserState { -// // StartUnknown, // Default or unknown state -// // StartSentence, // Start of sentence `$` -// // TalkerId(Text<2>), // Receiving the Talker ID `GP` -// // SentenceId(Text<3>), // Reading the Sentence ID `GGA` -// // // GGA components -// // GgaUtc(Text<16>), -// // GgaLat(f32, Text<16>), -// // GgaLatDir(f32, f32, bool), // true = N -// // GgaLon(f32, f32, bool, Text<16>), -// // GgaLonDir(f32, f32, bool, f32, bool), // true = E -// // GgaQual(f32, f32, bool, f32, bool, u8), -// // GgaSv(f32, f32, bool, f32, bool, u8, Text<3>), -// // GgaHdop(f32, f32, bool, f32, bool, u8, u8, Text<16>), -// // GgaOrthoHeight(f32, f32, bool, f32, u8, u8, f32, Text<16>), -// // GgaOrthoUnit(f32, f32, bool, f32, u8, u8, f32, Text<16>), -// // -// // } \ No newline at end of file diff --git a/src/ubx.rs b/src/ubx.rs new file mode 100644 index 0000000..980c099 --- /dev/null +++ b/src/ubx.rs @@ -0,0 +1,196 @@ +use tinyvec::ArrayVec; +use bytemuck::{Pod, Zeroable}; + +const UBX_BUF_SIZE: usize = 256; +type UbxBuf = ArrayVec<[u8; UBX_BUF_SIZE]>; + +#[derive(Copy, Clone)] +pub struct UbxChecksum(pub u8, pub u8); +impl UbxChecksum { + pub fn new() -> Self { + Self(0, 0) + } + + pub fn next(self, byte: u8) -> Self { + let Self(a, b) = self; + Self(a.wrapping_add(byte), b.wrapping_add(a).wrapping_add(byte)) + } +} + +impl PartialEq<(u8, u8)> for UbxChecksum { + fn eq(&self, (other_a, other_b): &(u8, u8)) -> bool { + let Self(a, b) = self; + a == other_a && b == other_b + } +} + +#[derive(defmt::Format)] +pub enum UbxError { + BadChecksum {expect: (u8, u8), saw: (u8, u8)}, + BadStart {expect: u8, saw: u8}, + BadPayload, + TooLarge(u16) +} +use UbxError::*; + +#[derive(Copy, Clone)] +enum ParserState { + Start, + Sync1, + Sync2, + Class {class: u8, checksum: UbxChecksum}, + Id {class: u8, id: u8, checksum: UbxChecksum}, + Len1 {class: u8, id: u8, len1: u8, checksum: UbxChecksum}, + Len2 {class: u8, id: u8, len: u16, checksum: UbxChecksum}, + Payload {class: u8, id: u8, len: u16, checksum: UbxChecksum}, + Checksum1 {class: u8, id: u8, expect: UbxChecksum, found: u8}, +} +use ParserState::*; + +pub struct UbxParser { + state: ParserState, + buf: UbxBuf +} + +impl UbxParser { + pub fn new() -> Self { + Self { + state: Start, + buf: UbxBuf::new() + } + } + + fn feed(&mut self, b: u8) -> Option> { + match self.state { + Start => + if b == 0xb5 { + self.state = Sync1; + None + } else { + self.state = Start; + Some(Err(BadStart {expect: 0xb5, saw: b})) + }, + Sync1 => + if b == 0x62 { + self.state = Sync2; + None + } else { + self.state = Start; + Some(Err(BadStart {expect: 0x62, saw: b})) + }, + Sync2 => { + self.state = Class {class: b, checksum: UbxChecksum::new().next(b)}; + None + }, + Class {class, checksum} => { + self.state = Id {class, id: b, checksum: checksum.next(b)}; + None + }, + Id {class, id, checksum} => { + self.state = Len1 {class, id, len1: b, checksum: checksum.next(b)}; + None + }, + Len1 {class, id, len1, checksum} => { + let len = (b as u16) << 8 | (len1 as u16); + self.state = Len2 {class, id, len, checksum: checksum.next(b)}; + if len as usize > UBX_BUF_SIZE { + Some(Err(TooLarge(len))) + } else { + None + } + }, + Len2 {class, id, len, checksum} => + if len > 0 { + self.buf.clear(); + let _ = self.buf.try_push(b); + self.state = Payload { class, id, len, checksum: checksum.next(b) }; + None + } else { + self.buf.clear(); + self.state = Checksum1 { class, id, expect: checksum, found: b }; + None + }, + Payload {class, id, len, checksum} => + if self.buf.len() == len as usize { + self.state = Checksum1 { class, id, expect: checksum, found: b }; + None + } else { + let _ = self.buf.try_push(b); + None + }, + Checksum1 { class, id, expect, found } => + if expect == (found, b) { + self.state = Start; + Some(Ok((class, id))) + } else { + self.state = Start; + Some(Err(BadChecksum {expect: (expect.0, expect.1), saw: (found, b)})) + } + } + } + + pub fn process_byte(&mut self, b: u8) -> Option> { + self.feed(b).map(|r| r.and_then(|(class, id)| match (class, id) { + (0x05, 0x01) => Ok(UbxPacket::AckAck {class: self.buf[0], id: self.buf[1]}), + (0x05, 0x00) => Ok(UbxPacket::AckNak {class: self.buf[0], id: self.buf[1]}), + (0x01, 0x07) => bytemuck::try_pod_read_unaligned(&self.buf) + .map(|x|UbxPacket::NavPvt(x)) + .map_err(|_| BadPayload), + _ => Ok(UbxPacket::OtherPacket) + })) + } +} + +pub enum UbxPacket { + AckAck {class: u8, id: u8}, + AckNak {class: u8, id: u8}, + NavPvt(NavPvt), + OtherPacket +} + +// 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)] +pub struct NavPvt { + pub i_tow: u32, + pub year: u16, + pub month: u8, + pub day: u8, + + pub hour: u8, + pub min: u8, + pub sec: u8, + pub valid: u8, + + pub t_acc: u32, + pub nano: i32, + + pub fix_type: u8, + pub flags: u8, + pub flags2: u8, + pub sum_sv: u8, + + pub lon: i32, + pub lat: i32, + pub height: i32, + pub h_msl: i32, + pub h_acc: u32, + pub v_acc: u32, + pub vel_n: i32, + pub vel_e: i32, + pub vel_d: i32, + pub g_speed: i32, + pub head_mot: i32, + pub s_acc: u32, + pub head_acc: u32, + + pub p_dop: u16, + pub flags3: u16, + + pub reserved0_a: [u8; 4], + pub head_veh: i32, + + pub mag_dec: i16, + pub mag_acc: u16 +} \ No newline at end of file