Stuff

SuperTails · Feb 24, 2024 · 298c182 · 298c182
1 parent 4096e5d
commit 298c182
Show file tree

Hide file tree

Showing 4 changed files with 122 additions and 34 deletions.
diff --git 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
@@ -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<SERIAL> {
     pub serial: SERIAL,
-    pub parser: Parser<ArrayVec<[u8; 256]>>
+    pub parser: Parser<ArrayVec<[u8; UBLOX_BUFFER_SIZE]>>,
+    pub state: State,
+
+    pub position: Position,
 }
 
 impl<SERIAL> Gps<SERIAL>
@@ -35,7 +45,9 @@ impl<SERIAL> Gps<SERIAL>
     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<SERIAL> Gps<SERIAL>
 
     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
@@ -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()
+    }
+}
diff --git a/ublox/ublox/src/parser.rs b/ublox/ublox/src/parser.rs
@@ -81,7 +81,7 @@ impl UnderlyingBuffer for Vec<u8> {
     }
 }
 
-impl UnderlyingBuffer for tinyvec::ArrayVec<[u8; 256]> {
+impl UnderlyingBuffer for tinyvec::ArrayVec<[u8; 512]> {
     fn clear(&mut self) {
         self.clear();
     }