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::firmware::Segment;
12use autd3_driver::ethercat::DcSysTime;
13
14use memory::Memory;
15
16use super::params::{
17    ADDR_CTL_FLAG, ADDR_FPGA_STATE, CTL_FLAG_FORCE_FAN_BIT, CTL_FLAG_MOD_SET_BIT,
18    CTL_FLAG_STM_SET_BIT,
19};
20
21pub use silencer::SilencerEmulator;
22
23const CTL_FLAG_MOD_SET: u16 = 1 << CTL_FLAG_MOD_SET_BIT;
24const CTL_FLAG_STM_SET: u16 = 1 << CTL_FLAG_STM_SET_BIT;
25
26pub struct FPGAEmulator {
27    pub(crate) mem: Memory,
28    mod_swapchain: swapchain::Swapchain<CTL_FLAG_MOD_SET>,
29    stm_swapchain: swapchain::Swapchain<CTL_FLAG_STM_SET>,
30}
31
32impl FPGAEmulator {
33    #[must_use]
34    pub(crate) fn new(num_transducers: usize) -> Self {
35        let mut fpga = Self {
36            mem: Memory::new(num_transducers),
37            mod_swapchain: swapchain::Swapchain::new(),
38            stm_swapchain: swapchain::Swapchain::new(),
39        };
40        fpga.init();
41        fpga
42    }
43
44    pub(crate) fn init(&mut self) {
45        self.mod_swapchain.init();
46        self.stm_swapchain.init();
47    }
48
49    pub(crate) fn write(&mut self, addr: u16, data: u16) {
50        self.mem.write(addr, data);
51    }
52
53    pub(crate) fn set_and_wait_update(&mut self, sys_time: DcSysTime) {
54        let addr = ((crate::fpga::params::BRAM_SELECT_CONTROLLER as u16 & 0x0003) << 14)
55            | (crate::fpga::params::ADDR_CTL_FLAG as u16 & 0x3FFF);
56        if (self.read(addr) & CTL_FLAG_MOD_SET) == CTL_FLAG_MOD_SET {
57            self.mod_swapchain.set(
58                sys_time,
59                self.modulation_loop_count(self.req_modulation_segment()),
60                self.modulation_freq_divide(self.req_modulation_segment()),
61                self.modulation_cycle(self.req_modulation_segment()),
62                self.req_modulation_segment(),
63                self.modulation_transition_mode(),
64            );
65        }
66        if (self.read(addr) & CTL_FLAG_STM_SET) == CTL_FLAG_STM_SET {
67            self.stm_swapchain.set(
68                sys_time,
69                self.stm_loop_count(self.req_stm_segment()),
70                self.stm_freq_divide(self.req_stm_segment()),
71                self.stm_cycle(self.req_stm_segment()),
72                self.req_stm_segment(),
73                self.stm_transition_mode(),
74            );
75        }
76    }
77
78    // GRCOV_EXCL_START
79    #[cfg(feature = "time")]
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.read(ADDR_FPGA_STATE)
109    }
110
111    pub fn assert_thermal_sensor(&mut self) {
112        let state = self.mem.controller_bram.read(ADDR_FPGA_STATE);
113        self.mem
114            .controller_bram
115            .write(ADDR_FPGA_STATE, state | (1 << 0));
116    }
117
118    pub fn deassert_thermal_sensor(&mut self) {
119        let state = self.mem.controller_bram.read(ADDR_FPGA_STATE);
120        self.mem
121            .controller_bram
122            .write(ADDR_FPGA_STATE, state & !(1 << 0));
123    }
124
125    #[must_use]
126    pub fn is_thermo_asserted(&self) -> bool {
127        (self.mem.controller_bram.read(ADDR_FPGA_STATE) & (1 << 0)) != 0
128    }
129
130    #[must_use]
131    pub fn is_force_fan(&self) -> bool {
132        (self.mem.controller_bram.read(ADDR_CTL_FLAG) & (1 << CTL_FLAG_FORCE_FAN_BIT)) != 0
133    }
134
135    pub fn set_phase_corr_bram(&mut self, value: u16) {
136        self.mem.phase_corr_bram.mem_mut().fill(value);
137    }
138}
139
140#[cfg(test)]
141mod tests {
142    use super::*;
143
144    #[test]
145    fn thermo() {
146        let mut fpga = FPGAEmulator::new(249);
147        assert!(!fpga.is_thermo_asserted());
148        fpga.assert_thermal_sensor();
149        assert!(fpga.is_thermo_asserted());
150        fpga.deassert_thermal_sensor();
151        assert!(!fpga.is_thermo_asserted());
152    }
153}
autd3_firmware_emulator/fpga/emulator/mod.rs

autd3_firmware_emulator/fpga/emulator/
mod.rs
