bhi160.rs 5.58 KB
Newer Older
1
2
3
4
5
use core::{
    fmt::{self, Display, Write},
    marker::PhantomData,
    mem::MaybeUninit,
};
Astro's avatar
Astro committed
6

7
use crate::{bindings::*, errno};
Astro's avatar
Astro committed
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44

pub trait SensorType {
    /// sensor_type in C, sensor_id in Python
    fn sensor_type() -> u32;
    fn convert_single(value: i16) -> f32;
}

pub struct Accelerometer;
impl SensorType for Accelerometer {
    fn sensor_type() -> u32 {
        0
    }
    fn convert_single(value: i16) -> f32 {
        2.0 * f32::from(value) / 32768.0
    }
}

pub struct Gyroscope;
impl SensorType for Gyroscope {
    fn sensor_type() -> u32 {
        3
    }
    fn convert_single(value: i16) -> f32 {
        360.0 * f32::from(value) / 32768.0
    }
}

pub struct Orientation;
impl SensorType for Orientation {
    fn sensor_type() -> u32 {
        2
    }
    fn convert_single(value: i16) -> f32 {
        360.0 * f32::from(value) / 32768.0
    }
}

Astro's avatar
Astro committed
45
const DATA_MAX: usize = 10;
Astro's avatar
Astro committed
46
47
48
49
50
51
52

pub struct Sensor<S: SensorType> {
    stream_id: i32,
    _kind: PhantomData<S>,
}

impl<S: SensorType> Sensor<S> {
53
54
55
56
57
58
59
60
    fn new(stream_id: i32) -> Self {
        Self {
            stream_id,
            _kind: PhantomData,
        }
    }

    pub fn start() -> Result<Self, Error> {
Astro's avatar
Astro committed
61
62
63
64
65
66
        let mut cfg = bhi160_sensor_config {
            sample_buffer_len: 200,
            sample_rate: 4,
            dynamic_range: 2,
            _padding: [0u8; 8],
        };
67
68
69
70
71
72
73
74
75

        let stream_id = unsafe { epic_bhi160_enable_sensor(S::sensor_type(), &mut cfg) };
        if stream_id < 0 {
            let error = match -stream_id {
                errno::EBUSY => Error::DriverBusy,
                _ => Error::Unknown(stream_id),
            };

            return Err(error);
Astro's avatar
Astro committed
76
        }
77
78

        Ok(Sensor::new(stream_id))
Astro's avatar
Astro committed
79
80
    }

81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
    pub fn read(&self) -> Result<SensorData<S>, Error> {
        let mut buffer = MaybeUninit::<[bhi160_data_vector; DATA_MAX]>::zeroed();
        let buffer_pointer = buffer.as_mut_ptr() as *mut _;

        let packet_count = unsafe { epic_stream_read(self.stream_id, buffer_pointer, DATA_MAX) };
        if packet_count < 0 {
            let error = match -packet_count {
                errno::ENODEV => Error::SensorUnavailable,
                errno::EBADF => Error::SensorDescriptorUnknown,
                errno::EINVAL => Error::InvalidSampleCount,
                errno::EBUSY => Error::CouldNotAcquireLock,
                _ => Error::Unknown(packet_count),
            };

            return Err(error);
Astro's avatar
Astro committed
96
        }
97
98
99
100

        Ok(SensorData {
            buf: unsafe { buffer.assume_init() },
            n: packet_count as usize,
Astro's avatar
Astro committed
101
            _kind: PhantomData,
102
        })
Astro's avatar
Astro committed
103
104
105
106
107
    }
}

impl<S: SensorType> Drop for Sensor<S> {
    fn drop(&mut self) {
108
109
110
        unsafe {
            epic_bhi160_disable_sensor(S::sensor_type());
        }
Astro's avatar
Astro committed
111
112
113
114
115
116
117
118
119
120
121
122
    }
}

pub struct SensorData<S> {
    buf: [bhi160_data_vector; DATA_MAX],
    n: usize,
    _kind: PhantomData<S>,
}

impl<'a, S: SensorType> IntoIterator for &'a SensorData<S> {
    type Item = SensorDataItem;
    type IntoIter = SensorDataIter<'a, S>;
123

Astro's avatar
Astro committed
124
    fn into_iter(self) -> Self::IntoIter {
125
        SensorDataIter { data: self, pos: 0 }
Astro's avatar
Astro committed
126
127
128
129
130
131
132
133
134
135
    }
}

pub struct SensorDataIter<'a, S> {
    data: &'a SensorData<S>,
    pos: usize,
}

impl<'a, S: SensorType> Iterator for SensorDataIter<'a, S> {
    type Item = SensorDataItem;
136

Astro's avatar
Astro committed
137
138
    fn next(&mut self) -> Option<Self::Item> {
        while self.pos < self.data.n {
139
140
141
142
            let vec = &self.data.buf[self.pos];
            if vec.data_type != DATA_TYPE_VECTOR {
                writeln!(crate::UART, "Sensor: skip type {}\r", vec.data_type).ok();
                continue;
Astro's avatar
Astro committed
143
            }
144
145
146
147
148
149
150
151

            let item = SensorDataItem {
                x: S::convert_single(vec.x),
                y: S::convert_single(vec.y),
                z: S::convert_single(vec.z),
                status: vec.status,
            };

Nora's avatar
Nora committed
152
153
            self.pos += 1;

154
            return Some(item);
Astro's avatar
Astro committed
155
        }
156

Astro's avatar
Astro committed
157
158
159
160
161
162
        None
    }
}

#[derive(Debug, Clone)]
pub struct SensorDataItem {
163
164
165
166
    pub x: f32,
    pub y: f32,
    pub z: f32,
    pub status: u8,
Astro's avatar
Astro committed
167
168
169
170
171
172
173
174
175
176
177
178
179
180
}

#[repr(C)]
#[derive(Debug, Copy, Clone)]
pub struct bhi160_data_vector {
    /// This one is wrongly defined by buildgen
    pub data_type: u8,
    pub x: i16,
    pub y: i16,
    pub z: i16,
    pub status: u8,
}

const DATA_TYPE_VECTOR: u8 = 0;
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215

// -----------------------------------------------------------------------------
// BHI160 Error
// -----------------------------------------------------------------------------

#[derive(Debug)]
pub enum Error {
    /// The descriptor table lock could not be acquired.
    CouldNotAcquireLock,
    /// The BHI160 driver is currently busy with other tasks and could not be
    /// acquired for enabling a sensor.
    DriverBusy,
    /// The requested sample `count` is not a multiple of the sensor's sample
    /// size.
    InvalidSampleCount,
    /// The given sensor descriptor is unknown.
    SensorDescriptorUnknown,
    /// Sensor is not currently available.
    SensorUnavailable,
    /// Not yet documented and therefore unknown error types.
    Unknown(i32),
}

impl Display for Error {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        match *self {
            Error::CouldNotAcquireLock => writeln!(f, "Could not acquire BHI160 lock."),
            Error::DriverBusy => writeln!(f, "The BHI160 Driver is busy."),
            Error::InvalidSampleCount => writeln!(f, "Sample couldn't invalid."),
            Error::SensorDescriptorUnknown => writeln!(f, "Unknown BHI160 sensor descriptor."),
            Error::SensorUnavailable => writeln!(f, "The BHI160 sensor is currently unavailable."),
            Error::Unknown(id) => writeln!(f, "Unknown error id: {}", id),
        }
    }
}