autd3_firmware_emulator/fpga/emulator/stm/
foci.rs

1use autd3_core::{
2    datagram::Segment,
3    gain::{Drive, Intensity, Phase},
4};
5
6use super::super::{super::params::*, FPGAEmulator};
7
8#[bitfield_struct::bitfield(u64)]
9struct STMFocus {
10    #[bits(18)]
11    pub x: i32,
12    #[bits(18)]
13    pub y: i32,
14    #[bits(18)]
15    pub z: i32,
16    #[bits(8)]
17    pub intensity: u8,
18    #[bits(2)]
19    __: u8,
20}
21
22impl FPGAEmulator {
23    #[must_use]
24    pub fn sound_speed(&self, segment: Segment) -> u16 {
25        self.mem
26            .controller_bram
27            .read(ADDR_STM_SOUND_SPEED0 + segment as usize)
28    }
29
30    #[must_use]
31    pub fn num_foci(&self, segment: Segment) -> u8 {
32        self.mem
33            .controller_bram
34            .read(ADDR_STM_NUM_FOCI0 + segment as usize) as u8
35    }
36
37    pub(crate) fn foci_stm_drives_inplace(&self, segment: Segment, idx: usize, dst: &mut [Drive]) {
38        let bram = &self.mem.stm_bram[&segment];
39        let sound_speed = self.sound_speed(segment);
40        let num_foci = self.num_foci(segment) as usize;
41
42        self.mem
43            .tr_pos
44            .iter()
45            .zip(self.phase_correction().iter())
46            .take(self.mem.num_transducers)
47            .enumerate()
48            .for_each(|(i, (&tr, &p))| {
49                let tr_z = ((tr >> 32) & 0xFFFF) as i16 as i32;
50                let tr_x = ((tr >> 16) & 0xFFFF) as i16 as i32;
51                let tr_y = (tr & 0xFFFF) as i16 as i32;
52                let mut intensity = 0x00;
53                let (sin, cos) = (0..num_foci).fold((0, 0), |acc, i| {
54                    let f = bram.read_bram_as::<STMFocus>(
55                        size_of::<STMFocus>() / size_of::<u16>() * (idx * num_foci + i),
56                    );
57                    let x = f.x();
58                    let y = f.y();
59                    let z = f.z();
60                    let intensity_or_offset = f.intensity();
61                    let offset = if i == 0 {
62                        intensity = intensity_or_offset;
63                        0x00
64                    } else {
65                        intensity_or_offset
66                    };
67
68                    let d2 =
69                        (x - tr_x) * (x - tr_x) + (y - tr_y) * (y - tr_y) + (z - tr_z) * (z - tr_z);
70                    let dist = d2.isqrt() as u32;
71                    let q = ((dist << 14) / sound_speed as u32) as usize;
72                    let q = q + offset as usize;
73                    (
74                        acc.0 + self.mem.sin_table[q % 256] as u16,
75                        acc.1 + self.mem.sin_table[(q + 64) % 256] as u16,
76                    )
77                });
78                let sin = ((sin / num_foci as u16) >> 1) as usize;
79                let cos = ((cos / num_foci as u16) >> 1) as usize;
80                let phase = self.mem.atan_table[(sin << 7) | cos];
81                dst[i] = Drive {
82                    phase: Phase(phase) + p,
83                    intensity: Intensity(intensity),
84                };
85            });
86    }
87
88    // GRCOV_EXCL_START
89    #[must_use]
90    pub fn local_tr_pos(&self) -> &[u64] {
91        self.mem.tr_pos.mem.as_slice()
92    }
93    // GRCOV_EXCL_STOP
94
95    #[must_use]
96    pub fn local_tr_pos_at(&self, idx: usize) -> u64 {
97        self.mem.tr_pos[idx]
98    }
99}
autd3_firmware_emulator/fpga/emulator/stm/foci.rs

autd3_firmware_emulator/fpga/emulator/stm/
foci.rs
