autd3_core/firmware/
sampling_config.rs

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

autd3_core/firmware/
sampling_config.rs
