diff --git a/src/applogic/mod.rs b/src/applogic/mod.rs index b9e6dd1..ed8b546 100644 --- a/src/applogic/mod.rs +++ b/src/applogic/mod.rs @@ -65,7 +65,46 @@ pub struct State { } #[cfg(feature = "spat")] -#[derive(Debug, Default)] +#[derive(Debug)] +pub enum GlosaOutput { + Error(alloc::string::String), + Searching(GlosaSearching), + Found(GlosaFound), + Locked(GlosaSignalInfo), +} + +#[cfg(feature = "spat")] +#[derive(Debug, PartialEq, Eq)] +pub struct GlosaSearching { + pub known_intersection_ids: Vec, +} + +#[cfg(feature = "spat")] +#[derive(Debug, PartialEq, Eq)] +pub struct GlosaFound { + pub intersection_id: u16, + pub approach_id: Option, +} + +#[cfg(feature = "spat")] +impl From<&TlData> for GlosaFound { + fn from(value: &TlData) -> Self { + Self { + intersection_id: value.intersection_id, + approach_id: value.approach_id, + } + } +} + +#[cfg(feature = "spat")] +#[derive(Debug, Clone, PartialEq, Eq)] +pub struct GlosaSignalInfo { + pub intersection_id: u16, + pub signal_groups: Vec, +} + +#[cfg(feature = "spat")] +#[derive(Debug, Default, Clone, PartialEq, Eq)] pub struct SignalGroupData { pub phase: SignalPhase, pub maneuver: map::Maneuvers, @@ -73,7 +112,7 @@ pub struct SignalGroupData { } #[cfg(feature = "spat")] -#[derive(Debug, Default)] +#[derive(Debug, Default, Clone, PartialEq, Eq)] pub enum SignalPhase { #[default] Unknown, @@ -212,17 +251,20 @@ impl State { } #[cfg(feature = "spat")] - pub fn run_glosa(&mut self) -> Vec { + pub fn run_glosa(&mut self) -> GlosaOutput { let Some(own_heading) = self.heading_deg else { - return alloc::vec![]; + use alloc::string::ToString; + + return GlosaOutput::Error("Heading unknown".to_string()); }; - let (state_update, sig_grps) = match &self.glosa_state { + let known_intersection_ids = self.tlc_cache.known_intersections(); + let (state_update, output_data) = match &self.glosa_state { // search for upcoming intersection GlosaState::Idle => { debug!( "GLOSA: Searching with {} known intersections", - self.tlc_cache.known_intersections().len() + known_intersection_ids.len() ); if let Some(search_result) = @@ -233,9 +275,18 @@ impl State { search_result.signal_groups ); - (Some(GlosaState::Locked(search_result)), None) + let glosa_found_data = (&search_result).into(); + ( + Some(GlosaState::Locked(search_result)), + GlosaOutput::Found(glosa_found_data), + ) } else { - (None, None) + ( + None, + GlosaOutput::Searching(GlosaSearching { + known_intersection_ids, + }), + ) } } GlosaState::Locked(info) => { @@ -251,7 +302,12 @@ impl State { .is_err() { info!("GLOSA: Intersection not known any more"); - (Some(GlosaState::Idle), None) + ( + Some(GlosaState::Idle), + GlosaOutput::Searching(GlosaSearching { + known_intersection_ids, + }), + ) } else { // determine if intersection is left let intersection = info.intersection.borrow(); @@ -262,7 +318,12 @@ impl State { &self.position, ) { info!("GLOSA: Intersection was left -> back to idle"); - (Some(GlosaState::Idle), None) + ( + Some(GlosaState::Idle), + GlosaOutput::Searching(GlosaSearching { + known_intersection_ids, + }), + ) } else { // get signal groups which are in our approach let mut approach_sig_grps = intersection @@ -317,7 +378,13 @@ impl State { }) .collect::>(); - (None, Some(sig_grps)) // keep state, return signal groups + ( + None, + GlosaOutput::Locked(GlosaSignalInfo { + intersection_id: intersection.id(), + signal_groups: sig_grps, + }), + ) // keep state, return signal groups } } } @@ -328,7 +395,7 @@ impl State { self.glosa_state = new; } - sig_grps.unwrap_or_default() + output_data } #[cfg(feature = "spat")] diff --git a/src/main.rs b/src/main.rs index 76afcb4..db182b4 100644 --- a/src/main.rs +++ b/src/main.rs @@ -36,6 +36,10 @@ mod radio; mod screen; const WIFI_CHANNEL: radio::Channel = 180; +#[cfg(feature = "screen")] +pub const DISPLAY_WIDTH: u16 = 320; +#[cfg(feature = "screen")] +pub const DISPLAY_HEIGHT: u16 = 172; // This creates a default app-descriptor required by the esp-idf bootloader. // For more information see: @@ -84,13 +88,13 @@ async fn main(spawner: Spawner) -> ! { // 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) + // DC: D4/ GPIO23 (display clear) + // CS: D5/ GPIO24 (chip select) + // BL: D1/ GPIO0 (backlight) #[cfg(feature = "screen")] let mut spi_buffer = [0_u8; 512]; #[cfg(feature = "screen")] - let mut display = { + let mut screen = { let (spi, cs) = setup_st7789_spi( peripherals.GPIO8, peripherals.GPIO10, @@ -109,20 +113,24 @@ async fn main(spawner: Spawner) -> ! { let spi_device = embedded_hal_bus::spi::ExclusiveDevice::new_no_delay(spi, cs) .expect("Failed to initialize SPI device"); - mipidsi::Builder::new( + let display = 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) + .display_size(DISPLAY_HEIGHT + 34, DISPLAY_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") + .expect("Failed to initialize display"); + + screen::Handler::new(display, DISPLAY_WIDTH, DISPLAY_HEIGHT) }; #[cfg(feature = "screen")] - let _ = screen::draw_slash_screen(&mut display).inspect_err(log_screen_error); + let _ = screen + .update(screen::ScreenState::Initial) + .inspect_err(log_screen_error); // Setup WiFi let (_wifi_controller, interfaces) = esp_radio::wifi::new( @@ -171,13 +179,7 @@ async fn main(spawner: Spawner) -> ! { let gnss_update_ref = GNSS_UPDATE.borrow(cs); if let Some(fix) = gnss_update_ref.replace(None) { - let prev_state = state.initialized(); state.update_with_gpsfix(&fix); - - if prev_state != state.initialized() { - let _ = screen::clear(&mut display).inspect_err(log_screen_error); - } - new_position = true; } }); @@ -188,18 +190,21 @@ async fn main(spawner: Spawner) -> ! { // run GLOSA algorithm #[cfg(feature = "spat")] { - let signal_groups = state.run_glosa(); + let glosa_data = state.run_glosa(); - for sig in &signal_groups { - let duration_str = sig - .end_sec - .map(|v| alloc::format!("for {v} s")) - .unwrap_or_default(); - println!("GLOSA: {} {} {duration_str}", sig.maneuver, sig.phase); + if let applogic::GlosaOutput::Locked(data) = &glosa_data { + for sig in &data.signal_groups { + let duration_str = sig + .end_sec + .map(|v| alloc::format!("for {v} s")) + .unwrap_or_default(); + println!("GLOSA: {} {} {duration_str}", sig.maneuver, sig.phase); + } } #[cfg(feature = "screen")] - let _ = screen::draw_glosa(&mut display, &signal_groups) + let _ = screen + .update(glosa_data.into()) .inspect_err(log_screen_error); } } diff --git a/src/screen.rs b/src/screen.rs index 28c1eb8..d72d0f3 100644 --- a/src/screen.rs +++ b/src/screen.rs @@ -5,183 +5,339 @@ 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> +pub struct Handler 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)?; + target: D, + width: u16, + height: u16, - Ok(()) + // internal state + prev_state: ScreenState, + hb_state: bool, } -#[allow(unused)] -pub fn test_img(target: &mut D) -> Result<(), D::Error> -where - D: DrawTarget, - C: RgbColor, -{ - mipidsi::TestImage::new().draw(target)?; - - Ok(()) +#[derive(Debug, Default, PartialEq, Eq)] +pub enum ScreenState { + #[default] + Uninitialized, + /// aka. "no GNSS fix" or "splash screen" + Initial, + Error(alloc::string::String), + #[cfg(feature = "spat")] + GlosaSearching(applogic::GlosaSearching), + #[cfg(feature = "spat")] + GlosaFound(applogic::GlosaFound), + #[cfg(feature = "spat")] + GlosaLocked(applogic::GlosaSignalInfo), } -#[allow(unused)] -pub fn draw_glosa(target: &mut D, data: &[applogic::SignalGroupData]) -> Result<(), D::Error> +#[cfg(feature = "spat")] +impl From for ScreenState { + fn from(value: applogic::GlosaOutput) -> Self { + match value { + applogic::GlosaOutput::Searching(glosa_searching) => { + Self::GlosaSearching(glosa_searching) + } + applogic::GlosaOutput::Found(glosa_found) => Self::GlosaFound(glosa_found), + applogic::GlosaOutput::Locked(glosa_signal_info) => { + Self::GlosaLocked(glosa_signal_info) + } + applogic::GlosaOutput::Error(err_str) => Self::Error(err_str), + } + } +} + +impl Handler where D: DrawTarget, { - let num_signals = data.len(); + const COLOR_BG: pixelcolor::Rgb565 = pixelcolor::Rgb565::BLACK; + const COLOR_FG: pixelcolor::Rgb565 = pixelcolor::Rgb565::WHITE; - 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(); + const BK_STYLE: primitives::PrimitiveStyle = + primitives::PrimitiveStyleBuilder::new() + .fill_color(Self::COLOR_BG) + .build(); + const RD_STYLE: primitives::PrimitiveStyle = + primitives::PrimitiveStyleBuilder::new() + .fill_color(pixelcolor::Rgb565::CSS_RED) + .build(); + const YE_STYLE: primitives::PrimitiveStyle = + primitives::PrimitiveStyleBuilder::new() + .fill_color(pixelcolor::Rgb565::new(31, 55, 0)) + .build(); + const GN_STYLE: primitives::PrimitiveStyle = + primitives::PrimitiveStyleBuilder::new() + .fill_color(pixelcolor::Rgb565::new(10, 50, 0)) + .build(); + const BLANK_SIGNAL_STYLE: primitives::PrimitiveStyle = + primitives::PrimitiveStyleBuilder::new() + .fill_color(pixelcolor::Rgb565::CSS_GRAY) + .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)?; + const DEFAULT_TEXT_STYLE: mono_font::MonoTextStyle<'static, pixelcolor::Rgb565> = + mono_font::MonoTextStyle::new(&profont::PROFONT_24_POINT, Self::COLOR_FG); + const SMALL_TEXT_STYLE: mono_font::MonoTextStyle<'static, pixelcolor::Rgb565> = + mono_font::MonoTextStyle::new(&profont::PROFONT_14_POINT, Self::COLOR_FG); - if num_signals == 0 { - text::Text::with_alignment( - "No upcoming SGs", - geometry::Point::new(i32::from(WIDTH) / 2, 130), - text_style, - text::Alignment::Center, - ) - .draw(target)?; - - return Ok(()); + pub fn new(target: D, width: u16, height: u16) -> Self { + Self { + target, + width, + height, + prev_state: ScreenState::Uninitialized, + hb_state: false, + } } - #[allow(clippy::cast_possible_truncation, clippy::cast_possible_wrap)] - let sig_width = i32::from(WIDTH) / (num_signals as i32); + pub fn update(&mut self, state: ScreenState) -> Result<(), D::Error> { + if self.prev_state != state { + match &state { + ScreenState::Uninitialized | ScreenState::Initial => self.draw_slash_screen()?, + ScreenState::GlosaSearching(_glosa_searching) => self.draw_glosa_searching()?, + ScreenState::GlosaFound(glosa_found) => self.draw_glosa_found(glosa_found)?, + ScreenState::GlosaLocked(glosa_data) => { + let prev_glosa_data = + if let ScreenState::GlosaLocked(prev_glosa_data) = &self.prev_state { + Some(prev_glosa_data.clone()) + } else { + None + }; + + self.draw_glosa(glosa_data, prev_glosa_data)?; + } + ScreenState::Error(msg) => self.draw_message(msg)?, + } + } + + // Update indicator/ heartbeat + { + let update_ind_dia = 10; + let circle_x_pos = i32::from(self.width) - update_ind_dia - 10; + + text::Text::with_alignment( + "HB", + geometry::Point::new(circle_x_pos - 5, 10), + Self::SMALL_TEXT_STYLE, + text::Alignment::Right, + ) + .draw(&mut self.target)?; + + primitives::Circle::new( + geometry::Point::new(circle_x_pos, 2), + update_ind_dia.cast_unsigned(), + ) + .into_styled(if self.hb_state { + Self::GN_STYLE + } else { + Self::BK_STYLE + }) + .draw(&mut self.target)?; + } + + self.prev_state = state; + self.hb_state = !self.hb_state; + + Ok(()) + } + + fn clear(&mut self) -> Result<(), D::Error> + where + D: DrawTarget, + { + primitives::Rectangle::new( + geometry::Point::new(0, 0), + geometry::Size::new(self.width.into(), self.height.into()), + ) + .into_styled(Self::BK_STYLE) + .draw(&mut self.target)?; + + Ok(()) + } + + #[allow(unused)] + pub fn test_img(&mut self) -> Result<(), D::Error> { + mipidsi::TestImage::new().draw(&mut self.target)?; + + Ok(()) + } + + #[allow(unused)] + pub fn draw_message(&mut self, msg: &str) -> Result<(), D::Error> { + // clear whole display and draw text + self.clear()?; + + text::Text::with_alignment( + msg, + geometry::Point::new(i32::from(self.width) / 2, 40), + Self::DEFAULT_TEXT_STYLE, + text::Alignment::Center, + ) + .draw(&mut self.target)?; + + Ok(()) + } + + #[allow(unused)] + fn draw_glosa_searching(&mut self) -> Result<(), D::Error> { + // clear whole display and draw text + self.clear()?; + + text::Text::with_alignment( + "No upcoming SPAT", + geometry::Point::new(i32::from(self.width) / 2, 130), + Self::DEFAULT_TEXT_STYLE, + text::Alignment::Center, + ) + .draw(&mut self.target)?; + + Ok(()) + } + + #[allow(unused)] + fn draw_glosa_found(&mut self, data: &applogic::GlosaFound) -> Result<(), D::Error> { + // clear whole display and draw text + self.clear()?; + + let text = if let Some(appr) = data.approach_id { + alloc::format!("Int. {}\nApproach {appr}", data.intersection_id) + } else { + alloc::format!("Int. {}\nno approach ID available", data.intersection_id) + }; + + text::Text::with_alignment( + &text, + geometry::Point::new(i32::from(self.width) / 2, 130), + Self::DEFAULT_TEXT_STYLE, + text::Alignment::Center, + ) + .draw(&mut self.target)?; + + Ok(()) + } + + #[allow(unused)] + fn draw_glosa( + &mut self, + data: &applogic::GlosaSignalInfo, + prev_data: Option, + ) -> Result<(), D::Error> { + let num_signals = data.signal_groups.len(); + + if let Some(prev) = prev_data + && prev.signal_groups.len() != num_signals + { + // clear everything if number of signals has changed + self.clear(); + } else { + // only clear maneuver and timing area otherwise + + primitives::Rectangle::new( + geometry::Point::new(0, 105), + geometry::Size::new(self.width.into(), 67), + ) + .into_styled(Self::BK_STYLE) + .draw(&mut self.target)?; + } + + // draw intersection ID + text::Text::new( + &alloc::format!("#{}", data.intersection_id), + geometry::Point::new(15, 10), + Self::SMALL_TEXT_STYLE, + ) + .draw(&mut self.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 sig_width = i32::from(self.width) / (num_signals as i32); + for (idx, sig) in data.signal_groups.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 => Self::BLANK_SIGNAL_STYLE, + applogic::SignalPhase::Red => Self::RD_STYLE, + applogic::SignalPhase::Green => Self::GN_STYLE, + applogic::SignalPhase::RedYellow | applogic::SignalPhase::Yellow => Self::YE_STYLE, + }; + primitives::Circle::new( + geometry::Point::new(offset + circle_margin, 15), + circle_dia.cast_unsigned(), + ) + .into_styled(style) + .draw(&mut self.target)?; + + // TODO: draw lane type? + + // draw maneuver + text::Text::with_alignment( + &maneuver_to_str(sig.maneuver), + geometry::Point::new(centerline, 130), + Self::DEFAULT_TEXT_STYLE, + text::Alignment::Center, + ) + .draw(&mut self.target)?; + + // draw timing indicator + if let Some(end_sec) = sig.end_sec { + text::Text::with_alignment( + &alloc::format!("{end_sec}s"), + geometry::Point::new(centerline, 160), + Self::DEFAULT_TEXT_STYLE, + text::Alignment::Center, + ) + .draw(&mut self.target)?; + } + } + + Ok(()) + } + + #[allow(unused)] + fn draw_slash_screen(&mut self) -> Result<(), D::Error> { + let text_style = mono_font::MonoTextStyle::new(&profont::PROFONT_18_POINT, Self::COLOR_FG); + + self.clear()?; + + let sig_width = i32::from(self.width) / 3; 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.cast_unsigned(), - ) - .into_styled(style) - .draw(target)?; + for (idx, style) in [Self::RD_STYLE, Self::YE_STYLE, Self::GN_STYLE] + .iter() + .enumerate() + { + #[allow(clippy::cast_possible_truncation, clippy::cast_possible_wrap)] + let offset = (idx as i32) * sig_width; - // TODO: draw lane type? + primitives::Circle::new( + geometry::Point::new(offset + circle_margin, 15), + circle_dia.cast_unsigned(), + ) + .into_styled(*style) + .draw(&mut self.target)?; + } - // draw maneuver + // draw welcome text + let centerline = i32::from(self.width) / 2; text::Text::with_alignment( - &maneuver_to_str(sig.maneuver), + "C-ITS Signal Phase Display\nWaiting for GNSS fix...", geometry::Point::new(centerline, 130), text_style, text::Alignment::Center, ) - .draw(target)?; + .draw(&mut self.target)?; - // draw timing indicator - if let Some(end_sec) = sig.end_sec { - text::Text::with_alignment( - &alloc::format!("{end_sec}s"), - geometry::Point::new(centerline, 160), - text_style, - text::Alignment::Center, - ) - .draw(target)?; - } + Ok(()) } - - 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 = i32::from(WIDTH) / 3; - let circle_dia = 50; - let circle_margin = (sig_width - circle_dia) / 2; - - for (idx, style) in [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.cast_unsigned(), - ) - .into_styled(*style) - .draw(target)?; - } - - // draw welcome text - let centerline = i32::from(WIDTH) / 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 {