From 640fbcd77bf7e9124b5c24edaee3c6efecd89609 Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Mon, 22 Jun 2026 22:57:48 +0200 Subject: [PATCH 01/14] WIP: try to debug V2X rx dropouts --- src/main.rs | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main.rs b/src/main.rs index 28c823e..12fa046 100644 --- a/src/main.rs +++ b/src/main.rs @@ -592,6 +592,8 @@ fn handle_frame(frame: esp_radio::wifi::sniffer::PromiscuousPkt<'_>) { .naive_utc() }; + esp_println::print!("."); // TODO: debug only! + match esp32_cits_core::rx::is_broadcast_frame(&frame) { Err(err) => { warn!("{err}"); From 5735f18ee2442c09cd6f89d71925d5503a3fb5e3 Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Mon, 3 Aug 2026 11:45:28 +0200 Subject: [PATCH 02/14] FIXUP: use user LED --- src/main.rs | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/src/main.rs b/src/main.rs index 12fa046..8882056 100644 --- a/src/main.rs +++ b/src/main.rs @@ -101,6 +101,7 @@ fn panic_handler(info: &core::panic::PanicInfo) -> ! { #[cfg(feature = "_gnss")] static GNSS_UPDATE: Mutex>> = Mutex::new(RefCell::new(None)); static WIFI_RX_QUEUE: Mutex>>>> = Mutex::new(RefCell::new(None)); +static mut USER_LED: Option> = None; #[cfg(feature = "_uart")] static UART_RX_BUF: Mutex>>> = Mutex::new(RefCell::new(None)); #[cfg(feature = "_uart")] @@ -203,6 +204,13 @@ async fn main(spawner: Spawner) -> ! { error!("Failed to set WiFi TX power: {err}"); } + unsafe { + USER_LED = Some(esp_hal::gpio::Output::new( + peripherals.GPIO27, + esp_hal::gpio::Level::Low, + esp_hal::gpio::OutputConfig::default(), + )); + } critical_section::with(|cs| { WIFI_RX_QUEUE.borrow(cs).replace(Some(ArrayQueue::new(2))); }); @@ -592,8 +600,6 @@ fn handle_frame(frame: esp_radio::wifi::sniffer::PromiscuousPkt<'_>) { .naive_utc() }; - esp_println::print!("."); // TODO: debug only! - match esp32_cits_core::rx::is_broadcast_frame(&frame) { Err(err) => { warn!("{err}"); @@ -609,6 +615,13 @@ fn handle_frame(frame: esp_radio::wifi::sniffer::PromiscuousPkt<'_>) { } // info!("Received frame with {} bytes", frame.len); + esp_println::print!("."); // TODO: debug only! + unsafe { + #[allow(static_mut_refs)] + if let Some(ref mut led) = USER_LED.as_mut() { + led.toggle(); + } + } // parse GN headers match c_its_parser::pcap::remove_wlan_headers(frame.data) From 1648a5eb0fa1a59d19f1fda7b6898af636b2ed05 Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Wed, 5 Aug 2026 11:43:04 +0200 Subject: [PATCH 03/14] FIXUP: more detailed printf logging --- src/main.rs | 25 ++++++++++++++++++++----- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/src/main.rs b/src/main.rs index 8882056..eb58ab3 100644 --- a/src/main.rs +++ b/src/main.rs @@ -615,7 +615,6 @@ fn handle_frame(frame: esp_radio::wifi::sniffer::PromiscuousPkt<'_>) { } // info!("Received frame with {} bytes", frame.len); - esp_println::print!("."); // TODO: debug only! unsafe { #[allow(static_mut_refs)] if let Some(ref mut led) = USER_LED.as_mut() { @@ -636,6 +635,7 @@ fn handle_frame(frame: esp_radio::wifi::sniffer::PromiscuousPkt<'_>) { Ok(packet) => { // drop hopped (for now) if packet.decoded.is_hopped() { + esp_println::print!("."); // TODO: debug only! return; } @@ -644,9 +644,15 @@ fn handle_frame(frame: esp_radio::wifi::sniffer::PromiscuousPkt<'_>) { // drop all unsupported message IDs and rate-limit SPATEMs if match message_id { #[cfg(feature = "denm")] - c_its_parser::standards::extensions::ItsMessageId::Denm => false, + c_its_parser::standards::extensions::ItsMessageId::Denm => { + esp_println::print!("D"); // TODO: debug only! + false + } #[cfg(feature = "cam")] - c_its_parser::standards::extensions::ItsMessageId::Cam => false, + c_its_parser::standards::extensions::ItsMessageId::Cam => { + esp_println::print!("C"); // TODO: debug only! + false + } #[cfg(feature = "spat")] c_its_parser::standards::extensions::ItsMessageId::Spatem => { // reduce SPAT rate per station ID @@ -662,7 +668,10 @@ fn handle_frame(frame: esp_radio::wifi::sniffer::PromiscuousPkt<'_>) { i.msg_count = i.msg_count.wrapping_add(1); if (i.msg_count % SPAT_RATE_LIMIT) > 0 { + esp_println::print!("s"); // TODO: debug only! drop_msg = true; + } else { + esp_println::print!("S"); // TODO: debug only! } }); } @@ -670,8 +679,14 @@ fn handle_frame(frame: esp_radio::wifi::sniffer::PromiscuousPkt<'_>) { drop_msg } #[cfg(feature = "spat")] - c_its_parser::standards::extensions::ItsMessageId::Mapem => false, - _ => true, + c_its_parser::standards::extensions::ItsMessageId::Mapem => { + esp_println::print!("M"); // TODO: debug only! + false + } + _ => { + esp_println::print!("?"); // TODO: debug only! + true + } } { return; } From 10653429e578a40f58dd23854f08c3d10e5b3880 Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Sat, 15 Aug 2026 14:18:06 +0200 Subject: [PATCH 04/14] GNSS: Increase update rate to 2 Hz --- src/main.rs | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main.rs b/src/main.rs index 28c823e..02e4855 100644 --- a/src/main.rs +++ b/src/main.rs @@ -825,6 +825,8 @@ async fn gnss_ubx_config(reset_pin: esp_hal::peripherals::GPIO25<'static>) { cfg_data: &[ ublox::cfg_val::CfgVal::Uart1OutProtNmea(false), ublox::cfg_val::CfgVal::Uart1OutProtUbx(true), + // CFG-RATE-MEAS + ublox::cfg_val::CfgVal::RateMeas(500), // CFG-MSGOUT-UBX_NAV_PVT_UART1 ublox::cfg_val::CfgVal::MsgOutUbxNavPvtUart1(1), // CFG-MSGOUT-UBX_NAV_SAT_UART1 From 38434b0d864ade6d784285ee6fdaafd75bfca70c Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Sat, 15 Aug 2026 21:08:50 +0200 Subject: [PATCH 05/14] map: Extract max end time --- src/applogic/v2x/map.rs | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/applogic/v2x/map.rs b/src/applogic/v2x/map.rs index bac8a69..d7e2195 100644 --- a/src/applogic/v2x/map.rs +++ b/src/applogic/v2x/map.rs @@ -173,6 +173,12 @@ impl Intersection { { sig_grp.likely_time = Some(likely.to_datetime_from_moy(moy, year)); } + if let Some(max) = &timing.max_end_time + && !max.is_out_of_range() + && !max.is_unknown() + { + sig_grp.max_end_time = Some(max.to_datetime_from_moy(moy, year)); + } } } } else { @@ -223,6 +229,7 @@ impl Intersection { target_lanes, phase: None, min_end_time: None, + max_end_time: None, likely_time: None, } }) @@ -410,7 +417,7 @@ pub struct SignalGroup { /// current signal phase phase: Option, min_end_time: Option>, - // max_end_time: Option>, + max_end_time: Option>, likely_time: Option>, } @@ -443,6 +450,9 @@ impl SignalGroup { pub fn likely_time(&self) -> Option> { self.likely_time } + pub fn max_end_time(&self) -> Option> { + self.max_end_time + } pub fn phase_to_str(&self) -> Option { self.phase.map(|v| { From 1a5012dec51d5daa7fe0cd2727c79b20b4ec63cf Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Sat, 1 Aug 2026 21:31:04 +0200 Subject: [PATCH 06/14] Adapt to new screen, add more GNSS and timing info --- Readme.md | 2 +- src/applogic/mod.rs | 70 +++++++---- src/main.rs | 20 +++- src/screen.rs | 277 +++++++++++++++++++++++++++++++++++++------- 4 files changed, 300 insertions(+), 69 deletions(-) diff --git a/Readme.md b/Readme.md index c2ecbab..5fb34e6 100644 --- a/Readme.md +++ b/Readme.md @@ -8,7 +8,7 @@ The code is currently tailored to: - U-Blox M10Q GNSS (connected on GPIO12 (RX), GPIO11 (TX), and GPIO25 (reset)) or Seeed XIAO L67K (connected on GPIO12 (RX), GPIO11 (TX), GPIO1 (wakeup) and GPIO25 (reset)) only available with `gnss_ubx` or `gnss_l67k` feature (which is included in `spat` feature) -- Some ST7789 172*320px screen (connected on GPIO8 (SCL/ SCK), GPIO10 (SDA/ MOSI), GPIO7 (reset), GPIO23 (DC), GPIO24 (CS), TODO GPIO0 (BL)) +- Some ST7789 240*280px screen (connected on GPIO8 (SCL/ SCK), GPIO10 (SDA/ MOSI), GPIO7 (reset), GPIO23 (DC), GPIO24 (CS), 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 7e7ebc4..55672a7 100644 --- a/src/applogic/mod.rs +++ b/src/applogic/mod.rs @@ -106,7 +106,51 @@ pub struct GlosaSignalInfo { pub struct SignalGroupData { pub phase: SignalPhase, pub maneuver: map::Maneuvers, - pub end_sec: Option, + pub timing: Option, +} + +#[cfg(feature = "spat")] +#[derive(Debug, Default, Clone, PartialEq, Eq)] +#[allow(clippy::struct_field_names)] +pub struct TimingData { + pub min_end_sec: i16, + pub likely_end_sec: Option, + pub max_end_sec: Option, +} + +impl TimingData { + pub fn new(value: &map::SignalGroup, now: chrono::DateTime) -> Option { + let min = value + .min_end_time() + .map(|val| Self::time_to_duration_sec(now, val)); + let likely_end_sec = value + .likely_time() + .map(|val| Self::time_to_duration_sec(now, val)); + let max_end_sec = value + .max_end_time() + .map(|val| Self::time_to_duration_sec(now, val)); + + min.map(|min_end_sec| Self { + min_end_sec, + likely_end_sec, + max_end_sec, + }) + } + + fn time_to_duration_sec( + now: chrono::DateTime, + time_point: chrono::DateTime, + ) -> i16 { + use core::ops::Sub as _; + + use num_traits::Float as _; + + let dur = time_point.sub(now).as_seconds_f32(); + #[allow(clippy::cast_possible_truncation)] + let duration_sec = dur.round() as i16; + + duration_sec + } } #[cfg(feature = "spat")] @@ -357,33 +401,17 @@ impl State { let sig_grps = approach_sig_grps .iter() .filter_map(|sig| { - if let Some(manv) = sig.maneuvers() { - use core::ops::Sub; - - use num_traits::Float; - + if let Some(maneuver) = sig.maneuvers() { let phase = sig .phase() .map(core::convert::Into::into) .unwrap_or_default(); - - let end_sec = sig - .likely_time() - .or_else(|| sig.min_end_time()) - .map(|time| { - let dur = - time.sub(self.time.and_utc()).as_seconds_f32(); - - #[allow(clippy::cast_possible_truncation)] - let end_sec = dur.round() as i16; - - end_sec - }); + let timing = TimingData::new(sig, self.time.and_utc()); Some(SignalGroupData { phase, - maneuver: manv, - end_sec, + maneuver, + timing, }) } else { None diff --git a/src/main.rs b/src/main.rs index 02e4855..13be5d6 100644 --- a/src/main.rs +++ b/src/main.rs @@ -154,8 +154,8 @@ async fn main(spawner: Spawner) -> ! { let mut spi_buffer = [0_u8; 512]; #[cfg(feature = "screen")] let mut screen = { - // 172*320px in landscape orientation - let (display, width, height) = screen::make_small_display( + // 240*280px in portrait orientation + let (display, size) = screen::make_large_display( peripherals.GPIO8, peripherals.GPIO10, peripherals.GPIO7, @@ -167,7 +167,7 @@ async fn main(spawner: Spawner) -> ! { ) .expect("Fatal error building screen"); - let mut screen = screen::Handler::new(display, width, height); + let mut screen = screen::Handler::new(display, size); let _ = screen .update(screen::ScreenState::Initial) @@ -381,6 +381,15 @@ async fn main(spawner: Spawner) -> ! { } Err(err) => warn!("{err}"), } + + // update GNSS HB on screen + let _ = screen.update_gnss(pvt); + } + ublox::proto27::PacketRef::AckNak(nak) => { + error!("UBX: {nak:?}"); + } + ublox::proto27::PacketRef::AckAck(ack) => { + info!("UBX: {ack:?}"); } _ => { println!("UBX: {packet_ref:?}"); @@ -460,8 +469,9 @@ async fn main(spawner: Spawner) -> ! { 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")) + .timing + .as_ref() + .map(|v| alloc::format!("for {} s", v.min_end_sec)) .unwrap_or_default(); println!("GLOSA: {} {} {duration_str}", sig.maneuver, sig.phase); } diff --git a/src/screen.rs b/src/screen.rs index b2ddf90..791d086 100644 --- a/src/screen.rs +++ b/src/screen.rs @@ -5,17 +5,24 @@ use embedded_graphics::{geometry, mono_font, pixelcolor, primitives, text}; use crate::applogic; +pub struct DisplaySize { + width: u16, + height: u16, + offset_top: i32, + corner_radius: i32, +} + pub struct Handler where D: DrawTarget, { target: D, - width: u16, - height: u16, + size: DisplaySize, // internal state prev_state: ScreenState, hb_state: bool, + gnss_hb_state: bool, } #[derive(Debug, Default, PartialEq, Eq)] @@ -79,16 +86,20 @@ where const DEFAULT_TEXT_STYLE: mono_font::MonoTextStyle<'static, pixelcolor::Rgb565> = mono_font::MonoTextStyle::new(&profont::PROFONT_24_POINT, Self::COLOR_FG); + const MID_TEXT_STYLE: mono_font::MonoTextStyle<'static, pixelcolor::Rgb565> = + mono_font::MonoTextStyle::new(&profont::PROFONT_18_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); - pub fn new(target: D, width: u16, height: u16) -> Self { + const BIG_HEIGHT_THLD: u16 = 172; + + pub fn new(target: D, display_size: DisplaySize) -> Self { Self { target, - width, - height, + size: display_size, prev_state: ScreenState::Uninitialized, hb_state: false, + gnss_hb_state: false, } } @@ -115,18 +126,19 @@ where // Update indicator/ heartbeat { let update_ind_dia = 10; - let circle_x_pos = i32::from(self.width) - update_ind_dia - 10; + let circle_x_pos = + i32::from(self.size.width) - update_ind_dia - self.size.corner_radius; text::Text::with_alignment( "HB", - geometry::Point::new(circle_x_pos - 5, 10), + geometry::Point::new(circle_x_pos - 5, 10 + self.size.offset_top), Self::SMALL_TEXT_STYLE, text::Alignment::Right, ) .draw(&mut self.target)?; primitives::Circle::new( - geometry::Point::new(circle_x_pos, 2), + geometry::Point::new(circle_x_pos, 2 + self.size.offset_top), update_ind_dia.cast_unsigned(), ) .into_styled(if self.hb_state { @@ -143,13 +155,67 @@ where Ok(()) } + // on big screns: uses full screen width and height from 250 to 280 pixels + pub fn update_gnss( + &mut self, + pvt: &ublox::nav_pvt::proto27::NavPvtRef, + ) -> Result<(), D::Error> { + use alloc::string::ToString as _; + + // this only applies to big screens + if !self.is_big_height() { + return Ok(()); + } + + let num_sat = pvt.num_satellites(); + let fix_str = match pvt.fix_type() { + ublox::GnssFixType::NoFix + | ublox::GnssFixType::DeadReckoningOnly + | ublox::GnssFixType::TimeOnlyFix => { + alloc::format!("{num_sat:2} sat") + } + ublox::GnssFixType::Fix2D => "2D fix".to_string(), + ublox::GnssFixType::Fix3D | ublox::GnssFixType::GPSPlusDeadReckoning => { + "3D fix".to_string() + } + _ => " ".to_string(), + }; + + // clear area and write information + // note: text position is x baseline, not top left corner + primitives::Rectangle::new( + geometry::Point::new(0, 250 + self.size.offset_top), + geometry::Size::new(self.size.width.into(), 20), + ) + .into_styled(Self::BK_STYLE) + .draw(&mut self.target)?; + + let colon = if self.gnss_hb_state { ":" } else { " " }; + let message = alloc::format!( + "{fix_str}{colon} {:2.0}km/h, {:3.0}°", + pvt.ground_speed_2d() * 3.6, + pvt.heading_motion() + ); + text::Text::with_alignment( + &message, + geometry::Point::new(self.size.corner_radius / 2, 265 + self.size.offset_top), + Self::SMALL_TEXT_STYLE, + text::Alignment::Left, + ) + .draw(&mut self.target)?; + + self.gnss_hb_state = !self.gnss_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()), + geometry::Size::new(self.size.width.into(), self.size.height.into()), ) .into_styled(Self::BK_STYLE) .draw(&mut self.target)?; @@ -157,6 +223,10 @@ where Ok(()) } + fn is_big_height(&self) -> bool { + self.size.height > Self::BIG_HEIGHT_THLD + } + #[allow(unused)] pub fn test_img(&mut self) -> Result<(), D::Error> { mipidsi::TestImage::new().draw(&mut self.target)?; @@ -171,7 +241,7 @@ where text::Text::with_alignment( msg, - geometry::Point::new(i32::from(self.width) / 2, 40), + geometry::Point::new(i32::from(self.size.width) / 2, 40 + self.size.offset_top), Self::DEFAULT_TEXT_STYLE, text::Alignment::Center, ) @@ -186,8 +256,8 @@ where self.clear()?; text::Text::with_alignment( - "No upcoming SPAT", - geometry::Point::new(i32::from(self.width) / 2, 130), + "Searching...", + geometry::Point::new(i32::from(self.size.width) / 2, 130 + self.size.offset_top), Self::DEFAULT_TEXT_STYLE, text::Alignment::Center, ) @@ -209,7 +279,7 @@ where text::Text::with_alignment( &text, - geometry::Point::new(i32::from(self.width) / 2, 130), + geometry::Point::new(i32::from(self.size.width) / 2, 130 + self.size.offset_top), Self::DEFAULT_TEXT_STYLE, text::Alignment::Center, ) @@ -218,6 +288,9 @@ where Ok(()) } + // uses full screen width and height + // - from 0 to 105+67 = 172 pixels for small screens + // - from 0 to 105+67+20 = 192 pixels for big screens #[allow(unused)] fn draw_glosa( &mut self, @@ -226,6 +299,8 @@ where ) -> Result<(), D::Error> { let num_signals = data.signal_groups.len(); + let is_big_height = self.is_big_height(); + if let Some(prev) = prev_data && prev.signal_groups.len() != num_signals { @@ -233,25 +308,30 @@ where self.clear(); } else { // only clear maneuver and timing area otherwise - + let sig_total_height = 67 + if is_big_height { 20 } else { 0 }; primitives::Rectangle::new( - geometry::Point::new(0, 105), - geometry::Size::new(self.width.into(), 67), + geometry::Point::new(0, 105 + self.size.offset_top), + geometry::Size::new(self.size.width.into(), sig_total_height), ) .into_styled(Self::BK_STYLE) .draw(&mut self.target)?; } + // do nothing more when no signals + if num_signals == 0 { + return Ok(()); + } + // draw intersection ID text::Text::new( &alloc::format!("#{}", data.intersection_id), - geometry::Point::new(15, 10), + geometry::Point::new(self.size.corner_radius + 5, 10 + self.size.offset_top), 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); + let sig_width = i32::from(self.size.width) / (num_signals as i32); for (idx, sig) in data.signal_groups.iter().enumerate() { #[allow(clippy::cast_possible_truncation, clippy::cast_possible_wrap)] @@ -269,7 +349,7 @@ where applogic::SignalPhase::RedYellow | applogic::SignalPhase::Yellow => Self::YE_STYLE, }; primitives::Circle::new( - geometry::Point::new(offset + circle_margin, 15), + geometry::Point::new(offset + circle_margin, 15 + self.size.offset_top), circle_dia.cast_unsigned(), ) .into_styled(style) @@ -280,24 +360,50 @@ where // draw maneuver text::Text::with_alignment( &maneuver_to_str(sig.maneuver), - geometry::Point::new(centerline, 130), + geometry::Point::new(centerline, 130 + self.size.offset_top), 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)?; + // draw timing indicator(s) + if let Some(timing) = &sig.timing { + if let (Some(likely), Some(max)) = (timing.likely_end_sec, timing.max_end_sec) { + text::Text::with_alignment( + &alloc::format!("{likely}s"), + geometry::Point::new(centerline, 160 + self.size.offset_top), + Self::DEFAULT_TEXT_STYLE, + text::Alignment::Center, + ) + .draw(&mut self.target)?; + + // only for big screen + if is_big_height { + // TODO: show as diff to likely time? + text::Text::with_alignment( + &alloc::format!("{}-{max}", timing.min_end_sec), + geometry::Point::new(centerline, 185 + self.size.offset_top), + Self::MID_TEXT_STYLE, + text::Alignment::Center, + ) + .draw(&mut self.target)?; + } + } else { + text::Text::with_alignment( + &alloc::format!("{}s", timing.min_end_sec), + geometry::Point::new(centerline, 160 + self.size.offset_top), + Self::DEFAULT_TEXT_STYLE, + text::Alignment::Center, + ) + .draw(&mut self.target)?; + } } } + // if is_big_height { + // // TODO: Add ETA + // } + Ok(()) } @@ -307,7 +413,7 @@ where self.clear()?; - let sig_width = i32::from(self.width) / 3; + let sig_width = i32::from(self.size.width) / 3; let circle_dia = 50; let circle_margin = (sig_width - circle_dia) / 2; @@ -319,7 +425,7 @@ where let offset = (idx as i32) * sig_width; primitives::Circle::new( - geometry::Point::new(offset + circle_margin, 15), + geometry::Point::new(offset + circle_margin, 15 + self.size.offset_top), circle_dia.cast_unsigned(), ) .into_styled(*style) @@ -327,10 +433,10 @@ where } // draw welcome text - let centerline = i32::from(self.width) / 2; + let centerline = i32::from(self.size.width) / 2; text::Text::with_alignment( - "C-ITS Signal Phase Display\nWaiting for GNSS fix...", - geometry::Point::new(centerline, 130), + "C-ITS Signal Phase\nWaiting for GNSS...", + geometry::Point::new(centerline, 130 + self.size.offset_top), text_style, text::Alignment::Center, ) @@ -353,7 +459,7 @@ fn maneuver_to_str(value: applogic::v2x::map::Maneuvers) -> alloc::string::Strin /// /// # Errors /// Human-readable fatal errors -#[allow(clippy::too_many_arguments, clippy::type_complexity)] +#[allow(unused, clippy::too_many_arguments, clippy::type_complexity)] pub fn make_small_display( scl: IoScl, sda: IoSda, @@ -378,8 +484,7 @@ pub fn make_small_display( mipidsi::models::ST7789, esp_hal::gpio::Output<'_>, >, - u16, - u16, + DisplaySize, ), alloc::string::String, > @@ -396,10 +501,98 @@ where esp_hal::gpio::interconnect::PeripheralOutput<'static> + esp_hal::gpio::OutputPin + 'static, Spi: esp_hal::spi::master::Instance + 'static, { - const DISPLAY_WIDTH: u16 = 320; - const DISPLAY_HEIGHT: u16 = 172; const DISPLAY_HIDDEN_X: u16 = 34; // somehow this display has 34 px of invisible space on the left (in native orientation) + let size = DisplaySize { + width: 320, + height: 172, + offset_top: 0, + corner_radius: 10, + }; + + let (spi, cs) = setup_st7789_spi(scl, sda, cs, spi) + .map_err(|err| alloc::format!("Failed to build SPI config: {err}"))?; + + let output_config = esp_hal::gpio::OutputConfig::default(); + let dc_output = esp_hal::gpio::Output::new(dc, esp_hal::gpio::Level::Low, output_config); + let rst_output = esp_hal::gpio::Output::new(res, esp_hal::gpio::Level::High, output_config); + let _ = esp_hal::gpio::Output::new(bl, esp_hal::gpio::Level::High, output_config); // enable backlight + + let mut display_delay = esp_hal::delay::Delay::new(); + + let spi_device = embedded_hal_bus::spi::ExclusiveDevice::new_no_delay(spi, cs) + .map_err(|err| alloc::format!("Failed to initialize SPI device: {err}"))?; + + // Note: height and width are swapped b/c of 90° display rotation + let display = mipidsi::Builder::new( + mipidsi::models::ST7789, + mipidsi::interface::SpiInterface::new(spi_device, dc_output, spi_buffer), + ) + .display_size(size.height + DISPLAY_HIDDEN_X, size.width) + .reset_pin(rst_output) + .invert_colors(mipidsi::options::ColorInversion::Inverted) + .orientation(mipidsi::options::Orientation::new().rotate(mipidsi::options::Rotation::Deg90)) + .init(&mut display_delay) + .map_err(|err| alloc::format!("Failed to initialize display: {err:?}"))?; + + Ok((display, size)) +} + +/// Creates an [`mipidsi::Display`], width, height tuple for an ST7789 240*280px screen in portrait orientation +/// +/// # Errors +/// Human-readable fatal errors +#[allow(unused, clippy::too_many_arguments, clippy::type_complexity)] +pub fn make_large_display( + scl: IoScl, + sda: IoSda, + res: IoRes, + dc: IoDc, + cs: IoCs, + bl: IoBl, + spi: Spi, + spi_buffer: &mut [u8], +) -> Result< + ( + mipidsi::Display< + mipidsi::interface::SpiInterface< + '_, + embedded_hal_bus::spi::ExclusiveDevice< + esp_hal::spi::master::Spi<'_, esp_hal::Blocking>, + esp_hal::gpio::Output<'_>, + embedded_hal_bus::spi::NoDelay, + >, + esp_hal::gpio::Output<'_>, + >, + mipidsi::models::ST7789, + esp_hal::gpio::Output<'_>, + >, + DisplaySize, + ), + alloc::string::String, +> +where + IoScl: esp_hal::gpio::interconnect::PeripheralOutput<'static>, + IoSda: esp_hal::gpio::interconnect::PeripheralOutput<'static>, + IoRes: + esp_hal::gpio::interconnect::PeripheralOutput<'static> + esp_hal::gpio::OutputPin + 'static, + IoDc: + esp_hal::gpio::interconnect::PeripheralOutput<'static> + esp_hal::gpio::OutputPin + 'static, + IoCs: + esp_hal::gpio::interconnect::PeripheralOutput<'static> + esp_hal::gpio::OutputPin + 'static, + IoBl: + esp_hal::gpio::interconnect::PeripheralOutput<'static> + esp_hal::gpio::OutputPin + 'static, + Spi: esp_hal::spi::master::Instance + 'static, +{ + const DISPLAY_HIDDEN_Y: u16 = 20; // somehow this display has 20 px of invisible space on the top (in native orientation) + + let size = DisplaySize { + width: 240, + height: 280 + DISPLAY_HIDDEN_Y, + offset_top: DISPLAY_HIDDEN_Y.into(), + corner_radius: 30, + }; + let (spi, cs) = setup_st7789_spi(scl, sda, cs, spi) .map_err(|err| alloc::format!("Failed to build SPI config: {err}"))?; @@ -417,14 +610,14 @@ where mipidsi::models::ST7789, mipidsi::interface::SpiInterface::new(spi_device, dc_output, spi_buffer), ) - .display_size(DISPLAY_HEIGHT + DISPLAY_HIDDEN_X, DISPLAY_WIDTH) + .display_size(size.width, size.height + DISPLAY_HIDDEN_Y) .reset_pin(rst_output) .invert_colors(mipidsi::options::ColorInversion::Inverted) - .orientation(mipidsi::options::Orientation::new().rotate(mipidsi::options::Rotation::Deg90)) + .orientation(mipidsi::options::Orientation::new().rotate(mipidsi::options::Rotation::Deg0)) .init(&mut display_delay) .map_err(|err| alloc::format!("Failed to initialize display: {err:?}"))?; - Ok((display, DISPLAY_WIDTH, DISPLAY_HEIGHT)) + Ok((display, size)) } /// Builds the SPI interface for the ST7789 display From fb47f17c3fb7ffe3807a12a91635873c2bf8d6e3 Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Sat, 15 Aug 2026 21:29:41 +0200 Subject: [PATCH 07/14] GNSS: Don't print warnings on no fix --- src/main.rs | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/main.rs b/src/main.rs index 13be5d6..5e4e0c5 100644 --- a/src/main.rs +++ b/src/main.rs @@ -372,14 +372,12 @@ async fn main(spawner: Spawner) -> ! { println!("{}", ubx::pretty_print_nav_sat(packet)); } ublox::proto27::PacketRef::NavPvt(pvt) => { - match applogic::GnssFix::try_from(pvt) { - Ok(pos_state) => { - critical_section::with(|cs| { - let gnss_update_ref = GNSS_UPDATE.borrow(cs); - gnss_update_ref.replace(Some(pos_state)); - }); - } - Err(err) => warn!("{err}"), + // error can be ignored b/c it's just when no GNSS fix yet + if let Ok(pos_state) = applogic::GnssFix::try_from(pvt) { + critical_section::with(|cs| { + let gnss_update_ref = GNSS_UPDATE.borrow(cs); + gnss_update_ref.replace(Some(pos_state)); + }); } // update GNSS HB on screen From a2b0819fb011a4a333bbbdc5b2bd806f05f0f0a1 Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Mon, 17 Aug 2026 15:00:00 +0200 Subject: [PATCH 08/14] main: Tweak vehicle size --- src/main.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main.rs b/src/main.rs index 5e4e0c5..6eb670c 100644 --- a/src/main.rs +++ b/src/main.rs @@ -68,9 +68,9 @@ const GN_IS_MOBILE: bool = true; #[cfg(feature = "cam_tx")] const OWN_STATION_TYPE: ItsStationType = ItsStationType::Cyclist; #[cfg(feature = "cam_tx")] -const OWN_VEHICLE_WIDTH: u8 = 5; // in 10cm steps! +const OWN_VEHICLE_WIDTH: u8 = 7; // in 10cm steps! #[cfg(feature = "cam_tx")] -const OWN_VEHICLE_LENGTH: u16 = 20; // in 10cm steps! +const OWN_VEHICLE_LENGTH: u16 = 18; // in 10cm steps! #[cfg(feature = "_uart")] const SERIAL_FIFO_SIZE_THLD: u8 = 100; // hardware has max. 128 byte From 352ef6fb7a1c945f4fddf8d7b8942b8a567daecc Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Mon, 17 Aug 2026 15:20:49 +0200 Subject: [PATCH 09/14] applogic: Add missing feature flag for `TimingData` impl --- src/applogic/mod.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/src/applogic/mod.rs b/src/applogic/mod.rs index 55672a7..0ced524 100644 --- a/src/applogic/mod.rs +++ b/src/applogic/mod.rs @@ -118,6 +118,7 @@ pub struct TimingData { pub max_end_sec: Option, } +#[cfg(feature = "spat")] impl TimingData { pub fn new(value: &map::SignalGroup, now: chrono::DateTime) -> Option { let min = value From 2dfcfef58d568bbf8686386acb0cbafb72fd7b92 Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Wed, 19 Aug 2026 15:35:38 +0200 Subject: [PATCH 10/14] BLE: Remove denm dependency, align default features with readme --- Cargo.toml | 4 ++-- Readme.md | 4 +--- feat-permut.sh | 2 ++ 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 0136b8e..6dc5d8e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -15,7 +15,7 @@ path = "./src/test.rs" required-features = ["std", "cam"] [features] -default = ["esp", "spat", "gnss_ubx", "screen", "ble"] +default = ["esp", "spat", "gnss_ubx", "screen"] # build for ESP (needed to build test for `host-tuple`) esp = [ @@ -57,7 +57,7 @@ screen = ["dep:mipidsi", "dep:embedded-graphics", "dep:embedded-hal-bus", "dep:p # enable I/O via UART uart = ["_uart", "_gnss"] # enable I/O via BLE -ble = ["_gnss", "denm"] +ble = ["_gnss"] # internal feature, that any UART I/O is enabled _uart = [] diff --git a/Readme.md b/Readme.md index 5fb34e6..16453c6 100644 --- a/Readme.md +++ b/Readme.md @@ -57,9 +57,7 @@ Enable UART I/O protocol according to [docs/uart-protocol.md](./docs/uart-protoc ### BLE Enable BLE I/O protocol according to [docs/ble-protocol.md](./docs/ble-protocol.md). -Will enable `denm` feature, but also enabling `cam` is recommended for full functionality. - -Note: Raw message reception is currently not implemented! +Enabling `denm` feature or `cam` feature is recommended, otherwise no data will be received. ## Usage diff --git a/feat-permut.sh b/feat-permut.sh index 1ef6b62..287dbcb 100755 --- a/feat-permut.sh +++ b/feat-permut.sh @@ -15,6 +15,8 @@ features_base=( spat,gnss_ubx,screen,cam_tx ) features_more=( + spat,gnss_ubx,screen,ble + spat,gnss_ubx,screen,ble,cam_tx spat,gnss_ubx,screen,cam,denm spat,gnss_ubx,screen,cam_tx,cam,denm ) From 9211c82d6fb395961223013c8b5176001b5b745a Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Wed, 19 Aug 2026 14:37:49 +0200 Subject: [PATCH 11/14] BLE: Use unique name --- src/ble.rs | 3 ++- src/main.rs | 9 ++++++++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/ble.rs b/src/ble.rs index b585e86..eb95c93 100644 --- a/src/ble.rs +++ b/src/ble.rs @@ -75,6 +75,7 @@ pub async fn run( esp_radio::ble::controller::BleConnector<'static>, CONTROLLER_SLOTS, >, + name: alloc::string::String, ) { let mut resources: HostResources = HostResources::new(); @@ -87,7 +88,7 @@ pub async fn run( info!("Starting advertising and GATT service"); let server = Server::new_with_config(GapConfig::Peripheral(PeripheralConfig { - name: "ESP32 C-ITS", + name: &name, appearance: &appearance::UNKNOWN, })) .unwrap(); diff --git a/src/main.rs b/src/main.rs index 6eb670c..d3adbbd 100644 --- a/src/main.rs +++ b/src/main.rs @@ -219,7 +219,14 @@ async fn main(spawner: Spawner) -> ! { #[cfg(feature = "ble")] { let controller = ble::make_controller(peripherals.BT); - spawner.spawn(ble::run(controller).expect("Failed to spawn BLE task")); + let serial = &mac.as_bytes()[3..6]; + let name = alloc::format!( + "ESP32 C-ITS {:02x}{:02x}{:02x}", + serial[0], + serial[1], + serial[2] + ); + spawner.spawn(ble::run(controller, name).expect("Failed to spawn BLE task")); } // Setup GNSS: From 610d4a925ce8b04e240433a2f3b876b1d49a971a Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Mon, 17 Aug 2026 11:35:00 +0200 Subject: [PATCH 12/14] BLE: Fix UUIDs --- docs/ble-protocol.md | 12 ++++++------ src/ble.rs | 4 ++-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/docs/ble-protocol.md b/docs/ble-protocol.md index 4560424..8d742e2 100644 --- a/docs/ble-protocol.md +++ b/docs/ble-protocol.md @@ -19,12 +19,12 @@ V2X data is exposed via the service UUID `c0b70000-d4f4-4000-ada8-f99a02ee315c` UUID | Type | Description ------------------------------------- | --------------- | ----------- c0b70001-d4f4-4001-ada8-f99a02ee315c | `PositionState` | Set position from external source -c0b70001-d4f4-4001-ada8-f99a02ee315c | `RawMsgRx` | Raw CAM message (de-duplicated/ rate-limited) -c0b70001-d4f4-4002-ada8-f99a02ee315c | `CamEvent` | Latest received CAM -c0b70001-d4f4-4003-ada8-f99a02ee315c | `RawMsgRx` | Raw DENM message (de-duplicated/ rate-limited) -c0b70001-d4f4-4004-ada8-f99a02ee315c | `DenmEvent` | Latest received DENM -c0b70001-d4f4-4005-ada8-f99a02ee315c | `RawMsgRx` | Raw MAPEM message (de-duplicated/ rate-limited) -c0b70001-d4f4-4006-ada8-f99a02ee315c | `RawMsgRx` | Raw SPATEM message (de-duplicated/ rate-limited) +c0b70001-d4f4-4002-ada8-f99a02ee315c | `RawMsgRx` | Raw CAM message (de-duplicated/ rate-limited) +c0b70001-d4f4-4003-ada8-f99a02ee315c | `CamEvent` | Latest received CAM +c0b70001-d4f4-4004-ada8-f99a02ee315c | `RawMsgRx` | Raw DENM message (de-duplicated/ rate-limited) +c0b70001-d4f4-4005-ada8-f99a02ee315c | `DenmEvent` | Latest received DENM +c0b70001-d4f4-4006-ada8-f99a02ee315c | `RawMsgRx` | Raw MAPEM message (de-duplicated/ rate-limited) +c0b70001-d4f4-4007-ada8-f99a02ee315c | `RawMsgRx` | Raw SPATEM message (de-duplicated/ rate-limited) ## Message Format diff --git a/src/ble.rs b/src/ble.rs index eb95c93..030d843 100644 --- a/src/ble.rs +++ b/src/ble.rs @@ -44,12 +44,12 @@ struct CITSEvents { #[cfg(feature = "cam")] #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "CAM_Event", read, value = "CamEvent proto msg", type = &'static str)] - #[characteristic(uuid = "c0b70001-d4f4-4002-ada8-f99a02ee315c", read, notify, value = [0u8; CAM_EVENT_SIZE])] + #[characteristic(uuid = "c0b70001-d4f4-4003-ada8-f99a02ee315c", read, notify, value = [0u8; CAM_EVENT_SIZE])] cam_event: [u8; CAM_EVENT_SIZE], #[cfg(feature = "denm")] #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "DENM_Event", read, value = "DenmEvent proto msg", type = &'static str)] - #[characteristic(uuid = "c0b70001-d4f4-4004-ada8-f99a02ee315c", read, notify, value = [0u8; DENM_EVENT_SIZE])] + #[characteristic(uuid = "c0b70001-d4f4-4005-ada8-f99a02ee315c", read, notify, value = [0u8; DENM_EVENT_SIZE])] denm_event: [u8; DENM_EVENT_SIZE], } From 059f47df561788403db8c8325ac677b42aaef0b1 Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Mon, 17 Aug 2026 14:53:48 +0200 Subject: [PATCH 13/14] BLE: Add raw messages BLE can only fit 251 bytes of payload in notifications which isn't nearly enough for a raw V2X message. So only publish a notification which message was received and let the client poll the content from the characteristic (since reads can get data in fragments) --- docs/ble-protocol.md | 33 ++++- docs/c_its-messages.proto | 4 + src/ble.rs | 247 ++++++++++++++++++++++++++++++-------- src/io.rs | 146 ++++++++++++++++++++-- src/main.rs | 67 ++++++++--- 5 files changed, 413 insertions(+), 84 deletions(-) diff --git a/docs/ble-protocol.md b/docs/ble-protocol.md index 8d742e2..e8ad492 100644 --- a/docs/ble-protocol.md +++ b/docs/ble-protocol.md @@ -19,12 +19,16 @@ V2X data is exposed via the service UUID `c0b70000-d4f4-4000-ada8-f99a02ee315c` UUID | Type | Description ------------------------------------- | --------------- | ----------- c0b70001-d4f4-4001-ada8-f99a02ee315c | `PositionState` | Set position from external source -c0b70001-d4f4-4002-ada8-f99a02ee315c | `RawMsgRx` | Raw CAM message (de-duplicated/ rate-limited) -c0b70001-d4f4-4003-ada8-f99a02ee315c | `CamEvent` | Latest received CAM -c0b70001-d4f4-4004-ada8-f99a02ee315c | `RawMsgRx` | Raw DENM message (de-duplicated/ rate-limited) -c0b70001-d4f4-4005-ada8-f99a02ee315c | `DenmEvent` | Latest received DENM -c0b70001-d4f4-4006-ada8-f99a02ee315c | `RawMsgRx` | Raw MAPEM message (de-duplicated/ rate-limited) -c0b70001-d4f4-4007-ada8-f99a02ee315c | `RawMsgRx` | Raw SPATEM message (de-duplicated/ rate-limited) +c0b70001-d4f4-4002-ada8-f99a02ee315c | `RawNotify` | Raw message notification +c0b70001-d4f4-4003-ada8-f99a02ee315c | `RawMsgRx` | Raw CAM message (de-duplicated/ rate-limited) +c0b70001-d4f4-4004-ada8-f99a02ee315c | `CamEvent` | Latest received CAM +c0b70001-d4f4-4005-ada8-f99a02ee315c | `RawMsgRx` | Raw DENM message (de-duplicated/ rate-limited) +c0b70001-d4f4-4006-ada8-f99a02ee315c | `DenmEvent` | Latest received DENM +c0b70001-d4f4-4007-ada8-f99a02ee315c | `RawMsgRx` | Raw MAPEM message (de-duplicated/ rate-limited) +c0b70001-d4f4-4008-ada8-f99a02ee315c | `RawMsgRx` | Raw SPATEM message (de-duplicated/ rate-limited) + +Since a modern BLE notification can only fit up tp 251 bytes of payload, the raw messages can only be polled. +Please subscribe to `c0b70001-d4f4-4002-ada8-f99a02ee315c` and use the message ID to determine from which service to read the incoming value. ## Message Format @@ -101,3 +105,20 @@ Repetitions of the same message should be dropped by the BLE server. Consecutive messages with no relevant changes may be dropped by the BLE server while keeping some minimal publishing rate, e.g.: - only publish MAPEMs every 10 seconds (since content doesn't change, but client may be restarted after first MAPEM from an intersection was received) - drop high-frequency CAMs (only publish "full" CAMs which include the low-frequency container) + +Maximum message size (assuming realistic value ranges): + +field name | tag size | (max.) data size +--------------------- | -------- | --------- +msg_type | 1 | 1 (VARINT(21)) +payload | 1 | 2+1394 (LEN(1394) + payload) +**SUM** | 2 + | 1397 = 1399 byte + +### Raw Message Notification + +Maximum message size (assuming realistic value ranges): + +field name | tag size | (max.) data size +--------------------- | -------- | --------- +msg_type | 1 | 1 (VARINT(21)) +**SUM** | 1 + | 1 = 2 byte diff --git a/docs/c_its-messages.proto b/docs/c_its-messages.proto index 47e523d..9323b3e 100644 --- a/docs/c_its-messages.proto +++ b/docs/c_its-messages.proto @@ -71,6 +71,10 @@ message RawMsgTx { optional uint32 stationType = 6; // for source position vector } +message RawNotify { + required uint32 message_type = 1; // ITS `MessageId` integer value +} + // ------------------------------ // Messages // ------------------------------ diff --git a/src/ble.rs b/src/ble.rs index 030d843..8fa5ead 100644 --- a/src/ble.rs +++ b/src/ble.rs @@ -6,6 +6,7 @@ use core::cell::RefCell; use critical_section::Mutex; +use crossbeam_queue::ArrayQueue; use esp_backtrace as _; use log::{info, warn}; use trouble_host::prelude::*; @@ -16,14 +17,26 @@ pub const CONTROLLER_SLOTS: usize = 20; const CONNECTIONS_MAX: usize = 1; const L2CAP_CHANNELS_MAX: usize = 1; -pub static TX_DATA: Mutex>> = Mutex::new(RefCell::new(None)); +pub static CLIENT_CONNECTED: Mutex> = Mutex::new(RefCell::new(false)); +pub static TX_DATA: Mutex>>> = + Mutex::new(RefCell::new(None)); pub static RX_DATA: Mutex>> = Mutex::new(RefCell::new(None)); pub enum BleSendable { + #[cfg(feature = "cam")] + CamRaw(io::msg::RawMsgRx), #[cfg(feature = "cam")] Cam(io::msg::CamEvent), + + #[cfg(feature = "denm")] + DenmRaw(io::msg::RawMsgRx), #[cfg(feature = "denm")] Denm(io::msg::DenmEvent), + + #[cfg(feature = "spat")] + MapemRaw(io::msg::RawMsgRx), + #[cfg(feature = "spat")] + SpatemRaw(io::msg::RawMsgRx), } #[gatt_server] @@ -31,26 +44,43 @@ struct Server { c_its_events: CITSEvents, } -#[cfg(feature = "cam")] -const CAM_EVENT_SIZE: usize = 38; -#[cfg(feature = "denm")] -const DENM_EVENT_SIZE: usize = 51; - #[gatt_service(uuid = "c0b70000-d4f4-4000-ada8-f99a02ee315c")] struct CITSEvents { #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "Position", read, value = "PositionState proto msg", type = &'static str)] #[characteristic(uuid = "c0b70001-d4f4-4001-ada8-f99a02ee315c", write)] position: [u8; 27], + #[cfg(any(feature = "cam", feature = "denm", feature = "spat"))] + #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "Raw_Notify", read, value = "Raw Message Notification", type = &'static str)] + #[characteristic(uuid = "c0b70001-d4f4-4002-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::RawNotify::PROTO_SIZE])] + raw_notify: [u8; io::msg::RawNotify::PROTO_SIZE], + + #[cfg(feature = "cam")] + #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "CAM", read, value = "RawMsgRx proto msg", type = &'static str)] + #[characteristic(uuid = "c0b70001-d4f4-4003-ada8-f99a02ee315c", read, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])] + cam_raw: [u8; io::msg::RawMsgRx::PROTO_SIZE], #[cfg(feature = "cam")] #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "CAM_Event", read, value = "CamEvent proto msg", type = &'static str)] - #[characteristic(uuid = "c0b70001-d4f4-4003-ada8-f99a02ee315c", read, notify, value = [0u8; CAM_EVENT_SIZE])] - cam_event: [u8; CAM_EVENT_SIZE], + #[characteristic(uuid = "c0b70001-d4f4-4004-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::CamEvent::PROTO_SIZE])] + cam_event: [u8; io::msg::CamEvent::PROTO_SIZE], + #[cfg(feature = "denm")] + #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "DENM", read, value = "RawMsgRx proto msg", type = &'static str)] + #[characteristic(uuid = "c0b70001-d4f4-4005-ada8-f99a02ee315c", read, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])] + denm_raw: [u8; io::msg::RawMsgRx::PROTO_SIZE], #[cfg(feature = "denm")] #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "DENM_Event", read, value = "DenmEvent proto msg", type = &'static str)] - #[characteristic(uuid = "c0b70001-d4f4-4005-ada8-f99a02ee315c", read, notify, value = [0u8; DENM_EVENT_SIZE])] - denm_event: [u8; DENM_EVENT_SIZE], + #[characteristic(uuid = "c0b70001-d4f4-4006-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::DenmEvent::PROTO_SIZE])] + denm_event: [u8; io::msg::DenmEvent::PROTO_SIZE], + + #[cfg(feature = "spat")] + #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "MAPEM", read, value = "RawMsgRx proto msg", type = &'static str)] + #[characteristic(uuid = "c0b70001-d4f4-4007-ada8-f99a02ee315c", read, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])] + mapem_raw: [u8; io::msg::RawMsgRx::PROTO_SIZE], + #[cfg(feature = "spat")] + #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "SPATEM", read, value = "RawMsgRx proto msg", type = &'static str)] + #[characteristic(uuid = "c0b70001-d4f4-4008-ada8-f99a02ee315c", read, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])] + spatem_raw: [u8; io::msg::RawMsgRx::PROTO_SIZE], } /// Creates the BLE controller @@ -60,6 +90,10 @@ pub fn make_controller( esp_radio::ble::controller::BleConnector<'static>, CONTROLLER_SLOTS, > { + critical_section::with(|cs| { + TX_DATA.borrow(cs).replace(Some(ArrayQueue::new(2))); + }); + let transport = esp_radio::ble::controller::BleConnector::new( bt_peripheral, esp_radio::ble::Config::default(), @@ -86,7 +120,21 @@ pub async fn run( .. } = stack.build(); - info!("Starting advertising and GATT service"); + let mut features = alloc::vec![]; + if cfg!(feature = "cam") { + features.push("cam"); + } + if cfg!(feature = "denm") { + features.push("denm"); + } + if cfg!(feature = "spat") { + features.push("mapem"); + features.push("spatem"); + } + info!( + "Starting advertising and GATT service for {}", + features.join(",") + ); let server = Server::new_with_config(GapConfig::Peripheral(PeripheralConfig { name: &name, appearance: &appearance::UNKNOWN, @@ -99,10 +147,15 @@ pub async fn run( Ok(conn) => { // set up tasks when the connection is established to a central, so they don't run when no one is connected. let a = gatt_events_task(&server, &conn); - let b = custom_task(&server, &conn); - // run until any task ends (usually because the connection has been closed), - // then return to advertising state. + let b = publishing_task(&server, &conn); + + // run until any task ends (usually because the connection has been closed), then return to advertising state. embassy_futures::select::select(a, b).await; + + critical_section::with(|cs| { + let value_ref = CLIENT_CONNECTED.borrow(cs); + let _ = value_ref.replace(false); + }); } Err(e) => { panic!("BLE ADV: fatal error {e:?}"); @@ -113,6 +166,21 @@ pub async fn run( .await; } +pub fn update(payload: BleSendable) { + critical_section::with(|cs| { + // only add to queue when someone is connected + if *CLIENT_CONNECTED.borrow(cs).borrow() { + let queue_rc = TX_DATA.borrow(cs).borrow(); + // unwrap is fine b/c we stored something in it before + let queue = queue_rc.as_ref().unwrap(); + + if queue.push(payload).is_err() { + log::error!("BLE TX queue is full"); + } + } + }); +} + /// Create an advertiser to use to connect to a BLE Central, and wait for it to connect. async fn advertise<'values, 'server, C: Controller>( name: &'values str, @@ -202,63 +270,142 @@ async fn gatt_events_task( Ok(()) } -/// Example task to use the BLE notifier interface. -/// This task will notify the connected central of a counter value every 2 seconds. -/// It will also read the RSSI value every 2 seconds. -/// and will stop when the connection is closed by the central or an error occurs. -async fn custom_task(server: &Server<'_>, conn: &GattConnection<'_, '_, P>) { +#[allow(clippy::too_many_lines)] +async fn publishing_task(server: &Server<'_>, conn: &GattConnection<'_, '_, P>) { + critical_section::with(|cs| { + let value_ref = CLIENT_CONNECTED.borrow(cs); + let _ = value_ref.replace(true); + }); + loop { embassy_time::Timer::after(embassy_time::Duration::from_millis(10)).await; let mut new_data = None; critical_section::with(|cs| { - let tx_data_ref = TX_DATA.borrow(cs); + let queue_rc = TX_DATA.borrow(cs).borrow(); + // unwrap is fine b/c we stored something in it before + let queue = queue_rc.as_ref().unwrap(); - if let Some(tx_data) = tx_data_ref.replace(None) { - new_data = Some(tx_data); + if let Some(data) = queue.pop() { + new_data = Some(data); } }); match new_data { #[cfg(feature = "cam")] - Some(BleSendable::Cam(cam_event)) => { - let mut buf = cam_event.serialize_ble_proto(); - - if buf.len() <= CAM_EVENT_SIZE { - // pad with zeros - let pad = alloc::vec![0u8; CAM_EVENT_SIZE - buf.len()]; - buf.extend_from_slice(&pad); - - // unwrap should be fine since we ensured the vector size before - let value = buf.try_into().unwrap(); - + Some(BleSendable::Cam(cam_event)) => match cam_event.serialize_ble_proto() { + Ok(value) => { if let Err(err) = server.c_its_events.cam_event.notify(conn, &value).await { warn!("BLE: failed notifying connection of new CAM event: {err:?}"); } - } else { - warn!("BLE: CAM event too big with {} bytes", buf.len()); } - } + Err(err) => warn!("BLE: CAM event {err}"), + }, + #[cfg(feature = "denm")] - Some(BleSendable::Denm(denm_event)) => { - let mut buf = denm_event.serialize_ble_proto(); - - if buf.len() <= DENM_EVENT_SIZE { - // pad with zeros - let pad = alloc::vec![0u8; DENM_EVENT_SIZE - buf.len()]; - buf.extend_from_slice(&pad); - - // unwrap should be fine since we ensured the vector size before - let value = buf.try_into().unwrap(); - + Some(BleSendable::Denm(denm_event)) => match denm_event.serialize_ble_proto() { + Ok(value) => { if let Err(err) = server.c_its_events.denm_event.notify(conn, &value).await { warn!("BLE: failed notifying connection of new DENM event: {err:?}"); } - } else { - warn!("BLE: DENM event too big with {} bytes", buf.len()); } - } + Err(err) => warn!("BLE: DENM event {err}"), + }, + + #[cfg(feature = "cam")] + Some(BleSendable::CamRaw(raw_msg)) => match raw_msg.serialize_ble_proto() { + Ok(value) => { + if let Err(err) = server.c_its_events.cam_raw.set(server, &value) { + warn!("BLE: failed setting new raw CAM value: {err:?}"); + } + + if let Err(err) = send_raw_notify( + server, + conn, + c_its_parser::standards::extensions::ItsMessageId::Cam, + ) + .await + { + warn!("BLE: {err}"); + } + } + Err(err) => warn!("BLE: Raw CAM too big: {err}"), + }, + #[cfg(feature = "denm")] + Some(BleSendable::DenmRaw(raw_msg)) => match raw_msg.serialize_ble_proto() { + Ok(value) => { + if let Err(err) = server.c_its_events.denm_raw.set(server, &value) { + warn!("BLE: failed setting new raw DENM value: {err:?}"); + } + + if let Err(err) = send_raw_notify( + server, + conn, + c_its_parser::standards::extensions::ItsMessageId::Denm, + ) + .await + { + warn!("BLE: {err}"); + } + } + Err(err) => warn!("BLE: Raw DENM {err}"), + }, + #[cfg(feature = "spat")] + Some(BleSendable::MapemRaw(raw_msg)) => match raw_msg.serialize_ble_proto() { + Ok(value) => { + if let Err(err) = server.c_its_events.mapem_raw.set(server, &value) { + warn!("BLE: failed setting new raw MAPEM value: {err:?}"); + } + + if let Err(err) = send_raw_notify( + server, + conn, + c_its_parser::standards::extensions::ItsMessageId::Mapem, + ) + .await + { + warn!("BLE: {err}"); + } + } + Err(err) => warn!("BLE: Raw MAPEM {err}"), + }, + #[cfg(feature = "spat")] + Some(BleSendable::SpatemRaw(raw_msg)) => match raw_msg.serialize_ble_proto() { + Ok(value) => { + if let Err(err) = server.c_its_events.spatem_raw.set(server, &value) { + warn!("BLE: failed setting new raw SPATEM value: {err:?}"); + } + + if let Err(err) = send_raw_notify( + server, + conn, + c_its_parser::standards::extensions::ItsMessageId::Spatem, + ) + .await + { + warn!("BLE: {err}"); + } + } + Err(err) => warn!("BLE: Raw SPATEM {err}"), + }, None => {} } } } + +async fn send_raw_notify( + server: &Server<'_>, + conn: &GattConnection<'_, '_, P>, + msg_id: c_its_parser::standards::extensions::ItsMessageId, +) -> Result<(), alloc::string::String> { + let notify_buf = io::msg::RawNotify::from(msg_id) + .serialize_ble_proto() + .expect("BLE: Raw notify too big"); + + server + .c_its_events + .raw_notify + .notify(conn, ¬ify_buf) + .await + .map_err(|err| alloc::format!("failed notifying connection of new raw {msg_id:?}: {err:?}")) +} diff --git a/src/io.rs b/src/io.rs index 3ef8c6d..642807f 100644 --- a/src/io.rs +++ b/src/io.rs @@ -40,20 +40,91 @@ impl msg::RawMsgRx { #[cfg(feature = "denm")] impl msg::DenmEvent { #[cfg(feature = "ble")] - pub fn serialize_ble_proto(&self) -> alloc::vec::Vec { + pub const PROTO_SIZE: usize = 51; + + #[cfg(feature = "ble")] + pub fn serialize_ble_proto(&self) -> Result<[u8; Self::PROTO_SIZE], alloc::string::String> { use prost::Message as _; - self.encode_to_vec() + let mut data = [0u8; Self::PROTO_SIZE]; + + let mut mut_buf = &mut data[..]; + if let Err(err) = self.encode(&mut mut_buf) { + Err(alloc::format!("Buffer too small: {err:?}")) + } else { + Ok(data) + } } } #[cfg(feature = "cam")] impl msg::CamEvent { #[cfg(feature = "ble")] - pub fn serialize_ble_proto(&self) -> alloc::vec::Vec { + pub const PROTO_SIZE: usize = 38; + + #[cfg(feature = "ble")] + pub fn serialize_ble_proto(&self) -> Result<[u8; Self::PROTO_SIZE], alloc::string::String> { use prost::Message as _; - self.encode_to_vec() + let mut data = [0u8; Self::PROTO_SIZE]; + + let mut mut_buf = &mut data[..]; + if let Err(err) = self.encode(&mut mut_buf) { + Err(alloc::format!("Buffer too small: {err:?}")) + } else { + Ok(data) + } + } +} + +#[cfg(any(feature = "cam", feature = "denm", feature = "spat"))] +impl msg::RawMsgRx { + #[cfg(feature = "ble")] + pub const PROTO_SIZE: usize = 1400; + + #[cfg(feature = "ble")] + pub fn serialize_ble_proto(&self) -> Result<[u8; Self::PROTO_SIZE], alloc::string::String> { + use prost::Message as _; + + let mut data = [0u8; Self::PROTO_SIZE]; + + let mut mut_buf = &mut data[..]; + if let Err(err) = self.encode(&mut mut_buf) { + Err(alloc::format!("Buffer too small: {err:?}")) + } else { + Ok(data) + } + } +} + +#[cfg(all( + feature = "ble", + any(feature = "cam", feature = "denm", feature = "spat") +))] +impl From for msg::RawNotify { + fn from(value: c_its_parser::standards::extensions::ItsMessageId) -> Self { + let message_type = value.as_u8().into(); + Self { message_type } + } +} + +#[cfg(any(feature = "cam", feature = "denm", feature = "spat"))] +impl msg::RawNotify { + #[cfg(feature = "ble")] + pub const PROTO_SIZE: usize = 2; + + #[cfg(feature = "ble")] + pub fn serialize_ble_proto(self) -> Result<[u8; Self::PROTO_SIZE], alloc::string::String> { + use prost::Message as _; + + let mut data = [0u8; Self::PROTO_SIZE]; + + let mut mut_buf = &mut data[..]; + if let Err(err) = self.encode(&mut mut_buf) { + Err(alloc::format!("Buffer too small: {err:?}")) + } else { + Ok(data) + } } } @@ -414,11 +485,11 @@ impl msg::RawMsgTx { } #[cfg(feature = "cam")] -impl From> +impl From<&alloc::boxed::Box> for msg::CamEvent { fn from( - value: alloc::boxed::Box, + value: &alloc::boxed::Box, ) -> Self { use c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions; @@ -429,12 +500,14 @@ impl From None, }); - let vehicle_data = match value.cam.cam_parameters.high_frequency_container { + let vehicle_data = match &value.cam.cam_parameters.high_frequency_container { cam_pdu_descriptions::HighFrequencyContainer::basicVehicleContainerHighFrequency( basic_vehicle_container_high_frequency, ) => Some(basic_vehicle_container_high_frequency.into()), @@ -460,8 +533,8 @@ impl From for msg::VehicleContainer { - fn from(value: c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions::BasicVehicleContainerHighFrequency) -> Self { +impl From<&c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions::BasicVehicleContainerHighFrequency> for msg::VehicleContainer { + fn from(value: &c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions::BasicVehicleContainerHighFrequency) -> Self { let heading_deg = if value.heading.heading_value.is_unavailable() { None } else { @@ -489,11 +562,11 @@ impl From> +impl From<&alloc::boxed::Box> for msg::DenmEvent { fn from( - value: alloc::boxed::Box, + value: &alloc::boxed::Box, ) -> Self { let denm_mgmt = &value.denm.management; @@ -512,7 +585,8 @@ impl From fo } } +#[cfg(any(feature = "cam", feature = "denm", feature = "spat"))] +impl TryFrom<&c_its_parser::ItsMessage<'_>> for msg::RawMsgRx { + type Error = alloc::string::String; + + fn try_from(value: &c_its_parser::ItsMessage<'_>) -> Result { + let msg_id = c_its_parser::standards::extensions::ItsMessageId::from(value); + + let payload = match value { + #[cfg(feature = "denm")] + c_its_parser::ItsMessage::DenmV1 { + geonetworking: _, + transport: _, + etsi, + } => etsi.encode_to_uper(), + #[cfg(feature = "denm")] + c_its_parser::ItsMessage::DenmV2 { + geonetworking: _, + transport: _, + etsi, + } => etsi.encode_to_uper(), + #[cfg(any(feature = "cam", feature = "cam_tx"))] + c_its_parser::ItsMessage::Cam { + geonetworking: _, + transport: _, + etsi, + } => etsi.encode_to_uper(), + #[cfg(feature = "spat")] + c_its_parser::ItsMessage::Spatem { + geonetworking: _, + transport: _, + etsi, + } => etsi.encode_to_uper(), + #[cfg(feature = "spat")] + c_its_parser::ItsMessage::Mapem { + geonetworking: _, + transport: _, + etsi, + } => etsi.encode_to_uper(), + }?; + + Ok(Self { + msg_type: msg_id.as_u8().into(), + payload, + }) + } +} + #[cfg(test)] mod tests { diff --git a/src/main.rs b/src/main.rs index d3adbbd..ae2a14e 100644 --- a/src/main.rs +++ b/src/main.rs @@ -513,16 +513,36 @@ async fn main(spawner: Spawner) -> ! { ItsMessage::Mapem { geonetworking: _, transport: _, - etsi, - } => state.handle_mapem(&etsi), + ref etsi, + } => { + state.handle_mapem(etsi); + + // Send data to BLE thread + #[cfg(feature = "ble")] + match io::msg::RawMsgRx::try_from(&msg) { + Ok(data) => { + ble::update(ble::BleSendable::MapemRaw(data)); + } + Err(err) => warn!("Failed to create RawMsgRx: {err}"), + } + } #[cfg(feature = "spat")] ItsMessage::Spatem { geonetworking: _, transport: _, - etsi, + ref etsi, } => { if state.initialized() { - state.handle_spatem(&etsi); + state.handle_spatem(etsi); + } + + // Send data to BLE thread + #[cfg(feature = "ble")] + match io::msg::RawMsgRx::try_from(&msg) { + Ok(data) => { + ble::update(ble::BleSendable::SpatemRaw(data)); + } + Err(err) => warn!("Failed to create RawMsgRx: {err}"), } } @@ -530,33 +550,48 @@ async fn main(spawner: Spawner) -> ! { ItsMessage::Cam { geonetworking: _, transport: _, - etsi, + ref etsi, } => { #[cfg(feature = "cam")] - applogic::cam::handle_cam(&etsi); + applogic::cam::handle_cam(etsi); // Send data to BLE thread #[cfg(all(feature = "ble", feature = "cam"))] - critical_section::with(|cs| { - let ble_update_ref = ble::TX_DATA.borrow(cs); - ble_update_ref.replace(Some(ble::BleSendable::Cam(etsi.into()))); - }); + { + ble::update(ble::BleSendable::Cam(etsi.into())); + + // raw data only when it's a full CAM + if etsi.cam.cam_parameters.low_frequency_container.is_some() { + match io::msg::RawMsgRx::try_from(&msg) { + Ok(data) => { + ble::update(ble::BleSendable::CamRaw(data)); + } + Err(err) => warn!("Failed to create RawMsgRx: {err}"), + } + } + } } #[cfg(feature = "denm")] ItsMessage::DenmV2 { geonetworking: _, transport: _, - etsi, + ref etsi, } => { - applogic::denm::handle_denm(&etsi); + applogic::denm::handle_denm(etsi); // Send data to BLE thread #[cfg(feature = "ble")] - critical_section::with(|cs| { - let ble_update_ref = ble::TX_DATA.borrow(cs); - ble_update_ref.replace(Some(ble::BleSendable::Denm(etsi.into()))); - }); + { + ble::update(ble::BleSendable::Denm(etsi.into())); + + match io::msg::RawMsgRx::try_from(&msg) { + Ok(data) => { + ble::update(ble::BleSendable::DenmRaw(data)); + } + Err(err) => warn!("Failed to create RawMsgRx: {err}"), + } + } } #[cfg(feature = "denm")] ItsMessage::DenmV1 { From 3213667eead4e77c7cb3c72ac4b0b320b0424f2c Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Mon, 24 Aug 2026 15:16:36 +0200 Subject: [PATCH 14/14] BLE: Add service UUID to advertisement, remove descriptors to save space --- Readme.md | 3 +++ src/ble.rs | 21 +++++++++++++-------- 2 files changed, 16 insertions(+), 8 deletions(-) diff --git a/Readme.md b/Readme.md index 16453c6..97dfd2b 100644 --- a/Readme.md +++ b/Readme.md @@ -59,6 +59,9 @@ Enable UART I/O protocol according to [docs/uart-protocol.md](./docs/uart-protoc Enable BLE I/O protocol according to [docs/ble-protocol.md](./docs/ble-protocol.md). Enabling `denm` feature or `cam` feature is recommended, otherwise no data will be received. +Note: When changing the set of supported messages on the BLE interface, you may need to turn bluetooth off and on again on your client device. +Otherwise the client may not re-scan the service's characteristics and still use the previous set of characteristics. + ## Usage diff --git a/src/ble.rs b/src/ble.rs index 8fa5ead..bdb32be 100644 --- a/src/ble.rs +++ b/src/ble.rs @@ -56,7 +56,7 @@ struct CITSEvents { raw_notify: [u8; io::msg::RawNotify::PROTO_SIZE], #[cfg(feature = "cam")] - #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "CAM", read, value = "RawMsgRx proto msg", type = &'static str)] + // #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "CAM", read, value = "RawMsgRx proto msg", type = &'static str)] #[characteristic(uuid = "c0b70001-d4f4-4003-ada8-f99a02ee315c", read, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])] cam_raw: [u8; io::msg::RawMsgRx::PROTO_SIZE], #[cfg(feature = "cam")] @@ -65,7 +65,7 @@ struct CITSEvents { cam_event: [u8; io::msg::CamEvent::PROTO_SIZE], #[cfg(feature = "denm")] - #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "DENM", read, value = "RawMsgRx proto msg", type = &'static str)] + // #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "DENM", read, value = "RawMsgRx proto msg", type = &'static str)] #[characteristic(uuid = "c0b70001-d4f4-4005-ada8-f99a02ee315c", read, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])] denm_raw: [u8; io::msg::RawMsgRx::PROTO_SIZE], #[cfg(feature = "denm")] @@ -74,11 +74,11 @@ struct CITSEvents { denm_event: [u8; io::msg::DenmEvent::PROTO_SIZE], #[cfg(feature = "spat")] - #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "MAPEM", read, value = "RawMsgRx proto msg", type = &'static str)] + // #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "MAPEM", read, value = "RawMsgRx proto msg", type = &'static str)] #[characteristic(uuid = "c0b70001-d4f4-4007-ada8-f99a02ee315c", read, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])] mapem_raw: [u8; io::msg::RawMsgRx::PROTO_SIZE], #[cfg(feature = "spat")] - #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "SPATEM", read, value = "RawMsgRx proto msg", type = &'static str)] + // #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "SPATEM", read, value = "RawMsgRx proto msg", type = &'static str)] #[characteristic(uuid = "c0b70001-d4f4-4008-ada8-f99a02ee315c", read, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])] spatem_raw: [u8; io::msg::RawMsgRx::PROTO_SIZE], } @@ -143,8 +143,10 @@ pub async fn run( let _ = embassy_futures::join::join(ble_task(runner), async { loop { - match advertise("ESP32 C-ITS", &mut peripheral, &server).await { + match advertise(&mut peripheral, &server).await { Ok(conn) => { + info!("BLE: New client connected"); + // set up tasks when the connection is established to a central, so they don't run when no one is connected. let a = gatt_events_task(&server, &conn); let b = publishing_task(&server, &conn); @@ -158,7 +160,7 @@ pub async fn run( }); } Err(e) => { - panic!("BLE ADV: fatal error {e:?}"); + panic!("BLE ADV: fatal error: {e:?}"); } } } @@ -183,15 +185,18 @@ pub fn update(payload: BleSendable) { /// Create an advertiser to use to connect to a BLE Central, and wait for it to connect. async fn advertise<'values, 'server, C: Controller>( - name: &'values str, peripheral: &mut Peripheral<'values, C, DefaultPacketPool>, server: &'server Server<'values>, ) -> Result, BleHostError> { let mut advertiser_data = [0; 35]; + let service_uuid = Uuid::from(0xc0b7_0000_d4f4_4000_ada8_f99a_02ee_315c_u128) + .as_raw() + .try_into() + .unwrap(); let len = AdStructure::encode_slice( &[ AdStructure::Flags(LE_GENERAL_DISCOVERABLE | BR_EDR_NOT_SUPPORTED), - AdStructure::CompleteLocalName(name.as_bytes()), + AdStructure::ServiceUuids128(&[service_uuid]), ], &mut advertiser_data[..], )?;