autd3_firmware_emulator/fpga/emulator/
mod.rs

1mod gpio_out;
2mod memory;
3mod modulation;
4mod output_mask;
5mod phase_corr;
6mod pwe;
7mod silencer;
8mod stm;
9mod swapchain;
10
11use autd3_core::datagram::Segment;
12use autd3_driver::ethercat::DcSysTime;
13
14use getset::{Getters, MutGetters};
15use memory::Memory;
16
17use super::params::{
18    ADDR_CTL_FLAG, ADDR_FPGA_STATE, CTL_FLAG_FORCE_FAN_BIT, CTL_FLAG_MOD_SET_BIT,
19    CTL_FLAG_STM_SET_BIT,
20};
21
22pub use silencer::SilencerEmulator;
23
24const CTL_FLAG_MOD_SET: u16 = 1 << CTL_FLAG_MOD_SET_BIT;
25const CTL_FLAG_STM_SET: u16 = 1 << CTL_FLAG_STM_SET_BIT;
26
27#[derive(Getters, MutGetters)]
28pub struct FPGAEmulator {
29    #[getset(get = "pub", get_mut = "pub")]
30    pub(crate) mem: Memory,
31    mod_swapchain: swapchain::Swapchain<CTL_FLAG_MOD_SET>,
32    stm_swapchain: swapchain::Swapchain<CTL_FLAG_STM_SET>,
33}
34
35impl FPGAEmulator {
36    #[must_use]
37    pub(crate) fn new(num_transducers: usize) -> Self {
38        let mut fpga = Self {
39            mem: Memory::new(num_transducers),
40            mod_swapchain: swapchain::Swapchain::new(),
41            stm_swapchain: swapchain::Swapchain::new(),
42        };
43        fpga.init();
44        fpga
45    }
46
47    pub(crate) fn init(&mut self) {
48        self.mod_swapchain.init();
49        self.stm_swapchain.init();
50    }
51
52    pub(crate) fn write(&mut self, addr: u16, data: u16) {
53        self.mem.write(addr, data);
54    }
55
56    pub(crate) fn set_and_wait_update(&mut self, sys_time: DcSysTime) {
57        let addr = ((crate::fpga::params::BRAM_SELECT_CONTROLLER as u16 & 0x0003) << 14)
58            | (crate::fpga::params::ADDR_CTL_FLAG as u16 & 0x3FFF);
59        if (self.read(addr) & CTL_FLAG_MOD_SET) == CTL_FLAG_MOD_SET {
60            self.mod_swapchain.set(
61                sys_time,
62                self.modulation_loop_behavior(self.req_modulation_segment()),
63                self.modulation_freq_divide(self.req_modulation_segment()),
64                self.modulation_cycle(self.req_modulation_segment()),
65                self.req_modulation_segment(),
66                self.modulation_transition_mode(),
67            );
68        }
69        if (self.read(addr) & CTL_FLAG_STM_SET) == CTL_FLAG_STM_SET {
70            self.stm_swapchain.set(
71                sys_time,
72                self.stm_loop_behavior(self.req_stm_segment()),
73                self.stm_freq_divide(self.req_stm_segment()),
74                self.stm_cycle(self.req_stm_segment()),
75                self.req_stm_segment(),
76                self.stm_transition_mode(),
77            );
78        }
79    }
80
81    // GRCOV_EXCL_START
82    pub fn update(&mut self) {
83        self.update_with_sys_time(DcSysTime::now());
84    }
85    // GRCOV_EXCL_STOP
86
87    pub fn update_with_sys_time(&mut self, sys_time: DcSysTime) {
88        self.mod_swapchain.update(self.gpio_in(), sys_time);
89        self.stm_swapchain.update(self.gpio_in(), sys_time);
90
91        let mut fpga_state = self.fpga_state();
92        match self.current_mod_segment() {
93            Segment::S0 => fpga_state &= !(1 << 1),
94            Segment::S1 => fpga_state |= 1 << 1,
95        }
96        match self.current_stm_segment() {
97            Segment::S0 => fpga_state &= !(1 << 2),
98            Segment::S1 => fpga_state |= 1 << 2,
99        }
100        if self.stm_cycle(self.current_stm_segment()) == 1 {
101            fpga_state |= 1 << 3;
102        } else {
103            fpga_state &= !(1 << 3);
104        }
105        self.mem.update(fpga_state);
106    }
107
108    #[must_use]
109    pub fn fpga_state(&self) -> u16 {
110        self.mem.controller_bram.read(ADDR_FPGA_STATE)
111    }
112
113    pub fn assert_thermal_sensor(&mut self) {
114        let state = self.mem.controller_bram.read(ADDR_FPGA_STATE);
115        self.mem
116            .controller_bram
117            .write(ADDR_FPGA_STATE, state | (1 << 0));
118    }
119
120    pub fn deassert_thermal_sensor(&mut self) {
121        let state = self.mem.controller_bram.read(ADDR_FPGA_STATE);
122        self.mem
123            .controller_bram
124            .write(ADDR_FPGA_STATE, state & !(1 << 0));
125    }
126
127    #[must_use]
128    pub fn is_thermo_asserted(&self) -> bool {
129        (self.mem.controller_bram.read(ADDR_FPGA_STATE) & (1 << 0)) != 0
130    }
131
132    #[must_use]
133    pub fn is_force_fan(&self) -> bool {
134        (self.mem.controller_bram.read(ADDR_CTL_FLAG) & (1 << CTL_FLAG_FORCE_FAN_BIT)) != 0
135    }
136}
137
138#[cfg(test)]
139mod tests {
140    use super::*;
141
142    #[test]
143    fn thermo() {
144        let mut fpga = FPGAEmulator::new(249);
145        assert!(!fpga.is_thermo_asserted());
146        fpga.assert_thermal_sensor();
147        assert!(fpga.is_thermo_asserted());
148        fpga.deassert_thermal_sensor();
149        assert!(!fpga.is_thermo_asserted());
150    }
151}
autd3_firmware_emulator/fpga/emulator/mod.rs

autd3_firmware_emulator/fpga/emulator/
mod.rs
