autd3_firmware_emulator/fpga/emulator/stm/
foci.rs

1use autd3_driver::firmware::fpga::{Drive, EmitIntensity, 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.controller_bram.borrow()[ADDR_STM_SOUND_SPEED0 + segment as usize]
23    }
24
25    #[must_use]
26    pub fn num_foci(&self, segment: Segment) -> u8 {
27        self.mem.controller_bram.borrow()[ADDR_STM_NUM_FOCI0 + segment as usize] as u8
28    }
29
30    pub(crate) fn foci_stm_drives_inplace(&self, segment: Segment, idx: usize, dst: &mut [Drive]) {
31        let bram = &self.mem.stm_bram.borrow()[&segment];
32        let sound_speed = self.sound_speed(segment);
33        let num_foci = self.num_foci(segment) as usize;
34
35        self.mem
36            .tr_pos
37            .iter()
38            .zip(self.phase_correction().iter())
39            .take(self.mem.num_transducers)
40            .enumerate()
41            .for_each(|(i, (&tr, &p))| {
42                let tr_z = ((tr >> 32) & 0xFFFF) as i16 as i32;
43                let tr_x = ((tr >> 16) & 0xFFFF) as i16 as i32;
44                let tr_y = (tr & 0xFFFF) as i16 as i32;
45                let mut intensity = 0x00;
46                let (sin, cos) = (0..num_foci).fold((0, 0), |acc, i| {
47                    let f = unsafe {
48                        (bram[size_of::<STMFocus>() / size_of::<u16>() * (idx * num_foci + i)..]
49                            .as_ptr() as *const STMFocus)
50                            .read_unaligned()
51                    };
52                    let x = f.x();
53                    let y = f.y();
54                    let z = f.z();
55                    let intensity_or_offset = f.intensity();
56                    let offset = if i == 0 {
57                        intensity = intensity_or_offset;
58                        0x00
59                    } else {
60                        intensity_or_offset
61                    };
62
63                    let d2 =
64                        (x - tr_x) * (x - tr_x) + (y - tr_y) * (y - tr_y) + (z - tr_z) * (z - tr_z);
65                    let dist = d2.isqrt() as u32;
66                    let q = ((dist << 14) / sound_speed as u32) as usize;
67                    let q = q + offset as usize;
68                    (
69                        acc.0 + self.mem.sin_table[q % 256] as u16,
70                        acc.1 + self.mem.sin_table[(q + 64) % 256] as u16,
71                    )
72                });
73                let sin = ((sin / num_foci as u16) >> 1) as usize;
74                let cos = ((cos / num_foci as u16) >> 1) as usize;
75                let phase = self.mem.atan_table[(sin << 7) | cos];
76                dst[i] = Drive {
77                    phase: Phase(phase) + p,
78                    intensity: EmitIntensity(intensity),
79                };
80            });
81    }
82
83    #[must_use]
84    pub fn local_tr_pos(&self) -> &[u64] {
85        &self.mem.tr_pos
86    }
87}
autd3_firmware_emulator/fpga/emulator/stm/foci.rs

autd3_firmware_emulator/fpga/emulator/stm/
foci.rs
