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,
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 dst = vec![Drive::NULL; self.mem.num_transducers];
98        self.drives_at_inplace(segment, idx, &mut dst);
99        dst
100    }
101
102    pub fn drives_at_inplace(&self, segment: Segment, idx: usize, dst: &mut [Drive]) {
103        if self.is_stm_gain_mode(segment) {
104            self.gain_stm_drives_inplace(segment, idx, dst)
105        } else {
106            self.foci_stm_drives_inplace(segment, idx, dst)
107        }
108    }
109}
autd3_firmware_emulator/fpga/emulator/stm/mod.rs

autd3_firmware_emulator/fpga/emulator/stm/
mod.rs
