spat: Add screen

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<c_its_parser::standards::dsrc_2_2_1::etsi_its_dsrc::MovementPhaseState> 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<SignalGroupData> {
         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::<Vec<_>>();
+
+                    (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")]

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 {

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<E>(error: &E)
+where
+    E: core::fmt::Debug,
+{
+    error!("TODO: {error:?}");
+}

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<D>(target: &mut D) -> Result<(), D::Error>
+where
+    D: DrawTarget<Color = pixelcolor::Rgb565>,
+{
+    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<C, D>(target: &mut D) -> Result<(), D::Error>
+where
+    D: DrawTarget<Color = C>,
+    C: RgbColor,
+{
+    mipidsi::TestImage::new().draw(target)?;
+
+    Ok(())
+}
+
+#[allow(unused)]
+pub fn draw_glosa<D>(target: &mut D, data: &[applogic::SignalGroupData]) -> Result<(), D::Error>
+where
+    D: DrawTarget<Color = pixelcolor::Rgb565>,
+{
+    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<D>(target: &mut D) -> Result<(), D::Error>
+where
+    D: DrawTarget<Color = pixelcolor::Rgb565>,
+{
+    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 { " " },
+    )
+}