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

spat: ensure that "our" intersection is kept in cache

Browse source 
Sometimes we may not receive V2X data for longer than the TLC cache time.
This leads to no SPAT updates in the GLOSA algorithm since that uses a cached reference to the intersection instead of querying the tlc cache every time.
This commit is contained in:
Jannik Beyerstedt 2026-06-22 22:54:37 +02:00 • committed by Jannik Beyerstedt
commit cc31d6226e
2 changed files with 72 additions and 53 deletions
Download patch file Download diff file Expand all files Collapse all files

102
src/applogic/mod.rs
View file

@ -255,48 +255,57 @@ impl State {
info.intersection_id, info.lane_id info.intersection_id, info.lane_id
); );
// determine if intersection is left // ensure that "our" intersection is kept in TLC cache
let intersection = info.intersection.borrow(); if self
if Self::is_intersection_left( .tlc_cache
&intersection, .keep_intersection(info.intersection_id, self.time)
info.lane_id, .is_err()
info.approach_id, {
&self.position, info!("GLOSA: Intersection not known any more");
) {
info!("GLOSA: Intersection was left -> back to idle");
(Some(GlosaState::Idle), None) (Some(GlosaState::Idle), None)
} else { } else {
// get signal groups which are in our approach // determine if intersection is left
let mut approach_sig_grps = intersection let intersection = info.intersection.borrow();
.signal_groups() if Self::is_intersection_left(
.iter() &intersection,
.filter(|i| { info.lane_id,
info.signal_groups info.approach_id,
.iter() &self.position,
.find(|sg_id| **sg_id == i.id()) ) {
.is_some() info!("GLOSA: Intersection was left -> back to idle");
}) (Some(GlosaState::Idle), None)
.collect::<Vec<_>>(); } else {
// get signal groups which are in our approach
let mut approach_sig_grps = intersection
.signal_groups()
.iter()
.filter(|i| {
info.signal_groups
.iter()
.find(|sg_id| **sg_id == i.id())
.is_some()
})
.collect::<Vec<_>>();
// sort signal groups by maneuver // sort signal groups by maneuver
approach_sig_grps.sort_by_key(|v| v.maneuvers()); approach_sig_grps.sort_by_key(|v| v.maneuvers());
// display signal phases // display signal phases
let sig_grps = approach_sig_grps let sig_grps = approach_sig_grps
.iter() .iter()
.filter_map(|sig| { .filter_map(|sig| {
if let Some(manv) = sig.maneuvers() { if let Some(manv) = sig.maneuvers() {
use core::ops::Sub; use core::ops::Sub;
use num_traits::Float; use num_traits::Float;
let phase = sig let phase = sig
.phase() .phase()
.map(core::convert::Into::into) .map(core::convert::Into::into)
.unwrap_or_default(); .unwrap_or_default();
let end_sec = let end_sec = sig
sig.likely_time() .likely_time()
.or_else(|| sig.min_end_time()) .or_else(|| sig.min_end_time())
.map(|time| { .map(|time| {
let dur = let dur =
@ -308,18 +317,19 @@ impl State {
end_sec end_sec
}); });
Some(SignalGroupData { Some(SignalGroupData {
phase, phase,
maneuver: manv, maneuver: manv,
end_sec, end_sec,
}) })
} else { } else {
None None
} }
}) })
.collect::<Vec<_>>(); .collect::<Vec<_>>();
(None, Some(sig_grps)) // keep state, return signal groups (None, Some(sig_grps)) // keep state, return signal groups
}
} }
} }
}; };

11
src/applogic/tlc.rs
View file

@ -10,7 +10,7 @@ use chrono::Datelike;
use esp_println::println; use esp_println::println;
use super::v2x::map; use super::v2x::map;
use crate::cache::Cache; use crate::cache::{Cache, CacheError};
pub struct Data { pub struct Data {
intersections: Cache<u16, map::Intersection>, intersections: Cache<u16, map::Intersection>,
@ -86,6 +86,15 @@ impl Data {
self.intersections.prune(time_now); self.intersections.prune(time_now);
} }
/// Bumps the timestamp of a certain intersection to keep it cached
pub fn keep_intersection(
&mut self,
intersection_id: u16,
time_now: chrono::NaiveDateTime,
) -> Result<(), CacheError<u16>> {
self.intersections.update(time_now, intersection_id, |_| {})
}
pub fn handle_spatem(&mut self, time_now: chrono::NaiveDateTime, etsi: &SPATEM) { pub fn handle_spatem(&mut self, time_now: chrono::NaiveDateTime, etsi: &SPATEM) {
for int in &etsi.spat.intersections.0 { for int in &etsi.spat.intersections.0 {
let int_id = int.id.id.0; let int_id = int.id.id.0;