1#![allow(clippy::type_complexity)]
2
3use std::mem::size_of;
4
5use crate::{
6 error::AUTDDriverError,
7 firmware::{
8 cpu::GainSTMMode,
9 fpga::{
10 Drive, GAIN_STM_BUF_SIZE_MAX, LoopBehavior, STM_BUF_SIZE_MIN, SamplingConfig, Segment,
11 TRANSITION_MODE_NONE, TransitionMode,
12 },
13 operation::{Operation, TypeTag, write_to_tx},
14 },
15 geometry::Device,
16};
17
18use autd3_core::gain::GainCalculator;
19use zerocopy::{FromBytes, Immutable, IntoBytes, KnownLayout};
20
21#[derive(Clone, Copy, IntoBytes, Immutable)]
22#[repr(C)]
23pub struct GainSTMControlFlags(u8);
24
25bitflags::bitflags! {
26 impl GainSTMControlFlags : u8 {
27 const NONE = 0;
28 const BEGIN = 1 << 0;
29 const END = 1 << 1;
30 const TRANSITION = 1 << 2;
31 const SEGMENT = 1 << 3;
32 const SEND_BIT0 = 1 << 6;
33 const SEND_BIT1 = 1 << 7;
34 }
35}
36
37#[derive(IntoBytes, Immutable, FromBytes, KnownLayout)]
38struct PhaseFull {
39 phase_0: u8,
40 phase_1: u8,
41}
42
43#[bitfield_struct::bitfield(u16)]
44#[derive(IntoBytes, Immutable, FromBytes, KnownLayout)]
45struct PhaseHalf {
46 #[bits(4)]
47 phase_0: u8,
48 #[bits(4)]
49 phase_1: u8,
50 #[bits(4)]
51 phase_2: u8,
52 #[bits(4)]
53 phase_3: u8,
54}
55
56#[repr(C, align(2))]
57#[derive(IntoBytes, Immutable)]
58struct GainSTMHead {
59 tag: TypeTag,
60 flag: GainSTMControlFlags,
61 mode: GainSTMMode,
62 transition_mode: u8,
63 freq_div: u16,
64 rep: u16,
65 transition_value: u64,
66}
67
68#[repr(C, align(2))]
69#[derive(IntoBytes, Immutable)]
70struct GainSTMSubseq {
71 tag: TypeTag,
72 flag: GainSTMControlFlags,
73}
74
75pub trait GainSTMIterator: Send + Sync {
79 type Calculator: GainCalculator;
81
82 fn next(&mut self) -> Option<Self::Calculator>;
84}
85
86pub struct GainSTMOp<G: GainCalculator, Iterator: GainSTMIterator<Calculator = G>> {
87 iter: Iterator,
88 size: usize,
89 sent: usize,
90 mode: GainSTMMode,
91 config: SamplingConfig,
92 loop_behavior: LoopBehavior,
93 segment: Segment,
94 transition_mode: Option<TransitionMode>,
95}
96
97impl<G: GainCalculator, Iterator: GainSTMIterator<Calculator = G>> GainSTMOp<G, Iterator> {
98 pub(crate) const fn new(
99 iter: Iterator,
100 size: usize,
101 mode: GainSTMMode,
102 config: SamplingConfig,
103 loop_behavior: LoopBehavior,
104 segment: Segment,
105 transition_mode: Option<TransitionMode>,
106 ) -> Self {
107 Self {
108 iter,
109 size,
110 sent: 0,
111 mode,
112 config,
113 loop_behavior,
114 segment,
115 transition_mode,
116 }
117 }
118}
119
120impl<G: GainCalculator, Iterator: GainSTMIterator<Calculator = G>> Operation
121 for GainSTMOp<G, Iterator>
122{
123 type Error = AUTDDriverError;
124
125 fn required_size(&self, device: &Device) -> usize {
126 if self.sent == 0 {
127 size_of::<GainSTMHead>() + device.num_transducers() * size_of::<Drive>()
128 } else {
129 size_of::<GainSTMSubseq>() + device.num_transducers() * size_of::<Drive>()
130 }
131 }
132
133 fn pack(&mut self, device: &Device, tx: &mut [u8]) -> Result<usize, AUTDDriverError> {
134 if !(STM_BUF_SIZE_MIN..=GAIN_STM_BUF_SIZE_MAX).contains(&self.size) {
135 return Err(AUTDDriverError::GainSTMSizeOutOfRange(self.size));
136 }
137
138 let is_first = self.sent == 0;
139
140 let send = {
141 let offset = if is_first {
142 size_of::<GainSTMHead>()
143 } else {
144 size_of::<GainSTMSubseq>()
145 };
146
147 let mut send = 0;
148 match self.mode {
149 GainSTMMode::PhaseIntensityFull => {
150 if let Some(g) = self.iter.next() {
151 tx[offset..]
152 .chunks_mut(size_of::<Drive>())
153 .zip(device.iter())
154 .for_each(|(dst, tr)| {
155 write_to_tx(dst, g.calc(tr));
156 });
157 send += 1;
158 }
159 }
160 GainSTMMode::PhaseFull => {
161 seq_macro::seq!(N in 0..2 {
162 #(
163 if let Some(g) = self.iter.next() {
164 tx[offset..].chunks_exact_mut(size_of::<PhaseFull>()).zip(device.iter()).for_each(|(dst, tr)| {
165 PhaseFull::mut_from_bytes(dst).unwrap().phase_~N = g.calc(tr).phase.0;
166 });
167 send += 1;
168 }
169 )*
170 });
171 }
172 GainSTMMode::PhaseHalf => {
173 seq_macro::seq!(N in 0..4 {
174 #(
175 if let Some(g) = self.iter.next() {
176 tx[offset..].chunks_exact_mut(size_of::<PhaseHalf>()).zip(device.iter()).for_each(|(dst, tr)| {
177 PhaseHalf::mut_from_bytes(dst).unwrap().set_phase_~N(g.calc(tr).phase.0 >> 4);
178 });
179 send += 1;
180 }
181 )*
182 });
183 }
184 }
185 send
186 };
187
188 self.sent += send;
189
190 let mut flag = if self.sent == self.size {
191 GainSTMControlFlags::END
192 | if self.transition_mode.is_some() {
193 GainSTMControlFlags::TRANSITION
194 } else {
195 GainSTMControlFlags::NONE
196 }
197 } else {
198 GainSTMControlFlags::NONE
199 };
200
201 flag.set(GainSTMControlFlags::SEGMENT, self.segment == Segment::S1);
202 flag.set(
203 GainSTMControlFlags::SEND_BIT0,
204 ((send as u8 - 1) & 0x01) != 0,
205 );
206 flag.set(
207 GainSTMControlFlags::SEND_BIT1,
208 ((send as u8 - 1) & 0x02) != 0,
209 );
210
211 if is_first {
212 write_to_tx(
213 tx,
214 GainSTMHead {
215 tag: TypeTag::GainSTM,
216 flag: GainSTMControlFlags::BEGIN | flag,
217 mode: self.mode,
218 transition_mode: self
219 .transition_mode
220 .map(|m| m.mode())
221 .unwrap_or(TRANSITION_MODE_NONE),
222 transition_value: self.transition_mode.map(TransitionMode::value).unwrap_or(0),
223 freq_div: self.config.division()?,
224 rep: self.loop_behavior.rep(),
225 },
226 );
227 } else {
228 write_to_tx(
229 tx,
230 GainSTMSubseq {
231 tag: TypeTag::GainSTM,
232 flag,
233 },
234 );
235 }
236
237 if is_first {
238 Ok(size_of::<GainSTMHead>() + device.num_transducers() * size_of::<Drive>())
239 } else {
240 Ok(size_of::<GainSTMSubseq>() + device.num_transducers() * size_of::<Drive>())
241 }
242 }
243
244 fn is_done(&self) -> bool {
245 self.sent == self.size
246 }
247}
248
249#[cfg(test)]
250mod tests {
251 use std::{collections::VecDeque, mem::offset_of, num::NonZeroU16};
252
253 use rand::prelude::*;
254 use zerocopy::FromZeros;
255
256 use super::*;
257 use crate::{
258 ethercat::DcSysTime,
259 firmware::{
260 cpu::TxMessage,
261 fpga::{EmitIntensity, Phase},
262 operation::tests::create_device,
263 },
264 geometry::Transducer,
265 };
266
267 const NUM_TRANS_IN_UNIT: usize = 249;
268
269 struct Impl {
270 g: Vec<Drive>,
271 }
272
273 impl GainCalculator for Impl {
274 fn calc(&self, tr: &Transducer) -> Drive {
275 self.g[tr.idx()]
276 }
277 }
278
279 struct STMIterator {
280 data: VecDeque<Vec<Drive>>,
281 }
282
283 impl GainSTMIterator for STMIterator {
284 type Calculator = Impl;
285
286 fn next(&mut self) -> Option<Impl> {
287 self.data.pop_front().map(|g| Impl { g })
288 }
289 }
290
291 #[test]
292 fn test_phase_intensity_full() {
293 const GAIN_STM_SIZE: usize = 3;
294 const FRAME_SIZE: usize = size_of::<GainSTMHead>() + NUM_TRANS_IN_UNIT * 2;
295
296 let device = create_device(NUM_TRANS_IN_UNIT as _);
297
298 let mut tx = vec![0x00u8; FRAME_SIZE];
299
300 let mut rng = rand::rng();
301
302 let gain_data: VecDeque<Vec<Drive>> = (0..GAIN_STM_SIZE)
303 .map(|_| {
304 (0..NUM_TRANS_IN_UNIT)
305 .map(|_| Drive {
306 phase: Phase(rng.random_range(0x00..=0xFF)),
307 intensity: EmitIntensity(rng.random_range(0..=0xFF)),
308 })
309 .collect()
310 })
311 .collect();
312
313 let freq_div = rng.random_range(0x0001..=0xFFFF);
314 let rep = rng.random_range(0x0001..0xFFFF);
315 let segment = Segment::S0;
316 let transition_value = 0x0123456789ABCDEF;
317 let transition_mode = TransitionMode::SysTime(
318 DcSysTime::from_utc(
319 time::macros::datetime!(2000-01-01 0:00 UTC)
320 + std::time::Duration::from_nanos(transition_value),
321 )
322 .unwrap(),
323 );
324
325 let mut op = GainSTMOp::new(
326 {
327 STMIterator {
328 data: gain_data.clone(),
329 }
330 },
331 GAIN_STM_SIZE,
332 GainSTMMode::PhaseIntensityFull,
333 SamplingConfig::new(NonZeroU16::new(freq_div).unwrap()),
334 LoopBehavior::Finite(NonZeroU16::new(rep + 1).unwrap()),
335 segment,
336 Some(transition_mode),
337 );
338
339 {
341 assert_eq!(
342 op.required_size(&device),
343 size_of::<GainSTMHead>() + NUM_TRANS_IN_UNIT * 2
344 );
345
346 assert_eq!(op.sent, 0);
347
348 assert_eq!(
349 op.pack(&device, &mut tx),
350 Ok(size_of::<GainSTMHead>() + NUM_TRANS_IN_UNIT * 2)
351 );
352
353 assert_eq!(op.sent, 1);
354
355 assert_eq!(TypeTag::GainSTM as u8, tx[0]);
356 assert_eq!(GainSTMControlFlags::BEGIN.bits(), tx[1] & 0x3F);
357 assert_eq!(0, tx[1] >> 6);
358 assert_eq!(GainSTMMode::PhaseIntensityFull as u8, tx[2]);
359 assert_eq!(transition_mode.mode(), tx[3]);
360 assert_eq!(freq_div as u8, tx[4]);
361 assert_eq!((freq_div >> 8) as u8, tx[5]);
362 assert_eq!(rep as u8, tx[6]);
363 assert_eq!((rep >> 8) as u8, tx[7]);
364 assert_eq!(transition_value as u8, tx[8]);
365 assert_eq!((transition_value >> 8) as u8, tx[9]);
366 assert_eq!((transition_value >> 16) as u8, tx[10]);
367 assert_eq!((transition_value >> 24) as u8, tx[11]);
368 assert_eq!((transition_value >> 32) as u8, tx[12]);
369 assert_eq!((transition_value >> 40) as u8, tx[13]);
370 assert_eq!((transition_value >> 48) as u8, tx[14]);
371 assert_eq!((transition_value >> 56) as u8, tx[15]);
372 tx[size_of::<GainSTMHead>()..]
373 .chunks(size_of::<Drive>())
374 .zip(gain_data[0].iter())
375 .for_each(|(d, g)| {
376 assert_eq!(d[0], g.phase.0);
377 assert_eq!(d[1], g.intensity.0);
378 })
379 }
380
381 {
383 assert_eq!(
384 op.required_size(&device),
385 size_of::<GainSTMSubseq>() + NUM_TRANS_IN_UNIT * 2
386 );
387
388 assert_eq!(
389 op.pack(&device, &mut tx),
390 Ok(size_of::<GainSTMSubseq>() + NUM_TRANS_IN_UNIT * 2)
391 );
392
393 assert_eq!(op.sent, 2);
394
395 assert_eq!(TypeTag::GainSTM as u8, tx[0]);
396 assert_eq!(
397 GainSTMControlFlags::NONE.bits(),
398 tx[offset_of!(GainSTMHead, flag)] & 0x3F
399 );
400 assert_eq!(0, tx[offset_of!(GainSTMHead, flag)] >> 6);
401
402 tx[size_of::<GainSTMSubseq>()..]
403 .chunks(size_of::<Drive>())
404 .zip(gain_data[1].iter())
405 .for_each(|(d, g)| {
406 assert_eq!(d[0], g.phase.0);
407 assert_eq!(d[1], g.intensity.0);
408 })
409 }
410
411 {
413 assert_eq!(
414 op.required_size(&device),
415 size_of::<GainSTMSubseq>() + NUM_TRANS_IN_UNIT * 2
416 );
417
418 assert_eq!(
419 op.pack(&device, &mut tx),
420 Ok(size_of::<GainSTMSubseq>() + NUM_TRANS_IN_UNIT * 2)
421 );
422
423 assert_eq!(op.sent, GAIN_STM_SIZE);
424
425 assert_eq!(TypeTag::GainSTM as u8, tx[0]);
426 assert_eq!(
427 (GainSTMControlFlags::END | GainSTMControlFlags::TRANSITION).bits(),
428 tx[offset_of!(GainSTMHead, flag)] & 0x3F
429 );
430 assert_eq!(0, tx[offset_of!(GainSTMHead, flag)] >> 6);
431 tx[size_of::<GainSTMSubseq>()..]
432 .chunks(size_of::<Drive>())
433 .zip(gain_data[2].iter())
434 .for_each(|(d, g)| {
435 assert_eq!(d[0], g.phase.0);
436 assert_eq!(d[1], g.intensity.0);
437 })
438 }
439 }
440
441 #[test]
442 fn test_phase_full() {
443 const GAIN_STM_SIZE: usize = 5;
444 const FRAME_SIZE: usize = size_of::<GainSTMHead>() + NUM_TRANS_IN_UNIT * 2;
445
446 let device = create_device(NUM_TRANS_IN_UNIT as _);
447
448 let mut tx = vec![0x00u8; FRAME_SIZE];
449
450 let mut rng = rand::rng();
451
452 let gain_data: VecDeque<Vec<Drive>> = (0..GAIN_STM_SIZE)
453 .map(|_| {
454 (0..NUM_TRANS_IN_UNIT)
455 .map(|_| Drive {
456 phase: Phase(rng.random_range(0x00..=0xFF)),
457 intensity: EmitIntensity(rng.random_range(0..=0xFF)),
458 })
459 .collect()
460 })
461 .collect();
462
463 let freq_div = rng.random_range(0x0001..=0xFFFF);
464 let rep = rng.random_range(0x0001..=0xFFFF);
465 let segment = Segment::S1;
466 let mut op = GainSTMOp::new(
467 STMIterator {
468 data: gain_data.clone(),
469 },
470 GAIN_STM_SIZE,
471 GainSTMMode::PhaseFull,
472 SamplingConfig::new(NonZeroU16::new(freq_div).unwrap()),
473 LoopBehavior::Finite(NonZeroU16::new(rep).unwrap()),
474 segment,
475 None,
476 );
477
478 assert_eq!(op.sent, 0);
479
480 {
482 assert_eq!(
483 op.required_size(&device),
484 size_of::<GainSTMHead>() + NUM_TRANS_IN_UNIT * 2
485 );
486
487 assert_eq!(
488 op.pack(&device, &mut tx),
489 Ok(size_of::<GainSTMHead>() + NUM_TRANS_IN_UNIT * 2)
490 );
491
492 assert_eq!(op.sent, 2);
493
494 assert_eq!(TypeTag::GainSTM as u8, tx[0]);
495 assert_eq!(
496 (GainSTMControlFlags::BEGIN | GainSTMControlFlags::SEGMENT).bits(),
497 tx[offset_of!(GainSTMHead, flag)] & 0x3F
498 );
499 assert_eq!(1, tx[offset_of!(GainSTMHead, flag)] >> 6);
500
501 assert_eq!(
502 GainSTMMode::PhaseFull as u8,
503 tx[offset_of!(GainSTMHead, mode)]
504 );
505 tx[size_of::<GainSTMHead>()..]
506 .chunks(size_of::<Drive>())
507 .zip(gain_data[0].iter())
508 .zip(gain_data[1].iter())
509 .for_each(|((d, g0), g1)| {
510 assert_eq!(d[0], g0.phase.0);
511 assert_eq!(d[1], g1.phase.0);
512 });
513 }
514
515 {
517 assert_eq!(
518 op.required_size(&device),
519 size_of::<GainSTMSubseq>() + NUM_TRANS_IN_UNIT * 2
520 );
521
522 assert_eq!(
523 op.pack(&device, &mut tx),
524 Ok(size_of::<GainSTMSubseq>() + NUM_TRANS_IN_UNIT * 2)
525 );
526
527 assert_eq!(op.sent, 4);
528
529 assert_eq!(TypeTag::GainSTM as u8, tx[0]);
530 assert_eq!(
531 GainSTMControlFlags::SEGMENT.bits(),
532 tx[offset_of!(GainSTMHead, flag)] & 0x3F
533 );
534 assert_eq!(1, tx[offset_of!(GainSTMHead, flag)] >> 6);
535 tx[size_of::<GainSTMSubseq>()..]
536 .chunks(size_of::<Drive>())
537 .zip(gain_data[2].iter())
538 .zip(gain_data[3].iter())
539 .for_each(|((d, g0), g1)| {
540 assert_eq!(d[0], g0.phase.0);
541 assert_eq!(d[1], g1.phase.0);
542 });
543 }
544
545 {
547 assert_eq!(
548 op.required_size(&device),
549 size_of::<GainSTMSubseq>() + NUM_TRANS_IN_UNIT * 2
550 );
551
552 assert_eq!(
553 op.pack(&device, &mut tx),
554 Ok(size_of::<GainSTMSubseq>() + NUM_TRANS_IN_UNIT * 2)
555 );
556
557 assert_eq!(op.sent, GAIN_STM_SIZE);
558
559 assert_eq!(TypeTag::GainSTM as u8, tx[0]);
560 assert_eq!(
561 (GainSTMControlFlags::END | GainSTMControlFlags::SEGMENT).bits(),
562 tx[offset_of!(GainSTMHead, flag)] & 0x3F
563 );
564 assert_eq!(0, tx[offset_of!(GainSTMHead, flag)] >> 6);
565 tx[size_of::<GainSTMSubseq>()..]
566 .chunks(size_of::<Drive>())
567 .zip(gain_data[4].iter())
568 .for_each(|(d, g)| {
569 assert_eq!(d[0], g.phase.0);
570 })
571 }
572 }
573
574 #[test]
575 fn test_phase_half() {
576 const GAIN_STM_SIZE: usize = 9;
577
578 let device = create_device(NUM_TRANS_IN_UNIT as _);
579
580 let mut tx = vec![TxMessage::new_zeroed(); 1];
581 let tx = tx[0].payload_mut();
582
583 let mut rng = rand::rng();
584
585 let gain_data: VecDeque<Vec<Drive>> = (0..GAIN_STM_SIZE)
586 .map(|_| {
587 (0..NUM_TRANS_IN_UNIT)
588 .map(|_| Drive {
589 phase: Phase(rng.random_range(0x00..=0xFF)),
590 intensity: EmitIntensity(rng.random_range(0..=0xFF)),
591 })
592 .collect()
593 })
594 .collect();
595
596 let freq_div = rng.random_range(0x0001..=0xFFFF);
597 let rep = rng.random_range(0x0001..=0xFFFF);
598 let segment = Segment::S0;
599 let mut op = GainSTMOp::new(
600 STMIterator {
601 data: gain_data.clone(),
602 },
603 GAIN_STM_SIZE,
604 GainSTMMode::PhaseHalf,
605 SamplingConfig::new(NonZeroU16::new(freq_div).unwrap()),
606 LoopBehavior::Finite(NonZeroU16::new(rep).unwrap()),
607 segment,
608 Some(TransitionMode::SyncIdx),
609 );
610
611 assert_eq!(op.sent, 0);
612
613 {
615 assert_eq!(
616 op.required_size(&device),
617 size_of::<GainSTMHead>() + NUM_TRANS_IN_UNIT * 2
618 );
619
620 assert_eq!(
621 op.pack(&device, tx),
622 Ok(size_of::<GainSTMHead>() + NUM_TRANS_IN_UNIT * 2)
623 );
624
625 assert_eq!(op.sent, 4);
626
627 assert_eq!(TypeTag::GainSTM as u8, tx[0]);
628 assert_eq!(
629 GainSTMControlFlags::BEGIN.bits(),
630 tx[offset_of!(GainSTMHead, flag)] & 0x3F
631 );
632 assert_eq!(3, tx[offset_of!(GainSTMHead, flag)] >> 6);
633 assert_eq!(
634 GainSTMMode::PhaseHalf as u8,
635 tx[offset_of!(GainSTMHead, mode)]
636 );
637
638 tx[size_of::<GainSTMHead>()..]
639 .chunks(size_of::<Drive>())
640 .zip(gain_data[0].iter())
641 .zip(gain_data[1].iter())
642 .zip(gain_data[2].iter())
643 .zip(gain_data[3].iter())
644 .for_each(|((((d, g0), g1), g2), g3)| {
645 assert_eq!(d[0] & 0x0F, g0.phase.0 >> 4);
646 assert_eq!(d[0] >> 4, g1.phase.0 >> 4);
647 assert_eq!(d[1] & 0x0F, g2.phase.0 >> 4);
648 assert_eq!(d[1] >> 4, g3.phase.0 >> 4);
649 });
650 }
651
652 {
654 assert_eq!(
655 op.required_size(&device),
656 size_of::<GainSTMSubseq>() + NUM_TRANS_IN_UNIT * 2
657 );
658
659 assert_eq!(
660 op.pack(&device, tx),
661 Ok(size_of::<GainSTMSubseq>() + NUM_TRANS_IN_UNIT * 2)
662 );
663
664 assert_eq!(op.sent, 8);
665
666 assert_eq!(TypeTag::GainSTM as u8, tx[0]);
667 assert_eq!(
668 GainSTMControlFlags::NONE.bits(),
669 tx[offset_of!(GainSTMHead, flag)] & 0x3F
670 );
671 assert_eq!(3, tx[offset_of!(GainSTMHead, flag)] >> 6);
672 tx[size_of::<GainSTMSubseq>()..]
673 .chunks(size_of::<Drive>())
674 .zip(gain_data[4].iter())
675 .zip(gain_data[5].iter())
676 .zip(gain_data[6].iter())
677 .zip(gain_data[7].iter())
678 .for_each(|((((d, g0), g1), g2), g3)| {
679 assert_eq!(d[0] & 0x0F, g0.phase.0 >> 4);
680 assert_eq!(d[0] >> 4, g1.phase.0 >> 4);
681 assert_eq!(d[1] & 0x0F, g2.phase.0 >> 4);
682 assert_eq!(d[1] >> 4, g3.phase.0 >> 4);
683 });
684 }
685
686 {
688 assert_eq!(
689 op.required_size(&device),
690 size_of::<GainSTMSubseq>() + NUM_TRANS_IN_UNIT * 2
691 );
692
693 assert_eq!(
694 op.pack(&device, tx),
695 Ok(size_of::<GainSTMSubseq>() + NUM_TRANS_IN_UNIT * 2)
696 );
697
698 assert_eq!(op.sent, GAIN_STM_SIZE);
699
700 assert_eq!(TypeTag::GainSTM as u8, tx[0]);
701 assert_eq!(
702 (GainSTMControlFlags::END | GainSTMControlFlags::TRANSITION).bits(),
703 tx[offset_of!(GainSTMHead, flag)] & 0x3F
704 );
705 assert_eq!(0, tx[offset_of!(GainSTMHead, flag)] >> 6);
706 tx[size_of::<GainSTMSubseq>()..]
707 .chunks(size_of::<Drive>())
708 .zip(gain_data[8].iter())
709 .for_each(|(d, g)| {
710 assert_eq!(d[0] & 0x0F, g.phase.0 >> 4);
711 });
712 }
713 }
714
715 #[rstest::rstest]
716 #[test]
717 #[case(Err(AUTDDriverError::GainSTMSizeOutOfRange(0)), 0)]
718 #[case(Err(AUTDDriverError::GainSTMSizeOutOfRange(STM_BUF_SIZE_MIN-1)), STM_BUF_SIZE_MIN-1)]
719 #[case(Ok(()), STM_BUF_SIZE_MIN)]
720 #[case(Ok(()), GAIN_STM_BUF_SIZE_MAX)]
721 #[case(
722 Err(AUTDDriverError::GainSTMSizeOutOfRange(GAIN_STM_BUF_SIZE_MAX+1)),
723 GAIN_STM_BUF_SIZE_MAX+1
724 )]
725 fn out_of_range(#[case] expected: Result<(), AUTDDriverError>, #[case] size: usize) {
726 let send = |n: usize| {
727 const FRAME_SIZE: usize = size_of::<GainSTMHead>() + NUM_TRANS_IN_UNIT * 2;
728 let device = create_device(NUM_TRANS_IN_UNIT as _);
729 let mut tx = vec![0x00u8; FRAME_SIZE];
730 let data = (0..n)
731 .map(|_| vec![Drive::NULL; NUM_TRANS_IN_UNIT])
732 .collect();
733 let mut op = GainSTMOp::new(
734 STMIterator { data },
735 n,
736 GainSTMMode::PhaseIntensityFull,
737 SamplingConfig::FREQ_40K,
738 LoopBehavior::Infinite,
739 Segment::S0,
740 None,
741 );
742 loop {
743 op.pack(&device, &mut tx)?;
744 if op.is_done() {
745 break;
746 }
747 }
748 Result::<(), AUTDDriverError>::Ok(())
749 };
750 assert_eq!(expected, send(size));
751 }
752}