diff --git a/src/applogic/mod.rs b/src/applogic/mod.rs index 120864f..18d2726 100644 --- a/src/applogic/mod.rs +++ b/src/applogic/mod.rs @@ -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::>(); + // 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::>(); - // 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::>(); + Some(SignalGroupData { + phase, + maneuver: manv, + end_sec, + }) + } else { + None + } + }) + .collect::>(); - (None, Some(sig_grps)) // keep state, return signal groups + (None, Some(sig_grps)) // keep state, return signal groups + } } } }; diff --git a/src/applogic/tlc.rs b/src/applogic/tlc.rs index 96cbd8a..d8671c8 100644 --- a/src/applogic/tlc.rs +++ b/src/applogic/tlc.rs @@ -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, @@ -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> { + 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;