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