autd3_firmware_emulator/fpga/emulator/stm/
mod.rs

1use std::num::NonZeroU16;
2
3use autd3_core::{
4    datagram::{GPIOIn, LoopBehavior, Segment, TransitionMode},
5    gain::{Drive, Phase},
6};
7use autd3_driver::ethercat::DcSysTime;
8
9use super::{super::params::*, FPGAEmulator};
10
11mod foci;
12mod gain;
13
14impl FPGAEmulator {
15    #[must_use]
16    pub fn is_stm_gain_mode(&self, segment: Segment) -> bool {
17        self.mem
18            .controller_bram
19            .read(ADDR_STM_MODE0 + segment as usize)
20            == STM_MODE_GAIN
21    }
22
23    #[must_use]
24    pub fn stm_freq_divide(&self, segment: Segment) -> u16 {
25        self.mem
26            .controller_bram
27            .read(ADDR_STM_FREQ_DIV0 + segment as usize)
28    }
29
30    #[must_use]
31    pub fn stm_cycle(&self, segment: Segment) -> usize {
32        self.mem
33            .controller_bram
34            .read(ADDR_STM_CYCLE0 + segment as usize) as usize
35            + 1
36    }
37
38    #[must_use]
39    pub fn stm_loop_behavior(&self, segment: Segment) -> LoopBehavior {
40        match self
41            .mem
42            .controller_bram
43            .read(ADDR_STM_REP0 + segment as usize)
44        {
45            0xFFFF => LoopBehavior::Infinite,
46            v => LoopBehavior::Finite(NonZeroU16::new(v + 1).unwrap()),
47        }
48    }
49
50    #[must_use]
51    pub fn stm_transition_mode(&self) -> TransitionMode {
52        match self.mem.controller_bram.read(ADDR_STM_TRANSITION_MODE) as u8 {
53            TRANSITION_MODE_SYNC_IDX => TransitionMode::SyncIdx,
54            TRANSITION_MODE_SYS_TIME => TransitionMode::SysTime(
55                DcSysTime::ZERO
56                    + std::time::Duration::from_nanos(
57                        self.mem
58                            .controller_bram
59                            .read_bram_as::<u64>(ADDR_STM_TRANSITION_VALUE_0),
60                    ),
61            ),
62            TRANSITION_MODE_GPIO => TransitionMode::GPIO(
63                match self
64                    .mem
65                    .controller_bram
66                    .read_bram_as::<u64>(ADDR_STM_TRANSITION_VALUE_0)
67                {
68                    0 => GPIOIn::I0,
69                    1 => GPIOIn::I1,
70                    2 => GPIOIn::I2,
71                    3 => GPIOIn::I3,
72                    _ => unreachable!(),
73                },
74            ),
75            TRANSITION_MODE_EXT => TransitionMode::Ext,
76            TRANSITION_MODE_IMMEDIATE => TransitionMode::Immediate,
77            _ => unreachable!(),
78        }
79    }
80
81    #[must_use]
82    pub fn req_stm_segment(&self) -> Segment {
83        match self.mem.controller_bram.read(ADDR_STM_REQ_RD_SEGMENT) {
84            0 => Segment::S0,
85            1 => Segment::S1,
86            _ => unreachable!(),
87        }
88    }
89
90    #[must_use]
91    pub fn drives(&self) -> Vec<Drive> {
92        self.drives_at(self.current_stm_segment(), self.current_stm_idx())
93    }
94
95    #[must_use]
96    pub fn drives_at(&self, segment: Segment, idx: usize) -> Vec<Drive> {
97        let mut phase_corr_buf = vec![Phase::ZERO; self.mem.num_transducers];
98        let mut output_mask_buf = vec![false; self.mem.num_transducers];
99        let mut dst = vec![Drive::NULL; self.mem.num_transducers];
100        self.drives_at_inplace(
101            segment,
102            idx,
103            &mut phase_corr_buf,
104            &mut output_mask_buf,
105            &mut dst,
106        );
107        dst
108    }
109
110    pub fn drives_at_inplace(
111        &self,
112        segment: Segment,
113        idx: usize,
114        phase_corr_buf: &mut [Phase],
115        output_mask_buf: &mut [bool],
116        dst: &mut [Drive],
117    ) {
118        if self.is_stm_gain_mode(segment) {
119            self.gain_stm_drives_inplace(segment, idx, phase_corr_buf, output_mask_buf, dst)
120        } else {
121            self.foci_stm_drives_inplace(segment, idx, phase_corr_buf, output_mask_buf, dst)
122        }
123    }
124}
autd3_firmware_emulator/fpga/emulator/stm/mod.rs

autd3_firmware_emulator/fpga/emulator/stm/
mod.rs
