Commit 67d6b191 authored by Astro's avatar Astro
Browse files

add bhi160

parent 4b6d2854
......@@ -8,11 +8,26 @@ main!(main);
fn main() {
writeln!(UART, "Hello from Rust\r").unwrap();
let bme = BME680::start();
let a = BHI160::<Accelerometer>::start();
let g = BHI160::<Gyroscope>::start();
let o = BHI160::<Orientation>::start();
let display = Display::open();
let light = LightSensor::start();
for t in 0..Display::W {
writeln!(UART, "BME: {:?}\r", bme.read()).unwrap();
writeln!(UART, "A:\r").unwrap();
for d in &a.read() {
writeln!(UART, " - {:?}\r", d).unwrap();
}
writeln!(UART, "O:\r").unwrap();
for d in &o.read() {
writeln!(UART, " - {:?}\r", d).unwrap();
}
writeln!(UART, "G:\r").unwrap();
for d in &g.read() {
writeln!(UART, " - {:?}\r", d).unwrap();
}
display.clear(Color::yellow());
display.print(160 - t, 10, b"Hello Rust\0", Color::white(), Color::black());
......
use core::mem::uninitialized;
use core::marker::PhantomData;
use super::bindings::*;
use core::fmt::Write;
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
}
}
pub struct Sensor<S: SensorType> {
stream_id: i32,
_kind: PhantomData<S>,
}
impl<S: SensorType> Sensor<S> {
pub fn start() -> Self {
let mut cfg = bhi160_sensor_config {
sample_buffer_len: 200,
sample_rate: 4,
dynamic_range: 2,
_padding: [0u8; 8],
};
let stream_id = unsafe {
epic_bhi160_enable_sensor(S::sensor_type(), &mut cfg)
};
Sensor {
stream_id,
_kind: PhantomData,
}
}
pub fn read(&self) -> SensorData<S> {
let mut buf: [bhi160_data_vector; 100] = unsafe {
uninitialized()
};
let n = unsafe {
epic_stream_read(self.stream_id, buf.as_mut_ptr() as *mut _, buf.len())
};
if n < 0 {
panic!("epic_stream_read fail");
}
let n = n as usize;
SensorData {
buf, n,
_kind: PhantomData,
}
}
}
impl<S: SensorType> Drop for Sensor<S> {
fn drop(&mut self) {
unsafe { epic_bhi160_disable_sensor(S::sensor_type()); }
}
}
const DATA_MAX: usize = 10;
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>;
fn into_iter(self) -> Self::IntoIter {
SensorDataIter {
data: self,
pos: 0,
}
}
}
pub struct SensorDataIter<'a, S> {
data: &'a SensorData<S>,
pos: usize,
}
impl<'a, S: SensorType> Iterator for SensorDataIter<'a, S> {
type Item = SensorDataItem;
fn next(&mut self) -> Option<Self::Item> {
while self.pos < self.data.n {
let vec = &self.data.buf[self.pos];
self.pos += 1;
if vec.data_type == DATA_TYPE_VECTOR {
let item = SensorDataItem {
x: S::convert_single(vec.x),
y: S::convert_single(vec.y),
z: S::convert_single(vec.z),
status: vec.status,
};
return Some(item);
} else {
writeln!(crate::UART, "Sensor: skip type {}\r", vec.data_type).unwrap();
}
}
None
}
}
#[derive(Debug, Clone)]
pub struct SensorDataItem {
x: f32,
y: f32,
z: f32,
status: u8,
}
#[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;
......@@ -83,6 +83,8 @@ mod fmt_buffer;
pub use fmt_buffer::FmtBuffer;
mod bme680;
pub use bme680::BME680;
mod bhi160;
pub use bhi160::{Sensor as BHI160, Accelerometer, Orientation, Gyroscope, SensorData as BHI160Data};
pub fn exit(ret: i32) -> ! {
unsafe {
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment