diff --git a/Cargo.toml b/Cargo.toml index 4b8512a..bc24bd5 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "sensehat" -version = "0.4.0" +version = "0.5.0" authors = ["Jonathan Pallant "] license-file = "LICENSE.md" documentation = "https://docs.rs/crate/sensehat" @@ -22,11 +22,12 @@ gcc = "0.3" [features] # The default set of optional packages. Most people will want to use these # packages, but they are strictly optional. -default = ["rtimu"] -rtimu = [ "libc" ] +default = ["rtimu", "led-matrix"] +# Extra packages required by these features. +rtimu = ["libc"] led-matrix = ["sensehat-screen"] [package.metadata.docs.rs] -features = [ ] +features = ["led-matrix"] all-features = false no-default-features = true diff --git a/src/lib.rs b/src/lib.rs index 12d4755..9f25102 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -52,6 +52,31 @@ pub struct Orientation { pub yaw: Angle, } +/// Represents a 3D vector +#[derive(Debug, Copy, Clone)] +pub struct Vector3D { + pub x: f64, + pub y: f64, + pub z: f64, +} + +/// Represents an RGB colour +#[cfg(feature = "led-matrix")] +pub use sensehat_screen::color::PixelColor as Colour; + +/// A collection of all the data from the IMU +#[derive(Debug, Default)] +struct ImuData { + timestamp: u64, + fusion_pose: Option, + gyro: Option, + accel: Option, + compass: Option, + pressure: Option, + temperature: Option, + humidity: Option, +} + /// Represents the Sense HAT itself pub struct SenseHat<'a> { /// LPS25H pressure sensor @@ -61,7 +86,7 @@ pub struct SenseHat<'a> { /// LSM9DS1 IMU device accelerometer_chip: lsm9ds1::Lsm9ds1<'a>, /// Cached data - orientation: Orientation, + data: ImuData, } /// Errors that this crate can return @@ -71,6 +96,8 @@ pub enum SenseHatError { GenericError, I2CError(LinuxI2CError), LSM9DS1Error(lsm9ds1::Error), + ScreenError, + CharacterError(std::string::FromUtf16Error) } /// A shortcut for Results that can return `T` or `SenseHatError` @@ -86,11 +113,7 @@ impl<'a> SenseHat<'a> { humidity_chip: hts221::Hts221::new(LinuxI2CDevice::new("/dev/i2c-1", 0x5f)?)?, pressure_chip: lps25h::Lps25h::new(LinuxI2CDevice::new("/dev/i2c-1", 0x5c)?)?, accelerometer_chip: lsm9ds1::Lsm9ds1::new()?, - orientation: Orientation { - roll: Angle::from_degrees(0.0), - pitch: Angle::from_degrees(0.0), - yaw: Angle::from_degrees(0.0), - }, + data: ImuData::default(), }) } @@ -148,9 +171,12 @@ impl<'a> SenseHat<'a> { pub fn get_orientation(&mut self) -> SenseHatResult { self.accelerometer_chip.set_fusion(); if self.accelerometer_chip.imu_read() { - self.orientation = self.accelerometer_chip.get_imu_data()?; + self.data = self.accelerometer_chip.get_imu_data()?; + } + match self.data.fusion_pose { + Some(o) => Ok(o), + None => Err(SenseHatError::NotReady) } - Ok(self.orientation) } /// Get the compass heading (ignoring gyro and magnetometer) @@ -158,8 +184,11 @@ impl<'a> SenseHat<'a> { self.accelerometer_chip.set_compass_only(); if self.accelerometer_chip.imu_read() { // Don't cache this data - let orientation = self.accelerometer_chip.get_imu_data()?; - Ok(orientation.yaw) + let data = self.accelerometer_chip.get_imu_data()?; + match data.fusion_pose { + Some(o) => Ok(o.yaw), + None => Err(SenseHatError::NotReady) + } } else { Err(SenseHatError::NotReady) } @@ -170,8 +199,11 @@ impl<'a> SenseHat<'a> { pub fn get_gyro(&mut self) -> SenseHatResult { self.accelerometer_chip.set_gyro_only(); if self.accelerometer_chip.imu_read() { - let orientation = self.accelerometer_chip.get_imu_data()?; - Ok(orientation) + let data = self.accelerometer_chip.get_imu_data()?; + match data.fusion_pose { + Some(o) => Ok(o), + None => Err(SenseHatError::NotReady) + } } else { Err(SenseHatError::NotReady) } @@ -182,12 +214,61 @@ impl<'a> SenseHat<'a> { pub fn get_accel(&mut self) -> SenseHatResult { self.accelerometer_chip.set_accel_only(); if self.accelerometer_chip.imu_read() { - let orientation = self.accelerometer_chip.get_imu_data()?; - Ok(orientation) + let data = self.accelerometer_chip.get_imu_data()?; + match data.fusion_pose { + Some(o) => Ok(o), + None => Err(SenseHatError::NotReady) + } } else { Err(SenseHatError::NotReady) } } + + /// Returns a vector representing the current acceleration in Gs. + pub fn get_accel_raw(&mut self) -> SenseHatResult { + self.accelerometer_chip.set_accel_only(); + if self.accelerometer_chip.imu_read() { + self.data = self.accelerometer_chip.get_imu_data()?; + } + match self.data.accel { + Some(a) => Ok(a), + None => Err(SenseHatError::NotReady) + } + } + + /// Displays a scrolling message on the LED matrix. + /// + /// Blocks until the entire message has scrolled past. + #[cfg(feature = "led-matrix")] + pub fn text(&mut self, message: &str, fg: Colour, bg: Colour) -> SenseHatResult<()> { + // Connect to our LED Matrix screen. + let mut screen = sensehat_screen::Screen::open("/dev/fb1").map_err(|_| SenseHatError::ScreenError)?; + // Get the default `FontCollection`. + let fonts = sensehat_screen::FontCollection::new(); + // Create a sanitized `FontString`. + let sanitized = fonts.sanitize_str(message)?; + // Render the `FontString` as a vector of pixel frames. + let pixel_frames = sanitized.pixel_frames(fg, bg); + // Create a `Scroll` from the pixel frame vector. + let scroll = sensehat_screen::Scroll::new(&pixel_frames); + // Consume the `FrameSequence` returned by the `left_to_right` method. + scroll.right_to_left().for_each(|frame| { + screen.write_frame(&frame.frame_line()); + ::std::thread::sleep(::std::time::Duration::from_millis(100)); + }); + Ok(()) + } + + /// Clears the LED matrix + #[cfg(feature = "led-matrix")] + pub fn clear(&mut self) -> SenseHatResult<()> { + // Connect to our LED Matrix screen. + let mut screen = sensehat_screen::Screen::open("/dev/fb1").map_err(|_| SenseHatError::ScreenError)?; + // Send a blank image to clear the screen + const OFF: [u8; 128] = [0x00; 128]; + screen.write_frame(&sensehat_screen::FrameLine::from_slice(&OFF)); + Ok(()) + } } impl From for SenseHatError { @@ -202,4 +283,11 @@ impl From for SenseHatError { } } +impl From for SenseHatError { + fn from(err: std::string::FromUtf16Error) -> SenseHatError { + SenseHatError::CharacterError(err) + } +} + + // End of file diff --git a/src/lsm9ds1.rs b/src/lsm9ds1.rs index 2a410f6..e5086ac 100644 --- a/src/lsm9ds1.rs +++ b/src/lsm9ds1.rs @@ -9,7 +9,7 @@ //! a C wrapper of the `RTIMULib` C++ API. We then call that unsafe C wrapper //! here, ensuring that any memory allocations were undone on drop. -use super::{Angle, Orientation}; +use super::{Angle, Orientation, Vector3D, ImuData}; use libc; enum RTIMULibContext {} @@ -26,12 +26,33 @@ extern "C" { fn rtimulib_wrapper_imu_read(p_context: *mut RTIMULibContext) -> libc::c_int; fn rtimulib_wrapper_get_imu_data( p_context: *mut RTIMULibContext, - orientation: *mut COrientation, + orientation: *mut CAllData, ) -> libc::c_int; } #[repr(C)] -struct COrientation { +#[derive(Default)] +struct CAllData { + timestamp: libc::uint64_t, + fusion_pose_valid: libc::c_int, + fusion_pose: CVector3D, + gyro_valid: libc::c_int, + gyro: CVector3D, + accel_valid: libc::c_int, + accel: CVector3D, + compass_valid: libc::c_int, + compass: CVector3D, + pressure_valid: libc::c_int, + pressure: libc::c_double, + temperature_valid: libc::c_int, + temperature: libc::c_double, + humidity_valid: libc::c_int, + humidity: libc::c_double, +} + +#[repr(C)] +#[derive(Default)] +struct CVector3D { x: libc::c_double, y: libc::c_double, z: libc::c_double, @@ -42,13 +63,13 @@ pub enum Error { RTIMULibError, } -pub struct Lsm9ds1<'a> { +pub(crate) struct Lsm9ds1<'a> { rtimulib_ref: &'a mut RTIMULibContext, } impl<'a> Lsm9ds1<'a> { /// Uses the `RTIMULib` library. - pub fn new() -> Result, Error> { + pub(crate) fn new() -> Result, Error> { let ctx_ref = unsafe { let ctx_p = rtimulib_wrapper_create(); if ctx_p.is_null() { @@ -64,47 +85,92 @@ impl<'a> Lsm9ds1<'a> { /// Make the IMU do some work. When this function returns true, the IMU /// has data we can fetch with `get_imu_data()`. - pub fn imu_read(&mut self) -> bool { + pub(crate) fn imu_read(&mut self) -> bool { let result = unsafe { rtimulib_wrapper_imu_read(self.rtimulib_ref) }; result != 0 } - pub fn set_fusion(&mut self) { + pub(crate) fn set_fusion(&mut self) { unsafe { rtimulib_set_sensors(self.rtimulib_ref, 1, 1, 1); } } - pub fn set_compass_only(&mut self) { + pub(crate) fn set_compass_only(&mut self) { unsafe { rtimulib_set_sensors(self.rtimulib_ref, 0, 0, 1); } } - pub fn set_gyro_only(&mut self) { + pub(crate) fn set_gyro_only(&mut self) { unsafe { rtimulib_set_sensors(self.rtimulib_ref, 1, 0, 0); } } - pub fn set_accel_only(&mut self) { + pub(crate) fn set_accel_only(&mut self) { unsafe { rtimulib_set_sensors(self.rtimulib_ref, 0, 1, 0); } } - pub fn get_imu_data(&mut self) -> Result { - let mut temp = COrientation { - x: 0.0, - y: 0.0, - z: 0.0, - }; + pub(crate) fn get_imu_data(&mut self) -> Result { + let mut temp = CAllData::default(); let result = unsafe { rtimulib_wrapper_get_imu_data(self.rtimulib_ref, &mut temp) }; if result != 0 { - Ok(Orientation { - roll: Angle::from_radians(temp.x), - pitch: Angle::from_radians(temp.y), - yaw: Angle::from_radians(temp.z), + Ok(ImuData { + timestamp: temp.timestamp, + fusion_pose: if temp.fusion_pose_valid != 0 { + Some(Orientation { + roll: Angle::from_radians(temp.fusion_pose.x), + pitch: Angle::from_radians(temp.fusion_pose.y), + yaw: Angle::from_radians(temp.fusion_pose.z), + }) + } else { + None + }, + gyro: if temp.gyro_valid != 0 { + Some(Vector3D { + x: temp.gyro.x, + y: temp.gyro.y, + z: temp.gyro.z, + }) + } else { + None + }, + accel: if temp.accel_valid != 0 { + Some(Vector3D { + x: temp.accel.x, + y: temp.accel.y, + z: temp.accel.z, + }) + } else { + None + }, + compass: if temp.compass_valid != 0{ + Some(Vector3D { + x: temp.compass.x, + y: temp.compass.y, + z: temp.compass.z, + }) + } else { + None + }, + pressure: if temp.pressure_valid != 0 { + Some(temp.pressure) + } else { + None + }, + temperature: if temp.temperature_valid != 0 { + Some(temp.temperature) + } else { + None + }, + humidity: if temp.humidity_valid != 0 { + Some(temp.humidity) + } else { + None + }, }) } else { Err(Error::RTIMULibError) diff --git a/src/lsm9ds1_dummy.rs b/src/lsm9ds1_dummy.rs index 538148e..c19a4da 100644 --- a/src/lsm9ds1_dummy.rs +++ b/src/lsm9ds1_dummy.rs @@ -2,11 +2,9 @@ //! //! This is just a placeholder so the the docs build without RTIMULib. -use super::{Angle, Orientation}; +use super::ImuData; use std::marker::PhantomData; -enum RTIMULibContext {} - #[derive(Debug)] pub enum Error { RTIMULibError, @@ -18,7 +16,7 @@ pub struct Lsm9ds1<'a> { impl<'a> Lsm9ds1<'a> { /// Uses the `RTIMULib` library. - pub fn new() -> Result, Error> { + pub(crate) fn new() -> Result, Error> { Ok(Lsm9ds1 { phantom: PhantomData, }) @@ -26,19 +24,19 @@ impl<'a> Lsm9ds1<'a> { /// Make the IMU do some work. When this function returns true, the IMU /// has data we can fetch with `get_imu_data()`. - pub fn imu_read(&mut self) -> bool { + pub(crate) fn imu_read(&mut self) -> bool { false } - pub fn set_fusion(&mut self) {} + pub(crate) fn set_fusion(&mut self) {} - pub fn set_compass_only(&mut self) {} + pub(crate) fn set_compass_only(&mut self) {} - pub fn set_gyro_only(&mut self) {} + pub(crate) fn set_gyro_only(&mut self) {} - pub fn set_accel_only(&mut self) {} + pub(crate) fn set_accel_only(&mut self) {} - pub fn get_imu_data(&mut self) -> Result { + pub(crate) fn get_imu_data(&mut self) -> Result { Err(Error::RTIMULibError) } } diff --git a/src/rtimulib_wrapper.cc b/src/rtimulib_wrapper.cc index 4998584..c8d4575 100644 --- a/src/rtimulib_wrapper.cc +++ b/src/rtimulib_wrapper.cc @@ -5,27 +5,42 @@ struct WrapperContext { RTIMUSettings* p_settings; RTIMU* p_imu; - uint32_t magic; }; -struct Orientation { +struct Vector3D { double x; double y; double z; }; +struct AllData { + uint64_t timestamp; + int fusionPoseValid; + Vector3D fusionPose; + int gyroValid; + Vector3D gyro; + int accelValid; + Vector3D accel; + int compassValid; + Vector3D compass; + int pressureValid; + double pressure; + int temperatureValid; + double temperature; + int humidityValid; + double humidity; +}; + extern "C" { WrapperContext* rtimulib_wrapper_create(void); void rtimulib_wrapper_destroy(WrapperContext* p_context); void rtimulib_set_sensors(WrapperContext* p_context, int gyro, int accel, int compass); int rtimulib_wrapper_imu_read(WrapperContext* p_context); - int rtimulib_wrapper_get_imu_data(WrapperContext* p_context, Orientation* p_output); + int rtimulib_wrapper_get_imu_data(WrapperContext* p_context, AllData* p_output); } WrapperContext* rtimulib_wrapper_create(void) { WrapperContext* p_context = new WrapperContext; - p_context->magic = 0xDEADBEEF; - printf("Magic at create is %08x\n", p_context->magic); // TODO: Should be ~/.config/sense_hat/RTIMULib p_context->p_settings = new RTIMUSettings("RTIMULib"); p_context->p_imu = RTIMU::createIMU(p_context->p_settings); @@ -36,9 +51,9 @@ WrapperContext* rtimulib_wrapper_create(void) { } void rtimulib_wrapper_destroy(WrapperContext* p_context) { - printf("Magic at destroy is %08x\n", p_context->magic); - delete p_context->p_settings; + // The settings object must outlive the IMU object delete p_context->p_imu; + delete p_context->p_settings; delete p_context; } @@ -52,12 +67,44 @@ int rtimulib_wrapper_imu_read(WrapperContext* p_context) { return p_context->p_imu->IMURead(); } -int rtimulib_wrapper_get_imu_data(WrapperContext* p_context, Orientation* p_output) { +int rtimulib_wrapper_get_imu_data(WrapperContext* p_context, AllData* p_output) { RTIMU_DATA imuData = p_context->p_imu->getIMUData(); - if (imuData.fusionPoseValid && p_output) { - p_output->x = imuData.fusionPose.x(); - p_output->y = imuData.fusionPose.y(); - p_output->z = imuData.fusionPose.z(); - }; - return imuData.fusionPoseValid; + p_output->timestamp = imuData.timestamp; + p_output->fusionPoseValid = imuData.fusionPoseValid; + if (p_output->fusionPoseValid) { + p_output->fusionPose.x = imuData.fusionPose.x(); + p_output->fusionPose.y = imuData.fusionPose.y(); + p_output->fusionPose.z = imuData.fusionPose.z(); + } + p_output->gyroValid = imuData.gyroValid; + if (p_output->gyroValid) { + p_output->gyro.x = imuData.gyro.x(); + p_output->gyro.y = imuData.gyro.y(); + p_output->gyro.z = imuData.gyro.z(); + } + p_output->accelValid = imuData.accelValid; + if (p_output->accelValid) { + p_output->accel.x = imuData.accel.x(); + p_output->accel.y = imuData.accel.y(); + p_output->accel.z = imuData.accel.z(); + } + p_output->compassValid = imuData.compassValid; + if (p_output->compassValid) { + p_output->compass.x = imuData.compass.x(); + p_output->compass.y = imuData.compass.y(); + p_output->compass.z = imuData.compass.z(); + } + p_output->pressureValid = imuData.pressureValid; + if (p_output->pressureValid) { + p_output->pressure = imuData.pressure; + } + p_output->temperatureValid = imuData.temperatureValid; + if (p_output->temperatureValid) { + p_output->temperature = imuData.temperature; + } + p_output->humidityValid = imuData.humidityValid; + if (p_output->humidityValid) { + p_output->humidity = imuData.humidity; + } + return 1; }