diff --git a/Cargo.toml b/Cargo.toml index bf6220d..d47374e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -17,6 +17,7 @@ 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" # cargo build/run [profile.dev] diff --git a/src/bin/minimal.rs b/src/bin/minimal.rs index d0215ae..5228a37 100644 --- a/src/bin/minimal.rs +++ b/src/bin/minimal.rs @@ -142,12 +142,11 @@ mod app { let mut gps_uart = hal::serial::Serial::usart1( cx.device.USART1, (tx, rx), - Config::default().baudrate(115200.bps()), + Config::default().baudrate(9600.bps()), clocks, &mut rcc.apb2 ); gps_uart.listen(serial::Event::Rxne); - // rtic::pend(Interrupt::USART1); // Spawn tasks display_task::spawn().unwrap(); @@ -182,13 +181,9 @@ mod app { #[task(binds = USART1, shared = [gps])] fn on_uart(mut cx: on_uart::Context) { - info!("foo"); - if let Ok(b) = cx.shared.gps.lock(|gps| gps.serial.read()) { - info!("hewwo {}", b); - } - // cx.shared.gps.lock(|gps| { - // gps.handle(); - // }); + cx.shared.gps.lock(|gps| { + gps.handle(); + }); } diff --git a/src/gps.rs b/src/gps.rs index 3ca7b86..0f4c2f5 100644 --- a/src/gps.rs +++ b/src/gps.rs @@ -1,21 +1,73 @@ +use core::fmt::{self, Write}; + use defmt::{error, info}; use stm32l4xx_hal::hal::serial; -use ublox::{PacketRef, Parser}; +use ublox::{CfgMsgAllPortsBuilder, CfgPrtUartBuilder, NavPvt, PacketRef, Parser, UartMode, UartPortId}; use tinyvec::ArrayVec; +struct WriteableArrayVec(ArrayVec<[u8; 256]>); + +impl Write for WriteableArrayVec { + fn write_str(&mut self, s: &str) -> fmt::Result { + for b in s.bytes() { + self.0.try_push(b); + } + Ok(()) + } +} + +impl WriteableArrayVec { + pub fn as_str(&self) -> Option<&str> { + core::str::from_utf8(self.0.as_slice()).ok() + } +} + pub struct Gps { pub serial: SERIAL, pub parser: Parser> } impl Gps - where SERIAL: serial::Read, - SERIAL::Error: core::fmt::Debug + where SERIAL: serial::Read + serial::Write, + >::Error: core::fmt::Debug, + >::Error: core::fmt::Debug, { pub fn new(serial: SERIAL) -> Self { - Self { + let mut s = Self { serial, parser: Parser::new(ArrayVec::new()) + }; + + s.configure(); + + s + } + + fn configure(&mut self) { + let packet = CfgPrtUartBuilder { + portid: UartPortId::Uart1, + reserved0: 0, + tx_ready: 0, + mode: UartMode::new( + ublox::DataBits::Eight, + ublox::Parity::None, + ublox::StopBits::One, + ), + baud_rate: 9600, + in_proto_mask: ublox::InProtoMask::UBLOX, + out_proto_mask: ublox::OutProtoMask::UBLOX, + flags: 0, + reserved5: 0, + }.into_packet_bytes(); + + for b in packet { + nb::block!(self.serial.write(b)).unwrap(); + } + + let packet = CfgMsgAllPortsBuilder::set_rate_for::([1, 1, 1, 1, 1, 1]).into_packet_bytes(); + + for b in packet { + nb::block!(self.serial.write(b)).unwrap(); } } @@ -28,7 +80,15 @@ impl Gps PacketRef::NavPvt(navpvt) => info!("lat: {}, lon: {}", navpvt.lat_degrees(), navpvt.lon_degrees()), _ => info!("other packet") }, - Some(Err(_)) => error!("ubx error"), + Some(Err(err)) => { + let mut s = WriteableArrayVec(Default::default()); + write!(&mut s, "{err:?}").unwrap(); + if let Some(s) = s.as_str() { + error!("ubx error {}", s); + } else { + error!("ubx error"); + } + } None => break } }