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

autd3_firmware_emulator/fpga/emulator/
mod.rs
