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

autd3_core/firmware/
sampling_config.rs
