autd3_core/firmware/
sampling_config.rs

1use core::{fmt::Debug, num::NonZeroU16, time::Duration};
2
3use crate::{
4    common::{Freq, Hz, ULTRASOUND_FREQ},
5    utils::float::is_integer,
6};
7
8#[derive(Debug, PartialEq, Copy, Clone)]
9/// An error produced by the sampling configuration.
10pub enum SamplingConfigError {
11    /// Invalid sampling frequency.
12    FreqInvalid(Freq<u32>),
13    /// Invalid sampling frequency.
14    FreqInvalidF(Freq<f32>),
15    /// Invalid sampling period.
16    PeriodInvalid(Duration),
17    /// Sampling frequency is out of range.
18    FreqOutOfRange(Freq<u32>, Freq<u32>, Freq<u32>),
19    /// Sampling frequency is out of range.
20    FreqOutOfRangeF(Freq<f32>, Freq<f32>, Freq<f32>),
21    /// Sampling period is out of range.
22    PeriodOutOfRange(Duration, Duration, Duration),
23}
24
25impl core::fmt::Display for SamplingConfigError {
26    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
27        match self {
28            SamplingConfigError::FreqInvalid(freq) => {
29                write!(
30                    f,
31                    "Sampling frequency ({:?}) must divide the ultrasound frequency",
32                    freq
33                )
34            }
35            SamplingConfigError::FreqInvalidF(freq) => {
36                write!(
37                    f,
38                    "Sampling frequency ({:?}) must divide the ultrasound frequency",
39                    freq
40                )
41            }
42            SamplingConfigError::PeriodInvalid(period) => {
43                write!(
44                    f,
45                    "Sampling period ({:?}) must be a multiple of the ultrasound period",
46                    period
47                )
48            }
49            SamplingConfigError::FreqOutOfRange(freq, min, max) => {
50                write!(
51                    f,
52                    "Sampling frequency ({:?}) is out of range ([{:?}, {:?}])",
53                    freq, min, max
54                )
55            }
56            SamplingConfigError::FreqOutOfRangeF(freq, min, max) => {
57                write!(
58                    f,
59                    "Sampling frequency ({:?}) is out of range ([{:?}, {:?}])",
60                    freq, min, max
61                )
62            }
63            SamplingConfigError::PeriodOutOfRange(period, min, max) => {
64                write!(
65                    f,
66                    "Sampling period ({:?}) is out of range ([{:?}, {:?}])",
67                    period, min, max
68                )
69            }
70        }
71    }
72}
73
74impl core::error::Error for SamplingConfigError {}
75
76/// Nearest type.
77#[derive(Copy, Clone, Debug, PartialEq)]
78pub struct Nearest<T: Copy + Clone + Debug + PartialEq>(pub T);
79
80/// The configuration for sampling.
81#[derive(Clone, Copy)]
82pub enum SamplingConfig {
83    #[doc(hidden)]
84    Divide(NonZeroU16),
85    #[doc(hidden)]
86    Freq(Freq<f32>),
87    #[doc(hidden)]
88    Period(core::time::Duration),
89    #[doc(hidden)]
90    FreqNearest(Nearest<Freq<f32>>),
91    #[doc(hidden)]
92    PeriodNearest(Nearest<core::time::Duration>),
93}
94
95impl PartialEq for SamplingConfig {
96    fn eq(&self, other: &Self) -> bool {
97        match (self.divide(), other.divide()) {
98            (Ok(lhs), Ok(rhs)) => lhs == rhs,
99            _ => false,
100        }
101    }
102}
103
104impl core::fmt::Debug for SamplingConfig {
105    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
106        match self {
107            SamplingConfig::Divide(div) => write!(f, "SamplingConfig::Divide({div})"),
108            SamplingConfig::Freq(freq) => write!(f, "SamplingConfig::Freq({freq:?})"),
109            SamplingConfig::Period(period) => write!(f, "SamplingConfig::Period({period:?})"),
110            SamplingConfig::FreqNearest(nearest) => {
111                write!(f, "SamplingConfig::FreqNearest({nearest:?})")
112            }
113            SamplingConfig::PeriodNearest(nearest) => {
114                write!(f, "SamplingConfig::PeriodNearest({nearest:?})")
115            }
116        }
117    }
118}
119
120impl From<NonZeroU16> for SamplingConfig {
121    fn from(value: NonZeroU16) -> Self {
122        Self::Divide(value)
123    }
124}
125
126impl From<Freq<f32>> for SamplingConfig {
127    fn from(value: Freq<f32>) -> Self {
128        Self::Freq(value)
129    }
130}
131
132impl From<core::time::Duration> for SamplingConfig {
133    fn from(value: core::time::Duration) -> Self {
134        Self::Period(value)
135    }
136}
137
138impl SamplingConfig {
139    /// A [`SamplingConfig`] of 40kHz.
140    pub const FREQ_40K: Self = SamplingConfig::Freq(Freq { freq: 40000. });
141    /// A [`SamplingConfig`] of 4kHz.
142    pub const FREQ_4K: Self = SamplingConfig::Freq(Freq { freq: 4000. });
143
144    /// Creates a new [`SamplingConfig`].
145    #[must_use]
146    pub fn new(value: impl Into<SamplingConfig>) -> Self {
147        value.into()
148    }
149
150    /// The divide number of the sampling frequency.
151    ///
152    /// The sampling frequency is [`ULTRASOUND_FREQ`] / `divide`.
153    pub fn divide(&self) -> Result<u16, SamplingConfigError> {
154        match *self {
155            SamplingConfig::Divide(div) => Ok(div.get()),
156            SamplingConfig::Freq(freq) => {
157                let freq_max = ULTRASOUND_FREQ.hz() as f32 * Hz;
158                let freq_min = freq_max / u16::MAX as f32;
159                if !(freq_min..=freq_max).contains(&freq) {
160                    return Err(SamplingConfigError::FreqOutOfRangeF(
161                        freq, freq_min, freq_max,
162                    ));
163                }
164                let divide = ULTRASOUND_FREQ.hz() as f32 / freq.hz();
165                if !is_integer(divide as _) {
166                    return Err(SamplingConfigError::FreqInvalidF(freq));
167                }
168                Ok(divide as _)
169            }
170            SamplingConfig::Period(duration) => {
171                use crate::common::ULTRASOUND_PERIOD;
172
173                let period_min = ULTRASOUND_PERIOD;
174                let period_max = core::time::Duration::from_micros(
175                    u16::MAX as u64 * ULTRASOUND_PERIOD.as_micros() as u64,
176                );
177                if !(period_min..=period_max).contains(&duration) {
178                    return Err(SamplingConfigError::PeriodOutOfRange(
179                        duration, period_min, period_max,
180                    ));
181                }
182                if duration.as_nanos() % ULTRASOUND_PERIOD.as_nanos() != 0 {
183                    return Err(SamplingConfigError::PeriodInvalid(duration));
184                }
185                Ok((duration.as_nanos() / ULTRASOUND_PERIOD.as_nanos()) as _)
186            }
187            SamplingConfig::FreqNearest(nearest) => Ok(((ULTRASOUND_FREQ.hz() as f32
188                / nearest.0.hz())
189            .clamp(1.0, u16::MAX as f32))
190            .round() as u16),
191            SamplingConfig::PeriodNearest(nearest) => {
192                use crate::common::ULTRASOUND_PERIOD;
193
194                Ok(((nearest.0.as_nanos() + ULTRASOUND_PERIOD.as_nanos() / 2)
195                    / ULTRASOUND_PERIOD.as_nanos())
196                .clamp(1, u16::MAX as u128) as u16)
197            }
198        }
199    }
200
201    /// The sampling frequency.
202    pub fn freq(&self) -> Result<Freq<f32>, SamplingConfigError> {
203        Ok(ULTRASOUND_FREQ.hz() as f32 / self.divide()? as f32 * Hz)
204    }
205
206    /// The sampling period.
207    pub fn period(&self) -> Result<core::time::Duration, SamplingConfigError> {
208        Ok(crate::common::ULTRASOUND_PERIOD * self.divide()? as u32)
209    }
210}
211
212impl SamplingConfig {
213    /// Converts to a [`SamplingConfig`] with the nearest frequency or period among the possible values.
214    #[must_use]
215    pub const fn into_nearest(self) -> SamplingConfig {
216        match self {
217            SamplingConfig::Freq(freq) => SamplingConfig::FreqNearest(Nearest(freq)),
218            SamplingConfig::Period(period) => SamplingConfig::PeriodNearest(Nearest(period)),
219            _ => self,
220        }
221    }
222}
223
224#[cfg(test)]
225mod tests {
226    use crate::common::{Hz, kHz};
227
228    use crate::common::ULTRASOUND_PERIOD;
229    use core::time::Duration;
230
231    use super::*;
232
233    #[rstest::rstest]
234    #[case(Ok(1), NonZeroU16::MIN)]
235    #[case(Ok(u16::MAX), NonZeroU16::MAX)]
236    #[case(Ok(1), 40000. * Hz)]
237    #[case(Ok(10), 4000. * Hz)]
238    #[case(Err(SamplingConfigError::FreqInvalidF((ULTRASOUND_FREQ.hz() as f32 - 1.) * Hz)), (ULTRASOUND_FREQ.hz() as f32 - 1.) * Hz)]
239    #[case(Err(SamplingConfigError::FreqOutOfRangeF(0. * Hz, ULTRASOUND_FREQ.hz() as f32 * Hz / u16::MAX as f32, ULTRASOUND_FREQ.hz() as f32 * Hz)), 0. * Hz)]
240    #[case(Err(SamplingConfigError::FreqOutOfRangeF(40000. * Hz + 1. * Hz, ULTRASOUND_FREQ.hz() as f32 * Hz / u16::MAX as f32, ULTRASOUND_FREQ.hz() as f32 * Hz)), 40000. * Hz + 1. * Hz)]
241    #[case(Ok(1), Duration::from_micros(25))]
242    #[case(Ok(10), Duration::from_micros(250))]
243    #[case(Err(SamplingConfigError::PeriodInvalid(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))]
244    #[case(Err(SamplingConfigError::PeriodOutOfRange(ULTRASOUND_PERIOD / 2, ULTRASOUND_PERIOD, Duration::from_micros(u16::MAX as u64 * ULTRASOUND_PERIOD.as_micros() as u64))), ULTRASOUND_PERIOD / 2)]
245    #[case(Err(SamplingConfigError::PeriodOutOfRange(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)]
246    fn divide(
247        #[case] expect: Result<u16, SamplingConfigError>,
248        #[case] value: impl Into<SamplingConfig>,
249    ) {
250        assert_eq!(expect, SamplingConfig::new(value).divide());
251    }
252
253    #[rstest::rstest]
254    #[case(Ok(40000. * Hz), NonZeroU16::MIN)]
255    #[case(Ok(0.61036086 * Hz), NonZeroU16::MAX)]
256    #[case(Ok(40000. * Hz), 40000. * Hz)]
257    #[case(Ok(4000. * Hz), 4000. * Hz)]
258    #[case(Ok(40000. * Hz), Duration::from_micros(25))]
259    #[case(Ok(4000. * Hz), Duration::from_micros(250))]
260    fn freq(
261        #[case] expect: Result<Freq<f32>, SamplingConfigError>,
262        #[case] value: impl Into<SamplingConfig>,
263    ) {
264        assert_eq!(expect, SamplingConfig::new(value).freq());
265    }
266
267    #[rstest::rstest]
268    #[case(Ok(Duration::from_micros(25)), NonZeroU16::MIN)]
269    #[case(Ok(Duration::from_micros(1638375)), NonZeroU16::MAX)]
270    #[case(Ok(Duration::from_micros(25)), 40000. * Hz)]
271    #[case(Ok(Duration::from_micros(250)), 4000. * Hz)]
272    #[case(Ok(Duration::from_micros(25)), Duration::from_micros(25))]
273    #[case(Ok(Duration::from_micros(250)), Duration::from_micros(250))]
274    fn period(
275        #[case] expect: Result<Duration, SamplingConfigError>,
276        #[case] value: impl Into<SamplingConfig>,
277    ) {
278        assert_eq!(expect, SamplingConfig::new(value).period());
279    }
280
281    #[rstest::rstest]
282    #[case::min(u16::MAX, (40000. / u16::MAX as f32) * Hz)]
283    #[case::max(1, 40000. * Hz)]
284    #[case::not_supported_max(1, (ULTRASOUND_FREQ.hz() as f32 - 1.) * Hz)]
285    #[case::out_of_range_min(u16::MAX, 0. * Hz)]
286    #[case::out_of_range_max(1, 40000. * Hz + 1. * Hz)]
287    fn from_freq_nearest(#[case] expected: u16, #[case] freq: Freq<f32>) {
288        assert_eq!(
289            Ok(expected),
290            SamplingConfig::new(freq).into_nearest().divide()
291        );
292    }
293
294    #[rstest::rstest]
295    #[case::min(1, ULTRASOUND_PERIOD)]
296    #[case::max(u16::MAX, Duration::from_micros(u16::MAX as u64 * ULTRASOUND_PERIOD.as_micros() as u64))]
297    #[case::not_supported_max(u16::MAX, Duration::from_micros(u16::MAX as u64 * ULTRASOUND_PERIOD.as_micros() as u64) - Duration::from_nanos(1))]
298    #[case::out_of_range_min(1, ULTRASOUND_PERIOD / 2)]
299    #[case::out_of_range_max(u16::MAX, Duration::from_micros(u16::MAX as u64 * ULTRASOUND_PERIOD.as_micros() as u64) * 2)]
300    fn from_period_nearest(#[case] expected: u16, #[case] p: Duration) {
301        assert_eq!(Ok(expected), SamplingConfig::new(p).into_nearest().divide());
302    }
303
304    #[rstest::rstest]
305    #[case(
306        SamplingConfig::Divide(NonZeroU16::MIN),
307        SamplingConfig::Divide(NonZeroU16::MIN)
308    )]
309    #[case(SamplingConfig::FreqNearest(Nearest(1. * Hz)), SamplingConfig::Freq(1. * Hz))]
310    #[case(
311        SamplingConfig::PeriodNearest(Nearest(Duration::from_micros(1))),
312        SamplingConfig::Period(Duration::from_micros(1))
313    )]
314    #[case(SamplingConfig::FreqNearest(Nearest(1. * Hz)), SamplingConfig::FreqNearest(Nearest(1. * Hz)))]
315    #[case(
316        SamplingConfig::PeriodNearest(Nearest(Duration::from_micros(1))),
317        SamplingConfig::PeriodNearest(Nearest(Duration::from_micros(1)))
318    )]
319    #[test]
320    fn into_nearest(#[case] expect: SamplingConfig, #[case] config: SamplingConfig) {
321        assert_eq!(expect, config.into_nearest());
322    }
323
324    #[rstest::rstest]
325    #[case(true, SamplingConfig::FREQ_40K, SamplingConfig::FREQ_40K)]
326    #[case(true, SamplingConfig::FREQ_40K, SamplingConfig::new(NonZeroU16::MIN))]
327    #[case(true, SamplingConfig::FREQ_40K, SamplingConfig::new(40. * kHz))]
328    #[case(
329        true,
330        SamplingConfig::FREQ_40K,
331        SamplingConfig::new(core::time::Duration::from_micros(25))
332    )]
333    #[case(false, SamplingConfig::new(41. * kHz), SamplingConfig::new(41. * kHz))]
334    #[test]
335    fn partial_eq(#[case] expect: bool, #[case] lhs: SamplingConfig, #[case] rhs: SamplingConfig) {
336        assert_eq!(expect, lhs == rhs);
337    }
338
339    #[rstest::rstest]
340    #[case("SamplingConfig::Divide(1)", SamplingConfig::Divide(NonZeroU16::MIN))]
341    #[case("SamplingConfig::Freq(1 Hz)", SamplingConfig::Freq(1. * Hz))]
342    #[case(
343        "SamplingConfig::Period(1µs)",
344        SamplingConfig::Period(Duration::from_micros(1))
345    )]
346    #[case("SamplingConfig::FreqNearest(Nearest(1 Hz))", SamplingConfig::FreqNearest(Nearest(1. * Hz)))]
347    #[case(
348        "SamplingConfig::PeriodNearest(Nearest(1µs))",
349        SamplingConfig::PeriodNearest(Nearest(Duration::from_micros(1)))
350    )]
351    #[test]
352    fn debug(#[case] expect: &str, #[case] config: SamplingConfig) {
353        assert_eq!(expect, format!("{config:?}"));
354    }
355
356    #[rstest::rstest]
357    #[case(
358        "Sampling frequency (39999 Hz) must divide the ultrasound frequency",
359        SamplingConfigError::FreqInvalid(39999 * Hz),
360    )]
361    #[case(
362        "Sampling frequency (39999 Hz) must divide the ultrasound frequency",
363        SamplingConfigError::FreqInvalidF(39999. * Hz),
364    )]
365    #[case(
366        "Sampling period (25.000025ms) must be a multiple of the ultrasound period",
367        SamplingConfigError::PeriodInvalid(Duration::from_micros(25000) + Duration::from_nanos(25)),
368    )]
369    #[case(
370        "Sampling frequency (0 Hz) is out of range ([1 Hz, 2 Hz])",
371        SamplingConfigError::FreqOutOfRange(0 * Hz, 1 * Hz, 2 * Hz),
372    )]
373    #[case(
374        "Sampling frequency (0 Hz) is out of range ([1 Hz, 2 Hz])",
375        SamplingConfigError::FreqOutOfRangeF(0. * Hz, 1. * Hz, 2. * Hz),
376    )]
377    #[case(
378        "Sampling period (12.5ms) is out of range ([25µs, 1.638375s])",
379        SamplingConfigError::PeriodOutOfRange(Duration::from_micros(12500), ULTRASOUND_PERIOD, Duration::from_micros(u16::MAX as u64 * ULTRASOUND_PERIOD.as_micros() as u64)),
380    )]
381    fn err_display(#[case] expect: &str, #[case] err: SamplingConfigError) {
382        assert_eq!(expect, format!("{}", err));
383    }
384}
autd3_core/firmware/sampling_config.rs

autd3_core/firmware/
sampling_config.rs
