em7180/
lib.rs

1#![no_std]
2
3//! Manages a new EM7180 sensor hub with MPU9250 gyro/accelerometer,
4//! embedded Asahi Kasei AK8963C magnetometer, Measurement Specialties'
5//! MS5637 Barometer and ST's M24512DFC I2C EEPROM module
6
7#![deny(
8    missing_docs,
9    missing_debug_implementations,
10    missing_copy_implementations,
11    trivial_casts,
12    unstable_features,
13    unused_import_braces,
14    unused_qualifications,
15    //warnings
16)]
17#![allow(
18    dead_code,
19    clippy::uninit_assumed_init,
20    clippy::too_many_arguments,
21    clippy::upper_case_acronyms
22)]
23
24extern crate cast;
25extern crate embedded_hal as ehal;
26extern crate generic_array;
27extern crate nb;
28extern crate safe_transmute;
29
30use core::mem::MaybeUninit;
31
32use ehal::blocking::i2c::WriteRead;
33use generic_array::typenum::consts::*;
34use generic_array::{ArrayLength, GenericArray};
35
36/// Sometimes it's correct (0x28 << 1) instead of 0x28
37const EM7180_ADDRESS: u8 = 0x28;
38
39/// Struct for USFS
40#[derive(Debug, Copy, Clone)]
41pub struct USFS<I2C> {
42    com: I2C,
43    address: u8,
44    int_pin: u8,
45    pass_thru: bool,
46}
47
48/// Defines errors
49#[derive(Debug, Copy, Clone)]
50pub enum Error<E> {
51    /// WHO_AM_I returned invalid value (returned value is argument)
52    InvalidDevice(u8),
53    /// Underlying bus error
54    BusError(E),
55    /// Timeout
56    Timeout,
57}
58
59impl<E> From<E> for Error<E> {
60    fn from(error: E) -> Self {
61        Error::BusError(error)
62    }
63}
64
65impl<I2C, E> USFS<I2C>
66where
67    I2C: WriteRead<Error = E>,
68{
69    /// Creates a sensor with default configuration
70    pub fn default(i2c: I2C) -> Result<USFS<I2C>, Error<E>>
71    where
72        I2C: WriteRead<Error = E>,
73    {
74        USFS::new(i2c, EM7180_ADDRESS, 8, false)
75    }
76
77    /// Creates a sensor with specific configuration
78    pub fn new(i2c: I2C, address: u8, int_pin: u8, pass_thru: bool) -> Result<USFS<I2C>, Error<E>>
79    where
80        I2C: WriteRead<Error = E>,
81    {
82        let mut chip = USFS {
83            com: i2c,
84            address,
85            int_pin,
86            pass_thru,
87        };
88
89        let wai = chip.get_product_id()?;
90
91        if wai == 0x80 {
92            /*
93                Choose EM7180, MPU9250 and MS5637 sample rates and bandwidths
94                Choices are:
95                accBW, gyroBW 0x00 = 250 Hz, 0x01 = 184 Hz, 0x02 = 92 Hz, 0x03 = 41 Hz, 0x04 = 20 Hz, 0x05 = 10 Hz, 0x06 = 5 Hz, 0x07 = no filter (3600 Hz)
96                QRtDiv 0x00, 0x01, 0x02, etc quat rate = gyroRt/(1 + QRtDiv)
97                magRt 8 Hz = 0x08 or 100 Hz 0x64
98                accRt, gyroRt 1000, 500, 250, 200, 125, 100, 50 Hz enter by choosing desired rate
99                and dividing by 10, so 200 Hz would be 200/10 = 20 = 0x14
100                sample rate of barometer is baroRt/2 so for 25 Hz enter 50 = 0x32
101
102                uint8_t accBW = 0x03, gyroBW = 0x03, QRtDiv = 0x01, magRt = 0x64, accRt = 0x14, gyroRt = 0x14, baroRt = 0x32;
103
104                Choose MPU9250 sensor full ranges
105                Choices are 2, 4, 8, 16 g for accFS, 250, 500, 1000, and 2000 dps for gyro FS and 1000 uT for magFS expressed as HEX values
106
107                uint16_t accFS = 0x08, gyroFS = 0x7D0, magFS = 0x3E8;
108            */
109
110            let acc_bw: u8 = 0x03;
111            let gyro_bw: u8 = 0x03;
112            let qrt_div: u8 = 0x01;
113            let mag_rt: u8 = 0x64;
114            let acc_rt: u8 = 0x14;
115            let gyro_rt: u8 = 0x14;
116            let baro_rt: u8 = 0x32;
117            let acc_fs: u16 = 0x08;
118            let gyro_fs: u16 = 0x7D0;
119            let mag_fs: u16 = 0x3E8;
120
121            chip.load_fw_from_eeprom()?;
122            chip.init_hardware(
123                acc_bw, gyro_bw, acc_fs, gyro_fs, mag_fs, qrt_div, mag_rt, acc_rt, gyro_rt, baro_rt,
124            )?;
125            Ok(chip)
126        } else {
127            Err(Error::InvalidDevice(wai))
128        }
129    }
130
131    fn read_byte(&mut self, reg: u8) -> Result<u8, E> {
132        let mut data: [u8; 1] = [0];
133        self.com.write_read(self.address, &[reg], &mut data)?;
134        Ok(data[0])
135    }
136
137    fn read_registers<N>(&mut self, reg: Register) -> Result<GenericArray<u8, N>, E>
138    where
139        N: ArrayLength<u8>,
140    {
141        let mut buffer: GenericArray<u8, N> =
142            unsafe { MaybeUninit::<GenericArray<u8, N>>::uninit().assume_init() };
143
144        {
145            let buffer: &mut [u8] = &mut buffer;
146            const I2C_AUTO_INCREMENT: u8 = 0;
147            self.com
148                .write_read(self.address, &[(reg as u8) | I2C_AUTO_INCREMENT], buffer)?;
149        }
150
151        Ok(buffer)
152    }
153
154    fn read_4bytes(&mut self, reg: Register) -> Result<[u8; 4], E> {
155        let buffer: GenericArray<u8, U4> = self.read_registers(reg)?;
156        let mut ret: [u8; 4] = Default::default();
157        ret.copy_from_slice(buffer.as_slice());
158
159        Ok(ret)
160    }
161
162    fn read_6bytes(&mut self, reg: Register) -> Result<[u8; 6], E> {
163        let buffer: GenericArray<u8, U6> = self.read_registers(reg)?;
164        let mut ret: [u8; 6] = Default::default();
165        ret.copy_from_slice(buffer.as_slice());
166
167        Ok(ret)
168    }
169
170    fn read_16bytes(&mut self, reg: Register) -> Result<[u8; 16], E> {
171        let buffer: GenericArray<u8, U16> = self.read_registers(reg)?;
172        let mut ret: [u8; 16] = Default::default();
173        ret.copy_from_slice(buffer.as_slice());
174
175        Ok(ret)
176    }
177
178    fn read_register(&mut self, reg: Register) -> Result<u8, E> {
179        let mut data: [u8; 1] = [0];
180        self.com.write_read(self.address, &[reg as u8], &mut data)?;
181        Ok(data[0])
182    }
183
184    fn write_register(&mut self, reg: Register, byte: u8) -> Result<(), E> {
185        let mut buffer = [0];
186        self.com
187            .write_read(self.address, &[reg as u8, byte], &mut buffer)
188    }
189
190    fn em7180_set_integer_param(&mut self, param: u8, param_val: u32) -> Result<(), E> {
191        let bytes_0 = (param_val & (0xFF)) as u8;
192        let bytes_1 = ((param_val >> 8) & (0xFF)) as u8;
193        let bytes_2 = ((param_val >> 16) & (0xFF)) as u8;
194        let bytes_3 = ((param_val >> 24) & (0xFF)) as u8;
195        let param = param | 0x80; // Parameter is the decimal value with the MSB set high to indicate a paramter write processs
196
197        self.write_register(Register::EM7180_LoadParamByte0, bytes_0)?; // Param LSB
198        self.write_register(Register::EM7180_LoadParamByte1, bytes_1)?;
199        self.write_register(Register::EM7180_LoadParamByte2, bytes_2)?;
200        self.write_register(Register::EM7180_LoadParamByte3, bytes_3)?; // Param MSB
201        self.write_register(Register::EM7180_ParamRequest, param)?;
202        self.write_register(Register::EM7180_AlgorithmControl, 0x80)?; // Request parameter transfer procedure
203        let mut stat = self.read_register(Register::EM7180_ParamAcknowledge)?; // Check the parameter acknowledge register and loop until the result matches parameter request byte
204
205        while stat != param {
206            stat = self.read_register(Register::EM7180_ParamAcknowledge)?;
207        }
208
209        self.write_register(Register::EM7180_ParamRequest, 0x00)?; // Parameter request = 0 to end parameter transfer process
210        self.write_register(Register::EM7180_AlgorithmControl, 0x00)?; // Re-start algorithm
211
212        Ok(())
213    }
214
215    fn em7180_set_mag_acc_fs(&mut self, mag_fs: u16, acc_fs: u16) -> Result<(), E> {
216        let bytes_0 = (mag_fs & (0xFF)) as u8;
217        let bytes_1 = ((mag_fs >> 8) & (0xFF)) as u8;
218        let bytes_2 = (acc_fs & (0xFF)) as u8;
219        let bytes_3 = ((acc_fs >> 8) & (0xFF)) as u8;
220
221        self.write_register(Register::EM7180_LoadParamByte0, bytes_0)?; // Mag LSB
222        self.write_register(Register::EM7180_LoadParamByte1, bytes_1)?; // Mag MSB
223        self.write_register(Register::EM7180_LoadParamByte2, bytes_2)?; // Acc LSB
224        self.write_register(Register::EM7180_LoadParamByte3, bytes_3)?; // Acc MSB
225        self.write_register(Register::EM7180_ParamRequest, 0xCA)?; // Parameter 74; 0xCA is 74 decimal with the MSB set high to indicate a paramter write processs
226        self.write_register(Register::EM7180_AlgorithmControl, 0x80)?; // Request parameter transfer procedure
227        let mut stat = self.read_register(Register::EM7180_ParamAcknowledge)?; // Check the parameter acknowledge register and loop until the result matches parameter request byte
228
229        while stat != 0xCA {
230            stat = self.read_register(Register::EM7180_ParamAcknowledge)?;
231        }
232
233        self.write_register(Register::EM7180_ParamRequest, 0x00)?; // Parameter request = 0 to end parameter transfer process
234        self.write_register(Register::EM7180_AlgorithmControl, 0x00)?; // Re-start algorithm
235
236        Ok(())
237    }
238
239    fn em7180_set_gyro_fs(&mut self, gyro_fs: u16) -> Result<(), E> {
240        let bytes_0 = (gyro_fs & (0xFF)) as u8;
241        let bytes_1 = ((gyro_fs >> 8) & (0xFF)) as u8;
242        let bytes_2 = 0x00;
243        let bytes_3 = 0x00;
244
245        self.write_register(Register::EM7180_LoadParamByte0, bytes_0)?; // Gyro LSB
246        self.write_register(Register::EM7180_LoadParamByte1, bytes_1)?; // Gyro MSB
247        self.write_register(Register::EM7180_LoadParamByte2, bytes_2)?; // Unused
248        self.write_register(Register::EM7180_LoadParamByte3, bytes_3)?; // Unused
249        self.write_register(Register::EM7180_ParamRequest, 0xCB)?; // Parameter 75; 0xCB is 75 decimal with the MSB set high to indicate a paramter write processs
250        self.write_register(Register::EM7180_AlgorithmControl, 0x80)?; // Request parameter transfer procedure
251        let mut stat = self.read_register(Register::EM7180_ParamAcknowledge)?; // Check the parameter acknowledge register and loop until the result matches parameter request byte
252
253        while stat != 0xCB {
254            stat = self.read_register(Register::EM7180_ParamAcknowledge)?;
255        }
256
257        self.write_register(Register::EM7180_ParamRequest, 0x00)?; // Parameter request = 0 to end parameter transfer process
258        self.write_register(Register::EM7180_AlgorithmControl, 0x00)?; // Re-start algorithm
259
260        Ok(())
261    }
262
263    fn load_fw_from_eeprom(&mut self) -> Result<(), E> {
264        let mut stat = self.read_register(Register::EM7180_SentralStatus)? & 0x01;
265
266        let mut count = 0;
267        while stat != 0 {
268            self.write_register(Register::EM7180_ResetRequest, 0x01)?;
269            count += 1;
270            stat = self.read_register(Register::EM7180_SentralStatus)? & 0x01;
271            if count > 10 {
272                break;
273            };
274        }
275
276        Ok(())
277    }
278
279    fn init_hardware(
280        &mut self,
281        acc_bw: u8,
282        gyro_bw: u8,
283        acc_fs: u16,
284        gyro_fs: u16,
285        mag_fs: u16,
286        qrt_div: u8,
287        mag_rt: u8,
288        acc_rt: u8,
289        gyro_rt: u8,
290        baro_rt: u8,
291    ) -> Result<(), Error<E>> {
292        self.write_register(Register::EM7180_HostControl, 0x00)?; // Set SENtral in initialized state to configure registers
293        self.write_register(Register::EM7180_PassThruControl, 0x00)?; // Make sure pass through mode is off
294        self.write_register(Register::EM7180_HostControl, 0x01)?; // Force initialize
295        self.write_register(Register::EM7180_HostControl, 0x00)?; // Set SENtral in initialized state to configure registers
296
297        // Setup LPF bandwidth (BEFORE setting ODR's)
298        self.write_register(Register::EM7180_ACC_LPF_BW, acc_bw)?; // accBW = 3 = 41Hz
299        self.write_register(Register::EM7180_GYRO_LPF_BW, gyro_bw)?; // gyroBW = 3 = 41Hz
300                                                                     // Set accel/gyro/mag desired ODR rates
301        self.write_register(Register::EM7180_QRateDivisor, qrt_div)?; // quat rate = gyroRt/(1 QRTDiv)
302        self.write_register(Register::EM7180_MagRate, mag_rt)?; // 0x64 = 100 Hz
303        self.write_register(Register::EM7180_AccelRate, acc_rt)?; // 200/10 Hz, 0x14 = 200 Hz
304        self.write_register(Register::EM7180_GyroRate, gyro_rt)?; // 200/10 Hz, 0x14 = 200 Hz
305        self.write_register(Register::EM7180_BaroRate, 0x80 | baro_rt)?; // Set enable bit and set Baro rate to 25 Hz, rate = baroRt/2, 0x32 = 25 Hz
306
307        // Configure operating mode
308        self.write_register(Register::EM7180_AlgorithmControl, 0x00)?; // Read scale sensor data
309                                                                       // Enable interrupt to host upon certain events
310                                                                       // choose host interrupts when any sensor updated (0x40), new gyro data (0x20), new accel data (0x10),
311                                                                       // New mag data (0x08), quaternions updated (0x04), an error occurs (0x02), or the SENtral needs to be reset(0x01)
312        self.write_register(Register::EM7180_EnableEvents, 0x07)?;
313        // Enable EM7180 run mode
314        self.write_register(Register::EM7180_HostControl, 0x01)?; // Set SENtral in normal run mode
315
316        // delay(100);
317
318        // Read sensor default FS values from parameter space
319        self.write_register(Register::EM7180_ParamRequest, 0x4A)?; // Request to read parameter 74
320
321        self.write_register(Register::EM7180_AlgorithmControl, 0x80)?; // Request parameter transfer process
322        let mut param_xfer = self.read_register(Register::EM7180_ParamAcknowledge)?;
323        while param_xfer != 0x4A {
324            param_xfer = self.read_register(Register::EM7180_ParamAcknowledge)?;
325        }
326
327        self.write_register(Register::EM7180_ParamRequest, 0x4B)?; // Request to read  parameter 75
328        let mut param_xfer = self.read_register(Register::EM7180_ParamAcknowledge)?;
329        while param_xfer != 0x4B {
330            param_xfer = self.read_register(Register::EM7180_ParamAcknowledge)?;
331        }
332
333        self.write_register(Register::EM7180_ParamRequest, 0x00)?; //End parameter transfer
334        self.write_register(Register::EM7180_AlgorithmControl, 0x00)?; // re-enable algorithm
335
336        // Disable stillness mode for balancing robot application
337        self.em7180_set_integer_param(0x49, 0x00)?;
338
339        // Write desired sensor full scale ranges to the EM7180
340        self.em7180_set_mag_acc_fs(mag_fs, acc_fs)?; // 1000 uT == 0x3E8, 8 g == 0x08
341        self.em7180_set_gyro_fs(gyro_fs)?; // 2000 dps == 0x7D0
342
343        // Read sensor new FS values from parameter space
344        self.write_register(Register::EM7180_ParamRequest, 0x4A)?; // Request to read  parameter 74
345        self.write_register(Register::EM7180_AlgorithmControl, 0x80)?; // Request parameter transfer process
346        let mut param_xfer = self.read_register(Register::EM7180_ParamAcknowledge)?;
347        while param_xfer != 0x4A {
348            param_xfer = self.read_register(Register::EM7180_ParamAcknowledge)?;
349        }
350
351        self.write_register(Register::EM7180_ParamRequest, 0x4B)?; // Request to read  parameter 75
352        let mut param_xfer = self.read_register(Register::EM7180_ParamAcknowledge)?;
353        while param_xfer != 0x4B {
354            param_xfer = self.read_register(Register::EM7180_ParamAcknowledge)?;
355        }
356
357        self.write_register(Register::EM7180_ParamRequest, 0x00)?; //End parameter transfer
358        self.write_register(Register::EM7180_AlgorithmControl, 0x00)?; // re-enable algorithm
359
360        Ok(())
361    }
362
363    /// Get ProductID, should be: 0x80
364    pub fn get_product_id(&mut self) -> Result<u8, E> {
365        self.read_register(Register::EM7180_ProductID)
366    }
367
368    /// Get ProductID, should be: 0xE609
369    pub fn get_rom_version(&mut self) -> Result<[u8; 2], E> {
370        let rom_version1 = self.read_register(Register::EM7180_ROMVersion1)?;
371        let rom_version2 = self.read_register(Register::EM7180_ROMVersion2)?;
372        Ok([rom_version1, rom_version2])
373    }
374
375    /// Get Sentral status
376    pub fn get_sentral_status(&mut self) -> Result<u8, E> {
377        self.read_register(Register::EM7180_SentralStatus)
378    }
379
380    /// Get run status
381    pub fn check_run_status(&mut self) -> Result<u8, E> {
382        self.read_register(Register::EM7180_RunStatus)
383    }
384
385    /// Check EventStatus on address 0x35
386    /// Non-zero value indicates a new event has been generated
387    /// bit | meaning
388    ///  0  | CPUReset
389    ///  1  | Error
390    ///  2  | QuaternionResult
391    ///  3  | MagResult
392    ///  4  | AccelResult
393    ///  5  | GyroResult
394    pub fn check_status(&mut self) -> Result<u8, E> {
395        self.read_register(Register::EM7180_EventStatus)
396    }
397
398    /// Check ErrorRegister Software-Related Error Conditions on address 0x50
399    /// Non-zero value indicates an error
400    /// value | error condition                         | response
401    /// 0x00  | no error                                |
402    /// 0x80  | invalid sample rate selected            | check sensor rate settings
403    /// 0x30  | mathematical error                      | check for software updates
404    /// 0x21  | Magnetometer initialization failed      | this error can be caused by a wrong driver,
405    /// 0x22  | accelerometer initialization failed     | physically bad sensor connection, or
406    /// 0x24  | gyroscope initialization failed         | incorrect I2C device address in the driver
407    /// 0x11  | magnetometer rate failure               | this error indicates the given sensor
408    /// 0x12  | accelerometer rate failure              | is unreliable and has stopped
409    /// 0x14  | Gyroscope rate failure                  | producing data
410    pub fn check_errors(&mut self) -> Result<u8, E> {
411        self.read_register(Register::EM7180_Error)
412    }
413
414    /// Check SensorStatus Sensor-Related Error Conditions on address 0x36
415    /// Non-zero value indicates sensor-related error
416    /// bit | meaning
417    ///  0  | MagNACK. 1 = NACK from magnetometer
418    ///  1  | AccelNACK. 1 = NACK from accelerometer
419    ///  2  | GyroNACK. 1 = NACK from gyroscope
420    ///  4  | MagDeviceIDErr. 1 = Unexpected DeviceID from magnetometer
421    ///  5  | AccelDeviceIDErr. 1 = Unexpected DeviceID from accelerometer
422    ///  6  | GyroDeviceIDErr. 1 = Unexpected DeviceID from gyroscope
423    pub fn check_sensor_status(&mut self) -> Result<u8, E> {
424        self.read_register(Register::EM7180_SensorStatus)
425    }
426
427    /// Check which sensors can be detected by the EM7180
428    /// value | response
429    /// 0x01  | A barometer is installed
430    /// 0x02  | A humidity sensor is installed
431    /// 0x04  | A temperature sensor is installed
432    /// 0x08  | A custom sensor is installed
433    /// 0x10  | A second custom sensor is installed
434    /// 0x20  | A third custom sensor is installed
435    pub fn check_feature_flags(&mut self) -> Result<u8, E> {
436        self.read_register(Register::EM7180_FeatureFlags)
437    }
438
439    /// Get actual magnetometer output data rate
440    pub fn get_actual_magnetometer_rate(&mut self) -> Result<u8, E> {
441        self.read_register(Register::EM7180_ActualMagRate)
442    }
443
444    /// Get actual accelerometer output data rate
445    pub fn get_actual_accel_rate(&mut self) -> Result<u8, E> {
446        Ok(self.read_register(Register::EM7180_ActualAccelRate)? * 10)
447    }
448
449    /// Get actual gyroscope output data rate
450    pub fn get_actual_gyroscope_rate(&mut self) -> Result<u8, E> {
451        Ok(self.read_register(Register::EM7180_ActualGyroRate)? * 10)
452    }
453
454    /// Read Linear Acceleration
455    /// AX Linear Acceleration – X Axis
456    /// AY Linear Acceleration – Y Axis
457    /// AZ Linear Acceleration – Z Axis
458    pub fn read_sentral_accel_data(&mut self) -> Result<[i16; 3], E> {
459        let raw_data = self.read_6bytes(Register::EM7180_AX)?;
460
461        let ax = (raw_data[1] as i16) << 8 | raw_data[0] as i16;
462        let ay = (raw_data[3] as i16) << 8 | raw_data[2] as i16;
463        let az = (raw_data[5] as i16) << 8 | raw_data[4] as i16;
464
465        Ok([ax, ay, az])
466    }
467
468    /// Read Rotational Velocity
469    /// GX Rotational Velocity – X Axis
470    /// GY Rotational Velocity – Y Axis
471    /// GZ Rotational Velocity – Z Axis
472    pub fn read_sentral_gyro_data(&mut self) -> Result<[i16; 3], E> {
473        let raw_data = self.read_6bytes(Register::EM7180_GX)?;
474
475        let ax = (raw_data[1] as i16) << 8 | raw_data[0] as i16;
476        let ay = (raw_data[3] as i16) << 8 | raw_data[2] as i16;
477        let az = (raw_data[5] as i16) << 8 | raw_data[4] as i16;
478
479        Ok([ax, ay, az])
480    }
481
482    /// Read Magnetic Field
483    /// MX Magnetic Field – X Axis
484    /// MY Magnetic Field – Y Axis
485    /// MZ Magnetic Field – Z Axis
486    pub fn read_sentral_mag_data(&mut self) -> Result<[i16; 3], E> {
487        let raw_data = self.read_6bytes(Register::EM7180_MX)?;
488
489        let ax = (raw_data[1] as i16) << 8 | raw_data[0] as i16;
490        let ay = (raw_data[3] as i16) << 8 | raw_data[2] as i16;
491        let az = (raw_data[5] as i16) << 8 | raw_data[4] as i16;
492
493        Ok([ax, ay, az])
494    }
495
496    /// Read Normalized Quaternion
497    /// QX Normalized Quaternion – X, or Heading    | Full-Scale Range 0.0 – 1.0 or ±π
498    /// QY Normalized Quaternion – Y, or Pitch      | Full-Scale Range 0.0 – 1.0 or ±π/2
499    /// QZ Normalized Quaternion – Z, or Roll       | Full-Scale Range 0.0 – 1.0 or ±π
500    /// QW Normalized Quaternion – W, or 0.0        | Full-Scale Range 0.0 – 1.0
501    pub fn read_sentral_quat_qata(&mut self) -> Result<[f32; 4], E> {
502        let raw_data_qx = self.read_4bytes(Register::EM7180_QX)?;
503        let qx = reg_to_float(&raw_data_qx);
504
505        let raw_data_qy = self.read_4bytes(Register::EM7180_QY)?;
506        let qy = reg_to_float(&raw_data_qy);
507
508        let raw_data_qz = self.read_4bytes(Register::EM7180_QZ)?;
509        let qz = reg_to_float(&raw_data_qz);
510
511        let raw_data_qw = self.read_4bytes(Register::EM7180_QW)?;
512        let qw = reg_to_float(&raw_data_qw);
513
514        Ok([qx, qy, qz, qw])
515    }
516
517    /// Read Pressure in mBar
518    pub fn read_sentral_baro_data(&mut self) -> Result<i16, E> {
519        let raw_data = self.read_6bytes(Register::EM7180_Baro)?; // Read the two raw data registers sequentially into data array
520        Ok((raw_data[1] as i16) << 8 | raw_data[0] as i16) // Turn the MSB and LSB into a signed 16-bit value
521    }
522
523    /// Read Temperature in °C
524    pub fn read_sentral_temp_data(&mut self) -> Result<i16, E> {
525        let raw_data = self.read_6bytes(Register::EM7180_Temp)?; // Read the two raw data registers sequentially into data array
526        Ok((raw_data[1] as i16) << 8 | raw_data[0] as i16) // Turn the MSB and LSB into a signed 16-bit value
527    }
528}
529
530#[derive(Debug, Copy, Clone)]
531#[allow(non_camel_case_types)]
532enum Register {
533    EM7180_EventStatus = 0x35,
534    EM7180_Error = 0x50,
535    EM7180_ProductID = 0x90,
536    EM7180_HostControl = 0x34,
537    EM7180_PassThruControl = 0xA0,
538    EM7180_ACC_LPF_BW = 0x5B,
539    EM7180_GYRO_LPF_BW = 0x5C,
540    EM7180_QRateDivisor = 0x32,
541    EM7180_MagRate = 0x55,
542    EM7180_AccelRate = 0x56,
543    EM7180_GyroRate = 0x57,
544    EM7180_BaroRate = 0x58,
545    EM7180_AlgorithmControl = 0x54,
546    EM7180_EnableEvents = 0x33,
547    EM7180_SensorStatus = 0x36,
548    EM7180_ActualMagRate = 0x45,
549    EM7180_ActualAccelRate = 0x46,
550    EM7180_ActualGyroRate = 0x47,
551    EM7180_ParamRequest = 0x64,
552    EM7180_ParamAcknowledge = 0x3A,
553    EM7180_ROMVersion1 = 0x70,
554    EM7180_ROMVersion2 = 0x71,
555    EM7180_LoadParamByte0 = 0x60,
556    EM7180_LoadParamByte1 = 0x61,
557    EM7180_LoadParamByte2 = 0x62,
558    EM7180_LoadParamByte3 = 0x63,
559    EM7180_SavedParamByte0 = 0x3B,
560    EM7180_SavedParamByte1 = 0x3C,
561    EM7180_SavedParamByte2 = 0x3D,
562    EM7180_SavedParamByte3 = 0x3E,
563    EM7180_FeatureFlags = 0x39,
564    EM7180_SentralStatus = 0x37,
565    EM7180_ResetRequest = 0x9B,
566    EM7180_RunStatus = 0x92,
567    EM7180_QX = 0x00, // this is a 32-bit normalized floating point number read from registers 0x00-03
568    EM7180_QY = 0x04, // this is a 32-bit normalized floating point number read from registers 0x04-07
569    EM7180_QZ = 0x08, // this is a 32-bit normalized floating point number read from registers 0x08-0B
570    EM7180_QW = 0x0C, // this is a 32-bit normalized floating point number read from registers 0x0C-0F
571    EM7180_AX = 0x1A, // i16 from registers 0x1A-1B
572    EM7180_GX = 0x22, // i16 from registers 0x22-23
573    EM7180_MX = 0x12, // int16_t from registers 0x12-13
574    EM7180_Baro = 0x2A, // start of two-byte MS5637 pressure data, 16-bit signed interger
575    EM7180_Temp = 0x2E, // start of two-byte MS5637 temperature data, 16-bit signed interger
576}
577
578/// Transform raw quaternion buffer data into something meaningfull
579fn reg_to_float(buf: &[u8]) -> f32 {
580    unsafe { safe_transmute::guarded_transmute::<f32>(buf).unwrap() }
581}
582
583/// Transform raw temperature data into Celsius degrees
584pub fn raw_temperature_to_celsius(raw_temperature: i16) -> f64 {
585    raw_temperature as f64 * 0.01f64
586}
587
588/// Transform raw temperature data into Fahrenheit degrees
589pub fn raw_temperature_to_fahrenheit(raw_temperature: i16) -> f64 {
590    9.0f64 * raw_temperature_to_celsius(raw_temperature) / 5.0f64 + 32.0f64
591}
592
593/// Transform raw pressure data into mbar pressure
594pub fn raw_pressure_to_mbar(raw_pressure: i16) -> f64 {
595    raw_pressure as f64 * 0.01f64 + 1013.25f64
596}
597
598/// Transform raw pressure data into feet altitude
599pub fn raw_pressure_to_feet(_raw_pressure: i16) -> f64 {
600    // powf error, let's check
601    //let x = raw_pressure_to_mbar(raw_pressure) / 1013.25f64;
602    //145366.45f64 * (1.0f64 - x.powf(0.190284f64))
603    0.0
604}
em7180/lib.rs

em7180/
lib.rs
