Implemented IMU reads. Untested.

Author: thejpster

.gitignore

@@ -8,3 +8,5 @@ Cargo.lock
 
 # Rustfmt backup files
 *.bk
+# Astyle backup files
+*.orig

Cargo.toml

@@ -13,6 +13,7 @@ readme = "README.md"
 measurements = "0.4.0"
 i2cdev = "0.3.1"
 byteorder = "1.0.0"
+libc = "0.2.0"
 
 [build-dependencies]
 gcc = "0.3"

examples/get_all.rs

@@ -16,4 +16,9 @@ fn main() {
     println!("It's {} relative humidity", rh);
     let pressure = sense_hat.get_pressure().expect("Couldn't get pressure");
     println!("The pressure is {}", pressure);
+    loop {
+        let orientation = sense_hat.get_orientation_degrees().expect("Couldn't get orientation");
+        println!("The orientation is {:?}", orientation);
+        ::std::thread::sleep(::std::time::Duration::from_millis(500));
+    }
 }

src/lib.rs

@@ -1,6 +1,7 @@
 extern crate byteorder;
 extern crate i2cdev;
 extern crate measurements;
+extern crate libc;
 
 pub use measurements::Temperature;
 pub use measurements::Pressure;
@@ -18,14 +19,24 @@ pub struct RelativeHumidity {
     value: f64,
 }
 
+/// Represents an orientation from the IMU
+#[derive(Debug, Copy, Clone)]
+pub struct OrientationDegrees {
+    pub roll: f64,
+    pub pitch: f64,
+    pub yaw: f64
+}
+
 /// Represents the SenseHat itself
 pub struct SenseHat<'a> {
-    // LPS25H pressure sensor
+    /// LPS25H pressure sensor
     pressure_chip: lps25h::Lps25h<LinuxI2CDevice>,
-    // HTS221 humidity sensor
+    /// HTS221 humidity sensor
     humidity_chip: hts221::Hts221<LinuxI2CDevice>,
-    // LSM9DS1 IMU device
+    /// LSM9DS1 IMU device
     accelerometer_chip: lsm9ds1::Lsm9ds1<'a>,
+    /// Cached data
+    orientation: OrientationDegrees,
 }
 
 /// Errors that this crate can return
@@ -50,6 +61,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: OrientationDegrees { roll: 0.0, pitch: 0.0, yaw: 0.0 },
            })
     }
 
@@ -97,6 +109,15 @@ impl<'a> SenseHat<'a> {
             Err(SenseHatError::NotReady)
         }
     }
+
+    /// Returns a vector representing the current orientation.
+    /// The values are in radians.
+    pub fn get_orientation_degrees(&mut self) -> SenseHatResult<OrientationDegrees> {
+        if self.accelerometer_chip.imu_read() {
+            self.orientation = self.accelerometer_chip.get_imu_data()?;
+        }
+        Ok(self.orientation)
+    }
 }
 
 impl From<LinuxI2CError> for SenseHatError {

src/lsm9ds1.rs

@@ -12,13 +12,26 @@
 //! undone on drop.
 
 
+use super::OrientationDegrees;
+use libc;
+
 enum RTIMULibContext {}
 
 extern "C" {
     fn rtimulib_wrapper_create() -> *mut RTIMULibContext;
     fn rtimulib_wrapper_destroy(p_context: *mut RTIMULibContext);
+    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) -> libc::c_int;
+}
+
+#[repr(C)]
+struct COrientation {
+    x: libc::c_double,
+    y: libc::c_double,
+    z: libc::c_double
 }
 
+
 #[derive(Debug)]
 pub enum Error {
     RTIMULibError,
@@ -28,6 +41,7 @@ pub struct Lsm9ds1<'a> {
     rtimulib_ref: &'a mut RTIMULibContext,
 }
 
+
 impl<'a> Lsm9ds1<'a> {
     /// Uses the RTIMULib library.
     pub fn new() -> Result<Lsm9ds1<'a>, Error> {
@@ -42,6 +56,39 @@ impl<'a> Lsm9ds1<'a> {
 
         Ok(Lsm9ds1 { rtimulib_ref: ctx_ref })
     }
+
+    /// 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 {
+        let result = unsafe {
+            rtimulib_wrapper_imu_read(self.rtimulib_ref)
+        };
+        return result == 0;
+    }
+
+    pub fn get_imu_data(&mut self) -> Result<OrientationDegrees, Error> {
+        let mut storage = COrientation {
+            x: 0.0,
+            y: 0.0,
+            z: 0.0
+        };
+        let result = unsafe {
+            rtimulib_wrapper_get_imu_data(self.rtimulib_ref, &mut storage)
+        };
+        if result == 1 {
+            Ok(OrientationDegrees {
+                roll: radians_to_degrees(storage.x),
+                pitch: radians_to_degrees(storage.y),
+                yaw: radians_to_degrees(storage.z),
+            })
+        } else {
+            Err(Error::RTIMULibError)
+        }
+    }
+}
+
+fn radians_to_degrees(radians: f64) -> f64 {
+    360.0 * (radians / (::std::f64::consts::PI * 2.0))
 }
 
 impl<'a> Drop for Lsm9ds1<'a> {

src/rtimulib_wrapper.cc

@@ -8,9 +8,17 @@ struct WrapperContext {
     uint32_t magic;
 };
 
+struct Orientation {
+    double x;
+    double y;
+    double z;
+};
+
 extern "C" {
     WrapperContext* rtimulib_wrapper_create(void);
     void rtimulib_wrapper_destroy(WrapperContext* p_context);
+    int rtimulib_wrapper_imu_read(WrapperContext* p_context);
+    int rtimulib_wrapper_get_imu_data(WrapperContext* p_context, Orientation* p_output);
 }
 
 WrapperContext* rtimulib_wrapper_create(void) {
@@ -33,3 +41,17 @@ void rtimulib_wrapper_destroy(WrapperContext* p_context) {
     delete p_context->p_imu;
     delete p_context;
 }
+
+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) {
+    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;
+}