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(
38        &self,
39        segment: Segment,
40        idx: usize,
41        phase_corr_buf: &mut [Phase],
42        output_mask_buf: &mut [bool],
43        dst: &mut [Drive],
44    ) {
45        let bram = &self.mem.stm_bram[&segment];
46        let sound_speed = self.sound_speed(segment);
47        let num_foci = self.num_foci(segment) as usize;
48
49        self.phase_correction_inplace(phase_corr_buf);
50        self.output_mask_inplace(segment, output_mask_buf);
51
52        self.mem
53            .tr_pos
54            .iter()
55            .zip(phase_corr_buf.iter())
56            .zip(output_mask_buf.iter())
57            .take(self.mem.num_transducers)
58            .enumerate()
59            .for_each(|(i, ((&tr, &p), &mask))| {
60                let tr_z = ((tr >> 32) & 0xFFFF) as i16 as i32;
61                let tr_x = ((tr >> 16) & 0xFFFF) as i16 as i32;
62                let tr_y = (tr & 0xFFFF) as i16 as i32;
63                let mut intensity = 0x00;
64                let (sin, cos) = (0..num_foci).fold((0, 0), |acc, i| {
65                    let f = bram.read_bram_as::<STMFocus>(
66                        size_of::<STMFocus>() / size_of::<u16>() * (idx * num_foci + i),
67                    );
68                    let x = f.x();
69                    let y = f.y();
70                    let z = f.z();
71                    let intensity_or_offset = f.intensity();
72                    let offset = if i == 0 {
73                        intensity = intensity_or_offset;
74                        0x00
75                    } else {
76                        intensity_or_offset
77                    };
78
79                    let d2 =
80                        (x - tr_x) * (x - tr_x) + (y - tr_y) * (y - tr_y) + (z - tr_z) * (z - tr_z);
81                    let dist = d2.isqrt() as u32;
82                    let q = ((dist << 14) / sound_speed as u32) as usize;
83                    let q = q + offset as usize;
84                    (
85                        acc.0 + self.mem.sin_table[q % 256] as u16,
86                        acc.1 + self.mem.sin_table[(q + 64) % 256] as u16,
87                    )
88                });
89                let sin = ((sin / num_foci as u16) >> 1) as usize;
90                let cos = ((cos / num_foci as u16) >> 1) as usize;
91                let phase = self.mem.atan_table[(sin << 7) | cos];
92                dst[i] = Drive {
93                    phase: Phase(phase) + p,
94                    intensity: Intensity(if mask { intensity } else { 0x00 }),
95                };
96            });
97    }
98
99    // GRCOV_EXCL_START
100    #[must_use]
101    pub fn local_tr_pos(&self) -> &[u64] {
102        self.mem.tr_pos.mem.as_slice()
103    }
104    // GRCOV_EXCL_STOP
105
106    #[must_use]
107    pub fn local_tr_pos_at(&self, idx: usize) -> u64 {
108        self.mem.tr_pos[idx]
109    }
110}
autd3_firmware_emulator/fpga/emulator/stm/foci.rs

autd3_firmware_emulator/fpga/emulator/stm/
foci.rs
