autd3_firmware_emulator/fpga/emulator/stm/
foci.rs

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

autd3_firmware_emulator/fpga/emulator/stm/
foci.rs
