jtbx/esp32-c_its-companion
0
0
Fork 0
Code Activity

screen, spat: Add more information and update indicator

Browse source
This commit is contained in:
Jannik Beyerstedt 2026-06-22 22:59:47 +02:00
commit fc0c9b61da
3 changed files with 403 additions and 175 deletions
Download patch file Download diff file Expand all files Collapse all files

91
src/applogic/mod.rs
View file

@ -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<u16>,
}
#[cfg(feature = "spat")]
#[derive(Debug, PartialEq, Eq)]
pub struct GlosaFound {
pub intersection_id: u16,
pub approach_id: Option<u8>,
}
#[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<SignalGroupData>,
}
#[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<SignalGroupData> {
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::<Vec<_>>();
(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")]

39
src/main.rs
View file

@ -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: <https://docs.espressif.com/projects/esp-idf/en/stable/esp32/api-reference/system/app_image_format.html#application-description>
@ -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 {
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);
}
}

344
src/screen.rs
View file

@ -5,87 +5,255 @@ 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>
pub struct Handler<D>
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)?;
target: D,
width: u16,
height: u16,
Ok(())
// internal state
prev_state: ScreenState,
hb_state: bool,
}
#[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(())
#[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<D>(target: &mut D, data: &[applogic::SignalGroupData]) -> Result<(), D::Error>
#[cfg(feature = "spat")]
impl From<applogic::GlosaOutput> 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<D> Handler<D>
where
D: DrawTarget<Color = pixelcolor::Rgb565>,
{
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)
const BK_STYLE: primitives::PrimitiveStyle<pixelcolor::Rgb565> =
primitives::PrimitiveStyleBuilder::new()
.fill_color(Self::COLOR_BG)
.build();
let rd_style = primitives::PrimitiveStyleBuilder::new()
const RD_STYLE: primitives::PrimitiveStyle<pixelcolor::Rgb565> =
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
const YE_STYLE: primitives::PrimitiveStyle<pixelcolor::Rgb565> =
primitives::PrimitiveStyleBuilder::new()
.fill_color(pixelcolor::Rgb565::new(31, 55, 0))
.build();
let gn_style = primitives::PrimitiveStyleBuilder::new()
.fill_color(pixelcolor::Rgb565::CSS_GREEN)
const GN_STYLE: primitives::PrimitiveStyle<pixelcolor::Rgb565> =
primitives::PrimitiveStyleBuilder::new()
.fill_color(pixelcolor::Rgb565::new(10, 50, 0))
.build();
const BLANK_SIGNAL_STYLE: primitives::PrimitiveStyle<pixelcolor::Rgb565> =
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
};
for (idx, sig) in data.iter().enumerate() {
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<Color = pixelcolor::Rgb565>,
{
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<applogic::GlosaSignalInfo>,
) -> 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)?;
#[allow(clippy::cast_possible_truncation, clippy::cast_possible_wrap)]
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;
@ -95,17 +263,17 @@ where
// 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,
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, 10),
geometry::Point::new(offset + circle_margin, 15),
circle_dia.cast_unsigned(),
)
.into_styled(style)
.draw(target)?;
.draw(&mut self.target)?;
// TODO: draw lane type?
@ -113,20 +281,20 @@ where
text::Text::with_alignment(
&maneuver_to_str(sig.maneuver),
geometry::Point::new(centerline, 130),
text_style,
Self::DEFAULT_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,
Self::DEFAULT_TEXT_STYLE,
text::Alignment::Center,
)
.draw(target)?;
.draw(&mut self.target)?;
}
}
@ -134,55 +302,43 @@ where
}
#[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();
fn draw_slash_screen(&mut self) -> Result<(), D::Error> {
let text_style = mono_font::MonoTextStyle::new(&profont::PROFONT_18_POINT, Self::COLOR_FG);
clear(target)?;
self.clear()?;
let sig_width = i32::from(WIDTH) / 3;
let sig_width = i32::from(self.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() {
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;
primitives::Circle::new(
geometry::Point::new(offset + circle_margin, 10),
geometry::Point::new(offset + circle_margin, 15),
circle_dia.cast_unsigned(),
)
.into_styled(*style)
.draw(target)?;
.draw(&mut self.target)?;
}
// draw welcome text
let centerline = i32::from(WIDTH) / 2;
let centerline = i32::from(self.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)?;
.draw(&mut self.target)?;
Ok(())
}
}
fn maneuver_to_str(value: applogic::v2x::map::Maneuvers) -> alloc::string::String {
alloc::format!(