autd3_firmware_emulator/fpga/emulator/stm/
foci.rs

1use autd3_core::firmware::{Drive, Intensity, Phase, Segment};
2
3use super::super::{super::params::*, FPGAEmulator};
4
5pub struct STMFocus(u64);
6
7impl STMFocus {
8    fn x(&self) -> i32 {
9        let x = (self.0 & 0x3_FFFF) as u32;
10        if x & 0x2_0000 != 0 {
11            (x | 0xFFFC_0000) as i32
12        } else {
13            x as i32
14        }
15    }
16
17    fn y(&self) -> i32 {
18        let y = ((self.0 >> 18) & 0x3_FFFF) as u32;
19        if y & 0x2_0000 != 0 {
20            (y | 0xFFFC_0000) as i32
21        } else {
22            y as i32
23        }
24    }
25
26    fn z(&self) -> i32 {
27        let z = ((self.0 >> 36) & 0x3_FFFF) as u32;
28        if z & 0x2_0000 != 0 {
29            (z | 0xFFFC_0000) as i32
30        } else {
31            z as i32
32        }
33    }
34
35    fn intensity(&self) -> u8 {
36        ((self.0 >> 54) & 0xFF) as u8
37    }
38}
39
40impl FPGAEmulator {
41    #[must_use]
42    pub fn sound_speed(&self, segment: Segment) -> u16 {
43        self.mem
44            .controller_bram
45            .read(ADDR_STM_SOUND_SPEED0 + segment as usize)
46    }
47
48    #[must_use]
49    pub fn num_foci(&self, segment: Segment) -> u8 {
50        self.mem
51            .controller_bram
52            .read(ADDR_STM_NUM_FOCI0 + segment as usize) as u8
53    }
54
55    pub(crate) fn foci_stm_drives_inplace(
56        &self,
57        segment: Segment,
58        idx: usize,
59        phase_corr_buf: *mut Phase,
60        output_mask_buf: *mut bool,
61        dst: *mut Drive,
62    ) {
63        let bram = &self.mem.stm_bram[&segment];
64        let sound_speed = self.sound_speed(segment);
65        let num_foci = self.num_foci(segment) as usize;
66
67        unsafe {
68            self.phase_correction_inplace(phase_corr_buf);
69            self.output_mask_inplace(segment, output_mask_buf)
70        };
71
72        self.local_tr_pos()
73            .iter()
74            .take(self.mem.num_transducers)
75            .enumerate()
76            .for_each(|(i, tr)| {
77                let tr_z = ((tr >> 32) & 0xFFFF) as i16 as i32;
78                let tr_x = ((tr >> 16) & 0xFFFF) as i16 as i32;
79                let tr_y = (tr & 0xFFFF) as i16 as i32;
80                let mut intensity = 0x00;
81                let (sin, cos) = (0..num_foci).fold((0, 0), |acc, i| {
82                    let f = bram.read_bram_as::<STMFocus>(
83                        size_of::<STMFocus>() / size_of::<u16>() * (idx * num_foci + i),
84                    );
85                    let x = f.x();
86                    let y = f.y();
87                    let z = f.z();
88                    let intensity_or_offset = f.intensity();
89                    let offset = if i == 0 {
90                        intensity = intensity_or_offset;
91                        0x00
92                    } else {
93                        intensity_or_offset
94                    };
95
96                    let d2 =
97                        (x - tr_x) * (x - tr_x) + (y - tr_y) * (y - tr_y) + (z - tr_z) * (z - tr_z);
98                    let dist = d2.isqrt() as u32;
99                    let q = ((dist << 14) / sound_speed as u32) as usize;
100                    let q = q + offset as usize;
101                    (
102                        acc.0 + self.mem.sin_table[q % 256] as u16,
103                        acc.1 + self.mem.sin_table[(q + 64) % 256] as u16,
104                    )
105                });
106                let sin = ((sin / num_foci as u16) >> 1) as usize;
107                let cos = ((cos / num_foci as u16) >> 1) as usize;
108                let phase = self.mem.atan_table[(sin << 7) | cos];
109                unsafe {
110                    let p = phase_corr_buf.add(i).read();
111                    let mask = output_mask_buf.add(i).read();
112                    dst.add(i).write(Drive {
113                        phase: Phase(phase) + p,
114                        intensity: Intensity(if mask { intensity } else { 0x00 }),
115                    })
116                };
117            });
118    }
119
120    #[must_use]
121    pub fn local_tr_pos(&self) -> &[u64] {
122        self.mem.tr_pos.mem.as_slice()
123    }
124
125    #[must_use]
126    pub fn local_tr_pos_at(&self, idx: usize) -> u64 {
127        self.mem.tr_pos[idx]
128    }
129}
autd3_firmware_emulator/fpga/emulator/stm/foci.rs

autd3_firmware_emulator/fpga/emulator/stm/
foci.rs
