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        self.phase_correction_inplace(phase_corr_buf);
68        self.output_mask_inplace(segment, output_mask_buf);
69
70        self.mem
71            .tr_pos
72            .iter()
73            .zip(phase_corr_buf.iter())
74            .zip(output_mask_buf.iter())
75            .take(self.mem.num_transducers)
76            .enumerate()
77            .for_each(|(i, ((&tr, &p), &mask))| {
78                let tr_z = ((tr >> 32) & 0xFFFF) as i16 as i32;
79                let tr_x = ((tr >> 16) & 0xFFFF) as i16 as i32;
80                let tr_y = (tr & 0xFFFF) as i16 as i32;
81                let mut intensity = 0x00;
82                let (sin, cos) = (0..num_foci).fold((0, 0), |acc, i| {
83                    let f = bram.read_bram_as::<STMFocus>(
84                        size_of::<STMFocus>() / size_of::<u16>() * (idx * num_foci + i),
85                    );
86                    let x = f.x();
87                    let y = f.y();
88                    let z = f.z();
89                    let intensity_or_offset = f.intensity();
90                    let offset = if i == 0 {
91                        intensity = intensity_or_offset;
92                        0x00
93                    } else {
94                        intensity_or_offset
95                    };
96
97                    let d2 =
98                        (x - tr_x) * (x - tr_x) + (y - tr_y) * (y - tr_y) + (z - tr_z) * (z - tr_z);
99                    let dist = d2.isqrt() as u32;
100                    let q = ((dist << 14) / sound_speed as u32) as usize;
101                    let q = q + offset as usize;
102                    (
103                        acc.0 + self.mem.sin_table[q % 256] as u16,
104                        acc.1 + self.mem.sin_table[(q + 64) % 256] as u16,
105                    )
106                });
107                let sin = ((sin / num_foci as u16) >> 1) as usize;
108                let cos = ((cos / num_foci as u16) >> 1) as usize;
109                let phase = self.mem.atan_table[(sin << 7) | cos];
110                dst[i] = Drive {
111                    phase: Phase(phase) + p,
112                    intensity: Intensity(if mask { intensity } else { 0x00 }),
113                };
114            });
115    }
116
117    // GRCOV_EXCL_START
118    #[must_use]
119    pub fn local_tr_pos(&self) -> &[u64] {
120        self.mem.tr_pos.mem.as_slice()
121    }
122    // GRCOV_EXCL_STOP
123
124    #[must_use]
125    pub fn local_tr_pos_at(&self, idx: usize) -> u64 {
126        self.mem.tr_pos[idx]
127    }
128}
autd3_firmware_emulator/fpga/emulator/stm/foci.rs

autd3_firmware_emulator/fpga/emulator/stm/
foci.rs
