autd3_firmware_emulator/fpga/emulator/stm/
foci.rs

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
use autd3_driver::{
    derive::Segment,
    firmware::fpga::{Drive, EmitIntensity, Phase, STMFocus},
};
use num_integer::Roots;

use super::super::{super::params::*, FPGAEmulator};

impl FPGAEmulator {
    pub fn sound_speed(&self, segment: Segment) -> u16 {
        self.mem.controller_bram()[match segment {
            Segment::S0 => ADDR_STM_SOUND_SPEED0,
            Segment::S1 => ADDR_STM_SOUND_SPEED1,
            _ => unimplemented!(),
        }]
    }

    pub fn num_foci(&self, segment: Segment) -> u8 {
        self.mem.controller_bram()[match segment {
            Segment::S0 => ADDR_STM_NUM_FOCI0,
            Segment::S1 => ADDR_STM_NUM_FOCI1,
            _ => unimplemented!(),
        }] as u8
    }

    pub(crate) fn foci_stm_drives_inplace(&self, segment: Segment, idx: usize, dst: &mut [Drive]) {
        let bram = match segment {
            Segment::S0 => self.mem.stm_bram_0(),
            Segment::S1 => self.mem.stm_bram_1(),
            _ => unimplemented!(),
        };
        let sound_speed = self.sound_speed(segment);

        self.mem
            .tr_pos
            .iter()
            .zip(self.phase_correction().iter())
            .take(self.mem.num_transducers)
            .enumerate()
            .for_each(|(i, (&tr, &p))| {
                let tr_z = ((tr >> 32) & 0xFFFF) as i16 as i32;
                let tr_x = ((tr >> 16) & 0xFFFF) as i16 as i32;
                let tr_y = (tr & 0xFFFF) as i16 as i32;
                let mut intensity = 0x00;
                let (sin, cos) = (0..self.num_foci(segment) as usize).fold((0, 0), |acc, i| {
                    let f = unsafe {
                        (bram[32 * idx + 4 * i..].as_ptr() as *const STMFocus).read_unaligned()
                    };
                    let x = f.x();
                    let y = f.y();
                    let z = f.z();
                    let intensity_or_offset = f.intensity();
                    let offset = if i == 0 {
                        intensity = intensity_or_offset;
                        0x00
                    } else {
                        intensity_or_offset
                    };

                    let d2 =
                        (x - tr_x) * (x - tr_x) + (y - tr_y) * (y - tr_y) + (z - tr_z) * (z - tr_z);
                    let dist = d2.sqrt() as u32;
                    let q = ((dist << 14) / sound_speed as u32) as usize;
                    let q = q + offset as usize;
                    (
                        acc.0 + self.mem.sin_table[q % 256] as u16,
                        acc.1 + self.mem.sin_table[(q + 64) % 256] as u16,
                    )
                });
                let sin = ((sin / self.num_foci(segment) as u16) >> 1) as usize;
                let cos = ((cos / self.num_foci(segment) as u16) >> 1) as usize;
                let phase = self.mem.atan_table[(sin << 7) | cos];
                dst[i] = Drive::new(Phase::new(phase) + p, EmitIntensity::new(intensity));
            });
    }

    pub fn local_tr_pos(&self) -> &[u64] {
        &self.mem.tr_pos
    }
}