1use std::collections::HashMap;
2
3use autd3_driver::{
4 defined::{Freq, Hz},
5 ethercat::DcSysTime,
6 firmware::fpga::{LoopBehavior, Segment, TransitionMode},
7};
8
9use super::FPGAEmulator;
10
11const FPGA_MAIN_CLK_FREQ: u32 = 10240000;
12
13pub(crate) struct Swapchain<const SET: u16> {
14 sys_time: DcSysTime,
15 pub(crate) fpga_clk_freq: Freq<u32>,
16 rep: u16,
17 start_lap: HashMap<Segment, usize>,
18 freq_div: HashMap<Segment, u16>,
19 cycle: HashMap<Segment, usize>,
20 tic_idx_offset: HashMap<Segment, usize>,
21 cur_segment: Segment,
22 req_segment: Segment,
23 cur_idx: usize,
24 transition_mode: TransitionMode,
25 stop: bool,
26 ext_mode: bool,
27 ext_last_lap: usize,
28 state: State,
29}
30
31#[derive(Debug, Clone, Copy, PartialEq, Eq)]
32enum State {
33 WaitStart,
34 FiniteLoop,
35 InfiniteLoop,
36}
37
38impl<const SET: u16> Swapchain<SET> {
39 #[must_use]
40 pub fn new() -> Self {
41 Self {
42 sys_time: DcSysTime::now(),
43 fpga_clk_freq: FPGA_MAIN_CLK_FREQ * Hz,
44 rep: 0,
45 freq_div: [(Segment::S0, 10u16), (Segment::S1, 10u16)]
46 .into_iter()
47 .collect(),
48 cycle: [(Segment::S0, 1), (Segment::S1, 1)].into_iter().collect(),
49 tic_idx_offset: [(Segment::S0, 0), (Segment::S1, 0)].into_iter().collect(),
50 start_lap: [(Segment::S0, 0), (Segment::S1, 0)].into_iter().collect(),
51 cur_segment: Segment::S0,
52 req_segment: Segment::S0,
53 cur_idx: 0,
54 transition_mode: TransitionMode::Immediate,
55 stop: false,
56 ext_mode: false,
57 ext_last_lap: 0,
58 state: State::WaitStart,
59 }
60 }
61
62 pub fn init(&mut self) {
63 self.cur_segment = Segment::S0;
64 }
65
66 pub fn update(&mut self, gpio_in: [bool; 4], sys_time: DcSysTime) {
67 let (last_lap, _) = self.lap_and_idx(self.req_segment, self.sys_time);
68 let (lap, idx) = self.lap_and_idx(self.req_segment, sys_time);
69 match self.state {
70 State::WaitStart => match self.transition_mode {
71 TransitionMode::SyncIdx => {
72 if last_lap < lap {
73 self.stop = false;
74 self.start_lap.insert(self.req_segment, lap);
75 self.tic_idx_offset.insert(self.req_segment, 0);
76 self.cur_segment = self.req_segment;
77 self.state = State::FiniteLoop;
78 }
79 }
80 TransitionMode::SysTime(v) => {
81 if v.sys_time() <= sys_time.sys_time() {
82 self.stop = false;
83 self.start_lap.insert(self.req_segment, lap);
84 self.cur_segment = self.req_segment;
85 self.tic_idx_offset.insert(self.req_segment, idx);
86 self.state = State::FiniteLoop;
87 }
88 }
89 TransitionMode::GPIO(gpio) => {
90 if gpio_in[gpio as usize] {
91 self.stop = false;
92 self.start_lap.insert(self.req_segment, lap);
93 self.cur_segment = self.req_segment;
94 self.tic_idx_offset.insert(self.req_segment, idx);
95 self.state = State::FiniteLoop;
96 }
97 }
98 _ => unreachable!(),
99 },
100 State::FiniteLoop => {
101 if (self.start_lap[&self.cur_segment] + self.rep as usize) + 1 < lap {
102 self.stop = true;
103 }
104 if ((self.start_lap[&self.cur_segment] + self.rep as usize) < lap)
105 && (self.tic_idx_offset[&self.cur_segment] <= idx)
106 {
107 self.stop = true;
108 }
109 }
110 State::InfiniteLoop => {
111 if self.ext_mode && self.ext_last_lap < lap && self.ext_last_lap % 2 != lap % 2 {
112 self.ext_last_lap = lap;
113 self.cur_segment = match self.cur_segment {
114 Segment::S0 => Segment::S1,
115 Segment::S1 => Segment::S0,
116 };
117 }
118 }
119 }
120 let (_, idx) = self.lap_and_idx(self.cur_segment, sys_time);
121 if self.stop {
122 self.cur_idx = self.cycle[&self.cur_segment] - 1;
123 } else {
124 self.cur_idx = (idx + self.cycle[&self.cur_segment]
125 - self.tic_idx_offset[&self.cur_segment])
126 % self.cycle[&self.cur_segment];
127 }
128 }
129
130 pub fn set(
131 &mut self,
132 sys_time: DcSysTime,
133 rep: LoopBehavior,
134 freq_div: u16,
135 cycle: usize,
136 req_segment: Segment,
137 transition_mode: TransitionMode,
138 ) {
139 if self.cur_segment == req_segment {
140 self.stop = false;
141 self.ext_mode = transition_mode == TransitionMode::Ext;
142 let (lap, _) = self.lap_and_idx(req_segment, sys_time);
143 self.ext_last_lap = lap;
144 self.tic_idx_offset.insert(req_segment, 0);
145 self.state = State::InfiniteLoop;
146 } else if rep.rep() == 0xFFFF {
147 self.stop = false;
148 self.cur_segment = req_segment;
149 self.ext_mode = transition_mode == TransitionMode::Ext;
150 let (lap, _) = self.lap_and_idx(req_segment, sys_time);
151 self.ext_last_lap = lap;
152 self.tic_idx_offset.insert(req_segment, 0);
153 self.state = State::InfiniteLoop;
154 } else {
155 self.rep = rep.rep();
156 self.req_segment = req_segment;
157 self.state = State::WaitStart;
158 }
159 self.sys_time = sys_time;
160
161 self.freq_div.insert(req_segment, freq_div);
162 self.cycle.insert(req_segment, cycle);
163 self.transition_mode = transition_mode;
164 }
165
166 #[must_use]
167 const fn fpga_sys_time(&self, dc_sys_time: DcSysTime) -> u64 {
168 ((dc_sys_time.sys_time() as u128 * self.fpga_clk_freq.hz() as u128) / 1000000000) as _
169 }
170
171 #[must_use]
172 fn lap_and_idx(&self, segment: Segment, sys_time: DcSysTime) -> (usize, usize) {
173 let a = ((self.fpga_sys_time(sys_time) >> 8) / self.freq_div[&segment] as u64) as usize;
174 let b = self.cycle[&segment];
175 let lap = a / b;
176 let idx = a % b;
177 (lap, idx)
178 }
179}
180
181impl FPGAEmulator {
182 #[must_use]
183 pub const fn current_mod_segment(&self) -> Segment {
184 self.mod_swapchain.cur_segment
185 }
186
187 #[must_use]
188 pub const fn current_stm_segment(&self) -> Segment {
189 self.stm_swapchain.cur_segment
190 }
191
192 #[must_use]
193 pub const fn current_mod_idx(&self) -> usize {
194 self.mod_swapchain.cur_idx
195 }
196
197 #[must_use]
198 pub const fn current_stm_idx(&self) -> usize {
199 self.stm_swapchain.cur_idx
200 }
201}
202
203#[cfg(test)]
204mod tests {
205 use autd3_driver::firmware::fpga::GPIOIn;
206
207 use super::*;
208
209 const CYCLE_PERIOD: std::time::Duration = std::time::Duration::from_micros(25);
210 const FREQ_DIV: u16 = 1;
211
212 #[test]
213 fn transition_same_segment() {
214 let mut fpga = FPGAEmulator::new(249);
215
216 assert_eq!(Segment::S0, fpga.current_mod_segment());
217
218 let sys_time = DcSysTime::ZERO;
219 fpga.mod_swapchain.set(
220 sys_time,
221 LoopBehavior::ONCE,
222 FREQ_DIV,
223 1,
224 Segment::S0,
225 TransitionMode::Immediate,
226 );
227
228 assert!(!fpga.mod_swapchain.stop);
229 assert!(!fpga.mod_swapchain.ext_mode);
230 assert_eq!(Segment::S0, fpga.current_mod_segment());
231 let (lap, _) = fpga.mod_swapchain.lap_and_idx(Segment::S0, sys_time);
232 assert_eq!(lap, fpga.mod_swapchain.ext_last_lap);
233 assert_eq!(0, fpga.mod_swapchain.tic_idx_offset[&Segment::S0]);
234 assert_eq!(State::InfiniteLoop, fpga.mod_swapchain.state);
235 }
236
237 #[test]
238 fn transition_infinite() {
239 let mut fpga = FPGAEmulator::new(249);
240
241 assert_eq!(Segment::S0, fpga.current_mod_segment());
242
243 let sys_time = DcSysTime::ZERO;
244 fpga.mod_swapchain.set(
245 sys_time,
246 LoopBehavior::Infinite,
247 FREQ_DIV,
248 1,
249 Segment::S1,
250 TransitionMode::Ext,
251 );
252
253 assert!(!fpga.mod_swapchain.stop);
254 assert!(fpga.mod_swapchain.ext_mode);
255 assert_eq!(Segment::S1, fpga.current_mod_segment());
256 let (lap, _) = fpga.mod_swapchain.lap_and_idx(Segment::S0, sys_time);
257 assert_eq!(lap, fpga.mod_swapchain.ext_last_lap);
258 assert_eq!(0, fpga.mod_swapchain.rep);
259 assert_eq!(0, fpga.mod_swapchain.tic_idx_offset[&Segment::S1]);
260 assert_eq!(State::InfiniteLoop, fpga.mod_swapchain.state);
261 }
262
263 #[test]
264 fn transition_finite() {
265 let mut fpga = FPGAEmulator::new(249);
266
267 assert_eq!(Segment::S0, fpga.current_mod_segment());
268
269 let sys_time = DcSysTime::ZERO;
270 fpga.mod_swapchain.set(
271 sys_time,
272 LoopBehavior::ONCE,
273 FREQ_DIV,
274 1,
275 Segment::S1,
276 TransitionMode::SyncIdx,
277 );
278
279 assert!(!fpga.mod_swapchain.stop);
280 assert!(!fpga.mod_swapchain.ext_mode);
281 assert_eq!(Segment::S1, fpga.mod_swapchain.req_segment);
282 assert_eq!(Segment::S0, fpga.current_mod_segment());
283 assert_eq!(0, fpga.mod_swapchain.rep);
284 assert_eq!(0, fpga.mod_swapchain.tic_idx_offset[&Segment::S1]);
285 assert_eq!(State::WaitStart, fpga.mod_swapchain.state);
286 }
287
288 #[test]
289 fn transition_sync_idx() {
290 let mut fpga = FPGAEmulator::new(249);
291
292 assert_eq!(Segment::S0, fpga.current_mod_segment());
293
294 const CYCLE: u32 = 10;
295 let sys_time = DcSysTime::ZERO + CYCLE_PERIOD * 5;
296 fpga.mod_swapchain.set(
297 sys_time,
298 LoopBehavior::ONCE,
299 FREQ_DIV,
300 CYCLE as _,
301 Segment::S1,
302 TransitionMode::SyncIdx,
303 );
304
305 fpga.mod_swapchain.update(
306 [false; 4],
307 sys_time + CYCLE_PERIOD * 5 - std::time::Duration::from_nanos(1),
308 );
309 assert!(!fpga.mod_swapchain.stop);
310 assert_eq!(0, fpga.current_mod_idx());
311 assert_eq!(0, fpga.mod_swapchain.start_lap[&Segment::S1]);
312 assert_eq!(0, fpga.mod_swapchain.tic_idx_offset[&Segment::S1]);
313 assert_eq!(Segment::S1, fpga.mod_swapchain.req_segment);
314 assert_eq!(Segment::S0, fpga.current_mod_segment());
315 assert_eq!(State::WaitStart, fpga.mod_swapchain.state);
316
317 fpga.mod_swapchain
318 .update([false; 4], sys_time + CYCLE_PERIOD * 5);
319 assert!(!fpga.mod_swapchain.stop);
320 assert_eq!(0, fpga.current_mod_idx());
321 assert_eq!(Segment::S1, fpga.mod_swapchain.req_segment);
322 assert_eq!(Segment::S1, fpga.current_mod_segment());
323 assert_eq!(State::FiniteLoop, fpga.mod_swapchain.state);
324
325 fpga.mod_swapchain
326 .update([false; 4], sys_time + CYCLE_PERIOD * 5);
327 assert_eq!(0, fpga.current_mod_idx());
328 assert!(!fpga.mod_swapchain.stop);
329
330 fpga.mod_swapchain
331 .update([false; 4], sys_time + CYCLE_PERIOD * (5 + CYCLE - 1));
332 assert_eq!(9, fpga.current_mod_idx());
333 assert!(!fpga.mod_swapchain.stop);
334
335 fpga.mod_swapchain
336 .update([false; 4], sys_time + CYCLE_PERIOD * (5 + CYCLE));
337 assert_eq!(9, fpga.current_mod_idx());
338 assert!(fpga.mod_swapchain.stop);
339 }
340
341 #[test]
342 fn transition_sys_time() {
343 let mut fpga = FPGAEmulator::new(249);
344
345 assert_eq!(Segment::S0, fpga.current_stm_segment());
346
347 const CYCLE: u32 = 10;
348 let sys_time = DcSysTime::ZERO + CYCLE_PERIOD * 5;
349 fpga.stm_swapchain.set(
350 sys_time,
351 LoopBehavior::ONCE,
352 FREQ_DIV,
353 CYCLE as _,
354 Segment::S1,
355 TransitionMode::SysTime(sys_time + CYCLE_PERIOD * 5),
356 );
357
358 fpga.stm_swapchain.update(
359 [false; 4],
360 sys_time + CYCLE_PERIOD * 5 - std::time::Duration::from_nanos(1),
361 );
362 assert!(!fpga.stm_swapchain.stop);
363 assert_eq!(0, fpga.current_stm_idx());
364 assert_eq!(0, fpga.stm_swapchain.start_lap[&Segment::S1]);
365 assert_eq!(0, fpga.stm_swapchain.tic_idx_offset[&Segment::S1]);
366 assert_eq!(Segment::S1, fpga.stm_swapchain.req_segment);
367 assert_eq!(Segment::S0, fpga.current_stm_segment());
368 assert_eq!(State::WaitStart, fpga.stm_swapchain.state);
369
370 fpga.stm_swapchain
371 .update([false; 4], sys_time + CYCLE_PERIOD * 5);
372 assert!(!fpga.stm_swapchain.stop);
373 assert_eq!(0, fpga.current_stm_idx());
374 assert_eq!(0, fpga.stm_swapchain.tic_idx_offset[&Segment::S1]);
375 assert_eq!(Segment::S1, fpga.stm_swapchain.req_segment);
376 assert_eq!(Segment::S1, fpga.current_stm_segment());
377 assert_eq!(State::FiniteLoop, fpga.stm_swapchain.state);
378
379 fpga.stm_swapchain
380 .update([false; 4], sys_time + CYCLE_PERIOD * 5);
381 assert_eq!(0, fpga.current_stm_idx());
382 assert!(!fpga.stm_swapchain.stop);
383
384 fpga.stm_swapchain
385 .update([false; 4], sys_time + CYCLE_PERIOD * (5 + CYCLE - 1));
386 assert_eq!(9, fpga.current_stm_idx());
387 assert!(!fpga.stm_swapchain.stop);
388
389 fpga.stm_swapchain
390 .update([false; 4], sys_time + CYCLE_PERIOD * (5 + CYCLE));
391 assert_eq!(9, fpga.current_stm_idx());
392 assert!(fpga.stm_swapchain.stop);
393 }
394
395 #[test]
396 fn transition_gpio() {
397 let mut fpga = FPGAEmulator::new(249);
398
399 assert_eq!(Segment::S0, fpga.current_mod_segment());
400
401 const CYCLE: u32 = 10;
402 let sys_time = DcSysTime::ZERO + CYCLE_PERIOD * 5;
403 fpga.mod_swapchain.set(
404 sys_time,
405 LoopBehavior::ONCE,
406 FREQ_DIV,
407 CYCLE as _,
408 Segment::S1,
409 TransitionMode::GPIO(GPIOIn::I0),
410 );
411
412 fpga.mod_swapchain.update(
413 [false; 4],
414 sys_time + CYCLE_PERIOD * 5 - std::time::Duration::from_nanos(1),
415 );
416 assert!(!fpga.mod_swapchain.stop);
417 assert_eq!(0, fpga.current_mod_idx());
418 assert_eq!(0, fpga.mod_swapchain.start_lap[&Segment::S1]);
419 assert_eq!(0, fpga.mod_swapchain.tic_idx_offset[&Segment::S1]);
420 assert_eq!(Segment::S1, fpga.mod_swapchain.req_segment);
421 assert_eq!(Segment::S0, fpga.current_mod_segment());
422 assert_eq!(State::WaitStart, fpga.mod_swapchain.state);
423
424 fpga.mod_swapchain
425 .update([true, false, false, false], sys_time + CYCLE_PERIOD * 10);
426 assert!(!fpga.mod_swapchain.stop);
427 assert_eq!(0, fpga.current_mod_idx());
428 assert_eq!(5, fpga.mod_swapchain.tic_idx_offset[&Segment::S1]);
429 assert_eq!(Segment::S1, fpga.mod_swapchain.req_segment);
430 assert_eq!(Segment::S1, fpga.current_mod_segment());
431 assert_eq!(State::FiniteLoop, fpga.mod_swapchain.state);
432
433 fpga.mod_swapchain
434 .update([false; 4], sys_time + CYCLE_PERIOD * 10);
435 assert_eq!(0, fpga.current_mod_idx());
436 assert!(!fpga.mod_swapchain.stop);
437
438 fpga.mod_swapchain
439 .update([false; 4], sys_time + CYCLE_PERIOD * 11);
440 assert_eq!(1, fpga.current_mod_idx());
441 assert!(!fpga.mod_swapchain.stop);
442
443 fpga.mod_swapchain
444 .update([false; 4], sys_time + CYCLE_PERIOD * 19);
445 assert_eq!(9, fpga.current_mod_idx());
446 assert!(!fpga.mod_swapchain.stop);
447
448 fpga.mod_swapchain
449 .update([false; 4], sys_time + CYCLE_PERIOD * 20);
450 assert_eq!(9, fpga.current_mod_idx());
451 assert!(fpga.mod_swapchain.stop);
452 }
453
454 #[test]
455 fn transition_gpio_over() {
456 let mut fpga = FPGAEmulator::new(249);
457
458 assert_eq!(Segment::S0, fpga.current_mod_segment());
459
460 const CYCLE: u32 = 10;
461 let sys_time = DcSysTime::ZERO + CYCLE_PERIOD * 5;
462 fpga.mod_swapchain.set(
463 sys_time,
464 LoopBehavior::ONCE,
465 FREQ_DIV,
466 CYCLE as _,
467 Segment::S1,
468 TransitionMode::GPIO(GPIOIn::I0),
469 );
470
471 fpga.mod_swapchain.update(
472 [false; 4],
473 sys_time + CYCLE_PERIOD * 5 - std::time::Duration::from_nanos(1),
474 );
475 assert!(!fpga.mod_swapchain.stop);
476 assert_eq!(0, fpga.current_mod_idx());
477 assert_eq!(0, fpga.mod_swapchain.start_lap[&Segment::S1]);
478 assert_eq!(0, fpga.mod_swapchain.tic_idx_offset[&Segment::S1]);
479 assert_eq!(Segment::S1, fpga.mod_swapchain.req_segment);
480 assert_eq!(Segment::S0, fpga.current_mod_segment());
481 assert_eq!(State::WaitStart, fpga.mod_swapchain.state);
482
483 fpga.mod_swapchain
484 .update([true, false, false, false], sys_time + CYCLE_PERIOD * 10);
485 assert!(!fpga.mod_swapchain.stop);
486 assert_eq!(0, fpga.current_mod_idx());
487 assert_eq!(5, fpga.mod_swapchain.tic_idx_offset[&Segment::S1]);
488 assert_eq!(Segment::S1, fpga.mod_swapchain.req_segment);
489 assert_eq!(Segment::S1, fpga.current_mod_segment());
490 assert_eq!(State::FiniteLoop, fpga.mod_swapchain.state);
491
492 fpga.mod_swapchain
493 .update([false; 4], sys_time + CYCLE_PERIOD * 30);
494 assert_eq!(9, fpga.current_mod_idx());
495 assert!(fpga.mod_swapchain.stop);
496 }
497
498 #[test]
499 fn transition_ext() {
500 let mut fpga = FPGAEmulator::new(249);
501
502 assert_eq!(Segment::S0, fpga.current_mod_segment());
503
504 const CYCLE: u32 = 10;
505 let sys_time = DcSysTime::ZERO;
506 fpga.mod_swapchain.set(
507 sys_time,
508 LoopBehavior::Infinite,
509 FREQ_DIV,
510 CYCLE as _,
511 Segment::S0,
512 TransitionMode::Ext,
513 );
514 fpga.mod_swapchain.set(
515 sys_time,
516 LoopBehavior::Infinite,
517 FREQ_DIV,
518 CYCLE as _,
519 Segment::S1,
520 TransitionMode::Ext,
521 );
522
523 fpga.mod_swapchain
524 .update([false; 4], sys_time + CYCLE_PERIOD * 5);
525 assert!(!fpga.mod_swapchain.stop);
526 assert_eq!(5, fpga.current_mod_idx());
527 assert_eq!(Segment::S1, fpga.current_mod_segment());
528
529 fpga.mod_swapchain
530 .update([false; 4], sys_time + CYCLE_PERIOD * 9);
531 assert!(!fpga.mod_swapchain.stop);
532 assert_eq!(9, fpga.current_mod_idx());
533 assert_eq!(Segment::S1, fpga.current_mod_segment());
534
535 fpga.mod_swapchain
536 .update([false; 4], sys_time + CYCLE_PERIOD * 10);
537 assert!(!fpga.mod_swapchain.stop);
538 assert_eq!(0, fpga.current_mod_idx());
539 assert_eq!(Segment::S0, fpga.current_mod_segment());
540
541 fpga.mod_swapchain
542 .update([false; 4], sys_time + CYCLE_PERIOD * 19);
543 assert!(!fpga.mod_swapchain.stop);
544 assert_eq!(9, fpga.current_mod_idx());
545 assert_eq!(Segment::S0, fpga.current_mod_segment());
546
547 fpga.mod_swapchain
548 .update([false; 4], sys_time + CYCLE_PERIOD * 20);
549 assert!(!fpga.mod_swapchain.stop);
550 assert_eq!(0, fpga.current_mod_idx());
551 assert_eq!(Segment::S1, fpga.current_mod_segment());
552 }
553}