diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 7f166f68..c24a26bb 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -43,6 +43,9 @@ jobs: - os: ubuntu-latest features: "lightweight async-trait" lint: true + - os: ubuntu-latest + features: "dynamic_freq" + lint: true steps: - uses: actions/checkout@v4 with: diff --git a/.github/workflows/pr.yml b/.github/workflows/pr.yml index d3eefe08..52a31276 100644 --- a/.github/workflows/pr.yml +++ b/.github/workflows/pr.yml @@ -78,6 +78,9 @@ jobs: - os: ubuntu-latest features: "lightweight async-trait" lint: true + - os: ubuntu-latest + features: "dynamic_freq" + lint: true steps: - uses: actions/checkout@v4 with: diff --git a/CHANGELOG.md b/CHANGELOG.md index 908a6466..ff429be6 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,7 @@ - Update firmware to v10.0.1 - phase correction bram and pulse width encoder table reset to default in clear op + - support for `dynamic_freq` version - Remove `Deref` and `DerefMut` for `Controller` - Impl `Deref` and `DerefMut` for `Controller` instead - Make `Transducer::new` public diff --git a/Cargo.toml b/Cargo.toml index e7e1f4bc..d6b87383 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -15,7 +15,7 @@ members = [ resolver = "2" [workspace.package] -version = "29.0.0-rc.12" +version = "29.0.0-rc.13" authors = ["shun suzuki "] edition = "2021" license = "MIT" @@ -68,12 +68,12 @@ seq-macro = "0.3.5" spin_sleep = "1.2.1" windows = { version = "0.58.0", default-features = false } zerocopy = { version = "0.8.10", features = ["derive"] } -autd3 = { path = "./autd3", version = "29.0.0-rc.12" } -autd3-driver = { path = "./autd3-driver", version = "29.0.0-rc.12", default-features = false } -autd3-derive = { path = "./autd3-derive", version = "29.0.0-rc.12" } -autd3-gain-holo = { path = "./autd3-gain-holo", version = "29.0.0-rc.12" } -autd3-link-simulator = { path = "./autd3-link-simulator", version = "29.0.0-rc.12" } -autd3-link-twincat = { path = "./autd3-link-twincat", version = "29.0.0-rc.12", default-features = false } -autd3-modulation-audio-file = { path = "./autd3-modulation-audio-file", version = "29.0.0-rc.12" } -autd3-firmware-emulator = { path = "./autd3-firmware-emulator", version = "29.0.0-rc.12" } -autd3-protobuf = { path = "./autd3-protobuf", version = "29.0.0-rc.12" } +autd3 = { path = "./autd3", version = "29.0.0-rc.13" } +autd3-driver = { path = "./autd3-driver", version = "29.0.0-rc.13", default-features = false } +autd3-derive = { path = "./autd3-derive", version = "29.0.0-rc.13" } +autd3-gain-holo = { path = "./autd3-gain-holo", version = "29.0.0-rc.13" } +autd3-link-simulator = { path = "./autd3-link-simulator", version = "29.0.0-rc.13" } +autd3-link-twincat = { path = "./autd3-link-twincat", version = "29.0.0-rc.13", default-features = false } +autd3-modulation-audio-file = { path = "./autd3-modulation-audio-file", version = "29.0.0-rc.13" } +autd3-firmware-emulator = { path = "./autd3-firmware-emulator", version = "29.0.0-rc.13" } +autd3-protobuf = { path = "./autd3-protobuf", version = "29.0.0-rc.13" } diff --git a/autd3-driver/Cargo.toml b/autd3-driver/Cargo.toml index d3f0c9d6..dffa5699 100644 --- a/autd3-driver/Cargo.toml +++ b/autd3-driver/Cargo.toml @@ -48,6 +48,7 @@ use_meter = [] left_handed = [] serde = ["dep:serde"] derive = [] +dynamic_freq = [] [lib] bench = false @@ -58,5 +59,5 @@ path = "benches/gain.rs" harness = false [package.metadata.docs.rs] -all-features = true +features = ["lightweight", "async-trait", "serde", "derive"] rustdoc-args = ["--cfg", "docsrs"] diff --git a/autd3-driver/src/datagram/clock.rs b/autd3-driver/src/datagram/clock.rs new file mode 100644 index 00000000..125e0192 --- /dev/null +++ b/autd3-driver/src/datagram/clock.rs @@ -0,0 +1,32 @@ +use crate::{defined::ultrasound_freq, firmware::operation::ConfigureClockOp}; + +use crate::datagram::*; + +#[derive(Default, Debug)] +#[doc(hidden)] +pub struct ConfigureFPGAClock {} + +impl ConfigureFPGAClock { + pub const fn new() -> Self { + Self {} + } +} + +pub struct ConfigureClockOpGenerator {} + +impl OperationGenerator for ConfigureClockOpGenerator { + type O1 = ConfigureClockOp; + type O2 = NullOp; + + fn generate(&mut self, _: &Device) -> (Self::O1, Self::O2) { + (Self::O1::new(ultrasound_freq()), Self::O2::new()) + } +} + +impl Datagram for ConfigureFPGAClock { + type G = ConfigureClockOpGenerator; + + fn operation_generator(self, _: &Geometry) -> Result { + Ok(ConfigureClockOpGenerator {}) + } +} diff --git a/autd3-driver/src/datagram/mod.rs b/autd3-driver/src/datagram/mod.rs index 3bcf5b46..f331fe82 100644 --- a/autd3-driver/src/datagram/mod.rs +++ b/autd3-driver/src/datagram/mod.rs @@ -1,4 +1,6 @@ mod clear; +#[cfg(feature = "dynamic_freq")] +mod clock; mod cpu_gpio_out; mod debug; mod force_fan; @@ -23,6 +25,8 @@ pub use super::firmware::operation::SwapSegment; #[doc(inline)] pub use super::firmware::operation::{ControlPoint, ControlPoints}; pub use clear::Clear; +#[cfg(feature = "dynamic_freq")] +pub use clock::ConfigureFPGAClock; #[doc(hidden)] pub use cpu_gpio_out::{CpuGPIO, CpuGPIOPort}; pub use debug::DebugSettings; @@ -37,7 +41,9 @@ pub use modulation::{ pub use phase_corr::PhaseCorrection; pub use pulse_width_encoder::PulseWidthEncoder; pub use reads_fpga_state::ReadsFPGAState; -pub use silencer::{FixedCompletionTime, FixedUpdateRate, HasSamplingConfig, Silencer}; +#[cfg(not(feature = "dynamic_freq"))] +pub use silencer::FixedCompletionTime; +pub use silencer::{FixedCompletionSteps, FixedUpdateRate, HasSamplingConfig, Silencer}; pub use stm::{ FociSTM, FociSTMContext, FociSTMContextGenerator, FociSTMGenerator, GainSTM, GainSTMContext, GainSTMContextGenerator, GainSTMGenerator, IntoFociSTMGenerator, IntoGainSTMGenerator, diff --git a/autd3-driver/src/datagram/silencer.rs b/autd3-driver/src/datagram/silencer.rs index 8b9555e9..e80a4ba3 100644 --- a/autd3-driver/src/datagram/silencer.rs +++ b/autd3-driver/src/datagram/silencer.rs @@ -1,9 +1,8 @@ -use std::{num::NonZeroU16, time::Duration}; +use std::num::NonZeroU16; use autd3_derive::Builder; use crate::{ - defined::ULTRASOUND_PERIOD, error::AUTDDriverError, firmware::{ fpga::{ @@ -30,20 +29,36 @@ pub trait HasSamplingConfig { pub trait SilencerConfig: std::fmt::Debug + Clone + Copy {} impl SilencerConfig for () {} +#[cfg(not(feature = "dynamic_freq"))] /// To configure the silencer by the completion time. #[derive(Debug, Clone, Copy, PartialEq, Eq)] pub struct FixedCompletionTime { - /// The completion time of the intensity change. The value must be multiple of [`ULTRASOUND_PERIOD`]. + /// The completion time of the intensity change. The value must be multiple of the ultrasound period. /// /// The larger this value, the more the noise is suppressed. - pub intensity: Duration, - /// The completion time of the phase change. The value must be multiple of [`ULTRASOUND_PERIOD`]. + pub intensity: std::time::Duration, + /// The completion time of the phase change. The value must be multiple of the ultrasound period. /// /// The larger this value, the more the noise is suppressed. - pub phase: Duration, + pub phase: std::time::Duration, } +#[cfg(not(feature = "dynamic_freq"))] impl SilencerConfig for FixedCompletionTime {} +/// To configure the silencer by the completion steps. +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub struct FixedCompletionSteps { + /// The completion steps of the intensity change. + /// + /// The larger this value, the more the noise is suppressed. + pub intensity: NonZeroU16, + /// The completion time of the phase change. + /// + /// The larger this value, the more the noise is suppressed. + pub phase: NonZeroU16, +} +impl SilencerConfig for FixedCompletionSteps {} + /// To configure the silencer by the update rate. #[derive(Debug, Clone, Copy, PartialEq, Eq)] pub struct FixedUpdateRate { @@ -72,15 +87,6 @@ pub struct Silencer { } impl Silencer<()> { - /// The default completion time of the intensity change. - pub const DEFAULT_COMPLETION_TIME_INTENSITY: Duration = Duration::from_micros( - ULTRASOUND_PERIOD.as_micros() as u64 * SILENCER_STEPS_INTENSITY_DEFAULT as u64, - ); - /// The default completion time of the phase change. - pub const DEFAULT_COMPLETION_TIME_PHASE: Duration = Duration::from_micros( - ULTRASOUND_PERIOD.as_micros() as u64 * SILENCER_STEPS_PHASE_DEFAULT as u64, - ); - /// Creates a [`Silencer`]. pub const fn new(config: T) -> Silencer { Silencer { @@ -91,14 +97,15 @@ impl Silencer<()> { } /// Creates a [`Silencer`] to disable the silencer. - pub const fn disable() -> Silencer { - Silencer::new(FixedCompletionTime { - intensity: ULTRASOUND_PERIOD, - phase: ULTRASOUND_PERIOD, + pub const fn disable() -> Silencer { + Silencer::new(FixedCompletionSteps { + intensity: NonZeroU16::MIN, + phase: NonZeroU16::MIN, }) } } +#[cfg(not(feature = "dynamic_freq"))] impl Silencer { /// Whether the strict mode is enabled. The default is `true`. /// @@ -124,16 +131,52 @@ impl Silencer { if !self.strict_mode { return true; } - self.config.intensity <= target.intensity().map_or(Duration::MAX, |c| c.period()) - && self.config.phase <= target.phase().map_or(Duration::MAX, |c| c.period()) + self.config.intensity + <= target + .intensity() + .map_or(std::time::Duration::MAX, |c| c.period()) + && self.config.phase + <= target + .phase() + .map_or(std::time::Duration::MAX, |c| c.period()) + } +} + +impl Silencer { + /// Whether the strict mode is enabled. The default is `true`. + /// + /// If the strict mode is enabled, an error is returned if the phase/intensity change of [`Modulation`], [`FociSTM`] or [`GainSTM`] cannot be completed within the time specified by the silencer. + /// + /// [`Modulation`]: crate::datagram::Modulation + /// [`FociSTM`]: crate::datagram::FociSTM + /// [`GainSTM`]: crate::datagram::GainSTM + pub const fn strict_mode(&self) -> bool { + self.strict_mode + } + + /// Sets the [`strict_mode`]. + /// + /// [`strict_mode`]: Self::strict_mode + pub const fn with_strict_mode(mut self, strict_mode: bool) -> Self { + self.strict_mode = strict_mode; + self + } + + /// Validate whether it is safe to use this [`Silencer`] with `target`. + pub fn is_valid(&self, target: &T) -> bool { + if !self.strict_mode { + return true; + } + self.config.intensity.get() <= target.intensity().map_or(u16::MAX, |c| c.division()) + && self.config.phase.get() <= target.phase().map_or(u16::MAX, |c| c.division()) } } -impl Default for Silencer { +impl Default for Silencer { fn default() -> Self { - Silencer::new(FixedCompletionTime { - intensity: Silencer::DEFAULT_COMPLETION_TIME_INTENSITY, - phase: Silencer::DEFAULT_COMPLETION_TIME_PHASE, + Silencer::new(FixedCompletionSteps { + intensity: NonZeroU16::new(SILENCER_STEPS_INTENSITY_DEFAULT).unwrap(), + phase: NonZeroU16::new(SILENCER_STEPS_PHASE_DEFAULT).unwrap(), }) } } @@ -156,7 +199,25 @@ impl OperationGenerator for SilencerOpGenerator { } } +#[cfg(not(feature = "dynamic_freq"))] impl OperationGenerator for SilencerOpGenerator { + type O1 = crate::firmware::operation::SilencerFixedCompletionTimeOp; + type O2 = NullOp; + + fn generate(&mut self, _: &Device) -> (Self::O1, Self::O2) { + ( + Self::O1::new( + self.config.intensity, + self.config.phase, + self.strict_mode, + self.target, + ), + Self::O2::new(), + ) + } +} + +impl OperationGenerator for SilencerOpGenerator { type O1 = SilencerFixedCompletionStepsOp; type O2 = NullOp; @@ -201,8 +262,8 @@ mod tests { #[test] fn disable() { let s = Silencer::disable(); - assert_eq!(ULTRASOUND_PERIOD, s.config().intensity); - assert_eq!(ULTRASOUND_PERIOD, s.config().phase); + assert_eq!(1, s.config().intensity.get()); + assert_eq!(1, s.config().phase.get()); assert!(s.strict_mode()); assert_eq!(SilencerTarget::Intensity, s.target()); } @@ -218,8 +279,11 @@ mod tests { assert_eq!(SilencerTarget::Intensity, s.target()); } + #[cfg(not(feature = "dynamic_freq"))] #[test] fn from_completion_time() { + use std::time::Duration; + let s = Silencer::new(FixedCompletionTime { intensity: Duration::from_secs(1), phase: Duration::from_secs(1), @@ -227,8 +291,54 @@ mod tests { assert_eq!(Duration::from_secs(1), s.config().intensity); assert_eq!(Duration::from_secs(1), s.config().phase); assert_eq!(SilencerTarget::Intensity, s.target()); + assert!(s.strict_mode()); + } + + #[test] + fn from_completion_steps() { + let s = Silencer::new(FixedCompletionSteps { + intensity: NonZeroU16::new(2).unwrap(), + phase: NonZeroU16::new(3).unwrap(), + }); + assert_eq!(2, s.config().intensity.get()); + assert_eq!(3, s.config().phase.get()); + assert_eq!(SilencerTarget::Intensity, s.target()); + assert!(s.strict_mode()); + } + + #[rstest::rstest] + #[test] + #[case(true, 10, 10, true, FociSTM::new(SamplingConfig::new(10).unwrap(), [Point3::origin()]).unwrap())] + #[case(false, 11, 10, true, FociSTM::new(SamplingConfig::new(10).unwrap(), [Point3::origin()]).unwrap())] + #[case(false, 10, 11, true, FociSTM::new(SamplingConfig::new(10).unwrap(), [Point3::origin()]).unwrap())] + #[case(true, 11, 10, false, FociSTM::new(SamplingConfig::new(10).unwrap(), [Point3::origin()]).unwrap())] + #[case(true, 10, 11, false, FociSTM::new(SamplingConfig::new(10).unwrap(), [Point3::origin()]).unwrap())] + #[case(true, 10, 10, true, GainSTM::new(SamplingConfig::new(10).unwrap(), [TestGain{ data: Default::default() }]).unwrap())] + #[case(false, 11, 10, true, GainSTM::new(SamplingConfig::new(10).unwrap(), [TestGain{ data: Default::default() }]).unwrap())] + #[case(false, 10, 11, true, GainSTM::new(SamplingConfig::new(10).unwrap(), [TestGain{ data: Default::default() }]).unwrap())] + #[case(true, 11, 10, false, GainSTM::new(SamplingConfig::new(10).unwrap(), [TestGain{ data: Default::default() }]).unwrap())] + #[case(true, 10, 11, false, GainSTM::new(SamplingConfig::new(10).unwrap(), [TestGain{ data: Default::default() }]).unwrap())] + #[case(true, 10, 10, true, TestModulation { config: SamplingConfig::new(10).unwrap(), loop_behavior: LoopBehavior::infinite() })] + #[case(false, 11, 10, true, TestModulation { config: SamplingConfig::new(10).unwrap(), loop_behavior: LoopBehavior::infinite() })] + #[case(true, 10, 11, true, TestModulation { config: SamplingConfig::new(10).unwrap(), loop_behavior: LoopBehavior::infinite() })] + #[case(true, 11, 10, false, TestModulation { config: SamplingConfig::new(10).unwrap(), loop_behavior: LoopBehavior::infinite() })] + #[case(true, 10, 11, false, TestModulation { config: SamplingConfig::new(10).unwrap(), loop_behavior: LoopBehavior::infinite() })] + fn fixed_completion_steps_is_valid( + #[case] expect: bool, + #[case] intensity: u16, + #[case] phase: u16, + #[case] strict: bool, + #[case] target: impl HasSamplingConfig, + ) { + let s = Silencer::new(FixedCompletionSteps { + intensity: NonZeroU16::new(intensity).unwrap(), + phase: NonZeroU16::new(phase).unwrap(), + }) + .with_strict_mode(strict); + assert_eq!(expect, s.is_valid(&target)); } + #[cfg(not(feature = "dynamic_freq"))] #[rstest::rstest] #[test] #[case(true, 10, 10, true, FociSTM::new(SamplingConfig::new(10).unwrap(), [Point3::origin()]).unwrap())] @@ -253,9 +363,11 @@ mod tests { #[case] strict: bool, #[case] target: impl HasSamplingConfig, ) { + use crate::defined::ultrasound_period; + let s = Silencer::new(FixedCompletionTime { - intensity: intensity * ULTRASOUND_PERIOD, - phase: phase * ULTRASOUND_PERIOD, + intensity: intensity * ultrasound_period(), + phase: phase * ultrasound_period(), }) .with_strict_mode(strict); assert_eq!(expect, s.is_valid(&target)); diff --git a/autd3-driver/src/datagram/stm/foci/implement.rs b/autd3-driver/src/datagram/stm/foci/implement.rs index d0c11a4c..e45f0b3f 100644 --- a/autd3-driver/src/datagram/stm/foci/implement.rs +++ b/autd3-driver/src/datagram/stm/foci/implement.rs @@ -56,6 +56,7 @@ where #[cfg(test)] mod tests { + #[cfg(not(feature = "dynamic_freq"))] use std::time::Duration; use super::{super::FociSTM, *}; @@ -99,6 +100,7 @@ mod tests { ); } + #[cfg(not(feature = "dynamic_freq"))] #[rstest::rstest] #[test] #[case( @@ -128,6 +130,7 @@ mod tests { ); } + #[cfg(not(feature = "dynamic_freq"))] #[rstest::rstest] #[test] #[case( @@ -180,6 +183,7 @@ mod tests { ); } + #[cfg(not(feature = "dynamic_freq"))] #[rstest::rstest] #[test] #[case(Ok(Duration::from_millis(2000)), 0.5*Hz, 2)] diff --git a/autd3-driver/src/datagram/stm/foci/mod.rs b/autd3-driver/src/datagram/stm/foci/mod.rs index a6156ff8..86ddee6e 100644 --- a/autd3-driver/src/datagram/stm/foci/mod.rs +++ b/autd3-driver/src/datagram/stm/foci/mod.rs @@ -113,9 +113,6 @@ impl> FociSTM { /// let stm = FociSTM::new(1.0 * Hz, vec![Point3::origin(), Point3::origin()])?; /// assert_eq!(1.0 * Hz, stm.freq()); /// - /// let stm = FociSTM::new(std::time::Duration::from_secs(1), vec![Point3::origin(), Point3::origin()])?; - /// assert_eq!(1.0 * Hz, stm.freq()); - /// /// let stm = FociSTM::new(SamplingConfig::new(1.0 * Hz)?, vec![Point3::origin(), Point3::origin()])?; /// assert_eq!(0.5 * Hz, stm.freq()); /// # Ok(()) @@ -147,6 +144,7 @@ impl> FociSTM { /// # Ok(()) /// # } /// ``` + #[cfg(not(feature = "dynamic_freq"))] pub fn period(&self) -> Duration { self.sampling_config().period() * self.gen.len() as u32 } diff --git a/autd3-driver/src/datagram/stm/gain/implement.rs b/autd3-driver/src/datagram/stm/gain/implement.rs index 31807889..8f7131c9 100644 --- a/autd3-driver/src/datagram/stm/gain/implement.rs +++ b/autd3-driver/src/datagram/stm/gain/implement.rs @@ -70,6 +70,7 @@ where #[cfg(test)] mod tests { + #[cfg(not(feature = "dynamic_freq"))] use std::time::Duration; use super::{super::GainSTM, *}; @@ -116,6 +117,7 @@ mod tests { ); } + #[cfg(not(feature = "dynamic_freq"))] #[rstest::rstest] #[test] #[case( @@ -145,6 +147,7 @@ mod tests { ); } + #[cfg(not(feature = "dynamic_freq"))] #[rstest::rstest] #[test] #[case( @@ -201,6 +204,7 @@ mod tests { ); } + #[cfg(not(feature = "dynamic_freq"))] #[rstest::rstest] #[test] #[case(Ok(Duration::from_millis(2000)), 0.5*Hz, 2)] diff --git a/autd3-driver/src/datagram/stm/gain/mod.rs b/autd3-driver/src/datagram/stm/gain/mod.rs index 1a5ff1a1..48a8dab5 100644 --- a/autd3-driver/src/datagram/stm/gain/mod.rs +++ b/autd3-driver/src/datagram/stm/gain/mod.rs @@ -131,6 +131,7 @@ impl GainSTM { /// Returns the period of the STM. See also [`FociSTM::period`]. /// /// [`FociSTM::period`]: crate::datagram::FociSTM::period + #[cfg(not(feature = "dynamic_freq"))] pub fn period(&self) -> Duration { self.sampling_config().period() * self.gen.len() as u32 } diff --git a/autd3-driver/src/datagram/stm/sampling_config.rs b/autd3-driver/src/datagram/stm/sampling_config.rs index 251d53ed..e3b9f7ee 100644 --- a/autd3-driver/src/datagram/stm/sampling_config.rs +++ b/autd3-driver/src/datagram/stm/sampling_config.rs @@ -1,3 +1,4 @@ +#[cfg(not(feature = "dynamic_freq"))] use std::time::Duration; use crate::{ @@ -12,6 +13,7 @@ use crate::{ pub enum STMConfig { #[doc(hidden)] Freq(Freq), + #[cfg(not(feature = "dynamic_freq"))] #[doc(hidden)] Period(Duration), #[doc(hidden)] @@ -24,6 +26,7 @@ pub enum STMConfig { pub enum STMConfigNearest { #[doc(hidden)] Freq(Freq), + #[cfg(not(feature = "dynamic_freq"))] #[doc(hidden)] Period(Duration), } @@ -35,6 +38,7 @@ impl TryFrom<(STMConfig, usize)> for SamplingConfig { let (config, size) = value; match config { STMConfig::Freq(f) => SamplingConfig::new(f * size as f32), + #[cfg(not(feature = "dynamic_freq"))] STMConfig::Period(p) => { if p.as_nanos() % size as u128 != 0 { return Err(AUTDDriverError::STMPeriodInvalid(size, p)); @@ -53,6 +57,7 @@ impl TryFrom<(STMConfigNearest, usize)> for SamplingConfig { let (config, size) = value; match config { STMConfigNearest::Freq(f) => Ok(SamplingConfig::new_nearest(f.hz() * size as f32 * Hz)), + #[cfg(not(feature = "dynamic_freq"))] STMConfigNearest::Period(p) => Ok(SamplingConfig::new_nearest(p / size as u32)), } } @@ -64,6 +69,7 @@ impl From> for STMConfig { } } +#[cfg(not(feature = "dynamic_freq"))] impl From for STMConfig { fn from(p: Duration) -> Self { Self::Period(p) @@ -82,6 +88,7 @@ impl From> for STMConfigNearest { } } +#[cfg(not(feature = "dynamic_freq"))] impl From for STMConfigNearest { fn from(p: Duration) -> Self { Self::Period(p) @@ -120,6 +127,7 @@ mod tests { ); } + #[cfg(not(feature = "dynamic_freq"))] #[rstest::rstest] #[test] #[case( @@ -164,6 +172,7 @@ mod tests { assert_eq!(expect, (STMConfigNearest::Freq(freq), size).try_into()); } + #[cfg(not(feature = "dynamic_freq"))] #[rstest::rstest] #[test] #[case( diff --git a/autd3-driver/src/defined/mod.rs b/autd3-driver/src/defined/mod.rs index 602981df..db2e3ac5 100644 --- a/autd3-driver/src/defined/mod.rs +++ b/autd3-driver/src/defined/mod.rs @@ -2,7 +2,6 @@ mod angle; mod freq; pub use std::f32::consts::PI; -use std::time::Duration; #[cfg(feature = "use_meter")] mod unit { @@ -34,13 +33,72 @@ pub const T4010A1_AMPLITUDE: f32 = 275.574_25 * 200.0 * MILLIMETER; // [㎩*mm] /// The default timeout duration pub const DEFAULT_TIMEOUT: std::time::Duration = std::time::Duration::from_millis(200); -/// The frequency of ultrasound -pub const ULTRASOUND_FREQ: Freq = Freq { freq: 40000 }; -/// The period of ultrasound -pub const ULTRASOUND_PERIOD: Duration = Duration::from_micros(25); /// The period of ultrasound in discrete time units pub const ULTRASOUND_PERIOD_COUNT: usize = 256; +#[cfg(not(feature = "dynamic_freq"))] +mod inner { + use super::Freq; + use std::time::Duration; + + #[inline(always)] + /// The frequency of ultrasound + pub const fn ultrasound_freq() -> Freq { + Freq { freq: 40000 } + } + + #[inline(always)] + /// The period of ultrasound + pub const fn ultrasound_period() -> Duration { + Duration::from_micros(25) + } +} + +#[cfg(feature = "dynamic_freq")] +mod inner { + use std::sync::Once; + + use super::Freq; + use crate::defined::Hz; + + static mut VAL: Freq = Freq { freq: 40000 }; + static FREQ: Once = Once::new(); + + #[inline] + /// The frequency of ultrasound + pub fn ultrasound_freq() -> Freq { + unsafe { + FREQ.call_once(|| { + VAL = match std::env::var("AUTD3_ULTRASOUND_FREQ") { + Ok(freq) => match freq.parse::() { + Ok(freq) => { + tracing::info!("Set ultrasound frequency to {} Hz.", freq); + freq * Hz + } + Err(_) => { + tracing::error!( + "Invalid ultrasound frequency ({} Hz), fallback to 40 kHz.", + freq + ); + Freq { freq: 40000 } + } + }, + Err(_) => { + tracing::warn!("Environment variable AUTD3_ULTRASOUND_FREQ is not set, fallback to 40 kHz."); + Freq { freq: 40000 } + } + }; + }); + VAL + } + } + + #[doc(hidden)] + pub const DRP_ROM_SIZE: usize = 32; +} + +pub use inner::*; + /// \[㎜\] #[allow(non_upper_case_globals)] pub const mm: f32 = MILLIMETER; diff --git a/autd3-driver/src/error.rs b/autd3-driver/src/error.rs index e3f760ba..5e8281fc 100644 --- a/autd3-driver/src/error.rs +++ b/autd3-driver/src/error.rs @@ -2,11 +2,7 @@ use std::time::Duration; use thiserror::Error; -use crate::{ - defined::{Freq, ULTRASOUND_FREQ, ULTRASOUND_PERIOD}, - firmware::cpu::GainSTMMode, - firmware::fpga::*, -}; +use crate::{defined::Freq, firmware::cpu::GainSTMMode, firmware::fpga::*}; /// A interface for error handling in autd3-driver. #[derive(Error, Debug, PartialEq, Clone)] @@ -21,16 +17,10 @@ pub enum AUTDDriverError { ModulationSizeOutOfRange(usize), /// Invalid silencer completion time. - #[error( - "Silencer completion time ({0:?}) must be a multiple of {period:?}", - period = ULTRASOUND_PERIOD - )] + #[error("Silencer completion time ({0:?}) must be a multiple of the ultrasound period")] InvalidSilencerCompletionTime(Duration), /// Silencer completion time is out of range. - #[error( - "Silencer completion time ({0:?}) is out of range ([{min:?}, {max:?}])", - min = ULTRASOUND_PERIOD, - max = ULTRASOUND_PERIOD * 256)] + #[error("Silencer completion time ({0:?}) is out of range")] SilencerCompletionTimeOutOfRange(Duration), /// Unknown group key. @@ -47,16 +37,13 @@ pub enum AUTDDriverError { #[error("Sampling division ({0}) must not be zero")] SamplingDivisionInvalid(u16), /// Invalid sampling frequency. - #[error("Sampling frequency ({0:?}) must divide {period:?}", period = ULTRASOUND_FREQ)] + #[error("Sampling frequency ({0:?}) must divide theultrasound frequency")] SamplingFreqInvalid(Freq), /// Invalid sampling frequency. - #[error("Sampling frequency ({0:?}) must divide {period:?}", period = ULTRASOUND_FREQ)] + #[error("Sampling frequency ({0:?}) must divide the ultrasound frequency")] SamplingFreqInvalidF(Freq), /// Invalid sampling period. - #[error( - "Sampling period ({0:?}) must be a multiple of {period:?}", - period = ULTRASOUND_PERIOD - )] + #[error("Sampling period ({0:?}) must be a multiple of the ultrasound period")] SamplingPeriodInvalid(Duration), /// Sampling frequency is out of range. #[error("Sampling frequency ({0:?}) is out of range ([{1:?}, {2:?}])")] @@ -137,6 +124,11 @@ pub enum AUTDDriverError { #[error("{0}")] WindowsError(#[from] windows::core::Error), + #[cfg(feature = "dynamic_freq")] + #[error("Ultrasound frequency ({0:?}) is not supported")] + /// Invalid ultrasound frequency. + InvalidFrequency(Freq), + /// Not supported tag. /// /// Occurs when the software is not compatible with the firmware. diff --git a/autd3-driver/src/firmware/fpga/mod.rs b/autd3-driver/src/firmware/fpga/mod.rs index ac21f537..059c595f 100644 --- a/autd3-driver/src/firmware/fpga/mod.rs +++ b/autd3-driver/src/firmware/fpga/mod.rs @@ -50,9 +50,9 @@ pub(crate) const FOCI_STM_FIXED_NUM_UPPER_Z: i32 = FOCI_STM_FIXED_NUM_UPPER; pub(crate) const FOCI_STM_FIXED_NUM_LOWER_Z: i32 = FOCI_STM_FIXED_NUM_LOWER; #[doc(hidden)] -pub const SILENCER_STEPS_INTENSITY_DEFAULT: u32 = 10; +pub const SILENCER_STEPS_INTENSITY_DEFAULT: u16 = 10; #[doc(hidden)] -pub const SILENCER_STEPS_PHASE_DEFAULT: u32 = 40; +pub const SILENCER_STEPS_PHASE_DEFAULT: u16 = 40; /// The minimum buffer size of [`Modulation`]. /// diff --git a/autd3-driver/src/firmware/fpga/sampling_config.rs b/autd3-driver/src/firmware/fpga/sampling_config.rs index b322fd98..04e198ac 100644 --- a/autd3-driver/src/firmware/fpga/sampling_config.rs +++ b/autd3-driver/src/firmware/fpga/sampling_config.rs @@ -1,9 +1,9 @@ -use std::{fmt::Debug, time::Duration}; +use std::fmt::Debug; use autd3_derive::Builder; use crate::{ - defined::{Freq, Hz, ULTRASOUND_FREQ, ULTRASOUND_PERIOD}, + defined::{ultrasound_freq, Freq, Hz}, error::AUTDDriverError, utils::float::is_integer, }; @@ -15,7 +15,7 @@ pub struct SamplingConfig { #[get] /// The division number of the sampling frequency. /// - /// The sampling frequency is [`ULTRASOUND_FREQ`] / `division`. + /// The sampling frequency is [`ultrasound_freq`] / `division`. division: u16, } @@ -34,28 +34,33 @@ impl IntoSamplingConfig for u16 { impl IntoSamplingConfig for Freq { fn into_sampling_config(self) -> Result { - if !(FREQ_MIN..=FREQ_MAX).contains(&self) { + const FREQ_MIN: Freq = Freq { freq: 1 }; + if !(FREQ_MIN..=ultrasound_freq()).contains(&self) { return Err(AUTDDriverError::SamplingFreqOutOfRange( - self, FREQ_MIN, FREQ_MAX, + self, + FREQ_MIN, + ultrasound_freq(), )); } - if ULTRASOUND_FREQ.hz() % self.hz() != 0 { + if ultrasound_freq().hz() % self.hz() != 0 { return Err(AUTDDriverError::SamplingFreqInvalid(self)); } Ok(SamplingConfig { - division: (ULTRASOUND_FREQ.hz() / self.hz()) as _, + division: (ultrasound_freq().hz() / self.hz()) as _, }) } } impl IntoSamplingConfig for Freq { fn into_sampling_config(self) -> Result { - if !(FREQ_MIN_F..=FREQ_MAX_F).contains(&self) { + let freq_max = ultrasound_freq().hz() as f32 * Hz; + let freq_min = freq_max / u16::MAX as f32; + if !(freq_min..=freq_max).contains(&self) { return Err(AUTDDriverError::SamplingFreqOutOfRangeF( - self, FREQ_MIN_F, FREQ_MAX_F, + self, freq_min, freq_max, )); } - let div = ULTRASOUND_FREQ.hz() as f32 / self.hz(); + let div = ultrasound_freq().hz() as f32 / self.hz(); if !is_integer(div as _) { return Err(AUTDDriverError::SamplingFreqInvalidF(self)); } @@ -63,18 +68,25 @@ impl IntoSamplingConfig for Freq { } } -impl IntoSamplingConfig for Duration { +#[cfg(not(feature = "dynamic_freq"))] +impl IntoSamplingConfig for std::time::Duration { fn into_sampling_config(self) -> Result { - if !(PERIOD_MIN..=PERIOD_MAX).contains(&self) { + use crate::defined::ultrasound_period; + + let period_min = ultrasound_period(); + let period_max = std::time::Duration::from_micros( + u16::MAX as u64 * ultrasound_period().as_micros() as u64, + ); + if !(period_min..=period_max).contains(&self) { return Err(AUTDDriverError::SamplingPeriodOutOfRange( - self, PERIOD_MIN, PERIOD_MAX, + self, period_min, period_max, )); } - if self.as_nanos() % ULTRASOUND_PERIOD.as_nanos() != 0 { + if self.as_nanos() % ultrasound_period().as_nanos() != 0 { return Err(AUTDDriverError::SamplingPeriodInvalid(self)); } Ok(SamplingConfig { - division: (self.as_nanos() / ULTRASOUND_PERIOD.as_nanos()) as _, + division: (self.as_nanos() / ultrasound_period().as_nanos()) as _, }) } } @@ -86,7 +98,7 @@ pub trait IntoSamplingConfigNearest { impl IntoSamplingConfigNearest for Freq { fn into_sampling_config_nearest(self) -> SamplingConfig { SamplingConfig::new( - (ULTRASOUND_FREQ.hz() as f32 / self.hz()) + (ultrasound_freq().hz() as f32 / self.hz()) .clamp(1.0, u16::MAX as f32) .round() as u16, ) @@ -97,7 +109,7 @@ impl IntoSamplingConfigNearest for Freq { impl IntoSamplingConfigNearest for Freq { fn into_sampling_config_nearest(self) -> SamplingConfig { SamplingConfig::new( - (ULTRASOUND_FREQ.hz() + self.hz() / 2) + (ultrasound_freq().hz() + self.hz() / 2) .checked_div(self.hz()) .unwrap_or(u32::MAX) .clamp(1, u16::MAX as u32) as u16, @@ -106,26 +118,20 @@ impl IntoSamplingConfigNearest for Freq { } } -impl IntoSamplingConfigNearest for Duration { +#[cfg(not(feature = "dynamic_freq"))] +impl IntoSamplingConfigNearest for std::time::Duration { fn into_sampling_config_nearest(self) -> SamplingConfig { + use crate::defined::ultrasound_period; + SamplingConfig::new( - ((self.as_nanos() + ULTRASOUND_PERIOD.as_nanos() / 2) / ULTRASOUND_PERIOD.as_nanos()) - .clamp(1, u16::MAX as u128) as u16, + ((self.as_nanos() + ultrasound_period().as_nanos() / 2) + / ultrasound_period().as_nanos()) + .clamp(1, u16::MAX as u128) as u16, ) .unwrap() } } -const FREQ_MIN: Freq = Freq { freq: 1 }; -const FREQ_MAX: Freq = ULTRASOUND_FREQ; -const FREQ_MIN_F: Freq = Freq { - freq: 40000. / u16::MAX as f32, -}; -const FREQ_MAX_F: Freq = Freq { freq: 40000. }; -const PERIOD_MIN: Duration = ULTRASOUND_PERIOD; -const PERIOD_MAX: Duration = - Duration::from_micros(u16::MAX as u64 * ULTRASOUND_PERIOD.as_micros() as u64); - impl SamplingConfig { /// A [`SamplingConfig`] of 40kHz. pub const FREQ_40K: SamplingConfig = SamplingConfig { division: 1 }; @@ -146,12 +152,13 @@ impl SamplingConfig { /// Gets the sampling frequency. pub fn freq(&self) -> Freq { - ULTRASOUND_FREQ.hz() as f32 / self.division() as f32 * Hz + ultrasound_freq().hz() as f32 / self.division() as f32 * Hz } /// Gets the sampling period. - pub fn period(&self) -> Duration { - ULTRASOUND_PERIOD * self.division() as u32 + #[cfg(not(feature = "dynamic_freq"))] + pub fn period(&self) -> std::time::Duration { + crate::defined::ultrasound_period() * self.division() as u32 } } @@ -172,7 +179,8 @@ impl TryInto for Freq { } } -impl TryInto for Duration { +#[cfg(not(feature = "dynamic_freq"))] +impl TryInto for std::time::Duration { type Error = AUTDDriverError; fn try_into(self) -> Result { @@ -185,6 +193,11 @@ impl TryInto for Duration { mod tests { use crate::defined::Hz; + #[cfg(not(feature = "dynamic_freq"))] + use crate::defined::ultrasound_period; + #[cfg(not(feature = "dynamic_freq"))] + use std::time::Duration; + use super::*; #[rstest::rstest] @@ -195,18 +208,21 @@ mod tests { #[case(Ok(10), 4000 * Hz)] #[case(Ok(1), 40000. * Hz)] #[case(Ok(10), 4000. * Hz)] - #[case(Ok(1), Duration::from_micros(25))] - #[case(Ok(10), Duration::from_micros(250))] #[case(Err(AUTDDriverError::SamplingDivisionInvalid(0)), 0)] - #[case(Err(AUTDDriverError::SamplingFreqInvalid(ULTRASOUND_FREQ - 1 * Hz)), ULTRASOUND_FREQ - 1 * Hz)] - #[case(Err(AUTDDriverError::SamplingFreqOutOfRange(0 * Hz, FREQ_MIN, FREQ_MAX)), 0 * Hz)] - #[case(Err(AUTDDriverError::SamplingFreqOutOfRange(ULTRASOUND_FREQ + 1 * Hz, FREQ_MIN, FREQ_MAX)), ULTRASOUND_FREQ + 1 * Hz)] - #[case(Err(AUTDDriverError::SamplingFreqInvalidF((ULTRASOUND_FREQ.hz() as f32 - 1.) * Hz)), (ULTRASOUND_FREQ.hz() as f32 - 1.) * Hz)] - #[case(Err(AUTDDriverError::SamplingFreqOutOfRangeF(0. * Hz, FREQ_MIN_F, FREQ_MAX_F)), 0. * Hz)] - #[case(Err(AUTDDriverError::SamplingFreqOutOfRangeF(40000. * Hz + 1. * Hz, FREQ_MIN_F, FREQ_MAX_F)), 40000. * Hz + 1. * Hz)] - #[case(Err(AUTDDriverError::SamplingPeriodInvalid(PERIOD_MAX - Duration::from_nanos(1))), PERIOD_MAX - Duration::from_nanos(1))] - #[case(Err(AUTDDriverError::SamplingPeriodOutOfRange(PERIOD_MIN / 2, PERIOD_MIN, PERIOD_MAX)), PERIOD_MIN / 2)] - #[case(Err(AUTDDriverError::SamplingPeriodOutOfRange(PERIOD_MAX * 2, PERIOD_MIN, PERIOD_MAX)), PERIOD_MAX * 2)] + #[case(Err(AUTDDriverError::SamplingFreqInvalid(ultrasound_freq() - 1 * Hz)), ultrasound_freq() - 1 * Hz)] + #[case(Err(AUTDDriverError::SamplingFreqOutOfRange(0 * Hz, 1 * Hz, ultrasound_freq())), 0 * Hz)] + #[case(Err(AUTDDriverError::SamplingFreqOutOfRange(ultrasound_freq() + 1 * Hz, 1 * Hz, ultrasound_freq())), ultrasound_freq() + 1 * Hz)] + #[case(Err(AUTDDriverError::SamplingFreqInvalidF((ultrasound_freq().hz() as f32 - 1.) * Hz)), (ultrasound_freq().hz() as f32 - 1.) * Hz)] + #[case(Err(AUTDDriverError::SamplingFreqOutOfRangeF(0. * Hz, ultrasound_freq().hz() as f32 * Hz / u16::MAX as f32, ultrasound_freq().hz() as f32 * Hz)), 0. * Hz)] + #[case(Err(AUTDDriverError::SamplingFreqOutOfRangeF(40000. * Hz + 1. * Hz, ultrasound_freq().hz() as f32 * Hz / u16::MAX as f32, ultrasound_freq().hz() as f32 * Hz)), 40000. * Hz + 1. * Hz)] + #[cfg_attr(not(feature = "dynamic_freq"), case(Ok(1), Duration::from_micros(25)))] + #[cfg_attr( + not(feature = "dynamic_freq"), + case(Ok(10), Duration::from_micros(250)) + )] + #[cfg_attr(not(feature = "dynamic_freq"), case(Err(AUTDDriverError::SamplingPeriodInvalid(Duration::from_micros(u16::MAX as u64 * ultrasound_period().as_micros() as u64) - Duration::from_nanos(1))), Duration::from_micros(u16::MAX as u64 * ultrasound_period().as_micros() as u64) - Duration::from_nanos(1)))] + #[cfg_attr(not(feature = "dynamic_freq"), case(Err(AUTDDriverError::SamplingPeriodOutOfRange(ultrasound_period() / 2, ultrasound_period(), Duration::from_micros(u16::MAX as u64 * ultrasound_period().as_micros() as u64))), ultrasound_period() / 2))] + #[cfg_attr(not(feature = "dynamic_freq"), case(Err(AUTDDriverError::SamplingPeriodOutOfRange(Duration::from_micros(u16::MAX as u64 * ultrasound_period().as_micros() as u64) * 2, ultrasound_period(), Duration::from_micros(u16::MAX as u64 * ultrasound_period().as_micros() as u64))), Duration::from_micros(u16::MAX as u64 * ultrasound_period().as_micros() as u64) * 2))] fn division( #[case] expect: Result, #[case] value: impl IntoSamplingConfig, @@ -222,8 +238,8 @@ mod tests { #[case(Ok(4000. * Hz), 4000 * Hz)] #[case(Ok(40000. * Hz), 40000. * Hz)] #[case(Ok(4000. * Hz), 4000. * Hz)] - #[case(Ok(40000. * Hz), Duration::from_micros(25))] - #[case(Ok(4000. * Hz), Duration::from_micros(250))] + #[cfg_attr(not(feature = "dynamic_freq"), case(Ok(40000. * Hz), Duration::from_micros(25)))] + #[cfg_attr(not(feature = "dynamic_freq"), case(Ok(4000. * Hz), Duration::from_micros(250)))] fn freq( #[case] expect: Result, AUTDDriverError>, #[case] value: impl IntoSamplingConfig, @@ -231,6 +247,7 @@ mod tests { assert_eq!(expect, SamplingConfig::new(value).map(|c| c.freq())); } + #[cfg(not(feature = "dynamic_freq"))] #[rstest::rstest] #[test] #[case(Ok(Duration::from_micros(25)), 1)] @@ -252,7 +269,7 @@ mod tests { #[test] #[case::min(u16::MAX, (40000. / u16::MAX as f32) * Hz)] #[case::max(1, 40000. * Hz)] - #[case::not_supported_max(1, (ULTRASOUND_FREQ.hz() as f32 - 1.) * Hz)] + #[case::not_supported_max(1, (ultrasound_freq().hz() as f32 - 1.) * Hz)] #[case::out_of_range_min(u16::MAX, 0. * Hz)] #[case::out_of_range_max(1, 40000. * Hz + 1. * Hz)] fn from_freq_f32_nearest(#[case] expected: u16, #[case] freq: Freq) { @@ -263,20 +280,21 @@ mod tests { #[test] #[case::min(40000, 1 * Hz)] #[case::max(1, 40000 * Hz)] - #[case::not_supported_max(1, ULTRASOUND_FREQ - 1 * Hz)] + #[case::not_supported_max(1, ultrasound_freq() - 1 * Hz)] #[case::out_of_range_min(0xFFFF, 0 * Hz)] - #[case::out_of_range_max(1, ULTRASOUND_FREQ + 1 * Hz)] + #[case::out_of_range_max(1, ultrasound_freq() + 1 * Hz)] fn from_freq_u32_nearest(#[case] expected: u16, #[case] freq: Freq) { assert_eq!(expected, SamplingConfig::new_nearest(freq).division()); } + #[cfg(not(feature = "dynamic_freq"))] #[rstest::rstest] #[test] - #[case::min(1, PERIOD_MIN)] - #[case::max(u16::MAX, PERIOD_MAX)] - #[case::not_supported_max(u16::MAX, PERIOD_MAX - Duration::from_nanos(1))] - #[case::out_of_range_min(1, PERIOD_MIN / 2)] - #[case::out_of_range_max(u16::MAX, PERIOD_MAX * 2)] + #[case::min(1, ultrasound_period())] + #[case::max(u16::MAX, Duration::from_micros(u16::MAX as u64 * ultrasound_period().as_micros() as u64))] + #[case::not_supported_max(u16::MAX, Duration::from_micros(u16::MAX as u64 * ultrasound_period().as_micros() as u64) - Duration::from_nanos(1))] + #[case::out_of_range_min(1, ultrasound_period() / 2)] + #[case::out_of_range_max(u16::MAX, Duration::from_micros(u16::MAX as u64 * ultrasound_period().as_micros() as u64) * 2)] fn from_period_nearest(#[case] expected: u16, #[case] p: Duration) { assert_eq!(expected, SamplingConfig::new_nearest(p).division()); } diff --git a/autd3-driver/src/firmware/operation/clock/drp.rs b/autd3-driver/src/firmware/operation/clock/drp.rs new file mode 100644 index 00000000..f1acd77a --- /dev/null +++ b/autd3-driver/src/firmware/operation/clock/drp.rs @@ -0,0 +1,257 @@ +// GRCOV_EXCL_START +pub(crate) const fn round_frac(decimal: u64, precision: u64) -> u64 { + if decimal & (1 << (10 - precision)) != 0 { + decimal + (1 << (10 - precision)) + } else { + decimal + } +} + +pub(crate) fn mmcm_divider(divide: u64) -> u64 { + let mut duty_cycle = 50000; + if divide >= 64 { + let duty_cycle_min = ((divide - 64) * 100_000) / divide; + let duty_cycle_max = ((64.5 / divide as f32) * 100000.) as u64; + if duty_cycle > duty_cycle_max { + duty_cycle = duty_cycle_max; + } + if duty_cycle < duty_cycle_min { + duty_cycle = duty_cycle_min; + } + } + + let duty_cycle_fix = (duty_cycle << 10) / 100_000; + + let mut high_time: u64; + let low_time: u64; + let no_count: u64; + let mut w_edge: u64; + if divide == 1 { + high_time = 1; + w_edge = 0; + low_time = 1; + no_count = 1; + } else { + let temp = round_frac(duty_cycle_fix * divide, 1); + + high_time = (temp & 0b111111100000000000) >> 11; + w_edge = (temp & 0b10000000000) >> 10; + + if high_time == 0 { + high_time = 1; + w_edge = 0; + } + + if high_time == divide { + high_time = divide - 1; + w_edge = 1; + } + + low_time = divide - high_time; + no_count = 0; + }; + + (w_edge << 13) | (no_count << 12) | ((high_time & 0b111111) << 6) | (low_time & 0b111111) +} + +pub(crate) fn mmcm_count_calc(divide: u64) -> u64 { + let div_calc = mmcm_divider(divide); + let phase_calc = 0; + + ((phase_calc & 0b11000000000) << 15) + | ((div_calc & 0b11000000000000) << 10) + | ((phase_calc & 0b111111) << 16) + | ((phase_calc & 0b111000000) << 13) + | (div_calc & 0b111111111111) +} + +pub(crate) const fn mmcm_lock_lookup(divide: u64) -> u64 { + let lookup: [u64; 64] = [ + 0b0011_0001_1011_1110_1000_1111_1010_0100_0000_0001, + 0b0011_0001_1011_1110_1000_1111_1010_0100_0000_0001, + 0b0100_0010_0011_1110_1000_1111_1010_0100_0000_0001, + 0b0101_1010_1111_1110_1000_1111_1010_0100_0000_0001, + 0b0111_0011_1011_1110_1000_1111_1010_0100_0000_0001, + 0b1000_1100_0111_1110_1000_1111_1010_0100_0000_0001, + 0b1001_1100_1111_1110_1000_1111_1010_0100_0000_0001, + 0b1011_0101_1011_1110_1000_1111_1010_0100_0000_0001, + 0b1100_1110_0111_1110_1000_1111_1010_0100_0000_0001, + 0b1110_0111_0011_1110_1000_1111_1010_0100_0000_0001, + 0b1111_1111_1111_1000_0100_1111_1010_0100_0000_0001, + 0b1111_1111_1111_0011_1001_1111_1010_0100_0000_0001, + 0b1111_1111_1110_1110_1110_1111_1010_0100_0000_0001, + 0b1111_1111_1110_1011_1100_1111_1010_0100_0000_0001, + 0b1111_1111_1110_1000_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1110_0111_0001_1111_1010_0100_0000_0001, + 0b1111_1111_1110_0011_1111_1111_1010_0100_0000_0001, + 0b1111_1111_1110_0010_0110_1111_1010_0100_0000_0001, + 0b1111_1111_1110_0000_1101_1111_1010_0100_0000_0001, + 0b1111_1111_1101_1111_0100_1111_1010_0100_0000_0001, + 0b1111_1111_1101_1101_1011_1111_1010_0100_0000_0001, + 0b1111_1111_1101_1100_0010_1111_1010_0100_0000_0001, + 0b1111_1111_1101_1010_1001_1111_1010_0100_0000_0001, + 0b1111_1111_1101_1001_0000_1111_1010_0100_0000_0001, + 0b1111_1111_1101_1001_0000_1111_1010_0100_0000_0001, + 0b1111_1111_1101_0111_0111_1111_1010_0100_0000_0001, + 0b1111_1111_1101_0101_1110_1111_1010_0100_0000_0001, + 0b1111_1111_1101_0101_1110_1111_1010_0100_0000_0001, + 0b1111_1111_1101_0100_0101_1111_1010_0100_0000_0001, + 0b1111_1111_1101_0100_0101_1111_1010_0100_0000_0001, + 0b1111_1111_1101_0010_1100_1111_1010_0100_0000_0001, + 0b1111_1111_1101_0010_1100_1111_1010_0100_0000_0001, + 0b1111_1111_1101_0010_1100_1111_1010_0100_0000_0001, + 0b1111_1111_1101_0001_0011_1111_1010_0100_0000_0001, + 0b1111_1111_1101_0001_0011_1111_1010_0100_0000_0001, + 0b1111_1111_1101_0001_0011_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + 0b1111_1111_1100_1111_1010_1111_1010_0100_0000_0001, + ]; + lookup[divide as usize - 1] +} + +pub(crate) const fn mmcm_filter_lookup(divide: u64) -> u64 { + let lookup_optimized: [u64; 64] = [ + 0b00_1011_1100, + 0b01_0011_1100, + 0b01_0110_1100, + 0b01_1101_1100, + 0b11_0101_1100, + 0b11_1010_1100, + 0b11_1011_0100, + 0b11_1100_1100, + 0b11_1001_0100, + 0b11_1101_0100, + 0b11_1110_0100, + 0b11_0100_0100, + 0b11_1110_0100, + 0b11_1110_0100, + 0b11_1110_0100, + 0b11_1110_0100, + 0b11_1101_0100, + 0b11_1101_0100, + 0b11_0000_0100, + 0b11_0000_0100, + 0b11_0000_0100, + 0b01_0111_0000, + 0b01_0111_0000, + 0b01_0111_0000, + 0b01_0111_0000, + 0b00_1101_0000, + 0b00_1101_0000, + 0b00_1101_0000, + 0b00_1101_0000, + 0b00_1101_0000, + 0b00_1101_0000, + 0b00_1101_0000, + 0b00_1101_0000, + 0b00_1101_0000, + 0b00_1101_0000, + 0b00_1101_0000, + 0b00_1101_0000, + 0b00_1101_0000, + 0b00_1101_0000, + 0b00_1101_0000, + 0b00_1101_0000, + 0b00_1010_0000, + 0b00_1010_0000, + 0b00_1010_0000, + 0b00_1010_0000, + 0b00_1010_0000, + 0b01_1100_0100, + 0b01_1100_0100, + 0b01_0011_0000, + 0b01_0011_0000, + 0b01_0011_0000, + 0b01_0011_0000, + 0b01_1000_0100, + 0b01_1000_0100, + 0b01_0101_1000, + 0b01_0101_1000, + 0b01_0101_1000, + 0b00_1001_0000, + 0b00_1001_0000, + 0b00_1001_0000, + 0b00_1001_0000, + 0b01_0010_1000, + 0b00_1111_0000, + 0b00_1111_0000, + ]; + lookup_optimized[divide as usize - 1] +} + +pub(crate) fn mmcm_frac_count_calc(divide: u64, frac: u64) -> u64 { + let clkout0_divide_frac: u64 = frac / 125; + let clkout0_divide_int: u64 = divide; + + let even_part_high: u64 = clkout0_divide_int >> 1; + let even_part_low: u64 = even_part_high; + + let odd: u64 = clkout0_divide_int - even_part_high - even_part_low; + let odd_and_frac: u64 = 8 * odd + clkout0_divide_frac; + + let lt_frac: u64 = even_part_high - (odd_and_frac <= 9) as u64; + let ht_frac: u64 = even_part_low - (odd_and_frac <= 8) as u64; + + let pm_fall: u64 = ((odd & 0b1111111) << 2) + ((clkout0_divide_frac & 0b110) >> 1); + + let wf_fall_frac = (2..=9).contains(&odd_and_frac) + || ((clkout0_divide_frac == 1) && (clkout0_divide_int == 2)); + let wf_rise_frac = (1..=8).contains(&odd_and_frac); + + let a_per_in_octets = 8 * divide + frac / 125; + let a_phase_in_cycles = 10 * a_per_in_octets / 360000; + let pm_rise_frac = if (a_phase_in_cycles & 0xFF) == 0 { + 0 + } else { + (a_phase_in_cycles & 0xFF) - (a_phase_in_cycles & 0b11111000) + }; + + let dt_calc = (10 * a_per_in_octets / 8) / 360000; + let dt = dt_calc & 0xFF; + + let pm_rise_frac_filtered = if pm_rise_frac >= 8 { + pm_rise_frac - 8 + } else { + pm_rise_frac + }; + + let pm_fall_frac = pm_fall + pm_rise_frac; + let pm_fall_frac_filtered = pm_fall + pm_rise_frac - (pm_fall_frac & 0b11111000); + + let drp_regshared: u64 = + 0b110000 | (pm_fall_frac_filtered & 0b111) << 1 | (wf_fall_frac as u64); + let drp_reg2: u64 = 0b0000_1000_0000_0000 + | (clkout0_divide_frac & 0b111) << 12 + | (wf_rise_frac as u64) << 10 + | (dt & 0b111111); + let drp_reg1: u64 = + (pm_rise_frac_filtered & 0b111) << 13 | (ht_frac & 0b111111) << 6 | (lt_frac & 0b111111); + (drp_regshared << 32) | (drp_reg2 << 16) | drp_reg1 +} +// GRCOV_EXCL_STOP diff --git a/autd3-driver/src/firmware/operation/clock/mod.rs b/autd3-driver/src/firmware/operation/clock/mod.rs new file mode 100644 index 00000000..bbb2412a --- /dev/null +++ b/autd3-driver/src/firmware/operation/clock/mod.rs @@ -0,0 +1,384 @@ +mod drp; + +use zerocopy::{Immutable, IntoBytes}; + +use crate::{ + defined::{Freq, DRP_ROM_SIZE, ULTRASOUND_PERIOD_COUNT}, + error::AUTDDriverError, + firmware::operation::{Operation, TypeTag}, + geometry::Device, +}; + +const DIVCLK_DIVIDE_MIN: u64 = 1; +const DIVCLK_DIVIDE_MAX: u64 = 106; +const MULT_MIN: f32 = 2.0; +const MULT_MAX: f32 = 64.0; +const DIV_MIN: f32 = 1.0; +const DIV_MAX: f32 = 128.0; +const INCREMENTS: f32 = 0.125; +const VCO_MIN: f32 = 600.0e6; +const VCO_MAX: f32 = 1600.0e6; + +#[derive(Clone, Copy)] +#[repr(C)] +#[derive(IntoBytes, Immutable)] +pub struct ClkControlFlags(u8); + +bitflags::bitflags! { + impl ClkControlFlags : u8 { + const NONE = 0; + const BEGIN = 1 << 0; + const END = 1 << 1; + } +} + +#[repr(C, align(2))] +#[derive(IntoBytes, Immutable)] +struct Clk { + tag: TypeTag, + flag: ClkControlFlags, + size: u16, +} + +pub struct ConfigureClockOp { + ultrasound_freq: Freq, + rom: Vec, + remains: usize, +} + +fn calculate_mult_div(frequency: u32) -> Option<(u64, u64, u64)> { + const FPGA_BASE_CLK_FREQ: u64 = 25600000; + + let f = frequency as u64; + let b = FPGA_BASE_CLK_FREQ; + itertools::iproduct!( + DIVCLK_DIVIDE_MIN..=DIVCLK_DIVIDE_MAX, + (MULT_MIN / INCREMENTS) as u64..=(MULT_MAX / INCREMENTS) as u64, + (DIV_MIN / INCREMENTS) as u64..=(DIV_MAX / INCREMENTS) as u64 + ) + .find(|&(div, m, d)| { + if !(VCO_MIN..=VCO_MAX).contains(&(b as f32 * m as f32 * INCREMENTS / div as f32)) { + return false; + } + f * d == b * m + }) +} + +impl ConfigureClockOp { + pub const fn new(ultrasound_freq: Freq) -> Self { + Self { + ultrasound_freq, + rom: vec![], + remains: DRP_ROM_SIZE, + } + } +} + +impl Operation for ConfigureClockOp { + fn pack(&mut self, _: &Device, tx: &mut [u8]) -> Result { + let sent = DRP_ROM_SIZE - self.remains; + + if sent == 0 { + let fpga_clk_freq = self.ultrasound_freq.hz() * ULTRASOUND_PERIOD_COUNT as u32; + if fpga_clk_freq % 125 != 0 { + return Err(AUTDDriverError::InvalidFrequency(self.ultrasound_freq)); + } + let (clkdiv, mult, div) = calculate_mult_div(fpga_clk_freq) + .ok_or(AUTDDriverError::InvalidFrequency(self.ultrasound_freq))?; + + let mut rom = vec![0; DRP_ROM_SIZE]; + + let clkout0_frac = drp::mmcm_frac_count_calc(div / 8, (div % 8) * 125); + let divclk = drp::mmcm_count_calc(clkdiv); + let clkfbout_frac = drp::mmcm_frac_count_calc(mult / 8, (mult % 8) * 125); + let lock = drp::mmcm_lock_lookup(mult / 8); + let digital_filt = drp::mmcm_filter_lookup(mult / 8); + + let clkout_unused = 0x0000400041; + + rom[0] = 0x28_0000_FFFF; + + rom[1] = 0x09_8000_0000 | (clkout0_frac & 0xFFFF0000) >> 16; + rom[2] = 0x08_1000_0000 | (clkout0_frac & 0xFFFF); + + rom[3] = 0x0A_1000_0000 | (clkout_unused & 0xFFFF); + rom[4] = 0x0B_FC00_0000 | (clkout_unused & 0xFFFF0000) >> 16; + + rom[5] = 0x0C_1000_0000 | (clkout_unused & 0xFFFF); + rom[6] = 0x0D_FC00_0000 | (clkout_unused & 0xFFFF0000) >> 16; + + rom[7] = 0x0E_1000_0000 | (clkout_unused & 0xFFFF); + rom[8] = 0x0F_FC00_0000 | (clkout_unused & 0xFFFF0000) >> 16; + + rom[9] = 0x10_1000_0000 | (clkout_unused & 0xFFFF); + rom[10] = 0x11_FC00_0000 | (clkout_unused & 0xFFFF0000) >> 16; + + rom[11] = 0x06_1000_0000 | (clkout_unused & 0xFFFF); + rom[12] = 0x07_C000_0000 + | (clkout_unused & 0xC0000000) >> 16 + | (clkout0_frac & 0xF00000000) >> 22 + | (clkout_unused & 0x3FF0000) >> 16; + + rom[13] = 0x12_1000_0000; + rom[14] = 0x13_C000_0000 + | (clkout_unused & 0xC0000000) >> 16 + | (clkfbout_frac & 0xF00000000) >> 22 + | (clkout_unused & 0x3FF0000) >> 16; + + rom[15] = 0x16_C000_0000 | (divclk & 0xC00000) >> 10 | (divclk & 0xFFF); + + rom[16] = 0x14_1000_0000 | (clkfbout_frac & 0xFFFF); + rom[17] = 0x15_8000_0000 | (clkfbout_frac & 0xFFFF0000) >> 16; + + rom[18] = 0x18_FC00_0000 | (lock & 0x3FF00000) >> 20; + rom[19] = 0x19_8000_0000 | (lock & 0x7C0000000) >> 20 | (lock & 0x3FF); + rom[20] = 0x1A_8000_0000 | (lock & 0xF800000000) >> 25 | (lock & 0xFFC00) >> 10; + + rom[21] = 0x4E_66FF_0000 + | (digital_filt & 0b1000000000) << 6 + | (digital_filt & 0b0110000000) << 4 + | (digital_filt & 0b0001000000) << 2; + + rom[22] = 0x4F_666F_0000 + | (digital_filt & 0b0000100000) << 10 + | (digital_filt & 0b0000011000) << 8 + | (digital_filt & 0b0000000110) << 6 + | (digital_filt & 0b0000000001) << 4; + + rom[31] = 1; + + self.rom = rom; + } + + let size = self + .remains + .min((tx.len() - size_of::()) / size_of::()); + + super::write_to_tx( + tx, + Clk { + tag: TypeTag::ConfigFPGAClock, + flag: if sent == 0 { + ClkControlFlags::BEGIN + } else { + ClkControlFlags::NONE + } | if sent + size == DRP_ROM_SIZE { + ClkControlFlags::END + } else { + ClkControlFlags::NONE + }, + size: size as _, + }, + ); + + tx[size_of::()..] + .chunks_mut(size_of::()) + .zip(self.rom[sent..].iter()) + .for_each(|(dst, &src)| { + super::write_to_tx(dst, src); + }); + + self.remains -= size; + Ok(size_of::() + size * size_of::()) + } + + fn required_size(&self, _: &Device) -> usize { + size_of::() + size_of::() + } + + fn is_done(&self) -> bool { + self.remains == 0 + } +} + +#[cfg(test)] +mod tests { + use std::mem::{offset_of, size_of}; + + use zerocopy::FromBytes; + + use crate::{ + defined::{Freq, Hz}, + geometry::tests::create_device, + }; + + use super::*; + + const NUM_TRANS_IN_UNIT: u8 = 249; + + #[rstest::rstest] + #[test] + #[case::f40k(vec![ + 0x000000280000ffff, + 0x0000000980006c00, + 0x000000081000071c, + 0x0000000a10000041, + 0x0000000bfc000040, + 0x0000000c10000041, + 0x0000000dfc000040, + 0x0000000e10000041, + 0x0000000ffc000040, + 0x0000001010000041, + 0x00000011fc000040, + 0x0000000610000041, + 0x00000007c0001c40, + 0x0000001210000000, + 0x00000013c0003040, + 0x00000016c0001041, + 0x00000014100002cb, + 0x0000001580004800, + 0x00000018fc0001a9, + 0x0000001980007c01, + 0x0000001a80007fe9, + 0x0000004e66ff1100, + 0x0000004f666f9000, + 0x0000000000000000, + 0x0000000000000000, + 0x0000000000000000, + 0x0000000000000000, + 0x0000000000000000, + 0x0000000000000000, + 0x0000000000000000, + 0x0000000000000000, + 0x0000000000000001, + ], 40000*Hz)] + #[case::f41k(vec![ + 0x000000280000ffff, + 0x0000000980004c00, + 0x000000081000079e, + 0x0000000a10000041, + 0x0000000bfc000040, + 0x0000000c10000041, + 0x0000000dfc000040, + 0x0000000e10000041, + 0x0000000ffc000040, + 0x0000001010000041, + 0x00000011fc000040, + 0x0000000610000041, + 0x00000007c0001440, + 0x0000001210000000, + 0x00000013c0003040, + 0x00000016c0001041, + 0x000000141000030c, + 0x0000001580005800, + 0x00000018fc000190, + 0x0000001980007c01, + 0x0000001a80007fe9, + 0x0000004e66ff1100, + 0x0000004f666f9000, + 0x0000000000000000, + 0x0000000000000000, + 0x0000000000000000, + 0x0000000000000000, + 0x0000000000000000, + 0x0000000000000000, + 0x0000000000000000, + 0x0000000000000000, + 0x0000000000000001, + ], 41000*Hz)] + fn config_clk(#[case] expect_rom: Vec, #[case] freq: Freq) { + const FRAME_SIZE: usize = size_of::() + 12 * size_of::(); + + let mut tx = vec![0x00u8; FRAME_SIZE]; + + let device = create_device(0, NUM_TRANS_IN_UNIT); + + let mut op = ConfigureClockOp::new(freq); + + // First frame + { + assert_eq!( + op.required_size(&device), + size_of::() + size_of::() + ); + + assert_eq!(op.remains, DRP_ROM_SIZE); + + assert_eq!( + op.pack(&device, &mut tx), + Ok(size_of::() + 12 * size_of::()) + ); + assert_eq!(op.remains, DRP_ROM_SIZE - 12); + + assert_eq!(TypeTag::ConfigFPGAClock as u8, tx[0]); + assert_eq!(ClkControlFlags::BEGIN.bits(), tx[offset_of!(Clk, flag)]); + assert_eq!(12, tx[offset_of!(Clk, size)]); + (0..12).for_each(|i| { + let offset = size_of::() + i * size_of::(); + assert_eq!( + expect_rom[i], + u64::read_from_bytes(&tx[offset..offset + size_of::()]).unwrap(), + ); + }); + } + + // Second frame + { + assert_eq!( + op.required_size(&device), + size_of::() + size_of::() + ); + + assert_eq!(op.remains, DRP_ROM_SIZE - 12); + + assert_eq!( + op.pack(&device, &mut tx), + Ok(size_of::() + 12 * size_of::()) + ); + + assert_eq!(op.remains, DRP_ROM_SIZE - 24); + + assert_eq!(TypeTag::ConfigFPGAClock as u8, tx[0]); + assert_eq!(ClkControlFlags::NONE.bits(), tx[offset_of!(Clk, flag)]); + assert_eq!(12, tx[offset_of!(Clk, size)]); + (0..12).for_each(|i| { + let offset = size_of::() + i * size_of::(); + assert_eq!( + expect_rom[12 + i], + u64::read_from_bytes(&tx[offset..offset + size_of::()]).unwrap(), + ); + }); + } + + // Final frame + { + assert_eq!( + op.required_size(&device), + size_of::() + size_of::() + ); + + assert_eq!(op.remains, DRP_ROM_SIZE - 12 - 12); + + assert_eq!( + op.pack(&device, &mut tx), + Ok(size_of::() + u64::BITS as usize) + ); + + assert_eq!(op.remains, 0); + + assert_eq!(TypeTag::ConfigFPGAClock as u8, tx[0]); + assert_eq!(ClkControlFlags::END.bits(), tx[offset_of!(Clk, flag)]); + assert_eq!(8, tx[offset_of!(Clk, size)]); + (0..8).for_each(|i| { + let offset = size_of::() + i * size_of::(); + assert_eq!( + expect_rom[24 + i], + u64::read_from_bytes(&tx[offset..offset + size_of::()]).unwrap(), + ); + }); + } + } + + #[rstest::rstest] + #[test] + #[case::f40k(Ok(()), 40000*Hz)] + #[case::f1(Err(AUTDDriverError::InvalidFrequency(1*Hz)), 1*Hz)] + #[case::f32(Err(AUTDDriverError::InvalidFrequency(125*Hz)), 125*Hz)] + fn config_clk_validate(#[case] expect: Result<(), AUTDDriverError>, #[case] freq: Freq) { + let device = create_device(0, NUM_TRANS_IN_UNIT); + let mut tx = vec![0x00u8; size_of::() + DRP_ROM_SIZE * size_of::()]; + + let mut op = ConfigureClockOp::new(freq); + assert_eq!(expect, op.pack(&device, &mut tx).map(|_| ())); + } +} diff --git a/autd3-driver/src/firmware/operation/mod.rs b/autd3-driver/src/firmware/operation/mod.rs index 6d317f00..0394d42e 100644 --- a/autd3-driver/src/firmware/operation/mod.rs +++ b/autd3-driver/src/firmware/operation/mod.rs @@ -1,4 +1,6 @@ mod clear; +#[cfg(feature = "dynamic_freq")] +mod clock; mod cpu_gpio_out; mod debug; mod force_fan; @@ -16,6 +18,8 @@ mod stm; mod sync; pub(crate) use clear::*; +#[cfg(feature = "dynamic_freq")] +pub(crate) use clock::*; pub(crate) use cpu_gpio_out::*; pub(crate) use debug::*; pub(crate) use force_fan::*; @@ -52,6 +56,8 @@ pub(crate) enum TypeTag { Clear = 0x01, Sync = 0x02, FirmwareVersion = 0x03, + #[cfg(feature = "dynamic_freq")] + ConfigFPGAClock = 0x04, Modulation = 0x10, ModulationSwapSegment = 0x11, Silencer = 0x21, diff --git a/autd3-driver/src/firmware/operation/silencer/completion_steps.rs b/autd3-driver/src/firmware/operation/silencer/completion_steps.rs index 24485463..82b39670 100644 --- a/autd3-driver/src/firmware/operation/silencer/completion_steps.rs +++ b/autd3-driver/src/firmware/operation/silencer/completion_steps.rs @@ -1,7 +1,6 @@ -use std::time::Duration; +use std::num::NonZeroU16; use crate::{ - defined::ULTRASOUND_FREQ, error::AUTDDriverError, firmware::{ fpga::SilencerTarget, @@ -29,30 +28,14 @@ struct SilencerFixedCompletionSteps { pub struct SilencerFixedCompletionStepsOp { #[new(default)] is_done: bool, - intensity: Duration, - phase: Duration, + intensity: NonZeroU16, + phase: NonZeroU16, strict_mode: bool, target: SilencerTarget, } impl Operation for SilencerFixedCompletionStepsOp { fn pack(&mut self, _: &Device, tx: &mut [u8]) -> Result { - let validate = |value: Duration| { - const NANOSEC: u128 = 1_000_000_000; - let v = value.as_nanos() * ULTRASOUND_FREQ.hz() as u128; - let v = if v % NANOSEC == 0 { - v / NANOSEC - } else { - return Err(AUTDDriverError::InvalidSilencerCompletionTime(value)); - }; - if v == 0 || v > u16::MAX as _ { - return Err(AUTDDriverError::SilencerCompletionTimeOutOfRange(value)); - } - Ok(v as u16) - }; - let step_intensity = validate(self.intensity)?; - let step_phase = validate(self.phase)?; - super::super::write_to_tx( tx, SilencerFixedCompletionSteps { @@ -65,8 +48,8 @@ impl Operation for SilencerFixedCompletionStepsOp { SilencerTarget::Intensity => SilencerControlFlags::NONE, SilencerTarget::PulseWidth => SilencerControlFlags::PULSE_WIDTH, }, - value_intensity: step_intensity, - value_phase: step_phase, + value_intensity: self.intensity.get(), + value_phase: self.phase.get(), }, ); @@ -88,9 +71,7 @@ mod tests { use std::mem::size_of; use super::*; - use crate::{ - defined::ULTRASOUND_PERIOD, firmware::fpga::SilencerTarget, geometry::tests::create_device, - }; + use crate::{firmware::fpga::SilencerTarget, geometry::tests::create_device}; const NUM_TRANS_IN_UNIT: u8 = 249; @@ -104,8 +85,8 @@ mod tests { let mut tx = [0x00u8; size_of::()]; let mut op = SilencerFixedCompletionStepsOp::new( - ULTRASOUND_PERIOD * 0x12, - ULTRASOUND_PERIOD * 0x34, + NonZeroU16::new(0x12).unwrap(), + NonZeroU16::new(0x34).unwrap(), strict_mode, SilencerTarget::Intensity, ); @@ -127,55 +108,4 @@ mod tests { assert_eq!(tx[4], 0x34); assert_eq!(tx[5], 0x00); } - - #[rstest::rstest] - #[test] - #[case( - AUTDDriverError::SilencerCompletionTimeOutOfRange(Duration::from_micros(0)), - Duration::from_micros(0), - Duration::from_micros(25) - )] - #[case( - AUTDDriverError::SilencerCompletionTimeOutOfRange(Duration::from_micros(25 * 65536)), - Duration::from_micros(25 * 65536), - Duration::from_micros(25) - )] - #[case( - AUTDDriverError::SilencerCompletionTimeOutOfRange(Duration::from_micros(0)), - Duration::from_micros(25), - Duration::from_micros(0) - )] - #[case( - AUTDDriverError::SilencerCompletionTimeOutOfRange(Duration::from_micros(25 * 65536)), - Duration::from_micros(25), - Duration::from_micros(25 * 65536), - )] - #[case( - AUTDDriverError::InvalidSilencerCompletionTime(Duration::from_micros(26)), - Duration::from_micros(26), - Duration::from_micros(50) - )] - #[case( - AUTDDriverError::InvalidSilencerCompletionTime(Duration::from_micros(51)), - Duration::from_micros(25), - Duration::from_micros(51) - )] - fn invalid_time( - #[case] expected: AUTDDriverError, - #[case] time_intensity: Duration, - #[case] time_phase: Duration, - ) { - let device = create_device(0, NUM_TRANS_IN_UNIT); - - let mut tx = [0x00u8; size_of::()]; - - let mut op = SilencerFixedCompletionStepsOp::new( - time_intensity, - time_phase, - true, - SilencerTarget::Intensity, - ); - - assert_eq!(expected, op.pack(&device, &mut tx).unwrap_err()); - } } diff --git a/autd3-driver/src/firmware/operation/silencer/completion_time.rs b/autd3-driver/src/firmware/operation/silencer/completion_time.rs new file mode 100644 index 00000000..a7042175 --- /dev/null +++ b/autd3-driver/src/firmware/operation/silencer/completion_time.rs @@ -0,0 +1,181 @@ +use std::time::Duration; + +use crate::{ + defined::ultrasound_freq, + error::AUTDDriverError, + firmware::{ + fpga::SilencerTarget, + operation::{Operation, TypeTag}, + }, + geometry::Device, +}; + +use super::SilencerControlFlags; + +use derive_new::new; +use zerocopy::{Immutable, IntoBytes}; + +#[repr(C, align(2))] +#[derive(IntoBytes, Immutable)] +struct SilencerFixedCompletionTime { + tag: TypeTag, + flag: SilencerControlFlags, + value_intensity: u16, + value_phase: u16, +} + +#[derive(new)] +#[new(visibility = "pub(crate)")] +pub struct SilencerFixedCompletionTimeOp { + #[new(default)] + is_done: bool, + intensity: Duration, + phase: Duration, + strict_mode: bool, + target: SilencerTarget, +} + +impl Operation for SilencerFixedCompletionTimeOp { + fn pack(&mut self, _: &Device, tx: &mut [u8]) -> Result { + let validate = |value: Duration| { + const NANOSEC: u128 = 1_000_000_000; + let v = value.as_nanos() * ultrasound_freq().hz() as u128; + let v = if v % NANOSEC == 0 { + v / NANOSEC + } else { + return Err(AUTDDriverError::InvalidSilencerCompletionTime(value)); + }; + if v == 0 || v > u16::MAX as _ { + return Err(AUTDDriverError::SilencerCompletionTimeOutOfRange(value)); + } + Ok(v as u16) + }; + let step_intensity = validate(self.intensity)?; + let step_phase = validate(self.phase)?; + + super::super::write_to_tx( + tx, + SilencerFixedCompletionTime { + tag: TypeTag::Silencer, + flag: if self.strict_mode { + SilencerControlFlags::STRICT_MODE + } else { + SilencerControlFlags::NONE + } | match self.target { + SilencerTarget::Intensity => SilencerControlFlags::NONE, + SilencerTarget::PulseWidth => SilencerControlFlags::PULSE_WIDTH, + }, + value_intensity: step_intensity, + value_phase: step_phase, + }, + ); + + self.is_done = true; + Ok(std::mem::size_of::()) + } + + fn required_size(&self, _: &Device) -> usize { + std::mem::size_of::() + } + + fn is_done(&self) -> bool { + self.is_done + } +} + +#[cfg(test)] +mod tests { + use std::mem::size_of; + + use super::*; + use crate::{ + defined::ultrasound_period, firmware::fpga::SilencerTarget, geometry::tests::create_device, + }; + + const NUM_TRANS_IN_UNIT: u8 = 249; + + #[rstest::rstest] + #[test] + #[case(SilencerControlFlags::STRICT_MODE.bits(), true)] + #[case(0x00, false)] + fn test(#[case] value: u8, #[case] strict_mode: bool) { + let device = create_device(0, NUM_TRANS_IN_UNIT); + + let mut tx = [0x00u8; size_of::()]; + + let mut op = SilencerFixedCompletionTimeOp::new( + ultrasound_period() * 0x12, + ultrasound_period() * 0x34, + strict_mode, + SilencerTarget::Intensity, + ); + + assert_eq!( + op.required_size(&device), + size_of::() + ); + assert!(!op.is_done()); + + assert!(op.pack(&device, &mut tx).is_ok()); + + assert!(op.is_done()); + + assert_eq!(tx[0], TypeTag::Silencer as u8); + assert_eq!(tx[1], value); + assert_eq!(tx[2], 0x12); + assert_eq!(tx[3], 0x00); + assert_eq!(tx[4], 0x34); + assert_eq!(tx[5], 0x00); + } + + #[rstest::rstest] + #[test] + #[case( + AUTDDriverError::SilencerCompletionTimeOutOfRange(Duration::from_micros(0)), + Duration::from_micros(0), + Duration::from_micros(25) + )] + #[case( + AUTDDriverError::SilencerCompletionTimeOutOfRange(Duration::from_micros(25 * 65536)), + Duration::from_micros(25 * 65536), + Duration::from_micros(25) + )] + #[case( + AUTDDriverError::SilencerCompletionTimeOutOfRange(Duration::from_micros(0)), + Duration::from_micros(25), + Duration::from_micros(0) + )] + #[case( + AUTDDriverError::SilencerCompletionTimeOutOfRange(Duration::from_micros(25 * 65536)), + Duration::from_micros(25), + Duration::from_micros(25 * 65536), + )] + #[case( + AUTDDriverError::InvalidSilencerCompletionTime(Duration::from_micros(26)), + Duration::from_micros(26), + Duration::from_micros(50) + )] + #[case( + AUTDDriverError::InvalidSilencerCompletionTime(Duration::from_micros(51)), + Duration::from_micros(25), + Duration::from_micros(51) + )] + fn invalid_time( + #[case] expected: AUTDDriverError, + #[case] time_intensity: Duration, + #[case] time_phase: Duration, + ) { + let device = create_device(0, NUM_TRANS_IN_UNIT); + + let mut tx = [0x00u8; size_of::()]; + + let mut op = SilencerFixedCompletionTimeOp::new( + time_intensity, + time_phase, + true, + SilencerTarget::Intensity, + ); + + assert_eq!(expected, op.pack(&device, &mut tx).unwrap_err()); + } +} diff --git a/autd3-driver/src/firmware/operation/silencer/mod.rs b/autd3-driver/src/firmware/operation/silencer/mod.rs index b76a6a9e..5886b804 100644 --- a/autd3-driver/src/firmware/operation/silencer/mod.rs +++ b/autd3-driver/src/firmware/operation/silencer/mod.rs @@ -1,6 +1,10 @@ mod completion_steps; +#[cfg(not(feature = "dynamic_freq"))] +mod completion_time; mod update_rate; +use zerocopy::{Immutable, IntoBytes}; + #[derive(Clone, Copy, PartialEq, Debug, IntoBytes, Immutable)] #[repr(C)] pub struct SilencerControlFlags(u8); @@ -15,5 +19,6 @@ bitflags::bitflags! { } pub use completion_steps::SilencerFixedCompletionStepsOp; +#[cfg(not(feature = "dynamic_freq"))] +pub use completion_time::SilencerFixedCompletionTimeOp; pub use update_rate::SilencerFixedUpdateRateOp; -use zerocopy::{Immutable, IntoBytes}; diff --git a/autd3-driver/src/firmware/operation/sync.rs b/autd3-driver/src/firmware/operation/sync.rs index e6b87f5c..829cbe18 100644 --- a/autd3-driver/src/firmware/operation/sync.rs +++ b/autd3-driver/src/firmware/operation/sync.rs @@ -1,4 +1,5 @@ use crate::{ + defined::ultrasound_freq, error::AUTDDriverError, firmware::operation::{Operation, TypeTag}, geometry::Device, @@ -12,6 +13,8 @@ use zerocopy::{Immutable, IntoBytes}; struct Sync { tag: TypeTag, __: u8, + ufreq_mult: u16, + base_cnt: u16, } #[derive(new)] @@ -23,11 +26,17 @@ pub struct SyncOp { impl Operation for SyncOp { fn pack(&mut self, _: &Device, tx: &mut [u8]) -> Result { + let ultrasound_freq = ultrasound_freq().hz(); + let mult = ultrasound_freq / 125; + let base_cnt = (ultrasound_freq as u64 * 256 * 500) / 1000000; + super::write_to_tx( tx, Sync { tag: TypeTag::Sync, __: 0, + ufreq_mult: mult as _, + base_cnt: base_cnt as _, }, ); diff --git a/autd3-driver/src/firmware/version.rs b/autd3-driver/src/firmware/version.rs index 6f8d96ad..dc2ef123 100644 --- a/autd3-driver/src/firmware/version.rs +++ b/autd3-driver/src/firmware/version.rs @@ -1,6 +1,7 @@ use autd3_derive::Builder; use derive_more::Display; use derive_new::new; +use itertools::Itertools; /// Major version number. #[derive(Debug, Clone, Copy, PartialEq, Eq, Display)] @@ -31,8 +32,7 @@ fn version_map(major: Major, minor: Minor) -> String { } /// FPGA firmware version. -#[derive(Debug, Clone, Copy, PartialEq, Eq, Display, Builder, new)] -#[display("{}{}", version_map(self.major, self.minor), if self.is_emulator() {" [Emulator]"} else { "" })] +#[derive(Debug, Clone, Copy, PartialEq, Eq, Builder, new)] pub struct FPGAVersion { #[get(no_doc)] major: Major, @@ -43,15 +43,39 @@ pub struct FPGAVersion { } impl FPGAVersion { + #[doc(hidden)] + pub const DYNAMIC_FREQ_BIT: u8 = 1 << 1; #[doc(hidden)] pub const ENABLED_EMULATOR_BIT: u8 = 1 << 7; + #[doc(hidden)] + pub const fn dynamic_freq_enabled(&self) -> bool { + (self.function_bits & Self::DYNAMIC_FREQ_BIT) == Self::DYNAMIC_FREQ_BIT + } + #[doc(hidden)] pub const fn is_emulator(&self) -> bool { (self.function_bits & Self::ENABLED_EMULATOR_BIT) == Self::ENABLED_EMULATOR_BIT } } +impl std::fmt::Display for FPGAVersion { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + write!(f, "{}", version_map(self.major, self.minor))?; + let features = [ + self.is_emulator().then_some("Emulator"), + self.dynamic_freq_enabled().then_some("DynamicFreq"), + ] + .iter() + .filter_map(Option::as_ref) + .join(", "); + if !features.is_empty() { + write!(f, " [{}]", features)?; + } + Ok(()) + } +} + /// CPU firmware version. #[derive(Debug, Clone, Copy, PartialEq, Eq, Display, Builder, new)] #[display("{}", version_map(self.major, self.minor))] @@ -194,20 +218,41 @@ mod tests { assert_eq!(info.fpga().function_bits(), 5); } + #[rstest::rstest] #[test] - fn display() { - let info = FirmwareVersion::new( + #[case( + "0: CPU = v0.4, FPGA = v0.5", + FirmwareVersion::new( 0, CPUVersion::new(Major(1), Minor(3)), FPGAVersion::new(Major(2), Minor(4), 0), - ); - assert_eq!(format!("{}", info), "0: CPU = v0.4, FPGA = v0.5"); - - let info = FirmwareVersion::new( + ) + )] + #[case( + "0: CPU = v0.4, FPGA = v0.5 [Emulator]", + FirmwareVersion::new( 0, CPUVersion::new(Major(1), Minor(3)), FPGAVersion::new(Major(2), Minor(4), FPGAVersion::ENABLED_EMULATOR_BIT), - ); - assert_eq!(format!("{}", info), "0: CPU = v0.4, FPGA = v0.5 [Emulator]"); + ) + )] + #[case( + "0: CPU = v0.4, FPGA = v0.5 [DynamicFreq]", + FirmwareVersion::new( + 0, + CPUVersion::new(Major(1), Minor(3)), + FPGAVersion::new(Major(2), Minor(4), FPGAVersion::DYNAMIC_FREQ_BIT), + ) + )] + #[case( + "0: CPU = v0.4, FPGA = v0.5 [Emulator, DynamicFreq]", + FirmwareVersion::new( + 0, + CPUVersion::new(Major(1), Minor(3)), + FPGAVersion::new(Major(2), Minor(4), FPGAVersion::ENABLED_EMULATOR_BIT | FPGAVersion::DYNAMIC_FREQ_BIT), + ) + )] + fn display(#[case] expected: &str, #[case] info: FirmwareVersion) { + assert_eq!(expected, format!("{}", info)); } } diff --git a/autd3-driver/src/geometry/device.rs b/autd3-driver/src/geometry/device.rs index 25d1cddd..f574c882 100644 --- a/autd3-driver/src/geometry/device.rs +++ b/autd3-driver/src/geometry/device.rs @@ -4,7 +4,7 @@ use autd3_derive::Builder; use bvh::aabb::Aabb; use derive_more::{Deref, IntoIterator}; -use crate::defined::{METER, ULTRASOUND_FREQ}; +use crate::defined::{ultrasound_freq, METER}; use super::{ Isometry, Point3, Quaternion, Transducer, Translation, UnitQuaternion, UnitVector3, Vector3, @@ -144,12 +144,12 @@ impl Device { /// Gets the wavelength of the ultrasound. pub fn wavelength(&self) -> f32 { - self.sound_speed / ULTRASOUND_FREQ.hz() as f32 + self.sound_speed / ultrasound_freq().hz() as f32 } /// Gets the wavenumber of the ultrasound. pub fn wavenumber(&self) -> f32 { - 2.0 * PI * ULTRASOUND_FREQ.hz() as f32 / self.sound_speed + 2.0 * PI * ultrasound_freq().hz() as f32 / self.sound_speed } fn get_direction(dir: Vector3, rotation: &UnitQuaternion) -> UnitVector3 { @@ -204,7 +204,7 @@ pub(crate) mod tests { #[case(0)] #[case(1)] fn idx(#[case] expect: u16) { - assert_eq!(expect, create_device(expect, 249).idx() as _); + assert_eq!(expect, create_device(expect, 249).idx() as u16); } #[rstest::rstest] @@ -212,7 +212,7 @@ pub(crate) mod tests { #[case(1)] #[case(249)] fn num_transducers(#[case] n: u8) { - assert_eq!(n, create_device(0, n).num_transducers() as _); + assert_eq!(n, create_device(0, n).num_transducers() as u8); } #[test] diff --git a/autd3-firmware-emulator/Cargo.toml b/autd3-firmware-emulator/Cargo.toml index 45e2bc71..b2007728 100644 --- a/autd3-firmware-emulator/Cargo.toml +++ b/autd3-firmware-emulator/Cargo.toml @@ -26,3 +26,4 @@ rstest = { workspace = true } [features] default = [] async-trait = ["autd3-driver/async-trait"] +dynamic_freq = ["autd3-driver/dynamic_freq"] diff --git a/autd3-firmware-emulator/src/cpu/emulator.rs b/autd3-firmware-emulator/src/cpu/emulator.rs index 3d226348..c78b4d27 100644 --- a/autd3-firmware-emulator/src/cpu/emulator.rs +++ b/autd3-firmware-emulator/src/cpu/emulator.rs @@ -44,6 +44,8 @@ pub struct CPUEmulator { pub(crate) silencer_strict_mode: bool, pub(crate) min_freq_div_intensity: u16, pub(crate) min_freq_div_phase: u16, + #[cfg(feature = "dynamic_freq")] + pub(crate) clk_write: u16, pub(crate) is_rx_data_used: bool, #[get] pub(crate) dc_sys_time: DcSysTime, @@ -80,6 +82,8 @@ impl CPUEmulator { silencer_strict_mode: true, min_freq_div_intensity: 10, min_freq_div_phase: 40, + #[cfg(feature = "dynamic_freq")] + clk_write: 0, is_rx_data_used: false, dc_sys_time: DcSysTime::now(), stm_rep: [0xFFFF, 0xFFFF], @@ -180,6 +184,8 @@ impl CPUEmulator { TAG_CLEAR => self.clear(data), TAG_SYNC => self.synchronize(data), TAG_FIRM_INFO => self.firm_info(data), + #[cfg(feature = "dynamic_freq")] + TAG_CONFIG_FPGA_CLK => self.configure_clk(data), TAG_MODULATION => self.write_mod(data), TAG_MODULATION_CHANGE_SEGMENT => self.change_mod_segment(data), TAG_SILENCER => self.config_silencer(data), diff --git a/autd3-firmware-emulator/src/cpu/operation/clock.rs b/autd3-firmware-emulator/src/cpu/operation/clock.rs new file mode 100644 index 00000000..e3e2b22f --- /dev/null +++ b/autd3-firmware-emulator/src/cpu/operation/clock.rs @@ -0,0 +1,48 @@ +use crate::{cpu::params::*, CPUEmulator}; + +#[repr(C, align(2))] +#[derive(Clone, Copy)] +struct Clk { + tag: u8, + flag: u8, + size: u16, +} + +impl CPUEmulator { + pub(crate) unsafe fn configure_clk(&mut self, data: &[u8]) -> u8 { + let d = Self::cast::(data); + + let size = d.size; + + if (d.flag & CLK_FLAG_BEGIN) == CLK_FLAG_BEGIN { + self.clk_write = 0; + } + + self.bram_cpy( + BRAM_SELECT_CONTROLLER, + (BRAM_CNT_SELECT_CLOCK as u16) << 8 | (self.clk_write << 2), + data[std::mem::size_of::()..].as_ptr() as *const u16, + (size << 2) as usize, + ); + self.clk_write += size; + + if ((d.flag & CLK_FLAG_END) == CLK_FLAG_END) && self.clk_write != 32 { + return ERR_CLK_INCOMPLETE_DATA; + } + + NO_ERR + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn clk_memory_layout() { + assert_eq!(4, std::mem::size_of::()); + assert_eq!(0, std::mem::offset_of!(Clk, tag)); + assert_eq!(1, std::mem::offset_of!(Clk, flag)); + assert_eq!(2, std::mem::offset_of!(Clk, size)); + } +} diff --git a/autd3-firmware-emulator/src/cpu/operation/mod.rs b/autd3-firmware-emulator/src/cpu/operation/mod.rs index aba1f4e8..b2365486 100644 --- a/autd3-firmware-emulator/src/cpu/operation/mod.rs +++ b/autd3-firmware-emulator/src/cpu/operation/mod.rs @@ -7,6 +7,8 @@ use super::params::{ }; mod clear; +#[cfg(feature = "dynamic_freq")] +mod clock; mod cpu_gpio_out; mod debug; mod force_fan; diff --git a/autd3-firmware-emulator/src/cpu/params.rs b/autd3-firmware-emulator/src/cpu/params.rs index e744f717..d2183e52 100644 --- a/autd3-firmware-emulator/src/cpu/params.rs +++ b/autd3-firmware-emulator/src/cpu/params.rs @@ -15,6 +15,7 @@ pub const BRAM_SELECT_STM: u8 = 0x3; pub const BRAM_CNT_SEL_MAIN: u8 = 0x00; pub const BRAM_CNT_SEL_PHASE_CORR: u8 = 0x01; +pub const BRAM_CNT_SELECT_CLOCK: u8 = 0x02; pub const TRANSITION_MODE_SYNC_IDX: u8 = 0x00; pub const TRANSITION_MODE_SYS_TIME: u8 = 0x01; @@ -123,6 +124,7 @@ pub const SILENCER_FLAG_STRICT_MODE: u8 = 1 << 2; pub const TAG_CLEAR: u8 = 0x01; pub const TAG_SYNC: u8 = 0x02; pub const TAG_FIRM_INFO: u8 = 0x03; +pub const TAG_CONFIG_FPGA_CLK: u8 = 0x04; pub const TAG_MODULATION: u8 = 0x10; pub const TAG_MODULATION_CHANGE_SEGMENT: u8 = 0x11; pub const TAG_SILENCER: u8 = 0x21; @@ -167,6 +169,9 @@ pub const GAIN_STM_MODE_INTENSITY_PHASE_FULL: u8 = 0; pub const GAIN_STM_MODE_PHASE_FULL: u8 = 1; pub const GAIN_STM_MODE_PHASE_HALF: u8 = 2; +pub const CLK_FLAG_BEGIN: u8 = 1 << 0; +pub const CLK_FLAG_END: u8 = 1 << 1; + pub const GPIO_IN_FLAG_0: u8 = 1 << 0; pub const GPIO_IN_FLAG_1: u8 = 1 << 1; pub const GPIO_IN_FLAG_2: u8 = 1 << 2; @@ -183,5 +188,6 @@ pub const ERR_INVALID_MODE: u8 = ERR_BIT | 0x07; pub const ERR_INVALID_SEGMENT_TRANSITION: u8 = ERR_BIT | 0x08; pub const ERR_INVALID_PWE_DATA_SIZE: u8 = ERR_BIT | 0x09; pub const ERR_MISS_TRANSITION_TIME: u8 = ERR_BIT | 0x0B; +pub const ERR_CLK_INCOMPLETE_DATA: u8 = ERR_BIT | 0x0D; pub const ERR_INVALID_SILENCER_SETTING: u8 = ERR_BIT | 0x0E; pub const ERR_INVALID_TRANSITION_MODE: u8 = ERR_BIT | 0x0F; diff --git a/autd3-firmware-emulator/src/fpga/emulator/memory.rs b/autd3-firmware-emulator/src/fpga/emulator/memory.rs index e88de68f..d2cc17bd 100644 --- a/autd3-firmware-emulator/src/fpga/emulator/memory.rs +++ b/autd3-firmware-emulator/src/fpga/emulator/memory.rs @@ -16,6 +16,9 @@ pub struct Memory { pub(crate) controller_bram: LazyCell>>, #[get] pub(crate) phase_corr_bram: LazyCell>>, + #[cfg(feature = "dynamic_freq")] + #[get] + pub(crate) drp_bram: LazyCell>>, #[get] pub(crate) modulation_bram: LazyCell>>>, #[get] @@ -41,6 +44,8 @@ impl Memory { phase_corr_bram: LazyCell::new(|| { RefCell::new(vec![0x0000; 256 / std::mem::size_of::()]) }), + #[cfg(feature = "dynamic_freq")] + drp_bram: LazyCell::new(|| RefCell::new(vec![0x0000; 32 * std::mem::size_of::()])), modulation_bram: LazyCell::new(|| { RefCell::new( [ @@ -142,6 +147,8 @@ impl Memory { BRAM_SELECT_CONTROLLER => match addr >> 8 { BRAM_CNT_SEL_MAIN => self.controller_bram_mut()[addr] = data, BRAM_CNT_SEL_PHASE_CORR => self.phase_corr_bram_mut()[addr & 0xFF] = data, + #[cfg(feature = "dynamic_freq")] + BRAM_CNT_SEL_CLOCK => self.drp_bram_mut()[addr & 0xFF] = data, _ => unreachable!(), }, BRAM_SELECT_MOD => { diff --git a/autd3-firmware-emulator/src/fpga/emulator/silencer.rs b/autd3-firmware-emulator/src/fpga/emulator/silencer.rs index 2e8088cd..98aa6ebf 100644 --- a/autd3-firmware-emulator/src/fpga/emulator/silencer.rs +++ b/autd3-firmware-emulator/src/fpga/emulator/silencer.rs @@ -1,8 +1,7 @@ use std::num::NonZeroU16; use autd3_driver::{ - datagram::{FixedCompletionTime, FixedUpdateRate}, - defined::ULTRASOUND_PERIOD, + datagram::{FixedCompletionSteps, FixedUpdateRate}, firmware::fpga::{EmitIntensity, Phase, SilencerTarget}, }; @@ -161,12 +160,16 @@ impl FPGAEmulator { } } - pub fn silencer_completion_steps(&self) -> FixedCompletionTime { - FixedCompletionTime { - intensity: self.mem.controller_bram()[ADDR_SILENCER_COMPLETION_STEPS_INTENSITY] as u32 - * ULTRASOUND_PERIOD, - phase: self.mem.controller_bram()[ADDR_SILENCER_COMPLETION_STEPS_PHASE] as u32 - * ULTRASOUND_PERIOD, + pub fn silencer_completion_steps(&self) -> FixedCompletionSteps { + FixedCompletionSteps { + intensity: NonZeroU16::new( + self.mem.controller_bram()[ADDR_SILENCER_COMPLETION_STEPS_INTENSITY], + ) + .unwrap(), + phase: NonZeroU16::new( + self.mem.controller_bram()[ADDR_SILENCER_COMPLETION_STEPS_PHASE], + ) + .unwrap(), } } @@ -196,8 +199,7 @@ impl FPGAEmulator { value: if self.silencer_fixed_update_rate_mode() { self.silencer_update_rate().phase.get() } else { - (self.silencer_completion_steps().phase.as_micros() / ULTRASOUND_PERIOD.as_micros()) - as u16 + self.silencer_completion_steps().phase.get() }, current_target: initial, diff_mem: 0, @@ -226,8 +228,7 @@ impl FPGAEmulator { value: if self.silencer_fixed_update_rate_mode() { self.silencer_update_rate().phase.get() } else { - (self.silencer_completion_steps().phase.as_micros() / ULTRASOUND_PERIOD.as_micros()) - as u16 + self.silencer_completion_steps().phase.get() }, current_target, diff_mem, @@ -243,8 +244,7 @@ impl FPGAEmulator { value: if self.silencer_fixed_update_rate_mode() { self.silencer_update_rate().intensity.get() } else { - (self.silencer_completion_steps().intensity.as_micros() - / ULTRASOUND_PERIOD.as_micros()) as u16 + self.silencer_completion_steps().intensity.get() }, current_target: initial, diff_mem: 0, @@ -273,8 +273,7 @@ impl FPGAEmulator { value: if self.silencer_fixed_update_rate_mode() { self.silencer_update_rate().intensity.get() } else { - (self.silencer_completion_steps().intensity.as_micros() - / ULTRASOUND_PERIOD.as_micros()) as u16 + self.silencer_completion_steps().intensity.get() }, current_target, diff_mem, diff --git a/autd3-firmware-emulator/src/fpga/params.rs b/autd3-firmware-emulator/src/fpga/params.rs index 192b66df..6c104e20 100644 --- a/autd3-firmware-emulator/src/fpga/params.rs +++ b/autd3-firmware-emulator/src/fpga/params.rs @@ -8,6 +8,7 @@ pub const BRAM_SELECT_STM: u8 = 0x3; pub const BRAM_CNT_SEL_MAIN: usize = 0x00; pub const BRAM_CNT_SEL_PHASE_CORR: usize = 0x01; +pub const BRAM_CNT_SEL_CLOCK: usize = 0x02; pub const TRANSITION_MODE_SYNC_IDX: u8 = 0x00; pub const TRANSITION_MODE_SYS_TIME: u8 = 0x01; diff --git a/autd3-firmware-emulator/tests/op/clear.rs b/autd3-firmware-emulator/tests/op/clear.rs index 7a5f2e81..a0f4383f 100644 --- a/autd3-firmware-emulator/tests/op/clear.rs +++ b/autd3-firmware-emulator/tests/op/clear.rs @@ -8,7 +8,6 @@ use crate::{ use autd3_driver::{ autd3_device::AUTD3, datagram::*, - defined::ULTRASOUND_PERIOD, derive::*, firmware::{ cpu::TxMessage, @@ -41,9 +40,9 @@ fn send_clear() -> anyhow::Result<()> { let mut tx = vec![TxMessage::new_zeroed(); 1]; { - let d = Silencer::new(FixedCompletionTime { - intensity: ULTRASOUND_PERIOD, - phase: ULTRASOUND_PERIOD, + let d = Silencer::new(FixedCompletionSteps { + intensity: NonZeroU16::MIN, + phase: NonZeroU16::MIN, }); assert_eq!(Ok(()), send(&mut cpu, d, &geometry, &mut tx)); @@ -72,10 +71,8 @@ fn send_clear() -> anyhow::Result<()> { assert_eq!(Ok(()), send(&mut cpu, d, &geometry, &mut tx)); let d = FociSTM::new( - SamplingConfig::new( - SILENCER_STEPS_INTENSITY_DEFAULT.max(SILENCER_STEPS_PHASE_DEFAULT) as u16, - ) - .unwrap(), + SamplingConfig::new(SILENCER_STEPS_INTENSITY_DEFAULT.max(SILENCER_STEPS_PHASE_DEFAULT)) + .unwrap(), gen_random_foci::<1>(2), )? .with_segment(Segment::S0, Some(TransitionMode::Ext)); @@ -101,9 +98,9 @@ fn send_clear() -> anyhow::Result<()> { cpu.fpga().silencer_update_rate() ); assert_eq!( - FixedCompletionTime { - intensity: SILENCER_STEPS_INTENSITY_DEFAULT * ULTRASOUND_PERIOD, - phase: SILENCER_STEPS_PHASE_DEFAULT * ULTRASOUND_PERIOD, + FixedCompletionSteps { + intensity: NonZeroU16::new(SILENCER_STEPS_INTENSITY_DEFAULT).unwrap(), + phase: NonZeroU16::new(SILENCER_STEPS_PHASE_DEFAULT).unwrap(), }, cpu.fpga().silencer_completion_steps() ); diff --git a/autd3-firmware-emulator/tests/op/modulation.rs b/autd3-firmware-emulator/tests/op/modulation.rs index 6c707f9e..14d9d02f 100644 --- a/autd3-firmware-emulator/tests/op/modulation.rs +++ b/autd3-firmware-emulator/tests/op/modulation.rs @@ -1,7 +1,7 @@ -use std::time::Duration; +use std::{num::NonZeroU16, time::Duration}; use autd3_driver::{ - datagram::{FixedCompletionTime, IntoDatagramWithSegment, Silencer, SwapSegment}, + datagram::{FixedCompletionSteps, IntoDatagramWithSegment, Silencer, SwapSegment}, derive::*, error::AUTDDriverError, ethercat::{DcSysTime, ECAT_DC_SYS_TIME_BASE}, @@ -94,9 +94,8 @@ fn send_mod( let mut tx = vec![TxMessage::new_zeroed(); 1]; let m: Vec<_> = (0..n).map(|_| rng.gen()).collect(); - let freq_div = rng.gen_range( - SILENCER_STEPS_INTENSITY_DEFAULT.max(SILENCER_STEPS_PHASE_DEFAULT) as u16..=u16::MAX, - ); + let freq_div = rng + .gen_range(SILENCER_STEPS_INTENSITY_DEFAULT.max(SILENCER_STEPS_PHASE_DEFAULT)..=u16::MAX); let d = TestModulation { buf: m.clone(), config: SamplingConfig::new(freq_div).unwrap(), @@ -127,7 +126,7 @@ fn swap_mod_segmemt() -> anyhow::Result<()> { let mut tx = vec![TxMessage::new_zeroed(); 1]; let m: Vec<_> = (0..MOD_BUF_SIZE_MIN).map(|_| 0x00).collect(); - let freq_div = SILENCER_STEPS_INTENSITY_DEFAULT.max(SILENCER_STEPS_PHASE_DEFAULT) as u16; + let freq_div = SILENCER_STEPS_INTENSITY_DEFAULT.max(SILENCER_STEPS_PHASE_DEFAULT); let d = TestModulation { buf: m.clone(), config: SamplingConfig::new(freq_div).unwrap(), @@ -175,20 +174,20 @@ fn mod_freq_div_too_small() -> anyhow::Result<()> { .with_segment(Segment::S0, Some(TransitionMode::Immediate)); assert_eq!(Ok(()), send(&mut cpu, d, &geometry, &mut tx)); - let d = Silencer::::default(); + let d = Silencer::::default(); assert_eq!(Ok(()), send(&mut cpu, d, &geometry, &mut tx)); let d = TestModulation { buf: (0..2).map(|_| u8::MAX).collect(), - config: SamplingConfig::new(SILENCER_STEPS_PHASE_DEFAULT as u16).unwrap(), + config: SamplingConfig::new(SILENCER_STEPS_PHASE_DEFAULT).unwrap(), loop_behavior: LoopBehavior::infinite(), } .with_segment(Segment::S1, None); assert_eq!(Ok(()), send(&mut cpu, d, &geometry, &mut tx)); - let d = Silencer::new(FixedCompletionTime { - intensity: Silencer::DEFAULT_COMPLETION_TIME_PHASE * 2, - phase: Silencer::DEFAULT_COMPLETION_TIME_PHASE, + let d = Silencer::new(FixedCompletionSteps { + intensity: NonZeroU16::new(SILENCER_STEPS_PHASE_DEFAULT * 2).unwrap(), + phase: NonZeroU16::new(SILENCER_STEPS_PHASE_DEFAULT).unwrap(), }); assert_eq!(Ok(()), send(&mut cpu, d, &geometry, &mut tx)); diff --git a/autd3-firmware-emulator/tests/op/silener.rs b/autd3-firmware-emulator/tests/op/silener.rs index c1f0cb98..fe7aec6b 100644 --- a/autd3-firmware-emulator/tests/op/silener.rs +++ b/autd3-firmware-emulator/tests/op/silener.rs @@ -2,7 +2,6 @@ use std::num::NonZeroU16; use autd3_driver::{ datagram::*, - defined::ULTRASOUND_PERIOD, derive::{LoopBehavior, SamplingConfig, Segment, TransitionMode}, error::AUTDDriverError, firmware::{cpu::TxMessage, fpga::SilencerTarget}, @@ -55,8 +54,11 @@ fn send_silencer_fixed_update_rate() -> anyhow::Result<()> { Ok(()) } +#[cfg(not(feature = "dynamic_freq"))] #[test] fn send_silencer_fixed_completion_time() { + use autd3_driver::defined::ultrasound_period; + let mut rng = rand::thread_rng(); let geometry = create_geometry(1); @@ -65,14 +67,21 @@ fn send_silencer_fixed_completion_time() { { let config = FixedCompletionTime { - intensity: rng.gen_range(1..=10) * ULTRASOUND_PERIOD, - phase: rng.gen_range(1..=u8::MAX) as u32 * ULTRASOUND_PERIOD, + intensity: ultrasound_period() * rng.gen_range(1..=10), + phase: ultrasound_period() * rng.gen_range(1..=u8::MAX) as u32, }; let d = Silencer::new(config); assert_eq!(Ok(()), send(&mut cpu, d, &geometry, &mut tx)); - assert_eq!(config, cpu.fpga().silencer_completion_steps()); + assert_eq!( + (config.intensity.as_nanos() / ultrasound_period().as_nanos()) as u16, + cpu.fpga().silencer_completion_steps().intensity.get() + ); + assert_eq!( + (config.phase.as_nanos() / ultrasound_period().as_nanos()) as u16, + cpu.fpga().silencer_completion_steps().phase.get() + ); assert!(cpu.fpga().silencer_fixed_completion_steps_mode()); assert!(cpu.silencer_strict_mode()); assert_eq!(SilencerTarget::Intensity, cpu.fpga().silencer_target()); @@ -80,8 +89,54 @@ fn send_silencer_fixed_completion_time() { { let config = FixedCompletionTime { - intensity: rng.gen_range(1..=10) * ULTRASOUND_PERIOD, - phase: rng.gen_range(1..=u8::MAX) as u32 * ULTRASOUND_PERIOD, + intensity: ultrasound_period() * rng.gen_range(1..=10), + phase: ultrasound_period() * rng.gen_range(1..=u8::MAX) as u32, + }; + let d = Silencer::new(config).with_target(SilencerTarget::PulseWidth); + + assert_eq!(Ok(()), send(&mut cpu, d, &geometry, &mut tx)); + + assert_eq!( + (config.intensity.as_nanos() / ultrasound_period().as_nanos()) as u16, + cpu.fpga().silencer_completion_steps().intensity.get() + ); + assert_eq!( + (config.phase.as_nanos() / ultrasound_period().as_nanos()) as u16, + cpu.fpga().silencer_completion_steps().phase.get() + ); + assert!(cpu.fpga().silencer_fixed_completion_steps_mode()); + assert!(cpu.silencer_strict_mode()); + assert_eq!(SilencerTarget::PulseWidth, cpu.fpga().silencer_target()); + } +} + +#[test] +fn send_silencer_fixed_completion_steps() { + let mut rng = rand::thread_rng(); + + let geometry = create_geometry(1); + let mut cpu = CPUEmulator::new(0, geometry.num_transducers()); + let mut tx = vec![TxMessage::new_zeroed(); 1]; + + { + let config = FixedCompletionSteps { + intensity: NonZeroU16::new(rng.gen_range(1..=10)).unwrap(), + phase: NonZeroU16::new(rng.gen_range(1..=u8::MAX) as u16).unwrap(), + }; + let d = Silencer::new(config); + + assert_eq!(Ok(()), send(&mut cpu, d, &geometry, &mut tx)); + + assert_eq!(config, cpu.fpga().silencer_completion_steps()); + assert!(cpu.fpga().silencer_fixed_completion_steps_mode()); + assert!(cpu.silencer_strict_mode()); + assert_eq!(SilencerTarget::Intensity, cpu.fpga().silencer_target()); + } + + { + let config = FixedCompletionSteps { + intensity: NonZeroU16::new(rng.gen_range(1..=10)).unwrap(), + phase: NonZeroU16::new(rng.gen_range(1..=u8::MAX) as u16).unwrap(), }; let d = Silencer::new(config).with_target(SilencerTarget::PulseWidth); @@ -101,7 +156,7 @@ fn send_silencer_fixed_completion_time() { #[cfg_attr(miri, ignore)] fn silencer_completetion_steps_too_large_mod( #[case] expect: Result<(), AUTDDriverError>, - #[case] steps_intensity: u32, + #[case] steps_intensity: u16, ) -> anyhow::Result<()> { use crate::op::modulation::TestModulation; @@ -109,9 +164,9 @@ fn silencer_completetion_steps_too_large_mod( let mut cpu = CPUEmulator::new(0, geometry.num_transducers()); let mut tx = vec![TxMessage::new_zeroed(); 1]; - let d = Silencer::new(FixedCompletionTime { - intensity: ULTRASOUND_PERIOD, - phase: ULTRASOUND_PERIOD, + let d = Silencer::new(FixedCompletionSteps { + intensity: NonZeroU16::MIN, + phase: NonZeroU16::MIN, }); assert_eq!(Ok(()), send(&mut cpu, d, &geometry, &mut tx)); @@ -128,9 +183,9 @@ fn silencer_completetion_steps_too_large_mod( } let steps_phase = 1; - let d = Silencer::new(FixedCompletionTime { - intensity: ULTRASOUND_PERIOD * steps_intensity, - phase: ULTRASOUND_PERIOD * steps_phase, + let d = Silencer::new(FixedCompletionSteps { + intensity: NonZeroU16::new(steps_intensity).unwrap(), + phase: NonZeroU16::new(steps_phase).unwrap(), }); assert_eq!(expect, send(&mut cpu, d, &geometry, &mut tx)); @@ -146,16 +201,16 @@ fn silencer_completetion_steps_too_large_mod( #[cfg_attr(miri, ignore)] fn silencer_completetion_steps_too_large_stm( #[case] expect: Result<(), AUTDDriverError>, - #[case] steps_intensity: u32, - #[case] steps_phase: u32, + #[case] steps_intensity: u16, + #[case] steps_phase: u16, ) -> anyhow::Result<()> { let geometry = create_geometry(1); let mut cpu = CPUEmulator::new(0, geometry.num_transducers()); let mut tx = vec![TxMessage::new_zeroed(); 1]; - let d = Silencer::new(FixedCompletionTime { - intensity: ULTRASOUND_PERIOD, - phase: ULTRASOUND_PERIOD, + let d = Silencer::new(FixedCompletionSteps { + intensity: NonZeroU16::MIN, + phase: NonZeroU16::MIN, }); assert_eq!(Ok(()), send(&mut cpu, d, &geometry, &mut tx)); @@ -167,9 +222,9 @@ fn silencer_completetion_steps_too_large_stm( assert_eq!(Ok(()), send(&mut cpu, d, &geometry, &mut tx)); } - let d = Silencer::new(FixedCompletionTime { - intensity: ULTRASOUND_PERIOD * steps_intensity, - phase: ULTRASOUND_PERIOD * steps_phase, + let d = Silencer::new(FixedCompletionSteps { + intensity: NonZeroU16::new(steps_intensity).unwrap(), + phase: NonZeroU16::new(steps_phase).unwrap(), }); assert_eq!(expect, send(&mut cpu, d, &geometry, &mut tx)); @@ -186,9 +241,9 @@ fn send_silencer_fixed_completion_steps_permissive() -> anyhow::Result<()> { let mut cpu = CPUEmulator::new(0, geometry.num_transducers()); let mut tx = vec![TxMessage::new_zeroed(); 1]; - let config = FixedCompletionTime { - intensity: ULTRASOUND_PERIOD * rng.gen_range(1..=u8::MAX as u32), - phase: ULTRASOUND_PERIOD * rng.gen_range(1..=u8::MAX as u32), + let config = FixedCompletionSteps { + intensity: NonZeroU16::new(rng.gen_range(1..=u16::MAX)).unwrap(), + phase: NonZeroU16::new(rng.gen_range(1..=u16::MAX)).unwrap(), }; let d = Silencer::new(config).with_strict_mode(false); @@ -210,9 +265,9 @@ fn send_silencer_fixed_completion_time_permissive() { let mut cpu = CPUEmulator::new(0, geometry.num_transducers()); let mut tx = vec![TxMessage::new_zeroed(); 1]; - let config = FixedCompletionTime { - intensity: rng.gen_range(1..=u8::MAX) as u32 * ULTRASOUND_PERIOD, - phase: rng.gen_range(1..=u8::MAX) as u32 * ULTRASOUND_PERIOD, + let config = FixedCompletionSteps { + intensity: NonZeroU16::new(rng.gen_range(1..=u16::MAX)).unwrap(), + phase: NonZeroU16::new(rng.gen_range(1..=u16::MAX)).unwrap(), }; let d = Silencer::new(config).with_strict_mode(false); diff --git a/autd3-firmware-emulator/tests/op/stm/foci.rs b/autd3-firmware-emulator/tests/op/stm/foci.rs index 7c425870..3635ec03 100644 --- a/autd3-firmware-emulator/tests/op/stm/foci.rs +++ b/autd3-firmware-emulator/tests/op/stm/foci.rs @@ -1,8 +1,8 @@ -use std::{collections::HashMap, time::Duration}; +use std::{collections::HashMap, num::NonZeroU16, time::Duration}; use autd3_driver::{ datagram::{ - ControlPoint, ControlPoints, FixedCompletionTime, FociSTM, GainSTM, + ControlPoint, ControlPoints, FixedCompletionSteps, FociSTM, GainSTM, IntoDatagramWithSegment, Silencer, SwapSegment, }, defined::{mm, METER}, @@ -180,14 +180,12 @@ fn test_foci_stm_freq_div_too_small() -> anyhow::Result<()> { }; assert_eq!(Ok(()), send(&mut cpu, g, &geometry, &mut tx)); - let d = Silencer::::default(); + let d = Silencer::::default(); assert_eq!(Ok(()), send(&mut cpu, d, &geometry, &mut tx)); let stm = FociSTM::new( - SamplingConfig::new( - SILENCER_STEPS_INTENSITY_DEFAULT.max(SILENCER_STEPS_PHASE_DEFAULT) as u16, - ) - .unwrap(), + SamplingConfig::new(SILENCER_STEPS_INTENSITY_DEFAULT.max(SILENCER_STEPS_PHASE_DEFAULT)) + .unwrap(), gen_random_foci::<1>(2), )? .with_loop_behavior(LoopBehavior::infinite()) @@ -195,9 +193,9 @@ fn test_foci_stm_freq_div_too_small() -> anyhow::Result<()> { assert_eq!(Ok(()), send(&mut cpu, stm, &geometry, &mut tx)); - let d = Silencer::new(FixedCompletionTime { - intensity: Silencer::DEFAULT_COMPLETION_TIME_INTENSITY, - phase: Silencer::DEFAULT_COMPLETION_TIME_PHASE * 2, + let d = Silencer::new(FixedCompletionSteps { + intensity: NonZeroU16::new(SILENCER_STEPS_INTENSITY_DEFAULT).unwrap(), + phase: NonZeroU16::new(SILENCER_STEPS_PHASE_DEFAULT * 2).unwrap(), }); assert_eq!(Ok(()), send(&mut cpu, d, &geometry, &mut tx)); diff --git a/autd3-firmware-emulator/tests/op/stm/gain.rs b/autd3-firmware-emulator/tests/op/stm/gain.rs index 4e1d2763..5d3ee9da 100644 --- a/autd3-firmware-emulator/tests/op/stm/gain.rs +++ b/autd3-firmware-emulator/tests/op/stm/gain.rs @@ -1,8 +1,8 @@ -use std::collections::HashMap; +use std::{collections::HashMap, num::NonZeroU16}; use autd3_driver::{ datagram::{ - ControlPoint, FixedCompletionTime, FociSTM, GainSTM, IntoDatagramWithSegment, Silencer, + ControlPoint, FixedCompletionSteps, FociSTM, GainSTM, IntoDatagramWithSegment, Silencer, SwapSegment, }, derive::*, @@ -132,10 +132,8 @@ fn send_gain_stm_phase_full(#[case] n: usize) -> anyhow::Result<()> { let segment = Segment::S1; let transition_mode = TransitionMode::Ext; let d = GainSTM::new( - SamplingConfig::new( - SILENCER_STEPS_INTENSITY_DEFAULT.max(SILENCER_STEPS_PHASE_DEFAULT) as u16, - ) - .unwrap(), + SamplingConfig::new(SILENCER_STEPS_INTENSITY_DEFAULT.max(SILENCER_STEPS_PHASE_DEFAULT)) + .unwrap(), bufs.iter().map(|buf| TestGain { data: buf.clone() }), )? .with_mode(GainSTMMode::PhaseFull) @@ -191,10 +189,8 @@ fn send_gain_stm_phase_half(#[case] n: usize) -> anyhow::Result<()> { let loop_behavior = LoopBehavior::once(); let transition_mode = TransitionMode::GPIO(gpio); let d = GainSTM::new( - SamplingConfig::new( - SILENCER_STEPS_INTENSITY_DEFAULT.max(SILENCER_STEPS_PHASE_DEFAULT) as u16, - ) - .unwrap(), + SamplingConfig::new(SILENCER_STEPS_INTENSITY_DEFAULT.max(SILENCER_STEPS_PHASE_DEFAULT)) + .unwrap(), bufs.iter().map(|buf| TestGain { data: buf.clone() }), )? .with_mode(GainSTMMode::PhaseHalf) @@ -284,14 +280,12 @@ fn gain_stm_freq_div_too_small() -> anyhow::Result<()> { .with_segment(Segment::S0, Some(TransitionMode::Immediate)); assert_eq!(Ok(()), send(&mut cpu, d, &geometry, &mut tx)); - let d = Silencer::::default(); + let d = Silencer::::default(); assert_eq!(Ok(()), send(&mut cpu, d, &geometry, &mut tx)); let d = GainSTM::new( - SamplingConfig::new( - SILENCER_STEPS_INTENSITY_DEFAULT.max(SILENCER_STEPS_PHASE_DEFAULT) as u16, - ) - .unwrap(), + SamplingConfig::new(SILENCER_STEPS_INTENSITY_DEFAULT.max(SILENCER_STEPS_PHASE_DEFAULT)) + .unwrap(), gen_random_buf(2, &geometry) .into_iter() .map(|buf| TestGain { data: buf.clone() }), @@ -299,9 +293,9 @@ fn gain_stm_freq_div_too_small() -> anyhow::Result<()> { .with_segment(Segment::S1, None); assert_eq!(Ok(()), send(&mut cpu, d, &geometry, &mut tx)); - let d = Silencer::new(FixedCompletionTime { - intensity: Silencer::DEFAULT_COMPLETION_TIME_INTENSITY, - phase: Silencer::DEFAULT_COMPLETION_TIME_PHASE * 2, + let d = Silencer::new(FixedCompletionSteps { + intensity: NonZeroU16::new(SILENCER_STEPS_INTENSITY_DEFAULT).unwrap(), + phase: NonZeroU16::new(SILENCER_STEPS_PHASE_DEFAULT * 2).unwrap(), }); assert_eq!(Ok(()), send(&mut cpu, d, &geometry, &mut tx)); diff --git a/autd3/Cargo.toml b/autd3/Cargo.toml index 7f10994c..b80d954a 100644 --- a/autd3/Cargo.toml +++ b/autd3/Cargo.toml @@ -31,6 +31,7 @@ windows = { workspace = true, features = ["Win32_Security"] } [features] default = [] async-trait = ["autd3-driver/async-trait"] +dynamic_freq = ["autd3-driver/dynamic_freq", "autd3-firmware-emulator/dynamic_freq"] [dev-dependencies] rand = { workspace = true } @@ -40,5 +41,5 @@ rstest = { workspace = true } tokio-test = { workspace = true } [package.metadata.docs.rs] -all-features = true +features = ["async-trait"] rustdoc-args = ["--cfg", "docsrs"] diff --git a/autd3/src/controller/mod.rs b/autd3/src/controller/mod.rs index 597a7f1b..915c44a9 100644 --- a/autd3/src/controller/mod.rs +++ b/autd3/src/controller/mod.rs @@ -76,6 +76,16 @@ impl Controller { pub(crate) async fn open_impl(mut self, timeout: Duration) -> Result { let timeout = Some(timeout); + #[cfg(feature = "dynamic_freq")] + { + tracing::debug!( + "Configuring ultrasound frequency to {:?}", + autd3_driver::defined::ultrasound_freq() + ); + self.send(autd3_driver::datagram::ConfigureFPGAClock::new().with_timeout(timeout)) + .await?; + } + // If the device is used continuously without powering off, the first data may be ignored because the first msg_id equals to the remaining msg_id in the device. // Therefore, send a meaningless data (here, we use `ForceFan` because it is the lightest). let _ = self diff --git a/autd3/src/datagram/modulation/sampling_mode.rs b/autd3/src/datagram/modulation/sampling_mode.rs index 02fc8a07..1bd9e3e9 100644 --- a/autd3/src/datagram/modulation/sampling_mode.rs +++ b/autd3/src/datagram/modulation/sampling_mode.rs @@ -1,5 +1,5 @@ use autd3_driver::{ - defined::{Freq, Hz, ULTRASOUND_FREQ}, + defined::{ultrasound_freq, Freq, Hz}, derive::SamplingConfig, error::AUTDDriverError, firmware::fpga::MOD_BUF_SIZE_MAX, @@ -47,7 +47,7 @@ impl SamplingMode for ExactFreq { } let fd = freq.hz() as u64 * sampling_config.division() as u64; - let fs = ULTRASOUND_FREQ.hz() as u64; + let fs = ultrasound_freq().hz() as u64; let k = gcd(fs, fd); Ok((fs / k, fd / k)) @@ -84,12 +84,12 @@ impl SamplingMode for ExactFreqFloat { } let fd = freq.hz() as f64 * sampling_config.division() as f64; - for n in (ULTRASOUND_FREQ.hz() as f64 / fd).floor() as u32..=MOD_BUF_SIZE_MAX as u32 { + for n in (ultrasound_freq().hz() as f64 / fd).floor() as u32..=MOD_BUF_SIZE_MAX as u32 { if !is_integer(fd * n as f64) { continue; } let fnd = (fd * n as f64) as u64; - let fs = ULTRASOUND_FREQ.hz() as u64; + let fs = ultrasound_freq().hz() as u64; if fnd % fs != 0 { continue; } diff --git a/autd3/src/prelude.rs b/autd3/src/prelude.rs index 353fc509..13dcc96b 100644 --- a/autd3/src/prelude.rs +++ b/autd3/src/prelude.rs @@ -12,12 +12,12 @@ pub use crate::{ pub use autd3_driver::{ autd3_device::AUTD3, datagram::{ - Clear, ControlPoint, ControlPoints, DebugSettings, FixedCompletionTime, FixedUpdateRate, - FociSTM, ForceFan, GainSTM, IntoDatagramWithParallelThreshold, IntoDatagramWithSegment, + Clear, ControlPoint, ControlPoints, DebugSettings, FixedUpdateRate, FociSTM, ForceFan, + GainSTM, IntoDatagramWithParallelThreshold, IntoDatagramWithSegment, IntoDatagramWithTimeout, Modulation, ModulationProperty, PhaseCorrection, PulseWidthEncoder, ReadsFPGAState, Silencer, SwapSegment, }, - defined::{deg, kHz, mm, rad, Hz, PI, ULTRASOUND_FREQ, ULTRASOUND_PERIOD}, + defined::{deg, kHz, mm, rad, ultrasound_freq, Hz, PI}, error::AUTDDriverError, ethercat::DcSysTime, firmware::{ @@ -29,3 +29,6 @@ pub use autd3_driver::{ }, geometry::{EulerAngle, Geometry, Point3, Quaternion, UnitQuaternion, UnitVector3, Vector3}, }; + +#[cfg(not(feature = "dynamic_freq"))] +pub use autd3_driver::datagram::FixedCompletionTime; diff --git a/examples/Cargo.toml b/examples/Cargo.toml index d66cffc6..eab0013a 100644 --- a/examples/Cargo.toml +++ b/examples/Cargo.toml @@ -60,3 +60,4 @@ lightweight = ["autd3-protobuf/lightweight", "autd3-driver/async-trait", "autd3- lightweight-server = ["autd3-protobuf/lightweight", "autd3-link-twincat/local", "autd3-link-twincat/async-trait", "autd3-driver/async-trait", "autd3-driver/lightweight", "tonic", "tokio/signal"] all = ["twincat", "remote_twincat", "simulator"] unity = ["autd3-driver/use_meter", "autd3-driver/left_handed"] +dynamic_freq = ["autd3-driver/dynamic_freq"]