Skip to content

Commit

Permalink
UBX parser
Browse files Browse the repository at this point in the history
  • Loading branch information
@sky-dragn
sky-dragn committed Feb 26, 2024
1 parent 298c182 commit 36d55df
Show file tree
Hide file tree
Showing 7 changed files with 288 additions and 181 deletions.
2 changes: 1 addition & 1 deletion Cargo.toml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down
81 changes: 65 additions & 16 deletions src/bin/minimal.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,37 +4,36 @@

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<
Spi<SPI1, (
PB3<Alternate<PushPull, 5>>,
PB4<Alternate<PushPull, 5>>,
PB5<Alternate<PushPull, 5>>
)>, PA11<Output<PushPull>>>;
)>, PA1<Output<PushPull>>>;

type GpsUart = Serial<USART1, (PA9<Alternate<PushPull, 7>>, PA10<Alternate<PushPull, 7>>)>;

Expand All @@ -46,6 +45,8 @@ type GpsUart = Serial<USART1, (PA9<Alternate<PushPull, 7>>, PA10<Alternate<PushP
dispatchers = [EXTI0],
)]
mod app {


use super::*;

// Shared resources go here
Expand All @@ -58,6 +59,7 @@ mod app {
// Local resources go here
#[local]
struct Local {
count: usize
// TODO: Add resources
}

Expand Down Expand Up @@ -103,7 +105,7 @@ mod app {
let mut gpiob = cx.device.GPIOB.split(&mut rcc.ahb2);

// Initialize SPI and display
let mut cs = gpioa.pa11.into_push_pull_output(&mut gpioa.moder, &mut gpioa.otyper);
let mut cs = gpioa.pa1.into_push_pull_output(&mut gpioa.moder, &mut gpioa.otyper);
cs.set_low();
let sck = gpiob.pb3.into_alternate(&mut gpiob.moder, &mut gpiob.otyper, &mut gpiob.afrl);
let mosi = gpiob.pb5.into_alternate(&mut gpiob.moder, &mut gpiob.otyper, &mut gpiob.afrl);
Expand Down Expand Up @@ -149,6 +151,29 @@ 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);

// let usb = hal::usb::Peripheral {
// usb: cx.device.USB,
// pin_dm: dm,
// pin_dp: dp
// };

// static USB_BUS: Once<UsbBus> = 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();

Expand All @@ -161,6 +186,7 @@ mod app {
},
Local {
// Initialization of local resources go here
count: 0
},
)
}
Expand All @@ -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)]
Expand All @@ -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)]
Expand All @@ -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;
}
Expand Down
7 changes: 3 additions & 4 deletions src/display.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,7 @@ impl<SPI, CS> SharpMemDisplayDriver<SPI, CS>
SpiTransaction::start(self, command)
}

pub fn clear(&mut self) {
pub fn clear_flush(&mut self) {
self.start(CLEAR_BIT).send(&[0x00]).finish();
}

Expand Down Expand Up @@ -147,10 +147,9 @@ impl<SPI, CS> SharpMemDisplay<SPI, CS>
}

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;
}
}

Expand Down
122 changes: 21 additions & 101 deletions src/gps.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<SERIAL> {
pub serial: SERIAL,
pub parser: Parser<ArrayVec<[u8; UBLOX_BUFFER_SIZE]>>,
pub state: State,
pub parser: UbxParser,

pub position: Position,
count: usize
}

impl<SERIAL> Gps<SERIAL>
Expand All @@ -45,9 +29,9 @@ impl<SERIAL> Gps<SERIAL>
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();
Expand Down Expand Up @@ -85,81 +69,17 @@ 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));

//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)
}
}
}
Expand Down
4 changes: 2 additions & 2 deletions src/lib.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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 {
Expand Down
Loading

0 comments on commit 36d55df

Please sign in to comment.