autd3_firmware_emulator/fpga/emulator/
mod.rs

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

autd3_firmware_emulator/fpga/emulator/
mod.rs
