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