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    pub fn update(&mut self) {
80        self.update_with_sys_time(DcSysTime::now());
81    }
82    // GRCOV_EXCL_STOP
83
84    pub fn update_with_sys_time(&mut self, sys_time: DcSysTime) {
85        self.mod_swapchain.update(self.gpio_in(), sys_time);
86        self.stm_swapchain.update(self.gpio_in(), sys_time);
87
88        let mut fpga_state = self.fpga_state();
89        match self.current_mod_segment() {
90            Segment::S0 => fpga_state &= !(1 << 1),
91            Segment::S1 => fpga_state |= 1 << 1,
92        }
93        match self.current_stm_segment() {
94            Segment::S0 => fpga_state &= !(1 << 2),
95            Segment::S1 => fpga_state |= 1 << 2,
96        }
97        if self.stm_cycle(self.current_stm_segment()) == 1 {
98            fpga_state |= 1 << 3;
99        } else {
100            fpga_state &= !(1 << 3);
101        }
102        self.mem.update(fpga_state);
103    }
104
105    #[must_use]
106    pub fn fpga_state(&self) -> u16 {
107        self.mem.controller_bram.read(ADDR_FPGA_STATE)
108    }
109
110    pub fn assert_thermal_sensor(&mut self) {
111        let state = self.mem.controller_bram.read(ADDR_FPGA_STATE);
112        self.mem
113            .controller_bram
114            .write(ADDR_FPGA_STATE, state | (1 << 0));
115    }
116
117    pub fn deassert_thermal_sensor(&mut self) {
118        let state = self.mem.controller_bram.read(ADDR_FPGA_STATE);
119        self.mem
120            .controller_bram
121            .write(ADDR_FPGA_STATE, state & !(1 << 0));
122    }
123
124    #[must_use]
125    pub fn is_thermo_asserted(&self) -> bool {
126        (self.mem.controller_bram.read(ADDR_FPGA_STATE) & (1 << 0)) != 0
127    }
128
129    #[must_use]
130    pub fn is_force_fan(&self) -> bool {
131        (self.mem.controller_bram.read(ADDR_CTL_FLAG) & (1 << CTL_FLAG_FORCE_FAN_BIT)) != 0
132    }
133
134    pub fn set_phase_corr_bram(&mut self, value: u16) {
135        self.mem.phase_corr_bram.mem_mut().fill(value);
136    }
137}
138
139#[cfg(test)]
140mod tests {
141    use super::*;
142
143    #[test]
144    fn thermo() {
145        let mut fpga = FPGAEmulator::new(249);
146        assert!(!fpga.is_thermo_asserted());
147        fpga.assert_thermal_sensor();
148        assert!(fpga.is_thermo_asserted());
149        fpga.deassert_thermal_sensor();
150        assert!(!fpga.is_thermo_asserted());
151    }
152}
autd3_firmware_emulator/fpga/emulator/mod.rs

autd3_firmware_emulator/fpga/emulator/
mod.rs
