1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
use std::net::UdpSocket;
use autd3_core::{
geometry::{Geometry, Transducer},
link::Link,
CPUControlFlags, FPGAControlFlags, RxDatagram, TxDatagram, MSG_EMU_GEOMETRY_SET,
MSG_RD_CPU_VERSION, MSG_RD_FPGA_FUNCTION, MSG_RD_FPGA_VERSION,
};
pub struct Emulator {
port: u16,
socket: Option<UdpSocket>,
last_msg_id: u8,
}
impl Emulator {
pub fn new(port: u16) -> Self {
Self {
port,
socket: None,
last_msg_id: 0,
}
}
}
impl Link for Emulator {
fn open<T: Transducer>(&mut self, geometry: &Geometry<T>) -> anyhow::Result<()> {
let mut geometry_buf = TxDatagram::new(geometry.num_devices());
geometry_buf.num_bodies = geometry.num_devices();
let header = geometry_buf.header_mut();
header.msg_id = MSG_EMU_GEOMETRY_SET;
header.cpu_flag = CPUControlFlags::NONE;
header.fpga_flag = FPGAControlFlags::NONE;
header.size = 0x00;
geometry
.devices()
.iter()
.zip(geometry_buf.body_mut())
.for_each(|(device, body)| {
let origin = device.transducers()[0].position();
let right = device.transducers()[0].x_direction();
let up = device.transducers()[0].y_direction();
let dst = body.data.as_mut_ptr() as *mut f32;
unsafe {
dst.add(0).write(origin.x as f32);
dst.add(1).write(origin.y as f32);
dst.add(2).write(origin.z as f32);
dst.add(3).write(right.x as f32);
dst.add(4).write(right.y as f32);
dst.add(5).write(right.z as f32);
dst.add(6).write(up.x as f32);
dst.add(7).write(up.y as f32);
dst.add(8).write(up.z as f32);
}
});
let socket = UdpSocket::bind("0.0.0.0:8080")?;
let remote_addr = format!("127.0.0.1:{}", self.port);
socket.connect(remote_addr)?;
socket.send(geometry_buf.data())?;
self.socket = Some(socket);
Ok(())
}
fn close(&mut self) -> anyhow::Result<()> {
self.socket = None;
Ok(())
}
fn send(&mut self, tx: &TxDatagram) -> anyhow::Result<bool> {
if let Some(socket) = &self.socket {
socket.try_clone()?.send(tx.data())?;
self.last_msg_id = tx.header().msg_id;
}
Ok(true)
}
fn receive(&mut self, rx: &mut RxDatagram) -> anyhow::Result<bool> {
for r in rx.messages_mut() {
r.msg_id = self.last_msg_id;
}
let mut set = |value: u8| {
for r in rx.messages_mut() {
r.ack = value;
}
};
match self.last_msg_id {
MSG_RD_CPU_VERSION | MSG_RD_FPGA_FUNCTION | MSG_RD_FPGA_VERSION => set(0xFF),
_ => (),
}
Ok(true)
}
fn is_open(&self) -> bool {
self.socket.is_some()
}
}
