1mod boxed;
2mod clear;
3#[cfg(feature = "dynamic_freq")]
4mod clock;
5mod cpu_gpio_out;
6mod debug;
7mod force_fan;
8mod gain;
9mod gpio_in;
10mod info;
11mod modulation;
12mod phase_corr;
13mod pulse_width_encoder;
14mod reads_fpga_state;
15mod segment;
16mod silencer;
17mod stm;
18mod sync;
19
20pub(crate) use autd3_core::datagram::NullOp;
21pub use boxed::BoxedOperation;
22pub(crate) use clear::*;
23#[cfg(feature = "dynamic_freq")]
24pub(crate) use clock::*;
25pub(crate) use cpu_gpio_out::*;
26pub(crate) use debug::*;
27pub(crate) use force_fan::*;
28pub(crate) use gain::*;
29pub(crate) use gpio_in::*;
30pub use info::FirmwareVersionType;
31pub(crate) use info::*;
32pub(crate) use modulation::*;
33pub(crate) use phase_corr::*;
34pub(crate) use pulse_width_encoder::*;
35pub(crate) use reads_fpga_state::*;
36pub use segment::SwapSegment;
37pub(crate) use segment::*;
38pub(crate) use silencer::*;
39pub(crate) use stm::*;
40pub use stm::{ControlPoint, ControlPoints, FociSTMIterator, GainSTMIterator};
41pub(crate) use sync::*;
42use zerocopy::{Immutable, IntoBytes};
43
44use crate::{
45 error::AUTDDriverError,
46 firmware::cpu::{MSG_ID_MAX, TxMessage},
47 geometry::{Device, Geometry},
48};
49
50use rayon::prelude::*;
51
52#[derive(PartialEq, Debug, IntoBytes, Immutable)]
53#[repr(u8)]
54#[non_exhaustive]
55pub(crate) enum TypeTag {
56 Clear = 0x01,
57 Sync = 0x02,
58 FirmwareVersion = 0x03,
59 #[cfg(feature = "dynamic_freq")]
60 ConfigFPGAClock = 0x04,
61 Modulation = 0x10,
62 ModulationSwapSegment = 0x11,
63 Silencer = 0x21,
64 Gain = 0x30,
65 GainSwapSegment = 0x31,
66 GainSTM = 0x41,
67 FociSTM = 0x42,
68 GainSTMSwapSegment = 0x43,
69 FociSTMSwapSegment = 0x44,
70 ForceFan = 0x60,
71 ReadsFPGAState = 0x61,
72 ConfigPulseWidthEncoder = 0x71,
73 PhaseCorrection = 0x80,
74 Debug = 0xF0,
75 EmulateGPIOIn = 0xF1,
76 CpuGPIOOut = 0xF2,
77}
78
79pub use autd3_core::datagram::Operation;
80
81#[doc(hidden)]
82pub trait OperationGenerator {
83 type O1: Operation;
84 type O2: Operation;
85 #[must_use]
86 fn generate(&mut self, device: &Device) -> (Self::O1, Self::O2);
87}
88
89#[doc(hidden)]
90pub struct OperationHandler {}
91
92impl OperationHandler {
93 #[must_use]
94 pub fn generate<G: OperationGenerator>(
95 mut generator: G,
96 geometry: &Geometry,
97 ) -> Vec<Option<(G::O1, G::O2)>> {
98 geometry
99 .devices()
100 .map(|dev| Some(generator.generate(dev)))
101 .collect()
102 }
103
104 #[must_use]
105 pub fn is_done<O1, O2>(operations: &[Option<(O1, O2)>]) -> bool
106 where
107 O1: Operation,
108 O2: Operation,
109 {
110 operations.iter().all(|op| {
111 op.as_ref()
112 .is_none_or(|(op1, op2)| op1.is_done() && op2.is_done())
113 })
114 }
115
116 pub fn pack<O1, O2>(
117 operations: &mut [Option<(O1, O2)>],
118 geometry: &Geometry,
119 tx: &mut [TxMessage],
120 parallel: bool,
121 ) -> Result<(), AUTDDriverError>
122 where
123 O1: Operation,
124 O2: Operation,
125 AUTDDriverError: From<O1::Error> + From<O2::Error>,
126 {
127 if parallel {
128 geometry
129 .iter()
130 .zip(tx.iter_mut())
131 .filter(|(dev, _)| dev.enable)
132 .zip(operations.iter_mut())
133 .par_bridge()
134 .try_for_each(|((dev, tx), op)| {
135 if let Some((op1, op2)) = op {
136 Self::pack_op2(op1, op2, dev, tx)
137 } else {
138 Ok(())
139 }
140 })
141 } else {
142 geometry
143 .iter()
144 .zip(tx.iter_mut())
145 .filter(|(dev, _)| dev.enable)
146 .zip(operations.iter_mut())
147 .try_for_each(|((dev, tx), op)| {
148 if let Some((op1, op2)) = op {
149 Self::pack_op2(op1, op2, dev, tx)
150 } else {
151 Ok(())
152 }
153 })
154 }
155 }
156
157 fn pack_op2<O1, O2>(
158 op1: &mut O1,
159 op2: &mut O2,
160 dev: &Device,
161 tx: &mut TxMessage,
162 ) -> Result<(), AUTDDriverError>
163 where
164 O1: Operation,
165 O2: Operation,
166 AUTDDriverError: From<O1::Error> + From<O2::Error>,
167 {
168 match (op1.is_done(), op2.is_done()) {
169 (true, true) => Result::<_, AUTDDriverError>::Ok(()),
170 (true, false) => Self::pack_op(op2, dev, tx).map(|_| Ok(()))?,
171 (false, true) => Self::pack_op(op1, dev, tx).map(|_| Ok(()))?,
172 (false, false) => {
173 let op1_size = Self::pack_op(op1, dev, tx)?;
174 if tx.payload().len() - op1_size >= op2.required_size(dev) {
175 op2.pack(dev, &mut tx.payload_mut()[op1_size..])?;
176 tx.header.slot_2_offset = op1_size as u16;
177 }
178 Ok(())
179 }
180 }
181 }
182
183 fn pack_op<O>(op: &mut O, dev: &Device, tx: &mut TxMessage) -> Result<usize, AUTDDriverError>
184 where
185 O: Operation,
186 AUTDDriverError: From<O::Error>,
187 {
188 tx.header.msg_id += 1;
189 tx.header.msg_id &= MSG_ID_MAX;
190 tx.header.slot_2_offset = 0;
191 Ok(op.pack(dev, tx.payload_mut())?)
192 }
193}
194
195#[inline(always)]
196pub(crate) fn write_to_tx<T: IntoBytes + Immutable>(tx: &mut [u8], data: T) {
197 tx[..size_of::<T>()].copy_from_slice(data.as_bytes());
198}
199
200#[cfg(test)]
201pub(crate) mod tests {
202
203 use std::mem::size_of;
204
205 use zerocopy::FromZeros;
206
207 use crate::{
208 ethercat::EC_OUTPUT_FRAME_SIZE,
209 firmware::cpu::Header,
210 geometry::{Point3, Transducer, UnitQuaternion},
211 };
212
213 use super::*;
214
215 pub fn create_device(idx: u16, n: u8) -> Device {
216 Device::new(
217 idx,
218 UnitQuaternion::identity(),
219 (0..n)
220 .map(|i| Transducer::new(i, idx, Point3::origin()))
221 .collect(),
222 )
223 }
224
225 struct OperationMock {
226 pub pack_size: usize,
227 pub required_size: usize,
228 pub num_frames: usize,
229 pub broken: bool,
230 }
231
232 impl Operation for OperationMock {
233 type Error = AUTDDriverError;
234
235 fn required_size(&self, _: &Device) -> usize {
236 self.required_size
237 }
238
239 fn pack(&mut self, _: &Device, _: &mut [u8]) -> Result<usize, AUTDDriverError> {
240 if self.broken {
241 return Err(AUTDDriverError::NotSupportedTag);
242 }
243 self.num_frames -= 1;
244 Ok(self.pack_size)
245 }
246
247 fn is_done(&self) -> bool {
248 self.num_frames == 0
249 }
250 }
251
252 #[rstest::rstest]
253 #[test]
254 #[case::serial(false)]
255 #[case::parallel(true)]
256 fn test(#[case] parallel: bool) {
257 use crate::geometry::Point3;
258
259 let geometry = Geometry::new(vec![Device::new(
260 0,
261 UnitQuaternion::identity(),
262 vec![Transducer::new(0, 0, Point3::origin())],
263 )]);
264
265 let mut op = vec![Some((
266 OperationMock {
267 pack_size: 1,
268 required_size: 2,
269 num_frames: 3,
270 broken: false,
271 },
272 OperationMock {
273 pack_size: 1,
274 required_size: 2,
275 num_frames: 3,
276 broken: false,
277 },
278 ))];
279
280 assert!(!OperationHandler::is_done(&op));
281
282 let mut tx = vec![TxMessage::new_zeroed(); 1];
283
284 assert!(OperationHandler::pack(&mut op, &geometry, &mut tx, parallel).is_ok());
285 assert_eq!(op[0].as_ref().unwrap().0.num_frames, 2);
286 assert_eq!(op[0].as_ref().unwrap().1.num_frames, 2);
287 assert!(!OperationHandler::is_done(&op));
288
289 op[0].as_mut().unwrap().0.pack_size =
290 EC_OUTPUT_FRAME_SIZE - size_of::<Header>() - op[0].as_ref().unwrap().1.required_size;
291 assert!(OperationHandler::pack(&mut op, &geometry, &mut tx, parallel).is_ok());
292 assert_eq!(op[0].as_ref().unwrap().0.num_frames, 1);
293 assert_eq!(op[0].as_ref().unwrap().1.num_frames, 1);
294 assert!(!OperationHandler::is_done(&op));
295
296 op[0].as_mut().unwrap().0.pack_size =
297 EC_OUTPUT_FRAME_SIZE - size_of::<Header>() - op[0].as_ref().unwrap().1.required_size
298 + 1;
299 assert!(OperationHandler::pack(&mut op, &geometry, &mut tx, parallel).is_ok());
300 assert_eq!(op[0].as_ref().unwrap().0.num_frames, 0);
301 assert_eq!(op[0].as_ref().unwrap().1.num_frames, 1);
302 assert!(!OperationHandler::is_done(&op));
303
304 assert!(OperationHandler::pack(&mut op, &geometry, &mut tx, parallel).is_ok());
305 assert_eq!(op[0].as_ref().unwrap().0.num_frames, 0);
306 assert_eq!(op[0].as_ref().unwrap().1.num_frames, 0);
307 assert!(OperationHandler::is_done(&op));
308 }
309
310 #[test]
311 fn test_first() {
312 let geometry = Geometry::new(vec![Device::new(
313 0,
314 UnitQuaternion::identity(),
315 vec![Transducer::new(0, 0, Point3::origin())],
316 )]);
317
318 let mut op = vec![Some((
319 OperationMock {
320 pack_size: 0,
321 required_size: 0,
322 num_frames: 1,
323 broken: false,
324 },
325 OperationMock {
326 pack_size: 0,
327 required_size: 0,
328 num_frames: 0,
329 broken: false,
330 },
331 ))];
332
333 assert!(!op[0].as_ref().unwrap().0.is_done());
334 assert!(op[0].as_ref().unwrap().1.is_done());
335 assert!(!OperationHandler::is_done(&op));
336
337 let mut tx = vec![TxMessage::new_zeroed(); 1];
338
339 assert!(OperationHandler::pack(&mut op, &geometry, &mut tx, false).is_ok());
340 assert!(op[0].as_ref().unwrap().0.is_done());
341 assert!(op[0].as_ref().unwrap().1.is_done());
342 assert!(OperationHandler::is_done(&op));
343 }
344
345 #[test]
346 fn test_second() {
347 let geometry = Geometry::new(vec![Device::new(
348 0,
349 UnitQuaternion::identity(),
350 vec![Transducer::new(0, 0, Point3::origin())],
351 )]);
352
353 let mut op = vec![Some((
354 OperationMock {
355 pack_size: 0,
356 required_size: 0,
357 num_frames: 0,
358 broken: false,
359 },
360 OperationMock {
361 pack_size: 0,
362 required_size: 0,
363 num_frames: 1,
364 broken: false,
365 },
366 ))];
367
368 assert!(op[0].as_ref().unwrap().0.is_done());
369 assert!(!op[0].as_ref().unwrap().1.is_done());
370 assert!(!OperationHandler::is_done(&op));
371
372 let mut tx = vec![TxMessage::new_zeroed(); 1];
373
374 assert!(OperationHandler::pack(&mut op, &geometry, &mut tx, false).is_ok());
375 assert!(op[0].as_ref().unwrap().0.is_done());
376 assert!(op[0].as_ref().unwrap().1.is_done());
377 assert!(OperationHandler::is_done(&op));
378 }
379
380 #[test]
381 fn test_broken_pack() {
382 let geometry = Geometry::new(vec![Device::new(
383 0,
384 UnitQuaternion::identity(),
385 vec![Transducer::new(0, 0, Point3::origin())],
386 )]);
387
388 let mut op = vec![Some((
389 OperationMock {
390 pack_size: 0,
391 required_size: 0,
392 num_frames: 1,
393 broken: true,
394 },
395 OperationMock {
396 pack_size: 0,
397 required_size: 0,
398 num_frames: 1,
399 broken: false,
400 },
401 ))];
402
403 let mut tx = vec![TxMessage::new_zeroed(); 1];
404
405 assert_eq!(
406 Err(AUTDDriverError::NotSupportedTag),
407 OperationHandler::pack(&mut op, &geometry, &mut tx, false)
408 );
409
410 op[0].as_mut().unwrap().0.broken = false;
411 op[0].as_mut().unwrap().1.broken = true;
412
413 assert_eq!(
414 Err(AUTDDriverError::NotSupportedTag),
415 OperationHandler::pack(&mut op, &geometry, &mut tx, false)
416 );
417
418 op[0].as_mut().unwrap().0.num_frames = 0;
419
420 assert_eq!(
421 Err(AUTDDriverError::NotSupportedTag),
422 OperationHandler::pack(&mut op, &geometry, &mut tx, false)
423 );
424
425 op[0].as_mut().unwrap().0.broken = true;
426 op[0].as_mut().unwrap().1.broken = false;
427
428 op[0].as_mut().unwrap().0.num_frames = 1;
429 op[0].as_mut().unwrap().1.num_frames = 0;
430
431 assert_eq!(
432 Err(AUTDDriverError::NotSupportedTag),
433 OperationHandler::pack(&mut op, &geometry, &mut tx, false)
434 );
435 }
436
437 #[test]
438 fn test_finished() {
439 let geometry = Geometry::new(vec![Device::new(
440 0,
441 UnitQuaternion::identity(),
442 vec![Transducer::new(0, 0, Point3::origin())],
443 )]);
444
445 let mut op = vec![Some((
446 OperationMock {
447 pack_size: 0,
448 required_size: 0,
449 num_frames: 0,
450 broken: false,
451 },
452 OperationMock {
453 pack_size: 0,
454 required_size: 0,
455 num_frames: 0,
456 broken: false,
457 },
458 ))];
459
460 assert!(OperationHandler::is_done(&op));
461
462 let mut tx = vec![TxMessage::new_zeroed(); 1];
463
464 assert!(OperationHandler::pack(&mut op, &geometry, &mut tx, false).is_ok());
465 }
466
467 #[test]
468 fn msg_id() {
469 let geometry = Geometry::new(vec![Device::new(
470 0,
471 UnitQuaternion::identity(),
472 vec![Transducer::new(0, 0, Point3::origin())],
473 )]);
474
475 let mut tx = vec![TxMessage::new_zeroed(); 1];
476
477 for i in 0..=MSG_ID_MAX {
478 assert_eq!(i, tx[0].header.msg_id);
479 let mut op = vec![Some((
480 OperationMock {
481 pack_size: 0,
482 required_size: 0,
483 num_frames: 1,
484 broken: false,
485 },
486 OperationMock {
487 pack_size: 0,
488 required_size: 0,
489 num_frames: 0,
490 broken: false,
491 },
492 ))];
493 assert!(OperationHandler::pack(&mut op, &geometry, &mut tx, false).is_ok());
494 }
495 assert_eq!(0, tx[0].header.msg_id);
496 }
497}