From 298c1828018cd63dad160e66bb926eceac667591 Mon Sep 17 00:00:00 2001 From: Carson Swoveland Date: Sat, 24 Feb 2024 15:41:18 -0500 Subject: [PATCH] Stuff --- src/bin/minimal.rs | 4 +- src/gps.rs | 128 +++++++++++++++++++++++++++++--------- src/lib.rs | 22 ++++++- ublox/ublox/src/parser.rs | 2 +- 4 files changed, 122 insertions(+), 34 deletions(-) diff --git a/src/bin/minimal.rs b/src/bin/minimal.rs index 5228a37..f47eaa9 100644 --- a/src/bin/minimal.rs +++ b/src/bin/minimal.rs @@ -17,6 +17,7 @@ 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; @@ -195,7 +196,7 @@ mod app { // TODO: Add tasks #[task( priority = 1, - shared = [display] + shared = [display, gps] )] async fn display_task(mut cx: display_task::Context) { trace!("display_task enter"); @@ -211,7 +212,6 @@ mod app { display.flush(); }); Systick::delay(500.millis()).await; - info!("loop {}", i.0); i += 1; } } diff --git a/src/gps.rs b/src/gps.rs index 0f4c2f5..b180945 100644 --- a/src/gps.rs +++ b/src/gps.rs @@ -5,26 +5,36 @@ use stm32l4xx_hal::hal::serial; use ublox::{CfgMsgAllPortsBuilder, CfgPrtUartBuilder, NavPvt, PacketRef, Parser, UartMode, UartPortId}; use tinyvec::ArrayVec; -struct WriteableArrayVec(ArrayVec<[u8; 256]>); +use crate::FmtBuf; -impl Write for WriteableArrayVec { - fn write_str(&mut self, s: &str) -> fmt::Result { - for b in s.bytes() { - self.0.try_push(b); - } - Ok(()) - } + + +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, } -impl WriteableArrayVec { - pub fn as_str(&self) -> Option<&str> { - core::str::from_utf8(self.0.as_slice()).ok() - } +pub struct Position { + latitude: u32, + longitude: u32, } pub struct Gps { pub serial: SERIAL, - pub parser: Parser> + pub parser: Parser>, + pub state: State, + + pub position: Position, } impl Gps @@ -35,7 +45,9 @@ impl Gps pub fn new(serial: SERIAL) -> Self { let mut s = Self { serial, - parser: Parser::new(ArrayVec::new()) + parser: Parser::new(ArrayVec::new()), + state: State::Sync0, + position: Position { latitude: 0, longitude: 0 } }; s.configure(); @@ -73,23 +85,81 @@ 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)); - 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 = WriteableArrayVec(Default::default()); - write!(&mut s, "{err:?}").unwrap(); - if let Some(s) = s.as_str() { - error!("ubx error {}", s); - } else { - error!("ubx error"); + //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, } } - None => break + + self.state = State::Sync0; } } } diff --git a/src/lib.rs b/src/lib.rs index 786064e..0d168e8 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,13 +1,14 @@ #![no_main] #![no_std] -use core::sync::atomic::{AtomicUsize, Ordering}; +use core::{fmt::{self, Write}, sync::atomic::{AtomicUsize, Ordering}}; use defmt_brtt as _; // global logger use panic_probe as _; // TODO(6) Import your HAL -use stm32l4xx_hal as _; // memory layout +use stm32l4xx_hal as _; +use tinyvec::ArrayVec; // memory layout pub mod display; pub mod gps; @@ -34,3 +35,20 @@ pub fn exit() -> ! { cortex_m::asm::bkpt(); } } + +pub struct FmtBuf(ArrayVec<[u8; 256]>); + +impl Write for FmtBuf { + fn write_str(&mut self, s: &str) -> fmt::Result { + for b in s.bytes() { + self.0.try_push(b); + } + Ok(()) + } +} + +impl FmtBuf { + pub fn as_str(&self) -> Option<&str> { + core::str::from_utf8(self.0.as_slice()).ok() + } +} \ No newline at end of file diff --git a/ublox/ublox/src/parser.rs b/ublox/ublox/src/parser.rs index aea26c3..338fc26 100644 --- a/ublox/ublox/src/parser.rs +++ b/ublox/ublox/src/parser.rs @@ -81,7 +81,7 @@ impl UnderlyingBuffer for Vec { } } -impl UnderlyingBuffer for tinyvec::ArrayVec<[u8; 256]> { +impl UnderlyingBuffer for tinyvec::ArrayVec<[u8; 512]> { fn clear(&mut self) { self.clear(); }