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
);
// determine if intersection is left
let intersection = info.intersection.borrow();
if Self::is_intersection_left(
&intersection,
info.lane_id,
info.approach_id,
&self.position,
) {
info!("GLOSA: Intersection was left -> back to idle");
// ensure that "our" intersection is kept in TLC cache
if self
.tlc_cache
.keep_intersection(info.intersection_id, self.time)
.is_err()
{
info!("GLOSA: Intersection not known any more");
(Some(GlosaState::Idle), None)
} 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<_>>();
// determine if intersection is left
let intersection = info.intersection.borrow();
if Self::is_intersection_left(
&intersection,
info.lane_id,
info.approach_id,
&self.position,
) {
info!("GLOSA: Intersection was left -> back to idle");
(Some(GlosaState::Idle), None)
} 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
approach_sig_grps.sort_by_key(|v| v.maneuvers());
// sort signal groups by maneuver
approach_sig_grps.sort_by_key(|v| v.maneuvers());
// display signal phases
let sig_grps = approach_sig_grps
.iter()
.filter_map(|sig| {
if let Some(manv) = sig.maneuvers() {
use core::ops::Sub;
// display signal phases
let sig_grps = approach_sig_grps
.iter()
.filter_map(|sig| {
if let Some(manv) = sig.maneuvers() {
use core::ops::Sub;
use num_traits::Float;
use num_traits::Float;
let phase = sig
.phase()
.map(core::convert::Into::into)
.unwrap_or_default();
let phase = sig
.phase()
.map(core::convert::Into::into)
.unwrap_or_default();
let end_sec =
sig.likely_time()
let end_sec = sig
.likely_time()
.or_else(|| sig.min_end_time())
.map(|time| {
let dur =
@ -308,18 +317,19 @@ impl State {
end_sec
});
Some(SignalGroupData {
phase,
maneuver: manv,
end_sec,
})
} else {
None
}
})
.collect::<Vec<_>>();
Some(SignalGroupData {
phase,
maneuver: manv,
end_sec,
})
} else {
None
}
})
.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 super::v2x::map;
use crate::cache::Cache;
use crate::cache::{Cache, CacheError};
pub struct Data {
intersections: Cache<u16, map::Intersection>,
@ -86,6 +86,15 @@ impl Data {
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) {
for int in &etsi.spat.intersections.0 {
let int_id = int.id.id.0;