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(n: u8) -> Device {
216 Device::new(
217 UnitQuaternion::identity(),
218 (0..n).map(|_| Transducer::new(Point3::origin())).collect(),
219 )
220 }
221
222 struct OperationMock {
223 pub pack_size: usize,
224 pub required_size: usize,
225 pub num_frames: usize,
226 pub broken: bool,
227 }
228
229 impl Operation for OperationMock {
230 type Error = AUTDDriverError;
231
232 fn required_size(&self, _: &Device) -> usize {
233 self.required_size
234 }
235
236 fn pack(&mut self, _: &Device, _: &mut [u8]) -> Result<usize, AUTDDriverError> {
237 if self.broken {
238 return Err(AUTDDriverError::NotSupportedTag);
239 }
240 self.num_frames -= 1;
241 Ok(self.pack_size)
242 }
243
244 fn is_done(&self) -> bool {
245 self.num_frames == 0
246 }
247 }
248
249 #[rstest::rstest]
250 #[test]
251 #[case::serial(false)]
252 #[case::parallel(true)]
253 fn test(#[case] parallel: bool) {
254 use crate::geometry::Point3;
255
256 let geometry = Geometry::new(vec![Device::new(
257 UnitQuaternion::identity(),
258 vec![Transducer::new(Point3::origin())],
259 )]);
260
261 let mut op = vec![Some((
262 OperationMock {
263 pack_size: 1,
264 required_size: 2,
265 num_frames: 3,
266 broken: false,
267 },
268 OperationMock {
269 pack_size: 1,
270 required_size: 2,
271 num_frames: 3,
272 broken: false,
273 },
274 ))];
275
276 assert!(!OperationHandler::is_done(&op));
277
278 let mut tx = vec![TxMessage::new_zeroed(); 1];
279
280 assert!(OperationHandler::pack(&mut op, &geometry, &mut tx, parallel).is_ok());
281 assert_eq!(op[0].as_ref().unwrap().0.num_frames, 2);
282 assert_eq!(op[0].as_ref().unwrap().1.num_frames, 2);
283 assert!(!OperationHandler::is_done(&op));
284
285 op[0].as_mut().unwrap().0.pack_size =
286 EC_OUTPUT_FRAME_SIZE - size_of::<Header>() - op[0].as_ref().unwrap().1.required_size;
287 assert!(OperationHandler::pack(&mut op, &geometry, &mut tx, parallel).is_ok());
288 assert_eq!(op[0].as_ref().unwrap().0.num_frames, 1);
289 assert_eq!(op[0].as_ref().unwrap().1.num_frames, 1);
290 assert!(!OperationHandler::is_done(&op));
291
292 op[0].as_mut().unwrap().0.pack_size =
293 EC_OUTPUT_FRAME_SIZE - size_of::<Header>() - op[0].as_ref().unwrap().1.required_size
294 + 1;
295 assert!(OperationHandler::pack(&mut op, &geometry, &mut tx, parallel).is_ok());
296 assert_eq!(op[0].as_ref().unwrap().0.num_frames, 0);
297 assert_eq!(op[0].as_ref().unwrap().1.num_frames, 1);
298 assert!(!OperationHandler::is_done(&op));
299
300 assert!(OperationHandler::pack(&mut op, &geometry, &mut tx, parallel).is_ok());
301 assert_eq!(op[0].as_ref().unwrap().0.num_frames, 0);
302 assert_eq!(op[0].as_ref().unwrap().1.num_frames, 0);
303 assert!(OperationHandler::is_done(&op));
304 }
305
306 #[test]
307 fn test_first() {
308 let geometry = Geometry::new(vec![Device::new(
309 UnitQuaternion::identity(),
310 vec![Transducer::new(Point3::origin())],
311 )]);
312
313 let mut op = vec![Some((
314 OperationMock {
315 pack_size: 0,
316 required_size: 0,
317 num_frames: 1,
318 broken: false,
319 },
320 OperationMock {
321 pack_size: 0,
322 required_size: 0,
323 num_frames: 0,
324 broken: false,
325 },
326 ))];
327
328 assert!(!op[0].as_ref().unwrap().0.is_done());
329 assert!(op[0].as_ref().unwrap().1.is_done());
330 assert!(!OperationHandler::is_done(&op));
331
332 let mut tx = vec![TxMessage::new_zeroed(); 1];
333
334 assert!(OperationHandler::pack(&mut op, &geometry, &mut tx, false).is_ok());
335 assert!(op[0].as_ref().unwrap().0.is_done());
336 assert!(op[0].as_ref().unwrap().1.is_done());
337 assert!(OperationHandler::is_done(&op));
338 }
339
340 #[test]
341 fn test_second() {
342 let geometry = Geometry::new(vec![Device::new(
343 UnitQuaternion::identity(),
344 vec![Transducer::new(Point3::origin())],
345 )]);
346
347 let mut op = vec![Some((
348 OperationMock {
349 pack_size: 0,
350 required_size: 0,
351 num_frames: 0,
352 broken: false,
353 },
354 OperationMock {
355 pack_size: 0,
356 required_size: 0,
357 num_frames: 1,
358 broken: false,
359 },
360 ))];
361
362 assert!(op[0].as_ref().unwrap().0.is_done());
363 assert!(!op[0].as_ref().unwrap().1.is_done());
364 assert!(!OperationHandler::is_done(&op));
365
366 let mut tx = vec![TxMessage::new_zeroed(); 1];
367
368 assert!(OperationHandler::pack(&mut op, &geometry, &mut tx, false).is_ok());
369 assert!(op[0].as_ref().unwrap().0.is_done());
370 assert!(op[0].as_ref().unwrap().1.is_done());
371 assert!(OperationHandler::is_done(&op));
372 }
373
374 #[test]
375 fn test_broken_pack() {
376 let geometry = Geometry::new(vec![Device::new(
377 UnitQuaternion::identity(),
378 vec![Transducer::new(Point3::origin())],
379 )]);
380
381 let mut op = vec![Some((
382 OperationMock {
383 pack_size: 0,
384 required_size: 0,
385 num_frames: 1,
386 broken: true,
387 },
388 OperationMock {
389 pack_size: 0,
390 required_size: 0,
391 num_frames: 1,
392 broken: false,
393 },
394 ))];
395
396 let mut tx = vec![TxMessage::new_zeroed(); 1];
397
398 assert_eq!(
399 Err(AUTDDriverError::NotSupportedTag),
400 OperationHandler::pack(&mut op, &geometry, &mut tx, false)
401 );
402
403 op[0].as_mut().unwrap().0.broken = false;
404 op[0].as_mut().unwrap().1.broken = true;
405
406 assert_eq!(
407 Err(AUTDDriverError::NotSupportedTag),
408 OperationHandler::pack(&mut op, &geometry, &mut tx, false)
409 );
410
411 op[0].as_mut().unwrap().0.num_frames = 0;
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.broken = true;
419 op[0].as_mut().unwrap().1.broken = false;
420
421 op[0].as_mut().unwrap().0.num_frames = 1;
422 op[0].as_mut().unwrap().1.num_frames = 0;
423
424 assert_eq!(
425 Err(AUTDDriverError::NotSupportedTag),
426 OperationHandler::pack(&mut op, &geometry, &mut tx, false)
427 );
428 }
429
430 #[test]
431 fn test_finished() {
432 let geometry = Geometry::new(vec![Device::new(
433 UnitQuaternion::identity(),
434 vec![Transducer::new(Point3::origin())],
435 )]);
436
437 let mut op = vec![Some((
438 OperationMock {
439 pack_size: 0,
440 required_size: 0,
441 num_frames: 0,
442 broken: false,
443 },
444 OperationMock {
445 pack_size: 0,
446 required_size: 0,
447 num_frames: 0,
448 broken: false,
449 },
450 ))];
451
452 assert!(OperationHandler::is_done(&op));
453
454 let mut tx = vec![TxMessage::new_zeroed(); 1];
455
456 assert!(OperationHandler::pack(&mut op, &geometry, &mut tx, false).is_ok());
457 }
458
459 #[test]
460 fn msg_id() {
461 let geometry = Geometry::new(vec![Device::new(
462 UnitQuaternion::identity(),
463 vec![Transducer::new(Point3::origin())],
464 )]);
465
466 let mut tx = vec![TxMessage::new_zeroed(); 1];
467
468 for i in 0..=MSG_ID_MAX {
469 assert_eq!(i, tx[0].header.msg_id);
470 let mut op = vec![Some((
471 OperationMock {
472 pack_size: 0,
473 required_size: 0,
474 num_frames: 1,
475 broken: false,
476 },
477 OperationMock {
478 pack_size: 0,
479 required_size: 0,
480 num_frames: 0,
481 broken: false,
482 },
483 ))];
484 assert!(OperationHandler::pack(&mut op, &geometry, &mut tx, false).is_ok());
485 }
486 assert_eq!(0, tx[0].header.msg_id);
487 }
488}