Skip to content

Commit 5e812f2

Browse files
thejpsterthejpster
committed
Getting more data from the IMU.
1 parent 875d504 commit 5e812f2

File tree

4 files changed

+208
-53
lines changed

4 files changed

+208
-53
lines changed

‎src/lib.rs‎

Lines changed: 55 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,27 @@ pub struct Orientation {
5252
pub yaw: Angle,
5353
}
5454

55+
/// Represents a 3D vector
56+
#[derive(Debug, Copy, Clone)]
57+
pub struct Vector3D {
58+
pub x: f64,
59+
pub y: f64,
60+
pub z: f64,
61+
}
62+
63+
/// A collection of all the data from the IMU
64+
#[derive(Debug, Default)]
65+
struct ImuData {
66+
timestamp: u64,
67+
fusion_pose: Option<Orientation>,
68+
gyro: Option<Vector3D>,
69+
accel: Option<Vector3D>,
70+
compass: Option<Vector3D>,
71+
pressure: Option<f64>,
72+
temperature: Option<f64>,
73+
humidity: Option<f64>,
74+
}
75+
5576
/// Represents the Sense HAT itself
5677
pub struct SenseHat<'a> {
5778
/// LPS25H pressure sensor
@@ -61,7 +82,7 @@ pub struct SenseHat<'a> {
6182
/// LSM9DS1 IMU device
6283
accelerometer_chip: lsm9ds1::Lsm9ds1<'a>,
6384
/// Cached data
64-
orientation: Orientation,
85+
data: ImuData,
6586
}
6687

6788
/// Errors that this crate can return
@@ -86,11 +107,7 @@ impl<'a> SenseHat<'a> {
86107
humidity_chip: hts221::Hts221::new(LinuxI2CDevice::new("/dev/i2c-1", 0x5f)?)?,
87108
pressure_chip: lps25h::Lps25h::new(LinuxI2CDevice::new("/dev/i2c-1", 0x5c)?)?,
88109
accelerometer_chip: lsm9ds1::Lsm9ds1::new()?,
89-
orientation: Orientation {
90-
roll: Angle::from_degrees(0.0),
91-
pitch: Angle::from_degrees(0.0),
92-
yaw: Angle::from_degrees(0.0),
93-
},
110+
data: ImuData::default(),
94111
})
95112
}
96113

