1use super::fusion::FusionError;
4
5#[derive(Clone, Copy, Debug, PartialEq, Eq)]
10pub struct FusionClockStamp {
11 clock_id: u64,
12 restart_epoch: u32,
13 ticks_ns: u64,
14 uncertainty_ns: u64,
15}
16
17impl FusionClockStamp {
18 pub const fn new(
19 clock_id: u64,
20 restart_epoch: u32,
21 ticks_ns: u64,
22 uncertainty_ns: u64,
23 ) -> Result<Self, FusionError> {
24 if clock_id == 0 {
25 return Err(FusionError::InvalidIdentifier);
26 }
27 if ticks_ns.checked_add(uncertainty_ns).is_none() {
28 return Err(FusionError::InvalidTime);
29 }
30 Ok(Self {
31 clock_id,
32 restart_epoch,
33 ticks_ns,
34 uncertainty_ns,
35 })
36 }
37}
38
39#[derive(Clone, Copy, Debug, PartialEq, Eq)]
42pub struct TimeAlignment {
43 alignment_id: u64,
44 source_clock_id: u64,
45 source_epoch: u32,
46 target_clock_id: u64,
47 target_epoch: u32,
48 valid_from_ns: u64,
49 valid_through_ns: u64,
50 offset_ns: i128,
51 uncertainty_ns: u64,
54}
55
56impl TimeAlignment {
57 #[allow(clippy::too_many_arguments)]
58 pub const fn new(
59 alignment_id: u64,
60 source_clock_id: u64,
61 source_epoch: u32,
62 target_clock_id: u64,
63 target_epoch: u32,
64 valid_from_ns: u64,
65 valid_through_ns: u64,
66 offset_ns: i128,
67 uncertainty_ns: u64,
68 ) -> Result<Self, FusionError> {
69 if alignment_id == 0 || source_clock_id == 0 || target_clock_id == 0 {
70 return Err(FusionError::InvalidIdentifier);
71 }
72 if valid_from_ns > valid_through_ns {
73 return Err(FusionError::InvalidTimeAlignment);
74 }
75 Ok(Self {
76 alignment_id,
77 source_clock_id,
78 source_epoch,
79 target_clock_id,
80 target_epoch,
81 valid_from_ns,
82 valid_through_ns,
83 offset_ns,
84 uncertainty_ns,
85 })
86 }
87}
88
89#[derive(Clone, Copy, Debug, PartialEq, Eq)]
91pub struct FusionTarget {
92 pub(super) clock_id: u64,
93 pub(super) restart_epoch: u32,
94 pub(super) coordinate_frame_id: u64,
95}
96
97impl FusionTarget {
98 pub const fn new(
99 clock_id: u64,
100 restart_epoch: u32,
101 coordinate_frame_id: u64,
102 ) -> Result<Self, FusionError> {
103 if clock_id == 0 || coordinate_frame_id == 0 {
104 return Err(FusionError::InvalidIdentifier);
105 }
106 Ok(Self {
107 clock_id,
108 restart_epoch,
109 coordinate_frame_id,
110 })
111 }
112}
113
114#[derive(Clone, Copy, Debug, PartialEq, Eq)]
116pub struct FusionTimeWindow {
117 pub(super) clock_id: u64,
118 pub(super) restart_epoch: u32,
119 pub(super) lower_ns: u64,
120 pub(super) upper_ns: u64,
121}
122
123impl FusionTimeWindow {
124 pub const fn clock_id(self) -> u64 {
125 self.clock_id
126 }
127
128 pub const fn restart_epoch(self) -> u32 {
129 self.restart_epoch
130 }
131
132 pub const fn lower_ns(self) -> u64 {
133 self.lower_ns
134 }
135
136 pub const fn upper_ns(self) -> u64 {
137 self.upper_ns
138 }
139}
140
141pub(super) fn align_time(
142 stamp: Option<FusionClockStamp>,
143 target: FusionTarget,
144 alignments: &[TimeAlignment],
145) -> Result<(FusionTimeWindow, Option<u64>), FusionError> {
146 let stamp = stamp.ok_or(FusionError::UnknownTime)?;
147 let matching_domain =
148 stamp.clock_id == target.clock_id && stamp.restart_epoch == target.restart_epoch;
149 let (center_ns, alignment_id, alignment_uncertainty) = if matching_domain {
150 (stamp.ticks_ns, None, 0)
151 } else {
152 let mut found = None;
153 for alignment in alignments {
154 if alignment.source_clock_id == stamp.clock_id
155 && alignment.source_epoch == stamp.restart_epoch
156 && alignment.target_clock_id == target.clock_id
157 && alignment.target_epoch == target.restart_epoch
158 {
159 if found.is_some() {
160 return Err(FusionError::AmbiguousTimeAlignment);
161 }
162 found = Some(*alignment);
163 }
164 }
165 let alignment = found.ok_or(FusionError::MissingTimeAlignment)?;
166 let source_lower = stamp.ticks_ns.saturating_sub(stamp.uncertainty_ns);
167 let source_upper = stamp
168 .ticks_ns
169 .checked_add(stamp.uncertainty_ns)
170 .ok_or(FusionError::InvalidTime)?;
171 if source_lower < alignment.valid_from_ns || source_upper > alignment.valid_through_ns {
172 return Err(FusionError::TimeAlignmentOutsideValidity);
173 }
174 let center = i128::from(stamp.ticks_ns)
175 .checked_add(alignment.offset_ns)
176 .ok_or(FusionError::InvalidTime)?;
177 if center < 0 || center > i128::from(u64::MAX) {
178 return Err(FusionError::InvalidTime);
179 }
180 (
181 center as u64,
182 Some(alignment.alignment_id),
183 alignment.uncertainty_ns,
184 )
185 };
186 let uncertainty_ns = stamp
187 .uncertainty_ns
188 .checked_add(alignment_uncertainty)
189 .ok_or(FusionError::InvalidTime)?;
190 let upper_ns = center_ns
191 .checked_add(uncertainty_ns)
192 .ok_or(FusionError::InvalidTime)?;
193 let lower_ns = center_ns.saturating_sub(uncertainty_ns);
194 Ok((
195 FusionTimeWindow {
196 clock_id: target.clock_id,
197 restart_epoch: target.restart_epoch,
198 lower_ns,
199 upper_ns,
200 },
201 alignment_id,
202 ))
203}