flight_computer/
utils.rs

1//! # Utilities
2
3/// Prevent a variable from being optimized out
4#[inline(never)]
5#[cfg(debug_assertions)]
6pub fn deoptimize<T>(var: &T) {
7    let var = unsafe { core::ptr::read_volatile(var) };
8    core::mem::forget(var);
9}
10
11#[derive(Debug)]
12/// A circular buffer that only provides push and look-behind capabilities.
13pub struct WheelBuffer<T, const N: usize> {
14    write_idx: usize,
15    buf: [T; N],
16}
17
18impl<T, const N: usize> WheelBuffer<T, N>
19where
20    T: Copy,
21{
22    #[inline]
23    pub fn new(fill_with: T) -> Self {
24        Self {
25            write_idx: 0,
26            buf: [fill_with; N],
27        }
28    }
29
30    #[inline]
31    pub fn push(&mut self, new: T) {
32        self.write_idx = if self.write_idx < N - 1 {
33            self.write_idx + 1
34        } else {
35            0
36        };
37
38        self.buf[self.write_idx] = new;
39    }
40
41    /// Get the element at the specified index
42    ///
43    /// # Errors
44    ///
45    /// May return `Err` if the index offset is larger than the buffer size.
46    #[inline]
47    pub fn peek(&self, offset: usize) -> Result<T, BufferError> {
48        if offset >= N {
49            return Err(BufferError::OffsetTooLarge);
50        }
51
52        let offset = self.write_idx as isize - offset as isize;
53
54        Ok(if offset >= 0 {
55            self.buf[offset as usize]
56        } else {
57            self.buf[(offset + N as isize) as usize]
58        })
59    }
60
61    #[inline]
62    pub fn latest(&self) -> T {
63        // Unwrap here because peek(0) should never fail
64        self.peek(0).unwrap()
65    }
66}
67
68#[derive(Clone, Copy, Debug)]
69pub enum BufferError {
70    OffsetTooLarge,
71}
72
73#[cfg(test)]
74mod tests {
75    use super::*;
76
77    #[test]
78    fn wheelbuffer_validate_offset() {
79        let fill_with = 0;
80        let mut wheelbuf: WheelBuffer<usize, 10> = WheelBuffer::new(fill_with);
81        wheelbuf.push(1);
82        wheelbuf.push(2);
83        wheelbuf.push(3);
84
85        assert_eq!(wheelbuf.peek(0).unwrap(), 3);
86        assert_eq!(wheelbuf.peek(1).unwrap(), 2);
87        assert_eq!(wheelbuf.peek(2).unwrap(), 1);
88
89        // Test a buffer index that hasn't been written to yet
90        assert_eq!(wheelbuf.peek(3).unwrap(), fill_with);
91        // Test the edge case of buffer length
92        assert_eq!(wheelbuf.peek(9).unwrap(), fill_with);
93    }
94
95    #[test]
96    #[should_panic]
97    fn wheelbuffer_offset_too_large() {
98        let wheelbuf: WheelBuffer<usize, 10> = WheelBuffer::new(0);
99        // Test the edge case of a too large index
100        let _ = wheelbuf.peek(10).unwrap();
101    }
102
103    #[test]
104    fn wheelbuffer_offset_larger_than_isize_max() {
105        let wheelbuf: WheelBuffer<isize, 10> = WheelBuffer::new(0);
106        assert!(wheelbuf.peek(usize::MAX - 1).is_err());
107    }
108}