@@ -148,18 +165,24 @@ impl<'a> SenseHat<'a> {
148165
pub fn get_orientation(&mut self) -> SenseHatResult<Orientation> {
149166
self.accelerometer_chip.set_fusion();
150167
if self.accelerometer_chip.imu_read() {
151-
self.orientation = self.accelerometer_chip.get_imu_data()?;
168+
self.data = self.accelerometer_chip.get_imu_data()?;
169+
}
170+
match self.data.fusion_pose {
171+
Some(o) => Ok(o),
172+
None => Err(SenseHatError::NotReady)
152173
}
153-
Ok(self.orientation)
154174
}
155175

156176
/// Get the compass heading (ignoring gyro and magnetometer)
157177
pub fn get_compass(&mut self) -> SenseHatResult<Angle> {
158178
self.accelerometer_chip.set_compass_only();
159179
if self.accelerometer_chip.imu_read() {
160180
// Don't cache this data
161-
let orientation = self.accelerometer_chip.get_imu_data()?;
162-
Ok(orientation.yaw)
181+
let data = self.accelerometer_chip.get_imu_data()?;
182+
match data.fusion_pose {
183+
Some(o) => Ok(o.yaw),
184+
None => Err(SenseHatError::NotReady)
185+
}
163186
} else {
164187
Err(SenseHatError::NotReady)
165188
}
@@ -170,8 +193,11 @@ impl<'a> SenseHat<'a> {
170193
pub fn get_gyro(&mut self) -> SenseHatResult<Orientation> {
171194
self.accelerometer_chip.set_gyro_only();
172195
if self.accelerometer_chip.imu_read() {
173-
let orientation = self.accelerometer_chip.get_imu_data()?;
174-
Ok(orientation)
196+
let data = self.accelerometer_chip.get_imu_data()?;
197+
match data.fusion_pose {
198+
Some(o) => Ok(o),
199+
None => Err(SenseHatError::NotReady)
200+
}
175201
} else {
176202
Err(SenseHatError::NotReady)
177203
}
@@ -182,12 +208,27 @@ impl<'a> SenseHat<'a> {
182208
pub fn get_accel(&mut self) -> SenseHatResult<Orientation> {
183209
self.accelerometer_chip.set_accel_only();
184210
if self.accelerometer_chip.imu_read() {
185-
let orientation = self.accelerometer_chip.get_imu_data()?;
186-
Ok(orientation)
211+
let data = self.accelerometer_chip.get_imu_data()?;
212+
match data.fusion_pose {
213+
Some(o) => Ok(o),
214+
None => Err(SenseHatError::NotReady)
215+
}
187216
} else {
188217
Err(SenseHatError::NotReady)
189218
}
190219
}
220+
221+
/// Returns a vector representing the current acceleration in Gs.
222+
pub fn get_accel_raw(&mut self) -> SenseHatResult<Vector3D> {
223+
self.accelerometer_chip.set_accel_only();
224+
if self.accelerometer_chip.imu_read() {
225+
self.data = self.accelerometer_chip.get_imu_data()?;
226+
}
227+
match self.data.accel {
228+
Some(a) => Ok(a),
229+
None => Err(SenseHatError::NotReady)
230+
}
231+
}
191232
}
192233

193234
impl From<LinuxI2CError> for SenseHatError {

‎src/lsm9ds1.rs‎

Lines changed: 86 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@
99
//! a C wrapper of the `RTIMULib` C++ API. We then call that unsafe C wrapper
1010
//! here, ensuring that any memory allocations were undone on drop.
1111
12-
use super::{Angle, Orientation};
12+
use super::{Angle, Orientation, Vector3D, ImuData};
1313
use libc;
1414

1515
enum RTIMULibContext {}
@@ -26,12 +26,33 @@ extern "C" {
2626
fn rtimulib_wrapper_imu_read(p_context: *mut RTIMULibContext) -> libc::c_int;
2727
fn rtimulib_wrapper_get_imu_data(
2828
p_context: *mut RTIMULibContext,
29-
orientation: *mut COrientation,
29+
orientation: *mut CAllData,
3030
) -> libc::c_int;
3131
}
3232

3333
#[repr(C)]
34-
struct COrientation {
34+
#[derive(Default)]
35+
struct CAllData {
36+
timestamp: libc::uint64_t,
37+
fusion_pose_valid: libc::c_int,
38+
fusion_pose: CVector3D,
39+
gyro_valid: libc::c_int,
40+
gyro: CVector3D,
41+
accel_valid: libc::c_int,
42+
accel: CVector3D,
43+
compass_valid: libc::c_int,
44+
compass: CVector3D,
45+
pressure_valid: libc::c_int,
46+
pressure: libc::c_double,
47+
temperature_valid: libc::c_int,
48+
temperature: libc::c_double,
49+
humidity_valid: libc::c_int,
50+
humidity: libc::c_double,
51+
}
52+
53+
#[repr(C)]
54+
#[derive(Default)]
55+
struct CVector3D {
3556
x: libc::c_double,
3657
y: libc::c_double,
3758
z: libc::c_double,
@@ -42,13 +63,13 @@ pub enum Error {
4263
RTIMULibError,
4364
}
4465

45-
pub struct Lsm9ds1<'a> {
66+
pub(crate) struct Lsm9ds1<'a> {
4667
rtimulib_ref: &'a mut RTIMULibContext,
4768
}
4869

4970
impl<'a> Lsm9ds1<'a> {
5071
/// Uses the `RTIMULib` library.
51-
pub fn new() -> Result<Lsm9ds1<'a>, Error> {
72+
pub(crate) fn new() -> Result<Lsm9ds1<'a>, Error> {
5273
let ctx_ref = unsafe {
5374
let ctx_p = rtimulib_wrapper_create();
5475
if ctx_p.is_null() {
@@ -64,47 +85,92 @@ impl<'a> Lsm9ds1<'a> {
6485

6586
/// Make the IMU do some work. When this function returns true, the IMU
6687
/// has data we can fetch with `get_imu_data()`.
67-
pub fn imu_read(&mut self) -> bool {
88+
pub(crate) fn imu_read(&mut self) -> bool {
6889
let result = unsafe { rtimulib_wrapper_imu_read(self.rtimulib_ref) };
6990
result != 0
7091
}
7192

72-
pub fn set_fusion(&mut self) {
93+
pub(crate) fn set_fusion(&mut self) {
7394
unsafe {
7495
rtimulib_set_sensors(self.rtimulib_ref, 1, 1, 1);
7596
}
7697
}
7798

78-
pub fn set_compass_only(&mut self) {
99+
pub(crate) fn set_compass_only(&mut self) {
79100
unsafe {
80101
rtimulib_set_sensors(self.rtimulib_ref, 0, 0, 1);
81102
}
82103
}
83104

84-
pub fn set_gyro_only(&mut self) {
105+
pub(crate) fn set_gyro_only(&mut self) {
85106
unsafe {
86107
rtimulib_set_sensors(self.rtimulib_ref, 1, 0, 0);
87108
}
88109
}
89110

90-
pub fn set_accel_only(&mut self) {
111+
pub(crate) fn set_accel_only(&mut self) {
91112
unsafe {
92113
rtimulib_set_sensors(self.rtimulib_ref, 0, 1, 0);
93114
}
94115
}
95116

96-
pub fn get_imu_data(&mut self) -> Result<Orientation, Error> {
97-
let mut temp = COrientation {
98-
x: 0.0,
99-
y: 0.0,
100-
z: 0.0,
101-
};
117+
pub(crate) fn get_imu_data(&mut self) -> Result<ImuData, Error> {
118+
let mut temp = CAllData::default();
102119
let result = unsafe { rtimulib_wrapper_get_imu_data(self.rtimulib_ref, &mut temp) };
103120
if result != 0 {
104-
Ok(Orientation {
105-
roll: Angle::from_radians(temp.x),
106-
pitch: Angle::from_radians(temp.y),
107-
yaw: Angle::from_radians(temp.z),
121+
Ok(ImuData {
122+
timestamp: temp.timestamp,
123+
fusion_pose: if temp.fusion_pose_valid != 0 {
124+
Some(Orientation {
125+
roll: Angle::from_radians(temp.fusion_pose.x),
126+
pitch: Angle::from_radians(temp.fusion_pose.y),
127+
yaw: Angle::from_radians(temp.fusion_pose.z),
128+
})
129+
} else {
130+
None
131+
},
132+
gyro: if temp.gyro_valid != 0 {
133+
Some(Vector3D {
134+
x: temp.gyro.x,
135+
y: temp.gyro.y,
136+
z: temp.gyro.z,
137+
})
138+
} else {
139+
None
140+
},
141+
accel: if temp.accel_valid != 0 {
142+
Some(Vector3D {
143+
x: temp.accel.x,
144+
y: temp.accel.y,
145+
z: temp.accel.z,
146+
})
147+
} else {
148+
None
149+
},
150+
compass: if temp.compass_valid != 0{
151+
Some(Vector3D {
152+
x: temp.compass.x,
153+
y: temp.compass.y,
154+
z: temp.compass.z,
155+
})
156+
} else {
157+
None
158+
},
159+
pressure: if temp.pressure_valid != 0 {
160+
Some(temp.pressure)
161+
} else {
162+
None
163+
},
164+
temperature: if temp.temperature_valid != 0 {
165+
Some(temp.temperature)
166+
} else {
167+
None
168+
},
169+
humidity: if temp.humidity_valid != 0 {
170+
Some(temp.humidity)
171+
} else {
172+
None
173+
},
108174
})
109175
} else {
110176
Err(Error::RTIMULibError)

‎src/lsm9ds1_dummy.rs‎

Lines changed: 8 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -2,11 +2,9 @@
22
//!
33
//! This is just a placeholder so the the docs build without RTIMULib.
44
5-
use super::{Angle, Orientation};
5+
use super::ImuData;
66
use std::marker::PhantomData;
77

8-
enum RTIMULibContext {}
9-
108
#[derive(Debug)]
119
pub enum Error {
1210
RTIMULibError,
@@ -18,27 +16,27 @@ pub struct Lsm9ds1<'a> {
1816

1917
impl<'a> Lsm9ds1<'a> {
2018
/// Uses the `RTIMULib` library.
21-
pub fn new() -> Result<Lsm9ds1<'a>, Error> {
19+
pub(crate) fn new() -> Result<Lsm9ds1<'a>, Error> {
2220
Ok(Lsm9ds1 {
2321
phantom: PhantomData,
2422
})
2523
}
2624

2725
/// Make the IMU do some work. When this function returns true, the IMU
2826
/// has data we can fetch with `get_imu_data()`.
29-
pub fn imu_read(&mut self) -> bool {
27+
pub(crate) fn imu_read(&mut self) -> bool {
3028
false
3129
}
3230

33-
pub fn set_fusion(&mut self) {}
31+
pub(crate) fn set_fusion(&mut self) {}
3432

35-
pub fn set_compass_only(&mut self) {}
33+
pub(crate) fn set_compass_only(&mut self) {}
3634

37-
pub fn set_gyro_only(&mut self) {}
35+
pub(crate) fn set_gyro_only(&mut self) {}
3836

39-
pub fn set_accel_only(&mut self) {}
37+
pub(crate) fn set_accel_only(&mut self) {}
4038

41-
pub fn get_imu_data(&mut self) -> Result<Orientation, Error> {
39+
pub(crate) fn get_imu_data(&mut self) -> Result<ImuData, Error> {
4240
Err(Error::RTIMULibError)
4341
}
4442
}

0 commit comments

Comments
 (0)