diff --git a/Cargo.lock b/Cargo.lock index 23c5a3a..82c05fa 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -91,6 +91,12 @@ version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "c08606f8c3cbf4ce6ec8e28fb0014a2c086708fe954eaa885384a6165172e7e8" +[[package]] +name = "az" +version = "1.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7b7e4c2464d97fe331d41de9d5db0def0a96f4d823b8b32a2efd503578988973" + [[package]] name = "base64" version = "0.13.1" @@ -604,6 +610,29 @@ dependencies = [ "nb 1.1.0", ] +[[package]] +name = "embedded-graphics" +version = "0.8.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4e8da660bb0c829b34a56a965490597f82a55e767b91f9543be80ce8ccb416fe" +dependencies = [ + "az", + "byteorder", + "embedded-graphics-core", + "float-cmp", + "micromath", +] + +[[package]] +name = "embedded-graphics-core" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "95743bef3ff70fcba3930246c4e6872882bbea0dcc6da2ca860112e0cd4bd09f" +dependencies = [ + "az", + "byteorder", +] + [[package]] name = "embedded-hal" version = "0.2.7" @@ -629,6 +658,16 @@ dependencies = [ "embedded-hal 1.0.0", ] +[[package]] +name = "embedded-hal-bus" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "513e0b3a8fb7d3013a8ae17a834283f170deaf7d0eeab0a7c1a36ad4dd356d22" +dependencies = [ + "critical-section", + "embedded-hal 1.0.0", +] + [[package]] name = "embedded-io" version = "0.6.1" @@ -1104,6 +1143,8 @@ dependencies = [ "embassy-executor", "embassy-time", "embassy_gps", + "embedded-graphics", + "embedded-hal-bus", "embedded-io 0.7.1", "embedded-io-async 0.7.0", "env_logger", @@ -1116,7 +1157,9 @@ dependencies = [ "esp-rtos", "geo-types", "log", + "mipidsi", "num-traits", + "profont", ] [[package]] @@ -1197,6 +1240,15 @@ version = "0.1.9" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5baebc0774151f905a1a2cc41989300b1e6fbb29aff0ceffa1064fdd3088d582" +[[package]] +name = "float-cmp" +version = "0.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "98de4bbd547a563b716d8dfa9aad1cb19bfab00f4fa09a6a4ed21dbcf44ce9c4" +dependencies = [ + "num-traits", +] + [[package]] name = "fnv" version = "1.0.7" @@ -1537,12 +1589,29 @@ version = "2.8.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f8ca58f447f06ed17d5fc4043ce1b10dd205e060fb3ce5b979b8ed8e59ff3f79" +[[package]] +name = "micromath" +version = "2.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c3c8dda44ff03a2f238717214da50f65d5a53b45cd213a7370424ffdb6fae815" + [[package]] name = "minimal-lexical" version = "0.2.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "68354c5c6bd36d73ff3feceb05efa59b6acb7626617f4962be322a825e61f79a" +[[package]] +name = "mipidsi" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "790ebd28bd67addbccf41b1c0c188c26bb9f5bdcd91d4d6da9bd558e20d97a1d" +dependencies = [ + "embedded-graphics-core", + "embedded-hal 1.0.0", + "heapless 0.8.0", +] + [[package]] name = "nb" version = "0.1.3" @@ -1771,6 +1840,15 @@ dependencies = [ "unicode-ident", ] +[[package]] +name = "profont" +version = "0.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "016681370a9dd6e7ddb4c1a959922fd59dc45e5ebaa5ff5b13090267898ced34" +dependencies = [ + "embedded-graphics", +] + [[package]] name = "quote" version = "1.0.45" diff --git a/Cargo.toml b/Cargo.toml index aa57862..f0b0806 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -40,9 +40,11 @@ cam = ["c-its-parser/cam"] # echo DENM events on serial and BLE denm = ["c-its-parser/denm"] # evaluate SPAT information -spat = ["gnss", "c-its-parser/spatem", "c-its-parser/mapem"] +spat = ["gnss", "screen", "c-its-parser/spatem", "c-its-parser/mapem"] # enable some debug printing (without enabling debug logging of the ESP RTOS) spat_debug = [] +# enable ST7789 based display +screen = ["dep:mipidsi", "dep:embedded-hal-bus", "dep:profont"] [dependencies] @@ -81,6 +83,8 @@ esp-radio = { version = "0.18.0", optional = true, features = [ ] } embassy_gps = { version = "0.1.0", optional = true, default-features = false, features = ["esp", "log-04"], git = "https://github.com/jbeyerstedt/embassy_gps.git", branch = "feat/rework-gpsfix" } +embedded-hal-bus = { version = "0.3.0", optional = true } +embedded-graphics = "0.8.2" c-its-parser = { version = "2.2.2", default-features = false, features = [ "time", "geo", @@ -90,7 +94,9 @@ c-its-parser = { version = "2.2.2", default-features = false, features = [ chrono = { version = "0.4.44", default-features = false, features = ["alloc"] } crossbeam-queue = { version = "0.3.12", default-features = false, features = ["alloc"] } geo-types = { version = "0.7.19", default-features = false } +mipidsi = { version = "0.10.0", optional = true } num-traits = { version = "0.2.19", default-features = false, features = ["libm"] } +profont = { version = "0.7.0", optional = true } [dev-dependencies] assert_float_eq = "1.2.0" diff --git a/Readme.md b/Readme.md index 7b386cc..f80ac99 100644 --- a/Readme.md +++ b/Readme.md @@ -7,7 +7,8 @@ The code is currently tailored to: - Seeed XIAO ESP32-C5 (8MB PSRAM) - Seeed XIAO L67K (connected on GPIO12 (RX), GPIO11 (TX), GPIO1 (wakeup) and GPIO25 (reset)) only available with `gnss` feature (which is included in `spat` feature) -- TODO: display (when using `spat` feature) +- Some ST7789 172*320px screen (connected on GPIO8 (SCL/ SCK), GPIO10 (SDA/ MOSI), GPIO7 (reset), GPIO23 (DC), GPIO24 (CS), TODO GPIO0 (BL)) +only available with `screen` feature (which is included in `spat` feature) ## Features diff --git a/src/applogic/mod.rs b/src/applogic/mod.rs index 158812c..8065e88 100644 --- a/src/applogic/mod.rs +++ b/src/applogic/mod.rs @@ -5,15 +5,13 @@ use alloc::rc::Rc; #[cfg(feature = "spat")] use alloc::vec::Vec; -#[cfg(all(target_arch = "riscv32", feature = "spat"))] -use esp_println::println; #[cfg(feature = "spat")] use log::{debug, error}; #[cfg(feature = "gnss")] use log::{info, warn}; #[cfg(feature = "spat")] -use crate::geo_alg; +use crate::{applogic::v2x::map, geo_alg}; pub mod v2x; @@ -63,6 +61,54 @@ pub struct State { glosa_state: GlosaState, } +#[cfg(feature = "spat")] +#[derive(Debug, Default)] +pub struct SignalGroupData { + pub phase: SignalPhase, + pub maneuver: map::Maneuvers, + pub min_end_sec: i16, +} + +#[cfg(feature = "spat")] +#[derive(Debug, Default)] +pub enum SignalPhase { + #[default] + Unknown, + Red, + RedYellow, + Green, + Yellow, +} + +impl core::fmt::Display for SignalPhase { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + match self { + SignalPhase::Unknown => write!(f, "__"), + SignalPhase::Red => write!(f, "RE"), + SignalPhase::RedYellow => write!(f, "RY"), + SignalPhase::Green => write!(f, "GN"), + SignalPhase::Yellow => write!(f, "YE"), + } + } +} + +impl From for SignalPhase { + fn from(value: c_its_parser::standards::dsrc_2_2_1::etsi_its_dsrc::MovementPhaseState) -> Self { + use c_its_parser::standards::dsrc_2_2_1::etsi_its_dsrc::MovementPhaseState; + + match value { + MovementPhaseState::stop_And_Remain => SignalPhase::Red, + MovementPhaseState::pre_Movement => SignalPhase::RedYellow, + MovementPhaseState::permissive_Movement_Allowed + | MovementPhaseState::protected_Movement_Allowed => SignalPhase::Green, + MovementPhaseState::permissive_clearance | MovementPhaseState::protected_clearance => { + SignalPhase::Yellow + } + _ => SignalPhase::Unknown, + } + } +} + #[cfg(feature = "spat")] #[derive(Debug, Default)] enum GlosaState { @@ -172,12 +218,12 @@ impl State { } #[cfg(feature = "spat")] - pub fn run_glosa(&mut self) { + pub fn run_glosa(&mut self) -> Vec { let Some(own_heading) = self.heading_deg else { - return; + return alloc::vec![]; }; - let state_update = match &self.glosa_state { + let (state_update, sig_grps) = match &self.glosa_state { // search for upcoming intersection GlosaState::Idle => { debug!( @@ -193,9 +239,9 @@ impl State { search_result.signal_groups ); - Some(GlosaState::Locked(search_result)) + (Some(GlosaState::Locked(search_result)), None) } else { - None + (None, None) } } GlosaState::Locked(info) => { @@ -213,7 +259,7 @@ impl State { &self.position, ) { info!("GLOSA: Intersection was left -> back to idle"); - Some(GlosaState::Idle) + (Some(GlosaState::Idle), None) } else { // get signal groups which are in our approach let mut approach_sig_grps = intersection @@ -231,22 +277,36 @@ impl State { approach_sig_grps.sort_by_key(|v| v.maneuvers()); // display signal phases - for sig in approach_sig_grps { - if let Some(phase) = sig.phase_to_str() - && let Some(manv) = sig.maneuvers() - { - use core::ops::Sub; + let sig_grps = approach_sig_grps + .iter() + .filter_map(|sig| { + if let Some(manv) = sig.maneuvers() { + use core::ops::Sub; - let min_end = sig.min_end_time(); - let dur = min_end.sub(self.time.and_utc()).as_seconds_f32(); - println!( - "GLOSA: {manv} {phase} for {dur:.1} s (until {})", - sig.min_end_time() - ); - } - } + use num_traits::Float; - None // keep state + let min_end = sig.min_end_time(); + let dur = min_end.sub(self.time.and_utc()).as_seconds_f32(); + + let phase = sig + .phase() + .map(core::convert::Into::into) + .unwrap_or_default(); + #[allow(clippy::cast_possible_truncation)] + let min_end_sec = dur.round() as i16; + + Some(SignalGroupData { + phase, + maneuver: manv, + min_end_sec, + }) + } else { + None + } + }) + .collect::>(); + + (None, Some(sig_grps)) // keep state, return signal groups } } }; @@ -255,6 +315,8 @@ impl State { if let Some(new) = state_update { self.glosa_state = new; } + + sig_grps.unwrap_or_default() } #[cfg(feature = "spat")] diff --git a/src/applogic/v2x/map.rs b/src/applogic/v2x/map.rs index b49e818..0df7f0f 100644 --- a/src/applogic/v2x/map.rs +++ b/src/applogic/v2x/map.rs @@ -472,9 +472,9 @@ pub enum UsageType { #[derive(Debug, Default, Copy, Clone, PartialEq, Eq)] pub struct Maneuvers { - left_allowed: bool, - staight_allowed: bool, - right_allowed: bool, + pub left_allowed: bool, + pub staight_allowed: bool, + pub right_allowed: bool, } impl core::fmt::Display for Maneuvers { diff --git a/src/main.rs b/src/main.rs index f00817f..6e2cf24 100644 --- a/src/main.rs +++ b/src/main.rs @@ -18,6 +18,10 @@ use embassy_gps::gps::l76k; use esp_backtrace as _; use esp_hal::clock::CpuClock; use esp_hal::timer::timg::TimerGroup; +#[cfg(feature = "screen")] +use esp_hal::{delay, gpio, spi, time}; +#[cfg(all(target_arch = "riscv32", feature = "spat"))] +use esp_println::println; use esp_radio::wifi; use log::{debug, error, info, warn}; @@ -28,6 +32,8 @@ mod applogic; mod cache; mod geo_alg; mod radio; +#[cfg(feature = "screen")] +mod screen; const WIFI_CHANNEL: radio::Channel = 180; @@ -106,8 +112,53 @@ async fn main(spawner: Spawner) -> ! { ) .expect("Failed to spawn GNSS task"), ); + #[cfg(feature = "gnss")] info!("GNSS task started, waiting for GNSS fix..."); + // ST7789 172*320px screen + // SCL: D8/ GPIO8 (SPI_SCK) + // SDA: D10/ GPIO10 (SPI_MOSI) + // RES: D3/ GPIO7 + // DC: D4/ GPIO23 + // CS: D5/ GPIO24 + // BL: TODO (hard-wire to high? or D1/ GPIO0) + #[cfg(feature = "screen")] + let mut spi_buffer = [0_u8; 512]; + #[cfg(feature = "screen")] + let mut display = { + let (spi, cs) = setup_st7789_spi( + peripherals.GPIO8, + peripherals.GPIO10, + peripherals.GPIO24, + peripherals.GPIO9, + peripherals.SPI2, + ) + .expect("Failed to build SPI config"); + let output_config = gpio::OutputConfig::default(); + let dc_output = gpio::Output::new(peripherals.GPIO23, gpio::Level::Low, output_config); + let rst_output = gpio::Output::new(peripherals.GPIO7, gpio::Level::High, output_config); + let _ = gpio::Output::new(peripherals.GPIO0, gpio::Level::High, output_config); // backlight + + let mut display_delay = delay::Delay::new(); + + let spi_device = embedded_hal_bus::spi::ExclusiveDevice::new_no_delay(spi, cs) + .expect("Failed to initialize SPI device"); + + mipidsi::Builder::new( + mipidsi::models::ST7789, + mipidsi::interface::SpiInterface::new(spi_device, dc_output, &mut spi_buffer), + ) + .display_size(screen::HEIGHT + 34, screen::WIDTH) // somehow this display has 34 px of invisible space on the left (in native orientation) + .reset_pin(rst_output) + .invert_colors(mipidsi::options::ColorInversion::Inverted) + .orientation(mipidsi::options::Orientation::new().rotate(mipidsi::options::Rotation::Deg90)) + .init(&mut display_delay) + .expect("Failed to initialize display") + }; + + #[cfg(feature = "screen")] + let _ = screen::draw_slash_screen(&mut display).inspect_err(log_screen_error); + let mut state = applogic::State::new(); loop { embassy_time::Timer::after(embassy_time::Duration::from_millis(5)).await; @@ -124,7 +175,18 @@ async fn main(spawner: Spawner) -> ! { // run GLOSA algorithm #[cfg(feature = "spat")] - state.run_glosa(); + let signal_groups = state.run_glosa(); + + for sig in &signal_groups { + println!( + "GLOSA: {} {} for {} s", + sig.maneuver, sig.phase, sig.min_end_sec + ); + } + + #[cfg(feature = "screen")] + let _ = screen::draw_glosa(&mut display, &signal_groups) + .inspect_err(log_screen_error); } } }); @@ -284,3 +346,41 @@ async fn gnss_task( } } } + +/// Builds the SPI interface for the ST7789 display +/// +/// Returns (spi master, chip-select) tuple or SPI config error +#[cfg(feature = "screen")] +fn setup_st7789_spi( + scl: esp_hal::peripherals::GPIO8<'static>, + sda: esp_hal::peripherals::GPIO10<'static>, + cs: esp_hal::peripherals::GPIO24<'static>, + miso: esp_hal::peripherals::GPIO9<'static>, + spi: esp_hal::peripherals::SPI2<'static>, +) -> Result< + ( + spi::master::Spi<'static, esp_hal::Blocking>, + gpio::Output<'static>, + ), + spi::master::ConfigError, +> { + let spi_config = spi::master::Config::default() + .with_frequency(time::Rate::from_mhz(10)) + .with_mode(spi::Mode::_0); + let spi: spi::master::Spi<'_, esp_hal::Blocking> = spi::master::Spi::new(spi, spi_config)? + .with_sck(scl) + .with_mosi(sda) + .with_miso(miso); + + let cs_output = gpio::Output::new(cs, gpio::Level::High, gpio::OutputConfig::default()); + + Ok((spi, cs_output)) +} + +#[cfg(feature = "screen")] +fn log_screen_error(error: &E) +where + E: core::fmt::Debug, +{ + error!("TODO: {error:?}"); +} diff --git a/src/screen.rs b/src/screen.rs new file mode 100644 index 0000000..7b84908 --- /dev/null +++ b/src/screen.rs @@ -0,0 +1,186 @@ +//! Screen Output + +use alloc::vec; + +use embedded_graphics::prelude::*; +use embedded_graphics::{geometry, mono_font, pixelcolor, primitives, text}; + +use crate::applogic; + +pub const WIDTH: u16 = 320; +pub const HEIGHT: u16 = 172; + +#[allow(unused)] +pub fn clear(target: &mut D) -> Result<(), D::Error> +where + D: DrawTarget, +{ + let style = primitives::PrimitiveStyleBuilder::new() + .fill_color(pixelcolor::Rgb565::BLACK) + .build(); + primitives::Rectangle::new( + geometry::Point::new(0, 0), + geometry::Size::new(WIDTH.into(), HEIGHT.into()), + ) + .into_styled(style) + .draw(target)?; + + Ok(()) +} + +#[allow(unused)] +pub fn test_img(target: &mut D) -> Result<(), D::Error> +where + D: DrawTarget, + C: RgbColor, +{ + mipidsi::TestImage::new().draw(target)?; + + Ok(()) +} + +#[allow(unused)] +pub fn draw_glosa(target: &mut D, data: &[applogic::SignalGroupData]) -> Result<(), D::Error> +where + D: DrawTarget, +{ + let num_signals = data.len(); + + if num_signals == 0 { + return clear(target); + } + + #[allow(clippy::cast_possible_truncation, clippy::cast_possible_wrap)] + let sig_width = (WIDTH as i32) / (num_signals as i32); + + let text_style = + mono_font::MonoTextStyle::new(&profont::PROFONT_24_POINT, pixelcolor::Rgb565::WHITE); + let blank_style = primitives::PrimitiveStyleBuilder::new() + .fill_color(pixelcolor::Rgb565::CSS_GRAY) + .build(); + let rd_style = primitives::PrimitiveStyleBuilder::new() + .fill_color(pixelcolor::Rgb565::CSS_RED) + .build(); + let ye_style = primitives::PrimitiveStyleBuilder::new() + .fill_color(pixelcolor::Rgb565::CSS_YELLOW) // TODO: select nicer color + .build(); + let gn_style = primitives::PrimitiveStyleBuilder::new() + .fill_color(pixelcolor::Rgb565::CSS_GREEN) + .build(); + + // clear maneuver and timing area + let style = primitives::PrimitiveStyleBuilder::new() + .fill_color(pixelcolor::Rgb565::BLACK) + .build(); + primitives::Rectangle::new( + geometry::Point::new(0, 105), + geometry::Size::new(WIDTH.into(), 67), + ) + .into_styled(style) + .draw(target)?; + + for (idx, sig) in data.iter().enumerate() { + #[allow(clippy::cast_possible_truncation, clippy::cast_possible_wrap)] + let offset = (idx as i32) * sig_width; + + let circle_dia = 50; + let circle_margin = (sig_width - circle_dia) / 2; + let centerline = offset + (sig_width / 2); + + // draw phase color + let style = match sig.phase { + applogic::SignalPhase::Unknown => blank_style, + applogic::SignalPhase::Red => rd_style, + applogic::SignalPhase::Green => gn_style, + applogic::SignalPhase::RedYellow | applogic::SignalPhase::Yellow => ye_style, + }; + primitives::Circle::new( + geometry::Point::new(offset + circle_margin, 10), + circle_dia as u32, + ) + .into_styled(style) + .draw(target)?; + + // TODO: draw lane type? + + // draw maneuver + text::Text::with_alignment( + &alloc::format!("{}", maneuver_to_str(&sig.maneuver)), + geometry::Point::new(centerline, 130), + text_style, + text::Alignment::Center, + ) + .draw(target)?; + + // draw timing indicator + text::Text::with_alignment( + &alloc::format!("{}s", sig.min_end_sec), + geometry::Point::new(centerline, 160), + text_style, + text::Alignment::Center, + ) + .draw(target)?; + } + + Ok(()) +} + +#[allow(unused)] +pub fn draw_slash_screen(target: &mut D) -> Result<(), D::Error> +where + D: DrawTarget, +{ + let text_style = + mono_font::MonoTextStyle::new(&profont::PROFONT_18_POINT, pixelcolor::Rgb565::WHITE); + let blank_style = primitives::PrimitiveStyleBuilder::new() + .fill_color(pixelcolor::Rgb565::CSS_GRAY) + .build(); + let rd_style = primitives::PrimitiveStyleBuilder::new() + .fill_color(pixelcolor::Rgb565::CSS_RED) + .build(); + let ye_style = primitives::PrimitiveStyleBuilder::new() + .fill_color(pixelcolor::Rgb565::CSS_YELLOW) // TODO: select nicer color + .build(); + let gn_style = primitives::PrimitiveStyleBuilder::new() + .fill_color(pixelcolor::Rgb565::CSS_GREEN) + .build(); + + clear(target)?; + + let sig_width = (WIDTH as i32) / 3; + let circle_dia = 50; + let circle_margin = (sig_width - circle_dia) / 2; + + for (idx, style) in vec![rd_style, ye_style, gn_style].iter().enumerate() { + #[allow(clippy::cast_possible_truncation, clippy::cast_possible_wrap)] + let offset = (idx as i32) * sig_width; + + primitives::Circle::new( + geometry::Point::new(offset + circle_margin, 10), + circle_dia as u32, + ) + .into_styled(*style) + .draw(target)?; + } + + // draw welcome text + let centerline = (WIDTH as i32) / 2; + text::Text::with_alignment( + "C-ITS Signal Phase Display\nWaiting for GNSS fix...", + geometry::Point::new(centerline, 130), + text_style, + text::Alignment::Center, + ) + .draw(target)?; + + Ok(()) +} + +fn maneuver_to_str(value: &applogic::v2x::map::Maneuvers) -> alloc::string::String { + alloc::format!( + "{}{}{}", + if value.left_allowed { "<" } else { " " }, + if value.staight_allowed { "^" } else { " " }, + if value.right_allowed { ">" } else { " " }, + ) +}