autd3_firmware_emulator/fpga/emulator/
mod.rs

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

autd3_firmware_emulator/fpga/emulator/
mod.rs
