autd3_firmware_emulator/fpga/emulator/stm/
mod.rs

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

autd3_firmware_emulator/fpga/emulator/stm/
mod.rs
