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