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

autd3_firmware_emulator/fpga/emulator/stm/
foci.rs
